Added new EPA penetration depth solver

improved gjk/minkowski sampling pd method
added original solver variant, use btSequentialImpulseConstraintSolver2
Added Pierre Terdimans PD testbed

Demos/EPAPenDepthDemo/PenetrationTestBullet.cpp

@@ -31,9 +31,14 @@
 #include "BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h"
 #include "BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h"
 #include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h"
+#include "BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h"
+//We can use the Bullet EPA or sampling penetration depth solver, but comparison might be useful
+//#define COMPARE_WITH_SOLID35_AND_OTHER_EPA 1
+#ifdef COMPARE_WITH_SOLID35_AND_OTHER_EPA
 #include "../Extras/ExtraSolid35/Solid3EpaPenetrationDepth.h"
 #include "../Extras/ExtraSolid35/Solid3JohnsonSimplexSolver.h"
 #include "../Extras/EPA/EpaPenetrationDepthSolver.h"
+#endif //COMPARE_WITH_SOLID35_AND_OTHER_EPA
 
 #define USE_ORIGINAL 1
 #ifndef USE_ORIGINAL
@@ -277,17 +282,22 @@ static float gDepth;
 static bool TestEPA(const MyConvex& hull0, const MyConvex& hull1)
 {
 	static btSimplexSolverInterface simplexSolver;
-	//static Solid3JohnsonSimplexSolver simplexSolver;
+#ifdef COMPARE_WITH_SOLID35_AND_OTHER_EPA
+//	static Solid3JohnsonSimplexSolver simplexSolver2;
+#endif //COMPARE_WITH_SOLID35_AND_OTHER_EPA
 
 	simplexSolver.reset();
 
 	btConvexHullShape convexA((float*)hull0.mVerts, hull0.mNbVerts, sizeof(btVector3));
 	btConvexHullShape convexB((float*)hull1.mVerts, hull1.mNbVerts, sizeof(btVector3));
 
-	static btMinkowskiPenetrationDepthSolver Solver0;
+	static btGjkEpaPenetrationDepthSolver Solver0;
-	static Solid3EpaPenetrationDepth Solver1;
+	static btMinkowskiPenetrationDepthSolver Solver1;
-	static EpaPenetrationDepthSolver Solver2;
 
+#ifdef COMPARE_WITH_SOLID35_AND_OTHER_EPA
+	static Solid3EpaPenetrationDepth Solver2;
+	static EpaPenetrationDepthSolver Solver3;
+#endif
 	
 
 	btConvexPenetrationDepthSolver* Solver;
@@ -295,8 +305,12 @@ static bool TestEPA(const MyConvex& hull0, const MyConvex& hull1)
 				Solver = &Solver0;
 	else	if(gMethod==1)	
 				Solver = &Solver1;
-	else					
+#ifdef COMPARE_WITH_SOLID35_AND_OTHER_EPA
+	else	if(gMethod==2)	
 				Solver = &Solver2;
+	else					
+				Solver = &Solver3;
+#endif //COMPARE_WITH_SOLID35_AND_OTHER_EPA
 
 
 #ifdef USE_ORIGINAL
@@ -321,13 +335,16 @@ static bool TestEPA(const MyConvex& hull0, const MyConvex& hull1)
 	btVector3	normal;
 	btScalar	depth;
 
+	btGjkEpaSolver::sResults results;
+	btScalar radialMargin = 0.01f;
+
 	btGjkEpaSolver::Collide(&convexA,hull0.mTransform,
 		&convexB,hull1.mTransform,
-		0.01,0.01,
+		radialMargin,
-		witnesses,normal,depth);
+		results);
-	if (depth>0)
+	if (results.depth>0)
 	{
-		output.addContactPoint(normal,witnesses[1],-depth);
+		output.addContactPoint(results.normal,results.witnesses[1],-results.depth);
 	}
 #endif
 	return true;
@@ -471,7 +488,11 @@ static void KeyboardCallback(unsigned char key, int x, int y)
 
 	case ' ':
 		gMethod++;
