- removed STL from the Bullet library: replace std::vector by btAlignedObjectArray. Also removed the std::set for overlapping pair set, and turned it into an overlapping pair array. The SAP only adds objects, never removed. Removal is postponed for during traversal of overlapping pairs (duplicates and non-overlapping pairs are removed during that traversal).

- added heap sort and binary search/linear search to btAlignedObjectArray
- fixed wrong cast, thanks Hamstray, http://www.continuousphysics.com/Bullet/phpBB2/viewtopic.php?t=1015

src/BulletCollision/BroadphaseCollision/btAxisSweep3.cpp

@@ -50,6 +50,7 @@ void	btAxisSweep3::setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,con
 btAxisSweep3::btAxisSweep3(const btPoint3& worldAabbMin,const btPoint3& worldAabbMax, int maxHandles)
 :btOverlappingPairCache()
 {
+	m_invalidPair = 0;
 	//assert(bounds.HasVolume());
 
 	// 1 handle is reserved as sentinel
@@ -249,6 +250,97 @@ void btAxisSweep3::removeHandle(unsigned short handle)
 	
 }
 
+extern int gOverlappingPairs;
+
+void	btAxisSweep3::processAllOverlappingPairs(btOverlapCallback* callback)
+{
+
+	//perform a sort, to find duplicates and to sort 'invalid' pairs to the end
+	m_overlappingPairArray.heapSort(btBroadphasePairSortPredicate());
+
+	//remove the 'invalid' ones
+#ifdef USE_POPBACK_REMOVAL
+	while (m_invalidPair>0)
+	{
+		m_invalidPair--;
+		m_overlappingPairArray.pop_back();
+	}
+#else	
+	m_overlappingPairArray.resize(m_overlappingPairArray.size() - m_invalidPair);
+	m_invalidPair = 0;
+#endif
+	int i;
+
+	btBroadphasePair previousPair;
+	previousPair.m_pProxy0 = 0;
+	previousPair.m_pProxy1 = 0;
+	previousPair.m_algorithm = 0;
+	
+	
+	for (i=0;i<m_overlappingPairArray.size();i++)
+	{
+	
+		btBroadphasePair& pair = m_overlappingPairArray[i];
+
+		bool isDuplicate = (pair == previousPair);
+
+		previousPair = pair;
+
+		bool needsRemoval = false;
+
+		if (!isDuplicate)
+		{
+			bool hasOverlap = testOverlap(pair.m_pProxy0,pair.m_pProxy1);
+
+			if (hasOverlap)
+			{
+				needsRemoval = callback->processOverlap(pair);
+			} else
+			{
+				needsRemoval = true;
+			}
+		} else
+		{
+			//remove duplicate
+			needsRemoval = true;
+			//should have no algorithm
+			btAssert(!pair.m_algorithm);
+		}
+		
+		if (needsRemoval)
+		{
+			cleanOverlappingPair(pair);
+
+	//		m_overlappingPairArray.swap(i,m_overlappingPairArray.size()-1);
+	//		m_overlappingPairArray.pop_back();
+			pair.m_pProxy0 = 0;
+			pair.m_pProxy1 = 0;
+			m_invalidPair++;
+			gOverlappingPairs--;
+		} 
+		
+	}
+}
+
+
+bool btAxisSweep3::testOverlap(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1)
+{
+	const Handle* pHandleA = static_cast<Handle*>(proxy0);
+	const Handle* pHandleB = static_cast<Handle*>(proxy1);
+	
+	//optimization 1: check the array index (memory address), instead of the m_pos
+
+	for (int axis = 0; axis < 3; axis++)
+	{ 
+		if (pHandleA->m_maxEdges[axis] < pHandleB->m_minEdges[axis] || 
+			pHandleB->m_maxEdges[axis] < pHandleA->m_minEdges[axis]) 
+		{ 
+			return false; 
+		} 
+	} 
+	return true;
+}
+
 bool btAxisSweep3::testOverlap(int ignoreAxis,const Handle* pHandleA, const Handle* pHandleB)
 {
 	//optimization 1: check the array index (memory address), instead of the m_pos
@@ -379,10 +471,12 @@ void btAxisSweep3::sortMinUp(int axis, unsigned short edge, bool updateOverlaps)
 			// 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);
+				*/
 
 			}
 
