diff --git a/src/lol/math/vector.h b/src/lol/math/vector.h index 49d79616..9d9ce076 100644 --- a/src/lol/math/vector.h +++ b/src/lol/math/vector.h @@ -934,7 +934,6 @@ template struct Quat static Quat rotate(T angle, T x, T y, T z); static Quat rotate(T angle, Vec3 const &v); - static Quat slerp(Quat QuatA,Quat QuatB, float const &Scalar); /* Convert from Euler angles. The axes in fromeuler_xyx are * x, then y', then x", ie. the axes are attached to the model. @@ -1019,11 +1018,14 @@ static inline Quat operator /(T x, Quat const &y) } template -static inline Quat operator /(Quat x, Quat const &y) +static inline Quat operator /(Quat const &x, Quat const &y) { return x * re(y); } +template +extern Quat slerp(Quat const &qa, Quat const &qb, T f); + /* * Common operators for all vector types, including quaternions */ diff --git a/src/math/vector.cpp b/src/math/vector.cpp index 21edf220..3066de3e 100644 --- a/src/math/vector.cpp +++ b/src/math/vector.cpp @@ -486,32 +486,25 @@ template<> quat quat::rotate(float angle, float x, float y, float z) return quat::rotate(angle, vec3(x, y, z)); } -template<> quat quat::slerp(quat QuatA, quat QuatB, float const &Scalar) -{ - float magnitude = lol::sqrt(sqlength(QuatA) * sqlength(QuatB)); - //btAssert(magnitude > btScalar(0)); - - float product = lol::dot(QuatA,QuatB) / magnitude; - if (product > -1.0f && product < 1.0f) - { - // Take care of long angle case see http://en.wikipedia.org/wiki/Slerp - const float sign = (product < 0.0f) ? -1.0f : 1.0f; - - const float theta = lol::acos(sign * product); - const float s1 = lol::sin(sign * Scalar * theta); - const float d = 1.0f / lol::sin(theta); - const float s0 = lol::sin((1.0f - Scalar) * theta); - - return quat( - (QuatA.w * s0 + QuatB.w * s1) * d, - (QuatA.x * s0 + QuatB.x * s1) * d, - (QuatA.y * s0 + QuatB.y * s1) * d, - (QuatA.z * s0 + QuatB.z * s1) * d); - } - else - { - return QuatA; - } +template<> quat slerp(quat const &qa, quat const &qb, float f) +{ + float const magnitude = lol::sqrt(sqlength(qa) * sqlength(qb)); + float const product = lol::dot(qa, qb) / magnitude; + + /* If quaternions are equal or opposite, there is no need + * to slerp anything, just return qa. */ + if (std::abs(product) >= 1.0f) + return qa; + + float const sign = (product < 0.0f) ? -1.0f : 1.0f; + float const theta = lol::acos(sign * product); + float const s1 = lol::sin(sign * f * theta); + float const s0 = lol::sin((1.0f - f) * theta); + + /* This is the same as 1/sin(theta) */ + float const d = 1.0f / lol::sqrt(1.f - product * product); + + return qa * (s0 * d) + qb * (s1 * d); } template<> vec3 vec3::toeuler(quat const &q)