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)

Demos/OpenGL/DemoApplication.cpp

@@ -1354,47 +1354,47 @@ void	DemoApplication::clientResetScene()
 	if (m_dynamicsWorld)
 	{
 		numObjects = m_dynamicsWorld->getNumCollisionObjects();
-	}
-
-	///create a copy of the array, not a reference!
-	btCollisionObjectArray copyArray = m_dynamicsWorld->getCollisionObjectArray();
-
 	
+		///create a copy of the array, not a reference!
+		btCollisionObjectArray copyArray = m_dynamicsWorld->getCollisionObjectArray();
+
+		
 
 
-	for (i=0;i<numObjects;i++)
+		for (i=0;i<numObjects;i++)
-	{
-		btCollisionObject* colObj = copyArray[i];
-		btRigidBody* body = btRigidBody::upcast(colObj);
-		if (body)
 		{
-			if (body->getMotionState())
+			btCollisionObject* colObj = copyArray[i];
-			{
-				btDefaultMotionState* myMotionState = (btDefaultMotionState*)body->getMotionState();
-				myMotionState->m_graphicsWorldTrans = myMotionState->m_startWorldTrans;
-				body->setCenterOfMassTransform( myMotionState->m_graphicsWorldTrans );
-				colObj->setInterpolationWorldTransform( myMotionState->m_startWorldTrans );
-				colObj->forceActivationState(ACTIVE_TAG);
-				colObj->activate();
-				colObj->setDeactivationTime(0);
-				//colObj->setActivationState(WANTS_DEACTIVATION);
-			}
-			//removed cached contact points (this is not necessary if all objects have been removed from the dynamics world)
-			//m_dynamicsWorld->getBroadphase()->getOverlappingPairCache()->cleanProxyFromPairs(colObj->getBroadphaseHandle(),getDynamicsWorld()->getDispatcher());
-
 			btRigidBody* body = btRigidBody::upcast(colObj);
-			if (body && !body->isStaticObject())
+			if (body)
 			{
-				btRigidBody::upcast(colObj)->setLinearVelocity(btVector3(0,0,0));
+				if (body->getMotionState())
-				btRigidBody::upcast(colObj)->setAngularVelocity(btVector3(0,0,0));
+				{
+					btDefaultMotionState* myMotionState = (btDefaultMotionState*)body->getMotionState();
+					myMotionState->m_graphicsWorldTrans = myMotionState->m_startWorldTrans;
+					body->setCenterOfMassTransform( myMotionState->m_graphicsWorldTrans );
+					colObj->setInterpolationWorldTransform( myMotionState->m_startWorldTrans );
+					colObj->forceActivationState(ACTIVE_TAG);
+					colObj->activate();
+					colObj->setDeactivationTime(0);
+					//colObj->setActivationState(WANTS_DEACTIVATION);
+				}
+				//removed cached contact points (this is not necessary if all objects have been removed from the dynamics world)
+				//m_dynamicsWorld->getBroadphase()->getOverlappingPairCache()->cleanProxyFromPairs(colObj->getBroadphaseHandle(),getDynamicsWorld()->getDispatcher());
+
+				btRigidBody* body = btRigidBody::upcast(colObj);
+				if (body && !body->isStaticObject())
+				{
+					btRigidBody::upcast(colObj)->setLinearVelocity(btVector3(0,0,0));
+					btRigidBody::upcast(colObj)->setAngularVelocity(btVector3(0,0,0));
+				}
 			}
+
 		}
 
+		///reset some internal cached data in the broadphase
+		m_dynamicsWorld->getBroadphase()->resetPool(getDynamicsWorld()->getDispatcher());
+		m_dynamicsWorld->getConstraintSolver()->reset();
+
 	}
 
-	///reset some internal cached data in the broadphase
-	m_dynamicsWorld->getBroadphase()->resetPool(getDynamicsWorld()->getDispatcher());
-	m_dynamicsWorld->getConstraintSolver()->reset();
-
-
 }

