fix in btParallelConstraintSolver to support double precision

fixes in SAT/polyhedral contact clipping, avoid adding GJK contacts (the contact margin causes different contact depths)
add polyhedral convex shape in InternalEdgeDemo as example of the new SAT/polyhedral contact clipping (added reference to Manual/what's new)
avoid glueing objecs with contacts that are positive (no gaps)

Demos/ConcaveRaycastDemo/ConcaveRaycastDemo.cpp

@@ -275,7 +275,7 @@ void ConcaveRaycastDemo::keyboardCallback(unsigned char key, int x, int y)
 
 void	ConcaveRaycastDemo::initPhysics()
 {
-	m_ShootBoxInitialSpeed = 1000;
+	
 	#define TRISIZE 10.f
 
 
@@ -378,37 +378,7 @@ int maxNumOutstandingTasks = 4;
 
 
 
-	{
-		btVector3 camPos(0.000000,10.260604,-28.190779);
-		btVector3 destination(6958.333333,-8539.096384,7501.875480);
-
-		float mass = 1.f;
-		btTransform startTransform;
-		startTransform.setIdentity();
-		startTransform.setOrigin(camPos);
-
-		setShootBoxShape ();
-
-		btRigidBody* body = this->localCreateRigidBody(mass, startTransform,m_shootBoxShape);
-		body->setLinearFactor(btVector3(1,1,1));
-		//body->setRestitution(1);
-
-		btVector3 linVel(destination[0]-camPos[0],destination[1]-camPos[1],destination[2]-camPos[2]);
-		linVel.normalize();
-		linVel*=m_ShootBoxInitialSpeed;
-
-		body->getWorldTransform().setOrigin(camPos);
-		body->getWorldTransform().setRotation(btQuaternion(0,0,0,1));
-		body->setLinearVelocity(linVel);
-		body->setAngularVelocity(btVector3(0,0,0));
-		body->setCcdMotionThreshold(0.5);
-		body->setCcdSweptSphereRadius(0.9f);
-		printf("shootBox uid=%d\n", body->getBroadphaseHandle()->getUid());
-		printf("camPos=%f,%f,%f\n",camPos.getX(),camPos.getY(),camPos.getZ());
-		printf("destination=%f,%f,%f\n",destination.getX(),destination.getY(),destination.getZ());
-
 	
-	}
 }
 
 void ConcaveRaycastDemo::clientMoveAndDisplay()

Demos/FractureDemo/btFractureDynamicsWorld.cpp

@@ -56,6 +56,14 @@ void btFractureDynamicsWorld::glueCallback()
 		if (!manifold->getNumContacts())
 			continue;
 
+		btScalar minDist = 1e30f;
+		for (int v=0;v<manifold->getNumContacts();v++)
+		{
+			minDist = btMin(minDist,manifold->getContactPoint(v).getDistance());
+		}
+		if (minDist>0.)
+			continue;
+		
 		btCollisionObject* colObj0 = (btCollisionObject*)manifold->getBody0();
 		btCollisionObject* colObj1 = (btCollisionObject*)manifold->getBody1();
 		int tag0 = (colObj0)->getIslandTag();

Demos/InternalEdgeDemo/InternalEdgeDemo.cpp

@@ -25,9 +25,10 @@ bool enable=true;
 #endif
 
 
+
 #include "btBulletDynamicsCommon.h"
 #include "BulletCollision/CollisionDispatch/btInternalEdgeUtility.h"
-
+#include "Taru.mdl"
 
 #include "LinearMath/btIDebugDraw.h"
 #include "GLDebugDrawer.h"
@@ -347,17 +348,18 @@ void	InternalEdgeDemo::initPhysics()
 	m_dispatcher = new	btCollisionDispatcher(m_collisionConfiguration);
 
 
-	btVector3 worldMin(-1000,-1000,-1000);
+	
-	btVector3 worldMax(1000,1000,1000);
+	m_broadphase = new btDbvtBroadphase();
-	m_broadphase = new btAxisSweep3(worldMin,worldMax);
 	m_solver = new btSequentialImpulseConstraintSolver();
 	m_dynamicsWorld = new btDiscreteDynamicsWorld(m_dispatcher,m_broadphase,m_solver,m_collisionConfiguration);
-	m_dynamicsWorld->getSolverInfo().m_splitImpulse = true;
+/*
+m_dynamicsWorld->getSolverInfo().m_splitImpulse = true;
 	m_dynamicsWorld->getSolverInfo().m_splitImpulsePenetrationThreshold = 1e30f;
 	m_dynamicsWorld->getSolverInfo().m_maxErrorReduction = 1e30f;
 	m_dynamicsWorld->getSolverInfo().m_erp  =1.f;
 	m_dynamicsWorld->getSolverInfo().m_erp2 = 1.f;
-	
+*/
+
 	m_dynamicsWorld->setGravity(btVector3(0,-10,0));
 
 	
@@ -366,24 +368,40 @@ void	InternalEdgeDemo::initPhysics()
 	startTransform.setIdentity();
 	startTransform.setOrigin(btVector3(0,-2,0));
 
-	btBoxShape* colShape = new btBoxShape(btVector3(1,1,1));
+
+	btConvexHullShape* colShape = new btConvexHullShape();
+	for (int i=0;i<TaruVtxCount;i++)
+	{
+		btVector3 vtx(TaruVtx[i*3],TaruVtx[i*3+1],TaruVtx[i*3+2]);
+		colShape->addPoint(vtx);
+	}
+	//this will enable polyhedral contact clipping, better quality, slightly slower
 	colShape->initializePolyhedralFeatures();
 
-	//colShape->setMargin(0.f);
+	//the polyhedral contact clipping can use either GJK or SAT test to find the separating axis
-	colShape->setMargin(0.1f);
+	m_dynamicsWorld->getDispatchInfo().m_enableSatConvex=false;
-
 
 	m_collisionShapes.push_back(colShape);
 
 	{
 		for (int i=0;i<1;i++)
 		{
-			startTransform.setOrigin(btVector3(-10.f+i*3.f,1.f+btScalar(i)*0.1f,-1.3f));
+			startTransform.setOrigin(btVector3(-10.f+i*3.f,2.2f+btScalar(i)*0.1f,-1.3f));
-			btRigidBody* body = localCreateRigidBody(100, startTransform,colShape);
+			btRigidBody* body = localCreateRigidBody(10, startTransform,colShape);
 			body->setActivationState(DISABLE_DEACTIVATION);
 			body->setLinearVelocity(btVector3(0,0,-1));
+			//body->setContactProcessingThreshold(0.f);
 		}
 	}
+	{
+		btBoxShape* colShape = new btBoxShape(btVector3(1,1,1));
+		colShape->initializePolyhedralFeatures();
+		m_collisionShapes.push_back(colShape);
+		startTransform.setOrigin(btVector3(-16.f+i*3.f,1.f+btScalar(i)*0.1f,-1.3f));
+		btRigidBody* body = localCreateRigidBody(10, startTransform,colShape);
+		body->setActivationState(DISABLE_DEACTIVATION);
+		body->setLinearVelocity(btVector3(0,0,-1));
+	}
 
 	startTransform.setIdentity();
 #ifdef ROTATE_GROUND
@@ -471,8 +489,9 @@ void InternalEdgeDemo::clientMoveAndDisplay()
 	
 #endif
 
-		//clear all contact points involving mesh proxy. Note: this is a slow/unoptimized operation.
+		
-		m_dynamicsWorld->getBroadphase()->getOverlappingPairCache()->cleanProxyFromPairs(staticBody->getBroadphaseHandle(),getDynamicsWorld()->getDispatcher());
+		//for debugging: clear all contact points involving mesh proxy. Note: this is a slow/unoptimized operation.
+		//m_dynamicsWorld->getBroadphase()->getOverlappingPairCache()->cleanProxyFromPairs(staticBody->getBroadphaseHandle(),getDynamicsWorld()->getDispatcher());
 	}
 
 

