NumCpp  2.12.0
A Templatized Header Only C++ Implementation of the Python NumPy Library
ECEFEulerToNEDRollPitchYaw.hpp
Go to the documentation of this file.
1
28#pragma once
29
30#include <cmath>
31
39
41{
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 the local NED unit vectors wrt the ECEF coordinate system https://gssc.esa.int/navipedia/index....
Definition: NEDUnitVecsInECEF.hpp:45
double hypot(dtype inValue1, dtype inValue2) noexcept
Definition: hypot.hpp:56
double wrap(dtype inAngle) noexcept
Wrap the input angle to [-pi, pi].
Definition: wrap.hpp:45
NdArray< dtype > roll(const NdArray< dtype > &inArray, int32 inShift, Axis inAxis=Axis::NONE)
Definition: roll.hpp:52