-		if(gMethod==3)	gMethod=0;
+#ifdef COMPARE_WITH_SOLID35_AND_OTHER_EPA
+		if(gMethod==4)	gMethod=0;
+#else
+		if(gMethod==2)	gMethod=0;
+#endif
 		break;
 
 	case '4':
@@ -675,13 +696,16 @@ static void RenderCallback()
 		switch (	gMethod)
 		{
 		case 0:
-			sprintf(buf,"Minkowski sampling Penetration depth solver\n" );
+			sprintf(buf,"Bullet GjkEpa Penetration depth solver (zlib free\n" );
 			break;
 		case 1:
-			sprintf(buf,"Solid35 EPA Penetration depth solver\n" );
+			sprintf(buf,"Bullet Minkowski sampling Penetration depth solver\n" );
 			break;
 		case 2:
-			sprintf(buf,"EPA Penetration depth solver (WorkInProgress, zlib free\n" );
+				sprintf(buf,"Solid35 EPA Penetration depth solver\n" );
+				break;
+		case 3:
+			sprintf(buf,"EPA Penetration depth solver (Experimental/WorkInProgress, zlib free\n" );
 			break;
 		default:
 				sprintf(buf,"Unknown Penetration Depth\n" );

Extras/EPA/EpaPenetrationDepthSolver.h

@@ -14,8 +14,8 @@ subject to the following restrictions:
 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.
 */
-#ifndef EPA_PENETRATION_DEPTH_H
+#ifndef EPA2_PENETRATION_DEPTH_H
-#define EPA_PENETRATION_DEPTH_H
+#define EPA2_PENETRATION_DEPTH_H
 
 /**
 *	EpaPenetrationDepthSolver uses the Expanding Polytope Algorithm to

VERSION

@@ -1,3 +1,3 @@
-Bullet Collision Detection and Physics Library version 2.22
+Bullet Collision Detection and Physics Library version 2.32
 http://bullet.sourceforge.net
 

changes.txt

@@ -1,6 +1,16 @@
 Bullet Continuous Collision Detection and Physics Library
 Erwin Coumans
 
+2006 Nov 15
+	Added EPA penetration depth algorithm, Expanding Polytope Algorithm
+	Added Pierre Terdiman penetration depth comparison/test DEMO
+	Fixed Bullet's Minkowski sampling penetration depth solver
+	Contributed by Nathanael Presson
+	
+2006 Nov 11
+	Added GIMPACT trimesh collision detection: concave versus concave,
+	Contributed by Francisco Le<4C>n N<>jera
+
 2006 Nov 2
 	Minor refactoring: btCollisionObject changes from struct into class, added accessor methods
 	Force use of btMotionState to synchronize graphics transform, disabled old btRigidBody constructor that accepts btTransform

configure.ac

@@ -9,7 +9,7 @@ AC_PREREQ([2.54])
 #----------------------------------------------------------------------------
 AC_INIT(
     [bullet],
-    [2.20],
+    [2.32],
     [bullet@erwincoumans.com])
 CS_PACKAGEINFO(
     [Bullet Continuous Collision Detection and Physics Library],

src/BulletCollision/NarrowPhaseCollision/btGjkEpa.cpp

@@ -20,7 +20,7 @@ Nov.2006
 
 
 #include "btGjkEpa.h"
-
+#include <string.h> //for memset
 
 namespace gjkepa_impl
 {

src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h

@@ -14,8 +14,8 @@ subject to the following restrictions:
 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.
 */
-#ifndef EPA_PENETRATION_DEPTH_H
+#ifndef BT_GJP_EPA_PENETRATION_DEPTH_H
-#define EPA_PENETRATION_DEPTH_H
+#define BT_GJP_EPA_PENETRATION_DEPTH_H
 
 #include "btConvexPenetrationDepthSolver.h"
 
@@ -35,5 +35,5 @@ class btGjkEpaPenetrationDepthSolver : public btConvexPenetrationDepthSolver
 
 };
 
