Rotation2D: fix slerp to take the shortest path, and add convenient method to get the angle in [-pi,pi] or [0,pi]

2 files changed, 53 insertions, 4 deletions

diff --git a/Eigen/src/Geometry/Rotation2D.h b/Eigen/src/Geometry/Rotation2D.h
index 4feb3d4d2..65aa83be5 100644
--- a/Eigen/src/Geometry/Rotation2D.h
+++ b/Eigen/src/Geometry/Rotation2D.h
@@ -69,6 +69,20 @@ public:
 
   /** \returns a read-write reference to the rotation angle */
   inline Scalar& angle() { return m_angle; }
+  
+  /** \returns the rotation angle in [0,2pi] */
+  inline Scalar smallestPositiveAngle() const {
+    Scalar tmp = fmod(m_angle,Scalar(2)*EIGEN_PI);
+    return tmp<Scalar(0) ? tmp + Scalar(2)*EIGEN_PI : tmp;
+  }
+  
+  /** \returns the rotation angle in [-pi,pi] */
+  inline Scalar smallestAngle() const {
+    Scalar tmp = fmod(m_angle,Scalar(2)*EIGEN_PI);
+    if(tmp>Scalar(EIGEN_PI))       tmp -= Scalar(2)*Scalar(EIGEN_PI);
+    else if(tmp<-Scalar(EIGEN_PI)) tmp += Scalar(2)*Scalar(EIGEN_PI);
+    return tmp;
+  }
 
   /** \returns the inverse rotation */
   inline Rotation2D inverse() const { return Rotation2D(-m_angle); }
@@ -93,7 +107,10 @@ public:
     * parameter \a t. It is in fact equivalent to a linear interpolation.
     */
   inline Rotation2D slerp(const Scalar& t, const Rotation2D& other) const
-  { return Rotation2D(m_angle * (1-t) + other.angle() * t); }
+  {
+    Scalar dist = Rotation2D(other.m_angle-m_angle).smallestAngle();
+    return Rotation2D(m_angle + dist*t);
+  }
 
   /** \returns \c *this with scalar type casted to \a NewScalarType
     *
@@ -119,6 +136,7 @@ public:
     * \sa MatrixBase::isApprox() */
   bool isApprox(const Rotation2D& other, const typename NumTraits<Scalar>::Real& prec = NumTraits<Scalar>::dummy_precision()) const
   { return internal::isApprox(m_angle,other.m_angle, prec); }
+  
 };
 
 /** \ingroup Geometry_Module
diff --git a/test/geo_transformations.cpp b/test/geo_transformations.cpp
index c4025f32f..e296267cf 100644
--- a/test/geo_transformations.cpp
+++ b/test/geo_transformations.cpp
@@ -408,7 +408,24 @@ template<typename Scalar, int Mode, int Options> void transformations()
   VERIFY_IS_APPROX(r2d1f.template cast<Scalar>(),r2d1);
   Rotation2D<double> r2d1d = r2d1.template cast<double>();
   VERIFY_IS_APPROX(r2d1d.template cast<Scalar>(),r2d1);
+  
+  for(int k=0; k<100; ++k)
+  {
+    Scalar angle = internal::random<Scalar>(-100,100);
+    Rotation2D<Scalar> rot2(angle);
+    VERIFY( rot2.smallestPositiveAngle() >= 0 );
+    VERIFY( rot2.smallestPositiveAngle() < Scalar(2)*Scalar(EIGEN_PI) );
+    VERIFY_IS_APPROX( std::cos(rot2.smallestPositiveAngle()), std::cos(rot2.angle()) );
+    VERIFY_IS_APPROX( std::sin(rot2.smallestPositiveAngle()), std::sin(rot2.angle()) );
+    
+    VERIFY( rot2.smallestAngle() >= -Scalar(EIGEN_PI) );
+    VERIFY( rot2.smallestAngle() <=  Scalar(EIGEN_PI) );
+    VERIFY_IS_APPROX( std::cos(rot2.smallestAngle()), std::cos(rot2.angle()) );
+    VERIFY_IS_APPROX( std::sin(rot2.smallestAngle()), std::sin(rot2.angle()) );
+  }
 
+  s0 = internal::random<Scalar>(-100,100);
+  s1 = internal::random<Scalar>(-100,100);
   Rotation2D<Scalar> R0(s0), R1(s1);
   
   t20 = Translation2(v20) * (R0 * Eigen::Scaling(s0));
@@ -420,9 +437,23 @@ template<typename Scalar, int Mode, int Options> void transformations()
   VERIFY_IS_APPROX(t20,t21);
   
   VERIFY_IS_APPROX(s0, (R0.slerp(0, R1)).angle());
-  VERIFY_IS_APPROX(s1, (R0.slerp(1, R1)).angle());
-  VERIFY_IS_APPROX(s0, (R0.slerp(0.5, R0)).angle());
-  VERIFY_IS_APPROX(Scalar(0), (R0.slerp(0.5, R0.inverse())).angle());
+  VERIFY_IS_APPROX(R1.smallestPositiveAngle(), (R0.slerp(1, R1)).smallestPositiveAngle());
+  VERIFY_IS_APPROX(R0.smallestPositiveAngle(), (R0.slerp(0.5, R0)).smallestPositiveAngle());
+
+  if(std::cos(s0)>0)
+    VERIFY_IS_MUCH_SMALLER_THAN((R0.slerp(0.5, R0.inverse())).smallestAngle(), Scalar(1));
+  else
+    VERIFY_IS_APPROX(Scalar(EIGEN_PI), (R0.slerp(0.5, R0.inverse())).smallestPositiveAngle());
+  
+  // Check path length
+  Scalar l = 0;
+  for(int k=0; k<100; ++k)
+  {
+    Scalar a1 = R0.slerp(Scalar(k)/Scalar(100), R1).angle();
+    Scalar a2 = R0.slerp(Scalar(k+1)/Scalar(100), R1).angle();
+    l += std::abs(a2-a1);
+  }
+  VERIFY(l<=EIGEN_PI);
   
   // check basic features
   {