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)