From 0405ce278a50b2f3b9fce5dc2c27b2acdc0db145 Mon Sep 17 00:00:00 2001
From: ejcoumans <ejcoumans@08e121b0-ae19-0410-a57b-3be3395fd4fd>
Date: Thu, 11 Oct 2007 03:17:54 +0000
Subject: [PATCH] use size_t instead of int, for allocator added hashtable
 based PairManager, thanks Pierre Terdiman and Erin Catto improved friction in
 'cachefriendly' solver moved 'refreshcontactpoints' into collision detection,
 instead of solver avoid linear search for contact manifolds, by storing an
 index ignore margin for sphere shape (its entire radius is already margin)
 avoid alignment checks in BVH serialization, they don't compile on 64-bit
 architectures made 'bomb' box more heavy

---
 Demos/OpenGL/DemoApplication.cpp              |   2 +-
 Demos/StressTest/StressTestDemo.cpp           | 377 +++++++++++++++++
 Demos/StressTest/StressTestDemo.h             |  99 +++++
 .../BoxBoxCollisionAlgorithm.cpp              |  13 +-
 .../BulletMultiThreaded/SpuParallelSolver.cpp |   4 +-
 .../BroadphaseCollision/btAxisSweep3.h        |  96 ++---
 .../btBroadphaseInterface.h                   |   4 +-
 .../BroadphaseCollision/btBroadphaseProxy.h   |   9 +
 .../btMultiSapBroadphase.cpp                  |  10 +-
 .../btMultiSapBroadphase.h                    |   4 +-
 .../btOverlappingPairCache.cpp                | 389 ++++++++++++++---
 .../btOverlappingPairCache.h                  | 392 ++++++++++++------
 .../btSimpleBroadphase.cpp                    | 248 ++++++-----
 .../BroadphaseCollision/btSimpleBroadphase.h  |  54 ++-
 .../btCollisionDispatcher.cpp                 |  17 +-
 .../CollisionDispatch/btCollisionWorld.cpp    |   5 +-
 .../btConvexConcaveCollisionAlgorithm.cpp     |   3 +-
 .../btConvexConvexAlgorithm.cpp               |   5 +
 .../CollisionDispatch/btManifoldResult.cpp    |   4 +-
 .../CollisionDispatch/btManifoldResult.h      |  18 +-
 .../btSphereBoxCollisionAlgorithm.cpp         |  11 +-
 .../btSphereSphereCollisionAlgorithm.cpp      |  12 +-
 .../btSphereTriangleCollisionAlgorithm.cpp    |  10 +-
 .../CollisionDispatch/btUnionFind.cpp         |   1 +
 .../CollisionShapes/btOptimizedBvh.cpp        |  13 +-
 .../CollisionShapes/btSphereShape.h           |   4 +-
 .../btPersistentManifold.cpp                  |  24 +-
 .../btPersistentManifold.h                    |  29 +-
 .../btSequentialImpulseConstraintSolver.cpp   |  56 ++-
 .../Dynamics/btContinuousDynamicsWorld.cpp    |   2 +-
 .../Dynamics/btDiscreteDynamicsWorld.cpp      |   2 +-
 .../Dynamics/btSimpleDynamicsWorld.cpp        |   2 +-
 src/LinearMath/btAlignedAllocator.cpp         |   6 +-
 src/LinearMath/btAlignedAllocator.h           |   2 +-
 src/LinearMath/btPoolAllocator.h              |  11 +
 src/LinearMath/btScalar.h                     |   7 +
 36 files changed, 1485 insertions(+), 460 deletions(-)
 create mode 100644 Demos/StressTest/StressTestDemo.cpp
 create mode 100644 Demos/StressTest/StressTestDemo.h

diff --git a/Demos/OpenGL/DemoApplication.cpp b/Demos/OpenGL/DemoApplication.cpp
index 48c5e431e..5dc6d0de6 100644
--- a/Demos/OpenGL/DemoApplication.cpp
+++ b/Demos/OpenGL/DemoApplication.cpp
@@ -445,7 +445,7 @@ void	DemoApplication::shootBox(const btVector3& destination)
 
 	if (m_dynamicsWorld)
 	{
-		float mass = 1.f;
+		float mass = 100.f;
 		btTransform startTransform;
 		startTransform.setIdentity();
 		btVector3 camPos = getCameraPosition();
diff --git a/Demos/StressTest/StressTestDemo.cpp b/Demos/StressTest/StressTestDemo.cpp
new file mode 100644
index 000000000..3540fac7f
--- /dev/null
+++ b/Demos/StressTest/StressTestDemo.cpp
@@ -0,0 +1,377 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "btBulletDynamicsCommon.h"
+#include "StressTestDemo.h"
+
+#define SHOW_NUM_DEEP_PENETRATIONS
+
+#include "LinearMath/btDefaultMotionState.h"
+#include "LinearMath/btIDebugDraw.h"
+#include "LinearMath/btQuickprof.h"
+#include "LinearMath/btDefaultMotionState.h"
+#include "BulletCollision/BroadphaseCollision/btMultiSapBroadphase.h"
+
+#include "BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.h"
+
+//#define REGISTER_BOX_BOX 1
+#ifdef REGISTER_BOX_BOX
+#include "../Extras/AlternativeCollisionAlgorithms/BoxBoxCollisionAlgorithm.h"
+#endif //REGISTER_BOX_BOX
+
+
+//#define SHOW_MEMORY_DETAILS 1
+
+//#ifdef SHOW_MEMORY_DETAILS
+//	#include "LinearMath/btGenericPoolAllocator.h"
+//#endif
+
+/// Including GIMPACT here
+
+
+
+#include "BMF_Api.h"
+
+#include "GLDebugDrawer.h"
+
+#include "GL_ShapeDrawer.h"
+#include "GlutStuff.h"
+
+
+#ifdef SHOW_NUM_DEEP_PENETRATIONS
+extern int gNumDeepPenetrationChecks;
+extern int gNumGjkChecks;
+#endif //
+
+
+
+GLDebugDrawer	debugDrawer;
+//Real			dts = 0.000001f;
+btScalar			dts = 1.0 / 60.0;
+
+
+///**************************************************************************************
+///	GIMPACT Test Demo made by DevO
+///
+///**************************************************************************************
+
+
+
+//------------------------------------------------------------------------------
+///User can override this material combiner by implementing gContactAddedCallback and setting body0->m_collisionFlags |= btCollisionObject::customMaterialCallback;
+inline btScalar	calculateCombinedFriction(float friction0,float friction1)
+{
+	btScalar friction = friction0 * friction1;
+
+	const btScalar MAX_FRICTION  = 10.f;
+	if (friction < -MAX_FRICTION)
+		friction = -MAX_FRICTION;
+	if (friction > MAX_FRICTION)
+		friction = MAX_FRICTION;
+	return friction;
+
+}
+
+//------------------------------------------------------------------------------
+inline btScalar	calculateCombinedRestitution(float restitution0,float restitution1)
+{
+	return restitution0 * restitution1;
+}
+
+
+//------------------------------------------------------------------------------
+bool CustomMaterialCombinerCallback(btManifoldPoint& cp,	const btCollisionObject* colObj0,int partId0,int index0,const btCollisionObject* colObj1,int partId1,int index1)
+{
+
+	float friction0 = colObj0->getFriction();
+	float friction1 = colObj1->getFriction();
+	float restitution0 = colObj0->getRestitution();
+	float restitution1 = colObj1->getRestitution();
+
+	if (colObj0->getCollisionFlags() & btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK)
+	{
+		friction0 = 1.0;//partId0,index0
+		restitution0 = 0.f;
+	}
+	if (colObj1->getCollisionFlags() & btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK)
+	{
+		if (index1&1)
+		{
+			friction1 = 1.0f;//partId1,index1
+		} else
+		{
+			friction1 = 0.f;
+		}
+		restitution1 = 0.f;
+	}
+
+	cp.m_combinedFriction = calculateCombinedFriction(friction0,friction1);
+	cp.m_combinedRestitution = calculateCombinedRestitution(restitution0,restitution1);
+
+	//this return value is currently ignored, but to be on the safe side: return false if you don't calculate friction
+	return true;
+}
+
+extern ContactAddedCallback		gContactAddedCallback;
+
+
+
+//################################## main #####################################
+int main(int argc,char** argv)
+{
+	//gContactAddedCallback = CustomMaterialCombinerCallback;
+
+	int sizeofbp = sizeof(btBroadphaseProxy);
+
+	StressTestDemo* concaveDemo = new StressTestDemo();  /// This will not be Deleted!!!
+	concaveDemo->initPhysics();
+	concaveDemo->setCameraDistance(45.f);
+
+//cannot run stepFront yet, the OpenGL context is not opened (stepFront updates camera...)
+//	concaveDemo->stepFront();
+//	concaveDemo->stepFront();
+//	concaveDemo->stepFront();
+//	concaveDemo->stepFront();
+
+	return glutmain(argc, argv,640,480,"DevO,s GIMPACT Test Demo",concaveDemo);
+}
+
+//--------------------------
+//------------------------------------------------------------------------------
+void	StressTestDemo::initPhysics()
+{
+	/// Init Bullet
+	m_collisionConfiguration = new btDefaultCollisionConfiguration();
+
+	m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration);
+
+#ifdef REGISTER_BOX_BOX
+	m_dispatcher->registerCollisionCreateFunc(BOX_SHAPE_PROXYTYPE,BOX_SHAPE_PROXYTYPE,new BoxBoxCollisionAlgorithm::CreateFunc);
+#endif //REGISTER_BOX_BOX
+	
+
+	LONG maxProxies = 4096;
+	btVector3 worldAabbMin(-1000,-1000,-1000);
+	btVector3 worldAabbMax( 1000, 1000, 1000);
+	m_broadphase = new btAxisSweep3(worldAabbMin,worldAabbMax,maxProxies);
+	//m_broadphase = new btMultiSapBroadphase();
+	//m_broadphase = new btSimpleBroadphase();
+
+	btSequentialImpulseConstraintSolver* solver = new btSequentialImpulseConstraintSolver();;
+	m_constraintSolver = solver;
+
+	btDiscreteDynamicsWorld* world = new btDiscreteDynamicsWorld(m_dispatcher,m_broadphase,m_constraintSolver);
+	world->getSolverInfo().m_numIterations = 4;
+//	solver->setSolverMode(btSequentialImpulseConstraintSolver::SOLVER_CACHE_FRIENDLY);//btSequentialImpulseConstraintSolver::SOLVER_RANDMIZE_ORDER);
+//	solver->setSolverMode(btSequentialImpulseConstraintSolver::SOLVER_RANDMIZE_ORDER);
+
+	m_dynamicsWorld = world;
+	m_dynamicsWorld->setDebugDrawer(&debugDrawer);
+
+	
+
+	// register algorithm
+	if(0)
+	{
+		m_spheresphere_collisionCreateFunc = new btSphereSphereCollisionAlgorithm::CreateFunc;  /// NEW
+
+		m_dispatcher->registerCollisionCreateFunc(SPHERE_SHAPE_PROXYTYPE,SPHERE_SHAPE_PROXYTYPE,m_spheresphere_collisionCreateFunc);
+
+		m_spherebox_collisionCreateFunc = new btSphereBoxCollisionAlgorithm::CreateFunc;
+
+		m_dispatcher->registerCollisionCreateFunc(SPHERE_SHAPE_PROXYTYPE,BOX_SHAPE_PROXYTYPE,m_spherebox_collisionCreateFunc);
+
+		m_dispatcher->registerCollisionCreateFunc(SPHERE_SHAPE_PROXYTYPE,BOX_SHAPE_PROXYTYPE,m_spherebox_collisionCreateFunc);
+
+	}
+
+
+
+	//create trimesh model and shape
+
+
+
+
+	/// Create Scene
+	float mass = 0.f;
+	btTransform	startTransform;
+	startTransform.setIdentity();
+
+	btCollisionShape* staticboxShape1 = new btBoxShape(btVector3(20,1,20));//floor
+	btCollisionShape* staticboxShape2 = new btBoxShape(btVector3(1,50,200));//left wall
+	btCollisionShape* staticboxShape3 = new btBoxShape(btVector3(1,50,200));//right wall
+	btCollisionShape* staticboxShape4 = new btBoxShape(btVector3(200,50,1));//front wall
+	btCollisionShape* staticboxShape5 = new btBoxShape(btVector3(200,50,1));//back wall
+#ifdef USE_COMPOUND
+
+	btCompoundShape* staticScenario = new btCompoundShape();//static scenario
+
+	startTransform.setOrigin(btVector3(0,-10,0));
+	staticScenario->addChildShape(startTransform,staticboxShape1);
+	startTransform.setOrigin(btVector3(-200,15,0));
+	staticScenario->addChildShape(startTransform,staticboxShape2);
+	startTransform.setOrigin(btVector3(200,15,0));
+	staticScenario->addChildShape(startTransform,staticboxShape3);
+	startTransform.setOrigin(btVector3(0,15,200));
+	staticScenario->addChildShape(startTransform,staticboxShape4);
+	startTransform.setOrigin(btVector3(0,15,-200));
+	staticScenario->addChildShape(startTransform,staticboxShape5);
+
+	startTransform.setOrigin(btVector3(0,0,0));
+
+	btRigidBody* staticBody = localCreateRigidBody(mass, startTransform,staticScenario);
+#else
+	startTransform.setOrigin(btVector3(0,-7,0));//;//-10,0));
+	btRigidBody* staticBody = localCreateRigidBody(mass, startTransform,staticboxShape1);
+	/*startTransform.setOrigin(btVector3(-200,15,0));
+	staticBody = localCreateRigidBody(mass, startTransform,staticboxShape2);
+	startTransform.setOrigin(btVector3(200,15,0));
+	staticBody = localCreateRigidBody(mass, startTransform,staticboxShape3);
+	startTransform.setOrigin(btVector3(0,15,200));
+	staticBody = localCreateRigidBody(mass, startTransform,staticboxShape4);
+	startTransform.setOrigin(btVector3(0,15,-200));
+	staticBody = localCreateRigidBody(mass, startTransform,staticboxShape5);
+	*/
+
+#endif //USE_COMPOUND
+
+	//enable custom material callback
+//	staticBody->setCollisionFlags(staticBody->getCollisionFlags()|btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK);
+
+	startTransform.setIdentity();
+	btCollisionShape* sphShape = new btSphereShape(1);
+	btScalar radii[1] = {btScalar(1)};
+	btVector3 poss[1]={btVector3(0,0,0)};
+
+//	btCollisionShape* sphShape = new btMultiSphereShape(btVector3(1,1,1),&poss[0],&radii[0],1);
+
+	btScalar margin(0.04);
+	btCollisionShape* boxShape = new btBoxShape(btVector3(1,1,1));//1.-margin,1.-margin,1.-margin));
+	boxShape->setMargin(margin);
+
+
+	/// Create Dynamic Boxes
+	{
+
+		float start_x = START_POS_X - ARRAY_SIZE_X/2;
+		float start_y = START_POS_Y;
+		float start_z = START_POS_Z - ARRAY_SIZE_Z/2;
+
+		for (int k=0;k<ARRAY_SIZE_Y;k++)
+		{
+			for (int i=0;i<ARRAY_SIZE_X;i++)
+			{
+				for(int j = 0;j<ARRAY_SIZE_Z;j++)
+				{
+					startTransform.setOrigin(btVector3(
+										2.0*i + start_x,
+										2.0*k + start_y,
+										2.0*j + start_z));
+
+					#ifdef SPHERES
+					
+					localCreateRigidBody(1, startTransform,sphShape);
+					#else					
+					localCreateRigidBody(1, startTransform,boxShape);
+					#endif
+
+
+
+				}
+			}
+		}
+	}
+
+	//m_debugMode |= btIDebugDraw::DBG_DrawWireframe;
+
+
+
+
+}
+
+
+//------------------------------------------------------------------------------
+void StressTestDemo::clientMoveAndDisplay()
+{
+	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
+
+	int start, end;
+
+    start = glutGet(GLUT_ELAPSED_TIME);
+
+	float dt = float(m_clock.getTimeMicroseconds()) * 0.000001f;
+
+	m_clock.reset();
+	m_dynamicsWorld->stepSimulation(dt,1);
+	m_steps_done++;
+
+	end = glutGet(GLUT_ELAPSED_TIME);
+	printf("Time %d \n", end-start);
+
+	renderme();
+
+	glFlush();
+	glutSwapBuffers();
+
+}
+
+//------------------------------------------------------------------------------
+void StressTestDemo::displayCallback(void) {
+
+	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
+
+	renderme();
+
+	glFlush();
+	glutSwapBuffers();
+}
+
+//------------------------------------------------------------------------------
+void StressTestDemo::clientResetScene()
+{
+	m_steps_done = 0;
+	DemoApplication::clientResetScene();
+}
+
+#define KEY_ESCAPE     0x1B
+
+
+//------------------------------------------------------------------------------
+void StressTestDemo::keyboardCallback(unsigned char key, int x, int y)
+{
+	switch (key)
+	{
+	case '.':
+		{
+			break;
+		}
+
+	case '2':
+		{
+			dts += 0.000001f;
+			break;
+		}
+	case '3':
+		{
+			dts -= 0.000001f; if(dts<0.000001f) dts = 0.000001f;
+			break;
+		}
+
+	default:
+		DemoApplication::keyboardCallback(key, x, y);
+	}
+}
+
+
diff --git a/Demos/StressTest/StressTestDemo.h b/Demos/StressTest/StressTestDemo.h
new file mode 100644
index 000000000..913ea55b8
--- /dev/null
+++ b/Demos/StressTest/StressTestDemo.h
@@ -0,0 +1,99 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+#ifndef TEST_CONCAVE_DEMO_H
+#define TEST_CONCAVE_DEMO_H
+
+#include "DemoApplication.h"
+
+
+
+
+struct btCollisionAlgorithmCreateFunc;
+
+///StressTestDemo shows usage of static concave triangle meshes
+///It also shows per-triangle material (friction/restitution) through CustomMaterialCombinerCallback
+class StressTestDemo : public DemoApplication
+{
+
+public:
+	StressTestDemo()
+		: m_collisionConfiguration(NULL),
+		  m_dispatcher(NULL),
+		  m_broadphase(NULL),
+		  m_constraintSolver(NULL),
+		  m_steps_done(0),
+		  m_spheresphere_collisionCreateFunc(0),
+		  m_spherebox_collisionCreateFunc(0)
+	{
+	}
+
+	virtual ~StressTestDemo()
+	{
+		delete m_spheresphere_collisionCreateFunc;
+		delete m_spherebox_collisionCreateFunc;
+
+		delete m_collisionConfiguration;
+		delete m_dispatcher;
+		delete m_broadphase;
+		delete m_constraintSolver;
+
+		delete m_dynamicsWorld;
+	}
+
+	void	initGImpactCollision();
+	void	initPhysics();
+
+	virtual void clientMoveAndDisplay();
+
+	virtual void displayCallback();
+
+	virtual void clientResetScene();
+
+	virtual void keyboardCallback(unsigned char key, int x, int y);
+
+
+
+public: ///data
+	unsigned int			m_steps_done;
+
+
+	btVector3				kinTorusTran;
+	btQuaternion			kinTorusRot;
+	btRigidBody				*kinematicTorus;
+
+
+	btCollisionAlgorithmCreateFunc*  m_spheresphere_collisionCreateFunc;
+	btCollisionAlgorithmCreateFunc*  m_spherebox_collisionCreateFunc;
+
+	btDefaultCollisionConfiguration* m_collisionConfiguration;
+	btCollisionDispatcher*			 m_dispatcher;
+	btBroadphaseInterface*			 m_broadphase;
+	btConstraintSolver*				 m_constraintSolver;
+};
+
+
+#define SPHERES 1
+
+#define START_POS_X -5
+#define START_POS_Y -5
+#define START_POS_Z -3
+
+
+#define ARRAY_SIZE_X 10
+#define ARRAY_SIZE_Y 10
+#define ARRAY_SIZE_Z 10
+
+#endif //CONCAVE_DEMO_H
+
diff --git a/Extras/AlternativeCollisionAlgorithms/BoxBoxCollisionAlgorithm.cpp b/Extras/AlternativeCollisionAlgorithms/BoxBoxCollisionAlgorithm.cpp
index 10a63bf75..0e7c95730 100644
--- a/Extras/AlternativeCollisionAlgorithms/BoxBoxCollisionAlgorithm.cpp
+++ b/Extras/AlternativeCollisionAlgorithms/BoxBoxCollisionAlgorithm.cpp
@@ -19,6 +19,8 @@ subject to the following restrictions:
 #include "BulletCollision/CollisionDispatch/btCollisionObject.h"
 #include "BoxBoxDetector.h"
 
