updating Bullet 2.x with latest changes. The integration + C-API will follow at some stage.

4 files changed, 200 insertions, 61 deletions

diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h
index 8889699b395..ef2b480fa62 100644
--- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h
+++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h
@@ -14,8 +14,8 @@ subject to the following restrictions:
 */
 
 
-#ifndef DISCRETE_COLLISION_DETECTOR_INTERFACE_H
-#define DISCRETE_COLLISION_DETECTOR_INTERFACE_H
+#ifndef DISCRETE_COLLISION_DETECTOR1_INTERFACE_H
+#define DISCRETE_COLLISION_DETECTOR1_INTERFACE_H
 #include "LinearMath/btTransform.h"
 #include "LinearMath/btVector3.h"
 
@@ -84,4 +84,4 @@ struct btStorageResult : public btDiscreteCollisionDetectorInterface::Result
 		}
 };
 
-#endif //DISCRETE_COLLISION_DETECTOR_INTERFACE_H
+#endif //DISCRETE_COLLISION_DETECTOR_INTERFACE1_H
diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp
index 4b38ced7f12..507fcaf21f7 100644
--- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp
+++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp
@@ -22,12 +22,13 @@ subject to the following restrictions:
 #include <stdio.h> //for debug printf
 #endif
 
-static const btScalar rel_error = btScalar(1.0e-5);
-btScalar rel_error2 = rel_error * rel_error;
-float maxdist2 = 1.e30f;
+//must be above the machine epsilon
+#define REL_ERROR2 1.0e-6f
+
 
 #ifdef __SPU__
 #include <spu_printf.h>
+#define printf spu_printf
 #endif //__SPU__
 
 btGjkPairDetector::btGjkPairDetector(btConvexShape* objectA,btConvexShape* objectB,btSimplexSolverInterface* simplexSolver,btConvexPenetrationDepthSolver*	penetrationDepthSolver)
@@ -36,7 +37,9 @@ m_penetrationDepthSolver(penetrationDepthSolver),
 m_simplexSolver(simplexSolver),
 m_minkowskiA(objectA),
 m_minkowskiB(objectB),
-m_ignoreMargin(false)
+m_ignoreMargin(false),
+m_lastUsedMethod(-1),
+m_catchDegeneracies(0)
 {
 }
 
@@ -45,6 +48,11 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result&
 	btScalar distance=0.f;
 	btVector3	normalInB(0.f,0.f,0.f);
 	btVector3 pointOnA,pointOnB;
+	btTransform	localTransA = input.m_transformA;
+	btTransform localTransB = input.m_transformB;
+	btVector3 positionOffset = (localTransA.getOrigin() + localTransB.getOrigin()) * 0.5f;
+	localTransA.getOrigin() -= positionOffset;
+	localTransB.getOrigin() -= positionOffset;
 
 	float marginA = m_minkowskiA->getMargin();
 	float marginB = m_minkowskiB->getMargin();
@@ -56,12 +64,16 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result&
 		marginB = 0.f;
 	}
 
-	int curIter = 0;
+	m_curIter = 0;
 	int gGjkMaxIter = 1000;//this is to catch invalid input, perhaps check for #NaN?
+	m_cachedSeparatingAxis.setValue(0,1,0);
 
 	bool isValid = false;
 	bool checkSimplex = false;
 	bool checkPenetration = true;
+	m_degenerateSimplex = false;
+
+	m_lastUsedMethod = -1;
 
 	{
 		btScalar squaredDistance = SIMD_INFINITY;
@@ -81,8 +93,8 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result&
 
 			btVector3 pInA = m_minkowskiA->localGetSupportingVertexWithoutMargin(seperatingAxisInA);
 			btVector3 qInB = m_minkowskiB->localGetSupportingVertexWithoutMargin(seperatingAxisInB);
-			btPoint3  pWorld = input.m_transformA(pInA);	
-			btPoint3  qWorld = input.m_transformB(qInB);
+			btPoint3  pWorld = localTransA(pInA);	
+			btPoint3  qWorld = localTransB(qInB);
 			
 			btVector3 w	= pWorld - qWorld;
 			delta = m_cachedSeparatingAxis.dot(w);
@@ -101,7 +113,16 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result&
 				break;
 			}
 			// are we getting any closer ?
