use size_t instead of int, for allocator

added hashtable based PairManager, thanks Pierre Terdiman and Erin Catto
improved friction in 'cachefriendly' solver
moved 'refreshcontactpoints' into collision detection, instead of solver
avoid linear search for contact manifolds, by storing an index
ignore margin for sphere shape (its entire radius is already margin)
avoid alignment checks in BVH serialization, they don't compile on 64-bit architectures
made 'bomb' box more heavy

Demos/OpenGL/DemoApplication.cpp

@@ -445,7 +445,7 @@ void	DemoApplication::shootBox(const btVector3& destination)
 
 	if (m_dynamicsWorld)
 	{
-		float mass = 1.f;
+		float mass = 100.f;
 		btTransform startTransform;
 		startTransform.setIdentity();
 		btVector3 camPos = getCameraPosition();

Demos/StressTest/StressTestDemo.cpp

@@ -0,0 +1,377 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "btBulletDynamicsCommon.h"
+#include "StressTestDemo.h"
+
+#define SHOW_NUM_DEEP_PENETRATIONS
+
+#include "LinearMath/btDefaultMotionState.h"
+#include "LinearMath/btIDebugDraw.h"
+#include "LinearMath/btQuickprof.h"
+#include "LinearMath/btDefaultMotionState.h"
+#include "BulletCollision/BroadphaseCollision/btMultiSapBroadphase.h"
+
+#include "BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.h"
+
+//#define REGISTER_BOX_BOX 1
+#ifdef REGISTER_BOX_BOX
+#include "../Extras/AlternativeCollisionAlgorithms/BoxBoxCollisionAlgorithm.h"
+#endif //REGISTER_BOX_BOX
+
+
+//#define SHOW_MEMORY_DETAILS 1
+
+//#ifdef SHOW_MEMORY_DETAILS
+//	#include "LinearMath/btGenericPoolAllocator.h"
+//#endif
+
+/// Including GIMPACT here
+
+
+
+#include "BMF_Api.h"
+
+#include "GLDebugDrawer.h"
+
+#include "GL_ShapeDrawer.h"
+#include "GlutStuff.h"
+
+
+#ifdef SHOW_NUM_DEEP_PENETRATIONS
+extern int gNumDeepPenetrationChecks;
+extern int gNumGjkChecks;
+#endif //
+
+
+
+GLDebugDrawer	debugDrawer;
+//Real			dts = 0.000001f;
+btScalar			dts = 1.0 / 60.0;
+
+
+///**************************************************************************************
+///	GIMPACT Test Demo made by DevO
+///
+///**************************************************************************************
+
+
+
+//------------------------------------------------------------------------------
+///User can override this material combiner by implementing gContactAddedCallback and setting body0->m_collisionFlags |= btCollisionObject::customMaterialCallback;
+inline btScalar	calculateCombinedFriction(float friction0,float friction1)
+{
+	btScalar friction = friction0 * friction1;
+
+	const btScalar MAX_FRICTION  = 10.f;
+	if (friction < -MAX_FRICTION)
+		friction = -MAX_FRICTION;
+	if (friction > MAX_FRICTION)
+		friction = MAX_FRICTION;
+	return friction;
+
+}
+
+//------------------------------------------------------------------------------
+inline btScalar	calculateCombinedRestitution(float restitution0,float restitution1)
+{
+	return restitution0 * restitution1;
+}
+
+
+//------------------------------------------------------------------------------
+bool CustomMaterialCombinerCallback(btManifoldPoint& cp,	const btCollisionObject* colObj0,int partId0,int index0,const btCollisionObject* colObj1,int partId1,int index1)
+{
+
+	float friction0 = colObj0->getFriction();
+	float friction1 = colObj1->getFriction();
+	float restitution0 = colObj0->getRestitution();
+	float restitution1 = colObj1->getRestitution();
+
+	if (colObj0->getCollisionFlags() & btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK)
+	{
+		friction0 = 1.0;//partId0,index0
+		restitution0 = 0.f;
+	}
+	if (colObj1->getCollisionFlags() & btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK)
+	{
+		if (index1&1)
+		{
+			friction1 = 1.0f;//partId1,index1
+		} else
+		{
+			friction1 = 0.f;
+		}
+		restitution1 = 0.f;
+	}
+
+	cp.m_combinedFriction = calculateCombinedFriction(friction0,friction1);
+	cp.m_combinedRestitution = calculateCombinedRestitution(restitution0,restitution1);
+
+	//this return value is currently ignored, but to be on the safe side: return false if you don't calculate friction
+	return true;
+}
+
+extern ContactAddedCallback		gContactAddedCallback;
+
+
+
+//################################## main #####################################
+int main(int argc,char** argv)
+{
+	//gContactAddedCallback = CustomMaterialCombinerCallback;
+
+	int sizeofbp = sizeof(btBroadphaseProxy);
+
+	StressTestDemo* concaveDemo = new StressTestDemo();  /// This will not be Deleted!!!
+	concaveDemo->initPhysics();
+	concaveDemo->setCameraDistance(45.f);
+
+//cannot run stepFront yet, the OpenGL context is not opened (stepFront updates camera...)
+//	concaveDemo->stepFront();
+//	concaveDemo->stepFront();
+//	concaveDemo->stepFront();
+//	concaveDemo->stepFront();
+
+	return glutmain(argc, argv,640,480,"DevO,s GIMPACT Test Demo",concaveDemo);
+}
+
+//--------------------------
+//------------------------------------------------------------------------------
+void	StressTestDemo::initPhysics()
+{
+	/// Init Bullet
+	m_collisionConfiguration = new btDefaultCollisionConfiguration();
+
+	m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration);
+
+#ifdef REGISTER_BOX_BOX
+	m_dispatcher->registerCollisionCreateFunc(BOX_SHAPE_PROXYTYPE,BOX_SHAPE_PROXYTYPE,new BoxBoxCollisionAlgorithm::CreateFunc);
+#endif //REGISTER_BOX_BOX
+	
+
+	LONG maxProxies = 4096;
+	btVector3 worldAabbMin(-1000,-1000,-1000);
+	btVector3 worldAabbMax( 1000, 1000, 1000);
+	m_broadphase = new btAxisSweep3(worldAabbMin,worldAabbMax,maxProxies);
+	//m_broadphase = new btMultiSapBroadphase();
+	//m_broadphase = new btSimpleBroadphase();
+
+	btSequentialImpulseConstraintSolver* solver = new btSequentialImpulseConstraintSolver();;
+	m_constraintSolver = solver;
+
+	btDiscreteDynamicsWorld* world = new btDiscreteDynamicsWorld(m_dispatcher,m_broadphase,m_constraintSolver);
+	world->getSolverInfo().m_numIterations = 4;
+//	solver->setSolverMode(btSequentialImpulseConstraintSolver::SOLVER_CACHE_FRIENDLY);//btSequentialImpulseConstraintSolver::SOLVER_RANDMIZE_ORDER);
+//	solver->setSolverMode(btSequentialImpulseConstraintSolver::SOLVER_RANDMIZE_ORDER);
+
+	m_dynamicsWorld = world;
+	m_dynamicsWorld->setDebugDrawer(&debugDrawer);
+
+	
+
+	// register algorithm
+	if(0)
+	{
+		m_spheresphere_collisionCreateFunc = new btSphereSphereCollisionAlgorithm::CreateFunc;  /// NEW
+
+		m_dispatcher->registerCollisionCreateFunc(SPHERE_SHAPE_PROXYTYPE,SPHERE_SHAPE_PROXYTYPE,m_spheresphere_collisionCreateFunc);
+
+		m_spherebox_collisionCreateFunc = new btSphereBoxCollisionAlgorithm::CreateFunc;
+
+		m_dispatcher->registerCollisionCreateFunc(SPHERE_SHAPE_PROXYTYPE,BOX_SHAPE_PROXYTYPE,m_spherebox_collisionCreateFunc);
+
+		m_dispatcher->registerCollisionCreateFunc(SPHERE_SHAPE_PROXYTYPE,BOX_SHAPE_PROXYTYPE,m_spherebox_collisionCreateFunc);
+
+	}
+
+
+
+	//create trimesh model and shape
+
+
+
+
+	/// Create Scene
+	float mass = 0.f;
+	btTransform	startTransform;
+	startTransform.setIdentity();
+
+	btCollisionShape* staticboxShape1 = new btBoxShape(btVector3(20,1,20));//floor
+	btCollisionShape* staticboxShape2 = new btBoxShape(btVector3(1,50,200));//left wall
+	btCollisionShape* staticboxShape3 = new btBoxShape(btVector3(1,50,200));//right wall
+	btCollisionShape* staticboxShape4 = new btBoxShape(btVector3(200,50,1));//front wall
+	btCollisionShape* staticboxShape5 = new btBoxShape(btVector3(200,50,1));//back wall
+#ifdef USE_COMPOUND
+
+	btCompoundShape* staticScenario = new btCompoundShape();//static scenario
+
+	startTransform.setOrigin(btVector3(0,-10,0));
+	staticScenario->addChildShape(startTransform,staticboxShape1);
+	startTransform.setOrigin(btVector3(-200,15,0));
+	staticScenario->addChildShape(startTransform,staticboxShape2);
+	startTransform.setOrigin(btVector3(200,15,0));
+	staticScenario->addChildShape(startTransform,staticboxShape3);
+	startTransform.setOrigin(btVector3(0,15,200));
+	staticScenario->addChildShape(startTransform,staticboxShape4);
+	startTransform.setOrigin(btVector3(0,15,-200));
+	staticScenario->addChildShape(startTransform,staticboxShape5);
+
+	startTransform.setOrigin(btVector3(0,0,0));
+
+	btRigidBody* staticBody = localCreateRigidBody(mass, startTransform,staticScenario);
+#else
+	startTransform.setOrigin(btVector3(0,-7,0));//;//-10,0));
+	btRigidBody* staticBody = localCreateRigidBody(mass, startTransform,staticboxShape1);
+	/*startTransform.setOrigin(btVector3(-200,15,0));
+	staticBody = localCreateRigidBody(mass, startTransform,staticboxShape2);
+	startTransform.setOrigin(btVector3(200,15,0));
+	staticBody = localCreateRigidBody(mass, startTransform,staticboxShape3);
+	startTransform.setOrigin(btVector3(0,15,200));
+	staticBody = localCreateRigidBody(mass, startTransform,staticboxShape4);
+	startTransform.setOrigin(btVector3(0,15,-200));
+	staticBody = localCreateRigidBody(mass, startTransform,staticboxShape5);
+	*/
+
+#endif //USE_COMPOUND
+
+	//enable custom material callback
+//	staticBody->setCollisionFlags(staticBody->getCollisionFlags()|btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK);
+
+	startTransform.setIdentity();
+	btCollisionShape* sphShape = new btSphereShape(1);
+	btScalar radii[1] = {btScalar(1)};
+	btVector3 poss[1]={btVector3(0,0,0)};
+
+//	btCollisionShape* sphShape = new btMultiSphereShape(btVector3(1,1,1),&poss[0],&radii[0],1);
+
+	btScalar margin(0.04);
+	btCollisionShape* boxShape = new btBoxShape(btVector3(1,1,1));//1.-margin,1.-margin,1.-margin));
+	boxShape->setMargin(margin);
+
+
+	/// Create Dynamic Boxes
+	{
+
+		float start_x = START_POS_X - ARRAY_SIZE_X/2;
+		float start_y = START_POS_Y;
+		float start_z = START_POS_Z - ARRAY_SIZE_Z/2;
+
+		for (int k=0;k<ARRAY_SIZE_Y;k++)
+		{
+			for (int i=0;i<ARRAY_SIZE_X;i++)
+			{
+				for(int j = 0;j<ARRAY_SIZE_Z;j++)
+				{
+					startTransform.setOrigin(btVector3(
+										2.0*i + start_x,
+										2.0*k + start_y,
+										2.0*j + start_z));
+
+					#ifdef SPHERES
+					
+					localCreateRigidBody(1, startTransform,sphShape);
+					#else					
+					localCreateRigidBody(1, startTransform,boxShape);
+					#endif
+
+
+
+				}
+			}
+		}
+	}
+
+	//m_debugMode |= btIDebugDraw::DBG_DrawWireframe;
+
+
+
+
+}
+
+
+//------------------------------------------------------------------------------
+void StressTestDemo::clientMoveAndDisplay()
+{
+	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
+
+	int start, end;
+
+    start = glutGet(GLUT_ELAPSED_TIME);
+
+	float dt = float(m_clock.getTimeMicroseconds()) * 0.000001f;
+
+	m_clock.reset();
+	m_dynamicsWorld->stepSimulation(dt,1);
+	m_steps_done++;
+
+	end = glutGet(GLUT_ELAPSED_TIME);
+	printf("Time %d \n", end-start);
+
+	renderme();
+
+	glFlush();
+	glutSwapBuffers();
+
+}
+
+//------------------------------------------------------------------------------
+void StressTestDemo::displayCallback(void) {
+
+	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
+
+	renderme();
+
+	glFlush();
+	glutSwapBuffers();
+}
+
+//------------------------------------------------------------------------------
+void StressTestDemo::clientResetScene()
+{
+	m_steps_done = 0;
+	DemoApplication::clientResetScene();
+}
+
+#define KEY_ESCAPE     0x1B
+
+
+//------------------------------------------------------------------------------
+void StressTestDemo::keyboardCallback(unsigned char key, int x, int y)
+{
+	switch (key)
+	{
+	case '.':
+		{
+			break;
+		}
+
+	case '2':
+		{
+			dts += 0.000001f;
+			break;
+		}
+	case '3':
+		{
+			dts -= 0.000001f; if(dts<0.000001f) dts = 0.000001f;
+			break;
+		}
+
+	default:
+		DemoApplication::keyboardCallback(key, x, y);
+	}
+}
+
+

