From 7d78c7b30ebb0f468fae560ee7d2b78b4158a1c0 Mon Sep 17 00:00:00 2001
From: Squareys <Squareys@googlemail.com>
Date: Wed, 7 Oct 2015 22:02:02 +0200
Subject: [PATCH] Math: Implement DualQuaternion screw interpolation (ScLERP).

Signed-off-by: Squareys <Squareys@googlemail.com>
---
 src/Magnum/Math/DualQuaternion.h | 52 ++++++++++++++++++++++++++++++++
 1 file changed, 52 insertions(+)

diff --git a/src/Magnum/Math/DualQuaternion.h b/src/Magnum/Math/DualQuaternion.h
index 526f91598..198591bdb 100644
--- a/src/Magnum/Math/DualQuaternion.h
+++ b/src/Magnum/Math/DualQuaternion.h
@@ -29,6 +29,8 @@
  * @brief Class @ref Magnum::Math::DualQuaternion
  */
 
+#include <cmath>
+
 #include "Magnum/Math/Dual.h"
 #include "Magnum/Math/Matrix4.h"
 #include "Magnum/Math/Quaternion.h"
@@ -39,6 +41,56 @@ namespace Implementation {
     template<class, class> struct DualQuaternionConverter;
 }
 
+/** @relatesalso Dual quaternion
+@brief Screw linear interpolation of two dual quaternions
+@param from   First quaternion
+@param to     Second quaternion
+@param t      Interpolation phase (from range @f$ [0; 1] @f$)
+
+Expects that the real parts of both dual quaternions are normalized. @f[
+\begin{array}{l}
+    {\hat q}_{ScLERP} = \hat p (\hat p^* \hat q)^t =
+        cos \left( t \frac {\hat a} 2 \right) + \hat {\boldsymbol n} \cdot sin \left( t \frac {\hat a} 2 \right) \\
+    \hat d = p^*q = [l, m] \\
+    \hat a = [\hat a_R, \hat a_D]; \hat a_R = 2 \cdot acos \left( l_S \right); \hat a_D = -2m_S \frac 1 {|l_V|} \\
+    \hat {\boldsymbol n} = [\hat l_V \frac 1 {|l_V|},
+        \left( m_V - \hat {\boldsymbol n}_R \frac {\hat a_D l_S} 2 \right)\frac 1 {|l_V|}]
+\end{array}
+@f]
+@see @ref Quaternion::isNormalized(), @ref slerp(const Quaternion<T>&, const Quaternion<T>&, T),
+    @ref lerp(const T&, const T&, U)
+*/
+template<class T> inline DualQuaternion<T> sclerp(const DualQuaternion<T>& from, const DualQuaternion<T>& to, const T t) {
+    CORRADE_ASSERT(from.real().isNormalized() && to.real().isNormalized(),
+        "Math::sclerp(): real parts of quaternions must be normalized", {});
+
+    const T dotResult = dot(from.real().vector(), to.real().vector());
+
+    /* Multiplying with -1.0f ensures shortest path when dot < 0. */
+    const DualQuaternion<T> diff = from.quaternionConjugated()*((dotResult < .0) ? -to : to);
+
+    const Vector3<T> diffReal = diff.real().vector();
+    const Vector3<T> diffDual = diff.dual().vector();
+
+    const T invr = 1/std::sqrt(diffReal.dot());
+
+    T angle = 2*acos(diff.real().scalar());
+    T pitch = -2*diff.dual().scalar()*invr;
+    const Vector3<T> direction = diffReal*invr;
+    const Vector3<T> moment = (diffDual - (direction*(pitch*diff.real().scalar()*.5f)))*invr;
+
+    angle *= t;
+    pitch *= t;
+
+    const T sinAngle = sin(.5f*angle);
+    const T cosAngle = cos(.5f*angle);
+
+    const Vector3<T> v = direction*sinAngle;
+    const Vector3<T> v2 = moment*sinAngle + direction*(pitch*.5f*cosAngle);
+
+    return from*DualQuaternion<T>{Quaternion<T>{v, cosAngle}, Quaternion<T>{v2, -pitch*.5f*sinAngle}};
+}
+
 /**
 @brief Dual quaternion
 @tparam T   Underlying data type