- 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

Demos/BasicDemo/BasicDemo.cpp

@@ -117,8 +117,9 @@ void	BasicDemo::initPhysics()
 
 
 	m_dispatcher = new	btCollisionDispatcher(true);
-	
+//#define USE_SWEEP_AND_PRUNE 1
 #ifdef USE_SWEEP_AND_PRUNE
+#define maxProxies 8192
 	btVector3 worldAabbMin(-10000,-10000,-10000);
 	btVector3 worldAabbMax(10000,10000,10000);
 	m_overlappingPairCache = new btAxisSweep3(worldAabbMin,worldAabbMax,maxProxies);

Demos/BasicDemo/BasicDemo.h

@@ -17,7 +17,6 @@ subject to the following restrictions:
 
 #include "DemoApplication.h"
 
-#include <vector>
 
 class btCollisionShape;
 class btOverlappingPairCache;
@@ -30,7 +29,7 @@ class BasicDemo : public DemoApplication
 {
 
 	//keep the collision shapes, for deletion/cleanup
-	std::vector<btCollisionShape*>	m_collisionShapes;
+	btAlignedObjectArray<btCollisionShape*>	m_collisionShapes;
 
 	btOverlappingPairCache*	m_overlappingPairCache;
 

Demos/BspDemo/BspConverter.cpp

@@ -17,6 +17,9 @@ subject to the following restrictions:
 #include "BspLoader.h"
 #include "LinearMath/btVector3.h"
 #include "LinearMath/btGeometryUtil.h"
+#include <stdio.h>
+#include <string.h>
+
 
 void BspConverter::convertBsp(BspLoader& bspLoader,float scaling)
 {

Demos/BspDemo/BspConverter.h

@@ -17,7 +17,6 @@ subject to the following restrictions:
 #define BSP_CONVERTER_H
 
 class BspLoader;
-#include <vector>
 #include "LinearMath/btVector3.h"
 #include "LinearMath/btAlignedObjectArray.h"
 

Demos/BspDemo/BspDemo.cpp

@@ -195,9 +195,9 @@ void	BspDemo::initPhysics(char* bspfilename)
 void BspDemo::clientMoveAndDisplay()
 {
 	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); 
-
 	float dt = m_clock.getTimeMicroseconds() * 0.000001f;
 	m_clock.reset();
+
 	m_dynamicsWorld->stepSimulation(dt);
 
 	renderme();

Demos/BspDemo/BspLoader.cpp

@@ -22,6 +22,8 @@ Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
 
 
 #include "BspLoader.h"
+#include <stdio.h>
+#include <string.h>
 
 typedef struct
 {
@@ -45,6 +47,20 @@ bool tokenready;                     // only true if UnGetToken was just called
 
 int extrasize = 100;
 
+BspLoader::BspLoader()
+	:m_num_entities(0)
+{
+	m_Endianness = getMachineEndianness();
+	if (m_Endianness == BSP_BIG_ENDIAN)
+	{
+		printf("Machine is BIG_ENDIAN\n");
+	} else
+	{
+		printf("Machine is Little Endian\n");
+	}
+}
+
+
 bool	BspLoader::loadBSPFile( void* memoryBuffer) {
 	
 	BSPHeader	*header = (BSPHeader*) memoryBuffer;

Demos/BspDemo/BspLoader.h

@@ -25,6 +25,8 @@ Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
 #ifndef BSP_LOADER_H
 #define BSP_LOADER_H
 
+#include "LinearMath/btAlignedObjectArray.h"
+
 #define	BSPMAXTOKEN	1024
 #define	BSPMAX_KEY				32
 #define	BSPMAX_VALUE			1024
@@ -49,7 +51,6 @@ Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
 #define	HEADER_LUMPS		17
 #define	MAX_QPATH		64
 
-#include <vector>
 
 
 typedef struct {
@@ -172,18 +173,8 @@ class BspLoader
 
 	public:
 
-		BspLoader()
+		BspLoader();
-			:m_num_entities(0)
+
-		{
-			m_Endianness = getMachineEndianness();
-			if (m_Endianness == BSP_BIG_ENDIAN)
-			{
-				printf("Machine is BIG_ENDIAN\n");
-			} else
-			{
-				printf("Machine is Little Endian\n");
-			}
-		}
 		bool	loadBSPFile( void* memoryBuffer);
 
 		const char* getValueForKey( const BSPEntity *ent, const char *key ) const;
@@ -236,53 +227,53 @@ class BspLoader
 	
 	public: //easier for conversion
 		int			m_num_entities;
-		std::vector<BSPEntity>	m_entities;
+		btAlignedObjectArray<BSPEntity>	m_entities;
 		
 		int			m_nummodels;
-		std::vector<BSPModel>	m_dmodels;
+		btAlignedObjectArray<BSPModel>	m_dmodels;
 
 		int			m_numShaders;
-		std::vector<BSPShader>	m_dshaders;
+		btAlignedObjectArray<BSPShader>	m_dshaders;
 
 		int			m_entdatasize;
-		std::vector<char>		m_dentdata;
+		btAlignedObjectArray<char>		m_dentdata;
 
 		int			m_numleafs;
-		std::vector<BSPLeaf>		m_dleafs;
+		btAlignedObjectArray<BSPLeaf>		m_dleafs;
 
 		int			m_numplanes;
-		std::vector<BSPPlane>	m_dplanes;
+		btAlignedObjectArray<BSPPlane>	m_dplanes;
 
 		int			m_numnodes;
-		std::vector<BSPNode>		m_dnodes;
+		btAlignedObjectArray<BSPNode>		m_dnodes;
 
 		int			m_numleafsurfaces;
-		std::vector<int>			m_dleafsurfaces;
+		btAlignedObjectArray<int>			m_dleafsurfaces;
 
 		int			m_numleafbrushes;
-		std::vector<int>			m_dleafbrushes;
+		btAlignedObjectArray<int>			m_dleafbrushes;
 
 		int			m_numbrushes;
-		std::vector<BSPBrush>	m_dbrushes;
+		btAlignedObjectArray<BSPBrush>	m_dbrushes;
 
 		int			m_numbrushsides;
-		std::vector<BSPBrushSide>	m_dbrushsides;
+		btAlignedObjectArray<BSPBrushSide>	m_dbrushsides;
 
 		int			m_numLightBytes;
-		std::vector<unsigned char>		m_lightBytes;
+		btAlignedObjectArray<unsigned char>		m_lightBytes;
 
 		int			m_numGridPoints;
-		std::vector<unsigned char>		m_gridData;
+		btAlignedObjectArray<unsigned char>		m_gridData;
 
 		int			m_numVisBytes;
-		std::vector<unsigned char>		m_visBytes;
+		btAlignedObjectArray<unsigned char>		m_visBytes;
 
 		
 		int			m_numDrawIndexes;
-		std::vector<int>			m_drawIndexes;
+		btAlignedObjectArray<int>			m_drawIndexes;
 
 		int			m_numDrawSurfaces;
-		std::vector<BSPSurface>	m_drawSurfaces;
+		btAlignedObjectArray<BSPSurface>	m_drawSurfaces;
 
 		enum
 		{

Demos/CcdPhysicsDemo/CcdPhysicsDemo.cpp

@@ -203,6 +203,7 @@ void CcdPhysicsDemo::clientMoveAndDisplay()
 
 #endif //USE_KINEMATIC_GROUND
 
+
 	float dt = m_clock.getTimeMicroseconds() * 0.000001f;
 	m_clock.reset();
 	printf("dt = %f: ",dt);

Demos/ColladaDemo/ColladaConverter.cpp

@@ -747,7 +747,10 @@ void	ColladaConverter::PreparePhysicsObject(struct btRigidBodyInput& input, bool
 
 	}
 
+	if (startScale.length() < 0.001)
+		printf("invalid scale\n");
 
+	colShape->setLocalScaling(startScale);
 
 	btRigidBody* body= createRigidBody(isDynamics,mass,startTransform,colShape);
 	if (body)
@@ -881,7 +884,7 @@ bool ColladaConverter::saveAs(const char* filename)
 }
 
 //some code that de-mangles the windows filename passed in as argument
-char cleaned_filename[512];
+char cleaned_filename[513];
 char* getLastFileName()
 {
 	return cleaned_filename;
@@ -928,7 +931,7 @@ char* fixFileName(const char* lpCmdLine)
 		out++;
 	}
 	
-	cleaned_filename[i] = '\0'; 
+	cleaned_filename[i+1] = '\0'; 
 	return cleaned_filename;
 }
 

Demos/ConcaveDemo/ConcavePhysicsDemo.cpp

@@ -192,7 +192,6 @@ void ConcaveDemo::clientMoveAndDisplay()
 
 	float dt = m_clock.getTimeMicroseconds() * 0.000001f;
 	m_clock.reset();
-	
 	m_dynamicsWorld->stepSimulation(dt);
 	
 	renderme();

Demos/ConstraintDemo/ConstraintDemo.cpp

@@ -172,7 +172,6 @@ void ConstraintDemo::clientMoveAndDisplay()
 
  	float dt = float(m_clock.getTimeMicroseconds()) * 0.000001f;
 	m_clock.reset();
-
 	//printf("dt = %f: ",dt);
 	
  {

Demos/MovingConcaveDemo/ConcaveDemo.h

@@ -17,7 +17,7 @@ subject to the following restrictions:
 
 #include "DemoApplication.h"
 
-class btCollisionAlgorithmCreateFunc;
+struct btCollisionAlgorithmCreateFunc;
 
 ///ConcaveDemo shows usage of static concave triangle meshes
 ///It also shows per-triangle material (friction/restitution) through CustomMaterialCombinerCallback

Demos/MovingConcaveDemo/ConcavePhysicsDemo.cpp

@@ -1868,7 +1868,6 @@ void ConcaveDemo::clientMoveAndDisplay()
 
  	float dt = float(m_clock.getTimeMicroseconds()) * 0.000001f;
 	m_clock.reset();
-
 	m_dynamicsWorld->stepSimulation(dt);
 
 	renderme();

Demos/OpenGL/DemoApplication.cpp

@@ -900,13 +900,16 @@ void	DemoApplication::clientResetScene()
 	{
 		btCollisionObject* colObj = m_dynamicsWorld->getCollisionObjectArray()[i];
 		btRigidBody* body = btRigidBody::upcast(colObj);
-		if (body && body->getMotionState())
+		if (body)
+		{
+			if (body->getMotionState())
 			{
 				btDefaultMotionState* myMotionState = (btDefaultMotionState*)body->getMotionState();
 				myMotionState->m_graphicsWorldTrans = myMotionState->m_startWorldTrans;
 				colObj->setWorldTransform( myMotionState->m_graphicsWorldTrans );
 				colObj->setInterpolationWorldTransform( myMotionState->m_startWorldTrans );
 				colObj->activate();
+			}
 			//removed cached contact points
 			m_dynamicsWorld->getBroadphase()->cleanProxyFromPairs(colObj->getBroadphaseHandle());
 

Demos/OpenGL/DemoApplication.h

@@ -50,8 +50,7 @@ class DemoApplication
 	
 	protected:
 
-
+	btClock m_clock;
-	hidden::Clock m_clock;
 
 	///this is the most important class
 	btDynamicsWorld*		m_dynamicsWorld;

Demos/OpenGL/GLDebugDrawer.cpp

@@ -40,6 +40,11 @@ void	GLDebugDrawer::setDebugMode(int debugMode)
 
 }
 
+void	GLDebugDrawer::reportErrorWarning(const char* warningString)
+{
+	printf(warningString);
+}
+
 void	GLDebugDrawer::drawContactPoint(const btVector3& pointOnB,const btVector3& normalOnB,btScalar distance,int lifeTime,const btVector3& color)
 {
 	if (m_debugMode & btIDebugDraw::DBG_DrawContactPoints)

Demos/OpenGL/GLDebugDrawer.h

@@ -19,6 +19,8 @@ public:
 
 	virtual void	drawContactPoint(const btVector3& PointOnB,const btVector3& normalOnB,btScalar distance,int lifeTime,const btVector3& color);
 
+	virtual void	reportErrorWarning(const char* warningString);
+
 	virtual void	setDebugMode(int debugMode);
 
 	virtual int		getDebugMode() const { return m_debugMode;}

Demos/UserCollisionAlgorithm/UserCollisionAlgorithm.cpp

@@ -143,11 +143,9 @@ void UserCollisionAlgorithm::clientMoveAndDisplay()
 {
 	 glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); 
 
-	
 	float dt = m_clock.getTimeMicroseconds() * 0.000001f;
 	m_clock.reset();
 
-
 	m_dynamicsWorld->stepSimulation(dt);
 	
 	renderme();

Demos/VehicleDemo/VehicleDemo.cpp

@@ -325,7 +325,6 @@ void VehicleDemo::clientMoveAndDisplay()
 
 	float dt = m_clock.getTimeMicroseconds() * 0.000001f;
 	m_clock.reset();
-		
 	if (m_dynamicsWorld)
 	{
 		//during idle mode, just run 1 simulation step maximum

Extras/AlternativeCollisionAlgorithms/BoxBoxDetector.cpp

@@ -25,6 +25,7 @@
 #include "BulletCollision/CollisionShapes/btBoxShape.h"
 
 #include <float.h>
+#include <string.h>
 
 BoxBoxDetector::BoxBoxDetector(btBoxShape* box1,btBoxShape* box2)
 : m_box1(box1),

Extras/GIMPACTBullet/btConcaveConcaveCollisionAlgorithm.cpp

@@ -296,7 +296,7 @@ class btConcaveTriangleCallback : public btTriangleCallback
 public:
 	btCollisionObject* m_body;
 	mat4f m_transform;
-	std::vector<CONCAVE_TRIANGLE_TOKEN> m_triangles;
+	btAlignedObjectArray<CONCAVE_TRIANGLE_TOKEN> m_triangles;
 
 	btConcaveTriangleCallback(btCollisionObject* body)
 	{

Extras/GIMPACTBullet/btConcaveConcaveCollisionAlgorithm.h

@@ -26,13 +26,13 @@ class btDispatcher;
 #include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h"
 #include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
 
-
+#include "LinearMath/btAlignedObjectArray.h"
 
 /// btConcaveConcaveCollisionAlgorithm  supports collision between btBvhTriangleMeshShape shapes
 class btConcaveConcaveCollisionAlgorithm : public btCollisionAlgorithm
 {
 protected:
-    std::vector<btPersistentManifold*>	m_mainfoldsPtr;
+    btAlignedObjectArray<btPersistentManifold*>	m_mainfoldsPtr;
 public:
 
 	btConcaveConcaveCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1);

Extras/GIMPACTBullet/btGIMPACTMeshShape.h

@@ -18,7 +18,8 @@ subject to the following restrictions:
 
 #include "BulletCollision/CollisionShapes/btConcaveShape.h"
 #include "BulletCollision/CollisionShapes/btStridingMeshInterface.h"
-#include <vector>
+#include "LinearMath/btAlignedObjectArray.h"
+
 
 //#define GIMPACT_SHAPE_PROXYTYPE (MAX_BROADPHASE_COLLISION_TYPES + 1)
 
@@ -29,7 +30,7 @@ Each mesh part must have a GIMPACT trimesh data (GIM_TRIMESH_DATA).
 typedef unsigned long BT_GIMPACT_TRIMESH_DATA_HANDLE;
 
 
-class BT_GIMPACT_TRIMESH_DATA_HANDLE_ARRAY: public std::vector<BT_GIMPACT_TRIMESH_DATA_HANDLE>
+class BT_GIMPACT_TRIMESH_DATA_HANDLE_ARRAY: public btAlignedObjectArray<BT_GIMPACT_TRIMESH_DATA_HANDLE>
 {
 public:
    
@@ -57,7 +58,7 @@ Each mesh part must have a GIMPACT trimesh (GIM_TRIMESH).
 typedef void * BT_GIMPACT_TRIMESH_HANDLE;
 
 
-class BT_GIMPACT_TRIMESH_HANDLE_ARRAY: public std::vector<BT_GIMPACT_TRIMESH_HANDLE>
+class BT_GIMPACT_TRIMESH_HANDLE_ARRAY: public btAlignedObjectArray<BT_GIMPACT_TRIMESH_HANDLE>
 {
 public:
    

Extras/quickstep/OdeConstraintSolver.cpp

@@ -25,6 +25,7 @@ subject to the following restrictions:
 #include "OdeJoint.h"
 #include "OdeContactJoint.h"
 #include "OdeSolverBody.h"
+#include <new.h>
 
 #include "LinearMath/btIDebugDraw.h"
 

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);
+	int findIndex = m_overlappingPairArray.findLinearSearch(findPair);
-//	assert(it != m_overlappingPairSet.end());
+	if (findIndex < m_overlappingPairArray.size())
-
-	if (it != m_overlappingPairSet.end())
 	{
 		gOverlappingPairs--;
-		btBroadphasePair* pair = (btBroadphasePair*)(&(*it));
+		btBroadphasePair& pair = m_overlappingPairArray[findIndex];
-		cleanOverlappingPair(*pair);	
+		cleanOverlappingPair(pair);
-		m_overlappingPairSet.erase(it);
+		
+		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);
+	int findIndex = m_overlappingPairArray.findLinearSearch(tmpPair);
-	if ((it == m_overlappingPairSet.end()))
-		return 0;
 
+	if (findIndex < m_overlappingPairArray.size())
+	{
 		//assert(it != m_overlappingPairSet.end());
-	 btBroadphasePair* pair = (btBroadphasePair*)(&(*it));
+		 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;
+			m_overlappingPairArray.swap(i,m_overlappingPairArray.size()-1);
-			//why does next line not compile under OS X??
+			m_overlappingPairArray.pop_back();
-#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
-
 			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>
+#include "../../LinearMath/btAlignedObjectArray.h"
-
+typedef btAlignedObjectArray<class btCollisionObject*> btCollisionObjectArray;
-typedef std::vector<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 =
+	///todo: this can be improved a lot, linear search might be slow part!
-		std::find(m_manifoldsPtr.begin(), m_manifoldsPtr.end(), manifold);
+	int findIndex = m_manifoldsPtr.findLinearSearch(manifold);
-	if (!(i == m_manifoldsPtr.end()))
+	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;
+	int i;
-
+	for (i=0;i<m_collisionObjects.size();i++)
-	for (i=m_collisionObjects.begin();
-	!(i==m_collisionObjects.end()); 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);
+		btAssert( m_collisionObjects.findLinearSearch(collisionObject)  == m_collisionObjects.size());
-	assert(i == m_collisionObjects.end());
-
 
 		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);
+	//swapremove
+	m_collisionObjects.remove(collisionObject);
 
-	if (!(i == m_collisionObjects.end()))
-		{
-			std::swap(*i, m_collisionObjects.back());
-			m_collisionObjects.pop_back();
-		}
 }
 
 
@@ -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;
+	int i;
-	
+	for (i=0;i<m_collisionObjects.size();i++)
-	for (iter=m_collisionObjects.begin();
-	!(iter==m_collisionObjects.end()); iter++)
 	{
-		
+		btCollisionObject*	collisionObject= m_collisionObjects[i];
-		btCollisionObject*	collisionObject= (*iter);
 
 		//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 "../../LinearMath/btAlignedObjectArray.h"
-#include <vector>
-
-
 
 ///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();
+		int i;
-		!(i==colWorld->getCollisionObjectArray().end()); i++)
+		for (i=0;i<colWorld->getCollisionObjectArray().size(); i++)
 		{
-			
+			btCollisionObject*	collisionObject= colWorld->getCollisionObjectArray()[i];
-			btCollisionObject*	collisionObject= (*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();
+		int i;
-		!(i==colWorld->getCollisionObjectArray().end()); 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

src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp

@@ -17,6 +17,7 @@ subject to the following restrictions:
 #include "btGeneric6DofConstraint.h"
 #include "BulletDynamics/Dynamics/btRigidBody.h"
 #include "LinearMath/btTransformUtil.h"
+#include "new.h"
 
 static const btScalar kSign[] = { btScalar(1.0), btScalar(-1.0), btScalar(1.0) };
 static const int kAxisA[] = { 1, 0, 0 };

src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp

@@ -17,7 +17,7 @@ subject to the following restrictions:
 #include "btHingeConstraint.h"
 #include "BulletDynamics/Dynamics/btRigidBody.h"
 #include "LinearMath/btTransformUtil.h"
-
+#include "new.h"
 
 btHingeConstraint::btHingeConstraint():
 m_enableAngularMotor(false)

src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp

@@ -16,7 +16,7 @@ subject to the following restrictions:
 
 #include "btPoint2PointConstraint.h"
 #include "BulletDynamics/Dynamics/btRigidBody.h"
-
+#include "new.h"
 
 
 

src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp

@@ -24,7 +24,7 @@ subject to the following restrictions:
 #include "btJacobianEntry.h"
 #include "LinearMath/btMinMax.h"
 #include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"
-
+#include "new.h"
 
 #ifdef USE_PROFILE
 #include "LinearMath/btQuickprof.h"

src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp

@@ -16,7 +16,6 @@ subject to the following restrictions:
 
 #include "btDiscreteDynamicsWorld.h"
 
-
 //collision detection
 #include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
 #include "BulletCollision/BroadphaseCollision/btSimpleBroadphase.h"
@@ -54,8 +53,6 @@ subject to the following restrictions:
 
 
 
-#include <algorithm>
-
 
 
 btDiscreteDynamicsWorld::btDiscreteDynamicsWorld(btDispatcher* dispatcher,btOverlappingPairCache* pairCache,btConstraintSolver* constraintSolver)
@@ -84,7 +81,7 @@ btDiscreteDynamicsWorld::~btDiscreteDynamicsWorld()
 void	btDiscreteDynamicsWorld::saveKinematicState(btScalar timeStep)
 {
 
-	for (unsigned int i=0;i<m_collisionObjects.size();i++)
+	for (int i=0;i<m_collisionObjects.size();i++)
 	{
 		btCollisionObject* colObj = m_collisionObjects[i];
 		btRigidBody* body = btRigidBody::upcast(colObj);
@@ -110,7 +107,7 @@ void	btDiscreteDynamicsWorld::synchronizeMotionStates()
 	
 	{
 		//todo: iterate over awake simulation islands!
-		for (unsigned int i=0;i<m_collisionObjects.size();i++)
+		for ( int i=0;i<m_collisionObjects.size();i++)
 		{
 			btCollisionObject* colObj = m_collisionObjects[i];
 			if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawWireframe)
@@ -155,7 +152,7 @@ void	btDiscreteDynamicsWorld::synchronizeMotionStates()
 
 	if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawWireframe)
 	{
-		for (unsigned int i=0;i<this->m_vehicles.size();i++)
+		for ( int i=0;i<this->m_vehicles.size();i++)
 		{
 			for (int v=0;v<m_vehicles[i]->getNumWheels();v++)
 			{
@@ -294,7 +291,7 @@ void	btDiscreteDynamicsWorld::internalSingleStepSimulation(btScalar timeStep)
 void	btDiscreteDynamicsWorld::setGravity(const btVector3& gravity)
 {
 	m_gravity = gravity;
-	for (unsigned int i=0;i<m_collisionObjects.size();i++)
+	for ( int i=0;i<m_collisionObjects.size();i++)
 	{
 		btCollisionObject* colObj = m_collisionObjects[i];
 		btRigidBody* body = btRigidBody::upcast(colObj);
@@ -333,7 +330,7 @@ void	btDiscreteDynamicsWorld::updateVehicles(btScalar timeStep)
 {
 	BEGIN_PROFILE("updateVehicles");
 
-	for (unsigned int i=0;i<m_vehicles.size();i++)
+	for ( int i=0;i<m_vehicles.size();i++)
 	{
 		btRaycastVehicle* vehicle = m_vehicles[i];
 		vehicle->updateVehicle( timeStep);
@@ -345,7 +342,7 @@ void	btDiscreteDynamicsWorld::updateActivationState(btScalar timeStep)
 {
 	BEGIN_PROFILE("updateActivationState");
 
-	for (unsigned int i=0;i<m_collisionObjects.size();i++)
+	for ( int i=0;i<m_collisionObjects.size();i++)
 	{
 		btCollisionObject* colObj = m_collisionObjects[i];
 		btRigidBody* body = btRigidBody::upcast(colObj);
@@ -380,11 +377,7 @@ void	btDiscreteDynamicsWorld::addConstraint(btTypedConstraint* constraint)
 
 void	btDiscreteDynamicsWorld::removeConstraint(btTypedConstraint* constraint)
 {
-	std::vector<btTypedConstraint*>::iterator cit = std::find(m_constraints.begin(),m_constraints.end(),constraint);
+	m_constraints.remove(constraint);
-	if (!(cit==m_constraints.end()))
-	{
-		m_constraints.erase(cit);
-	}
 }
 
 void	btDiscreteDynamicsWorld::addVehicle(btRaycastVehicle* vehicle)
@@ -394,11 +387,7 @@ void	btDiscreteDynamicsWorld::addVehicle(btRaycastVehicle* vehicle)
 
 void	btDiscreteDynamicsWorld::removeVehicle(btRaycastVehicle* vehicle)
 {
-	std::vector<btRaycastVehicle*>::iterator vit = std::find(m_vehicles.begin(),m_vehicles.end(),vehicle);
+	m_vehicles.remove(vehicle);
-	if (!(vit==m_vehicles.end()))
-	{
-		m_vehicles.erase(vit);
-	}
 }
 
 inline	int	btGetConstraintIslandId(const btTypedConstraint* lhs)
@@ -420,6 +409,7 @@ static bool btSortConstraintOnIslandPredicate(const btTypedConstraint* lhs, cons
 	return lIslandId0 < rIslandId0;
 }
 
+
 void	btDiscreteDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
 {
 	
@@ -482,11 +472,8 @@ void	btDiscreteDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
 	};
 
 
-
-	
-
 	//sorted version of all btTypedConstraint, based on islandId
-	std::vector<btTypedConstraint*>	sortedConstraints;
+	btAlignedObjectArray<btTypedConstraint*>	sortedConstraints;
 	sortedConstraints.resize( m_constraints.size());
 	int i; 
 	for (i=0;i<getNumConstraints();i++)
@@ -494,7 +481,13 @@ void	btDiscreteDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
 		sortedConstraints[i] = m_constraints[i];
 	}
 
-	std::sort(sortedConstraints.begin(),sortedConstraints.end(),btSortConstraintOnIslandPredicate);
+//	assert(0);
+		
+	
+
+	sortedConstraints.heapSort(btAlignedObjectArray<btTypedConstraint*>::less());
+	//std::sort(sortedConstraints.begin(),sortedConstraints.end(),btSortConstraintOnIslandPredicate);
+
 	
 	btTypedConstraint** constraintsPtr = getNumConstraints() ? &sortedConstraints[0] : 0;
 	
@@ -586,7 +579,7 @@ void	btDiscreteDynamicsWorld::updateAabbs()
 	
 	btVector3 colorvec(1,0,0);
 	btTransform predictedTrans;
-	for (unsigned int i=0;i<m_collisionObjects.size();i++)
+	for ( int i=0;i<m_collisionObjects.size();i++)
 	{
 		btCollisionObject* colObj = m_collisionObjects[i];
 		
@@ -597,7 +590,7 @@ void	btDiscreteDynamicsWorld::updateAabbs()
 			{
 				btPoint3 minAabb,maxAabb;
 				colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb,maxAabb);
-				btSimpleBroadphase* bp = (btSimpleBroadphase*)m_broadphasePairCache;
+				btBroadphaseInterface* bp = (btBroadphaseInterface*)m_broadphasePairCache;
 
 				//moving objects should be moderately sized, probably something wrong if not
 				if ( colObj->isStaticObject() || ((maxAabb-minAabb).length2() < btScalar(1e12)))
@@ -610,13 +603,13 @@ void	btDiscreteDynamicsWorld::updateAabbs()
 					body->setActivationState(DISABLE_SIMULATION);
 					
 					static bool reportMe = true;
-					if (reportMe)
+					if (reportMe && m_debugDrawer)
 					{
 						reportMe = false;
-						printf("Overflow in AABB, object removed from simulation \n");
+						m_debugDrawer->reportErrorWarning("Overflow in AABB, object removed from simulation");
-						printf("If you can reproduce this, please email bugs@continuousphysics.com\n");
+						m_debugDrawer->reportErrorWarning("If you can reproduce this, please email bugs@continuousphysics.com\n");
-						printf("Please include above information, your Platform, version of OS.\n");
+						m_debugDrawer->reportErrorWarning("Please include above information, your Platform, version of OS.\n");
-						printf("Thanks.\n");
+						m_debugDrawer->reportErrorWarning("Thanks.\n");
 					}
 
 
@@ -636,7 +629,7 @@ void	btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep)
 {
 	BEGIN_PROFILE("integrateTransforms");
 	btTransform predictedTrans;
-	for (unsigned int i=0;i<m_collisionObjects.size();i++)
+	for ( int i=0;i<m_collisionObjects.size();i++)
 	{
 		btCollisionObject* colObj = m_collisionObjects[i];
 		btRigidBody* body = btRigidBody::upcast(colObj);
@@ -657,7 +650,7 @@ void	btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep)
 void	btDiscreteDynamicsWorld::predictUnconstraintMotion(btScalar timeStep)
 {
 	BEGIN_PROFILE("predictUnconstraintMotion");
-	for (unsigned int i=0;i<m_collisionObjects.size();i++)
+	for ( int i=0;i<m_collisionObjects.size();i++)
 	{
 		btCollisionObject* colObj = m_collisionObjects[i];
 		btRigidBody* body = btRigidBody::upcast(colObj);

src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h

@@ -27,8 +27,8 @@ class btTypedConstraint;
 
 class btRaycastVehicle;
 class btIDebugDraw;
+#include "../../LinearMath/btAlignedObjectArray.h"
 
-#include <vector>
 
 ///btDiscreteDynamicsWorld provides discrete rigid body simulation
 ///those classes replace the obsolete CcdPhysicsEnvironment/CcdPhysicsController
@@ -40,7 +40,7 @@ protected:
 
 	btSimulationIslandManager*	m_islandManager;
 
-	std::vector<btTypedConstraint*> m_constraints;
+	btAlignedObjectArray<btTypedConstraint*> m_constraints;
 
 	btIDebugDraw*	m_debugDrawer;
 
@@ -56,7 +56,7 @@ protected:
 	btContactSolverInfo	m_solverInfo;
 
 
-	std::vector<btRaycastVehicle*>	m_vehicles;
+	btAlignedObjectArray<btRaycastVehicle*>	m_vehicles;
 
 	int	m_profileTimings;
 

src/BulletDynamics/Dynamics/btRigidBody.h

@@ -16,7 +16,6 @@ subject to the following restrictions:
 #ifndef RIGIDBODY_H
 #define RIGIDBODY_H
 
-#include <vector>
 #include "../../LinearMath/btPoint3.h"
 #include "../../LinearMath/btTransform.h"
 #include "../../BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
@@ -28,7 +27,6 @@ class btMotionState;
 
 
 extern btScalar gLinearAirDamping;
-extern bool gUseEpa;
 
 extern btScalar gDeactivationTime;
 extern bool gDisableDeactivation;

src/LinearMath/btAlignedObjectArray.h

@@ -75,6 +75,8 @@ class btAlignedObjectArray
 		}
 
 	
+
+
 	public:
 		
 		btAlignedObjectArray()
@@ -172,6 +174,127 @@ class btAlignedObjectArray
 			}
 		}
 
+
+		class less
+		{
+			public:
+
+				bool operator() ( const T& a, const T& b )
+				{
+					return ( a < b );
+				}
+		};
+	
+
+		///heap sort from http://www.csse.monash.edu.au/~lloyd/tildeAlgDS/Sort/Heap/
+		template <typename L>
+		void downHeap(T *pArr, int k, int n,L CompareFunc)
+		{
+			/*  PRE: a[k+1..N] is a heap */
+			/* POST:  a[k..N]  is a heap */
+			
+			T temp = pArr[k - 1];
+			/* k has child(s) */
+			while (k <= n/2) 
+			{
+				int child = 2*k;
+				
+				if ((child < n) && CompareFunc(pArr[child - 1] , pArr[child]))
+				{
+					child++;
+				}
+				/* pick larger child */
+				if (CompareFunc(temp , pArr[child - 1]))
+				{
+					/* move child up */
+					pArr[k - 1] = pArr[child - 1];
+					k = child;
+				}
+				else
+				{
+					break;
+				}
+			}
+			pArr[k - 1] = temp;
+		} /*downHeap*/
+
+		void	swap(int index0,int index1)
+		{
+			T temp = m_data[index0];
+			m_data[index0] = m_data[index1];
+			m_data[index1] = temp;
+		}
+
+	template <typename L>
+	void heapSort(L CompareFunc)
+	{
+		/* sort a[0..N-1],  N.B. 0 to N-1 */
+		int k;
+		int n = m_size;
+		for (k = n/2; k > 0; k--) 
+		{
+			downHeap(m_data, k, n, CompareFunc);
+		}
+
+		/* a[1..N] is now a heap */
+		while ( n>=1 ) 
+		{
+			swap(0,n-1); /* largest of a[0..n-1] */
+
+
+			n = n - 1;
+			/* restore a[1..i-1] heap */
+			downHeap(m_data, 1, n, CompareFunc);
+		} 
+	}
+
+	///non-recursive binary search, assumes sorted array
+	int	findBinarySearch(const T& key) const
+	{
+		int first = 0;
+		int last = size();
+
+		//assume sorted array
+		while (first <= last) {
+			int mid = (first + last) / 2;  // compute mid point.
+			if (key > m_data[mid]) 
+				first = mid + 1;  // repeat search in top half.
+			else if (key < m_data[mid]) 
+				last = mid - 1; // repeat search in bottom half.
+			else
+				return mid;     // found it. return position /////
+		}
+		return size();    // failed to find key
+	}
+
+
+	int	findLinearSearch(const T& key) const
+	{
+		int index=size();
+		int i;
+
+		for (i=0;i<size();i++)
+		{
+			if (m_data[i] == key)
+			{
+				index = i;
+				break;
+			}
+		}
+		return index;
+	}
+
+	void	remove(const T& key)
+	{
+
+		int findIndex = findLinearSearch(key);
+		if (findIndex<size())
+		{
+			swap( findIndex,size()-1);
+			pop_back();
+		}
+	}
+
 };
 
 #endif //BT_OBJECT_ARRAY__

src/LinearMath/btIDebugDraw.h

@@ -58,11 +58,14 @@ class	btIDebugDraw
 
 	virtual void	drawContactPoint(const btVector3& PointOnB,const btVector3& normalOnB,btScalar distance,int lifeTime,const btVector3& color)=0;
 
+	virtual void	reportErrorWarning(const char* warningString) = 0;
+
 	virtual void	setDebugMode(int debugMode) =0;
 	
 	virtual int		getDebugMode() const = 0;
 
 
+
 };
 
 #endif //IDEBUG_DRAW__H

src/LinearMath/btQuickprof.h

@@ -22,15 +22,6 @@ subject to the following restrictions:
 #ifndef QUICK_PROF_H
 #define QUICK_PROF_H
 
-#define USE_QUICKPROF 1
-
-#ifdef USE_QUICKPROF
-
-
-#include <iostream>
-#include <fstream>
-#include <string>
-#include <map>
 
 #ifdef __PPU__
 #include <sys/sys_time.h>
@@ -51,39 +42,12 @@ typedef uint64_t __int64;
 #endif
 
 #define mymin(a,b) (a > b ? a : b)
-namespace hidden
-{
-	/// A simple data structure representing a single timed block 
-	/// of code.
-	struct ProfileBlock
-	{
-		ProfileBlock()
-		{
-			currentBlockStartMicroseconds = 0;
-			currentCycleTotalMicroseconds = 0;
-			lastCycleTotalMicroseconds = 0;
-			totalMicroseconds = 0;
-		}
 
-		/// The starting time (in us) of the current block update.
+/// basic clock
-		unsigned long int currentBlockStartMicroseconds;
+class btClock
-
-		/// The accumulated time (in us) spent in this block during the 
-		/// current profiling cycle.
-		unsigned long int currentCycleTotalMicroseconds;
-
-		/// The accumulated time (in us) spent in this block during the 
-		/// past profiling cycle.
-		unsigned long int lastCycleTotalMicroseconds;
-
-		/// The total accumulated time (in us) spent in this block.
-		unsigned long int totalMicroseconds;
-	};
-
-	class Clock
 	{
 	public:
-		Clock()
+		btClock()
 		{
 #ifdef USE_WINDOWS_TIMERS
 			QueryPerformanceFrequency(&mClockFrequency);
@@ -91,7 +55,7 @@ namespace hidden
 			reset();
 		}
 
-		~Clock()
+		~btClock()
 		{
 		}
 
@@ -118,7 +82,7 @@ namespace hidden
 		}
 
 		/// Returns the time in ms since the last call to reset or since 
-		/// the Clock was created.
+		/// the btClock was created.
 		unsigned long int getTimeMilliseconds()
 		{
 #ifdef USE_WINDOWS_TIMERS
@@ -248,6 +212,51 @@ namespace hidden
 #endif //__PPU__
 
 	};
+
+
+//#define USE_QUICKPROF 1
+//Don't use quickprof for now, because it contains STL. TODO: replace STL by Bullet container classes.
+
+#ifdef USE_QUICKPROF
+
+
+//#include <iostream>
+#include <fstream>
+#include <string>
+#include <map>
+
+
+
+
+namespace hidden
+{
+	/// A simple data structure representing a single timed block 
+	/// of code.
+	struct ProfileBlock
+	{
+		ProfileBlock()
+		{
+			currentBlockStartMicroseconds = 0;
+			currentCycleTotalMicroseconds = 0;
+			lastCycleTotalMicroseconds = 0;
+			totalMicroseconds = 0;
+		}
+
+		/// The starting time (in us) of the current block update.
+		unsigned long int currentBlockStartMicroseconds;
+
+		/// The accumulated time (in us) spent in this block during the 
+		/// current profiling cycle.
+		unsigned long int currentCycleTotalMicroseconds;
+
+		/// The accumulated time (in us) spent in this block during the 
+		/// past profiling cycle.
+		unsigned long int lastCycleTotalMicroseconds;
+
+		/// The total accumulated time (in us) spent in this block.
+		unsigned long int totalMicroseconds;
+	};
+
 };
 
 /// A static class that manages timing for a set of profiling blocks.
@@ -343,7 +352,7 @@ public:
 	static bool mEnabled;
 
 	/// The clock used to time profile blocks.
-	static hidden::Clock mClock;
+	static btClock mClock;
 
 	/// The starting time (in us) of the current profiling cycle.
 	static unsigned long int mCurrentCycleStartMicroseconds;