- added quickSort to btAlignedObjectArray (has generally better performance then heapSort)

- replaced all usage of heapSort by quickSort
- finished btMultiSapBroadphase. Still some work to be done to increase performance (faster add/remove from child broadphases. 
  uses currently AABB tree to locate child broadphase (should use grid), and sorted array takes too much time.

Extras/BulletMultiThreaded/SpuParallelSolver.cpp

@@ -278,11 +278,11 @@ void btParallelSequentialImpulseSolver::allSolved (const btContactSolverInfo& in
 
 	// Sort the manifolds list
 	int numManifolds = m_sortedManifolds.size();
-	m_sortedManifolds.heapSort(CellHolderPredicate<ManifoldCellHolder>());
+	m_sortedManifolds.quickSort(CellHolderPredicate<ManifoldCellHolder>());
 
 	// Sort the constraint list
 	int numConstraints = m_sortedConstraints.size();
-	m_sortedConstraints.heapSort(CellHolderPredicate<ConstraintCellHolder>());
+	m_sortedConstraints.quickSort(CellHolderPredicate<ConstraintCellHolder>());
 
 
 	// Sort the body list

Extras/GIMPACT/src/Bullet/btContactProcessing.cpp

@@ -92,7 +92,7 @@ void btContactArray::merge_contacts(
 	}
 
 	//sort keys
-	keycontacts.heapSort(CONTACT_KEY_TOKEN_COMP());
+	keycontacts.quickSort(CONTACT_KEY_TOKEN_COMP());
 
 	// Merge contacts
 	int coincident_count=0;

src/BulletCollision/BroadphaseCollision/btAxisSweep3.h

@@ -117,10 +117,14 @@ public:
 
 	virtual	~btAxisSweep3Internal();
 
+	BP_FP_INT_TYPE getNumHandles() const
+	{
+		return m_numHandles;
+	}
 
 	virtual void	calculateOverlappingPairs(btDispatcher* dispatcher);
 	
-	BP_FP_INT_TYPE addHandle(const btPoint3& aabbMin,const btPoint3& aabbMax, void* pOwner,short int collisionFilterGroup,short int collisionFilterMask,btDispatcher* dispatcher);
+	BP_FP_INT_TYPE addHandle(const btPoint3& aabbMin,const btPoint3& aabbMax, void* pOwner,short int collisionFilterGroup,short int collisionFilterMask,btDispatcher* dispatcher,void* multiSapProxy);
 	void removeHandle(BP_FP_INT_TYPE handle,btDispatcher* dispatcher);
 	void updateHandle(BP_FP_INT_TYPE handle, const btPoint3& aabbMin,const btPoint3& aabbMax,btDispatcher* dispatcher);
 	SIMD_FORCE_INLINE Handle* getHandle(BP_FP_INT_TYPE index) const {return m_pHandles + index;}
@@ -128,7 +132,7 @@ public:
 	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,btDispatcher* dispatcher);
+	virtual btBroadphaseProxy*	createProxy(  const btVector3& aabbMin,  const btVector3& aabbMax,int shapeType,void* userPtr ,short int collisionFilterGroup,short int collisionFilterMask,btDispatcher* dispatcher,void* multiSapProxy);
 	virtual void	destroyProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher);
 	virtual void	setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax,btDispatcher* dispatcher);
 	
@@ -160,6 +164,15 @@ public:
 		aabbMax = m_worldAabbMax;
 	}
 
+	virtual void	printStats()
+	{
+/*		printf("btAxisSweep3.h\n");
+		printf("numHandles = %d, maxHandles = %d\n",m_numHandles,m_maxHandles);
+		printf("aabbMin=%f,%f,%f,aabbMax=%f,%f,%f\n",m_worldAabbMin.getX(),m_worldAabbMin.getY(),m_worldAabbMin.getZ(),
+			m_worldAabbMax.getX(),m_worldAabbMax.getY(),m_worldAabbMax.getZ());
+			*/
+
+	}
 
 };
 
