From 014d9f1d9b60206deaeb7ac5349816cb556fb35b Mon Sep 17 00:00:00 2001
From: Hongkai Dai <daihongkai@gmail.com>
Date: Thu, 13 Oct 2016 14:45:51 -0700
Subject: implement euler angles with the right ranges

---
 unsupported/test/EulerAngles.cpp | 77 ++++++++++++++++++----------------------
 1 file changed, 35 insertions(+), 42 deletions(-)

(limited to 'unsupported/test/EulerAngles.cpp')

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)
-- 
cgit v1.2.3