@@ -421,6 +515,8 @@ void btAxisSweep3::sortMaxDown(int axis, unsigned short edge, bool updateOverlap
 			// if previous edge was a minimum remove any overlap between the two handles
 			if (updateOverlaps)
 			{
+				//this is done during the overlappingpairarray iteration/narrowphase collision
+				/*
 				Handle* handle0 = getHandle(pEdge->m_handle);
 				Handle* handle1 = getHandle(pPrev->m_handle);
 				btBroadphasePair* pair = findPair(handle0,handle1);
@@ -430,6 +526,8 @@ void btAxisSweep3::sortMaxDown(int axis, unsigned short edge, bool updateOverlap
 				{
 					removeOverlappingPair(*pair);
 				}
+				*/
+
 			}
 
 			// update edge reference in other handle

src/BulletCollision/BroadphaseCollision/btAxisSweep3.h

@@ -72,6 +72,7 @@ private:
 
 	Edge* m_pEdges[3];						// edge arrays for the 3 axes (each array has m_maxHandles * 2 + 2 sentinel entries)
 
+	int m_invalidPair;
 
 	// allocation/deallocation
 	unsigned short allocHandle();
@@ -80,6 +81,7 @@ private:
 
 	bool testOverlap(int ignoreAxis,const Handle* pHandleA, const Handle* pHandleB);
 
+
 	//Overlap* AddOverlap(unsigned short handleA, unsigned short handleB);
 	//void RemoveOverlap(unsigned short handleA, unsigned short handleB);
 
@@ -96,7 +98,7 @@ public:
 
 	virtual void	refreshOverlappingPairs()
 	{
-		//this is replace by sweep and prune
+		//this is performed incrementally by sweep and prune (add pair), and during pair traversal (remove pair)
 	}
 	
 	unsigned short addHandle(const btPoint3& aabbMin,const btPoint3& aabbMax, void* pOwner,short int collisionFilterGroup,short int collisionFilterMask);
@@ -104,11 +106,13 @@ public:
 	void updateHandle(unsigned short handle, const btPoint3& aabbMin,const btPoint3& aabbMax);
 	inline Handle* getHandle(unsigned short index) const {return m_pHandles + index;}
 
+	void	processAllOverlappingPairs(btOverlapCallback* callback);
 
 	//Broadphase Interface
 	virtual btBroadphaseProxy*	createProxy(  const btVector3& min,  const btVector3& max,int shapeType,void* userPtr ,short int collisionFilterGroup,short int collisionFilterMask);
 	virtual void	destroyProxy(btBroadphaseProxy* proxy);
 	virtual void	setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax);
+	bool testOverlap(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1);
 
 };
 

src/BulletCollision/BroadphaseCollision/btBroadphaseProxy.h

@@ -16,6 +16,7 @@ subject to the following restrictions:
 #ifndef BROADPHASE_PROXY_H
 #define BROADPHASE_PROXY_H
 
+#include "../../LinearMath/btScalar.h" //for SIMD_FORCE_INLINE
 
 
 /// btDispatcher uses these types
@@ -164,13 +165,33 @@ struct btBroadphasePair
 	mutable btCollisionAlgorithm* m_algorithm;
 };
 
-
+/*
 //comparison for set operation, see Solid DT_Encounter
-inline bool operator<(const btBroadphasePair& a, const btBroadphasePair& b) 
+SIMD_FORCE_INLINE bool operator<(const btBroadphasePair& a, const btBroadphasePair& b) 
 { 
     return a.m_pProxy0 < b.m_pProxy0 || 
         (a.m_pProxy0 == b.m_pProxy0 && a.m_pProxy1 < b.m_pProxy1); 
 }
+*/
+
+
+class btBroadphasePairSortPredicate
+{
+	public:
+
+		bool operator() ( const btBroadphasePair& a, const btBroadphasePair& b )
+		{
+			 return a.m_pProxy0 > b.m_pProxy0 || 
+				(a.m_pProxy0 == b.m_pProxy0 && a.m_pProxy1 > b.m_pProxy1) ||
+				(a.m_pProxy0 == b.m_pProxy0 && a.m_pProxy1 == b.m_pProxy1 && a.m_algorithm > b.m_algorithm); 
+		}
+};
+
+
+SIMD_FORCE_INLINE bool operator==(const btBroadphasePair& a, const btBroadphasePair& b) 
+{
+	 return (a.m_pProxy0 == b.m_pProxy0) && (a.m_pProxy1 == b.m_pProxy1);
+}
 
 
 #endif //BROADPHASE_PROXY_H

