From f06b53724c449a46afb7d5b57cd425e413971a81 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Vladim=C3=ADr=20Vondru=C5=A1?= <mosra@centrum.cz>
Date: Sun, 6 Jan 2013 19:35:38 +0100
Subject: [PATCH] Math: spherical linear Quaternion interpolation.

---
 src/Math/Quaternion.h            | 27 ++++++++++++++++++++++++++-
 src/Math/Test/QuaternionTest.cpp | 24 ++++++++++++++++++++++++
 2 files changed, 50 insertions(+), 1 deletion(-)

diff --git a/src/Math/Quaternion.h b/src/Math/Quaternion.h
index 794677ed3..737f3978c 100644
--- a/src/Math/Quaternion.h
+++ b/src/Math/Quaternion.h
@@ -56,7 +56,7 @@ template<class T> class Quaternion {
         inline static T angle(const Quaternion<T>& normalizedA, const Quaternion<T>& normalizedB) {
             CORRADE_ASSERT(MathTypeTraits<T>::equals(normalizedA.dot(), T(1)) && MathTypeTraits<T>::equals(normalizedB.dot(), T(1)),
                            "Math::Quaternion::angle(): quaternions must be normalized", std::numeric_limits<T>::quiet_NaN());
-            return std::acos(dot(normalizedA, normalizedB));
+            return angleInternal(normalizedA, normalizedB);
         }
 
         /**
@@ -76,6 +76,26 @@ template<class T> class Quaternion {
             return ((T(1) - t)*normalizedA + t*normalizedB).normalized();
         }
 
+        /**
+         * @brief Spherical linear interpolation of two quaternions
+         * @param normalizedA   First quaternion
+         * @param normalizedB   Second quaternion
+         * @param t             Interpolation phase (from range @f$ [0; 1] @f$)
+         *
+         * Expects that both quaternions are normalized. @f[
+         *      q_{SLERP} = \frac{sin((1 - t) \theta) q_A + sin(t \theta) q_B}{sin \theta}
+         *      ~~~~~~~~~~
+         *      \theta = acos \left( \frac{q_A \cdot q_B}{|q_A| \cdot |q_B|} \right)
+         * @f]
+         */
+        inline static Quaternion<T> slerp(const Quaternion<T>& normalizedA, const Quaternion<T>& normalizedB, T t) {
+            CORRADE_ASSERT(MathTypeTraits<T>::equals(normalizedA.dot(), T(1)) && MathTypeTraits<T>::equals(normalizedB.dot(), T(1)),
+                           "Math::Quaternion::slerp(): quaternions must be normalized",
+                           Quaternion<T>({}, std::numeric_limits<T>::quiet_NaN()));
+            T a = angleInternal(normalizedA, normalizedB);
+            return (std::sin((T(1) - t)*a)*normalizedA + std::sin(t*a)*normalizedB)/std::sin(a);
+        }
+
         /**
          * @brief Create quaternion from rotation
          * @param angle             Rotation angle (counterclockwise, in radians)
@@ -344,6 +364,11 @@ template<class T> class Quaternion {
         }
 
     private:
+        /* Used in angle() and slerp() (no assertions) */
+        inline static T angleInternal(const Quaternion<T>& normalizedA, const Quaternion<T>& normalizedB) {
+            return std::acos(dot(normalizedA, normalizedB));
+        }
+
         Vector3<T> _vector;
         T _scalar;
 };
diff --git a/src/Math/Test/QuaternionTest.cpp b/src/Math/Test/QuaternionTest.cpp
index 4f1422084..40176ec9f 100644
--- a/src/Math/Test/QuaternionTest.cpp
+++ b/src/Math/Test/QuaternionTest.cpp
@@ -42,6 +42,7 @@ class QuaternionTest: public Corrade::TestSuite::Tester {
         void angle();
         void matrix();
         void lerp();
+        void slerp();
 
         void debug();
 };
@@ -66,6 +67,7 @@ QuaternionTest::QuaternionTest() {
              &QuaternionTest::angle,
              &QuaternionTest::matrix,
              &QuaternionTest::lerp,
+             &QuaternionTest::slerp,
              &QuaternionTest::debug);
 }
 
@@ -223,6 +225,28 @@ void QuaternionTest::lerp() {
     CORRADE_COMPARE(lerp, Quaternion({0.119127f, 0.049134f, 0.049134f}, 0.990445f));
 }
 
+void QuaternionTest::slerp() {
+    Quaternion a = Quaternion::fromRotation(deg(15.0f), Vector3(1.0f/Constants<float>::sqrt3()));
+    Quaternion b = Quaternion::fromRotation(deg(23.0f), Vector3::xAxis());
+
+    std::ostringstream o;
+    Corrade::Utility::Error::setOutput(&o);
+
+    Quaternion notSlerpA = Quaternion::slerp(a*3.0f, b, 0.35f);
+    CORRADE_COMPARE(notSlerpA.vector(), Vector3());
+    CORRADE_COMPARE(notSlerpA.scalar(), std::numeric_limits<float>::quiet_NaN());
+    CORRADE_COMPARE(o.str(), "Math::Quaternion::slerp(): quaternions must be normalized\n");
+
+    o.str("");
+    Quaternion notSlerpB = Quaternion::slerp(a, b*-3.0f, 0.35f);
+    CORRADE_COMPARE(notSlerpB.vector(), Vector3());
+    CORRADE_COMPARE(notSlerpB.scalar(), std::numeric_limits<float>::quiet_NaN());
+    CORRADE_COMPARE(o.str(), "Math::Quaternion::slerp(): quaternions must be normalized\n");
+
+    Quaternion slerp = Quaternion::slerp(a, b, 0.35f);
+    CORRADE_COMPARE(slerp, Quaternion({0.119165f, 0.0491109f, 0.0491109f}, 0.990442f));
+}
+
 void QuaternionTest::debug() {
     std::ostringstream o;