Based on feedback from another professional game company, there are several improvements, including some API change...

Some dynamic memory allocations have been replace by pool allocation or stack allocations. quantized aabb versus quantized aabb overlap check is made branch-free (helps a lot on consoles PS3/XBox 360) Collision algorithms are now created through a new btDefaultCollisionConfiguration, to decouple dependency (this is the API change): Example: btDefaultCollisionConfiguration* collisionConfiguration = new btDefaultCollisionConfiguration(); m_dispatcher = new btCollisionDispatcher(collisionConfiguration);
2007-09-08 05:40:01 +00:00
parent 30b1887f40
commit 87df3d0f32
67 changed files with 1116 additions and 972 deletions
--- a/Demos/BasicDemo/BasicDemo.cpp
+++ b/Demos/BasicDemo/BasicDemo.cpp
@@ -129,6 +129,7 @@ void BasicDemo::displayCallback(void) {
 void	BasicDemo::initPhysics()
 {

+	btDefaultCollisionConfiguration* collisionConfiguration = new btDefaultCollisionConfiguration();

 #ifdef USE_PARALLEL_DISPATCHER

@@ -149,9 +150,9 @@ void	BasicDemo::initPhysics()
 #endif


-	m_dispatcher = new	SpuGatheringCollisionDispatcher(threadSupport,maxNumOutstandingTasks);
+	m_dispatcher = new	SpuGatheringCollisionDispatcher(threadSupport,maxNumOutstandingTasks,collisionConfiguration);
 #else
-	m_dispatcher = new	btCollisionDispatcher(true);
+	m_dispatcher = new	btCollisionDispatcher(collisionConfiguration);
 #endif //USE_PARALLEL_DISPATCHER

 #define USE_SWEEP_AND_PRUNE 1
@@ -167,18 +168,8 @@ void	BasicDemo::initPhysics()
 	m_overlappingPairCache = new btSimpleBroadphase;
 #endif //USE_SWEEP_AND_PRUNE

-#ifndef USE_PARALLEL_DISPATCHER
-	m_sphereSphereCF = new btSphereSphereCollisionAlgorithm::CreateFunc;
-	m_dispatcher->registerCollisionCreateFunc(SPHERE_SHAPE_PROXYTYPE,SPHERE_SHAPE_PROXYTYPE,m_sphereSphereCF);


-	m_sphereBoxCF = new btSphereBoxCollisionAlgorithm::CreateFunc;
-	m_boxSphereCF = new btSphereBoxCollisionAlgorithm::CreateFunc;
-	m_boxSphereCF->m_swapped = true;
-	m_dispatcher->registerCollisionCreateFunc(SPHERE_SHAPE_PROXYTYPE,BOX_SHAPE_PROXYTYPE,m_sphereBoxCF);
-	m_dispatcher->registerCollisionCreateFunc(BOX_SHAPE_PROXYTYPE,SPHERE_SHAPE_PROXYTYPE,m_boxSphereCF);
-#endif //USE_PARALLEL_DISPATCHER
-
 	btSequentialImpulseConstraintSolver* sol = new btSequentialImpulseConstraintSolver;
 	m_solver = sol;
 	
@@ -267,13 +258,6 @@ void	BasicDemo::exitPhysics()
 	//delete dynamics world
 	delete m_dynamicsWorld;

-	//delete collision algorithms creation functions
-	delete m_sphereSphereCF;
-	
-
-	delete m_sphereBoxCF;
-	delete m_boxSphereCF;
-
 	//delete solver
 	delete m_solver;

--- a/Demos/BspDemo/BspDemo.cpp
+++ b/Demos/BspDemo/BspDemo.cpp
@@ -134,8 +134,9 @@ void	BspDemo::initPhysics(char* bspfilename)

 	///Setup a Physics Simulation Environment

+	btDefaultCollisionConfiguration* collisionConfiguration = new btDefaultCollisionConfiguration();
 //	btCollisionShape* groundShape = new btBoxShape(btVector3(50,3,50));
-	btCollisionDispatcher* dispatcher = new btCollisionDispatcher();
+	btCollisionDispatcher* dispatcher = new btCollisionDispatcher(collisionConfiguration);
 	btVector3 worldMin(-1000,-1000,-1000);
 	btVector3 worldMax(1000,1000,1000);
 	btBroadphaseInterface* pairCache = new btAxisSweep3(worldMin,worldMax);
--- a/Demos/CcdPhysicsDemo/CcdPhysicsDemo.cpp
+++ b/Demos/CcdPhysicsDemo/CcdPhysicsDemo.cpp
@@ -388,7 +388,8 @@ int maxNumOutstandingTasks = 4;//number of maximum outstanding tasks
 	dispatcher = new	SpuGatheringCollisionDispatcher(threadSupportCollision,maxNumOutstandingTasks);
 //	dispatcher = new	btCollisionDispatcher();
 #else
-	dispatcher = new	btCollisionDispatcher();
+	btDefaultCollisionConfiguration* collisionConfiguration = new btDefaultCollisionConfiguration();
+	dispatcher = new	btCollisionDispatcher(collisionConfiguration);
 #endif //USE_PARALLEL_DISPATCHER

 #ifdef USE_CUSTOM_NEAR_CALLBACK
--- a/Demos/ColladaDemo/ColladaDemo.cpp
+++ b/Demos/ColladaDemo/ColladaDemo.cpp
@@ -170,7 +170,8 @@ void	ColladaDemo::initPhysics(const char* filename)
 	m_ele = 60;
 	m_forwardAxis = 1;

-	btCollisionDispatcher* dispatcher = new btCollisionDispatcher();
+	btDefaultCollisionConfiguration* collisionConfiguration = new btDefaultCollisionConfiguration();
+	btCollisionDispatcher* dispatcher = new btCollisionDispatcher(collisionConfiguration);
 	btVector3 worldMin(-1000,-1000,-1000);
 	btVector3 worldMax(1000,1000,1000);
 	btBroadphaseInterface* pairCache = new btAxisSweep3(worldMin,worldMax);
--- a/Demos/CollisionInterfaceDemo/CollisionInterfaceDemo.cpp
+++ b/Demos/CollisionInterfaceDemo/CollisionInterfaceDemo.cpp
@@ -81,7 +81,8 @@ void	CollisionInterfaceDemo::initPhysics()
 	objects[0].setCollisionShape(boxA);//&hullA;
 	objects[1].setCollisionShape(boxB);//&hullB;

-	btCollisionDispatcher* dispatcher = new btCollisionDispatcher;
+	btDefaultCollisionConfiguration* collisionConfiguration = new btDefaultCollisionConfiguration();
+	btCollisionDispatcher* dispatcher = new btCollisionDispatcher(collisionConfiguration);
 	btVector3	worldAabbMin(-1000,-1000,-1000);
 	btVector3	worldAabbMax(1000,1000,1000);

--- a/Demos/ConcaveDemo/ConcavePhysicsDemo.cpp
+++ b/Demos/ConcaveDemo/ConcavePhysicsDemo.cpp
@@ -217,7 +217,8 @@ void	ConcaveDemo::initPhysics()

 	btCollisionDispatcher* dispatcher = new	SpuGatheringCollisionDispatcher(threadSupport,maxNumOutstandingTasks);
 #else
-		btCollisionDispatcher* dispatcher = new	btCollisionDispatcher();
+		btDefaultCollisionConfiguration* collisionConfiguration = new btDefaultCollisionConfiguration();
+		btCollisionDispatcher* dispatcher = new	btCollisionDispatcher(collisionConfiguration);
 #endif//USE_PARALLEL_DISPATCHER


@@ -276,7 +277,7 @@ void ConcaveDemo::clientMoveAndDisplay()
 		trimeshShape->refitTree();

 		//clear all contact points involving mesh proxy. Note: this is a slow/unoptimized operation.
-		m_dynamicsWorld->getBroadphase()->getOverlappingPairCache()->cleanProxyFromPairs(staticBody->getBroadphaseHandle());
+		m_dynamicsWorld->getBroadphase()->getOverlappingPairCache()->cleanProxyFromPairs(staticBody->getBroadphaseHandle(),getDynamicsWorld()->getDispatcher());
 	}

 	m_dynamicsWorld->stepSimulation(dt);
--- a/Demos/ConstraintDemo/ConstraintDemo.cpp
+++ b/Demos/ConstraintDemo/ConstraintDemo.cpp
@@ -76,7 +76,8 @@ void	drawLimit()
 void	ConstraintDemo::initPhysics()
 {
 //	btCollisionShape* groundShape = new btBoxShape(btVector3(50,3,50));
-	btCollisionDispatcher* dispatcher = new btCollisionDispatcher();
+	btDefaultCollisionConfiguration* collisionConfiguration = new btDefaultCollisionConfiguration();
+	btCollisionDispatcher* dispatcher = new btCollisionDispatcher(collisionConfiguration);
 	btVector3 worldMin(-1000,-1000,-1000);
 	btVector3 worldMax(1000,1000,1000);
 	btBroadphaseInterface* pairCache = new btAxisSweep3(worldMin,worldMax);
--- a/Demos/ConvexDecompositionDemo/ConvexDecompositionDemo.cpp
+++ b/Demos/ConvexDecompositionDemo/ConvexDecompositionDemo.cpp
@@ -95,6 +95,8 @@ void ConvexDecompositionDemo::initPhysics(const char* filename)
 		tcount = wo.loadObj("../../file.obj");
 	}

+	btDefaultCollisionConfiguration* collisionConfiguration = new btDefaultCollisionConfiguration();
+
 #ifdef USE_PARALLEL_DISPATCHER
 #ifdef USE_WIN32_THREADING

@@ -112,9 +114,9 @@ void ConvexDecompositionDemo::initPhysics(const char* filename)
 ///you can hook it up to your custom task scheduler by deriving from btThreadSupportInterface
 #endif

-	btCollisionDispatcher* dispatcher = new	SpuGatheringCollisionDispatcher(threadSupport,maxNumOutstandingTasks);
+	btCollisionDispatcher* dispatcher = new	SpuGatheringCollisionDispatcher(threadSupport,maxNumOutstandingTasks,collisionConfiguration);
 #else
-		btCollisionDispatcher* dispatcher = new	btCollisionDispatcher();
+		btCollisionDispatcher* dispatcher = new	btCollisionDispatcher(collisionConfiguration);
 #endif//USE_PARALLEL_DISPATCHER


--- a/Demos/DoublePrecisionDemo/DoublePrecisionDemo.cpp
+++ b/Demos/DoublePrecisionDemo/DoublePrecisionDemo.cpp
@@ -82,7 +82,8 @@ void	DoublePrecisionDemo::initPhysics()
 	objects[0].setCollisionShape(boxA);//&hullA;
 	objects[1].setCollisionShape(boxB);//&hullB;

-	btCollisionDispatcher* dispatcher = new btCollisionDispatcher;
+	btDefaultCollisionConfiguration* collisionConfiguration = new btDefaultCollisionConfiguration();
+	btCollisionDispatcher* dispatcher = new btCollisionDispatcher(collisionConfiguration);
 	btVector3	worldAabbMin(80000,80000,80000);
 	btVector3	worldAabbMax(120000,120000,120000);

--- a/Demos/MovingConcaveDemo/ConcavePhysicsDemo.cpp
+++ b/Demos/MovingConcaveDemo/ConcavePhysicsDemo.cpp
@@ -1644,9 +1644,10 @@ void	ConcaveDemo::initGImpactCollision()
 void	ConcaveDemo::initPhysics()
 {

+	btDefaultCollisionConfiguration* collisionConfiguration = new btDefaultCollisionConfiguration();

 	//btConstraintSolver* solver = new btSequentialImpulseConstraintSolver;
-	btCollisionDispatcher* dispatcher = new btCollisionDispatcher();
+	btCollisionDispatcher* dispatcher = new btCollisionDispatcher(collisionConfiguration);
 	//btOverlappingPairCache* broadphase = new btSimpleBroadphase();
 	btBroadphaseInterface* broadphase = new btSimpleBroadphase();

--- a/Demos/OpenGL/DemoApplication.cpp
+++ b/Demos/OpenGL/DemoApplication.cpp
@@ -932,7 +932,7 @@ void	DemoApplication::clientResetScene()
 				colObj->activate();
 			}
 			//removed cached contact points
-			m_dynamicsWorld->getBroadphase()->getOverlappingPairCache()->cleanProxyFromPairs(colObj->getBroadphaseHandle());
+			m_dynamicsWorld->getBroadphase()->getOverlappingPairCache()->cleanProxyFromPairs(colObj->getBroadphaseHandle(),getDynamicsWorld()->getDispatcher());

 			btRigidBody* body = btRigidBody::upcast(colObj);
 			if (body && !body->isStaticObject())
--- a/Demos/RagdollDemo/RagdollDemo.cpp
+++ b/Demos/RagdollDemo/RagdollDemo.cpp
@@ -307,7 +307,9 @@ void RagdollDemo::initPhysics()
 {
 	// Setup the basic world

-	btCollisionDispatcher* dispatcher = new btCollisionDispatcher;
+	btDefaultCollisionConfiguration* collisionConfiguration = new btDefaultCollisionConfiguration();
+
+	btCollisionDispatcher* dispatcher = new btCollisionDispatcher(collisionConfiguration);

 	btPoint3 worldAabbMin(-10000,-10000,-10000);
 	btPoint3 worldAabbMax(10000,10000,10000);
--- a/Demos/UserCollisionAlgorithm/UserCollisionAlgorithm.cpp
+++ b/Demos/UserCollisionAlgorithm/UserCollisionAlgorithm.cpp
@@ -104,8 +104,8 @@ void	UserCollisionAlgorithm::initPhysics()
 	btCollisionShape* trimeshShape  = new btBvhTriangleMeshShape(trimesh,useQuantizedBvhTree);
 		
 	//ConstraintSolver* solver = new btSequentialImpulseConstraintSolver;
-	
-	btCollisionDispatcher* dispatcher = new	btCollisionDispatcher();
+	btDefaultCollisionConfiguration* collisionConfiguration = new btDefaultCollisionConfiguration();
+	btCollisionDispatcher* dispatcher = new	btCollisionDispatcher(collisionConfiguration);
 		
 	btVector3 maxAabb(10000,10000,10000);
 	btBroadphaseInterface* broadphase = new btAxisSweep3(-maxAabb,maxAabb);//SimpleBroadphase();
--- a/Demos/VehicleDemo/VehicleDemo.cpp
+++ b/Demos/VehicleDemo/VehicleDemo.cpp
@@ -118,7 +118,8 @@ void VehicleDemo::setupPhysics()
 #endif

 	btCollisionShape* groundShape = new btBoxShape(btVector3(50,3,50));
-	btCollisionDispatcher* dispatcher = new btCollisionDispatcher();
+	btDefaultCollisionConfiguration* collisionConfiguration = new btDefaultCollisionConfiguration();
+	btCollisionDispatcher* dispatcher = new btCollisionDispatcher(collisionConfiguration);
 	btVector3 worldMin(-1000,-1000,-1000);
 	btVector3 worldMax(1000,1000,1000);
 	btBroadphaseInterface* pairCache = new btAxisSweep3(worldMin,worldMax);
@@ -490,7 +491,7 @@ void VehicleDemo::clientResetScene()
 	m_carChassis->setCenterOfMassTransform(btTransform::getIdentity());
 	m_carChassis->setLinearVelocity(btVector3(0,0,0));
 	m_carChassis->setAngularVelocity(btVector3(0,0,0));
-	m_dynamicsWorld->getBroadphase()->getOverlappingPairCache()->cleanProxyFromPairs(m_carChassis->getBroadphaseHandle());
+	m_dynamicsWorld->getBroadphase()->getOverlappingPairCache()->cleanProxyFromPairs(m_carChassis->getBroadphaseHandle(),getDynamicsWorld()->getDispatcher());
 	if (m_vehicle)
 	{
 		m_vehicle->resetSuspension();
--- a/Extras/BulletMultiThreaded/Jamfile
+++ b/Extras/BulletMultiThreaded/Jamfile
@@ -2,7 +2,7 @@ SubDir TOP Extras BulletMultiThreaded ;

 #IncludeDir Extras/BulletMultiThreaded ;

-Library bulletmultithreaded : [ Wildcard . : */.h *.cpp ] [ Wildcard SpuNarrowPhaseCollisionTask : *.h *.cpp  ] : noinstall ;
+Library bulletmultithreaded : [ Wildcard . : */.h *.cpp ] [ Wildcard SpuNarrowPhaseCollisionTask : *.h *.cpp  ] [ Wildcard SpuSolverTask : *.h *.cpp  ] : noinstall ;
 CFlags bulletmultithreaded : [ FIncludes $(TOP)/Extras/BulletMultiThreaded ] ;
 LibDepends bulletmultithreaded :  ;

--- a/Extras/BulletMultiThreaded/SpuContactManifoldCollisionAlgorithm.h
+++ b/Extras/BulletMultiThreaded/SpuContactManifoldCollisionAlgorithm.h
@@ -19,6 +19,8 @@ subject to the following restrictions:
 #include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h"
 #include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
 #include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h"
+#include "BulletCollision/BroadphaseCollision/btDispatcher.h"
+
 class btPersistentManifold;

 /// SpuContactManifoldCollisionAlgorithm  provides contact manifold and should be processed on SPU.
@@ -71,7 +73,8 @@ public:
 	{
 		virtual	btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1)
 		{
-			return new SpuContactManifoldCollisionAlgorithm(ci,body0,body1);
+			void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(SpuContactManifoldCollisionAlgorithm));
+			return new(mem) SpuContactManifoldCollisionAlgorithm(ci,body0,body1);
 		}
 	};

--- a/Extras/BulletMultiThreaded/SpuGatheringCollisionDispatcher.cpp
+++ b/Extras/BulletMultiThreaded/SpuGatheringCollisionDispatcher.cpp
@@ -26,8 +26,9 @@ subject to the following restrictions:



-SpuGatheringCollisionDispatcher::SpuGatheringCollisionDispatcher(class	btThreadSupportInterface*	threadInterface, unsigned int	maxNumOutstandingTasks)
-:m_spuCollisionTaskProcess(0),
+SpuGatheringCollisionDispatcher::SpuGatheringCollisionDispatcher(class	btThreadSupportInterface*	threadInterface, unsigned int	maxNumOutstandingTasks,btCollisionConfiguration* collisionConfiguration)
+:btCollisionDispatcher(collisionConfiguration),
+m_spuCollisionTaskProcess(0),
 m_threadInterface(threadInterface),
 m_maxNumOutstandingTasks(maxNumOutstandingTasks)
 {
@@ -115,7 +116,7 @@ public:
 				btCollisionObject* colObj1 = (btCollisionObject*)collisionPair.m_pProxy1->m_clientObject;

 				btCollisionAlgorithmConstructionInfo ci;
-				ci.m_dispatcher = m_dispatcher;
+				ci.m_dispatcher1 = m_dispatcher;
 				ci.m_manifold = 0;

 				if (m_dispatcher->needsCollision(colObj0,colObj1))
@@ -124,7 +125,9 @@ public:
 					int	proxyType1 = colObj1->getCollisionShape()->getShapeType();
 					if (m_dispatcher->supportsDispatchPairOnSpu(proxyType0,proxyType1))
 					{
-						collisionPair.m_algorithm = new SpuContactManifoldCollisionAlgorithm(ci,colObj0,colObj1);
+						int so = sizeof(SpuContactManifoldCollisionAlgorithm);
+						void* mem = m_dispatcher->allocateCollisionAlgorithm(so);
+						collisionPair.m_algorithm = new(mem) SpuContactManifoldCollisionAlgorithm(ci,colObj0,colObj1);
 						collisionPair.m_userInfo = (void*) 2;
 					} else
 					{
@@ -138,7 +141,7 @@ public:
 	}
 };

-void	SpuGatheringCollisionDispatcher::dispatchAllCollisionPairs(btOverlappingPairCache* pairCache,btDispatcherInfo& dispatchInfo)
+void	SpuGatheringCollisionDispatcher::dispatchAllCollisionPairs(btOverlappingPairCache* pairCache,btDispatcherInfo& dispatchInfo,btDispatcher* dispatcher)
 {

 	if (dispatchInfo.m_enableSPU)
@@ -152,7 +155,7 @@ void	SpuGatheringCollisionDispatcher::dispatchAllCollisionPairs(btOverlappingPai
 		{
 			btSpuCollisionPairCallback	collisionCallback(dispatchInfo,this);

-			pairCache->processAllOverlappingPairs(&collisionCallback);
+			pairCache->processAllOverlappingPairs(&collisionCallback,dispatcher);
 		}

 		//send one big batch
@@ -203,7 +206,7 @@ void	SpuGatheringCollisionDispatcher::dispatchAllCollisionPairs(btOverlappingPai
 	{
 		///PPU fallback
 		///!Need to make sure to clear all 'algorithms' when switching between SPU and PPU
-		btCollisionDispatcher::dispatchAllCollisionPairs(pairCache,dispatchInfo);
+		btCollisionDispatcher::dispatchAllCollisionPairs(pairCache,dispatchInfo,dispatcher);
 	}
 }

--- a/Extras/BulletMultiThreaded/SpuGatheringCollisionDispatcher.h
+++ b/Extras/BulletMultiThreaded/SpuGatheringCollisionDispatcher.h
@@ -49,13 +49,13 @@ public:
 			return m_spuCollisionTaskProcess;
 	}
 	
-	SpuGatheringCollisionDispatcher (class	btThreadSupportInterface*	threadInterface, unsigned int	maxNumOutstandingTasks);
+	SpuGatheringCollisionDispatcher (class	btThreadSupportInterface*	threadInterface, unsigned int	maxNumOutstandingTasks,btCollisionConfiguration* collisionConfiguration);
 	
 	virtual ~SpuGatheringCollisionDispatcher();

 	bool	supportsDispatchPairOnSpu(int proxyType0,int proxyType1);

-	virtual void	dispatchAllCollisionPairs(btOverlappingPairCache* pairCache,btDispatcherInfo& dispatchInfo);
+	virtual void	dispatchAllCollisionPairs(btOverlappingPairCache* pairCache,btDispatcherInfo& dispatchInfo,btDispatcher* dispatcher);

 };

--- a/Extras/BulletMultiThreaded/SpuParallelSolver.cpp
+++ b/Extras/BulletMultiThreaded/SpuParallelSolver.cpp
@@ -105,7 +105,7 @@ void btParallelSequentialImpulseSolver::prepareSolve(int numBodies, int numManif
 	m_allObjects.reserve(numBodies);
 }

-btScalar btParallelSequentialImpulseSolver::solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints, const btContactSolverInfo& info,class btIDebugDraw* debugDrawer, btStackAlloc* stackAlloc)
+btScalar btParallelSequentialImpulseSolver::solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints, const btContactSolverInfo& info,class btIDebugDraw* debugDrawer, btStackAlloc* stackAlloc,btDispatcher* dispatcher)
 {
 	if (!numManifolds && !numConstraints)
 		return 0;
--- a/Extras/BulletMultiThreaded/SpuParallelSolver.h
+++ b/Extras/BulletMultiThreaded/SpuParallelSolver.h
@@ -67,7 +67,7 @@ public:
 	virtual ~btParallelSequentialImpulseSolver();

 	virtual void prepareSolve (int numBodies, int numManifolds);
-	virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints, const btContactSolverInfo& info,class btIDebugDraw* debugDrawer, btStackAlloc* stackAlloc);
+	virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints, const btContactSolverInfo& info,class btIDebugDraw* debugDrawer, btStackAlloc* stackAlloc,btDispatcher* dispatcher);
 	virtual void allSolved (const btContactSolverInfo& info,class btIDebugDraw* debugDrawer, btStackAlloc* stackAlloc);
 	virtual void reset ();
 };
