Index: code/trunk/src/external/bullet/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp
===================================================================
--- code/trunk/src/external/bullet/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp	(revision 5781)
+++ code/trunk/src/external/bullet/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp	(revision 8351)
@@ -21,49 +21,4 @@
 
 #define NUM_UNITSPHERE_POINTS 42
-static btVector3	sPenetrationDirections[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2] = 
-{
-btVector3(btScalar(0.000000) , btScalar(-0.000000),btScalar(-1.000000)),
-btVector3(btScalar(0.723608) , btScalar(-0.525725),btScalar(-0.447219)),
-btVector3(btScalar(-0.276388) , btScalar(-0.850649),btScalar(-0.447219)),
-btVector3(btScalar(-0.894426) , btScalar(-0.000000),btScalar(-0.447216)),
-btVector3(btScalar(-0.276388) , btScalar(0.850649),btScalar(-0.447220)),
-btVector3(btScalar(0.723608) , btScalar(0.525725),btScalar(-0.447219)),
-btVector3(btScalar(0.276388) , btScalar(-0.850649),btScalar(0.447220)),
-btVector3(btScalar(-0.723608) , btScalar(-0.525725),btScalar(0.447219)),
-btVector3(btScalar(-0.723608) , btScalar(0.525725),btScalar(0.447219)),
-btVector3(btScalar(0.276388) , btScalar(0.850649),btScalar(0.447219)),
-btVector3(btScalar(0.894426) , btScalar(0.000000),btScalar(0.447216)),
-btVector3(btScalar(-0.000000) , btScalar(0.000000),btScalar(1.000000)),
-btVector3(btScalar(0.425323) , btScalar(-0.309011),btScalar(-0.850654)),
-btVector3(btScalar(-0.162456) , btScalar(-0.499995),btScalar(-0.850654)),
-btVector3(btScalar(0.262869) , btScalar(-0.809012),btScalar(-0.525738)),
-btVector3(btScalar(0.425323) , btScalar(0.309011),btScalar(-0.850654)),
-btVector3(btScalar(0.850648) , btScalar(-0.000000),btScalar(-0.525736)),
-btVector3(btScalar(-0.525730) , btScalar(-0.000000),btScalar(-0.850652)),
-btVector3(btScalar(-0.688190) , btScalar(-0.499997),btScalar(-0.525736)),
-btVector3(btScalar(-0.162456) , btScalar(0.499995),btScalar(-0.850654)),
-btVector3(btScalar(-0.688190) , btScalar(0.499997),btScalar(-0.525736)),
-btVector3(btScalar(0.262869) , btScalar(0.809012),btScalar(-0.525738)),
-btVector3(btScalar(0.951058) , btScalar(0.309013),btScalar(0.000000)),
-btVector3(btScalar(0.951058) , btScalar(-0.309013),btScalar(0.000000)),
-btVector3(btScalar(0.587786) , btScalar(-0.809017),btScalar(0.000000)),
-btVector3(btScalar(0.000000) , btScalar(-1.000000),btScalar(0.000000)),
-btVector3(btScalar(-0.587786) , btScalar(-0.809017),btScalar(0.000000)),
-btVector3(btScalar(-0.951058) , btScalar(-0.309013),btScalar(-0.000000)),
-btVector3(btScalar(-0.951058) , btScalar(0.309013),btScalar(-0.000000)),
-btVector3(btScalar(-0.587786) , btScalar(0.809017),btScalar(-0.000000)),
-btVector3(btScalar(-0.000000) , btScalar(1.000000),btScalar(-0.000000)),
-btVector3(btScalar(0.587786) , btScalar(0.809017),btScalar(-0.000000)),
-btVector3(btScalar(0.688190) , btScalar(-0.499997),btScalar(0.525736)),
-btVector3(btScalar(-0.262869) , btScalar(-0.809012),btScalar(0.525738)),
-btVector3(btScalar(-0.850648) , btScalar(0.000000),btScalar(0.525736)),
-btVector3(btScalar(-0.262869) , btScalar(0.809012),btScalar(0.525738)),
-btVector3(btScalar(0.688190) , btScalar(0.499997),btScalar(0.525736)),
-btVector3(btScalar(0.525730) , btScalar(0.000000),btScalar(0.850652)),
-btVector3(btScalar(0.162456) , btScalar(-0.499995),btScalar(0.850654)),
-btVector3(btScalar(-0.425323) , btScalar(-0.309011),btScalar(0.850654)),
-btVector3(btScalar(-0.425323) , btScalar(0.309011),btScalar(0.850654)),
-btVector3(btScalar(0.162456) , btScalar(0.499995),btScalar(0.850654))
-};
 
 
@@ -79,4 +34,5 @@
 	(void)v;
 	
