Simplify GJK. Still needs double precision for large differences of feature scales.

Extract faces directly from btConvexHullComputer (in initializePolyhedralFeatures), instead of reconstructing them, thanks to Josh Klint in #1654
PyBullet: use initializePolyhedralFeatures for convex hulls and boxes (to allow SAT)
PyBullet: expose setPhysicsEngineParameter(enableSAT=0 or 1) to enable Separating Axis Test based collision detection for convex vs convex/box and convex versus concave triangles (in a triangle mesh).

examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp

@@ -513,6 +513,7 @@ static btCollisionShape* createConvexHullFromShapes(std::vector<tinyobj::shape_t
 
 		convexHull->recalcLocalAabb();
 		convexHull->optimizeConvexHull();
+		convexHull->initializePolyhedralFeatures();
 		compound->addChildShape(identity,convexHull);
 	}
 
@@ -694,6 +695,7 @@ btCollisionShape* BulletURDFImporter::convertURDFToCollisionShape(const UrdfColl
 			btVector3 extents = collision->m_geometry.m_boxSize;
 			btBoxShape* boxShape = new btBoxShape(extents*0.5f);
 			//btConvexShape* boxShape = new btConeShapeX(extents[2]*0.5,extents[0]*0.5);
+			boxShape->initializePolyhedralFeatures();
             shape = boxShape;
 			shape ->setMargin(gUrdfDefaultCollisionMargin);
             break;
@@ -905,7 +907,7 @@ upAxisMat.setIdentity();
 			BT_PROFILE("convert btConvexHullShape");
 			btConvexHullShape* convexHull = new btConvexHullShape(&convertedVerts[0].getX(), convertedVerts.size(), sizeof(btVector3));
 			convexHull->optimizeConvexHull();
-			//convexHull->initializePolyhedralFeatures();
+			convexHull->initializePolyhedralFeatures();
 			convexHull->setMargin(gUrdfDefaultCollisionMargin);
 			convexHull->recalcLocalAabb();
 			//convexHull->setLocalScaling(collision->m_geometry.m_meshScale);

examples/SharedMemory/PhysicsClientC_API.cpp

@@ -611,8 +611,14 @@ B3_SHARED_API	int b3PhysicsParamSetContactSlop(b3SharedMemoryCommandHandle comma
 	return 0;
 }
 
-
+B3_SHARED_API	int b3PhysicsParameterSetEnableSAT(b3SharedMemoryCommandHandle commandHandle, int enableSAT)
-
+{
+	struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
+	b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS);
+	command->m_physSimParamArgs.m_enableSAT = enableSAT;
+	command->m_updateFlags |= SIM_PARAM_ENABLE_SAT;
+	return 0;
+}
 
 B3_SHARED_API int b3PhysicsParamSetCollisionFilterMode(b3SharedMemoryCommandHandle commandHandle, int filterMode)
 {

examples/SharedMemory/PhysicsClientC_API.h

@@ -317,6 +317,7 @@ B3_SHARED_API	int b3PhysicsParameterSetAllowedCcdPenetration(b3SharedMemoryComma
 B3_SHARED_API	int b3PhysicsParameterSetJointFeedbackMode(b3SharedMemoryCommandHandle commandHandle, int jointFeedbackMode);
 B3_SHARED_API	int b3PhysicsParamSetSolverResidualThreshold(b3SharedMemoryCommandHandle commandHandle, double solverResidualThreshold);
 B3_SHARED_API	int b3PhysicsParamSetContactSlop(b3SharedMemoryCommandHandle commandHandle, double contactSlop);
+B3_SHARED_API	int b3PhysicsParameterSetEnableSAT(b3SharedMemoryCommandHandle commandHandle, int enableSAT);
 
 
 

examples/SharedMemory/PhysicsServerCommandProcessor.cpp

@@ -6865,6 +6865,8 @@ bool PhysicsServerCommandProcessor::processRequestPhysicsSimulationParametersCom
 	serverCmd.m_simulationParameterResultArgs.m_deltaTime =  m_data->m_physicsDeltaTime;
 	serverCmd.m_simulationParameterResultArgs.m_contactBreakingThreshold = gContactBreakingThreshold;
 	serverCmd.m_simulationParameterResultArgs.m_contactSlop = m_data->m_dynamicsWorld->getSolverInfo().m_linearSlop;
+	serverCmd.m_simulationParameterResultArgs.m_enableSAT = m_data->m_dynamicsWorld->getDispatchInfo().m_enableSatConvex;
+	
 	serverCmd.m_simulationParameterResultArgs.m_defaultGlobalCFM = m_data->m_dynamicsWorld->getSolverInfo().m_globalCfm;
 	serverCmd.m_simulationParameterResultArgs.m_defaultContactERP = m_data->m_dynamicsWorld->getSolverInfo().m_erp2;
 	serverCmd.m_simulationParameterResultArgs.m_defaultNonContactERP = m_data->m_dynamicsWorld->getSolverInfo().m_erp;
@@ -6988,6 +6990,11 @@ bool PhysicsServerCommandProcessor::processSendPhysicsParametersCommand(const st
 		m_data->m_dynamicsWorld->getSolverInfo().m_linearSlop = clientCmd.m_physSimParamArgs.m_contactSlop;
 	}
 
+	if (clientCmd.m_updateFlags&SIM_PARAM_ENABLE_SAT)
+	{
+		m_data->m_dynamicsWorld->getDispatchInfo().m_enableSatConvex = clientCmd.m_physSimParamArgs.m_enableSAT;
+	}
+
 
 	if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_COLLISION_FILTER_MODE)
 	{

examples/SharedMemory/SharedMemoryCommands.h

@@ -455,6 +455,7 @@ enum EnumSimParamUpdateFlags
 	SIM_PARAM_UPDATE_DEFAULT_FRICTION_CFM = 1048576,
 	SIM_PARAM_UPDATE_SOLVER_RESIDULAL_THRESHOLD = 2097152,
 	SIM_PARAM_UPDATE_CONTACT_SLOP = 4194304,
+	SIM_PARAM_ENABLE_SAT = 8388608,
 };
 
 enum EnumLoadSoftBodyUpdateFlags

examples/SharedMemory/SharedMemoryPublic.h

@@ -797,6 +797,7 @@ struct b3PhysicsSimulationParameters
 	int m_jointFeedbackMode;
 	double m_solverResidualThreshold;
 	double m_contactSlop;
+	int m_enableSAT;
 };
 
 