-			if (squaredDistance - delta <= squaredDistance * rel_error2)
+			float f0 = squaredDistance - delta;
+			float f1 = squaredDistance * REL_ERROR2;
+
+			if (f0 <= 0.f)
+			{
+				m_degenerateSimplex = 2;
+			}
+
+			if (f0 >= 0.f && (f0 <= f1))
+			//if (f0 <= f1)
 			{
 				checkSimplex = true;
 				break;
@@ -112,6 +133,7 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result&
 			//calculate the closest point to the origin (update vector v)
 			if (!m_simplexSolver->closest(m_cachedSeparatingAxis))
 			{
+				m_degenerateSimplex = 1;
 				checkSimplex = true;
 				break;
 			}
@@ -130,10 +152,11 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result&
 			}
 
 			  //degeneracy, this is typically due to invalid/uninitialized worldtransforms for a btCollisionObject   
-              if (curIter++ > gGjkMaxIter)   
+              if (m_curIter++ > gGjkMaxIter)   
               {   
                       #if defined(DEBUG) || defined (_DEBUG)   
-                              printf("btGjkPairDetector maxIter exceeded:%i\n",curIter);   
+
+                              printf("btGjkPairDetector maxIter exceeded:%i\n",m_curIter);   
                               printf("sepAxis=(%f,%f,%f), squaredDistance = %f, shapeTypeA=%i,shapeTypeB=%i\n",   
                               m_cachedSeparatingAxis.getX(),   
                               m_cachedSeparatingAxis.getY(),   
@@ -141,6 +164,7 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result&
                               squaredDistance,   
                               m_minkowskiA->getShapeType(),   
                               m_minkowskiB->getShapeType());   
+
                       #endif   
                       break;   
 
@@ -164,20 +188,27 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result&
 			normalInB = pointOnA-pointOnB;
 			float lenSqr = m_cachedSeparatingAxis.length2();
 			//valid normal
-			if (lenSqr > (SIMD_EPSILON*SIMD_EPSILON))
+			//if (lenSqr > (0.1f*margin)) //SIMD_EPSILON*SIMD_EPSILON))
+			if (lenSqr > SIMD_EPSILON*SIMD_EPSILON)
 			{
 				float rlen = 1.f / btSqrt(lenSqr );
 				normalInB *= rlen; //normalize
 				btScalar s = btSqrt(squaredDistance);
+			
 				ASSERT(s > btScalar(0.0));
 				pointOnA -= m_cachedSeparatingAxis * (marginA / s);
 				pointOnB += m_cachedSeparatingAxis * (marginB / s);
 				distance = ((1.f/rlen) - margin);
 				isValid = true;
+				m_lastUsedMethod = 1;
+			} else
+			{
+				m_lastUsedMethod = 2;
 			}
 		}
 
-		if (checkPenetration && !isValid)
+		//if (checkPenetration && !isValid)
+		if (checkPenetration && (!isValid || (m_catchDegeneracies && m_penetrationDepthSolver && m_degenerateSimplex) ))
 		{
 			//penetration case
 		
@@ -185,27 +216,46 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result&
 			if (m_penetrationDepthSolver)
 			{
 				// Penetration depth case.
-				isValid = m_penetrationDepthSolver->calcPenDepth( 
+				btVector3 tmpPointOnA,tmpPointOnB;
+				bool isValid2 = m_penetrationDepthSolver->calcPenDepth( 
 					*m_simplexSolver, 
 					m_minkowskiA,m_minkowskiB,
-					input.m_transformA,input.m_transformB,
-					m_cachedSeparatingAxis, pointOnA, pointOnB,
+					localTransA,localTransB,
+					m_cachedSeparatingAxis, tmpPointOnA, tmpPointOnB,
 					debugDraw
 					);
 
-				if (isValid)
+				if (isValid2)
 				{
-					normalInB = pointOnB-pointOnA;
-					float lenSqr = normalInB.length2();
+					btVector3 tmpNormalInB = tmpPointOnB-tmpPointOnA;
+					float lenSqr = tmpNormalInB.length2();
 					if (lenSqr > (SIMD_EPSILON*SIMD_EPSILON))
 					{
-						normalInB /= btSqrt(lenSqr);
-						distance = -(pointOnA-pointOnB).length();
+						tmpNormalInB /= btSqrt(lenSqr);
+						float distance2 = -(tmpPointOnA-tmpPointOnB).length();
+						//only replace valid penetrations when the result is deeper (check)
+						if (!isValid || (distance2 < distance))
+						{
+							distance = distance2;
+							pointOnA = tmpPointOnA;
+							pointOnB = tmpPointOnB;
+							normalInB = tmpNormalInB;
+							isValid = true;
+							m_lastUsedMethod = 3;
+						} else
+						{
+							
+						}
 					} else
 					{
-						isValid = false;
+						//isValid = false;
+						m_lastUsedMethod = 4;
 					}
+				} else
+				{
+					m_lastUsedMethod = 5;
 				}
+				
 			}
 		}
 	}
