PX4 Firmware
PX4 Autopilot Software http://px4.io
Quaternion.hpp
Go to the documentation of this file.
1 /**
2  * @file Quaternion.hpp
3  *
4  * All rotations and axis systems follow the right-hand rule.
5  * The Hamilton quaternion convention including its product definition is used.
6  *
7  * In order to rotate a vector in frame b (v_b) to frame n by a righthand
8  * rotation defined by the quaternion q_nb (from frame b to n)
9  * one can use the following operation:
10  * v_n = q_nb * [0;v_b] * q_nb^(-1)
11  *
12  * Just like DCM's: v_n = C_nb * v_b (vector rotation)
13  * M_n = C_nb * M_b * C_nb^(-1) (matrix rotation)
14  *
15  * or similarly the reverse operation
16  * v_b = q_nb^(-1) * [0;v_n] * q_nb
17  *
18  * where q_nb^(-1) represents the inverse of the quaternion q_nb^(-1) = q_bn
19  *
20  * The product z of two quaternions z = q2 * q1 represents an intrinsic rotation
21  * in the order of first q1 followed by q2.
22  * The first element of the quaternion
23  * represents the real part, thus, a quaternion representing a zero-rotation
24  * is defined as (1,0,0,0).
25  *
26  * @author James Goppert <james.goppert@gmail.com>
27  */
28 
29 #pragma once
30 
31 #include "math.hpp"
32 
33 namespace matrix
34 {
35 
36 template <typename Type>
37 class Dcm;
38 
39 template <typename Type>
40 class Euler;
41 
42 template <typename Type>
43 class AxisAngle;
44 
45 
46 /**
47  * Quaternion class
48  *
49  * The rotation between two coordinate frames is
50  * described by this class.
51  */
52 template<typename Type>
53 class Quaternion : public Vector<Type, 4>
54 {
55 public:
58 
59  /**
60  * Constructor from array
61  *
62  * @param data_ array
63  */
64  explicit Quaternion(const Type data_[4]) :
65  Vector<Type, 4>(data_)
66  {
67  }
68 
69  /**
70  * Standard constructor
71  */
73  {
74  Quaternion &q = *this;
75  q(0) = 1;
76  q(1) = 0;
77  q(2) = 0;
78  q(3) = 0;
79  }
80 
81  /**
82  * Constructor from Matrix41
83  *
84  * @param other Matrix41 to copy
85  */
86  Quaternion(const Matrix41 &other) :
87  Vector<Type, 4>(other)
88  {
89  }
90 
91  /**
92  * Constructor from dcm
93  *
94  * Instance is initialized from a dcm representing coordinate transformation
95  * from frame 2 to frame 1.
96  *
97  * @param dcm dcm to set quaternion to
98  */
100  {
101  Quaternion &q = *this;
102  Type t = R.trace();
103  if (t > Type(0)) {
104  t = sqrt(Type(1) + t);
105  q(0) = Type(0.5) * t;
106  t = Type(0.5) / t;
107  q(1) = (R(2,1) - R(1,2)) * t;
108  q(2) = (R(0,2) - R(2,0)) * t;
109  q(3) = (R(1,0) - R(0,1)) * t;
110  } else if (R(0,0) > R(1,1) && R(0,0) > R(2,2)) {
111  t = sqrt(Type(1) + R(0,0) - R(1,1) - R(2,2));
112  q(1) = Type(0.5) * t;
113  t = Type(0.5) / t;
114  q(0) = (R(2,1) - R(1,2)) * t;
115  q(2) = (R(1,0) + R(0,1)) * t;
116  q(3) = (R(0,2) + R(2,0)) * t;
117  } else if (R(1,1) > R(2,2)) {
118  t = sqrt(Type(1) - R(0,0) + R(1,1) - R(2,2));
119  q(2) = Type(0.5) * t;
120  t = Type(0.5) / t;
121  q(0) = (R(0,2) - R(2,0)) * t;
122  q(1) = (R(1,0) + R(0,1)) * t;
123  q(3) = (R(2,1) + R(1,2)) * t;
124  } else {
125  t = sqrt(Type(1) - R(0,0) - R(1,1) + R(2,2));
126  q(3) = Type(0.5) * t;
127  t = Type(0.5) / t;
128  q(0) = (R(1,0) - R(0,1)) * t;
129  q(1) = (R(0,2) + R(2,0)) * t;
130  q(2) = (R(2,1) + R(1,2)) * t;
131  }
132  }
133 
134  /**
135  * Constructor from euler angles
136  *
137  * This sets the instance to a quaternion representing coordinate transformation from
138  * frame 2 to frame 1 where the rotation from frame 1 to frame 2 is described
139  * by a 3-2-1 intrinsic Tait-Bryan rotation sequence.
140  *
141  * @param euler euler angle instance
142  */
143  Quaternion(const Euler<Type> &euler)
144  {
145  Quaternion &q = *this;
146  Type cosPhi_2 = Type(cos(euler.phi() / Type(2)));
147  Type cosTheta_2 = Type(cos(euler.theta() / Type(2)));
148  Type cosPsi_2 = Type(cos(euler.psi() / Type(2)));
149  Type sinPhi_2 = Type(sin(euler.phi() / Type(2)));
150  Type sinTheta_2 = Type(sin(euler.theta() / Type(2)));
151  Type sinPsi_2 = Type(sin(euler.psi() / Type(2)));
152  q(0) = cosPhi_2 * cosTheta_2 * cosPsi_2 +
153  sinPhi_2 * sinTheta_2 * sinPsi_2;
154  q(1) = sinPhi_2 * cosTheta_2 * cosPsi_2 -
155  cosPhi_2 * sinTheta_2 * sinPsi_2;
156  q(2) = cosPhi_2 * sinTheta_2 * cosPsi_2 +
157  sinPhi_2 * cosTheta_2 * sinPsi_2;
158  q(3) = cosPhi_2 * cosTheta_2 * sinPsi_2 -
159  sinPhi_2 * sinTheta_2 * cosPsi_2;
160  }
161 
162  /**
163  * Quaternion from AxisAngle
164  *
165  * @param aa axis-angle vector
166  */
168  {
169  Quaternion &q = *this;
170  Type angle = aa.norm();
171  Vector<Type, 3> axis = aa.unit();
172  if (angle < Type(1e-10)) {
173  q(0) = Type(1);
174  q(1) = q(2) = q(3) = 0;
175  } else {
176  Type magnitude = sin(angle / Type(2));
177  q(0) = cos(angle / 2.0f);
178  q(1) = axis(0) * magnitude;
179  q(2) = axis(1) * magnitude;
180  q(3) = axis(2) * magnitude;
181  }
182  }
183 
184  /**
185  * Quaternion from two vectors
186  * Generates shortest rotation from source to destination vector
187  *
188  * @param dst destination vector (no need to normalize)
189  * @param src source vector (no need to normalize)
190  * @param eps epsilon threshold which decides if a value is considered zero
191  */
192  Quaternion(const Vector3<Type> &src, const Vector3<Type> &dst, const Type eps = Type(1e-5))
193  {
194  Quaternion &q = *this;
195  Vector3<Type> cr = src.cross(dst);
196  const float dt = src.dot(dst);
197  if (cr.norm() < eps && dt < 0) {
198  // handle corner cases with 180 degree rotations
199  // if the two vectors are parallel, cross product is zero
200  // if they point opposite, the dot product is negative
201  cr = src.abs();
202  if (cr(0) < cr(1)) {
203  if (cr(0) < cr(2)) {
204  cr = Vector3<Type>(1, 0, 0);
205  } else {
206  cr = Vector3<Type>(0, 0, 1);
207  }
208  } else {
209  if (cr(1) < cr(2)) {
210  cr = Vector3<Type>(0, 1, 0);
211  } else {
212  cr = Vector3<Type>(0, 0, 1);
213  }
214  }
215  q(0) = Type(0);
216  cr = src.cross(cr);
217  } else {
218  // normal case, do half-way quaternion solution
219  q(0) = dt + sqrt(src.norm_squared() * dst.norm_squared());
220  }
221  q(1) = cr(0);
222  q(2) = cr(1);
223  q(3) = cr(2);
224  q.normalize();
225  }
226 
227 
228  /**
229  * Constructor from quaternion values
230  *
231  * Instance is initialized from quaternion values representing coordinate
232  * transformation from frame 2 to frame 1.
233  * A zero-rotation quaternion is represented by (1,0,0,0).
234  *
235  * @param a set quaternion value 0
236  * @param b set quaternion value 1
237  * @param c set quaternion value 2
238  * @param d set quaternion value 3
239  */
240  Quaternion(Type a, Type b, Type c, Type d)
241  {
242  Quaternion &q = *this;
243  q(0) = a;
244  q(1) = b;
245  q(2) = c;
246  q(3) = d;
247  }
248 
249  /**
250  * Quaternion multiplication operator
251  *
252  * @param q quaternion to multiply with
253  * @return product
254  */
256  {
257  const Quaternion &p = *this;
258  Quaternion r;
259  r(0) = p(0) * q(0) - p(1) * q(1) - p(2) * q(2) - p(3) * q(3);
260  r(1) = p(0) * q(1) + p(1) * q(0) + p(2) * q(3) - p(3) * q(2);
261  r(2) = p(0) * q(2) - p(1) * q(3) + p(2) * q(0) + p(3) * q(1);
262  r(3) = p(0) * q(3) + p(1) * q(2) - p(2) * q(1) + p(3) * q(0);
263  return r;
264  }
265 
266  /**
267  * Self-multiplication operator
268  *
269  * @param other quaternion to multiply with
270  */
271  void operator*=(const Quaternion &other)
272  {
273  Quaternion &self = *this;
274  self = self * other;
275  }
276 
277  /**
278  * Scalar multiplication operator
279  *
280  * @param scalar scalar to multiply with
281  * @return product
282  */
283  Quaternion operator*(Type scalar) const
284  {
285  const Quaternion &q = *this;
286  return scalar * q;
287  }
288 
289  /**
290  * Scalar self-multiplication operator
291  *
292  * @param scalar scalar to multiply with
293  */
294  void operator*=(Type scalar)
295  {
296  Quaternion &q = *this;
297  q = q * scalar;
298  }
299 
300  /**
301  * Computes the derivative of q_21 when
302  * rotated with angular velocity expressed in frame 1
303  * v_2 = q_21 * v_1 * q_21^-1
304  * d/dt q_21 = 0.5 * q_21 * omega_2
305  *
306  * @param w angular rate in frame 1 (typically body frame)
307  */
308  Matrix41 derivative1(const Matrix31 &w) const
309  {
310  const Quaternion &q = *this;
311  Quaternion<Type> v(0, w(0, 0), w(1, 0), w(2, 0));
312  return q * v * Type(0.5);
313  }
314 
315  /**
316  * Computes the derivative of q_21 when
317  * rotated with angular velocity expressed in frame 2
318  * v_2 = q_21 * v_1 * q_21^-1
319  * d/dt q_21 = 0.5 * omega_1 * q_21
320  *
321  * @param w angular rate in frame 2 (typically reference frame)
322  */
323  Matrix41 derivative2(const Matrix31 &w) const
324  {
325  const Quaternion &q = *this;
326  Quaternion<Type> v(0, w(0, 0), w(1, 0), w(2, 0));
327  return v * q * Type(0.5);
328  }
329 
330  /**
331  * Invert quaternion in place
332  */
333  void invert()
334  {
335  *this = this->inversed();
336  }
337 
338  /**
339  * Invert quaternion
340  *
341  * @return inverted quaternion
342  */
344  {
345  const Quaternion &q = *this;
346  Type normSq = q.dot(q);
347  return Quaternion(
348  q(0)/normSq,
349  -q(1)/normSq,
350  -q(2)/normSq,
351  -q(3)/normSq);
352  }
353 
354  /**
355  * Bring quaternion to canonical form
356  */
358  {
359  *this = this->canonical();
360  }
361 
362 
363  /**
364  * Return canonical form of the quaternion
365  *
366  * @return quaternion in canonical from
367  */
369  {
370  const Quaternion &q = *this;
371  if (q(0) < Type(0)) {
372  return Quaternion(-q(0),-q(1),-q(2),-q(3));
373  } else {
374  return Quaternion(q(0),q(1),q(2),q(3));
375  }
376  }
377 
378  /**
379  * Rotate quaternion from rotation vector
380  *
381  * @param vec rotation vector
382  */
383  void rotate(const AxisAngle<Type> &vec)
384  {
385  Quaternion res(vec);
386  (*this) = res * (*this);
387  }
388 
389  /**
390  * Rotates vector v_1 in frame 1 to vector v_2 in frame 2
391  * using the rotation quaternion q_21
392  * describing the rotation from frame 1 to 2
393  * v_2 = q_21 * v_1 * q_21^-1
394  *
395  * @param vec vector to rotate in frame 1 (typically body frame)
396  * @return rotated vector in frame 2 (typically reference frame)
397  */
399  const Quaternion& q = *this;
400  Quaternion v(Type(0), vec(0), vec(1), vec(2));
401  Quaternion res = q*v*q.inversed();
402  return Vector3<Type>(res(1), res(2), res(3));
403  }
404 
405  /**
406  * Rotates vector v_2 in frame 2 to vector v_1 in frame 1
407  * using the rotation quaternion q_21
408  * describing the rotation from frame 1 to 2
409  * v_1 = q_21^-1 * v_1 * q_21
410  *
411  * @param vec vector to rotate in frame 2 (typically reference frame)
412  * @return rotated vector in frame 1 (typically body frame)
413  */
415  {
416  const Quaternion& q = *this;
417  Quaternion v(Type(0), vec(0), vec(1), vec(2));
418  Quaternion res = q.inversed()*v*q;
419  return Vector3<Type>(res(1), res(2), res(3));
420  }
421 
422  /**
423  * Rotation quaternion from vector
424  *
425  * The axis of rotation is given by vector direction and
426  * the angle is given by the norm.
427  *
428  * @param vec rotation vector
429  * @return quaternion representing the rotation
430  */
432  {
433  Quaternion &q = *this;
434  Type theta = vec.norm();
435 
436  if (theta < Type(1e-10)) {
437  q(0) = Type(1);
438  q(1) = q(2) = q(3) = 0;
439  return;
440  }
441 
442  vec /= theta;
443  from_axis_angle(vec, theta);
444  }
445 
446  /**
447  * Rotation quaternion from axis and angle
448  * XXX DEPRECATED, use AxisAngle class
449  *
450  * @param axis axis of rotation
451  * @param theta scalar describing angle of rotation
452  * @return quaternion representing the rotation
453  */
454  void from_axis_angle(const Vector<Type, 3> &axis, Type theta)
455  {
456  Quaternion &q = *this;
457 
458  if (theta < Type(1e-10)) {
459  q(0) = Type(1);
460  q(1) = q(2) = q(3) = Type(0);
461  }
462 
463  Type magnitude = sin(theta / 2.0f);
464 
465  q(0) = cos(theta / 2.0f);
466  q(1) = axis(0) * magnitude;
467  q(2) = axis(1) * magnitude;
468  q(3) = axis(2) * magnitude;
469  }
470 
471 
472  /**
473  * Rotation vector from quaternion
474  * XXX DEPRECATED, use AxisAngle class
475  *
476  * The axis of rotation is given by vector direction and
477  * the angle is given by the norm.
478  *
479  * @return vector, direction representing rotation axis and norm representing angle
480  */
482  {
483  const Quaternion &q = *this;
484  Type axis_magnitude = Type(sqrt(q(1) * q(1) + q(2) * q(2) + q(3) * q(3)));
485  Vector<Type, 3> vec;
486  vec(0) = q(1);
487  vec(1) = q(2);
488  vec(2) = q(3);
489 
490  if (axis_magnitude >= Type(1e-10)) {
491  vec = vec / axis_magnitude;
492  vec = vec * wrap_pi(Type(2) * atan2(axis_magnitude, q(0)));
493  }
494 
495  return vec;
496  }
497 
498  /**
499  * Imaginary components of quaternion
500  */
502  {
503  const Quaternion &q = *this;
504  return Vector3<Type>(q(1), q(2), q(3));
505  }
506 
507  /**
508  * Corresponding body z-axis to an attitude quaternion /
509  * last orthogonal unit basis vector
510  *
511  * == last column of the equivalent rotation matrix
512  * but calculated more efficiently than a full conversion
513  */
515  {
516  const Quaternion &q = *this;
517  Vector3<Type> R_z;
518  const Type a = q(0);
519  const Type b = q(1);
520  const Type c = q(2);
521  const Type d = q(3);
522  R_z(0) = 2 * (a * c + b * d);
523  R_z(1) = 2 * (c * d - a * b);
524  R_z(2) = a * a - b * b - c * c + d * d;
525  return R_z;
526  }
527 
528  /**
529  * XXX DEPRECATED, can use assignment or ctor
530  */
532  return Quaternion(Dcmf(dcm));
533  }
534 
535  /**
536  * XXX DEPRECATED, can use assignment or ctor
537  */
539  return Dcm<Type>(*this);
540  }
541 
542 };
543 
546 
547 } // namespace matrix
548 
549 /* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */
Quaternion(const Vector3< Type > &src, const Vector3< Type > &dst, const Type eps=Type(1e-5))
Quaternion from two vectors Generates shortest rotation from source to destination vector...
Definition: Quaternion.hpp:192
void canonicalize()
Bring quaternion to canonical form.
Definition: Quaternion.hpp:357
Vector3 cross(const Matrix31 &b) const
Definition: Vector3.hpp:59
Quaternion operator*(Type scalar) const
Scalar multiplication operator.
Definition: Quaternion.hpp:283
Type theta() const
Definition: Euler.hpp:132
Dcm< float > Dcmf
Definition: Dcm.hpp:185
Dcm< Type > to_dcm()
XXX DEPRECATED, can use assignment or ctor.
Definition: Quaternion.hpp:538
Vector3< Type > dcm_z() const
Corresponding body z-axis to an attitude quaternion / last orthogonal unit basis vector.
Definition: Quaternion.hpp:514
Quaternion(const AxisAngle< Type > &aa)
Quaternion from AxisAngle.
Definition: Quaternion.hpp:167
Quaternion from_dcm(const Matrix< Type, 3, 3 > &dcm)
XXX DEPRECATED, can use assignment or ctor.
Definition: Quaternion.hpp:531
void invert()
Invert quaternion in place.
Definition: Quaternion.hpp:333
Direction cosine matrix class.
Definition: AxisAngle.hpp:15
void from_axis_angle(const Vector< Type, 3 > &axis, Type theta)
Rotation quaternion from axis and angle XXX DEPRECATED, use AxisAngle class.
Definition: Quaternion.hpp:454
Quaternion(const Dcm< Type > &R)
Constructor from dcm.
Definition: Quaternion.hpp:99
Quaternion canonical() const
Return canonical form of the quaternion.
Definition: Quaternion.hpp:368
Vector3< Type > conjugate(const Vector3< Type > &vec) const
Rotates vector v_1 in frame 1 to vector v_2 in frame 2 using the rotation quaternion q_21 describing ...
Definition: Quaternion.hpp:398
Type norm_squared() const
Definition: Vector.hpp:78
Type phi() const
Definition: Euler.hpp:128
Matrix< Type, 3, 1 > Matrix31
Definition: Quaternion.hpp:57
Quaternion class.
Definition: Dcm.hpp:24
Type psi() const
Definition: Euler.hpp:136
Quaternion< float > Quaternionf
Definition: Quaternion.hpp:545
void operator*=(Type scalar)
Scalar self-multiplication operator.
Definition: Quaternion.hpp:294
Vector< Type, 3 > to_axis_angle() const
Rotation vector from quaternion XXX DEPRECATED, use AxisAngle class.
Definition: Quaternion.hpp:481
void rotate(const AxisAngle< Type > &vec)
Rotate quaternion from rotation vector.
Definition: Quaternion.hpp:383
Matrix41 derivative2(const Matrix31 &w) const
Computes the derivative of q_21 when rotated with angular velocity expressed in frame 2 v_2 = q_21 * ...
Definition: Quaternion.hpp:323
Dual< Scalar, N > cos(const Dual< Scalar, N > &a)
Definition: Dual.hpp:286
void from_axis_angle(Vector< Type, 3 > vec)
Rotation quaternion from vector.
Definition: Quaternion.hpp:431
Matrix41 derivative1(const Matrix31 &w) const
Computes the derivative of q_21 when rotated with angular velocity expressed in frame 1 v_2 = q_21 * ...
Definition: Quaternion.hpp:308
Quaternion operator*(const Quaternion &q) const
Quaternion multiplication operator.
Definition: Quaternion.hpp:255
Type dot(const MatrixM1 &b) const
Definition: Vector.hpp:55
Matrix< Type, 4, 1 > Matrix41
Definition: Quaternion.hpp:56
Vector unit() const
Definition: Vector.hpp:91
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
Definition: integration.cpp:8
Matrix< Type, M, N > abs() const
Definition: Matrix.hpp:492
Euler angles class.
Definition: AxisAngle.hpp:18
Vector3< Type > imag() const
Imaginary components of quaternion.
Definition: Quaternion.hpp:501
Type wrap_pi(Type x)
Wrap value in range [-π, π)
Quaternion(Type a, Type b, Type c, Type d)
Constructor from quaternion values.
Definition: Quaternion.hpp:240
Quaternion inversed() const
Invert quaternion.
Definition: Quaternion.hpp:343
Quaternion< float > Quatf
Definition: Quaternion.hpp:544
float dt
Definition: px4io.c:73
void operator*=(const Quaternion &other)
Self-multiplication operator.
Definition: Quaternion.hpp:271
Quaternion(const Type data_[4])
Constructor from array.
Definition: Quaternion.hpp:64
AxisAngle class.
Definition: AxisAngle.hpp:21
Quaternion(const Matrix41 &other)
Constructor from Matrix41.
Definition: Quaternion.hpp:86
Quaternion()
Standard constructor.
Definition: Quaternion.hpp:72
Vector3< Type > conjugate_inversed(const Vector3< Type > &vec) const
Rotates vector v_2 in frame 2 to vector v_1 in frame 1 using the rotation quaternion q_21 describing ...
Definition: Quaternion.hpp:414
Dual< Scalar, N > sin(const Dual< Scalar, N > &a)
Definition: Dual.hpp:279
Vector sqrt() const
Definition: Vector.hpp:111
Quaternion(const Euler< Type > &euler)
Constructor from euler angles.
Definition: Quaternion.hpp:143
Dual< Scalar, N > atan2(const Dual< Scalar, N > &a, const Dual< Scalar, N > &b)
Definition: Dual.hpp:325