--- a/Extras/GIMPACT/include/GIMPACT/Bullet/btGImpactCollisionAlgorithm.h
+++ b/Extras/GIMPACT/include/GIMPACT/Bullet/btGImpactCollisionAlgorithm.h
@@ -84,7 +84,8 @@ protected:
 	{
 		if(m_convex_algorithm)
 		{
-			delete m_convex_algorithm;
+			m_convex_algorithm->~btCollisionAlgorithm();
+			m_dispatcher->freeCollisionAlgorithm( m_convex_algorithm);
 			m_convex_algorithm = NULL;
 		}
 	}
@@ -188,7 +189,8 @@ public:
 	{
 		virtual	btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1)
 		{
-			return new btGImpactCollisionAlgorithm(ci,body0,body1);
+			void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btGImpactCollisionAlgorithm));
+			return new(mem) btGImpactCollisionAlgorithm(ci,body0,body1);
 		}
 	};

--- a/Extras/quickstep/OdeConstraintSolver.cpp
+++ b/Extras/quickstep/OdeConstraintSolver.cpp
@@ -72,7 +72,7 @@ m_erp(0.4f)


 //iterative lcp and penalty method
-btScalar OdeConstraintSolver::solveGroup(btCollisionObject** bodies,int numBulletBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc)
+btScalar OdeConstraintSolver::solveGroup(btCollisionObject** bodies,int numBulletBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc,btDispatcher* dispatcher)
 {
 	BEGIN_PROFILE("prepareConstraints");

@@ -291,3 +291,7 @@ void OdeConstraintSolver::ConvertConstraint(btPersistentManifold* manifold,BU_Jo
 	//create a new contact constraint
 };

+
+void	OdeConstraintSolver::reset()
+{
+}
--- a/Extras/quickstep/OdeConstraintSolver.h
+++ b/Extras/quickstep/OdeConstraintSolver.h
@@ -20,6 +20,7 @@ subject to the following restrictions:

 class btRigidBody;
 struct	OdeSolverBody;
+class btDispatcher;
 class BU_Joint;

 /// OdeConstraintSolver is one of the available solvers for Bullet dynamics framework
@@ -45,7 +46,7 @@ public:

 	virtual ~OdeConstraintSolver() {}
 	
-	virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc);
+	virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc,btDispatcher* dispatcher);

 	///setConstraintForceMixing, the cfm adds some positive value to the main diagonal
 	///This can improve convergence (make matrix positive semidefinite), but it can make the simulation look more 'springy'
@@ -59,6 +60,8 @@ public:
 	{
 		m_erp = erp;
 	}
+
+	virtual	void	reset();
 };


--- a/src/BulletCollision/BroadphaseCollision/btAxisSweep3.cpp
+++ b/src/BulletCollision/BroadphaseCollision/btAxisSweep3.cpp
@@ -55,10 +55,10 @@ btBroadphaseProxy*	btAxisSweep3::createProxy(  const btVector3& aabbMin,  const
 		return handle;
 }

-void	btAxisSweep3::destroyProxy(btBroadphaseProxy* proxy)
+void	btAxisSweep3::destroyProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher)
 {
 	Handle* handle = static_cast<Handle*>(proxy);
-	removeHandle(handle->m_handleId);
+	removeHandle(handle->m_handleId,dispatcher);
 }

 void	btAxisSweep3::setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax)
@@ -248,7 +248,7 @@ BP_FP_INT_TYPE btAxisSweep3::addHandle(const btPoint3& aabbMin,const btPoint3& a
 }