@@ -219,7 +269,7 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result&
 
 		output.addContactPoint(
 			normalInB,
-			pointOnB,
+			pointOnB+positionOffset,
 			distance);
 		//printf("gjk add:%f",distance);
 	}
diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h
index c4842cd3023..09c1669bd78 100644
--- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h
+++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h
@@ -43,6 +43,12 @@ class btGjkPairDetector : public btDiscreteCollisionDetectorInterface
 
 public:
 
+	//some debugging to fix degeneracy problems
+	int			m_lastUsedMethod;
+	int			m_curIter;
+	int			m_degenerateSimplex;
+	int			m_catchDegeneracies;
+
 
 	btGjkPairDetector(btConvexShape* objectA,btConvexShape* objectB,btSimplexSolverInterface* simplexSolver,btConvexPenetrationDepthSolver*	penetrationDepthSolver);
 	virtual ~btGjkPairDetector() {};
@@ -68,11 +74,13 @@ public:
 		m_penetrationDepthSolver = penetrationDepthSolver;
 	}
 
+	///don't use setIgnoreMargin, it's for Bullet's internal use
 	void	setIgnoreMargin(bool ignoreMargin)
 	{
 		m_ignoreMargin = ignoreMargin;
 	}
 
+
 };
 
 #endif //GJK_PAIR_DETECTOR_H
diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp
index 34daacf26ac..74cf2b7c071 100644
--- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp
+++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp
@@ -20,32 +20,10 @@ subject to the following restrictions:
 #include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h"
 
 
-struct MyResult : public btDiscreteCollisionDetectorInterface::Result
-{
 
-	MyResult():m_hasResult(false)
-	{
-	}
-	
-	btVector3 m_normalOnBInWorld;
-	btVector3 m_pointInWorld;
-	float m_depth;
-	bool	m_hasResult;
-
-	virtual void setShapeIdentifiers(int partId0,int index0,	int partId1,int index1)
-	{
-	}
-	void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,float depth)
-	{
-		m_normalOnBInWorld = normalOnBInWorld;
-		m_pointInWorld = pointInWorld;
-		m_depth = depth;
-		m_hasResult = true;
-	}
-};
 
 #define NUM_UNITSPHERE_POINTS 42
-static btVector3	sPenetrationDirections[NUM_UNITSPHERE_POINTS] = 
+static btVector3	sPenetrationDirections[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2] = 
 {
 btVector3(0.000000f , -0.000000f,-1.000000f),
 btVector3(0.723608f , -0.525725f,-0.447219f),
@@ -100,6 +78,31 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s
 												   )
 {
 
+
+	struct btIntermediateResult : public btDiscreteCollisionDetectorInterface::Result
+	{
+
+		btIntermediateResult():m_hasResult(false)
+		{
+		}
+		
+		btVector3 m_normalOnBInWorld;
+		btVector3 m_pointInWorld;
+		float m_depth;
+		bool	m_hasResult;
+
+		virtual void setShapeIdentifiers(int partId0,int index0,	int partId1,int index1)
+		{
+		}
+		void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,float depth)
+		{
+			m_normalOnBInWorld = normalOnBInWorld;
+			m_pointInWorld = pointInWorld;
+			m_depth = depth;
+			m_hasResult = true;
+		}
+	};
+
 	//just take fixed number of orientation, and sample the penetration depth in that direction
 	float minProj = 1e30f;
 	btVector3 minNorm;
@@ -110,22 +113,62 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s
 
 #define USE_BATCHED_SUPPORT 1
 #ifdef USE_BATCHED_SUPPORT
-	btVector3	supportVerticesABatch[NUM_UNITSPHERE_POINTS];
-	btVector3	supportVerticesBBatch[NUM_UNITSPHERE_POINTS];
-	btVector3	seperatingAxisInABatch[NUM_UNITSPHERE_POINTS];
-	btVector3	seperatingAxisInBBatch[NUM_UNITSPHERE_POINTS];
+
+	btVector3	supportVerticesABatch[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2];
+	btVector3	supportVerticesBBatch[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2];
+	btVector3	seperatingAxisInABatch[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2];
+	btVector3	seperatingAxisInBBatch[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2];
 	int i;
 
