Control Surface main
MIDI Control Surface library for Arduino
Quaternion.hpp
Go to the documentation of this file.
1
22#pragma once
23
25AH_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
38
60struct Quaternion {
61 float w = 1.0;
62 float x = 0.0;
63 float y = 0.0;
64 float z = 0.0;
65
67 Quaternion() = default;
69 Quaternion(float w, float x, float y, float z) : w(w), x(x), y(y), z(z) {}
70
74 return *this = hamiltonianProduct(*this, rhs);
75 }
79 return hamiltonianProduct(*this, rhs);
80 }
81
83 Quaternion conjugated() const { return {w, -x, -y, -z}; }
85 Quaternion operator-() const { return conjugated(); }
86
90 Quaternion &operator-=(Quaternion rhs) { return *this += -rhs; }
95 Quaternion result = *this;
96 result -= rhs;
97 return result;
98 }
99
101 Quaternion &operator*=(float rhs) {
102 w *= rhs;
103 x *= rhs;
104 y *= rhs;
105 z *= rhs;
106 return *this;
107 }
109 Quaternion operator*(float rhs) const {
110 Quaternion result = *this;
111 result *= rhs;
112 return result;
113 }
114
116 Quaternion &operator/=(float rhs) {
117 w /= rhs;
118 x /= rhs;
119 y /= rhs;
120 z /= rhs;
121 return *this;
122 }
124 Quaternion operator/(float rhs) const {
125 Quaternion result = *this;
126 result /= rhs;
127 return result;
128 }
129
131 float normSquared() const { return w * w + x * x + y * y + z * z; }
133 float norm() const { return std::sqrt(normSquared()); }
135 Quaternion &normalize() { return *this /= norm(); }
138 Quaternion normalized() const { return *this / norm(); }
139
149 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 Quaternion q = normalized();
159
160 // Rotation matrix.
161 float M11 = 1 - 2 * q.y * q.y - 2 * q.z * q.z;
162 float M12 = 2 * (q.x * q.y - q.w * q.z);
163 float M13 = 2 * (q.x * q.z + q.w * q.y);
164 float M21 = 2 * (q.x * q.y + q.w * q.z);
165 float M22 = 1 - 2 * q.x * q.x - 2 * q.z * q.z;
166 float M23 = 2 * (q.y * q.z - q.w * q.x);
167 float M31 = 2 * (q.x * q.z - q.w * q.y);
168 float M32 = 2 * (q.y * q.z + q.w * q.x);
169 float M33 = 1 - 2 * q.x * q.x - 2 * q.y * q.y;
170
171 return Vec3f{
172 M11 * v.x + M12 * v.y + M13 * v.z,
173 M21 * v.x + M22 * v.y + M23 * v.z,
174 M31 * v.x + M32 * v.y + M33 * v.z,
175 };
176 }
177
179 bool operator==(Quaternion rhs) const {
180 return this->w == rhs.w && this->x == rhs.x && this->y == rhs.y &&
181 this->z == rhs.z;
182 }
184 bool operator!=(Quaternion rhs) const { return !(*this == rhs); }
185
187 static Quaternion identity() { return {1, 0, 0, 0}; }
188
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 float eps = std::numeric_limits<float>::epsilon();
209
210 // Ensure that the norm of v is not zero
211 float v_norm = v.norm();
212 if (v_norm <= eps)
213 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 float x = v.x / v_norm;
217 float y = v.y / v_norm;
218
219 // Check the edge case, where v ≃ (0 0 ±1).
220 if (std::abs(x) <= eps && std::abs(y) <= eps)
221 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 Vec2f cross = {-y, x};
226 float crossNorm = min(cross.norm(), 1);
227 cross /= crossNorm;
228
229 // Calculate the angle ϑ.
230 float angle = std::asin(crossNorm);
231 if (v.z < 0)
232 angle = float(180_deg) - angle;
233
234 // Calculate the resulting quaternion.
235 return {
236 std::cos(angle / 2), //
237 std::sin(angle / 2) * cross.x, //
238 std::sin(angle / 2) * cross.y, //
239 0, // = std::sin(angle / 2) * cross.z
240 };
241 }
242
256 static Quaternion fromXYAngle(float xAngle, float yAngle) {
257 Vec3f v = {
258 +std::tan(yAngle), // x
259 -std::tan(xAngle), // y
260 1, // z
261 };
262 return Quaternion::fromDirection(v);
263 }
264
267 return {
268 r.w * q.w - r.x * q.x - r.y * q.y - r.z * q.z,
269 r.w * q.x + r.x * q.w - r.y * q.z + r.z * q.y,
270 r.w * q.y + r.x * q.z + r.y * q.w - r.z * q.x,
271 r.w * q.z - r.x * q.y + r.y * q.x + r.z * q.w,
272 };
273 }
274};
275
278inline Quaternion operator*(float lhs, Quaternion rhs) {
279 return {lhs * rhs.w, lhs * rhs.x, lhs * rhs.y, lhs * rhs.z};
280}
281
292 float yaw = 0.0;
293 float pitch = 0.0;
294 float roll = 0.0;
295
297 EulerAngles() = default;
299 EulerAngles(float yaw, float pitch, float roll)
300 : yaw(yaw), pitch(pitch), roll(roll) {}
302 EulerAngles(Quaternion q) : EulerAngles{quat2eul(q)} {}
303
305 operator Quaternion() const { return eul2quat(*this); }
306
308 bool operator==(EulerAngles rhs) const {
309 return this->yaw == rhs.yaw && this->pitch == rhs.pitch &&
310 this->roll == rhs.roll;
311 }
313 bool operator!=(EulerAngles rhs) const { return !(*this == rhs); }
314
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 q.normalize();
321
322 float phi = std::atan2(2 * (q.w * q.x + q.y * q.z),
323 1 - 2 * (q.x * q.x + q.y * q.y));
324 float theta = std::asin(2 * (q.w * q.y - q.z * q.x));
325 float psi = std::atan2(2 * (q.w * q.z + q.x * q.y),
326 1 - 2 * (q.y * q.y + q.z * q.z));
327 return {psi, theta, phi};
328 }
329
331 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 float cy = std::cos(eulerAngles.yaw / 2);
335 float sy = std::sin(eulerAngles.yaw / 2);
336 float cp = std::cos(eulerAngles.pitch / 2);
337 float sp = std::sin(eulerAngles.pitch / 2);
338 float cr = std::cos(eulerAngles.roll / 2);
339 float sr = std::sin(eulerAngles.roll / 2);
340
341 return {
342 cy * cp * cr + sy * sp * sr,
343 cy * cp * sr - sy * sp * cr,
344 sy * cp * sr + cy * sp * cr,
345 sy * cp * cr - cy * sp * sr,
346 };
347 }
348};
349
350#ifndef ARDUINO
351
354std::ostream &operator<<(std::ostream &os, Quaternion q);
355
358std::ostream &operator<<(std::ostream &os, EulerAngles e);
359
360#endif // ARDUINO
361
364Print &operator<<(Print &os, Quaternion e);
365
368Print &operator<<(Print &os, EulerAngles e);
369
371
373
#define END_AH_NAMESPACE
#define BEGIN_AH_NAMESPACE
Conversions between radians and degrees.
Definition of Vec2f and Vec3f.
#define AH_DIAGNOSTIC_POP()
Definition: Warnings.hpp:37
#define AH_DIAGNOSTIC_WERROR()
Definition: Warnings.hpp:36
constexpr auto min(const T &a, const U &b) -> decltype(b< a ? b :a)
Return the smaller of two numbers/objects.
Definition: MinMaxFix.hpp:15
Print & operator<<(Print &os, Quaternion e)
Printing.
Definition: Quaternion.cpp:28
Quaternion operator*(float lhs, Quaternion rhs)
Scalar multiplication.
Definition: Quaternion.hpp:278
Struct for Euler angles of floating point numbers.
Definition: Quaternion.hpp:291
bool operator==(EulerAngles rhs) const
Equality check.
Definition: Quaternion.hpp:308
EulerAngles(Quaternion q)
Create Euler angles from the given quaternion.
Definition: Quaternion.hpp:302
float roll
X": drone X = world +Y.
Definition: Quaternion.hpp:294
float pitch
Y': drone Y = world -X.
Definition: Quaternion.hpp:293
static EulerAngles quat2eul(Quaternion q)
Convert the given quaternion to Euler angles.
Definition: Quaternion.hpp:316
bool operator!=(EulerAngles rhs) const
Inequality check.
Definition: Quaternion.hpp:313
float yaw
Z : drone Z = world +Z.
Definition: Quaternion.hpp:292
EulerAngles(float yaw, float pitch, float roll)
Create Euler angles with the given values for yaw, pitch and roll.
Definition: Quaternion.hpp:299
EulerAngles()=default
Create Euler angles that are initialized to (0 0 0), or upright.
static Quaternion eul2quat(EulerAngles eulerAngles)
Convert the given Euler angles to a quaternion.
Definition: Quaternion.hpp:331
Type for quaternions of floating point numbers.
Definition: Quaternion.hpp:60
Quaternion & operator+=(Quaternion rhs)
Sum of two quaterions uses quaternion multiplication.
Definition: Quaternion.hpp:73
static Quaternion fromDirection(Vec3f v)
Calculate the quaternion that satisfies the following: result.rotate(Vec3f{0, 0, 1}) == v....
Definition: Quaternion.hpp:193
Quaternion & operator/=(float rhs)
Scalar division.
Definition: Quaternion.hpp:116
Quaternion operator-() const
Negated quaternion is its conjugate.
Definition: Quaternion.hpp:85
float w
Scalar (real) component.
Definition: Quaternion.hpp:61
static Quaternion hamiltonianProduct(Quaternion q, Quaternion r)
Quaternion multiplication.
Definition: Quaternion.hpp:266
float normSquared() const
Norm squared.
Definition: Quaternion.hpp:131
bool operator==(Quaternion rhs) const
Equality check.
Definition: Quaternion.hpp:179
Quaternion(float w, float x, float y, float z)
Create a quaterion with the given values for w, x, y and z.
Definition: Quaternion.hpp:69
bool operator!=(Quaternion rhs) const
Inequality check.
Definition: Quaternion.hpp:184
Quaternion operator/(float rhs) const
Scalar division.
Definition: Quaternion.hpp:124
float y
Second vector (imaginary) component .
Definition: Quaternion.hpp:63
Quaternion()=default
Create a quaternion that is initialized to the identity quaternion.
Quaternion operator+(Quaternion rhs) const
Sum of two quaternions uses quaternion multiplication.
Definition: Quaternion.hpp:78
Quaternion & operator-=(Quaternion rhs)
Difference of two quaternions a and b is the quaternion multiplication of a and the conjugate of b.
Definition: Quaternion.hpp:90
Quaternion operator*(float rhs) const
Scalar multiplication.
Definition: Quaternion.hpp:109
static Quaternion fromXYAngle(float xAngle, float yAngle)
Calculate the quaternion from a vector that makes a given angle with the XZ plane and a given angle w...
Definition: Quaternion.hpp:256
Vec3f rotate(Vec3f v) const
Rotate vector by this quaternion.
Definition: Quaternion.hpp:149
Quaternion normalized() const
Normalize a copy of this quaternion (doesn't change the original quaternion).
Definition: Quaternion.hpp:138
Quaternion & normalize()
Normalize this quaternion.
Definition: Quaternion.hpp:135
float x
First vector (imaginary) component .
Definition: Quaternion.hpp:62
static Quaternion identity()
Identity quaternion (1,0,0,0).
Definition: Quaternion.hpp:187
float norm() const
Norm.
Definition: Quaternion.hpp:133
Quaternion operator-(Quaternion rhs) const
Difference of two quaternions a and b is the quaternion multiplication of a and the conjugate of b.
Definition: Quaternion.hpp:94
Quaternion & operator*=(float rhs)
Scalar multiplication.
Definition: Quaternion.hpp:101
float z
Third vector (imaginary) component .
Definition: Quaternion.hpp:64
Quaternion conjugated() const
Complex conjugate (doesn't change the original quaternion).
Definition: Quaternion.hpp:83
Type for 2D vectors of floating point numbers.
Definition: Vector.hpp:42
float y
The y component of the vector.
Definition: Vector.hpp:44
float x
The x component of the vector.
Definition: Vector.hpp:43
float norm() const
Norm.
Definition: Vector.hpp:109
Type for 3D vectors of floating point numbers.
Definition: Vector.hpp:143
float y
The y component of the vector.
Definition: Vector.hpp:145
float x
The x component of the vector.
Definition: Vector.hpp:144
float norm() const
Norm.
Definition: Vector.hpp:222
float z
The z component of the vector.
Definition: Vector.hpp:146