-void btAxisSweep3::removeHandle(BP_FP_INT_TYPE handle)
+void btAxisSweep3::removeHandle(BP_FP_INT_TYPE handle,btDispatcher* dispatcher)
 {

 	Handle* pHandle = getHandle(handle);
@@ -256,7 +256,7 @@ void btAxisSweep3::removeHandle(BP_FP_INT_TYPE handle)
 	//explicitly remove the pairs containing the proxy
 	//we could do it also in the sortMinUp (passing true)
 	//todo: compare performance
-	m_pairCache->removeOverlappingPairsContainingProxy(pHandle);
+	m_pairCache->removeOverlappingPairsContainingProxy(pHandle,dispatcher);


 	// compute current limit of edge arrays
@@ -305,7 +305,7 @@ void btAxisSweep3::removeHandle(BP_FP_INT_TYPE handle)
 extern int gOverlappingPairs;


-void	btAxisSweep3::calculateOverlappingPairs()
+void	btAxisSweep3::calculateOverlappingPairs(btDispatcher* dispatcher)
 {
 	
 	if (m_ownsPairCache)
@@ -359,7 +359,7 @@ void	btAxisSweep3::calculateOverlappingPairs()
 			
 			if (needsRemoval)
 			{
-				m_pairCache->cleanOverlappingPair(pair);
+				m_pairCache->cleanOverlappingPair(pair,dispatcher);

 		//		m_overlappingPairArray.swap(i,m_overlappingPairArray.size()-1);
 		//		m_overlappingPairArray.pop_back();
--- a/src/BulletCollision/BroadphaseCollision/btAxisSweep3.h
+++ b/src/BulletCollision/BroadphaseCollision/btAxisSweep3.h
@@ -121,10 +121,10 @@ public:
 	btAxisSweep3(const btPoint3& worldAabbMin,const btPoint3& worldAabbMax, int maxHandles = 16384, btOverlappingPairCache* pairCache=0);
 	virtual	~btAxisSweep3();

-	virtual void	calculateOverlappingPairs();
+	virtual void	calculateOverlappingPairs(btDispatcher* dispatcher);
 	
 	BP_FP_INT_TYPE addHandle(const btPoint3& aabbMin,const btPoint3& aabbMax, void* pOwner,short int collisionFilterGroup,short int collisionFilterMask);
-	void removeHandle(BP_FP_INT_TYPE handle);
+	void removeHandle(BP_FP_INT_TYPE handle,btDispatcher* dispatcher);
 	void updateHandle(BP_FP_INT_TYPE handle, const btPoint3& aabbMin,const btPoint3& aabbMax);
 	inline Handle* getHandle(BP_FP_INT_TYPE index) const {return m_pHandles + index;}

@@ -132,7 +132,7 @@ public:

 	//Broadphase Interface
 	virtual btBroadphaseProxy*	createProxy(  const btVector3& aabbMin,  const btVector3& aabbMax,int shapeType,void* userPtr ,short int collisionFilterGroup,short int collisionFilterMask);
-	virtual void	destroyProxy(btBroadphaseProxy* proxy);
+	virtual void	destroyProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher);
 	virtual void	setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax);
 	
 	bool	testAabbOverlap(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1);
--- a/src/BulletCollision/BroadphaseCollision/btBroadphaseInterface.h
+++ b/src/BulletCollision/BroadphaseCollision/btBroadphaseInterface.h
@@ -32,11 +32,11 @@ public:
 	virtual ~btBroadphaseInterface() {}

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

 	virtual	btOverlappingPairCache*	getOverlappingPairCache()=0;
 	virtual	const btOverlappingPairCache*	getOverlappingPairCache() const =0;
--- a/src/BulletCollision/BroadphaseCollision/btCollisionAlgorithm.cpp
+++ b/src/BulletCollision/BroadphaseCollision/btCollisionAlgorithm.cpp
@@ -18,6 +18,6 @@ subject to the following restrictions:

 btCollisionAlgorithm::btCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci)
 {
-	m_dispatcher = ci.m_dispatcher;
+	m_dispatcher = ci.m_dispatcher1;
 }

--- a/src/BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h
+++ b/src/BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h
@@ -29,17 +29,17 @@ class	btPersistentManifold;
 struct btCollisionAlgorithmConstructionInfo
 {
 	btCollisionAlgorithmConstructionInfo()
-		:m_dispatcher(0),
+		:m_dispatcher1(0),
 		m_manifold(0)
 	{
 	}
 	btCollisionAlgorithmConstructionInfo(btDispatcher* dispatcher,int temp)
-		:m_dispatcher(dispatcher)
+		:m_dispatcher1(dispatcher)
 	{
 		(void)temp;
 	}

-	btDispatcher*	m_dispatcher;
+	btDispatcher*	m_dispatcher1;
 	btPersistentManifold*	m_manifold;

 	int	getDispatcherId();
--- a/src/BulletCollision/BroadphaseCollision/btDispatcher.h
+++ b/src/BulletCollision/BroadphaseCollision/btDispatcher.h
@@ -81,12 +81,18 @@ public:

 	virtual bool	needsResponse(btCollisionObject* body0,btCollisionObject* body1)=0;

-	virtual void	dispatchAllCollisionPairs(btOverlappingPairCache* pairCache,btDispatcherInfo& dispatchInfo)=0;
+	virtual void	dispatchAllCollisionPairs(btOverlappingPairCache* pairCache,btDispatcherInfo& dispatchInfo,btDispatcher* dispatcher)=0;

 	virtual int getNumManifolds() const = 0;

 	virtual btPersistentManifold* getManifoldByIndexInternal(int index) = 0;

+	virtual	btPersistentManifold**	getInternalManifoldPointer() = 0;
+
+	virtual	void* allocateCollisionAlgorithm(int size) = 0;
+
+	virtual	void freeCollisionAlgorithm(void* ptr) = 0;
+
 };


--- a/src/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.cpp
+++ b/src/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.cpp
@@ -76,7 +76,7 @@ btBroadphaseProxy*	btMultiSapBroadphase::createProxy(  const btVector3& aabbMin,
 	return proxy;
 }