+	bool check2d= convexA->isConvex2d() && convexB->isConvex2d();
 
 	struct btIntermediateResult : public btDiscreteCollisionDetectorInterface::Result
@@ -92,8 +48,11 @@
 		bool	m_hasResult;
 
-		virtual void setShapeIdentifiers(int partId0,int index0,	int partId1,int index1)
+		virtual void setShapeIdentifiersA(int partId0,int index0)
 		{
 			(void)partId0;
 			(void)index0;
+		}
+		virtual void setShapeIdentifiersB(int partId1,int index1)
+		{
 			(void)partId1;
 			(void)index1;
@@ -109,5 +68,5 @@
 
 	//just take fixed number of orientation, and sample the penetration depth in that direction
-	btScalar minProj = btScalar(1e30);
+	btScalar minProj = btScalar(BT_LARGE_FLOAT);
 	btVector3 minNorm(btScalar(0.), btScalar(0.), btScalar(0.));
 	btVector3 minA,minB;
@@ -130,5 +89,5 @@
 	for (i=0;i<numSampleDirections;i++)
 	{
-		const btVector3& norm = sPenetrationDirections[i];
+		btVector3 norm = getPenetrationDirections()[i];
 		seperatingAxisInABatch[i] =  (-norm) * transA.getBasis() ;
 		seperatingAxisInBBatch[i] =  norm   * transB.getBasis() ;
@@ -144,5 +103,5 @@
 				convexA->getPreferredPenetrationDirection(i,norm);
 				norm  = transA.getBasis() * norm;
-				sPenetrationDirections[numSampleDirections] = norm;
+				getPenetrationDirections()[numSampleDirections] = norm;
 				seperatingAxisInABatch[numSampleDirections] = (-norm) * transA.getBasis();
 				seperatingAxisInBBatch[numSampleDirections] = norm * transB.getBasis();
@@ -161,5 +120,5 @@
 				convexB->getPreferredPenetrationDirection(i,norm);
 				norm  = transB.getBasis() * norm;
-				sPenetrationDirections[numSampleDirections] = norm;
+				getPenetrationDirections()[numSampleDirections] = norm;
 				seperatingAxisInABatch[numSampleDirections] = (-norm) * transA.getBasis();
 				seperatingAxisInBBatch[numSampleDirections] = norm * transB.getBasis();
@@ -171,4 +130,5 @@
 
 
+
 	convexA->batchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInABatch,supportVerticesABatch,numSampleDirections);
 	convexB->batchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInBBatch,supportVerticesBBatch,numSampleDirections);
