Use slerp for animating rotation channels

Change-Id: Ic6aa6fd1c1a1e7757dba9466b59d20834b56e246
Reviewed-by: Paul Lemire <paul.lemire@kdab.com>

8 files changed, 220 insertions, 14 deletions

diff --git a/src/animation/backend/animationutils.cpp b/src/animation/backend/animationutils.cpp
index c6147fe7e..0496ff681 100644
--- a/src/animation/backend/animationutils.cpp
+++ b/src/animation/backend/animationutils.cpp
@@ -259,8 +259,58 @@ ClipResults evaluateClipAtLocalTime(AnimationClip *clip, float localTime)
     const QVector<Channel> &channels = clip->channels();
     int i = 0;
     for (const Channel &channel : channels) {
-        for (const auto &channelComponent : qAsConst(channel.channelComponents))
-            channelResults[i++] = channelComponent.fcurve.evaluateAtTime(localTime);
+        if (channel.name.contains(QStringLiteral("Rotation")) &&
+                        channel.channelComponents.size() == 4) {
+
+            // Try to SLERP
+            const int nbKeyframes = channel.channelComponents[0].fcurve.keyframeCount();
+            const bool canSlerp = std::find_if(std::begin(channel.channelComponents)+1,
+                                               std::end(channel.channelComponents),
+                                               [nbKeyframes](const ChannelComponent &v) {
+                return v.fcurve.keyframeCount() != nbKeyframes;
+            }) == std::end(channel.channelComponents);
+
+            if (!canSlerp) {
+                // Interpolate per component
+                for (const auto &channelComponent : qAsConst(channel.channelComponents)) {
+                    const int lowerKeyframeBound = channelComponent.fcurve.lowerKeyframeBound(localTime);
+                    channelResults[i++] = channelComponent.fcurve.evaluateAtTime(localTime, lowerKeyframeBound);
+                }
+            } else {
+                // There's only one keyframe. We cant compute omega. Interoplate per component
+                const int lowerKeyframeBound = channel.channelComponents[0].fcurve.lowerKeyframeBound(localTime);
+                if (lowerKeyframeBound + 1 >= channel.channelComponents[0].fcurve.keyframeCount()) {
+                    for (const auto &channelComponent : qAsConst(channel.channelComponents))
+                        channelResults[i++] = channelComponent.fcurve.evaluateAtTime(localTime, lowerKeyframeBound);
+                } else {
+
+                    auto quaternionFromChannel = [channel](const int keyframe) {
+                        const float w = channel.channelComponents[0].fcurve.keyframe(keyframe).value;
+                        const float x = channel.channelComponents[1].fcurve.keyframe(keyframe).value;
+                        const float y = channel.channelComponents[2].fcurve.keyframe(keyframe).value;
+                        const float z = channel.channelComponents[3].fcurve.keyframe(keyframe).value;
+                        QQuaternion quat{w,x,y,z};
+                        quat.normalize();
+                        return quat;
+                    };
+
+                    const auto lowerQuat = quaternionFromChannel(lowerKeyframeBound);
+                    const auto higherQuat = quaternionFromChannel(lowerKeyframeBound + 1);
+
+                    const float omega = std::acos(QQuaternion::dotProduct(lowerQuat, higherQuat));
+                    for (const auto &channelComponent : qAsConst(channel.channelComponents))
+                        channelResults[i++] = channelComponent.fcurve.evaluateAtTimeAsSlerp(localTime, lowerKeyframeBound, omega);
+                }
+            }
+        } else {
+            // If the channel is not a Rotation, apply linear interpolation per channel component
+            // TODO How do we handle other interpolations. For exammple, color interpolation
+            // in a linear perceptual way or other non linear spaces?
+            for (const auto &channelComponent : qAsConst(channel.channelComponents)) {
+                const int lowerKeyframeBound = channelComponent.fcurve.lowerKeyframeBound(localTime);
+                channelResults[i++] = channelComponent.fcurve.evaluateAtTime(localTime, lowerKeyframeBound);
+            }
+        }
     }
     return channelResults;
 }
