implement euler angles with the right ranges

3 files changed, 113 insertions, 127 deletions

diff --git a/unsupported/Eigen/src/EulerAngles/EulerAngles.h b/unsupported/Eigen/src/EulerAngles/EulerAngles.h
index 13a0da1ab..a737a221a 100644
--- a/unsupported/Eigen/src/EulerAngles/EulerAngles.h
+++ b/unsupported/Eigen/src/EulerAngles/EulerAngles.h
@@ -79,8 +79,8 @@ namespace Eigen
     *
     * ##### run-time time ranges #####
     * Run-time ranges are also supported.
-    * \sa EulerAngles(const MatrixBase<Derived>&, bool, bool, bool)
-    * \sa EulerAngles(const RotationBase<Derived, 3>&, bool, bool, bool)
+    * \sa EulerAngles(const MatrixBase<Derived>&, bool, bool)
+    * \sa EulerAngles(const RotationBase<Derived, 3>&, bool, bool)
     *
     * ### Convenient user typedefs ###
     *
@@ -160,22 +160,24 @@ namespace Eigen
       /** Constructs and initialize Euler angles from a 3x3 rotation matrix \p m,
         *  with options to choose for each angle the requested range.
         *
-        * If positive range is true, then the specified angle will be in the range [0, +2*PI].
+        * For angle alpha and gamma, if positive range is true, then the
+        * specified angle will be in the range [0, +2*PI].
         * Otherwise, the specified angle will be in the range [-PI, +PI].
+        * For angle beta, depending on whether AlphaAxis is the same as GammaAxis
+        * if AlphaAxis is the same as Gamma ais, then the range of beta is [0, PI];
+        * otherwise the range of beta is [-PI/2, PI/2]
         *
         * \param m The 3x3 rotation matrix to convert
         * \param positiveRangeAlpha If true, alpha will be in [0, 2*PI]. Otherwise, in [-PI, +PI].
-        * \param positiveRangeBeta If true, beta will be in [0, 2*PI]. Otherwise, in [-PI, +PI].
         * \param positiveRangeGamma If true, gamma will be in [0, 2*PI]. Otherwise, in [-PI, +PI].
       */
       template<typename Derived>
       EulerAngles(
         const MatrixBase<Derived>& m,
         bool positiveRangeAlpha,
-        bool positiveRangeBeta,
         bool positiveRangeGamma) {
         
-        System::CalcEulerAngles(*this, m, positiveRangeAlpha, positiveRangeBeta, positiveRangeGamma);
+        System::CalcEulerAngles(*this, m, positiveRangeAlpha, positiveRangeGamma);
       }
       
       /** Constructs and initialize Euler angles from a rotation \p rot.
@@ -195,17 +197,15 @@ namespace Eigen
         *
         * \param rot The 3x3 rotation matrix to convert
         * \param positiveRangeAlpha If true, alpha will be in [0, 2*PI]. Otherwise, in [-PI, +PI].
-        * \param positiveRangeBeta If true, beta will be in [0, 2*PI]. Otherwise, in [-PI, +PI].
         * \param positiveRangeGamma If true, gamma will be in [0, 2*PI]. Otherwise, in [-PI, +PI].
       */
       template<typename Derived>
       EulerAngles(
         const RotationBase<Derived, 3>& rot,
         bool positiveRangeAlpha,
-        bool positiveRangeBeta,
         bool positiveRangeGamma) {
         
-        System::CalcEulerAngles(*this, rot.toRotationMatrix(), positiveRangeAlpha, positiveRangeBeta, positiveRangeGamma);
+        System::CalcEulerAngles(*this, rot.toRotationMatrix(), positiveRangeAlpha, positiveRangeGamma);
       }
 
       /** \returns The angle values stored in a vector (alpha, beta, gamma). */
@@ -254,12 +254,10 @@ namespace Eigen
         *
         * \param m The 3x3 rotation matrix to convert
         * \tparam positiveRangeAlpha If true, alpha will be in [0, 2*PI]. Otherwise, in [-PI, +PI].
-        * \tparam positiveRangeBeta If true, beta will be in [0, 2*PI]. Otherwise, in [-PI, +PI].
         * \tparam positiveRangeGamma If true, gamma will be in [0, 2*PI]. Otherwise, in [-PI, +PI].
         */
       template<
         bool PositiveRangeAlpha,
-        bool PositiveRangeBeta,
         bool PositiveRangeGamma,
         typename Derived>
       static EulerAngles FromRotation(const MatrixBase<Derived>& m)
