perform GrahamScanConvexHull2D around an arbitrary oriented 2D plane in 3D, to fix some convex hull face merging problems

add compound shape support to BulletXmlWorldImporter and fix some compile issue under Debian (hopefully)
object picking change in the demos: create a ball-socket picking constraint when holding shift while mouse dragging, otherwise a fixed (6dof) constraint
add assert in constraint solver, when both objects have their inertia tensor rows set to zero
btPolyhedralContactClipping: add edge-edge contact point in  findSeparatingAxis (similar to the default GJK case)

Demos/BasicDemo/BasicDemo.cpp

@@ -34,6 +34,9 @@ subject to the following restrictions:
 #include "btBulletDynamicsCommon.h"
 
 #include <stdio.h> //printf debugging
+#include "GLDebugDrawer.h"
+
+static GLDebugDrawer gDebugDraw;
 
 
 void BasicDemo::clientMoveAndDisplay()
@@ -100,11 +103,13 @@ void	BasicDemo::initPhysics()
 	m_solver = sol;
 
 	m_dynamicsWorld = new btDiscreteDynamicsWorld(m_dispatcher,m_broadphase,m_solver,m_collisionConfiguration);
+	m_dynamicsWorld->setDebugDrawer(&gDebugDraw);
 	
 	m_dynamicsWorld->setGravity(btVector3(0,-10,0));
 
 	///create a few basic rigid bodies
-	btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(50.),btScalar(50.),btScalar(50.)));
+	btBoxShape* groundShape = new btBoxShape(btVector3(btScalar(50.),btScalar(50.),btScalar(50.)));
+	//groundShape->initializePolyhedralFeatures();
 //	btCollisionShape* groundShape = new btStaticPlaneShape(btVector3(0,1,0),50);
 	
 	m_collisionShapes.push_back(groundShape);
@@ -138,7 +143,7 @@ void	BasicDemo::initPhysics()
 		//create a few dynamic rigidbodies
 		// Re-using the same collision is better for memory usage and performance
 
-		btCollisionShape* colShape = new btBoxShape(btVector3(SCALING*1,SCALING*1,SCALING*1));
+		btBoxShape* colShape = new btBoxShape(btVector3(SCALING*1,SCALING*1,SCALING*1));
 		//btCollisionShape* colShape = new btSphereShape(btScalar(1.));
 		m_collisionShapes.push_back(colShape);
 

Demos/BasicDemo/main.cpp

@@ -19,8 +19,6 @@ subject to the following restrictions:
 #include "LinearMath/btHashMap.h"
 
 
-
-
 	
 int main(int argc,char** argv)
 {

Demos/BulletXmlImportDemo/BulletXmlImportDemo.cpp

@@ -28,15 +28,24 @@ subject to the following restrictions:
 #include <stdio.h> //printf debugging
 
 
+
+#include "GLDebugDrawer.h"
+GLDebugDrawer	gDebugDrawer;
+
+
 void BulletXmlImportDemo::initPhysics()
 {
-		setTexturing(true);
+	setTexturing(true);
 	setShadows(true);
 
-	setCameraDistance(btScalar(30.));
+	
 
 	setupEmptyDynamicsWorld();
 
+	
+	m_dynamicsWorld->setDebugDrawer(&gDebugDrawer);
+
+
 	btBulletXmlWorldImporter* importer = new btBulletXmlWorldImporter(m_dynamicsWorld);
 	importer->loadFile("bullet_basic.xml");
 //	importer->loadFile("bulletser.xml");
@@ -54,6 +63,8 @@ void BulletXmlImportDemo::clientMoveAndDisplay()
 	///step the simulation
 	if (m_dynamicsWorld)
 	{
+	
+
 		m_dynamicsWorld->stepSimulation(ms / 1000000.f);
 		m_dynamicsWorld->debugDrawWorld();
 	}
@@ -90,6 +101,7 @@ void	BulletXmlImportDemo::setupEmptyDynamicsWorld()
 
 	///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded)
 	m_dispatcher = new	btCollisionDispatcher(m_collisionConfiguration);
+
 	btGImpactCollisionAlgorithm::registerAlgorithm(m_dispatcher);
 
 	m_broadphase = new btDbvtBroadphase();
@@ -101,7 +113,7 @@ void	BulletXmlImportDemo::setupEmptyDynamicsWorld()
 
 	//btGImpactCollisionAlgorithm::registerAlgorithm((btCollisionDispatcher*)m_dynamicsWorld->getDispatcher());
 
-	m_dynamicsWorld->setGravity(btVector3(0,-10,0));
+	
 
 }
 
@@ -115,7 +127,11 @@ BulletXmlImportDemo::~BulletXmlImportDemo()
 	exitPhysics();
 }
 
-
+void BulletXmlImportDemo::clientResetScene()
+{
+	exitPhysics();
+	initPhysics();
+}
 
 
 void	BulletXmlImportDemo::exitPhysics()

Demos/BulletXmlImportDemo/BulletXmlImportDemo.h

@@ -55,9 +55,13 @@ class BulletXmlImportDemo : public PlatformDemoApplication
 
 	BulletXmlImportDemo()
 	{
+		//m_idle=true;
+		setCameraDistance(btScalar(30.));
 	}
 	virtual ~BulletXmlImportDemo();
 
+	virtual void clientResetScene();
+
 	void	initPhysics();
 
 	void	setupEmptyDynamicsWorld();

Demos/BulletXmlImportDemo/CMakeLists.txt

@@ -11,9 +11,6 @@ SET(GLUT_ROOT ${BULLET_PHYSICS_SOURCE_DIR}/Glut)
 # You shouldn't have to modify anything below this line 
 ########################################################
 