+#define USE_PERSISTENT_CONTACTS 1
+
 BoxBoxCollisionAlgorithm::BoxBoxCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* obj0,btCollisionObject* obj1)
 : btCollisionAlgorithm(ci),
 m_ownManifold(false),
@@ -54,8 +56,9 @@ void BoxBoxCollisionAlgorithm::processCollision (btCollisionObject* body0,btColl
 
 	/// report a contact. internally this will be kept persistent, and contact reduction is done
 	resultOut->setPersistentManifold(m_manifoldPtr);
-	
+#ifndef USE_PERSISTENT_CONTACTS	
 	m_manifoldPtr->clearManifold();
+#endif //USE_PERSISTENT_CONTACTS
 
 	btDiscreteCollisionDetectorInterface::ClosestPointInput input;
 	input.m_maximumDistanceSquared = 1e30f;
@@ -65,6 +68,14 @@ void BoxBoxCollisionAlgorithm::processCollision (btCollisionObject* body0,btColl
 	BoxBoxDetector detector(box0,box1);
 	detector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw);
 
+#ifdef USE_PERSISTENT_CONTACTS
+	//  refreshContactPoints is only necessary when using persistent contact points. otherwise all points are newly added
+	if (m_ownManifold)
+	{
+		resultOut->refreshContactPoints();
+	}
+#endif //USE_PERSISTENT_CONTACTS
+
 }
 
 btScalar BoxBoxCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
diff --git a/Extras/BulletMultiThreaded/SpuParallelSolver.cpp b/Extras/BulletMultiThreaded/SpuParallelSolver.cpp
index 5ea559ba4..9fcb2382a 100644
--- a/Extras/BulletMultiThreaded/SpuParallelSolver.cpp
+++ b/Extras/BulletMultiThreaded/SpuParallelSolver.cpp
@@ -109,7 +109,8 @@ btScalar btParallelSequentialImpulseSolver::solveGroup(btCollisionObject** bodie
 {
 	if (!numManifolds && !numConstraints)
 		return 0;
-
+///refresh contact points is not needed anymore, it has been moved into the processCollision detection part.
+#ifdef FORCE_REFESH_CONTACT_MANIFOLDS
 	for (int i = 0; i < numManifolds; ++i)
 	{
 		btPersistentManifold* currManifold = manifold[i];
@@ -118,6 +119,7 @@ btScalar btParallelSequentialImpulseSolver::solveGroup(btCollisionObject** bodie
 
 		currManifold->refreshContactPoints(rb0->getCenterOfMassTransform(),rb1->getCenterOfMassTransform());
 	}
+#endif //FORCE_REFESH_CONTACT_MANIFOLDS
 
 	// Record and mark the manifolds to the cells
 	for (int i = 0; i < numManifolds; ++i)
diff --git a/src/BulletCollision/BroadphaseCollision/btAxisSweep3.h b/src/BulletCollision/BroadphaseCollision/btAxisSweep3.h
index 9656de78c..7ee0942a7 100644
--- a/src/BulletCollision/BroadphaseCollision/btAxisSweep3.h
+++ b/src/BulletCollision/BroadphaseCollision/btAxisSweep3.h
@@ -58,7 +58,7 @@ public:
 	
 		// indexes into the edge arrays
 		BP_FP_INT_TYPE m_minEdges[3], m_maxEdges[3];		// 6 * 2 = 12
-		BP_FP_INT_TYPE m_handleId;
+//		BP_FP_INT_TYPE m_uniqueId;
 		BP_FP_INT_TYPE m_pad;
 		
 		//void* m_pOwner; this is now in btBroadphaseProxy.m_clientObject
@@ -102,10 +102,10 @@ protected:
 
 	void quantize(BP_FP_INT_TYPE* out, const btPoint3& point, int isMax) const;
 
-	void sortMinDown(int axis, BP_FP_INT_TYPE edge, bool updateOverlaps = true);
-	void sortMinUp(int axis, BP_FP_INT_TYPE edge, bool updateOverlaps = true);
-	void sortMaxDown(int axis, BP_FP_INT_TYPE edge, bool updateOverlaps = true);
-	void sortMaxUp(int axis, BP_FP_INT_TYPE edge, bool updateOverlaps = true);
+	void sortMinDown(int axis, BP_FP_INT_TYPE edge, btDispatcher* dispatcher, bool updateOverlaps );
+	void sortMinUp(int axis, BP_FP_INT_TYPE edge, btDispatcher* dispatcher, bool updateOverlaps );
+	void sortMaxDown(int axis, BP_FP_INT_TYPE edge, btDispatcher* dispatcher, bool updateOverlaps );
+	void sortMaxUp(int axis, BP_FP_INT_TYPE edge, btDispatcher* dispatcher, bool updateOverlaps );
 
 public:
 
@@ -116,17 +116,17 @@ public:
 
 	virtual void	calculateOverlappingPairs(btDispatcher* dispatcher);
 	
-	BP_FP_INT_TYPE addHandle(const btPoint3& aabbMin,const btPoint3& aabbMax, void* pOwner,short int collisionFilterGroup,short int collisionFilterMask);
+	BP_FP_INT_TYPE addHandle(const btPoint3& aabbMin,const btPoint3& aabbMax, void* pOwner,short int collisionFilterGroup,short int collisionFilterMask,btDispatcher* dispatcher);
 	void removeHandle(BP_FP_INT_TYPE handle,btDispatcher* dispatcher);
-	void updateHandle(BP_FP_INT_TYPE handle, const btPoint3& aabbMin,const btPoint3& aabbMax);
+	void updateHandle(BP_FP_INT_TYPE handle, const btPoint3& aabbMin,const btPoint3& aabbMax,btDispatcher* dispatcher);
 	inline Handle* getHandle(BP_FP_INT_TYPE index) const {return m_pHandles + index;}
 
 	void	processAllOverlappingPairs(btOverlapCallback* callback);
 
 	//Broadphase Interface
-	virtual btBroadphaseProxy*	createProxy(  const btVector3& aabbMin,  const btVector3& aabbMax,int shapeType,void* userPtr ,short int collisionFilterGroup,short int collisionFilterMask);
+	virtual btBroadphaseProxy*	createProxy(  const btVector3& aabbMin,  const btVector3& aabbMax,int shapeType,void* userPtr ,short int collisionFilterGroup,short int collisionFilterMask,btDispatcher* dispatcher);
 	virtual void	destroyProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher);
-	virtual void	setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax);
+	virtual void	setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax,btDispatcher* dispatcher);
 	
 	bool	testAabbOverlap(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1);
 
@@ -173,10 +173,10 @@ void btAxisSweep3<BP_FP_INT_TYPE>::debugPrintAxis(int axis, bool checkCardinalit
 #endif //DEBUG_BROADPHASE
 
 template <typename BP_FP_INT_TYPE>
-btBroadphaseProxy*	btAxisSweep3Internal<BP_FP_INT_TYPE>::createProxy(  const btVector3& aabbMin,  const btVector3& aabbMax,int shapeType,void* userPtr,short int collisionFilterGroup,short int collisionFilterMask)
+btBroadphaseProxy*	btAxisSweep3Internal<BP_FP_INT_TYPE>::createProxy(  const btVector3& aabbMin,  const btVector3& aabbMax,int shapeType,void* userPtr,short int collisionFilterGroup,short int collisionFilterMask,btDispatcher* dispatcher)
 {
 		(void)shapeType;
-		BP_FP_INT_TYPE handleId = addHandle(aabbMin,aabbMax, userPtr,collisionFilterGroup,collisionFilterMask);
+		BP_FP_INT_TYPE handleId = addHandle(aabbMin,aabbMax, userPtr,collisionFilterGroup,collisionFilterMask,dispatcher);
 		
 		Handle* handle = getHandle(handleId);
 				
@@ -189,14 +189,14 @@ template <typename BP_FP_INT_TYPE>
 void	btAxisSweep3Internal<BP_FP_INT_TYPE>::destroyProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher)
 {
 	Handle* handle = static_cast<Handle*>(proxy);
-	removeHandle(handle->m_handleId,dispatcher);
+	removeHandle(handle->m_uniqueId,dispatcher);
 }
 
 template <typename BP_FP_INT_TYPE>
-void	btAxisSweep3Internal<BP_FP_INT_TYPE>::setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax)
+void	btAxisSweep3Internal<BP_FP_INT_TYPE>::setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax,btDispatcher* dispatcher)
 {
 	Handle* handle = static_cast<Handle*>(proxy);
-	updateHandle(handle->m_handleId,aabbMin,aabbMax);
+	updateHandle(handle->m_uniqueId,aabbMin,aabbMax,dispatcher);
 
 }
 
@@ -335,7 +335,7 @@ void btAxisSweep3Internal<BP_FP_INT_TYPE>::freeHandle(BP_FP_INT_TYPE handle)
 
 
 template <typename BP_FP_INT_TYPE>
