diff --git a/libs/math/include/math/TQuatHelpers.h b/libs/math/include/math/TQuatHelpers.h index 9a8f889635..2c63a01b62 100644 --- a/libs/math/include/math/TQuatHelpers.h +++ b/libs/math/include/math/TQuatHelpers.h @@ -245,17 +245,17 @@ public: friend inline QUATERNION MATH_PURE slerp(const QUATERNION& p, const QUATERNION& q, T t) { // could also be computed as: pow(q * inverse(p), t) * p; - const T d = dot(p, q); + const T d = std::abs(dot(p, q)); + static constexpr T value_eps = T(10) * std::numeric_limits::epsilon(); + // Prevent blowing up when slerping between two quaternions that are very near each other. + if ((T(1) - d) < value_eps) { + return lerp(p, q, t); + } const T npq = sqrt(dot(p, p) * dot(q, q)); // ||p|| * ||q|| - const T a = std::acos(std::abs(d) / npq); + const T a = std::acos(d / npq); const T a0 = a * (1 - t); const T a1 = a * t; const T sina = sin(a); - // Prevent blowing up when slerping between two quaternions that are very near each other. - static constexpr T value_eps = T(10) * std::numeric_limits::epsilon(); - if (std::abs(sina) < value_eps) { - return lerp(p, q, t); - } const T isina = 1 / sina; const T s0 = std::sin(a0) * isina; const T s1 = std::sin(a1) * isina;