diff options
-rw-r--r-- | Eigen/src/Geometry/Quaternion.h | 29 |
1 files 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); - if (absD>=one) - return Quaternion<Scalar>(derived()); - // theta is the angle between the 2 quaternions - Scalar theta = std::acos(absD); - Scalar sinTheta = ei_sin(theta); + Scalar scale0; + Scalar scale1; - Scalar scale0 = ei_sin( ( Scalar(1) - t ) * theta) / sinTheta; - Scalar scale1 = ei_sin( ( t * theta) ) / sinTheta; - if (d<0) - scale1 = -scale1; + if (absD>=one) + { + 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); + + 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()); } |