From 6c71f37da14e34f7765f07e6833cb81c9ae8e616 Mon Sep 17 00:00:00 2001 From: T4Larson Date: Wed, 5 Aug 2015 21:06:13 +0200 Subject: [PATCH] fix: btQuaternion.slerp() may result in NaN-quaternions --- src/LinearMath/btQuaternion.h | 38 +++++++++++++++++++---------------- 1 file changed, 21 insertions(+), 17 deletions(-) diff --git a/src/LinearMath/btQuaternion.h b/src/LinearMath/btQuaternion.h index ede769384..7755b4eed 100644 --- a/src/LinearMath/btQuaternion.h +++ b/src/LinearMath/btQuaternion.h @@ -533,25 +533,29 @@ public: * Slerp interpolates assuming constant velocity. */ btQuaternion slerp(const btQuaternion& q, const btScalar& t) const { - btScalar magnitude = btSqrt(length2() * q.length2()); - btAssert(magnitude > btScalar(0)); - btScalar product = dot(q) / magnitude; - if (btFabs(product) < btScalar(1)) + const btScalar magnitude = btSqrt(length2() * q.length2()); + btAssert(magnitude > btScalar(0)); + + const btScalar product = dot(q) / magnitude; + const btScalar absproduct = btFabs(product); + + if(absproduct < btScalar(1.0 - SIMD_EPSILON)) { - // 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); + // Take care of long angle case see http://en.wikipedia.org/wiki/Slerp + const btScalar theta = btAcos(absproduct); + const btScalar d = btSin(theta); + btAssert(d > btScalar(0)) + + const btScalar sign = (product < 0) ? btScalar(-1) : btScalar(1); + const btScalar s0 = btSin((btScalar(1.0) - t) * theta) / d; + const btScalar s1 = btSin(sign * t * theta) / d; + + return btQuaternion( + (m_floats[0] * s0 + q.x() * s1), + (m_floats[1] * s0 + q.y() * s1), + (m_floats[2] * s0 + q.z() * s1), + (m_floats[3] * s0 + q.w() * s1)); } else {