-void	btMultiSapBroadphase::destroyProxy(btBroadphaseProxy* proxy)
+void	btMultiSapBroadphase::destroyProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher)
 {
 	///not yet
 	btAssert(0);
@@ -97,9 +97,9 @@ void	btMultiSapBroadphase::setAabb(btBroadphaseProxy* proxy,const btVector3& aab
 }
 	
 	///calculateOverlappingPairs is optional: incremental algorithms (sweep and prune) might do it during the set aabb
-void	btMultiSapBroadphase::calculateOverlappingPairs()
+void	btMultiSapBroadphase::calculateOverlappingPairs(btDispatcher* dispatcher)
 {
-	m_simpleBroadphase->calculateOverlappingPairs();
+	m_simpleBroadphase->calculateOverlappingPairs(dispatcher);

 	btBroadphasePairArray&	overlappingPairArray = m_overlappingPairs->getOverlappingPairArray();

@@ -149,7 +149,7 @@ void	btMultiSapBroadphase::calculateOverlappingPairs()
 		
 		if (needsRemoval)
 		{
-			m_overlappingPairs->cleanOverlappingPair(pair);
+			m_overlappingPairs->cleanOverlappingPair(pair,dispatcher);

 	//		m_overlappingPairArray.swap(i,m_overlappingPairArray.size()-1);
 	//		m_overlappingPairArray.pop_back();
--- a/src/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.h
+++ b/src/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.h
@@ -96,11 +96,11 @@ public:
 	virtual ~btMultiSapBroadphase();

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

 	bool	testAabbOverlap(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1);

--- a/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp
+++ b/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp
@@ -37,7 +37,7 @@ btOverlappingPairCache::~btOverlappingPairCache()
 }


-void	btOverlappingPairCache::removeOverlappingPair(btBroadphasePair& findPair)
+void	btOverlappingPairCache::removeOverlappingPair(btBroadphasePair& findPair, btDispatcher* dispatcher )
 {
 	
 	int findIndex = m_overlappingPairArray.findLinearSearch(findPair);
@@ -45,7 +45,7 @@ void	btOverlappingPairCache::removeOverlappingPair(btBroadphasePair& findPair)
 	{
 		gOverlappingPairs--;
 		btBroadphasePair& pair = m_overlappingPairArray[findIndex];
-		cleanOverlappingPair(pair);
+		cleanOverlappingPair(pair,dispatcher);
 		
 		m_overlappingPairArray.swap(findIndex,m_overlappingPairArray.size()-1);
 		m_overlappingPairArray.pop_back();
@@ -53,12 +53,13 @@ void	btOverlappingPairCache::removeOverlappingPair(btBroadphasePair& findPair)
 }


-void	btOverlappingPairCache::cleanOverlappingPair(btBroadphasePair& pair)
+void	btOverlappingPairCache::cleanOverlappingPair(btBroadphasePair& pair,btDispatcher* dispatcher)
 {
 	if (pair.m_algorithm)
 	{
 		{
-			delete pair.m_algorithm;;
+			pair.m_algorithm->~btCollisionAlgorithm();
+			dispatcher->freeCollisionAlgorithm(pair.m_algorithm);
 			pair.m_algorithm=0;
 		}
 	}
@@ -109,18 +110,20 @@ void	btOverlappingPairCache::addOverlappingPair(btBroadphaseProxy* proxy0,btBroa



-void	btOverlappingPairCache::cleanProxyFromPairs(btBroadphaseProxy* proxy)
+void	btOverlappingPairCache::cleanProxyFromPairs(btBroadphaseProxy* proxy,btDispatcher* dispatcher)
 {

 	class	CleanPairCallback : public btOverlapCallback
 	{
 		btBroadphaseProxy* m_cleanProxy;
 		btOverlappingPairCache*	m_pairCache;
+		btDispatcher* m_dispatcher;

 	public:
-		CleanPairCallback(btBroadphaseProxy* cleanProxy,btOverlappingPairCache* pairCache)
+		CleanPairCallback(btBroadphaseProxy* cleanProxy,btOverlappingPairCache* pairCache,btDispatcher* dispatcher)
 			:m_cleanProxy(cleanProxy),
-			m_pairCache(pairCache)
+			m_pairCache(pairCache),
+			m_dispatcher(dispatcher)
 		{
 		}
 		virtual	bool	processOverlap(btBroadphasePair& pair)
@@ -128,22 +131,22 @@ void	btOverlappingPairCache::cleanProxyFromPairs(btBroadphaseProxy* proxy)
 			if ((pair.m_pProxy0 == m_cleanProxy) ||
 				(pair.m_pProxy1 == m_cleanProxy))
 			{
-				m_pairCache->cleanOverlappingPair(pair);
+				m_pairCache->cleanOverlappingPair(pair,m_dispatcher);
 			}
 			return false;
 		}
 		
 	};

-	CleanPairCallback cleanPairs(proxy,this);
+	CleanPairCallback cleanPairs(proxy,this,dispatcher);

-	processAllOverlappingPairs(&cleanPairs);
+	processAllOverlappingPairs(&cleanPairs,dispatcher);

 }



-void	btOverlappingPairCache::removeOverlappingPairsContainingProxy(btBroadphaseProxy* proxy)
+void	btOverlappingPairCache::removeOverlappingPairsContainingProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher)
 {

 	class	RemovePairCallback : public btOverlapCallback
@@ -166,12 +169,12 @@ void	btOverlappingPairCache::removeOverlappingPairsContainingProxy(btBroadphaseP

 	RemovePairCallback removeCallback(proxy);

-	processAllOverlappingPairs(&removeCallback);
+	processAllOverlappingPairs(&removeCallback,dispatcher);
 }



-void	btOverlappingPairCache::processAllOverlappingPairs(btOverlapCallback* callback)
+void	btOverlappingPairCache::processAllOverlappingPairs(btOverlapCallback* callback,btDispatcher* dispatcher)
 {

 	int i;
@@ -182,7 +185,7 @@ void	btOverlappingPairCache::processAllOverlappingPairs(btOverlapCallback* callb
 		btBroadphasePair* pair = &m_overlappingPairArray[i];
 		if (callback->processOverlap(*pair))
 		{
-			cleanOverlappingPair(*pair);
+			cleanOverlappingPair(*pair,dispatcher);

 			m_overlappingPairArray.swap(i,m_overlappingPairArray.size()-1);
 			m_overlappingPairArray.pop_back();
--- a/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h
+++ b/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h
@@ -22,7 +22,7 @@ subject to the following restrictions:
 #include "btBroadphaseProxy.h"
 #include "LinearMath/btPoint3.h"
 #include "LinearMath/btAlignedObjectArray.h"
-
+class btDispatcher;

 struct	btOverlapCallback
 {
@@ -61,20 +61,20 @@ class	btOverlappingPairCache
 		btOverlappingPairCache();	
 		virtual ~btOverlappingPairCache();

-		virtual void	processAllOverlappingPairs(btOverlapCallback*);
+		virtual void	processAllOverlappingPairs(btOverlapCallback*,btDispatcher* dispatcher);

-		void	removeOverlappingPair(btBroadphasePair& pair);
+		void	removeOverlappingPair(btBroadphasePair& pair,btDispatcher* dispatcher);

-		void	cleanOverlappingPair(btBroadphasePair& pair);
+		void	cleanOverlappingPair(btBroadphasePair& pair,btDispatcher* dispatcher);
 		
 		void	addOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1);

 		btBroadphasePair*	findPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1);
 			
 		
-		void	cleanProxyFromPairs(btBroadphaseProxy* proxy);
+		void	cleanProxyFromPairs(btBroadphaseProxy* proxy,btDispatcher* dispatcher);

-		void	removeOverlappingPairsContainingProxy(btBroadphaseProxy* proxy);
+		void	removeOverlappingPairsContainingProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher);


 		inline bool needsBroadphaseCollision(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1) const
--- a/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp
+++ b/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp
@@ -139,7 +139,7 @@ protected:
 	};
 };

-void	btSimpleBroadphase::destroyProxy(btBroadphaseProxy* proxyOrg)
+void	btSimpleBroadphase::destroyProxy(btBroadphaseProxy* proxyOrg,btDispatcher* dispatcher)
 {
 		
 		int i;
@@ -151,7 +151,7 @@ void	btSimpleBroadphase::destroyProxy(btBroadphaseProxy* proxyOrg)
 		btAssert (index < m_maxProxies);
 		m_freeProxies[--m_firstFreeProxy] = index;

-		m_pairCache->removeOverlappingPairsContainingProxy(proxyOrg);
+		m_pairCache->removeOverlappingPairsContainingProxy(proxyOrg,dispatcher);
 		
 		for (i=0;i<m_numProxies;i++)
 		{
@@ -201,7 +201,7 @@ public:
 	}
 };

-void	btSimpleBroadphase::calculateOverlappingPairs()
+void	btSimpleBroadphase::calculateOverlappingPairs(btDispatcher* dispatcher)
 {
 	//first check for new overlapping pairs
 	int i,j;
@@ -276,7 +276,7 @@ void	btSimpleBroadphase::calculateOverlappingPairs()
 			
 			if (needsRemoval)
 			{
-				m_pairCache->cleanOverlappingPair(pair);
+				m_pairCache->cleanOverlappingPair(pair,dispatcher);

 		//		m_overlappingPairArray.swap(i,m_overlappingPairArray.size()-1);
 		//		m_overlappingPairArray.pop_back();
--- a/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.h
+++ b/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.h
@@ -82,9 +82,9 @@ public:

 	virtual btBroadphaseProxy*	createProxy(  const btVector3& aabbMin,  const btVector3& aabbMax,int shapeType,void* userPtr ,short int collisionFilterGroup,short int collisionFilterMask);

-	virtual void	calculateOverlappingPairs();
+	virtual void	calculateOverlappingPairs(btDispatcher* dispatcher);

-	virtual void	destroyProxy(btBroadphaseProxy* proxy);
+	virtual void	destroyProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher);
 	virtual void	setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax);
 		
 	btOverlappingPairCache*	getOverlappingPairCache()
--- a/src/BulletCollision/CMakeLists.txt
+++ b/src/BulletCollision/CMakeLists.txt
@@ -16,6 +16,7 @@ ADD_LIBRARY(LibBulletCollision
 				CollisionDispatch/btCollisionWorld.cpp
 				CollisionDispatch/btCompoundCollisionAlgorithm.cpp
 				CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp
+				CollisionDispatch/btDefaultCollisionConfiguration.cpp
 				CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp
 				CollisionDispatch/btSphereBoxCollisionAlgorithm.cpp
 				CollisionDispatch/btConvexConvexAlgorithm.cpp
--- a/src/BulletCollision/CollisionDispatch/btCollisionConfiguration.h
+++ b/src/BulletCollision/CollisionDispatch/btCollisionConfiguration.h
@@ -15,6 +15,8 @@ subject to the following restrictions:

 #ifndef BT_COLLISION_CONFIGURATION
 #define BT_COLLISION_CONFIGURATION
+struct btCollisionAlgorithmCreateFunc;
+
 ///btCollisionConfiguration allows to configure Bullet collision detection
 ///stack allocator size, default collision algorithms and persistent manifold pool size
 ///todo: describe the meaning
@@ -28,7 +30,7 @@ public:
 	}

 	///pool size for the persistent contact manifold
-	virtual int	getMaxPersistentManifoldPoolSize() = 0;
+	virtual int	getPersistentManifoldPoolSize() = 0;

 	virtual int	getStackAllocatorSize() = 0;

@@ -36,17 +38,7 @@ public:

 	virtual int	getCollisionAlgorithmMaxElementSize() = 0;

-	virtual btCollisionAlgorithmCreateFunc* getConvexConvexCollisionCreateFunc() = 0;
-
-	virtual btCollisionAlgorithmCreateFunc* getConvexConcaveCollisionCreateFunc() = 0;
-
-	virtual btCollisionAlgorithmCreateFunc* getSwappedConvexConcaveCollisionCreateFunc() = 0;
-
-	virtual btCollisionAlgorithmCreateFunc* getCompoundCollisionCreateFunc() = 0;
-
-	virtual btCollisionAlgorithmCreateFunc* getSwappedCompoundCollisionCreateFunc() = 0;
-
-	virtual btCollisionAlgorithmCreateFunc* getEmptyCollisionCreateFunc() = 0;
+	virtual btCollisionAlgorithmCreateFunc* getCollisionAlgorithmCreateFunc(int proxyType0,int proxyType1) =0;

 };

--- a/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp
+++ b/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp
@@ -19,69 +19,40 @@ subject to the following restrictions:


 #include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h"
-#include "BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h"
-#include "BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.h"
-#include "BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.h"
-#include "BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h"
+
 #include "BulletCollision/CollisionShapes/btCollisionShape.h"
 #include "BulletCollision/CollisionDispatch/btCollisionObject.h"
 #include "BulletCollision/BroadphaseCollision/btOverlappingPairCache.h"
+#include "LinearMath/btPoolAllocator.h"
+#include "BulletCollision/CollisionDispatch/btCollisionConfiguration.h"

 int gNumManifold = 0;

 #include <stdio.h>


-btCollisionDispatcher::btCollisionDispatcher(bool noDefaultAlgorithms):
-m_count(0),
-m_useIslands(true),
-m_convexConvexCreateFunc(0),
-m_convexConcaveCreateFunc(0),
-m_swappedConvexConcaveCreateFunc(0),
-m_compoundCreateFunc(0),
-m_swappedCompoundCreateFunc(0),
-m_emptyCreateFunc(0)
-{
-	(void)noDefaultAlgorithms;
-	int i;

-	setNearCallback(defaultNearCallback);
-
-	m_emptyCreateFunc = new btEmptyAlgorithm::CreateFunc;
-	for (i=0;i<MAX_BROADPHASE_COLLISION_TYPES;i++)
-	{
-		for (int j=0;j<MAX_BROADPHASE_COLLISION_TYPES;j++)
-		{
-			m_doubleDispatch[i][j] = m_emptyCreateFunc;
-		}
-	}
-}
-//if you want to not link with the default collision algorithms, you can
-//define BT_EXCLUDE_DEFAULT_COLLISIONALGORITHM_REGISTRATION 
-//in your Bullet library build system
-#ifndef BT_EXCLUDE_DEFAULT_COLLISIONALGORITHM_REGISTRATION
-
-btCollisionDispatcher::btCollisionDispatcher (): 
+btCollisionDispatcher::btCollisionDispatcher (btCollisionConfiguration* collisionConfiguration): 
 	m_count(0),
-	m_useIslands(true)
+	m_useIslands(true),
+	m_collisionConfiguration(collisionConfiguration)
 {
 	int i;

 	setNearCallback(defaultNearCallback);
 	
-	//default CreationFunctions, filling the m_doubleDispatch table
-	m_convexConvexCreateFunc = new btConvexConvexAlgorithm::CreateFunc;
-	m_convexConcaveCreateFunc = new btConvexConcaveCollisionAlgorithm::CreateFunc;
-	m_swappedConvexConcaveCreateFunc = new btConvexConcaveCollisionAlgorithm::SwappedCreateFunc;
-	m_compoundCreateFunc = new btCompoundCollisionAlgorithm::CreateFunc;
-	m_swappedCompoundCreateFunc = new btCompoundCollisionAlgorithm::SwappedCreateFunc;
-	m_emptyCreateFunc = new btEmptyAlgorithm::CreateFunc;
+	
+
+	int maxElements = 16384;
+	m_collisionAlgorithmPoolAllocator = new btPoolAllocator(m_collisionConfiguration->getCollisionAlgorithmMaxElementSize(),m_collisionConfiguration->getCollisionAlgorithmPoolSize());
+
+	m_persistentManifoldPoolAllocator = new btPoolAllocator(sizeof(btPersistentManifold),m_collisionConfiguration->getPersistentManifoldPoolSize());

 	for (i=0;i<MAX_BROADPHASE_COLLISION_TYPES;i++)
 	{
 		for (int j=0;j<MAX_BROADPHASE_COLLISION_TYPES;j++)
 		{
-			m_doubleDispatch[i][j] = internalFindCreateFunc(i,j);
+			m_doubleDispatch[i][j] = m_collisionConfiguration->getCollisionAlgorithmCreateFunc(i,j);
 			assert(m_doubleDispatch[i][j]);
 		}
 	}
@@ -89,8 +60,6 @@ btCollisionDispatcher::btCollisionDispatcher ():
 	
 };

-#endif //BT_EXCLUDE_DEFAULT_COLLISIONALGORITHM_REGISTRATION
-

 void btCollisionDispatcher::registerCollisionCreateFunc(int proxyType0, int proxyType1, btCollisionAlgorithmCreateFunc *createFunc)
 {
@@ -99,12 +68,7 @@ void btCollisionDispatcher::registerCollisionCreateFunc(int proxyType0, int prox

 btCollisionDispatcher::~btCollisionDispatcher()
 {
-	delete m_convexConvexCreateFunc;
-	delete m_convexConcaveCreateFunc;
-	delete m_swappedConvexConcaveCreateFunc;
-	delete m_compoundCreateFunc;
-	delete m_swappedCompoundCreateFunc;
-	delete m_emptyCreateFunc;
+	
 }

 btPersistentManifold*	btCollisionDispatcher::getNewManifold(void* b0,void* b1) 
@@ -117,7 +81,8 @@ btPersistentManifold*	btCollisionDispatcher::getNewManifold(void* b0,void* b1)
 	btCollisionObject* body0 = (btCollisionObject*)b0;
 	btCollisionObject* body1 = (btCollisionObject*)b1;
 	
-	btPersistentManifold* manifold = new btPersistentManifold (body0,body1);
+	void* mem = m_persistentManifoldPoolAllocator->allocate(sizeof(btPersistentManifold));
+	btPersistentManifold* manifold = new(mem) btPersistentManifold (body0,body1,0);
 	m_manifoldsPtr.push_back(manifold);

 	return manifold;
@@ -143,7 +108,9 @@ void btCollisionDispatcher::releaseManifold(btPersistentManifold* manifold)
 	{
 		m_manifoldsPtr.swap(findIndex,m_manifoldsPtr.size()-1);
 		m_manifoldsPtr.pop_back();
-		delete manifold;
+
+		manifold->~btPersistentManifold();
+		m_persistentManifoldPoolAllocator->free(manifold);
 	}
 	
 }
@@ -156,7 +123,8 @@ btCollisionAlgorithm* btCollisionDispatcher::findAlgorithm(btCollisionObject* bo
 #ifdef USE_DISPATCH_REGISTRY_ARRAY
 	
 	btCollisionAlgorithmConstructionInfo ci;
-	ci.m_dispatcher = this;
+
+	ci.m_dispatcher1 = this;
 	ci.m_manifold = sharedManifold;
 	btCollisionAlgorithm* algo = m_doubleDispatch[body0->getCollisionShape()->getShapeType()][body1->getCollisionShape()->getShapeType()]
 	->CreateCollisionAlgorithm(ci,body0,body1);
@@ -167,42 +135,6 @@ btCollisionAlgorithm* btCollisionDispatcher::findAlgorithm(btCollisionObject* bo
 }


-#ifndef BT_EXCLUDE_DEFAULT_COLLISIONALGORITHM_REGISTRATION
-
-btCollisionAlgorithmCreateFunc* btCollisionDispatcher::internalFindCreateFunc(int proxyType0,int proxyType1)
-{
-	
-	if (btBroadphaseProxy::isConvex(proxyType0) && btBroadphaseProxy::isConvex(proxyType1))
-	{
-		return m_convexConvexCreateFunc;
-	}
-
-	if (btBroadphaseProxy::isConvex(proxyType0) && btBroadphaseProxy::isConcave(proxyType1))
-	{
-		return m_convexConcaveCreateFunc;
-	}
-
-	if (btBroadphaseProxy::isConvex(proxyType1) && btBroadphaseProxy::isConcave(proxyType0))
-	{
-		return m_swappedConvexConcaveCreateFunc;
-	}
-
-	if (btBroadphaseProxy::isCompound(proxyType0))
-	{
-		return m_compoundCreateFunc;
-	} else
-	{
-		if (btBroadphaseProxy::isCompound(proxyType1))
-		{
-			return m_swappedCompoundCreateFunc;
-		}
-	}
-
-	//failed to find an algorithm
-	return m_emptyCreateFunc;
-}
-
-#endif //BT_EXCLUDE_DEFAULT_COLLISIONALGORITHM_REGISTRATION


 #ifndef USE_DISPATCH_REGISTRY_ARRAY
@@ -316,13 +248,13 @@ public:
 };


-void	btCollisionDispatcher::dispatchAllCollisionPairs(btOverlappingPairCache* pairCache,btDispatcherInfo& dispatchInfo)
+void	btCollisionDispatcher::dispatchAllCollisionPairs(btOverlappingPairCache* pairCache,btDispatcherInfo& dispatchInfo,btDispatcher* dispatcher)
 {
 	//m_blockedForChanges = true;

 	btCollisionPairCallback	collisionCallback(dispatchInfo,this);

-	pairCache->processAllOverlappingPairs(&collisionCallback);
+	pairCache->processAllOverlappingPairs(&collisionCallback,dispatcher);

 	//m_blockedForChanges = false;

@@ -365,3 +297,14 @@ void btCollisionDispatcher::defaultNearCallback(btBroadphasePair& collisionPair,
 		}

 }
+
+
+void* btCollisionDispatcher::allocateCollisionAlgorithm(int size)
+{
+	return m_collisionAlgorithmPoolAllocator->allocate(size);
+}
+
+void btCollisionDispatcher::freeCollisionAlgorithm(void* ptr)
+{
+	m_collisionAlgorithmPoolAllocator->free(ptr);
+}
--- a/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.h
+++ b/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.h
@@ -26,7 +26,8 @@ subject to the following restrictions:

 class btIDebugDraw;
 class btOverlappingPairCache;
-
+class btPoolAllocator;
+class btCollisionConfiguration;

 #include "btCollisionCreateFunc.h"

@@ -51,21 +52,15 @@ class btCollisionDispatcher : public btDispatcher

 	btNearCallback		m_nearCallback;
 	
+	btPoolAllocator*	m_collisionAlgorithmPoolAllocator;
+
+	btPoolAllocator*	m_persistentManifoldPoolAllocator;
+
 	btCollisionAlgorithmCreateFunc* m_doubleDispatch[MAX_BROADPHASE_COLLISION_TYPES][MAX_BROADPHASE_COLLISION_TYPES];
 	
-	btCollisionAlgorithmCreateFunc* internalFindCreateFunc(int proxyType0,int proxyType1);

-	//default CreationFunctions, filling the m_doubleDispatch table
-	btCollisionAlgorithmCreateFunc*	m_convexConvexCreateFunc;
-	btCollisionAlgorithmCreateFunc*	m_convexConcaveCreateFunc;
-	btCollisionAlgorithmCreateFunc*	m_swappedConvexConcaveCreateFunc;
-	btCollisionAlgorithmCreateFunc*	m_compoundCreateFunc;
-	btCollisionAlgorithmCreateFunc*	m_swappedCompoundCreateFunc;
-	btCollisionAlgorithmCreateFunc*   m_emptyCreateFunc;
+	btCollisionConfiguration*	m_collisionConfiguration;

-#ifndef USE_DISPATCH_REGISTRY_ARRAY
-	btCollisionAlgorithm* internalFindAlgorithm(btCollisionObject* body0,btCollisionObject* body1,btPersistentManifold* sharedManifold = 0);
-#endif //USE_DISPATCH_REGISTRY_ARRAY

 public:

@@ -92,11 +87,7 @@ public:
 		return m_manifoldsPtr[index];
 	}

-	///the default constructor creates/register default collision algorithms, for convex, compound and concave shape support
-	btCollisionDispatcher ();
-
-	///a special constructor that doesn't create/register the default collision algorithms
-	btCollisionDispatcher(bool noDefaultAlgorithms);
+	btCollisionDispatcher (btCollisionConfiguration* collisionConfiguration);

 	virtual ~btCollisionDispatcher();

@@ -114,7 +105,7 @@ public:
 	
 	virtual bool	needsResponse(btCollisionObject* body0,btCollisionObject* body1);
 	
-	virtual void	dispatchAllCollisionPairs(btOverlappingPairCache* pairCache,btDispatcherInfo& dispatchInfo);
+	virtual void	dispatchAllCollisionPairs(btOverlappingPairCache* pairCache,btDispatcherInfo& dispatchInfo,btDispatcher* dispatcher);

 	void	setNearCallback(btNearCallback	nearCallback)
 	{
@@ -129,6 +120,11 @@ public:
 	//by default, Bullet will use this near callback
 	static void  defaultNearCallback(btBroadphasePair& collisionPair, btCollisionDispatcher& dispatcher, btDispatcherInfo& dispatchInfo);

+	virtual	void* allocateCollisionAlgorithm(int size);
+
+	virtual	void freeCollisionAlgorithm(void* ptr);
+
+
 };

 #endif //COLLISION__DISPATCHER_H
--- a/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp
+++ b/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp
@@ -66,8 +66,8 @@ btCollisionWorld::~btCollisionWorld()
 			//
 			// only clear the cached algorithms
 			//
-			getBroadphase()->getOverlappingPairCache()->cleanProxyFromPairs(bp);
-			getBroadphase()->destroyProxy(bp);
+			getBroadphase()->getOverlappingPairCache()->cleanProxyFromPairs(bp,m_dispatcher1);
+			getBroadphase()->destroyProxy(bp,m_dispatcher1);
 		}
 	}

