WPILibC++ 2025.3.1
Loading...
Searching...
No Matches
Rotation3d.h
Go to the documentation of this file.
1// Copyright (c) FIRST and other WPILib contributors.
2// Open Source Software; you can modify and/or share it under the terms of
3// the WPILib BSD license file in the root directory of this project.
4
5#pragma once
6
7#include <string>
8#include <type_traits>
9
10#include <Eigen/Core>
11#include <fmt/format.h>
12#include <gcem.hpp>
13#include <wpi/SymbolExports.h>
14#include <wpi/json_fwd.h>
15
16#include "frc/ct_matrix.h"
17#include "frc/fmt/Eigen.h"
20#include "units/angle.h"
21#include "units/math.h"
22#include "wpimath/MathShared.h"
23
24namespace frc {
25
26/**
27 * A rotation in a 3D coordinate frame represented by a quaternion.
28 */
30 public:
31 /**
32 * Constructs a Rotation3d representing no rotation.
33 */
34 constexpr Rotation3d() = default;
35
36 /**
37 * Constructs a Rotation3d from a quaternion.
38 *
39 * @param q The quaternion.
40 */
41 constexpr explicit Rotation3d(const Quaternion& q) { m_q = q.Normalize(); }
42
43 /**
44 * Constructs a Rotation3d from extrinsic roll, pitch, and yaw.
45 *
46 * Extrinsic rotations occur in that order around the axes in the fixed global
47 * frame rather than the body frame.
48 *
49 * Angles are measured counterclockwise with the rotation axis pointing "out
50 * of the page". If you point your right thumb along the positive axis
51 * direction, your fingers curl in the direction of positive rotation.
52 *
53 * @param roll The counterclockwise rotation angle around the X axis (roll).
54 * @param pitch The counterclockwise rotation angle around the Y axis (pitch).
55 * @param yaw The counterclockwise rotation angle around the Z axis (yaw).
56 */
57 constexpr Rotation3d(units::radian_t roll, units::radian_t pitch,
58 units::radian_t yaw) {
59 // https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Euler_angles_to_quaternion_conversion
60 double cr = units::math::cos(roll * 0.5);
61 double sr = units::math::sin(roll * 0.5);
62
63 double cp = units::math::cos(pitch * 0.5);
64 double sp = units::math::sin(pitch * 0.5);
65
66 double cy = units::math::cos(yaw * 0.5);
67 double sy = units::math::sin(yaw * 0.5);
68
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};
71 }
72
73 /**
74 * Constructs a Rotation3d with the given axis-angle representation. The axis
75 * doesn't have to be normalized.
76 *
77 * @param axis The rotation axis.
78 * @param angle The rotation around the axis.
79 */
80 constexpr Rotation3d(const Eigen::Vector3d& axis, units::radian_t angle) {
81 double norm = ct_matrix{axis}.norm();
82 if (norm == 0.0) {
83 return;
84 }
85
86 // https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Definition
87 Eigen::Vector3d v{{axis(0) / norm * units::math::sin(angle / 2.0),
88 axis(1) / norm * units::math::sin(angle / 2.0),
89 axis(2) / norm * units::math::sin(angle / 2.0)}};
90 m_q = Quaternion{units::math::cos(angle / 2.0), v(0), v(1), v(2)};
91 }
92
93 /**
94 * Constructs a Rotation3d with the given rotation vector representation. This
95 * representation is equivalent to axis-angle, where the normalized axis is
96 * multiplied by the rotation around the axis in radians.
97 *
98 * @param rvec The rotation vector.
99 */
100 constexpr explicit Rotation3d(const Eigen::Vector3d& rvec)
101 : Rotation3d{rvec, units::radian_t{ct_matrix{rvec}.norm()}} {}
102
103 /**
104 * Constructs a Rotation3d from a rotation matrix.
105 *
106 * @param rotationMatrix The rotation matrix.
107 * @throws std::domain_error if the rotation matrix isn't special orthogonal.
108 */
109 constexpr explicit Rotation3d(const Eigen::Matrix3d& rotationMatrix) {
110 auto impl = []<typename Matrix3d>(const Matrix3d& R) -> Quaternion {
111 // Require that the rotation matrix is special orthogonal. This is true if
112 // the matrix is orthogonal (RRᵀ = I) and normalized (determinant is 1).
113 if ((R * R.transpose() - Matrix3d::Identity()).norm() > 1e-9) {
114 throw std::domain_error("Rotation matrix isn't orthogonal");
115 }
116 // HACK: Uses ct_matrix instead of <Eigen/LU> for determinant because
117 // including <Eigen/LU> doubles compilation times on MSVC, even if
118 // this constructor is unused. MSVC's frontend inefficiently parses
119 // large headers; GCC and Clang are largely unaffected.
120 if (gcem::abs(ct_matrix{R}.determinant() - 1.0) > 1e-9) {
121 throw std::domain_error(
122 "Rotation matrix is orthogonal but not special orthogonal");
123 }
124
125 // Turn rotation matrix into a quaternion
126 // https://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/
127 double trace = R(0, 0) + R(1, 1) + R(2, 2);
128 double w;
129 double x;
130 double y;
131 double z;
132
133 if (trace > 0.0) {
134 double s = 0.5 / gcem::sqrt(trace + 1.0);
135 w = 0.25 / s;
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;
139 } else {
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;
143 x = 0.25 * 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;
150 y = 0.25 * s;
151 z = (R(1, 2) + R(2, 1)) / s;
152 } else {
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;
157 z = 0.25 * s;
158 }
159 }
160
161 return Quaternion{w, x, y, z};
162 };
163
164 if (std::is_constant_evaluated()) {
165 m_q = impl(ct_matrix3d{rotationMatrix});
166 } else {
167 m_q = impl(rotationMatrix);
168 }
169 }
170
171 /**
172 * Constructs a Rotation3d that rotates the initial vector onto the final
173 * vector.
174 *
175 * This is useful for turning a 3D vector (final) into an orientation relative
176 * to a coordinate system vector (initial).
177 *
178 * @param initial The initial vector.
179 * @param final The final vector.
180 */
181 constexpr Rotation3d(const Eigen::Vector3d& initial,
182 const Eigen::Vector3d& final) {
183 double dot = ct_matrix{initial}.dot(ct_matrix{final});
184 double normProduct = ct_matrix{initial}.norm() * ct_matrix{final}.norm();
185 double dotNorm = dot / normProduct;
186
187 if (dotNorm > 1.0 - 1E-9) {
188 // If the dot product is 1, the two vectors point in the same direction so
189 // there's no rotation. The default initialization of m_q will work.
190 return;
191 } else if (dotNorm < -1.0 + 1E-9) {
192 // If the dot product is -1, the two vectors are antiparallel, so a 180°
193 // rotation is required. Any other vector can be used to generate an
194 // orthogonal one.
195
196 double x = gcem::abs(initial(0));
197 double y = gcem::abs(initial(1));
198 double z = gcem::abs(initial(2));
199
200 // Find vector that is most orthogonal to initial vector
201 Eigen::Vector3d other;
202 if (x < y) {
203 if (x < z) {
204 // Use x-axis
205 other = Eigen::Vector3d{{1, 0, 0}};
206 } else {
207 // Use z-axis
208 other = Eigen::Vector3d{{0, 0, 1}};
209 }
210 } else {
211 if (y < z) {
212 // Use y-axis
213 other = Eigen::Vector3d{{0, 1, 0}};
214 } else {
215 // Use z-axis
216 other = Eigen::Vector3d{{0, 0, 1}};
217 }
218 }
219
220 auto axis = ct_matrix{initial}.cross(ct_matrix{other});
221
222 double axisNorm = axis.norm();
223 m_q = Quaternion{0.0, axis(0) / axisNorm, axis(1) / axisNorm,
224 axis(2) / axisNorm};
225 } else {
226 auto axis = ct_matrix{initial}.cross(final);
227
228 // https://stackoverflow.com/a/11741520
229 m_q =
230 Quaternion{normProduct + dot, axis(0), axis(1), axis(2)}.Normalize();
231 }
232 }
233
234 /**
235 * Constructs a 3D rotation from a 2D rotation in the X-Y plane.
236 *
237 * @param rotation The 2D rotation.
238 * @see Pose3d(Pose2d)
239 * @see Transform3d(Transform2d)
240 */
241 constexpr explicit Rotation3d(const Rotation2d& rotation)
242 : Rotation3d{0_rad, 0_rad, rotation.Radians()} {}
243
244 /**
245 * Adds two rotations together.
246 *
247 * @param other The rotation to add.
248 *
249 * @return The sum of the two rotations.
250 */
251 constexpr Rotation3d operator+(const Rotation3d& other) const {
252 return RotateBy(other);
253 }
254
255 /**
256 * Subtracts the new rotation from the current rotation and returns the new
257 * rotation.
258 *
259 * @param other The rotation to subtract.
260 *
261 * @return The difference between the two rotations.
262 */
263 constexpr Rotation3d operator-(const Rotation3d& other) const {
264 return *this + -other;
265 }
266
267 /**
268 * Takes the inverse of the current rotation.
269 *
270 * @return The inverse of the current rotation.
271 */
272 constexpr Rotation3d operator-() const { return Rotation3d{m_q.Inverse()}; }
273
274 /**
275 * Multiplies the current rotation by a scalar.
276 *
277 * @param scalar The scalar.
278 *
279 * @return The new scaled Rotation3d.
280 */
281 constexpr Rotation3d operator*(double scalar) const {
282 // https://en.wikipedia.org/wiki/Slerp#Quaternion_Slerp
283 if (m_q.W() >= 0.0) {
284 return Rotation3d{Eigen::Vector3d{{m_q.X(), m_q.Y(), m_q.Z()}},
285 2.0 * units::radian_t{scalar * gcem::acos(m_q.W())}};
286 } else {
287 return Rotation3d{Eigen::Vector3d{{-m_q.X(), -m_q.Y(), -m_q.Z()}},
288 2.0 * units::radian_t{scalar * gcem::acos(-m_q.W())}};
289 }
290 }
291
292 /**
293 * Divides the current rotation by a scalar.
294 *
295 * @param scalar The scalar.
296 *
297 * @return The new scaled Rotation3d.
298 */
299 constexpr Rotation3d operator/(double scalar) const {
300 return *this * (1.0 / scalar);
301 }
302
303 /**
304 * Checks equality between this Rotation3d and another object.
305 */
306 constexpr bool operator==(const Rotation3d& other) const {
307 return gcem::abs(gcem::abs(m_q.Dot(other.m_q)) -
308 m_q.Norm() * other.m_q.Norm()) < 1e-9;
309 }
310
311 /**
312 * Adds the new rotation to the current rotation. The other rotation is
313 * applied extrinsically, which means that it rotates around the global axes.
314 * For example, Rotation3d{90_deg, 0, 0}.RotateBy(Rotation3d{0, 45_deg, 0})
315 * rotates by 90 degrees around the +X axis and then by 45 degrees around the
316 * global +Y axis. (This is equivalent to Rotation3d{90_deg, 45_deg, 0})
317 *
318 * @param other The extrinsic rotation to rotate by.
319 *
320 * @return The new rotated Rotation3d.
321 */
322 constexpr Rotation3d RotateBy(const Rotation3d& other) const {
323 return Rotation3d{other.m_q * m_q};
324 }
325
326 /**
327 * Returns the quaternion representation of the Rotation3d.
328 */
329 constexpr const Quaternion& GetQuaternion() const { return m_q; }
330
331 /**
332 * Returns the counterclockwise rotation angle around the X axis (roll).
333 */
334 constexpr units::radian_t X() const {
335 double w = m_q.W();
336 double x = m_q.X();
337 double y = m_q.Y();
338 double z = m_q.Z();
339
340 // wpimath/algorithms.md
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;
344 if (cy_sq > 1e-20) {
345 return units::radian_t{gcem::atan2(sxcy, cxcy)};
346 } else {
347 return 0_rad;
348 }
349 }
350
351 /**
352 * Returns the counterclockwise rotation angle around the Y axis (pitch).
353 */
354 constexpr units::radian_t Y() const {
355 double w = m_q.W();
356 double x = m_q.X();
357 double y = m_q.Y();
358 double z = m_q.Z();
359
360 // https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Quaternion_to_Euler_angles_(in_3-2-1_sequence)_conversion
361 double ratio = 2.0 * (w * y - z * x);
362 if (gcem::abs(ratio) >= 1.0) {
363 return units::radian_t{gcem::copysign(std::numbers::pi / 2.0, ratio)};
364 } else {
365 return units::radian_t{gcem::asin(ratio)};
366 }
367 }
368
369 /**
370 * Returns the counterclockwise rotation angle around the Z axis (yaw).
371 */
372 constexpr units::radian_t Z() const {
373 double w = m_q.W();
374 double x = m_q.X();
375 double y = m_q.Y();
376 double z = m_q.Z();
377
378 // wpimath/algorithms.md
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;
382 if (cy_sq > 1e-20) {
383 return units::radian_t{gcem::atan2(cysz, cycz)};
384 } else {
385 return units::radian_t{gcem::atan2(2.0 * w * z, w * w - z * z)};
386 }
387 }
388
389 /**
390 * Returns the axis in the axis-angle representation of this rotation.
391 */
392 constexpr Eigen::Vector3d Axis() const {
393 double norm = gcem::hypot(m_q.X(), m_q.Y(), m_q.Z());
394 if (norm == 0.0) {
395 return Eigen::Vector3d{{0.0, 0.0, 0.0}};
396 } else {
397 return Eigen::Vector3d{{m_q.X() / norm, m_q.Y() / norm, m_q.Z() / norm}};
398 }
399 }
400
401 /**
402 * Returns the angle in the axis-angle representation of this rotation.
403 */
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())};
407 }
408
409 /**
410 * Returns rotation matrix representation of this rotation.
411 */
412 constexpr Eigen::Matrix3d ToMatrix() const {
413 double w = m_q.W();
414 double x = m_q.X();
415 double y = m_q.Y();
416 double z = m_q.Z();
417
418 // https://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation#Quaternion-derived_rotation_matrix
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)}};
425 }
426
427 /**
428 * Returns rotation vector representation of this rotation.
429 *
430 * @return Rotation vector representation of this rotation.
431 */
432 constexpr Eigen::Vector3d ToVector() const { return m_q.ToRotationVector(); }
433
434 /**
435 * Returns a Rotation2d representing this Rotation3d projected into the X-Y
436 * plane.
437 */
438 constexpr Rotation2d ToRotation2d() const { return Rotation2d{Z()}; }
439
440 private:
441 Quaternion m_q;
442};
443
445void to_json(wpi::json& json, const Rotation3d& rotation);
446
448void from_json(const wpi::json& json, Rotation3d& rotation);
449
450} // namespace frc
451
#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
Definition CAN.h:11
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