EulerAngles.h (15367B)
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@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 /** \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 */ 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 */ 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 */ 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. */ 140 EulerAngles() {} 141 /** Constructs and initialize an EulerAngles (\p alpha, \p beta, \p 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} */ 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> 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> 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). */ 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). */ 195 Vector3& angles() { return m_angles; } 196 197 /** \returns The value of the first angle. */ 198 Scalar alpha() const { return m_angles[0]; } 199 /** \returns A read-write reference to the angle of the first angle. */ 200 Scalar& alpha() { return m_angles[0]; } 201 202 /** \returns The value of the second angle. */ 203 Scalar beta() const { return m_angles[1]; } 204 /** \returns A read-write reference to the angle of the second angle. */ 205 Scalar& beta() { return m_angles[1]; } 206 207 /** \returns The value of the third angle. */ 208 Scalar gamma() const { return m_angles[2]; } 209 /** \returns A read-write reference to the angle of the third angle. */ 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 */ 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. */ 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. */ 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> 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 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