fix bug #398, the quaternion returned by slerp was not always normalized,

add a proper unit test for slerp

2 files changed, 50 insertions, 4 deletions

diff --git a/Eigen/src/Geometry/Quaternion.h b/Eigen/src/Geometry/Quaternion.h
index 5a9a45c25..ce6faecd9 100644
--- a/Eigen/src/Geometry/Quaternion.h
+++ b/Eigen/src/Geometry/Quaternion.h
@@ -688,7 +688,7 @@ QuaternionBase<Derived>::slerp(Scalar t, const QuaternionBase<OtherDerived>& oth
   Scalar scale0;
   Scalar scale1;
 
-  if (absD>=one)
+  if(absD>=one)
   {
     scale0 = Scalar(1) - t;
     scale1 = t;
@@ -701,9 +701,8 @@ QuaternionBase<Derived>::slerp(Scalar t, const QuaternionBase<OtherDerived>& oth
 
     scale0 = internal::sin( ( Scalar(1) - t ) * theta) / sinTheta;
     scale1 = internal::sin( ( t * theta) ) / sinTheta;
-    if (d<0)
-      scale1 = -scale1;
   }
+  if(d<0) scale1 = -scale1;
 
   return Quaternion<Scalar>(scale0 * coeffs() + scale1 * other.coeffs());
 }
diff --git a/test/geo_quaternion.cpp b/test/geo_quaternion.cpp
index 1e7b2cba0..7adbe0b3d 100644
--- a/test/geo_quaternion.cpp
+++ b/test/geo_quaternion.cpp
@@ -28,6 +28,35 @@
 #include <Eigen/LU>
 #include <Eigen/SVD>
 
+template<typename T> T bounded_acos(T v)
+{
+  using std::acos;
+  using std::min;
+  using std::max;
+  return acos((max)(T(-1),(min)(v,T(1))));
+}
+
+template<typename QuatType> void check_slerp(const QuatType& q0, const QuatType& q1)
+{
+  typedef typename QuatType::Scalar Scalar;
+  typedef Matrix<Scalar,3,1> VectorType;
+  typedef AngleAxis<Scalar> AA;
+
+  Scalar largeEps = test_precision<Scalar>();
+
+  Scalar theta_tot = AA(q1*q0.inverse()).angle();
+  if(theta_tot>M_PI)
+    theta_tot = 2.*M_PI-theta_tot;
+  for(Scalar t=0; t<=1.001; t+=0.1)
+  {
+    QuatType q = q0.slerp(t,q1);
+    Scalar theta = AA(q*q0.inverse()).angle();
+    VERIFY(internal::abs(q.norm() - 1) < largeEps);
+    if(theta_tot==0)  VERIFY(theta_tot==0);
+    else              VERIFY(internal::abs(theta/theta_tot - t) < largeEps);
+  }
+}
+
 template<typename Scalar, int Options> void quaternion(void)
 {
   /* this test covers the following files:
@@ -36,6 +65,7 @@ template<typename Scalar, int Options> void quaternion(void)
 
   typedef Matrix<Scalar,3,3> Matrix3;
   typedef Matrix<Scalar,3,1> Vector3;
+  typedef Matrix<Scalar,4,1> Vector4;
   typedef Quaternion<Scalar,Options> Quaternionx;
   typedef AngleAxis<Scalar> AngleAxisx;
 
@@ -50,7 +80,8 @@ template<typename Scalar, int Options> void quaternion(void)
           v2 = Vector3::Random(),
           v3 = Vector3::Random();
 
-  Scalar a = internal::random<Scalar>(-Scalar(M_PI), Scalar(M_PI));
+  Scalar  a = internal::random<Scalar>(-Scalar(M_PI), Scalar(M_PI)),
+          b = internal::random<Scalar>(-Scalar(M_PI), Scalar(M_PI));
 
   // Quaternion: Identity(), setIdentity();
   Quaternionx q1, q2;
@@ -124,6 +155,22 @@ template<typename Scalar, int Options> void quaternion(void)
   // test bug 369 - improper alignment.
   Quaternionx *q = new Quaternionx;
   delete q;
+
+  q1 = AngleAxisx(a, v0.normalized());
+  q2 = AngleAxisx(b, v1.normalized());
+  check_slerp(q1,q2);
+
+  q1 = AngleAxisx(b, v1.normalized());
+  q2 = AngleAxisx(b+M_PI, v1.normalized());
+  check_slerp(q1,q2);
+
+  q1 = AngleAxisx(b,  v1.normalized());
+  q2 = AngleAxisx(-b, -v1.normalized());
+  check_slerp(q1,q2);
+
+  q1.coeffs() = Vector4::Random().normalized();
+  q2.coeffs() = -q1.coeffs();
+  check_slerp(q1,q2);
 }
 
 template<typename Scalar> void mapQuaternion(void){