Initial fork of unsupported module EulerAngles.

7 files changed, 453 insertions, 0 deletions

diff --git a/unsupported/Eigen/EulerAngles b/unsupported/Eigen/EulerAngles
new file mode 100644
index 000000000..a1aa5d829
--- /dev/null
+++ b/unsupported/Eigen/EulerAngles
@@ -0,0 +1,32 @@
+// 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_EULERANGLES_MODULE_H
+#define EIGEN_EULERANGLES_MODULE_H
+
+
+#include "Eigen/Core"
+#include "Eigen/Geometry"
+
+#include "Eigen/src/Core/util/DisableStupidWarnings.h"
+
+/**
+ *  \defgroup EulerAngles_Module EulerAngles module
+ *
+ *
+ *
+ *
+ */
+
+#include "src/EulerAngles/EulerSystem.h"
+#include "src/EulerAngles/EulerAngles.h"
+
+#include "Eigen/src/Core/util/ReenableStupidWarnings.h"
+
+#endif // EIGEN_EULERANGLES_MODULE_H
diff --git a/unsupported/Eigen/src/CMakeLists.txt b/unsupported/Eigen/src/CMakeLists.txt
index 8eb2808e3..fae1c5854 100644
--- a/unsupported/Eigen/src/CMakeLists.txt
+++ b/unsupported/Eigen/src/CMakeLists.txt
@@ -12,3 +12,4 @@ ADD_SUBDIRECTORY(Skyline)
 ADD_SUBDIRECTORY(SparseExtra)
 ADD_SUBDIRECTORY(KroneckerProduct)
 ADD_SUBDIRECTORY(Splines)