Demos/StressTest/StressTestDemo.h

@@ -0,0 +1,99 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+#ifndef TEST_CONCAVE_DEMO_H
+#define TEST_CONCAVE_DEMO_H
+
+#include "DemoApplication.h"
+
+
+
+
+struct btCollisionAlgorithmCreateFunc;
+
+///StressTestDemo shows usage of static concave triangle meshes
+///It also shows per-triangle material (friction/restitution) through CustomMaterialCombinerCallback
+class StressTestDemo : public DemoApplication
+{
+
+public:
+	StressTestDemo()
+		: m_collisionConfiguration(NULL),
+		  m_dispatcher(NULL),
+		  m_broadphase(NULL),
+		  m_constraintSolver(NULL),
+		  m_steps_done(0),
+		  m_spheresphere_collisionCreateFunc(0),
+		  m_spherebox_collisionCreateFunc(0)
+	{
+	}
+
+	virtual ~StressTestDemo()
+	{
+		delete m_spheresphere_collisionCreateFunc;
+		delete m_spherebox_collisionCreateFunc;
+
+		delete m_collisionConfiguration;
+		delete m_dispatcher;
+		delete m_broadphase;
+		delete m_constraintSolver;
+
+		delete m_dynamicsWorld;
+	}
+
+	void	initGImpactCollision();
+	void	initPhysics();
+
+	virtual void clientMoveAndDisplay();
+
+	virtual void displayCallback();
+
+	virtual void clientResetScene();
+
+	virtual void keyboardCallback(unsigned char key, int x, int y);
+
+
+
+public: ///data
+	unsigned int			m_steps_done;
+
+
+	btVector3				kinTorusTran;
+	btQuaternion			kinTorusRot;
+	btRigidBody				*kinematicTorus;
+
+
+	btCollisionAlgorithmCreateFunc*  m_spheresphere_collisionCreateFunc;
+	btCollisionAlgorithmCreateFunc*  m_spherebox_collisionCreateFunc;
+
+	btDefaultCollisionConfiguration* m_collisionConfiguration;
+	btCollisionDispatcher*			 m_dispatcher;
+	btBroadphaseInterface*			 m_broadphase;
+	btConstraintSolver*				 m_constraintSolver;
+};
+
+
+#define SPHERES 1
+
+#define START_POS_X -5
+#define START_POS_Y -5
+#define START_POS_Z -3
+
+
+#define ARRAY_SIZE_X 10
+#define ARRAY_SIZE_Y 10
+#define ARRAY_SIZE_Z 10
+
+#endif //CONCAVE_DEMO_H
+

Extras/AlternativeCollisionAlgorithms/BoxBoxCollisionAlgorithm.cpp

@@ -19,6 +19,8 @@ subject to the following restrictions:
 #include "BulletCollision/CollisionDispatch/btCollisionObject.h"
 #include "BoxBoxDetector.h"
 
+#define USE_PERSISTENT_CONTACTS 1
+
 BoxBoxCollisionAlgorithm::BoxBoxCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* obj0,btCollisionObject* obj1)
 : btCollisionAlgorithm(ci),
 m_ownManifold(false),
@@ -54,8 +56,9 @@ void BoxBoxCollisionAlgorithm::processCollision (btCollisionObject* body0,btColl
 
 	/// report a contact. internally this will be kept persistent, and contact reduction is done
 	resultOut->setPersistentManifold(m_manifoldPtr);
-	
+#ifndef USE_PERSISTENT_CONTACTS	
 	m_manifoldPtr->clearManifold();
+#endif //USE_PERSISTENT_CONTACTS
 
 	btDiscreteCollisionDetectorInterface::ClosestPointInput input;
 	input.m_maximumDistanceSquared = 1e30f;
@@ -65,6 +68,14 @@ void BoxBoxCollisionAlgorithm::processCollision (btCollisionObject* body0,btColl
 	BoxBoxDetector detector(box0,box1);
 	detector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw);
 
+#ifdef USE_PERSISTENT_CONTACTS
+	//  refreshContactPoints is only necessary when using persistent contact points. otherwise all points are newly added
+	if (m_ownManifold)
+	{
+		resultOut->refreshContactPoints();
+	}
+#endif //USE_PERSISTENT_CONTACTS
+
 }
 
 btScalar BoxBoxCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)

Extras/BulletMultiThreaded/SpuParallelSolver.cpp

@@ -109,7 +109,8 @@ btScalar btParallelSequentialImpulseSolver::solveGroup(btCollisionObject** bodie
 {
 	if (!numManifolds && !numConstraints)
 		return 0;
-
+///refresh contact points is not needed anymore, it has been moved into the processCollision detection part.
+#ifdef FORCE_REFESH_CONTACT_MANIFOLDS
 	for (int i = 0; i < numManifolds; ++i)
 	{
 		btPersistentManifold* currManifold = manifold[i];
@@ -118,6 +119,7 @@ btScalar btParallelSequentialImpulseSolver::solveGroup(btCollisionObject** bodie
 
 		currManifold->refreshContactPoints(rb0->getCenterOfMassTransform(),rb1->getCenterOfMassTransform());
 	}
+#endif //FORCE_REFESH_CONTACT_MANIFOLDS
 
 	// Record and mark the manifolds to the cells
 	for (int i = 0; i < numManifolds; ++i)

src/BulletCollision/BroadphaseCollision/btAxisSweep3.h

@@ -58,7 +58,7 @@ public:
 	
 		// indexes into the edge arrays
 		BP_FP_INT_TYPE m_minEdges[3], m_maxEdges[3];		// 6 * 2 = 12
-		BP_FP_INT_TYPE m_handleId;
+//		BP_FP_INT_TYPE m_uniqueId;
 		BP_FP_INT_TYPE m_pad;
 		
 		//void* m_pOwner; this is now in btBroadphaseProxy.m_clientObject
@@ -102,10 +102,10 @@ protected:
 
 	void quantize(BP_FP_INT_TYPE* out, const btPoint3& point, int isMax) const;
 
-	void sortMinDown(int axis, BP_FP_INT_TYPE edge, bool updateOverlaps = true);
-	void sortMinUp(int axis, BP_FP_INT_TYPE edge, bool updateOverlaps = true);
-	void sortMaxDown(int axis, BP_FP_INT_TYPE edge, bool updateOverlaps = true);
-	void sortMaxUp(int axis, BP_FP_INT_TYPE edge, bool updateOverlaps = true);
+	void sortMinDown(int axis, BP_FP_INT_TYPE edge, btDispatcher* dispatcher, bool updateOverlaps );
+	void sortMinUp(int axis, BP_FP_INT_TYPE edge, btDispatcher* dispatcher, bool updateOverlaps );
+	void sortMaxDown(int axis, BP_FP_INT_TYPE edge, btDispatcher* dispatcher, bool updateOverlaps );
+	void sortMaxUp(int axis, BP_FP_INT_TYPE edge, btDispatcher* dispatcher, bool updateOverlaps );
 
 public:
 
@@ -116,17 +116,17 @@ public:
 
 	virtual void	calculateOverlappingPairs(btDispatcher* dispatcher);
 	
-	BP_FP_INT_TYPE addHandle(const btPoint3& aabbMin,const btPoint3& aabbMax, void* pOwner,short int collisionFilterGroup,short int collisionFilterMask);
+	BP_FP_INT_TYPE addHandle(const btPoint3& aabbMin,const btPoint3& aabbMax, void* pOwner,short int collisionFilterGroup,short int collisionFilterMask,btDispatcher* dispatcher);
 	void removeHandle(BP_FP_INT_TYPE handle,btDispatcher* dispatcher);
-	void updateHandle(BP_FP_INT_TYPE handle, const btPoint3& aabbMin,const btPoint3& aabbMax);
+	void updateHandle(BP_FP_INT_TYPE handle, const btPoint3& aabbMin,const btPoint3& aabbMax,btDispatcher* dispatcher);
 	inline Handle* getHandle(BP_FP_INT_TYPE index) const {return m_pHandles + index;}
 
 	void	processAllOverlappingPairs(btOverlapCallback* callback);
 
 	//Broadphase Interface
-	virtual btBroadphaseProxy*	createProxy(  const btVector3& aabbMin,  const btVector3& aabbMax,int shapeType,void* userPtr ,short int collisionFilterGroup,short int collisionFilterMask);
+	virtual btBroadphaseProxy*	createProxy(  const btVector3& aabbMin,  const btVector3& aabbMax,int shapeType,void* userPtr ,short int collisionFilterGroup,short int collisionFilterMask,btDispatcher* dispatcher);
 	virtual void	destroyProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher);
-	virtual void	setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax);
+	virtual void	setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax,btDispatcher* dispatcher);
 	
 	bool	testAabbOverlap(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1);
 
@@ -173,10 +173,10 @@ void btAxisSweep3<BP_FP_INT_TYPE>::debugPrintAxis(int axis, bool checkCardinalit
 #endif //DEBUG_BROADPHASE
 
 template <typename BP_FP_INT_TYPE>
-btBroadphaseProxy*	btAxisSweep3Internal<BP_FP_INT_TYPE>::createProxy(  const btVector3& aabbMin,  const btVector3& aabbMax,int shapeType,void* userPtr,short int collisionFilterGroup,short int collisionFilterMask)
+btBroadphaseProxy*	btAxisSweep3Internal<BP_FP_INT_TYPE>::createProxy(  const btVector3& aabbMin,  const btVector3& aabbMax,int shapeType,void* userPtr,short int collisionFilterGroup,short int collisionFilterMask,btDispatcher* dispatcher)
 {
 		(void)shapeType;
-		BP_FP_INT_TYPE handleId = addHandle(aabbMin,aabbMax, userPtr,collisionFilterGroup,collisionFilterMask);
+		BP_FP_INT_TYPE handleId = addHandle(aabbMin,aabbMax, userPtr,collisionFilterGroup,collisionFilterMask,dispatcher);
 		
 		Handle* handle = getHandle(handleId);
 				
@@ -189,14 +189,14 @@ template <typename BP_FP_INT_TYPE>
 void	btAxisSweep3Internal<BP_FP_INT_TYPE>::destroyProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher)
 {
 	Handle* handle = static_cast<Handle*>(proxy);
-	removeHandle(handle->m_handleId,dispatcher);
+	removeHandle(handle->m_uniqueId,dispatcher);
 }
 
 template <typename BP_FP_INT_TYPE>
-void	btAxisSweep3Internal<BP_FP_INT_TYPE>::setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax)
+void	btAxisSweep3Internal<BP_FP_INT_TYPE>::setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax,btDispatcher* dispatcher)
 {
 	Handle* handle = static_cast<Handle*>(proxy);
-	updateHandle(handle->m_handleId,aabbMin,aabbMax);
+	updateHandle(handle->m_uniqueId,aabbMin,aabbMax,dispatcher);
 
 }
 
@@ -335,7 +335,7 @@ void btAxisSweep3Internal<BP_FP_INT_TYPE>::freeHandle(BP_FP_INT_TYPE handle)
 
 
 template <typename BP_FP_INT_TYPE>