@@ -137,7 +137,7 @@ void	btCollisionWorld::performDiscreteCollisionDetection()
 		m_broadphasePairCache->setAabb(m_collisionObjects[i]->getBroadphaseHandle(),aabbMin,aabbMax);
 	}

-	m_broadphasePairCache->calculateOverlappingPairs();
+	m_broadphasePairCache->calculateOverlappingPairs(m_dispatcher1);
 	
 	END_PROFILE("perform Broadphase Collision Detection");

@@ -145,7 +145,7 @@ void	btCollisionWorld::performDiscreteCollisionDetection()

 	btDispatcher* dispatcher = getDispatcher();
 	if (dispatcher)
-		dispatcher->dispatchAllCollisionPairs(m_broadphasePairCache->getOverlappingPairCache(),dispatchInfo);
+		dispatcher->dispatchAllCollisionPairs(m_broadphasePairCache->getOverlappingPairCache(),dispatchInfo,m_dispatcher1);

 	END_PROFILE("performDiscreteCollisionDetection");

@@ -166,8 +166,8 @@ void	btCollisionWorld::removeCollisionObject(btCollisionObject* collisionObject)
 			//
 			// only clear the cached algorithms
 			//
-			getBroadphase()->getOverlappingPairCache()->cleanProxyFromPairs(bp);
-			getBroadphase()->destroyProxy(bp);
+			getBroadphase()->getOverlappingPairCache()->cleanProxyFromPairs(bp,m_dispatcher1);
+			getBroadphase()->destroyProxy(bp,m_dispatcher1);
 			collisionObject->setBroadphaseHandle(0);
 		}
 	}
@@ -212,19 +212,28 @@ void	btCollisionWorld::objectQuerySingle(const btConvexShape* castShape,const bt

 				btConvexShape* convexShape = (btConvexShape*) collisionShape;
 				btVoronoiSimplexSolver	simplexSolver;
+#define  USE_SUBSIMPLEX_CONVEX_CAST 1
+#ifdef USE_SUBSIMPLEX_CONVEX_CAST
 				btSubsimplexConvexCast convexCaster(castShape,convexShape,&simplexSolver);
+#else
 				//btGjkConvexCast	convexCaster(castShape,convexShape,&simplexSolver);
 				//btContinuousConvexCollision convexCaster(castShape,convexShape,&simplexSolver,0);
+#endif //#USE_SUBSIMPLEX_CONVEX_CAST
 			
 				if (convexCaster.calcTimeOfImpact(rayFromTrans,rayToTrans,colObjWorldTransform,colObjWorldTransform,castResult))
 				{
 					//add hit
 					if (castResult.m_normal.length2() > btScalar(0.0001))
 					{
-						castResult.m_normal.normalize();
+						
 						if (castResult.m_fraction < resultCallback.m_closestHitFraction)
 						{
+#ifdef USE_SUBSIMPLEX_CONVEX_CAST
+							//rotate normal into worldspace
+							castResult.m_normal = rayFromTrans.getBasis() * castResult.m_normal;
+#endif //USE_SUBSIMPLEX_CONVEX_CAST

+							castResult.m_normal.normalize();
 							btCollisionWorld::LocalRayResult localRayResult
 								(
 									collisionObject, 
--- a/src/BulletCollision/CollisionDispatch/btCollisionWorld.h
+++ b/src/BulletCollision/CollisionDispatch/btCollisionWorld.h
@@ -98,7 +98,7 @@ protected:
 public:

 	//this constructor doesn't own the dispatcher and paircache/broadphase
-	btCollisionWorld(btDispatcher* dispatcher,btBroadphaseInterface* broadphasePairCache, int stackSize = 2*1024*1024);
+	btCollisionWorld(btDispatcher* dispatcher,btBroadphaseInterface* broadphasePairCache, int stackSize = 10*1024*1024);

 	virtual ~btCollisionWorld();

--- a/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp
+++ b/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp
@@ -35,7 +35,7 @@ btCompoundCollisionAlgorithm::btCompoundCollisionAlgorithm( const btCollisionAlg
 		btCollisionShape* childShape = compoundShape->getChildShape(i);
 		btCollisionShape* orgShape = colObj->getCollisionShape();
 		colObj->setCollisionShape( childShape );
-		m_childCollisionAlgorithms[i] = ci.m_dispatcher->findAlgorithm(colObj,otherObj);
+		m_childCollisionAlgorithms[i] = ci.m_dispatcher1->findAlgorithm(colObj,otherObj);
 		colObj->setCollisionShape( orgShape );
 	}
 }
@@ -47,7 +47,8 @@ btCompoundCollisionAlgorithm::~btCompoundCollisionAlgorithm()
 	int i;
 	for (i=0;i<numChildren;i++)
 	{
-		delete m_childCollisionAlgorithms[i];
+		m_childCollisionAlgorithms[i]->~btCollisionAlgorithm();
+		m_dispatcher->freeCollisionAlgorithm(m_childCollisionAlgorithms[i]);
 	}
 }

--- a/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h
+++ b/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h
@@ -25,6 +25,7 @@ class btDispatcher;
 #include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
 #include "btCollisionCreateFunc.h"
 #include "LinearMath/btAlignedObjectArray.h"
+class btDispatcher;

 /// btCompoundCollisionAlgorithm  supports collision between CompoundCollisionShapes and other collision shapes
 /// Place holder, not fully implemented yet
@@ -47,7 +48,8 @@ public:
 	{
 		virtual	btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1)
 		{
-			return new btCompoundCollisionAlgorithm(ci,body0,body1,false);
+			void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btCompoundCollisionAlgorithm));
+			return new(mem) btCompoundCollisionAlgorithm(ci,body0,body1,false);
 		}
 	};

@@ -55,7 +57,8 @@ public:
 	{
 		virtual	btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1)
 		{
-			return new btCompoundCollisionAlgorithm(ci,body0,body1,true);
+			void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btCompoundCollisionAlgorithm));
+			return new(mem) btCompoundCollisionAlgorithm(ci,body0,body1,true);
 		}
 	};

--- a/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp
+++ b/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp
@@ -29,7 +29,7 @@ subject to the following restrictions:
 btConvexConcaveCollisionAlgorithm::btConvexConcaveCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1,bool isSwapped)
 : btCollisionAlgorithm(ci),
 m_isSwapped(isSwapped),
-m_btConvexTriangleCallback(ci.m_dispatcher,body0,body1,isSwapped)
+m_btConvexTriangleCallback(ci.m_dispatcher1,body0,body1,isSwapped)
 {
 }

@@ -79,7 +79,7 @@ void btConvexTriangleCallback::processTriangle(btVector3* triangle,int partId, i
 	//aabb filter is already applied!	

 	btCollisionAlgorithmConstructionInfo ci;
-	ci.m_dispatcher = m_dispatcher;
+	ci.m_dispatcher1 = m_dispatcher;

 	btCollisionObject* ob = static_cast<btCollisionObject*>(m_triBody);

@@ -115,7 +115,7 @@ void btConvexTriangleCallback::processTriangle(btVector3* triangle,int partId, i
 		ob->setCollisionShape( &tm );
 		

-		btCollisionAlgorithm* colAlgo = ci.m_dispatcher->findAlgorithm(m_convexBody,m_triBody,m_manifoldPtr);
+		btCollisionAlgorithm* colAlgo = ci.m_dispatcher1->findAlgorithm(m_convexBody,m_triBody,m_manifoldPtr);
 		///this should use the btDispatcher, so the actual registered algorithm is used
 		//		btConvexConvexAlgorithm cvxcvxalgo(m_manifoldPtr,ci,m_convexBody,m_triBody);

@@ -123,7 +123,8 @@ void btConvexTriangleCallback::processTriangle(btVector3* triangle,int partId, i
 	//	cvxcvxalgo.setShapeIdentifiers(-1,-1,partId,triangleIndex);
 //		cvxcvxalgo.processCollision(m_convexBody,m_triBody,*m_dispatchInfoPtr,m_resultOut);
 		colAlgo->processCollision(m_convexBody,m_triBody,*m_dispatchInfoPtr,m_resultOut);
-		delete colAlgo;
+		colAlgo->~btCollisionAlgorithm();
+		ci.m_dispatcher1->freeCollisionAlgorithm(colAlgo);
 		ob->setCollisionShape( tmpShape );

 	}
--- a/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.h
+++ b/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.h
@@ -94,7 +94,8 @@ public:
 	{
 		virtual	btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1)
 		{
-			return new btConvexConcaveCollisionAlgorithm(ci,body0,body1,false);
+			void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btConvexConcaveCollisionAlgorithm));
+			return new(mem) btConvexConcaveCollisionAlgorithm(ci,body0,body1,false);
 		}
 	};

@@ -102,7 +103,8 @@ public:
 	{
 		virtual	btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1)
 		{
-			return new btConvexConcaveCollisionAlgorithm(ci,body0,body1,true);
+			void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btConvexConcaveCollisionAlgorithm));
+			return new(mem) btConvexConcaveCollisionAlgorithm(ci,body0,body1,true);
 		}
 	};

--- a/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h
+++ b/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h
@@ -22,6 +22,7 @@ subject to the following restrictions:
 #include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
 #include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h"
 #include "btCollisionCreateFunc.h"
+#include "btCollisionDispatcher.h"

 class btConvexPenetrationDepthSolver;

@@ -66,7 +67,8 @@ public:

 		virtual	btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1)
 		{
-			return new btConvexConvexAlgorithm(ci.m_manifold,ci,body0,body1,m_simplexSolver,m_pdSolver);
+			void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btConvexConvexAlgorithm));
+			return new(mem) btConvexConvexAlgorithm(ci.m_manifold,ci,body0,body1,m_simplexSolver,m_pdSolver);
 		}
 	};

--- a/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp
+++ b/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp
@@ -20,6 +20,8 @@ subject to the following restrictions:
 #include "BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.h"
 #include "BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.h"
 #include "BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h"
+#include "BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.h"
+#include "BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.h"

 btDefaultCollisionConfiguration::btDefaultCollisionConfiguration()
 :m_persistentManifoldPoolSize(16384),
@@ -35,6 +37,11 @@ m_collisionAlgorithmMaxElementSize(0)
 	m_compoundCreateFunc = new btCompoundCollisionAlgorithm::CreateFunc;
 	m_swappedCompoundCreateFunc = new btCompoundCollisionAlgorithm::SwappedCreateFunc;
 	m_emptyCreateFunc = new btEmptyAlgorithm::CreateFunc;
+	m_sphereSphereCF = new btSphereSphereCollisionAlgorithm::CreateFunc;
+	m_sphereBoxCF = new btSphereBoxCollisionAlgorithm::CreateFunc;
+	m_boxSphereCF = new btSphereBoxCollisionAlgorithm::CreateFunc;
+	m_boxSphereCF->m_swapped = true;
+	

 	///calculate maximum element size, big enough to fit any collision algorithm in the memory pool
 	int maxSize = sizeof(btConvexConvexAlgorithm);
@@ -43,8 +50,8 @@ m_collisionAlgorithmMaxElementSize(0)
 	int maxSize4 = sizeof(btEmptyAlgorithm);
 	
 	m_collisionAlgorithmMaxElementSize = btMax(maxSize,maxSize2);
-	m_collisionAlgorithmMaxElementSize = btMax(elemSize,maxSize3);
-	m_collisionAlgorithmMaxElementSize = btMax(elemSize,maxSize4);
+	m_collisionAlgorithmMaxElementSize = btMax(m_collisionAlgorithmMaxElementSize,maxSize3);
+	m_collisionAlgorithmMaxElementSize = btMax(m_collisionAlgorithmMaxElementSize,maxSize4);

 }

@@ -56,6 +63,9 @@ btDefaultCollisionConfiguration::~btDefaultCollisionConfiguration()
 	delete m_compoundCreateFunc;
 	delete m_swappedCompoundCreateFunc;
 	delete m_emptyCreateFunc;
+	delete m_sphereSphereCF;
+	delete m_sphereBoxCF;
+	delete m_boxSphereCF;

 }

@@ -81,32 +91,52 @@ int	btDefaultCollisionConfiguration::getCollisionAlgorithmMaxElementSize()

 }

-btCollisionAlgorithmCreateFunc* btDefaultCollisionConfiguration::getConvexConvexCollisionCreateFunc()
+
+btCollisionAlgorithmCreateFunc* btDefaultCollisionConfiguration::getCollisionAlgorithmCreateFunc(int proxyType0,int proxyType1)
 {
+	
+
+	if ((proxyType0 == SPHERE_SHAPE_PROXYTYPE) && (proxyType1==SPHERE_SHAPE_PROXYTYPE))
+	{
+		return	m_sphereSphereCF;
+	}
+
+	if ((proxyType0 == SPHERE_SHAPE_PROXYTYPE) && (proxyType1==BOX_SHAPE_PROXYTYPE))
+	{
+		return	m_sphereBoxCF;
+	}
+
+	if ((proxyType0 == BOX_SHAPE_PROXYTYPE ) && (proxyType1==SPHERE_SHAPE_PROXYTYPE))
+	{
+		return	m_boxSphereCF;
+	}
+
+	if (btBroadphaseProxy::isConvex(proxyType0) && btBroadphaseProxy::isConvex(proxyType1))
+	{
 		return m_convexConvexCreateFunc;
-}
+	}