+ADD_SUBDIRECTORY(EulerAngles)
diff --git a/unsupported/Eigen/src/EulerAngles/CMakeLists.txt b/unsupported/Eigen/src/EulerAngles/CMakeLists.txt
new file mode 100644
index 000000000..7986afc5e
--- /dev/null
+++ b/unsupported/Eigen/src/EulerAngles/CMakeLists.txt
@@ -0,0 +1,6 @@
+FILE(GLOB Eigen_IterativeSolvers_SRCS "*.h")
+
+INSTALL(FILES
+  ${Eigen_IterativeSolvers_SRCS}
+  DESTINATION ${INCLUDE_INSTALL_DIR}/unsupported/Eigen/src/IterativeSolvers COMPONENT Devel
+  )
diff --git a/unsupported/Eigen/src/EulerAngles/EulerAngles.h b/unsupported/Eigen/src/EulerAngles/EulerAngles.h
new file mode 100644
index 000000000..b3bd66441
--- /dev/null
+++ b/unsupported/Eigen/src/EulerAngles/EulerAngles.h
@@ -0,0 +1,183 @@
+// 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
+
+namespace Eigen
+{
+  /*template<typename Other,
+         int OtherRows=Other::RowsAtCompileTime,
+         int OtherCols=Other::ColsAtCompileTime>
+  struct ei_eulerangles_assign_impl;*/
+
+  /** \class EulerAngles
+    *
+    * \brief Represents a rotation in a 3 dimensional space as three Euler angles
+    *
+    * \param _Scalar the scalar type, i.e., the type of the angles.
+    *
+    * \sa cl
+    */
+  template <typename _Scalar, class _System>
+  class EulerAngles : public RotationBase<EulerAngles<_Scalar, _System>, 3>
+  {
+    public:
+      /** the scalar type of the coefficients */
+      typedef _Scalar Scalar;
+      typedef _System System;
+    
+      typedef Matrix<Scalar,3,3> Matrix3;
+      typedef Matrix<Scalar,3,1> Vector3;
+      typedef Quaternion<Scalar> QuaternionType;
+      typedef AngleAxis<Scalar> AngleAxisType;
+
+    protected:
+
+      Vector3 m_angles;
+
+    public:
+
+      EulerAngles() {}
+      inline EulerAngles(Scalar a0, Scalar a1, Scalar a2) : m_angles(a0, a1, a2) {}
+      inline EulerAngles(Vector3 angles) : m_angles(angles) {}
+      inline EulerAngles(const QuaternionType& q) { *this = q; }
+      inline EulerAngles(const AngleAxisType& aa) { *this = aa; }
+      template<typename Derived>
+      inline EulerAngles(const MatrixBase<Derived>& m) { *this = m; }
+
+      // TODO: Support assignment from euler to euler
+
+      Scalar angle(int i) const { return m_angles.coeff(i); }
+      Scalar& angle(int i) { return m_angles.coeffRef(i); }
+
+      const Vector3& coeffs() const { return m_angles; }
+      Vector3& coeffs() { return m_angles; }
+
+      // TODO: Add set/get functions
+
+      Scalar h() const { return m_angles[0]; }
+      Scalar& h() { return m_angles[0]; }
+
+      Scalar p() const { return m_angles[1]; }
+      Scalar& p() { return m_angles[1]; }
+
+      Scalar r() const { return m_angles[2]; }
+      Scalar& r() { return m_angles[2]; }
+
+      EulerAngles invert() const
+      {
+        //m_angles = -m_angles;// I want to do this but there could be an aliasing issue!
+        m_angles *= -1;
+        
+        return *this;
+      }
+
+      EulerAngles inverse() const
+      {
+        EulerAngles res;
+        res.m_angles = -m_angles;
+        return res;
+      }
+
+      EulerAngles operator -() const
+      {
+        return inverse();
+      }
+      
+      /** Constructs and \returns an equivalent 3x3 rotation matrix.
+        */
+      template<typename Derived>
+      // TODO: Add booleans which let the user control desired output angles range( (-PI, PI) or [0, 2*PI) )
+      EulerAngles& fromRotationMatrix(const MatrixBase<Derived>& m)
+      {
+        System::eulerAngles(*this, m);
+        return *this;
+      }
+      
+      /** Set \c *this from a rotation matrix(i.e. pure orthogonal matrix with determinent of +1).
+        */
+      template<typename Derived>
+      EulerAngles& operator=(const MatrixBase<Derived>& mat){
+        return fromRotationMatrix(mat);
+      }
+
+      // TODO: Assign and construct from another EulerAngle (with different system)
+      
+      /** Set \c *this from a quaternion.
+        * The axis is normalized.
+        */
+      EulerAngles& operator=(const QuaternionType& q){
+        // TODO: Implement it in a better way
+        // 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.
+        // Currently we compute all matrix cells from quaternion.
+
+        fromRotationMatrix(q.toRotationMatrix());
+
+        // 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)));
+        */
+        
+        return *this;
+      }
+      
+      /** Set \c *this from AngleAxis \a ea.
+       */
+      EulerAngles& operator=(const AngleAxisType& ea)
+      {
+        // TODO: Implement it in a better way
+        return *this = ea.toRotationMatrix();
+      }
+
+      // TODO: Fix this function, and make it generic
+      Matrix3 toRotationMatrix(void) const
+      {
+        return static_cast<QuaternionType>(*this).toRotationMatrix();
+      }
+    
+      operator QuaternionType() const
+      {
+        return
+          AngleAxisType((System::IsHeadingOpposite ? -1 : 1) * h(), Vector3::Unit(System::HeadingAxisAbs - 1)) *
+          AngleAxisType((System::IsPitchOpposite ? -1 : 1) * p(), Vector3::Unit(System::PitchAxisAbs - 1)) *
+          AngleAxisType((System::IsRollOpposite ? -1 : 1) * r(), Vector3::Unit(System::RollAxisAbs - 1));
+      }
+  };
+
+  typedef EulerAngles<double, EulerSystemXYZ> EulerAnglesXYZd;
+  typedef EulerAngles<double, EulerSystemXYX> EulerAnglesXYXd;
+  typedef EulerAngles<double, EulerSystemXZY> EulerAnglesXZYd;
+  typedef EulerAngles<double, EulerSystemXZX> EulerAnglesXZXd;
+
+  typedef EulerAngles<double, EulerSystemYZX> EulerAnglesYZXd;
+  typedef EulerAngles<double, EulerSystemYZY> EulerAnglesYZYd;
+  typedef EulerAngles<double, EulerSystemYXZ> EulerAnglesYXZd;
+  typedef EulerAngles<double, EulerSystemYXY> EulerAnglesYXYd;
+
+  typedef EulerAngles<double, EulerSystemZXY> EulerAnglesZXYd;
+  typedef EulerAngles<double, EulerSystemZXZ> EulerAnglesZXZd;
+  typedef EulerAngles<double, EulerSystemZYX> EulerAnglesZYXd;
+  typedef EulerAngles<double, EulerSystemZYZ> EulerAnglesZYZd;
+
+  namespace internal
+  {
+    template<typename _Scalar, class _System>
+    struct traits<EulerAngles<_Scalar, _System> >
+    {
+      typedef _Scalar Scalar;
+    };
+  }
+  
+}
+
+#endif // EIGEN_EULERANGLESCLASS_H
diff --git a/unsupported/Eigen/src/EulerAngles/EulerSystem.h b/unsupported/Eigen/src/EulerAngles/EulerSystem.h
new file mode 100644
index 000000000..fc782e914
--- /dev/null
+++ b/unsupported/Eigen/src/EulerAngles/EulerSystem.h
@@ -0,0 +1,211 @@
+// 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
+
+namespace Eigen
+{
+  // Forward declerations
+  template <typename _Scalar, class _System>
+  class EulerAngles;
+  
+  namespace internal
+  {
+    // TODO: Check if already exists on the rest API
+    template <int Num, bool IsPossitive = (Num > 0)>
+    struct Abs
+    {
+      enum { value = Num };
+    };
+  
+    template <int Num>
+    struct Abs<Num, false>
+    {
+      enum { value = -Num };
+    };
+  
+    template <bool Cond>
+    struct NegateIf
+    {
+      template <typename T>
+      static void run(T& t)
+      {
+        t = -t;
+      }
+    };
+  
+    template <>
+    struct NegateIf<false>
+    {
+      template <typename T>
+      static void run(T& t)
+      {
+        // no op
+      }
+    };
+  
+    template <bool Cond1, bool Cond2>
+    struct NegateIfXor : NegateIf<Cond1 != Cond2> {};
+
+    template <int Axis>
+    struct IsValidAxis
+    {
+      enum { value = Axis != 0 && Abs<Axis>::value <= 3 };
+    };
+  }
+  
+  enum EulerAxis
+  {
+    EULER_X = 1,
+    EULER_Y = 2,
+    EULER_Z = 3
+  };
+
+  template <int _HeadingAxis, int _PitchAxis, int _RollAxis>
+  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 const int HeadingAxis = _HeadingAxis;
+    static const int PitchAxis = _PitchAxis;
+    static const int RollAxis = _RollAxis;
+
+    enum
+    {
+      HeadingAxisAbs = internal::Abs<HeadingAxis>::value,
+      PitchAxisAbs = internal::Abs<PitchAxis>::value,
+      RollAxisAbs = internal::Abs<RollAxis>::value,
+      
+      IsHeadingOpposite = (HeadingAxis < 0) ? 1 : 0,
+      IsPitchOpposite = (PitchAxis < 0) ? 1 : 0,
+      IsRollOpposite = (RollAxis < 0) ? 1 : 0,
+      
+      IsOdd = ((HeadingAxisAbs)%3 == (PitchAxisAbs - 1)%3) ? 0 : 1,
+      IsEven = IsOdd ? 0 : 1,
+      
+      // TODO: Assert this, and sort it in a better way
+      IsValid = ((unsigned)HeadingAxisAbs != (unsigned)PitchAxisAbs &&
+        (unsigned)PitchAxisAbs != (unsigned)RollAxisAbs &&
+        internal::IsValidAxis<HeadingAxis>::value && internal::IsValidAxis<PitchAxis>::value && internal::IsValidAxis<RollAxis>::value) ? 1 : 0,
+
+      // TODO: After a proper assertation, remove the "IsValid" from this expression
+      IsTaitBryan = (IsValid && (unsigned)HeadingAxisAbs != (unsigned)RollAxisAbs) ? 1 : 0
+    };
+
+    private:
+
+    enum
+    {
+      // 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 = HeadingAxisAbs - 1,
+      J = (HeadingAxisAbs - 1 + 1 + IsOdd)%3,
+      K = (HeadingAxisAbs - 1 + 2 - IsOdd)%3
+    };
+    
+    template <typename Derived>
+    static void eulerAngles_imp(Matrix<typename MatrixBase<Derived>::Scalar, 3, 1>& res, const MatrixBase<Derived>& mat, internal::true_type isTaitBryan)
+    {
+      using std::atan2;
+      using std::sin;
+      using std::cos;
+      
+      typedef typename Derived::Scalar Scalar;
+      typedef Matrix<Scalar,2,1> Vector2;
+      
+      res[0] = atan2(mat(J,K), mat(K,K));
+      Scalar c2 = Vector2(mat(I,I), mat(I,J)).norm();
+      if((IsOdd && res[0]<Scalar(0)) || ((!IsOdd) && res[0]>Scalar(0))) {
+        res[0] = (res[0] > Scalar(0)) ? res[0] - Scalar(M_PI) : res[0] + Scalar(M_PI);
+        res[1] = atan2(-mat(I,K), -c2);
+      }
+      else
+        res[1] = atan2(-mat(I,K), c2);
+      Scalar s1 = sin(res[0]);
+      Scalar c1 = cos(res[0]);
+      res[2] = atan2(s1*mat(K,I)-c1*mat(J,I), c1*mat(J,J) - s1 * mat(K,J));
+    }
+
+    template <typename Derived>
+    static void eulerAngles_imp(Matrix<typename MatrixBase<Derived>::Scalar,3,1>& res, const MatrixBase<Derived>& mat, internal::false_type isTaitBryan)
+    {
+      using std::atan2;
+      using std::sin;
+      using std::cos;
+
+      typedef typename Derived::Scalar Scalar;
+      typedef Matrix<Scalar,2,1> Vector2;
+      
+      res[0] = atan2(mat(J,I), mat(K,I));
+      if((IsOdd && res[0]<Scalar(0)) || ((!IsOdd) && res[0]>Scalar(0)))
+      {
+        res[0] = (res[0] > Scalar(0)) ? res[0] - Scalar(M_PI) : res[0] + Scalar(M_PI);
+        Scalar s2 = Vector2(mat(J,I), mat(K,I)).norm();
+        res[1] = -atan2(s2, mat(I,I));
+      }
+      else
+      {
+        Scalar s2 = Vector2(mat(J,I), mat(K,I)).norm();
+        res[1] = atan2(s2, mat(I,I));
+      }
+
+      // With a=(0,1,0), we have i=0; j=1; k=2, and after computing the first two angles,
+      // we can compute their respective rotation, and apply its inverse to M. Since the result must
+      // be a rotation around x, we have:
+      //
+      //  c2  s1.s2 c1.s2                   1  0   0 
+      //  0   c1    -s1       *    M    =   0  c3  s3
+      //  -s2 s1.c2 c1.c2                   0 -s3  c3
+      //
+      //  Thus:  m11.c1 - m21.s1 = c3  &   m12.c1 - m22.s1 = s3
+
+      Scalar s1 = sin(res[0]);
+      Scalar c1 = cos(res[0]);
+      res[2] = atan2(c1*mat(J,K)-s1*mat(K,K), c1*mat(J,J) - s1 * mat(K,J));
+    }
+
+    public:
+
+    // TODO: Support headingAxisVector(), ..
+    
+    template<typename Scalar>
+    static void eulerAngles(EulerAngles<Scalar, EulerSystem>& res, const typename EulerAngles<Scalar, EulerSystem>::Matrix3& mat)
+    {
+      eulerAngles_imp(
+        res.coeffs(), mat,
+        typename internal::conditional<IsTaitBryan, internal::true_type, internal::false_type>::type());
+
+      internal::NegateIfXor<IsHeadingOpposite, IsEven>::run(res.h());
+
+      internal::NegateIfXor<IsPitchOpposite, IsEven>::run(res.p());
+
+      internal::NegateIfXor<IsRollOpposite, IsEven>::run(res.r());
+    }
+  };
+
+  typedef EulerSystem<EULER_X, EULER_Y, EULER_Z> EulerSystemXYZ;
+  typedef EulerSystem<EULER_X, EULER_Y, EULER_X> EulerSystemXYX;
+  typedef EulerSystem<EULER_X, EULER_Z, EULER_Y> EulerSystemXZY;
+  typedef EulerSystem<EULER_X, EULER_Z, EULER_X> EulerSystemXZX;
+
+  typedef EulerSystem<EULER_Y, EULER_Z, EULER_X> EulerSystemYZX;
+  typedef EulerSystem<EULER_Y, EULER_Z, EULER_Y> EulerSystemYZY;
+  typedef EulerSystem<EULER_Y, EULER_X, EULER_Z> EulerSystemYXZ;
+  typedef EulerSystem<EULER_Y, EULER_X, EULER_Y> EulerSystemYXY;
+
+  typedef EulerSystem<EULER_Z, EULER_X, EULER_Y> EulerSystemZXY;
+  typedef EulerSystem<EULER_Z, EULER_X, EULER_Z> EulerSystemZXZ;
+  typedef EulerSystem<EULER_Z, EULER_Y, EULER_X> EulerSystemZYX;
+  typedef EulerSystem<EULER_Z, EULER_Y, EULER_Z> EulerSystemZYZ;
+}
+
+#endif // EIGEN_EULERSYSTEM_H
diff --git a/unsupported/test/CMakeLists.txt b/unsupported/test/CMakeLists.txt
index 79e70ced4..653392e40 100644
--- a/unsupported/test/CMakeLists.txt
+++ b/unsupported/test/CMakeLists.txt
@@ -45,6 +45,8 @@ ei_add_test(alignedvector3)
 
 ei_add_test(FFT)
 
+ei_add_test(EulerAngles)
+
 find_package(MPFR 2.3.0)
 find_package(GMP)
 if(MPFR_FOUND)
diff --git a/unsupported/test/EulerAngles.cpp b/unsupported/test/EulerAngles.cpp
new file mode 100644
index 000000000..d03db1ac3
--- /dev/null
+++ b/unsupported/test/EulerAngles.cpp
@@ -0,0 +1,18 @@
+// 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/.
+
+#include "main.h"
+
+#include <unsupported/Eigen/EulerAngles>
+
+void test_EulerAngles()
+{
+  //CALL_SUBTEST( test_return_by_value(32) );
+  
+}