Demos/InternalEdgeDemo/Taru.mdl

@@ -0,0 +1,49 @@
+#define TaruVtxCount 43
+#define TaruIdxCount 132
+
+static float TaruVtx[] = {
+1.08664f,-1.99237f,0.0f,
+0.768369f,-1.99237f,-0.768369f,
+1.28852f,1.34412e-007f,-1.28852f,
+1.82224f,1.90735e-007f,0.0f,
+0.0f,-1.99237f,-1.08664f,
+0.0f,0.0f,-1.82224f,
+0.0f,-1.99237f,-1.08664f,
+-0.768369f,-1.99237f,-0.768369f,
+-1.28852f,1.34412e-007f,-1.28852f,
+0.0f,0.0f,-1.82224f,
+-1.08664f,-1.99237f,1.82086e-007f,
+-1.82224f,1.90735e-007f,1.59305e-007f,
+-0.768369f,-1.99237f,0.76837f,
+-1.28852f,2.47058e-007f,1.28852f,
+1.42495e-007f,-1.99237f,1.08664f,
+2.38958e-007f,2.70388e-007f,1.82224f,
+0.768369f,-1.99237f,0.768369f,
+1.28852f,2.47058e-007f,1.28852f,
+0.768369f,1.99237f,-0.768369f,
+1.08664f,1.99237f,0.0f,
+0.0f,1.99237f,-1.08664f,
+-0.768369f,1.99237f,-0.768369f,
+0.0f,1.99237f,-1.08664f,
+-1.08664f,1.99237f,0.0f,
+-0.768369f,1.99237f,0.768369f,
+1.42495e-007f,1.99237f,1.08664f,
+0.768369f,1.99237f,0.768369f,
+1.42495e-007f,-1.99237f,1.08664f,
+-0.768369f,-1.99237f,0.76837f,
+-1.08664f,-1.99237f,1.82086e-007f,
+-0.768369f,-1.99237f,-0.768369f,
+0.0f,-1.99237f,-1.08664f,
+0.768369f,-1.99237f,-0.768369f,
+1.08664f,-1.99237f,0.0f,
+0.768369f,-1.99237f,0.768369f,
+0.768369f,1.99237f,-0.768369f,
+0.0f,1.99237f,-1.08664f,
+-0.768369f,1.99237f,-0.768369f,
+-1.08664f,1.99237f,0.0f,
+-0.768369f,1.99237f,0.768369f,
+1.42495e-007f,1.99237f,1.08664f,
+0.768369f,1.99237f,0.768369f,
+1.08664f,1.99237f,0.0f,
+};
+

