Use RotationBase, test quaternions and support ranges.

3 files changed, 263 insertions, 72 deletions

diff --git a/unsupported/Eigen/src/EulerAngles/EulerAngles.h b/unsupported/Eigen/src/EulerAngles/EulerAngles.h
index ccde28eb6..3362d9c3e 100644
--- a/unsupported/Eigen/src/EulerAngles/EulerAngles.h
+++ b/unsupported/Eigen/src/EulerAngles/EulerAngles.h
@@ -57,10 +57,32 @@ namespace Eigen
 
       EulerAngles() {}
       inline EulerAngles(Scalar a0, Scalar a1, Scalar a2) : m_angles(a0, a1, a2) {}
-      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; }
+      
+      template<typename Derived>
+      inline EulerAngles(
+        const MatrixBase<Derived>& m,
+        bool positiveRangeHeading,
+        bool positiveRangePitch,
+        bool positiveRangeRoll) {
+        
+        fromRotation(m, positiveRangeHeading, positiveRangePitch, positiveRangeRoll);
+      }
+      
+      template<typename Derived>
+      inline EulerAngles(const RotationBase<Derived, 3>& rot) { *this = rot; }
+      
+      template<typename Derived>
+      inline EulerAngles(
+        const RotationBase<Derived, 3>& rot,
+        bool positiveRangeHeading,
+        bool positiveRangePitch,
+        bool positiveRangeRoll) {
+        
+        fromRotation(rot, positiveRangeHeading, positiveRangePitch, positiveRangeRoll);
+      }
 
       // TODO: Support assignment from euler to euler
 
@@ -104,65 +126,108 @@ namespace Eigen
       /** 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)
+      EulerAngles& fromRotation(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<
+        bool PositiveRangeHeading,
+        bool PositiveRangePitch,
+        bool PositiveRangeRoll,
+        typename Derived>
+      EulerAngles& fromRotation(const MatrixBase<Derived>& m)
+      {
+        System::eulerAngles<PositiveRangeHeading, PositiveRangePitch, PositiveRangeRoll>(*this, m);
+        return *this;
+      }
+      
       template<typename Derived>
-      EulerAngles& operator=(const MatrixBase<Derived>& mat){
-        return fromRotationMatrix(mat);
+      EulerAngles& fromRotation(
+        const MatrixBase<Derived>& m,
+        bool positiveRangeHeading,
+        bool positiveRangePitch,
+        bool positiveRangeRoll)
+      {
+        System::eulerAngles(*this, m, positiveRangeHeading, positiveRangePitch, positiveRangeRoll);
+        return *this;
       }
-
-      // 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
+      template<typename Derived>
+      EulerAngles& fromRotation(const RotationBase<Derived, 3>& rot)
+      {
+        return fromRotation(rot.toRotationMatrix());
+      }
+      
+      template<
+        bool PositiveRangeHeading,
+        bool PositiveRangePitch,
+        bool PositiveRangeRoll,
+        typename Derived>
+      EulerAngles& fromRotation(const RotationBase<Derived, 3>& rot)
+      {
+        return fromRotation<PositiveRangeHeading, PositiveRangePitch, PositiveRangeRoll>(rot.toRotationMatrix());
+      }
+      
+      template<typename Derived>
+      EulerAngles& fromRotation(
+        const RotationBase<Derived, 3>& rot,
+        bool positiveRangeHeading,
+        bool positiveRangePitch,
+        bool positiveRangeRoll)
+      {
+        return fromRotation(rot.toRotationMatrix(), positiveRangeHeading, positiveRangePitch, positiveRangeRoll);
+      }
+      
+      /*EulerAngles& fromQuaternion(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.
 
-        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)));
+        //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)));
+      }*/
+      
+      /** Set \c *this from a rotation matrix(i.e. pure orthogonal matrix with determinent of +1).
         */
-        
-        return *this;
+      template<typename Derived>
+      EulerAngles& operator=(const MatrixBase<Derived>& mat) {
+        return fromRotation(mat);
       }
+
+      // TODO: Assign and construct from another EulerAngle (with different system)
       
-      // TODO: Support isApprox function
+      /** Set \c *this from a rotation.
+        */
+      template<typename Derived>
+      EulerAngles& operator=(const RotationBase<Derived, 3>& rot) {
+        return fromRotation(rot.toRotationMatrix());
+      }
       
-      /** Set \c *this from AngleAxis \a ea.
-       */
-      EulerAngles& operator=(const AngleAxisType& ea)
+      // TODO: Support isApprox function
+
+      Matrix3 toRotationMatrix() const
       {
-        // TODO: Implement it in a better way
-        return *this = ea.toRotationMatrix();
+        return static_cast<QuaternionType>(*this).toRotationMatrix();
       }
 
