EulerAngles.h

// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2015 Tal Hadad <tal_hd@hotmail.com>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
 
#ifndef EIGEN_EULERANGLESCLASS_H// TODO: Fix previous "EIGEN_EULERANGLES_H" definition?
#define EIGEN_EULERANGLESCLASS_H
 
#include "./InternalHeaderCheck.h"
 
namespace Eigen
{
  template <typename Scalar_, class _System>
  class EulerAngles : public RotationBase<EulerAngles<Scalar_, _System>, 3>
  {
    public:
      typedef RotationBase<EulerAngles<Scalar_, _System>, 3> Base;
      
      typedef Scalar_ Scalar;
      typedef typename NumTraits<Scalar>::Real RealScalar;
      
      typedef _System System;
    
      typedef Matrix<Scalar,3,3> Matrix3; 
      typedef Matrix<Scalar,3,1> Vector3; 
      typedef Quaternion<Scalar> QuaternionType; 
      typedef AngleAxis<Scalar> AngleAxisType; 
      static Vector3 AlphaAxisVector() {
        const Vector3& u = Vector3::Unit(System::AlphaAxisAbs - 1);
        return System::IsAlphaOpposite ? -u : u;
      }
      
      static Vector3 BetaAxisVector() {
        const Vector3& u = Vector3::Unit(System::BetaAxisAbs - 1);
        return System::IsBetaOpposite ? -u : u;
      }
      
      static Vector3 GammaAxisVector() {
        const Vector3& u = Vector3::Unit(System::GammaAxisAbs - 1);
        return System::IsGammaOpposite ? -u : u;
      }
 
    private:
      Vector3 m_angles;
 
    public:
      EulerAngles() {}
      EulerAngles(const Scalar& alpha, const Scalar& beta, const Scalar& gamma) :
        m_angles(alpha, beta, gamma) {}
      
      // TODO: Test this constructor
      explicit EulerAngles(const Scalar* data) : m_angles(data) {}
      
      template<typename Derived>
      explicit EulerAngles(const MatrixBase<Derived>& other) { *this = other; }
      
      template<typename Derived>
      EulerAngles(const RotationBase<Derived, 3>& rot) { System::CalcEulerAngles(*this, rot.toRotationMatrix()); }
      
      /*EulerAngles(const QuaternionType& q)
      {
        // TODO: Implement it in a faster way for quaternions
        // According to http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToEuler/
        //  we can compute only the needed matrix cells and then convert to euler angles. (see ZYX example below)
        // Currently we compute all matrix cells from quaternion.
 
        // Special case only for ZYX
        //Scalar y2 = q.y() * q.y();
        //m_angles[0] = std::atan2(2*(q.w()*q.z() + q.x()*q.y()), (1 - 2*(y2 + q.z()*q.z())));
        //m_angles[1] = std::asin( 2*(q.w()*q.y() - q.z()*q.x()));
        //m_angles[2] = std::atan2(2*(q.w()*q.x() + q.y()*q.z()), (1 - 2*(q.x()*q.x() + y2)));
      }*/
 
      const Vector3& angles() const { return m_angles; }
      Vector3& angles() { return m_angles; }
 
      Scalar alpha() const { return m_angles[0]; }
      Scalar& alpha() { return m_angles[0]; }
 
      Scalar beta() const { return m_angles[1]; }
      Scalar& beta() { return m_angles[1]; }
 
      Scalar gamma() const { return m_angles[2]; }
      Scalar& gamma() { return m_angles[2]; }
 
      EulerAngles inverse() const
      {
        EulerAngles res;
        res.m_angles = -m_angles;
        return res;
      }
 
      EulerAngles operator -() const
      {
        return inverse();
      }
      
      template<class Derived>
      EulerAngles& operator=(const MatrixBase<Derived>& other)
      {
        EIGEN_STATIC_ASSERT((internal::is_same<Scalar, typename Derived::Scalar>::value),
         YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY)
        
        internal::eulerangles_assign_impl<System, Derived>::run(*this, other.derived());
        return *this;
      }
 
      // TODO: Assign and construct from another EulerAngles (with different system)
      
      template<typename Derived>
      EulerAngles& operator=(const RotationBase<Derived, 3>& rot) {
        System::CalcEulerAngles(*this, rot.toRotationMatrix());
        return *this;
      }
      
