From a71e9c3e5a744eb7f9f3ad95c634a91ec7324360 Mon Sep 17 00:00:00 2001
From: ejcoumans <ejcoumans@08e121b0-ae19-0410-a57b-3be3395fd4fd>
Date: Tue, 11 Sep 2007 02:49:39 +0000
Subject: [PATCH] fixed issue with de-normalized quaternion, causing acosf to
 fail See http://www.bulletphysics.com/Bullet/phpBB3/viewtopic.php?f=9&t=1440
 Thanks Proctoid for reporting.

---
 src/LinearMath/btScalar.h        | 5 ++++-
 src/LinearMath/btTransformUtil.h | 4 ++++
 2 files changed, 8 insertions(+), 1 deletion(-)

diff --git a/src/LinearMath/btScalar.h b/src/LinearMath/btScalar.h
index cd8c6925d..a5036d32d 100644
--- a/src/LinearMath/btScalar.h
+++ b/src/LinearMath/btScalar.h
@@ -116,7 +116,10 @@ SIMD_FORCE_INLINE btScalar btFabs(btScalar x) { return fabsf(x); }
 SIMD_FORCE_INLINE btScalar btCos(btScalar x) { return cosf(x); }
 SIMD_FORCE_INLINE btScalar btSin(btScalar x) { return sinf(x); }
 SIMD_FORCE_INLINE btScalar btTan(btScalar x) { return tanf(x); }
-SIMD_FORCE_INLINE btScalar btAcos(btScalar x) { return acosf(x); }
+SIMD_FORCE_INLINE btScalar btAcos(btScalar x) { 
+	btAssert(x <= btScalar(1.));
+	return acosf(x); 
+}
 SIMD_FORCE_INLINE btScalar btAsin(btScalar x) { return asinf(x); }
 SIMD_FORCE_INLINE btScalar btAtan(btScalar x) { return atanf(x); }
 SIMD_FORCE_INLINE btScalar btAtan2(btScalar x, btScalar y) { return atan2f(x, y); }
diff --git a/src/LinearMath/btTransformUtil.h b/src/LinearMath/btTransformUtil.h
index bc42fd166..8e1929d76 100644
--- a/src/LinearMath/btTransformUtil.h
+++ b/src/LinearMath/btTransformUtil.h
@@ -121,6 +121,10 @@ public:
 		dmat.getRotation(dorn);
 #endif//USE_QUATERNION_DIFF
 	
+		///floating point inaccuracy can lead to w component > 1..., which breaks 
+
+		dorn.normalize();
+		
 		angle = dorn.getAngle();
 		axis = btVector3(dorn.x(),dorn.y(),dorn.z());
 		axis[3] = btScalar(0.);