NumCpp  2.11.0
A Templatized Header Only C++ Implementation of the Python NumPy Library
ECEFEulerToNEDRollPitchYaw.hpp
Go to the documentation of this file.
1 #pragma once
29 
30 #include <cmath>
31 
38 #include "NumCpp/Vector/Vec3.hpp"
39 
41 {
49  [[nodiscard]] inline Orientation ECEFEulerToNEDRollPitchYaw(const reference_frames::ECEF& location,
50  const Euler& orientation) noexcept
51  {
52  const auto x0 = Vec3::right();
53  const auto y0 = Vec3::up();
54  const auto z0 = Vec3::forward();
55 
56  // first rotation array, z0 by psi
57  const auto quatPsi = rotations::Quaternion{ z0, orientation.psi };
58 
59  // rotate
60  const auto x1 = quatPsi * x0;
61  const auto y1 = quatPsi * y0;
62 
63  // second rotation array, y1 by theta
64  const auto quatTheta = rotations::Quaternion{ y1, orientation.theta };
65 
66  // rotate
67  const auto x2 = quatTheta * x1;
68  const auto y2 = quatTheta * y1;
69 
70  // third rotation array, x2 by phi
71  const auto quatPhi = rotations::Quaternion{ x2, orientation.phi };
72 
73  // rotate
74  const auto x3 = quatPhi * x2;
75  const auto y3 = quatPhi * y2;
76 
77  // get the local NED unit vectors wrt the ECEF coordinate system
78  const auto& [xHat0, yHat0, zHat0] = NEDUnitVecsInECEF(location);
79 
80  // calculate yaw and pitch
81  const auto yaw = std::atan2(x3.dot(yHat0), x3.dot(xHat0));
82  const auto pitch = std::atan(-x3.dot(zHat0) / std::hypot(x3.dot(xHat0), x3.dot(yHat0)));
83 
84  // calculate roll
85  const auto yHat2 = (rotations::Quaternion{ zHat0, yaw } * yHat0);
86  const auto zHat2 = (rotations::Quaternion{ yHat2, pitch } * zHat0);
87  const auto roll = std::atan2(y3.dot(zHat2), y3.dot(yHat2));
88 
89  return { wrap(roll), pitch, wrap(yaw) };
90  }
91 } // namespace nc::coordinates::transforms
static constexpr Vec3 up() noexcept
Definition: Vec3.hpp:349
static constexpr Vec3 forward() noexcept
Definition: Vec3.hpp:235
static constexpr Vec3 right() noexcept
Definition: Vec3.hpp:313
Euler.
Definition: Euler.hpp:40
Orientation.
Definition: Orientation.hpp:40
ECEF coordinates.
Definition: ECEF.hpp:40
Holds a unit quaternion.
Definition: Quaternion.hpp:56
Definition: AERtoECEF.hpp:38
Orientation ECEFEulerToNEDRollPitchYaw(const reference_frames::ECEF &location, const Euler &orientation) noexcept
Converts ECEF euler angles to body roll/pitch/yaw.
Definition: ECEFEulerToNEDRollPitchYaw.hpp:49
std::array< Vec3, 3 > NEDUnitVecsInECEF(const reference_frames::ECEF &location) noexcept
get&#160;the&#160;local&#160;NED&#160;unit&#160;vectors&#160;wrt&#160;the&#160;ECEF&#160;coordinate&#160;system https://gssc.esa.int/navipedia/index....
Definition: NEDUnitVecsInECEF.hpp:45
double hypot(dtype inValue1, dtype inValue2) noexcept
Definition: hypot.hpp:56
NdArray< dtype > roll(const NdArray< dtype > &inArray, int32 inShift, Axis inAxis=Axis::NONE)
Definition: roll.hpp:52
double wrap(dtype inAngle) noexcept
Wrap the input angle to [-pi, pi].
Definition: wrap.hpp:45