NumCpp  2.12.0
A Templatized Header Only C++ Implementation of the Python NumPy Library
NEDRollPitchYawToECEFEuler.hpp
Go to the documentation of this file.
1
28#pragma once
29
30#include <cmath>
31
39
41{
49 [[nodiscard]] inline Euler NEDRollPitchYawToECEFEuler(const reference_frames::ECEF& location,
50 const Orientation& orientation) noexcept
51 {
52 // get the local NED unit vectors wrt the ECEF coordinate system
53 const auto& [x0, y0, z0] = NEDUnitVecsInECEF(location);
54
55 // first rotation array, z0 by yaw
56 const auto quatYaw = rotations::Quaternion{ z0, orientation.yaw };
57
58 // rotate
59 const auto x1 = quatYaw * x0;
60 const auto y1 = quatYaw * y0;
61
62 // second rotation array, y1 by pitch
63 const auto quatPitch = rotations::Quaternion{ y1, orientation.pitch };
64
65 // rotate
66 const auto x2 = quatPitch * x1;
67 const auto y2 = quatPitch * y1;
68
69 // third rotation array, x2 by roll
70 const auto quatRoll = rotations::Quaternion{ x2, orientation.roll };
71
72 // rotate
73 const auto x3 = quatRoll * x2;
74 const auto y3 = quatRoll * y2;
75
76 // calculate phi and theta
77 const auto xHat0 = Vec3::right();
78 const auto yHat0 = Vec3::up();
79 const auto zHat0 = Vec3::forward();
80
81 const auto psi = std::atan2(x3.dot(yHat0), x3.dot(xHat0));
82 const auto theta = std::atan(-x3.dot(zHat0) / std::hypot(x3.dot(xHat0), x3.dot(yHat0)));
83
84 // calculate phi
85 const auto yHat2 = (rotations::Quaternion{ zHat0, psi } * yHat0);
86 const auto zHat2 = (rotations::Quaternion{ yHat2, theta } * zHat0);
87 const auto phi = std::atan2(y3.dot(zHat2), y3.dot(yHat2));
88
89 return { wrap(psi), theta, wrap(phi) };
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
double roll() const noexcept
Definition: Quaternion.hpp:402
double yaw() const noexcept
Definition: Quaternion.hpp:607
double pitch() const noexcept
Definition: Quaternion.hpp:370
Definition: AERtoECEF.hpp:38
Euler NEDRollPitchYawToECEFEuler(const reference_frames::ECEF &location, const Orientation &orientation) noexcept
Converts NED body roll/pitch/yaw to ECEF euler angles.
Definition: NEDRollPitchYawToECEFEuler.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