Free Electron
Euler.h
Go to the documentation of this file.
1 /* Copyright (C) 2003-2021 Free Electron Organization
2  Any use of this software requires a license. If a valid license
3  was not distributed with this file, visit freeelectron.org. */
4 
5 /** @file */
6 
7 #ifndef __math_Euler_h__
8 #define __math_Euler_h__
9 
10 
11 namespace fe
12 {
13 
14 /**************************************************************************//**
15  @brief Special vector for concatenated Euler angles
16 
17  This represents a multiplication of three rotation matrices,
18  about X, Y, and Z, in that order.
19 
20  These are raw post-multiplication steps,
21  not the pre-multiplication concatenation convention like Matrix::rotate,
22  so the conversion to matrix rotates in the reverse order
23  (rotate Z, rotate Y, then rotate X).
24 
25  In a X-forward Z-up world this maps into a transformation
26  of heading CCW, pitch down, and then bank right.
27 
28  @ingroup math
29 *//***************************************************************************/
30 template<typename T>
31 class Euler: public Vector<3,T>
32 {
33  public:
34  Euler(void):
35  Vector<3,T>() {}
36 
37  Euler(T x,T y,T z):
38  Vector<3,T>(x,y,z) {}
39 
40  Euler(const Vector<3,T> &other):
41  Vector<3,T>(other) {}
42  Euler(const Euler<T> &other):
43  Vector<3,T>() { operator=(other); }
44  Euler(const Quaternion<T>& quaternion)
45  { operator=(quaternion); }
46  Euler(const Matrix<3,4,T>& matrix):
47  Vector<3,T>() { operator=(matrix); }
48 
49  Euler<T>& operator=(const Euler<T>& euler);
50  Euler<T>& operator=(const Quaternion<T>& quaternion);
51  Euler<T>& operator=(const Matrix<3,4,T>& matrix);
52 
53  operator Matrix<3,4,T>(void) const;
54 
55 using Vector<3,T>::operator[];
56 using Vector<3,T>::operator=;
57 };
58 
59 template<class T>
60 inline Euler<T>& Euler<T>::operator=(const Euler<T>& other)
61 {
62  set(*this,other[0],other[1],other[2]);
63  return *this;
64 }
65 
66 template<class T>
67 inline Euler<T>& Euler<T>::operator=(const Matrix<3,4,T>& matrix)
68 {
69  const T pi2=T(0.5)*fe::pi-1e-6f;
70  (*this)[1]=asin(-matrix(2,0));
71  if((*this)[1] < pi2)
72  {
73  if((*this)[1] > -pi2)
74  {
75  (*this)[0]=atan2(matrix(2,1),matrix(2,2));
76  (*this)[2]=atan2(matrix(1,0),matrix(0,0));
77  }
78  else
79  {
80  // singularity (twist is x+z)
81  (*this)[0]=T(0);
82  (*this)[2]=atan2(-matrix(0,1),-matrix(0,2));
83  }
84  }
85  else
86  {
87  // singularity (twist is x-z)
88  (*this)[0]=T(0);
89  (*this)[2]= -atan2(matrix(0,1),matrix(0,2));
90  }
91  return *this;
92 }
93 
94 // https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles
95 template<class T>
96 inline Euler<T>& Euler<T>::operator=(const Quaternion<T>& quat)
97 {
98 
99  // roll (x-axis rotation)
100  const double sinr_cosp = 2 * (quat[3] * quat[0] + quat[1] * quat[2]);
101  const double cosr_cosp = 1 - 2 * (quat[0] * quat[0] + quat[1] * quat[1]);
102  (*this)[0] = atan2(sinr_cosp, cosr_cosp);
103 
104  // pitch (y-axis rotation)
105  const double sinp = 2 * (quat[3] * quat[1] - quat[2] * quat[0]);
106  if(fabs(sinp) >= 1)
107  {
108  // use 90 degrees if out of range
109  (*this)[1] = copysign(M_PI / 2, sinp);
110  }
111  else
112  {
113  (*this)[1] = asin(sinp);
114  }
115 
116  // yaw (z-axis rotation)
117  const double siny_cosp = 2 * (quat[3] * quat[2] + quat[0] * quat[1]);
118  const double cosy_cosp = 1 - 2 * (quat[1] * quat[1] + quat[2] * quat[2]);
119  (*this)[2] = atan2(siny_cosp, cosy_cosp);
120 
121  return *this;
122 }
123 
124 template<class T>
125 inline Euler<T>::operator Matrix<3,4,T>(void) const
126 {
127  Matrix<3,4,T> matrix;
128  fe::setIdentity(matrix);
129  rotate(matrix,(*this)[2],e_zAxis);
130  rotate(matrix,(*this)[1],e_yAxis);
131  rotate(matrix,(*this)[0],e_xAxis);
132  return matrix;
133 }
134 
135 typedef Euler<F32> Eulerf;
136 typedef Euler<F64> Eulerd;
137 typedef Euler<Real> SpatialEuler;
138 
139 } /* namespace */
140 
141 #endif /* __math_Euler_h__ */
Matrix for 3D transformations.
Definition: Matrix3x4.h:47
kernel
Definition: namespace.dox:3
Dense vector - size fixed by template.
Definition: Vector.h:19
Special vector for concatenated Euler angles.
Definition: Euler.h:31
Four-dimensional complex number.
Definition: Quaternion.h:32