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

// IWYU pragma: private
#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)

// Specifically-referenced instantiations.
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; }
};
}  // namespace internal
}  // namespace Eigen

#endif  // EIGEN_EULERANGLESCLASS_H