diff --git a/src/animation/backend/fcurve.cpp b/src/animation/backend/fcurve.cpp
index c4361a37a..490866d54 100644
--- a/src/animation/backend/fcurve.cpp
+++ b/src/animation/backend/fcurve.cpp
@@ -53,6 +53,12 @@ FCurve::FCurve()
 
 float FCurve::evaluateAtTime(float localTime) const
 {
+    return evaluateAtTime(localTime, lowerKeyframeBound(localTime));
+}
+
+
+float FCurve::evaluateAtTime(float localTime, int lowerBound) const
+{
     // TODO: Implement extrapolation beyond first/last keyframes
     if (localTime < m_localTimes.first()) {
         return m_keyframes.first().value;
@@ -60,14 +66,13 @@ float FCurve::evaluateAtTime(float localTime) const
         return m_keyframes.last().value;
     } else {
         // Find keyframes that sandwich the requested localTime
-        const int idx = m_rangeFinder.findLowerBound(localTime);
-        if (idx < 0) // only one keyframe
+        if (lowerBound < 0) // only one keyframe
             return m_keyframes.first().value;
 
-        const float t0 = m_localTimes[idx];
-        const float t1 = m_localTimes[idx + 1];
-        const Keyframe &keyframe0(m_keyframes[idx]);
-        const Keyframe &keyframe1(m_keyframes[idx + 1]);
+        const float t0 = m_localTimes[lowerBound];
+        const float t1 = m_localTimes[lowerBound + 1];
+        const Keyframe &keyframe0(m_keyframes[lowerBound]);
+        const Keyframe &keyframe1(m_keyframes[lowerBound + 1]);
 
         switch (keyframe0.interpolation) {
         case QKeyFrame::ConstantInterpolation:
@@ -92,6 +97,52 @@ float FCurve::evaluateAtTime(float localTime) const
     return m_keyframes.first().value;
 }
 
+float FCurve::evaluateAtTimeAsSlerp(float localTime, int lowerBound, float omega) const
+{
+    // TODO: Implement extrapolation beyond first/last keyframes
+    if (localTime < m_localTimes.first())
+        return m_keyframes.first().value;
+
+    if (localTime > m_localTimes.last())
+        return m_keyframes.last().value;
+    // Find keyframes that sandwich the requested localTime
+    if (lowerBound < 0) // only one keyframe
+        return m_keyframes.first().value;
+
+    const float t0 = m_localTimes[lowerBound];
+    const float t1 = m_localTimes[lowerBound + 1];
+    const Keyframe &keyframe0(m_keyframes[lowerBound]);
+    const Keyframe &keyframe1(m_keyframes[lowerBound + 1]);
+
+    switch (keyframe0.interpolation) {
+    case QKeyFrame::ConstantInterpolation:
+        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;
+        }
+        break;
+    case QKeyFrame::BezierInterpolation:
+        // TODO implement a proper slerp bezier interpolation
+        BezierEvaluator evaluator(t0, keyframe0, t1, keyframe1);
+        return evaluator.valueForTime(localTime);
+    }
+
+    return m_keyframes.first().value;
+}
+
+int FCurve::lowerKeyframeBound(float localTime) const
+{
+    if (localTime < m_localTimes.first())
+        return 0;
+    if (localTime > m_localTimes.last())
+        return 0;
+    return m_rangeFinder.findLowerBound(localTime);
+}
+
 float FCurve::startTime() const
 {
     if (!m_localTimes.isEmpty())
diff --git a/src/animation/backend/fcurve_p.h b/src/animation/backend/fcurve_p.h
index f96c98c1c..337eb615d 100644
--- a/src/animation/backend/fcurve_p.h
+++ b/src/animation/backend/fcurve_p.h
@@ -83,6 +83,9 @@ public:
     float endTime() const;
 
     float evaluateAtTime(float localTime) const;
+    float evaluateAtTime(float localTime, int lowerBound) const;
+    float evaluateAtTimeAsSlerp(float localTime, int lowerBound, float omega) const;
+    int lowerKeyframeBound(float localTime) const;
 
     void read(const QJsonObject &json);
     void setFromQChannelComponent(const QChannelComponent &qcc);
diff --git a/src/animation/frontend/qchannel.cpp b/src/animation/frontend/qchannel.cpp
index 801ad385b..eab7f0df4 100644
--- a/src/animation/frontend/qchannel.cpp
+++ b/src/animation/frontend/qchannel.cpp
@@ -53,6 +53,16 @@ public:
     int m_jointIndex = -1;
 };
 
+/*!
+    \class QChannel
+    \inmodule Qt3DAnimation
+    \brief Defines a channel for a QAnimationClipData.
+    The animation system interpolates each channel component independently
+    except in the case the QChannel is called "Rotation" (case sensitive),
+    it has four QChannelComponents and the same number of keyframes for
+    each QChannelComponent. In that case the interpolation will be performed
+    using SLERP.
+*/
 QChannel::QChannel()
     : d(new QChannelPrivate)
 {
@@ -138,14 +148,12 @@ QChannel::const_iterator QChannel::end() const Q_DECL_NOTHROW
 
 bool operator==(const QChannel &lhs, const QChannel &rhs) Q_DECL_NOTHROW
 {
-    return lhs.d->m_name == rhs.d->m_name &&
-           lhs.d->m_channelComponents == rhs.d->m_channelComponents;
+    return lhs.d->m_name == rhs.d->m_name && lhs.d->m_channelComponents == rhs.d->m_channelComponents;
 }
 
 bool operator!=(const QChannel &lhs, const QChannel &rhs) Q_DECL_NOTHROW
 {
-    return lhs.d->m_name != rhs.d->m_name ||
-           lhs.d->m_channelComponents != rhs.d->m_channelComponents;
+    return lhs.d->m_name != rhs.d->m_name || lhs.d->m_channelComponents != rhs.d->m_channelComponents;
 }
 
 } // namespace Qt3DAnimation
diff --git a/tests/auto/animation/animationutils/animationutils.qrc b/tests/auto/animation/animationutils/animationutils.qrc
index 0b499ed76..af041e8a3 100644
--- a/tests/auto/animation/animationutils/animationutils.qrc
+++ b/tests/auto/animation/animationutils/animationutils.qrc
@@ -5,5 +5,6 @@
         <file>clip3.json</file>
         <file>clip4.json</file>
         <file>clip5.json</file>
+        <file>clip6.json</file>
     </qresource>
 </RCC>
diff --git a/tests/auto/animation/animationutils/clip2.json b/tests/auto/animation/animationutils/clip2.json
index 3faff409c..4c70f8493 100644
--- a/tests/auto/animation/animationutils/clip2.json
+++ b/tests/auto/animation/animationutils/clip2.json
@@ -242,9 +242,9 @@
               ]
             }
           ],