-BP_FP_INT_TYPE btAxisSweep3Internal<BP_FP_INT_TYPE>::addHandle(const btPoint3& aabbMin,const btPoint3& aabbMax, void* pOwner,short int collisionFilterGroup,short int collisionFilterMask)
+BP_FP_INT_TYPE btAxisSweep3Internal<BP_FP_INT_TYPE>::addHandle(const btPoint3& aabbMin,const btPoint3& aabbMax, void* pOwner,short int collisionFilterGroup,short int collisionFilterMask,btDispatcher* dispatcher)
 {
 	// quantize the bounds
 	BP_FP_INT_TYPE min[3], max[3];
@@ -348,7 +348,7 @@ BP_FP_INT_TYPE btAxisSweep3Internal<BP_FP_INT_TYPE>::addHandle(const btPoint3& a
 
 	Handle* pHandle = getHandle(handle);
 	
-	pHandle->m_handleId = handle;
+	pHandle->m_uniqueId = handle;
 	//pHandle->m_pOverlaps = 0;
 	pHandle->m_clientObject = pOwner;
 	pHandle->m_collisionFilterGroup = collisionFilterGroup;
@@ -377,12 +377,12 @@ BP_FP_INT_TYPE btAxisSweep3Internal<BP_FP_INT_TYPE>::addHandle(const btPoint3& a
 	}
 
 	// now sort the new edges to their correct position
-	sortMinDown(0, pHandle->m_minEdges[0], false);
-	sortMaxDown(0, pHandle->m_maxEdges[0], false);
-	sortMinDown(1, pHandle->m_minEdges[1], false);
-	sortMaxDown(1, pHandle->m_maxEdges[1], false);
-	sortMinDown(2, pHandle->m_minEdges[2], true);
-	sortMaxDown(2, pHandle->m_maxEdges[2], true);
+	sortMinDown(0, pHandle->m_minEdges[0], dispatcher,false);
+	sortMaxDown(0, pHandle->m_maxEdges[0], dispatcher,false);
+	sortMinDown(1, pHandle->m_minEdges[1], dispatcher,false);
+	sortMaxDown(1, pHandle->m_maxEdges[1], dispatcher,false);
+	sortMinDown(2, pHandle->m_minEdges[2], dispatcher,true);
+	sortMaxDown(2, pHandle->m_maxEdges[2], dispatcher,true);
 
 
 	return handle;
@@ -418,14 +418,14 @@ void btAxisSweep3Internal<BP_FP_INT_TYPE>::removeHandle(BP_FP_INT_TYPE handle,bt
 		BP_FP_INT_TYPE max = pHandle->m_maxEdges[axis];
 		pEdges[max].m_pos = m_handleSentinel;
 
-		sortMaxUp(axis,max,false);
+		sortMaxUp(axis,max,dispatcher,false);
 
 
 		BP_FP_INT_TYPE i = pHandle->m_minEdges[axis];
 		pEdges[i].m_pos = m_handleSentinel;
 
 
-		sortMinUp(axis,i,false);
+		sortMinUp(axis,i,dispatcher,false);
 
 		pEdges[limit-1].m_handle = 0;
 		pEdges[limit-1].m_pos = m_handleSentinel;
@@ -445,13 +445,16 @@ void btAxisSweep3Internal<BP_FP_INT_TYPE>::removeHandle(BP_FP_INT_TYPE handle,bt
 }
 
 extern int gOverlappingPairs;
+#include <stdio.h>
 
 template <typename BP_FP_INT_TYPE>
 void	btAxisSweep3Internal<BP_FP_INT_TYPE>::calculateOverlappingPairs(btDispatcher* dispatcher)
 {
+#ifdef USE_LAZY_REMOVAL
 
 	if (m_ownsPairCache)
 	{
+	
 		btBroadphasePairArray&	overlappingPairArray = m_pairCache->getOverlappingPairArray();
 
 		//perform a sort, to find duplicates and to sort 'invalid' pairs to the end
@@ -524,7 +527,12 @@ void	btAxisSweep3Internal<BP_FP_INT_TYPE>::calculateOverlappingPairs(btDispatche
 		m_invalidPair = 0;
 	#endif//CLEAN_INVALID_PAIRS
 		
+		//printf("overlappingPairArray.size()=%d\n",overlappingPairArray.size());
 	}
+#endif //USE_LAZY_REMOVAL
+
+
+	
 
 }
 
@@ -581,7 +589,7 @@ bool btAxisSweep3Internal<BP_FP_INT_TYPE>::testOverlap(int ignoreAxis,const Hand
 }
 
 template <typename BP_FP_INT_TYPE>
-void btAxisSweep3Internal<BP_FP_INT_TYPE>::updateHandle(BP_FP_INT_TYPE handle, const btPoint3& aabbMin,const btPoint3& aabbMax)
+void btAxisSweep3Internal<BP_FP_INT_TYPE>::updateHandle(BP_FP_INT_TYPE handle, const btPoint3& aabbMin,const btPoint3& aabbMax,btDispatcher* dispatcher)
 {
 //	assert(bounds.IsFinite());
 	//assert(bounds.HasVolume());
@@ -607,17 +615,17 @@ void btAxisSweep3Internal<BP_FP_INT_TYPE>::updateHandle(BP_FP_INT_TYPE handle, c
 
 		// expand (only adds overlaps)
 		if (dmin < 0)
-			sortMinDown(axis, emin);
+			sortMinDown(axis, emin,dispatcher,true);
 
 		if (dmax > 0)
-			sortMaxUp(axis, emax);
+			sortMaxUp(axis, emax,dispatcher,true);
 
 		// shrink (only removes overlaps)
 		if (dmin > 0)
-			sortMinUp(axis, emin);
+			sortMinUp(axis, emin,dispatcher,true);
 
 		if (dmax < 0)
-			sortMaxDown(axis, emax);
+			sortMaxDown(axis, emax,dispatcher,true);
 
 #ifdef DEBUG_BROADPHASE
 	debugPrintAxis(axis);
@@ -632,7 +640,7 @@ void btAxisSweep3Internal<BP_FP_INT_TYPE>::updateHandle(BP_FP_INT_TYPE handle, c
 
 // sorting a min edge downwards can only ever *add* overlaps
 template <typename BP_FP_INT_TYPE>
-void btAxisSweep3Internal<BP_FP_INT_TYPE>::sortMinDown(int axis, BP_FP_INT_TYPE edge, bool updateOverlaps)
+void btAxisSweep3Internal<BP_FP_INT_TYPE>::sortMinDown(int axis, BP_FP_INT_TYPE edge, btDispatcher* dispatcher, bool updateOverlaps)
 {
 
 	Edge* pEdge = m_pEdges[axis] + edge;
@@ -680,7 +688,7 @@ void btAxisSweep3Internal<BP_FP_INT_TYPE>::sortMinDown(int axis, BP_FP_INT_TYPE
 
 // sorting a min edge upwards can only ever *remove* overlaps
 template <typename BP_FP_INT_TYPE>
-void btAxisSweep3Internal<BP_FP_INT_TYPE>::sortMinUp(int axis, BP_FP_INT_TYPE edge, bool updateOverlaps)
+void btAxisSweep3Internal<BP_FP_INT_TYPE>::sortMinUp(int axis, BP_FP_INT_TYPE edge, btDispatcher* dispatcher, bool updateOverlaps)
 {
 	Edge* pEdge = m_pEdges[axis] + edge;
 	Edge* pNext = pEdge + 1;
@@ -692,17 +700,16 @@ void btAxisSweep3Internal<BP_FP_INT_TYPE>::sortMinUp(int axis, BP_FP_INT_TYPE ed
 
 		if (pNext->IsMax())
 		{
+#ifndef USE_LAZY_REMOVAL
 			// if next edge is maximum remove any overlap between the two handles
 			if (updateOverlaps)
 			{
-				/*
 				Handle* handle0 = getHandle(pEdge->m_handle);
 				Handle* handle1 = getHandle(pNext->m_handle);
-				btBroadphasePair tmpPair(*handle0,*handle1);
-				removeOverlappingPair(tmpPair);
-				*/
 
+				m_pairCache->removeOverlappingPair(handle0,handle1,dispatcher);	
 			}
+#endif //USE_LAZY_REMOVAL
 
 			// update edge reference in other handle
 			pHandleNext->m_maxEdges[axis]--;
@@ -727,7 +734,7 @@ void btAxisSweep3Internal<BP_FP_INT_TYPE>::sortMinUp(int axis, BP_FP_INT_TYPE ed
 
 // sorting a max edge downwards can only ever *remove* overlaps
 template <typename BP_FP_INT_TYPE>
-void btAxisSweep3Internal<BP_FP_INT_TYPE>::sortMaxDown(int axis, BP_FP_INT_TYPE edge, bool updateOverlaps)
+void btAxisSweep3Internal<BP_FP_INT_TYPE>::sortMaxDown(int axis, BP_FP_INT_TYPE edge, btDispatcher* dispatcher, bool updateOverlaps)
 {
 
 	Edge* pEdge = m_pEdges[axis] + edge;
@@ -744,17 +751,12 @@ void btAxisSweep3Internal<BP_FP_INT_TYPE>::sortMaxDown(int axis, BP_FP_INT_TYPE
 			if (updateOverlaps)
 			{
 				//this is done during the overlappingpairarray iteration/narrowphase collision
-				/*
+#ifndef USE_LAZY_REMOVAL
 				Handle* handle0 = getHandle(pEdge->m_handle);
 				Handle* handle1 = getHandle(pPrev->m_handle);
-				btBroadphasePair* pair = findPair(handle0,handle1);
-				//assert(pair);
+				m_pairCache->removeOverlappingPair(handle0,handle1,dispatcher);
 			
-				if (pair)
-				{
-					removeOverlappingPair(*pair);
-				}
-				*/
+#endif //USE_LAZY_REMOVAL		
 
 			}
 
@@ -785,7 +787,7 @@ void btAxisSweep3Internal<BP_FP_INT_TYPE>::sortMaxDown(int axis, BP_FP_INT_TYPE
 
 // sorting a max edge upwards can only ever *add* overlaps
 template <typename BP_FP_INT_TYPE>
-void btAxisSweep3Internal<BP_FP_INT_TYPE>::sortMaxUp(int axis, BP_FP_INT_TYPE edge, bool updateOverlaps)
+void btAxisSweep3Internal<BP_FP_INT_TYPE>::sortMaxUp(int axis, BP_FP_INT_TYPE edge, btDispatcher* dispatcher, bool updateOverlaps)
 {
 	Edge* pEdge = m_pEdges[axis] + edge;
 	Edge* pNext = pEdge + 1;

src/BulletCollision/BroadphaseCollision/btBroadphaseInterface.h

@@ -31,9 +31,9 @@ class btBroadphaseInterface
 public:
 	virtual ~btBroadphaseInterface() {}
 
-	virtual btBroadphaseProxy*	createProxy(  const btVector3& aabbMin,  const btVector3& aabbMax,int shapeType,void* userPtr, short int collisionFilterGroup,short int collisionFilterMask) =0;
+	virtual btBroadphaseProxy*	createProxy(  const btVector3& aabbMin,  const btVector3& aabbMax,int shapeType,void* userPtr, short int collisionFilterGroup,short int collisionFilterMask, btDispatcher* dispatcher) =0;
 	virtual void	destroyProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher)=0;
-	virtual void	setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax)=0;
+	virtual void	setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax, btDispatcher* dispatcher)=0;
 	
 	///calculateOverlappingPairs is optional: incremental algorithms (sweep and prune) might do it during the set aabb
 	virtual void	calculateOverlappingPairs(btDispatcher* dispatcher)=0;

src/BulletCollision/BroadphaseCollision/btBroadphaseProxy.h

@@ -65,6 +65,7 @@ CONCAVE_SHAPES_END_HERE,
 
 ///btBroadphaseProxy
 ATTRIBUTE_ALIGNED16(struct) btBroadphaseProxy
+//struct btBroadphaseProxy
 {
 
 BT_DECLARE_ALIGNED_ALLOCATOR();
@@ -96,6 +97,12 @@ BT_DECLARE_ALIGNED_ALLOCATOR();
 
 	};
 
+	int			m_uniqueId;//m_uniqueId is introduced for paircache. could get rid of this, by calculating the address offset etc.
+	inline int getUid()
+	{
+		return m_uniqueId;//(int)this;
+	}
+
 	//used for memory pools
 	btBroadphaseProxy() :m_clientObject(0){}
 
@@ -106,6 +113,8 @@ BT_DECLARE_ALIGNED_ALLOCATOR();
 	{
 	}
 
+	
+
 	static inline bool isPolyhedral(int proxyType)
 	{
 		return (proxyType  < IMPLICIT_CONVEX_SHAPES_START_HERE);

src/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.cpp

@@ -56,14 +56,14 @@ btMultiSapBroadphase::~btMultiSapBroadphase()
 {
 }
 
-btBroadphaseProxy*	btMultiSapBroadphase::createProxy(  const btVector3& aabbMin,  const btVector3& aabbMax,int shapeType,void* userPtr, short int collisionFilterGroup,short int collisionFilterMask)
+btBroadphaseProxy*	btMultiSapBroadphase::createProxy(  const btVector3& aabbMin,  const btVector3& aabbMax,int shapeType,void* userPtr, short int collisionFilterGroup,short int collisionFilterMask, btDispatcher* dispatcher)
 {
 	btMultiSapProxy* proxy = new btMultiSapProxy(aabbMin,  aabbMax,shapeType,userPtr, collisionFilterGroup,collisionFilterMask);
 	m_multiSapProxies.push_back(proxy);
 
 	///we don't pass the userPtr but our multisap proxy. We need to patch this, before processing an actual collision
 	///this is needed to be able to calculate the aabb overlap
-	btBroadphaseProxy* simpleProxy = m_simpleBroadphase->createProxy(aabbMin,aabbMax,shapeType,userPtr,collisionFilterGroup,collisionFilterMask);
+	btBroadphaseProxy* simpleProxy = m_simpleBroadphase->createProxy(aabbMin,aabbMax,shapeType,userPtr,collisionFilterGroup,collisionFilterMask, dispatcher);
 	simpleProxy->m_multiSapParentProxy = proxy;
 
 	btChildProxy* childProxyRef = new btChildProxy();
@@ -72,7 +72,7 @@ btBroadphaseProxy*	btMultiSapBroadphase::createProxy(  const btVector3& aabbMin,
 	proxy->m_childProxies.push_back(childProxyRef);
 
 	///this should deal with inserting/removal into child broadphases
-	setAabb(proxy,aabbMin,aabbMax);
+	setAabb(proxy,aabbMin,aabbMax,dispatcher);
 	return proxy;
 }
 
@@ -82,7 +82,7 @@ void	btMultiSapBroadphase::destroyProxy(btBroadphaseProxy* proxy,btDispatcher* d
 	btAssert(0);
 
 }
-void	btMultiSapBroadphase::setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax)
+void	btMultiSapBroadphase::setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax, btDispatcher* dispatcher)
 {
 	btMultiSapProxy* multiProxy = static_cast<btMultiSapProxy*>(proxy);
 	multiProxy->m_aabbMin = aabbMin;
@@ -91,7 +91,7 @@ void	btMultiSapBroadphase::setAabb(btBroadphaseProxy* proxy,const btVector3& aab
 	for (int i=0;i<multiProxy->m_childProxies.size();i++)
 	{
 		btChildProxy* childProxyRef = multiProxy->m_childProxies[i];
-		childProxyRef->m_childBroadphase->setAabb(childProxyRef->m_proxy,aabbMin,aabbMax);
+		childProxyRef->m_childBroadphase->setAabb(childProxyRef->m_proxy,aabbMin,aabbMax,dispatcher);
 	}
 
 }

src/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.h

@@ -95,9 +95,9 @@ public:
 
 	virtual ~btMultiSapBroadphase();
 
-	virtual btBroadphaseProxy*	createProxy(  const btVector3& aabbMin,  const btVector3& aabbMax,int shapeType,void* userPtr, short int collisionFilterGroup,short int collisionFilterMask);
+	virtual btBroadphaseProxy*	createProxy(  const btVector3& aabbMin,  const btVector3& aabbMax,int shapeType,void* userPtr, short int collisionFilterGroup,short int collisionFilterMask, btDispatcher* dispatcher);
 	virtual void	destroyProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher);
-	virtual void	setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax);
+	virtual void	setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax, btDispatcher* dispatcher);
 	
 	///calculateOverlappingPairs is optional: incremental algorithms (sweep and prune) might do it during the set aabb
 	virtual void	calculateOverlappingPairs(btDispatcher* dispatcher);

src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp

@@ -28,6 +28,19 @@ m_blockedForChanges(false),
 m_overlapFilterCallback(0)
 //m_NumOverlapBroadphasePair(0)
 {
+
+#ifdef USE_HASH_PAIRCACHE
+	m_overlappingPairArray.reserve(b2_maxPairs);
+
+	for (int32 i = 0; i < b2_tableCapacity; ++i)
+	{
+		m_hashTable[i] = b2_nullPair;
+	}
+	for (int32 i = 0; i < b2_maxPairs; ++i)
+	{
+		m_next[i] = b2_nullPair;
+	}
+#endif //USE_HASH_PAIRCACHE
 }
 
 
@@ -36,23 +49,6 @@ btOverlappingPairCache::~btOverlappingPairCache()
 	//todo/test: show we erase/delete data, or is it automatic
 }
 
-
-void	btOverlappingPairCache::removeOverlappingPair(btBroadphasePair& findPair, btDispatcher* dispatcher )
-{
-	
-	int findIndex = m_overlappingPairArray.findLinearSearch(findPair);
-	if (findIndex < m_overlappingPairArray.size())
-	{
-		gOverlappingPairs--;
-		btBroadphasePair& pair = m_overlappingPairArray[findIndex];
-		cleanOverlappingPair(pair,dispatcher);
-		
-		m_overlappingPairArray.swap(findIndex,m_overlappingPairArray.size()-1);
-		m_overlappingPairArray.pop_back();
-	}
-}
-
-
 void	btOverlappingPairCache::cleanOverlappingPair(btBroadphasePair& pair,btDispatcher* dispatcher)
 {
 	if (pair.m_algorithm)
@@ -66,50 +62,6 @@ void	btOverlappingPairCache::cleanOverlappingPair(btBroadphasePair& pair,btDispa
 }
 
 
-
-
-
-void	btOverlappingPairCache::addOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1)
-{
-	//don't add overlap with own
-	assert(proxy0 != proxy1);
-
-	if (!needsBroadphaseCollision(proxy0,proxy1))
-		return;
-
-
-	btBroadphasePair pair(*proxy0,*proxy1);
-	
-	m_overlappingPairArray.push_back(pair);
-	gOverlappingPairs++;
-	
-}
-
-///this findPair becomes really slow. Either sort the list to speedup the query, or
-///use a different solution. It is mainly used for Removing overlapping pairs. Removal could be delayed.
-///we could keep a linked list in each proxy, and store pair in one of the proxies (with lowest memory address)
-///Also we can use a 2D bitmap, which can be useful for a future GPU implementation
- btBroadphasePair*	btOverlappingPairCache::findPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1)
-{
-	if (!needsBroadphaseCollision(proxy0,proxy1))
-		return 0;
-
-	btBroadphasePair tmpPair(*proxy0,*proxy1);
-	int findIndex = m_overlappingPairArray.findLinearSearch(tmpPair);
-
-	if (findIndex < m_overlappingPairArray.size())
-	{
-		//assert(it != m_overlappingPairSet.end());
-		 btBroadphasePair* pair = &m_overlappingPairArray[findIndex];
-		return pair;
-	}
-	return 0;
-}
-
-
-
-
-
 void	btOverlappingPairCache::cleanProxyFromPairs(btBroadphaseProxy* proxy,btDispatcher* dispatcher)
 {
 
@@ -144,8 +96,6 @@ void	btOverlappingPairCache::cleanProxyFromPairs(btBroadphaseProxy* proxy,btDisp
 
 }
 
-
-
 void	btOverlappingPairCache::removeOverlappingPairsContainingProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher)
 {
 
@@ -173,6 +123,317 @@ void	btOverlappingPairCache::removeOverlappingPairsContainingProxy(btBroadphaseP
 }
 
 
+#ifdef USE_HASH_PAIRCACHE
+
+// Thomas Wang's hash, see: http://www.concentric.net/~Ttwang/tech/inthash.htm
+// This assumes proxyId1 and proxyId2 are 16-bit.
+inline uint32 Hash(uint32 proxyId1, uint32 proxyId2)
+{
+	uint32 key = (proxyId2 << 16) | proxyId1;
+	key = ~key + (key << 15);
+	key = key ^ (key >> 12);
+	key = key + (key << 2);
+	key = key ^ (key >> 4);
+	key = key * 2057;
+	key = key ^ (key >> 16);
+	return key;
+}
+
+inline bool Equals(const btBroadphasePair& pair, int32 proxyId1, int32 proxyId2)
+{	
+	return pair.m_pProxy0->getUid() == proxyId1 && pair.m_pProxy1->getUid() == proxyId2;
+}
+
+inline btBroadphasePair* btOverlappingPairCache::Find(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1, uint32 hash)
+{
+	int32 proxyId1 = proxy0->getUid();
+	int32 proxyId2 = proxy1->getUid();
+	if (proxyId1 > proxyId2) 
+		btSwap(proxyId1, proxyId2);
+
+	int32 index = m_hashTable[hash];
+	
+	while( index != b2_nullPair && Equals(m_overlappingPairArray[index], proxyId1, proxyId2) == false)
+	{
+		index = m_next[index];
+	}
+
+	if ( index == b2_nullPair )
+	{
+		return NULL;
+	}
+
+	btAssert(index < m_overlappingPairArray.size());
+
+	return &m_overlappingPairArray[index];
+}
+
+btBroadphasePair* btOverlappingPairCache::Find(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1)
+{
+	int32 proxyId1 = proxy0->getUid();
+	int32 proxyId2 = proxy1->getUid();
+
+	if (proxyId1 > proxyId2) 
+		btSwap(proxyId1, proxyId2);
+
+	int32 hash = Hash(proxyId1, proxyId2) & b2_tableMask;
+
+	int32 index = m_hashTable[hash];
+	while (index != b2_nullPair && Equals(m_overlappingPairArray[index], proxyId1, proxyId2) == false)
+	{
+		index = m_next[index];
+	}
+
+	if (index == b2_nullPair)
+	{
+		return NULL;
+	}
+
+	btAssert(index < m_overlappingPairArray.size());
+
+	return &m_overlappingPairArray[index];
+}
+
+#include <stdio.h>
+
+btBroadphasePair* btOverlappingPairCache::Add(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1)
+{
+	int32 proxyId1 = proxy0->getUid();
+	int32 proxyId2 = proxy1->getUid();
+
+	if (proxyId1 > proxyId2) 
+		btSwap(proxyId1, proxyId2);
+
+	int32 hash = Hash(proxyId1, proxyId2) & b2_tableMask;
+
+	
+
+	btBroadphasePair* pair = Find(proxy0, proxy1, hash);
+	if (pair != NULL)
+	{
+		return pair;
+	}
+
+	if (m_overlappingPairArray.size() == b2_maxPairs)
+	{
+		btAssert(false);
+		return NULL;
+	}
+	
+	int count = m_overlappingPairArray.size();
+	void* mem = &m_overlappingPairArray.expand();
+	
+	pair = new (mem) btBroadphasePair(*proxy0,*proxy1);
+//	pair->m_pProxy0 = proxy0;
+//	pair->m_pProxy1 = proxy1;
+	pair->m_algorithm = 0;
+	pair->m_userInfo = 0;
+	
+
+	m_next[count] = m_hashTable[hash];
+	m_hashTable[hash] = (uint16)count;
+
+	return pair;
+}
+
+
+void* btOverlappingPairCache::Remove(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1,btDispatcher* dispatcher)
+{
+	int32 proxyId1 = proxy0->getUid();
+	int32 proxyId2 = proxy1->getUid();
+
+	if (proxyId1 > proxyId2) 
+		btSwap(proxyId1, proxyId2);
+
+	int32 hash = Hash(proxyId1, proxyId2) & b2_tableMask;
+
+	btBroadphasePair* pair = Find(proxy0, proxy1, hash);
+	if (pair == NULL)
+	{
+		return NULL;
+	}
+
+	cleanOverlappingPair(*pair,dispatcher);
+
+	void* userData = pair->m_userInfo;
+
+	btAssert(pair->m_pProxy0->getUid() == proxyId1);
+	btAssert(pair->m_pProxy1->getUid() == proxyId2);
+
+	int32 pairIndex = int32(pair - &m_overlappingPairArray[0]);
+	btAssert(pairIndex < m_overlappingPairArray.size());
+
+	// Remove the pair from the hash table.
+	int32 index = m_hashTable[hash];
+	btAssert(index != b2_nullPair);
+
+	int32 previous = b2_nullPair;
+	while (index != pairIndex)
+	{
+		previous = index;
+		index = m_next[index];
+	}
+
+	if (previous != b2_nullPair)
+	{
+		btAssert(m_next[previous] == pairIndex);
+		m_next[previous] = m_next[pairIndex];
+	}
+	else
+	{
+		m_hashTable[hash] = m_next[pairIndex];
+	}
+
+	// We now move the last pair into spot of the
+	// pair being removed. We need to fix the hash
+	// table indices to support the move.
+
+	int32 lastPairIndex = m_overlappingPairArray.size() - 1;
+
+	// If the removed pair is the last pair, we are done.
+	if (lastPairIndex == pairIndex)
+	{
+		m_overlappingPairArray.pop_back();
+		return userData;
+	}
+
+	// Remove the last pair from the hash table.
+	const btBroadphasePair* last = &m_overlappingPairArray[lastPairIndex];
+	int32 lastHash = Hash(last->m_pProxy0->getUid(), last->m_pProxy1->getUid()) & b2_tableMask;
+
+	index = m_hashTable[lastHash];
+	btAssert(index != b2_nullPair);
+
+	previous = b2_nullPair;
+	while (index != lastPairIndex)
+	{
+		previous = index;
+		index = m_next[index];
+	}
+
+	if (previous != b2_nullPair)
+	{
+		btAssert(m_next[previous] == lastPairIndex);
+		m_next[previous] = m_next[lastPairIndex];
+	}
+	else
+	{
+		m_hashTable[lastHash] = m_next[lastPairIndex];
+	}
+
+	// Copy the last pair into the remove pair's spot.
+	m_overlappingPairArray[pairIndex] = m_overlappingPairArray[lastPairIndex];
+
+	// Insert the last pair into the hash table
+	m_next[pairIndex] = m_hashTable[lastHash];
+	m_hashTable[lastHash] = (uint16)pairIndex;
+
+	m_overlappingPairArray.pop_back();
+
+	return userData;
+}
+#include <stdio.h>
+
+void	btOverlappingPairCache::processAllOverlappingPairs(btOverlapCallback* callback,btDispatcher* dispatcher)
+{
+
+	int i;
+
+	//printf("m_overlappingPairArray.size()=%d\n",m_overlappingPairArray.size());
+	for (i=0;i<m_overlappingPairArray.size();)
+	{
+	
+		btBroadphasePair* pair = &m_overlappingPairArray[i];
+		if (callback->processOverlap(*pair))
+		{
+			removeOverlappingPair(pair->m_pProxy0,pair->m_pProxy1,dispatcher);
+
+			gOverlappingPairs--;
+		} else
+		{
+			i++;
+		}
+	}
+}
+
+#else
+
+
+
+
+void	btOverlappingPairCache::removeOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1, btDispatcher* dispatcher )
+{
+#ifndef USE_LAZY_REMOVAL
+
+	btBroadphasePair findPair(*proxy0,*proxy1);
+
+	int findIndex = m_overlappingPairArray.findLinearSearch(findPair);
+	if (findIndex < m_overlappingPairArray.size())
+	{
+		gOverlappingPairs--;
+		btBroadphasePair& pair = m_overlappingPairArray[findIndex];
+		cleanOverlappingPair(pair,dispatcher);
+		
+		m_overlappingPairArray.swap(findIndex,m_overlappingPairArray.size()-1);
+		m_overlappingPairArray.pop_back();
+	}
+#endif //USE_LAZY_REMOVAL
+
+}
+
+
+
+
+
+
+
+
+void	btOverlappingPairCache::addOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1)
+{
+	//don't add overlap with own
+	assert(proxy0 != proxy1);
+
+	if (!needsBroadphaseCollision(proxy0,proxy1))
+		return;
+
+
+	btBroadphasePair pair(*proxy0,*proxy1);
+	m_overlappingPairArray.push_back(pair);
+	gOverlappingPairs++;
+	
+}
+
+///this findPair becomes really slow. Either sort the list to speedup the query, or
+///use a different solution. It is mainly used for Removing overlapping pairs. Removal could be delayed.
+///we could keep a linked list in each proxy, and store pair in one of the proxies (with lowest memory address)
+///Also we can use a 2D bitmap, which can be useful for a future GPU implementation
+ btBroadphasePair*	btOverlappingPairCache::findPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1)
+{
+	if (!needsBroadphaseCollision(proxy0,proxy1))
+		return 0;
+
+	btBroadphasePair tmpPair(*proxy0,*proxy1);
+	int findIndex = m_overlappingPairArray.findLinearSearch(tmpPair);
+
+	if (findIndex < m_overlappingPairArray.size())
+	{
+		//assert(it != m_overlappingPairSet.end());
+		 btBroadphasePair* pair = &m_overlappingPairArray[findIndex];
+		return pair;
+	}
+	return 0;
+}
+
+
+
+
+
+
+
+
+
+
+#include <stdio.h>
 
 void	btOverlappingPairCache::processAllOverlappingPairs(btOverlapCallback* callback,btDispatcher* dispatcher)
 {
@@ -199,4 +460,4 @@ void	btOverlappingPairCache::processAllOverlappingPairs(btOverlapCallback* callb
 
 
 
-
+#endif //USE_HASH_PAIRCACHE

src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h

@@ -24,6 +24,9 @@ subject to the following restrictions:
 #include "LinearMath/btAlignedObjectArray.h"
 class btDispatcher;
 
+#define USE_HASH_PAIRCACHE 1
+
+
 struct	btOverlapCallback
 {
 	virtual ~btOverlapCallback()
@@ -42,6 +45,132 @@ struct btOverlapFilterCallback
 
 typedef btAlignedObjectArray<btBroadphasePair>	btBroadphasePairArray;
 
+#ifdef USE_HASH_PAIRCACHE
+
+
+
+const int b2_maxPairs = 65536;//32768;
+typedef unsigned short int uint16;
+typedef int int32;
+typedef unsigned int uint32;
+
+/// Hash-space based Pair Cache, thanks to Erin Catto, Box2D, http://www.box2d.org, and Pierre Terdiman, Codercorner, http://codercorner.com
+const uint16 b2_nullPair = 0xffff;
+const uint16 b2_nullProxy = 0xffff;
+const int32 b2_tableCapacity = b2_maxPairs;	// must be a power of two
+const int32 b2_tableMask = b2_tableCapacity - 1;
+
+
+class btOverlappingPairCache
+{
+	btBroadphasePairArray	m_overlappingPairArray;
+	btOverlapFilterCallback* m_overlapFilterCallback;
+	bool		m_blockedForChanges;
+
+public:
+	btOverlappingPairCache();
+	~btOverlappingPairCache();
+
+	
+	void	removeOverlappingPairsContainingProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher);
+
+	void	removeOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1,btDispatcher* dispatcher)
+	{
+		Remove(proxy0,proxy1,dispatcher);
+	}
+
+	inline bool needsBroadphaseCollision(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1) const
+	{
+		if (m_overlapFilterCallback)
+			return m_overlapFilterCallback->needBroadphaseCollision(proxy0,proxy1);
+
+		bool collides = (proxy0->m_collisionFilterGroup & proxy1->m_collisionFilterMask) != 0;
+		collides = collides && (proxy1->m_collisionFilterGroup & proxy0->m_collisionFilterMask);
+		
+		return collides;
+	}
+
+	void	addOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1)
+	{
+		if (!needsBroadphaseCollision(proxy0,proxy1))
+			return;
+
+		Add(proxy0,proxy1);
+	}
+
+	btBroadphasePair*	findPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1)
+	{
+		return Find(proxy0,proxy1);
+	}
+
+	void	cleanProxyFromPairs(btBroadphaseProxy* proxy,btDispatcher* dispatcher);
+
+	
+	virtual void	processAllOverlappingPairs(btOverlapCallback*,btDispatcher* dispatcher);
+
+	btBroadphasePair*	getOverlappingPairArrayPtr()
+	{
+		return &m_overlappingPairArray[0];
+	}
+
+	const btBroadphasePair*	getOverlappingPairArrayPtr() const
+	{
+		return &m_overlappingPairArray[0];
+	}
+
+	btBroadphasePairArray&	getOverlappingPairArray()
+	{
+		return m_overlappingPairArray;
+	}
+
+	const btBroadphasePairArray&	getOverlappingPairArray() const
+	{
+		return m_overlappingPairArray;
+	}
+
+	void	cleanOverlappingPair(btBroadphasePair& pair,btDispatcher* dispatcher);
+
+	// Add a pair and return the new pair. If the pair already exists,
+	// no new pair is created and the old one is returned.
+	btBroadphasePair* Add(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1);
+
+	// Remove a pair, return the pair's userData.
+	void* Remove(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1,btDispatcher* dispatcher);
+
+	btBroadphasePair* Find(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1);
+
+	int32 GetCount() const { return m_overlappingPairArray.size(); }
+//	btBroadphasePair* GetPairs() { return m_pairs; }
+
+	btOverlapFilterCallback* getOverlapFilterCallback()
+	{
+		return m_overlapFilterCallback;
+	}
+
+	void setOverlapFilterCallback(btOverlapFilterCallback* callback)
+	{
+		m_overlapFilterCallback = callback;
+	}
+
+	int	getNumOverlappingPairs() const
+	{
+		return m_overlappingPairArray.size();
+	}
+private:
+	btBroadphasePair* Find(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1, uint32 hashValue);
+
+public:
+	
+	uint16 m_hashTable[b2_tableCapacity];
+	uint16 m_next[b2_maxPairs];
+};
+
+
+
+#else//USE_HASH_PAIRCACHE
+
+#define USE_LAZY_REMOVAL 1
+
 ///btOverlappingPairCache maintains the objects with overlapping AABB
 ///Typically managed by the Broadphase, Axis3Sweep or btSimpleBroadphase
 class	btOverlappingPairCache
@@ -63,7 +192,7 @@ class	btOverlappingPairCache
 
 		virtual void	processAllOverlappingPairs(btOverlapCallback*,btDispatcher* dispatcher);
 
-		void	removeOverlappingPair(btBroadphasePair& pair,btDispatcher* dispatcher);
+		void	removeOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1,btDispatcher* dispatcher);
 
 		void	cleanOverlappingPair(btBroadphasePair& pair,btDispatcher* dispatcher);
 		
@@ -98,6 +227,9 @@ class	btOverlappingPairCache
 			return m_overlappingPairArray;
 		}
 
+		
+
+
 		btBroadphasePair*	getOverlappingPairArrayPtr()
 		{
 			return &m_overlappingPairArray[0];
@@ -124,6 +256,8 @@ class	btOverlappingPairCache
 		}
 
 };
+#endif //USE_HASH_PAIRCACHE
+
 #endif //OVERLAPPING_PAIR_CACHE_H
 
 

src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp

@@ -26,22 +26,18 @@ extern int gOverlappingPairs;
 
 void	btSimpleBroadphase::validate()
 {
-	for (int i=0;i<m_numProxies;i++)
+	for (int i=0;i<m_numHandles;i++)
 	{
-		for (int j=i+1;j<m_numProxies;j++)
+		for (int j=i+1;j<m_numHandles;j++)
 		{
-			assert(m_pProxies[i] != m_pProxies[j]);
+			btAssert(&m_pHandles[i] != &m_pHandles[j]);
 		}
 	}
 	
 }
 
 btSimpleBroadphase::btSimpleBroadphase(int maxProxies, btOverlappingPairCache* overlappingPairCache)
-	:
-	m_firstFreeProxy(0),
-	m_numProxies(0),
-	m_maxProxies(maxProxies),
-	m_invalidPair(0),
+	:m_invalidPair(0),
 	m_pairCache(overlappingPairCache),
 	m_ownsPairCache(false)
 {
@@ -52,31 +48,31 @@ btSimpleBroadphase::btSimpleBroadphase(int maxProxies, btOverlappingPairCache* o
 		m_ownsPairCache = true;
 	}
 
-	m_proxies = new btSimpleBroadphaseProxy[maxProxies];
-	m_freeProxies = new int[maxProxies];
-	m_pProxies = new btSimpleBroadphaseProxy*[maxProxies];
+	// allocate handles buffer and put all handles on free list
+	void* ptr = btAlignedAlloc(sizeof(btSimpleBroadphaseProxy)*maxProxies,16);
+	m_pHandles = new(ptr) btSimpleBroadphaseProxy[maxProxies];
+	m_maxHandles = maxProxies;
+	m_numHandles = 0;
+	m_firstFreeHandle = 0;
+	m_firstAllocatedHandle = -1;
 
-
-	int i;
-	for (i=0;i<m_maxProxies;i++)
 	{
-		m_freeProxies[i] = i;
+		for (int i = m_firstFreeHandle; i < maxProxies; i++)
+		{
+			m_pHandles[i].SetNextFree(i + 1);
+			m_pHandles[i].m_uniqueId = i+2;//any UID will do, we just avoid too trivial values (0,1) for debugging purposes
+			m_pHandles[i].SetNextAllocated(-1);
+		}
+		m_pHandles[maxProxies - 1].SetNextFree(0);
+		m_pHandles[maxProxies - 1].SetNextAllocated(-1);
+	
 	}
+
 }
 
 btSimpleBroadphase::~btSimpleBroadphase()
 {
-	delete[] m_proxies;
-	delete []m_freeProxies;
-	delete [] m_pProxies;
-
-	/*int i;
-	for (i=m_numProxies-1;i>=0;i--)
-	{
-		BP_Proxy* proxy = m_pProxies[i]; 
-		destroyProxy(proxy);
-	}
-	*/
+	btAlignedFree(m_pHandles);
 
 	if (m_ownsPairCache)
 	{
@@ -85,27 +81,17 @@ btSimpleBroadphase::~btSimpleBroadphase()
 }
 
 
-btBroadphaseProxy*	btSimpleBroadphase::createProxy(  const btVector3& aabbMin,  const btVector3& aabbMax,int shapeType,void* userPtr ,short int collisionFilterGroup,short int collisionFilterMask)
+btBroadphaseProxy*	btSimpleBroadphase::createProxy(  const btVector3& aabbMin,  const btVector3& aabbMax,int shapeType,void* userPtr ,short int collisionFilterGroup,short int collisionFilterMask, btDispatcher* dispatcher)
 {
-	if (m_numProxies >= m_maxProxies)
+	if (m_numHandles >= m_maxHandles)
 	{
-		assert(0);
+		btAssert(0);
 		return 0; //should never happen, but don't let the game crash ;-)
 	}
 	assert(aabbMin[0]<= aabbMax[0] && aabbMin[1]<= aabbMax[1] && aabbMin[2]<= aabbMax[2]);
 
-	int freeIndex= m_freeProxies[m_firstFreeProxy];
-	btSimpleBroadphaseProxy* proxy = new (&m_proxies[freeIndex])btSimpleBroadphaseProxy(aabbMin,aabbMax,shapeType,userPtr,collisionFilterGroup,collisionFilterMask);
-	m_firstFreeProxy++;
-
-	btSimpleBroadphaseProxy* proxy1 = &m_proxies[0];
-		
-	int	index = int(proxy - proxy1);
-	btAssert(index == freeIndex);
-
-	m_pProxies[m_numProxies] = proxy;
-	m_numProxies++;
-	//validate();
+	int newHandleIndex = allocHandle();
+	btSimpleBroadphaseProxy* proxy = new (&m_pHandles[newHandleIndex])btSimpleBroadphaseProxy(aabbMin,aabbMax,shapeType,userPtr,collisionFilterGroup,collisionFilterMask);
 
 	return proxy;
 }
@@ -145,28 +131,15 @@ void	btSimpleBroadphase::destroyProxy(btBroadphaseProxy* proxyOrg,btDispatcher*
 		int i;
 		
 		btSimpleBroadphaseProxy* proxy0 = static_cast<btSimpleBroadphaseProxy*>(proxyOrg);
-		btSimpleBroadphaseProxy* proxy1 = &m_proxies[0];
-	
-		int index = int(proxy0 - proxy1);
-		btAssert (index < m_maxProxies);
-		m_freeProxies[--m_firstFreeProxy] = index;
+		freeHandle(proxy0);
 
 		m_pairCache->removeOverlappingPairsContainingProxy(proxyOrg,dispatcher);
 
-		for (i=0;i<m_numProxies;i++)
-		{
-			if (m_pProxies[i] == proxyOrg)
-			{
-				m_pProxies[i] = m_pProxies[m_numProxies-1];
-				break;
-			}
-		}
-		m_numProxies--;
 		//validate();
 		
 }
 
-void	btSimpleBroadphase::setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax)
+void	btSimpleBroadphase::setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax, btDispatcher* dispatcher)
 {
 	btSimpleBroadphaseProxy* sbp = getSimpleProxyFromProxy(proxy);
 	sbp->m_min = aabbMin;
@@ -206,99 +179,124 @@ void	btSimpleBroadphase::calculateOverlappingPairs(btDispatcher* dispatcher)
 	//first check for new overlapping pairs
 	int i,j;
 
-	for (i=0;i<m_numProxies;i++)
+	if (m_firstAllocatedHandle >= 0)
 	{
-		btBroadphaseProxy* proxy0 = m_pProxies[i];
-		for (j=i+1;j<m_numProxies;j++)
-		{
-			btBroadphaseProxy* proxy1 = m_pProxies[j];
-			btSimpleBroadphaseProxy* p0 = getSimpleProxyFromProxy(proxy0);
-			btSimpleBroadphaseProxy* p1 = getSimpleProxyFromProxy(proxy1);
 
-			if (aabbOverlap(p0,p1))
+		btSimpleBroadphaseProxy* proxy0 = &m_pHandles[m_firstAllocatedHandle];
+
+		for (i=0;i<m_numHandles;i++)
+		{
+			btSimpleBroadphaseProxy* proxy1 = &m_pHandles[m_firstAllocatedHandle];
+
+			for (j=0;j<m_numHandles;j++)
 			{
-				if ( !m_pairCache->findPair(proxy0,proxy1))
+				
+				if (proxy0 != proxy1)
 				{
-					m_pairCache->addOverlappingPair(proxy0,proxy1);
+					btSimpleBroadphaseProxy* p0 = getSimpleProxyFromProxy(proxy0);
+					btSimpleBroadphaseProxy* p1 = getSimpleProxyFromProxy(proxy1);
+
+					if (aabbOverlap(p0,p1))
+					{
+						if ( !m_pairCache->findPair(proxy0,proxy1))
+						{
+							m_pairCache->addOverlappingPair(proxy0,proxy1);
+						}
+					} else
+					{
+		#ifdef USE_HASH_PAIRCACHE
+						if ( m_pairCache->findPair(proxy0,proxy1))
+						{
+							m_pairCache->removeOverlappingPair(proxy0,proxy1,dispatcher);
+						}
+		#endif //USE_HASH_PAIRCACHE
+
+					}
 				}
+				proxy1 = &m_pHandles[proxy1->GetNextAllocated()];
+
 			}
+			proxy0 = &m_pHandles[proxy0->GetNextAllocated()];
 
 		}
-	}
 
-	if (m_ownsPairCache)
-	{
+	#ifndef USE_HASH_PAIRCACHE
 
-		btBroadphasePairArray&	overlappingPairArray = m_pairCache->getOverlappingPairArray();
-
-		//perform a sort, to find duplicates and to sort 'invalid' pairs to the end
-		overlappingPairArray.heapSort(btBroadphasePairSortPredicate());
-
-		overlappingPairArray.resize(overlappingPairArray.size() - m_invalidPair);
-		m_invalidPair = 0;
-
-
-		btBroadphasePair previousPair;
-		previousPair.m_pProxy0 = 0;
-		previousPair.m_pProxy1 = 0;
-		previousPair.m_algorithm = 0;
-		
-		
-		for (i=0;i<overlappingPairArray.size();i++)
+		if (m_ownsPairCache)
 		{
 			
-			btBroadphasePair& pair = overlappingPairArray[i];
+			btBroadphasePairArray&	overlappingPairArray = m_pairCache->getOverlappingPairArray();
 
-			bool isDuplicate = (pair == previousPair);
+			//perform a sort, to find duplicates and to sort 'invalid' pairs to the end
+			overlappingPairArray.heapSort(btBroadphasePairSortPredicate());
 
-			previousPair = pair;
+			overlappingPairArray.resize(overlappingPairArray.size() - m_invalidPair);
+			m_invalidPair = 0;
 
-			bool needsRemoval = false;
 
-			if (!isDuplicate)
+			btBroadphasePair previousPair;
+			previousPair.m_pProxy0 = 0;
+			previousPair.m_pProxy1 = 0;
+			previousPair.m_algorithm = 0;
+			
+			
+			for (i=0;i<overlappingPairArray.size();i++)
 			{
-				bool hasOverlap = testAabbOverlap(pair.m_pProxy0,pair.m_pProxy1);
 			
-				if (hasOverlap)
+				btBroadphasePair& pair = overlappingPairArray[i];
+
+				bool isDuplicate = (pair == previousPair);
+
+				previousPair = pair;
+
+				bool needsRemoval = false;
+
+				if (!isDuplicate)
 				{
-					needsRemoval = false;//callback->processOverlap(pair);
+					bool hasOverlap = testAabbOverlap(pair.m_pProxy0,pair.m_pProxy1);
+
+					if (hasOverlap)
+					{
+						needsRemoval = false;//callback->processOverlap(pair);
+					} else
+					{
+						needsRemoval = true;
+					}
 				} else
 				{
+					//remove duplicate
 					needsRemoval = true;
+					//should have no algorithm
+					btAssert(!pair.m_algorithm);
 				}
-			} else
-			{
-				//remove duplicate
-				needsRemoval = true;
-				//should have no algorithm
-				btAssert(!pair.m_algorithm);
+				
+				if (needsRemoval)
+				{
+					m_pairCache->cleanOverlappingPair(pair,dispatcher);
+
+			//		m_overlappingPairArray.swap(i,m_overlappingPairArray.size()-1);
+			//		m_overlappingPairArray.pop_back();
+					pair.m_pProxy0 = 0;
+					pair.m_pProxy1 = 0;
+					m_invalidPair++;
+					gOverlappingPairs--;
+				} 
+				
 			}
 
-			if (needsRemoval)
-			{
-				m_pairCache->cleanOverlappingPair(pair,dispatcher);
+		///if you don't like to skip the invalid pairs in the array, execute following code:
+		#define CLEAN_INVALID_PAIRS 1
+		#ifdef CLEAN_INVALID_PAIRS
 
-		//		m_overlappingPairArray.swap(i,m_overlappingPairArray.size()-1);
-		//		m_overlappingPairArray.pop_back();
-				pair.m_pProxy0 = 0;
-				pair.m_pProxy1 = 0;
-				m_invalidPair++;
-				gOverlappingPairs--;
-			} 
+			//perform a sort, to sort 'invalid' pairs to the end
+			overlappingPairArray.heapSort(btBroadphasePairSortPredicate());
+
+			overlappingPairArray.resize(overlappingPairArray.size() - m_invalidPair);
+			m_invalidPair = 0;
+		#endif//CLEAN_INVALID_PAIRS
 
 		}
-
-	///if you don't like to skip the invalid pairs in the array, execute following code:
-	#define CLEAN_INVALID_PAIRS 1
-	#ifdef CLEAN_INVALID_PAIRS
-
-		//perform a sort, to sort 'invalid' pairs to the end
-		overlappingPairArray.heapSort(btBroadphasePairSortPredicate());
-
-		overlappingPairArray.resize(overlappingPairArray.size() - m_invalidPair);
-		m_invalidPair = 0;
-	#endif//CLEAN_INVALID_PAIRS
-	
+	#endif //USE_HASH_PAIRCACHE
 	}
 }
 

src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.h

@@ -24,6 +24,10 @@ struct btSimpleBroadphaseProxy : public btBroadphaseProxy
 {
 	btVector3	m_min;
 	btVector3	m_max;
+	int			m_nextFree;
+	int			m_nextAllocated;
+//	int			m_handleId;
+
 	
 	btSimpleBroadphaseProxy() {};
 
@@ -35,6 +39,13 @@ struct btSimpleBroadphaseProxy : public btBroadphaseProxy
 	}
 	
 	
+	inline void SetNextFree(int next) {m_nextFree = next;}
+	inline int GetNextFree() const {return m_nextFree;}
+
+	inline void SetNextAllocated(int next) {m_nextAllocated = next;}
+	inline int GetNextAllocated() const {return m_nextAllocated;}
+
+
 };
 
 ///SimpleBroadphase is a brute force aabb culling broadphase based on O(n^2) aabb checks
@@ -43,19 +54,46 @@ class btSimpleBroadphase : public btBroadphaseInterface
 
 protected:
 
-	btSimpleBroadphaseProxy*	m_proxies;
-	int*				m_freeProxies;
-	int				m_firstFreeProxy;
+	int		m_numHandles;						// number of active handles
+	int		m_maxHandles;						// max number of handles
+	btSimpleBroadphaseProxy* m_pHandles;						// handles pool
+	int		m_firstFreeHandle;		// free handles list
+	int		m_firstAllocatedHandle;
+
+	int allocHandle()
+	{
+
+		int freeHandle = m_firstFreeHandle;
+		m_firstFreeHandle = m_pHandles[freeHandle].GetNextFree();
+		
+		m_pHandles[freeHandle].SetNextAllocated(m_firstAllocatedHandle);
+		m_firstAllocatedHandle = freeHandle;
+		
+		m_numHandles++;
+
+		return freeHandle;
+	}
+
+	void freeHandle(btSimpleBroadphaseProxy* proxy)
+	{
+		int handle = int(proxy-m_pHandles);
+		btAssert(handle > 0 && handle < m_maxHandles);
+
+		proxy->SetNextFree(m_firstFreeHandle);
+		m_firstFreeHandle = handle;
+
+		m_firstAllocatedHandle = proxy->GetNextAllocated();
+		proxy->SetNextAllocated(-1);
+
+		m_numHandles--;
+	}
 
-	btSimpleBroadphaseProxy** m_pProxies;
-	int				m_numProxies;
 
 	btOverlappingPairCache*	m_pairCache;
 	bool	m_ownsPairCache;
 
 	int	m_invalidPair;
 
-	int m_maxProxies;
 	
 	
 	inline btSimpleBroadphaseProxy*	getSimpleProxyFromProxy(btBroadphaseProxy* proxy)
@@ -80,12 +118,12 @@ public:
 		static bool	aabbOverlap(btSimpleBroadphaseProxy* proxy0,btSimpleBroadphaseProxy* proxy1);
 
 
-	virtual btBroadphaseProxy*	createProxy(  const btVector3& aabbMin,  const btVector3& aabbMax,int shapeType,void* userPtr ,short int collisionFilterGroup,short int collisionFilterMask);
+	virtual btBroadphaseProxy*	createProxy(  const btVector3& aabbMin,  const btVector3& aabbMax,int shapeType,void* userPtr ,short int collisionFilterGroup,short int collisionFilterMask, btDispatcher* dispatcher);
 
 	virtual void	calculateOverlappingPairs(btDispatcher* dispatcher);
 
 	virtual void	destroyProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher);
-	virtual void	setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax);
+	virtual void	setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax, btDispatcher* dispatcher);
 		
 	btOverlappingPairCache*	getOverlappingPairCache()
 	{

src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp

@@ -81,6 +81,7 @@ btPersistentManifold*	btCollisionDispatcher::getNewManifold(void* b0,void* b1)
 	
 	void* mem = m_persistentManifoldPoolAllocator->allocate(sizeof(btPersistentManifold));
 	btPersistentManifold* manifold = new(mem) btPersistentManifold (body0,body1,0);
+	manifold->m_index1a = m_manifoldsPtr.size();
 	m_manifoldsPtr.push_back(manifold);
 
 	return manifold;
@@ -100,16 +101,14 @@ void btCollisionDispatcher::releaseManifold(btPersistentManifold* manifold)
 	//printf("releaseManifold: gNumManifold %d\n",gNumManifold);
 	clearManifold(manifold);
 
-	///todo: this can be improved a lot, linear search might be slow part!
-	int findIndex = m_manifoldsPtr.findLinearSearch(manifold);
-	if (findIndex < m_manifoldsPtr.size())
-	{
-		m_manifoldsPtr.swap(findIndex,m_manifoldsPtr.size()-1);
-		m_manifoldsPtr.pop_back();
+	int findIndex = manifold->m_index1a;
+	btAssert(findIndex < m_manifoldsPtr.size());
+	m_manifoldsPtr.swap(findIndex,m_manifoldsPtr.size()-1);
+	m_manifoldsPtr[findIndex]->m_index1a = findIndex;
+	m_manifoldsPtr.pop_back();
 
-		manifold->~btPersistentManifold();
-		m_persistentManifoldPoolAllocator->free(manifold);
-	}
+	manifold->~btPersistentManifold();
+	m_persistentManifoldPoolAllocator->free(manifold);
 	
 }
 