-      // TODO: Fix this function, and make it generic
-      Matrix3 toRotationMatrix(void) const
+      QuaternionType toQuaternion() const
       {
-        return static_cast<QuaternionType>(*this).toRotationMatrix();
+        return
+          AngleAxisType(h(), HeadingAxisVector()) *
+          AngleAxisType(p(), PitchAxisVector()) *
+          AngleAxisType(r(), RollAxisVector());
       }
     
       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));
+        return toQuaternion();
       }
   };
 
diff --git a/unsupported/Eigen/src/EulerAngles/EulerSystem.h b/unsupported/Eigen/src/EulerAngles/EulerSystem.h
index ba33d5400..9699dd10d 100644
--- a/unsupported/Eigen/src/EulerAngles/EulerSystem.h
+++ b/unsupported/Eigen/src/EulerAngles/EulerSystem.h
@@ -19,7 +19,7 @@ namespace Eigen
   namespace internal
   {
     // TODO: Check if already exists on the rest API
-    template <int Num, bool IsPossitive = (Num > 0)>
+    template <int Num, bool IsPositive = (Num > 0)>
     struct Abs
     {
       enum { value = Num };
@@ -73,6 +73,26 @@ namespace Eigen
   
     template <bool Cond1, bool Cond2>
     struct NegateIfXor : NegateIf<Cond1 != Cond2> {};
+    
+    template <typename Type, Type value, bool Cond>
+    struct AddConstIf
+    {
+      template <typename T>
+      static void run(T& t)
+      {
+        t += T(value);
+      }
+    };
+  
+    template <typename Type, Type value>
+    struct AddConstIf<Type, value, false>
+    {
+      template <typename T>
+      static void run(T&)
+      {
+        // no op
+      }
+    };
 
     template <int Axis>
     struct IsValidAxis
@@ -196,17 +216,50 @@ namespace Eigen
     public:
     
     template<typename Scalar>
-    static void eulerAngles(EulerAngles<Scalar, EulerSystem>& res, const typename EulerAngles<Scalar, EulerSystem>::Matrix3& mat)
+    static void eulerAngles(
+      EulerAngles<Scalar, EulerSystem>& res,
+      const typename EulerAngles<Scalar, EulerSystem>::Matrix3& mat)
+    {
+      eulerAngles(res, mat, false, false, false);
+    }
+    
+    template<
+      typename Scalar,
+      bool PositiveRangeHeading,
+      bool PositiveRangePitch,
+      bool PositiveRangeRoll>
+    static void eulerAngles(
+      EulerAngles<Scalar, EulerSystem>& res,
+      const typename EulerAngles<Scalar, EulerSystem>::Matrix3& mat)
+    {
+      eulerAngles(res, mat, PositiveRangeHeading, PositiveRangePitch, PositiveRangeRoll);
+    }
+    
+    template<typename Scalar>
+    static void eulerAngles(
+      EulerAngles<Scalar, EulerSystem>& res,
+      const typename EulerAngles<Scalar, EulerSystem>::Matrix3& mat,
+      bool positiveRangeHeading,
+      bool positiveRangePitch,
+      bool positiveRangeRoll)
     {
       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());
+      
+      // Saturate results to the requested range
+      if (positiveRangeHeading && (res.h() < 0))
+        res.h() += Scalar(2 * EIGEN_PI);
+      
+      if (positiveRangePitch && (res.p() < 0))
+        res.p() += Scalar(2 * EIGEN_PI);
+      
+      if (positiveRangeRoll && (res.r() < 0))
+        res.r() += Scalar(2 * EIGEN_PI);
     }
   };
 
diff --git a/unsupported/test/EulerAngles.cpp b/unsupported/test/EulerAngles.cpp
index 57a34776a..e65399ffa 100644
--- a/unsupported/test/EulerAngles.cpp
+++ b/unsupported/test/EulerAngles.cpp
@@ -14,14 +14,53 @@
 using namespace Eigen;
 
 template<typename EulerSystem, typename Scalar>