examples/pybullet/pybullet.c

@@ -1434,12 +1434,13 @@ static PyObject* pybullet_setPhysicsEngineParameter(PyObject* self, PyObject* ar
 	int jointFeedbackMode =-1;
 	double solverResidualThreshold = -1;
 	double contactSlop = -1;
+	int enableSAT = -1;
 
 	int physicsClientId = 0;
-	static char* kwlist[] = {"fixedTimeStep", "numSolverIterations", "useSplitImpulse", "splitImpulsePenetrationThreshold", "numSubSteps", "collisionFilterMode", "contactBreakingThreshold", "maxNumCmdPer1ms", "enableFileCaching","restitutionVelocityThreshold", "erp", "contactERP", "frictionERP", "enableConeFriction", "deterministicOverlappingPairs", "allowedCcdPenetration", "jointFeedbackMode", "solverResidualThreshold", "contactSlop", "physicsClientId", NULL};
+	static char* kwlist[] = {"fixedTimeStep", "numSolverIterations", "useSplitImpulse", "splitImpulsePenetrationThreshold", "numSubSteps", "collisionFilterMode", "contactBreakingThreshold", "maxNumCmdPer1ms", "enableFileCaching","restitutionVelocityThreshold", "erp", "contactERP", "frictionERP", "enableConeFriction", "deterministicOverlappingPairs", "allowedCcdPenetration", "jointFeedbackMode", "solverResidualThreshold", "contactSlop", "enableSAT", "physicsClientId", NULL};
 
-	if (!PyArg_ParseTupleAndKeywords(args, keywds, "|diidiidiiddddiididdi", kwlist, &fixedTimeStep, &numSolverIterations, &useSplitImpulse, &splitImpulsePenetrationThreshold, &numSubSteps,
+	if (!PyArg_ParseTupleAndKeywords(args, keywds, "|diidiidiiddddiididdii", kwlist, &fixedTimeStep, &numSolverIterations, &useSplitImpulse, &splitImpulsePenetrationThreshold, &numSubSteps,
-									 &collisionFilterMode, &contactBreakingThreshold, &maxNumCmdPer1ms, &enableFileCaching, &restitutionVelocityThreshold, &erp, &contactERP, &frictionERP, &enableConeFriction, &deterministicOverlappingPairs, &allowedCcdPenetration, &jointFeedbackMode, &solverResidualThreshold, &contactSlop, &physicsClientId))
+									 &collisionFilterMode, &contactBreakingThreshold, &maxNumCmdPer1ms, &enableFileCaching, &restitutionVelocityThreshold, &erp, &contactERP, &frictionERP, &enableConeFriction, &deterministicOverlappingPairs, &allowedCcdPenetration, &jointFeedbackMode, &solverResidualThreshold, &contactSlop, &enableSAT, &physicsClientId))
 	{
 		return NULL;
 	}
@@ -1540,6 +1541,10 @@ static PyObject* pybullet_setPhysicsEngineParameter(PyObject* self, PyObject* ar
 			b3PhysicsParameterSetJointFeedbackMode(command,jointFeedbackMode);
 		}
 
+		if (enableSAT>=0)
+		{
+			b3PhysicsParameterSetEnableSAT(command, enableSAT);
+		}
 		statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
 	}
 