Demos/OpenGL/GL_ShapeDrawer.cpp

@@ -290,11 +290,10 @@ void GL_ShapeDrawer::drawSphere(btScalar radius, int lats, int longs)
 			btScalar lng = 2 * SIMD_PI * (btScalar) (j - 1) / longs;
 			btScalar x = cos(lng);
 			btScalar y = sin(lng);
-
-			glNormal3f(x * zr0, y * zr0, z0);
-			glVertex3f(x * zr0, y * zr0, z0);
 			glNormal3f(x * zr1, y * zr1, z1);
 			glVertex3f(x * zr1, y * zr1, z1);
+			glNormal3f(x * zr0, y * zr0, z0);
+			glVertex3f(x * zr0, y * zr0, z0);
 		}
 		glEnd();
 	}

Extras/MayaPlugin/constraint/bt_constraint.h

@@ -0,0 +1,54 @@
+/*
+Bullet Continuous Collision Detection and Physics Library Maya Plugin
+Copyright (c) 2008 Walt Disney Studios
+ 
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising
+from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+ 
+1. The origin of this software must not be misrepresented; you must
+not claim that you wrote the original software. If you use this
+software in a product, an acknowledgment in the product documentation
+would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must
+not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+ 
+Written by: Nicola Candussi <nicola@fluidinteractive.com>
+*/
+
+//bt_constraint.h
+
+#ifndef DYN_BT_CONSTRAINT_H
+#define DYN_BT_CONSTRAINT_H
+
+#include "btBulletCollisionCommon.h"
+#include "btBulletDynamicsCommon.h"
+#include "shared_ptr.h"
+
+class bt_constraint_t 
+{
+public:
+
+protected:
+    friend class bt_solver_t;
+
+    bt_constraint_t() { }
+
+    btTypedConstraint* constraint()                     { return m_constraint.get(); }
+    void set_constraint(btTypedConstraint *constraint)  { return m_constraint.reset(constraint); }
+	virtual void update_constraint() = 0;
+
+public:
+    friend class bt_rigid_body_t;
+
+    virtual ~bt_constraint_t() { }
+
+protected:
+    shared_ptr<btTypedConstraint> m_constraint; 
+};
+
+#endif

Extras/MayaPlugin/constraint/hinge_constraint.h