-BP_FP_INT_TYPE btAxisSweep3Internal<BP_FP_INT_TYPE>::addHandle(const btPoint3& aabbMin,const btPoint3& aabbMax, void* pOwner,short int collisionFilterGroup,short int collisionFilterMask)
+BP_FP_INT_TYPE btAxisSweep3Internal<BP_FP_INT_TYPE>::addHandle(const btPoint3& aabbMin,const btPoint3& aabbMax, void* pOwner,short int collisionFilterGroup,short int collisionFilterMask,btDispatcher* dispatcher)
 {
 	// quantize the bounds
 	BP_FP_INT_TYPE min[3], max[3];
@@ -348,7 +348,7 @@ BP_FP_INT_TYPE btAxisSweep3Internal<BP_FP_INT_TYPE>::addHandle(const btPoint3& a
 
 	Handle* pHandle = getHandle(handle);
 	
-	pHandle->m_handleId = handle;
+	pHandle->m_uniqueId = handle;
 	//pHandle->m_pOverlaps = 0;
 	pHandle->m_clientObject = pOwner;
 	pHandle->m_collisionFilterGroup = collisionFilterGroup;
@@ -377,12 +377,12 @@ BP_FP_INT_TYPE btAxisSweep3Internal<BP_FP_INT_TYPE>::addHandle(const btPoint3& a
 	}
 
 	// now sort the new edges to their correct position
-	sortMinDown(0, pHandle->m_minEdges[0], false);
-	sortMaxDown(0, pHandle->m_maxEdges[0], false);
-	sortMinDown(1, pHandle->m_minEdges[1], false);
-	sortMaxDown(1, pHandle->m_maxEdges[1], false);
-	sortMinDown(2, pHandle->m_minEdges[2], true);
-	sortMaxDown(2, pHandle->m_maxEdges[2], true);
+	sortMinDown(0, pHandle->m_minEdges[0], dispatcher,false);
+	sortMaxDown(0, pHandle->m_maxEdges[0], dispatcher,false);
+	sortMinDown(1, pHandle->m_minEdges[1], dispatcher,false);
+	sortMaxDown(1, pHandle->m_maxEdges[1], dispatcher,false);
+	sortMinDown(2, pHandle->m_minEdges[2], dispatcher,true);
+	sortMaxDown(2, pHandle->m_maxEdges[2], dispatcher,true);
 
 
 	return handle;
@@ -418,14 +418,14 @@ void btAxisSweep3Internal<BP_FP_INT_TYPE>::removeHandle(BP_FP_INT_TYPE handle,bt
 		BP_FP_INT_TYPE max = pHandle->m_maxEdges[axis];
 		pEdges[max].m_pos = m_handleSentinel;
 
-		sortMaxUp(axis,max,false);
+		sortMaxUp(axis,max,dispatcher,false);
 
 
 		BP_FP_INT_TYPE i = pHandle->m_minEdges[axis];
 		pEdges[i].m_pos = m_handleSentinel;
 
 
-		sortMinUp(axis,i,false);
+		sortMinUp(axis,i,dispatcher,false);
 
 		pEdges[limit-1].m_handle = 0;
 		pEdges[limit-1].m_pos = m_handleSentinel;
@@ -445,13 +445,16 @@ void btAxisSweep3Internal<BP_FP_INT_TYPE>::removeHandle(BP_FP_INT_TYPE handle,bt
 }
 
 extern int gOverlappingPairs;
+#include <stdio.h>
 
 template <typename BP_FP_INT_TYPE>
 void	btAxisSweep3Internal<BP_FP_INT_TYPE>::calculateOverlappingPairs(btDispatcher* dispatcher)
 {
-	
+#ifdef USE_LAZY_REMOVAL
+
 	if (m_ownsPairCache)
 	{
+	
 		btBroadphasePairArray&	overlappingPairArray = m_pairCache->getOverlappingPairArray();
 
 		//perform a sort, to find duplicates and to sort 'invalid' pairs to the end
@@ -524,7 +527,12 @@ void	btAxisSweep3Internal<BP_FP_INT_TYPE>::calculateOverlappingPairs(btDispatche
 		m_invalidPair = 0;
 	#endif//CLEAN_INVALID_PAIRS
 		
+		//printf("overlappingPairArray.size()=%d\n",overlappingPairArray.size());
 	}
+#endif //USE_LAZY_REMOVAL
+
+
+	
 
 }
 
@@ -581,7 +589,7 @@ bool btAxisSweep3Internal<BP_FP_INT_TYPE>::testOverlap(int ignoreAxis,const Hand
 }
 
 template <typename BP_FP_INT_TYPE>
-void btAxisSweep3Internal<BP_FP_INT_TYPE>::updateHandle(BP_FP_INT_TYPE handle, const btPoint3& aabbMin,const btPoint3& aabbMax)
+void btAxisSweep3Internal<BP_FP_INT_TYPE>::updateHandle(BP_FP_INT_TYPE handle, const btPoint3& aabbMin,const btPoint3& aabbMax,btDispatcher* dispatcher)
 {
 //	assert(bounds.IsFinite());
 	//assert(bounds.HasVolume());
@@ -607,17 +615,17 @@ void btAxisSweep3Internal<BP_FP_INT_TYPE>::updateHandle(BP_FP_INT_TYPE handle, c
 
 		// expand (only adds overlaps)
 		if (dmin < 0)
-			sortMinDown(axis, emin);
+			sortMinDown(axis, emin,dispatcher,true);
 
 		if (dmax > 0)
-			sortMaxUp(axis, emax);
+			sortMaxUp(axis, emax,dispatcher,true);
 
 		// shrink (only removes overlaps)
 		if (dmin > 0)
-			sortMinUp(axis, emin);
+			sortMinUp(axis, emin,dispatcher,true);
 
 		if (dmax < 0)
-			sortMaxDown(axis, emax);
+			sortMaxDown(axis, emax,dispatcher,true);
 
 #ifdef DEBUG_BROADPHASE
 	debugPrintAxis(axis);
@@ -632,7 +640,7 @@ void btAxisSweep3Internal<BP_FP_INT_TYPE>::updateHandle(BP_FP_INT_TYPE handle, c
 
 // sorting a min edge downwards can only ever *add* overlaps
 template <typename BP_FP_INT_TYPE>
-void btAxisSweep3Internal<BP_FP_INT_TYPE>::sortMinDown(int axis, BP_FP_INT_TYPE edge, bool updateOverlaps)
+void btAxisSweep3Internal<BP_FP_INT_TYPE>::sortMinDown(int axis, BP_FP_INT_TYPE edge, btDispatcher* dispatcher, bool updateOverlaps)
 {
 
 	Edge* pEdge = m_pEdges[axis] + edge;
@@ -680,7 +688,7 @@ void btAxisSweep3Internal<BP_FP_INT_TYPE>::sortMinDown(int axis, BP_FP_INT_TYPE
 
 // sorting a min edge upwards can only ever *remove* overlaps
 template <typename BP_FP_INT_TYPE>
-void btAxisSweep3Internal<BP_FP_INT_TYPE>::sortMinUp(int axis, BP_FP_INT_TYPE edge, bool updateOverlaps)
+void btAxisSweep3Internal<BP_FP_INT_TYPE>::sortMinUp(int axis, BP_FP_INT_TYPE edge, btDispatcher* dispatcher, bool updateOverlaps)
 {
 	Edge* pEdge = m_pEdges[axis] + edge;
 	Edge* pNext = pEdge + 1;
@@ -692,17 +700,16 @@ void btAxisSweep3Internal<BP_FP_INT_TYPE>::sortMinUp(int axis, BP_FP_INT_TYPE ed
 
 		if (pNext->IsMax())
 		{
+#ifndef USE_LAZY_REMOVAL
 			// if next edge is maximum remove any overlap between the two handles
 			if (updateOverlaps)
 			{
-				/*
 				Handle* handle0 = getHandle(pEdge->m_handle);
 				Handle* handle1 = getHandle(pNext->m_handle);
-				btBroadphasePair tmpPair(*handle0,*handle1);
-				removeOverlappingPair(tmpPair);
-				*/
 
+				m_pairCache->removeOverlappingPair(handle0,handle1,dispatcher);	
 			}
+#endif //USE_LAZY_REMOVAL
 
 			// update edge reference in other handle
 			pHandleNext->m_maxEdges[axis]--;
@@ -727,7 +734,7 @@ void btAxisSweep3Internal<BP_FP_INT_TYPE>::sortMinUp(int axis, BP_FP_INT_TYPE ed
 
 // sorting a max edge downwards can only ever *remove* overlaps
 template <typename BP_FP_INT_TYPE>
-void btAxisSweep3Internal<BP_FP_INT_TYPE>::sortMaxDown(int axis, BP_FP_INT_TYPE edge, bool updateOverlaps)
+void btAxisSweep3Internal<BP_FP_INT_TYPE>::sortMaxDown(int axis, BP_FP_INT_TYPE edge, btDispatcher* dispatcher, bool updateOverlaps)
 {
 
 	Edge* pEdge = m_pEdges[axis] + edge;
@@ -744,17 +751,12 @@ void btAxisSweep3Internal<BP_FP_INT_TYPE>::sortMaxDown(int axis, BP_FP_INT_TYPE
 			if (updateOverlaps)
 			{
 				//this is done during the overlappingpairarray iteration/narrowphase collision
-				/*
+#ifndef USE_LAZY_REMOVAL
 				Handle* handle0 = getHandle(pEdge->m_handle);
 				Handle* handle1 = getHandle(pPrev->m_handle);
-				btBroadphasePair* pair = findPair(handle0,handle1);
-				//assert(pair);
-
-				if (pair)
-				{
-					removeOverlappingPair(*pair);
-				}
-				*/
+				m_pairCache->removeOverlappingPair(handle0,handle1,dispatcher);
+			
+#endif //USE_LAZY_REMOVAL		
 
 			}
 
@@ -785,7 +787,7 @@ void btAxisSweep3Internal<BP_FP_INT_TYPE>::sortMaxDown(int axis, BP_FP_INT_TYPE
 
 // sorting a max edge upwards can only ever *add* overlaps
 template <typename BP_FP_INT_TYPE>
-void btAxisSweep3Internal<BP_FP_INT_TYPE>::sortMaxUp(int axis, BP_FP_INT_TYPE edge, bool updateOverlaps)
+void btAxisSweep3Internal<BP_FP_INT_TYPE>::sortMaxUp(int axis, BP_FP_INT_TYPE edge, btDispatcher* dispatcher, bool updateOverlaps)
 {
 	Edge* pEdge = m_pEdges[axis] + edge;
 	Edge* pNext = pEdge + 1;
diff --git a/src/BulletCollision/BroadphaseCollision/btBroadphaseInterface.h b/src/BulletCollision/BroadphaseCollision/btBroadphaseInterface.h
index eba825f1b..97ba20743 100644
--- a/src/BulletCollision/BroadphaseCollision/btBroadphaseInterface.h
+++ b/src/BulletCollision/BroadphaseCollision/btBroadphaseInterface.h
@@ -31,9 +31,9 @@ class btBroadphaseInterface
 public:
 	virtual ~btBroadphaseInterface() {}
 
-	virtual btBroadphaseProxy*	createProxy(  const btVector3& aabbMin,  const btVector3& aabbMax,int shapeType,void* userPtr, short int collisionFilterGroup,short int collisionFilterMask) =0;
+	virtual btBroadphaseProxy*	createProxy(  const btVector3& aabbMin,  const btVector3& aabbMax,int shapeType,void* userPtr, short int collisionFilterGroup,short int collisionFilterMask, btDispatcher* dispatcher) =0;
 	virtual void	destroyProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher)=0;
-	virtual void	setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax)=0;
+	virtual void	setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax, btDispatcher* dispatcher)=0;
 	
 	///calculateOverlappingPairs is optional: incremental algorithms (sweep and prune) might do it during the set aabb
 	virtual void	calculateOverlappingPairs(btDispatcher* dispatcher)=0;
diff --git a/src/BulletCollision/BroadphaseCollision/btBroadphaseProxy.h b/src/BulletCollision/BroadphaseCollision/btBroadphaseProxy.h
index b4333cf69..fedef1000 100644
--- a/src/BulletCollision/BroadphaseCollision/btBroadphaseProxy.h
+++ b/src/BulletCollision/BroadphaseCollision/btBroadphaseProxy.h
@@ -65,6 +65,7 @@ CONCAVE_SHAPES_END_HERE,
 
 ///btBroadphaseProxy
 ATTRIBUTE_ALIGNED16(struct) btBroadphaseProxy
+//struct btBroadphaseProxy
 {
 
 BT_DECLARE_ALIGNED_ALLOCATOR();
@@ -96,6 +97,12 @@ BT_DECLARE_ALIGNED_ALLOCATOR();
 
 	};
 
+	int			m_uniqueId;//m_uniqueId is introduced for paircache. could get rid of this, by calculating the address offset etc.
+	inline int getUid()
+	{
+		return m_uniqueId;//(int)this;
+	}
+
 	//used for memory pools
 	btBroadphaseProxy() :m_clientObject(0){}
 
@@ -106,6 +113,8 @@ BT_DECLARE_ALIGNED_ALLOCATOR();
 	{
 	}
 
+	
+
 	static inline bool isPolyhedral(int proxyType)
 	{
 		return (proxyType  < IMPLICIT_CONVEX_SHAPES_START_HERE);
diff --git a/src/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.cpp b/src/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.cpp
index 0e50b9749..ec26a7af1 100644
--- a/src/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.cpp
+++ b/src/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.cpp
@@ -56,14 +56,14 @@ btMultiSapBroadphase::~btMultiSapBroadphase()
 {
 }
 
-btBroadphaseProxy*	btMultiSapBroadphase::createProxy(  const btVector3& aabbMin,  const btVector3& aabbMax,int shapeType,void* userPtr, short int collisionFilterGroup,short int collisionFilterMask)
+btBroadphaseProxy*	btMultiSapBroadphase::createProxy(  const btVector3& aabbMin,  const btVector3& aabbMax,int shapeType,void* userPtr, short int collisionFilterGroup,short int collisionFilterMask, btDispatcher* dispatcher)
 {
 	btMultiSapProxy* proxy = new btMultiSapProxy(aabbMin,  aabbMax,shapeType,userPtr, collisionFilterGroup,collisionFilterMask);
 	m_multiSapProxies.push_back(proxy);
 
 	///we don't pass the userPtr but our multisap proxy. We need to patch this, before processing an actual collision
 	///this is needed to be able to calculate the aabb overlap
-	btBroadphaseProxy* simpleProxy = m_simpleBroadphase->createProxy(aabbMin,aabbMax,shapeType,userPtr,collisionFilterGroup,collisionFilterMask);
+	btBroadphaseProxy* simpleProxy = m_simpleBroadphase->createProxy(aabbMin,aabbMax,shapeType,userPtr,collisionFilterGroup,collisionFilterMask, dispatcher);
 	simpleProxy->m_multiSapParentProxy = proxy;
 
 	btChildProxy* childProxyRef = new btChildProxy();
@@ -72,7 +72,7 @@ btBroadphaseProxy*	btMultiSapBroadphase::createProxy(  const btVector3& aabbMin,
 	proxy->m_childProxies.push_back(childProxyRef);
 
 	///this should deal with inserting/removal into child broadphases
-	setAabb(proxy,aabbMin,aabbMax);
+	setAabb(proxy,aabbMin,aabbMax,dispatcher);
 	return proxy;
 }
 
@@ -82,7 +82,7 @@ void	btMultiSapBroadphase::destroyProxy(btBroadphaseProxy* proxy,btDispatcher* d
 	btAssert(0);
 
 }
-void	btMultiSapBroadphase::setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax)
+void	btMultiSapBroadphase::setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax, btDispatcher* dispatcher)
 {
 	btMultiSapProxy* multiProxy = static_cast<btMultiSapProxy*>(proxy);
 	multiProxy->m_aabbMin = aabbMin;
@@ -91,7 +91,7 @@ void	btMultiSapBroadphase::setAabb(btBroadphaseProxy* proxy,const btVector3& aab
 	for (int i=0;i<multiProxy->m_childProxies.size();i++)
 	{
 		btChildProxy* childProxyRef = multiProxy->m_childProxies[i];
-		childProxyRef->m_childBroadphase->setAabb(childProxyRef->m_proxy,aabbMin,aabbMax);
+		childProxyRef->m_childBroadphase->setAabb(childProxyRef->m_proxy,aabbMin,aabbMax,dispatcher);
 	}
 
 }
diff --git a/src/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.h b/src/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.h
index 2788c0994..aff7c6c50 100644
--- a/src/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.h
+++ b/src/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.h
@@ -95,9 +95,9 @@ public:
 
 	virtual ~btMultiSapBroadphase();
 
-	virtual btBroadphaseProxy*	createProxy(  const btVector3& aabbMin,  const btVector3& aabbMax,int shapeType,void* userPtr, short int collisionFilterGroup,short int collisionFilterMask);
+	virtual btBroadphaseProxy*	createProxy(  const btVector3& aabbMin,  const btVector3& aabbMax,int shapeType,void* userPtr, short int collisionFilterGroup,short int collisionFilterMask, btDispatcher* dispatcher);
 	virtual void	destroyProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher);
-	virtual void	setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax);
+	virtual void	setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax, btDispatcher* dispatcher);
 	
 	///calculateOverlappingPairs is optional: incremental algorithms (sweep and prune) might do it during the set aabb
 	virtual void	calculateOverlappingPairs(btDispatcher* dispatcher);
diff --git a/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp b/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp
index 5642fac41..5e1ca15c1 100644
--- a/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp
+++ b/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp
@@ -28,6 +28,19 @@ m_blockedForChanges(false),
 m_overlapFilterCallback(0)
 //m_NumOverlapBroadphasePair(0)
 {
+
+#ifdef USE_HASH_PAIRCACHE
+	m_overlappingPairArray.reserve(b2_maxPairs);
+
+	for (int32 i = 0; i < b2_tableCapacity; ++i)
+	{
+		m_hashTable[i] = b2_nullPair;
+	}
+	for (int32 i = 0; i < b2_maxPairs; ++i)
+	{
+		m_next[i] = b2_nullPair;
+	}
+#endif //USE_HASH_PAIRCACHE
 }
 
 
@@ -36,23 +49,6 @@ btOverlappingPairCache::~btOverlappingPairCache()
 	//todo/test: show we erase/delete data, or is it automatic
 }
 
-
-void	btOverlappingPairCache::removeOverlappingPair(btBroadphasePair& findPair, btDispatcher* dispatcher )
-{
-	
-	int findIndex = m_overlappingPairArray.findLinearSearch(findPair);
-	if (findIndex < m_overlappingPairArray.size())
-	{
-		gOverlappingPairs--;
-		btBroadphasePair& pair = m_overlappingPairArray[findIndex];
-		cleanOverlappingPair(pair,dispatcher);
-		
-		m_overlappingPairArray.swap(findIndex,m_overlappingPairArray.size()-1);
-		m_overlappingPairArray.pop_back();
-	}
-}
-
-
 void	btOverlappingPairCache::cleanOverlappingPair(btBroadphasePair& pair,btDispatcher* dispatcher)
 {
 	if (pair.m_algorithm)
@@ -66,50 +62,6 @@ void	btOverlappingPairCache::cleanOverlappingPair(btBroadphasePair& pair,btDispa
 }
 
 
-
-
-
-void	btOverlappingPairCache::addOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1)
-{
-	//don't add overlap with own
-	assert(proxy0 != proxy1);
-
-	if (!needsBroadphaseCollision(proxy0,proxy1))
-		return;
-
-
-	btBroadphasePair pair(*proxy0,*proxy1);
-	
-	m_overlappingPairArray.push_back(pair);
-	gOverlappingPairs++;
-	
-}
-
-///this findPair becomes really slow. Either sort the list to speedup the query, or
-///use a different solution. It is mainly used for Removing overlapping pairs. Removal could be delayed.
-///we could keep a linked list in each proxy, and store pair in one of the proxies (with lowest memory address)
-///Also we can use a 2D bitmap, which can be useful for a future GPU implementation
- btBroadphasePair*	btOverlappingPairCache::findPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1)
-{
-	if (!needsBroadphaseCollision(proxy0,proxy1))
-		return 0;
-
-	btBroadphasePair tmpPair(*proxy0,*proxy1);
-	int findIndex = m_overlappingPairArray.findLinearSearch(tmpPair);
-
-	if (findIndex < m_overlappingPairArray.size())
-	{
-		//assert(it != m_overlappingPairSet.end());
-		 btBroadphasePair* pair = &m_overlappingPairArray[findIndex];
-		return pair;
-	}
-	return 0;
-}
-
-
-
-
-
 void	btOverlappingPairCache::cleanProxyFromPairs(btBroadphaseProxy* proxy,btDispatcher* dispatcher)
 {
 
@@ -144,8 +96,6 @@ void	btOverlappingPairCache::cleanProxyFromPairs(btBroadphaseProxy* proxy,btDisp
 
 }
 
-
-
 void	btOverlappingPairCache::removeOverlappingPairsContainingProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher)
 {
 
@@ -173,6 +123,317 @@ void	btOverlappingPairCache::removeOverlappingPairsContainingProxy(btBroadphaseP
 }
 
 
+#ifdef USE_HASH_PAIRCACHE
+
+// Thomas Wang's hash, see: http://www.concentric.net/~Ttwang/tech/inthash.htm
+// This assumes proxyId1 and proxyId2 are 16-bit.
+inline uint32 Hash(uint32 proxyId1, uint32 proxyId2)
+{
+	uint32 key = (proxyId2 << 16) | proxyId1;
+	key = ~key + (key << 15);
+	key = key ^ (key >> 12);
+	key = key + (key << 2);
+	key = key ^ (key >> 4);
+	key = key * 2057;
+	key = key ^ (key >> 16);
+	return key;
+}
+
+inline bool Equals(const btBroadphasePair& pair, int32 proxyId1, int32 proxyId2)
+{	
+	return pair.m_pProxy0->getUid() == proxyId1 && pair.m_pProxy1->getUid() == proxyId2;
+}
+
+inline btBroadphasePair* btOverlappingPairCache::Find(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1, uint32 hash)
+{
+	int32 proxyId1 = proxy0->getUid();
+	int32 proxyId2 = proxy1->getUid();
+	if (proxyId1 > proxyId2) 
+		btSwap(proxyId1, proxyId2);
+
+	int32 index = m_hashTable[hash];
+	
+	while( index != b2_nullPair && Equals(m_overlappingPairArray[index], proxyId1, proxyId2) == false)
+	{
+		index = m_next[index];
+	}
+
+	if ( index == b2_nullPair )
+	{
+		return NULL;
+	}
+
+	btAssert(index < m_overlappingPairArray.size());
+
+	return &m_overlappingPairArray[index];
+}
+
+btBroadphasePair* btOverlappingPairCache::Find(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1)
+{
+	int32 proxyId1 = proxy0->getUid();
+	int32 proxyId2 = proxy1->getUid();
+
+	if (proxyId1 > proxyId2) 
+		btSwap(proxyId1, proxyId2);
+
+	int32 hash = Hash(proxyId1, proxyId2) & b2_tableMask;
+
+	int32 index = m_hashTable[hash];
+	while (index != b2_nullPair && Equals(m_overlappingPairArray[index], proxyId1, proxyId2) == false)
+	{
+		index = m_next[index];
+	}
+
+	if (index == b2_nullPair)
+	{
+		return NULL;
+	}
+
+	btAssert(index < m_overlappingPairArray.size());
+
+	return &m_overlappingPairArray[index];
+}
+
+#include <stdio.h>
+
+btBroadphasePair* btOverlappingPairCache::Add(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1)
+{
+	int32 proxyId1 = proxy0->getUid();
+	int32 proxyId2 = proxy1->getUid();
+
+	if (proxyId1 > proxyId2) 
+		btSwap(proxyId1, proxyId2);
+
+	int32 hash = Hash(proxyId1, proxyId2) & b2_tableMask;
+
+	
+
+	btBroadphasePair* pair = Find(proxy0, proxy1, hash);
+	if (pair != NULL)
+	{
+		return pair;
+	}
+
+	if (m_overlappingPairArray.size() == b2_maxPairs)
+	{
+		btAssert(false);
+		return NULL;
+	}
+	
+	int count = m_overlappingPairArray.size();
+	void* mem = &m_overlappingPairArray.expand();
+	
+	pair = new (mem) btBroadphasePair(*proxy0,*proxy1);
+//	pair->m_pProxy0 = proxy0;
+//	pair->m_pProxy1 = proxy1;
+	pair->m_algorithm = 0;
+	pair->m_userInfo = 0;
+	
+
+	m_next[count] = m_hashTable[hash];
+	m_hashTable[hash] = (uint16)count;
+
+	return pair;
+}
+
+
+void* btOverlappingPairCache::Remove(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1,btDispatcher* dispatcher)
+{
+	int32 proxyId1 = proxy0->getUid();
+	int32 proxyId2 = proxy1->getUid();
+
+	if (proxyId1 > proxyId2) 
+		btSwap(proxyId1, proxyId2);
+
+	int32 hash = Hash(proxyId1, proxyId2) & b2_tableMask;
+
+	btBroadphasePair* pair = Find(proxy0, proxy1, hash);
+	if (pair == NULL)
+	{
+		return NULL;
+	}
+
+	cleanOverlappingPair(*pair,dispatcher);
+
+	void* userData = pair->m_userInfo;
+
+	btAssert(pair->m_pProxy0->getUid() == proxyId1);
+	btAssert(pair->m_pProxy1->getUid() == proxyId2);
+
+	int32 pairIndex = int32(pair - &m_overlappingPairArray[0]);
+	btAssert(pairIndex < m_overlappingPairArray.size());
+
+	// Remove the pair from the hash table.
+	int32 index = m_hashTable[hash];
+	btAssert(index != b2_nullPair);
+
+	int32 previous = b2_nullPair;
+	while (index != pairIndex)
+	{
+		previous = index;
+		index = m_next[index];
+	}
+
+	if (previous != b2_nullPair)
+	{
+		btAssert(m_next[previous] == pairIndex);
+		m_next[previous] = m_next[pairIndex];
+	}
+	else
+	{
+		m_hashTable[hash] = m_next[pairIndex];
+	}
+
+	// We now move the last pair into spot of the
+	// pair being removed. We need to fix the hash
+	// table indices to support the move.
+
+	int32 lastPairIndex = m_overlappingPairArray.size() - 1;
+
+	// If the removed pair is the last pair, we are done.
+	if (lastPairIndex == pairIndex)
+	{
+		m_overlappingPairArray.pop_back();
+		return userData;
+	}
+
+	// Remove the last pair from the hash table.
+	const btBroadphasePair* last = &m_overlappingPairArray[lastPairIndex];
+	int32 lastHash = Hash(last->m_pProxy0->getUid(), last->m_pProxy1->getUid()) & b2_tableMask;
+
+	index = m_hashTable[lastHash];
+	btAssert(index != b2_nullPair);
+
+	previous = b2_nullPair;
+	while (index != lastPairIndex)
+	{
+		previous = index;
+		index = m_next[index];
+	}
+
+	if (previous != b2_nullPair)
+	{
+		btAssert(m_next[previous] == lastPairIndex);
+		m_next[previous] = m_next[lastPairIndex];
+	}
+	else
+	{
+		m_hashTable[lastHash] = m_next[lastPairIndex];
+	}
+
+	// Copy the last pair into the remove pair's spot.
+	m_overlappingPairArray[pairIndex] = m_overlappingPairArray[lastPairIndex];
+
+	// Insert the last pair into the hash table
+	m_next[pairIndex] = m_hashTable[lastHash];
+	m_hashTable[lastHash] = (uint16)pairIndex;
+
+	m_overlappingPairArray.pop_back();
+
+	return userData;
+}
+#include <stdio.h>
+
+void	btOverlappingPairCache::processAllOverlappingPairs(btOverlapCallback* callback,btDispatcher* dispatcher)
+{
+
+	int i;
+
+	//printf("m_overlappingPairArray.size()=%d\n",m_overlappingPairArray.size());
+	for (i=0;i<m_overlappingPairArray.size();)
+	{
+	
+		btBroadphasePair* pair = &m_overlappingPairArray[i];
+		if (callback->processOverlap(*pair))
+		{
+			removeOverlappingPair(pair->m_pProxy0,pair->m_pProxy1,dispatcher);
+
+			gOverlappingPairs--;
+		} else
+		{
+			i++;
+		}
+	}
+}
+
+#else
+
+
+
+
+void	btOverlappingPairCache::removeOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1, btDispatcher* dispatcher )
+{
+#ifndef USE_LAZY_REMOVAL
+
+	btBroadphasePair findPair(*proxy0,*proxy1);
+
+	int findIndex = m_overlappingPairArray.findLinearSearch(findPair);
+	if (findIndex < m_overlappingPairArray.size())
+	{
+		gOverlappingPairs--;
+		btBroadphasePair& pair = m_overlappingPairArray[findIndex];
+		cleanOverlappingPair(pair,dispatcher);
+		
+		m_overlappingPairArray.swap(findIndex,m_overlappingPairArray.size()-1);
+		m_overlappingPairArray.pop_back();
+	}
+#endif //USE_LAZY_REMOVAL
+
+}
+
+
+
+
+
+
+
+
+void	btOverlappingPairCache::addOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1)
+{
+	//don't add overlap with own
+	assert(proxy0 != proxy1);
+
+	if (!needsBroadphaseCollision(proxy0,proxy1))
+		return;
+
+
+	btBroadphasePair pair(*proxy0,*proxy1);
+	m_overlappingPairArray.push_back(pair);
+	gOverlappingPairs++;
+	
+}
+
+///this findPair becomes really slow. Either sort the list to speedup the query, or
+///use a different solution. It is mainly used for Removing overlapping pairs. Removal could be delayed.
+///we could keep a linked list in each proxy, and store pair in one of the proxies (with lowest memory address)
+///Also we can use a 2D bitmap, which can be useful for a future GPU implementation
+ btBroadphasePair*	btOverlappingPairCache::findPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1)
+{
+	if (!needsBroadphaseCollision(proxy0,proxy1))
+		return 0;
+
+	btBroadphasePair tmpPair(*proxy0,*proxy1);
+	int findIndex = m_overlappingPairArray.findLinearSearch(tmpPair);
+
+	if (findIndex < m_overlappingPairArray.size())
+	{
+		//assert(it != m_overlappingPairSet.end());
+		 btBroadphasePair* pair = &m_overlappingPairArray[findIndex];
+		return pair;
+	}
+	return 0;
+}
+
+
+
+
+
+
+
+
+
+
+#include <stdio.h>
 
 void	btOverlappingPairCache::processAllOverlappingPairs(btOverlapCallback* callback,btDispatcher* dispatcher)
 {
@@ -199,4 +460,4 @@ void	btOverlappingPairCache::processAllOverlappingPairs(btOverlapCallback* callb
 
 
 
-
+#endif //USE_HASH_PAIRCACHE
diff --git a/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h b/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h
index 219d7a13d..693c66d56 100644
--- a/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h
+++ b/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h
@@ -1,129 +1,263 @@
-
-/*
-Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
-
-This software is provided 'as-is', without any express or implied warranty.
-In no event will the authors be held liable for any damages arising from the use of this software.
-Permission is granted to anyone to use this software for any purpose, 
-including commercial applications, and to alter it and redistribute it freely, 
-subject to the following restrictions:
-
-1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
-2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
-3. This notice may not be removed or altered from any source distribution.
-*/
-
-#ifndef OVERLAPPING_PAIR_CACHE_H
-#define OVERLAPPING_PAIR_CACHE_H
-
-
-#include "btBroadphaseInterface.h"
-#include "btBroadphaseProxy.h"
-#include "LinearMath/btPoint3.h"
-#include "LinearMath/btAlignedObjectArray.h"
-class btDispatcher;
-
-struct	btOverlapCallback
-{
-	virtual ~btOverlapCallback()
-	{}
-	//return true for deletion of the pair
-	virtual bool	processOverlap(btBroadphasePair& pair) = 0;
-};
-
-struct btOverlapFilterCallback
-{
-	virtual ~btOverlapFilterCallback()
-	{}
-	// return true when pairs need collision
-	virtual bool	needBroadphaseCollision(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1) const = 0;
-};
-
-typedef btAlignedObjectArray<btBroadphasePair>	btBroadphasePairArray;
-
-///btOverlappingPairCache maintains the objects with overlapping AABB
-///Typically managed by the Broadphase, Axis3Sweep or btSimpleBroadphase
-class	btOverlappingPairCache
-{
-	protected:
-		//avoid brute-force finding all the time
-		btBroadphasePairArray	m_overlappingPairArray;
-		
-		//during the dispatch, check that user doesn't destroy/create proxy
-		bool		m_blockedForChanges;
-		
-		//if set, use the callback instead of the built in filter in needBroadphaseCollision
-		btOverlapFilterCallback* m_overlapFilterCallback;
-
-	public:
-			
-		btOverlappingPairCache();	
-		virtual ~btOverlappingPairCache();
-
-		virtual void	processAllOverlappingPairs(btOverlapCallback*,btDispatcher* dispatcher);
-
-		void	removeOverlappingPair(btBroadphasePair& pair,btDispatcher* dispatcher);
-
-		void	cleanOverlappingPair(btBroadphasePair& pair,btDispatcher* dispatcher);
-		
-		void	addOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1);
-
-		btBroadphasePair*	findPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1);
-			
-		
-		void	cleanProxyFromPairs(btBroadphaseProxy* proxy,btDispatcher* dispatcher);
-
-		void	removeOverlappingPairsContainingProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher);
-
-
-		inline bool needsBroadphaseCollision(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1) const
-		{
-			if (m_overlapFilterCallback)
-				return m_overlapFilterCallback->needBroadphaseCollision(proxy0,proxy1);
-
-			bool collides = (proxy0->m_collisionFilterGroup & proxy1->m_collisionFilterMask) != 0;
-			collides = collides && (proxy1->m_collisionFilterGroup & proxy0->m_collisionFilterMask);
-			
-			return collides;
-		}
-		
-		btBroadphasePairArray&	getOverlappingPairArray()
-		{
-			return m_overlappingPairArray;
-		}
-
-		const btBroadphasePairArray&	getOverlappingPairArray() const
-		{
-			return m_overlappingPairArray;
-		}
-
-		btBroadphasePair*	getOverlappingPairArrayPtr()
-		{
-			return &m_overlappingPairArray[0];
-		}
-
-		const btBroadphasePair*	getOverlappingPairArrayPtr() const
-		{
-			return &m_overlappingPairArray[0];
-		}
-
-		int	getNumOverlappingPairs() const
-		{
-			return m_overlappingPairArray.size();
-		}
-		
-		btOverlapFilterCallback* getOverlapFilterCallback()
-		{
-			return m_overlapFilterCallback;
-		}
-
-		void setOverlapFilterCallback(btOverlapFilterCallback* callback)
-		{
-			m_overlapFilterCallback = callback;
-		}
-
-};
-#endif //OVERLAPPING_PAIR_CACHE_H
-
-
+
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef OVERLAPPING_PAIR_CACHE_H
+#define OVERLAPPING_PAIR_CACHE_H
+
+
+#include "btBroadphaseInterface.h"
+#include "btBroadphaseProxy.h"
+#include "LinearMath/btPoint3.h"
+#include "LinearMath/btAlignedObjectArray.h"
+class btDispatcher;
+
+#define USE_HASH_PAIRCACHE 1
+
+
+struct	btOverlapCallback
+{
+	virtual ~btOverlapCallback()
+	{}
+	//return true for deletion of the pair
+	virtual bool	processOverlap(btBroadphasePair& pair) = 0;
+};
+
+struct btOverlapFilterCallback
+{
+	virtual ~btOverlapFilterCallback()
+	{}
+	// return true when pairs need collision
+	virtual bool	needBroadphaseCollision(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1) const = 0;
+};
+
+typedef btAlignedObjectArray<btBroadphasePair>	btBroadphasePairArray;
+
+#ifdef USE_HASH_PAIRCACHE
+
+
+
+const int b2_maxPairs = 65536;//32768;
+typedef unsigned short int uint16;
+typedef int int32;
+typedef unsigned int uint32;
+
+/// Hash-space based Pair Cache, thanks to Erin Catto, Box2D, http://www.box2d.org, and Pierre Terdiman, Codercorner, http://codercorner.com
+const uint16 b2_nullPair = 0xffff;
+const uint16 b2_nullProxy = 0xffff;
+const int32 b2_tableCapacity = b2_maxPairs;	// must be a power of two
+const int32 b2_tableMask = b2_tableCapacity - 1;
+
+
+class btOverlappingPairCache
+{
+	btBroadphasePairArray	m_overlappingPairArray;
+	btOverlapFilterCallback* m_overlapFilterCallback;
+	bool		m_blockedForChanges;
+
+public:
+	btOverlappingPairCache();
+	~btOverlappingPairCache();
+
+	
+	void	removeOverlappingPairsContainingProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher);
+
+	void	removeOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1,btDispatcher* dispatcher)
+	{
+		Remove(proxy0,proxy1,dispatcher);
+	}
+
+	inline bool needsBroadphaseCollision(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1) const
+	{
+		if (m_overlapFilterCallback)
+			return m_overlapFilterCallback->needBroadphaseCollision(proxy0,proxy1);
+
+		bool collides = (proxy0->m_collisionFilterGroup & proxy1->m_collisionFilterMask) != 0;
+		collides = collides && (proxy1->m_collisionFilterGroup & proxy0->m_collisionFilterMask);
+		
+		return collides;
+	}
+
+	void	addOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1)
+	{
+		if (!needsBroadphaseCollision(proxy0,proxy1))
+			return;
+
+		Add(proxy0,proxy1);
+	}
+
+	btBroadphasePair*	findPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1)
+	{
+		return Find(proxy0,proxy1);
+	}
+
+	void	cleanProxyFromPairs(btBroadphaseProxy* proxy,btDispatcher* dispatcher);
+
+	
+	virtual void	processAllOverlappingPairs(btOverlapCallback*,btDispatcher* dispatcher);
+
+	btBroadphasePair*	getOverlappingPairArrayPtr()
+	{
+		return &m_overlappingPairArray[0];
+	}
+
+	const btBroadphasePair*	getOverlappingPairArrayPtr() const
+	{
+		return &m_overlappingPairArray[0];
+	}
+
+	btBroadphasePairArray&	getOverlappingPairArray()
+	{
+		return m_overlappingPairArray;
+	}
+
+	const btBroadphasePairArray&	getOverlappingPairArray() const
+	{
+		return m_overlappingPairArray;
+	}
+
+	void	cleanOverlappingPair(btBroadphasePair& pair,btDispatcher* dispatcher);
+
+	// Add a pair and return the new pair. If the pair already exists,
+	// no new pair is created and the old one is returned.
+	btBroadphasePair* Add(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1);
+
+	// Remove a pair, return the pair's userData.
+	void* Remove(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1,btDispatcher* dispatcher);
+
+	btBroadphasePair* Find(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1);
+
+	int32 GetCount() const { return m_overlappingPairArray.size(); }
+//	btBroadphasePair* GetPairs() { return m_pairs; }
+
+	btOverlapFilterCallback* getOverlapFilterCallback()
+	{
+		return m_overlapFilterCallback;
+	}
+
+	void setOverlapFilterCallback(btOverlapFilterCallback* callback)
+	{
+		m_overlapFilterCallback = callback;
+	}
+
+	int	getNumOverlappingPairs() const
+	{
+		return m_overlappingPairArray.size();
+	}
+private:
+	btBroadphasePair* Find(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1, uint32 hashValue);
+
+public:
+	
+	uint16 m_hashTable[b2_tableCapacity];
+	uint16 m_next[b2_maxPairs];
+};
+
+
+
+#else//USE_HASH_PAIRCACHE
+
+#define USE_LAZY_REMOVAL 1
+
+///btOverlappingPairCache maintains the objects with overlapping AABB
+///Typically managed by the Broadphase, Axis3Sweep or btSimpleBroadphase
+class	btOverlappingPairCache
+{
+	protected:
+		//avoid brute-force finding all the time
+		btBroadphasePairArray	m_overlappingPairArray;
+
+		//during the dispatch, check that user doesn't destroy/create proxy
+		bool		m_blockedForChanges;
+		
+		//if set, use the callback instead of the built in filter in needBroadphaseCollision
+		btOverlapFilterCallback* m_overlapFilterCallback;
+
+	public:
+			
+		btOverlappingPairCache();	
+		virtual ~btOverlappingPairCache();
+
+		virtual void	processAllOverlappingPairs(btOverlapCallback*,btDispatcher* dispatcher);
+
+		void	removeOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1,btDispatcher* dispatcher);
+
+		void	cleanOverlappingPair(btBroadphasePair& pair,btDispatcher* dispatcher);
+		
+		void	addOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1);
+
+		btBroadphasePair*	findPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1);
+			
+		
+		void	cleanProxyFromPairs(btBroadphaseProxy* proxy,btDispatcher* dispatcher);
+
+		void	removeOverlappingPairsContainingProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher);
+
+
+		inline bool needsBroadphaseCollision(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1) const
+		{
+			if (m_overlapFilterCallback)
+				return m_overlapFilterCallback->needBroadphaseCollision(proxy0,proxy1);
+
+			bool collides = (proxy0->m_collisionFilterGroup & proxy1->m_collisionFilterMask) != 0;
+			collides = collides && (proxy1->m_collisionFilterGroup & proxy0->m_collisionFilterMask);
+			
+			return collides;
+		}
+		
+		btBroadphasePairArray&	getOverlappingPairArray()
+		{
+			return m_overlappingPairArray;
+		}
+
+		const btBroadphasePairArray&	getOverlappingPairArray() const
+		{
+			return m_overlappingPairArray;
+		}
+
+		
+
+
+		btBroadphasePair*	getOverlappingPairArrayPtr()
+		{
+			return &m_overlappingPairArray[0];
+		}
+
+		const btBroadphasePair*	getOverlappingPairArrayPtr() const
+		{
+			return &m_overlappingPairArray[0];
+		}
+
+		int	getNumOverlappingPairs() const
+		{
+			return m_overlappingPairArray.size();
+		}
+		
+		btOverlapFilterCallback* getOverlapFilterCallback()
+		{
+			return m_overlapFilterCallback;
+		}
+
+		void setOverlapFilterCallback(btOverlapFilterCallback* callback)
+		{
+			m_overlapFilterCallback = callback;
+		}
+
+};
+#endif //USE_HASH_PAIRCACHE
+
+#endif //OVERLAPPING_PAIR_CACHE_H
+
+
diff --git a/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp b/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp
index ef4cff3d8..04f3ea144 100644
--- a/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp
+++ b/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp
@@ -26,22 +26,18 @@ extern int gOverlappingPairs;
 
 void	btSimpleBroadphase::validate()
 {
-	for (int i=0;i<m_numProxies;i++)
+	for (int i=0;i<m_numHandles;i++)
 	{
-		for (int j=i+1;j<m_numProxies;j++)
+		for (int j=i+1;j<m_numHandles;j++)
 		{
-			assert(m_pProxies[i] != m_pProxies[j]);
+			btAssert(&m_pHandles[i] != &m_pHandles[j]);
 		}
 	}
 	
 }
 
 btSimpleBroadphase::btSimpleBroadphase(int maxProxies, btOverlappingPairCache* overlappingPairCache)