-	for (i=0;i<NUM_UNITSPHERE_POINTS;i++)
+	int numSampleDirections = NUM_UNITSPHERE_POINTS;
+
+	for (i=0;i<numSampleDirections;i++)
 	{
 		const btVector3& norm = sPenetrationDirections[i];
-		seperatingAxisInABatch[i] = (-norm)* transA.getBasis();
-		seperatingAxisInBBatch[i] = norm * transB.getBasis();
+		seperatingAxisInABatch[i] =  (-norm) * transA.getBasis() ;
+		seperatingAxisInBBatch[i] =  norm   * transB.getBasis() ;
 	}
 
-	convexA->batchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInABatch,supportVerticesABatch,NUM_UNITSPHERE_POINTS);
-	convexB->batchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInBBatch,supportVerticesBBatch,NUM_UNITSPHERE_POINTS);
-	for (i=0;i<NUM_UNITSPHERE_POINTS;i++)
+	{
+		int numPDA = convexA->getNumPreferredPenetrationDirections();
+		if (numPDA)
+		{
+			for (int i=0;i<numPDA;i++)
+			{
+				btVector3 norm;
+				convexA->getPreferredPenetrationDirection(i,norm);
+				norm  = transA.getBasis() * norm;
+				sPenetrationDirections[numSampleDirections] = norm;
+				seperatingAxisInABatch[numSampleDirections] = (-norm) * transA.getBasis();
+				seperatingAxisInBBatch[numSampleDirections] = norm * transB.getBasis();
+				numSampleDirections++;
+			}
+		}
+	}
+
+	{
+		int numPDB = convexB->getNumPreferredPenetrationDirections();
+		if (numPDB)
+		{
+			for (int i=0;i<numPDB;i++)
+			{
+				btVector3 norm;
+				convexB->getPreferredPenetrationDirection(i,norm);
+				norm  = transB.getBasis() * norm;
+				sPenetrationDirections[numSampleDirections] = norm;
+				seperatingAxisInABatch[numSampleDirections] = (-norm) * transA.getBasis();
+				seperatingAxisInBBatch[numSampleDirections] = norm * transB.getBasis();
+				numSampleDirections++;
+			}
+		}
+	}
+
+
+
+	convexA->batchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInABatch,supportVerticesABatch,numSampleDirections);
+	convexB->batchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInBBatch,supportVerticesBBatch,numSampleDirections);
+
+	for (i=0;i<numSampleDirections;i++)
 	{
 		const btVector3& norm = sPenetrationDirections[i];
 		seperatingAxisInA = seperatingAxisInABatch[i];
@@ -148,7 +191,40 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s
 		}
 	}	
 #else
-	for (int i=0;i<NUM_UNITSPHERE_POINTS;i++)
+
+	int numSampleDirections = NUM_UNITSPHERE_POINTS;
+
+	{
+		int numPDA = convexA->getNumPreferredPenetrationDirections();
+		if (numPDA)
+		{
+			for (int i=0;i<numPDA;i++)
+			{
+				btVector3 norm;
+				convexA->getPreferredPenetrationDirection(i,norm);
+				norm  = transA.getBasis() * norm;
+				sPenetrationDirections[numSampleDirections] = norm;
+				numSampleDirections++;
+			}
+		}
+	}
+
+	{
+		int numPDB = convexB->getNumPreferredPenetrationDirections();
+		if (numPDB)
+		{
+			for (int i=0;i<numPDB;i++)
+			{
+				btVector3 norm;
+				convexB->getPreferredPenetrationDirection(i,norm);
+				norm  = transB.getBasis() * norm;
+				sPenetrationDirections[numSampleDirections] = norm;
+				numSampleDirections++;
+			}
+		}
+	}
+
+	for (int i=0;i<numSampleDirections;i++)
 	{
 		const btVector3& norm = sPenetrationDirections[i];
 		seperatingAxisInA = (-norm)* transA.getBasis();
@@ -174,10 +250,15 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s
 
 	minA += minNorm*convexA->getMargin();
 	minB -= minNorm*convexB->getMargin();
+	//no penetration
+	if (minProj < 0.f)
+		return false;
+
 	minProj += (convexA->getMargin() + convexB->getMargin());
 
 
-	
+
+
 
 //#define DEBUG_DRAW 1
 #ifdef DEBUG_DRAW
@@ -213,7 +294,7 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s
 	input.m_transformB = transB;
 	input.m_maximumDistanceSquared = 1e30f;//minProj;
 	
-	MyResult res;
+	btIntermediateResult res;
 	gjkdet.getClosestPoints(input,res,debugDraw);
 
 	float correctedMinNorm = minProj - res.m_depth;