1 // This file is part of Eigen, a lightweight C++ template library
2 // for linear algebra.
3 //
4 // Copyright (C) 2015 Tal Hadad <[email protected]>
5 //
6 // This Source Code Form is subject to the terms of the Mozilla
7 // Public License v. 2.0. If a copy of the MPL was not distributed
8 // with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
9
10 #ifndef EIGEN_EULERANGLESCLASS_H// TODO: Fix previous "EIGEN_EULERANGLES_H" definition?
11 #define EIGEN_EULERANGLESCLASS_H
12
13 namespace Eigen
14 {
15 /** \class EulerAngles
16 *
17 * \ingroup EulerAngles_Module
18 *
19 * \brief Represents a rotation in a 3 dimensional space as three Euler angles.
20 *
21 * Euler rotation is a set of three rotation of three angles over three fixed axes, defined by the EulerSystem given as a template parameter.
22 *
23 * Here is how intrinsic Euler angles works:
24 * - first, rotate the axes system over the alpha axis in angle alpha
25 * - then, rotate the axes system over the beta axis(which was rotated in the first stage) in angle beta
26 * - then, rotate the axes system over the gamma axis(which was rotated in the two stages above) in angle gamma
27 *
28 * \note This class support only intrinsic Euler angles for simplicity,
29 * see EulerSystem how to easily overcome this for extrinsic systems.
30 *
31 * ### Rotation representation and conversions ###
32 *
33 * It has been proved(see Wikipedia link below) that every rotation can be represented
34 * by Euler angles, but there is no single representation (e.g. unlike rotation matrices).
35 * Therefore, you can convert from Eigen rotation and to them
36 * (including rotation matrices, which is not called "rotations" by Eigen design).
37 *
38 * Euler angles usually used for:
39 * - convenient human representation of rotation, especially in interactive GUI.
40 * - gimbal systems and robotics
41 * - efficient encoding(i.e. 3 floats only) of rotation for network protocols.
42 *
43 * However, Euler angles are slow comparing to quaternion or matrices,
44 * because their unnatural math definition, although it's simple for human.
45 * To overcome this, this class provide easy movement from the math friendly representation
46 * to the human friendly representation, and vise-versa.
47 *
48 * All the user need to do is a safe simple C++ type conversion,
49 * and this class take care for the math.
50 * Additionally, some axes related computation is done in compile time.
51 *
52 * #### Euler angles ranges in conversions ####
53 * Rotations representation as EulerAngles are not single (unlike matrices),
54 * and even have infinite EulerAngles representations.<BR>
55 * For example, add or subtract 2*PI from either angle of EulerAngles
56 * and you'll get the same rotation.
57 * This is the general reason for infinite representation,
58 * but it's not the only general reason for not having a single representation.
59 *
60 * When converting rotation to EulerAngles, this class convert it to specific ranges
61 * When converting some rotation to EulerAngles, the rules for ranges are as follow:
62 * - If the rotation we converting from is an EulerAngles
63 * (even when it represented as RotationBase explicitly), angles ranges are __undefined__.
64 * - otherwise, alpha and gamma angles will be in the range [-PI, PI].<BR>
65 * As for Beta angle:
66 * - If the system is Tait-Bryan, the beta angle will be in the range [-PI/2, PI/2].
67 * - otherwise:
68 * - If the beta axis is positive, the beta angle will be in the range [0, PI]
69 * - If the beta axis is negative, the beta angle will be in the range [-PI, 0]
70 *
71 * \sa EulerAngles(const MatrixBase<Derived>&)
72 * \sa EulerAngles(const RotationBase<Derived, 3>&)
73 *
74 * ### Convenient user typedefs ###
75 *
76 * Convenient typedefs for EulerAngles exist for float and double scalar,
77 * in a form of EulerAngles{A}{B}{C}{scalar},
78 * e.g. \ref EulerAnglesXYZd, \ref EulerAnglesZYZf.
79 *
80 * Only for positive axes{+x,+y,+z} Euler systems are have convenient typedef.
81 * If you need negative axes{-x,-y,-z}, it is recommended to create you own typedef with
82 * a word that represent what you need.
83 *
84 * ### Example ###
85 *
86 * \include EulerAngles.cpp
87 * Output: \verbinclude EulerAngles.out
88 *
89 * ### Additional reading ###
90 *
91 * If you're want to get more idea about how Euler system work in Eigen see EulerSystem.
92 *
93 * More information about Euler angles: https://en.wikipedia.org/wiki/Euler_angles
94 *
95 * \tparam _Scalar the scalar type, i.e. the type of the angles.
96 *
97 * \tparam _System the EulerSystem to use, which represents the axes of rotation.
98 */
99 template <typename _Scalar, class _System>
100 class EulerAngles : public RotationBase<EulerAngles<_Scalar, _System>, 3>
101 {
102 public:
103 typedef RotationBase<EulerAngles<_Scalar, _System>, 3> Base;
104
105 /** the scalar type of the angles */
106 typedef _Scalar Scalar;
107 typedef typename NumTraits<Scalar>::Real RealScalar;
108
109 /** the EulerSystem to use, which represents the axes of rotation. */
110 typedef _System System;
111
112 typedef Matrix<Scalar,3,3> Matrix3; /*!< the equivalent rotation matrix type */
113 typedef Matrix<Scalar,3,1> Vector3; /*!< the equivalent 3 dimension vector type */
114 typedef Quaternion<Scalar> QuaternionType; /*!< the equivalent quaternion type */
115 typedef AngleAxis<Scalar> AngleAxisType; /*!< the equivalent angle-axis type */
116
117 /** \returns the axis vector of the first (alpha) rotation */
AlphaAxisVector()118 static Vector3 AlphaAxisVector() {
119 const Vector3& u = Vector3::Unit(System::AlphaAxisAbs - 1);
120 return System::IsAlphaOpposite ? -u : u;
121 }
122
123 /** \returns the axis vector of the second (beta) rotation */
BetaAxisVector()124 static Vector3 BetaAxisVector() {
125 const Vector3& u = Vector3::Unit(System::BetaAxisAbs - 1);
126 return System::IsBetaOpposite ? -u : u;
127 }
128
129 /** \returns the axis vector of the third (gamma) rotation */
GammaAxisVector()130 static Vector3 GammaAxisVector() {
131 const Vector3& u = Vector3::Unit(System::GammaAxisAbs - 1);
132 return System::IsGammaOpposite ? -u : u;
133 }
134
135 private:
136 Vector3 m_angles;
137
138 public:
139 /** Default constructor without initialization. */
EulerAngles()140 EulerAngles() {}
141 /** Constructs and initialize an EulerAngles (\p alpha, \p beta, \p gamma). */
EulerAngles(const Scalar & alpha,const Scalar & beta,const Scalar & gamma)142 EulerAngles(const Scalar& alpha, const Scalar& beta, const Scalar& gamma) :
143 m_angles(alpha, beta, gamma) {}
144
145 // TODO: Test this constructor
146 /** Constructs and initialize an EulerAngles from the array data {alpha, beta, gamma} */
EulerAngles(const Scalar * data)147 explicit EulerAngles(const Scalar* data) : m_angles(data) {}
148
149 /** Constructs and initializes an EulerAngles from either:
150 * - a 3x3 rotation matrix expression(i.e. pure orthogonal matrix with determinant of +1),
151 * - a 3D vector expression representing Euler angles.
152 *
153 * \note If \p other is a 3x3 rotation matrix, the angles range rules will be as follow:<BR>
154 * Alpha and gamma angles will be in the range [-PI, PI].<BR>
155 * As for Beta angle:
156 * - If the system is Tait-Bryan, the beta angle will be in the range [-PI/2, PI/2].
157 * - otherwise:
158 * - If the beta axis is positive, the beta angle will be in the range [0, PI]
159 * - If the beta axis is negative, the beta angle will be in the range [-PI, 0]
160 */
161 template<typename Derived>
EulerAngles(const MatrixBase<Derived> & other)162 explicit EulerAngles(const MatrixBase<Derived>& other) { *this = other; }
163
164 /** Constructs and initialize Euler angles from a rotation \p rot.
165 *
166 * \note If \p rot is an EulerAngles (even when it represented as RotationBase explicitly),
167 * angles ranges are __undefined__.
168 * Otherwise, alpha and gamma angles will be in the range [-PI, PI].<BR>
169 * As for Beta angle:
170 * - If the system is Tait-Bryan, the beta angle will be in the range [-PI/2, PI/2].
171 * - otherwise:
172 * - If the beta axis is positive, the beta angle will be in the range [0, PI]
173 * - If the beta axis is negative, the beta angle will be in the range [-PI, 0]
174 */
175 template<typename Derived>
EulerAngles(const RotationBase<Derived,3> & rot)176 EulerAngles(const RotationBase<Derived, 3>& rot) { System::CalcEulerAngles(*this, rot.toRotationMatrix()); }
177
178 /*EulerAngles(const QuaternionType& q)
179 {
180 // TODO: Implement it in a faster way for quaternions
181 // According to http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToEuler/
182 // we can compute only the needed matrix cells and then convert to euler angles. (see ZYX example below)
183 // Currently we compute all matrix cells from quaternion.
184
185 // Special case only for ZYX
186 //Scalar y2 = q.y() * q.y();
187 //m_angles[0] = std::atan2(2*(q.w()*q.z() + q.x()*q.y()), (1 - 2*(y2 + q.z()*q.z())));
188 //m_angles[1] = std::asin( 2*(q.w()*q.y() - q.z()*q.x()));
189 //m_angles[2] = std::atan2(2*(q.w()*q.x() + q.y()*q.z()), (1 - 2*(q.x()*q.x() + y2)));
190 }*/
191
192 /** \returns The angle values stored in a vector (alpha, beta, gamma). */
angles()193 const Vector3& angles() const { return m_angles; }
194 /** \returns A read-write reference to the angle values stored in a vector (alpha, beta, gamma). */
angles()195 Vector3& angles() { return m_angles; }
196
197 /** \returns The value of the first angle. */
alpha()198 Scalar alpha() const { return m_angles[0]; }
199 /** \returns A read-write reference to the angle of the first angle. */
alpha()200 Scalar& alpha() { return m_angles[0]; }
201
202 /** \returns The value of the second angle. */
beta()203 Scalar beta() const { return m_angles[1]; }
204 /** \returns A read-write reference to the angle of the second angle. */
beta()205 Scalar& beta() { return m_angles[1]; }
206
207 /** \returns The value of the third angle. */
gamma()208 Scalar gamma() const { return m_angles[2]; }
209 /** \returns A read-write reference to the angle of the third angle. */
gamma()210 Scalar& gamma() { return m_angles[2]; }
211
212 /** \returns The Euler angles rotation inverse (which is as same as the negative),
213 * (-alpha, -beta, -gamma).
214 */
inverse()215 EulerAngles inverse() const
216 {
217 EulerAngles res;
218 res.m_angles = -m_angles;
219 return res;
220 }
221
222 /** \returns The Euler angles rotation negative (which is as same as the inverse),
223 * (-alpha, -beta, -gamma).
224 */
225 EulerAngles operator -() const
226 {
227 return inverse();
228 }
229
230 /** Set \c *this from either:
231 * - a 3x3 rotation matrix expression(i.e. pure orthogonal matrix with determinant of +1),
232 * - a 3D vector expression representing Euler angles.
233 *
234 * See EulerAngles(const MatrixBase<Derived, 3>&) for more information about
235 * angles ranges output.
236 */
237 template<class Derived>
238 EulerAngles& operator=(const MatrixBase<Derived>& other)
239 {
240 EIGEN_STATIC_ASSERT((internal::is_same<Scalar, typename Derived::Scalar>::value),
241 YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY)
242
243 internal::eulerangles_assign_impl<System, Derived>::run(*this, other.derived());
244 return *this;
245 }
246
247 // TODO: Assign and construct from another EulerAngles (with different system)
248
249 /** Set \c *this from a rotation.
250 *
251 * See EulerAngles(const RotationBase<Derived, 3>&) for more information about
252 * angles ranges output.
253 */
254 template<typename Derived>
255 EulerAngles& operator=(const RotationBase<Derived, 3>& rot) {
256 System::CalcEulerAngles(*this, rot.toRotationMatrix());
257 return *this;
258 }
259
260 /** \returns \c true if \c *this is approximately equal to \a other, within the precision
261 * determined by \a prec.
262 *
263 * \sa MatrixBase::isApprox() */
264 bool isApprox(const EulerAngles& other,
265 const RealScalar& prec = NumTraits<Scalar>::dummy_precision()) const
266 { return angles().isApprox(other.angles(), prec); }
267
268 /** \returns an equivalent 3x3 rotation matrix. */
toRotationMatrix()269 Matrix3 toRotationMatrix() const
270 {
271 // TODO: Calc it faster
272 return static_cast<QuaternionType>(*this).toRotationMatrix();
273 }
274
275 /** Convert the Euler angles to quaternion. */
QuaternionType()276 operator QuaternionType() const
277 {
278 return
279 AngleAxisType(alpha(), AlphaAxisVector()) *
280 AngleAxisType(beta(), BetaAxisVector()) *
281 AngleAxisType(gamma(), GammaAxisVector());
282 }
283
284 friend std::ostream& operator<<(std::ostream& s, const EulerAngles<Scalar, System>& eulerAngles)
285 {
286 s << eulerAngles.angles().transpose();
287 return s;
288 }
289
290 /** \returns \c *this with scalar type casted to \a NewScalarType */
291 template <typename NewScalarType>
cast()292 EulerAngles<NewScalarType, System> cast() const
293 {
294 EulerAngles<NewScalarType, System> e;
295 e.angles() = angles().template cast<NewScalarType>();
296 return e;
297 }
298 };
299
300 #define EIGEN_EULER_ANGLES_SINGLE_TYPEDEF(AXES, SCALAR_TYPE, SCALAR_POSTFIX) \
301 /** \ingroup EulerAngles_Module */ \
302 typedef EulerAngles<SCALAR_TYPE, EulerSystem##AXES> EulerAngles##AXES##SCALAR_POSTFIX;
303
304 #define EIGEN_EULER_ANGLES_TYPEDEFS(SCALAR_TYPE, SCALAR_POSTFIX) \
305 EIGEN_EULER_ANGLES_SINGLE_TYPEDEF(XYZ, SCALAR_TYPE, SCALAR_POSTFIX) \
306 EIGEN_EULER_ANGLES_SINGLE_TYPEDEF(XYX, SCALAR_TYPE, SCALAR_POSTFIX) \
307 EIGEN_EULER_ANGLES_SINGLE_TYPEDEF(XZY, SCALAR_TYPE, SCALAR_POSTFIX) \
308 EIGEN_EULER_ANGLES_SINGLE_TYPEDEF(XZX, SCALAR_TYPE, SCALAR_POSTFIX) \
309 \
310 EIGEN_EULER_ANGLES_SINGLE_TYPEDEF(YZX, SCALAR_TYPE, SCALAR_POSTFIX) \
311 EIGEN_EULER_ANGLES_SINGLE_TYPEDEF(YZY, SCALAR_TYPE, SCALAR_POSTFIX) \
312 EIGEN_EULER_ANGLES_SINGLE_TYPEDEF(YXZ, SCALAR_TYPE, SCALAR_POSTFIX) \
313 EIGEN_EULER_ANGLES_SINGLE_TYPEDEF(YXY, SCALAR_TYPE, SCALAR_POSTFIX) \
314 \
315 EIGEN_EULER_ANGLES_SINGLE_TYPEDEF(ZXY, SCALAR_TYPE, SCALAR_POSTFIX) \
316 EIGEN_EULER_ANGLES_SINGLE_TYPEDEF(ZXZ, SCALAR_TYPE, SCALAR_POSTFIX) \
317 EIGEN_EULER_ANGLES_SINGLE_TYPEDEF(ZYX, SCALAR_TYPE, SCALAR_POSTFIX) \
318 EIGEN_EULER_ANGLES_SINGLE_TYPEDEF(ZYZ, SCALAR_TYPE, SCALAR_POSTFIX)
319
EIGEN_EULER_ANGLES_TYPEDEFS(float,f)320 EIGEN_EULER_ANGLES_TYPEDEFS(float, f)
321 EIGEN_EULER_ANGLES_TYPEDEFS(double, d)
322
323 namespace internal
324 {
325 template<typename _Scalar, class _System>
326 struct traits<EulerAngles<_Scalar, _System> >
327 {
328 typedef _Scalar Scalar;
329 };
330
331 // set from a rotation matrix
332 template<class System, class Other>
333 struct eulerangles_assign_impl<System,Other,3,3>
334 {
335 typedef typename Other::Scalar Scalar;
336 static void run(EulerAngles<Scalar, System>& e, const Other& m)
337 {
338 System::CalcEulerAngles(e, m);
339 }
340 };
341
342 // set from a vector of Euler angles
343 template<class System, class Other>
344 struct eulerangles_assign_impl<System,Other,3,1>
345 {
346 typedef typename Other::Scalar Scalar;
347 static void run(EulerAngles<Scalar, System>& e, const Other& vec)
348 {
349 e.angles() = vec;
350 }
351 };
352 }
353 }
354
355 #endif // EIGEN_EULERANGLESCLASS_H
356