-	:
-	m_firstFreeProxy(0),
-	m_numProxies(0),
-	m_maxProxies(maxProxies),
-	m_invalidPair(0),
+	:m_invalidPair(0),
 	m_pairCache(overlappingPairCache),
 	m_ownsPairCache(false)
 {
@@ -52,31 +48,31 @@ btSimpleBroadphase::btSimpleBroadphase(int maxProxies, btOverlappingPairCache* o
 		m_ownsPairCache = true;
 	}
 
-	m_proxies = new btSimpleBroadphaseProxy[maxProxies];
-	m_freeProxies = new int[maxProxies];
-	m_pProxies = new btSimpleBroadphaseProxy*[maxProxies];
-	
+	// allocate handles buffer and put all handles on free list
+	void* ptr = btAlignedAlloc(sizeof(btSimpleBroadphaseProxy)*maxProxies,16);
+	m_pHandles = new(ptr) btSimpleBroadphaseProxy[maxProxies];
+	m_maxHandles = maxProxies;
+	m_numHandles = 0;
+	m_firstFreeHandle = 0;
+	m_firstAllocatedHandle = -1;
 
-	int i;
-	for (i=0;i<m_maxProxies;i++)
 	{
-		m_freeProxies[i] = i;
+		for (int i = m_firstFreeHandle; i < maxProxies; i++)
+		{
+			m_pHandles[i].SetNextFree(i + 1);
+			m_pHandles[i].m_uniqueId = i+2;//any UID will do, we just avoid too trivial values (0,1) for debugging purposes
+			m_pHandles[i].SetNextAllocated(-1);
+		}
+		m_pHandles[maxProxies - 1].SetNextFree(0);
+		m_pHandles[maxProxies - 1].SetNextAllocated(-1);
+	
 	}
+
 }
 
 btSimpleBroadphase::~btSimpleBroadphase()
 {
-	delete[] m_proxies;
-	delete []m_freeProxies;
-	delete [] m_pProxies;
-
-	/*int i;
-	for (i=m_numProxies-1;i>=0;i--)
-	{
-		BP_Proxy* proxy = m_pProxies[i]; 
-		destroyProxy(proxy);
-	}
-	*/
+	btAlignedFree(m_pHandles);
 
 	if (m_ownsPairCache)
 	{
@@ -85,27 +81,17 @@ btSimpleBroadphase::~btSimpleBroadphase()
 }
 
 
-btBroadphaseProxy*	btSimpleBroadphase::createProxy(  const btVector3& aabbMin,  const btVector3& aabbMax,int shapeType,void* userPtr ,short int collisionFilterGroup,short int collisionFilterMask)
+btBroadphaseProxy*	btSimpleBroadphase::createProxy(  const btVector3& aabbMin,  const btVector3& aabbMax,int shapeType,void* userPtr ,short int collisionFilterGroup,short int collisionFilterMask, btDispatcher* dispatcher)
 {
-	if (m_numProxies >= m_maxProxies)
+	if (m_numHandles >= m_maxHandles)
 	{
-		assert(0);
+		btAssert(0);
 		return 0; //should never happen, but don't let the game crash ;-)
 	}
 	assert(aabbMin[0]<= aabbMax[0] && aabbMin[1]<= aabbMax[1] && aabbMin[2]<= aabbMax[2]);
 
-	int freeIndex= m_freeProxies[m_firstFreeProxy];
-	btSimpleBroadphaseProxy* proxy = new (&m_proxies[freeIndex])btSimpleBroadphaseProxy(aabbMin,aabbMax,shapeType,userPtr,collisionFilterGroup,collisionFilterMask);
-	m_firstFreeProxy++;
-
-	btSimpleBroadphaseProxy* proxy1 = &m_proxies[0];
-		
-	int	index = int(proxy - proxy1);
-	btAssert(index == freeIndex);
-
-	m_pProxies[m_numProxies] = proxy;
-	m_numProxies++;
-	//validate();
+	int newHandleIndex = allocHandle();
+	btSimpleBroadphaseProxy* proxy = new (&m_pHandles[newHandleIndex])btSimpleBroadphaseProxy(aabbMin,aabbMax,shapeType,userPtr,collisionFilterGroup,collisionFilterMask);
 
 	return proxy;
 }
@@ -145,28 +131,15 @@ void	btSimpleBroadphase::destroyProxy(btBroadphaseProxy* proxyOrg,btDispatcher*
 		int i;
 		
 		btSimpleBroadphaseProxy* proxy0 = static_cast<btSimpleBroadphaseProxy*>(proxyOrg);
-		btSimpleBroadphaseProxy* proxy1 = &m_proxies[0];
-	
-		int index = int(proxy0 - proxy1);
-		btAssert (index < m_maxProxies);
-		m_freeProxies[--m_firstFreeProxy] = index;
+		freeHandle(proxy0);
 
 		m_pairCache->removeOverlappingPairsContainingProxy(proxyOrg,dispatcher);
-		
-		for (i=0;i<m_numProxies;i++)
-		{
-			if (m_pProxies[i] == proxyOrg)
-			{
-				m_pProxies[i] = m_pProxies[m_numProxies-1];
-				break;
-			}
-		}
-		m_numProxies--;
+
 		//validate();
 		
 }
 
-void	btSimpleBroadphase::setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax)
+void	btSimpleBroadphase::setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax, btDispatcher* dispatcher)
 {
 	btSimpleBroadphaseProxy* sbp = getSimpleProxyFromProxy(proxy);
 	sbp->m_min = aabbMin;
@@ -206,99 +179,124 @@ void	btSimpleBroadphase::calculateOverlappingPairs(btDispatcher* dispatcher)
 	//first check for new overlapping pairs
 	int i,j;
 
-	for (i=0;i<m_numProxies;i++)
+	if (m_firstAllocatedHandle >= 0)
 	{
-		btBroadphaseProxy* proxy0 = m_pProxies[i];
-		for (j=i+1;j<m_numProxies;j++)
-		{
-			btBroadphaseProxy* proxy1 = m_pProxies[j];
-			btSimpleBroadphaseProxy* p0 = getSimpleProxyFromProxy(proxy0);
-			btSimpleBroadphaseProxy* p1 = getSimpleProxyFromProxy(proxy1);
 
-			if (aabbOverlap(p0,p1))
+		btSimpleBroadphaseProxy* proxy0 = &m_pHandles[m_firstAllocatedHandle];
+
+		for (i=0;i<m_numHandles;i++)
+		{
+			btSimpleBroadphaseProxy* proxy1 = &m_pHandles[m_firstAllocatedHandle];
+
+			for (j=0;j<m_numHandles;j++)
 			{
-				if ( !m_pairCache->findPair(proxy0,proxy1))
+				
+				if (proxy0 != proxy1)
 				{
-					m_pairCache->addOverlappingPair(proxy0,proxy1);
+					btSimpleBroadphaseProxy* p0 = getSimpleProxyFromProxy(proxy0);
+					btSimpleBroadphaseProxy* p1 = getSimpleProxyFromProxy(proxy1);
+
+					if (aabbOverlap(p0,p1))
+					{
+						if ( !m_pairCache->findPair(proxy0,proxy1))
+						{
+							m_pairCache->addOverlappingPair(proxy0,proxy1);
+						}
+					} else
+					{
+		#ifdef USE_HASH_PAIRCACHE
+						if ( m_pairCache->findPair(proxy0,proxy1))
+						{
+							m_pairCache->removeOverlappingPair(proxy0,proxy1,dispatcher);
+						}
+		#endif //USE_HASH_PAIRCACHE
+
+					}
 				}
+				proxy1 = &m_pHandles[proxy1->GetNextAllocated()];
+
 			}
+			proxy0 = &m_pHandles[proxy0->GetNextAllocated()];
 
 		}
-	}
 
-	if (m_ownsPairCache)
-	{
-		
-		btBroadphasePairArray&	overlappingPairArray = m_pairCache->getOverlappingPairArray();
+	#ifndef USE_HASH_PAIRCACHE
 
-		//perform a sort, to find duplicates and to sort 'invalid' pairs to the end
-		overlappingPairArray.heapSort(btBroadphasePairSortPredicate());
-
-		overlappingPairArray.resize(overlappingPairArray.size() - m_invalidPair);
-		m_invalidPair = 0;
-
-
-		btBroadphasePair previousPair;
-		previousPair.m_pProxy0 = 0;
-		previousPair.m_pProxy1 = 0;
-		previousPair.m_algorithm = 0;
-		
-		
-		for (i=0;i<overlappingPairArray.size();i++)
+		if (m_ownsPairCache)
 		{
-		
-			btBroadphasePair& pair = overlappingPairArray[i];
+			
+			btBroadphasePairArray&	overlappingPairArray = m_pairCache->getOverlappingPairArray();
 
-			bool isDuplicate = (pair == previousPair);
+			//perform a sort, to find duplicates and to sort 'invalid' pairs to the end
+			overlappingPairArray.heapSort(btBroadphasePairSortPredicate());
 
-			previousPair = pair;
+			overlappingPairArray.resize(overlappingPairArray.size() - m_invalidPair);
+			m_invalidPair = 0;
 
-			bool needsRemoval = false;
 
-			if (!isDuplicate)
+			btBroadphasePair previousPair;
+			previousPair.m_pProxy0 = 0;
+			previousPair.m_pProxy1 = 0;
+			previousPair.m_algorithm = 0;
+			
+			
+			for (i=0;i<overlappingPairArray.size();i++)
 			{
-				bool hasOverlap = testAabbOverlap(pair.m_pProxy0,pair.m_pProxy1);
+			
+				btBroadphasePair& pair = overlappingPairArray[i];
 
-				if (hasOverlap)
+				bool isDuplicate = (pair == previousPair);
+
+				previousPair = pair;
+
+				bool needsRemoval = false;
+
+				if (!isDuplicate)
 				{
-					needsRemoval = false;//callback->processOverlap(pair);
+					bool hasOverlap = testAabbOverlap(pair.m_pProxy0,pair.m_pProxy1);
+
+					if (hasOverlap)
+					{
+						needsRemoval = false;//callback->processOverlap(pair);
+					} else
+					{
+						needsRemoval = true;
+					}
 				} else
 				{
+					//remove duplicate
 					needsRemoval = true;
+					//should have no algorithm
+					btAssert(!pair.m_algorithm);
 				}
-			} else
-			{
-				//remove duplicate
-				needsRemoval = true;
-				//should have no algorithm
-				btAssert(!pair.m_algorithm);
+				
+				if (needsRemoval)
+				{
+					m_pairCache->cleanOverlappingPair(pair,dispatcher);
+
+			//		m_overlappingPairArray.swap(i,m_overlappingPairArray.size()-1);
+			//		m_overlappingPairArray.pop_back();
+					pair.m_pProxy0 = 0;
+					pair.m_pProxy1 = 0;
+					m_invalidPair++;
+					gOverlappingPairs--;
+				} 
+				
 			}
-			
-			if (needsRemoval)
-			{
-				m_pairCache->cleanOverlappingPair(pair,dispatcher);
 
-		//		m_overlappingPairArray.swap(i,m_overlappingPairArray.size()-1);
-		//		m_overlappingPairArray.pop_back();
-				pair.m_pProxy0 = 0;
-				pair.m_pProxy1 = 0;
-				m_invalidPair++;
-				gOverlappingPairs--;
-			} 
-			
+		///if you don't like to skip the invalid pairs in the array, execute following code:
+		#define CLEAN_INVALID_PAIRS 1
+		#ifdef CLEAN_INVALID_PAIRS
+
+			//perform a sort, to sort 'invalid' pairs to the end
+			overlappingPairArray.heapSort(btBroadphasePairSortPredicate());
+
+			overlappingPairArray.resize(overlappingPairArray.size() - m_invalidPair);
+			m_invalidPair = 0;
+		#endif//CLEAN_INVALID_PAIRS
+
 		}
-
-	///if you don't like to skip the invalid pairs in the array, execute following code:
-	#define CLEAN_INVALID_PAIRS 1
-	#ifdef CLEAN_INVALID_PAIRS
-
-		//perform a sort, to sort 'invalid' pairs to the end
-		overlappingPairArray.heapSort(btBroadphasePairSortPredicate());
-
-		overlappingPairArray.resize(overlappingPairArray.size() - m_invalidPair);
-		m_invalidPair = 0;
-	#endif//CLEAN_INVALID_PAIRS
-	
+	#endif //USE_HASH_PAIRCACHE
 	}
 }
 
diff --git a/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.h b/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.h
index 1cd4bde08..82847e7d1 100644
--- a/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.h
+++ b/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.h
@@ -24,6 +24,10 @@ struct btSimpleBroadphaseProxy : public btBroadphaseProxy
 {
 	btVector3	m_min;
 	btVector3	m_max;
+	int			m_nextFree;
+	int			m_nextAllocated;
+//	int			m_handleId;
+
 	
 	btSimpleBroadphaseProxy() {};
 
@@ -34,6 +38,13 @@ struct btSimpleBroadphaseProxy : public btBroadphaseProxy
 		(void)shapeType;
 	}
 	
+	
+	inline void SetNextFree(int next) {m_nextFree = next;}
+	inline int GetNextFree() const {return m_nextFree;}
+
+	inline void SetNextAllocated(int next) {m_nextAllocated = next;}
+	inline int GetNextAllocated() const {return m_nextAllocated;}
+
 
 };
 
