- 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
+	short int m_collisionFilterGroup;
-	union
+	short int m_collisionFilterMask;
-	{
-		struct
-		{
-			short int m_collisionFilterGroup;
-			short int m_collisionFilterMask;
-		};
 
-		void*	m_multiSapParentProxy;
+	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);
+	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,7 +293,16 @@ 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);
+	}
+*/
 
 
 	//update
@@ -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,8 +373,7 @@ 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;
 
 			bool needsRemoval = false;
@@ -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

@@ -1,127 +1,127 @@
-/*
+/*
-Bullet Continuous Collision Detection and Physics Library
+Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
-
+
-This software is provided 'as-is', without any express or implied warranty.
+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.
+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, 
+Permission is granted to anyone to use this software for any purpose, 
-including commercial applications, and to alter it and redistribute it freely, 
+including commercial applications, and to alter it and redistribute it freely, 
-subject to the following restrictions:
+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.
+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.
+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.
+3. This notice may not be removed or altered from any source distribution.
-*/
+*/
-#ifndef BT_MULTI_SAP_BROADPHASE
+#ifndef BT_MULTI_SAP_BROADPHASE
-#define BT_MULTI_SAP_BROADPHASE
+#define BT_MULTI_SAP_BROADPHASE
-
+
-#include "btBroadphaseInterface.h"
+#include "btBroadphaseInterface.h"
-#include "LinearMath/btAlignedObjectArray.h"
+#include "LinearMath/btAlignedObjectArray.h"
-#include "btOverlappingPairCache.h"
+#include "btOverlappingPairCache.h"
-
+
-
+
-class btBroadphaseInterface;
+class btBroadphaseInterface;
-class btSimpleBroadphase;
+class btSimpleBroadphase;
-
+
-
+
-typedef btAlignedObjectArray<btBroadphaseInterface*> btSapBroadphaseArray;
+typedef btAlignedObjectArray<btBroadphaseInterface*> btSapBroadphaseArray;
-
+
-///multi SAP broadphase
+///multi SAP broadphase
-///See http://www.continuousphysics.com/Bullet/phpBB2/viewtopic.php?t=328
+///See http://www.continuousphysics.com/Bullet/phpBB2/viewtopic.php?t=328
-///and http://www.continuousphysics.com/Bullet/phpBB2/viewtopic.php?t=1329
+///and http://www.continuousphysics.com/Bullet/phpBB2/viewtopic.php?t=1329
-class btMultiSapBroadphase :public btBroadphaseInterface
+class btMultiSapBroadphase :public btBroadphaseInterface
-{
+{
-	btSapBroadphaseArray	m_sapBroadphases;
+	btSapBroadphaseArray	m_sapBroadphases;
-	
+	
-	btSimpleBroadphase*		m_simpleBroadphase;
+	btSimpleBroadphase*		m_simpleBroadphase;
-
+
-	btOverlappingPairCache*	m_overlappingPairs;
+	btOverlappingPairCache*	m_overlappingPairs;
-
+
-	class btOptimizedBvh*			m_optimizedAabbTree;
+	class btOptimizedBvh*			m_optimizedAabbTree;
-
+
-
+
-	bool					m_ownsPairCache;
+	bool					m_ownsPairCache;
-	
+	
-	btOverlapFilterCallback*	m_filterCallback;
+	btOverlapFilterCallback*	m_filterCallback;
-
+
-	int			m_invalidPair;
+	int			m_invalidPair;
-
+
-	struct	btBridgeProxy
+	struct	btBridgeProxy
-	{
+	{
-		btBroadphaseProxy*		m_childProxy;
+		btBroadphaseProxy*		m_childProxy;
-		btBroadphaseInterface*	m_childBroadphase;
+		btBroadphaseInterface*	m_childBroadphase;
-	};
+	};
-
+
-
+
-public:
+public:
-
+
-	struct	btMultiSapProxy	: public btBroadphaseProxy
+	struct	btMultiSapProxy	: public btBroadphaseProxy
-	{
+	{
-
+
-		///array with all the entries that this proxy belongs to
+		///array with all the entries that this proxy belongs to
-		btAlignedObjectArray<btBridgeProxy*> m_bridgeProxies;
+		btAlignedObjectArray<btBridgeProxy*> m_bridgeProxies;
-		btVector3	m_aabbMin;
+		btVector3	m_aabbMin;
-		btVector3	m_aabbMax;
+		btVector3	m_aabbMax;
-
+
-		int	m_shapeType;
+		int	m_shapeType;
-
+
-/*		void*	m_userPtr;
+/*		void*	m_userPtr;
-		short int	m_collisionFilterGroup;
+		short int	m_collisionFilterGroup;
-		short int	m_collisionFilterMask;
+		short int	m_collisionFilterMask;
-*/
+*/
-		btMultiSapProxy(const btVector3& aabbMin,  const btVector3& aabbMax,int shapeType,void* userPtr, short int collisionFilterGroup,short int 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_aabbMin(aabbMin),
-			m_aabbMax(aabbMax),
+			m_aabbMax(aabbMax),
-			m_shapeType(shapeType)
+			m_shapeType(shapeType)
-		{
+		{
-
+
-		}
+		}
-
+
-		
+		
-	};
+	};
-
+
-protected:
+protected:
-
+
-	void	addToChildBroadphase(btMultiSapProxy* parentMultiSapProxy, btBroadphaseProxy* childProxy, btBroadphaseInterface*	childBroadphase);
+	void	addToChildBroadphase(btMultiSapProxy* parentMultiSapProxy, btBroadphaseProxy* childProxy, btBroadphaseInterface*	childBroadphase);
-
+
-	btAlignedObjectArray<btMultiSapProxy*> m_multiSapProxies;
+	btAlignedObjectArray<btMultiSapProxy*> m_multiSapProxies;
-
+
-public:
+public:
-
+
-	btMultiSapBroadphase(int maxProxies = 16384,btOverlappingPairCache* pairCache=0);
+	btMultiSapBroadphase(int maxProxies = 16384,btOverlappingPairCache* pairCache=0);
-
+
-
+
-	btSapBroadphaseArray&	getBroadphaseArray()
+	btSapBroadphaseArray&	getBroadphaseArray()
-	{
+	{
-		return m_sapBroadphases;
+		return m_sapBroadphases;
-	}
+	}
-
+
-	const btSapBroadphaseArray&	getBroadphaseArray() const
+	const btSapBroadphaseArray&	getBroadphaseArray() const
-	{
+	{
-		return m_sapBroadphases;
+		return m_sapBroadphases;
-	}
+	}
-
+
-	virtual ~btMultiSapBroadphase();
+	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	destroyProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher);
-	virtual void	setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax, btDispatcher* dispatcher);
+	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
+	///calculateOverlappingPairs is optional: incremental algorithms (sweep and prune) might do it during the set aabb
-	virtual void	calculateOverlappingPairs(btDispatcher* dispatcher);
+	virtual void	calculateOverlappingPairs(btDispatcher* dispatcher);
-
+
-	bool	testAabbOverlap(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1);
+	bool	testAabbOverlap(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1);
-
+
-	virtual	btOverlappingPairCache*	getOverlappingPairCache()
+	virtual	btOverlappingPairCache*	getOverlappingPairCache()
-	{
+	{
-		return m_overlappingPairs;
+		return m_overlappingPairs;
-	}
+	}
-	virtual	const btOverlappingPairCache*	getOverlappingPairCache() const
+	virtual	const btOverlappingPairCache*	getOverlappingPairCache() const
-	{
+	{
-		return m_overlappingPairs;
+		return m_overlappingPairs;
-	}
+	}
-
+
 	///getAabb returns the axis aligned bounding box in the 'global' coordinate frame
 	///will add some transform later
 	virtual void getBroadphaseAabb(btVector3& aabbMin,btVector3& aabbMax) const
@@ -131,7 +131,11 @@ public:
 	}
 
 	void	buildTree(const btVector3& bvhAabbMin,const btVector3& bvhAabbMax);
-
+
-};
+	virtual void	printStats();
-
+
-#endif //BT_MULTI_SAP_BROADPHASE
+	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"