@@ -268,7 +266,7 @@ namespace Eigen
         
         EulerAngles e;
         System::template CalcEulerAngles<
-          PositiveRangeAlpha, PositiveRangeBeta, PositiveRangeGamma, _Scalar>(e, m);
+          PositiveRangeAlpha, PositiveRangeGamma, _Scalar>(e, m);
         return e;
       }
       
@@ -280,17 +278,15 @@ namespace Eigen
         *
         * \param rot The 3x3 rotation matrix to convert
         * \tparam positiveRangeAlpha If true, alpha will be in [0, 2*PI]. Otherwise, in [-PI, +PI].
-        * \tparam positiveRangeBeta If true, beta will be in [0, 2*PI]. Otherwise, in [-PI, +PI].
         * \tparam positiveRangeGamma If true, gamma will be in [0, 2*PI]. Otherwise, in [-PI, +PI].
       */
       template<
         bool PositiveRangeAlpha,
-        bool PositiveRangeBeta,
         bool PositiveRangeGamma,
         typename Derived>
       static EulerAngles FromRotation(const RotationBase<Derived, 3>& rot)
       {
-        return FromRotation<PositiveRangeAlpha, PositiveRangeBeta, PositiveRangeGamma>(rot.toRotationMatrix());
+        return FromRotation<PositiveRangeAlpha, PositiveRangeGamma>(rot.toRotationMatrix());
       }
       
       /*EulerAngles& fromQuaternion(const QuaternionType& q)
diff --git a/unsupported/Eigen/src/EulerAngles/EulerSystem.h b/unsupported/Eigen/src/EulerAngles/EulerSystem.h
index 98f9f647d..76d0b7c57 100644
--- a/unsupported/Eigen/src/EulerAngles/EulerSystem.h
+++ b/unsupported/Eigen/src/EulerAngles/EulerSystem.h
@@ -112,9 +112,9 @@ namespace Eigen
     *
     * \tparam _AlphaAxis the first fixed EulerAxis
     *
-    * \tparam _AlphaAxis the second fixed EulerAxis
+    * \tparam _BetaAxis the second fixed EulerAxis
     *
-    * \tparam _AlphaAxis the third fixed EulerAxis
+    * \tparam _GammaAxis the third fixed EulerAxis
     */
   template <int _AlphaAxis, int _BetaAxis, int _GammaAxis>
   class EulerSystem
@@ -138,14 +138,16 @@ namespace Eigen
       BetaAxisAbs = internal::Abs<BetaAxis>::value, /*!< the second rotation axis unsigned */
       GammaAxisAbs = internal::Abs<GammaAxis>::value, /*!< the third rotation axis unsigned */
       
-      IsAlphaOpposite = (AlphaAxis < 0) ? 1 : 0, /*!< weather alpha axis is negative */
-      IsBetaOpposite = (BetaAxis < 0) ? 1 : 0, /*!< weather beta axis is negative */
-      IsGammaOpposite = (GammaAxis < 0) ? 1 : 0, /*!< weather gamma axis is negative */
-      
-      IsOdd = ((AlphaAxisAbs)%3 == (BetaAxisAbs - 1)%3) ? 0 : 1, /*!< weather the Euler system is odd */
-      IsEven = IsOdd ? 0 : 1, /*!< weather the Euler system is even */
+      IsAlphaOpposite = (AlphaAxis < 0) ? 1 : 0, /*!< whether alpha axis is negative */
+      IsBetaOpposite = (BetaAxis < 0) ? 1 : 0, /*!< whether beta axis is negative */
+      IsGammaOpposite = (GammaAxis < 0) ? 1 : 0, /*!< whether gamma axis is negative */
+
+      // 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, /*!< whether the Euler system is odd */
+      IsEven = IsOdd ? 0 : 1, /*!< whether the Euler system is even */
 
-      IsTaitBryan = ((unsigned)AlphaAxisAbs != (unsigned)GammaAxisAbs) ? 1 : 0 /*!< weather the Euler system is tait bryan */
+      IsTaitBryan = ((unsigned)AlphaAxisAbs != (unsigned)GammaAxisAbs) ? 1 : 0 /*!< whether the Euler system is tait bryan */
     };
     
     private:
@@ -180,71 +182,70 @@ namespace Eigen
     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::sin;