@@ -43,19 +54,46 @@ class btSimpleBroadphase : public btBroadphaseInterface
 
 protected:
 
-	btSimpleBroadphaseProxy*	m_proxies;
-	int*				m_freeProxies;
-	int				m_firstFreeProxy;
+	int		m_numHandles;						// number of active handles
+	int		m_maxHandles;						// max number of handles
+	btSimpleBroadphaseProxy* m_pHandles;						// handles pool
+	int		m_firstFreeHandle;		// free handles list
+	int		m_firstAllocatedHandle;
+
+	int allocHandle()
+	{
+
+		int freeHandle = m_firstFreeHandle;
+		m_firstFreeHandle = m_pHandles[freeHandle].GetNextFree();
+		
+		m_pHandles[freeHandle].SetNextAllocated(m_firstAllocatedHandle);
+		m_firstAllocatedHandle = freeHandle;
+		
+		m_numHandles++;
+
+		return freeHandle;
+	}
+
+	void freeHandle(btSimpleBroadphaseProxy* proxy)
+	{
+		int handle = int(proxy-m_pHandles);
+		btAssert(handle > 0 && handle < m_maxHandles);
+
+		proxy->SetNextFree(m_firstFreeHandle);
+		m_firstFreeHandle = handle;
+
+		m_firstAllocatedHandle = proxy->GetNextAllocated();
+		proxy->SetNextAllocated(-1);
+
+		m_numHandles--;
+	}
 
