Work on fixing some GJK issues reported by Pierre Terdiman (thanks Pierre for the testbed!)

Improved this penetration test with more verbose output
retrieve worldtransform from motionstate when rigidbody gets motionstate assigned

Demos/BasicDemo/BasicDemo.cpp

@@ -129,7 +129,9 @@ void	BasicDemo::initPhysics()
 
 	m_solver = new btSequentialImpulseConstraintSolver;
 
-	m_dynamicsWorld = new btSimpleDynamicsWorld(m_dispatcher,m_overlappingPairCache,m_solver);
+	//m_dynamicsWorld = new btSimpleDynamicsWorld(m_dispatcher,m_overlappingPairCache,m_solver);
+	m_dynamicsWorld = new btDiscreteDynamicsWorld(m_dispatcher,m_overlappingPairCache,m_solver);
+
 	m_dynamicsWorld->setGravity(btVector3(0,-10,0));
 
 	m_dynamicsWorld->setDebugDrawer(&debugDrawer);

Demos/BspDemo/BspDemo.cpp

@@ -135,6 +135,7 @@ void	BspDemo::initPhysics(char* bspfilename)
 	///Setup a Physics Simulation Environment
 	m_dynamicsWorld = new btDiscreteDynamicsWorld();
 	m_dynamicsWorld->setGravity(-m_cameraUp * 10);
+	m_dynamicsWorld->setDebugDrawer(&debugDrawer);
 
 
 #ifdef QUAKE_BSP_IMPORTING

Demos/ConstraintDemo/ConstraintDemo.cpp

@@ -41,7 +41,7 @@ int main(int argc,char** argv)
 
 	constraintDemo->initPhysics();	
 
-	constraintDemo->setCameraDistance(46.f);
+	constraintDemo->setCameraDistance(26.f);
 
 	return glutmain(argc, argv,640,480,"Constraint Demo. http://www.continuousphysics.com/Bullet/phpBB2/",constraintDemo);
 }

Demos/EPAPenDepthDemo/PenetrationTestBullet.cpp

@@ -20,6 +20,10 @@
 #include <stdio.h>
 #include <math.h>
 
+#define VERBOSE_TEXT_ONSCREEN 1
+#ifdef VERBOSE_TEXT_ONSCREEN
+#include "BMF_Api.h"
+#endif
 
 #include "btBulletCollisionCommon.h"
 
@@ -35,6 +39,10 @@
 
 static bool gRefMode = false;
 static int gMethod = 0;
+static int gLastUsedMethod = -1;
+static int gNumGjkIterations = -1;
+static int gLastDegenerateSimplex = -1;
+
 static const float gDisp = 0.01f;
 static const float gCamSpeed = 0.1f;
 static btVector3 Eye(3.0616338f, 1.1985892f, 2.5769043f);
@@ -42,6 +50,8 @@ static btVector3 Dir(-0.66853905,-0.14004262,-0.73037237);
 static btVector3 N;
 static int mx = 0;
 static int my = 0;
+static int glutScreenHeight = 512;
+static int glutScreenWidth = 512;
 
 static void DrawLine(const btVector3& p0, const btVector3& p1, const btVector3& color, float line_width)
 {
@@ -144,6 +154,36 @@ bool MyConvex::LoadFromFile(const char* filename)
 	return true;
 }
 
