WPILibC++ 2025.2.1
Loading...
Searching...
No Matches
Quaternion.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 <numbers>
8
9#include <Eigen/Core>
10#include <gcem.hpp>
11#include <wpi/SymbolExports.h>
12#include <wpi/json_fwd.h>
13
14namespace frc {
15
16/**
17 * Represents a quaternion.
18 */
20 public:
21 /**
22 * Constructs a quaternion with a default angle of 0 degrees.
23 */
24 constexpr Quaternion() = default;
25
26 /**
27 * Constructs a quaternion with the given components.
28 *
29 * @param w W component of the quaternion.
30 * @param x X component of the quaternion.
31 * @param y Y component of the quaternion.
32 * @param z Z component of the quaternion.
33 */
34 constexpr Quaternion(double w, double x, double y, double z)
35 : m_w{w}, m_x{x}, m_y{y}, m_z{z} {}
36
37 /**
38 * Adds with another quaternion.
39 *
40 * @param other the other quaternion
41 */
42 constexpr Quaternion operator+(const Quaternion& other) const {
43 return Quaternion{m_w + other.m_w, m_x + other.m_x, m_y + other.m_y,
44 m_z + other.m_z};
45 }
46
47 /**
48 * Subtracts another quaternion.
49 *
50 * @param other the other quaternion
51 */
52 constexpr Quaternion operator-(const Quaternion& other) const {
53 return Quaternion{m_w - other.m_w, m_x - other.m_x, m_y - other.m_y,
54 m_z - other.m_z};
55 }
56
57 /**
58 * Multiples with a scalar value.
59 *
60 * @param other the scalar value
61 */
62 constexpr Quaternion operator*(double other) const {
63 return Quaternion{m_w * other, m_x * other, m_y * other, m_z * other};
64 }
65
66 /**
67 * Divides by a scalar value.
68 *
69 * @param other the scalar value
70 */
71 constexpr Quaternion operator/(double other) const {
72 return Quaternion{m_w / other, m_x / other, m_y / other, m_z / other};
73 }
74
75 /**
76 * Multiply with another quaternion.
77 *
78 * @param other The other quaternion.
79 */
80 constexpr Quaternion operator*(const Quaternion& other) const {
81 // https://en.wikipedia.org/wiki/Quaternion#Scalar_and_vector_parts
82 const auto& r1 = m_w;
83 const auto& r2 = other.m_w;
84
85 // v₁ ⋅ v₂
86 double dot = m_x * other.m_x + m_y * other.m_y + m_z * other.m_z;
87
88 // v₁ x v₂
89 double cross_x = m_y * other.m_z - other.m_y * m_z;
90 double cross_y = other.m_x * m_z - m_x * other.m_z;
91 double cross_z = m_x * other.m_y - other.m_x * m_y;
92
93 return Quaternion{// r = r₁r₂ − v₁ ⋅ v₂
94 r1 * r2 - dot,
95 // v = r₁v₂ + r₂v₁ + v₁ x v₂
96 r1 * other.m_x + r2 * m_x + cross_x,
97 r1 * other.m_y + r2 * m_y + cross_y,
98 r1 * other.m_z + r2 * m_z + cross_z};
99 }
100
101 /**
102 * Checks equality between this Quaternion and another object.
103 *
104 * @param other The other object.
105 * @return Whether the two objects are equal.
106 */
107 constexpr bool operator==(const Quaternion& other) const {
108 return gcem::abs(Dot(other) - Norm() * other.Norm()) < 1e-9 &&
109 gcem::abs(Norm() - other.Norm()) < 1e-9;
110 }
111
112 /**
113 * Returns the elementwise product of two quaternions.
114 */
115 constexpr double Dot(const Quaternion& other) const {
116 return W() * other.W() + X() * other.X() + Y() * other.Y() +
117 Z() * other.Z();
118 }
119
120 /**
121 * Returns the conjugate of the quaternion.
122 */
123 constexpr Quaternion Conjugate() const {
124 return Quaternion{W(), -X(), -Y(), -Z()};
125 }
126
127 /**
128 * Returns the inverse of the quaternion.
129 */
130 constexpr Quaternion Inverse() const {
131 double norm = Norm();
132 return Conjugate() / (norm * norm);
133 }
134
135 /**
136 * Normalizes the quaternion.
137 */
138 constexpr Quaternion Normalize() const {
139 double norm = Norm();
140 if (norm == 0.0) {
141 return Quaternion{};
142 } else {
143 return Quaternion{W(), X(), Y(), Z()} / norm;
144 }
145 }
146
147 /**
148 * Calculates the L2 norm of the quaternion.
149 */
150 constexpr double Norm() const { return gcem::sqrt(Dot(*this)); }
151
152 /**
153 * Calculates this quaternion raised to a power.
154 *
155 * @param t the power to raise this quaternion to.
156 */
157 constexpr Quaternion Pow(double t) const { return (Log() * t).Exp(); }
158
159 /**
160 * Matrix exponential of a quaternion.
161 *
162 * @param other the "Twist" that will be applied to this quaternion.
163 */
164 constexpr Quaternion Exp(const Quaternion& other) const {
165 return other.Exp() * *this;
166 }
167
168 /**
169 * Matrix exponential of a quaternion.
170 *
171 * source: wpimath/algorithms.md
172 *
173 * If this quaternion is in 𝖘𝖔(3) and you are looking for an element of
174 * SO(3), use FromRotationVector
175 */
176 constexpr Quaternion Exp() const {
177 double scalar = gcem::exp(m_w);
178
179 double axial_magnitude = gcem::hypot(m_x, m_y, m_z);
180 double cosine = gcem::cos(axial_magnitude);
181
182 double axial_scalar;
183
184 if (axial_magnitude < 1e-9) {
185 // Taylor series of sin(x)/x near x=0: 1 − x²/6 + x⁴/120 + O(n⁶)
186 double axial_magnitude_sq = axial_magnitude * axial_magnitude;
187 double axial_magnitude_sq_sq = axial_magnitude_sq * axial_magnitude_sq;
188 axial_scalar =
189 1.0 - axial_magnitude_sq / 6.0 + axial_magnitude_sq_sq / 120.0;
190 } else {
191 axial_scalar = gcem::sin(axial_magnitude) / axial_magnitude;
192 }
193
194 return Quaternion(cosine * scalar, X() * axial_scalar * scalar,
195 Y() * axial_scalar * scalar, Z() * axial_scalar * scalar);
196 }
197
198 /**
199 * Log operator of a quaternion.
200 *
201 * @param other The quaternion to map this quaternion onto
202 */
203 constexpr Quaternion Log(const Quaternion& other) const {
204 return (other * Inverse()).Log();
205 }
206
207 /**
208 * Log operator of a quaternion.
209 *
210 * source: wpimath/algorithms.md
211 *
212 * If this quaternion is in SO(3) and you are looking for an element of 𝖘𝖔(3),
213 * use ToRotationVector
214 */
215 constexpr Quaternion Log() const {
216 double norm = Norm();
217 double scalar = gcem::log(norm);
218
219 double v_norm = gcem::hypot(m_x, m_y, m_z);
220
221 double s_norm = W() / norm;
222
223 if (gcem::abs(s_norm + 1) < 1e-9) {
224 return Quaternion{scalar, -std::numbers::pi, 0, 0};
225 }
226
227 double v_scalar;
228
229 if (v_norm < 1e-9) {
230 // Taylor series expansion of atan2(y/x)/y at y = 0:
231 //
232 // 1/x - 1/3 y²/x³ + O(y⁴)
233 v_scalar = 1.0 / W() - 1.0 / 3.0 * v_norm * v_norm / (W() * W() * W());
234 } else {
235 v_scalar = gcem::atan2(v_norm, W()) / v_norm;
236 }
237
238 return Quaternion{scalar, v_scalar * m_x, v_scalar * m_y, v_scalar * m_z};
239 }
240
241 /**
242 * Returns W component of the quaternion.
243 */
244 constexpr double W() const { return m_w; }
245
246 /**
247 * Returns X component of the quaternion.
248 */
249 constexpr double X() const { return m_x; }
250
251 /**
252 * Returns Y component of the quaternion.
253 */
254 constexpr double Y() const { return m_y; }
255
256 /**
257 * Returns Z component of the quaternion.
258 */
259 constexpr double Z() const { return m_z; }
260
261 /**
262 * Returns the rotation vector representation of this quaternion.
263 *
264 * This is also the log operator of SO(3).
265 */
266 constexpr Eigen::Vector3d ToRotationVector() const {
267 // See equation (31) in "Integrating Generic Sensor Fusion Algorithms with
268 // Sound State Representation through Encapsulation of Manifolds"
269 //
270 // https://arxiv.org/pdf/1107.1119.pdf
271
272 double norm = gcem::hypot(m_x, m_y, m_z);
273
274 double scalar;
275 if (norm < 1e-9) {
276 scalar = (2.0 / W() - 2.0 / 3.0 * norm * norm / (W() * W() * W()));
277 } else {
278 if (W() < 0.0) {
279 scalar = 2.0 * gcem::atan2(-norm, -W()) / norm;
280 } else {
281 scalar = 2.0 * gcem::atan2(norm, W()) / norm;
282 }
283 }
284
285 return Eigen::Vector3d{{scalar * m_x, scalar * m_y, scalar * m_z}};
286 }
287
288 /**
289 * Returns the quaternion representation of this rotation vector.
290 *
291 * This is also the exp operator of 𝖘𝖔(3).
292 *
293 * source: wpimath/algorithms.md
294 */
295 constexpr static Quaternion FromRotationVector(const Eigen::Vector3d& rvec) {
296 // 𝑣⃗ = θ * v̂
297 // v̂ = 𝑣⃗ / θ
298
299 // 𝑞 = std::cos(θ/2) + std::sin(θ/2) * v̂
300 // 𝑞 = std::cos(θ/2) + std::sin(θ/2) / θ * 𝑣⃗
301
302 double theta = gcem::hypot(rvec(0), rvec(1), rvec(2));
303 double cos = gcem::cos(theta / 2);
304
305 double axial_scalar;
306
307 if (theta < 1e-9) {
308 // taylor series expansion of sin(θ/2) / θ around θ = 0 = 1/2 - θ²/48 +
309 // O(θ⁴)
310 axial_scalar = 1.0 / 2.0 - theta * theta / 48.0;
311 } else {
312 axial_scalar = gcem::sin(theta / 2) / theta;
313 }
314
315 return Quaternion{cos, axial_scalar * rvec(0), axial_scalar * rvec(1),
316 axial_scalar * rvec(2)};
317 }
318
319 private:
320 // Scalar r in versor form
321 double m_w = 1.0;
322
323 // Vector v in versor form
324 double m_x = 0.0;
325 double m_y = 0.0;
326 double m_z = 0.0;
327};
328
330void to_json(wpi::json& json, const Quaternion& quaternion);
331
333void from_json(const wpi::json& json, Quaternion& quaternion);
334
335} // namespace frc
336
#define WPILIB_DLLEXPORT
Definition SymbolExports.h:36
namespace for Niels Lohmann
Definition json.h:99
Represents a quaternion.
Definition Quaternion.h:19
constexpr Quaternion Log(const Quaternion &other) const
Log operator of a quaternion.
Definition Quaternion.h:203
constexpr double Dot(const Quaternion &other) const
Returns the elementwise product of two quaternions.
Definition Quaternion.h:115
constexpr bool operator==(const Quaternion &other) const
Checks equality between this Quaternion and another object.
Definition Quaternion.h:107
constexpr double Z() const
Returns Z component of the quaternion.
Definition Quaternion.h:259
constexpr Quaternion operator+(const Quaternion &other) const
Adds with another quaternion.
Definition Quaternion.h:42
constexpr Quaternion Normalize() const
Normalizes the quaternion.
Definition Quaternion.h:138
constexpr Quaternion operator*(const Quaternion &other) const
Multiply with another quaternion.
Definition Quaternion.h:80
constexpr Quaternion(double w, double x, double y, double z)
Constructs a quaternion with the given components.
Definition Quaternion.h:34
constexpr Quaternion operator*(double other) const
Multiples with a scalar value.
Definition Quaternion.h:62
constexpr Quaternion Conjugate() const
Returns the conjugate of the quaternion.
Definition Quaternion.h:123
constexpr Quaternion Pow(double t) const
Calculates this quaternion raised to a power.
Definition Quaternion.h:157
constexpr Quaternion operator/(double other) const
Divides by a scalar value.
Definition Quaternion.h:71
constexpr double Y() const
Returns Y component of the quaternion.
Definition Quaternion.h:254
constexpr Quaternion()=default
Constructs a quaternion with a default angle of 0 degrees.
constexpr Quaternion operator-(const Quaternion &other) const
Subtracts another quaternion.
Definition Quaternion.h:52
constexpr double Norm() const
Calculates the L2 norm of the quaternion.
Definition Quaternion.h:150
static constexpr Quaternion FromRotationVector(const Eigen::Vector3d &rvec)
Returns the quaternion representation of this rotation vector.
Definition Quaternion.h:295
constexpr Eigen::Vector3d ToRotationVector() const
Returns the rotation vector representation of this quaternion.
Definition Quaternion.h:266
constexpr double X() const
Returns X component of the quaternion.
Definition Quaternion.h:249
constexpr double W() const
Returns W component of the quaternion.
Definition Quaternion.h:244
constexpr Quaternion Exp() const
Matrix exponential of a quaternion.
Definition Quaternion.h:176
constexpr Quaternion Inverse() const
Returns the inverse of the quaternion.
Definition Quaternion.h:130
constexpr Quaternion Log() const
Log operator of a quaternion.
Definition Quaternion.h:215
constexpr Quaternion Exp(const Quaternion &other) const
Matrix exponential of a quaternion.
Definition Quaternion.h:164
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 > log(const T x) noexcept
Compile-time natural logarithm function.
Definition log.hpp:186
constexpr return_t< T > cos(const T x) noexcept
Compile-time cosine function.
Definition cos.hpp:83
constexpr return_t< T > sin(const T x) noexcept
Compile-time sine function.
Definition sin.hpp:85
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 > exp(const T x) noexcept
Compile-time exponential function.
Definition exp.hpp:130
constexpr common_return_t< T1, T2 > atan2(const T1 y, const T2 x) noexcept
Compile-time two-argument arctangent function.
Definition atan2.hpp:88