src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp

@@ -109,7 +109,8 @@ void	btCollisionWorld::addCollisionObject(btCollisionObject* collisionObject,sho
 			type,
 			collisionObject,
 			collisionFilterGroup,
-			collisionFilterMask
+			collisionFilterMask,
+			m_dispatcher1
 			))	;
 
 		
@@ -134,7 +135,7 @@ void	btCollisionWorld::performDiscreteCollisionDetection()
 	for (int i=0;i<m_collisionObjects.size();i++)
 	{
 		m_collisionObjects[i]->getCollisionShape()->getAabb(m_collisionObjects[i]->getWorldTransform(),aabbMin,aabbMax);
-		m_broadphasePairCache->setAabb(m_collisionObjects[i]->getBroadphaseHandle(),aabbMin,aabbMax);
+		m_broadphasePairCache->setAabb(m_collisionObjects[i]->getBroadphaseHandle(),aabbMin,aabbMax,m_dispatcher1);
 	}
 
 	m_broadphasePairCache->calculateOverlappingPairs(m_dispatcher1);

src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp

@@ -189,6 +189,7 @@ void btConvexConcaveCollisionAlgorithm::processCollision (btCollisionObject* bod
 
 			concaveShape->processAllTriangles( &m_btConvexTriangleCallback,m_btConvexTriangleCallback.getAabbMin(),m_btConvexTriangleCallback.getAabbMax());
 			
+			resultOut->refreshContactPoints();
 	
 		}
 	