+
+
+//See http://www.lighthouse3d.com/opengl/glut/index.php?bmpfontortho
+static void setOrthographicProjection() 
+{
+
+	// switch to projection mode
+	glMatrixMode(GL_PROJECTION);
+	// save previous matrix which contains the 
+	//settings for the perspective projection
+	glPushMatrix();
+	// reset matrix
+	glLoadIdentity();
+	// set a 2D orthographic projection
+	gluOrtho2D(0, glutScreenWidth, 0, glutScreenHeight);
+	// invert the y axis, down is positive
+	glScalef(1, -1, 1);
+	// mover the origin from the bottom left corner
+	// to the upper left corner
+	glTranslatef(0, -glutScreenHeight, 0);
+	glMatrixMode(GL_MODELVIEW);
+}
+
+static void resetPerspectiveProjection() 
+{
+	glMatrixMode(GL_PROJECTION);
+	glPopMatrix();
+	glMatrixMode(GL_MODELVIEW);
+}
+
 void MyConvex::Render(bool only_wireframe, const btVector3& wire_color) const
 {
 	const float Scale = 1.0f;
@@ -233,17 +273,19 @@ static float gDepth;
 
 static bool TestEPA(const MyConvex& hull0, const MyConvex& hull1)
 {
-	//static btSimplexSolverInterface simplexSolver;
-	static Solid3JohnsonSimplexSolver simplexSolver;
+	static btSimplexSolverInterface simplexSolver;
+	//static Solid3JohnsonSimplexSolver simplexSolver;
 
 	simplexSolver.reset();
 
 	btConvexHullShape convexA((float*)hull0.mVerts, hull0.mNbVerts, sizeof(btVector3));
 	btConvexHullShape convexB((float*)hull1.mVerts, hull1.mNbVerts, sizeof(btVector3));
 
-	static Solid3EpaPenetrationDepth Solver0;
-	static EpaPenetrationDepthSolver Solver1;
-	static btMinkowskiPenetrationDepthSolver Solver2;
+	static btMinkowskiPenetrationDepthSolver Solver0;
+	static Solid3EpaPenetrationDepth Solver1;
+	static EpaPenetrationDepthSolver Solver2;
+	
+	
 
 	btConvexPenetrationDepthSolver* Solver;
 			if(gMethod==0)	
@@ -254,7 +296,7 @@ static bool TestEPA(const MyConvex& hull0, const MyConvex& hull1)
 		Solver = &Solver2;
 
 	btGjkPairDetector GJK(&convexA, &convexB, &simplexSolver, Solver);
-	//GJK.setIgnoreMargin(true);
+	GJK.m_catchDegeneracies = 1;
 	convexA.setMargin(0.01f);
 	convexB.setMargin(0.01f);
 
@@ -264,6 +306,9 @@ static bool TestEPA(const MyConvex& hull0, const MyConvex& hull1)
 
 	MyResult output;
 	GJK.getClosestPoints(input, output, 0);
+	gLastUsedMethod = GJK.m_lastUsedMethod;
+	gNumGjkIterations = GJK.m_curIter;
+	gLastDegenerateSimplex= GJK.m_degenerateSimplex;
 	return true;
 }
 
@@ -539,6 +584,14 @@ static void RenderCallback()
 
 	glEnable(GL_LIGHTING);
 
+	//clear previous frames result
+	gNormal.setValue(10,0,0);
+	gPoint.setValue(0,0,0);
+	gDepth = 999.999;
+	gLastUsedMethod = -1;
+	gNumGjkIterations = -1;
+
+
 	TestEPA(gConvex0, gConvex1);
 	glMatrixMode(GL_MODELVIEW);
 	glLoadIdentity();
@@ -552,6 +605,71 @@ static void RenderCallback()
 //	DrawLine(gPoint, gPoint + gNormal*20.0f, btVector3(1,0,0), 2.0f);
 //	printf("%f:  %f  %f  %f\n", gDepth, gNormal.x(), gNormal.y(), gNormal.z());
 
+#ifdef VERBOSE_TEXT_ONSCREEN
+	glColor3f(255.f, 255.f, 255.f);
+
+	setOrthographicProjection();
+	float xOffset = 10.f;
+	float yStart = 20.f;
+	float yIncr = 20.f;
+	char buf[124];
+
+	glRasterPos3f(xOffset,yStart,0);
+	sprintf(buf,"gDepth=%f:  gNormal=(%f  %f  %f)\n", gDepth, gNormal.x(), gNormal.y(), gNormal.z());
+	BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
+	yStart += yIncr;
+
+	glRasterPos3f(xOffset,yStart,0);
+	sprintf(buf,"num GJK iterations =%d\n", gNumGjkIterations);
+	BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
+	yStart += yIncr;
+
+
+
+	
+
+	if (gLastUsedMethod >= 3)
+	{
+		switch (	gMethod)
+		{
+		case 0:
+			sprintf(buf,"Minkowski sampling Penetration depth solver\n" );
+			break;
+		case 1:
+			sprintf(buf,"Solid35 EPA Penetration depth solver\n" );
+			break;
+		case 2:
+			sprintf(buf,"EPA Penetration depth solver (WorkInProgress, zlib free\n" );
+			break;
+		default:
+				sprintf(buf,"Unknown Penetration Depth\n" );
+		}
+		glRasterPos3f(xOffset,yStart,0);
+		BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
+		yStart += yIncr;
+
+	} else
+	{
+		glRasterPos3f(xOffset,yStart,0);
+		sprintf(buf,"Hybrid GJK method %d\n", gLastUsedMethod);
+		BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
+		yStart += yIncr;
+	}
+
+	if (gLastDegenerateSimplex)
+	{
+		glRasterPos3f(xOffset,yStart,0);
+		sprintf(buf,"DegenerateSimplex %d\n", gLastDegenerateSimplex);
+		BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
+		yStart += yIncr;
+	}
+
+	
+
+
+	resetPerspectiveProjection();
+#endif //VERBOSE_TEXT_ONSCREEN
+
 	btVector3 color(0,0,0);
 	gConvex0.Render(false, color);
 	gConvex1.Render(false, color);
@@ -593,7 +711,7 @@ int main(int argc, char** argv)
 {
 	// Initialize Glut
 	glutInit(&argc, argv);
-	glutInitWindowSize(512, 512);
+	glutInitWindowSize(glutScreenWidth, glutScreenHeight);
 	glutInitDisplayMode(GLUT_RGB | GLUT_DOUBLE | GLUT_DEPTH);
 	int mainHandle = glutCreateWindow("TestBullet");
 	glutSetWindow(mainHandle);

Demos/OpenGL/DemoApplication.cpp

@@ -666,7 +666,33 @@ btRigidBody*	DemoApplication::localCreateRigidBody(float mass, const btTransform
 	return body;
 }
 
+//See http://www.lighthouse3d.com/opengl/glut/index.php?bmpfontortho
+void DemoApplication::setOrthographicProjection() 
+{
 
+	// switch to projection mode
+	glMatrixMode(GL_PROJECTION);
+	// save previous matrix which contains the 
+	//settings for the perspective projection
+	glPushMatrix();
+	// reset matrix
+	glLoadIdentity();
+	// set a 2D orthographic projection
+	gluOrtho2D(0, m_glutScreenWidth, 0, m_glutScreenHeight);
+	// invert the y axis, down is positive
+	glScalef(1, -1, 1);
+	// mover the origin from the bottom left corner
+	// to the upper left corner
+	glTranslatef(0, -m_glutScreenHeight, 0);
+	glMatrixMode(GL_MODELVIEW);
+}
+
+void DemoApplication::resetPerspectiveProjection() 
+{
+	glMatrixMode(GL_PROJECTION);
+	glPopMatrix();
+	glMatrixMode(GL_MODELVIEW);
+}
 
 
 void DemoApplication::renderme()
@@ -726,11 +752,15 @@ void DemoApplication::renderme()
 
 			float xOffset = 10.f;
 			float yStart = 20.f;
-			float yIncr = -2.f;
+			float yIncr = 20.f;
 			char buf[124];
 
 			glColor3f(0, 0, 0);
 
+			if ((m_debugMode & btIDebugDraw::DBG_NoHelpText)==0)
+				{
+				setOrthographicProjection();
+
 	#ifdef USE_QUICKPROF
 
 
@@ -751,70 +781,83 @@ void DemoApplication::renderme()
 			}
 	#endif //USE_QUICKPROF
 
-		
-			glRasterPos3f(xOffset,yStart,0);
-			sprintf(buf,"mouse to interact");
-			BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
-			yStart += yIncr;
+			
+			
+				glRasterPos3f(xOffset,yStart,0);
+				sprintf(buf,"mouse to interact");
+				BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
+				yStart += yIncr;
 
-		/*	glRasterPos3f(xOffset,yStart,0);
-			sprintf(buf,"space to reset");
-			BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
-			yStart += yIncr;
-		*/
-			glRasterPos3f(xOffset,yStart,0);
-			sprintf(buf,"cursor keys and z,x to navigate");
-			BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
-			yStart += yIncr;
+				glRasterPos3f(xOffset,yStart,0);
+				sprintf(buf,"space to reset");
+				BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
+				yStart += yIncr;
+			
+				glRasterPos3f(xOffset,yStart,0);
+				sprintf(buf,"cursor keys and z,x to navigate");
+				BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
+				yStart += yIncr;
 
-			glRasterPos3f(xOffset,yStart,0);
-			sprintf(buf,"i to toggle simulation, s single step");
-			BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
-			yStart += yIncr;
+				glRasterPos3f(xOffset,yStart,0);
+				sprintf(buf,"i to toggle simulation, s single step");
+				BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
+				yStart += yIncr;
 
-			glRasterPos3f(xOffset,yStart,0);
-			sprintf(buf,"q to quit");
-			BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
-			yStart += yIncr;
+				glRasterPos3f(xOffset,yStart,0);
+				sprintf(buf,"q to quit");
+				BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
+				yStart += yIncr;
 
-			glRasterPos3f(xOffset,yStart,0);
-			sprintf(buf,". to shoot box");
-			BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
-			yStart += yIncr;
+				glRasterPos3f(xOffset,yStart,0);
+				sprintf(buf,". to shoot box");
+				BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
+				yStart += yIncr;
 
-			// not yet hooked up again after refactoring...
+				// not yet hooked up again after refactoring...
 
-/*			glRasterPos3f(xOffset,yStart,0);
-			sprintf(buf,"d to toggle deactivation");
-			BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
-			yStart += yIncr;
-*/
+				glRasterPos3f(xOffset,yStart,0);
+				sprintf(buf,"d to toggle deactivation");
+				BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
+				yStart += yIncr;
 
-		/*	
-			glRasterPos3f(xOffset,yStart,0);
-			sprintf(buf,"a to draw temporal AABBs");
-			BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
-			yStart += yIncr;
-		*/
 
-			glRasterPos3f(xOffset,yStart,0);
-			sprintf(buf,"h to toggle help text");
-			BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
-			yStart += yIncr;
+			/*	
+				glRasterPos3f(xOffset,yStart,0);
+				sprintf(buf,"a to draw temporal AABBs");
+				BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
+				yStart += yIncr;
+			*/
 
-			//bool useBulletLCP = !(getDebugMode() & btIDebugDraw::DBG_DisableBulletLCP);
+				glRasterPos3f(xOffset,yStart,0);
+				sprintf(buf,"h to toggle help text");
+				BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
+				yStart += yIncr;
 
-			bool useCCD = (getDebugMode() & btIDebugDraw::DBG_EnableCCD);
+				
+				glRasterPos3f(xOffset,yStart,0);
+				sprintf(buf,"p to toggle profiling (+results to file)");
+				BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
+				yStart += yIncr;
 
-			glRasterPos3f(xOffset,yStart,0);
-			sprintf(buf,"1 CCD mode (adhoc) = %i",useCCD);
-			BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
-			yStart += yIncr;
 
-			glRasterPos3f(xOffset,yStart,0);
-			sprintf(buf,"+- shooting speed = %10.2f",m_ShootBoxInitialSpeed);
-			BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
-			yStart += yIncr;
+				//bool useBulletLCP = !(getDebugMode() & btIDebugDraw::DBG_DisableBulletLCP);
+
+				bool useCCD = (getDebugMode() & btIDebugDraw::DBG_EnableCCD);
+
+/*				glRasterPos3f(xOffset,yStart,0);
+				sprintf(buf,"1 CCD mode (adhoc) = %i",useCCD);
+				BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
+				yStart += yIncr;
+				*/
+
+
+				glRasterPos3f(xOffset,yStart,0);
+				sprintf(buf,"+- shooting speed = %10.2f",m_ShootBoxInitialSpeed);
+				BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
+				yStart += yIncr;
+
+				resetPerspectiveProjection();
+			}
 		
 	}
 