@@ -194,10 +207,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,btDispatcher* dispatcher)
+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* multiSapProxy)
 {
 		(void)shapeType;
-		BP_FP_INT_TYPE handleId = addHandle(aabbMin,aabbMax, userPtr,collisionFilterGroup,collisionFilterMask,dispatcher);
+		BP_FP_INT_TYPE handleId = addHandle(aabbMin,aabbMax, userPtr,collisionFilterGroup,collisionFilterMask,dispatcher,multiSapProxy);
 		
 		Handle* handle = getHandle(handleId);
 				
@@ -360,7 +373,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,btDispatcher* dispatcher)
+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,void* multiSapProxy)
 {
 	// quantize the bounds
 	BP_FP_INT_TYPE min[3], max[3];
@@ -378,6 +391,7 @@ BP_FP_INT_TYPE btAxisSweep3Internal<BP_FP_INT_TYPE>::addHandle(const btPoint3& a
 	pHandle->m_clientObject = pOwner;
 	pHandle->m_collisionFilterGroup = collisionFilterGroup;
 	pHandle->m_collisionFilterMask = collisionFilterMask;
+	pHandle->m_multiSapParentProxy = multiSapProxy;
 
 	// compute current limit of edge arrays
 	BP_FP_INT_TYPE limit = m_numHandles * 2;
@@ -472,7 +486,7 @@ void btAxisSweep3Internal<BP_FP_INT_TYPE>::removeHandle(BP_FP_INT_TYPE handle,bt
 }
 
 extern int gOverlappingPairs;
-#include <stdio.h>
+//#include <stdio.h>
 
 template <typename BP_FP_INT_TYPE>
 void	btAxisSweep3Internal<BP_FP_INT_TYPE>::calculateOverlappingPairs(btDispatcher* dispatcher)
@@ -484,7 +498,7 @@ void	btAxisSweep3Internal<BP_FP_INT_TYPE>::calculateOverlappingPairs(btDispatche
 		btBroadphasePairArray&	overlappingPairArray = m_pairCache->getOverlappingPairArray();
 
 		//perform a sort, to find duplicates and to sort 'invalid' pairs to the end
-		overlappingPairArray.heapSort(btBroadphasePairSortPredicate());
+		overlappingPairArray.quickSort(btBroadphasePairSortPredicate());
 
 		overlappingPairArray.resize(overlappingPairArray.size() - m_invalidPair);
 		m_invalidPair = 0;
@@ -547,7 +561,7 @@ void	btAxisSweep3Internal<BP_FP_INT_TYPE>::calculateOverlappingPairs(btDispatche
 	#ifdef CLEAN_INVALID_PAIRS
 
 		//perform a sort, to sort 'invalid' pairs to the end
-		overlappingPairArray.heapSort(btBroadphasePairSortPredicate());
+		overlappingPairArray.quickSort(btBroadphasePairSortPredicate());
 
 		overlappingPairArray.resize(overlappingPairArray.size() - m_invalidPair);
 		m_invalidPair = 0;

src/BulletCollision/BroadphaseCollision/btBroadphaseInterface.h

@@ -31,7 +31,7 @@ class btBroadphaseInterface
 public:
 	virtual ~btBroadphaseInterface() {}
 
-	virtual btBroadphaseProxy*	createProxy(  const btVector3& aabbMin,  const btVector3& aabbMax,int shapeType,void* userPtr, short int collisionFilterGroup,short int collisionFilterMask, btDispatcher* dispatcher) =0;
+	virtual btBroadphaseProxy*	createProxy(  const btVector3& aabbMin,  const btVector3& aabbMax,int shapeType,void* userPtr, short int collisionFilterGroup,short int collisionFilterMask, btDispatcher* dispatcher,void* multiSapProxy) =0;
 	virtual void	destroyProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher)=0;
 	virtual void	setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax, btDispatcher* dispatcher)=0;
 	
@@ -45,6 +45,8 @@ public:
 	///will add some transform later
 	virtual void getBroadphaseAabb(btVector3& aabbMin,btVector3& aabbMax) const =0;
 
+	virtual void	printStats() = 0;
+
 };
 
 #endif //BROADPHASE_INTERFACE_H

src/BulletCollision/BroadphaseCollision/btBroadphaseProxy.h

@@ -62,6 +62,7 @@ CONCAVE_SHAPES_END_HERE,
 	MAX_BROADPHASE_COLLISION_TYPES
 };
 
+//#include <stdio.h>
 
 ///btBroadphaseProxy
 ATTRIBUTE_ALIGNED16(struct) btBroadphaseProxy
@@ -83,21 +84,13 @@ BT_DECLARE_ALIGNED_ALLOCATOR();
 	//Usually the client btCollisionObject or Rigidbody class
 	void*	m_clientObject;
 
