57 constexpr Rotation3d(units::radian_t roll, units::radian_t pitch,
58 units::radian_t yaw) {
69 m_q =
Quaternion{cr * cp * cy + sr * sp * sy, sr * cp * cy - cr * sp * sy,
70 cr * sp * cy + sr * cp * sy, cr * cp * sy - sr * sp * cy};
80 constexpr Rotation3d(
const Eigen::Vector3d& axis, units::radian_t angle) {
109 constexpr explicit Rotation3d(
const Eigen::Matrix3d& rotationMatrix) {
110 auto impl = []<
typename Matrix3d>(
const Matrix3d& R) ->
Quaternion {
113 if ((R * R.transpose() - Matrix3d::Identity()).norm() > 1e-9) {
114 throw std::domain_error(
"Rotation matrix isn't orthogonal");
121 throw std::domain_error(
122 "Rotation matrix is orthogonal but not special orthogonal");
127 double trace = R(0, 0) + R(1, 1) + R(2, 2);
136 x = (R(2, 1) - R(1, 2)) * s;
137 y = (R(0, 2) - R(2, 0)) * s;
138 z = (R(1, 0) - R(0, 1)) * s;
140 if (R(0, 0) > R(1, 1) && R(0, 0) > R(2, 2)) {
141 double s = 2.0 *
gcem::sqrt(1.0 + R(0, 0) - R(1, 1) - R(2, 2));
142 w = (R(2, 1) - R(1, 2)) / s;
144 y = (R(0, 1) + R(1, 0)) / s;
145 z = (R(0, 2) + R(2, 0)) / s;
146 }
else if (R(1, 1) > R(2, 2)) {
147 double s = 2.0 *
gcem::sqrt(1.0 + R(1, 1) - R(0, 0) - R(2, 2));
148 w = (R(0, 2) - R(2, 0)) / s;
149 x = (R(0, 1) + R(1, 0)) / s;
151 z = (R(1, 2) + R(2, 1)) / s;
153 double s = 2.0 *
gcem::sqrt(1.0 + R(2, 2) - R(0, 0) - R(1, 1));
154 w = (R(1, 0) - R(0, 1)) / s;
155 x = (R(0, 2) + R(2, 0)) / s;
156 y = (R(1, 2) + R(2, 1)) / s;
164 if (std::is_constant_evaluated()) {
167 m_q = impl(rotationMatrix);
182 const Eigen::Vector3d&
final) {
185 double dotNorm = dot / normProduct;
187 if (dotNorm > 1.0 - 1E-9) {
191 }
else if (dotNorm < -1.0 + 1E-9) {
201 Eigen::Vector3d other;
205 other = Eigen::Vector3d{{1, 0, 0}};
208 other = Eigen::Vector3d{{0, 0, 1}};
213 other = Eigen::Vector3d{{0, 1, 0}};
216 other = Eigen::Vector3d{{0, 0, 1}};
222 double axisNorm = axis.norm();
223 m_q =
Quaternion{0.0, axis(0) / axisNorm, axis(1) / axisNorm,
242 :
Rotation3d{0_rad, 0_rad, rotation.Radians()} {}
252 return RotateBy(other);
264 return *
this + -other;
283 if (m_q.W() >= 0.0) {
284 return Rotation3d{Eigen::Vector3d{{m_q.X(), m_q.Y(), m_q.Z()}},
287 return Rotation3d{Eigen::Vector3d{{-m_q.X(), -m_q.Y(), -m_q.Z()}},
300 return *
this * (1.0 /
scalar);
308 m_q.Norm() * other.m_q.Norm()) < 1e-9;
334 constexpr units::radian_t
X()
const {
341 double cxcy = 1.0 - 2.0 * (x * x + y * y);
342 double sxcy = 2.0 * (w * x + y * z);
343 double cy_sq = cxcy * cxcy + sxcy * sxcy;
354 constexpr units::radian_t
Y()
const {
361 double ratio = 2.0 * (w * y - z * x);
363 return units::radian_t{
gcem::copysign(std::numbers::pi / 2.0, ratio)};
372 constexpr units::radian_t
Z()
const {
379 double cycz = 1.0 - 2.0 * (y * y + z * z);
380 double cysz = 2.0 * (w * z + x * y);
381 double cy_sq = cycz * cycz + cysz * cysz;
385 return units::radian_t{
gcem::atan2(2.0 * w * z, w * w - z * z)};
392 constexpr Eigen::Vector3d
Axis()
const {
393 double norm =
gcem::hypot(m_q.X(), m_q.Y(), m_q.Z());
395 return Eigen::Vector3d{{0.0, 0.0, 0.0}};
397 return Eigen::Vector3d{{m_q.X() / norm, m_q.Y() / norm, m_q.Z() / norm}};
404 constexpr units::radian_t
Angle()
const {
405 double norm =
gcem::hypot(m_q.X(), m_q.Y(), m_q.Z());
406 return units::radian_t{2.0 *
gcem::atan2(norm, m_q.W())};
419 return Eigen::Matrix3d{{1.0 - 2.0 * (y * y + z * z), 2.0 * (x * y - w * z),
420 2.0 * (x * z + w * y)},
421 {2.0 * (x * y + w * z), 1.0 - 2.0 * (x * x + z * z),
422 2.0 * (y * z - w * x)},
423 {2.0 * (x * z - w * y), 2.0 * (y * z + w * x),
424 1.0 - 2.0 * (x * x + y * y)}};
432 constexpr Eigen::Vector3d
ToVector()
const {
return m_q.ToRotationVector(); }
#define WPILIB_DLLEXPORT
Definition SymbolExports.h:36
namespace for Niels Lohmann
Definition json.h:99
Represents a quaternion.
Definition Quaternion.h:19
constexpr Quaternion Normalize() const
Normalizes the quaternion.
Definition Quaternion.h:138
A rotation in a 2D coordinate frame represented by a point on the unit circle (cosine and sine).
Definition Rotation2d.h:31
A rotation in a 3D coordinate frame represented by a quaternion.
Definition Rotation3d.h:29
constexpr const Quaternion & GetQuaternion() const
Returns the quaternion representation of the Rotation3d.
Definition Rotation3d.h:329
constexpr Rotation3d(const Quaternion &q)
Constructs a Rotation3d from a quaternion.
Definition Rotation3d.h:41
constexpr Rotation3d operator+(const Rotation3d &other) const
Adds two rotations together.
Definition Rotation3d.h:251
constexpr Eigen::Matrix3d ToMatrix() const
Returns rotation matrix representation of this rotation.
Definition Rotation3d.h:412
constexpr units::radian_t Z() const
Returns the counterclockwise rotation angle around the Z axis (yaw).
Definition Rotation3d.h:372
constexpr Rotation3d()=default
Constructs a Rotation3d representing no rotation.
constexpr Rotation3d(const Eigen::Vector3d &initial, const Eigen::Vector3d &final)
Constructs a Rotation3d that rotates the initial vector onto the final vector.
Definition Rotation3d.h:181
constexpr Rotation3d operator/(double scalar) const
Divides the current rotation by a scalar.
Definition Rotation3d.h:299
constexpr Rotation2d ToRotation2d() const
Returns a Rotation2d representing this Rotation3d projected into the X-Y plane.
Definition Rotation3d.h:438
constexpr Rotation3d(const Eigen::Matrix3d &rotationMatrix)
Constructs a Rotation3d from a rotation matrix.
Definition Rotation3d.h:109
constexpr Rotation3d operator*(double scalar) const
Multiplies the current rotation by a scalar.
Definition Rotation3d.h:281
constexpr Rotation3d operator-(const Rotation3d &other) const
Subtracts the new rotation from the current rotation and returns the new rotation.
Definition Rotation3d.h:263
constexpr Eigen::Vector3d Axis() const
Returns the axis in the axis-angle representation of this rotation.
Definition Rotation3d.h:392
constexpr Rotation3d operator-() const
Takes the inverse of the current rotation.
Definition Rotation3d.h:272
constexpr units::radian_t Angle() const
Returns the angle in the axis-angle representation of this rotation.
Definition Rotation3d.h:404
constexpr units::radian_t X() const
Returns the counterclockwise rotation angle around the X axis (roll).
Definition Rotation3d.h:334
constexpr Eigen::Vector3d ToVector() const
Returns rotation vector representation of this rotation.
Definition Rotation3d.h:432
constexpr Rotation3d RotateBy(const Rotation3d &other) const
Adds the new rotation to the current rotation.
Definition Rotation3d.h:322
constexpr Rotation3d(const Eigen::Vector3d &rvec)
Constructs a Rotation3d with the given rotation vector representation.
Definition Rotation3d.h:100
constexpr Rotation3d(const Eigen::Vector3d &axis, units::radian_t angle)
Constructs a Rotation3d with the given axis-angle representation.
Definition Rotation3d.h:80
constexpr units::radian_t Y() const
Returns the counterclockwise rotation angle around the Y axis (pitch).
Definition Rotation3d.h:354
constexpr bool operator==(const Rotation3d &other) const
Checks equality between this Rotation3d and another object.
Definition Rotation3d.h:306
constexpr Rotation3d(units::radian_t roll, units::radian_t pitch, units::radian_t yaw)
Constructs a Rotation3d from extrinsic roll, pitch, and yaw.
Definition Rotation3d.h:57
constexpr Rotation3d(const Rotation2d &rotation)
Constructs a 3D rotation from a 2D rotation in the X-Y plane.
Definition Rotation3d.h:241
Compile-time wrapper for Eigen::Matrix.
Definition ct_matrix.h:26
constexpr Scalar dot(const ct_matrix< Scalar, RhsRows, RhsCols > &rhs) const
Constexpr version of Eigen's vector dot member function.
Definition ct_matrix.h:262
constexpr Scalar determinant() const
Constexpr version of Eigen's 2x2 matrix determinant member function.
Definition ct_matrix.h:316
constexpr Scalar norm() const
Constexpr version of Eigen's norm member function.
Definition ct_matrix.h:281
constexpr ct_matrix< Scalar, 3, 1 > cross(const ct_matrix< Scalar, 3, 1 > &rhs)
Constexpr version of Eigen's 3D vector cross member function.
Definition ct_matrix.h:303
constexpr dimensionless::scalar_t sin(const AngleUnit angle) noexcept
Compute sine.
Definition math.h:83
constexpr dimensionless::scalar_t cos(const AngleUnit angle) noexcept
Compute cosine.
Definition math.h:63
WPILIB_DLLEXPORT void to_json(wpi::json &json, const Rotation3d &rotation)
WPILIB_DLLEXPORT void from_json(const wpi::json &json, Rotation3d &rotation)
constexpr T abs(const T x) noexcept
Compile-time absolute value function.
Definition abs.hpp:40
constexpr return_t< T > asin(const T x) noexcept
Compile-time arcsine function.
Definition asin.hpp:82
constexpr T1 copysign(const T1 x, const T2 y) noexcept
Compile-time copy sign function.
Definition copysign.hpp:41
constexpr common_return_t< T1, T2 > hypot(const T1 x, const T2 y) noexcept
Compile-time Pythagorean addition function.
Definition hypot.hpp:147
constexpr return_t< T > sqrt(const T x) noexcept
Compile-time square-root function.
Definition sqrt.hpp:109
constexpr return_t< T > acos(const T x) noexcept
Compile-time arccosine function.
Definition acos.hpp:84
constexpr common_return_t< T1, T2 > atan2(const T1 y, const T2 x) noexcept
Compile-time two-argument arctangent function.
Definition atan2.hpp:88
Unit Conversion Library namespace.
Definition acceleration.h:33
Type representing an arbitrary unit.
Definition base.h:888