-      using std::cos;
+      using std::sqrt;
       
       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))) {
-        if(res[0] > Scalar(0)) {
-          res[0] -= Scalar(EIGEN_PI);
-        }
-        else {
-          res[0] += Scalar(EIGEN_PI);
-        }
-        res[1] = atan2(-mat(I,K), -c2);
+
+      Scalar plusMinus = IsEven? 1 : -1;
+      Scalar minusPlus = IsOdd?  1 : -1;
+
+      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()) {
+        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) {
+        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 {
+        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;
       }
-      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 CalcEulerAngles_imp(Matrix<typename MatrixBase<Derived>::Scalar,3,1>& res, const MatrixBase<Derived>& mat, internal::false_type /*isTaitBryan*/)
+    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::sin;
-      using std::cos;
+      using std::sqrt;
 
       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)))
-      {
-        if(res[0] > Scalar(0)) {
-          res[0] -= Scalar(EIGEN_PI);
-        }
-        else {
-          res[0] += Scalar(EIGEN_PI);
-        }
-        Scalar s2 = Vector2(mat(J,I), mat(K,I)).norm();
-        res[1] = -atan2(s2, mat(I,I));
+
+      Scalar plusMinus = IsEven? 1 : -1;
+      Scalar minusPlus = IsOdd?  1 : -1;
+
+      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));
+
+      if(Rsum > 4 * NumTraits<Scalar>::epsilon()) {
+        res[0] = atan2(mat(J, I), minusPlus * mat(K, I));
+        res[2] = atan2(mat(I, J), plusMinus * mat(I, K));
       }
