From c68c695b87b1e72bae6eb09e6ebcc35551c22044 Mon Sep 17 00:00:00 2001
From: Gael Guennebaud <g.gael@free.fr>
Date: Fri, 4 Dec 2009 15:01:17 +0100
Subject: [PATCH] Fix poor Quaternion::slerp snapping

---
 Eigen/src/Geometry/Quaternion.h | 29 +++++++++++++++++++----------
 1 file changed, 19 insertions(+), 10 deletions(-)

diff --git a/Eigen/src/Geometry/Quaternion.h b/Eigen/src/Geometry/Quaternion.h
index 5578ce9f4..b6eb74d66 100644
--- a/Eigen/src/Geometry/Quaternion.h
+++ b/Eigen/src/Geometry/Quaternion.h
@@ -583,20 +583,29 @@ template <class OtherDerived>
 Quaternion<typename ei_traits<Derived>::Scalar>
 QuaternionBase<Derived>::slerp(Scalar t, const QuaternionBase<OtherDerived>& other) const
 {
-  static const Scalar one = Scalar(1) - dummy_precision<Scalar>();
+  static const Scalar one = Scalar(1) - epsilon<Scalar>();
   Scalar d = this->dot(other);
   Scalar absD = ei_abs(d);
+
+  Scalar scale0;
+  Scalar scale1;
+
   if (absD>=one)
-    return Quaternion<Scalar>(derived());
+  {
+    scale0 = Scalar(1) - t;
+    scale1 = t;
+  }
+  else
+  {
+    // theta is the angle between the 2 quaternions
+    Scalar theta = std::acos(absD);
+    Scalar sinTheta = ei_sin(theta);
 
-  // theta is the angle between the 2 quaternions
-  Scalar theta = std::acos(absD);
-  Scalar sinTheta = ei_sin(theta);
-
-  Scalar scale0 = ei_sin( ( Scalar(1) - t ) * theta) / sinTheta;
-  Scalar scale1 = ei_sin( ( t * theta) ) / sinTheta;
-  if (d<0)
-    scale1 = -scale1;
+    scale0 = ei_sin( ( Scalar(1) - t ) * theta) / sinTheta;
+    scale1 = ei_sin( ( t * theta) ) / sinTheta;
+    if (d<0)
+      scale1 = -scale1;
+  }
 
   return Quaternion<Scalar>(scale0 * coeffs() + scale1 * other.coeffs());
 }