From 8f389d13e7eebc699c932a7db0fc8459cb28dde0 Mon Sep 17 00:00:00 2001 From: "erwin.coumans" Date: Wed, 29 Feb 2012 04:50:39 +0000 Subject: [PATCH] fix btQuaternion::slerp See issue 583. Thanks to macbearchen for the report and joshua downer for the fix --- src/LinearMath/btQuaternion.h | 34 ++++++++++++++++++---------------- 1 file changed, 18 insertions(+), 16 deletions(-) diff --git a/src/LinearMath/btQuaternion.h b/src/LinearMath/btQuaternion.h index b3196a91c..ee79f6eae 100644 --- a/src/LinearMath/btQuaternion.h +++ b/src/LinearMath/btQuaternion.h @@ -280,23 +280,25 @@ public: * Slerp interpolates assuming constant velocity. */ btQuaternion slerp(const btQuaternion& q, const btScalar& t) const { - btScalar theta = angle(q); - if (theta != btScalar(0.0)) + btScalar magnitude = btSqrt(length2() * q.length2()); + btAssert(magnitude > btScalar(0)); + + btScalar product = dot(q) / magnitude; + if (btFabs(product) != btScalar(1)) { - btScalar d = btScalar(1.0) / btSin(theta); - btScalar s0 = btSin((btScalar(1.0) - t) * theta); - btScalar s1 = btSin(t * theta); - if (dot(q) < 0) // Take care of long angle case see http://en.wikipedia.org/wiki/Slerp - return btQuaternion((m_floats[0] * s0 + -q.x() * s1) * d, - (m_floats[1] * s0 + -q.y() * s1) * d, - (m_floats[2] * s0 + -q.z() * s1) * d, - (m_floats[3] * s0 + -q.m_floats[3] * s1) * d); - else - return btQuaternion((m_floats[0] * s0 + q.x() * s1) * d, - (m_floats[1] * s0 + q.y() * s1) * d, - (m_floats[2] * s0 + q.z() * s1) * d, - (m_floats[3] * s0 + q.m_floats[3] * s1) * d); - + // Take care of long angle case see http://en.wikipedia.org/wiki/Slerp + const btScalar sign = (product < 0) ? btScalar(-1) : btScalar(1); + + const btScalar theta = btAcos(sign * product); + const btScalar s1 = btSin(sign * t * theta); + const btScalar d = btScalar(1.0) / btSin(theta); + const btScalar s0 = btSin((btScalar(1.0) - t) * theta); + + return btQuaternion( + (m_floats[0] * s0 + q.x() * s1) * d, + (m_floats[1] * s0 + q.y() * s1) * d, + (m_floats[2] * s0 + q.z() * s1) * d, + (m_floats[3] * s0 + q.m_floats[3] * s1) * d); } else {