diff --git a/libs/geom/include/psemek/geom/quaternion.hpp b/libs/geom/include/psemek/geom/quaternion.hpp index 89b49175..ee4ba907 100644 --- a/libs/geom/include/psemek/geom/quaternion.hpp +++ b/libs/geom/include/psemek/geom/quaternion.hpp @@ -134,4 +134,31 @@ namespace psemek::geom return {res[1], res[2], res[3]}; } + 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}); + + auto const d = clamp(dot(normalized(q0.coords), normalized(q1.coords)), {T(-1), T(1)}); + auto const omega = std::acos(std::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}; + } + } + + }