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

@@ -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<T>&, const Quaternion<T>&, T) and
+    @ref Math::slerpShortestPath(const Quaternion<T>&, const Quaternion<T>&, 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
 

src/Magnum/Math/Quaternion.h

@@ -140,17 +140,23 @@ template<class T> inline Quaternion<T> lerpShortestPath(const Quaternion<T>& 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
+Expects that both quaternions are normalized. If the quaternions are nearly the
-or one is a negation of the other, it just returns the first argument: @f[
+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<class T> inline Quaternion<T> slerp(const Quaternion<T>& normalizedA, c
         "Math::slerp(): quaternions" << normalizedA << "and" << normalizedB << "are not normalized", {});
     const T cosHalfAngle = dot(normalizedA, normalizedB);
 
-    /* Avoid division by zero */
+    /* Avoid division by zero if the quats are very close and instead fall back
-    if(std::abs(cosHalfAngle) >= T(1) - TypeTraits<T>::epsilon())
+       to a linear interpolation. This is intentionally not doing any
-        return normalizedA;
+       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<T>::epsilon()) {
+        const Quaternion<T> 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<class T> inline Quaternion<T> slerp(const Quaternion<T>& normalizedA, c
 
 Unlike @ref slerp(const Quaternion<T>&, const Quaternion<T>&, 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,
+normalized. If the quaternions are nearly the same or one is a negation of the
-it just returns the first argument: @f[
+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 \\
+        d & = & q_A \cdot q_B \\[15pt]
-        q_{SLERP} & = & q_A, ~ {\color{m-primary} \text{if} ~ d \ge 1}
+        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<class T> inline Quaternion<T> slerpShortestPath(const Quaternion<T>& 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<T>::epsilon())
-        return normalizedA;
-
     const Quaternion<T> 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<T>::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);
 }

src/Magnum/Math/Test/QuaternionTest.cpp

@@ -28,6 +28,7 @@
 #include <Corrade/TestSuite/Compare/Numeric.h>
 #include <Corrade/Utility/DebugStl.h>
 
+#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<class T> void slerpLinearFallbackIsNormalized();
     void slerp2D();
     void slerpNotNormalized();
     void slerpShortestPath();
+    void slerpShortestPathLinearFallback();
+    template<class T> void slerpShortestPathLinearFallbackIsNormalized();
     void slerpShortestPathNotNormalized();
 
     void transformVector();
@@ -182,9 +187,15 @@ QuaternionTest::QuaternionTest() {
               &QuaternionTest::lerpShortestPath,
               &QuaternionTest::lerpShortestPathNotNormalized,
               &QuaternionTest::slerp,
+              &QuaternionTest::slerpLinearFallback,
+              &QuaternionTest::slerpLinearFallbackIsNormalized<Float>,
+              &QuaternionTest::slerpLinearFallbackIsNormalized<Double>,
               &QuaternionTest::slerp2D,
               &QuaternionTest::slerpNotNormalized,
               &QuaternionTest::slerpShortestPath,
+              &QuaternionTest::slerpShortestPathLinearFallback,
+              &QuaternionTest::slerpShortestPathLinearFallbackIsNormalized<Float>,
+              &QuaternionTest::slerpShortestPathLinearFallbackIsNormalized<Double>,
               &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);
+    /* Returning the second when negated */
-    CORRADE_COMPARE(Math::slerpShortestPath(a, -a, 0.25f), a);
+    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<class T> void QuaternionTest::slerpLinearFallbackIsNormalized() {
+    setTestCaseTemplateName(TypeTraits<T>::name());
+
+    Math::Quaternion<T> a = Math::Quaternion<T>::rotation({}, Math::Vector3<T>::xAxis());
+    Math::Quaternion<T> b = Math::Quaternion<T>::rotation(Math::acos(T(1) - T(0.49999)*TypeTraits<T>::epsilon()), Math::Vector3<T>::xAxis());
+
+    /* Ensure we're in the special case */
+    CORRADE_VERIFY(std::abs(Math::dot(a, b)) > T(1) - T(0.5)*TypeTraits<T>::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<class T> void QuaternionTest::slerpShortestPathLinearFallbackIsNormalized() {
+    setTestCaseTemplateName(TypeTraits<T>::name());
+
+    Math::Quaternion<T> a = Math::Quaternion<T>::rotation({}, Math::Vector3<T>::xAxis());
+    Math::Quaternion<T> b = Math::Quaternion<T>::rotation(Math::acos(T(1) - T(0.49999)*TypeTraits<T>::epsilon()), Math::Vector3<T>::xAxis());
+
+    /* Ensure we're in the special case */
+    CORRADE_VERIFY(std::abs(Math::dot(a, b)) > T(1) - T(0.5)*TypeTraits<T>::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};