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