diff --git a/Demos/BasicDemo/BasicDemo.cpp b/Demos/BasicDemo/BasicDemo.cpp
index ba2a8628b..7f12eac9c 100644
--- a/Demos/BasicDemo/BasicDemo.cpp
+++ b/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);
diff --git a/Demos/BasicDemo/BasicDemo.h b/Demos/BasicDemo/BasicDemo.h
index acad21c27..21b7cba7f 100644
--- a/Demos/BasicDemo/BasicDemo.h
+++ b/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;
 
diff --git a/Demos/BspDemo/BspConverter.cpp b/Demos/BspDemo/BspConverter.cpp
index 9b4d94137..c85a35e1a 100644
--- a/Demos/BspDemo/BspConverter.cpp
+++ b/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)
 {
diff --git a/Demos/BspDemo/BspConverter.h b/Demos/BspDemo/BspConverter.h
index 743d46da9..775309330 100644
--- a/Demos/BspDemo/BspConverter.h
+++ b/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"
 
diff --git a/Demos/BspDemo/BspDemo.cpp b/Demos/BspDemo/BspDemo.cpp
index fc836a4ef..17f3f8393 100644
--- a/Demos/BspDemo/BspDemo.cpp
+++ b/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();
diff --git a/Demos/BspDemo/BspLoader.cpp b/Demos/BspDemo/BspLoader.cpp
index 65ffc845e..c5f274913 100644
--- a/Demos/BspDemo/BspLoader.cpp
+++ b/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;
diff --git a/Demos/BspDemo/BspLoader.h b/Demos/BspDemo/BspLoader.h
index f2ceed470..cf57e64f9 100644
--- a/Demos/BspDemo/BspLoader.h
+++ b/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()
-			: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");
-			}
-		}
+		BspLoader();
+
 		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
 		{
diff --git a/Demos/CcdPhysicsDemo/CcdPhysicsDemo.cpp b/Demos/CcdPhysicsDemo/CcdPhysicsDemo.cpp
index 6026c3cf4..9d1317e42 100644
--- a/Demos/CcdPhysicsDemo/CcdPhysicsDemo.cpp
+++ b/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);
diff --git a/Demos/ColladaDemo/ColladaConverter.cpp b/Demos/ColladaDemo/ColladaConverter.cpp
index f8603b17b..bffa01af6 100644
--- a/Demos/ColladaDemo/ColladaConverter.cpp
+++ b/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;
 }
 
diff --git a/Demos/ConcaveDemo/ConcavePhysicsDemo.cpp b/Demos/ConcaveDemo/ConcavePhysicsDemo.cpp
index bee786e07..8fced2c13 100644
--- a/Demos/ConcaveDemo/ConcavePhysicsDemo.cpp
+++ b/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();
diff --git a/Demos/ConstraintDemo/ConstraintDemo.cpp b/Demos/ConstraintDemo/ConstraintDemo.cpp
index 33482de67..cc5128023 100644
--- a/Demos/ConstraintDemo/ConstraintDemo.cpp
+++ b/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);
 	
  {
diff --git a/Demos/MovingConcaveDemo/ConcaveDemo.h b/Demos/MovingConcaveDemo/ConcaveDemo.h
index d29ae5d22..f6f736ba7 100644
--- a/Demos/MovingConcaveDemo/ConcaveDemo.h
+++ b/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
diff --git a/Demos/MovingConcaveDemo/ConcavePhysicsDemo.cpp b/Demos/MovingConcaveDemo/ConcavePhysicsDemo.cpp
index f21c25e95..03df126b7 100644
--- a/Demos/MovingConcaveDemo/ConcavePhysicsDemo.cpp
+++ b/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();
diff --git a/Demos/OpenGL/DemoApplication.cpp b/Demos/OpenGL/DemoApplication.cpp
index 15344b41f..de2fcf269 100644
--- a/Demos/OpenGL/DemoApplication.cpp
+++ b/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)
 		{
-			btDefaultMotionState* myMotionState = (btDefaultMotionState*)body->getMotionState();
-			myMotionState->m_graphicsWorldTrans = myMotionState->m_startWorldTrans;
-			colObj->setWorldTransform( myMotionState->m_graphicsWorldTrans );
-			colObj->setInterpolationWorldTransform( myMotionState->m_startWorldTrans );
-			colObj->activate();
+			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());
 