-          "channelName": "Rotation"
+          "channelName": "rotation"
         }
       ]
     }
   ]
-}
\ No newline at end of file
+}
diff --git a/tests/auto/animation/animationutils/clip6.json b/tests/auto/animation/animationutils/clip6.json
new file mode 100644
index 000000000..ba521df55
--- /dev/null
+++ b/tests/auto/animation/animationutils/clip6.json
@@ -0,0 +1,82 @@
+{
+  "animations": [
+    {
+      "animationName": "Rotation",
+      "channels": [
+        {
+          "channelComponents": [
+            {
+              "channelComponentName": "W",
+              "keyFrames": [
+                {
+                  "coords": [
+                    0.0,
+                    1.0
+                  ]
+                },
+                {
+                  "coords": [
+                    10.0,
+                    0.707
+                  ]
+                }
+              ]
+            },
+            {
+              "channelComponentName": "X",
+              "keyFrames": [
+                {
+                  "coords": [
+                    0.0,
+                    0.0
+                  ]
+                },
+                {
+                  "coords": [
+                    10.0,
+                    0.707
+                  ]
+                }
+              ]
+            },
+            {
+              "channelComponentName": "Y",
+              "keyFrames": [
+                {
+                  "coords": [
+                    0.0,
+                    0.0
+                  ]
+                },
+                {
+                  "coords": [
+                    10.0,
+                    0.0
+                  ]
+                }
+              ]
+            },
+            {
+              "channelComponentName": "Z",
+              "keyFrames": [
+                {
+                  "coords": [
+                    0.0,
+                    0.0
+                  ]
+                },
+                {
+                  "coords": [
+                    10.0,
+                    0.0
+                  ]
+                }
+              ]
+            }
+          ],
+          "channelName": "Rotation"
+        }
+      ]
+    }
+  ]
+}
diff --git a/tests/auto/animation/animationutils/tst_animationutils.cpp b/tests/auto/animation/animationutils/tst_animationutils.cpp
index 75ae65ea7..ee393b366 100644
--- a/tests/auto/animation/animationutils/tst_animationutils.cpp
+++ b/tests/auto/animation/animationutils/tst_animationutils.cpp
@@ -1248,6 +1248,17 @@ private Q_SLOTS:
                     << handler << clip << localTime << expectedResults;
             expectedResults.clear();
         }
+        {
+            // a clip with slerp interpolation
+            handler = new Handler();
+            clip = createAnimationClipLoader(handler, QUrl("qrc:/clip6.json"));
+            localTime = clip->duration() / 2.0f;
+            expectedResults = QVector<float>() << 0.923822f << 0.382626f << 0.0f << 0.0f;
+
+            QTest::newRow("clip6.json, slerp, t = duration/2")
+                    << handler << clip << localTime << expectedResults;
+            expectedResults.clear();
+        }
     }
 
     void checkEvaluateClipAtLocalTime()