Merge 3b5ba3f21f into 3adc93888b

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,62 @@ 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 ensures shortest path when dot < 0 */
+    /* diff = d = p^*q */
+    const DualQuaternion<T> diff = from.quaternionConjugated()*((dotResult < T(0)) ? -to : to);
+
+    /* angle = t*a_D */
+    const T angle = std::acos(diff.real().scalar())*t;
+    /* Precompute sin/cos for manifold use */
+    const T sinAngle = std::sin(angle);
+    const T cosAngle = std::cos(angle);
+
+    /* Merely a shortcut */
+    const Vector3<T>& m = diff.real().vector();
+    /* invr = \frac 1 {|l_V|} */
+    const T invr = m.lengthInverted();
+    /* direction = n = l_V * 1/|l_V| */
+    const Vector3<T> direction = m*invr;
+
+    /* Vector of real part of q_{ScLERP} = n_R*sin(t*a_R/2)*/
+    const Vector3<T> v = direction*sinAngle;
+
+    /* pitch = a_D/2 = m_S*1/|l_V| */
+    const T pitch = -diff.dual().scalar()*invr;
+    /* moment = n_D */
+    const Vector3<T> moment = (diff.dual().vector() - (direction*(pitch*diff.real().scalar())))*invr;
+    const T pitchT = pitch*t;
+    /* Vector of dual part of q_{ScLERP} */
+    const Vector3<T> v2 = moment*sinAngle + direction*(pitchT*cosAngle);
+
+    return from*DualQuaternion<T>{Quaternion<T>{v, cosAngle}, Quaternion<T>{v2, -pitchT*sinAngle}};
+}
+
 /**
 @brief Dual quaternion
 @tparam T   Underlying data type

src/Magnum/Math/Quaternion.h

@@ -89,7 +89,7 @@ Expects that both quaternions are normalized. @f[
 @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 Quaternion<T> lerp(const Quaternion<T>& normalizedA, const Quaternion<T>& normalizedB, T t) {
     CORRADE_ASSERT(normalizedA.isNormalized() && normalizedB.isNormalized(),
         "Math::lerp(): quaternions must be normalized", {});

src/Magnum/Math/Test/DualQuaternionTest.cpp

@@ -86,6 +86,8 @@ struct DualQuaternionTest: Corrade::TestSuite::Tester {
     void transformPoint();
     void transformPointNormalized();
 
+    void sclerp();
+
     void debug();
 };
 
@@ -125,6 +127,8 @@ DualQuaternionTest::DualQuaternionTest() {
               &DualQuaternionTest::transformPoint,
               &DualQuaternionTest::transformPointNormalized,
 
+              &DualQuaternionTest::sclerp,
+
               &DualQuaternionTest::debug});
 }
 
@@ -383,6 +387,27 @@ void DualQuaternionTest::debug() {
     CORRADE_COMPARE(o.str(), "DualQuaternion({{1, 2, 3}, -4}, {{0.5, -3.1, 3.3}, 2})\n");
 }
 
+void DualQuaternionTest::sclerp() {
+
+    const DualQuaternion from = DualQuaternion::translation(Vector3{20.0f, .0f, .0f})*DualQuaternion::rotation(180.0_degf, Vector3{.0f, 1.0f, .0f});
+    const DualQuaternion to = DualQuaternion::translation(Vector3{42.0f, 42.0f, 42.0f})*DualQuaternion::rotation(75.0_degf, Vector3{1.0f, .0f, .0f});
+
+    constexpr DualQuaternion expected1{Quaternion{{.23296291314453416f, .9238795325112867f, .0f}, .303603179340959f},
+                                       Quaternion{{2.235619101917766f, 2.8169719855488395f, 10.722240915237789f}, -10.287636336847847f}};
+    constexpr DualQuaternion expected2{Quaternion{{.4437679833315842f, .6845471059286887f, .0f}, .5783296955322937f},
+                                       Quaternion{{5.764394870292371f, 11.161306653193549f, 9.671267015501789f}, -17.634394590712066f}};
+    constexpr DualQuaternion expected3{Quaternion{{.5979785904506439f, .18738131458572468f, .0f}, .7793008714910992f},
+                                       Quaternion{{13.409627907069353f, 25.452124456683414f, 5.681581047706807f}, -16.409481115504978f}};
+
+    const DualQuaternion interp1 = Math::sclerp(from, to, 0.25f);
+    const DualQuaternion interp2 = Math::sclerp(from, to, 0.52f);
+    const DualQuaternion interp3 = Math::sclerp(from, to, 0.88f);
+
+    CORRADE_COMPARE(interp1, expected1);
+    CORRADE_COMPARE(interp2, expected2);
+    CORRADE_COMPARE(interp3, expected3);
+}
+
 }}}
 
 CORRADE_TEST_MAIN(Magnum::Math::Test::DualQuaternionTest)