-btCollisionAlgorithmCreateFunc* btDefaultCollisionConfiguration::getConvexConcaveCollisionCreateFunc()
-{
+	if (btBroadphaseProxy::isConvex(proxyType0) && btBroadphaseProxy::isConcave(proxyType1))
+	{
 		return m_convexConcaveCreateFunc;
-}
+	}

-btCollisionAlgorithmCreateFunc* btDefaultCollisionConfiguration::getSwappedConvexConcaveCollisionCreateFunc()
-{
+	if (btBroadphaseProxy::isConvex(proxyType1) && btBroadphaseProxy::isConcave(proxyType0))
+	{
 		return m_swappedConvexConcaveCreateFunc;
-}
+	}

-btCollisionAlgorithmCreateFunc* btDefaultCollisionConfiguration::getCompoundCollisionCreateFunc()
-{
+	if (btBroadphaseProxy::isCompound(proxyType0))
+	{
 		return m_compoundCreateFunc;
-}
-
-btCollisionAlgorithmCreateFunc* btDefaultCollisionConfiguration::getSwappedCompoundCollisionCreateFunc()
-{
+	} else
+	{
+		if (btBroadphaseProxy::isCompound(proxyType1))
+		{
 			return m_swappedCompoundCreateFunc;
-}
+		}
+	}

-btCollisionAlgorithmCreateFunc* btDefaultCollisionConfiguration::getEmptyCollisionCreateFunc()
-{
+	//failed to find an algorithm
 	return m_emptyCreateFunc;
 }
--- a/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h
+++ b/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h
@@ -21,16 +21,16 @@ subject to the following restrictions:
 ///btCollisionConfiguration allows to configure Bullet collision detection
 ///stack allocator size, default collision algorithms and persistent manifold pool size
 ///todo: describe the meaning
-class	btDefaultCollisionConfiguration
+class	btDefaultCollisionConfiguration : public btCollisionConfiguration
 {

 	int	m_persistentManifoldPoolSize;
 	
 	int	m_stackAllocatorSize;

-	int collisionAlgorithmPoolSize;
+	int m_collisionAlgorithmPoolSize;

-	int collisionAlgorithmMaxElementSize;
+	int m_collisionAlgorithmMaxElementSize;

 	//default CreationFunctions, filling the m_doubleDispatch table
 	btCollisionAlgorithmCreateFunc*	m_convexConvexCreateFunc;
@@ -39,6 +39,9 @@ class	btDefaultCollisionConfiguration
 	btCollisionAlgorithmCreateFunc*	m_compoundCreateFunc;
 	btCollisionAlgorithmCreateFunc*	m_swappedCompoundCreateFunc;
 	btCollisionAlgorithmCreateFunc* m_emptyCreateFunc;
+	btCollisionAlgorithmCreateFunc* m_sphereSphereCF;
+	btCollisionAlgorithmCreateFunc* m_sphereBoxCF;
+	btCollisionAlgorithmCreateFunc* m_boxSphereCF;

 public:

@@ -55,18 +58,22 @@ public:

 	virtual int	getCollisionAlgorithmMaxElementSize();

-	virtual btCollisionAlgorithmCreateFunc* getConvexConvexCollisionCreateFunc();
+	btCollisionAlgorithmCreateFunc* getCollisionAlgorithmCreateFunc(int proxyType0,int proxyType1);

-	virtual btCollisionAlgorithmCreateFunc* getConvexConcaveCollisionCreateFunc();
+	void	setStackAllocatorSize(int size)
+	{
+		m_stackAllocatorSize = size;
+	}

-	virtual btCollisionAlgorithmCreateFunc* getSwappedConvexConcaveCollisionCreateFunc();
-
-	virtual btCollisionAlgorithmCreateFunc* getCompoundCollisionCreateFunc();
-
-	virtual btCollisionAlgorithmCreateFunc* getSwappedCompoundCollisionCreateFunc();
-
-	virtual btCollisionAlgorithmCreateFunc* getEmptyCollisionCreateFunc();
+	void	setPersistentManifoldPoolSize(int size)
+	{
+		m_persistentManifoldPoolSize = size;
+	}

+	void	setCollisionAlgorithmPoolSize(int size)
+	{
+		m_collisionAlgorithmPoolSize = size;
+	}
 };

 #endif //BT_DEFAULT_COLLISION_CONFIGURATION
--- a/src/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.h
+++ b/src/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.h
@@ -17,6 +17,7 @@ subject to the following restrictions:
 #define EMPTY_ALGORITH
 #include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h"
 #include "btCollisionCreateFunc.h"
+#include "btCollisionDispatcher.h"

 #define ATTRIBUTE_ALIGNED(a)

@@ -39,7 +40,8 @@ public:
 		{
 			(void)body0;
 			(void)body1;
-			return new btEmptyAlgorithm(ci);
+			void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btEmptyAlgorithm));
+			return new(mem) btEmptyAlgorithm(ci);
 		}
 	};

--- a/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp
+++ b/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp
@@ -85,6 +85,17 @@ void btManifoldResult::addContactPoint(const btVector3& normalOnBInWorld,const b
 	newPt.m_combinedFriction = calculateCombinedFriction(m_body0,m_body1);
 	newPt.m_combinedRestitution = calculateCombinedRestitution(m_body0,m_body1);

+	
+	///todo, check this for any side effects
+	if (insertIndex >= 0)
+	{
+		//const btManifoldPoint& oldPoint = m_manifoldPtr->getContactPoint(insertIndex);
+		m_manifoldPtr->replaceContactPoint(newPt,insertIndex);
+	} else
+	{
+		m_manifoldPtr->AddManifoldPoint(newPt);
+	}
+
 	//User can override friction and/or restitution
 	if (gContactAddedCallback &&
 		//and if either of the two bodies requires custom material
@@ -97,13 +108,5 @@ void btManifoldResult::addContactPoint(const btVector3& normalOnBInWorld,const b
 		(*gContactAddedCallback)(newPt,obj0,m_partId0,m_index0,obj1,m_partId1,m_index1);
 	}

-	if (insertIndex >= 0)
-	{
-		//const btManifoldPoint& oldPoint = m_manifoldPtr->getContactPoint(insertIndex);
-		m_manifoldPtr->replaceContactPoint(newPt,insertIndex);
-	} else
-	{
-		m_manifoldPtr->AddManifoldPoint(newPt);
-	}
 }

--- a/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp
+++ b/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp
@@ -138,19 +138,6 @@ class btPersistentManifoldSortPredicate
 void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher,btCollisionObjectArray& collisionObjects, IslandCallback* callback)
 {

-	
-	
-	/*if (0)
-	{
-		int maxNumManifolds = dispatcher->getNumManifolds();
-		btCollisionDispatcher* colDis = (btCollisionDispatcher*)dispatcher;
-		btPersistentManifold** manifold = colDis->getInternalManifoldPointer();
-		callback->ProcessIsland(&collisionObjects[0],collisionObjects.size(),manifold,maxNumManifolds, 0);
-		return;
-	}
-	*/
-
-
 	BEGIN_PROFILE("islandUnionFindAndHeapSort");
 	
 	//we are going to sort the unionfind array, and store the element id in the size
@@ -247,10 +234,17 @@ void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher,
 		}
 	}

-	btAlignedObjectArray<btPersistentManifold*>  islandmanifold;
+	
 	int i;
 	int maxNumManifolds = dispatcher->getNumManifolds();
+
+#define SPLIT_ISLANDS 1
+#ifdef SPLIT_ISLANDS
+
+	btAlignedObjectArray<btPersistentManifold*>  islandmanifold;
 	islandmanifold.reserve(maxNumManifolds);
+#endif //SPLIT_ISLANDS
+
 	
 	for (i=0;i<maxNumManifolds ;i++)
 	{
@@ -273,19 +267,25 @@ void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher,
 			{
 				colObj0->activate();
 			}
-
-			//filtering for response
+#ifdef SPLIT_ISLANDS
+	//		//filtering for response
 			if (dispatcher->needsResponse(colObj0,colObj1))
 				islandmanifold.push_back(manifold);
+#endif //SPLIT_ISLANDS
 		}
 	}

-	int numManifolds = int (islandmanifold.size());
+#ifndef SPLIT_ISLANDS
+	btPersistentManifold** manifold = dispatcher->getInternalManifoldPointer();
 	
+	callback->ProcessIsland(&collisionObjects[0],collisionObjects.size(),manifold,maxNumManifolds, -1);
+#else
 	// Sort manifolds, based on islands
 	// Sort the vector using predicate and std::sort
 	//std::sort(islandmanifold.begin(), islandmanifold.end(), btPersistentManifoldSortPredicate);

+	int numManifolds = int (islandmanifold.size());
+
 	//we should do radix sort, it it much faster (O(n) instead of O (n log2(n))
 	islandmanifold.heapSort(btPersistentManifoldSortPredicate());

@@ -352,6 +352,6 @@ void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher,

 		islandBodies.resize(0);
 	}
-
+#endif //SPLIT_ISLANDS
 	
 }
--- a/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.h
+++ b/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.h
@@ -20,6 +20,8 @@ subject to the following restrictions:
 #include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
 #include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h"
 class btPersistentManifold;
+#include "btCollisionDispatcher.h"
+
 #include "LinearMath/btVector3.h"

 /// btSphereBoxCollisionAlgorithm  provides sphere-box collision detection.
@@ -48,12 +50,13 @@ public:
 	{
 		virtual	btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1)
 		{
+			void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btSphereBoxCollisionAlgorithm));
 			if (!m_swapped)
 			{
-				return new btSphereBoxCollisionAlgorithm(0,ci,body0,body1,false);
+				return new(mem) btSphereBoxCollisionAlgorithm(0,ci,body0,body1,false);
 			} else
 			{
-				return new btSphereBoxCollisionAlgorithm(0,ci,body0,body1,true);
+				return new(mem) btSphereBoxCollisionAlgorithm(0,ci,body0,body1,true);
 			}
 		}
 	};
--- a/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.h
+++ b/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.h
@@ -19,6 +19,8 @@ subject to the following restrictions:
 #include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h"
 #include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
 #include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h"
+#include "btCollisionDispatcher.h"
+
 class btPersistentManifold;

 /// btSphereSphereCollisionAlgorithm  provides sphere-sphere collision detection.
@@ -46,7 +48,8 @@ public:
 	{
 		virtual	btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1)
 		{
-			return new btSphereSphereCollisionAlgorithm(0,ci,body0,body1);
+			void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btSphereSphereCollisionAlgorithm));
+			return new(mem) btSphereSphereCollisionAlgorithm(0,ci,body0,body1);
 		}
 	};

--- a/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.h
+++ b/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.h
@@ -20,6 +20,7 @@ subject to the following restrictions:
 #include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
 #include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h"
 class btPersistentManifold;
+#include "btCollisionDispatcher.h"

 /// btSphereSphereCollisionAlgorithm  provides sphere-sphere collision detection.
 /// Other features are frame-coherency (persistent data) and collision response.
@@ -49,7 +50,9 @@ public:
 		virtual	btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1)
 		{
 			
-				return new btSphereTriangleCollisionAlgorithm(ci.m_manifold,ci,body0,body1,m_swapped);
+			void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btSphereTriangleCollisionAlgorithm));
+
+			return new(mem) btSphereTriangleCollisionAlgorithm(ci.m_manifold,ci,body0,body1,m_swapped);
 		}
 	};

--- a/src/BulletCollision/CollisionShapes/btOptimizedBvh.cpp
+++ b/src/BulletCollision/CollisionShapes/btOptimizedBvh.cpp
@@ -19,10 +19,31 @@ subject to the following restrictions:
 #include "LinearMath/btIDebugDraw.h"


+inline bool testQuantizedAabbAgainstQuantizedAabb2(unsigned short int* aabbMin1,unsigned short int* aabbMax1,const unsigned short int* aabbMin2,const unsigned short int* aabbMax2) 
+{
+	bool overlap = true;
+	overlap = (aabbMin1[0] > aabbMax2[0] || aabbMax1[0] < aabbMin2[0]) ? false : overlap;
+	overlap = (aabbMin1[2] > aabbMax2[2] || aabbMax1[2] < aabbMin2[2]) ? false : overlap;
+	overlap = (aabbMin1[1] > aabbMax2[1] || aabbMax1[1] < aabbMin2[1]) ? false : overlap;
+	return overlap;
+}
+
+
+
+///Branch-free version of quantized aabb versus quantized aabb
+inline unsigned testQuantizedAabbAgainstQuantizedAabb(unsigned short int* aabbMin1,unsigned short int* aabbMax1,const unsigned short int* aabbMin2,const unsigned short int* aabbMax2)
+{		
+	return btSelect((unsigned)((aabbMin1[0] <= aabbMax2[0]) & (aabbMax1[0] >= aabbMin2[0])
+		& (aabbMin1[2] <= aabbMax2[2]) & (aabbMax1[2] >= aabbMin2[2])
+		& (aabbMin1[1] <= aabbMax2[1]) & (aabbMax1[1] >= aabbMin2[1])),
+		1, 0);
+}
+
+

 btOptimizedBvh::btOptimizedBvh() : m_useQuantization(false), 
 					m_traversalMode(TRAVERSAL_STACKLESS_CACHE_FRIENDLY)
-					//m_traversalMode(TRAVERSAL_STACKLESS)
+//					m_traversalMode(TRAVERSAL_STACKLESS)
 					//m_traversalMode(TRAVERSAL_RECURSIVE)
 { 

@@ -170,6 +191,9 @@ void btOptimizedBvh::build(btStridingMeshInterface* triangles, bool useQuantized
 		subtree.m_rootNodeIndex = 0;
 		subtree.m_subtreeSize = m_quantizedContiguousNodes[0].isLeafNode() ? 1 : m_quantizedContiguousNodes[0].getEscapeIndex();
 	}
+
+	m_leafNodes.clear();
+	m_quantizedLeafNodes.clear();
 }


