diff --git a/libs/math/include/psemek/math/quaternion.hpp b/libs/math/include/psemek/math/quaternion.hpp index df6f6dbb..b341a9d2 100644 --- a/libs/math/include/psemek/math/quaternion.hpp +++ b/libs/math/include/psemek/math/quaternion.hpp @@ -258,27 +258,24 @@ namespace psemek::math template quaternion slerp(quaternion const & q0, quaternion const & q1, T const & t) { - // threshold is chosen so that for abs(x) < threshold the second term in - // sin(x) Taylor series is less than the minimum value representable by T - static auto const threshold = std::pow(6 * std::numeric_limits::min(), T{1}/T{3}); + using std::sin; + using std::acos; + using std::abs; - auto const d = clamp(dot(normalized(q0.coords), normalized(q1.coords)), {T(-1), T(1)}); - auto const omega = std::acos(std::abs(d)); + auto const d = dot(q0.coords, q1.coords); + + // prevent division by zero + if (d >= T{1}) + return quaternion{lerp(q0.coords, q1.coords, t)}; + + auto const omega = acos(abs(d)); // NB: the case of omega ~ pi is ambiguous and isn't handled in any special way - if (std::abs(omega) < threshold) - { - // prevent division by zero - return quaternion{normalized(lerp(q0.coords, q1.coords, t))}; - } - else - { - auto const s = std::sin(omega); - auto const w0 = std::sin((1 - t) * omega) / s; - auto const w1 = std::sin(t * omega) / s * ((d > 0) ? 1 : -1); - return quaternion{q0.coords * w0 + q1.coords * w1}; - } + auto const s = sin(omega); + auto const w0 = sin((1 - t) * omega) / s; + auto const w1 = sin(t * omega) / s * ((d > T{0}) ? T{1} : -T{1}); + return quaternion{q0.coords * w0 + q1.coords * w1}; } template diff --git a/libs/math/include/psemek/math/vector.hpp b/libs/math/include/psemek/math/vector.hpp index fa017bb8..9060db95 100644 --- a/libs/math/include/psemek/math/vector.hpp +++ b/libs/math/include/psemek/math/vector.hpp @@ -473,31 +473,26 @@ namespace psemek::math return v0 * (T(1) - t) + v1 * t; } + // NB: v0 and v1 are assumed to be normalized template vector slerp(vector const & v0, vector const & v1, T const & t) { - using std::pow; - using std::abs; using std::sin; + using std::acos; - // threshold is chosen so that for abs(x) < threshold the second term in - // sin(x) Taylor series is less than the minimum value representable by T - static auto const threshold = pow(T{6} * std::numeric_limits::min(), T{1}/T{3}); + auto const d = dot(v0, v1); - auto const omega = angle(normalized(v0), normalized(v1)); + // prevent division by zero + if (d >= T{1}) + return lerp(v0, v1, t); + + auto const omega = acos(d); // NB: the case of omega ~ pi is ambiguous and isn't handled in any special way - - if (abs(omega) < threshold) - { - // prevent division by zero - return normalized(lerp(v0, v1, t)); - } - else - { - auto const s = sin(omega); - return v0 * sin((1 - t) * omega) / s + v1 * sin(t * omega) / s; - } + auto const s = sin(omega); + auto const w0 = std::sin((1 - t) * omega) / s; + auto const w1 = (sin(t * omega) / s); + return w0 * v0 + w1 * v1; } // Return vector orthogonal to n