src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp

@@ -28,6 +28,7 @@ subject to the following restrictions:
 #include "BulletCollision/CollisionShapes/btConvexShape.h"
 #include "BulletCollision/CollisionShapes/btCapsuleShape.h"
 #include "BulletCollision/CollisionShapes/btTriangleShape.h"
+#include "BulletCollision/CollisionShapes/btConvexPolyhedron.h"
 
 
 
@@ -442,10 +443,26 @@ void btConvexConvexAlgorithm ::processCollision (const btCollisionObjectWrapper*
 
 		struct btDummyResult : public btDiscreteCollisionDetectorInterface::Result
 		{
+			btVector3 m_normalOnBInWorld;
+			btVector3 m_pointInWorld;
+			btScalar m_depth;
+			bool m_hasContact;
+			
+
+			btDummyResult()
+				: m_hasContact(false)
+			{
+			}
+			
+			
 			virtual void setShapeIdentifiersA(int partId0,int index0){}
 			virtual void setShapeIdentifiersB(int partId1,int index1){}
 			virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth) 
 			{
+				m_hasContact = true;
+				m_normalOnBInWorld = normalOnBInWorld;
+				m_pointInWorld = pointInWorld;
+				m_depth = depth;
 			}
 		};
 
@@ -560,15 +577,18 @@ void btConvexConvexAlgorithm ::processCollision (const btCollisionObjectWrapper*
 
 		} else
 		{
+
+
 			//we can also deal with convex versus triangle (without connectivity data)
-			if (polyhedronA->getConvexPolyhedron() && polyhedronB->getShapeType()==TRIANGLE_SHAPE_PROXYTYPE)
+			if (dispatchInfo.m_enableSatConvex && polyhedronA->getConvexPolyhedron() && polyhedronB->getShapeType()==TRIANGLE_SHAPE_PROXYTYPE)
 			{
 
-				btVertexArray vertices;
+
+				btVertexArray worldSpaceVertices;
 				btTriangleShape* tri = (btTriangleShape*)polyhedronB;
-				vertices.push_back(	body1Wrap->getWorldTransform()*tri->m_vertices1[0]);
+				worldSpaceVertices.push_back(	body1Wrap->getWorldTransform()*tri->m_vertices1[0]);
-				vertices.push_back(	body1Wrap->getWorldTransform()*tri->m_vertices1[1]);
+				worldSpaceVertices.push_back(	body1Wrap->getWorldTransform()*tri->m_vertices1[1]);
-				vertices.push_back(	body1Wrap->getWorldTransform()*tri->m_vertices1[2]);
+				worldSpaceVertices.push_back(	body1Wrap->getWorldTransform()*tri->m_vertices1[2]);
 				
 				//tri->initializePolyhedralFeatures();
 
@@ -579,17 +599,97 @@ void btConvexConvexAlgorithm ::processCollision (const btCollisionObjectWrapper*
 				btScalar maxDist = threshold;
 				
 				bool foundSepAxis = false;
-				if (0)
+				bool useSatSepNormal = true;
+
+				if (useSatSepNormal)
 				{
-					polyhedronB->initializePolyhedralFeatures();
+					if (0)
+					{
+						//initializePolyhedralFeatures performs a convex hull computation, not needed for a single triangle
+						polyhedronB->initializePolyhedralFeatures();
+					} else
+					{
+
+						btVector3 uniqueEdges[3] = {tri->m_vertices1[1]-tri->m_vertices1[0],
+							tri->m_vertices1[2]-tri->m_vertices1[1],
+							tri->m_vertices1[0]-tri->m_vertices1[2]};
+
+						uniqueEdges[0].normalize();
+						uniqueEdges[1].normalize();
+						uniqueEdges[2].normalize();
+
+						btConvexPolyhedron polyhedron;
+						polyhedron.m_vertices.push_back(tri->m_vertices1[2]);
+						polyhedron.m_vertices.push_back(tri->m_vertices1[0]);
+						polyhedron.m_vertices.push_back(tri->m_vertices1[1]);
+						
+						
+						{
+							btFace combinedFaceA;
+							combinedFaceA.m_indices.push_back(0);
+							combinedFaceA.m_indices.push_back(1);
+							combinedFaceA.m_indices.push_back(2);
+							btVector3 faceNormal = uniqueEdges[0].cross(uniqueEdges[1]);
+							faceNormal.normalize();
+							btScalar planeEq=1e30f;
+							for (int v=0;v<combinedFaceA.m_indices.size();v++)
+							{
+								btScalar eq = tri->m_vertices1[combinedFaceA.m_indices[v]].dot(faceNormal);
+								if (planeEq>eq)
+								{
+									planeEq=eq;
+								}
+							}
+							combinedFaceA.m_plane[0] = faceNormal[0];
+							combinedFaceA.m_plane[1] = faceNormal[1];
+							combinedFaceA.m_plane[2] = faceNormal[2];
+							combinedFaceA.m_plane[3] = -planeEq;
+							polyhedron.m_faces.push_back(combinedFaceA);
+						}
+						{
+							btFace combinedFaceB;
+							combinedFaceB.m_indices.push_back(0);
+							combinedFaceB.m_indices.push_back(2);
+							combinedFaceB.m_indices.push_back(1);
+							btVector3 faceNormal = -uniqueEdges[0].cross(uniqueEdges[1]);
+							faceNormal.normalize();
+							btScalar planeEq=1e30f;
+							for (int v=0;v<combinedFaceB.m_indices.size();v++)
+							{
+								btScalar eq = tri->m_vertices1[combinedFaceB.m_indices[v]].dot(faceNormal);
+								if (planeEq>eq)
+								{
+									planeEq=eq;
+								}
+							}
+
+							combinedFaceB.m_plane[0] = faceNormal[0];
+							combinedFaceB.m_plane[1] = faceNormal[1];
+							combinedFaceB.m_plane[2] = faceNormal[2];
+							combinedFaceB.m_plane[3] = -planeEq;
+							polyhedron.m_faces.push_back(combinedFaceB);
+						}
+
+						
+						polyhedron.m_uniqueEdges.push_back(uniqueEdges[0]);
+						polyhedron.m_uniqueEdges.push_back(uniqueEdges[1]);
+						polyhedron.m_uniqueEdges.push_back(uniqueEdges[2]);
+						polyhedron.initialize2();
+
+						polyhedronB->setPolyhedralFeatures(polyhedron);
+					}
+
+					
+
 					 foundSepAxis = btPolyhedralContactClipping::findSeparatingAxis(
-					*polyhedronA->getConvexPolyhedron(), *polyhedronB->getConvexPolyhedron(),
+						*polyhedronA->getConvexPolyhedron(), *polyhedronB->getConvexPolyhedron(),
-					body0Wrap->getWorldTransform(), 
+						body0Wrap->getWorldTransform(), 
-					body1Wrap->getWorldTransform(),
+						body1Wrap->getWorldTransform(),
-					sepNormalWorldSpace,*resultOut);
+						sepNormalWorldSpace,*resultOut);
 				//	 printf("sepNormalWorldSpace=%f,%f,%f\n",sepNormalWorldSpace.getX(),sepNormalWorldSpace.getY(),sepNormalWorldSpace.getZ());
 
-				} else
+				} 
+				else
 				{
 #ifdef ZERO_MARGIN
 					gjkPairDetector.setIgnoreMargin(true);
@@ -598,6 +698,24 @@ void btConvexConvexAlgorithm ::processCollision (const btCollisionObjectWrapper*
 					gjkPairDetector.getClosestPoints(input,dummy,dispatchInfo.m_debugDraw);
 #endif//ZERO_MARGIN
 					
+					if (dummy.m_hasContact && dummy.m_depth<0)
+					{
+						
+						if (foundSepAxis)
+						{
+							if (dummy.m_normalOnBInWorld.dot(sepNormalWorldSpace)<0.99)
+							{
+								printf("?\n");
+							}
+						} else
+						{
+							printf("!\n");
+						}
+						sepNormalWorldSpace.setValue(0,0,1);// = dummy.m_normalOnBInWorld;
+						//minDist = dummy.m_depth;
+						foundSepAxis = true;
+					}
+#if 0
 					btScalar l2 = gjkPairDetector.getCachedSeparatingAxis().length2();
 					if (l2>SIMD_EPSILON)
 					{
@@ -607,6 +725,7 @@ void btConvexConvexAlgorithm ::processCollision (const btCollisionObjectWrapper*
 						minDist = gjkPairDetector.getCachedSeparatingDistance()-min0->getMargin()-min1->getMargin();
 						foundSepAxis = true;
 					}
+#endif
 				}
 
 				
@@ -614,7 +733,7 @@ void btConvexConvexAlgorithm ::processCollision (const btCollisionObjectWrapper*
 			{
 				worldVertsB2.resize(0);
 				btPolyhedralContactClipping::clipFaceAgainstHull(sepNormalWorldSpace, *polyhedronA->getConvexPolyhedron(), 
-					body0Wrap->getWorldTransform(), vertices, worldVertsB2,minDist-threshold, maxDist, *resultOut);
+					body0Wrap->getWorldTransform(), worldSpaceVertices, worldVertsB2,minDist-threshold, maxDist, *resultOut);
 			}
 				
 				
@@ -626,6 +745,7 @@ void btConvexConvexAlgorithm ::processCollision (const btCollisionObjectWrapper*
 				return;
 			}
 
+			
 		}
 
 

src/BulletCollision/CollisionShapes/btConvexPolyhedron.cpp

@@ -104,9 +104,9 @@ void	btConvexPolyhedron::initialize()
 
 	btHashMap<btInternalVertexPair,btInternalEdge> edges;
 
-	btScalar TotalArea = 0.0f;
 	
-	m_localCenter.setValue(0, 0, 0);
+	
+	
 	for(int i=0;i<m_faces.size();i++)
 	{
 		int numVertices = m_faces[i].m_indices.size();
@@ -172,6 +172,13 @@ void	btConvexPolyhedron::initialize()
 	}
 #endif//USE_CONNECTED_FACES
 
+	initialize2();
+}
+
+void	btConvexPolyhedron::initialize2()
+{
+	m_localCenter.setValue(0, 0, 0);
+	btScalar TotalArea = 0.0f;
 	for(int i=0;i<m_faces.size();i++)
 	{
 		int numVertices = m_faces[i].m_indices.size();
@@ -274,7 +281,6 @@ void	btConvexPolyhedron::initialize()
 	}
 #endif
 }
-
 void btConvexPolyhedron::project(const btTransform& trans, const btVector3& dir, btScalar& minProj, btScalar& maxProj, btVector3& witnesPtMin,btVector3& witnesPtMax) const
 {
 	minProj = FLT_MAX;

src/BulletCollision/CollisionShapes/btConvexPolyhedron.h

@@ -54,6 +54,7 @@ ATTRIBUTE_ALIGNED16(class) btConvexPolyhedron
 	btVector3		mE;
 
 	void	initialize();
+	void	initialize2();
 	bool testContainment() const;
 
 	void project(const btTransform& trans, const btVector3& dir, btScalar& minProj, btScalar& maxProj, btVector3& witnesPtMin,btVector3& witnesPtMax) const;

src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp

@@ -39,6 +39,17 @@ btPolyhedralConvexShape::~btPolyhedralConvexShape()
 	}
 }
 
