1 files changed, 17 insertions, 36 deletions

diff --git a/extern/bullet2/src/LinearMath/btTransformUtil.h b/extern/bullet2/src/LinearMath/btTransformUtil.h
index e8328da4ca6..626110ce4ee 100644
--- a/extern/bullet2/src/LinearMath/btTransformUtil.h
+++ b/extern/bullet2/src/LinearMath/btTransformUtil.h
@@ -21,9 +21,6 @@ subject to the following restrictions:
 
 
 
-#define SIMDSQRT12 btScalar(0.7071067811865475244008443621048490)
-
-#define btRecipSqrt(x) ((btScalar)(btScalar(1.0)/btSqrt(btScalar(x))))		/* reciprocal square root */
 
 SIMD_FORCE_INLINE btVector3 btAabbSupport(const btVector3& halfExtents,const btVector3& supportDir)
 {
@@ -33,25 +30,7 @@ SIMD_FORCE_INLINE btVector3 btAabbSupport(const btVector3& halfExtents,const btV
 }
 
 
-SIMD_FORCE_INLINE void btPlaneSpace1 (const btVector3& n, btVector3& p, btVector3& q)
-{
-  if (btFabs(n.z()) > SIMDSQRT12) {
-    // choose p in y-z plane
-    btScalar a = n[1]*n[1] + n[2]*n[2];
-    btScalar k = btRecipSqrt (a);
-    p.setValue(0,-n[2]*k,n[1]*k);
-    // set q = n x p
-    q.setValue(a*k,-n[0]*p[2],n[0]*p[1]);
-  }
-  else {
-    // choose p in x-y plane
-    btScalar a = n.x()*n.x() + n.y()*n.y();
-    btScalar k = btRecipSqrt (a);
-    p.setValue(-n.y()*k,n.x()*k,0);
-    // set q = n x p
-    q.setValue(-n.z()*p.y(),n.z()*p.x(),a*k);
-  }
-}
+
 
 
 
@@ -117,10 +96,8 @@ public:
 
 	static void calculateDiffAxisAngleQuaternion(const btQuaternion& orn0,const btQuaternion& orn1a,btVector3& axis,btScalar& angle)
 	{
-		btQuaternion orn1 = orn0.farthest(orn1a);
+		btQuaternion orn1 = orn0.nearest(orn1a);
 		btQuaternion dorn = orn1 * orn0.inverse();
-		///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.);
@@ -209,7 +186,7 @@ public:
 			btTransformUtil::calculateVelocityQuaternion(m_posB,toPosB,m_ornB,toOrnB,btScalar(1.),linVelB,angVelB);
 			btScalar maxAngularProjectedVelocity = angVelA.length() * m_boundingRadiusA + angVelB.length() * m_boundingRadiusB;
 			btVector3 relLinVel = (linVelB-linVelA);
-			btScalar relLinVelocLength = (linVelB-linVelA).dot(m_separatingNormal);
+			btScalar relLinVelocLength = relLinVel.dot(m_separatingNormal);
 			if (relLinVelocLength<0.f)
 			{
 				relLinVelocLength = 0.f;
@@ -227,17 +204,21 @@ public:
 
 	void	initSeparatingDistance(const btVector3& separatingVector,btScalar separatingDistance,const btTransform& transA,const btTransform& transB)
 	{
-		m_separatingNormal = separatingVector;
 		m_separatingDistance = separatingDistance;
-		
-		const btVector3& toPosA = transA.getOrigin();
-		const btVector3& toPosB = transB.getOrigin();
-		btQuaternion toOrnA = transA.getRotation();
-		btQuaternion toOrnB = transB.getRotation();
-		m_posA = toPosA;
-		m_posB = toPosB;
-		m_ornA = toOrnA;
-		m_ornB = toOrnB;
+
+		if (m_separatingDistance>0.f)
+		{
+			m_separatingNormal = separatingVector;
+			
+			const btVector3& toPosA = transA.getOrigin();
+			const btVector3& toPosB = transB.getOrigin();
+			btQuaternion toOrnA = transA.getRotation();
+			btQuaternion toOrnB = transB.getRotation();
+			m_posA = toPosA;
+			m_posB = toPosB;
+			m_ornA = toOrnA;
+			m_ornB = toOrnB;
+		}
 	}
 
 };