src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp

@@ -39,15 +39,15 @@ btOverlappingPairCache::~btOverlappingPairCache()
 void	btOverlappingPairCache::removeOverlappingPair(btBroadphasePair& findPair)
 {
 	
-	std::set<btBroadphasePair>::iterator it = m_overlappingPairSet.find(findPair);
-//	assert(it != m_overlappingPairSet.end());
-
-	if (it != m_overlappingPairSet.end())
+	int findIndex = m_overlappingPairArray.findLinearSearch(findPair);
+	if (findIndex < m_overlappingPairArray.size())
 	{
 		gOverlappingPairs--;
-		btBroadphasePair* pair = (btBroadphasePair*)(&(*it));
-		cleanOverlappingPair(*pair);	
-		m_overlappingPairSet.erase(it);
+		btBroadphasePair& pair = m_overlappingPairArray[findIndex];
+		cleanOverlappingPair(pair);
+		
+		m_overlappingPairArray.swap(findIndex,m_overlappingPairArray.size()-1);
+		m_overlappingPairArray.pop_back();
 	}
 }
 
@@ -78,7 +78,7 @@ void	btOverlappingPairCache::addOverlappingPair(btBroadphaseProxy* proxy0,btBroa
 
 	btBroadphasePair pair(*proxy0,*proxy1);
 	
-	m_overlappingPairSet.insert(pair);
+	m_overlappingPairArray.push_back(pair);
 	gOverlappingPairs++;
 	
 }
@@ -93,13 +93,15 @@ void	btOverlappingPairCache::addOverlappingPair(btBroadphaseProxy* proxy0,btBroa
 		return 0;
 
 	btBroadphasePair tmpPair(*proxy0,*proxy1);
-	std::set<btBroadphasePair>::iterator it = m_overlappingPairSet.find(tmpPair);
-	if ((it == m_overlappingPairSet.end()))
-		return 0;
+	int findIndex = m_overlappingPairArray.findLinearSearch(tmpPair);
 
-	//assert(it != m_overlappingPairSet.end());
-	 btBroadphasePair* pair = (btBroadphasePair*)(&(*it));
-	return pair;
+	if (findIndex < m_overlappingPairArray.size())
+	{
+		//assert(it != m_overlappingPairSet.end());
+		 btBroadphasePair* pair = &m_overlappingPairArray[findIndex];
+		return pair;
+	}
+	return 0;
 }
 
 
