Take shortest path on quaternion slerp

Change-Id: I4499e945481a22adfbafcc82198f7c411d04301b
Reviewed-by: Sean Harmer <sean.harmer@kdab.com>

3 files changed, 38 insertions, 12 deletions

diff --git a/src/animation/backend/animationutils.cpp b/src/animation/backend/animationutils.cpp
index 29de69df6..92e614236 100644
--- a/src/animation/backend/animationutils.cpp
+++ b/src/animation/backend/animationutils.cpp
@@ -55,6 +55,10 @@
 
 QT_BEGIN_NAMESPACE
 
+namespace {
+const auto slerpThreshold = 0.01f;
+}
+
 namespace Qt3DAnimation {
 namespace Animation {
 
@@ -270,17 +274,38 @@ ClipResults evaluateClipAtLocalTime(AnimationClip *clip, float localTime)
                     const int lowerKeyframeBound = channel.channelComponents[0].fcurve.lowerKeyframeBound(localTime);
                     const auto lowerQuat = quaternionFromChannel(lowerKeyframeBound);
                     const auto higherQuat = quaternionFromChannel(lowerKeyframeBound + 1);
-                    const float omega = std::acos(qBound(-1.0f, QQuaternion::dotProduct(lowerQuat, higherQuat), 1.0f));
-
-                    if (qFuzzyIsNull(omega)) {
-                        // If the two keyframe quaternions are equal, just return the first one as the interpolated value.
+                    auto cosHalfTheta = QQuaternion::dotProduct(lowerQuat, higherQuat);
+                    // If the two keyframe quaternions are equal, just return the first one as the interpolated value.
+                    if (std::abs(cosHalfTheta) >= 1.0f) {
                         channelResults[i++] = lowerQuat.scalar();
                         channelResults[i++] = lowerQuat.x();
                         channelResults[i++] = lowerQuat.y();
                         channelResults[i++] = lowerQuat.z();
                     } else {
-                        for (const auto &channelComponent : qAsConst(channel.channelComponents))
-                            channelResults[i++] = channelComponent.fcurve.evaluateAtTimeAsSlerp(localTime, lowerKeyframeBound, omega);
+                        const auto sinHalfTheta = std::sqrt(1.0f - std::pow(cosHalfTheta,2.0f));
+                        if (std::abs(sinHalfTheta) < ::slerpThreshold) {
+                            auto initial_i = i;
+                            for (const auto &channelComponent : qAsConst(channel.channelComponents))
+                                channelResults[i++] = channelComponent.fcurve.evaluateAtTime(localTime, lowerKeyframeBound);
+
+                            // Normalize the resulting quaternion
+                            QQuaternion quat{channelResults[initial_i], channelResults[initial_i+1], channelResults[initial_i+2], channelResults[initial_i+3]};
+                            quat.normalize();
+                            channelResults[initial_i+0] = quat.scalar();
+                            channelResults[initial_i+1] = quat.x();
+                            channelResults[initial_i+2] = quat.y();
+                            channelResults[initial_i+3] = quat.z();
+                        } else {
+                            const auto reverseQ1 = cosHalfTheta < 0 ? -1.0f : 1.0f;
+                            cosHalfTheta *= reverseQ1;
+                            const auto halfTheta = std::acos(cosHalfTheta);
+                            for (const auto &channelComponent : qAsConst(channel.channelComponents))
+                                channelResults[i++] = channelComponent.fcurve.evaluateAtTimeAsSlerp(localTime,
+                                                                                                    lowerKeyframeBound,
+                                                                                                    halfTheta,
+                                                                                                    sinHalfTheta,
+                                                                                                    reverseQ1);
+                        }
                     }
                 }
             }
diff --git a/src/animation/backend/fcurve.cpp b/src/animation/backend/fcurve.cpp
index 490866d54..18f1f427e 100644
--- a/src/animation/backend/fcurve.cpp
+++ b/src/animation/backend/fcurve.cpp
@@ -97,7 +97,7 @@ float FCurve::evaluateAtTime(float localTime, int lowerBound) const
     return m_keyframes.first().value;
 }
 
-float FCurve::evaluateAtTimeAsSlerp(float localTime, int lowerBound, float omega) const
+float FCurve::evaluateAtTimeAsSlerp(float localTime, int lowerBound, float halfTheta, float sinHalfTheta, float reverseQ1) const
 {
     // TODO: Implement extrapolation beyond first/last keyframes
     if (localTime < m_localTimes.first())
@@ -119,10 +119,11 @@ float FCurve::evaluateAtTimeAsSlerp(float localTime, int lowerBound, float omega
         return keyframe0.value;
     case QKeyFrame::LinearInterpolation:
         if (localTime >= t0 && localTime <= t1 && t1 > t0) {
-            const float t = (localTime - t0) / (t1 - t0);
-            const float div = 1.0f / std::sin(omega);
-            return std::sin((1 - t) * omega) * div * keyframe0.value  +
-                    std::sin(t * omega) * div * keyframe1.value;
+            const auto t = (localTime - t0) / (t1 - t0);
+
+            const auto A = std::sin((1.0f-t) * halfTheta) / sinHalfTheta;
+            const auto B = std::sin(t * halfTheta) / sinHalfTheta;
+            return A * keyframe0.value + reverseQ1 * B * keyframe1.value;
         }
         break;
     case QKeyFrame::BezierInterpolation:
diff --git a/src/animation/backend/fcurve_p.h b/src/animation/backend/fcurve_p.h
index 337eb615d..935db5922 100644
--- a/src/animation/backend/fcurve_p.h
+++ b/src/animation/backend/fcurve_p.h
@@ -84,7 +84,7 @@ public:
 
     float evaluateAtTime(float localTime) const;
     float evaluateAtTime(float localTime, int lowerBound) const;
-    float evaluateAtTimeAsSlerp(float localTime, int lowerBound, float omega) const;
+    float evaluateAtTimeAsSlerp(float localTime, int lowerBound, float halfTheta, float sinHalfTheta, float reverseQ1) const;
     int lowerKeyframeBound(float localTime) const;
 
     void read(const QJsonObject &json);