@@ -838,6 +881,10 @@ void	DemoApplication::clientResetScene()
 			myMotionState->m_graphicsWorldTrans = myMotionState->m_startWorldTrans;
 			colObj->setWorldTransform( myMotionState->m_graphicsWorldTrans );
 			colObj->setInterpolationWorldTransform( myMotionState->m_startWorldTrans );
+			colObj->activate();
+			//removed cached contact points
+			m_dynamicsWorld->getBroadphase()->cleanProxyFromPairs(colObj->getBroadphaseHandle());
+
 			btRigidBody* body = btRigidBody::upcast(colObj);
 			if (body && !body->isStaticObject())
 			{

Demos/OpenGL/DemoApplication.h

@@ -93,6 +93,9 @@ public:
 		return m_dynamicsWorld;
 	}
 	
+	void setOrthographicProjection();
+	void resetPerspectiveProjection();
+
 	int		getDebugMode()
 	{
 		return m_debugMode ;

src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp

@@ -37,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)
 {
 }
 
@@ -62,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;
@@ -107,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;
@@ -118,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;
 			}
@@ -136,11 +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(),   
@@ -172,7 +188,8 @@ 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
@@ -183,10 +200,15 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result&
 				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
 		
@@ -194,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,
 					localTransA,localTransB,
-					m_cachedSeparatingAxis, pointOnA, pointOnB,
+					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;
 				}
+				
 			}
 		}
 	}

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

src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp

@@ -20,29 +20,7 @@ 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+MAX_PREFERRED_PENETRATION_DIRECTIONS*2] = 
@@ -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;
@@ -247,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
@@ -286,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;

src/BulletDynamics/Dynamics/btRigidBody.h

@@ -313,6 +313,8 @@ public:
 	void	setMotionState(btMotionState* motionState)
 	{
 		m_optionalMotionState = motionState;
+		if (m_optionalMotionState)
+			motionState->getWorldTransform(m_worldTransform);
 	}
 
 	//for experimental overriding of friction/contact solver func