src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp

@@ -152,6 +152,11 @@ void btConvexConvexAlgorithm ::processCollision (btCollisionObject* body0,btColl
 	m_gjkPairDetector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw);
 #endif
 
+	if (m_ownManifold)
+	{
+		resultOut->refreshContactPoints();
+	}
+
 }
 
 

src/BulletCollision/CollisionDispatch/btManifoldResult.cpp

@@ -79,6 +79,8 @@ void btManifoldResult::addContactPoint(const btVector3& normalOnBInWorld,const b
 	}
 
 	btManifoldPoint newPt(localA,localB,normalOnBInWorld,depth);
+	newPt.m_positionWorldOnA = pointA;
+	newPt.m_positionWorldOnB = pointInWorld;
 	
 	int insertIndex = m_manifoldPtr->getCacheEntry(newPt);
 

src/BulletCollision/CollisionDispatch/btManifoldResult.h

@@ -18,7 +18,7 @@ subject to the following restrictions:
 #define MANIFOLD_RESULT_H
 
 class btCollisionObject;
-class btPersistentManifold;
+#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
 class btManifoldPoint;
 
 #include "BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h"
@@ -70,6 +70,22 @@ public:
 
 	virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth);
 
+	SIMD_FORCE_INLINE	void refreshContactPoints()
+	{
+		btAssert(m_manifoldPtr);
+		if (!m_manifoldPtr->getNumContacts())
+			return;
+
+		bool isSwapped = m_manifoldPtr->getBody0() != m_body0;
+
+		if (isSwapped)
+		{
+			m_manifoldPtr->refreshContactPoints(m_rootTransB,m_rootTransA);
+		} else
+		{
+			m_manifoldPtr->refreshContactPoints(m_rootTransA,m_rootTransB);
+		}
+	}
 
 
 };