-      else
-      {
-        Scalar s2 = Vector2(mat(J,I), mat(K,I)).norm();
-        res[1] = atan2(s2, mat(I,I));
+      else if( mat(I, I) > 0) {
+        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 {
+        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[1] = 0;
       }
 
-      // 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));
     }
     
     template<typename Scalar>
@@ -257,14 +258,13 @@ namespace Eigen
     
     template<
       bool PositiveRangeAlpha,
-      bool PositiveRangeBeta,
       bool PositiveRangeGamma,
       typename Scalar>
     static void CalcEulerAngles(
       EulerAngles<Scalar, EulerSystem>& res,
       const typename EulerAngles<Scalar, EulerSystem>::Matrix3& mat)
     {
-      CalcEulerAngles(res, mat, PositiveRangeAlpha, PositiveRangeBeta, PositiveRangeGamma);
+      CalcEulerAngles(res, mat, PositiveRangeAlpha, PositiveRangeGamma);
     }
     
     template<typename Scalar>
@@ -272,28 +272,25 @@ namespace Eigen
       EulerAngles<Scalar, EulerSystem>& res,
       const typename EulerAngles<Scalar, EulerSystem>::Matrix3& mat,
       bool PositiveRangeAlpha,
-      bool PositiveRangeBeta,
       bool PositiveRangeGamma)
     {
       CalcEulerAngles_imp(
         res.angles(), mat,
         typename internal::conditional<IsTaitBryan, internal::true_type, internal::false_type>::type());
 
-      if (IsAlphaOpposite == IsOdd)
+      if (IsAlphaOpposite)
         res.alpha() = -res.alpha();
         
-      if (IsBetaOpposite == IsOdd)
+      if (IsBetaOpposite)
         res.beta() = -res.beta();
         
-      if (IsGammaOpposite == IsOdd)
+      if (IsGammaOpposite)
         res.gamma() = -res.gamma();
       
       // Saturate results to the requested range
       if (PositiveRangeAlpha && (res.alpha() < 0))
         res.alpha() += Scalar(2 * EIGEN_PI);
-      
-      if (PositiveRangeBeta && (res.beta() < 0))
-        res.beta() += Scalar(2 * EIGEN_PI);
+
       
       if (PositiveRangeGamma && (res.gamma() < 0))
         res.gamma() += Scalar(2 * EIGEN_PI);
diff --git a/unsupported/test/EulerAngles.cpp b/unsupported/test/EulerAngles.cpp
index a8cb52864..4d0831dc2 100644
--- a/unsupported/test/EulerAngles.cpp
+++ b/unsupported/test/EulerAngles.cpp
@@ -15,7 +15,7 @@ using namespace Eigen;
 
 template<typename EulerSystem, typename Scalar>
 void verify_euler_ranged(const Matrix<Scalar,3,1>& ea,
-  bool positiveRangeAlpha, bool positiveRangeBeta, bool positiveRangeGamma)
+  bool positiveRangeAlpha, bool positiveRangeGamma)
 {
   typedef EulerAngles<Scalar, EulerSystem> EulerAnglesType;
   typedef Matrix<Scalar,3,3> Matrix3;
@@ -39,10 +39,10 @@ void verify_euler_ranged(const Matrix<Scalar,3,1>& ea,
     alphaRangeEnd = Scalar(EIGEN_PI);
   }
   
-  if (positiveRangeBeta)
+  if (EulerSystem::IsTaitBryan)
   {
-    betaRangeStart = Scalar(0);
-    betaRangeEnd = Scalar(2 * EIGEN_PI);
+    betaRangeStart = -Scalar(EIGEN_PI / 2);
+    betaRangeEnd = Scalar(EIGEN_PI / 2);
   }
   else
   {
@@ -61,77 +61,70 @@ void verify_euler_ranged(const Matrix<Scalar,3,1>& ea,
     gammaRangeEnd = Scalar(EIGEN_PI);
   }
   
-  const int i = EulerSystem::AlphaAxisAbs - 1;
+  /*const int i = EulerSystem::AlphaAxisAbs - 1;
   const int j = EulerSystem::BetaAxisAbs - 1;
   const int k = EulerSystem::GammaAxisAbs - 1;
   
   const int iFactor = EulerSystem::IsAlphaOpposite ? -1 : 1;
   const int jFactor = EulerSystem::IsBetaOpposite ? -1 : 1;
-  const int kFactor = EulerSystem::IsGammaOpposite ? -1 : 1;
+  const int kFactor = EulerSystem::IsGammaOpposite ? -1 : 1;*/
   
   const Vector3 I = EulerAnglesType::AlphaAxisVector();
   const Vector3 J = EulerAnglesType::BetaAxisVector();
   const Vector3 K = EulerAnglesType::GammaAxisVector();
   
   EulerAnglesType e(ea[0], ea[1], ea[2]);
-  
+
   Matrix3 m(e);
-  Vector3 eabis = EulerAnglesType(m, positiveRangeAlpha, positiveRangeBeta, positiveRangeGamma).angles();
+
+
+  Vector3 eabis = EulerAnglesType(m, positiveRangeAlpha, positiveRangeGamma).angles();
   
   // Check that eabis in range
   VERIFY(alphaRangeStart <= eabis[0] && eabis[0] <= alphaRangeEnd);
   VERIFY(betaRangeStart <= eabis[1] && eabis[1] <= betaRangeEnd);
   VERIFY(gammaRangeStart <= eabis[2] && eabis[2] <= gammaRangeEnd);
-  
-  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 (positiveRangeAlpha && (eabis2[0] < 0))
-    eabis2[0] += Scalar(2 * EIGEN_PI);
-  if (positiveRangeBeta && (eabis2[1] < 0))
-    eabis2[1] += Scalar(2 * EIGEN_PI);
-  if (positiveRangeGamma && (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(AngleAxisType(eabis[0], I) * AngleAxisType(eabis[1], J) * AngleAxisType(eabis[2], K));
   VERIFY_IS_APPROX(m,  mbis);
-  
-  // Tests that are only relevant for no possitive range
-  if (!(positiveRangeAlpha || positiveRangeBeta || positiveRangeGamma))
+
+  // Test if ea and eabis are the same
+  // Need to check both singular and non-singular cases
+  // There are two singular cases.
+  // 1. When I==K and sin(ea(1)) == 0
+  // 2. When I!=K and cos(ea(1)) == 0
+
+  // Tests that are only relevant for no positive range
+  /*if (!(positiveRangeAlpha || positiveRangeGamma))
   {
-    /* 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, 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, positiveRangeAlpha, positiveRangeBeta, positiveRangeGamma).angles();
-  VERIFY_IS_APPROX(eabis, eabis2);// Verify that the euler angles are still the same
+  eabis = EulerAnglesType(q, positiveRangeAlpha, positiveRangeGamma).angles();
+  QuaternionType qbis(AngleAxisType(eabis[0], I) * AngleAxisType(eabis[1], J) * AngleAxisType(eabis[2], K));
+  VERIFY_IS_APPROX(std::abs(q.dot(qbis)), static_cast<Scalar>(1));
+  //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);
+  verify_euler_ranged<EulerSystem>(ea, false, false);
+  verify_euler_ranged<EulerSystem>(ea, false, true);
+  verify_euler_ranged<EulerSystem>(ea, false, false);
+  verify_euler_ranged<EulerSystem>(ea, false, true);
+  verify_euler_ranged<EulerSystem>(ea, true, false);
+  verify_euler_ranged<EulerSystem>(ea, true, true);
+  verify_euler_ranged<EulerSystem>(ea, true, false);
+  verify_euler_ranged<EulerSystem>(ea, true, true);
 }
 
 template<typename Scalar> void check_all_var(const Matrix<Scalar,3,1>& ea)