-	///in the case of btMultiSapBroadphase, we store the collifionFilterGroup/Mask in the m_multiSapParentProxy
-	union
-	{
-		struct
-		{
 	short int m_collisionFilterGroup;
 	short int m_collisionFilterMask;
-		};
 
 	void*	m_multiSapParentProxy;		
 
-	};
 
 	int			m_uniqueId;//m_uniqueId is introduced for paircache. could get rid of this, by calculating the address offset etc.
-	int m_unusedPadding; //making the structure 16 bytes, better for alignment etc.
 
 	SIMD_FORCE_INLINE int getUid() const
 	{
@@ -105,13 +98,16 @@ BT_DECLARE_ALIGNED_ALLOCATOR();
 	}
 
 	//used for memory pools
-	btBroadphaseProxy() :m_clientObject(0){}
+	btBroadphaseProxy() :m_clientObject(0),m_multiSapParentProxy(0)
+	{
+	}
 
-	btBroadphaseProxy(void* userPtr,short int collisionFilterGroup, short int collisionFilterMask)
+	btBroadphaseProxy(void* userPtr,short int collisionFilterGroup, short int collisionFilterMask,void* multiSapParentProxy=0)
 		:m_clientObject(userPtr),
 		m_collisionFilterGroup(collisionFilterGroup),
 		m_collisionFilterMask(collisionFilterMask)
 	{
+		m_multiSapParentProxy = multiSapParentProxy;
 	}
 
 	

src/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.cpp

@@ -57,9 +57,11 @@ m_optimizedAabbTree(0)
 		// return true when pairs need collision
 		virtual bool	needBroadphaseCollision(btBroadphaseProxy* childProxy0,btBroadphaseProxy* childProxy1) const
 		{
+			btBroadphaseProxy* multiProxy0 = (btBroadphaseProxy*)childProxy0->m_multiSapParentProxy;
+			btBroadphaseProxy* multiProxy1 = (btBroadphaseProxy*)childProxy1->m_multiSapParentProxy;
 			
-			bool collides = (childProxy0->m_collisionFilterGroup & childProxy1->m_collisionFilterMask) != 0;
+			bool collides = (multiProxy0->m_collisionFilterGroup & multiProxy1->m_collisionFilterMask) != 0;
-			collides = collides && (childProxy1->m_collisionFilterGroup & childProxy0->m_collisionFilterMask);
+			collides = collides && (multiProxy1->m_collisionFilterGroup & multiProxy0->m_collisionFilterMask);
 	
 			return collides;
 		}
@@ -69,8 +71,8 @@ m_optimizedAabbTree(0)
 	m_filterCallback = new (mem)btMultiSapOverlapFilterCallback();
 
 	m_overlappingPairs->setOverlapFilterCallback(m_filterCallback);
-	mem = btAlignedAlloc(sizeof(btSimpleBroadphase),16);
+//	mem = btAlignedAlloc(sizeof(btSimpleBroadphase),16);
-	m_simpleBroadphase = new (mem) btSimpleBroadphase(maxProxies,m_overlappingPairs);
+//	m_simpleBroadphase = new (mem) btSimpleBroadphase(maxProxies,m_overlappingPairs);
 }
 
 btMultiSapBroadphase::~btMultiSapBroadphase()
@@ -102,8 +104,10 @@ void	btMultiSapBroadphase::buildTree(const btVector3& bvhAabbMin,const btVector3
 	m_optimizedAabbTree->buildInternal();
 }
 