-	btSimpleBroadphaseProxy** m_pProxies;
-	int				m_numProxies;
 
 	btOverlappingPairCache*	m_pairCache;
 	bool	m_ownsPairCache;
 
 	int	m_invalidPair;
 
-	int m_maxProxies;
 	
 	
 	inline btSimpleBroadphaseProxy*	getSimpleProxyFromProxy(btBroadphaseProxy* proxy)
@@ -80,12 +118,12 @@ public:
 		static bool	aabbOverlap(btSimpleBroadphaseProxy* proxy0,btSimpleBroadphaseProxy* proxy1);
 
 
-	virtual btBroadphaseProxy*	createProxy(  const btVector3& aabbMin,  const btVector3& aabbMax,int shapeType,void* userPtr ,short int collisionFilterGroup,short int collisionFilterMask);
+	virtual btBroadphaseProxy*	createProxy(  const btVector3& aabbMin,  const btVector3& aabbMax,int shapeType,void* userPtr ,short int collisionFilterGroup,short int collisionFilterMask, btDispatcher* dispatcher);
 
 	virtual void	calculateOverlappingPairs(btDispatcher* dispatcher);
 
 	virtual void	destroyProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher);
-	virtual void	setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax);
+	virtual void	setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax, btDispatcher* dispatcher);
 		
 	btOverlappingPairCache*	getOverlappingPairCache()
 	{
diff --git a/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp b/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp
index 37879b772..65c09bd85 100644
--- a/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp
+++ b/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp
@@ -81,6 +81,7 @@ btPersistentManifold*	btCollisionDispatcher::getNewManifold(void* b0,void* b1)
 	
 	void* mem = m_persistentManifoldPoolAllocator->allocate(sizeof(btPersistentManifold));
 	btPersistentManifold* manifold = new(mem) btPersistentManifold (body0,body1,0);
+	manifold->m_index1a = m_manifoldsPtr.size();
 	m_manifoldsPtr.push_back(manifold);
 
 	return manifold;
@@ -100,16 +101,14 @@ void btCollisionDispatcher::releaseManifold(btPersistentManifold* manifold)
 	//printf("releaseManifold: gNumManifold %d\n",gNumManifold);
 	clearManifold(manifold);
 
-	///todo: this can be improved a lot, linear search might be slow part!
-	int findIndex = m_manifoldsPtr.findLinearSearch(manifold);
-	if (findIndex < m_manifoldsPtr.size())
-	{
-		m_manifoldsPtr.swap(findIndex,m_manifoldsPtr.size()-1);
-		m_manifoldsPtr.pop_back();
+	int findIndex = manifold->m_index1a;
+	btAssert(findIndex < m_manifoldsPtr.size());
+	m_manifoldsPtr.swap(findIndex,m_manifoldsPtr.size()-1);
+	m_manifoldsPtr[findIndex]->m_index1a = findIndex;
+	m_manifoldsPtr.pop_back();
 
-		manifold->~btPersistentManifold();
-		m_persistentManifoldPoolAllocator->free(manifold);
-	}
+	manifold->~btPersistentManifold();
+	m_persistentManifoldPoolAllocator->free(manifold);
 	
 }
 
