EulerSystem.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_EULERSYSTEM_H
#define EIGEN_EULERSYSTEM_H
 
#include "./InternalHeaderCheck.h"
 
namespace Eigen
{
  // Forward declarations
  template <typename Scalar_, class _System>
  class EulerAngles;
  
  namespace internal
  {
    // TODO: Add this trait to the Eigen internal API?
    template <int Num, bool IsPositive = (Num > 0)>
    struct Abs
    {
      enum { value = Num };
    };
  
    template <int Num>
    struct Abs<Num, false>
    {
      enum { value = -Num };
    };
 
    template <int Axis>
    struct IsValidAxis
    {
      enum { value = Axis != 0 && Abs<Axis>::value <= 3 };
    };
    
    template<typename System,
            typename Other,
            int OtherRows=Other::RowsAtCompileTime,
            int OtherCols=Other::ColsAtCompileTime>
    struct eulerangles_assign_impl;
  }
  
  #define EIGEN_EULER_ANGLES_CLASS_STATIC_ASSERT(COND,MSG) typedef char static_assertion_##MSG[(COND)?1:-1]
  
  enum EulerAxis
  {
    EULER_X = 1, 
    EULER_Y = 2, 
    EULER_Z = 3  
  };
  
  template <int _AlphaAxis, int _BetaAxis, int _GammaAxis>
  class EulerSystem
  {
    public:
    // It's defined this way and not as enum, because I think
    //  that enum is not guerantee to support negative numbers
    
    static constexpr int AlphaAxis = _AlphaAxis;
    
    static constexpr int BetaAxis = _BetaAxis;
    
    static constexpr int GammaAxis = _GammaAxis;
 
    enum
    {
      AlphaAxisAbs = internal::Abs<AlphaAxis>::value, 
      BetaAxisAbs = internal::Abs<BetaAxis>::value, 
      GammaAxisAbs = internal::Abs<GammaAxis>::value, 
      IsAlphaOpposite = (AlphaAxis < 0) ? 1 : 0, 
      IsBetaOpposite = (BetaAxis < 0) ? 1 : 0, 
      IsGammaOpposite = (GammaAxis < 0) ? 1 : 0, 
      // Parity is even if alpha axis X is followed by beta axis Y, or Y is followed
      // by Z, or Z is followed by X; otherwise it is odd.
      IsOdd = ((AlphaAxisAbs)%3 == (BetaAxisAbs - 1)%3) ? 0 : 1, 
      IsEven = IsOdd ? 0 : 1, 
      IsTaitBryan = ((unsigned)AlphaAxisAbs != (unsigned)GammaAxisAbs) ? 1 : 0 
    };
    
    private:
    
    EIGEN_EULER_ANGLES_CLASS_STATIC_ASSERT(internal::IsValidAxis<AlphaAxis>::value,
      ALPHA_AXIS_IS_INVALID);
      
    EIGEN_EULER_ANGLES_CLASS_STATIC_ASSERT(internal::IsValidAxis<BetaAxis>::value,
      BETA_AXIS_IS_INVALID);
      
    EIGEN_EULER_ANGLES_CLASS_STATIC_ASSERT(internal::IsValidAxis<GammaAxis>::value,
      GAMMA_AXIS_IS_INVALID);
      
    EIGEN_EULER_ANGLES_CLASS_STATIC_ASSERT((unsigned)AlphaAxisAbs != (unsigned)BetaAxisAbs,
      ALPHA_AXIS_CANT_BE_EQUAL_TO_BETA_AXIS);
      
    EIGEN_EULER_ANGLES_CLASS_STATIC_ASSERT((unsigned)BetaAxisAbs != (unsigned)GammaAxisAbs,
      BETA_AXIS_CANT_BE_EQUAL_TO_GAMMA_AXIS);
 
    static const int
      // I, J, K are the pivot indexes permutation for the rotation matrix, that match this Euler system. 
      // They are used in this class converters.
      // They are always different from each other, and their possible values are: 0, 1, or 2.
      I_ = AlphaAxisAbs - 1,
      J_ = (AlphaAxisAbs - 1 + 1 + IsOdd)%3,
      K_ = (AlphaAxisAbs - 1 + 2 - IsOdd)%3
    ;
    
