diff --git a/src/Magnum/Math/DualQuaternion.h b/src/Magnum/Math/DualQuaternion.h index 198591bdb..c289c9a27 100644 --- a/src/Magnum/Math/DualQuaternion.h +++ b/src/Magnum/Math/DualQuaternion.h @@ -69,26 +69,22 @@ template inline DualQuaternion sclerp(const DualQuaternion& from, /* Multiplying with -1.0f ensures shortest path when dot < 0. */ const DualQuaternion diff = from.quaternionConjugated()*((dotResult < .0) ? -to : to); - const Vector3 diffReal = diff.real().vector(); - const Vector3 diffDual = diff.dual().vector(); + const T angle = acos(diff.real().scalar())*t; + const T sinAngle = sin(angle); + const T cosAngle = cos(angle); + const Vector3& diffReal = diff.real().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 direction = diffReal*invr; - const Vector3 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 v = direction*sinAngle; - const Vector3 v2 = moment*sinAngle + direction*(pitch*.5f*cosAngle); - return from*DualQuaternion{Quaternion{v, cosAngle}, Quaternion{v2, -pitch*.5f*sinAngle}}; + T pitch = -diff.dual().scalar()*invr; + const Vector3 moment = (diff.dual().vector() - (direction*(pitch*diff.real().scalar())))*invr; + pitch *= t; + const Vector3 v2 = moment*sinAngle + direction*(pitch*cosAngle); + + return from*DualQuaternion{Quaternion{v, cosAngle}, Quaternion{v2, -pitch*sinAngle}}; } /**