src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.cpp

@@ -68,18 +68,25 @@ void btSphereBoxCollisionAlgorithm::processCollision (btCollisionObject* body0,b
 	
 	btScalar dist = getSphereDistance(boxObj,pOnBox,pOnSphere,sphereCenter,radius);
 
+	resultOut->setPersistentManifold(m_manifoldPtr);
+
 	if (dist < SIMD_EPSILON)
 	{
 		btVector3 normalOnSurfaceB = (pOnBox- pOnSphere).normalize();
 
 		/// report a contact. internally this will be kept persistent, and contact reduction is done
 
-		resultOut->setPersistentManifold(m_manifoldPtr);
 		resultOut->addContactPoint(normalOnSurfaceB,pOnBox,dist);
 		
 	}
 
-	
+	if (m_ownManifold)
+	{
+		if (m_manifoldPtr->getNumContacts())
+		{
+			resultOut->refreshContactPoints();
+		}
+	}
 
 }
 

src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp

@@ -46,6 +46,8 @@ void btSphereSphereCollisionAlgorithm::processCollision (btCollisionObject* col0
 	if (!m_manifoldPtr)
 		return;
 
+	resultOut->setPersistentManifold(m_manifoldPtr);
+
 	btSphereShape* sphere0 = (btSphereShape*)col0->getCollisionShape();
 	btSphereShape* sphere1 = (btSphereShape*)col1->getCollisionShape();
 
@@ -54,10 +56,13 @@ void btSphereSphereCollisionAlgorithm::processCollision (btCollisionObject* col0
 	btScalar radius0 = sphere0->getRadius();
 	btScalar radius1 = sphere1->getRadius();
 
+	m_manifoldPtr->clearManifold();
+
 	///iff distance positive, don't generate a new contact
 	if ( len > (radius0+radius1))
+	{
 		return;
-
+	}
 	///distance (negative means penetration)
 	btScalar dist = len - (radius0+radius1);
 
@@ -68,9 +73,12 @@ void btSphereSphereCollisionAlgorithm::processCollision (btCollisionObject* col0
 	btVector3 pos1 = col1->getWorldTransform().getOrigin() + radius1* normalOnSurfaceB;
 
 	/// report a contact. internally this will be kept persistent, and contact reduction is done
-	resultOut->setPersistentManifold(m_manifoldPtr);
+	
+	
 	resultOut->addContactPoint(normalOnSurfaceB,pos1,dist);
 
+	//no resultOut->refreshContactPoints(); needed, because of clearManifold (all points are new)
+
 }
 
 btScalar btSphereSphereCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)

src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.cpp

@@ -48,8 +48,11 @@ void btSphereTriangleCollisionAlgorithm::processCollision (btCollisionObject* co
 	if (!m_manifoldPtr)
 		return;
 
-	btSphereShape* sphere = (btSphereShape*)col0->getCollisionShape();
-	btTriangleShape* triangle = (btTriangleShape*)col1->getCollisionShape();
+	btCollisionObject* sphereObj = m_swapped? col1 : col0;
+	btCollisionObject* triObj = m_swapped? col0 : col1;
+
+	btSphereShape* sphere = (btSphereShape*)sphereObj->getCollisionShape();
+	btTriangleShape* triangle = (btTriangleShape*)triObj->getCollisionShape();
 	
 	/// report a contact. internally this will be kept persistent, and contact reduction is done
 	resultOut->setPersistentManifold(m_manifoldPtr);
@@ -62,6 +65,9 @@ void btSphereTriangleCollisionAlgorithm::processCollision (btCollisionObject* co
 
 	detector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw);
 
+	if (m_ownManifold)
+		resultOut->refreshContactPoints();
+	
 }
 
 btScalar btSphereTriangleCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)

