NumCpp  2.11.0
A Templatized Header Only C++ Implementation of the Python NumPy Library
NEDRollPitchYawToECEFEuler.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 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
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
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
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