diff --git a/src/Magnum/Math/DualQuaternion.h b/src/Magnum/Math/DualQuaternion.h index 526f91598..621450662 100644 --- a/src/Magnum/Math/DualQuaternion.h +++ b/src/Magnum/Math/DualQuaternion.h @@ -29,6 +29,8 @@ * @brief Class @ref Magnum::Math::DualQuaternion */ +#include + #include "Magnum/Math/Dual.h" #include "Magnum/Math/Matrix4.h" #include "Magnum/Math/Quaternion.h" @@ -39,6 +41,62 @@ namespace Implementation { template 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&, const Quaternion&, T), + @ref lerp(const T&, const T&, U) +*/ +template inline DualQuaternion sclerp(const DualQuaternion& from, const DualQuaternion& 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 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& m = diff.real().vector(); + /* invr = \frac 1 {|l_V|} */ + const T invr = m.lengthInverted(); + /* direction = n = l_V * 1/|l_V| */ + const Vector3 direction = m*invr; + + /* Vector of real part of q_{ScLERP} = n_R*sin(t*a_R/2)*/ + const Vector3 v = direction*sinAngle; + + /* pitch = a_D/2 = m_S*1/|l_V| */ + const T pitch = -diff.dual().scalar()*invr; + /* moment = n_D */ + const Vector3 moment = (diff.dual().vector() - (direction*(pitch*diff.real().scalar())))*invr; + const T pitchT = pitch*t; + /* Vector of dual part of q_{ScLERP} */ + const Vector3 v2 = moment*sinAngle + direction*(pitchT*cosAngle); + + return from*DualQuaternion{Quaternion{v, cosAngle}, Quaternion{v2, -pitchT*sinAngle}}; +} + /** @brief Dual quaternion @tparam T Underlying data type diff --git a/src/Magnum/Math/Quaternion.h b/src/Magnum/Math/Quaternion.h index b3617931b..c05e0993e 100644 --- a/src/Magnum/Math/Quaternion.h +++ b/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&, const Quaternion&, T), @ref lerp(const T&, const T&, U) - */ +*/ template inline Quaternion lerp(const Quaternion& normalizedA, const Quaternion& normalizedB, T t) { CORRADE_ASSERT(normalizedA.isNormalized() && normalizedB.isNormalized(), "Math::lerp(): quaternions must be normalized", {}); diff --git a/src/Magnum/Math/Test/DualQuaternionTest.cpp b/src/Magnum/Math/Test/DualQuaternionTest.cpp index 69a54ea61..856ca6e53 100644 --- a/src/Magnum/Math/Test/DualQuaternionTest.cpp +++ b/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)