@@ -201,7 +225,7 @@ void	btOptimizedBvh::refitPartial(btStridingMeshInterface* meshInterface,const b
 	{
 		btBvhSubtreeInfo& subtree = m_SubtreeHeaders[i];

-		bool overlap = testQuantizedAabbAgainstQuantizedAabb(quantizedQueryAabbMin,quantizedQueryAabbMax,subtree.m_quantizedAabbMin,subtree.m_quantizedAabbMax);
+		unsigned int overlap = testQuantizedAabbAgainstQuantizedAabb(quantizedQueryAabbMin,quantizedQueryAabbMax,subtree.m_quantizedAabbMin,subtree.m_quantizedAabbMax);
 		if (overlap)
 		{
 			updateBvhNodes(meshInterface,subtree.m_rootNodeIndex,subtree.m_rootNodeIndex+subtree.m_subtreeSize,i);
@@ -668,7 +692,7 @@ void btOptimizedBvh::walkRecursiveQuantizedTreeAgainstQueryAabb(const btQuantize
 {
 	btAssert(m_useQuantization);
 	
-	bool aabbOverlap, isLeafNode;
+	unsigned int aabbOverlap, isLeafNode;

 	aabbOverlap = testQuantizedAabbAgainstQuantizedAabb(quantizedQueryAabbMin,quantizedQueryAabbMax,currentNode->m_quantizedAabbMin,currentNode->m_quantizedAabbMax);
 	isLeafNode = currentNode->isLeafNode();
@@ -707,7 +731,7 @@ void	btOptimizedBvh::walkStacklessQuantizedTree(btNodeOverlapCallback* nodeCallb
 	const btQuantizedBvhNode* rootNode = &m_quantizedContiguousNodes[startNodeIndex];
 	int escapeIndex;
 	
-	bool aabbOverlap, isLeafNode;
+	unsigned int aabbOverlap, isLeafNode;

 	while (curIndex < endNodeIndex)
 	{
@@ -768,7 +792,7 @@ void	btOptimizedBvh::walkStacklessQuantizedTreeCacheFriendly(btNodeOverlapCallba
 	{
 		const btBvhSubtreeInfo& subtree = m_SubtreeHeaders[i];

-		bool overlap = testQuantizedAabbAgainstQuantizedAabb(quantizedQueryAabbMin,quantizedQueryAabbMax,subtree.m_quantizedAabbMin,subtree.m_quantizedAabbMax);
+		unsigned int overlap = testQuantizedAabbAgainstQuantizedAabb(quantizedQueryAabbMin,quantizedQueryAabbMax,subtree.m_quantizedAabbMin,subtree.m_quantizedAabbMax);
 		if (overlap)
 		{
 			walkStacklessQuantizedTree(nodeCallback,quantizedQueryAabbMin,quantizedQueryAabbMax,
--- a/src/BulletCollision/CollisionShapes/btOptimizedBvh.h
+++ b/src/BulletCollision/CollisionShapes/btOptimizedBvh.h
@@ -30,6 +30,7 @@ class btStridingMeshInterface;
 #define MAX_SUBTREE_SIZE_IN_BYTES  2048


+
 ///btQuantizedBvhNode is a compressed aabb node, 16 bytes.
 ///Node can be used for leafnode or internal node. Leafnodes can point to 32-bit triangle index (non-negative range).
 ATTRIBUTE_ALIGNED16	(struct) btQuantizedBvhNode
@@ -275,14 +276,7 @@ protected:
 	void	walkRecursiveQuantizedTreeAgainstQuantizedTree(const btQuantizedBvhNode* treeNodeA,const btQuantizedBvhNode* treeNodeB,btNodeOverlapCallback* nodeCallback) const;
 	

-	inline bool testQuantizedAabbAgainstQuantizedAabb(unsigned short int* aabbMin1,unsigned short int* aabbMax1,const unsigned short int* aabbMin2,const unsigned short int* aabbMax2) const
-	{
-		bool overlap = true;
-		overlap = (aabbMin1[0] > aabbMax2[0] || aabbMax1[0] < aabbMin2[0]) ? false : overlap;
-		overlap = (aabbMin1[2] > aabbMax2[2] || aabbMax1[2] < aabbMin2[2]) ? false : overlap;
-		overlap = (aabbMin1[1] > aabbMax2[1] || aabbMax1[1] < aabbMin2[1]) ? false : overlap;
-		return overlap;
-	}
+

 	void	updateSubtreeHeaders(int leftChildNodexIndex,int rightChildNodexIndex);

--- a/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h
+++ b/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h
@@ -59,7 +59,7 @@ public:

 	btPersistentManifold();

-	btPersistentManifold(void* body0,void* body1)
+	btPersistentManifold(void* body0,void* body1,int bla)
 		: m_body0(body0),m_body1(body1),m_cachedPoints(0)
 	{
 	}
--- a/src/BulletDynamics/ConstraintSolver/btConstraintSolver.h
+++ b/src/BulletDynamics/ConstraintSolver/btConstraintSolver.h
@@ -26,7 +26,7 @@ struct btContactSolverInfo;
 struct btBroadphaseProxy;
 class btIDebugDraw;
 class btStackAlloc;
-
+class	btDispatcher;
 /// btConstraintSolver provides solver interface
 class btConstraintSolver
 {
@@ -38,7 +38,7 @@ public:
 	virtual void prepareSolve (int numBodies, int numManifolds) {;}

 	///solve a group of constraints
-	virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints, const btContactSolverInfo& info,class btIDebugDraw* debugDrawer, btStackAlloc* stackAlloc) = 0;
+	virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints, const btContactSolverInfo& info,class btIDebugDraw* debugDrawer, btStackAlloc* stackAlloc,btDispatcher* dispatcher) = 0;

 	virtual void allSolved (const btContactSolverInfo& info,class btIDebugDraw* debugDrawer, btStackAlloc* stackAlloc) {;}

--- a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp
+++ b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp
@@ -29,7 +29,7 @@ subject to the following restrictions:
 #include "LinearMath/btQuickprof.h"
 #include "btSolverBody.h"
 #include "btSolverConstraint.h"
-
+#include "BulletCollision/BroadphaseCollision/btDispatcher.h"
 #include "LinearMath/btAlignedObjectArray.h"

 #ifdef USE_PROFILE
@@ -138,6 +138,7 @@ void	initSolverBody(btSolverBody* solverBody, btRigidBody* rigidbody)
 	solverBody->m_invMass = rigidbody->getInvMass();
 	solverBody->m_linearVelocity = rigidbody->getLinearVelocity();
 	solverBody->m_originalBody = rigidbody;
+	btAssert(rigidbody);
 	solverBody->m_angularFactor = rigidbody->getAngularFactor();
 }

@@ -333,12 +334,10 @@ btScalar resolveSingleFrictionCacheFriendly(

 #endif //NO_FRICTION_TANGENTIALS

-btAlignedObjectArray<btSolverBody>	tmpSolverBodyPool;
-btAlignedObjectArray<btSolverConstraint>	tmpSolverConstraintPool;
-btAlignedObjectArray<btSolverConstraint>	tmpSolverFrictionConstraintPool;


-btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc)
+
+btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc,btDispatcher* dispatcher)
 {
 	(void)stackAlloc;
 	(void)debugDrawer;
@@ -351,15 +350,36 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisio

 	BEGIN_PROFILE("refreshManifolds");

+	int numActiveBodies = 0;
+
+	for (int i=0;i<numBodies;i++)
+	{
+		btRigidBody* rb = btRigidBody::upcast(bodies[i]);
+		if (rb && 	(rb->getIslandTag() >= 0))
+		{
+			numActiveBodies++;
+		}
+	}
+
+	int numActiveManifolds = 0;
+	int totalContacts = 0;
+
 	int i;
 	for (i=0;i<numManifolds;i++)
 	{
 		btPersistentManifold* manifold = manifoldPtr[i];
 		btRigidBody* rb0 = (btRigidBody*)manifold->getBody0();
 		btRigidBody* rb1 = (btRigidBody*)manifold->getBody1();
-
+		 if (((rb0) && rb0->getActivationState() != ISLAND_SLEEPING) ||
+			((rb1) && rb1->getActivationState() != ISLAND_SLEEPING))
+		{
+			if (dispatcher->needsResponse(rb0,rb1))
+			{
 				manifold->refreshContactPoints(rb0->getCenterOfMassTransform(),rb1->getCenterOfMassTransform());
-
+				totalContacts += manifold->getNumContacts();
+				numActiveManifolds++;
+			}
+		}
 	}
 	
 	END_PROFILE("refreshManifolds");
@@ -367,24 +387,33 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisio

 	BEGIN_PROFILE("gatherSolverData");

+	btBlock*					sablock;
+	sablock = stackAlloc->beginBlock();
+
+
+	int tmpSolverBodyPoolSize = 0;
+	int mem1 = sizeof(btSolverBody) * numActiveBodies;
+	btSolverBody* tmpSolverBodyPool = (btSolverBody*) stackAlloc->allocate(sizeof(btSolverBody) * numActiveBodies*2);
+	int tmpSolverConstraintPoolSize = 0;
+	int mem2 = sizeof(btSolverConstraint)*numActiveManifolds;
+
+	btSolverConstraint* tmpSolverConstraintPool = (btSolverConstraint*) stackAlloc->allocate(sizeof(btSolverConstraint)*totalContacts);
+
+	int tmpSolverFrictionConstraintPoolSize = 0;
+	btSolverConstraint*	tmpSolverFrictionConstraintPool = (btSolverConstraint*) stackAlloc->allocate(sizeof(btSolverConstraint)*totalContacts*2);
+
 	//int sizeofSB = sizeof(btSolverBody);
 	//int sizeofSC = sizeof(btSolverConstraint);


 	//if (1)
 	{
-		//if m_stackAlloc, try to pack bodies/constraints to speed up solving
-//		btBlock*					sablock;
-//		sablock = stackAlloc->beginBlock();

-	//	int memsize = 16;
-//		unsigned char* stackMemory = stackAlloc->allocate(memsize);

 		
 		//todo: use stack allocator for this temp memory
 		int minReservation = numManifolds*2;

-		tmpSolverBodyPool.reserve(minReservation);

 		{
 			for (int i=0;i<numBodies;i++)
@@ -393,8 +422,8 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisio
 				if (rb && 	(rb->getIslandTag() >= 0))
 				{
 					btAssert(rb->getCompanionId() < 0);
-					int solverBodyId = tmpSolverBodyPool.size();
-					btSolverBody& solverBody = tmpSolverBodyPool.expand();
+					int solverBodyId = tmpSolverBodyPoolSize;
+					btSolverBody& solverBody = tmpSolverBodyPool[tmpSolverBodyPoolSize++];
 					initSolverBody(&solverBody,rb);
 					rb->setCompanionId(solverBodyId);
 				} 
@@ -402,8 +431,6 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisio
 		}

 		
-		tmpSolverConstraintPool.reserve(minReservation);
-		tmpSolverFrictionConstraintPool.reserve(minReservation);
 		{
 			int i;

@@ -413,6 +440,12 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisio
 				btRigidBody* rb0 = (btRigidBody*)manifold->getBody0();
 				btRigidBody* rb1 = (btRigidBody*)manifold->getBody1();

+				if (((rb0) && rb0->getActivationState() != ISLAND_SLEEPING) ||
+					((rb1) && rb1->getActivationState() != ISLAND_SLEEPING))
+				{
+					if (dispatcher->needsResponse(rb0,rb1))
+					{
+

 						int solverBodyIdA=-1;
 						int solverBodyIdB=-1;
@@ -428,8 +461,8 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisio
 							} else
 							{
 								//create a static body
-						solverBodyIdA = tmpSolverBodyPool.size();
-						btSolverBody& solverBody = tmpSolverBodyPool.expand();
+								solverBodyIdA = tmpSolverBodyPoolSize;
+								btSolverBody& solverBody = tmpSolverBodyPool[tmpSolverBodyPoolSize++];
 								initSolverBody(&solverBody,rb0);
 							}

@@ -439,8 +472,8 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisio
 							} else
 							{
 								//create a static body
-						solverBodyIdB = tmpSolverBodyPool.size();
-						btSolverBody& solverBody = tmpSolverBodyPool.expand();
+								solverBodyIdB = tmpSolverBodyPoolSize;
+								btSolverBody& solverBody = tmpSolverBodyPool[tmpSolverBodyPoolSize++];
 								initSolverBody(&solverBody,rb1);
 							}
 						}
@@ -450,7 +483,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisio

 							btManifoldPoint& cp = manifold->getContactPoint(j);

-					int frictionIndex = tmpSolverConstraintPool.size();
+							int frictionIndex = tmpSolverConstraintPoolSize;

 							if (cp.getDistance() <= btScalar(0.))
 							{
@@ -465,7 +498,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisio
 								btScalar relaxation = 1.f;

 								{
-							btSolverConstraint& solverConstraint = tmpSolverConstraintPool.expand();
+									btSolverConstraint& solverConstraint = tmpSolverConstraintPool[tmpSolverConstraintPoolSize++];

 									solverConstraint.m_solverBodyIdA = solverBodyIdA;
 									solverConstraint.m_solverBodyIdB = solverBodyIdB;
@@ -529,7 +562,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisio
 								btPlaneSpace1(cp.m_normalWorldOnB,frictionTangential0a,frictionTangential1b);

 								{
-							btSolverConstraint& solverConstraint = tmpSolverFrictionConstraintPool.expand();
+									btSolverConstraint& solverConstraint = tmpSolverFrictionConstraintPool[tmpSolverFrictionConstraintPoolSize++];
 									solverConstraint.m_contactNormal = frictionTangential0a;

 									solverConstraint.m_solverBodyIdA = solverBodyIdA;
@@ -563,7 +596,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisio

 								{

-							btSolverConstraint& solverConstraint = tmpSolverFrictionConstraintPool.expand();
+									btSolverConstraint& solverConstraint = tmpSolverFrictionConstraintPool[tmpSolverFrictionConstraintPoolSize++];
 									solverConstraint.m_contactNormal = frictionTangential1b;

 									solverConstraint.m_solverBodyIdA = solverBodyIdA;
@@ -596,6 +629,9 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisio
 						}
 					}
 				}
+
+			}
+		}
 	}
 	END_PROFILE("gatherSolverData");

@@ -612,15 +648,16 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisio
 		}
 	}
 	
-	btAlignedObjectArray<int>	gOrderTmpConstraintPool;
-	btAlignedObjectArray<int>	gOrderFrictionConstraintPool;
+	int numConstraintPool = tmpSolverConstraintPoolSize;
+	int numFrictionPool = tmpSolverFrictionConstraintPoolSize;

-	int numConstraintPool = tmpSolverConstraintPool.size();
-	int numFrictionPool = tmpSolverFrictionConstraintPool.size();
+	///use the stack allocator for temporarily memory
+	
+	int gOrderTmpConstraintPoolSize = numConstraintPool;
+	int*	gOrderTmpConstraintPool = (int*) stackAlloc->allocate(sizeof(int)*numConstraintPool);
+	int gOrderFrictionConstraintPoolSize = numFrictionPool;
+	int*	gOrderFrictionConstraintPool = (int*) stackAlloc->allocate(sizeof(int)*numFrictionPool);

-	///todo: use stack allocator for such temporarily memory, same for solver bodies/constraints
-	gOrderTmpConstraintPool.resize(numConstraintPool);
-	gOrderFrictionConstraintPool.resize(numFrictionPool);
 	{
 		int i;
 		for (i=0;i<numConstraintPool;i++)
@@ -695,7 +732,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisio
 			}

 			{
-				int numPoolConstraints = tmpSolverConstraintPool.size();
+				int numPoolConstraints = tmpSolverConstraintPoolSize;
 				for (j=0;j<numPoolConstraints;j++)
 				{
 					btSolverConstraint& solveManifold = tmpSolverConstraintPool[gOrderTmpConstraintPool[j]];
@@ -705,7 +742,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisio
 			}

 			{
-				 int numFrictionPoolConstraints = tmpSolverFrictionConstraintPool.size();
+				 int numFrictionPoolConstraints = tmpSolverFrictionConstraintPoolSize;
 				for (j=0;j<numFrictionPoolConstraints;j++)
 				{
 					btSolverConstraint& solveManifold = tmpSolverFrictionConstraintPool[gOrderFrictionConstraintPool[j]];
@@ -721,36 +758,22 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisio
 		}
 	}
 		
-	for ( i=0;i<tmpSolverBodyPool.size();i++)
+	for ( i=0;i<tmpSolverBodyPoolSize;i++)
 	{
 		tmpSolverBodyPool[i].writebackVelocity();
 	}

 	END_PROFILE("solveConstraints");

-//	printf("tmpSolverConstraintPool.size() = %i\n",tmpSolverConstraintPool.size());
-
-/*
-	printf("tmpSolverBodyPool.size() = %i\n",tmpSolverBodyPool.size());
-	printf("tmpSolverConstraintPool.size() = %i\n",tmpSolverConstraintPool.size());
-	printf("tmpSolverFrictionConstraintPool.size() = %i\n",tmpSolverFrictionConstraintPool.size());
-
-	
-	printf("tmpSolverBodyPool.capacity() = %i\n",tmpSolverBodyPool.capacity());
-	printf("tmpSolverConstraintPool.capacity() = %i\n",tmpSolverConstraintPool.capacity());
-	printf("tmpSolverFrictionConstraintPool.capacity() = %i\n",tmpSolverFrictionConstraintPool.capacity());
-*/
-
-	tmpSolverBodyPool.resize(0);
-	tmpSolverConstraintPool.resize(0);
-	tmpSolverFrictionConstraintPool.resize(0);
+	///release stack memory

+	stackAlloc->endBlock(sablock);

 	return 0.f;
 }

 /// btSequentialImpulseConstraintSolver Sequentially applies impulses
-btScalar btSequentialImpulseConstraintSolver::solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc)
+btScalar btSequentialImpulseConstraintSolver::solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc,btDispatcher* dispatcher)
 {

 	if (getSolverMode() & SOLVER_CACHE_FRIENDLY)
@@ -759,7 +782,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroup(btCollisionObject** bod
 		//btSimpleDynamicsWorld needs to switch off SOLVER_CACHE_FRIENDLY
 		btAssert(bodies);
 		btAssert(numBodies);
-		return solveGroupCacheFriendly(bodies,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer,stackAlloc);
+		return solveGroupCacheFriendly(bodies,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer,stackAlloc,dispatcher);
 	}


--- a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h
+++ b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h
@@ -70,9 +70,9 @@ public:

 	virtual ~btSequentialImpulseConstraintSolver() {}
 	
-	virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btStackAlloc* stackAlloc);
+	virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btStackAlloc* stackAlloc,btDispatcher* dispatcher);

-	virtual btScalar solveGroupCacheFriendly(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc);
+	virtual btScalar solveGroupCacheFriendly(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc,btDispatcher* dispatcher);

 	///clear internal cached data and reset random seed
 	virtual	void	reset();
--- a/src/BulletDynamics/Dynamics/Bullet-C-API.cpp
+++ b/src/BulletDynamics/Dynamics/Bullet-C-API.cpp
@@ -65,7 +65,8 @@ void		plDeletePhysicsSdk(plPhysicsSdkHandle	physicsSdk)
 plDynamicsWorldHandle plCreateDynamicsWorld(plPhysicsSdkHandle physicsSdkHandle)
 {
 	btPhysicsSdk* physicsSdk = reinterpret_cast<btPhysicsSdk*>(physicsSdkHandle);
-	btDispatcher*				dispatcher = new btCollisionDispatcher();
+	btDefaultCollisionConfiguration* collisionConfiguration = new btDefaultCollisionConfiguration();
+	btDispatcher*				dispatcher = new btCollisionDispatcher(collisionConfiguration);
 	btBroadphaseInterface*		pairCache = new btAxisSweep3(physicsSdk->m_worldAabbMin,physicsSdk->m_worldAabbMax);
 	btConstraintSolver*			constraintSolver = new btSequentialImpulseConstraintSolver();

--- a/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp
+++ b/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp
@@ -452,7 +452,7 @@ void	btDiscreteDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
 		int						m_numConstraints;
 		btIDebugDraw*			m_debugDrawer;
 		btStackAlloc*			m_stackAlloc;
-
+		btDispatcher*			m_dispatcher;

 		InplaceSolverIslandCallback(
 			btContactSolverInfo& solverInfo,
@@ -460,13 +460,15 @@ void	btDiscreteDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
 			btTypedConstraint** sortedConstraints,
 			int	numConstraints,
 			btIDebugDraw*	debugDrawer,
-			btStackAlloc*			stackAlloc)
+			btStackAlloc*			stackAlloc,
+			btDispatcher* dispatcher)
 			:m_solverInfo(solverInfo),
 			m_solver(solver),
 			m_sortedConstraints(sortedConstraints),
 			m_numConstraints(numConstraints),
 			m_debugDrawer(debugDrawer),
-			m_stackAlloc(stackAlloc)
+			m_stackAlloc(stackAlloc),
+			m_dispatcher(dispatcher)
 		{

 		}
@@ -478,6 +480,12 @@ void	btDiscreteDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
 			return *this;
 		}
 		virtual	void	ProcessIsland(btCollisionObject** bodies,int numBodies,btPersistentManifold**	manifolds,int numManifolds, int islandId)
+		{
+			if (islandId<0)
+			{
+				///we don't split islands, so all constraints/contact manifolds/bodies are passed into the solver regardless the island id
+				m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,&m_sortedConstraints[0],m_numConstraints,m_solverInfo,m_debugDrawer,m_stackAlloc,m_dispatcher);
+			} else
 			{
 					//also add all non-contact constraints/joints for this island
 				btTypedConstraint** startConstraint = 0;
@@ -502,7 +510,9 @@ void	btDiscreteDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
 					}
 				}

-			m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,startConstraint,numCurConstraints,m_solverInfo,m_debugDrawer,m_stackAlloc);
+				m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,startConstraint,numCurConstraints,m_solverInfo,m_debugDrawer,m_stackAlloc,m_dispatcher);
+		
+			}
 		}

 	};
