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