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/.
// SPDX-License-Identifier: MPL-2.0
 
#ifndef EIGEN_EULERSYSTEM_H
#define EIGEN_EULERSYSTEM_H
 
// IWYU pragma: private
#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 : std::integral_constant<int, Num> {};
 
template <int Num>
struct Abs<Num, false> : std::integral_constant<int, -Num> {};
 
template <int Axis>
struct IsValidAxis : bool_constant<Axis != 0 && Abs<Axis>::value <= 3> {};
 
template <typename System, typename Other, int OtherRows = Other::RowsAtCompileTime,
          int OtherCols = Other::ColsAtCompileTime>
struct eulerangles_assign_impl;
}  // namespace internal
 
#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:
  // Defined as static constexpr rather than enum to ensure support for negative values.

  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);
 
  template <typename Scalar>
  static void CalcEulerAngles(EulerAngles<Scalar, EulerSystem>& res,
                              const typename EulerAngles<Scalar, EulerSystem>::Matrix3& mat) {
    res.angles() = mat.canonicalEulerAngles(AlphaAxisAbs - 1, BetaAxisAbs - 1, GammaAxisAbs - 1);
 
    EIGEN_IF_CONSTEXPR (IsAlphaOpposite) res.alpha() = -res.alpha();
 
    EIGEN_IF_CONSTEXPR (IsBetaOpposite) res.beta() = -res.beta();
 
    EIGEN_IF_CONSTEXPR (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)
}  // namespace Eigen
 
#undef EIGEN_EULER_ANGLES_CLASS_STATIC_ASSERT
#undef EIGEN_EULER_SYSTEM_TYPEDEF
 
#endif  // EIGEN_EULERSYSTEM_H