Line data Source code
1 : /**
2 : * @file
3 : * @brief Definition of Quaternion and EulerAngles.
4 : *
5 : * Quaternions can be multiplied (Hamiltonian product), normalized and can
6 : * perform rotations of vectors. Quaternion also has an implementation of the
7 : * following operators:
8 : *
9 : * - `-` (conjugate)
10 : * - `+`, `+=`, `-`, `-=` (Hamiltonian product of quaternions, adds and
11 : * subtracts angles)
12 : * - `*`, `*=`, `/`, `/=` (multiplication and division by scalars)
13 : * - `==`, `!=` (equality)
14 : * - `<<` (printing)
15 : *
16 : * EulerAngles provides the conversions between Euler angles and quaternions.
17 : * It also has an implementation of the following operators:
18 : *
19 : * - `==`, `!=` (equality)
20 : * - `<<` (printing)
21 : */
22 : #pragma once
23 :
24 : #include <AH/Settings/Warnings.hpp>
25 : AH_DIAGNOSTIC_WERROR() // Enable errors on warnings
26 :
27 : #include <AH/Arduino-Wrapper.h> // Print
28 : #include <AH/Math/Degrees.hpp> // rad2deg()
29 : #include <AH/Math/Vector.hpp> // Vec3f
30 : #include <AH/STL/cmath> // std::sqrt
31 : #include <AH/STL/limits> // std::numeric_limits
32 :
33 : #ifndef ARDUINO
34 : #include <iosfwd> // std::ostream
35 : #endif
36 :
37 : BEGIN_AH_NAMESPACE
38 :
39 : /**
40 : * @addtogroup math-types Math Types
41 : * Vector and Quaternion types with the necessary operators and
42 : * functions.
43 : * @{
44 : */
45 :
46 : /**
47 : * @brief Type for quaternions of floating point numbers.
48 : *
49 : * Quaternions can be multiplied (Hamiltonian product), normalized and can
50 : * perform rotations of vectors. Quaternion also has an implementation of the
51 : * following operators:
52 : *
53 : * - `-` (conjugate)
54 : * - `+`, `+=`, `-`, `-=` (Hamiltonian product of quaternions, adds and
55 : * subtracts angles)
56 : * - `*`, `*=`, `/`, `/=` (multiplication and division by scalars)
57 : * - `==`, `!=` (equality)
58 : * - `<<` (printing)
59 : */
60 : struct Quaternion {
61 : float w = 1.0; ///< Scalar (real) component.
62 : float x = 0.0; ///< First vector (imaginary) component @f$ \mathbf{i} @f$.
63 : float y = 0.0; ///< Second vector (imaginary) component @f$ \mathbf{j} @f$.
64 : float z = 0.0; ///< Third vector (imaginary) component @f$ \mathbf{k} @f$.
65 :
66 : /// Create a quaternion that is initialized to the identity quaternion.
67 : Quaternion() = default;
68 : /// Create a quaterion with the given values for w, x, y and z.
69 1000073 : Quaternion(float w, float x, float y, float z) : w(w), x(x), y(y), z(z) {}
70 :
71 : /// Sum of two quaterions uses quaternion multiplication.
72 : /// (Composition of the two rotations.)
73 3 : Quaternion &operator+=(Quaternion rhs) {
74 3 : return *this = hamiltonianProduct(*this, rhs);
75 : }
76 : /// Sum of two quaternions uses quaternion multiplication.
77 : /// (Composition of the two rotations.)
78 1 : Quaternion operator+(Quaternion rhs) const {
79 1 : return hamiltonianProduct(*this, rhs);
80 : }
81 :
82 : /// Complex conjugate (doesn't change the original quaternion).
83 4 : Quaternion conjugated() const { return {w, -x, -y, -z}; }
84 : /// Negated quaternion is its conjugate.
85 3 : Quaternion operator-() const { return conjugated(); }
86 :
87 : /// Difference of two quaternions `a` and `b` is the quaternion
88 : /// multiplication of `a` and the conjugate of `b`.
89 : /// (Composition of the rotation of `a` and the inverse rotation of `b`.)
90 2 : Quaternion &operator-=(Quaternion rhs) { return *this += -rhs; }
91 : /// Difference of two quaternions `a` and `b` is the quaternion
92 : /// multiplication of `a` and the conjugate of `b`.
93 : /// (Composition of the rotation of `a` and the inverse rotation of `b`.)
94 1 : Quaternion operator-(Quaternion rhs) const {
95 1 : Quaternion result = *this;
96 1 : result -= rhs;
97 1 : return result;
98 : }
99 :
100 : /// Scalar multiplication.
101 2 : Quaternion &operator*=(float rhs) {
102 2 : w *= rhs;
103 2 : x *= rhs;
104 2 : y *= rhs;
105 2 : z *= rhs;
106 2 : return *this;
107 : }
108 : /// Scalar multiplication.
109 1 : Quaternion operator*(float rhs) const {
110 1 : Quaternion result = *this;
111 1 : result *= rhs;
112 1 : return result;
113 : }
114 :
115 : /// Scalar division.
116 1000016 : Quaternion &operator/=(float rhs) {
117 1000016 : w /= rhs;
118 1000016 : x /= rhs;
119 1000016 : y /= rhs;
120 1000016 : z /= rhs;
121 1000016 : return *this;
122 : }
123 : /// Scalar division.
124 1000008 : Quaternion operator/(float rhs) const {
125 1000008 : Quaternion result = *this;
126 1000008 : result /= rhs;
127 1000008 : return result;
128 : }
129 :
130 : /// Norm squared.
131 1000016 : float normSquared() const { return w * w + x * x + y * y + z * z; }
132 : /// Norm.
133 1000015 : float norm() const { return std::sqrt(normSquared()); }
134 : /// Normalize this quaternion.
135 7 : Quaternion &normalize() { return *this /= norm(); }
136 : /// Normalize a copy of this quaternion (doesn't change the original
137 : /// quaternion).
138 1000007 : Quaternion normalized() const { return *this / norm(); }
139 :
140 : /**
141 : * @brief Rotate vector by this quaternion.
142 : *
143 : * This function uses the normalized version of this quaternion.
144 : *
145 : * @note This function is not the same as `quatrotate` in MATLAB!
146 : * MATLAB rotates by the conjugate of the quaternion, while this
147 : * function rotates by the quaternion itself.
148 : */
149 1000006 : Vec3f rotate(Vec3f v) const {
150 : // Source:
151 : // https://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation#Quaternion-derived_rotation_matrix
152 :
153 : // Compare to the MATLAB convetions:
154 : // https://www.mathworks.com/matlabcentral/answers/352465-what-is-the-aerospace-blockset-quaternion-convention
155 : // https://www.mathworks.com/help/aerotbx/ug/quatrotate.html#mw_f4c2628b-30a7-4b62-a227-7fb9ef158187
156 :
157 : // Normalized quaternion.
158 1000006 : Quaternion q = normalized();
159 :
160 : // Rotation matrix.
161 1000006 : float M11 = 1 - 2 * q.y * q.y - 2 * q.z * q.z;
162 1000006 : float M12 = 2 * (q.x * q.y - q.w * q.z);
163 1000006 : float M13 = 2 * (q.x * q.z + q.w * q.y);
164 1000006 : float M21 = 2 * (q.x * q.y + q.w * q.z);
165 1000006 : float M22 = 1 - 2 * q.x * q.x - 2 * q.z * q.z;
166 1000006 : float M23 = 2 * (q.y * q.z - q.w * q.x);
167 1000006 : float M31 = 2 * (q.x * q.z - q.w * q.y);
168 1000006 : float M32 = 2 * (q.y * q.z + q.w * q.x);
169 1000006 : float M33 = 1 - 2 * q.x * q.x - 2 * q.y * q.y;
170 :
171 : return Vec3f{
172 1000006 : M11 * v.x + M12 * v.y + M13 * v.z,
173 1000006 : M21 * v.x + M22 * v.y + M23 * v.z,
174 1000006 : M31 * v.x + M32 * v.y + M33 * v.z,
175 3000018 : };
176 : }
177 :
178 : /// Equality check.
179 25 : bool operator==(Quaternion rhs) const {
180 48 : return this->w == rhs.w && this->x == rhs.x && this->y == rhs.y &&
181 48 : this->z == rhs.z;
182 : }
183 : /// Inequality check.
184 2 : bool operator!=(Quaternion rhs) const { return !(*this == rhs); }
185 :
186 : /// Identity quaternion (1,0,0,0).
187 2 : static Quaternion identity() { return {1, 0, 0, 0}; }
188 :
189 : /**
190 : * @brief Calculate the quaternion that satisfies the following:
191 : * `result.rotate(Vec3f{0, 0, 1}) == v.normalized()`.
192 : */
193 1000005 : static Quaternion fromDirection(Vec3f v) {
194 : /*
195 : * Formula:
196 : * q = cos(ϑ / 2) + sin(ϑ / 2)·(x·i + y·j + z·k)
197 : * where (x y z) is a unit vector representing the axis about which
198 : * the body is rotated; ϑ is the angle by which it is rotated.
199 : *
200 : * Source:
201 : * https://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation#Using_quaternion_as_rotations
202 : *
203 : * The rotational axis (x y z) can be calcuated by taking the normalized
204 : * cross product of (0 0 1) and the given vector. The angle of rotation
205 : * ϑ can be found using |A×B| = |A||B|·sin(ϑ).
206 : */
207 :
208 1000005 : float eps = std::numeric_limits<float>::epsilon();
209 :
210 : // Ensure that the norm of v is not zero
211 1000005 : float v_norm = v.norm();
212 1000005 : if (v_norm <= eps)
213 1 : return Quaternion(0, 0, 0, 0); // invalid zero quaternion
214 :
215 : // Normalize the x and y components (only the sign of v.z matters now)
216 1000004 : float x = v.x / v_norm;
217 1000004 : float y = v.y / v_norm;
218 :
219 : // Check the edge case, where v ≃ (0 0 ±1).
220 1000004 : if (std::abs(x) <= eps && std::abs(y) <= eps)
221 1 : return v.z > 0 ? Quaternion(1, 0, 0, 0) : Quaternion(0, 1, 0, 0);
222 :
223 : // Calculate the cross product and its norm.
224 : // (z component is always zero)
225 1000003 : Vec2f cross = {-y, x};
226 1000003 : float crossNorm = min(cross.norm(), 1);
227 1000003 : cross /= crossNorm;
228 :
229 : // Calculate the angle ϑ.
230 1000003 : float angle = std::asin(crossNorm);
231 1000003 : if (v.z < 0)
232 499712 : angle = float(180_deg) - angle;
233 :
234 : // Calculate the resulting quaternion.
235 : return {
236 1000003 : std::cos(angle / 2), //
237 1000003 : std::sin(angle / 2) * cross.x, //
238 2000006 : std::sin(angle / 2) * cross.y, //
239 : 0, // = std::sin(angle / 2) * cross.z
240 2000006 : };
241 : }
242 :
243 : /**
244 : * @brief Calculate the quaternion from a vector that makes a given angle
245 : * with the XZ plane and a given angle with the YZ plane.
246 : *
247 : * @param xAngle
248 : * The angle the vector should make with the XZ plane. A positive
249 : * value represents a positive rotation about the x-axis.
250 : * @param yAngle
251 : * The angle the vector should make with the YZ plane. A positive
252 : * value represents a positive rotation about the y-axis.
253 : *
254 : * @return A quaternion from the vector {tan(yAngle), -tan(xAngle), 1}.
255 : */
256 1 : static Quaternion fromXYAngle(float xAngle, float yAngle) {
257 1 : Vec3f v = {
258 : +std::tan(yAngle), // x
259 1 : -std::tan(xAngle), // y
260 : 1, // z
261 2 : };
262 2 : return Quaternion::fromDirection(v);
263 : }
264 :
265 : /// Quaternion multiplication.
266 5 : static Quaternion hamiltonianProduct(Quaternion q, Quaternion r) {
267 : return {
268 5 : r.w * q.w - r.x * q.x - r.y * q.y - r.z * q.z,
269 5 : r.w * q.x + r.x * q.w - r.y * q.z + r.z * q.y,
270 5 : r.w * q.y + r.x * q.z + r.y * q.w - r.z * q.x,
271 5 : r.w * q.z - r.x * q.y + r.y * q.x + r.z * q.w,
272 5 : };
273 : }
274 : };
275 :
276 : /// Scalar multiplication.
277 : /// @related Quaternion
278 1 : inline Quaternion operator*(float lhs, Quaternion rhs) {
279 1 : return {lhs * rhs.w, lhs * rhs.x, lhs * rhs.y, lhs * rhs.z};
280 : }
281 :
282 : /**
283 : * @brief Struct for Euler angles of floating point numbers.
284 : *
285 : * EulerAngles provides the conversions between Euler angles and quaternions.
286 : * It also has an implementation of the following operators:
287 : *
288 : * - `==`, `!=` (equality)
289 : * - `<<` (printing)
290 : */
291 : struct EulerAngles {
292 : float yaw = 0.0; ///< Z : drone Z = world +Z.
293 : float pitch = 0.0; ///< Y': drone Y = world -X.
294 : float roll = 0.0; ///< X": drone X = world +Y.
295 :
296 : /// Create Euler angles that are initialized to (0 0 0), or upright.
297 : EulerAngles() = default;
298 : /// Create Euler angles with the given values for yaw, pitch and roll.
299 11 : EulerAngles(float yaw, float pitch, float roll)
300 11 : : yaw(yaw), pitch(pitch), roll(roll) {}
301 : /// Create Euler angles from the given quaternion.
302 3 : EulerAngles(Quaternion q) : EulerAngles{quat2eul(q)} {}
303 :
304 : /// Implicitly convert these Euler angles to a quaternion.
305 3 : operator Quaternion() const { return eul2quat(*this); }
306 :
307 : /// Equality check.
308 1 : bool operator==(EulerAngles rhs) const {
309 2 : return this->yaw == rhs.yaw && this->pitch == rhs.pitch &&
310 2 : this->roll == rhs.roll;
311 : }
312 : /// Inequality check.
313 : bool operator!=(EulerAngles rhs) const { return !(*this == rhs); }
314 :
315 : /// Convert the given quaternion to Euler angles.
316 5 : static EulerAngles quat2eul(Quaternion q) {
317 : // Source: https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Quaternion_to_Euler_Angles_Conversion
318 :
319 : // Normalize quaternion (q is passed by value, so this is a copy of q).
320 5 : q.normalize();
321 :
322 5 : float phi = std::atan2(2 * (q.w * q.x + q.y * q.z),
323 5 : 1 - 2 * (q.x * q.x + q.y * q.y));
324 5 : float theta = std::asin(2 * (q.w * q.y - q.z * q.x));
325 5 : float psi = std::atan2(2 * (q.w * q.z + q.x * q.y),
326 5 : 1 - 2 * (q.y * q.y + q.z * q.z));
327 5 : return {psi, theta, phi};
328 : }
329 :
330 : /// Convert the given Euler angles to a quaternion.
331 5 : static Quaternion eul2quat(EulerAngles eulerAngles) {
332 : // Source: https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Euler_Angles_to_Quaternion_Conversion
333 :
334 5 : float cy = std::cos(eulerAngles.yaw / 2);
335 5 : float sy = std::sin(eulerAngles.yaw / 2);
336 5 : float cp = std::cos(eulerAngles.pitch / 2);
337 5 : float sp = std::sin(eulerAngles.pitch / 2);
338 5 : float cr = std::cos(eulerAngles.roll / 2);
339 5 : float sr = std::sin(eulerAngles.roll / 2);
340 :
341 : return {
342 5 : cy * cp * cr + sy * sp * sr,
343 5 : cy * cp * sr - sy * sp * cr,
344 5 : sy * cp * sr + cy * sp * cr,
345 5 : sy * cp * cr - cy * sp * sr,
346 5 : };
347 : }
348 : };
349 :
350 : #ifndef ARDUINO
351 :
352 : /// Printing.
353 : /// @related Quaternion
354 : std::ostream &operator<<(std::ostream &os, Quaternion q);
355 :
356 : /// Printing.
357 : /// @related EulerAngles
358 : std::ostream &operator<<(std::ostream &os, EulerAngles e);
359 :
360 : #endif // ARDUINO
361 :
362 : /// Printing.
363 : /// @related Quaternion
364 : Print &operator<<(Print &os, Quaternion e);
365 :
366 : /// Printing.
367 : /// @related EulerAngles
368 : Print &operator<<(Print &os, EulerAngles e);
369 :
370 : /// @}
371 :
372 : END_AH_NAMESPACE
373 :
374 : AH_DIAGNOSTIC_POP()
|
