diff --git a/Eigen/src/Geometry/Quaternion.h b/Eigen/src/Geometry/Quaternion.h index 3fcbff4e7..51dfc270c 100644 --- a/Eigen/src/Geometry/Quaternion.h +++ b/Eigen/src/Geometry/Quaternion.h @@ -450,22 +450,31 @@ inline Scalar Quaternion::angularDistance(const Quaternion& other) const template Quaternion Quaternion::slerp(Scalar t, const Quaternion& other) const { - static const Scalar one = Scalar(1) - precision(); + static const Scalar one = Scalar(1) - machine_epsilon(); Scalar d = this->dot(other); Scalar absD = ei_abs(d); + + Scalar scale0; + Scalar scale1; + if (absD>=one) - return *this; + { + 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); + scale0 = ei_sin( ( Scalar(1) - t ) * theta) / sinTheta; + scale1 = ei_sin( ( t * theta) ) / sinTheta; + if (d<0) + scale1 = -scale1; + } - Scalar scale0 = ei_sin( ( Scalar(1) - t ) * theta) / sinTheta; - Scalar scale1 = ei_sin( ( t * theta) ) / sinTheta; - if (d<0) - scale1 = -scale1; - - return Quaternion(scale0 * m_coeffs + scale1 * other.m_coeffs); + return Quaternion(scale0 * coeffs() + scale1 * other.coeffs()); } // set from a rotation matrix