diff --git a/src/Magnum/Math/DualQuaternion.h b/src/Magnum/Math/DualQuaternion.h index 0ff09de7b..a173950bc 100644 --- a/src/Magnum/Math/DualQuaternion.h +++ b/src/Magnum/Math/DualQuaternion.h @@ -41,60 +41,51 @@ namespace Implementation { template struct DualQuaternionConverter; } -/** @relatesalso Dual quaternion +/** @relatesalso DualQuaternion @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|}] +@param normalizedA First dual quaternion +@param normalizedB Second dual quaternion +@param t Interpolation phase (from range @f$ [0; 1] @f$) + +Expects that both dual quaternions are normalized. @f[ +\begin{array}{rcl} + l + \epsilon m & = & \hat q_A^* \hat q_B \\ + \frac{\hat a} 2 & = & acos \left( l_S \right) - \epsilon m_S \frac 1 {|l_V|} \\ + \hat {\boldsymbol n} & = & \boldsymbol n_0 + \epsilon \boldsymbol n_\epsilon + ~~~~~~~~ \boldsymbol n_0 = l_V \frac 1 {|l_V|} + ~~~~~~~~ \boldsymbol n_\epsilon = \left( m_V - {\boldsymbol n}_0 \frac {a_\epsilon} 2 l_S \right)\frac 1 {|l_V|} \\ + {\hat q}_{ScLERP} & = & \hat q_A (\hat q_A^* \hat q_B)^t = + \hat q_A \left[ \hat {\boldsymbol n} sin \left( t \frac {\hat a} 2 \right), cos \left( t \frac {\hat a} 2 \right) \right] \\ \end{array} @f] -@see @ref Quaternion::isNormalized(), @ref slerp(const Quaternion&, const Quaternion&, T), +@see @ref DualQuaternion::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}}; +template inline DualQuaternion sclerp(const DualQuaternion& normalizedA, const DualQuaternion& normalizedB, const T t) { + CORRADE_ASSERT(normalizedA.isNormalized() && normalizedB.isNormalized(), + "Math::sclerp(): dual quaternions must be normalized", {}); + const T dotResult = dot(normalizedA.real().vector(), normalizedB.real().vector()); + + /* l + εm = q_A^**q_B, multiplying with -1 ensures shortest path when dot < 0 */ + const DualQuaternion diff = normalizedA.quaternionConjugated()*(dotResult < T(0) ? -normalizedB : normalizedB); + const Quaternion& l = diff.real(); + const Quaternion& m = diff.dual(); + + /* a/2 = acos(l_S) - εm_S/|l_V| */ + const T invr = l.vector().lengthInverted(); + const Dual aHalf{std::acos(l.scalar()), -m.scalar()*invr}; + + /* direction = n_0 = l_V/|l_V| + moment = n_ε = (m_V - n_0*(a_ε/2)*l_S)/|l_V| */ + const Vector3 direction = l.vector()*invr; + const Vector3 moment = (m.vector() - direction*(aHalf.dual()*l.scalar()))*invr; + const Dual> n{direction, moment}; + + /* q_ScLERP = q_A*(cos(t*a/2) + n*sin(t*a/2)) */ + Dual sin, cos; + std::tie(sin, cos) = Math::sincos(t*Dual>(aHalf)); + return normalizedA*DualQuaternion{n*sin, cos}; } /**