diff --git a/Demos/OpenGL/DemoApplication.h b/Demos/OpenGL/DemoApplication.h
index 4af74af0d..f1388462a 100644
--- a/Demos/OpenGL/DemoApplication.h
+++ b/Demos/OpenGL/DemoApplication.h
@@ -49,9 +49,8 @@ class DemoApplication
 {
 	
 	protected:
-		
 
-	hidden::Clock m_clock;
+	btClock m_clock;
 
 	///this is the most important class
 	btDynamicsWorld*		m_dynamicsWorld;
diff --git a/Demos/OpenGL/GLDebugDrawer.cpp b/Demos/OpenGL/GLDebugDrawer.cpp
index 1994fbaea..3de66b155 100644
--- a/Demos/OpenGL/GLDebugDrawer.cpp
+++ b/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)
diff --git a/Demos/OpenGL/GLDebugDrawer.h b/Demos/OpenGL/GLDebugDrawer.h
index 96451cc1f..015812033 100644
--- a/Demos/OpenGL/GLDebugDrawer.h
+++ b/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;}
diff --git a/Demos/UserCollisionAlgorithm/UserCollisionAlgorithm.cpp b/Demos/UserCollisionAlgorithm/UserCollisionAlgorithm.cpp
index 7f17b3e1c..01da0965d 100644
--- a/Demos/UserCollisionAlgorithm/UserCollisionAlgorithm.cpp
+++ b/Demos/UserCollisionAlgorithm/UserCollisionAlgorithm.cpp
@@ -143,10 +143,8 @@ 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);
 	
diff --git a/Demos/VehicleDemo/VehicleDemo.cpp b/Demos/VehicleDemo/VehicleDemo.cpp
index 4c7782bc8..933cd95fd 100644
--- a/Demos/VehicleDemo/VehicleDemo.cpp
+++ b/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
diff --git a/Extras/AlternativeCollisionAlgorithms/BoxBoxDetector.cpp b/Extras/AlternativeCollisionAlgorithms/BoxBoxDetector.cpp
index 002690375..ae851693b 100644
--- a/Extras/AlternativeCollisionAlgorithms/BoxBoxDetector.cpp
+++ b/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),
diff --git a/Extras/GIMPACTBullet/btConcaveConcaveCollisionAlgorithm.cpp b/Extras/GIMPACTBullet/btConcaveConcaveCollisionAlgorithm.cpp
index 27ff9fb96..285aac89a 100644
--- a/Extras/GIMPACTBullet/btConcaveConcaveCollisionAlgorithm.cpp
+++ b/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)
 	{
diff --git a/Extras/GIMPACTBullet/btConcaveConcaveCollisionAlgorithm.h b/Extras/GIMPACTBullet/btConcaveConcaveCollisionAlgorithm.h
index 8f5c5a934..952a8185a 100644
--- a/Extras/GIMPACTBullet/btConcaveConcaveCollisionAlgorithm.h
+++ b/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);
diff --git a/Extras/GIMPACTBullet/btGIMPACTMeshShape.h b/Extras/GIMPACTBullet/btGIMPACTMeshShape.h
index ce42629a4..d333e369c 100644
--- a/Extras/GIMPACTBullet/btGIMPACTMeshShape.h
+++ b/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:
    
diff --git a/Extras/quickstep/OdeConstraintSolver.cpp b/Extras/quickstep/OdeConstraintSolver.cpp
index 4d3dea7c2..43d433369 100644
--- a/Extras/quickstep/OdeConstraintSolver.cpp
+++ b/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"
 