      bool isApprox(const EulerAngles& other,
        const RealScalar& prec = NumTraits<Scalar>::dummy_precision()) const
      { return angles().isApprox(other.angles(), prec); }
 
      Matrix3 toRotationMatrix() const
      {
        // TODO: Calc it faster
        return static_cast<QuaternionType>(*this).toRotationMatrix();
      }
 
      operator QuaternionType() const
      {
        return
          AngleAxisType(alpha(), AlphaAxisVector()) *
          AngleAxisType(beta(), BetaAxisVector())   *
          AngleAxisType(gamma(), GammaAxisVector());
      }
      
      friend std::ostream& operator<<(std::ostream& s, const EulerAngles<Scalar, System>& eulerAngles)
      {
        s << eulerAngles.angles().transpose();
        return s;
      }
      
      template <typename NewScalarType>
      EulerAngles<NewScalarType, System> cast() const
      {
        EulerAngles<NewScalarType, System> e;
        e.angles() = angles().template cast<NewScalarType>();
        return e;
      }
  };
 
#define EIGEN_EULER_ANGLES_SINGLE_TYPEDEF(AXES, SCALAR_TYPE, SCALAR_POSTFIX) \
 \
  typedef EulerAngles<SCALAR_TYPE, EulerSystem##AXES> EulerAngles##AXES##SCALAR_POSTFIX;
 
#define EIGEN_EULER_ANGLES_TYPEDEFS(SCALAR_TYPE, SCALAR_POSTFIX) \
  EIGEN_EULER_ANGLES_SINGLE_TYPEDEF(XYZ, SCALAR_TYPE, SCALAR_POSTFIX) \
  EIGEN_EULER_ANGLES_SINGLE_TYPEDEF(XYX, SCALAR_TYPE, SCALAR_POSTFIX) \
  EIGEN_EULER_ANGLES_SINGLE_TYPEDEF(XZY, SCALAR_TYPE, SCALAR_POSTFIX) \
  EIGEN_EULER_ANGLES_SINGLE_TYPEDEF(XZX, SCALAR_TYPE, SCALAR_POSTFIX) \
 \
  EIGEN_EULER_ANGLES_SINGLE_TYPEDEF(YZX, SCALAR_TYPE, SCALAR_POSTFIX) \
  EIGEN_EULER_ANGLES_SINGLE_TYPEDEF(YZY, SCALAR_TYPE, SCALAR_POSTFIX) \
  EIGEN_EULER_ANGLES_SINGLE_TYPEDEF(YXZ, SCALAR_TYPE, SCALAR_POSTFIX) \
  EIGEN_EULER_ANGLES_SINGLE_TYPEDEF(YXY, SCALAR_TYPE, SCALAR_POSTFIX) \
 \
  EIGEN_EULER_ANGLES_SINGLE_TYPEDEF(ZXY, SCALAR_TYPE, SCALAR_POSTFIX) \
  EIGEN_EULER_ANGLES_SINGLE_TYPEDEF(ZXZ, SCALAR_TYPE, SCALAR_POSTFIX) \
  EIGEN_EULER_ANGLES_SINGLE_TYPEDEF(ZYX, SCALAR_TYPE, SCALAR_POSTFIX) \
  EIGEN_EULER_ANGLES_SINGLE_TYPEDEF(ZYZ, SCALAR_TYPE, SCALAR_POSTFIX)
 
EIGEN_EULER_ANGLES_TYPEDEFS(float, f)
EIGEN_EULER_ANGLES_TYPEDEFS(double, d)
 
  namespace internal
  {
    template<typename Scalar_, class _System>
    struct traits<EulerAngles<Scalar_, _System> >
    {
      typedef Scalar_ Scalar;
    };
    
    // set from a rotation matrix
    template<class System, class Other>
    struct eulerangles_assign_impl<System,Other,3,3>
    {
      typedef typename Other::Scalar Scalar;
      static void run(EulerAngles<Scalar, System>& e, const Other& m)
      {
        System::CalcEulerAngles(e, m);
      }
    };
    
    // set from a vector of Euler angles
    template<class System, class Other>
    struct eulerangles_assign_impl<System,Other,3,1>
    {
      typedef typename Other::Scalar Scalar;
      static void run(EulerAngles<Scalar, System>& e, const Other& vec)
      {
        e.angles() = vec;
      }
    };
  }
}
 
#endif // EIGEN_EULERANGLESCLASS_H