-#endif	// EPA_PENETRATION_DEPTH_H
+#endif	// BT_GJP_EPA_PENETRATION_DEPTH_H
 

src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp

@@ -63,8 +63,14 @@ bool  MyContactDestroyedCallback(void* userPersistentData)
 	return true;
 }
 
+btSequentialImpulseConstraintSolver2::btSequentialImpulseConstraintSolver2()
+{
+	setSolverMode(SOLVER_USE_WARMSTARTING);
+}
+
 
 btSequentialImpulseConstraintSolver::btSequentialImpulseConstraintSolver()
+:m_solverMode(SOLVER_RANDMIZE_ORDER)
 {
 	gContactDestroyedCallback = &MyContactDestroyedCallback;
 
@@ -92,18 +98,13 @@ float btSequentialImpulseConstraintSolver::solveGroup(btPersistentManifold** man
 
 	int totalPoints = 0;
 
-	{
-		int j;
-		for (j=0;j<numManifolds;j++)
-		{
-			prepareConstraints(manifoldPtr[j],info,debugDrawer);
-		}
-	}
 
 	{
 		int j;
 		for (j=0;j<numManifolds;j++)
 		{
+			btPersistentManifold* manifold = manifoldPtr[j];
+			prepareConstraints(manifold,info,debugDrawer);
 			for (int p=0;p<manifoldPtr[j]->getNumContacts();p++)
 			{
 				gOrder[totalPoints].m_manifoldIndex = j;
@@ -113,12 +114,15 @@ float btSequentialImpulseConstraintSolver::solveGroup(btPersistentManifold** man
 		}
 	}
 	
-	
 	//should traverse the contacts random order...
 	int iteration;
+
+	{
 		for ( iteration = 0;iteration<numiter-1;iteration++)
 		{
 			int j;
+			if (m_solverMode & SOLVER_RANDMIZE_ORDER)
+			{
 				if ((iteration & 7) == 0) {
 					for (j=0; j<totalPoints; ++j) {
 						btOrderIndex tmp = gOrder[j];
@@ -127,6 +131,7 @@ float btSequentialImpulseConstraintSolver::solveGroup(btPersistentManifold** man
 						gOrder[swapi] = tmp;
 					}
 				}
+			}
 
 			for (j=0;j<totalPoints;j++)
 			{
@@ -143,6 +148,77 @@ float btSequentialImpulseConstraintSolver::solveGroup(btPersistentManifold** man
 					(btRigidBody*)manifold->getBody1(),manifold->getContactPoint(gOrder[j].m_pointIndex),info,iteration,debugDrawer);
 			}
 		}
+	}
+		
+#ifdef USE_PROFILE
+	btProfiler::endBlock("solve");
+#endif //USE_PROFILE
+
+	return 0.f;
+}
+
+
+/// btSequentialImpulseConstraintSolver Sequentially applies impulses
+float btSequentialImpulseConstraintSolver2::solveGroup(btPersistentManifold** manifoldPtr, int numManifolds,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
+{
+	
+	btContactSolverInfo info = infoGlobal;
+
+	int numiter = infoGlobal.m_numIterations;
+#ifdef USE_PROFILE
+	btProfiler::beginBlock("solve");
+#endif //USE_PROFILE
+
+	{
+		int j;
+		for (j=0;j<numManifolds;j++)
+		{
+			btPersistentManifold* manifold = manifoldPtr[j];
+			prepareConstraints(manifold,info,debugDrawer);
+			for (int p=0;p<manifoldPtr[j]->getNumContacts();p++)
+			{
+				//interleaving here gives better results
+				solve( (btRigidBody*)manifold->getBody0(),
+									(btRigidBody*)manifold->getBody1()
+				,manifoldPtr[j]->getContactPoint(p),info,0,debugDrawer);
+			}
+		}
+	}
+	
+	//should traverse the contacts random order...
+	int iteration;
+
+	for ( iteration = 0;iteration<numiter-1;iteration++)
+	{
+		int j;
+
+		for (j=0;j<numManifolds;j++)
+		{
+			btPersistentManifold* manifold = manifoldPtr[j];
+			for (int p=0;p<manifold->getNumContacts();p++)
+			{
+				solve( (btRigidBody*)manifold->getBody0(),
+									(btRigidBody*)manifold->getBody1()
+				,manifold->getContactPoint(p),info,iteration,debugDrawer);
+			}
+		}
+	
+	}
+
+	for ( iteration = 0;iteration<numiter-1;iteration++)
+	{
+		int j;
+		for (j=0;j<numManifolds;j++)
+		{
+			btPersistentManifold* manifold = manifoldPtr[j];
+			for (int p=0;p<manifold->getNumContacts();p++)
+			{
+				solveFriction((btRigidBody*)manifold->getBody0(),
+					(btRigidBody*)manifold->getBody1(),manifold->getContactPoint(p),info,iteration,debugDrawer);
+			}
+		}
+	}
+
 		
 #ifdef USE_PROFILE
 	btProfiler::endBlock("solve");
@@ -264,7 +340,14 @@ void	btSequentialImpulseConstraintSolver::prepareConstraints(btPersistentManifol
 				
 
 				float relaxation = info.m_damping;
-				cpd->m_appliedImpulse =0.f;//*= relaxation;
+				if (m_solverMode & SOLVER_USE_WARMSTARTING)
+				{
+					cpd->m_appliedImpulse *= relaxation;
+				} else
+				{
+					cpd->m_appliedImpulse =0.f;
+				}
+	
 				//for friction
 				cpd->m_prevAppliedImpulse = cpd->m_appliedImpulse;
 				

src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h

@@ -30,6 +30,7 @@ class btIDebugDraw;
 class btSequentialImpulseConstraintSolver : public btConstraintSolver
 {
 
+protected:
 	float solve(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer);
 	float solveFriction(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer);
 	void  prepareConstraints(btPersistentManifold* manifoldPtr, const btContactSolverInfo& info,btIDebugDraw* debugDrawer);
@@ -37,8 +38,18 @@ class btSequentialImpulseConstraintSolver : public btConstraintSolver
 	ContactSolverFunc m_contactDispatch[MAX_CONTACT_SOLVER_TYPES][MAX_CONTACT_SOLVER_TYPES];
 	ContactSolverFunc m_frictionDispatch[MAX_CONTACT_SOLVER_TYPES][MAX_CONTACT_SOLVER_TYPES];
 
+	//choose between several modes, different friction model etc.
+	int	m_solverMode;
+
 public:
 
+	enum	eSolverMode
+	{
+		SOLVER_RANDMIZE_ORDER = 1,
+		SOLVER_FRICTION_SEPARATE = 2,
+		SOLVER_USE_WARMSTARTING = 4
+	};
+
 	btSequentialImpulseConstraintSolver();
 
 	///Advanced: Override the default contact solving function for contacts, for certain types of rigidbody
@@ -59,7 +70,29 @@ public:
 	
 	virtual float solveGroup(btPersistentManifold** manifold,int numManifolds,const btContactSolverInfo& info, btIDebugDraw* debugDrawer=0);
 
+	void	setSolverMode(int mode)
+	{
+		m_solverMode = mode;
+	}
+
+	int	getSolverMode() const
+	{
+		return m_solverMode;
+	}
 };
 
+/// Small variation on btSequentialImpulseConstraintSolver: warmstarting, separate friction, non-randomized ordering
+class btSequentialImpulseConstraintSolver2 : public btSequentialImpulseConstraintSolver
+{
+public:
+
+	btSequentialImpulseConstraintSolver2();
+
+	virtual float solveGroup(btPersistentManifold** manifold,int numManifolds,const btContactSolverInfo& info, btIDebugDraw* debugDrawer=0);
+
+
+};
+
+
 #endif //SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_H