@@ -524,7 +534,7 @@ void	btDiscreteDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
 	
 	btTypedConstraint** constraintsPtr = getNumConstraints() ? &sortedConstraints[0] : 0;
 	
-	InplaceSolverIslandCallback	solverCallback(	solverInfo,	m_constraintSolver, constraintsPtr,sortedConstraints.size(),	m_debugDrawer,m_stackAlloc);
+	InplaceSolverIslandCallback	solverCallback(	solverInfo,	m_constraintSolver, constraintsPtr,sortedConstraints.size(),	m_debugDrawer,m_stackAlloc,m_dispatcher1);
 	
 	m_constraintSolver->prepareSolve(getCollisionWorld()->getNumCollisionObjects(), getCollisionWorld()->getDispatcher()->getNumManifolds());
 	
--- a/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp
+++ b/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp
@@ -75,7 +75,7 @@ int		btSimpleDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps, b
 		btContactSolverInfo infoGlobal;
 		infoGlobal.m_timeStep = timeStep;
 		m_constraintSolver->prepareSolve(0,numManifolds);
-		m_constraintSolver->solveGroup(0,0,manifoldPtr, numManifolds,0,0,infoGlobal,m_debugDrawer, m_stackAlloc);
+		m_constraintSolver->solveGroup(0,0,manifoldPtr, numManifolds,0,0,infoGlobal,m_debugDrawer, m_stackAlloc,m_dispatcher1);
 		m_constraintSolver->allSolved(infoGlobal,m_debugDrawer, m_stackAlloc);
 	}

--- a/src/LinearMath/btAlignedAllocator.cpp
+++ b/src/LinearMath/btAlignedAllocator.cpp
@@ -21,11 +21,14 @@ subject to the following restrictions:
 #include <malloc.h>
 void*	btAlignedAlloc	(int size, int alignment)
 {
-	return _aligned_malloc(size,alignment);
+	void* ptr = _aligned_malloc(size,alignment);
+//	printf("btAlignedAlloc %d, %x\n",size,ptr);
+	return ptr;
 }

 void	btAlignedFree	(void* ptr)
 {
+//	printf("btAlignedFree %x\n",ptr);
 	_aligned_free(ptr);
 }

--- a/src/LinearMath/btScalar.h
+++ b/src/LinearMath/btScalar.h
@@ -23,6 +23,51 @@ subject to the following restrictions:
 #include <cfloat>
 #include <float.h>

+
+#ifdef WIN32
+
+//added __cdecl, thanks Jack
+
+// default new and delete overrides that guarantee 16 byte alignment and zero allocated memory
+void* __cdecl operator new(size_t sz) throw();
+void* __cdecl operator new[](size_t sz) throw();
+void __cdecl operator delete(void* m) throw();
+void __cdecl operator delete[](void* m) throw();
+
+#include <malloc.h>
+#include <stdio.h>
+#define BULLET_ALIGNED_NEW_AND_DELETE \
+\
+inline void* operator new(size_t sz) throw()	\
+{												\
+	printf("new %d\n",sz);						\
+	void* mem = _aligned_malloc(sz + 64, 16);	\
+	return mem;									\
+}												\
+												\
+inline void* operator new[](size_t sz) throw()	\
+{												\
+	printf("new[] %d\n",sz);					\
+	void* mem = _aligned_malloc(sz + 64, 16);	\
+	return mem;									\
+}												\
+												\
+inline void operator delete(void* m) throw()	\
+{					\
+printf("delete %x\n",m);						\
+	if (m == 0)									\
+		return;									\
+	_aligned_free(m);							\
+}												\
+												\
+inline void operator delete[](void* m) throw()	\
+{												\
+	printf("delete[] %x\n",m); \
+_aligned_free(m);							\
+}												\
+
+#endif
+
 #ifdef WIN32

 		#if defined(__MINGW32__) || defined(__CYGWIN__) || (defined (_MSC_VER) && _MSC_VER < 1300)
@@ -163,14 +208,6 @@ SIMD_FORCE_INLINE bool	btGreaterEqual (btScalar a, btScalar eps) {
 	return (!((a) <= eps));
 }

-/*SIMD_FORCE_INLINE btScalar btCos(btScalar x) { return cosf(x); }
-SIMD_FORCE_INLINE btScalar btSin(btScalar x) { return sinf(x); }
-SIMD_FORCE_INLINE btScalar btTan(btScalar x) { return tanf(x); }
-SIMD_FORCE_INLINE btScalar btAcos(btScalar x) { return acosf(x); }
-SIMD_FORCE_INLINE btScalar btAsin(btScalar x) { return asinf(x); }
-SIMD_FORCE_INLINE btScalar btAtan(btScalar x) { return atanf(x); }
-SIMD_FORCE_INLINE btScalar btAtan2(btScalar x, btScalar y) { return atan2f(x, y); }
-*/

 SIMD_FORCE_INLINE int       btIsNegative(btScalar x) {
    return x < btScalar(0.0) ? 1 : 0;
@@ -189,4 +226,33 @@ SIMD_FORCE_INLINE btScalar btFsel(btScalar a, btScalar b, btScalar c)
 #endif
 #define btFsels(a,b,c) (btScalar)btFsel(a,b,c)

+
+///btSelect avoids branches, which makes performance much better for consoles like Playstation 3 and XBox 360
+///Thanks Phil Knight. See also http://www.cellperformance.com/articles/2006/04/more_techniques_for_eliminatin_1.html
+SIMD_FORCE_INLINE unsigned btSelect(unsigned condition, unsigned valueIfConditionNonZero, unsigned valueIfConditionZero) 
+{
+    // Set testNz to 0xFFFFFFFF if condition is nonzero, 0x00000000 if condition is zero
+    // Rely on positive value or'ed with its negative having sign bit on
+    // and zero value or'ed with its negative (which is still zero) having sign bit off 
+    // Use arithmetic shift right, shifting the sign bit through all 32 bits
+    unsigned testNz = (unsigned)(((int)condition | -(int)condition) >> 31);
+    unsigned testEqz = ~testNz;
+    return ((valueIfConditionNonZero & testNz) | (valueIfConditionZero & testEqz)); 
+}
+SIMD_FORCE_INLINE int btSelect(unsigned condition, int valueIfConditionNonZero, int valueIfConditionZero)
+{
+    unsigned testNz = (unsigned)(((int)condition | -(int)condition) >> 31);
+    unsigned testEqz = ~testNz; 
+    return ((valueIfConditionNonZero & testNz) | (valueIfConditionZero & testEqz));
+}
+SIMD_FORCE_INLINE float btSelect(unsigned condition, float valueIfConditionNonZero, float valueIfConditionZero)
+{
+#ifdef BT_HAVE_NATIVE_FSEL
+    return (float)btFsel((btScalar)condition - btScalar(1.0f), valueIfConditionNonZero, valueIfConditionZero);
+#else
+    return (condition != 0) ? valueIfConditionNonZero : valueIfConditionZero; 
+#endif
+}
+
+
 #endif //SIMD___SCALAR_H
--- a/src/btBulletCollisionCommon.h
+++ b/src/btBulletCollisionCommon.h
@@ -44,6 +44,7 @@ subject to the following restrictions:
 ///Narrowphase Collision Detector
 #include "BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.h"
 #include "BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.h"
+#include "BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h"

 ///Dispatching and generation of collision pairs (broadphase)
 #include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"