From 6f4f12d1eda685f03a2e4b54b479813d134248cf Mon Sep 17 00:00:00 2001
From: Tal Hadad <tal_hd@hotmail.com>
Date: Mon, 17 Oct 2016 22:23:47 +0300
Subject: [PATCH] Add isApprox() and cast() functions.

test cases included
---
 .../Eigen/src/EulerAngles/EulerAngles.h       | 20 +++++++++++++++-
 unsupported/test/EulerAngles.cpp              | 24 ++++++++++++++++++-
 2 files changed, 42 insertions(+), 2 deletions(-)

diff --git a/unsupported/Eigen/src/EulerAngles/EulerAngles.h b/unsupported/Eigen/src/EulerAngles/EulerAngles.h
index 8a723d9ee..6594e4d13 100644
--- a/unsupported/Eigen/src/EulerAngles/EulerAngles.h
+++ b/unsupported/Eigen/src/EulerAngles/EulerAngles.h
@@ -105,8 +105,11 @@ namespace Eigen
   class EulerAngles : public RotationBase<EulerAngles<_Scalar, _System>, 3>
   {
     public:
+      typedef RotationBase<EulerAngles<_Scalar, _System>, 3> Base;
+      
       /** the scalar type of the angles */
       typedef _Scalar Scalar;
+      typedef typename NumTraits<Scalar>::Real RealScalar;
       
       /** the EulerSystem to use, which represents the axes of rotation. */
       typedef _System System;
@@ -248,7 +251,13 @@ namespace Eigen
         return *this;
       }
       
-      // TODO: Support isApprox function
+      /** \returns \c true if \c *this is approximately equal to \a other, within the precision
+        * determined by \a prec.
+        *
+        * \sa MatrixBase::isApprox() */
+      bool isApprox(const EulerAngles& other,
+        const RealScalar& prec = NumTraits<Scalar>::dummy_precision()) const
+      { return angles().isApprox(other.angles(), prec); }
 
       /** \returns an equivalent 3x3 rotation matrix. */
       Matrix3 toRotationMatrix() const
@@ -271,6 +280,15 @@ namespace Eigen
         s << eulerAngles.angles().transpose();
         return s;
       }
+      
+      /** \returns \c *this with scalar type casted to \a NewScalarType */
+      template <typename NewScalarType>
+      EulerAngles<NewScalarType, System> cast() const
+      {
+        EulerAngles<NewScalarType, System> e;
+        e.angles() = angles().cast<NewScalarType>();
+        return e;
+      }
   };
 
 #define EIGEN_EULER_ANGLES_SINGLE_TYPEDEF(AXES, SCALAR_TYPE, SCALAR_POSTFIX) \
diff --git a/unsupported/test/EulerAngles.cpp b/unsupported/test/EulerAngles.cpp
index 3f4523ccc..149cf7f94 100644
--- a/unsupported/test/EulerAngles.cpp
+++ b/unsupported/test/EulerAngles.cpp
@@ -13,6 +13,13 @@
 
 using namespace Eigen;
 
+// Unfortunately, we need to specialize it in order to work. (We could add it in main.h test framework)
+template <typename Scalar, class System>
+bool verifyIsApprox(const Eigen::EulerAngles<Scalar, System>& a, const Eigen::EulerAngles<Scalar, System>& b)
+{
+  return verifyIsApprox(a.angles(), b.angles());
+}
+
 // Verify that x is in the approxed range [a, b]
 #define VERIFY_APPROXED_RANGE(a, x, b) \
   do { \
@@ -24,7 +31,7 @@ const char X = EULER_X;
 const char Y = EULER_Y;
 const char Z = EULER_Z;
 
-template<typename Scalar, typename EulerSystem>
+template<typename Scalar, class EulerSystem>
 void verify_euler(const EulerAngles<Scalar, EulerSystem>& e)
 {
   typedef EulerAngles<Scalar, EulerSystem> EulerAnglesType;
@@ -68,6 +75,11 @@ void verify_euler(const EulerAngles<Scalar, EulerSystem>& e)
   const Vector3 I = EulerAnglesType::AlphaAxisVector();
   const Vector3 J = EulerAnglesType::BetaAxisVector();
   const Vector3 K = EulerAnglesType::GammaAxisVector();
+  
+  // Is approx checks
+  VERIFY(e.isApprox(e));
+  VERIFY_IS_APPROX(e, e);
+  VERIFY_IS_NOT_APPROX(e, EulerAnglesType(e.alpha() + ONE, e.beta() + ONE, e.gamma() + ONE));
 
   const Matrix3 m(e);
   VERIFY_IS_APPROX(Scalar(m.determinant()), ONE);
@@ -108,6 +120,11 @@ void verify_euler(const EulerAngles<Scalar, EulerSystem>& e)
   const QuaternionType qbis(AngleAxisType(eabis[0], I) * AngleAxisType(eabis[1], J) * AngleAxisType(eabis[2], K));
   VERIFY(internal::isApprox<Scalar>(std::abs(q.dot(qbis)), ONE, precision));
   //VERIFY_IS_APPROX(eabis, eabis2);// Verify that the euler angles are still the same
+  
+  // A suggestion for simple product test when will be supported.
+  /*EulerAnglesType e2(PI/2, PI/2, PI/2);
+  Matrix3 m2(e2);
+  VERIFY_IS_APPROX(e*e2, m*m2);*/
 }
 
 template<signed char A, signed char B, signed char C, typename Scalar>
@@ -250,6 +267,11 @@ template<typename Scalar> void eulerangles_rand()
 
 void test_EulerAngles()
 {
+  // Simple cast test
+  EulerAnglesXYZd onesEd(1, 1, 1);
+  EulerAnglesXYZf onesEf = onesEd.cast<float>();
+  VERIFY_IS_APPROX(onesEd, onesEf.cast<double>());
+  
   CALL_SUBTEST_1( eulerangles_manual<float>() );
   CALL_SUBTEST_2( eulerangles_manual<double>() );