diff --git a/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp b/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp
index a7bb08172..1b643d62f 100644
--- a/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp
+++ b/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp
@@ -109,7 +109,8 @@ void	btCollisionWorld::addCollisionObject(btCollisionObject* collisionObject,sho
 			type,
 			collisionObject,
 			collisionFilterGroup,
-			collisionFilterMask
+			collisionFilterMask,
+			m_dispatcher1
 			))	;
 
 		
@@ -134,7 +135,7 @@ void	btCollisionWorld::performDiscreteCollisionDetection()
 	for (int i=0;i<m_collisionObjects.size();i++)
 	{
 		m_collisionObjects[i]->getCollisionShape()->getAabb(m_collisionObjects[i]->getWorldTransform(),aabbMin,aabbMax);
-		m_broadphasePairCache->setAabb(m_collisionObjects[i]->getBroadphaseHandle(),aabbMin,aabbMax);
+		m_broadphasePairCache->setAabb(m_collisionObjects[i]->getBroadphaseHandle(),aabbMin,aabbMax,m_dispatcher1);
 	}
 
 	m_broadphasePairCache->calculateOverlappingPairs(m_dispatcher1);
diff --git a/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp b/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp
index 8d7e81af4..559b633fe 100644
--- a/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp
+++ b/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp
@@ -189,9 +189,10 @@ void btConvexConcaveCollisionAlgorithm::processCollision (btCollisionObject* bod
 
 			concaveShape->processAllTriangles( &m_btConvexTriangleCallback,m_btConvexTriangleCallback.getAabbMin(),m_btConvexTriangleCallback.getAabbMax());
 			
+			resultOut->refreshContactPoints();
 	
 		}
-
+	
 	}
 
 }
diff --git a/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp b/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp
index 9105fe20b..76dc2a84d 100644
--- a/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp
+++ b/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp
@@ -152,6 +152,11 @@ void btConvexConvexAlgorithm ::processCollision (btCollisionObject* body0,btColl
 	m_gjkPairDetector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw);
 #endif
 
+	if (m_ownManifold)
+	{
+		resultOut->refreshContactPoints();
+	}
+
 }
 
 
diff --git a/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp b/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp
index 3cdef20cc..61c4c231d 100644
--- a/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp
+++ b/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp
@@ -79,7 +79,9 @@ void btManifoldResult::addContactPoint(const btVector3& normalOnBInWorld,const b
 	}
 
 	btManifoldPoint newPt(localA,localB,normalOnBInWorld,depth);
-
+	newPt.m_positionWorldOnA = pointA;
+	newPt.m_positionWorldOnB = pointInWorld;
+	
 	int insertIndex = m_manifoldPtr->getCacheEntry(newPt);
 
 	newPt.m_combinedFriction = calculateCombinedFriction(m_body0,m_body1);
diff --git a/src/BulletCollision/CollisionDispatch/btManifoldResult.h b/src/BulletCollision/CollisionDispatch/btManifoldResult.h
index 3beda95a9..c4007ca9d 100644
--- a/src/BulletCollision/CollisionDispatch/btManifoldResult.h
+++ b/src/BulletCollision/CollisionDispatch/btManifoldResult.h
@@ -18,7 +18,7 @@ subject to the following restrictions:
 #define MANIFOLD_RESULT_H
 
 class btCollisionObject;
-class btPersistentManifold;
+#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
 class btManifoldPoint;
 
 #include "BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h"
@@ -70,6 +70,22 @@ public:
 
 	virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth);
 
+	SIMD_FORCE_INLINE	void refreshContactPoints()
+	{
+		btAssert(m_manifoldPtr);
+		if (!m_manifoldPtr->getNumContacts())
+			return;
+
+		bool isSwapped = m_manifoldPtr->getBody0() != m_body0;
+
+		if (isSwapped)
+		{
+			m_manifoldPtr->refreshContactPoints(m_rootTransB,m_rootTransA);
+		} else
+		{
+			m_manifoldPtr->refreshContactPoints(m_rootTransA,m_rootTransB);
+		}
+	}
 
 
 };
diff --git a/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.cpp b/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.cpp
index 5c67165ed..70dc00f83 100644
--- a/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.cpp
+++ b/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.cpp
@@ -68,18 +68,25 @@ void btSphereBoxCollisionAlgorithm::processCollision (btCollisionObject* body0,b
 	
 	btScalar dist = getSphereDistance(boxObj,pOnBox,pOnSphere,sphereCenter,radius);
 
+	resultOut->setPersistentManifold(m_manifoldPtr);
+
 	if (dist < SIMD_EPSILON)
 	{
 		btVector3 normalOnSurfaceB = (pOnBox- pOnSphere).normalize();
 
 		/// report a contact. internally this will be kept persistent, and contact reduction is done
 
-		resultOut->setPersistentManifold(m_manifoldPtr);
 		resultOut->addContactPoint(normalOnSurfaceB,pOnBox,dist);
 		
 	}
 
-	
+	if (m_ownManifold)
+	{
+		if (m_manifoldPtr->getNumContacts())
+		{
+			resultOut->refreshContactPoints();
+		}
+	}
 
 }
 
