EulerAngles.h
00001 // This file is part of Eigen, a lightweight C++ template library
00002 // for linear algebra.
00003 //
00004 // Copyright (C) 2015 Tal Hadad <tal_hd@hotmail.com>
00005 //
00006 // This Source Code Form is subject to the terms of the Mozilla
00007 // Public License v. 2.0. If a copy of the MPL was not distributed
00008 // with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
00009 
00010 #ifndef EIGEN_EULERANGLESCLASS_H// TODO: Fix previous "EIGEN_EULERANGLES_H" definition?
00011 #define EIGEN_EULERANGLESCLASS_H
00012 
00013 namespace Eigen
00014 {
00015   /*template<typename Other,
00016          int OtherRows=Other::RowsAtCompileTime,
00017          int OtherCols=Other::ColsAtCompileTime>
00018   struct ei_eulerangles_assign_impl;*/
00019 
00110   template <typename _Scalar, class _System>
00111   class EulerAngles : public RotationBase<EulerAngles<_Scalar, _System>, 3>
00112   {
00113     public:
00115       typedef _Scalar Scalar;
00116       
00118       typedef _System System;
00119     
00120       typedef Matrix<Scalar,3,3> Matrix3; 
00121       typedef Matrix<Scalar,3,1> Vector3; 
00122       typedef Quaternion<Scalar> QuaternionType; 
00123       typedef AngleAxis<Scalar> AngleAxisType; 
00126       static Vector3 AlphaAxisVector() {
00127         const Vector3& u = Vector3::Unit(System::AlphaAxisAbs - 1);
00128         return System::IsAlphaOpposite ? -u : u;
00129       }
00130       
00132       static Vector3 BetaAxisVector() {
00133         const Vector3& u = Vector3::Unit(System::BetaAxisAbs - 1);
00134         return System::IsBetaOpposite ? -u : u;
00135       }
00136       
00138       static Vector3 GammaAxisVector() {
00139         const Vector3& u = Vector3::Unit(System::GammaAxisAbs - 1);
00140         return System::IsGammaOpposite ? -u : u;
00141       }
00142 
00143     private:
00144       Vector3 m_angles;
00145 
00146     public:
00148       EulerAngles() {}
00150       EulerAngles(const Scalar& alpha, const Scalar& beta, const Scalar& gamma) :
00151         m_angles(alpha, beta, gamma) {}
00152       
00157       template<typename Derived>
00158       EulerAngles(const MatrixBase<Derived>& m) { *this = m; }
00159       
00171       template<typename Derived>
00172       EulerAngles(
00173         const MatrixBase<Derived>& m,
00174         bool positiveRangeAlpha,
00175         bool positiveRangeBeta,
00176         bool positiveRangeGamma) {
00177         
00178         System::CalcEulerAngles(*this, m, positiveRangeAlpha, positiveRangeBeta, positiveRangeGamma);
00179       }
00180       
00187       template<typename Derived>
00188       EulerAngles(const RotationBase<Derived, 3>& rot) { *this = rot; }
00189       
00201       template<typename Derived>
00202       EulerAngles(
00203         const RotationBase<Derived, 3>& rot,
00204         bool positiveRangeAlpha,
00205         bool positiveRangeBeta,
00206         bool positiveRangeGamma) {
00207         
00208         System::CalcEulerAngles(*this, rot.toRotationMatrix(), positiveRangeAlpha, positiveRangeBeta, positiveRangeGamma);
00209       }
00210 
00212       const Vector3& angles() const { return m_angles; }
00214       Vector3& angles() { return m_angles; }
00215 
00217       Scalar alpha() const { return m_angles[0]; }
00219       Scalar& alpha() { return m_angles[0]; }
00220 
00222       Scalar beta() const { return m_angles[1]; }
00224       Scalar& beta() { return m_angles[1]; }
00225 
00227       Scalar gamma() const { return m_angles[2]; }
00229       Scalar& gamma() { return m_angles[2]; }
00230 
00234       EulerAngles inverse() const
00235       {
00236         EulerAngles res;
00237         res.m_angles = -m_angles;
00238         return res;
00239       }
00240 
00244       EulerAngles operator -() const
00245       {
00246         return inverse();
00247       }
00248       
00260       template<
00261         bool PositiveRangeAlpha,
00262         bool PositiveRangeBeta,
00263         bool PositiveRangeGamma,
00264         typename Derived>
00265       static EulerAngles FromRotation(const MatrixBase<Derived>& m)
00266       {
00267         EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived, 3, 3)
00268         
00269         EulerAngles e;
00270         System::template CalcEulerAngles<
00271           PositiveRangeAlpha, PositiveRangeBeta, PositiveRangeGamma, _Scalar>(e, m);
00272         return e;
00273       }
00274       
00286       template<
00287         bool PositiveRangeAlpha,
00288         bool PositiveRangeBeta,
00289         bool PositiveRangeGamma,
00290         typename Derived>
00291       static EulerAngles FromRotation(const RotationBase<Derived, 3>& rot)
00292       {
00293         return FromRotation<PositiveRangeAlpha, PositiveRangeBeta, PositiveRangeGamma>(rot.toRotationMatrix());
00294       }
00295       
00296       /*EulerAngles& fromQuaternion(const QuaternionType& q)
00297       {
00298         // TODO: Implement it in a faster way for quaternions
00299         // According to http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToEuler/
00300         //  we can compute only the needed matrix cells and then convert to euler angles. (see ZYX example below)
00301         // Currently we compute all matrix cells from quaternion.
00302 
00303         // Special case only for ZYX
00304         //Scalar y2 = q.y() * q.y();
00305         //m_angles[0] = std::atan2(2*(q.w()*q.z() + q.x()*q.y()), (1 - 2*(y2 + q.z()*q.z())));
00306         //m_angles[1] = std::asin( 2*(q.w()*q.y() - q.z()*q.x()));
00307         //m_angles[2] = std::atan2(2*(q.w()*q.x() + q.y()*q.z()), (1 - 2*(q.x()*q.x() + y2)));
00308       }*/
00309       
00311       template<typename Derived>
00312       EulerAngles& operator=(const MatrixBase<Derived>& m) {
00313         EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived, 3, 3)
00314         
00315         System::CalcEulerAngles(*this, m);
00316         return *this;
00317       }
00318 
00319       // TODO: Assign and construct from another EulerAngles (with different system)
00320       
00322       template<typename Derived>
00323       EulerAngles& operator=(const RotationBase<Derived, 3>& rot) {
00324         System::CalcEulerAngles(*this, rot.toRotationMatrix());
00325         return *this;
00326       }
00327       
00328       // TODO: Support isApprox function
00329 
00331       Matrix3 toRotationMatrix() const
00332       {
00333         return static_cast<QuaternionType>(*this).toRotationMatrix();
00334       }
00335 
00337       operator QuaternionType() const
00338       {
00339         return
00340           AngleAxisType(alpha(), AlphaAxisVector()) *
00341           AngleAxisType(beta(), BetaAxisVector())   *
00342           AngleAxisType(gamma(), GammaAxisVector());
00343       }
00344       
00345       friend std::ostream& operator<<(std::ostream& s, const EulerAngles<Scalar, System>& eulerAngles)
00346       {
00347         s << eulerAngles.angles().transpose();
00348         return s;
00349       }
00350   };
00351 
00352 #define EIGEN_EULER_ANGLES_SINGLE_TYPEDEF(AXES, SCALAR_TYPE, SCALAR_POSTFIX) \
00353  \
00354   typedef EulerAngles<SCALAR_TYPE, EulerSystem##AXES> EulerAngles##AXES##SCALAR_POSTFIX;
00355 
00356 #define EIGEN_EULER_ANGLES_TYPEDEFS(SCALAR_TYPE, SCALAR_POSTFIX) \
00357   EIGEN_EULER_ANGLES_SINGLE_TYPEDEF(XYZ, SCALAR_TYPE, SCALAR_POSTFIX) \
00358   EIGEN_EULER_ANGLES_SINGLE_TYPEDEF(XYX, SCALAR_TYPE, SCALAR_POSTFIX) \
00359   EIGEN_EULER_ANGLES_SINGLE_TYPEDEF(XZY, SCALAR_TYPE, SCALAR_POSTFIX) \
00360   EIGEN_EULER_ANGLES_SINGLE_TYPEDEF(XZX, SCALAR_TYPE, SCALAR_POSTFIX) \
00361  \
00362   EIGEN_EULER_ANGLES_SINGLE_TYPEDEF(YZX, SCALAR_TYPE, SCALAR_POSTFIX) \
00363   EIGEN_EULER_ANGLES_SINGLE_TYPEDEF(YZY, SCALAR_TYPE, SCALAR_POSTFIX) \
00364   EIGEN_EULER_ANGLES_SINGLE_TYPEDEF(YXZ, SCALAR_TYPE, SCALAR_POSTFIX) \
00365   EIGEN_EULER_ANGLES_SINGLE_TYPEDEF(YXY, SCALAR_TYPE, SCALAR_POSTFIX) \
00366  \
00367   EIGEN_EULER_ANGLES_SINGLE_TYPEDEF(ZXY, SCALAR_TYPE, SCALAR_POSTFIX) \
00368   EIGEN_EULER_ANGLES_SINGLE_TYPEDEF(ZXZ, SCALAR_TYPE, SCALAR_POSTFIX) \
00369   EIGEN_EULER_ANGLES_SINGLE_TYPEDEF(ZYX, SCALAR_TYPE, SCALAR_POSTFIX) \
00370   EIGEN_EULER_ANGLES_SINGLE_TYPEDEF(ZYZ, SCALAR_TYPE, SCALAR_POSTFIX)
00371 
00372 EIGEN_EULER_ANGLES_TYPEDEFS(float, f)
00373 EIGEN_EULER_ANGLES_TYPEDEFS(double, d)
00374 
00375   namespace internal
00376   {
00377     template<typename _Scalar, class _System>
00378     struct traits<EulerAngles<_Scalar, _System> >
00379     {
00380       typedef _Scalar Scalar;
00381     };
00382   }
00383   
00384 }
00385 
00386 #endif // EIGEN_EULERANGLESCLASS_H
 All Classes Functions Variables Typedefs Enumerator