diff --git a/src/Magnum/Math/DualQuaternion.h b/src/Magnum/Math/DualQuaternion.h index c289c9a27..3f8f10674 100644 --- a/src/Magnum/Math/DualQuaternion.h +++ b/src/Magnum/Math/DualQuaternion.h @@ -67,21 +67,32 @@ template inline DualQuaternion sclerp(const DualQuaternion& from, const T dotResult = dot(from.real().vector(), to.real().vector()); /* Multiplying with -1.0f ensures shortest path when dot < 0. */ + /* diff = \hat d = p^*q */ const DualQuaternion diff = from.quaternionConjugated()*((dotResult < .0) ? -to : to); + /* angle = t \hat a_D */ const T angle = acos(diff.real().scalar())*t; + /* precompute sin/cos for manifold use */ const T sinAngle = sin(angle); const T cosAngle = cos(angle); - const Vector3& diffReal = diff.real().vector(); - const T invr = 1/std::sqrt(diffReal.dot()); - const Vector3 direction = diffReal*invr; + /* merely a shortcut */ + const Vector3& m = diff.real().vector(); + /* invr = \frac 1 {|l_V|} */ + const T invr = 1/m.length(); + /* direction = \hat {\boldsymbol n} = \hat l_V \frac 1 {|l_V|} */ + const Vector3 direction = m*invr; + /* Vector of real part of q_{ScLERP} + = \hat {\boldsymbol n}_R \cdot sin \left( t \frac {\hat a_R} 2 \right)*/ const Vector3 v = direction*sinAngle; + /* pitch = \frac {\hat a_D} 2 = m_S \frac 1 {|l_V|} */ T pitch = -diff.dual().scalar()*invr; + /* moment = \hat {\boldsymbol n}_D */ const Vector3 moment = (diff.dual().vector() - (direction*(pitch*diff.real().scalar())))*invr; pitch *= t; + /* Vector of dual part of q_{ScLERP} */ const Vector3 v2 = moment*sinAngle + direction*(pitch*cosAngle); return from*DualQuaternion{Quaternion{v, cosAngle}, Quaternion{v2, -pitch*sinAngle}};