diff --git a/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp b/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp
index 087c1753e..45960cc76 100644
--- a/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp
+++ b/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp
@@ -46,6 +46,8 @@ void btSphereSphereCollisionAlgorithm::processCollision (btCollisionObject* col0
 	if (!m_manifoldPtr)
 		return;
 
+	resultOut->setPersistentManifold(m_manifoldPtr);
+
 	btSphereShape* sphere0 = (btSphereShape*)col0->getCollisionShape();
 	btSphereShape* sphere1 = (btSphereShape*)col1->getCollisionShape();
 
@@ -54,10 +56,13 @@ void btSphereSphereCollisionAlgorithm::processCollision (btCollisionObject* col0
 	btScalar radius0 = sphere0->getRadius();
 	btScalar radius1 = sphere1->getRadius();
 
+	m_manifoldPtr->clearManifold();
+
 	///iff distance positive, don't generate a new contact
 	if ( len > (radius0+radius1))
+	{
 		return;
-
+	}
 	///distance (negative means penetration)
 	btScalar dist = len - (radius0+radius1);
 
@@ -68,9 +73,12 @@ void btSphereSphereCollisionAlgorithm::processCollision (btCollisionObject* col0
 	btVector3 pos1 = col1->getWorldTransform().getOrigin() + radius1* normalOnSurfaceB;
 
 	/// report a contact. internally this will be kept persistent, and contact reduction is done
-	resultOut->setPersistentManifold(m_manifoldPtr);
+	
+	
 	resultOut->addContactPoint(normalOnSurfaceB,pos1,dist);
 
+	//no resultOut->refreshContactPoints(); needed, because of clearManifold (all points are new)
+
 }
 
 btScalar btSphereSphereCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
diff --git a/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.cpp b/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.cpp
index 9f7b32145..b3b612d52 100644
--- a/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.cpp
+++ b/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.cpp
@@ -48,8 +48,11 @@ void btSphereTriangleCollisionAlgorithm::processCollision (btCollisionObject* co
 	if (!m_manifoldPtr)
 		return;
 
-	btSphereShape* sphere = (btSphereShape*)col0->getCollisionShape();
-	btTriangleShape* triangle = (btTriangleShape*)col1->getCollisionShape();
+	btCollisionObject* sphereObj = m_swapped? col1 : col0;
+	btCollisionObject* triObj = m_swapped? col0 : col1;
+
+	btSphereShape* sphere = (btSphereShape*)sphereObj->getCollisionShape();
+	btTriangleShape* triangle = (btTriangleShape*)triObj->getCollisionShape();
 	
 	/// report a contact. internally this will be kept persistent, and contact reduction is done
 	resultOut->setPersistentManifold(m_manifoldPtr);
@@ -62,6 +65,9 @@ void btSphereTriangleCollisionAlgorithm::processCollision (btCollisionObject* co
 
 	detector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw);
 
+	if (m_ownManifold)
+		resultOut->refreshContactPoints();
+	
 }
 
 btScalar btSphereTriangleCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
diff --git a/src/BulletCollision/CollisionDispatch/btUnionFind.cpp b/src/BulletCollision/CollisionDispatch/btUnionFind.cpp
index 622543357..c81be8aa7 100644
--- a/src/BulletCollision/CollisionDispatch/btUnionFind.cpp
+++ b/src/BulletCollision/CollisionDispatch/btUnionFind.cpp
@@ -18,6 +18,7 @@ subject to the following restrictions:
 
 
 
+
 btUnionFind::~btUnionFind()
 {
 	Free();
diff --git a/src/BulletCollision/CollisionShapes/btOptimizedBvh.cpp b/src/BulletCollision/CollisionShapes/btOptimizedBvh.cpp
index 7065d4e11..7dfd7694a 100644
--- a/src/BulletCollision/CollisionShapes/btOptimizedBvh.cpp
+++ b/src/BulletCollision/CollisionShapes/btOptimizedBvh.cpp
@@ -906,12 +906,13 @@ bool btOptimizedBvh::serialize(void *o_alignedDataBuffer, unsigned i_dataBufferS
 	assert(m_subtreeHeaderCount == m_SubtreeHeaders.size());
 	m_subtreeHeaderCount = m_SubtreeHeaders.size();
 
-	if (i_dataBufferSize < calculateSerializeBufferSize() || o_alignedDataBuffer == NULL || (((unsigned)o_alignedDataBuffer & BVH_ALIGNMENT_MASK) != 0))
+/*	if (i_dataBufferSize < calculateSerializeBufferSize() || o_alignedDataBuffer == NULL || (((unsigned)o_alignedDataBuffer & BVH_ALIGNMENT_MASK) != 0))
 	{
 		///check alignedment for buffer?
 		btAssert(0);
 		return false;
 	}
+*/
 
 	btOptimizedBvh *targetBvh = (btOptimizedBvh *)o_alignedDataBuffer;
 
@@ -946,7 +947,7 @@ bool btOptimizedBvh::serialize(void *o_alignedDataBuffer, unsigned i_dataBufferS
 	unsigned char *nodeData = (unsigned char *)targetBvh;
 	nodeData += sizeof(btOptimizedBvh);
 	
-	unsigned sizeToAdd = (BVH_ALIGNMENT-((unsigned)nodeData & BVH_ALIGNMENT_MASK))&BVH_ALIGNMENT_MASK;
+	unsigned sizeToAdd = 0;//(BVH_ALIGNMENT-((unsigned)nodeData & BVH_ALIGNMENT_MASK))&BVH_ALIGNMENT_MASK;
 	nodeData += sizeToAdd;
 	
 	int nodeCount = m_curNodeIndex;
@@ -1021,7 +1022,7 @@ bool btOptimizedBvh::serialize(void *o_alignedDataBuffer, unsigned i_dataBufferS
 		nodeData += sizeof(btOptimizedBvhNode) * nodeCount;
 	}
 
-	sizeToAdd = (BVH_ALIGNMENT-((unsigned)nodeData & BVH_ALIGNMENT_MASK))&BVH_ALIGNMENT_MASK;
+	sizeToAdd = 0;//(BVH_ALIGNMENT-((unsigned)nodeData & BVH_ALIGNMENT_MASK))&BVH_ALIGNMENT_MASK;
 	nodeData += sizeToAdd;
 
 	// Now serialize the subtree headers
@@ -1068,7 +1069,7 @@ bool btOptimizedBvh::serialize(void *o_alignedDataBuffer, unsigned i_dataBufferS
 btOptimizedBvh *btOptimizedBvh::deSerializeInPlace(void *i_alignedDataBuffer, unsigned i_dataBufferSize, bool i_swapEndian)
 {
 
-	if (i_alignedDataBuffer == NULL || (((unsigned)i_alignedDataBuffer & BVH_ALIGNMENT_MASK) != 0))
+	if (i_alignedDataBuffer == NULL)// || (((unsigned)i_alignedDataBuffer & BVH_ALIGNMENT_MASK) != 0))
 	{
 		return NULL;
 	}
@@ -1097,7 +1098,7 @@ btOptimizedBvh *btOptimizedBvh::deSerializeInPlace(void *i_alignedDataBuffer, un
 	unsigned char *nodeData = (unsigned char *)bvh;
 	nodeData += sizeof(btOptimizedBvh);
 	
-	unsigned sizeToAdd = (BVH_ALIGNMENT-((unsigned)nodeData & BVH_ALIGNMENT_MASK))&BVH_ALIGNMENT_MASK;
+	unsigned sizeToAdd = 0;//(BVH_ALIGNMENT-((unsigned)nodeData & BVH_ALIGNMENT_MASK))&BVH_ALIGNMENT_MASK;
 	nodeData += sizeToAdd;
 	
 	int nodeCount = bvh->m_curNodeIndex;
@@ -1146,7 +1147,7 @@ btOptimizedBvh *btOptimizedBvh::deSerializeInPlace(void *i_alignedDataBuffer, un
 		nodeData += sizeof(btOptimizedBvhNode) * nodeCount;
 	}
 
-	sizeToAdd = (BVH_ALIGNMENT-((unsigned)nodeData & BVH_ALIGNMENT_MASK))&BVH_ALIGNMENT_MASK;
+	sizeToAdd = 0;//(BVH_ALIGNMENT-((unsigned)nodeData & BVH_ALIGNMENT_MASK))&BVH_ALIGNMENT_MASK;
 	nodeData += sizeToAdd;
 
 	// Now serialize the subtree headers
diff --git a/src/BulletCollision/CollisionShapes/btSphereShape.h b/src/BulletCollision/CollisionShapes/btSphereShape.h
index 7666794cb..45ccb3308 100644
--- a/src/BulletCollision/CollisionShapes/btSphereShape.h
+++ b/src/BulletCollision/CollisionShapes/btSphereShape.h
@@ -42,7 +42,7 @@ public:
 
 	virtual int	getShapeType() const { return SPHERE_SHAPE_PROXYTYPE; }
 
-	btScalar	getRadius() const { return m_implicitShapeDimensions.getX();}
+	btScalar	getRadius() const { return m_implicitShapeDimensions.getX() * m_localScaling.getX();}
 
 	//debugging
 	virtual char*	getName()const {return "SPHERE";}
@@ -55,7 +55,7 @@ public:
 	{
 		//to improve gjk behaviour, use radius+margin as the full margin, so never get into the penetration case
 		//this means, non-uniform scaling is not supported anymore
-		return m_localScaling.getX() * getRadius() + btConvexInternalShape::getMargin();
+		return getRadius();
 	}
 
 
diff --git a/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp b/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp
index 08cb3ed33..ee94ee011 100644
--- a/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp
+++ b/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp
@@ -27,20 +27,12 @@ btPersistentManifold::btPersistentManifold()
 :m_body0(0),
 m_body1(0),
 m_cachedPoints (0),
-m_index1(0)
+m_index1a(0)
 {
 }
 
 
-void	btPersistentManifold::clearManifold()
-{
-	int i;
-	for (i=0;i<m_cachedPoints;i++)
-	{
-		clearUserCache(m_pointCache[i]);
-	}
-	m_cachedPoints = 0;
-}
+
 
 #ifdef DEBUG_PERSISTENCY
 #include <stdio.h>
@@ -198,10 +190,20 @@ btScalar	btPersistentManifold::getContactBreakingThreshold() const
 	return gContactBreakingThreshold;
 }
 
+
+
 void btPersistentManifold::refreshContactPoints(const btTransform& trA,const btTransform& trB)
 {
 	int i;
-
+#ifdef DEBUG_PERSISTENCY
+	printf("refreshContactPoints posA = (%f,%f,%f) posB = (%f,%f,%f)\n",
+		trA.getOrigin().getX(),
+		trA.getOrigin().getY(),
+		trA.getOrigin().getZ(),
+		trB.getOrigin().getX(),
+		trB.getOrigin().getY(),
+		trB.getOrigin().getZ());
+#endif //DEBUG_PERSISTENCY
 	/// first refresh worldspace positions and distance
 	for (i=getNumContacts()-1;i>=0;i--)
 	{
diff --git a/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h b/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h
index 5977d8509..af05a1e05 100644
--- a/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h
+++ b/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h
@@ -58,7 +58,7 @@ public:
 
 	BT_DECLARE_ALIGNED_ALLOCATOR();
 
-	int m_index1;
+	int m_index1a;
 
 	btPersistentManifold();
 
@@ -67,11 +67,11 @@ public:
 	{
 	}
 
-	inline void* getBody0() { return m_body0;}
-	inline void* getBody1() { return m_body1;}
+	SIMD_FORCE_INLINE void* getBody0() { return m_body0;}
+	SIMD_FORCE_INLINE void* getBody1() { return m_body1;}
 
-	inline const void* getBody0() const { return m_body0;}
-	inline const void* getBody1() const { return m_body1;}
+	SIMD_FORCE_INLINE const void* getBody0() const { return m_body0;}
+	SIMD_FORCE_INLINE const void* getBody1() const { return m_body1;}
 
 	void	setBodies(void* body0,void* body1)
 	{
@@ -85,22 +85,22 @@ public:
 	void	DebugPersistency();
 #endif //
 	
-	inline int	getNumContacts() const { return m_cachedPoints;}
+	SIMD_FORCE_INLINE int	getNumContacts() const { return m_cachedPoints;}
 
-	inline const btManifoldPoint& getContactPoint(int index) const
+	SIMD_FORCE_INLINE const btManifoldPoint& getContactPoint(int index) const
 	{
 		btAssert(index < m_cachedPoints);
 		return m_pointCache[index];
 	}
 
-	inline btManifoldPoint& getContactPoint(int index)
+	SIMD_FORCE_INLINE btManifoldPoint& getContactPoint(int index)
 	{
 		btAssert(index < m_cachedPoints);
 		return m_pointCache[index];
 	}
 
 	/// todo: get this margin from the current physics / collision environment
-	btScalar	getContactBreakingThreshold() const;
+	SIMD_FORCE_INLINE	btScalar	getContactBreakingThreshold() const;
 	
 	int getCacheEntry(const btManifoldPoint& newPoint) const;
 
@@ -150,7 +150,16 @@ public:
 	/// calculated new worldspace coordinates and depth, and reject points that exceed the collision margin
 	void	refreshContactPoints(  const btTransform& trA,const btTransform& trB);
 
-	void	clearManifold();
+	
+	SIMD_FORCE_INLINE	void	clearManifold()
+	{
+		int i;
+		for (i=0;i<m_cachedPoints;i++)
+		{
+			clearUserCache(m_pointCache[i]);
+		}
+		m_cachedPoints = 0;
+	}
 
 
 
diff --git a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp
index a468bdf72..8829cba84 100644
--- a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp
+++ b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp
@@ -14,6 +14,8 @@ subject to the following restrictions:
 */
 
 //#define COMPUTE_IMPULSE_DENOM 1
+//It is not necessary (redundant) to refresh contact manifolds, this refresh has been moved to the collision algorithms.
+//#define FORCE_REFESH_CONTACT_MANIFOLDS 1
 
 #include "btSequentialImpulseConstraintSolver.h"
 #include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
@@ -397,21 +399,30 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
 //		printf("empty\n");
 		return 0.f;
 	}
+	btPersistentManifold* manifold = 0;
+	btRigidBody* rb0=0,*rb1=0;
+
+#ifdef FORCE_REFESH_CONTACT_MANIFOLDS
 
 	BEGIN_PROFILE("refreshManifolds");
 
 	int i;
+	
+	
+
 	for (i=0;i<numManifolds;i++)
 	{
-		btPersistentManifold* manifold = manifoldPtr[i];
-		btRigidBody* rb0 = (btRigidBody*)manifold->getBody0();
-		btRigidBody* rb1 = (btRigidBody*)manifold->getBody1();
-
+		manifold = manifoldPtr[i];
+		rb1 = (btRigidBody*)manifold->getBody1();
+		rb0 = (btRigidBody*)manifold->getBody0();
+		
 		manifold->refreshContactPoints(rb0->getCenterOfMassTransform(),rb1->getCenterOfMassTransform());
 
 	}
-	
+
 	END_PROFILE("refreshManifolds");
+#endif //FORCE_REFESH_CONTACT_MANIFOLDS
+
 
 
 	BEGIN_PROFILE("gatherSolverData");
@@ -458,10 +469,11 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
 
 			for (i=0;i<numManifolds;i++)
 			{
-				btPersistentManifold* manifold = manifoldPtr[i];
-				btRigidBody* rb0 = (btRigidBody*)manifold->getBody0();
-				btRigidBody* rb1 = (btRigidBody*)manifold->getBody1();
+				manifold = manifoldPtr[i];
+				rb1 = (btRigidBody*)manifold->getBody1();
 
+				rb0 = (btRigidBody*)manifold->getBody0();
+				
 			
 				int solverBodyIdA=-1;
 				int solverBodyIdB=-1;
@@ -503,8 +515,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
 					
 					btManifoldPoint& cp = manifold->getContactPoint(j);
 
-					int frictionIndex = tmpSolverConstraintPool.size();
-
+					
 					if (cp.getDistance() <= btScalar(0.))
 					{
 						
@@ -519,6 +530,8 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
 						btScalar rel_vel;
 						btVector3 vel;
 
+						int frictionIndex = tmpSolverConstraintPool.size();
+
 						{
 							btSolverConstraint& solverConstraint = tmpSolverConstraintPool.expand();
 
@@ -586,18 +599,22 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
 
 						
 						{
-							btVector3 lat_vel = vel - cp.m_normalWorldOnB * rel_vel;
-							btScalar lat_rel_vel = lat_vel.length2();
+							btVector3 frictionDir1 = vel - cp.m_normalWorldOnB * rel_vel;
+							btScalar lat_rel_vel = frictionDir1.length2();
 							if (lat_rel_vel > SIMD_EPSILON)//0.0f)
 							{
-								lat_vel /= btSqrt(lat_rel_vel);
-
-								addFrictionConstraint(lat_vel,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,rb0,rb1, relaxation);
-								btVector3 frictionDir2 = lat_vel.cross(cp.m_normalWorldOnB);
+								frictionDir1 /= btSqrt(lat_rel_vel);
+								addFrictionConstraint(frictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,rb0,rb1, relaxation);
+								btVector3 frictionDir2 = frictionDir1.cross(cp.m_normalWorldOnB);
 								frictionDir2.normalize();//??
 								addFrictionConstraint(frictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,rb0,rb1, relaxation);
-
-
+							} else
+							{
+								//re-calculate friction direction every frame, todo: check if this is really needed
+								btVector3	frictionDir1,frictionDir2;
+								btPlaneSpace1(cp.m_normalWorldOnB,frictionDir1,frictionDir2);
+								addFrictionConstraint(frictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,rb0,rb1, relaxation);
+								addFrictionConstraint(frictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,rb0,rb1, relaxation);
 							}
 
 						}
@@ -907,8 +924,9 @@ void	btSequentialImpulseConstraintSolver::prepareConstraints(btPersistentManifol
 
 	//only necessary to refresh the manifold once (first iteration). The integration is done outside the loop
 	{
+#ifdef FORCE_REFESH_CONTACT_MANIFOLDS
 		manifoldPtr->refreshContactPoints(body0->getCenterOfMassTransform(),body1->getCenterOfMassTransform());
-		
+#endif //FORCE_REFESH_CONTACT_MANIFOLDS		
 		int numpoints = manifoldPtr->getNumContacts();
 
 		gTotalContactPoints += numpoints;
diff --git a/src/BulletDynamics/Dynamics/btContinuousDynamicsWorld.cpp b/src/BulletDynamics/Dynamics/btContinuousDynamicsWorld.cpp
index 044493256..8f72d5c21 100644
--- a/src/BulletDynamics/Dynamics/btContinuousDynamicsWorld.cpp
+++ b/src/BulletDynamics/Dynamics/btContinuousDynamicsWorld.cpp
@@ -179,7 +179,7 @@ void	btContinuousDynamicsWorld::updateTemporalAabbs(btScalar timeStep)
 			temporalAabbMin -= angularMotion3d;
 			temporalAabbMax += angularMotion3d;
 
-			m_broadphasePairCache->setAabb(body->getBroadphaseHandle(),temporalAabbMin,temporalAabbMax);
+			m_broadphasePairCache->setAabb(body->getBroadphaseHandle(),temporalAabbMin,temporalAabbMax,m_dispatcher1);
 		}
 	}
 
diff --git a/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp b/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp
index 81bd2091a..afdb226ea 100644
--- a/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp
+++ b/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp
@@ -606,7 +606,7 @@ void	btDiscreteDynamicsWorld::updateAabbs()
 				//moving objects should be moderately sized, probably something wrong if not
 				if ( colObj->isStaticObject() || ((maxAabb-minAabb).length2() < btScalar(1e12)))
 				{
-					bp->setAabb(body->getBroadphaseHandle(),minAabb,maxAabb);
+					bp->setAabb(body->getBroadphaseHandle(),minAabb,maxAabb, m_dispatcher1);
 				} else
 				{
 					//something went wrong, investigate
diff --git a/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp b/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp
index 0af074957..61659669a 100644
--- a/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp
+++ b/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp
@@ -134,7 +134,7 @@ void	btSimpleDynamicsWorld::updateAabbs()
 				btPoint3 minAabb,maxAabb;
 				colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb,maxAabb);
 				btBroadphaseInterface* bp = getBroadphase();
-				bp->setAabb(body->getBroadphaseHandle(),minAabb,maxAabb);
+				bp->setAabb(body->getBroadphaseHandle(),minAabb,maxAabb, m_dispatcher1);
 			}
 		}
 	}
diff --git a/src/LinearMath/btAlignedAllocator.cpp b/src/LinearMath/btAlignedAllocator.cpp
index 5e49d1f2c..520c95566 100644
--- a/src/LinearMath/btAlignedAllocator.cpp
+++ b/src/LinearMath/btAlignedAllocator.cpp
@@ -19,7 +19,7 @@ subject to the following restrictions:
 #if defined (BT_HAS_ALIGNED_ALOCATOR)
 
 #include <malloc.h>
-void*	btAlignedAlloc	(int size, int alignment)
+void*	btAlignedAlloc	(size_t size, int alignment)
 {
 	void* ptr = _aligned_malloc(size,alignment);
 //	printf("btAlignedAlloc %d, %x\n",size,ptr);
@@ -41,7 +41,7 @@ void	btAlignedFree	(void* ptr)
 int numAllocs = 0;
 int numFree = 0;
 
-void*	btAlignedAlloc	(int size, int alignment)
+void*	btAlignedAlloc	(size_t size, int alignment)
 {
 	numAllocs++;
 	return memalign(alignment, size);
@@ -55,7 +55,7 @@ void	btAlignedFree	(void* ptr)
 
 #else
 
-void*	btAlignedAlloc	(int size, int alignment)
+void*	btAlignedAlloc	(size_t size, int alignment)
 {
  void *ret;
   char *real;
diff --git a/src/LinearMath/btAlignedAllocator.h b/src/LinearMath/btAlignedAllocator.h
index ea1acf8ab..82ce94926 100644
--- a/src/LinearMath/btAlignedAllocator.h
+++ b/src/LinearMath/btAlignedAllocator.h
@@ -22,7 +22,7 @@ subject to the following restrictions:
 
 #include "btScalar.h"
 
-void*	btAlignedAlloc	(int size, int alignment);
+void*	btAlignedAlloc	(size_t size, int alignment);
 
 void	btAlignedFree	(void* ptr);
 
diff --git a/src/LinearMath/btPoolAllocator.h b/src/LinearMath/btPoolAllocator.h
index fc1b40df5..27f9e3b46 100755
--- a/src/LinearMath/btPoolAllocator.h
+++ b/src/LinearMath/btPoolAllocator.h
@@ -61,6 +61,17 @@ public:
         return result;
 	}
 
+	bool validPtr(void* ptr)
+	{
+		if (ptr) {
+			if (((unsigned char*)ptr >= m_pool && (unsigned char*)ptr < m_pool + m_maxElements * m_elemSize))
+			{
+				return true;
+			}
+		}
+		return false;
+	}
+
 	void	free(void* ptr)
 	{
 		 if (ptr) {
diff --git a/src/LinearMath/btScalar.h b/src/LinearMath/btScalar.h
index 0e8a517ae..436cccaf1 100644
--- a/src/LinearMath/btScalar.h
+++ b/src/LinearMath/btScalar.h
@@ -288,6 +288,13 @@ SIMD_FORCE_INLINE float btSelect(unsigned condition, float valueIfConditionNonZe
 #endif
 }
 
+template<typename T> inline void btSwap(T& a, T& b)
+{
+	T tmp = a;
+	a = b;
+	b = tmp;
+}
+
 
 //PCK: endian swapping functions
 SIMD_FORCE_INLINE unsigned btSwapEndian(unsigned val)