+void btPolyhedralConvexShape::setPolyhedralFeatures(btConvexPolyhedron& polyhedron)
+{
+	if (m_polyhedron)
+	{
+		*m_polyhedron = polyhedron;
+	} else
+	{
+		void* mem = btAlignedAlloc(sizeof(btConvexPolyhedron),16);
+		m_polyhedron = new (mem) btConvexPolyhedron(polyhedron);
+	}
+}
 
 bool	btPolyhedralConvexShape::initializePolyhedralFeatures(int shiftVerticesByMargin)
 {
@@ -87,7 +98,71 @@ bool	btPolyhedralConvexShape::initializePolyhedralFeatures(int shiftVerticesByMa
 		conv.compute(&orgVertices[0].getX(), sizeof(btVector3),orgVertices.size(),0.f,0.f);
 	}
 
+#ifndef BT_RECONSTRUCT_FACES
 	
+	int numVertices = conv.vertices.size();
+	m_polyhedron->m_vertices.resize(numVertices);
+	for (int p=0;p<numVertices;p++)
+	{
+		m_polyhedron->m_vertices[p] = conv.vertices[p];
+	}
+
+	int v0, v1;
+	for (int j = 0; j < conv.faces.size(); j++)
+	{
+		btVector3 edges[3];
+		int numEdges = 0;
+		btFace combinedFace;
+		const btConvexHullComputer::Edge* edge = &conv.edges[conv.faces[j]];
+		v0 = edge->getSourceVertex();
+		int prevVertex=v0;
+		combinedFace.m_indices.push_back(v0);
+		v1 = edge->getTargetVertex();
+		while (v1 != v0)
+		{
+		
+			btVector3 wa = conv.vertices[prevVertex];
+			btVector3 wb = conv.vertices[v1];
+			btVector3 newEdge = wb-wa;
+			newEdge.normalize();
+			if (numEdges<2)
+				edges[numEdges++] = newEdge;
+
+
+			//face->addIndex(v1);
+			combinedFace.m_indices.push_back(v1);
+			edge = edge->getNextEdgeOfFace();
+			prevVertex = v1;
+			int v01 = edge->getSourceVertex();
+			v1 = edge->getTargetVertex();
+
+		}
+		
+		btAssert(combinedFace.m_indices.size() > 2);
+
+		btVector3 faceNormal = edges[0].cross(edges[1]);
+		faceNormal.normalize();
+
+		btScalar planeEq=1e30f;
+
+		for (int v=0;v<combinedFace.m_indices.size();v++)
+		{
+			btScalar eq = m_polyhedron->m_vertices[combinedFace.m_indices[v]].dot(faceNormal);
+			if (planeEq>eq)
+			{
+				planeEq=eq;
+			}
+		}
+		combinedFace.m_plane[0] = faceNormal.getX();
+		combinedFace.m_plane[1] = faceNormal.getY();
+		combinedFace.m_plane[2] = faceNormal.getZ();
+		combinedFace.m_plane[3] = -planeEq;
+		
+		m_polyhedron->m_faces.push_back(combinedFace);
+	}
+
+
+#else//BT_RECONSTRUCT_FACES
 
 	btAlignedObjectArray<btVector3> faceNormals;
 	int numFaces = conv.faces.size();
@@ -312,6 +387,8 @@ bool	btPolyhedralConvexShape::initializePolyhedralFeatures(int shiftVerticesByMa
 
 	}
 
+#endif //BT_RECONSTRUCT_FACES
+
 	m_polyhedron->initialize();
 
 	return true;

src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.h

@@ -43,6 +43,9 @@ public:
 	///experimental/work-in-progress
 	virtual bool	initializePolyhedralFeatures(int shiftVerticesByMargin=0);
 
+	virtual void setPolyhedralFeatures(btConvexPolyhedron& polyhedron);
+	
+
 	const btConvexPolyhedron*	getConvexPolyhedron() const
 	{
 		return m_polyhedron;

src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp

@@ -19,7 +19,6 @@ subject to the following restrictions:
 #include "BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h"
 
 
-
 #if defined(DEBUG) || defined (_DEBUG)
 //#define TEST_NON_VIRTUAL 1
 #include <stdio.h> //for debug printf
@@ -29,14 +28,6 @@ subject to the following restrictions:
 #endif //__SPU__
 #endif
 
-//must be above the machine epsilon
-#ifdef  BT_USE_DOUBLE_PRECISION
-	#define REL_ERROR2 btScalar(1.0e-12)
-	btScalar gGjkEpaPenetrationTolerance = 1.0e-12;
-#else
-	#define REL_ERROR2 btScalar(1.0e-6)
-	btScalar gGjkEpaPenetrationTolerance = 0.001;
-#endif
 
 //temp globals, to improve GJK/EPA/penetration calculations
 int gNumDeepPenetrationChecks = 0;
@@ -136,177 +127,7 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu
 
 		m_simplexSolver->reset();
 		
-		for ( ; ; )
+		bool catchDegeneratePenetrationCase = 1;
-		//while (true)
-		{
-
-			btVector3 seperatingAxisInA = (-m_cachedSeparatingAxis)* input.m_transformA.getBasis();
-			btVector3 seperatingAxisInB = m_cachedSeparatingAxis* input.m_transformB.getBasis();
-
-
-			btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA);
-			btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB);
-
-			btVector3  pWorld = localTransA(pInA);	
-			btVector3  qWorld = localTransB(qInB);
-
-
-			if (check2d)
-			{
-				pWorld[2] = 0.f;
-				qWorld[2] = 0.f;
-			}
-
-			btVector3 w	= pWorld - qWorld;
-			delta = m_cachedSeparatingAxis.dot(w);
-
-			// potential exit, they don't overlap
-			if ((delta > btScalar(0.0)) && (delta * delta > squaredDistance * input.m_maximumDistanceSquared)) 
-			{
-				m_degenerateSimplex = 10;
-				checkSimplex=true;
-				//checkPenetration = false;
-				break;
-			}
-
-			//exit 0: the new point is already in the simplex, or we didn't come any closer
-			if (m_simplexSolver->inSimplex(w))
-			{
-				m_degenerateSimplex = 1;
-				checkSimplex = true;
-				break;
-			}
-			// are we getting any closer ?
-			btScalar f0 = squaredDistance - delta;
-			btScalar f1 = squaredDistance * REL_ERROR2;
-
-			if (f0 <= f1)
-			{
-				if (f0 <= btScalar(0.))
-				{
-					m_degenerateSimplex = 2;
-				} else
-				{
-					m_degenerateSimplex = 11;
-				}
-				checkSimplex = true;
-				break;
-			}
-
-			//add current vertex to simplex
-			m_simplexSolver->addVertex(w, pWorld, qWorld);
-			btVector3 newCachedSeparatingAxis;
-
-			//calculate the closest point to the origin (update vector v)
-			if (!m_simplexSolver->closest(newCachedSeparatingAxis))
-			{
-				m_degenerateSimplex = 3;
-				checkSimplex = true;
-				break;
-			}
-
-			if(newCachedSeparatingAxis.length2()<REL_ERROR2)
-            {
-				m_cachedSeparatingAxis = newCachedSeparatingAxis;
-                m_degenerateSimplex = 6;
-                checkSimplex = true;
-                break;
-            }
-
-			btScalar previousSquaredDistance = squaredDistance;
-			squaredDistance = newCachedSeparatingAxis.length2();
-#if 0
-///warning: this termination condition leads to some problems in 2d test case see Bullet/Demos/Box2dDemo
-			if (squaredDistance>previousSquaredDistance)
-			{
-				m_degenerateSimplex = 7;
-				squaredDistance = previousSquaredDistance;
-                checkSimplex = false;
-                break;
-			}
-#endif //
-			
-
-			//redundant m_simplexSolver->compute_points(pointOnA, pointOnB);
-
-			//are we getting any closer ?
-			if (previousSquaredDistance - squaredDistance <= SIMD_EPSILON * previousSquaredDistance) 
-			{ 
-//				m_simplexSolver->backup_closest(m_cachedSeparatingAxis);
-				checkSimplex = true;
-				m_degenerateSimplex = 12;
-				
-				break;
-			}
-
-			m_cachedSeparatingAxis = newCachedSeparatingAxis;
-
-			  //degeneracy, this is typically due to invalid/uninitialized worldtransforms for a btCollisionObject   
-              if (m_curIter++ > gGjkMaxIter)   
-              {   
-                      #if defined(DEBUG) || defined (_DEBUG)
-
-                              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(),   
-                              m_cachedSeparatingAxis.getZ(),   
-                              squaredDistance,   
-                              m_minkowskiA->getShapeType(),   
-                              m_minkowskiB->getShapeType());   
-
-                      #endif   
-                      break;   
-
-              } 
-
-
-			bool check = (!m_simplexSolver->fullSimplex());
-			//bool check = (!m_simplexSolver->fullSimplex() && squaredDistance > SIMD_EPSILON * m_simplexSolver->maxVertex());
-
-			if (!check)
-			{
-				//do we need this backup_closest here ?
-//				m_simplexSolver->backup_closest(m_cachedSeparatingAxis);
-				m_degenerateSimplex = 13;
-				break;
-			}
-		}
-
-		if (checkSimplex)
-		{
-			m_simplexSolver->compute_points(pointOnA, pointOnB);
-			normalInB = m_cachedSeparatingAxis;
-
-			btScalar lenSqr =m_cachedSeparatingAxis.length2();
-			
-			//valid normal
-			if (lenSqr < REL_ERROR2)
-			{
-				m_degenerateSimplex = 5;
-			} 
-			if (lenSqr > SIMD_EPSILON*SIMD_EPSILON)
-			{
-				btScalar rlen = btScalar(1.) / btSqrt(lenSqr );
-				normalInB *= rlen; //normalize
-
-				btScalar s = btSqrt(squaredDistance);
-			
-				btAssert(s > btScalar(0.0));
-				pointOnA -= m_cachedSeparatingAxis * (marginA / s);
-				pointOnB += m_cachedSeparatingAxis * (marginB / s);
-				distance = ((btScalar(1.)/rlen) - margin);
-				isValid = true;
-				
-				m_lastUsedMethod = 1;
-			} else
-			{
-				m_lastUsedMethod = 2;
-			}
-		}
-
-		bool catchDegeneratePenetrationCase = 
-			(m_catchDegeneracies && m_penetrationDepthSolver && m_degenerateSimplex && ((distance+margin) < gGjkEpaPenetrationTolerance));
 
 		//if (checkPenetration && !isValid)
 		if (checkPenetration && (!isValid || catchDegeneratePenetrationCase ))
@@ -451,14 +272,9 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu
 			} 
 
 		}
-		output.addContactPoint(
+		output.addContactPoint( normalInB, pointOnB+positionOffset, distance);
-			normalInB,
-			pointOnB+positionOffset,
-			distance);
-
 	}
 
-
 }