    // TODO: Get @mat parameter in form that avoids double evaluation.
    template <typename Derived>
    static void CalcEulerAngles_imp(Matrix<typename MatrixBase<Derived>::Scalar, 3, 1>& res, const MatrixBase<Derived>& mat, internal::true_type /*isTaitBryan*/)
    {
      using std::atan2;
      using std::sqrt;
      
      typedef typename Derived::Scalar Scalar;
 
      const Scalar plusMinus = IsEven? 1 : -1;
      const Scalar minusPlus = IsOdd?  1 : -1;
 
      const Scalar Rsum = sqrt((mat(I_,I_) * mat(I_,I_) + mat(I_,J_) * mat(I_,J_) + mat(J_,K_) * mat(J_,K_) + mat(K_,K_) * mat(K_,K_))/2);
      res[1] = atan2(plusMinus * mat(I_,K_), Rsum);
 
      // There is a singularity when cos(beta) == 0
      if(Rsum > 4 * NumTraits<Scalar>::epsilon()) {// cos(beta) != 0
        res[0] = atan2(minusPlus * mat(J_, K_), mat(K_, K_));
        res[2] = atan2(minusPlus * mat(I_, J_), mat(I_, I_));
      }
      else if(plusMinus * mat(I_, K_) > 0) {// cos(beta) == 0 and sin(beta) == 1
        Scalar spos = mat(J_, I_) + plusMinus * mat(K_, J_); // 2*sin(alpha + plusMinus * gamma
        Scalar cpos = mat(J_, J_) + minusPlus * mat(K_, I_); // 2*cos(alpha + plusMinus * gamma)
        Scalar alphaPlusMinusGamma = atan2(spos, cpos);
        res[0] = alphaPlusMinusGamma;
        res[2] = 0;
      }
      else {// cos(beta) == 0 and sin(beta) == -1
        Scalar sneg = plusMinus * (mat(K_, J_) + minusPlus * mat(J_, I_)); // 2*sin(alpha + minusPlus*gamma)
        Scalar cneg = mat(J_, J_) + plusMinus * mat(K_, I_);               // 2*cos(alpha + minusPlus*gamma)
        Scalar alphaMinusPlusBeta = atan2(sneg, cneg);
        res[0] = alphaMinusPlusBeta;
        res[2] = 0;
      }
    }
 
    template <typename Derived>
    static void CalcEulerAngles_imp(Matrix<typename MatrixBase<Derived>::Scalar,3,1>& res,
                                    const MatrixBase<Derived>& mat, internal::false_type /*isTaitBryan*/)
    {
      using std::atan2;
      using std::sqrt;
 
      typedef typename Derived::Scalar Scalar;
 
      const Scalar plusMinus = IsEven? 1 : -1;
      const Scalar minusPlus = IsOdd?  1 : -1;
 
      const Scalar Rsum = sqrt((mat(I_, J_) * mat(I_, J_) + mat(I_, K_) * mat(I_, K_) + mat(J_, I_) * mat(J_, I_) + mat(K_, I_) * mat(K_, I_)) / 2);
 
      res[1] = atan2(Rsum, mat(I_, I_));
 
      // There is a singularity when sin(beta) == 0
      if(Rsum > 4 * NumTraits<Scalar>::epsilon()) {// sin(beta) != 0
        res[0] = atan2(mat(J_, I_), minusPlus * mat(K_, I_));
        res[2] = atan2(mat(I_, J_), plusMinus * mat(I_, K_));
      }
      else if(mat(I_, I_) > 0) {// sin(beta) == 0 and cos(beta) == 1
        Scalar spos = plusMinus * mat(K_, J_) + minusPlus * mat(J_, K_); // 2*sin(alpha + gamma)
        Scalar cpos = mat(J_, J_) + mat(K_, K_);                         // 2*cos(alpha + gamma)
        res[0] = atan2(spos, cpos);
        res[2] = 0;
      }
      else {// sin(beta) == 0 and cos(beta) == -1
        Scalar sneg = plusMinus * mat(K_, J_) + plusMinus * mat(J_, K_); // 2*sin(alpha - gamma)
        Scalar cneg = mat(J_, J_) - mat(K_, K_);                         // 2*cos(alpha - gamma)
        res[0] = atan2(sneg, cneg);
        res[2] = 0;
      }
    }
    
    template<typename Scalar>
    static void CalcEulerAngles(
      EulerAngles<Scalar, EulerSystem>& res,
      const typename EulerAngles<Scalar, EulerSystem>::Matrix3& mat)
    {
      CalcEulerAngles_imp(
        res.angles(), mat,
        std::conditional_t<IsTaitBryan, internal::true_type, internal::false_type>());
 
      if (IsAlphaOpposite)
        res.alpha() = -res.alpha();
        
      if (IsBetaOpposite)
        res.beta() = -res.beta();
        
      if (IsGammaOpposite)
        res.gamma() = -res.gamma();
    }
    
    template <typename Scalar_, class _System>
    friend class Eigen::EulerAngles;
    
    template<typename System,
            typename Other,
            int OtherRows,
            int OtherCols>
    friend struct internal::eulerangles_assign_impl;
  };
 
#define EIGEN_EULER_SYSTEM_TYPEDEF(A, B, C) \
 \
  typedef EulerSystem<EULER_##A, EULER_##B, EULER_##C> EulerSystem##A##B##C;
  
  EIGEN_EULER_SYSTEM_TYPEDEF(X,Y,Z)
  EIGEN_EULER_SYSTEM_TYPEDEF(X,Y,X)
  EIGEN_EULER_SYSTEM_TYPEDEF(X,Z,Y)
  EIGEN_EULER_SYSTEM_TYPEDEF(X,Z,X)
  
  EIGEN_EULER_SYSTEM_TYPEDEF(Y,Z,X)
  EIGEN_EULER_SYSTEM_TYPEDEF(Y,Z,Y)
  EIGEN_EULER_SYSTEM_TYPEDEF(Y,X,Z)
  EIGEN_EULER_SYSTEM_TYPEDEF(Y,X,Y)
  
  EIGEN_EULER_SYSTEM_TYPEDEF(Z,X,Y)
  EIGEN_EULER_SYSTEM_TYPEDEF(Z,X,Z)
  EIGEN_EULER_SYSTEM_TYPEDEF(Z,Y,X)
  EIGEN_EULER_SYSTEM_TYPEDEF(Z,Y,Z)
}
 
#endif // EIGEN_EULERSYSTEM_H