-void verify_euler(const Matrix<Scalar,3,1>& ea)
+void verify_euler_ranged(const Matrix<Scalar,3,1>& ea,
+  bool positiveRangeHeading, bool positiveRangePitch, bool positiveRangeRoll)
 {
   typedef EulerAngles<Scalar, EulerSystem> EulerAnglesType;
   typedef Matrix<Scalar,3,3> Matrix3;
   typedef Matrix<Scalar,3,1> Vector3;
-  typedef AngleAxis<Scalar> AngleAxisx;
+  typedef Quaternion<Scalar> QuaternionType;
+  typedef AngleAxis<Scalar> AngleAxisType;
   using std::abs;
   
+  Scalar headingRangeStart, headingRangeEnd;
+  Scalar pitchRangeStart, pitchRangeEnd;
+  Scalar rollRangeStart, rollRangeEnd;
+  
+  if (positiveRangeHeading)
+  {
+    headingRangeStart = Scalar(0);
+    headingRangeEnd = Scalar(2 * EIGEN_PI);
+  }
+  else
+  {
+    headingRangeStart = -Scalar(EIGEN_PI);
+    headingRangeEnd = Scalar(EIGEN_PI);
+  }
+  
+  if (positiveRangePitch)
+  {
+    pitchRangeStart = Scalar(0);
+    pitchRangeEnd = Scalar(2 * EIGEN_PI);
+  }
+  else
+  {
+    pitchRangeStart = -Scalar(EIGEN_PI);
+    pitchRangeEnd = Scalar(EIGEN_PI);
+  }
+  
+  if (positiveRangeRoll)
+  {
+    rollRangeStart = Scalar(0);
+    rollRangeEnd = Scalar(2 * EIGEN_PI);
+  }
+  else
+  {
+    rollRangeStart = -Scalar(EIGEN_PI);
+    rollRangeEnd = Scalar(EIGEN_PI);
+  }
+  
   const int i = EulerSystem::HeadingAxisAbs - 1;
   const int j = EulerSystem::PitchAxisAbs - 1;
   const int k = EulerSystem::RollAxisAbs - 1;
@@ -37,46 +76,80 @@ void verify_euler(const Matrix<Scalar,3,1>& ea)
   EulerAnglesType e(ea[0], ea[1], ea[2]);
   
   Matrix3 m(e);
-  Vector3 eabis = EulerAnglesType(m).coeffs();
+  Vector3 eabis = EulerAnglesType(m, positiveRangeHeading, positiveRangePitch, positiveRangeRoll).coeffs();
+  
+  // Check that eabis in range
+  VERIFY(headingRangeStart <= eabis[0] && eabis[0] <= headingRangeEnd);
+  VERIFY(pitchRangeStart <= eabis[1] && eabis[1] <= pitchRangeEnd);
+  VERIFY(rollRangeStart <= eabis[2] && eabis[2] <= rollRangeEnd);
+  
   Vector3 eabis2 = m.eulerAngles(i, j, k);
+  
+  // Invert the relevant axes
   eabis2[0] *= iFactor;
   eabis2[1] *= jFactor;
   eabis2[2] *= kFactor;
   
+  // Saturate the angles to the correct range
+  if (positiveRangeHeading && (eabis2[0] < 0))
+    eabis2[0] += Scalar(2 * EIGEN_PI);
+  if (positiveRangePitch && (eabis2[1] < 0))
+    eabis2[1] += Scalar(2 * EIGEN_PI);
+  if (positiveRangeRoll && (eabis2[2] < 0))
+    eabis2[2] += Scalar(2 * EIGEN_PI);
+  
   VERIFY_IS_APPROX(eabis, eabis2);// Verify that our estimation is the same as m.eulerAngles() is
   
-  Matrix3 mbis(AngleAxisx(eabis[0], I) * AngleAxisx(eabis[1], J) * AngleAxisx(eabis[2], K));
+  Matrix3 mbis(AngleAxisType(eabis[0], I) * AngleAxisType(eabis[1], J) * AngleAxisType(eabis[2], K));
   VERIFY_IS_APPROX(m,  mbis);
-  /* If I==K, and ea[1]==0, then there no unique solution. */ 
-  /* The remark apply in the case where I!=K, and |ea[1]| is close to pi/2. */ 
-  if( (i!=k || ea[1]!=0) && (i==k || !internal::isApprox(abs(ea[1]),Scalar(EIGEN_PI/2),test_precision<Scalar>())) ) 
-    VERIFY((ea-eabis).norm() <= test_precision<Scalar>());
-  
-  // approx_or_less_than does not work for 0
-  VERIFY(0 < eabis[0] || test_isMuchSmallerThan(eabis[0], Scalar(1)));
-  VERIFY_IS_APPROX_OR_LESS_THAN(eabis[0], Scalar(EIGEN_PI));
-  VERIFY_IS_APPROX_OR_LESS_THAN(-Scalar(EIGEN_PI), eabis[1]);
-  VERIFY_IS_APPROX_OR_LESS_THAN(eabis[1], Scalar(EIGEN_PI));
-  VERIFY_IS_APPROX_OR_LESS_THAN(-Scalar(EIGEN_PI), eabis[2]);
-  VERIFY_IS_APPROX_OR_LESS_THAN(eabis[2], Scalar(EIGEN_PI));
+  
+  // Tests that are only relevant for no possitive range
+  if (!(positiveRangeHeading || positiveRangePitch || positiveRangeRoll))
+  {
+    /* If I==K, and ea[1]==0, then there no unique solution. */ 
+    /* The remark apply in the case where I!=K, and |ea[1]| is close to pi/2. */ 
+    if( (i!=k || ea[1]!=0) && (i==k || !internal::isApprox(abs(ea[1]),Scalar(EIGEN_PI/2),test_precision<Scalar>())) ) 
+      VERIFY((ea-eabis).norm() <= test_precision<Scalar>());
+    
+    // approx_or_less_than does not work for 0
+    VERIFY(0 < eabis[0] || test_isMuchSmallerThan(eabis[0], Scalar(1)));
+  }
+  
+  // Quaternions
+  QuaternionType q(e);
+  eabis = EulerAnglesType(q, positiveRangeHeading, positiveRangePitch, positiveRangeRoll).coeffs();
+  VERIFY_IS_APPROX(eabis, eabis2);// Verify that the euler angles are still the same
+}
+
+template<typename EulerSystem, typename Scalar>
+void verify_euler(const Matrix<Scalar,3,1>& ea)
+{
+  verify_euler_ranged<EulerSystem>(ea, false, false, false);
+  verify_euler_ranged<EulerSystem>(ea, false, false, true);
+  verify_euler_ranged<EulerSystem>(ea, false, true, false);
+  verify_euler_ranged<EulerSystem>(ea, false, true, true);
+  verify_euler_ranged<EulerSystem>(ea, true, false, false);
+  verify_euler_ranged<EulerSystem>(ea, true, false, true);
+  verify_euler_ranged<EulerSystem>(ea, true, true, false);
+  verify_euler_ranged<EulerSystem>(ea, true, true, true);
 }
 
 template<typename Scalar> void check_all_var(const Matrix<Scalar,3,1>& ea)
 {
-  verify_euler<EulerSystemXYZ, Scalar>(ea);
-  verify_euler<EulerSystemXYX, Scalar>(ea);
-  verify_euler<EulerSystemXZY, Scalar>(ea);
-  verify_euler<EulerSystemXZX, Scalar>(ea);
-  
-  verify_euler<EulerSystemYZX, Scalar>(ea);
-  verify_euler<EulerSystemYZY, Scalar>(ea);
-  verify_euler<EulerSystemYXZ, Scalar>(ea);
-  verify_euler<EulerSystemYXY, Scalar>(ea);
-  
-  verify_euler<EulerSystemZXY, Scalar>(ea);
-  verify_euler<EulerSystemZXZ, Scalar>(ea);
-  verify_euler<EulerSystemZYX, Scalar>(ea);
-  verify_euler<EulerSystemZYZ, Scalar>(ea);
+  verify_euler<EulerSystemXYZ>(ea);
+  verify_euler<EulerSystemXYX>(ea);
+  verify_euler<EulerSystemXZY>(ea);
+  verify_euler<EulerSystemXZX>(ea);
+  
+  verify_euler<EulerSystemYZX>(ea);
+  verify_euler<EulerSystemYZY>(ea);
+  verify_euler<EulerSystemYXZ>(ea);
+  verify_euler<EulerSystemYXY>(ea);
+  
+  verify_euler<EulerSystemZXY>(ea);
+  verify_euler<EulerSystemZXZ>(ea);
+  verify_euler<EulerSystemZYX>(ea);
+  verify_euler<EulerSystemZYZ>(ea);
 }
 
 template<typename Scalar> void eulerangles()
@@ -85,11 +158,11 @@ template<typename Scalar> void eulerangles()
   typedef Matrix<Scalar,3,1> Vector3;
   typedef Array<Scalar,3,1> Array3;
   typedef Quaternion<Scalar> Quaternionx;
-  typedef AngleAxis<Scalar> AngleAxisx;
+  typedef AngleAxis<Scalar> AngleAxisType;
 
   Scalar a = internal::random<Scalar>(-Scalar(EIGEN_PI), Scalar(EIGEN_PI));
   Quaternionx q1;
-  q1 = AngleAxisx(a, Vector3::Random().normalized());
+  q1 = AngleAxisType(a, Vector3::Random().normalized());
   Matrix3 m;
   m = q1;