src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp

@@ -376,7 +376,7 @@ void btConvexConvexAlgorithm ::processCollision (btCollisionObject* body0,btColl
 	input.m_transformA = body0->getWorldTransform();
 	input.m_transformB = body1->getWorldTransform();
 
-	gjkPairDetector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw);
+
 
 	
 
@@ -396,10 +396,29 @@ void btConvexConvexAlgorithm ::processCollision (btCollisionObject* body0,btColl
 
 	if (min0->isPolyhedral() && min1->isPolyhedral())
 	{
+
+
+		struct btDummyResult : public btDiscreteCollisionDetectorInterface::Result
+		{
+			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) 
+			{
+			}
+		};
+		
+		btDummyResult dummy;
+
+
 		btPolyhedralConvexShape* polyhedronA = (btPolyhedralConvexShape*) min0;
 		btPolyhedralConvexShape* polyhedronB = (btPolyhedralConvexShape*) min1;
 		if (polyhedronA->getConvexPolyhedron() && polyhedronB->getConvexPolyhedron())
 		{
+
+
+			gjkPairDetector.getClosestPoints(input,dummy,dispatchInfo.m_debugDraw);
+			
+
 			btScalar threshold = m_manifoldPtr->getContactBreakingThreshold();
 
 			btScalar minDist = 0.f;
@@ -438,7 +457,8 @@ void btConvexConvexAlgorithm ::processCollision (btCollisionObject* body0,btColl
 			//we can also deal with convex versus triangle (without connectivity data)
 			if (polyhedronA->getConvexPolyhedron() && polyhedronB->getShapeType()==TRIANGLE_SHAPE_PROXYTYPE)
 			{
-
+				gjkPairDetector.getClosestPoints(input,dummy,dispatchInfo.m_debugDraw);
+		
 				btVector3 sepNormalWorldSpace = gjkPairDetector.getCachedSeparatingAxis().normalized();
 
 				btVertexArray vertices;
@@ -453,16 +473,20 @@ void btConvexConvexAlgorithm ::processCollision (btCollisionObject* body0,btColl
 					body0->getWorldTransform(), vertices, minDist-threshold, threshold, *resultOut);
 				
 				
+				if (m_ownManifold)
+				{
+					resultOut->refreshContactPoints();
+				}
+				
+				return;
 			}
-			if (m_ownManifold)
+			
-			{
-				resultOut->refreshContactPoints();
-			}
 		}
 
-		return;
 
 	}
+	
+	gjkPairDetector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw);
 
 	//now perform 'm_numPerturbationIterations' collision queries with the perturbated collision objects
 	

src/BulletMultiThreaded/btParallelConstraintSolver.cpp

@@ -20,6 +20,7 @@ subject to the following restrictions:
 #include "BulletCollision/BroadphaseCollision/btDispatcher.h"
 #include "LinearMath/btPoolAllocator.h"
 
+#include "BulletMultiThreaded/vectormath2bullet.h"
 
 #include "LinearMath/btQuickprof.h"
 #include "BulletMultiThreaded/btThreadSupportInterface.h"