diff --git a/src/BulletCollision/BroadphaseCollision/btAxisSweep3.cpp b/src/BulletCollision/BroadphaseCollision/btAxisSweep3.cpp
index 63e6069d7..e415df922 100644
--- a/src/BulletCollision/BroadphaseCollision/btAxisSweep3.cpp
+++ b/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
diff --git a/src/BulletCollision/BroadphaseCollision/btAxisSweep3.h b/src/BulletCollision/BroadphaseCollision/btAxisSweep3.h
index bc6e8b0d4..18fcf4cda 100644
--- a/src/BulletCollision/BroadphaseCollision/btAxisSweep3.h
+++ b/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);
 
 };
 
diff --git a/src/BulletCollision/BroadphaseCollision/btBroadphaseProxy.h b/src/BulletCollision/BroadphaseCollision/btBroadphaseProxy.h
index 8b080f3f1..e4c24f019 100644
--- a/src/BulletCollision/BroadphaseCollision/btBroadphaseProxy.h
+++ b/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
diff --git a/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp b/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp
index e52bb914f..6653b8b30 100644
--- a/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp
+++ b/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++;
 		}
 	}
 }
diff --git a/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h b/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h
index cd8aa095b..534f31881 100644
--- a/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h
+++ b/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);
 
diff --git a/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp b/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp
index 6281e93ee..8bb127c9d 100644
--- a/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp
+++ b/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()
diff --git a/src/BulletCollision/CollisionDispatch/btCollisionCreateFunc.h b/src/BulletCollision/CollisionDispatch/btCollisionCreateFunc.h
index b4a46ca61..07e6eaf55 100644
--- a/src/BulletCollision/CollisionDispatch/btCollisionCreateFunc.h
+++ b/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;
 
diff --git a/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp b/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp
index 309357939..c4824d7c5 100644
--- a/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp
+++ b/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;
-
 	}
 	
-	
 }
 
 	
diff --git a/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.h b/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.h
index 6c5cf8429..ca5aba8f0 100644
--- a/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.h
+++ b/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;
 	
diff --git a/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp b/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp
index d63644e43..57becfd85 100644
--- a/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp
+++ b/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;
diff --git a/src/BulletCollision/CollisionDispatch/btCollisionWorld.h b/src/BulletCollision/CollisionDispatch/btCollisionWorld.h
index d8e28bb35..eb0bd2b22 100644
--- a/src/BulletCollision/CollisionDispatch/btCollisionWorld.h
+++ b/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;
 
diff --git a/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h b/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h
index ac1914a46..7091b233b 100644
--- a/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h
+++ b/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:
diff --git a/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp b/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp
index 70631c627..f1a7eb0dc 100644
--- a/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp
+++ b/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)
 
diff --git a/src/BulletCollision/CollisionDispatch/btUnionFind.cpp b/src/BulletCollision/CollisionDispatch/btUnionFind.cpp
index 046f348a1..622543357 100644
--- a/src/BulletCollision/CollisionDispatch/btUnionFind.cpp
+++ b/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());
 
 }
 
diff --git a/src/BulletCollision/CollisionDispatch/btUnionFind.h b/src/BulletCollision/CollisionDispatch/btUnionFind.h
index b1baca9ff..b6c30f7a7 100644
--- a/src/BulletCollision/CollisionDispatch/btUnionFind.h
+++ b/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:
 	  
diff --git a/src/BulletCollision/CollisionShapes/btCompoundShape.h b/src/BulletCollision/CollisionShapes/btCompoundShape.h
index 15c93e53b..2d384bd21 100644
--- a/src/BulletCollision/CollisionShapes/btCompoundShape.h
+++ b/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"
 
