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