59 const auto x1 = quatYaw * x0;
60 const auto y1 = quatYaw * y0;
66 const auto x2 = quatPitch * x1;
67 const auto y2 = quatPitch * y1;
73 const auto x3 = quatRoll * x2;
74 const auto y3 = quatRoll * y2;
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)));
87 const auto phi = std::atan2(y3.dot(zHat2), y3.dot(yHat2));
89 return {
wrap(psi), theta,
wrap(phi) };
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
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