diff --git a/src/BulletCollision/CollisionShapes/btConvexShape.h b/src/BulletCollision/CollisionShapes/btConvexShape.h
index bf640aa37..5f7cfc7a4 100644
--- a/src/BulletCollision/CollisionShapes/btConvexShape.h
+++ b/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!
diff --git a/src/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.h b/src/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.h
index 372f3d4b7..a0db2797b 100644
--- a/src/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.h
+++ b/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.
diff --git a/src/BulletCollision/CollisionShapes/btEmptyShape.h b/src/BulletCollision/CollisionShapes/btEmptyShape.h
index 572692e64..8ec534449 100644
--- a/src/BulletCollision/CollisionShapes/btEmptyShape.h
+++ b/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"
 
 
diff --git a/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h b/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h
index a727226c6..eea34f2ba 100644
--- a/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h
+++ b/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h
@@ -137,4 +137,6 @@ public:
 
 
 
+
+
 #endif //PERSISTENT_MANIFOLD_H
diff --git a/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp b/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp
index e1c7fb168..3e87dbf95 100644
--- a/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp
+++ b/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 };
diff --git a/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp b/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp
index ccd2d9ee8..ea2dbde33 100644
--- a/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp
+++ b/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)
diff --git a/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp b/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp
index e2c4c54f1..40531861e 100644
--- a/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp
+++ b/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"
 
 
 
diff --git a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp
index cab15203a..bee3c24de 100644
--- a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp
+++ b/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"
diff --git a/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp b/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp
index 404cf243f..792912d96 100644
--- a/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp
+++ b/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);
-	if (!(cit==m_constraints.end()))
-	{
-		m_constraints.erase(cit);
-	}
+	m_constraints.remove(constraint);
 }
 
 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);
-	if (!(vit==m_vehicles.end()))
-	{
-		m_vehicles.erase(vit);
-	}
+	m_vehicles.remove(vehicle);
 }
 
 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)
 {
 	
@@ -481,20 +471,23 @@ 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++)
 	{
 		sortedConstraints[i] = m_constraints[i];
 	}
+
+//	assert(0);
+		
 	
-	std::sort(sortedConstraints.begin(),sortedConstraints.end(),btSortConstraintOnIslandPredicate);
+
+	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");
-						printf("If you can reproduce this, please email bugs@continuousphysics.com\n");
-						printf("Please include above information, your Platform, version of OS.\n");
-						printf("Thanks.\n");
+						m_debugDrawer->reportErrorWarning("Overflow in AABB, object removed from simulation");
+						m_debugDrawer->reportErrorWarning("If you can reproduce this, please email bugs@continuousphysics.com\n");
+						m_debugDrawer->reportErrorWarning("Please include above information, your Platform, version of OS.\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);
diff --git a/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h b/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h
index 06ad920ab..2c05c5f86 100644
--- a/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h
+++ b/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;
 
diff --git a/src/BulletDynamics/Dynamics/btRigidBody.h b/src/BulletDynamics/Dynamics/btRigidBody.h
index 4775a8089..9da545adb 100644
--- a/src/BulletDynamics/Dynamics/btRigidBody.h
+++ b/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;
diff --git a/src/LinearMath/btAlignedObjectArray.h b/src/LinearMath/btAlignedObjectArray.h
index 7ecf027ac..e2f630623 100644
--- a/src/LinearMath/btAlignedObjectArray.h
+++ b/src/LinearMath/btAlignedObjectArray.h
@@ -74,6 +74,8 @@ class btAlignedObjectArray
 			}
 		}
 
+	
+
 
 	public:
 		
@@ -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__
diff --git a/src/LinearMath/btIDebugDraw.h b/src/LinearMath/btIDebugDraw.h
index 44b3af94a..9c675483e 100644
--- a/src/LinearMath/btIDebugDraw.h
+++ b/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
diff --git a/src/LinearMath/btQuickprof.h b/src/LinearMath/btQuickprof.h
index ec560c8e3..3f9076b93 100644
--- a/src/LinearMath/btQuickprof.h
+++ b/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.
-		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;
-	};
-
-	class Clock
+/// basic clock
+class btClock
 	{
 	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;