@@ -176,22 +136,36 @@
 	for (i=0;i<numSampleDirections;i++)
 	{
-		const btVector3& norm = sPenetrationDirections[i];
-		seperatingAxisInA = seperatingAxisInABatch[i];
-		seperatingAxisInB = seperatingAxisInBBatch[i];
-
-		pInA = supportVerticesABatch[i];
-		qInB = supportVerticesBBatch[i];
-
-		pWorld = transA(pInA);	
-		qWorld = transB(qInB);
-		w	= qWorld - pWorld;
-		btScalar delta = norm.dot(w);
-		//find smallest delta
-		if (delta < minProj)
-		{
-			minProj = delta;
-			minNorm = norm;
-			minA = pWorld;
-			minB = qWorld;
+		btVector3 norm = getPenetrationDirections()[i];
+		if (check2d)
+		{
+			norm[2] = 0.f;
+		}
+		if (norm.length2()>0.01)
+		{
+
+			seperatingAxisInA = seperatingAxisInABatch[i];
+			seperatingAxisInB = seperatingAxisInBBatch[i];
+
+			pInA = supportVerticesABatch[i];
+			qInB = supportVerticesBBatch[i];
+
+			pWorld = transA(pInA);	
+			qWorld = transB(qInB);
+			if (check2d)
+			{
+				pWorld[2] = 0.f;
+				qWorld[2] = 0.f;
+			}
+
+			w	= qWorld - pWorld;
+			btScalar delta = norm.dot(w);
+			//find smallest delta
+			if (delta < minProj)
+			{
+				minProj = delta;
+				minNorm = norm;
+				minA = pWorld;
+				minB = qWorld;
+			}
 		}
 	}	
@@ -210,5 +184,5 @@
 				convexA->getPreferredPenetrationDirection(i,norm);
 				norm  = transA.getBasis() * norm;
-				sPenetrationDirections[numSampleDirections] = norm;
+				getPenetrationDirections()[numSampleDirections] = norm;
 				numSampleDirections++;
 			}
@@ -225,5 +199,5 @@
 				convexB->getPreferredPenetrationDirection(i,norm);
 				norm  = transB.getBasis() * norm;
-				sPenetrationDirections[numSampleDirections] = norm;
+				getPenetrationDirections()[numSampleDirections] = norm;
 				numSampleDirections++;
 			}
@@ -234,5 +208,5 @@
 	for (int i=0;i<numSampleDirections;i++)
 	{
-		const btVector3& norm = sPenetrationDirections[i];
+		const btVector3& norm = getPenetrationDirections()[i];
 		seperatingAxisInA = (-norm)* transA.getBasis();
 		seperatingAxisInB = norm* transB.getBasis();
@@ -262,5 +236,6 @@
 		return false;
 
-	minProj += (convexA->getMarginNonVirtual() + convexB->getMarginNonVirtual());
+	btScalar extraSeparation = 0.5f;///scale dependent
+	minProj += extraSeparation+(convexA->getMarginNonVirtual() + convexB->getMarginNonVirtual());
 
 
@@ -300,7 +275,8 @@
 	input.m_transformA = displacedTrans;
 	input.m_transformB = transB;
-	input.m_maximumDistanceSquared = btScalar(1e30);//minProj;
+	input.m_maximumDistanceSquared = btScalar(BT_LARGE_FLOAT);//minProj;
 	
 	btIntermediateResult res;
+	gjkdet.setCachedSeperatingAxis(-minNorm);
 	gjkdet.getClosestPoints(input,res,debugDraw);
 
@@ -311,4 +287,5 @@
 	btScalar penetration_relaxation= btScalar(1.);
 	minNorm*=penetration_relaxation;
+	
 
 	if (res.m_hasResult)
@@ -317,4 +294,5 @@
 		pa = res.m_pointInWorld - minNorm * correctedMinNorm;
 		pb = res.m_pointInWorld;
+		v = minNorm;
 		
 #ifdef DEBUG_DRAW
@@ -331,4 +309,54 @@
 }
 