-btBroadphaseProxy*	btMultiSapBroadphase::createProxy(  const btVector3& aabbMin,  const btVector3& aabbMax,int shapeType,void* userPtr, short int collisionFilterGroup,short int collisionFilterMask, btDispatcher* dispatcher)
+btBroadphaseProxy*	btMultiSapBroadphase::createProxy(  const btVector3& aabbMin,  const btVector3& aabbMax,int shapeType,void* userPtr, short int collisionFilterGroup,short int collisionFilterMask, btDispatcher* dispatcher,void* ignoreMe)
 {
+	//void* ignoreMe -> we could think of recursive multi-sap, if someone is interested
+
 	void* mem = btAlignedAlloc(sizeof(btMultiSapProxy),16);
 	btMultiSapProxy* proxy = new (mem)btMultiSapProxy(aabbMin,  aabbMax,shapeType,userPtr, collisionFilterGroup,collisionFilterMask);
 	m_multiSapProxies.push_back(proxy);
@@ -144,8 +148,7 @@ amin.getZ() >= bmin.getZ() && amax.getZ() <= bmax.getZ();
 
 
 
-int  doTree=1;
+//#include <stdio.h>
-#include <stdio.h>
 
 void	btMultiSapBroadphase::setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax, btDispatcher* dispatcher)
 {
@@ -157,25 +160,6 @@ void	btMultiSapBroadphase::setAabb(btBroadphaseProxy* proxy,const btVector3& aab
 	bool fullyContained = false;
 	bool alreadyInSimple = false;
 	
-	for (int i=0;i<multiProxy->m_bridgeProxies.size();i++)
-	{
-		btVector3 worldAabbMin,worldAabbMax;
-		multiProxy->m_bridgeProxies[i]->m_childBroadphase->getBroadphaseAabb(worldAabbMin,worldAabbMax);
-		bool overlapsBroadphase = TestAabbAgainstAabb2(worldAabbMin,worldAabbMax,multiProxy->m_aabbMin,multiProxy->m_aabbMax);
-		if (!overlapsBroadphase)
-		{
-			//remove it now
-			btBridgeProxy* bridgeProxy = multiProxy->m_bridgeProxies[i];
-
-			btBroadphaseProxy* childProxy = bridgeProxy->m_childProxy;
-			bridgeProxy->m_childBroadphase->destroyProxy(childProxy,dispatcher);
-			
-			multiProxy->m_bridgeProxies.swap( i,multiProxy->m_bridgeProxies.size()-1);
-			multiProxy->m_bridgeProxies.pop_back();
-
-		}
-	}
-	
 
 
 	
@@ -211,8 +195,7 @@ void	btMultiSapBroadphase::setAabb(btBroadphaseProxy* proxy,const btVector3& aab
 			if (containingBroadphaseIndex<0)
 			{
 				//add it
-				btBroadphaseProxy* childProxy = childBroadphase->createProxy(m_multiProxy->m_aabbMin,m_multiProxy->m_aabbMax,m_multiProxy->m_shapeType,m_multiProxy->m_clientObject,m_multiProxy->m_collisionFilterGroup,m_multiProxy->m_collisionFilterMask, m_dispatcher);
+				btBroadphaseProxy* childProxy = childBroadphase->createProxy(m_multiProxy->m_aabbMin,m_multiProxy->m_aabbMax,m_multiProxy->m_shapeType,m_multiProxy->m_clientObject,m_multiProxy->m_collisionFilterGroup,m_multiProxy->m_collisionFilterMask, m_dispatcher,m_multiProxy);
-				childProxy->m_multiSapParentProxy = m_multiProxy;
 				m_multiSap->addToChildBroadphase(m_multiProxy,childProxy,childBroadphase);
 
 			}
@@ -223,14 +206,32 @@ void	btMultiSapBroadphase::setAabb(btBroadphaseProxy* proxy,const btVector3& aab
 
 
 
-	if (doTree)
-	{
 	
 	m_optimizedAabbTree->reportAabbOverlappingNodex(&myNodeCallback,aabbMin,aabbMax);
+
+		for (int i=0;i<multiProxy->m_bridgeProxies.size();i++)
+	{
+		btVector3 worldAabbMin,worldAabbMax;
+		multiProxy->m_bridgeProxies[i]->m_childBroadphase->getBroadphaseAabb(worldAabbMin,worldAabbMax);
+		bool overlapsBroadphase = TestAabbAgainstAabb2(worldAabbMin,worldAabbMax,multiProxy->m_aabbMin,multiProxy->m_aabbMax);
+		if (!overlapsBroadphase)
+		{
+			//remove it now
+			btBridgeProxy* bridgeProxy = multiProxy->m_bridgeProxies[i];
+
+			btBroadphaseProxy* childProxy = bridgeProxy->m_childProxy;
+			bridgeProxy->m_childBroadphase->destroyProxy(childProxy,dispatcher);
+			
+			multiProxy->m_bridgeProxies.swap( i,multiProxy->m_bridgeProxies.size()-1);
+			multiProxy->m_bridgeProxies.pop_back();
+
+		}
 	}
 
 
-	/*if (!stopUpdating1)
+	/*
+
+	if (1)
 	{
 
 		//find broadphase that contain this multiProxy
@@ -292,6 +293,15 @@ void	btMultiSapBroadphase::setAabb(btBroadphaseProxy* proxy,const btVector3& aab
 			addToChildBroadphase(multiProxy,childProxy,m_simpleBroadphase);
 		}
 	}
+
+	if (!multiProxy->m_bridgeProxies.size())
+	{
+		///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* childProxy = m_simpleBroadphase->createProxy(aabbMin,aabbMax,multiProxy->m_shapeType,multiProxy->m_clientObject,multiProxy->m_collisionFilterGroup,multiProxy->m_collisionFilterMask, dispatcher);
+		childProxy->m_multiSapParentProxy = multiProxy;
+		addToChildBroadphase(multiProxy,childProxy,m_simpleBroadphase);
+	}
 */
 
 
@@ -306,7 +316,6 @@ void	btMultiSapBroadphase::setAabb(btBroadphaseProxy* proxy,const btVector3& aab
 bool stopUpdating=false;
 
 
-
 	///calculateOverlappingPairs is optional: incremental algorithms (sweep and prune) might do it during the set aabb
 void	btMultiSapBroadphase::calculateOverlappingPairs(btDispatcher* dispatcher)
 {
@@ -327,15 +336,19 @@ void	btMultiSapBroadphase::calculateOverlappingPairs(btDispatcher* dispatcher)
 		}
 };
 
-	m_simpleBroadphase->calculateOverlappingPairs(dispatcher);
+//	m_simpleBroadphase->calculateOverlappingPairs(dispatcher);
 
 	if (!stopUpdating && getOverlappingPairCache()->hasDeferredRemoval())
 	{
 	
 		btBroadphasePairArray&	overlappingPairArray = getOverlappingPairCache()->getOverlappingPairArray();
 
+	//	quicksort(overlappingPairArray,0,overlappingPairArray.size());
+
+		overlappingPairArray.quickSort(btMultiSapBroadphasePairSortPredicate());
+
 		//perform a sort, to find duplicates and to sort 'invalid' pairs to the end
-		overlappingPairArray.heapSort(btMultiSapBroadphasePairSortPredicate());
+	//	overlappingPairArray.heapSort(btMultiSapBroadphasePairSortPredicate());
 
 		overlappingPairArray.resize(overlappingPairArray.size() - m_invalidPair);
 		m_invalidPair = 0;
@@ -360,7 +373,6 @@ void	btMultiSapBroadphase::calculateOverlappingPairs(btDispatcher* dispatcher)
 			btMultiSapProxy* bProxy1 = previousPair.m_pProxy1 ? (btMultiSapProxy*)previousPair.m_pProxy1->m_multiSapParentProxy : 0;
 
 			bool isDuplicate = (aProxy0 == bProxy0) && (aProxy1 == bProxy1);
-			(pair == previousPair);
 			
 			previousPair = pair;
 
@@ -404,7 +416,8 @@ void	btMultiSapBroadphase::calculateOverlappingPairs(btDispatcher* dispatcher)
 	#ifdef CLEAN_INVALID_PAIRS
 
 		//perform a sort, to sort 'invalid' pairs to the end
-		overlappingPairArray.heapSort(btMultiSapBroadphasePairSortPredicate());
+		//overlappingPairArray.heapSort(btMultiSapBroadphasePairSortPredicate());
+		overlappingPairArray.quickSort(btMultiSapBroadphasePairSortPredicate());
 
 		overlappingPairArray.resize(overlappingPairArray.size() - m_invalidPair);
 		m_invalidPair = 0;
@@ -428,3 +441,21 @@ bool	btMultiSapBroadphase::testAabbOverlap(btBroadphaseProxy* childProxy0,btBroa
 }
 
 
+void	btMultiSapBroadphase::printStats()
+{
+/*	printf("---------------------------------\n");
+	
+		printf("btMultiSapBroadphase.h\n");
+		printf("numHandles = %d\n",m_multiSapProxies.size());
+			//find broadphase that contain this multiProxy
+		int numChildBroadphases = getBroadphaseArray().size();
+		for (int i=0;i<numChildBroadphases;i++)
+		{
+
+			btBroadphaseInterface* childBroadphase = getBroadphaseArray()[i];
+			childBroadphase->printStats();
+
+		}
+		*/
+
+}

src/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.h

@@ -70,7 +70,7 @@ public:
 		short int	m_collisionFilterMask;
 */
 		btMultiSapProxy(const btVector3& aabbMin,  const btVector3& aabbMax,int shapeType,void* userPtr, short int collisionFilterGroup,short int collisionFilterMask)
-			:btBroadphaseProxy(userPtr,collisionFilterGroup,collisionFilterMask),
+			:btBroadphaseProxy(userPtr,collisionFilterGroup,collisionFilterMask,this),
 			m_aabbMin(aabbMin),
 			m_aabbMax(aabbMax),
 			m_shapeType(shapeType)
@@ -104,7 +104,7 @@ public:
 
 	virtual ~btMultiSapBroadphase();
 
-	virtual btBroadphaseProxy*	createProxy(  const btVector3& aabbMin,  const btVector3& aabbMax,int shapeType,void* userPtr, short int collisionFilterGroup,short int collisionFilterMask, btDispatcher* dispatcher);
+	virtual btBroadphaseProxy*	createProxy(  const btVector3& aabbMin,  const btVector3& aabbMax,int shapeType,void* userPtr, short int collisionFilterGroup,short int collisionFilterMask, btDispatcher* dispatcher,void* multiSapProxy);
 	virtual void	destroyProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher);
 	virtual void	setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax, btDispatcher* dispatcher);
 	
@@ -132,6 +132,10 @@ public:
 
 	void	buildTree(const btVector3& bvhAabbMin,const btVector3& bvhAabbMax);
 
+	virtual void	printStats();
+
+	void quicksort (btBroadphasePairArray& a, int lo, int hi);
+
 };
 
 #endif //BT_MULTI_SAP_BROADPHASE

src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp

@@ -163,7 +163,7 @@ btBroadphasePair* btHashedOverlappingPairCache::findPair(btBroadphaseProxy* prox
 	return &m_overlappingPairArray[index];
 }
 
-#include <stdio.h>
+//#include <stdio.h>
 
 void	btHashedOverlappingPairCache::growTables()
 {
@@ -349,7 +349,7 @@ void* btHashedOverlappingPairCache::removeOverlappingPair(btBroadphaseProxy* pro
 
 	return userData;
 }
-#include <stdio.h>
+//#include <stdio.h>
 
 void	btHashedOverlappingPairCache::processAllOverlappingPairs(btOverlapCallback* callback,btDispatcher* dispatcher)
 {
@@ -450,7 +450,7 @@ btBroadphasePair*	btSortedOverlappingPairCache::addOverlappingPair(btBroadphaseP
 
 
 
-#include <stdio.h>
+//#include <stdio.h>
 
 void	btSortedOverlappingPairCache::processAllOverlappingPairs(btOverlapCallback* callback,btDispatcher* dispatcher)
 {

src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp

@@ -83,7 +83,7 @@ btSimpleBroadphase::~btSimpleBroadphase()
 }
 
 
-btBroadphaseProxy*	btSimpleBroadphase::createProxy(  const btVector3& aabbMin,  const btVector3& aabbMax,int shapeType,void* userPtr ,short int collisionFilterGroup,short int collisionFilterMask, btDispatcher* dispatcher)
+btBroadphaseProxy*	btSimpleBroadphase::createProxy(  const btVector3& aabbMin,  const btVector3& aabbMax,int shapeType,void* userPtr ,short int collisionFilterGroup,short int collisionFilterMask, btDispatcher* dispatcher,void* multiSapProxy)
 {
 	if (m_numHandles >= m_maxHandles)
 	{
@@ -93,7 +93,7 @@ btBroadphaseProxy*	btSimpleBroadphase::createProxy(  const btVector3& aabbMin,
 	assert(aabbMin[0]<= aabbMax[0] && aabbMin[1]<= aabbMax[1] && aabbMin[2]<= aabbMax[2]);
 
 	int newHandleIndex = allocHandle();
-	btSimpleBroadphaseProxy* proxy = new (&m_pHandles[newHandleIndex])btSimpleBroadphaseProxy(aabbMin,aabbMax,shapeType,userPtr,collisionFilterGroup,collisionFilterMask);
+	btSimpleBroadphaseProxy* proxy = new (&m_pHandles[newHandleIndex])btSimpleBroadphaseProxy(aabbMin,aabbMax,shapeType,userPtr,collisionFilterGroup,collisionFilterMask,multiSapProxy);
 
 	return proxy;
 }
@@ -227,7 +227,7 @@ void	btSimpleBroadphase::calculateOverlappingPairs(btDispatcher* dispatcher)
 			btBroadphasePairArray&	overlappingPairArray = m_pairCache->getOverlappingPairArray();
 
 			//perform a sort, to find duplicates and to sort 'invalid' pairs to the end
-			overlappingPairArray.heapSort(btBroadphasePairSortPredicate());
+			overlappingPairArray.quickSort(btBroadphasePairSortPredicate());
 
 			overlappingPairArray.resize(overlappingPairArray.size() - m_invalidPair);
 			m_invalidPair = 0;
@@ -288,7 +288,7 @@ void	btSimpleBroadphase::calculateOverlappingPairs(btDispatcher* dispatcher)
 		#ifdef CLEAN_INVALID_PAIRS
 
 			//perform a sort, to sort 'invalid' pairs to the end
-			overlappingPairArray.heapSort(btBroadphasePairSortPredicate());
+			overlappingPairArray.quickSort(btBroadphasePairSortPredicate());
 
 			overlappingPairArray.resize(overlappingPairArray.size() - m_invalidPair);
 			m_invalidPair = 0;

src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.h

@@ -31,8 +31,8 @@ struct btSimpleBroadphaseProxy : public btBroadphaseProxy
 	
 	btSimpleBroadphaseProxy() {};
 
-	btSimpleBroadphaseProxy(const btPoint3& minpt,const btPoint3& maxpt,int shapeType,void* userPtr,short int collisionFilterGroup,short int collisionFilterMask)
+	btSimpleBroadphaseProxy(const btPoint3& minpt,const btPoint3& maxpt,int shapeType,void* userPtr,short int collisionFilterGroup,short int collisionFilterMask,void* multiSapProxy)
-	:btBroadphaseProxy(userPtr,collisionFilterGroup,collisionFilterMask),
+	:btBroadphaseProxy(userPtr,collisionFilterGroup,collisionFilterMask,multiSapProxy),
 	m_min(minpt),m_max(maxpt)		
 	{
 		(void)shapeType;
@@ -120,7 +120,7 @@ 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, btDispatcher* dispatcher);
+	virtual btBroadphaseProxy*	createProxy(  const btVector3& aabbMin,  const btVector3& aabbMax,int shapeType,void* userPtr ,short int collisionFilterGroup,short int collisionFilterMask, btDispatcher* dispatcher,void* multiSapProxy);
 
 	virtual void	calculateOverlappingPairs(btDispatcher* dispatcher);
 
@@ -146,6 +146,12 @@ public:
 		aabbMin.setValue(-1e30f,-1e30f,-1e30f);
 		aabbMax.setValue(1e30f,1e30f,1e30f);
 	}
+
+	virtual void	printStats()
+	{
+//		printf("btSimpleBroadphase.h\n");
+//		printf("numHandles = %d, maxHandles = %d\n",m_numHandles,m_maxHandles);
+	}
 };
 
 

src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp

@@ -28,8 +28,9 @@ subject to the following restrictions:
 
 int gNumManifold = 0;
 
+#ifdef BT_DEBUG
 #include <stdio.h>
-
+#endif
 
 
 btCollisionDispatcher::btCollisionDispatcher (btCollisionConfiguration* collisionConfiguration): 

src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp

@@ -104,7 +104,7 @@ void	btCollisionWorld::addCollisionObject(btCollisionObject* collisionObject,sho
 			collisionObject,
 			collisionFilterGroup,
 			collisionFilterMask,
-			m_dispatcher1
+			m_dispatcher1,0
 			))	;
 
 		

src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp

@@ -15,7 +15,7 @@ subject to the following restrictions:
 
 #include "btConvexConvexAlgorithm.h"
 
-#include <stdio.h>
+//#include <stdio.h>
 #include "BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h"
 #include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h"
 #include "BulletCollision/CollisionDispatch/btCollisionObject.h"

src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp

@@ -7,7 +7,7 @@
 #include "BulletCollision/CollisionDispatch/btCollisionObject.h"
 #include "BulletCollision/CollisionDispatch/btCollisionWorld.h"
 
-#include <stdio.h>
+//#include <stdio.h>
 #include "LinearMath/btQuickprof.h"
 
 btSimulationIslandManager::btSimulationIslandManager()
@@ -138,7 +138,7 @@ class btPersistentManifoldSortPredicate
 void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher,btCollisionObjectArray& collisionObjects, IslandCallback* callback)
 {
 
-	BT_PROFILE("islandUnionFindAndHeapSort");
+	BT_PROFILE("islandUnionFindAndQuickSort");
 	
 	//we are going to sort the unionfind array, and store the element id in the size
 	//afterwards, we clean unionfind, to make sure no-one uses it anymore
@@ -170,7 +170,7 @@ void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher,
 			btCollisionObject* colObj0 = collisionObjects[i];
 			if ((colObj0->getIslandTag() != islandId) && (colObj0->getIslandTag() != -1))
 			{
-				printf("error in island management\n");
+//				printf("error in island management\n");
 			}
 
 			assert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
@@ -197,7 +197,7 @@ void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher,
 				btCollisionObject* colObj0 = collisionObjects[i];
 				if ((colObj0->getIslandTag() != islandId) && (colObj0->getIslandTag() != -1))
 				{
-					printf("error in island management\n");
+//					printf("error in island management\n");
 				}
 
 				assert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
@@ -218,7 +218,7 @@ void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher,
 				btCollisionObject* colObj0 = collisionObjects[i];
 				if ((colObj0->getIslandTag() != islandId) && (colObj0->getIslandTag() != -1))
 				{
-					printf("error in island management\n");
+//					printf("error in island management\n");
 				}
 
 				assert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
@@ -286,7 +286,7 @@ void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher,
 	int numManifolds = int (m_islandmanifold.size());
 
 	//we should do radix sort, it it much faster (O(n) instead of O (n log2(n))
-	m_islandmanifold.heapSort(btPersistentManifoldSortPredicate());
+	m_islandmanifold.quickSort(btPersistentManifoldSortPredicate());
 
 	//now process all active islands (sets of manifolds for now)
 

src/BulletCollision/CollisionDispatch/btUnionFind.cpp

@@ -77,8 +77,7 @@ void	btUnionFind::sortIslands()
 	
 	 // Sort the vector using predicate and std::sort
 	  //std::sort(m_elements.begin(), m_elements.end(), btUnionFindElementSortPredicate);
-	//perhaps use radix sort?
+	  m_elements.quickSort(btUnionFindElementSortPredicate());
-	  m_elements.heapSort(btUnionFindElementSortPredicate());
 
 }
 

src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.cpp

@@ -13,7 +13,7 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
-#include <stdio.h>
+//#include <stdio.h>
 
 #include "BulletCollision/CollisionShapes/btConvexShape.h"
 #include "BulletCollision/CollisionShapes/btTriangleShape.h"

src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.cpp

@@ -26,7 +26,7 @@ subject to the following restrictions:
 
 #include "btVoronoiSimplexSolver.h"
 #include <assert.h>
-#include <stdio.h>
+//#include <stdio.h>
 
 #define VERTA  0
 #define VERTB  1

src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp

@@ -625,7 +625,7 @@ void	btDiscreteDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
 		
 	
 
-	sortedConstraints.heapSort(btSortConstraintOnIslandPredicate());
+	sortedConstraints.quickSort(btSortConstraintOnIslandPredicate());
 	
 	btTypedConstraint** constraintsPtr = getNumConstraints() ? &sortedConstraints[0] : 0;
 	

src/LinearMath/btAlignedObjectArray.h

@@ -253,6 +253,46 @@ class btAlignedObjectArray
 				}
 		};
 	
+		template <typename L>
+		void quickSortInternal(L CompareFunc,int lo, int hi)
+		{
+		//  lo is the lower index, hi is the upper index
+		//  of the region of array a that is to be sorted
+			int i=lo, j=hi;
+			T x=m_data[(lo+hi)/2];
+
+			//  partition
+			do
+			{    
+				while (CompareFunc(m_data[i],x)) 
+					i++; 
+				while (CompareFunc(x,m_data[j])) 
+					j--;
+				if (i<=j)
+				{
+					swap(i,j);
+					i++; j--;
+				}
+			} while (i<=j);
+
+			//  recursion
+			if (lo<j) 
+				quickSortInternal( CompareFunc, lo, j);
+			if (i<hi) 
+				quickSortInternal( CompareFunc, i, hi);
+		}
+
+
+		template <typename L>
+		void quickSort(L CompareFunc)
+		{
+			//don't sort 0 or 1 elements
+			if (size()>1)
+			{
+				quickSortInternal(CompareFunc,0,size()-1);
+			}
+		}
+
 
 		///heap sort from http://www.csse.monash.edu.au/~lloyd/tildeAlgDS/Sort/Heap/
 		template <typename L>

src/btBulletCollisionCommon.h

@@ -50,7 +50,7 @@ subject to the following restrictions:
 #include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
 #include "BulletCollision/BroadphaseCollision/btSimpleBroadphase.h"
 #include "BulletCollision/BroadphaseCollision/btAxisSweep3.h"
-
+#include "BulletCollision/BroadphaseCollision/btMultiSapBroadphase.h"
 
 ///Math library & Utils
 #include "LinearMath/btQuaternion.h"