@@ -53,8 +54,8 @@ unsigned char ATTRIBUTE_ALIGNED128(tmp_buff[TMP_BUFF_BYTES]);
 {
 
 	btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm;
-	const btScalar deltaVel1Dotn	=	c.m_contactNormal.dot((btVector3&)body1.mDeltaLinearVelocity) 	+ c.m_relpos1CrossNormal.dot((btVector3&)body1.mDeltaAngularVelocity);
+	const btScalar deltaVel1Dotn	=	c.m_contactNormal.dot(getBtVector3(body1.mDeltaLinearVelocity)) 	+ c.m_relpos1CrossNormal.dot(getBtVector3(body1.mDeltaAngularVelocity));
-	const btScalar deltaVel2Dotn	=	-c.m_contactNormal.dot((btVector3&)body2.mDeltaLinearVelocity) + c.m_relpos2CrossNormal.dot((btVector3&)body2.mDeltaAngularVelocity);
+	const btScalar deltaVel2Dotn	=	-c.m_contactNormal.dot(getBtVector3(body2.mDeltaLinearVelocity)) + c.m_relpos2CrossNormal.dot(getBtVector3(body2.mDeltaAngularVelocity));
 
 //	const btScalar delta_rel_vel	=	deltaVel1Dotn-deltaVel2Dotn;
 	deltaImpulse	-=	deltaVel1Dotn*c.m_jacDiagABInv;
@@ -80,15 +81,17 @@ unsigned char ATTRIBUTE_ALIGNED128(tmp_buff[TMP_BUFF_BYTES]);
 	if (body1.mMassInv)
 	{
 		btVector3 linearComponent = c.m_contactNormal*body1.mMassInv;
-		((btVector3&)body1.mDeltaLinearVelocity) += linearComponent*deltaImpulse;
+		body1.mDeltaLinearVelocity += vmVector3(linearComponent.getX()*deltaImpulse,linearComponent.getY()*deltaImpulse,linearComponent.getZ()*deltaImpulse);
-		((btVector3&)body1.mDeltaAngularVelocity) += c.m_angularComponentA*(btVector3(deltaImpulse,deltaImpulse,deltaImpulse));//*m_angularFactor);
+		btVector3 tmp=c.m_angularComponentA*(btVector3(deltaImpulse,deltaImpulse,deltaImpulse));
+		body1.mDeltaAngularVelocity += vmVector3(tmp.getX(),tmp.getY(),tmp.getZ());
 	}
 
 	if (body2.mMassInv)
 	{
 		btVector3 linearComponent = -c.m_contactNormal*body2.mMassInv;
-		((btVector3&)body2.mDeltaLinearVelocity) += linearComponent*deltaImpulse;
+		body2.mDeltaLinearVelocity += vmVector3(linearComponent.getX()*deltaImpulse,linearComponent.getY()*deltaImpulse,linearComponent.getZ()*deltaImpulse);
-		((btVector3&)body2.mDeltaAngularVelocity) += c.m_angularComponentB*((btVector3(deltaImpulse,deltaImpulse,deltaImpulse)));//*m_angularFactor);
+		btVector3 tmp = c.m_angularComponentB*((btVector3(deltaImpulse,deltaImpulse,deltaImpulse)));//*m_angularFactor);
+		body2.mDeltaAngularVelocity += vmVector3(tmp.getX(),tmp.getY(),tmp.getZ());
 	}
 
 	//body1.internalApplyImpulse(c.m_contactNormal*body1.internalGetInvMass(),c.m_angularComponentA,deltaImpulse);
@@ -1035,7 +1038,7 @@ btScalar btParallelConstraintSolver::solveGroup(btCollisionObject** bodies1,int
 		state.reset();
 		const btQuaternion& orgOri = obj->getWorldTransform().getRotation();
 		vmQuat orn(orgOri.getX(),orgOri.getY(),orgOri.getZ(),orgOri.getW());
-		state.setPosition((vmVector3&) obj->getWorldTransform().getOrigin());
+		state.setPosition(getVmVector3(obj->getWorldTransform().getOrigin()));
 		state.setOrientation(orn);
 		state.setPosition(state.getPosition());
 		state.setRigidBodyId(i);
@@ -1140,7 +1143,7 @@ btScalar btParallelConstraintSolver::solveGroup(btCollisionObject** bodies1,int
 				int contactId = m-offsetContactManifolds;
 				//likely the contact pool is not contiguous, make sure to allocate large enough contact pool
 				btAssert(contactId>=0);
-				btAssert(contactId<dispatcher->getInternalManifoldPool()->getUsedCount());
+				btAssert(contactId<dispatcher->getInternalManifoldPool()->getMaxCount());
 				
 				pfxSetContactId(pair,contactId);
 				pfxSetNumConstraints(pair,numPosPoints);//manifoldPtr[i]->getNumContacts());

src/LinearMath/btPoolAllocator.h

@@ -62,6 +62,11 @@ public:
 		return m_maxElements - m_freeCount;
 	}
 
+	int getMaxCount() const
+	{
+		return m_maxElements;
+	}
+
 	void*	allocate(int size)
 	{
 		// release mode fix