src/BulletCollision/CollisionDispatch/btUnionFind.cpp

@@ -18,6 +18,7 @@ subject to the following restrictions:
 
 
 
+
 btUnionFind::~btUnionFind()
 {
 	Free();

src/BulletCollision/CollisionShapes/btOptimizedBvh.cpp

@@ -906,12 +906,13 @@ bool btOptimizedBvh::serialize(void *o_alignedDataBuffer, unsigned i_dataBufferS
 	assert(m_subtreeHeaderCount == m_SubtreeHeaders.size());
 	m_subtreeHeaderCount = m_SubtreeHeaders.size();
 
-	if (i_dataBufferSize < calculateSerializeBufferSize() || o_alignedDataBuffer == NULL || (((unsigned)o_alignedDataBuffer & BVH_ALIGNMENT_MASK) != 0))
+/*	if (i_dataBufferSize < calculateSerializeBufferSize() || o_alignedDataBuffer == NULL || (((unsigned)o_alignedDataBuffer & BVH_ALIGNMENT_MASK) != 0))
 	{
 		///check alignedment for buffer?
 		btAssert(0);
 		return false;
 	}
+*/
 
 	btOptimizedBvh *targetBvh = (btOptimizedBvh *)o_alignedDataBuffer;
 
@@ -946,7 +947,7 @@ bool btOptimizedBvh::serialize(void *o_alignedDataBuffer, unsigned i_dataBufferS
 	unsigned char *nodeData = (unsigned char *)targetBvh;
 	nodeData += sizeof(btOptimizedBvh);
 	
-	unsigned sizeToAdd = (BVH_ALIGNMENT-((unsigned)nodeData & BVH_ALIGNMENT_MASK))&BVH_ALIGNMENT_MASK;
+	unsigned sizeToAdd = 0;//(BVH_ALIGNMENT-((unsigned)nodeData & BVH_ALIGNMENT_MASK))&BVH_ALIGNMENT_MASK;
 	nodeData += sizeToAdd;
 	
 	int nodeCount = m_curNodeIndex;
@@ -1021,7 +1022,7 @@ bool btOptimizedBvh::serialize(void *o_alignedDataBuffer, unsigned i_dataBufferS
 		nodeData += sizeof(btOptimizedBvhNode) * nodeCount;
 	}
 
-	sizeToAdd = (BVH_ALIGNMENT-((unsigned)nodeData & BVH_ALIGNMENT_MASK))&BVH_ALIGNMENT_MASK;
+	sizeToAdd = 0;//(BVH_ALIGNMENT-((unsigned)nodeData & BVH_ALIGNMENT_MASK))&BVH_ALIGNMENT_MASK;
 	nodeData += sizeToAdd;
 
 	// Now serialize the subtree headers
@@ -1068,7 +1069,7 @@ bool btOptimizedBvh::serialize(void *o_alignedDataBuffer, unsigned i_dataBufferS
 btOptimizedBvh *btOptimizedBvh::deSerializeInPlace(void *i_alignedDataBuffer, unsigned i_dataBufferSize, bool i_swapEndian)
 {
 
-	if (i_alignedDataBuffer == NULL || (((unsigned)i_alignedDataBuffer & BVH_ALIGNMENT_MASK) != 0))
+	if (i_alignedDataBuffer == NULL)// || (((unsigned)i_alignedDataBuffer & BVH_ALIGNMENT_MASK) != 0))
 	{
 		return NULL;
 	}
@@ -1097,7 +1098,7 @@ btOptimizedBvh *btOptimizedBvh::deSerializeInPlace(void *i_alignedDataBuffer, un
 	unsigned char *nodeData = (unsigned char *)bvh;
 	nodeData += sizeof(btOptimizedBvh);
 	
-	unsigned sizeToAdd = (BVH_ALIGNMENT-((unsigned)nodeData & BVH_ALIGNMENT_MASK))&BVH_ALIGNMENT_MASK;
+	unsigned sizeToAdd = 0;//(BVH_ALIGNMENT-((unsigned)nodeData & BVH_ALIGNMENT_MASK))&BVH_ALIGNMENT_MASK;
 	nodeData += sizeToAdd;
 	
 	int nodeCount = bvh->m_curNodeIndex;
@@ -1146,7 +1147,7 @@ btOptimizedBvh *btOptimizedBvh::deSerializeInPlace(void *i_alignedDataBuffer, un
 		nodeData += sizeof(btOptimizedBvhNode) * nodeCount;
 	}
 
-	sizeToAdd = (BVH_ALIGNMENT-((unsigned)nodeData & BVH_ALIGNMENT_MASK))&BVH_ALIGNMENT_MASK;
+	sizeToAdd = 0;//(BVH_ALIGNMENT-((unsigned)nodeData & BVH_ALIGNMENT_MASK))&BVH_ALIGNMENT_MASK;
 	nodeData += sizeToAdd;
 
 	// Now serialize the subtree headers

src/BulletCollision/CollisionShapes/btSphereShape.h

@@ -42,7 +42,7 @@ public:
 
 	virtual int	getShapeType() const { return SPHERE_SHAPE_PROXYTYPE; }
 
-	btScalar	getRadius() const { return m_implicitShapeDimensions.getX();}
+	btScalar	getRadius() const { return m_implicitShapeDimensions.getX() * m_localScaling.getX();}
 
 	//debugging
 	virtual char*	getName()const {return "SPHERE";}
@@ -55,7 +55,7 @@ public:
 	{
 		//to improve gjk behaviour, use radius+margin as the full margin, so never get into the penetration case
 		//this means, non-uniform scaling is not supported anymore
-		return m_localScaling.getX() * getRadius() + btConvexInternalShape::getMargin();
+		return getRadius();
 	}
 
 

src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp

@@ -27,20 +27,12 @@ btPersistentManifold::btPersistentManifold()
 :m_body0(0),
 m_body1(0),
 m_cachedPoints (0),
-m_index1(0)
+m_index1a(0)
 {
 }
 
 
-void	btPersistentManifold::clearManifold()
-{
-	int i;
-	for (i=0;i<m_cachedPoints;i++)
-	{
-		clearUserCache(m_pointCache[i]);
-	}
-	m_cachedPoints = 0;
-}
+
 
 #ifdef DEBUG_PERSISTENCY
 #include <stdio.h>
@@ -198,10 +190,20 @@ btScalar	btPersistentManifold::getContactBreakingThreshold() const
 	return gContactBreakingThreshold;
 }
 
+
+
 void btPersistentManifold::refreshContactPoints(const btTransform& trA,const btTransform& trB)
 {
 	int i;
-
+#ifdef DEBUG_PERSISTENCY
+	printf("refreshContactPoints posA = (%f,%f,%f) posB = (%f,%f,%f)\n",
+		trA.getOrigin().getX(),
+		trA.getOrigin().getY(),
+		trA.getOrigin().getZ(),
+		trB.getOrigin().getX(),
+		trB.getOrigin().getY(),
+		trB.getOrigin().getZ());
+#endif //DEBUG_PERSISTENCY
 	/// first refresh worldspace positions and distance
 	for (i=getNumContacts()-1;i>=0;i--)
 	{

src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h

@@ -58,7 +58,7 @@ public:
 
 	BT_DECLARE_ALIGNED_ALLOCATOR();
 
-	int m_index1;
+	int m_index1a;
 
 	btPersistentManifold();
 
@@ -67,11 +67,11 @@ public:
 	{
 	}
 
-	inline void* getBody0() { return m_body0;}
-	inline void* getBody1() { return m_body1;}
+	SIMD_FORCE_INLINE void* getBody0() { return m_body0;}
+	SIMD_FORCE_INLINE void* getBody1() { return m_body1;}
 
-	inline const void* getBody0() const { return m_body0;}
-	inline const void* getBody1() const { return m_body1;}
+	SIMD_FORCE_INLINE const void* getBody0() const { return m_body0;}
+	SIMD_FORCE_INLINE const void* getBody1() const { return m_body1;}
 
 	void	setBodies(void* body0,void* body1)
 	{
@@ -85,22 +85,22 @@ public:
 	void	DebugPersistency();
 #endif //
 	
-	inline int	getNumContacts() const { return m_cachedPoints;}
+	SIMD_FORCE_INLINE int	getNumContacts() const { return m_cachedPoints;}
 
-	inline const btManifoldPoint& getContactPoint(int index) const
+	SIMD_FORCE_INLINE const btManifoldPoint& getContactPoint(int index) const
 	{
 		btAssert(index < m_cachedPoints);
 		return m_pointCache[index];
 	}
 
-	inline btManifoldPoint& getContactPoint(int index)
+	SIMD_FORCE_INLINE btManifoldPoint& getContactPoint(int index)
 	{
 		btAssert(index < m_cachedPoints);
 		return m_pointCache[index];
 	}
 
 	/// todo: get this margin from the current physics / collision environment
-	btScalar	getContactBreakingThreshold() const;
+	SIMD_FORCE_INLINE	btScalar	getContactBreakingThreshold() const;
 	
 	int getCacheEntry(const btManifoldPoint& newPoint) const;
 
@@ -150,7 +150,16 @@ public:
 	/// calculated new worldspace coordinates and depth, and reject points that exceed the collision margin
 	void	refreshContactPoints(  const btTransform& trA,const btTransform& trB);
 
-	void	clearManifold();
+	
+	SIMD_FORCE_INLINE	void	clearManifold()
+	{
+		int i;
+		for (i=0;i<m_cachedPoints;i++)
+		{
+			clearUserCache(m_pointCache[i]);
+		}
+		m_cachedPoints = 0;
+	}
 
 
 

src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp

@@ -14,6 +14,8 @@ subject to the following restrictions:
 */
 
 //#define COMPUTE_IMPULSE_DENOM 1
+//It is not necessary (redundant) to refresh contact manifolds, this refresh has been moved to the collision algorithms.
+//#define FORCE_REFESH_CONTACT_MANIFOLDS 1
 
 #include "btSequentialImpulseConstraintSolver.h"
 #include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
@@ -397,21 +399,30 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
 //		printf("empty\n");
 		return 0.f;
 	}
+	btPersistentManifold* manifold = 0;
+	btRigidBody* rb0=0,*rb1=0;
+
+#ifdef FORCE_REFESH_CONTACT_MANIFOLDS
 
 	BEGIN_PROFILE("refreshManifolds");
 
 	int i;
+	
+	
+
 	for (i=0;i<numManifolds;i++)
 	{
-		btPersistentManifold* manifold = manifoldPtr[i];
-		btRigidBody* rb0 = (btRigidBody*)manifold->getBody0();
-		btRigidBody* rb1 = (btRigidBody*)manifold->getBody1();
+		manifold = manifoldPtr[i];
+		rb1 = (btRigidBody*)manifold->getBody1();
+		rb0 = (btRigidBody*)manifold->getBody0();
 		
 		manifold->refreshContactPoints(rb0->getCenterOfMassTransform(),rb1->getCenterOfMassTransform());
 
 	}
 
 	END_PROFILE("refreshManifolds");
+#endif //FORCE_REFESH_CONTACT_MANIFOLDS
+
 
 
 	BEGIN_PROFILE("gatherSolverData");
@@ -458,9 +469,10 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
 
 			for (i=0;i<numManifolds;i++)
 			{
-				btPersistentManifold* manifold = manifoldPtr[i];
-				btRigidBody* rb0 = (btRigidBody*)manifold->getBody0();
-				btRigidBody* rb1 = (btRigidBody*)manifold->getBody1();
+				manifold = manifoldPtr[i];
+				rb1 = (btRigidBody*)manifold->getBody1();
+
+				rb0 = (btRigidBody*)manifold->getBody0();
 				
 			
 				int solverBodyIdA=-1;
@@ -503,7 +515,6 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
 					
 					btManifoldPoint& cp = manifold->getContactPoint(j);
 
-					int frictionIndex = tmpSolverConstraintPool.size();
 					
 					if (cp.getDistance() <= btScalar(0.))
 					{
@@ -519,6 +530,8 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
 						btScalar rel_vel;
 						btVector3 vel;
 
+						int frictionIndex = tmpSolverConstraintPool.size();
+
 						{
 							btSolverConstraint& solverConstraint = tmpSolverConstraintPool.expand();
 
@@ -586,18 +599,22 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
 
 						
 						{
-							btVector3 lat_vel = vel - cp.m_normalWorldOnB * rel_vel;
-							btScalar lat_rel_vel = lat_vel.length2();
+							btVector3 frictionDir1 = vel - cp.m_normalWorldOnB * rel_vel;
+							btScalar lat_rel_vel = frictionDir1.length2();
 							if (lat_rel_vel > SIMD_EPSILON)//0.0f)
 							{
-								lat_vel /= btSqrt(lat_rel_vel);
-
-								addFrictionConstraint(lat_vel,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,rb0,rb1, relaxation);
-								btVector3 frictionDir2 = lat_vel.cross(cp.m_normalWorldOnB);
+								frictionDir1 /= btSqrt(lat_rel_vel);
+								addFrictionConstraint(frictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,rb0,rb1, relaxation);
+								btVector3 frictionDir2 = frictionDir1.cross(cp.m_normalWorldOnB);
 								frictionDir2.normalize();//??
 								addFrictionConstraint(frictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,rb0,rb1, relaxation);
-
-
+							} else
+							{
+								//re-calculate friction direction every frame, todo: check if this is really needed
+								btVector3	frictionDir1,frictionDir2;
+								btPlaneSpace1(cp.m_normalWorldOnB,frictionDir1,frictionDir2);
+								addFrictionConstraint(frictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,rb0,rb1, relaxation);
+								addFrictionConstraint(frictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,rb0,rb1, relaxation);
 							}
 
 						}
@@ -907,8 +924,9 @@ void	btSequentialImpulseConstraintSolver::prepareConstraints(btPersistentManifol
 
 	//only necessary to refresh the manifold once (first iteration). The integration is done outside the loop
 	{
+#ifdef FORCE_REFESH_CONTACT_MANIFOLDS
 		manifoldPtr->refreshContactPoints(body0->getCenterOfMassTransform(),body1->getCenterOfMassTransform());
-		
+#endif //FORCE_REFESH_CONTACT_MANIFOLDS		
 		int numpoints = manifoldPtr->getNumContacts();
 
 		gTotalContactPoints += numpoints;

src/BulletDynamics/Dynamics/btContinuousDynamicsWorld.cpp

@@ -179,7 +179,7 @@ void	btContinuousDynamicsWorld::updateTemporalAabbs(btScalar timeStep)
 			temporalAabbMin -= angularMotion3d;
 			temporalAabbMax += angularMotion3d;
 
-			m_broadphasePairCache->setAabb(body->getBroadphaseHandle(),temporalAabbMin,temporalAabbMax);
+			m_broadphasePairCache->setAabb(body->getBroadphaseHandle(),temporalAabbMin,temporalAabbMax,m_dispatcher1);
 		}
 	}
 

src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp

@@ -606,7 +606,7 @@ void	btDiscreteDynamicsWorld::updateAabbs()
 				//moving objects should be moderately sized, probably something wrong if not
 				if ( colObj->isStaticObject() || ((maxAabb-minAabb).length2() < btScalar(1e12)))
 				{
-					bp->setAabb(body->getBroadphaseHandle(),minAabb,maxAabb);
+					bp->setAabb(body->getBroadphaseHandle(),minAabb,maxAabb, m_dispatcher1);
 				} else
 				{
 					//something went wrong, investigate

src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp

@@ -134,7 +134,7 @@ void	btSimpleDynamicsWorld::updateAabbs()
 				btPoint3 minAabb,maxAabb;
 				colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb,maxAabb);
 				btBroadphaseInterface* bp = getBroadphase();
-				bp->setAabb(body->getBroadphaseHandle(),minAabb,maxAabb);
+				bp->setAabb(body->getBroadphaseHandle(),minAabb,maxAabb, m_dispatcher1);
 			}
 		}
 	}

src/LinearMath/btAlignedAllocator.cpp

@@ -19,7 +19,7 @@ subject to the following restrictions:
 #if defined (BT_HAS_ALIGNED_ALOCATOR)
 
 #include <malloc.h>
-void*	btAlignedAlloc	(int size, int alignment)
+void*	btAlignedAlloc	(size_t size, int alignment)
 {
 	void* ptr = _aligned_malloc(size,alignment);
 //	printf("btAlignedAlloc %d, %x\n",size,ptr);
@@ -41,7 +41,7 @@ void	btAlignedFree	(void* ptr)
 int numAllocs = 0;
 int numFree = 0;
 
-void*	btAlignedAlloc	(int size, int alignment)
+void*	btAlignedAlloc	(size_t size, int alignment)
 {
 	numAllocs++;
 	return memalign(alignment, size);
@@ -55,7 +55,7 @@ void	btAlignedFree	(void* ptr)
 
 #else
 
-void*	btAlignedAlloc	(int size, int alignment)
+void*	btAlignedAlloc	(size_t size, int alignment)
 {
  void *ret;
   char *real;

src/LinearMath/btAlignedAllocator.h

@@ -22,7 +22,7 @@ subject to the following restrictions:
 
 #include "btScalar.h"
 
-void*	btAlignedAlloc	(int size, int alignment);
+void*	btAlignedAlloc	(size_t size, int alignment);
 
 void	btAlignedFree	(void* ptr);
 

src/LinearMath/btPoolAllocator.h

@@ -61,6 +61,17 @@ public:
         return result;
 	}
 
+	bool validPtr(void* ptr)
+	{
+		if (ptr) {
+			if (((unsigned char*)ptr >= m_pool && (unsigned char*)ptr < m_pool + m_maxElements * m_elemSize))
+			{
+				return true;
+			}
+		}
+		return false;
+	}
+
 	void	free(void* ptr)
 	{
 		 if (ptr) {

src/LinearMath/btScalar.h

@@ -288,6 +288,13 @@ SIMD_FORCE_INLINE float btSelect(unsigned condition, float valueIfConditionNonZe
 #endif
 }
 
+template<typename T> inline void btSwap(T& a, T& b)
+{
+	T tmp = a;
+	a = b;
+	b = tmp;
+}
+
 
 //PCK: endian swapping functions
 SIMD_FORCE_INLINE unsigned btSwapEndian(unsigned val)