@@ -0,0 +1,118 @@
+/*
+Bullet Continuous Collision Detection and Physics Library Maya Plugin
+Copyright (c) 2008 Herbert Law
+ 
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising
+from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+ 
+1. The origin of this software must not be misrepresented; you must
+not claim that you wrote the original software. If you use this
+software in a product, an acknowledgment in the product documentation
+would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must
+not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+ 
+Written by: Herbert Law <Herbert.Law@gmail.com>
+*/
+
+//hinge_constraint.h
+
+#ifndef DYN_HINGE_CONSTRAINT_H
+#define DYN_HINGE_CONSTRAINT_H
+
+#include "shared_ptr.h"
+#include "rigid_body.h"
+#include "mathUtils.h"
+
+#include "constraint.h"
+#include "hinge_constraint_impl.h"
+
+class hinge_constraint_t: public constraint_t
+{
+public:
+    //typedefs
+    typedef shared_ptr<hinge_constraint_t> pointer;
+
+    //
+    rigid_body_t::pointer rigid_body()  {   return m_rigid_body;   }
+
+    //
+    void set_pivot(vec3f const& p) 
+    {
+        hinge_constraint_impl_t* hinge_impl = dynamic_cast<hinge_constraint_impl_t*>(impl());
+        hinge_impl->set_pivot(p);
+    }
+
+    //local space pivot
+    void get_pivot(vec3f& p) const  {    
+        hinge_constraint_impl_t const* hinge_impl = dynamic_cast<hinge_constraint_impl_t const*>(impl());
+        hinge_impl->get_pivot(p);   
+    }
+
+    //
+    void get_world_pivot(vec3f& p) const  {    
+        hinge_constraint_impl_t const* hinge_impl = dynamic_cast<hinge_constraint_impl_t const*>(impl());
+        hinge_impl->get_world_pivot(p);   
+    }
+
+    //
+    void set_damping(float d) {
+        hinge_constraint_impl_t* hinge_impl = dynamic_cast<hinge_constraint_impl_t*>(impl());
+        hinge_impl->set_damping(d);
+    }
+
+	void set_limit(float lower, float upper, float softness, float bias_factor, float relaxation_factor) {
+        hinge_constraint_impl_t* hinge_impl = dynamic_cast<hinge_constraint_impl_t*>(impl());
+        hinge_impl->set_limit(lower, upper, softness, bias_factor, relaxation_factor);
+    }
+
+	void set_axis(vec3f const& p) {
+        hinge_constraint_impl_t* hinge_impl = dynamic_cast<hinge_constraint_impl_t*>(impl());
+        hinge_impl->set_axis(p);
+    }
+
+    float damping() const {
+        hinge_constraint_impl_t const* hinge_impl = dynamic_cast<hinge_constraint_impl_t const*>(impl());
+        return hinge_impl->damping();  
+    }
+
+	void set_world(vec3f const& p) 
+    {
+        hinge_constraint_impl_t* hinge_impl = dynamic_cast<hinge_constraint_impl_t*>(impl());
+        hinge_impl->set_world(p);
+    }
+
+    //local space pivot
+    void get_world(vec3f& p) const  {    
+        hinge_constraint_impl_t const* hinge_impl = dynamic_cast<hinge_constraint_impl_t const*>(impl());
+        hinge_impl->get_world(p);   
+    }
+
+	void enable_motor(bool enable, float velocity, float impulse) {
+        hinge_constraint_impl_t* hinge_impl = dynamic_cast<hinge_constraint_impl_t*>(impl());
+        hinge_impl->enable_motor(enable, velocity, impulse);
+    }
+
+public:
+    virtual ~hinge_constraint_t() {};
+
+protected:
+    friend class solver_t;    
+    hinge_constraint_t(hinge_constraint_impl_t* impl, rigid_body_t::pointer& rigid_body): 
+        constraint_t(impl),
+        m_rigid_body(rigid_body) 
+    {
+    }
+
+private:
+    rigid_body_t::pointer                   m_rigid_body;
+};
+
+
+
+#endif //DYN_HINGE_CONSTRAINT_H

Extras/MayaPlugin/constraint/slider_constraint_impl.h

@@ -0,0 +1,50 @@
+/*
+Bullet Continuous Collision Detection and Physics Library Maya Plugin
+Copyright (c) 2008 Herbert Law
+ 
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising
+from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+ 
+1. The origin of this software must not be misrepresented; you must
+not claim that you wrote the original software. If you use this
+software in a product, an acknowledgment in the product documentation
+would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must
+not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+ 
+Written by: Herbert Law <Herbert.Law@gmail.com>
+*/
+
+//slider_constraint_impl.h
+
+#ifndef DYN_SLIDER_CONSTRAINT_IMPL_H
+#define DYN_SLIDER_CONSTRAINT_IMPL_H
+
+#include "constraint_impl.h"
+
+class slider_constraint_impl_t: public constraint_impl_t
+{
+public:
+    //
+    virtual void set_pivot(vec3f const& p) = 0; 
+    virtual void get_pivot(vec3f& p) const = 0; 
+    virtual void get_world_pivot(vec3f& p) const = 0; 
+    virtual void set_world(vec3f const& p) = 0; 
+    virtual void get_world(vec3f& p) const = 0; 
+
+    //
+    virtual void set_damping(float d) = 0;
+    virtual float damping() const = 0;
+    virtual void set_LinLimit(float lower, float upper) = 0;
+    virtual void set_AngLimit(float lower, float upper) = 0;
+
+public:
+    virtual ~slider_constraint_impl_t() {};
+};
+
+#endif //DYN_SLIDER_CONSTRAINT_IMPL_H

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