More GJK degeneracy fixes, thanks Jacob Langford for the feedback:

http://code.google.com/p/bullet/issues/detail?id=250

Added missing files for Maya Dynamica plugin
Thanks Herbert Law for the patch, and damrit and others for the report
http://code.google.com/p/bullet/issues/detail?id=231

Fix btQuaternion shortestArcQuat, thanks Stan Melax for original fix and shogun for reminder
http://bulletphysics.com/Bullet/phpBB3/viewtopic.php?f=9&t=1989

Implemented btDiscreteDynamicsWorld::removeCollisionObject (and btSoftBodyDynamicsWorld) to avoid
crashes. Thanks Jacob Langford for bringing that up.

Minor sphere-debug drawing issue (spheres were drawn inside-out (wrong face winding)

src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp

@@ -208,7 +208,7 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result&
 			{
 				m_degenerateSimplex = 7;
 				squaredDistance = previousSquaredDistance;
-                checkSimplex = true;
+                checkSimplex = false;
                 break;
 			}
 			
@@ -315,13 +315,11 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result&
 					if (lenSqr > (SIMD_EPSILON*SIMD_EPSILON))
 					{
 						tmpNormalInB /= btSqrt(lenSqr);
-						btScalar distance2 = -(tmpPointOnA-tmpPointOnB).length()-margin;
+						btScalar distance2 = -(tmpPointOnA-tmpPointOnB).length();
 						//only replace valid penetrations when the result is deeper (check)
 						if (!isValid || (distance2 < distance))
 						{
 							distance = distance2;
-							pointOnA -= m_cachedSeparatingAxis * (marginA / distance);
-							pointOnB += m_cachedSeparatingAxis * (marginB / distance);
 							pointOnA = tmpPointOnA;
 							pointOnB = tmpPointOnB;
 							normalInB = tmpNormalInB;
@@ -349,13 +347,15 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result&
 					if (lenSqr > (SIMD_EPSILON*SIMD_EPSILON))
 					{
 						tmpNormalInB /= btSqrt(lenSqr);
-						btScalar distance2 = (tmpPointOnA-tmpPointOnB).length();
+						btScalar distance2 = (tmpPointOnA-tmpPointOnB).length()-margin;
 						//only replace valid distances when the distance is less
 						if (!isValid || (distance2 < distance))
 						{
 							distance = distance2;
 							pointOnA = tmpPointOnA;
 							pointOnB = tmpPointOnB;
+							pointOnA -= tmpNormalInB * marginA ;
+							pointOnB += tmpNormalInB * marginB ;
 							normalInB = tmpNormalInB;
 							isValid = true;
 							m_lastUsedMethod = 6;

src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp

@@ -416,11 +416,19 @@ btVector3 btDiscreteDynamicsWorld::getGravity () const
 	return m_gravity;
 }
 
+void	btDiscreteDynamicsWorld::removeCollisionObject(btCollisionObject* collisionObject)
+{
+	btRigidBody* body = btRigidBody::upcast(collisionObject);
+	if (body)
+		removeRigidBody(body);
+	else
+		btCollisionWorld::removeCollisionObject(collisionObject);
+}
 
 void	btDiscreteDynamicsWorld::removeRigidBody(btRigidBody* body)
 {
 	m_nonStaticRigidBodies.remove(body);
-	removeCollisionObject(body);
+	btCollisionWorld::removeCollisionObject(body);
 }
 
 void	btDiscreteDynamicsWorld::addRigidBody(btRigidBody* body)

src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h

@@ -130,6 +130,9 @@ public:
 
 	virtual void	removeRigidBody(btRigidBody* body);
 
+	///removeCollisionObject will first check if it is a rigid body, if so call removeRigidBody otherwise call btCollisionWorld::removeCollisionObject
+	virtual void	removeCollisionObject(btCollisionObject* collisionObject);
+
 	void	debugDrawObject(const btTransform& worldTransform, const btCollisionShape* shape, const btVector3& color);
 
 	void	debugDrawConstraint(btTypedConstraint* constraint);

src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp

@@ -132,9 +132,19 @@ btVector3 btSimpleDynamicsWorld::getGravity () const
 
 void	btSimpleDynamicsWorld::removeRigidBody(btRigidBody* body)
 {
-	removeCollisionObject(body);
+	btCollisionWorld::removeCollisionObject(body);
 }
 
+void	btSimpleDynamicsWorld::removeCollisionObject(btCollisionObject* collisionObject)
+{
+	btRigidBody* body = btRigidBody::upcast(collisionObject);
+	if (body)
+		removeRigidBody(body);
+	else
+		btCollisionWorld::removeCollisionObject(collisionObject);
+}
+
+
 void	btSimpleDynamicsWorld::addRigidBody(btRigidBody* body)
 {
 	body->setGravity(m_gravity);

src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.h

@@ -57,6 +57,9 @@ public:
 	virtual void	addRigidBody(btRigidBody* body);
 
 	virtual void	removeRigidBody(btRigidBody* body);
+
+	///removeCollisionObject will first check if it is a rigid body, if so call removeRigidBody otherwise call btCollisionWorld::removeCollisionObject
+	virtual void	removeCollisionObject(btCollisionObject* collisionObject);
 	
 	virtual void	updateAabbs();
 

src/BulletSoftBody/btSoftRigidDynamicsWorld.cpp

@@ -119,6 +119,15 @@ void	btSoftRigidDynamicsWorld::removeSoftBody(btSoftBody* body)
 	btCollisionWorld::removeCollisionObject(body);
 }
 
+void	btSoftRigidDynamicsWorld::removeCollisionObject(btCollisionObject* collisionObject)
+{
+	btSoftBody* body = btSoftBody::upcast(collisionObject);
+	if (body)
+		removeSoftBody(body);
+	else
+		btDiscreteDynamicsWorld::removeCollisionObject(collisionObject);
+}
+
 void	btSoftRigidDynamicsWorld::debugDrawWorld()
 {
 	btDiscreteDynamicsWorld::debugDrawWorld();

src/BulletSoftBody/btSoftRigidDynamicsWorld.h

@@ -54,6 +54,9 @@ public:
 
 	void	removeSoftBody(btSoftBody* body);
 
+	///removeCollisionObject will first check if it is a rigid body, if so call removeRigidBody otherwise call btDiscreteDynamicsWorld::removeCollisionObject
+	virtual void	removeCollisionObject(btCollisionObject* collisionObject);
+
 	int		getDrawFlags() const { return(m_drawFlags); }
 	void	setDrawFlags(int f)	{ m_drawFlags=f; }
 

src/LinearMath/btQuaternion.h

@@ -387,7 +387,11 @@ shortestArcQuat(const btVector3& v0, const btVector3& v1) // Game Programming Ge
 	btScalar  d = v0.dot(v1);
 
 	if (d < -1.0 + SIMD_EPSILON)
-		return btQuaternion(0.0f,1.0f,0.0f,0.0f); // just pick any vector
+	{
+		btVector3 n,unused;
+		btPlaneSpace1(v0,n,unused);
+		return btQuaternion(n.x(),n.y(),n.z(),0.0f); // just pick any vector that is orthogonal to v0
+	}
 
 	btScalar  s = btSqrt((1.0f + d) * 2.0f);
 	btScalar rs = 1.0f / s;

src/LinearMath/btScalar.h

@@ -242,6 +242,10 @@ SIMD_FORCE_INLINE btScalar btFmod(btScalar x,btScalar y) { return fmodf(x,y); }
 #define SIMD_HALF_PI      (SIMD_2_PI * btScalar(0.25))
 #define SIMD_RADS_PER_DEG (SIMD_2_PI / btScalar(360.0))
 #define SIMD_DEGS_PER_RAD  (btScalar(360.0) / SIMD_2_PI)
+#define SIMDSQRT12 btScalar(0.7071067811865475244008443621048490)
+
+#define btRecipSqrt(x) ((btScalar)(btScalar(1.0)/btSqrt(btScalar(x))))		/* reciprocal square root */
+
 
 #ifdef BT_USE_DOUBLE_PRECISION
 #define SIMD_EPSILON      DBL_EPSILON

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);
-  }
-}
+
 
 
 

src/LinearMath/btVector3.h

@@ -635,4 +635,24 @@ SIMD_FORCE_INLINE void	btUnSwapVector3Endian(btVector3& vector)
 	vector = swappedVec;
 }
 
+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);
+  }
+}
+
 #endif //SIMD__VECTOR3_H