-IF(BUILD_AMD_OPENCL_DEMOS)
-	SUBDIRS(AMD)
-ENDIF()
 
 INCLUDE_DIRECTORIES(
 ${BULLET_PHYSICS_SOURCE_DIR}/src 

Demos/BulletXmlImportDemo/main.cpp

@@ -15,12 +15,11 @@ subject to the following restrictions:
 
 #include "BulletXmlImportDemo.h"
 #include "GlutStuff.h"
-#include "GLDebugDrawer.h"
+
 #include "btBulletDynamicsCommon.h"
 #include "LinearMath/btHashMap.h"
 
 
-
 #ifdef USE_AMD_OPENCL
 
 
@@ -84,7 +83,7 @@ bool initCL( void* glCtx, void* glDC )
 	
 int main(int argc,char** argv)
 {
-	GLDebugDrawer	gDebugDrawer;
+	
 #ifdef USE_AMD_OPENCL
 	
 	bool initialized = initCL(0,0);
@@ -94,8 +93,7 @@ int main(int argc,char** argv)
 	
 	BulletXmlImportDemo serializeDemo;
 	serializeDemo.initPhysics();
-	serializeDemo.getDynamicsWorld()->setDebugDrawer(&gDebugDrawer);
+	
-
 
 #ifdef CHECK_MEMORY_LEAKS
 	serializeDemo.exitPhysics();

Demos/FractureDemo/CMakeLists.txt

@@ -62,6 +62,7 @@ ENDIF()
 					POST_BUILD
 #		  		COMMAND copy /Y ${BULLET_PHYSICS_SOURCE_DIR}/GLUT32.DLL ${CMAKE_CURRENT_BINARY_DIR}
 					COMMAND ${CMAKE_COMMAND} ARGS -E copy_if_different ${BULLET_PHYSICS_SOURCE_DIR}/GLUT32.DLL ${CMAKE_CURRENT_BINARY_DIR}
+					COMMAND ${CMAKE_COMMAND} ARGS -E copy_if_different ${BULLET_PHYSICS_SOURCE_DIR}/GLUT32.DLL ${CMAKE_CURRENT_BINARY_DIR}/Debug
 				)
 			ENDIF(CMAKE_CL_64)
 		ENDIF (NOT INTERNAL_CREATE_DISTRIBUTABLE_MSVC_PROJECTFILES)

Demos/FractureDemo/btFractureBody.cpp

@@ -22,12 +22,13 @@ void	btFractureBody::recomputeConnectivity(btCollisionWorld* world)
 				struct   MyContactResultCallback : public btCollisionWorld::ContactResultCallback
 				{
 					bool m_connected;
-					MyContactResultCallback() :m_connected(false)
+					btScalar m_margin;
+					MyContactResultCallback() :m_connected(false),m_margin(0.05)
 					{
 					}
 					virtual   btScalar   addSingleResult(btManifoldPoint& cp,   const btCollisionObjectWrapper* colObj0Wrap,int partId0,int index0,const btCollisionObjectWrapper* colObj1Wrap,int partId1,int index1)
 					{
-						if (cp.getDistance()<=0)
+						if (cp.getDistance()<=m_margin)
 							m_connected = true;
 						return 1.f;
 					}

Demos/OpenGL/DemoApplication.cpp

@@ -33,7 +33,7 @@ subject to the following restrictions:
 #include "LinearMath/btSerializer.h"
 #include "GLDebugFont.h"
 
-static bool use6Dof = false;
+
 extern bool gDisableDeactivation;
 int numObjects = 0;
 const int maxNumObjects = 16384;
@@ -535,7 +535,7 @@ void	DemoApplication::setShootBoxShape ()
 	if (!m_shootBoxShape)
 	{
 		btBoxShape* box = new btBoxShape(btVector3(0.5,0.5,0.5));
-        box->initializePolyhedralFeatures();
+      //  box->initializePolyhedralFeatures();
 		m_shootBoxShape = box;
 	}
 }
@@ -776,7 +776,7 @@ void DemoApplication::mouseFunc(int button, int state, int x, int y)
 								
 
 
-								if (use6Dof)
+								if ((m_modifierKeys& BT_ACTIVE_SHIFT)==0)
 								{
 									btTransform tr;
 									tr.setIdentity();
@@ -822,8 +822,7 @@ void DemoApplication::mouseFunc(int button, int state, int x, int y)
 									
 
 								}
-								use6Dof = !use6Dof;
+					
-
 								//save mouse position for dragging
 								gOldPickingPos = rayTo;
 								gHitPos = pickPos;

Extras/Serialize/BulletWorldImporter/btWorldImporter.cpp

@@ -215,7 +215,10 @@ btCollisionShape* btWorldImporter::convertCollisionShape(  btCollisionShapeData*
 				{
 					case BOX_SHAPE_PROXYTYPE:
 						{
-							shape = createBoxShape(implicitShapeDimensions/localScaling+margin);
+							btBoxShape* box= (btBoxShape*)createBoxShape(implicitShapeDimensions/localScaling+margin);
+							//box->initializePolyhedralFeatures();
+							shape = box;
+							
 							break;
 						}
 					case SPHERE_SHAPE_PROXYTYPE:
@@ -333,6 +336,7 @@ btCollisionShape* btWorldImporter::convertCollisionShape(  btCollisionShapeData*
 							{
 								hullShape->addPoint(tmpPoints[i]);
 							}
+							hullShape->setMargin(bsd->m_collisionMargin);
 							//hullShape->initializePolyhedralFeatures();
 							shape = hullShape;
 							break;
@@ -345,7 +349,8 @@ btCollisionShape* btWorldImporter::convertCollisionShape(  btCollisionShapeData*
 
 				if (shape)
 				{
-					//shape->setMargin(bsd->m_collisionMargin);
+					shape->setMargin(bsd->m_collisionMargin);
+					
 					btVector3 localScaling;
 					localScaling.deSerializeFloat(bsd->m_localScaling);
 					shape->setLocalScaling(localScaling);

Extras/Serialize/BulletXmlWorldImporter/btBulletXmlWorldImporter.cpp

@@ -238,7 +238,7 @@ void btBulletXmlWorldImporter::deSerializeConvexHullShapeData(TiXmlNode* pParent
 	SET_INT_VALUE(pParent,convexHullData,m_numUnscaledPoints);
 
 	m_collisionShapeData.push_back((btCollisionShapeData*)convexHullData);
-	m_pointerLookup.insert(ptr,convexHullData);
+	m_pointerLookup.insert((void*)ptr,convexHullData);
 }
 
 void btBulletXmlWorldImporter::deSerializeCompoundShapeChildData(TiXmlNode* pParent)
@@ -292,7 +292,7 @@ void btBulletXmlWorldImporter::deSerializeCompoundShapeChildData(TiXmlNode* pPar
 	{
 		m_compoundShapeChildDataArrays.push_back(compoundChildArrayPtr);
 		btCompoundShapeChildData* cd = &compoundChildArrayPtr->at(0);
-		m_pointerLookup.insert(ptr,cd);
+		m_pointerLookup.insert((void*)ptr,cd);
 	}
 	
 }
@@ -328,7 +328,7 @@ void btBulletXmlWorldImporter::deSerializeCompoundShapeData(TiXmlNode* pParent)
 	SET_FLOAT_VALUE(pParent, compoundData,m_collisionMargin);
 
 	m_collisionShapeData.push_back((btCollisionShapeData*)compoundData);
-	m_pointerLookup.insert(ptr,compoundData);
+	m_pointerLookup.insert((void*)ptr,compoundData);
 
 }
 
@@ -348,12 +348,17 @@ void btBulletXmlWorldImporter::deSerializeStaticPlaneShapeData(TiXmlNode* pParen
 	SET_FLOAT_VALUE(pParent, planeData,m_planeConstant);
 
 	m_collisionShapeData.push_back((btCollisionShapeData*)planeData);
-	m_pointerLookup.insert(ptr,planeData);
+	m_pointerLookup.insert((void*)ptr,planeData);
 
 }
 
 void btBulletXmlWorldImporter::deSerializeDynamicsWorldData(TiXmlNode* pParent)
 {
+	btContactSolverInfo solverInfo;
+	//btVector3 gravity(0,0,0);
+
+	//setDynamicsWorldInfo(gravity,solverInfo);
+
 	//gravity and world info
 }
 
@@ -371,12 +376,13 @@ void btBulletXmlWorldImporter::deSerializeConvexInternalShapeData(TiXmlNode* pPa
 
 	deSerializeCollisionShapeData(xmlShapeData,&convexShape->m_collisionShapeData);
 
+	
 	SET_FLOAT_VALUE(pParent,convexShape,m_collisionMargin)
 	SET_VECTOR4_VALUE(pParent,convexShape,m_localScaling)
 	SET_VECTOR4_VALUE(pParent,convexShape,m_implicitShapeDimensions)
 
 	m_collisionShapeData.push_back((btCollisionShapeData*)convexShape);
-	m_pointerLookup.insert(ptr,convexShape);
+	m_pointerLookup.insert((void*)ptr,convexShape);
 
 }
 
@@ -433,7 +439,7 @@ void btBulletXmlWorldImporter::deSerializeGeneric6DofConstraintData(TiXmlNode* p
 	SET_INT_VALUE(pParent, dof6Data,m_useOffsetForConstraintFrame);
 	
 	m_constraintData.push_back((btTypedConstraintData*)dof6Data);
-	m_pointerLookup.insert(ptr,dof6Data);
+	m_pointerLookup.insert((void*)ptr,dof6Data);
 }
 
 void btBulletXmlWorldImporter::deSerializeRigidBodyFloatData(TiXmlNode* pParent)
@@ -500,7 +506,7 @@ void btBulletXmlWorldImporter::deSerializeRigidBodyFloatData(TiXmlNode* pParent)
 
 
 	m_rigidBodyData.push_back(rbData);
-	m_pointerLookup.insert(ptr,rbData);
+	m_pointerLookup.insert((void*)ptr,rbData);
 	
 //	rbData->m_collisionObjectData.m_collisionShape = (void*) (int)atof(txt);
 }
@@ -558,13 +564,13 @@ void	btBulletXmlWorldImporter::fixupConstraintData(btTypedConstraintData* tcd)
 {
 	if (tcd->m_rbA)
 	{
-		btRigidBodyData** ptrptr = (btRigidBodyData**)m_pointerLookup.find((intptr_t)tcd->m_rbA);
+		btRigidBodyData** ptrptr = (btRigidBodyData**)m_pointerLookup.find(tcd->m_rbA);
 		btAssert(ptrptr);
 		tcd->m_rbA = ptrptr? *ptrptr : 0;
 	}
 	if (tcd->m_rbB)
 	{
-		btRigidBodyData** ptrptr = (btRigidBodyData**)m_pointerLookup.find((intptr_t)tcd->m_rbB);
+		btRigidBodyData** ptrptr = (btRigidBodyData**)m_pointerLookup.find(tcd->m_rbB);
 		btAssert(ptrptr);
 		tcd->m_rbB = ptrptr? *ptrptr : 0;
 	}
@@ -581,7 +587,7 @@ void	btBulletXmlWorldImporter::fixupCollisionDataPointers(btCollisionShapeData*
 			{
 				btCompoundShapeData* compound = (btCompoundShapeData*) shapeData;
 				int ptr = (intptr_t) compound->m_childShapePtr;
-				void** cdptr = m_pointerLookup.find(ptr);
+				void** cdptr = m_pointerLookup.find((void*)ptr);
 				btCompoundShapeChildData** c = (btCompoundShapeChildData**)cdptr;
 				btAssert(c);
 				if (c)
@@ -598,7 +604,7 @@ void	btBulletXmlWorldImporter::fixupCollisionDataPointers(btCollisionShapeData*
 			{
 				btConvexHullShapeData* convexData = (btConvexHullShapeData*)shapeData;
 				int ptr = (intptr_t)convexData->m_unscaledPointsFloatPtr;
-				btVector3FloatData** ptrptr = (btVector3FloatData**)m_pointerLookup.find(ptr);
+				btVector3FloatData** ptrptr = (btVector3FloatData**)m_pointerLookup.find((void*)ptr);
 				btAssert(ptrptr);
 				if (ptrptr)
 				{
@@ -646,7 +652,7 @@ void btBulletXmlWorldImporter::auto_serialize_root_level_children(TiXmlNode* pPa
 				for (int i=0;i<numVectors;i++)
 					vectors[i] = v[i];
 				m_floatVertexArrays.push_back(vectors);
-				m_pointerLookup.insert(ptr,vectors);
+				m_pointerLookup.insert((void*)ptr,vectors);
 				continue;
 			}
 
@@ -713,8 +719,7 @@ void btBulletXmlWorldImporter::auto_serialize_root_level_children(TiXmlNode* pPa
 		for (int c=0;c<childDataArray->size();c++)
 		{
 			btCompoundShapeChildData* childData = &childDataArray->at(c);
-			int hashKey = (intptr_t) childData->m_childShape;
+			btCollisionShapeData** ptrptr = (btCollisionShapeData**)m_pointerLookup[childData->m_childShape];
-			btCollisionShapeData** ptrptr = (btCollisionShapeData**)m_pointerLookup[hashKey];
 			btAssert(ptrptr);
 			if (ptrptr)
 			{
@@ -734,8 +739,8 @@ void btBulletXmlWorldImporter::auto_serialize_root_level_children(TiXmlNode* pPa
 	for (int i=0;i<m_rigidBodyData.size();i++)
 	{
 		btRigidBodyData* rbData = m_rigidBodyData[i];
-		int hashKey = (intptr_t)rbData->m_collisionObjectData.m_collisionShape;
+		
-		void** ptrptr = m_pointerLookup.find(hashKey);
+		void** ptrptr = m_pointerLookup.find(rbData->m_collisionObjectData.m_collisionShape);
 		//btAssert(ptrptr);
 		rbData->m_collisionObjectData.m_broadphaseHandle = 0;
 		rbData->m_collisionObjectData.m_rootCollisionShape = 0;

Extras/Serialize/BulletXmlWorldImporter/btBulletXmlWorldImporter.h

@@ -43,7 +43,7 @@ protected:
 	btAlignedObjectArray<btAlignedObjectArray<btCompoundShapeChildData>* >		m_compoundShapeChildDataArrays;
 	btAlignedObjectArray<btRigidBodyData*>				m_rigidBodyData;
 	btAlignedObjectArray<btTypedConstraintData*>		m_constraintData;
-	btHashMap<btHashInt,void*>							m_pointerLookup;
+	btHashMap<btHashPtr,void*>							m_pointerLookup;
 	int													m_fileVersion;
 	bool												m_fileOk;
 

src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp

@@ -84,7 +84,7 @@ btPersistentManifold*	btCollisionDispatcher::getNewManifold(const btCollisionObj
 
 	btScalar contactProcessingThreshold = btMin(body0->getContactProcessingThreshold(),body1->getContactProcessingThreshold());
 		
-	void* mem = 0;
+ 	void* mem = 0;
 	
 	if (m_persistentManifoldPoolAllocator->getFreeCount())
 	{

src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp

@@ -407,9 +407,51 @@ void btConvexConvexAlgorithm ::processCollision (const btCollisionObjectWrapper*
 			{
 			}
 		};
+
+		
+		struct btWithoutMarginResult : public btDiscreteCollisionDetectorInterface::Result
+		{
+			btDiscreteCollisionDetectorInterface::Result* m_originalResult;
+			btVector3	m_reportedNormalOnWorld;
+			btScalar m_marginOnA;
+			btScalar m_marginOnB;
+			btScalar	m_reportedDistance;
+			
+			bool		m_foundResult;
+			btWithoutMarginResult(btDiscreteCollisionDetectorInterface::Result* result, btScalar marginOnA, btScalar marginOnB)
+			:m_originalResult(result),
+			m_marginOnA(marginOnA),
+			m_marginOnB(marginOnB),
+			m_foundResult(false)
+			{
+			}
+			
+			virtual void setShapeIdentifiersA(int partId0,int index0){}
+			virtual void setShapeIdentifiersB(int partId1,int index1){}
+			virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorldOrg,btScalar depthOrg) 
+			{
+				m_reportedDistance = depthOrg;
+				m_reportedNormalOnWorld = normalOnBInWorld;
+				
+				btVector3 adjustedPointB = pointInWorldOrg - normalOnBInWorld*m_marginOnB;
+				m_reportedDistance = depthOrg+(m_marginOnA+m_marginOnB);
+				if (m_reportedDistance<0.f)
+				{
+					m_foundResult = true;					
+				}
+				m_originalResult->addContactPoint(normalOnBInWorld,adjustedPointB,m_reportedDistance);
+			}
+		};
+
 		
 		btDummyResult dummy;
 
+///btBoxShape is an exception: its vertices are created WITH margin so don't subtract it
+
+		btScalar min0Margin = min0->getShapeType()==BOX_SHAPE_PROXYTYPE? 0.f : min0->getMargin();
+		btScalar min1Margin = min1->getShapeType()==BOX_SHAPE_PROXYTYPE? 0.f : min1->getMargin();
+
+		btWithoutMarginResult	withoutMargin(resultOut, min0Margin,min1Margin);
 
 		btPolyhedralConvexShape* polyhedronA = (btPolyhedralConvexShape*) min0;
 		btPolyhedralConvexShape* polyhedronB = (btPolyhedralConvexShape*) min1;
@@ -431,32 +473,35 @@ void btConvexConvexAlgorithm ::processCollision (const btCollisionObjectWrapper*
 					*polyhedronA->getConvexPolyhedron(), *polyhedronB->getConvexPolyhedron(),
 					body0Wrap->getWorldTransform(), 
 					body1Wrap->getWorldTransform(),
-					sepNormalWorldSpace);
+					sepNormalWorldSpace,*resultOut);
 			} else
 			{
 #ifdef ZERO_MARGIN
 				gjkPairDetector.setIgnoreMargin(true);
 				gjkPairDetector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw);
 #else
-				//gjkPairDetector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw);
+
-				gjkPairDetector.getClosestPoints(input,dummy,dispatchInfo.m_debugDraw);
+
+				gjkPairDetector.getClosestPoints(input,withoutMargin,dispatchInfo.m_debugDraw);
+				//gjkPairDetector.getClosestPoints(input,dummy,dispatchInfo.m_debugDraw);
 #endif //ZERO_MARGIN
-				btScalar l2 = gjkPairDetector.getCachedSeparatingAxis().length2();
+				//btScalar l2 = gjkPairDetector.getCachedSeparatingAxis().length2();
-				if (l2>SIMD_EPSILON)
+				//if (l2>SIMD_EPSILON)
 				{
-					sepNormalWorldSpace = gjkPairDetector.getCachedSeparatingAxis()*(1.f/l2);
+					sepNormalWorldSpace = withoutMargin.m_reportedNormalOnWorld;//gjkPairDetector.getCachedSeparatingAxis()*(1.f/l2);
 					//minDist = -1e30f;//gjkPairDetector.getCachedSeparatingDistance();
-					minDist = gjkPairDetector.getCachedSeparatingDistance()-min0->getMargin()-min1->getMargin();
+					minDist = withoutMargin.m_reportedDistance;//gjkPairDetector.getCachedSeparatingDistance()+min0->getMargin()+min1->getMargin();
 	
 #ifdef ZERO_MARGIN
 					foundSepAxis = true;//gjkPairDetector.getCachedSeparatingDistance()<0.f;
 #else
-					foundSepAxis = gjkPairDetector.getCachedSeparatingDistance()<(min0->getMargin()+min1->getMargin());
+					foundSepAxis = withoutMargin.m_foundResult && minDist<0;//-(min0->getMargin()+min1->getMargin());
 #endif
 				}
 			}
 			if (foundSepAxis)
 			{
+				
 //				printf("sepNormalWorldSpace=%f,%f,%f\n",sepNormalWorldSpace.getX(),sepNormalWorldSpace.getY(),sepNormalWorldSpace.getZ());
 
 				btPolyhedralContactClipping::clipHullAgainstHull(sepNormalWorldSpace, *polyhedronA->getConvexPolyhedron(), *polyhedronB->getConvexPolyhedron(),
@@ -498,7 +543,7 @@ void btConvexConvexAlgorithm ::processCollision (const btCollisionObjectWrapper*
 					*polyhedronA->getConvexPolyhedron(), *polyhedronB->getConvexPolyhedron(),
 					body0Wrap->getWorldTransform(), 
 					body1Wrap->getWorldTransform(),
-					sepNormalWorldSpace);
+					sepNormalWorldSpace,*resultOut);
 				//	 printf("sepNormalWorldSpace=%f,%f,%f\n",sepNormalWorldSpace.getX(),sepNormalWorldSpace.getY(),sepNormalWorldSpace.getZ());
 
 				} else

src/BulletCollision/CollisionShapes/btConvexHullShape.cpp

@@ -201,13 +201,11 @@ const char*	btConvexHullShape::serialize(void* dataBuffer, btSerializer* seriali
 	return "btConvexHullShapeData";
 }
 
-void btConvexHullShape::project(const btTransform& trans, const btVector3& dir, btScalar& min, btScalar& max) const
+void btConvexHullShape::project(const btTransform& trans, const btVector3& dir, btScalar& minProj, btScalar& maxProj, btVector3& witnesPtMin,btVector3& witnesPtMax) const
 {
 #if 1
-	min = FLT_MAX;
+	minProj = FLT_MAX;
-	max = -FLT_MAX;
+	maxProj = -FLT_MAX;
-	btVector3 witnesPtMin;
-	btVector3 witnesPtMax;
 
 	int numVerts = m_unscaledPoints.size();
 	for(int i=0;i<numVerts;i++)
@@ -215,31 +213,30 @@ void btConvexHullShape::project(const btTransform& trans, const btVector3& dir,
 		btVector3 vtx = m_unscaledPoints[i] * m_localScaling;
 		btVector3 pt = trans * vtx;
 		btScalar dp = pt.dot(dir);
-		if(dp < min)	
+		if(dp < minProj)	
 		{
-			min = dp;
+			minProj = dp;
 			witnesPtMin = pt;
 		}
-		if(dp > max)	
+		if(dp > maxProj)	
 		{
-			max = dp;
+			maxProj = dp;
 			witnesPtMax=pt;
 		}
 	}
 #else
 	btVector3 localAxis = dir*trans.getBasis();
-	btVector3 vtx1 = trans(localGetSupportingVertex(localAxis));
+	witnesPtMin  = trans(localGetSupportingVertex(localAxis));
-	btVector3 vtx2 = trans(localGetSupportingVertex(-localAxis));
+	witnesPtMax = trans(localGetSupportingVertex(-localAxis));
 
-	min = vtx1.dot(dir);
+	minProj = witnesPtMin.dot(dir);
-	max = vtx2.dot(dir);
+	maxProj = witnesPtMax.dot(dir);
 #endif
 
-	if(min>max)
+	if(minProj>maxProj)
 	{
-		btScalar tmp = min;
+		btSwap(minProj,maxProj);
-		min = max;
+		btSwap(witnesPtMin,witnesPtMax);
-		max = tmp;
 	}
 
 

src/BulletCollision/CollisionShapes/btConvexHullShape.h

@@ -73,7 +73,7 @@ public:
 	virtual void	batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const;
 	
 
-	virtual void project(const btTransform& trans, const btVector3& dir, btScalar& min, btScalar& max) const;
+	virtual void project(const btTransform& trans, const btVector3& dir, btScalar& minProj, btScalar& maxProj, btVector3& witnesPtMin,btVector3& witnesPtMax) const;
 
 
 	//debugging

src/BulletCollision/CollisionShapes/btConvexPolyhedron.cpp

@@ -274,23 +274,29 @@ void	btConvexPolyhedron::initialize()
 #endif
 }
 
-
+void btConvexPolyhedron::project(const btTransform& trans, const btVector3& dir, btScalar& minProj, btScalar& maxProj, btVector3& witnesPtMin,btVector3& witnesPtMax) const
-void btConvexPolyhedron::project(const btTransform& trans, const btVector3& dir, btScalar& min, btScalar& max) const
 {
-	min = FLT_MAX;
+	minProj = FLT_MAX;
-	max = -FLT_MAX;
+	maxProj = -FLT_MAX;
 	int numVerts = m_vertices.size();
 	for(int i=0;i<numVerts;i++)
 	{
 		btVector3 pt = trans * m_vertices[i];
 		btScalar dp = pt.dot(dir);
-		if(dp < min)	min = dp;
+		if(dp < minProj)
-		if(dp > max)	max = dp;
+		{
+			minProj = dp;
+			witnesPtMin = pt;
+		}
+		if(dp > maxProj)
+		{
+			maxProj = dp;
+			witnesPtMax = pt;
+		}
 	}
-	if(min>max)
+	if(minProj>maxProj)
 	{
-		btScalar tmp = min;
+		btSwap(minProj,maxProj);
-		min = max;
+		btSwap(witnesPtMin,witnesPtMax);
-		max = tmp;
 	}
 }

src/BulletCollision/CollisionShapes/btConvexPolyhedron.h

@@ -56,7 +56,7 @@ ATTRIBUTE_ALIGNED16(class) btConvexPolyhedron
 	void	initialize();
 	bool testContainment() const;
 
-	void project(const btTransform& trans, const btVector3& dir, btScalar& min, btScalar& max) const;
+	void project(const btTransform& trans, const btVector3& dir, btScalar& minProj, btScalar& maxProj, btVector3& witnesPtMin,btVector3& witnesPtMax) const;
 };
 
 	

src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp

@@ -37,7 +37,7 @@ btPolyhedralConvexShape::~btPolyhedralConvexShape()
 }
 
 
-bool	btPolyhedralConvexShape::initializePolyhedralFeatures()
+bool	btPolyhedralConvexShape::initializePolyhedralFeatures(int shiftVerticesByMargin)
 {
 
 	if (m_polyhedron)
@@ -56,31 +56,34 @@ bool	btPolyhedralConvexShape::initializePolyhedralFeatures()
 		btVector3& newVertex = orgVertices.expand();
 		getVertex(i,newVertex);
 	}
-
+	
-#if 0
+	btConvexHullComputer conv;
-	btAlignedObjectArray<btVector3> planeEquations;
+	
-	btGeometryUtil::getPlaneEquationsFromVertices(orgVertices,planeEquations);
+	if (shiftVerticesByMargin)
-
-	btAlignedObjectArray<btVector3> shiftedPlaneEquations;
-	for (int p=0;p<planeEquations.size();p++)
 	{
-		   btVector3 plane = planeEquations[p];
+		btAlignedObjectArray<btVector3> planeEquations;
-		   plane[3] -= getMargin();
+		btGeometryUtil::getPlaneEquationsFromVertices(orgVertices,planeEquations);
-		   shiftedPlaneEquations.push_back(plane);
+
+		btAlignedObjectArray<btVector3> shiftedPlaneEquations;
+		for (int p=0;p<planeEquations.size();p++)
+		{
+			   btVector3 plane = planeEquations[p];
+			   btScalar margin = getMargin();
+			   plane[3] -= getMargin();
+			   shiftedPlaneEquations.push_back(plane);
+		}
+
+		btAlignedObjectArray<btVector3> tmpVertices;
+
+		btGeometryUtil::getVerticesFromPlaneEquations(shiftedPlaneEquations,tmpVertices);
+	
+		conv.compute(&tmpVertices[0].getX(), sizeof(btVector3),tmpVertices.size(),0.f,0.f);
+	} else
+	{
+		
+		conv.compute(&orgVertices[0].getX(), sizeof(btVector3),orgVertices.size(),0.f,0.f);
 	}
 
-	btAlignedObjectArray<btVector3> tmpVertices;
-
-	btGeometryUtil::getVerticesFromPlaneEquations(shiftedPlaneEquations,tmpVertices);
-	btConvexHullComputer conv;
-	conv.compute(&tmpVertices[0].getX(), sizeof(btVector3),tmpVertices.size(),0.f,0.f);
-
-#else
-	btConvexHullComputer conv;
-	conv.compute(&orgVertices[0].getX(), sizeof(btVector3),orgVertices.size(),0.f,0.f);
-
-#endif
-
 
 
 	btAlignedObjectArray<btVector3> faceNormals;
@@ -193,7 +196,8 @@ bool	btPolyhedralConvexShape::initializePolyhedralFeatures()
 		{
 			//do the merge: use Graham Scan 2d convex hull
 
-			btAlignedObjectArray<GrahamVector2> orgpoints;
+			btAlignedObjectArray<GrahamVector3> orgpoints;
+			btVector3 averageFaceNormal(0,0,0);
 
 			for (int i=0;i<coplanarFaceGroup.size();i++)
 			{
@@ -201,16 +205,12 @@ bool	btPolyhedralConvexShape::initializePolyhedralFeatures()
 
 				btFace& face = tmpFaces[coplanarFaceGroup[i]];
 				btVector3 faceNormal(face.m_plane[0],face.m_plane[1],face.m_plane[2]);
-				btVector3 xyPlaneNormal(0,0,1);
+				averageFaceNormal+=faceNormal;
-
-				btQuaternion rotationArc = shortestArcQuat(faceNormal,xyPlaneNormal);
-				
 				for (int f=0;f<face.m_indices.size();f++)
 				{
 					int orgIndex = face.m_indices[f];
 					btVector3 pt = m_polyhedron->m_vertices[orgIndex];
-					btVector3 rotatedPt =  quatRotate(rotationArc,pt);
+					
-					rotatedPt.setZ(0);
 					bool found = false;
 
 					for (int i=0;i<orgpoints.size();i++)
@@ -223,34 +223,45 @@ bool	btPolyhedralConvexShape::initializePolyhedralFeatures()
 						}
 					}
 					if (!found)
-						orgpoints.push_back(GrahamVector2(rotatedPt,orgIndex));
+						orgpoints.push_back(GrahamVector3(pt,orgIndex));
 				}
 			}
 
+			
+
 			btFace combinedFace;
 			for (int i=0;i<4;i++)
 				combinedFace.m_plane[i] = tmpFaces[coplanarFaceGroup[0]].m_plane[i];
 
-			btAlignedObjectArray<GrahamVector2> hull;
+			btAlignedObjectArray<GrahamVector3> hull;
-			GrahamScanConvexHull2D(orgpoints,hull);
+
+			averageFaceNormal.normalize();
+			GrahamScanConvexHull2D(orgpoints,hull,averageFaceNormal);
 
 			for (int i=0;i<hull.size();i++)
 			{
 				combinedFace.m_indices.push_back(hull[i].m_orgIndex);
-				for(int k = 0; k < orgpoints.size(); k++) {
+				for(int k = 0; k < orgpoints.size(); k++) 
-					if(orgpoints[k].m_orgIndex == hull[i].m_orgIndex) {
+				{
+					if(orgpoints[k].m_orgIndex == hull[i].m_orgIndex) 
+					{
 						orgpoints[k].m_orgIndex = -1; // invalidate...
 						break;
-			}
+					}
 				}
 			}
+
 			// are there rejected vertices?
 			bool reject_merge = false;
+			
+
+
 			for(int i = 0; i < orgpoints.size(); i++) {
 				if(orgpoints[i].m_orgIndex == -1)
 					continue; // this is in the hull...
 				// this vertex is rejected -- is anybody else using this vertex?
 				for(int j = 0; j < tmpFaces.size(); j++) {
+					
 					btFace& face = tmpFaces[j];
 					// is this a face of the current coplanar group?
 					bool is_in_current_group = false;
@@ -276,20 +287,23 @@ bool	btPolyhedralConvexShape::initializePolyhedralFeatures()
 				if(reject_merge)
 					break;
 			}
-			if(!reject_merge) {
+
+			if (!reject_merge)
+			{
 				// do this merge!
 				did_merge = true;
-			m_polyhedron->m_faces.push_back(combinedFace);
+				m_polyhedron->m_faces.push_back(combinedFace);
 			}
 		}
 		if(!did_merge)
 		{
 			for (int i=0;i<coplanarFaceGroup.size();i++)
 			{
-				m_polyhedron->m_faces.push_back(tmpFaces[coplanarFaceGroup[i]]);
+				btFace face = tmpFaces[coplanarFaceGroup[i]];
+				m_polyhedron->m_faces.push_back(face);
 			}
 
-		}
+		} 
 
 
 

src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.h

@@ -40,7 +40,8 @@ public:
 	virtual ~btPolyhedralConvexShape();
 
 	///optional method mainly used to generate multiple contact points by clipping polyhedral features (faces/edges)
-	virtual bool	initializePolyhedralFeatures();
+	///experimental/work-in-progress
+	virtual bool	initializePolyhedralFeatures(int shiftVerticesByMargin=0);
 
 	const btConvexPolyhedron*	getConvexPolyhedron() const
 	{

src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.cpp

@@ -77,12 +77,15 @@ void btPolyhedralContactClipping::clipFace(const btVertexArray& pVtxIn, btVertex
 }
 
 
-static bool TestSepAxis(const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA,const btTransform& transB, const btVector3& sep_axis, btScalar& depth)
+static bool TestSepAxis(const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA,const btTransform& transB, const btVector3& sep_axis, btScalar& depth, btVector3& witnessPointA, btVector3& witnessPointB)
 {
 	btScalar Min0,Max0;
 	btScalar Min1,Max1;
-	hullA.project(transA,sep_axis, Min0, Max0);
+	btVector3 witnesPtMinA,witnesPtMaxA;
-	hullB.project(transB, sep_axis, Min1, Max1);
+	btVector3 witnesPtMinB,witnesPtMaxB;
+
+	hullA.project(transA,sep_axis, Min0, Max0,witnesPtMinA,witnesPtMaxA);
+	hullB.project(transB, sep_axis, Min1, Max1,witnesPtMinB,witnesPtMaxB);
 
 	if(Max0<Min1 || Max1<Min0)
 		return false;
@@ -91,7 +94,19 @@ static bool TestSepAxis(const btConvexPolyhedron& hullA, const btConvexPolyhedro
 	btAssert(d0>=0.0f);
 	btScalar d1 = Max1 - Min0;
 	btAssert(d1>=0.0f);
-	depth = d0<d1 ? d0:d1;
+	if (d0<d1)
+	{
+		depth = d0;
+		witnessPointA = witnesPtMaxA;
+		witnessPointB = witnesPtMinB;
+
+	} else
+	{
+		depth = d1;
+		witnessPointA = witnesPtMinA;
+		witnessPointB = witnesPtMaxB;
+	}
+	
 	return true;
 }
 
@@ -163,8 +178,66 @@ void InverseTransformPoint3x3(btVector3& out, const btVector3& in, const btTrans
 }
 #endif //TEST_INTERNAL_OBJECTS
 
+ 
+ 
+ SIMD_FORCE_INLINE void btSegmentsClosestPoints(
+	btVector3& ptsVector,
+	btVector3& offsetA,
+	btVector3& offsetB,
+	btScalar& tA, btScalar& tB,
+	const btVector3& translation,
+	const btVector3& dirA, btScalar hlenA,
+	const btVector3& dirB, btScalar hlenB )
+{
+	// compute the parameters of the closest points on each line segment
+
+	btScalar dirA_dot_dirB = btDot(dirA,dirB);
+	btScalar dirA_dot_trans = btDot(dirA,translation);
+	btScalar dirB_dot_trans = btDot(dirB,translation);
+
+	btScalar denom = 1.0f - dirA_dot_dirB * dirA_dot_dirB;
+
+	if ( denom == 0.0f ) {
+		tA = 0.0f;
+	} else {
+		tA = ( dirA_dot_trans - dirB_dot_trans * dirA_dot_dirB ) / denom;
+		if ( tA < -hlenA )
+			tA = -hlenA;
+		else if ( tA > hlenA )
+			tA = hlenA;
+	}
+
+	tB = tA * dirA_dot_dirB - dirB_dot_trans;
+
+	if ( tB < -hlenB ) {
+		tB = -hlenB;
+		tA = tB * dirA_dot_dirB + dirA_dot_trans;
+
+		if ( tA < -hlenA )
+			tA = -hlenA;
+		else if ( tA > hlenA )
+			tA = hlenA;
+	} else if ( tB > hlenB ) {
+		tB = hlenB;
+		tA = tB * dirA_dot_dirB + dirA_dot_trans;
+
+		if ( tA < -hlenA )
+			tA = -hlenA;
+		else if ( tA > hlenA )
+			tA = hlenA;
+	}
+
+	// compute the closest points relative to segment centers.
+
+	offsetA = dirA * tA;
+	offsetB = dirB * tB;
+
+	ptsVector = translation - offsetA + offsetB;
+}
 
-bool btPolyhedralContactClipping::findSeparatingAxis(	const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA,const btTransform& transB, btVector3& sep)
+
+
+bool btPolyhedralContactClipping::findSeparatingAxis(	const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA,const btTransform& transB, btVector3& sep, btDiscreteCollisionDetectorInterface::Result& resultOut)
 {
 	gActualSATPairTests++;
 
@@ -182,9 +255,9 @@ bool btPolyhedralContactClipping::findSeparatingAxis(	const btConvexPolyhedron&
 	for(int i=0;i<numFacesA;i++)
 	{
 		const btVector3 Normal(hullA.m_faces[i].m_plane[0], hullA.m_faces[i].m_plane[1], hullA.m_faces[i].m_plane[2]);
-		const btVector3 faceANormalWS = transA.getBasis() * Normal;
+		btVector3 faceANormalWS = transA.getBasis() * Normal;
 		if (DeltaC2.dot(faceANormalWS)<0)
-			continue;
+			faceANormalWS*=-1.f;
 
 		curPlaneTests++;
 #ifdef TEST_INTERNAL_OBJECTS
@@ -195,7 +268,8 @@ bool btPolyhedralContactClipping::findSeparatingAxis(	const btConvexPolyhedron&
 #endif
 
 		btScalar d;
-		if(!TestSepAxis( hullA, hullB, transA,transB, faceANormalWS, d))
+		btVector3 wA,wB;
+		if(!TestSepAxis( hullA, hullB, transA,transB, faceANormalWS, d,wA,wB))
 			return false;
 
 		if(d<dmin)
@@ -210,9 +284,9 @@ bool btPolyhedralContactClipping::findSeparatingAxis(	const btConvexPolyhedron&
 	for(int i=0;i<numFacesB;i++)
 	{
 		const btVector3 Normal(hullB.m_faces[i].m_plane[0], hullB.m_faces[i].m_plane[1], hullB.m_faces[i].m_plane[2]);
-		const btVector3 WorldNormal = transB.getBasis() * Normal;
+		btVector3 WorldNormal = transB.getBasis() * Normal;
 		if (DeltaC2.dot(WorldNormal)<0)
-			continue;
+			WorldNormal *=-1.f;
 
 		curPlaneTests++;
 #ifdef TEST_INTERNAL_OBJECTS
@@ -223,7 +297,8 @@ bool btPolyhedralContactClipping::findSeparatingAxis(	const btConvexPolyhedron&
 #endif
 
 		btScalar d;
-		if(!TestSepAxis(hullA, hullB,transA,transB, WorldNormal,d))
+		btVector3 wA,wB;
+		if(!TestSepAxis(hullA, hullB,transA,transB, WorldNormal,d,wA,wB))
 			return false;
 
 		if(d<dmin)
@@ -234,6 +309,12 @@ bool btPolyhedralContactClipping::findSeparatingAxis(	const btConvexPolyhedron&
 	}
 
 	btVector3 edgeAstart,edgeAend,edgeBstart,edgeBend;
+	int edgeA=-1;
+	int edgeB=-1;
+	btVector3 worldEdgeA;
+	btVector3 worldEdgeB;
+	btVector3 witnessPointA,witnessPointB;
+	
 
 	int curEdgeEdge = 0;
 	// Test edges
@@ -252,7 +333,7 @@ bool btPolyhedralContactClipping::findSeparatingAxis(	const btConvexPolyhedron&
 			{
 				Cross = Cross.normalize();
 				if (DeltaC2.dot(Cross)<0)
-					continue;
+					Cross *= -1.f;
 
 
 #ifdef TEST_INTERNAL_OBJECTS
@@ -263,20 +344,68 @@ bool btPolyhedralContactClipping::findSeparatingAxis(	const btConvexPolyhedron&
 #endif
 
 				btScalar dist;
-				if(!TestSepAxis( hullA, hullB, transA,transB, Cross, dist))
+				btVector3 wA,wB;
+				if(!TestSepAxis( hullA, hullB, transA,transB, Cross, dist,wA,wB))
 					return false;
 
 				if(dist<dmin)
 				{
 					dmin = dist;
 					sep = Cross;
+					edgeA=e0;
+					edgeB=e1;
+					worldEdgeA = WorldEdge0;
+					worldEdgeB = WorldEdge1;
+					witnessPointA=wA;
+					witnessPointB=wB;
 				}
 			}
 		}
 
 	}
 
-	if((DeltaC2.dot(sep))>0.0f)
+	if (edgeA>=0&&edgeB>=0)
+	{
+//		printf("edge-edge\n");
+		//add an edge-edge contact
+
+		btVector3 ptsVector;
+		btVector3 offsetA;
+		btVector3 offsetB;
+		btScalar tA;
+		btScalar tB;
+
+		btVector3 translation = witnessPointB-witnessPointA;
+
+		btVector3 dirA = worldEdgeA;
+		btVector3 dirB = worldEdgeB;
+		
+		btScalar hlenB = 1e30f;
+		btScalar hlenA = 1e30f;
+
+		btSegmentsClosestPoints(ptsVector,offsetA,offsetB,tA,tB,
+			translation,
+			dirA, hlenA,
+			dirB,hlenB);
+
+		btScalar nlSqrt = ptsVector.length2();
+		if (nlSqrt>SIMD_EPSILON)
+		{
+			btScalar nl = btSqrt(nlSqrt);
+			ptsVector *= 1.f/nl;
+			if (ptsVector.dot(DeltaC2)<0.f)
+			{
+				ptsVector*=-1.f;
+			}
+			btVector3 ptOnB = witnessPointB + offsetB;
+			btScalar distance = nl;
+			resultOut.addContactPoint(ptsVector, ptOnB,-distance);
+		}
+
+	}
+
+
+	if((DeltaC2.dot(sep))<0.0f)
 		sep = -sep;
 
 	return true;
@@ -359,8 +488,8 @@ void	btPolyhedralContactClipping::clipFaceAgainstHull(const btVector3& separatin
 		btScalar planeEqWS=localPlaneEq-planeNormalWS.dot(transA.getOrigin());
 		for (int i=0;i<pVtxIn->size();i++)
 		{
-			
+			btVector3 vtx = pVtxIn->at(i);
-			btScalar depth = planeNormalWS.dot(pVtxIn->at(i))+planeEqWS;
+			btScalar depth = planeNormalWS.dot(vtx)+planeEqWS;
 			if (depth <=minDist)
 			{
 //				printf("clamped: depth=%f to minDist=%f\n",depth,minDist);
@@ -395,6 +524,9 @@ void	btPolyhedralContactClipping::clipFaceAgainstHull(const btVector3& separatin
 }
 
 
+
+
+
 void	btPolyhedralContactClipping::clipHullAgainstHull(const btVector3& separatingNormal1, const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA,const btTransform& transB, const btScalar minDist, btScalar maxDist,btDiscreteCollisionDetectorInterface::Result& resultOut)
 {
 
@@ -404,6 +536,7 @@ void	btPolyhedralContactClipping::clipHullAgainstHull(const btVector3& separatin
 	//const btVector3 DeltaC2 = c0 - c1;
 
 
+
 	int closestFaceB=-1;
 	btScalar dmax = -FLT_MAX;
 	{

src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.h

@@ -35,7 +35,7 @@ struct btPolyhedralContactClipping
 	static void clipHullAgainstHull(const btVector3& separatingNormal, const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA,const btTransform& transB, const btScalar minDist, btScalar maxDist, btDiscreteCollisionDetectorInterface::Result& resultOut);
 	static void	clipFaceAgainstHull(const btVector3& separatingNormal, const btConvexPolyhedron& hullA,  const btTransform& transA, btVertexArray& worldVertsB1, const btScalar minDist, btScalar maxDist,btDiscreteCollisionDetectorInterface::Result& resultOut);
 
-	static bool findSeparatingAxis(	const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA,const btTransform& transB, btVector3& sep);
+	static bool findSeparatingAxis(	const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA,const btTransform& transB, btVector3& sep, btDiscreteCollisionDetectorInterface::Result& resultOut);
 
 	///the clipFace method is used internally
 	static void clipFace(const btVertexArray& pVtxIn, btVertexArray& ppVtxOut, const btVector3& planeNormalWS,btScalar planeEqWS);

src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp

@@ -1146,8 +1146,9 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
 							sum += iMJaA.dot(solverConstraint.m_relpos1CrossNormal);
 							sum += iMJlB.dot(solverConstraint.m_contactNormal);
 							sum += iMJaB.dot(solverConstraint.m_relpos2CrossNormal);
-
+							btScalar fsum = btFabs(sum);
-							solverConstraint.m_jacDiagABInv = btScalar(1.)/sum;
+							btAssert(fsum > SIMD_EPSILON);
+							solverConstraint.m_jacDiagABInv = fsum>SIMD_EPSILON?btScalar(1.)/sum : 0.f;
 						}
 
 

src/LinearMath/btGrahamScan2dConvexHull.h

@@ -21,9 +21,9 @@ subject to the following restrictions:
 #include "btVector3.h"
 #include "btAlignedObjectArray.h"
 
-struct GrahamVector2 : public btVector3
+struct GrahamVector3 : public btVector3
 {
-	GrahamVector2(const btVector3& org, int orgIndex)
+	GrahamVector3(const btVector3& org, int orgIndex)
 		:btVector3(org),
 			m_orgIndex(orgIndex)
 	{
@@ -39,7 +39,7 @@ struct btAngleCompareFunc {
 	: m_anchor(anchor) 
 	{
 	}
-	bool operator()(const GrahamVector2& a, const GrahamVector2& b) const {
+	bool operator()(const GrahamVector3& a, const GrahamVector3& b) const {
 		if (a.m_angle != b.m_angle)
 			return a.m_angle < b.m_angle;
 		else
@@ -56,32 +56,38 @@ struct btAngleCompareFunc {
 	}
 };
 
-inline void GrahamScanConvexHull2D(btAlignedObjectArray<GrahamVector2>& originalPoints, btAlignedObjectArray<GrahamVector2>& hull)
+inline void GrahamScanConvexHull2D(btAlignedObjectArray<GrahamVector3>& originalPoints, btAlignedObjectArray<GrahamVector3>& hull, const btVector3& normalAxis)
 {
+	btVector3 axis0,axis1;
+	btPlaneSpace1(normalAxis,axis0,axis1);
+	
+
 	if (originalPoints.size()<=1)
 	{
 		for (int i=0;i<originalPoints.size();i++)
 			hull.push_back(originalPoints[0]);
 		return;
 	}
-	//step1 : find anchor point with smallest x/y and move it to first location
+	//step1 : find anchor point with smallest projection on axis0 and move it to first location
-	//also precompute angles
 	for (int i=0;i<originalPoints.size();i++)
 	{
 		const btVector3& left = originalPoints[i];
 		const btVector3& right = originalPoints[0];
-		if (left.x() < right.x() || 
+		btScalar projL = originalPoints[i].dot(axis0);
-            (!(right.x() < left.x()) && left.y() < right.y()))
+		btScalar projR = originalPoints[0].dot(axis0);
+		if (projL < projR)
 		{
 			originalPoints.swap(0,i);
 		}
 	}
 
-	for (int i=0;i<originalPoints.size();i++)
+	//also precompute angles
+	originalPoints[0].m_angle = -1e30f;
+	for (int i=1;i<originalPoints.size();i++)
 	{
-		btVector3 xvec(1,0,0);
+		btVector3 xvec = axis0;
 		btVector3 ar = originalPoints[i]-originalPoints[0];
-		originalPoints[i].m_angle = btCross(xvec, ar).dot(btVector3(0,0,1)) / ar.length();
+		originalPoints[i].m_angle = btCross(xvec, ar).dot(normalAxis) / ar.length();
 	}
 
 	//step 2: sort all points, based on 'angle' with this anchor
@@ -99,7 +105,7 @@ inline void GrahamScanConvexHull2D(btAlignedObjectArray<GrahamVector2>& original
 		while (!isConvex&& hull.size()>1) {
 			btVector3& a = hull[hull.size()-2];
 			btVector3& b = hull[hull.size()-1];
-			isConvex = btCross(a-b,a-originalPoints[i]).dot(btVector3(0,0,1))> 0;
+			isConvex = btCross(a-b,a-originalPoints[i]).dot(normalAxis)> 0;
 			if (!isConvex)
 				hull.pop_back();
 			else