-
-
+btVector3*	btMinkowskiPenetrationDepthSolver::getPenetrationDirections()
+{
+	static btVector3	sPenetrationDirections[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2] = 
+	{
+	btVector3(btScalar(0.000000) , btScalar(-0.000000),btScalar(-1.000000)),
+	btVector3(btScalar(0.723608) , btScalar(-0.525725),btScalar(-0.447219)),
+	btVector3(btScalar(-0.276388) , btScalar(-0.850649),btScalar(-0.447219)),
+	btVector3(btScalar(-0.894426) , btScalar(-0.000000),btScalar(-0.447216)),
+	btVector3(btScalar(-0.276388) , btScalar(0.850649),btScalar(-0.447220)),
+	btVector3(btScalar(0.723608) , btScalar(0.525725),btScalar(-0.447219)),
+	btVector3(btScalar(0.276388) , btScalar(-0.850649),btScalar(0.447220)),
+	btVector3(btScalar(-0.723608) , btScalar(-0.525725),btScalar(0.447219)),
+	btVector3(btScalar(-0.723608) , btScalar(0.525725),btScalar(0.447219)),
+	btVector3(btScalar(0.276388) , btScalar(0.850649),btScalar(0.447219)),
+	btVector3(btScalar(0.894426) , btScalar(0.000000),btScalar(0.447216)),
+	btVector3(btScalar(-0.000000) , btScalar(0.000000),btScalar(1.000000)),
+	btVector3(btScalar(0.425323) , btScalar(-0.309011),btScalar(-0.850654)),
+	btVector3(btScalar(-0.162456) , btScalar(-0.499995),btScalar(-0.850654)),
+	btVector3(btScalar(0.262869) , btScalar(-0.809012),btScalar(-0.525738)),
+	btVector3(btScalar(0.425323) , btScalar(0.309011),btScalar(-0.850654)),
+	btVector3(btScalar(0.850648) , btScalar(-0.000000),btScalar(-0.525736)),
+	btVector3(btScalar(-0.525730) , btScalar(-0.000000),btScalar(-0.850652)),
+	btVector3(btScalar(-0.688190) , btScalar(-0.499997),btScalar(-0.525736)),
+	btVector3(btScalar(-0.162456) , btScalar(0.499995),btScalar(-0.850654)),
+	btVector3(btScalar(-0.688190) , btScalar(0.499997),btScalar(-0.525736)),
+	btVector3(btScalar(0.262869) , btScalar(0.809012),btScalar(-0.525738)),
+	btVector3(btScalar(0.951058) , btScalar(0.309013),btScalar(0.000000)),
+	btVector3(btScalar(0.951058) , btScalar(-0.309013),btScalar(0.000000)),
+	btVector3(btScalar(0.587786) , btScalar(-0.809017),btScalar(0.000000)),
+	btVector3(btScalar(0.000000) , btScalar(-1.000000),btScalar(0.000000)),
+	btVector3(btScalar(-0.587786) , btScalar(-0.809017),btScalar(0.000000)),
+	btVector3(btScalar(-0.951058) , btScalar(-0.309013),btScalar(-0.000000)),
+	btVector3(btScalar(-0.951058) , btScalar(0.309013),btScalar(-0.000000)),
+	btVector3(btScalar(-0.587786) , btScalar(0.809017),btScalar(-0.000000)),
+	btVector3(btScalar(-0.000000) , btScalar(1.000000),btScalar(-0.000000)),
+	btVector3(btScalar(0.587786) , btScalar(0.809017),btScalar(-0.000000)),
+	btVector3(btScalar(0.688190) , btScalar(-0.499997),btScalar(0.525736)),
+	btVector3(btScalar(-0.262869) , btScalar(-0.809012),btScalar(0.525738)),
+	btVector3(btScalar(-0.850648) , btScalar(0.000000),btScalar(0.525736)),
+	btVector3(btScalar(-0.262869) , btScalar(0.809012),btScalar(0.525738)),
+	btVector3(btScalar(0.688190) , btScalar(0.499997),btScalar(0.525736)),
+	btVector3(btScalar(0.525730) , btScalar(0.000000),btScalar(0.850652)),
+	btVector3(btScalar(0.162456) , btScalar(-0.499995),btScalar(0.850654)),
+	btVector3(btScalar(-0.425323) , btScalar(-0.309011),btScalar(0.850654)),
+	btVector3(btScalar(-0.425323) , btScalar(0.309011),btScalar(0.850654)),
+	btVector3(btScalar(0.162456) , btScalar(0.499995),btScalar(0.850654))
+	};
+
+	return sPenetrationDirections;
+}
+
+
