From b2328f57cdf487d9500cbd27e4abcbb0e687afd4 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Vladim=C3=ADr=20Vondru=C5=A1?= Date: Tue, 4 Feb 2020 00:50:25 +0100 Subject: [PATCH] Math: fall back to lerp when slerp'd quaternions are too close. Returning just the first argument had several issues, one of them was causing iterative interpolations to get stuck at the initial position forever. --- doc/changelog.dox | 4 ++ src/Magnum/Math/Quaternion.h | 74 ++++++++++++++++++------- src/Magnum/Math/Test/QuaternionTest.cpp | 71 ++++++++++++++++++++++-- 3 files changed, 125 insertions(+), 24 deletions(-) diff --git a/doc/changelog.dox b/doc/changelog.dox index 58369276c..1a0f366e0 100644 --- a/doc/changelog.dox +++ b/doc/changelog.dox @@ -162,6 +162,10 @@ See also: - Functions in @ref Magnum/Math/FunctionsBatch.h now accept any type that's convertible to a @ref Corrade::Containers::StridedArrayView without having to add explicit casts or template parameters +- @ref Math::slerp(const Quaternion&, const Quaternion&, T) and + @ref Math::slerpShortestPath(const Quaternion&, const Quaternion&, T) + now fall back to linear interpolation when the quaternions are close to + each other, instead of unconditionally returning the first argument @subsubsection changelog-latest-changes-meshtools MeshTools library diff --git a/src/Magnum/Math/Quaternion.h b/src/Magnum/Math/Quaternion.h index a419be10b..f0d61aabd 100644 --- a/src/Magnum/Math/Quaternion.h +++ b/src/Magnum/Math/Quaternion.h @@ -140,17 +140,23 @@ template inline Quaternion lerpShortestPath(const Quaternion& nor @param normalizedB Second quaternion @param t Interpolation phase (from range @f$ [0; 1] @f$) -Expects that both quaternions are normalized. If the quaternions are the same -or one is a negation of the other, it just returns the first argument: @f[ +Expects that both quaternions are normalized. If the quaternions are nearly the +same or one is a negation of the other, it falls back to a linear interpolation +(shortest-path to avoid a degenerate case of returning a zero quaternion for +@f$ t = 0.5 @f$), but without post-normalization as the interpolation result +can still be considered sufficiently normalized: @f[ \begin{array}{rcl} d & = & q_A \cdot q_B \\[5pt] - q_{SLERP} & = & q_A, ~ {\color{m-primary} \text{if} ~ d \ge 1} + q_{SLERP} & = & (1 - t) \left\{ \begin{array}{lr} + \phantom{-}q_A, & d \ge 0 \\ + -q_A, & d < 0 + \end{array} \right\} + t q_B, ~ {\color{m-primary} \text{if} ~ |d| \ge 1 - \frac{\epsilon}{2}} \end{array} @f] @m_class{m-noindent} -otherwise, the interpolation is performed as: @f[ +Otherwise, the interpolation is performed as: @f[ \begin{array}{rcl} \theta & = & \arccos \left( \frac{q_A \cdot q_B}{|q_A| |q_B|} \right) = \arccos(q_A \cdot q_B) = \arccos(d) \\[5pt] q_{SLERP} & = & \cfrac{\sin((1 - t) \theta) q_A + \sin(t \theta) q_B}{\sin(\theta)} @@ -169,9 +175,31 @@ template inline Quaternion slerp(const Quaternion& normalizedA, c "Math::slerp(): quaternions" << normalizedA << "and" << normalizedB << "are not normalized", {}); const T cosHalfAngle = dot(normalizedA, normalizedB); - /* Avoid division by zero */ - if(std::abs(cosHalfAngle) >= T(1) - TypeTraits::epsilon()) - return normalizedA; + /* Avoid division by zero if the quats are very close and instead fall back + to a linear interpolation. This is intentionally not doing any + normalization as that's not needed. For a maximum angle α satisfying the + condition below, the two quaternions form two sides of an isosceles + triangle and its altitude x is length of the "shortest" possible + interpolated quaternion: + + + + /|\ cos(α) > 1 - ε/2 + /α|α\ α < arccos(1 - ε/2) + /-_|_-\ + 1 / | \ 1 x/1 < cos(α) + / |x \ x/1 < cos(arccos(1 - ε/2)) + / | \ x < 1 - ε/2 + +------+------+ + + Magnum's isNormalized() check treats all lengths in (1 - ε, 1 + ε) as + normalized, thus for an safety headroom this stops only at 1 - ε/2. + Additionally this needs to account for the case of the quaternions being + mutual negatives, in which case a simple lerp() would return a zero + quaternion for t = 0.5. */ + if(std::abs(cosHalfAngle) > T(1) - T(0.5)*TypeTraits::epsilon()) { + const Quaternion shortestNormalizedA = cosHalfAngle < 0 ? -normalizedA : normalizedA; + return (T(1) - t)*shortestNormalizedA + t*normalizedB; + } const T a = std::acos(cosHalfAngle); return (std::sin((T(1) - t)*a)*normalizedA + std::sin(t*a)*normalizedB)/std::sin(a); @@ -185,22 +213,25 @@ template inline Quaternion slerp(const Quaternion& normalizedA, c Unlike @ref slerp(const Quaternion&, const Quaternion&, T) this function interpolates on the shortest path. Expects that both quaternions are -normalized. If the quaternions are the same or one is a negation of the other, -it just returns the first argument: @f[ +normalized. If the quaternions are nearly the same or one is a negation of the +other, it falls back to a linear interpolation (shortest-path to avoid a +degenerate case of returning a zero quaternion for @f$ t = 0.5 @f$) but without +post-normalization as the interpolation result can still be considered +sufficiently normalized: @f[ \begin{array}{rcl} - d & = & q_A \cdot q_B \\ - q_{SLERP} & = & q_A, ~ {\color{m-primary} \text{if} ~ d \ge 1} + d & = & q_A \cdot q_B \\[15pt] + q'_A & = & \begin{cases} + \phantom{-}q_A, & d \ge 0 \\ + -q_A, & d < 0 + \end{cases} \\[15pt] + q_{SLERP} & = & (1 - t) q'_A + t q_B, ~ {\color{m-primary} \text{if} ~ |d| \ge 1 - \frac{\epsilon}{2}} \end{array} @f] @m_class{m-noindent} -otherwise, the interpolation is performed as: @f[ +Otherwise, the interpolation is performed as: @f[ \begin{array}{rcl} - q'_A & = & \begin{cases} - \phantom{-}q_A, & d \ge 0 \\ - -q_A, & d < 0 - \end{cases} \\[15pt] \theta & = & \arccos \left( \frac{|q'_A \cdot q_B|}{|q'_A| |q_B|} \right) = \arccos(|q'_A \cdot q_B|) = \arccos(|d|) \\[5pt] q_{SLERP} & = & \cfrac{\sin((1 - t) \theta) q'_A + \sin(t \theta) q_B}{\sin(\theta)} \end{array} @@ -215,12 +246,15 @@ template inline Quaternion slerpShortestPath(const Quaternion& no "Math::slerpShortestPath(): quaternions" << normalizedA << "and" << normalizedB << "are not normalized", {}); const T cosHalfAngle = dot(normalizedA, normalizedB); - /* Avoid division by zero */ - if(std::abs(cosHalfAngle) >= T(1) - TypeTraits::epsilon()) - return normalizedA; - const Quaternion shortestNormalizedA = cosHalfAngle < 0 ? -normalizedA : normalizedA; + /* Avoid division by zero if the quats are very close and instead fall back + to a linear interpolation. This is intentionally not doing any + normalization, see slerp() above for more information. */ + if(std::abs(cosHalfAngle) >= T(1) - TypeTraits::epsilon()) { + return (T(1) - t)*shortestNormalizedA + t*normalizedB; + } + const T a = std::acos(std::abs(cosHalfAngle)); return (std::sin((T(1) - t)*a)*shortestNormalizedA + std::sin(t*a)*normalizedB)/std::sin(a); } diff --git a/src/Magnum/Math/Test/QuaternionTest.cpp b/src/Magnum/Math/Test/QuaternionTest.cpp index 1c2c2d4cc..6704f06e4 100644 --- a/src/Magnum/Math/Test/QuaternionTest.cpp +++ b/src/Magnum/Math/Test/QuaternionTest.cpp @@ -28,6 +28,7 @@ #include #include +#include "Magnum/Math/Functions.h" #include "Magnum/Math/Matrix4.h" #include "Magnum/Math/Quaternion.h" #include "Magnum/Math/StrictWeakOrdering.h" @@ -105,9 +106,13 @@ struct QuaternionTest: Corrade::TestSuite::Tester { void lerpShortestPath(); void lerpShortestPathNotNormalized(); void slerp(); + void slerpLinearFallback(); + template void slerpLinearFallbackIsNormalized(); void slerp2D(); void slerpNotNormalized(); void slerpShortestPath(); + void slerpShortestPathLinearFallback(); + template void slerpShortestPathLinearFallbackIsNormalized(); void slerpShortestPathNotNormalized(); void transformVector(); @@ -182,9 +187,15 @@ QuaternionTest::QuaternionTest() { &QuaternionTest::lerpShortestPath, &QuaternionTest::lerpShortestPathNotNormalized, &QuaternionTest::slerp, + &QuaternionTest::slerpLinearFallback, + &QuaternionTest::slerpLinearFallbackIsNormalized, + &QuaternionTest::slerpLinearFallbackIsNormalized, &QuaternionTest::slerp2D, &QuaternionTest::slerpNotNormalized, &QuaternionTest::slerpShortestPath, + &QuaternionTest::slerpShortestPathLinearFallback, + &QuaternionTest::slerpShortestPathLinearFallbackIsNormalized, + &QuaternionTest::slerpShortestPathLinearFallbackIsNormalized, &QuaternionTest::slerpShortestPathNotNormalized, &QuaternionTest::transformVector, @@ -669,12 +680,35 @@ void QuaternionTest::slerp() { CORRADE_COMPARE(slerp, expected); CORRADE_VERIFY(slerpShortestPath.isNormalized()); CORRADE_COMPARE(slerpShortestPath, expected); +} + +void QuaternionTest::slerpLinearFallback() { + Quaternion a = Quaternion::rotation(23.0_degf, Vector3::xAxis()); - /* Avoid division by zero */ + /* Returning the same */ CORRADE_COMPARE(Math::slerp(a, a, 0.25f), a); - CORRADE_COMPARE(Math::slerp(a, -a, 0.42f), a); - CORRADE_COMPARE(Math::slerpShortestPath(a, a, 0.25f), a); - CORRADE_COMPARE(Math::slerpShortestPath(a, -a, 0.25f), a); + + /* Returning the second when negated */ + CORRADE_COMPARE(Math::slerp(a, -a, 0.0f), -a); + CORRADE_COMPARE(Math::slerp(a, -a, 0.5f), -a); + CORRADE_COMPARE(Math::slerp(a, -a, 1.0f), -a); +} + +template void QuaternionTest::slerpLinearFallbackIsNormalized() { + setTestCaseTemplateName(TypeTraits::name()); + + Math::Quaternion a = Math::Quaternion::rotation({}, Math::Vector3::xAxis()); + Math::Quaternion b = Math::Quaternion::rotation(Math::acos(T(1) - T(0.49999)*TypeTraits::epsilon()), Math::Vector3::xAxis()); + + /* Ensure we're in the special case */ + CORRADE_VERIFY(std::abs(Math::dot(a, b)) > T(1) - T(0.5)*TypeTraits::epsilon()); + + /* Edges */ + CORRADE_COMPARE(Math::slerp(a, b, T(0.0)), a); + CORRADE_COMPARE(Math::slerp(a, b, T(1.0)), b); + + /* Midpoint should still be normalized */ + CORRADE_VERIFY(Math::slerp(a, b, T(0.5)).isNormalized()); } void QuaternionTest::slerp2D() { @@ -718,6 +752,35 @@ void QuaternionTest::slerpShortestPath() { CORRADE_COMPARE(slerpShortestPath, (Quaternion{{0.0f, 0.0f, 0.290285f}, -0.95694f})); } +void QuaternionTest::slerpShortestPathLinearFallback() { + Quaternion a = Quaternion::rotation(23.0_degf, Vector3::xAxis()); + + /* Returning the same */ + CORRADE_COMPARE(Math::slerpShortestPath(a, a, 0.25f), a); + + /* Returning the second when negated */ + CORRADE_COMPARE(Math::slerpShortestPath(a, -a, 0.0f), -a); + CORRADE_COMPARE(Math::slerpShortestPath(a, -a, 0.5f), -a); + CORRADE_COMPARE(Math::slerpShortestPath(a, -a, 1.0f), -a); +} + +template void QuaternionTest::slerpShortestPathLinearFallbackIsNormalized() { + setTestCaseTemplateName(TypeTraits::name()); + + Math::Quaternion a = Math::Quaternion::rotation({}, Math::Vector3::xAxis()); + Math::Quaternion b = Math::Quaternion::rotation(Math::acos(T(1) - T(0.49999)*TypeTraits::epsilon()), Math::Vector3::xAxis()); + + /* Ensure we're in the special case */ + CORRADE_VERIFY(std::abs(Math::dot(a, b)) > T(1) - T(0.5)*TypeTraits::epsilon()); + + /* Edges */ + CORRADE_COMPARE(Math::slerpShortestPath(a, b, T(0.0)), a); + CORRADE_COMPARE(Math::slerpShortestPath(a, b, T(1.0)), b); + + /* Midpoint should still be normalized */ + CORRADE_VERIFY(Math::slerpShortestPath(a, b, T(0.5)).isNormalized()); +} + void QuaternionTest::slerpShortestPathNotNormalized() { std::ostringstream out; Error redirectError{&out};