diff --git a/Demos/BasicDemo/BasicDemo.cpp b/Demos/BasicDemo/BasicDemo.cpp
index 8f4801759..9620a4cd0 100644
--- a/Demos/BasicDemo/BasicDemo.cpp
+++ b/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);
diff --git a/Demos/BspDemo/BspDemo.cpp b/Demos/BspDemo/BspDemo.cpp
index feb592c80..0989a3ada 100644
--- a/Demos/BspDemo/BspDemo.cpp
+++ b/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
diff --git a/Demos/ConstraintDemo/ConstraintDemo.cpp b/Demos/ConstraintDemo/ConstraintDemo.cpp
index 2ff91c6fd..cced57a7f 100644
--- a/Demos/ConstraintDemo/ConstraintDemo.cpp
+++ b/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);
 }
diff --git a/Demos/EPAPenDepthDemo/PenetrationTestBullet.cpp b/Demos/EPAPenDepthDemo/PenetrationTestBullet.cpp
index 5c9d8e7c9..2b2b3635f 100644
--- a/Demos/EPAPenDepthDemo/PenetrationTestBullet.cpp
+++ b/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);
diff --git a/Demos/OpenGL/DemoApplication.cpp b/Demos/OpenGL/DemoApplication.cpp
index 3055afe96..17cceb192 100644
--- a/Demos/OpenGL/DemoApplication.cpp
+++ b/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())
 			{
diff --git a/Demos/OpenGL/DemoApplication.h b/Demos/OpenGL/DemoApplication.h
index d00a32bf9..4af74af0d 100644
--- a/Demos/OpenGL/DemoApplication.h
+++ b/Demos/OpenGL/DemoApplication.h
@@ -93,6 +93,9 @@ public:
 		return m_dynamicsWorld;
 	}
 	
+	void setOrthographicProjection();
+	void resetPerspectiveProjection();
+
 	int		getDebugMode()
 	{
 		return m_debugMode ;
diff --git a/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp b/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp
index e61c15d93..507fcaf21 100644
--- a/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp
+++ b/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;
 				}
+				
 			}
 		}
 	}
diff --git a/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h b/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h
index c4842cd30..09c1669bd 100644
--- a/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h
+++ b/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/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp b/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp
index 6b2112ef3..74cf2b7c0 100644
--- a/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp
+++ b/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;
diff --git a/src/BulletDynamics/Dynamics/btRigidBody.h b/src/BulletDynamics/Dynamics/btRigidBody.h
index 62a29fa59..c5fecdb87 100644
--- a/src/BulletDynamics/Dynamics/btRigidBody.h
+++ b/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