@@ -143,6 +145,8 @@ void	btOverlappingPairCache::cleanProxyFromPairs(btBroadphaseProxy* proxy)
 void	btOverlappingPairCache::removeOverlappingPairsContainingProxy(btBroadphaseProxy* proxy)
 {
 
+	assert(0);
+	//todo, implement more efficient
 	class	RemovePairCallback : public btOverlapCallback
 	{
 		btBroadphaseProxy* m_obsoleteProxy;
@@ -170,30 +174,23 @@ void	btOverlappingPairCache::removeOverlappingPairsContainingProxy(btBroadphaseP
 
 void	btOverlappingPairCache::processAllOverlappingPairs(btOverlapCallback* callback)
 {
-	std::set<btBroadphasePair>::iterator it = m_overlappingPairSet.begin();
-	for (; !(it==m_overlappingPairSet.end());)
+
+	int i;
+
+	for (i=0;i<m_overlappingPairArray.size();)
 	{
 	
-		btBroadphasePair* pair = (btBroadphasePair*)(&(*it));
+		btBroadphasePair* pair = &m_overlappingPairArray[i];
 		if (callback->processOverlap(*pair))
 		{
 			cleanOverlappingPair(*pair);
 
-			std::set<btBroadphasePair>::iterator it2 = it;
-			//why does next line not compile under OS X??
-#ifdef MAC_OSX_FIXED_STL_SET
-			it2++;
-			it = m_overlappingPairSet.erase(it);
-			assert(it == it2);
-#else
-			it++;
-			m_overlappingPairSet.erase(it2);
-#endif //MAC_OSX_FIXED_STL_SET
-
+			m_overlappingPairArray.swap(i,m_overlappingPairArray.size()-1);
+			m_overlappingPairArray.pop_back();
 			gOverlappingPairs--;
 		} else
 		{
-			it++;
+			i++;
 		}
 	}
 }

src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h

@@ -21,7 +21,7 @@ subject to the following restrictions:
 #include "btBroadphaseInterface.h"
 #include "btBroadphaseProxy.h"
 #include "../../LinearMath/btPoint3.h"
-#include <set>
+#include "../../LinearMath/btAlignedObjectArray.h"
 
 
 struct	btOverlapCallback
@@ -37,8 +37,9 @@ virtual ~btOverlapCallback()
 ///Typically managed by the Broadphase, Axis3Sweep or btSimpleBroadphase
 class	btOverlappingPairCache : public btBroadphaseInterface
 {
+protected:
 	//avoid brute-force finding all the time
-	std::set<btBroadphasePair>	m_overlappingPairSet;
+	btAlignedObjectArray<btBroadphasePair>	m_overlappingPairArray;
 	
 	//during the dispatch, check that user doesn't destroy/create proxy
 	bool		m_blockedForChanges;
@@ -48,7 +49,7 @@ class	btOverlappingPairCache : public btBroadphaseInterface
 	btOverlappingPairCache();	
 	virtual ~btOverlappingPairCache();
 
-	void	processAllOverlappingPairs(btOverlapCallback*);
+	virtual void	processAllOverlappingPairs(btOverlapCallback*);
 
 	void	removeOverlappingPair(btBroadphasePair& pair);
 

src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp

@@ -20,7 +20,7 @@ subject to the following restrictions:
 #include "LinearMath/btVector3.h"
 #include "LinearMath/btTransform.h"
 #include "LinearMath/btMatrix3x3.h"
-#include <vector>
+#include <new.h>
 
 
 void	btSimpleBroadphase::validate()

src/BulletCollision/CollisionDispatch/btCollisionCreateFunc.h

@@ -16,9 +16,8 @@ subject to the following restrictions:
 #ifndef COLLISION_CREATE_FUNC
 #define COLLISION_CREATE_FUNC
 
-#include <vector>
-
-typedef std::vector<class btCollisionObject*> btCollisionObjectArray;
+#include "../../LinearMath/btAlignedObjectArray.h"
+typedef btAlignedObjectArray<class btCollisionObject*> btCollisionObjectArray;
 class btCollisionAlgorithm;
 class btCollisionObject;
 

src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp

@@ -25,7 +25,6 @@ subject to the following restrictions:
 #include "BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h"
 #include "BulletCollision/CollisionShapes/btCollisionShape.h"
 #include "BulletCollision/CollisionDispatch/btCollisionObject.h"
-#include <algorithm>
 #include "BulletCollision/BroadphaseCollision/btOverlappingPairCache.h"
 
 int gNumManifold = 0;
@@ -135,20 +134,17 @@ void btCollisionDispatcher::releaseManifold(btPersistentManifold* manifold)
 	gNumManifold--;
 
 	//printf("releaseManifold: gNumManifold %d\n",gNumManifold);
-
 	clearManifold(manifold);
 
-	std::vector<btPersistentManifold*>::iterator i =
-		std::find(m_manifoldsPtr.begin(), m_manifoldsPtr.end(), manifold);
-	if (!(i == m_manifoldsPtr.end()))
+	///todo: this can be improved a lot, linear search might be slow part!
+	int findIndex = m_manifoldsPtr.findLinearSearch(manifold);
+	if (findIndex < m_manifoldsPtr.size())
 	{
-		std::swap(*i, m_manifoldsPtr.back());
+		m_manifoldsPtr.swap(findIndex,m_manifoldsPtr.size()-1);
 		m_manifoldsPtr.pop_back();
 		delete manifold;
-
 	}
 	
-	
 }
 
 	

src/BulletCollision/CollisionDispatch/btCollisionDispatcher.h

@@ -22,7 +22,7 @@ subject to the following restrictions:
 #include "../CollisionDispatch/btManifoldResult.h"
 
 #include "../BroadphaseCollision/btBroadphaseProxy.h"
-
+#include "../../LinearMath/btAlignedObjectArray.h"
 
 class btIDebugDraw;
 class btOverlappingPairCache;
@@ -43,7 +43,7 @@ class btCollisionDispatcher : public btDispatcher
 {
 	int m_count;
 	
-	std::vector<btPersistentManifold*>	m_manifoldsPtr;
+	btAlignedObjectArray<btPersistentManifold*>	m_manifoldsPtr;
 
 	bool m_useIslands;
 	

src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp

@@ -31,7 +31,6 @@ subject to the following restrictions:
 #include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
 #include "BulletCollision/BroadphaseCollision/btSimpleBroadphase.h"
 
-#include <algorithm>
 
 btCollisionWorld::btCollisionWorld(btDispatcher* dispatcher,btOverlappingPairCache* pairCache, int stackSize)
 :m_dispatcher1(dispatcher),
@@ -50,13 +49,10 @@ btCollisionWorld::~btCollisionWorld()
 	delete m_stackAlloc;
 
 	//clean up remaining objects
-	std::vector<btCollisionObject*>::iterator i;
-
-	for (i=m_collisionObjects.begin();
-	!(i==m_collisionObjects.end()); i++)
-
+	int i;
+	for (i=0;i<m_collisionObjects.size();i++)
 	{
-		btCollisionObject* collisionObject= (*i);
+		btCollisionObject* collisionObject= m_collisionObjects[i];
 		
 		btBroadphaseProxy* bp = collisionObject->getBroadphaseHandle();
 		if (bp)
@@ -89,9 +85,7 @@ void	btCollisionWorld::addCollisionObject(btCollisionObject* collisionObject,sho
 {
 
 	//check that the object isn't already added
-	std::vector<btCollisionObject*>::iterator i =	std::find(m_collisionObjects.begin(), m_collisionObjects.end(), collisionObject);
-	assert(i == m_collisionObjects.end());
-
+		btAssert( m_collisionObjects.findLinearSearch(collisionObject)  == m_collisionObjects.size());
 
 		m_collisionObjects.push_back(collisionObject);
 
@@ -131,7 +125,7 @@ void	btCollisionWorld::performDiscreteCollisionDetection()
 	//update aabb (of all moved objects)
 
 	btVector3 aabbMin,aabbMax;
-	for (size_t i=0;i<m_collisionObjects.size();i++)
+	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);
@@ -169,13 +163,9 @@ void	btCollisionWorld::removeCollisionObject(btCollisionObject* collisionObject)
 	}
 
 
-	std::vector<btCollisionObject*>::iterator i =	std::find(m_collisionObjects.begin(), m_collisionObjects.end(), collisionObject);
-		
-	if (!(i == m_collisionObjects.end()))
-		{
-			std::swap(*i, m_collisionObjects.back());
-			m_collisionObjects.pop_back();
-		}
+	//swapremove
+	m_collisionObjects.remove(collisionObject);
+
 }
 
 
@@ -324,13 +314,10 @@ void	btCollisionWorld::rayTest(const btVector3& rayFromWorld, const btVector3& r
 
 	/// go over all objects, and if the ray intersects their aabb, do a ray-shape query using convexCaster (CCD)
 	
-	std::vector<btCollisionObject*>::iterator iter;
-	
-	for (iter=m_collisionObjects.begin();
-	!(iter==m_collisionObjects.end()); iter++)
+	int i;
+	for (i=0;i<m_collisionObjects.size();i++)
 	{
-		
-		btCollisionObject*	collisionObject= (*iter);
+		btCollisionObject*	collisionObject= m_collisionObjects[i];
 
 		//RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject();
 		btVector3 collisionObjectAabbMin,collisionObjectAabbMax;

src/BulletCollision/CollisionDispatch/btCollisionWorld.h

@@ -72,10 +72,7 @@ class btBroadphaseInterface;
 #include "btCollisionObject.h"
 #include "btCollisionDispatcher.h" //for definition of btCollisionObjectArray
 #include "../BroadphaseCollision/btOverlappingPairCache.h"
-
-#include <vector>
-
-
+#include "../../LinearMath/btAlignedObjectArray.h"
 
 ///CollisionWorld is interface and container for the collision detection
 class btCollisionWorld
@@ -84,7 +81,7 @@ class btCollisionWorld
 	
 protected:
 
-	std::vector<btCollisionObject*>	m_collisionObjects;
+	btAlignedObjectArray<btCollisionObject*>	m_collisionObjects;
 	
 	btDispatcher*	m_dispatcher1;
 

src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h

@@ -23,14 +23,14 @@ subject to the following restrictions:
 #include "../NarrowPhaseCollision/btPersistentManifold.h"
 class btDispatcher;
 #include "../BroadphaseCollision/btBroadphaseProxy.h"
-#include <vector>
 #include "btCollisionCreateFunc.h"
+#include "../../LinearMath/btAlignedObjectArray.h"
 
 /// btCompoundCollisionAlgorithm  supports collision between CompoundCollisionShapes and other collision shapes
 /// Place holder, not fully implemented yet
 class btCompoundCollisionAlgorithm  : public btCollisionAlgorithm
 {
-	std::vector<btCollisionAlgorithm*> m_childCollisionAlgorithms;
+	btAlignedObjectArray<btCollisionAlgorithm*> m_childCollisionAlgorithms;
 	bool m_isSwapped;
 	
 public:

src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp

@@ -8,7 +8,6 @@
 #include "BulletCollision/CollisionDispatch/btCollisionWorld.h"
 
 #include <stdio.h>
-#include <algorithm>
 
 
 btSimulationIslandManager::btSimulationIslandManager()
@@ -57,14 +56,12 @@ void	btSimulationIslandManager::updateActivationState(btCollisionWorld* colWorld
 	
 	// put the index into m_controllers into m_tag	
 	{
-		std::vector<btCollisionObject*>::iterator i;
 		
 		int index = 0;
-		for (i=colWorld->getCollisionObjectArray().begin();
-		!(i==colWorld->getCollisionObjectArray().end()); i++)
+		int i;
+		for (i=0;i<colWorld->getCollisionObjectArray().size(); i++)
 		{
-			
-			btCollisionObject*	collisionObject= (*i);
+			btCollisionObject*	collisionObject= colWorld->getCollisionObjectArray()[i];
 			collisionObject->setIslandTag(index);
 			collisionObject->setHitFraction(btScalar(1.));
 			index++;
@@ -88,14 +85,11 @@ void	btSimulationIslandManager::storeIslandActivationState(btCollisionWorld* col
 	{
 		
 		
-		std::vector<btCollisionObject*>::iterator i;
-		
 		int index = 0;
-		for (i=colWorld->getCollisionObjectArray().begin();
-		!(i==colWorld->getCollisionObjectArray().end()); i++)
+		int i;
+		for (i=0;i<colWorld->getCollisionObjectArray().size();i++)
 		{
-			btCollisionObject* collisionObject= (*i);
-			
+			btCollisionObject* collisionObject= colWorld->getCollisionObjectArray()[i];
 			if (collisionObject->mergesSimulationIslands())
 			{
 				collisionObject->setIslandTag( m_unionFind.find(index) );
@@ -127,6 +121,24 @@ bool btPersistentManifoldSortPredicate(const btPersistentManifold* lhs, const bt
 }
 
 
+/// function object that routes calls to operator<
+class btPersistentManifoldSortPredicate
+{
+	public:
+
+		bool operator() ( const btPersistentManifold* lhs, const btPersistentManifold* rhs )
+		{
+			int rIslandId0,lIslandId0;
+			rIslandId0 = getIslandId(rhs);
+			lIslandId0 = getIslandId(lhs);
+			return lIslandId0 < rIslandId0;
+		}
+};
+
+
+
+
+
 //
 // todo: this is random access, it can be walked 'cache friendly'!
 //
@@ -224,7 +236,7 @@ void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher,
 		}
 	}
 
-	std::vector<btPersistentManifold*>  islandmanifold;
+	btAlignedObjectArray<btPersistentManifold*>  islandmanifold;
 	int i;
 	int maxNumManifolds = dispatcher->getNumManifolds();
 	islandmanifold.reserve(maxNumManifolds);
@@ -261,7 +273,10 @@ void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher,
 
 	// Sort manifolds, based on islands
 	// Sort the vector using predicate and std::sort
-	std::sort(islandmanifold.begin(), islandmanifold.end(), btPersistentManifoldSortPredicate);
+	//std::sort(islandmanifold.begin(), islandmanifold.end(), btPersistentManifoldSortPredicate);
+
+	//we should do radix sort, it it much faster (O(n) instead of O (n log2(n))
+	islandmanifold.heapSort(btPersistentManifoldSortPredicate);
 
 	//now process all active islands (sets of manifolds for now)
 

src/BulletCollision/CollisionDispatch/btUnionFind.cpp

@@ -15,7 +15,6 @@ subject to the following restrictions:
 
 #include "btUnionFind.h"
 #include <assert.h>
-#include <algorithm>
 
 
 
@@ -50,11 +49,16 @@ void	btUnionFind::reset(int N)
 	} 
 }
 
-bool btUnionFindElementSortPredicate(const btElement& lhs, const btElement& rhs)
-{
-	return lhs.m_id < rhs.m_id;
-}
 
+class btUnionFindElementSortPredicate
+{
+	public:
+
+		bool operator() ( const btElement& lhs, const btElement& rhs )
+		{
+			return lhs.m_id < rhs.m_id;
+		}
+};
 
 ///this is a special operation, destroying the content of btUnionFind.
 ///it sorts the elements, based on island id, in order to make it easy to iterate over islands
@@ -71,7 +75,9 @@ void	btUnionFind::sortIslands()
 	}
 	
 	 // Sort the vector using predicate and std::sort
-	  std::sort(m_elements.begin(), m_elements.end(), btUnionFindElementSortPredicate);
+	  //std::sort(m_elements.begin(), m_elements.end(), btUnionFindElementSortPredicate);
+	//perhaps use radix sort?
+	  m_elements.heapSort(btUnionFindElementSortPredicate());
 
 }
 

src/BulletCollision/CollisionDispatch/btUnionFind.h

@@ -16,7 +16,8 @@ subject to the following restrictions:
 #ifndef UNION_FIND_H
 #define UNION_FIND_H
 
-#include <vector>
+#include "../../LinearMath/btAlignedObjectArray.h"
+
 struct	btElement
 {
 	int	m_id;
@@ -29,7 +30,7 @@ struct	btElement
 class btUnionFind
   {
     private:
-		std::vector<btElement>	m_elements;
+		btAlignedObjectArray<btElement>	m_elements;
 
     public:
 	  

src/BulletCollision/CollisionShapes/btCompoundShape.h

@@ -21,7 +21,6 @@ subject to the following restrictions:
 #include "../../LinearMath/btVector3.h"
 #include "../../LinearMath/btTransform.h"
 #include "../../LinearMath/btMatrix3x3.h"
-#include <vector>
 #include "btCollisionMargin.h"
 #include "../../LinearMath/btAlignedObjectArray.h"
 

src/BulletCollision/CollisionShapes/btConvexShape.h

@@ -21,7 +21,6 @@ subject to the following restrictions:
 #include "../../LinearMath/btVector3.h"
 #include "../../LinearMath/btTransform.h"
 #include "../../LinearMath/btMatrix3x3.h"
-#include <vector>
 #include "btCollisionMargin.h"
 
 //todo: get rid of this btConvexCastResult thing!

src/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.h

@@ -5,7 +5,6 @@
 #include "btPolyhedralConvexShape.h"
 #include "../BroadphaseCollision/btBroadphaseProxy.h" // for the types
 
-#include <vector>
 
 /// btConvexTriangleMeshShape is a convex hull of a triangle mesh. If you just have a point cloud, you can use btConvexHullShape instead.
 /// It uses the btStridingMeshInterface instead of a point cloud. This can avoid the duplication of the triangle mesh data.

src/BulletCollision/CollisionShapes/btEmptyShape.h

@@ -21,7 +21,6 @@ subject to the following restrictions:
 #include "../../LinearMath/btVector3.h"
 #include "../../LinearMath/btTransform.h"
 #include "../../LinearMath/btMatrix3x3.h"
-#include <vector>
 #include "btCollisionMargin.h"
 
 

src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h

@@ -137,4 +137,6 @@ public:
 
 
 
+
+
 #endif //PERSISTENT_MANIFOLD_H