more work on hashed pairmanager. growing doesn't work yet, so need to allocate enough room for the overlapping pairs in advance.

boxbox reports contact point in B, rather then average point box, cylinder use halfextents corrected for scaling and margin. made the margin in this halfextents explicit in the 'getHalfExtentsWithMargin' and 'getHalfExtentsWithoutMargin' integrated changed for ODE quickstep solver replaced inline with SIMD_FORCE_INLINE some minor optimizations in the btSequentialImpulseConstraintSolver added cone drawing (for X,Y,Z cones)
2007-10-12 02:52:28 +00:00
parent 1baa61bc8d
commit eff4fe8ec8
39 changed files with 1882 additions and 1336 deletions
--- a/Demos/CcdPhysicsDemo/CcdPhysicsDemo.cpp
+++ b/Demos/CcdPhysicsDemo/CcdPhysicsDemo.cpp
@@ -20,8 +20,8 @@ subject to the following restrictions:
 //Note: some of those settings need 'DO_WALL' demo
 //#define USE_KINEMATIC_GROUND 1
 //#define PRINT_CONTACT_STATISTICS 1
-//#define REGISTER_CUSTOM_COLLISION_ALGORITHM 1
-//#define REGISTER_BOX_BOX 1 //needs to be used in combination with REGISTER_CUSTOM_COLLISION_ALGORITHM
+#define REGISTER_CUSTOM_COLLISION_ALGORITHM 1
+#define REGISTER_BOX_BOX 1 //needs to be used in combination with REGISTER_CUSTOM_COLLISION_ALGORITHM
 //#define USER_DEFINED_FRICTION_MODEL 1
 //#define USE_CUSTOM_NEAR_CALLBACK 1
 //#define CENTER_OF_MASS_SHIFT 1
@@ -121,13 +121,13 @@ btCollisionShape* shapePtr[numShapes] =
 #ifdef DO_BENCHMARK_PYRAMIDS
 		new btBoxShape (btVector3(CUBE_HALF_EXTENTS,CUBE_HALF_EXTENTS,CUBE_HALF_EXTENTS)),
 #else
-		new btCylinderShape (btVector3(CUBE_HALF_EXTENTS-gCollisionMargin,CUBE_HALF_EXTENTS-gCollisionMargin,CUBE_HALF_EXTENTS-gCollisionMargin)),
+		//new btCylinderShape (btVector3(CUBE_HALF_EXTENTS,CUBE_HALF_EXTENTS,CUBE_HALF_EXTENTS)),
 #endif

 		//new btSphereShape (CUBE_HALF_EXTENTS),
-		//new btCapsuleShape(0.5*CUBE_HALF_EXTENTS-gCollisionMargin,CUBE_HALF_EXTENTS-gCollisionMargin),
+		//new btCapsuleShape(0.5*CUBE_HALF_EXTENTS,CUBE_HALF_EXTENTS),
 		//new btCylinderShape (btVector3(1-gCollisionMargin,CUBE_HALF_EXTENTS-gCollisionMargin,1-gCollisionMargin)),
-		//new btConeShape(CUBE_HALF_EXTENTS-gCollisionMargin,2.f*CUBE_HALF_EXTENTS-gCollisionMargin),
+		new btConeShapeX(CUBE_HALF_EXTENTS,2.f*CUBE_HALF_EXTENTS),
 		
 		new btSphereShape (CUBE_HALF_EXTENTS),
 		
@@ -446,7 +446,9 @@ int maxNumOutstandingTasks = 4;//number of maximum outstanding tasks
 	solver->setSolverMode(btSequentialImpulseConstraintSolver::SOLVER_RANDMIZE_ORDER);
 #endif //USER_DEFINED_FRICTION_MODEL

-		m_dynamicsWorld = new btDiscreteDynamicsWorld(dispatcher,broadphase,solver);
+		btDiscreteDynamicsWorld* world = new btDiscreteDynamicsWorld(dispatcher,broadphase,solver);
+		m_dynamicsWorld = world;
+//		world->getSolverInfo().m_numIterations = 4;
 		m_dynamicsWorld->getDispatchInfo().m_enableSPU = true;
 		m_dynamicsWorld->setGravity(btVector3(0,-10,0));

--- a/Demos/ConcaveDemo/ConcavePhysicsDemo.cpp
+++ b/Demos/ConcaveDemo/ConcavePhysicsDemo.cpp
@@ -286,8 +286,8 @@ btDefaultCollisionConfiguration* collisionConfiguration = new btDefaultCollision
 	{
 		for (int i=0;i<10;i++)
 		{
-			//btCollisionShape* colShape = new btBoxShape(btVector3(1,1,1));
-			btCollisionShape* colShape = new btCapsuleShape(0.5,2.0);//boxShape = new btSphereShape(1.f);
+			btCollisionShape* colShape = new btBoxShape(btVector3(1,1,1));
+			//btCollisionShape* colShape = new btCapsuleShape(0.5,2.0);//boxShape = new btSphereShape(1.f);
 			startTransform.setOrigin(btVector3(2*i,10,1));
 			localCreateRigidBody(1, startTransform,colShape);
 		}
--- a/Demos/OpenGL/GL_ShapeDrawer.cpp
+++ b/Demos/OpenGL/GL_ShapeDrawer.cpp
@@ -363,7 +363,7 @@ void GL_ShapeDrawer::drawOpenGL(btScalar* m, const btCollisionShape* shape, cons
 			case BOX_SHAPE_PROXYTYPE:
 				{
 					const btBoxShape* boxShape = static_cast<const btBoxShape*>(shape);
-					btVector3 halfExtent = boxShape->getHalfExtents();
+					btVector3 halfExtent = boxShape->getHalfExtentsWithMargin();
 					glScaled(2*halfExtent[0], 2*halfExtent[1], 2*halfExtent[2]);
 					glutSolidCube(1.0);
 					useWireframeFallback = false;
@@ -412,9 +412,24 @@ void GL_ShapeDrawer::drawOpenGL(btScalar* m, const btCollisionShape* shape, cons
 			case CONE_SHAPE_PROXYTYPE:
 				{
 					const btConeShape* coneShape = static_cast<const btConeShape*>(shape);
+					int upIndex = coneShape->getConeUpIndex();
 					float radius = coneShape->getRadius();//+coneShape->getMargin();
 					float height = coneShape->getHeight();//+coneShape->getMargin();
-					//glRotatef(-90.0, 1.0, 0.0, 0.0);
+					switch (upIndex)
+					{
+					case 0:
+						glRotatef(90.0, 0.0, 1.0, 0.0);
+						break;
+					case 1:
+						glRotatef(-90.0, 1.0, 0.0, 0.0);
+						break;
+					case 2:
+						break;
+					default:
+						{
+						}
+					};
+					
 					glTranslatef(0.0, 0.0, -0.5*height);
 					glutSolidCone(radius,height,10,10);
 					useWireframeFallback = false;
@@ -435,7 +450,7 @@ void GL_ShapeDrawer::drawOpenGL(btScalar* m, const btCollisionShape* shape, cons
 					
 					
 					float radius = cylinder->getRadius();
-					float halfHeight = cylinder->getHalfExtents()[upAxis];
+					float halfHeight = cylinder->getHalfExtentsWithMargin()[upAxis];

 					drawCylinder(radius,halfHeight,upAxis);

--- a/Extras/AlternativeCollisionAlgorithms/BoxBoxDetector.cpp
+++ b/Extras/AlternativeCollisionAlgorithms/BoxBoxDetector.cpp
@@ -412,10 +412,13 @@ int dBoxBox2 (const btVector3& p1, const dMatrix3 R1,
 		//contact[0].depth = *depth;
 		btVector3 pointInWorld;

+#ifdef USE_CENTER_POINT
 	    for (i=0; i<3; i++) 
 			pointInWorld[i] = (pa[i]+pb[i])*btScalar(0.5);
 		output.addContactPoint(-normal,pointInWorld,-*depth);
-
+#else
+		output.addContactPoint(-normal,pb,-*depth);
+#endif //
 		*return_code = code;
 	}
    return 1;
@@ -668,10 +671,10 @@ void	BoxBoxDetector::getClosestPoints(const ClosestPointInput& input,Result& out

 	dBoxBox2 (transformA.getOrigin(), 
 	R1,
-	2.f*m_box1->getHalfExtents(),
+	2.f*m_box1->getHalfExtentsWithMargin(),
 	transformB.getOrigin(),
 	R2, 
-	2.f*m_box2->getHalfExtents(),
+	2.f*m_box2->getHalfExtentsWithMargin(),
 	normal, &depth, &return_code,
 	maxc, contact, skip,
 	output
--- a/Extras/quickstep/OdeConstraintSolver.cpp
+++ b/Extras/quickstep/OdeConstraintSolver.cpp
@@ -58,15 +58,6 @@ class BU_Joint;
 //see below
 //to bridge with ODE quickstep, we make a temp copy of the rigidbodies in each simultion island

-// Remotion 10.10.07: we do not need thi any more!
-//#define ODE_MAX_SOLVER_BODIES 16384
-//#define ODE_MAX_SOLVER_JOINTS 65535
-//static OdeSolverBody	gSolverBodyArray[ODE_MAX_SOLVER_BODIES];
-//static ContactJoint		gJointArray[ODE_MAX_SOLVER_JOINTS];
-//static OdeTypedJoint    gTypedJointArray[ODE_MAX_SOLVER_JOINTS];
-
-
-

 OdeConstraintSolver::OdeConstraintSolver():
        m_cfm(0.f),//1e-5f),
@@ -84,7 +75,6 @@ btScalar OdeConstraintSolver::solveGroup(btCollisionObject** bodies,int numBulle
    m_CurJoint = 0;
    m_CurTypedJoint = 0;

-	// Remotion 10.10.07: to be sure just find max_contacts 
 	int max_contacts = 0; /// should be 4 //Remotion
 	for (int j=0;j<numManifolds;j++){
 		btPersistentManifold* manifold = manifoldPtr[j];
@@ -138,7 +128,7 @@ btScalar OdeConstraintSolver::solveGroup(btCollisionObject** bodies,int numBulle

    END_PROFILE("prepareConstraints");
    BEGIN_PROFILE("solveConstraints");
-    SolveInternal1(m_cfm,m_erp,m_odeBodies,numBodies,m_joints,numJoints,infoGlobal);
+    m_SorLcpSolver.SolveInternal1(m_cfm,m_erp,m_odeBodies,numBodies,m_joints,numJoints,infoGlobal,stackAlloc); ///do

    //write back resulting velocities
    for (int i=0;i<numBodies;i++)
@@ -278,11 +268,8 @@ void OdeConstraintSolver::ConvertConstraint(btPersistentManifold* manifold,
 {


-///refresh contact points is not needed anymore, it has been moved into the processCollision detection part.
-#ifdef	FORCE_REFESH_CONTACT_MANIFOLDS
    manifold->refreshContactPoints(((btRigidBody*)manifold->getBody0())->getCenterOfMassTransform(),
                                   ((btRigidBody*)manifold->getBody1())->getCenterOfMassTransform());
-#endif //FORCE_REFESH_CONTACT_MANIFOLDS

    int bodyId0 = _bodyId0,bodyId1 = _bodyId1;

--- a/Extras/quickstep/OdeConstraintSolver.h
+++ b/Extras/quickstep/OdeConstraintSolver.h
@@ -22,6 +22,7 @@ subject to the following restrictions:
 #include "OdeContactJoint.h"
 #include "OdeTypedJoint.h"
 #include "OdeSolverBody.h"
+#include "SorLcp.h"

 class btRigidBody;
 struct	OdeSolverBody;
@@ -39,6 +40,8 @@ private:
 	float	m_cfm;
 	float	m_erp;

+	SorLcpSolver	m_SorLcpSolver;
+
 	btAlignedObjectArray<OdeSolverBody*> m_odeBodies;
 	btAlignedObjectArray<BU_Joint*>		 m_joints;

@@ -47,6 +50,7 @@ private:
 	btAlignedObjectArray<OdeTypedJoint>  m_TypedJointArray;


+private:
 	int  ConvertBody(btRigidBody* body,btAlignedObjectArray< OdeSolverBody*> &bodies,int& numBodies);
 	void ConvertConstraint(btPersistentManifold* manifold,
 							btAlignedObjectArray<BU_Joint*> &joints,int& numJoints,
@@ -80,8 +84,19 @@ public:
 		m_erp = erp;
 	}

+	///clear internal cached data and reset random seed
 	void reset()
 	{
+		m_SorLcpSolver.dRand2_seed = 0;
+	}
+
+	void	setRandSeed(unsigned long seed)
+	{
+		m_SorLcpSolver.dRand2_seed = seed;
+	}
+	unsigned long	getRandSeed() const
+	{
+		return m_SorLcpSolver.dRand2_seed;
 	}
 };

--- a/Extras/quickstep/OdeMacros.h
+++ b/Extras/quickstep/OdeMacros.h
@@ -93,6 +93,7 @@ do { \
  (A)[8] op dDOT1((B+8),(C)); \
  (A)[9] op dDOT1((B+8),(C+4)); \
  (A)[10] op dDOT1((B+8),(C+8));
+
 #define dMULTIPLYOP0_333(A,op,B,C) \
  (A)[0] op dDOT14((B),(C)); \
  (A)[1] op dDOT14((B),(C+1)); \
@@ -119,52 +120,30 @@ do { \
 #define dALLOCA16(n) \
  ((char*)dEFFICIENT_SIZE(((size_t)(alloca((n)+(EFFICIENT_ALIGNMENT-1))))))

-
-
-/////////////////////////////////////////////////////////////////////
-/////////////////////////////////////////////////////////////////////
-
-#ifdef DEBUG
-#define ANSI_FTOL 1
-
-extern "C" {
-    __declspec(naked) void _ftol2() {
-        __asm    {
-#if ANSI_FTOL
-            fnstcw   WORD PTR [esp-2]
-            mov      ax, WORD PTR [esp-2]
-
-            OR AX,	 0C00h
-
-            mov      WORD PTR [esp-4], ax
-            fldcw    WORD PTR [esp-4]
-            fistp    QWORD PTR [esp-12]
-            fldcw    WORD PTR [esp-2]
-            mov      eax, DWORD PTR [esp-12]
-            mov      edx, DWORD PTR [esp-8]
-#else
-            fistp    DWORD PTR [esp-12]
-            mov		 eax, DWORD PTR [esp-12]
-            mov		 ecx, DWORD PTR [esp-8]
-#endif
-            ret
-        }
-    }
-}
-#endif //DEBUG
-
-
-
-
-
-
-#define ALLOCA dALLOCA16
+//#define ALLOCA dALLOCA16

 typedef const btScalar *dRealPtr;
 typedef btScalar *dRealMutablePtr;
-#define dRealArray(name,n) btScalar name[n];
-#define dRealAllocaArray(name,n) btScalar *name = (btScalar*) ALLOCA ((n)*sizeof(btScalar));
+//#define dRealArray(name,n) btScalar name[n];
+//#define dRealAllocaArray(name,n) btScalar *name = (btScalar*) ALLOCA ((n)*sizeof(btScalar));

+///////////////////////////////////////////////////////////////////////////////
+
+ //Remotion: 10.10.2007
+#define ALLOCA(size) stackAlloc->allocate( dEFFICIENT_SIZE(size) );
+
+//#define dRealAllocaArray(name,size) btScalar *name = (btScalar*) stackAlloc->allocate(dEFFICIENT_SIZE(size)*sizeof(btScalar));
+#define dRealAllocaArray(name,size) btScalar *name = NULL; \
+	int memNeeded_##name = dEFFICIENT_SIZE(size)*sizeof(btScalar); \
+	if (memNeeded_##name < stackAlloc->getAvailableMemory()) name = (btScalar*) stackAlloc->allocate(memNeeded_##name); \
+	else{ btAssert(memNeeded_##name < stackAlloc->getAvailableMemory()); name = (btScalar*) alloca(memNeeded_##name); } 
+
+
+
+
+
+///////////////////////////////////////////////////////////////////////////////
+#if 0
 inline void dSetZero1 (btScalar *a, int n)
 {
  dAASSERT (a && n >= 0);
@@ -182,6 +161,76 @@ inline void dSetValue1 (btScalar *a, int n, btScalar value)
    n--;
  }
 }
+#else
+
+/// This macros are for MSVC and XCode compilers. Remotion.
+
+#if  _MSC_VER   //Visual Studio Win32, Win64
+	#include <xmmintrin.h> // SSE
+	#include <emmintrin.h> // SSE2	
+	#include <intrin.h>	   // SSE3
+	
+	#define __USE_SSE__
+
+/*
+	#ifdef _WIN64
+		typedef unsigned __int64 size_t;
+	#else
+		typedef unsigned int size_t;
+	#endif
+*/
+#elif  __GNUC__  // XCode GCC
+	#if defined(__ppc__) || defined(__ppc64__) // Mac PPC
+		///PPC or PPC64 Mac no SSE support
+	#elif defined(__i386__)	// Intel Mac with SSE support 
+		#include <xmmintrin.h> // SSE
+		#include <emmintrin.h> // SSE2	
+		#include <pmmintrin.h> // SSE3
+		#define __USE_SSE__
+	#endif
+	#include <string.h>
+#endif
+
+
+//Remotion: 10.10.2007
+//------------------------------------------------------------------------------
+#define IS_ALIGNED_16(x)	((size_t(x)&15)==0)
+//------------------------------------------------------------------------------
+inline void dSetZero1 (btScalar *dest, int size)
+{
+	dAASSERT (dest && size >= 0);
+	memset(dest, 0, size * sizeof(btScalar));
+}
+//------------------------------------------------------------------------------
+inline void dSetValue1 (btScalar *dest, int size, btScalar val)
+{
+	dAASSERT (dest && size >= 0);
+	int n_mod4 = size & 3;		
+	int n4 = size - n_mod4;
+#ifdef __USE_SSE__
+	if(IS_ALIGNED_16(dest)){
+		__m128 xmm0 = _mm_set_ps1(val);
+		for (int i=0; i<n4; i+=4)
+		{
+			_mm_store_ps(&dest[i],xmm0);
+		}
+	}else
+#endif
+	{
+		for (int i=0; i<n4; i+=4) // Unrolled Loop
+		{
+			dest[i  ] = val;
+			dest[i+1] = val;
+			dest[i+2] = val;
+			dest[i+3] = val;
+		}
+	}
+	for (int  i=n4; i<size; i++){
+		dest[i] = val;
+	}
+}
+#endif
+/////////////////////////////////////////////////////////////////////


 #endif //USE_SOR_SOLVER
--- a/Extras/quickstep/SorLcp.cpp
+++ b/Extras/quickstep/SorLcp.cpp
@@ -19,7 +19,6 @@
 * LICENSE.TXT and LICENSE-BSD.TXT for more details.                     *
 *                                                                       *
 *************************************************************************/
-
 #include "SorLcp.h"
 #include "OdeSolverBody.h"

@@ -49,10 +48,8 @@

 #include "OdeJoint.h"
 #include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h"
-
 ////////////////////////////////////////////////////////////////////
 //math stuff
-
 #include "OdeMacros.h"

 //***************************************************************************
@@ -72,14 +69,10 @@

 #define RANDOMLY_REORDER_CONSTRAINTS 1

-
-
 //***************************************************************************
 // various common computations involving the matrix J
-
 // compute iMJ = inv(M)*J'
-
-static void compute_invM_JT (int m, dRealMutablePtr J, dRealMutablePtr iMJ, int *jb,
+inline void compute_invM_JT (int m, dRealMutablePtr J, dRealMutablePtr iMJ, int *jb,
 	//OdeSolverBody* const *body,
 	 const btAlignedObjectArray<OdeSolverBody*> &body,
 	dRealPtr invI)
@@ -183,8 +176,7 @@ static void multiply_invM_JT (int m, int nb, dRealMutablePtr iMJ, int *jb,


 // compute out = J*in.
-
-static void multiply_J (int m, dRealMutablePtr J, int *jb,
+inline void multiply_J (int m, dRealMutablePtr J, int *jb,
 	dRealMutablePtr in, dRealMutablePtr out)
 {
 	int i,j;
@@ -218,35 +210,25 @@ static void multiply_J (int m, dRealMutablePtr J, int *jb,
 //
 // b, lo and hi are modified on exit

-
-struct IndexError {
+//------------------------------------------------------------------------------
+ATTRIBUTE_ALIGNED16(struct) IndexError {
 	btScalar error;		// error to sort on
 	int findex;
 	int index;		// row index
 };

-static unsigned long seed2 = 0;
-
-unsigned long dRand2()
-{
-  seed2 = (1664525L*seed2 + 1013904223L) & 0xffffffff;
-  return seed2;
-}
-
-int dRandInt2 (int n)
-{
-  float a = float(n) / 4294967296.0f;
-  return (int) (float(dRand2()) * a);
-}
-
-
-static void SOR_LCP (int m, int nb, dRealMutablePtr J, int *jb, 
-	//OdeSolverBody * const *body,
+//------------------------------------------------------------------------------
+void SorLcpSolver::SOR_LCP(int m, int nb, dRealMutablePtr J, int *jb, 
 	const btAlignedObjectArray<OdeSolverBody*> &body,
 	dRealPtr invI, dRealMutablePtr lambda, dRealMutablePtr invMforce, dRealMutablePtr rhs,
 	dRealMutablePtr lo, dRealMutablePtr hi, dRealPtr cfm, int *findex,
-	int numiter,float overRelax)
+	int numiter,float overRelax,
+	btStackAlloc* stackAlloc
+	)
 {
+	//btBlock* saBlock = stackAlloc->beginBlock();//Remo: 10.10.2007
+	AutoBlockSa asaBlock(stackAlloc);
+
 	const int num_iterations = numiter;
 	const float sor_w = overRelax;		// SOR over-relaxation parameter

@@ -310,7 +292,9 @@ static void SOR_LCP (int m, int nb, dRealMutablePtr J, int *jb,
 		Ad[i] *= cfm[i];

 	// order to solve constraint rows in
-	IndexError *order = (IndexError*) alloca (m*sizeof(IndexError));
+	//IndexError *order = (IndexError*) alloca (m*sizeof(IndexError));
+	IndexError *order = (IndexError*) ALLOCA (m*sizeof(IndexError));
+	

 #ifndef REORDER_CONSTRAINTS
 	// make sure constraints with findex < 0 come first.
@@ -455,24 +439,20 @@ static void SOR_LCP (int m, int nb, dRealMutablePtr J, int *jb,
 			}
 		}
 	}
+	//stackAlloc->endBlock(saBlock);//Remo: 10.10.2007
 }

-
-/*
-void SolveInternal1 (float global_cfm,
-					 float global_erp,
-					 OdeSolverBody* const *body, int nb,
-					BU_Joint **joint,
-					int nj,
-					const btContactSolverInfo& solverInfo)
-*/
-void SolveInternal1 (
+//------------------------------------------------------------------------------
+void SorLcpSolver::SolveInternal1 (
 			float global_cfm,
 			float global_erp,
 			const btAlignedObjectArray<OdeSolverBody*> &body, int nb,
 			btAlignedObjectArray<BU_Joint*> &joint, 
-			int nj, const btContactSolverInfo& solverInfo)
+			int nj, const btContactSolverInfo& solverInfo,
+			btStackAlloc* stackAlloc)
 {
+	//btBlock* saBlock = stackAlloc->beginBlock();//Remo: 10.10.2007
+	AutoBlockSa asaBlock(stackAlloc);

 	int numIter = solverInfo.m_numIterations;
 	float sOr = solverInfo.m_sor;
@@ -529,7 +509,8 @@ void SolveInternal1 (
 	// joints with m=0 are inactive and are removed from the joints array
 	// entirely, so that the code that follows does not consider them.
 	//@@@ do we really need to save all the info1's
-	BU_Joint::Info1 *info = (BU_Joint::Info1*) alloca (nj*sizeof(BU_Joint::Info1));
+	BU_Joint::Info1 *info = (BU_Joint::Info1*) ALLOCA (nj*sizeof(BU_Joint::Info1));
+	
 	for (i=0, j=0; j<nj; j++) {	// i=dest, j=src
 		joint[j]->GetInfo1 (info+i);
 		dIASSERT (info[i].m >= 0 && info[i].m <= 6 && info[i].nub >= 0 && info[i].nub <= info[i].m);
@@ -542,7 +523,7 @@ void SolveInternal1 (

 	// create the row offset array
 	int m = 0;
-	int *ofs = (int*) alloca (nj*sizeof(int));
+	int *ofs = (int*) ALLOCA (nj*sizeof(int));
 	for (i=0; i<nj; i++) {
 		ofs[i] = m;
 		m += info[i].m;
@@ -550,7 +531,7 @@ void SolveInternal1 (

 	// if there are constraints, compute the constraint force
 	dRealAllocaArray (J,m*12);
-	int *jb = (int*) alloca (m*2*sizeof(int));
+	int *jb = (int*) ALLOCA (m*2*sizeof(int));
 	if (m > 0) {
 		// create a constraint equation right hand side vector `c', a constraint
 		// force mixing vector `cfm', and LCP low and high bound vectors, and an
@@ -559,7 +540,9 @@ void SolveInternal1 (
 		dRealAllocaArray (cfm,m);
 		dRealAllocaArray (lo,m);
 		dRealAllocaArray (hi,m);
-		int *findex = (int*) alloca (m*sizeof(int));
+
+		int *findex = (int*) ALLOCA (m*sizeof(int));
+
 		dSetZero1 (c,m);
 		dSetValue1 (cfm,m,global_cfm);
 		dSetValue1 (lo,m,-dInfinity);
@@ -599,9 +582,6 @@ void SolveInternal1 (
 			if (Jinfo.c[0] > solverInfo.m_maxErrorReduction)
 				Jinfo.c[0] = solverInfo.m_maxErrorReduction;

-
-
-
 			// adjust returned findex values for global index numbering
 			for (j=0; j<info[i].m; j++) {
 				if (findex[ofs[i] + j] >= 0)
@@ -657,7 +637,8 @@ void SolveInternal1 (
 		// solve the LCP problem and get lambda and invM*constraint_force
 		dRealAllocaArray (cforce,nb*6);

-		SOR_LCP (m,nb,J,jb,body,invI,lambda,cforce,rhs,lo,hi,cfm,findex,numIter,sOr);
+		/// SOR_LCP
+		SOR_LCP (m,nb,J,jb,body,invI,lambda,cforce,rhs,lo,hi,cfm,findex,numIter,sOr,stackAlloc);

 #ifdef WARM_STARTING
 		// save lambda for the next iteration
@@ -668,10 +649,8 @@ void SolveInternal1 (
 		}
 #endif

-
 		// note that the SOR method overwrites rhs and J at this point, so
 		// they should not be used again.
-
 		// add stepsize * cforce to the body velocity
 		for (i=0; i<nb; i++) {
 			for (j=0; j<3; j++)
@@ -682,11 +661,8 @@ void SolveInternal1 (
 		}
 	}

-
-
 	// compute the velocity update:
 	// add stepsize * invM * fe to the body velocity
-
 	for (i=0; i<nb; i++) {
 		btScalar body_invMass = body[i]->m_invMass;
 		btVector3 linvel = body[i]->m_linearVelocity;
@@ -703,7 +679,7 @@ void SolveInternal1 (
 		dMULTIPLY0_331NEW(angvel,+=,invI + i*12,body[i]->m_tacc);
 		body[i]->m_angularVelocity = angvel;
 	}
-
+	//stackAlloc->endBlock(saBlock);//Remo: 10.10.2007
 }


--- a/Extras/quickstep/SorLcp.h
+++ b/Extras/quickstep/SorLcp.h
@@ -29,23 +29,85 @@ struct	OdeSolverBody;
 class BU_Joint;
 #include "LinearMath/btScalar.h"
 #include "LinearMath/btAlignedObjectArray.h"
+#include "LinearMath/btStackAlloc.h"

 struct btContactSolverInfo;

-/*
-void SolveInternal1 (float global_cfm,
-					 float global_erp,
-					 OdeSolverBody * const *body, int nb,
-					BU_Joint **_joint, int nj, const btContactSolverInfo& info);
-*/
- 
-void SolveInternal1 (float global_cfm,
-					 float global_erp,
-					 const btAlignedObjectArray<OdeSolverBody*> &body, int nb,
-					 btAlignedObjectArray<BU_Joint*> &joint, 
-					 int nj, const btContactSolverInfo& solverInfo);

-int dRandInt2 (int n);
+//=============================================================================
+class SorLcpSolver //Remotion: 11.10.2007
+{
+public:
+	SorLcpSolver()
+	{
+		dRand2_seed = 0;
+	}
+
+	void SolveInternal1 (float global_cfm,
+		float global_erp,
+		const btAlignedObjectArray<OdeSolverBody*> &body, int nb,
+		btAlignedObjectArray<BU_Joint*> &joint, 
+		int nj, const btContactSolverInfo& solverInfo,
+		btStackAlloc* stackAlloc
+		);
+
+public: //data
+	unsigned long dRand2_seed;
+
+protected: //typedef
+	typedef const btScalar *dRealPtr;
+	typedef btScalar *dRealMutablePtr;
+
+protected: //members
+	//------------------------------------------------------------------------------
+	SIMD_FORCE_INLINE unsigned long dRand2()
+	{
+		dRand2_seed = (1664525L*dRand2_seed + 1013904223L) & 0xffffffff;
+		return dRand2_seed;
+	}
+	//------------------------------------------------------------------------------
+	SIMD_FORCE_INLINE int dRandInt2 (int n)
+	{
+		float a = float(n) / 4294967296.0f;
+		return (int) (float(dRand2()) * a);
+	}
+	//------------------------------------------------------------------------------
+	void SOR_LCP(int m, int nb, dRealMutablePtr J, int *jb, 
+		const btAlignedObjectArray<OdeSolverBody*> &body,
+		dRealPtr invI, dRealMutablePtr lambda, dRealMutablePtr invMforce, dRealMutablePtr rhs,
+		dRealMutablePtr lo, dRealMutablePtr hi, dRealPtr cfm, int *findex,
+		int numiter,float overRelax,
+		btStackAlloc* stackAlloc
+		);
+};
+
+
+//=============================================================================
+class AutoBlockSa //Remotion: 10.10.2007
+{
+	btStackAlloc* stackAlloc;
+	btBlock*	  saBlock;
+public:
+	AutoBlockSa(btStackAlloc* stackAlloc_)
+	{
+		stackAlloc = stackAlloc_;
+		saBlock = stackAlloc->beginBlock();
+	}
+	~AutoBlockSa()
+	{
+		stackAlloc->endBlock(saBlock);
+	}
+	//operator btBlock* () { return saBlock; }
+};
+// //Usage
+//void function(btStackAlloc* stackAlloc)
+//{
+//	AutoBlockSa(stackAlloc);
+//   ...
+//	if(...) return;
+//	return;
+//}
+//------------------------------------------------------------------------------


 #endif //SOR_LCP_H
--- a/msvc/8/appBasicDemo.vcproj
+++ b/msvc/8/appBasicDemo.vcproj
@@ -1,324 +1,455 @@
-<?xml version="1.0" encoding = "Windows-1252"?>
+<?xml version="1.0" encoding="Windows-1252"?>
 <VisualStudioProject
 	ProjectType="Visual C++"
 	Version="8.00"
 	Name="appBasicDemo"
 	ProjectGUID="{3578834A-4B06-DE6F-78AC-FE11F7226D35}"
-	SccProjectName=""
-	SccLocalPath="">
+	>
 	<Platforms>
 		<Platform
-			Name="Win32"/>
+			Name="Win32"
+		/>
 	</Platforms>
+	<ToolFiles>
+	</ToolFiles>
 	<Configurations>
 		<Configuration
 			Name="Release|Win32"
 			OutputDirectory="..\..\out\release8\build\appBasicDemo\"
 			IntermediateDirectory="..\..\out\release8\build\appBasicDemo\"
 			ConfigurationType="1"
+			UseOfMFC="0"
+			ATLMinimizesCRunTimeLibraryUsage="false"
 			CharacterSet="2"
 			WholeProgramOptimization="1"
-			UseOfMFC="0"
-			ATLMinimizesCRunTimeLibraryUsage="FALSE">
+			>
+			<Tool
+				Name="VCPreBuildEventTool"
+			/>
+			<Tool
+				Name="VCCustomBuildTool"
+			/>
+			<Tool
+				Name="VCXMLDataGeneratorTool"
+			/>
+			<Tool
+				Name="VCWebServiceProxyGeneratorTool"
+			/>
+			<Tool
+				Name="VCMIDLTool"
+				PreprocessorDefinitions="NDEBUG;_CONSOLE"
+				MkTypLibCompatible="true"
+				SuppressStartupBanner="true"
+				TargetEnvironment="1"
+				TypeLibraryName="..\..\out\release8\build\appBasicDemo\appBasicDemo.tlb"
+			/>
 			<Tool
 				Name="VCCLCompilerTool"
-				Optimization="2"
-				StringPooling="TRUE"
-				EnableFunctionLevelLinking="TRUE"
-				RuntimeLibrary="0"
-				DebugInformationFormat="3"
-				BufferSecurityCheck="FALSE"
-				PreprocessorDefinitions="NDEBUG;_CONSOLE;WIN32"
-				OptimizeForProcessor="1"
-				ExceptionHandling="0"
 				AdditionalOptions=" "
+				Optimization="2"
 				AdditionalIncludeDirectories=".;..\..;..\..\src;..\..\Glut;..\..\Demos\OpenGL"
+				PreprocessorDefinitions="NDEBUG;_CONSOLE;WIN32"
+				StringPooling="true"
+				ExceptionHandling="0"
+				RuntimeLibrary="0"
+				BufferSecurityCheck="false"
+				EnableFunctionLevelLinking="true"
+				TreatWChar_tAsBuiltInType="false"
 				PrecompiledHeaderFile="..\..\out\release8\build\appBasicDemo\appBasicDemo.pch"
 				AssemblerListingLocation="..\..\out\release8\build\appBasicDemo\"
 				ObjectFile="..\..\out\release8\build\appBasicDemo\"
 				ProgramDataBaseFileName="..\..\out\release8\build\appBasicDemo\BasicDemo.pdb"
 				WarningLevel="3"
-				SuppressStartupBanner="TRUE"
-				Detect64BitPortabilityProblems="TRUE"
-				TreatWChar_tAsBuiltInType="false"
-				CompileAs="0"/>
+				SuppressStartupBanner="true"
+				Detect64BitPortabilityProblems="true"
+				DebugInformationFormat="3"
+				CompileAs="0"
+			/>
 			<Tool
-				Name="VCCustomBuildTool"/>
-			<Tool
-				Name="VCLinkerTool"
-				LinkIncremental="1"
-				OptimizeReferences="2"
-				EnableCOMDATFolding="2"
-				GenerateDebugInformation="TRUE"
-				IgnoreDefaultLibraryNames="LIBC,LIBCD"
-				OutputFile="..\..\ReleaseBasicDemo.exe"
-				SubSystem="1"
-				AdditionalOptions=" "
-				AdditionalDependencies="glut32.lib"
-				IgnoreImportLibrary="TRUE"
-				SuppressStartupBanner="TRUE"
-				GenerateManifest="false"
-				AdditionalLibraryDirectories="..\..\Glut"
-				ProgramDatabaseFile="..\..\out\release8\build\appBasicDemo\BasicDemo.pdb"
-				TargetMachine="1"/>
-			<Tool
-				Name="VCLibrarianTool"
-				SuppressStartupBanner="TRUE"/>
-			<Tool
-				Name="VCMIDLTool"
-				PreprocessorDefinitions="NDEBUG;_CONSOLE"
-				MkTypLibCompatible="TRUE"
-				SuppressStartupBanner="TRUE"
-				TargetEnvironment="1"
-				TypeLibraryName="..\..\out\release8\build\appBasicDemo\appBasicDemo.tlb"/>
-			<Tool
-				Name="VCPostBuildEventTool"
-				/>
-			<Tool
-				Name="VCPreBuildEventTool"/>
-			<Tool
-				Name="VCPreLinkEventTool"/>
+				Name="VCManagedResourceCompilerTool"
+			/>
 			<Tool
 				Name="VCResourceCompilerTool"
 				PreprocessorDefinitions="NDEBUG;_CONSOLE;PROJECTGEN_VERSION=8"
+				Culture="1033"
 				AdditionalIncludeDirectories=".;..\..;..\..\src;..\..\Glut;..\..\Demos\OpenGL"
-				Culture="1033"/>
+			/>
 			<Tool
-				Name="VCWebServiceProxyGeneratorTool"/>
+				Name="VCPreLinkEventTool"
+			/>
 			<Tool
-				Name="VCWebDeploymentTool"/>
+				Name="VCLinkerTool"
+				IgnoreImportLibrary="true"
+				AdditionalOptions=" "
+				AdditionalDependencies="glut32.lib"
+				OutputFile="..\..\ReleaseBasicDemo.exe"
+				LinkIncremental="1"
+				SuppressStartupBanner="true"
+				AdditionalLibraryDirectories="..\..\Glut"
+				GenerateManifest="false"
+				IgnoreDefaultLibraryNames="LIBC,LIBCD"
+				GenerateDebugInformation="true"
+				ProgramDatabaseFile="..\..\out\release8\build\appBasicDemo\BasicDemo.pdb"
+				SubSystem="1"
+				OptimizeReferences="2"
+				EnableCOMDATFolding="2"
+				TargetMachine="1"
+			/>
+			<Tool
+				Name="VCALinkTool"
+			/>
+			<Tool
+				Name="VCManifestTool"
+			/>
+			<Tool
+				Name="VCXDCMakeTool"
+			/>
+			<Tool
+				Name="VCBscMakeTool"
+			/>
+			<Tool
+				Name="VCFxCopTool"
+			/>
+			<Tool
+				Name="VCAppVerifierTool"
+			/>
+			<Tool
+				Name="VCWebDeploymentTool"
+			/>
+			<Tool
+				Name="VCPostBuildEventTool"
+			/>
 		</Configuration>
 		<Configuration
 			Name="ReleaseDoublePrecision|Win32"
 			OutputDirectory="..\..\out\release_dbl8\build\appBasicDemo\"
 			IntermediateDirectory="..\..\out\release_dbl8\build\appBasicDemo\"
 			ConfigurationType="1"
-			CharacterSet="2"
 			UseOfMFC="0"
-			ATLMinimizesCRunTimeLibraryUsage="FALSE">
+			ATLMinimizesCRunTimeLibraryUsage="false"
+			CharacterSet="2"
+			>
+			<Tool
+				Name="VCPreBuildEventTool"
+			/>
+			<Tool
+				Name="VCCustomBuildTool"
+			/>
+			<Tool
+				Name="VCXMLDataGeneratorTool"
+			/>
+			<Tool
+				Name="VCWebServiceProxyGeneratorTool"
+			/>
+			<Tool
+				Name="VCMIDLTool"
+				PreprocessorDefinitions="NDEBUG;BT_USE_DOUBLE_PRECISION;_CONSOLE"
+				MkTypLibCompatible="true"
+				SuppressStartupBanner="true"
+				TargetEnvironment="1"
+				TypeLibraryName="..\..\out\release_dbl8\build\appBasicDemo\appBasicDemo.tlb"
+			/>
 			<Tool
 				Name="VCCLCompilerTool"
-				PreprocessorDefinitions="NDEBUG;BT_USE_DOUBLE_PRECISION;_CONSOLE;WIN32"
-				OptimizeForProcessor="1"
-				ExceptionHandling="0"
 				AdditionalOptions=" "
 				AdditionalIncludeDirectories=".;..\..;..\..\src;..\..\Glut;..\..\Demos\OpenGL"
+				PreprocessorDefinitions="NDEBUG;BT_USE_DOUBLE_PRECISION;_CONSOLE;WIN32"
+				ExceptionHandling="0"
+				TreatWChar_tAsBuiltInType="false"
 				PrecompiledHeaderFile="..\..\out\release_dbl8\build\appBasicDemo\appBasicDemo.pch"
 				AssemblerListingLocation="..\..\out\release_dbl8\build\appBasicDemo\"
 				ObjectFile="..\..\out\release_dbl8\build\appBasicDemo\"
 				ProgramDataBaseFileName="..\..\out\release_dbl8\build\appBasicDemo\BasicDemo.pdb"
 				WarningLevel="3"
-				SuppressStartupBanner="TRUE"
-				Detect64BitPortabilityProblems="TRUE"
-				TreatWChar_tAsBuiltInType="false"
-				CompileAs="0"/>
+				SuppressStartupBanner="true"
+				Detect64BitPortabilityProblems="true"
+				CompileAs="0"
+			/>
 			<Tool
-				Name="VCCustomBuildTool"/>
-			<Tool
-				Name="VCLinkerTool"
-                                OutputFile="..\..\ReleaseDblBasicDemo.exe"
-                                SubSystem="1"
-				AdditionalOptions=" "
-				AdditionalDependencies="glut32.lib"
-				IgnoreImportLibrary="TRUE"
-				SuppressStartupBanner="TRUE"
-				GenerateManifest="false"
-				AdditionalLibraryDirectories="..\..\Glut"
-				ProgramDatabaseFile="..\..\out\release_dbl8\build\appBasicDemo\BasicDemo.pdb"
-				TargetMachine="1"/>
-			<Tool
-				Name="VCLibrarianTool"
-				SuppressStartupBanner="TRUE"/>
-			<Tool
-				Name="VCMIDLTool"
-				PreprocessorDefinitions="NDEBUG;BT_USE_DOUBLE_PRECISION;_CONSOLE"
-				MkTypLibCompatible="TRUE"
-				SuppressStartupBanner="TRUE"
-				TargetEnvironment="1"
-				TypeLibraryName="..\..\out\release_dbl8\build\appBasicDemo\appBasicDemo.tlb"/>
-			<Tool
-				Name="VCPostBuildEventTool"
-				/>
-			<Tool
-				Name="VCPreBuildEventTool"/>
-			<Tool
-				Name="VCPreLinkEventTool"/>
+				Name="VCManagedResourceCompilerTool"
+			/>
 			<Tool
 				Name="VCResourceCompilerTool"
 				PreprocessorDefinitions="NDEBUG;BT_USE_DOUBLE_PRECISION;_CONSOLE;PROJECTGEN_VERSION=8"
+				Culture="1033"
 				AdditionalIncludeDirectories=".;..\..;..\..\src;..\..\Glut;..\..\Demos\OpenGL"
-				Culture="1033"/>
+			/>
 			<Tool
-				Name="VCWebServiceProxyGeneratorTool"/>
+				Name="VCPreLinkEventTool"
+			/>
 			<Tool
-				Name="VCWebDeploymentTool"/>
+				Name="VCLinkerTool"
+				IgnoreImportLibrary="true"
+				AdditionalOptions=" "
+				AdditionalDependencies="glut32.lib"
+				OutputFile="..\..\ReleaseDblBasicDemo.exe"
+				SuppressStartupBanner="true"
+				AdditionalLibraryDirectories="..\..\Glut"
+				GenerateManifest="false"
+				ProgramDatabaseFile="..\..\out\release_dbl8\build\appBasicDemo\BasicDemo.pdb"
+				SubSystem="1"
+				TargetMachine="1"
+			/>
+			<Tool
+				Name="VCALinkTool"
+			/>
+			<Tool
+				Name="VCManifestTool"
+			/>
+			<Tool
+				Name="VCXDCMakeTool"
+			/>
+			<Tool
+				Name="VCBscMakeTool"
+			/>
+			<Tool
+				Name="VCFxCopTool"
+			/>
+			<Tool
+				Name="VCAppVerifierTool"
+			/>
+			<Tool
+				Name="VCWebDeploymentTool"
+			/>
+			<Tool
+				Name="VCPostBuildEventTool"
+			/>
 		</Configuration>
 		<Configuration
 			Name="Debug|Win32"
 			OutputDirectory="..\..\out\debug8\build\appBasicDemo\"
 			IntermediateDirectory="..\..\out\debug8\build\appBasicDemo\"
 			ConfigurationType="1"
-			CharacterSet="2"
 			UseOfMFC="0"
-			ATLMinimizesCRunTimeLibraryUsage="FALSE">
+			ATLMinimizesCRunTimeLibraryUsage="false"
+			CharacterSet="2"
+			>
+			<Tool
+				Name="VCPreBuildEventTool"
+			/>
+			<Tool
+				Name="VCCustomBuildTool"
+			/>
+			<Tool
+				Name="VCXMLDataGeneratorTool"
+			/>
+			<Tool
+				Name="VCWebServiceProxyGeneratorTool"
+			/>
+			<Tool
+				Name="VCMIDLTool"
+				PreprocessorDefinitions="_DEBUG;_CONSOLE"
+				MkTypLibCompatible="true"
+				SuppressStartupBanner="true"
+				TargetEnvironment="1"
+				TypeLibraryName="..\..\out\debug8\build\appBasicDemo\appBasicDemo.tlb"
+			/>
 			<Tool
 				Name="VCCLCompilerTool"
-				Optimization="0"
-				MinimalRebuild="TRUE"
-				DebugInformationFormat="4"
-				RuntimeTypeInfo="FALSE"
-				RuntimeLibrary="1"
-				PreprocessorDefinitions="_DEBUG;_CONSOLE;WIN32"
-				OptimizeForProcessor="1"
-				ExceptionHandling="0"
 				AdditionalOptions=" "
+				Optimization="0"
 				AdditionalIncludeDirectories=".;..\..;..\..\src;..\..\Glut;..\..\Demos\OpenGL"
+				PreprocessorDefinitions="_DEBUG;_CONSOLE;WIN32"
+				MinimalRebuild="true"
+				ExceptionHandling="0"
+				RuntimeLibrary="1"
+				TreatWChar_tAsBuiltInType="false"
+				RuntimeTypeInfo="false"
 				PrecompiledHeaderFile="..\..\out\debug8\build\appBasicDemo\appBasicDemo.pch"
 				AssemblerListingLocation="..\..\out\debug8\build\appBasicDemo\"
 				ObjectFile="..\..\out\debug8\build\appBasicDemo\"
 				ProgramDataBaseFileName="..\..\out\debug8\build\appBasicDemo\BasicDemo.pdb"
 				WarningLevel="3"
-				SuppressStartupBanner="TRUE"
-				Detect64BitPortabilityProblems="TRUE"
-				TreatWChar_tAsBuiltInType="false"
-				CompileAs="0"/>
+				SuppressStartupBanner="true"
+				Detect64BitPortabilityProblems="true"
+				DebugInformationFormat="4"
+				CompileAs="0"
+			/>
 			<Tool
-				Name="VCCustomBuildTool"/>
-			<Tool
-				Name="VCLinkerTool"
-				LinkIncremental="2"
-				GenerateDebugInformation="TRUE"
-				IgnoreDefaultLibraryNames="LIBC,LIBCD"
-				OutputFile="..\..\DebugBasicDemo.exe"
-				SubSystem="1"
-				AdditionalOptions=" "
-				AdditionalDependencies="glut32.lib"
-				IgnoreImportLibrary="TRUE"
-				SuppressStartupBanner="TRUE"
-				GenerateManifest="false"
-				AdditionalLibraryDirectories="..\..\Glut"
-				ProgramDatabaseFile="..\..\out\debug8\build\appBasicDemo\BasicDemo.pdb"
-				TargetMachine="1"/>
-			<Tool
-				Name="VCLibrarianTool"
-				SuppressStartupBanner="TRUE"/>
-			<Tool
-				Name="VCMIDLTool"
-				PreprocessorDefinitions="_DEBUG;_CONSOLE"
-				MkTypLibCompatible="TRUE"
-				SuppressStartupBanner="TRUE"
-				TargetEnvironment="1"
-				TypeLibraryName="..\..\out\debug8\build\appBasicDemo\appBasicDemo.tlb"/>
-			<Tool
-				Name="VCPostBuildEventTool"
-				/>
-			<Tool
-				Name="VCPreBuildEventTool"/>
-			<Tool
-				Name="VCPreLinkEventTool"/>
+				Name="VCManagedResourceCompilerTool"
+			/>
 			<Tool
 				Name="VCResourceCompilerTool"
 				PreprocessorDefinitions="_DEBUG;_CONSOLE;PROJECTGEN_VERSION=8"
+				Culture="1033"
 				AdditionalIncludeDirectories=".;..\..;..\..\src;..\..\Glut;..\..\Demos\OpenGL"
-				Culture="1033"/>
+			/>
 			<Tool
-				Name="VCWebServiceProxyGeneratorTool"/>
+				Name="VCPreLinkEventTool"
+			/>
 			<Tool
-				Name="VCWebDeploymentTool"/>
+				Name="VCLinkerTool"
+				IgnoreImportLibrary="true"
+				AdditionalOptions=" "
+				AdditionalDependencies="glut32.lib"
+				OutputFile="..\..\DebugBasicDemo.exe"
+				LinkIncremental="2"
+				SuppressStartupBanner="true"
+				AdditionalLibraryDirectories="..\..\Glut"
+				GenerateManifest="false"
+				IgnoreDefaultLibraryNames="LIBC,LIBCD"
+				GenerateDebugInformation="true"
+				ProgramDatabaseFile="..\..\out\debug8\build\appBasicDemo\BasicDemo.pdb"
+				SubSystem="1"
+				TargetMachine="1"
+			/>
+			<Tool
+				Name="VCALinkTool"
+			/>
+			<Tool
+				Name="VCManifestTool"
+			/>
+			<Tool
+				Name="VCXDCMakeTool"
+			/>
+			<Tool
+				Name="VCBscMakeTool"
+			/>
+			<Tool
+				Name="VCFxCopTool"
+			/>
+			<Tool
+				Name="VCAppVerifierTool"
+			/>
+			<Tool
+				Name="VCWebDeploymentTool"
+			/>
+			<Tool
+				Name="VCPostBuildEventTool"
+			/>
 		</Configuration>
 		<Configuration
 			Name="DebugDoublePrecision|Win32"
 			OutputDirectory="..\..\out\debug_dbl8\build\appBasicDemo\"
 			IntermediateDirectory="..\..\out\debug_dbl8\build\appBasicDemo\"
 			ConfigurationType="1"
-			CharacterSet="2"
 			UseOfMFC="0"
-			ATLMinimizesCRunTimeLibraryUsage="FALSE">
+			ATLMinimizesCRunTimeLibraryUsage="false"
+			CharacterSet="2"
+			>
+			<Tool
+				Name="VCPreBuildEventTool"
+			/>
+			<Tool
+				Name="VCCustomBuildTool"
+			/>
+			<Tool
+				Name="VCXMLDataGeneratorTool"
+			/>
+			<Tool
+				Name="VCWebServiceProxyGeneratorTool"
+			/>
+			<Tool
+				Name="VCMIDLTool"
+				PreprocessorDefinitions="_DEBUG;BT_USE_DOUBLE_PRECISION;_CONSOLE"
+				MkTypLibCompatible="true"
+				SuppressStartupBanner="true"
+				TargetEnvironment="1"
+				TypeLibraryName="..\..\out\debug_dbl8\build\appBasicDemo\appBasicDemo.tlb"
+			/>
 			<Tool
 				Name="VCCLCompilerTool"
-                                Optimization="0"
-                                MinimalRebuild="TRUE"
-                                DebugInformationFormat="4"
-                                RuntimeTypeInfo="FALSE"
-                                RuntimeLibrary="1"
-				PreprocessorDefinitions="_DEBUG;BT_USE_DOUBLE_PRECISION;_CONSOLE;WIN32"
-				OptimizeForProcessor="1"
-				ExceptionHandling="0"
 				AdditionalOptions=" "
+				Optimization="0"
 				AdditionalIncludeDirectories=".;..\..;..\..\src;..\..\Glut;..\..\Demos\OpenGL"
+				PreprocessorDefinitions="_DEBUG;BT_USE_DOUBLE_PRECISION;_CONSOLE;WIN32"
+				MinimalRebuild="true"
+				ExceptionHandling="0"
+				RuntimeLibrary="1"
+				TreatWChar_tAsBuiltInType="false"
+				RuntimeTypeInfo="false"
 				PrecompiledHeaderFile="..\..\out\debug_dbl8\build\appBasicDemo\appBasicDemo.pch"
 				AssemblerListingLocation="..\..\out\debug_dbl8\build\appBasicDemo\"
 				ObjectFile="..\..\out\debug_dbl8\build\appBasicDemo\"
 				ProgramDataBaseFileName="..\..\out\debug_dbl8\build\appBasicDemo\BasicDemo.pdb"
 				WarningLevel="3"
-				SuppressStartupBanner="TRUE"
-				Detect64BitPortabilityProblems="TRUE"
-				TreatWChar_tAsBuiltInType="false"
-				CompileAs="0"/>
+				SuppressStartupBanner="true"
+				Detect64BitPortabilityProblems="true"
+				DebugInformationFormat="4"
+				CompileAs="0"
+			/>
 			<Tool
-				Name="VCCustomBuildTool"/>
-			<Tool
-				Name="VCLinkerTool"
-                                OutputFile="..\..\DebugDblBasicDemo.exe"
-                                SubSystem="1"
-				AdditionalOptions=" "
-				AdditionalDependencies="glut32.lib"
-				IgnoreImportLibrary="TRUE"
-				SuppressStartupBanner="TRUE"
-				GenerateManifest="false"
-				AdditionalLibraryDirectories="..\..\Glut"
-				ProgramDatabaseFile="..\..\out\debug_dbl8\build\appBasicDemo\BasicDemo.pdb"
-				TargetMachine="1"/>
-			<Tool
-				Name="VCLibrarianTool"
-				SuppressStartupBanner="TRUE"/>
-			<Tool
-				Name="VCMIDLTool"
-				PreprocessorDefinitions="_DEBUG;BT_USE_DOUBLE_PRECISION;_CONSOLE"
-				MkTypLibCompatible="TRUE"
-				SuppressStartupBanner="TRUE"
-				TargetEnvironment="1"
-				TypeLibraryName="..\..\out\debug_dbl8\build\appBasicDemo\appBasicDemo.tlb"/>
-			<Tool
-				Name="VCPostBuildEventTool"
-				/>
-			<Tool
-				Name="VCPreBuildEventTool"/>
-			<Tool
-				Name="VCPreLinkEventTool"/>
+				Name="VCManagedResourceCompilerTool"
+			/>
 			<Tool
 				Name="VCResourceCompilerTool"
 				PreprocessorDefinitions="_DEBUG;BT_USE_DOUBLE_PRECISION;_CONSOLE;PROJECTGEN_VERSION=8"
+				Culture="1033"
 				AdditionalIncludeDirectories=".;..\..;..\..\src;..\..\Glut;..\..\Demos\OpenGL"
-				Culture="1033"/>
+			/>
 			<Tool
-				Name="VCWebServiceProxyGeneratorTool"/>
+				Name="VCPreLinkEventTool"
+			/>
 			<Tool
-				Name="VCWebDeploymentTool"/>
+				Name="VCLinkerTool"
+				IgnoreImportLibrary="true"
+				AdditionalOptions=" "
+				AdditionalDependencies="glut32.lib"
+				OutputFile="..\..\DebugDblBasicDemo.exe"
+				SuppressStartupBanner="true"
+				AdditionalLibraryDirectories="..\..\Glut"
+				GenerateManifest="false"
+				ProgramDatabaseFile="..\..\out\debug_dbl8\build\appBasicDemo\BasicDemo.pdb"
+				SubSystem="1"
+				TargetMachine="1"
+			/>
+			<Tool
+				Name="VCALinkTool"
+			/>
+			<Tool
+				Name="VCManifestTool"
+			/>
+			<Tool
+				Name="VCXDCMakeTool"
+			/>
+			<Tool
+				Name="VCBscMakeTool"
+			/>
+			<Tool
+				Name="VCFxCopTool"
+			/>
+			<Tool
+				Name="VCAppVerifierTool"
+			/>
+			<Tool
+				Name="VCWebDeploymentTool"
+			/>
+			<Tool
+				Name="VCPostBuildEventTool"
+			/>
 		</Configuration>
 	</Configurations>
+	<References>
+	</References>
 	<Files>
 		<Filter
 			Name="Source Files"
-			Filter="">
-			<File
-				RelativePath="..\..\Demos\BasicDemo\BasicDemo.cpp">
-			</File>
+			>
 		</Filter>
 		<Filter
 			Name="Header Files"
-			Filter="">
+			>
 			<File
-				RelativePath="..\..\Demos\BasicDemo\BasicDemo.h">
+				RelativePath="..\..\Demos\BasicDemo\BasicDemo.h"
+				>
 			</File>
 		</Filter>
 		<Filter
 			Name="Resource Files"
-			Filter="">
+			>
 			<File
-				RelativePath="..\..\msvc\appBasicDemo.rc">
+				RelativePath="..\..\msvc\appBasicDemo.rc"
+				>
 			</File>
 		</Filter>
+		<File
+			RelativePath="..\..\Demos\BasicDemo\StressTestDemo.cpp"
+			>
+		</File>
+		<File
+			RelativePath="..\..\Demos\BasicDemo\StressTestDemo.h"
+			>
+		</File>
 	</Files>
 	<Globals>
 	</Globals>
--- a/msvc/8/libbulletopenglsupport.vcproj
+++ b/msvc/8/libbulletopenglsupport.vcproj
@@ -1,15 +1,17 @@
-<?xml version="1.0" encoding = "Windows-1252"?>
+<?xml version="1.0" encoding="Windows-1252"?>
 <VisualStudioProject
 	ProjectType="Visual C++"
 	Version="8.00"
 	Name="libbulletopenglsupport"
 	ProjectGUID="{7C428E76-9271-6284-20F0-9B38ED6931E3}"
-	SccProjectName=""
-	SccLocalPath="">
+	>
 	<Platforms>
 		<Platform
-			Name="Win32"/>
+			Name="Win32"
+		/>
 	</Platforms>
+	<ToolFiles>
+	</ToolFiles>
 	<Configurations>
 		<Configuration
 			Name="Release|Win32"
@@ -17,74 +19,83 @@
 			IntermediateDirectory="..\..\out\release8\build\libbulletopenglsupport\"
 			ConfigurationType="4"
 			UseOfMFC="0"
-			ATLMinimizesCRunTimeLibraryUsage="FALSE">
+			ATLMinimizesCRunTimeLibraryUsage="false"
+			>
+			<Tool
+				Name="VCPreBuildEventTool"
+			/>
+			<Tool
+				Name="VCCustomBuildTool"
+			/>
+			<Tool
+				Name="VCXMLDataGeneratorTool"
+			/>
+			<Tool
+				Name="VCWebServiceProxyGeneratorTool"
+			/>
+			<Tool
+				Name="VCMIDLTool"
+				PreprocessorDefinitions="NDEBUG;_LIB;_WINDOWS"
+				MkTypLibCompatible="true"
+				SuppressStartupBanner="true"
+				TargetEnvironment="1"
+				TypeLibraryName="..\..\out\release8\build\libbulletopenglsupport\libbulletopenglsupport.tlb"
+			/>
 			<Tool
 				Name="VCCLCompilerTool"
-				Optimization="2"
-				StringPooling="TRUE"
-				EnableFunctionLevelLinking="TRUE"
-				RuntimeLibrary="0"
-				DebugInformationFormat="3"
-				BufferSecurityCheck="FALSE"
-				PreprocessorDefinitions="NDEBUG;_LIB;_WINDOWS;WIN32"
-				OptimizeForProcessor="1"
-				ExceptionHandling="0"
 				AdditionalOptions=" "
+				Optimization="2"
 				AdditionalIncludeDirectories=".;..\..;..\..\src;..\..\Glut"
+				PreprocessorDefinitions="NDEBUG;_LIB;_WINDOWS;WIN32"
+				StringPooling="true"
+				ExceptionHandling="0"
+				RuntimeLibrary="0"
+				BufferSecurityCheck="false"
+				EnableFunctionLevelLinking="true"
+				TreatWChar_tAsBuiltInType="false"
 				PrecompiledHeaderFile="..\..\out\release8\build\libbulletopenglsupport\libbulletopenglsupport.pch"
 				AssemblerListingLocation="..\..\out\release8\build\libbulletopenglsupport\"
 				ObjectFile="..\..\out\release8\build\libbulletopenglsupport\"
 				ProgramDataBaseFileName="..\..\out\release8\build\libbulletopenglsupport\bulletopenglsupport.pdb"
 				WarningLevel="3"
-				SuppressStartupBanner="TRUE"
-				Detect64BitPortabilityProblems="TRUE"
-				TreatWChar_tAsBuiltInType="false"
-				CompileAs="0"/>
+				SuppressStartupBanner="true"
+				Detect64BitPortabilityProblems="true"
+				DebugInformationFormat="3"
+				CompileAs="0"
+			/>
 			<Tool
-				Name="VCCustomBuildTool"/>
-			<Tool
-				Name="VCLinkerTool"
-				LinkIncremental="1"
-				OptimizeReferences="2"
-				EnableCOMDATFolding="2"
-				GenerateDebugInformation="TRUE"
-				IgnoreDefaultLibraryNames="LIBC,LIBCD"
-				AdditionalOptions=" "
-				AdditionalDependencies="glut32.lib"
-				IgnoreImportLibrary="TRUE"
-				SuppressStartupBanner="TRUE"
-				GenerateManifest="false"
-				AdditionalLibraryDirectories="..\..\Glut"
-				ProgramDatabaseFile="..\..\out\release8\build\libbulletopenglsupport\bulletopenglsupport.pdb"
-				TargetMachine="1"/>
-			<Tool
-				Name="VCLibrarianTool"
-				OutputFile="..\..\out\release8\libs\libbulletopenglsupport.lib"
-				SuppressStartupBanner="TRUE"/>
-			<Tool
-				Name="VCMIDLTool"
-				PreprocessorDefinitions="NDEBUG;_LIB;_WINDOWS"
-				MkTypLibCompatible="TRUE"
-				SuppressStartupBanner="TRUE"
-				TargetEnvironment="1"
-				TypeLibraryName="..\..\out\release8\build\libbulletopenglsupport\libbulletopenglsupport.tlb"/>
-			<Tool
-				Name="VCPostBuildEventTool"
-		CommandLine="xcopy /r /y $(TARGETPATH) $(PROJECTDIR)..\..\lib\"
-				/>
-			<Tool
-				Name="VCPreBuildEventTool"/>
-			<Tool
-				Name="VCPreLinkEventTool"/>
+				Name="VCManagedResourceCompilerTool"
+			/>
 			<Tool
 				Name="VCResourceCompilerTool"
 				PreprocessorDefinitions="NDEBUG;_LIB;_WINDOWS;PROJECTGEN_VERSION=8"
+				Culture="1033"
 				AdditionalIncludeDirectories=".;..\..;..\..\src;..\..\Glut"
-				Culture="1033"/>
+			/>
 			<Tool
-				Name="VCWebServiceProxyGeneratorTool"/>
+				Name="VCPreLinkEventTool"
+			/>
 			<Tool
-				Name="VCWebDeploymentTool"/>
+				Name="VCLibrarianTool"
+				OutputFile="..\..\out\release8\libs\libbulletopenglsupport.lib"
+				SuppressStartupBanner="true"
+			/>
+			<Tool
+				Name="VCALinkTool"
+			/>
+			<Tool
+				Name="VCXDCMakeTool"
+			/>
+			<Tool
+				Name="VCBscMakeTool"
+			/>
+			<Tool
+				Name="VCFxCopTool"
+			/>
+			<Tool
+				Name="VCPostBuildEventTool"
+				CommandLine="xcopy /r /y $(TARGETPATH) $(PROJECTDIR)..\..\lib\"
+			/>
 		</Configuration>
 		<Configuration
 			Name="ReleaseDoublePrecision|Win32"
@@ -92,62 +103,76 @@
 			IntermediateDirectory="..\..\out\release_dbl8\build\libbulletopenglsupport\"
 			ConfigurationType="4"
 			UseOfMFC="0"
-			ATLMinimizesCRunTimeLibraryUsage="FALSE">
+			ATLMinimizesCRunTimeLibraryUsage="false"
+			>
+			<Tool
+				Name="VCPreBuildEventTool"
+			/>
+			<Tool
+				Name="VCCustomBuildTool"
+			/>
+			<Tool
+				Name="VCXMLDataGeneratorTool"
+			/>
+			<Tool
+				Name="VCWebServiceProxyGeneratorTool"
+			/>
+			<Tool
+				Name="VCMIDLTool"
+				PreprocessorDefinitions="NDEBUG;BT_USE_DOUBLE_PRECISION;_LIB;_WINDOWS"
+				MkTypLibCompatible="true"
+				SuppressStartupBanner="true"
+				TargetEnvironment="1"
+				TypeLibraryName="..\..\out\release_dbl8\build\libbulletopenglsupport\libbulletopenglsupport.tlb"
+			/>
 			<Tool
 				Name="VCCLCompilerTool"
-				PreprocessorDefinitions="NDEBUG;BT_USE_DOUBLE_PRECISION;_LIB;_WINDOWS;WIN32"
-				OptimizeForProcessor="1"
-				ExceptionHandling="0"
 				AdditionalOptions=" "
 				AdditionalIncludeDirectories=".;..\..;..\..\src;..\..\Glut"
+				PreprocessorDefinitions="NDEBUG;BT_USE_DOUBLE_PRECISION;_LIB;_WINDOWS;WIN32"
+				ExceptionHandling="0"
+				TreatWChar_tAsBuiltInType="false"
 				PrecompiledHeaderFile="..\..\out\release_dbl8\build\libbulletopenglsupport\libbulletopenglsupport.pch"
 				AssemblerListingLocation="..\..\out\release_dbl8\build\libbulletopenglsupport\"
 				ObjectFile="..\..\out\release_dbl8\build\libbulletopenglsupport\"
 				ProgramDataBaseFileName="..\..\out\release_dbl8\build\libbulletopenglsupport\bulletopenglsupport.pdb"
 				WarningLevel="3"
-				SuppressStartupBanner="TRUE"
-				Detect64BitPortabilityProblems="TRUE"
-				TreatWChar_tAsBuiltInType="false"
-				CompileAs="0"/>
+				SuppressStartupBanner="true"
+				Detect64BitPortabilityProblems="true"
+				CompileAs="0"
+			/>
 			<Tool
-				Name="VCCustomBuildTool"/>
-			<Tool
-				Name="VCLinkerTool"
-				AdditionalOptions=" "
-				AdditionalDependencies="glut32.lib"
-				IgnoreImportLibrary="TRUE"
-				SuppressStartupBanner="TRUE"
-				GenerateManifest="false"
-				AdditionalLibraryDirectories="..\..\Glut"
-				ProgramDatabaseFile="..\..\out\release_dbl8\build\libbulletopenglsupport\bulletopenglsupport.pdb"
-				TargetMachine="1"/>
-			<Tool
-				Name="VCLibrarianTool"
-				SuppressStartupBanner="TRUE"/>
-			<Tool
-				Name="VCMIDLTool"
-				PreprocessorDefinitions="NDEBUG;BT_USE_DOUBLE_PRECISION;_LIB;_WINDOWS"
-				MkTypLibCompatible="TRUE"
-				SuppressStartupBanner="TRUE"
-				TargetEnvironment="1"
-				TypeLibraryName="..\..\out\release_dbl8\build\libbulletopenglsupport\libbulletopenglsupport.tlb"/>
-			<Tool
-				Name="VCPostBuildEventTool"
-		CommandLine="xcopy /r /y $(TARGETPATH) $(PROJECTDIR)..\..\lib\"
-				/>
-			<Tool
-				Name="VCPreBuildEventTool"/>
-			<Tool
-				Name="VCPreLinkEventTool"/>
+				Name="VCManagedResourceCompilerTool"
+			/>
 			<Tool
 				Name="VCResourceCompilerTool"
 				PreprocessorDefinitions="NDEBUG;BT_USE_DOUBLE_PRECISION;_LIB;_WINDOWS;PROJECTGEN_VERSION=8"
+				Culture="1033"
 				AdditionalIncludeDirectories=".;..\..;..\..\src;..\..\Glut"
-				Culture="1033"/>
+			/>
 			<Tool
-				Name="VCWebServiceProxyGeneratorTool"/>
+				Name="VCPreLinkEventTool"
+			/>
 			<Tool
-				Name="VCWebDeploymentTool"/>
+				Name="VCLibrarianTool"
+				SuppressStartupBanner="true"
+			/>
+			<Tool
+				Name="VCALinkTool"
+			/>
+			<Tool
+				Name="VCXDCMakeTool"
+			/>
+			<Tool
+				Name="VCBscMakeTool"
+			/>
+			<Tool
+				Name="VCFxCopTool"
+			/>
+			<Tool
+				Name="VCPostBuildEventTool"
+				CommandLine="xcopy /r /y $(TARGETPATH) $(PROJECTDIR)..\..\lib\"
+			/>
 		</Configuration>
 		<Configuration
 			Name="Debug|Win32"
@@ -155,71 +180,82 @@
 			IntermediateDirectory="..\..\out\debug8\build\libbulletopenglsupport\"
 			ConfigurationType="4"
 			UseOfMFC="0"
-			ATLMinimizesCRunTimeLibraryUsage="FALSE">
+			ATLMinimizesCRunTimeLibraryUsage="false"
+			>
+			<Tool
+				Name="VCPreBuildEventTool"
+			/>
+			<Tool
+				Name="VCCustomBuildTool"
+			/>
+			<Tool
+				Name="VCXMLDataGeneratorTool"
+			/>
+			<Tool
+				Name="VCWebServiceProxyGeneratorTool"
+			/>
+			<Tool
+				Name="VCMIDLTool"
+				PreprocessorDefinitions="_DEBUG;_LIB;_WINDOWS"
+				MkTypLibCompatible="true"
+				SuppressStartupBanner="true"
+				TargetEnvironment="1"
+				TypeLibraryName="..\..\out\debug8\build\libbulletopenglsupport\libbulletopenglsupport.tlb"
+			/>
 			<Tool
 				Name="VCCLCompilerTool"
-				Optimization="0"
-				MinimalRebuild="TRUE"
-				DebugInformationFormat="4"
-				RuntimeTypeInfo="FALSE"
-				RuntimeLibrary="1"
-				PreprocessorDefinitions="_DEBUG;_LIB;_WINDOWS;WIN32"
-				OptimizeForProcessor="1"
-				ExceptionHandling="0"
 				AdditionalOptions=" "
+				Optimization="0"
 				AdditionalIncludeDirectories=".;..\..;..\..\src;..\..\Glut"
+				PreprocessorDefinitions="_DEBUG;_LIB;_WINDOWS;WIN32"
+				MinimalRebuild="true"
+				ExceptionHandling="0"
+				RuntimeLibrary="1"
+				TreatWChar_tAsBuiltInType="false"
+				RuntimeTypeInfo="false"
 				PrecompiledHeaderFile="..\..\out\debug8\build\libbulletopenglsupport\libbulletopenglsupport.pch"
 				AssemblerListingLocation="..\..\out\debug8\build\libbulletopenglsupport\"
 				ObjectFile="..\..\out\debug8\build\libbulletopenglsupport\"
 				ProgramDataBaseFileName="..\..\out\debug8\build\libbulletopenglsupport\bulletopenglsupport.pdb"
 				WarningLevel="3"
-				SuppressStartupBanner="TRUE"
-				Detect64BitPortabilityProblems="TRUE"
-				TreatWChar_tAsBuiltInType="false"
-				CompileAs="0"/>
+				SuppressStartupBanner="true"
+				Detect64BitPortabilityProblems="true"
+				DebugInformationFormat="4"
+				CompileAs="0"
+			/>
 			<Tool
-				Name="VCCustomBuildTool"/>
-			<Tool
-				Name="VCLinkerTool"
-				LinkIncremental="2"
-				GenerateDebugInformation="TRUE"
-				IgnoreDefaultLibraryNames="LIBC,LIBCD"
-				AdditionalOptions=" "
-				AdditionalDependencies="glut32.lib"
-				IgnoreImportLibrary="TRUE"
-				SuppressStartupBanner="TRUE"
-				GenerateManifest="false"
-				AdditionalLibraryDirectories="..\..\Glut"
-				ProgramDatabaseFile="..\..\out\debug8\build\libbulletopenglsupport\bulletopenglsupport.pdb"
-				TargetMachine="1"/>
-			<Tool
-				Name="VCLibrarianTool"
-				OutputFile="..\..\out\debug8\libs\libbulletopenglsupport_d.lib"
-				SuppressStartupBanner="TRUE"/>
-			<Tool
-				Name="VCMIDLTool"
-				PreprocessorDefinitions="_DEBUG;_LIB;_WINDOWS"
-				MkTypLibCompatible="TRUE"
-				SuppressStartupBanner="TRUE"
-				TargetEnvironment="1"
-				TypeLibraryName="..\..\out\debug8\build\libbulletopenglsupport\libbulletopenglsupport.tlb"/>
-			<Tool
-				Name="VCPostBuildEventTool"
-		CommandLine="xcopy /r /y $(TARGETPATH) $(PROJECTDIR)..\..\lib\"
-				/>
-			<Tool
-				Name="VCPreBuildEventTool"/>
-			<Tool
-				Name="VCPreLinkEventTool"/>
+				Name="VCManagedResourceCompilerTool"
+			/>
 			<Tool
 				Name="VCResourceCompilerTool"
 				PreprocessorDefinitions="_DEBUG;_LIB;_WINDOWS;PROJECTGEN_VERSION=8"
+				Culture="1033"
 				AdditionalIncludeDirectories=".;..\..;..\..\src;..\..\Glut"
-				Culture="1033"/>
+			/>
 			<Tool
-				Name="VCWebServiceProxyGeneratorTool"/>
+				Name="VCPreLinkEventTool"
+			/>
 			<Tool
-				Name="VCWebDeploymentTool"/>
+				Name="VCLibrarianTool"
+				OutputFile="..\..\out\debug8\libs\libbulletopenglsupport_d.lib"
+				SuppressStartupBanner="true"
+			/>
+			<Tool
+				Name="VCALinkTool"
+			/>
+			<Tool
+				Name="VCXDCMakeTool"
+			/>
+			<Tool
+				Name="VCBscMakeTool"
+			/>
+			<Tool
+				Name="VCFxCopTool"
+			/>
+			<Tool
+				Name="VCPostBuildEventTool"
+				CommandLine="xcopy /r /y $(TARGETPATH) $(PROJECTDIR)..\..\lib\"
+			/>
 		</Configuration>
 		<Configuration
 			Name="DebugDoublePrecision|Win32"
@@ -227,139 +263,176 @@
 			IntermediateDirectory="..\..\out\debug_dbl8\build\libbulletopenglsupport\"
 			ConfigurationType="4"
 			UseOfMFC="0"
-			ATLMinimizesCRunTimeLibraryUsage="FALSE">
+			ATLMinimizesCRunTimeLibraryUsage="false"
+			>
+			<Tool
+				Name="VCPreBuildEventTool"
+			/>
+			<Tool
+				Name="VCCustomBuildTool"
+			/>
+			<Tool
+				Name="VCXMLDataGeneratorTool"
+			/>
+			<Tool
+				Name="VCWebServiceProxyGeneratorTool"
+			/>
+			<Tool
+				Name="VCMIDLTool"
+				PreprocessorDefinitions="_DEBUG;BT_USE_DOUBLE_PRECISION;_LIB;_WINDOWS"
+				MkTypLibCompatible="true"
+				SuppressStartupBanner="true"
+				TargetEnvironment="1"
+				TypeLibraryName="..\..\out\debug_dbl8\build\libbulletopenglsupport\libbulletopenglsupport.tlb"
+			/>
 			<Tool
 				Name="VCCLCompilerTool"
-                                Optimization="0"
-                                MinimalRebuild="TRUE"
-                                DebugInformationFormat="4"
-                                RuntimeTypeInfo="FALSE"
-                                RuntimeLibrary="1"
-				PreprocessorDefinitions="_DEBUG;BT_USE_DOUBLE_PRECISION;_LIB;_WINDOWS;WIN32"
-				OptimizeForProcessor="1"
-				ExceptionHandling="0"
 				AdditionalOptions=" "
+				Optimization="0"
 				AdditionalIncludeDirectories=".;..\..;..\..\src;..\..\Glut"
+				PreprocessorDefinitions="_DEBUG;BT_USE_DOUBLE_PRECISION;_LIB;_WINDOWS;WIN32"
+				MinimalRebuild="true"
+				ExceptionHandling="0"
+				RuntimeLibrary="1"
+				TreatWChar_tAsBuiltInType="false"
+				RuntimeTypeInfo="false"
 				PrecompiledHeaderFile="..\..\out\debug_dbl8\build\libbulletopenglsupport\libbulletopenglsupport.pch"
 				AssemblerListingLocation="..\..\out\debug_dbl8\build\libbulletopenglsupport\"
 				ObjectFile="..\..\out\debug_dbl8\build\libbulletopenglsupport\"
 				ProgramDataBaseFileName="..\..\out\debug_dbl8\build\libbulletopenglsupport\bulletopenglsupport.pdb"
 				WarningLevel="3"
-				SuppressStartupBanner="TRUE"
-				Detect64BitPortabilityProblems="TRUE"
-				TreatWChar_tAsBuiltInType="false"
-				CompileAs="0"/>
+				SuppressStartupBanner="true"
+				Detect64BitPortabilityProblems="true"
+				DebugInformationFormat="4"
+				CompileAs="0"
+			/>
 			<Tool
-				Name="VCCustomBuildTool"/>
-			<Tool
-				Name="VCLinkerTool"
-				AdditionalOptions=" "
-				AdditionalDependencies="glut32.lib"
-				IgnoreImportLibrary="TRUE"
-				SuppressStartupBanner="TRUE"
-				GenerateManifest="false"
-				AdditionalLibraryDirectories="..\..\Glut"
-				ProgramDatabaseFile="..\..\out\debug_dbl8\build\libbulletopenglsupport\bulletopenglsupport.pdb"
-				TargetMachine="1"/>
-			<Tool
-				Name="VCLibrarianTool"
-				SuppressStartupBanner="TRUE"/>
-			<Tool
-				Name="VCMIDLTool"
-				PreprocessorDefinitions="_DEBUG;BT_USE_DOUBLE_PRECISION;_LIB;_WINDOWS"
-				MkTypLibCompatible="TRUE"
-				SuppressStartupBanner="TRUE"
-				TargetEnvironment="1"
-				TypeLibraryName="..\..\out\debug_dbl8\build\libbulletopenglsupport\libbulletopenglsupport.tlb"/>
-			<Tool
-				Name="VCPostBuildEventTool"
-		CommandLine="xcopy /r /y $(TARGETPATH) $(PROJECTDIR)..\..\lib\"
-				/>
-			<Tool
-				Name="VCPreBuildEventTool"/>
-			<Tool
-				Name="VCPreLinkEventTool"/>
+				Name="VCManagedResourceCompilerTool"
+			/>
 			<Tool
 				Name="VCResourceCompilerTool"
 				PreprocessorDefinitions="_DEBUG;BT_USE_DOUBLE_PRECISION;_LIB;_WINDOWS;PROJECTGEN_VERSION=8"
+				Culture="1033"
 				AdditionalIncludeDirectories=".;..\..;..\..\src;..\..\Glut"
-				Culture="1033"/>
+			/>
 			<Tool
-				Name="VCWebServiceProxyGeneratorTool"/>
+				Name="VCPreLinkEventTool"
+			/>
 			<Tool
-				Name="VCWebDeploymentTool"/>
+				Name="VCLibrarianTool"
+				SuppressStartupBanner="true"
+			/>
+			<Tool
+				Name="VCALinkTool"
+			/>
+			<Tool
+				Name="VCXDCMakeTool"
+			/>
+			<Tool
+				Name="VCBscMakeTool"
+			/>
+			<Tool
+				Name="VCFxCopTool"
+			/>
+			<Tool
+				Name="VCPostBuildEventTool"
+				CommandLine="xcopy /r /y $(TARGETPATH) $(PROJECTDIR)..\..\lib\"
+			/>
 		</Configuration>
 	</Configurations>
+	<References>
+	</References>
 	<Files>
 		<Filter
 			Name="Source Files"
-			Filter="">
+			>
 			<File
-				RelativePath="..\..\Demos\OpenGL\BMF_Api.cpp">
+				RelativePath="..\..\Demos\OpenGL\BMF_Api.cpp"
+				>
 			</File>
 			<File
-				RelativePath="..\..\Demos\OpenGL\BMF_BitmapFont.cpp">
+				RelativePath="..\..\Demos\OpenGL\BMF_BitmapFont.cpp"
+				>
 			</File>
 			<File
-				RelativePath="..\..\Demos\OpenGL\BMF_font_helv10.cpp">
+				RelativePath="..\..\Demos\OpenGL\BMF_font_helv10.cpp"
+				>
 			</File>
 			<File
-				RelativePath="..\..\Demos\OpenGL\DemoApplication.cpp">
+				RelativePath="..\..\Demos\OpenGL\DemoApplication.cpp"
+				>
 			</File>
 			<File
-				RelativePath="..\..\Demos\OpenGL\GL_ShapeDrawer.cpp">
+				RelativePath="..\..\Demos\OpenGL\GL_ShapeDrawer.cpp"
+				>
 			</File>
 			<File
-				RelativePath="..\..\Demos\OpenGL\GL_Simplex1to4.cpp">
+				RelativePath="..\..\Demos\OpenGL\GL_Simplex1to4.cpp"
+				>
 			</File>
 			<File
-				RelativePath="..\..\Demos\OpenGL\GLDebugDrawer.cpp">
+				RelativePath="..\..\Demos\OpenGL\GLDebugDrawer.cpp"
+				>
 			</File>
 			<File
-				RelativePath="..\..\Demos\OpenGL\GlutStuff.cpp">
+				RelativePath="..\..\Demos\OpenGL\GlutStuff.cpp"
+				>
 			</File>
 			<File
-				RelativePath="..\..\Demos\OpenGL\RenderTexture.cpp">
+				RelativePath="..\..\Demos\OpenGL\RenderTexture.cpp"
+				>
 			</File>
 		</Filter>
 		<Filter
 			Name="Header Files"
-			Filter="">
+			>
 			<File
-				RelativePath="..\..\Demos\OpenGL\BMF_Api.h">
+				RelativePath="..\..\Demos\OpenGL\BMF_Api.h"
+				>
 			</File>
 			<File
-				RelativePath="..\..\Demos\OpenGL\BMF_BitmapFont.h">
+				RelativePath="..\..\Demos\OpenGL\BMF_BitmapFont.h"
+				>
 			</File>
 			<File
-				RelativePath="..\..\Demos\OpenGL\BMF_FontData.h">
+				RelativePath="..\..\Demos\OpenGL\BMF_FontData.h"
+				>
 			</File>
 			<File
-				RelativePath="..\..\Demos\OpenGL\BMF_Fonts.h">
+				RelativePath="..\..\Demos\OpenGL\BMF_Fonts.h"
+				>
 			</File>
 			<File
-				RelativePath="..\..\Demos\OpenGL\BMF_Settings.h">
+				RelativePath="..\..\Demos\OpenGL\BMF_Settings.h"
+				>
 			</File>
 			<File
-				RelativePath="..\..\Demos\OpenGL\DebugCastResult.h">
+				RelativePath="..\..\Demos\OpenGL\DebugCastResult.h"
+				>
 			</File>
 			<File
-				RelativePath="..\..\Demos\OpenGL\DemoApplication.h">
+				RelativePath="..\..\Demos\OpenGL\DemoApplication.h"
+				>
 			</File>
 			<File
-				RelativePath="..\..\Demos\OpenGL\GL_ShapeDrawer.h">
+				RelativePath="..\..\Demos\OpenGL\GL_ShapeDrawer.h"
+				>
 			</File>
 			<File
-				RelativePath="..\..\Demos\OpenGL\GL_Simplex1to4.h">
+				RelativePath="..\..\Demos\OpenGL\GL_Simplex1to4.h"
+				>
 			</File>
 			<File
-				RelativePath="..\..\Demos\OpenGL\GLDebugDrawer.h">
+				RelativePath="..\..\Demos\OpenGL\GLDebugDrawer.h"
+				>
 			</File>
 			<File
-				RelativePath="..\..\Demos\OpenGL\GlutStuff.h">
+				RelativePath="..\..\Demos\OpenGL\GlutStuff.h"
+				>
 			</File>
 			<File
-				RelativePath="..\..\Demos\OpenGL\RenderTexture.h">
+				RelativePath="..\..\Demos\OpenGL\RenderTexture.h"
+				>
 			</File>
 		</Filter>
 	</Files>
--- a/msvc/8/wksbullet.sln
+++ b/msvc/8/wksbullet.sln
--- a/src/BulletCollision/BroadphaseCollision/btAxisSweep3.h
+++ b/src/BulletCollision/BroadphaseCollision/btAxisSweep3.h
@@ -63,8 +63,8 @@ public:
 		
 		//void* m_pOwner; this is now in btBroadphaseProxy.m_clientObject
 	
-		inline void SetNextFree(BP_FP_INT_TYPE next) {m_minEdges[0] = next;}
-		inline BP_FP_INT_TYPE GetNextFree() const {return m_minEdges[0];}
+		SIMD_FORCE_INLINE void SetNextFree(BP_FP_INT_TYPE next) {m_minEdges[0] = next;}
+		SIMD_FORCE_INLINE BP_FP_INT_TYPE GetNextFree() const {return m_minEdges[0];}
 	};		// 24 bytes + 24 for Edge structures = 44 bytes total per entry

 	
@@ -119,7 +119,7 @@ public:
 	BP_FP_INT_TYPE addHandle(const btPoint3& aabbMin,const btPoint3& aabbMax, void* pOwner,short int collisionFilterGroup,short int collisionFilterMask,btDispatcher* dispatcher);
 	void removeHandle(BP_FP_INT_TYPE handle,btDispatcher* dispatcher);
 	void updateHandle(BP_FP_INT_TYPE handle, const btPoint3& aabbMin,const btPoint3& aabbMax,btDispatcher* dispatcher);
-	inline Handle* getHandle(BP_FP_INT_TYPE index) const {return m_pHandles + index;}
+	SIMD_FORCE_INLINE Handle* getHandle(BP_FP_INT_TYPE index) const {return m_pHandles + index;}

 	void	processAllOverlappingPairs(btOverlapCallback* callback);

@@ -344,7 +344,7 @@ BP_FP_INT_TYPE btAxisSweep3Internal<BP_FP_INT_TYPE>::addHandle(const btPoint3& a

 	// allocate a handle
 	BP_FP_INT_TYPE handle = allocHandle();
-	assert(handle!= 0xcdcd);
+	

 	Handle* pHandle = getHandle(handle);
 	
--- a/src/BulletCollision/BroadphaseCollision/btBroadphaseProxy.h
+++ b/src/BulletCollision/BroadphaseCollision/btBroadphaseProxy.h
@@ -98,7 +98,7 @@ BT_DECLARE_ALIGNED_ALLOCATOR();
 	};

 	int			m_uniqueId;//m_uniqueId is introduced for paircache. could get rid of this, by calculating the address offset etc.
-	inline int getUid()
+	SIMD_FORCE_INLINE int getUid()
 	{
 		return m_uniqueId;//(int)this;
 	}
@@ -115,26 +115,26 @@ BT_DECLARE_ALIGNED_ALLOCATOR();

 	

-	static inline bool isPolyhedral(int proxyType)
+	static SIMD_FORCE_INLINE bool isPolyhedral(int proxyType)
 	{
 		return (proxyType  < IMPLICIT_CONVEX_SHAPES_START_HERE);
 	}

-	static inline bool	isConvex(int proxyType)
+	static SIMD_FORCE_INLINE bool	isConvex(int proxyType)
 	{
 		return (proxyType < CONCAVE_SHAPES_START_HERE);
 	}

-	static inline bool	isConcave(int proxyType)
+	static SIMD_FORCE_INLINE bool	isConcave(int proxyType)
 	{
 		return ((proxyType > CONCAVE_SHAPES_START_HERE) &&
 			(proxyType < CONCAVE_SHAPES_END_HERE));
 	}
-	static inline bool	isCompound(int proxyType)
+	static SIMD_FORCE_INLINE bool	isCompound(int proxyType)
 	{
 		return (proxyType == COMPOUND_SHAPE_PROXYTYPE);
 	}
-	static inline bool isInfinite(int proxyType)
+	static SIMD_FORCE_INLINE bool isInfinite(int proxyType)
 	{
 		return (proxyType == STATIC_PLANE_PROXYTYPE);
 	}
--- a/src/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.cpp
+++ b/src/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.cpp
@@ -23,10 +23,17 @@ subject to the following restrictions:
 ///	btOverlappingPairCache*	m_overlappingPairs;
 extern int gOverlappingPairs;

-btMultiSapBroadphase::btMultiSapBroadphase(int maxProxies)
-:m_invalidPair(0)
+btMultiSapBroadphase::btMultiSapBroadphase(int maxProxies,btOverlappingPairCache* pairCache)
+:m_invalidPair(0),
+m_ownsPairCache(false),
+m_overlappingPairs(pairCache)
 {
-	m_overlappingPairs = new btOverlappingPairCache();
+	if (!m_overlappingPairs)
+	{
+		m_ownsPairCache = true;
+		void* mem = btAlignedAlloc(sizeof(btOverlappingPairCache),16);
+		m_overlappingPairs = new (mem)btOverlappingPairCache();
+	}

 	struct btMultiSapOverlapFilterCallback : public btOverlapFilterCallback
 	{
@@ -54,6 +61,10 @@ btMultiSapBroadphase::btMultiSapBroadphase(int maxProxies)

 btMultiSapBroadphase::~btMultiSapBroadphase()
 {
+	if (m_ownsPairCache)
+	{
+		btAlignedFree(m_overlappingPairs);
+	}
 }

 btBroadphaseProxy*	btMultiSapBroadphase::createProxy(  const btVector3& aabbMin,  const btVector3& aabbMax,int shapeType,void* userPtr, short int collisionFilterGroup,short int collisionFilterMask, btDispatcher* dispatcher)
--- a/src/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.h
+++ b/src/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.h
@@ -35,6 +35,8 @@ class btMultiSapBroadphase :public btBroadphaseInterface
 	btSimpleBroadphase*		m_simpleBroadphase;

 	btOverlappingPairCache*	m_overlappingPairs;
+
+	bool					m_ownsPairCache;
 	
 	btOverlapFilterCallback*	m_filterCallback;

@@ -81,7 +83,7 @@ protected:

 public:

-	btMultiSapBroadphase(int maxProxies = 16384);
+	btMultiSapBroadphase(int maxProxies = 16384,btOverlappingPairCache* pairCache=0);

 	btSapBroadphaseArray	getBroadphaseArray()
 	{
--- a/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp
+++ b/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp
@@ -23,24 +23,18 @@ subject to the following restrictions:

 int	gOverlappingPairs = 0;

+int gRemovePairs =0;
+int gAddedPairs =0;
+int gFindPairs =0;
+
 btOverlappingPairCache::btOverlappingPairCache():
-m_blockedForChanges(false),
-m_overlapFilterCallback(0)
-//m_NumOverlapBroadphasePair(0)
+	m_blockedForChanges(false),
+	m_overlapFilterCallback(0)
 {
+	int initialAllocatedSize= 65536*256;//2;//this needs to be a power of 2!
+	m_overlappingPairArray.reserve(initialAllocatedSize);

-#ifdef USE_HASH_PAIRCACHE
-	m_overlappingPairArray.reserve(b2_maxPairs);
-
-	for (int32 i = 0; i < b2_tableCapacity; ++i)
-	{
-		m_hashTable[i] = b2_nullPair;
-	}
-	for (int32 i = 0; i < b2_maxPairs; ++i)
-	{
-		m_next[i] = b2_nullPair;
-	}
-#endif //USE_HASH_PAIRCACHE
+	growTables();
 }


@@ -125,66 +119,31 @@ void	btOverlappingPairCache::removeOverlappingPairsContainingProxy(btBroadphaseP

 #ifdef USE_HASH_PAIRCACHE

-// Thomas Wang's hash, see: http://www.concentric.net/~Ttwang/tech/inthash.htm
-// This assumes proxyId1 and proxyId2 are 16-bit.
-inline uint32 Hash(uint32 proxyId1, uint32 proxyId2)
+
+
+
+
+
+
+btBroadphasePair* btOverlappingPairCache::findPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1)
 {
-	uint32 key = (proxyId2 << 16) | proxyId1;
-	key = ~key + (key << 15);
-	key = key ^ (key >> 12);
-	key = key + (key << 2);
-	key = key ^ (key >> 4);
-	key = key * 2057;
-	key = key ^ (key >> 16);
-	return key;
-}
+	gFindPairs++;

-inline bool Equals(const btBroadphasePair& pair, int32 proxyId1, int32 proxyId2)
-{	
-	return pair.m_pProxy0->getUid() == proxyId1 && pair.m_pProxy1->getUid() == proxyId2;
-}
-
-inline btBroadphasePair* btOverlappingPairCache::Find(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1, uint32 hash)
-{
-	int32 proxyId1 = proxy0->getUid();
-	int32 proxyId2 = proxy1->getUid();
-	if (proxyId1 > proxyId2) 
-		btSwap(proxyId1, proxyId2);
-
-	int32 index = m_hashTable[hash];
-	
-	while( index != b2_nullPair && Equals(m_overlappingPairArray[index], proxyId1, proxyId2) == false)
-	{
-		index = m_next[index];
-	}
-
-	if ( index == b2_nullPair )
-	{
-		return NULL;
-	}
-
-	btAssert(index < m_overlappingPairArray.size());
-
-	return &m_overlappingPairArray[index];
-}
-
-btBroadphasePair* btOverlappingPairCache::Find(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1)
-{
-	int32 proxyId1 = proxy0->getUid();
-	int32 proxyId2 = proxy1->getUid();
+	int proxyId1 = proxy0->getUid();
+	int proxyId2 = proxy1->getUid();

 	if (proxyId1 > proxyId2) 
 		btSwap(proxyId1, proxyId2);

-	int32 hash = Hash(proxyId1, proxyId2) & b2_tableMask;
+	int hash = getHash(proxyId1, proxyId2) & (m_overlappingPairArray.capacity()-1);

-	int32 index = m_hashTable[hash];
-	while (index != b2_nullPair && Equals(m_overlappingPairArray[index], proxyId1, proxyId2) == false)
+	int index = m_hashTable[hash];
+	while (index != BT_NULL_PAIR && equalsPair(m_overlappingPairArray[index], proxyId1, proxyId2) == false)
 	{
 		index = m_next[index];
 	}

-	if (index == b2_nullPair)
+	if (index == BT_NULL_PAIR)
 	{
 		return NULL;
 	}
@@ -196,32 +155,79 @@ btBroadphasePair* btOverlappingPairCache::Find(btBroadphaseProxy* proxy0, btBroa

 #include <stdio.h>

-btBroadphasePair* btOverlappingPairCache::Add(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1)
+void	btOverlappingPairCache::growTables()
 {
-	int32 proxyId1 = proxy0->getUid();
-	int32 proxyId2 = proxy1->getUid();
+	//or put an assert here instead?
+
+	int newCapacity = m_overlappingPairArray.capacity();
+
+	if (m_hashTable.size() < newCapacity)
+	{
+		//grow hashtable and next table
+		int curHashtableSize = m_hashTable.size();
+		int curNextTableSize = m_next.size();
+
+		m_hashTable.resize(newCapacity);
+		m_next.resize(newCapacity);
+
+		for (int i= curHashtableSize; i < newCapacity; ++i)
+		{
+			m_hashTable[i] = BT_NULL_PAIR;
+		}
+		for (int i = curNextTableSize; i < newCapacity; ++i)
+		{
+			m_next[i] = BT_NULL_PAIR;
+		}
+
+		for(int	i=0;i<curHashtableSize;i++)
+		{
+			btAssert(0);
+			//this is not working yet, work in progress... please allocate enough room up front to avoid growing for now.
+	
+
+			const btBroadphasePair& pair = m_overlappingPairArray[i];
+			int proxyId1 = pair.m_pProxy0->getUid();
+			int proxyId2 = pair.m_pProxy1->getUid();
+			if (proxyId1 > proxyId2) 
+				btSwap(proxyId1, proxyId2);
+			int	hashValue = getHash(proxyId1,proxyId2) & (m_overlappingPairArray.capacity()-1);	// New hash value with new mask
+			m_next[i] = m_hashTable[hashValue];
+			m_hashTable[hashValue] = i;
+		}
+
+
+	}
+}
+
+btBroadphasePair* btOverlappingPairCache::internalAddPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1)
+{
+	int proxyId1 = proxy0->getUid();
+	int proxyId2 = proxy1->getUid();

 	if (proxyId1 > proxyId2) 
 		btSwap(proxyId1, proxyId2);

-	int32 hash = Hash(proxyId1, proxyId2) & b2_tableMask;
+	int hash = getHash(proxyId1, proxyId2) & (m_overlappingPairArray.capacity()-1);

 	

-	btBroadphasePair* pair = Find(proxy0, proxy1, hash);
+	btBroadphasePair* pair = internalFindPair(proxy0, proxy1, hash);
 	if (pair != NULL)
 	{
 		return pair;
 	}

-	if (m_overlappingPairArray.size() == b2_maxPairs)
-	{
-		btAssert(false);
-		return NULL;
-	}
-	
 	int count = m_overlappingPairArray.size();
+	int oldCapacity = m_overlappingPairArray.capacity();
 	void* mem = &m_overlappingPairArray.expand();
+	int newCapacity = m_overlappingPairArray.capacity();
+
+	if (oldCapacity < newCapacity)
+	{
+		growTables();
+		//hash with new capacity
+		hash = getHash(proxyId1, proxyId2) & (m_overlappingPairArray.capacity()-1);
+	}
 	
 	pair = new (mem) btBroadphasePair(*proxy0,*proxy1);
 //	pair->m_pProxy0 = proxy0;
@@ -231,26 +237,29 @@ btBroadphasePair* btOverlappingPairCache::Add(btBroadphaseProxy* proxy0, btBroad
 	

 	m_next[count] = m_hashTable[hash];
-	m_hashTable[hash] = (uint16)count;
+	m_hashTable[hash] = count;

 	return pair;
 }


-void* btOverlappingPairCache::Remove(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1,btDispatcher* dispatcher)
+
+void* btOverlappingPairCache::removeOverlappingPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1,btDispatcher* dispatcher)
 {
-	int32 proxyId1 = proxy0->getUid();
-	int32 proxyId2 = proxy1->getUid();
+	gRemovePairs++;
+
+	int proxyId1 = proxy0->getUid();
+	int proxyId2 = proxy1->getUid();

 	if (proxyId1 > proxyId2) 
 		btSwap(proxyId1, proxyId2);

-	int32 hash = Hash(proxyId1, proxyId2) & b2_tableMask;
+	int hash = getHash(proxyId1, proxyId2) & (m_overlappingPairArray.capacity()-1);

-	btBroadphasePair* pair = Find(proxy0, proxy1, hash);
+	btBroadphasePair* pair = internalFindPair(proxy0, proxy1, hash);
 	if (pair == NULL)
 	{
-		return NULL;
+		return 0;
 	}

 	cleanOverlappingPair(*pair,dispatcher);
@@ -260,21 +269,21 @@ void* btOverlappingPairCache::Remove(btBroadphaseProxy* proxy0, btBroadphaseProx
 	btAssert(pair->m_pProxy0->getUid() == proxyId1);
 	btAssert(pair->m_pProxy1->getUid() == proxyId2);

-	int32 pairIndex = int32(pair - &m_overlappingPairArray[0]);
+	int pairIndex = int(pair - &m_overlappingPairArray[0]);
 	btAssert(pairIndex < m_overlappingPairArray.size());

 	// Remove the pair from the hash table.
-	int32 index = m_hashTable[hash];
-	btAssert(index != b2_nullPair);
+	int index = m_hashTable[hash];
+	btAssert(index != BT_NULL_PAIR);

-	int32 previous = b2_nullPair;
+	int previous = BT_NULL_PAIR;
 	while (index != pairIndex)
 	{
 		previous = index;
 		index = m_next[index];
 	}

-	if (previous != b2_nullPair)
+	if (previous != BT_NULL_PAIR)
 	{
 		btAssert(m_next[previous] == pairIndex);
 		m_next[previous] = m_next[pairIndex];
@@ -288,7 +297,7 @@ void* btOverlappingPairCache::Remove(btBroadphaseProxy* proxy0, btBroadphaseProx
 	// pair being removed. We need to fix the hash
 	// table indices to support the move.

-	int32 lastPairIndex = m_overlappingPairArray.size() - 1;
+	int lastPairIndex = m_overlappingPairArray.size() - 1;

 	// If the removed pair is the last pair, we are done.
 	if (lastPairIndex == pairIndex)
@@ -299,19 +308,19 @@ void* btOverlappingPairCache::Remove(btBroadphaseProxy* proxy0, btBroadphaseProx

 	// Remove the last pair from the hash table.
 	const btBroadphasePair* last = &m_overlappingPairArray[lastPairIndex];
-	int32 lastHash = Hash(last->m_pProxy0->getUid(), last->m_pProxy1->getUid()) & b2_tableMask;
+	int lastHash = getHash(last->m_pProxy0->getUid(), last->m_pProxy1->getUid()) & (m_overlappingPairArray.capacity()-1);

 	index = m_hashTable[lastHash];
-	btAssert(index != b2_nullPair);
+	btAssert(index != BT_NULL_PAIR);

-	previous = b2_nullPair;
+	previous = BT_NULL_PAIR;
 	while (index != lastPairIndex)
 	{
 		previous = index;
 		index = m_next[index];
 	}

-	if (previous != b2_nullPair)
+	if (previous != BT_NULL_PAIR)
 	{
 		btAssert(m_next[previous] == lastPairIndex);
 		m_next[previous] = m_next[lastPairIndex];
@@ -326,7 +335,7 @@ void* btOverlappingPairCache::Remove(btBroadphaseProxy* proxy0, btBroadphaseProx

 	// Insert the last pair into the hash table
 	m_next[pairIndex] = m_hashTable[lastHash];
-	m_hashTable[lastHash] = (uint16)pairIndex;
+	m_hashTable[lastHash] = pairIndex;

 	m_overlappingPairArray.pop_back();

@@ -361,7 +370,7 @@ void	btOverlappingPairCache::processAllOverlappingPairs(btOverlapCallback* callb



-void	btOverlappingPairCache::removeOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1, btDispatcher* dispatcher )
+void*	btOverlappingPairCache::removeOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1, btDispatcher* dispatcher )
 {
 #ifndef USE_LAZY_REMOVAL

@@ -372,13 +381,16 @@ void	btOverlappingPairCache::removeOverlappingPair(btBroadphaseProxy* proxy0,btB
 	{
 		gOverlappingPairs--;
 		btBroadphasePair& pair = m_overlappingPairArray[findIndex];
+		void* userData = pair.m_userInfo;
 		cleanOverlappingPair(pair,dispatcher);
 		
-		m_overlappingPairArray.swap(findIndex,m_overlappingPairArray.size()-1);
+		m_overlappingPairArray.swap(findIndex,m_overlappingPairArray.capacity()-1);
 		m_overlappingPairArray.pop_back();
+		return userData;
 	}
 #endif //USE_LAZY_REMOVAL

+	return 0;
 }


@@ -388,18 +400,18 @@ void	btOverlappingPairCache::removeOverlappingPair(btBroadphaseProxy* proxy0,btB



-void	btOverlappingPairCache::addOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1)
+btBroadphasePair*	btOverlappingPairCache::addOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1)
 {
 	//don't add overlap with own
 	assert(proxy0 != proxy1);

 	if (!needsBroadphaseCollision(proxy0,proxy1))
-		return;
-
-
-	btBroadphasePair pair(*proxy0,*proxy1);
-	m_overlappingPairArray.push_back(pair);
+		return 0;
+	
+	void* mem = &m_overlappingPairArray.expand();
+	btBroadphasePair* pair = new (mem) btBroadphasePair(*proxy0,*proxy1);
 	gOverlappingPairs++;
+	return pair;
 	
 }

@@ -448,7 +460,7 @@ void	btOverlappingPairCache::processAllOverlappingPairs(btOverlapCallback* callb
 		{
 			cleanOverlappingPair(*pair,dispatcher);

-			m_overlappingPairArray.swap(i,m_overlappingPairArray.size()-1);
+			m_overlappingPairArray.swap(i,m_overlappingPairArray.capacity()-1);
 			m_overlappingPairArray.pop_back();
 			gOverlappingPairs--;
 		} else
--- a/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h
+++ b/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h
@@ -24,6 +24,7 @@ subject to the following restrictions:
 #include "LinearMath/btAlignedObjectArray.h"
 class btDispatcher;

+///disable the USE_HASH_PAIRCACHE define to use a pair manager that sorts the pairs to find duplicates/non-overlap
 #define USE_HASH_PAIRCACHE 1


@@ -48,18 +49,13 @@ typedef btAlignedObjectArray<btBroadphasePair>	btBroadphasePairArray;
 #ifdef USE_HASH_PAIRCACHE


-
-const int b2_maxPairs = 65536;//32768;
-typedef unsigned short int uint16;
-typedef int int32;
-typedef unsigned int uint32;
-
 /// Hash-space based Pair Cache, thanks to Erin Catto, Box2D, http://www.box2d.org, and Pierre Terdiman, Codercorner, http://codercorner.com
-const uint16 b2_nullPair = 0xffff;
-const uint16 b2_nullProxy = 0xffff;
-const int32 b2_tableCapacity = b2_maxPairs;	// must be a power of two
-const int32 b2_tableMask = b2_tableCapacity - 1;

+extern int gRemovePairs;
+extern int gAddedPairs;
+extern int gFindPairs;
+
+#define BT_NULL_PAIR 0xffffffff

 class btOverlappingPairCache
 {
@@ -67,6 +63,8 @@ class btOverlappingPairCache
 	btOverlapFilterCallback* m_overlapFilterCallback;
 	bool		m_blockedForChanges;

+	
+
 public:
 	btOverlappingPairCache();
 	~btOverlappingPairCache();
@@ -74,12 +72,9 @@ public:
 	
 	void	removeOverlappingPairsContainingProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher);

-	void	removeOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1,btDispatcher* dispatcher)
-	{
-		Remove(proxy0,proxy1,dispatcher);
-	}
-
-	inline bool needsBroadphaseCollision(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1) const
+	void*	removeOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1,btDispatcher* dispatcher);
+	
+	SIMD_FORCE_INLINE bool needsBroadphaseCollision(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1) const
 	{
 		if (m_overlapFilterCallback)
 			return m_overlapFilterCallback->needBroadphaseCollision(proxy0,proxy1);
@@ -90,18 +85,19 @@ public:
 		return collides;
 	}

-	void	addOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1)
+	// Add a pair and return the new pair. If the pair already exists,
+	// no new pair is created and the old one is returned.
+	SIMD_FORCE_INLINE	btBroadphasePair* 	addOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1)
 	{
+		gAddedPairs++;
+
 		if (!needsBroadphaseCollision(proxy0,proxy1))
-			return;
+			return 0;

-		Add(proxy0,proxy1);
+		return internalAddPair(proxy0,proxy1);
 	}

-	btBroadphasePair*	findPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1)
-	{
-		return Find(proxy0,proxy1);
-	}
+	

 	void	cleanProxyFromPairs(btBroadphaseProxy* proxy,btDispatcher* dispatcher);

@@ -130,16 +126,11 @@ public:

 	void	cleanOverlappingPair(btBroadphasePair& pair,btDispatcher* dispatcher);

-	// Add a pair and return the new pair. If the pair already exists,
-	// no new pair is created and the old one is returned.
-	btBroadphasePair* Add(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1);

-	// Remove a pair, return the pair's userData.
-	void* Remove(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1,btDispatcher* dispatcher);

-	btBroadphasePair* Find(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1);
+	btBroadphasePair* findPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1);

-	int32 GetCount() const { return m_overlappingPairArray.size(); }
+	int GetCount() const { return m_overlappingPairArray.size(); }
 //	btBroadphasePair* GetPairs() { return m_pairs; }

 	btOverlapFilterCallback* getOverlapFilterCallback()
@@ -157,12 +148,82 @@ public:
 		return m_overlappingPairArray.size();
 	}
 private:
-	btBroadphasePair* Find(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1, uint32 hashValue);
+	
+	btBroadphasePair* 	internalAddPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1);
+
+	void	growTables();
+
+	SIMD_FORCE_INLINE bool equalsPair(const btBroadphasePair& pair, int proxyId1, int proxyId2)
+	{	
+		return pair.m_pProxy0->getUid() == proxyId1 && pair.m_pProxy1->getUid() == proxyId2;
+	}
+
+	/*
+	// Thomas Wang's hash, see: http://www.concentric.net/~Ttwang/tech/inthash.htm
+	// This assumes proxyId1 and proxyId2 are 16-bit.
+	SIMD_FORCE_INLINE int getHash(int proxyId1, int proxyId2)
+	{
+		int key = (proxyId2 << 16) | proxyId1;
+		key = ~key + (key << 15);
+		key = key ^ (key >> 12);
+		key = key + (key << 2);
+		key = key ^ (key >> 4);
+		key = key * 2057;
+		key = key ^ (key >> 16);
+		return key;
+	}
+	*/
+
+
+	
+	SIMD_FORCE_INLINE	unsigned int getHash(unsigned int proxyId1, unsigned int proxyId2)
+	{
+		int key = ((unsigned int)proxyId1) | (((unsigned int)proxyId1) <<16);
+		// Thomas Wang's hash
+
+		key += ~(key << 15);
+		key ^=  (key >> 10);
+		key +=  (key << 3);
+		key ^=  (key >> 6);
+		key += ~(key << 11);
+		key ^=  (key >> 16);
+		return key;
+	}
+	
+
+
+
+
+	SIMD_FORCE_INLINE btBroadphasePair* internalFindPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1, int hash)
+	{
+		int proxyId1 = proxy0->getUid();
+		int proxyId2 = proxy1->getUid();
+		if (proxyId1 > proxyId2) 
+			btSwap(proxyId1, proxyId2);
+
+		int index = m_hashTable[hash];
+		
+		while( index != BT_NULL_PAIR && equalsPair(m_overlappingPairArray[index], proxyId1, proxyId2) == false)
+		{
+			index = m_next[index];
+		}
+
+		if ( index == BT_NULL_PAIR )
+		{
+			return NULL;
+		}
+
+		btAssert(index < m_overlappingPairArray.size());
+
+		return &m_overlappingPairArray[index];
+	}
+

 public:
 	
-	uint16 m_hashTable[b2_tableCapacity];
-	uint16 m_next[b2_maxPairs];
+	btAlignedObjectArray<int>	m_hashTable;
+	btAlignedObjectArray<int>	m_next;
+	
 };


@@ -192,11 +253,11 @@ class	btOverlappingPairCache

 		virtual void	processAllOverlappingPairs(btOverlapCallback*,btDispatcher* dispatcher);

-		void	removeOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1,btDispatcher* dispatcher);
+		void*	removeOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1,btDispatcher* dispatcher);

 		void	cleanOverlappingPair(btBroadphasePair& pair,btDispatcher* dispatcher);
 		
-		void	addOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1);
+		btBroadphasePair*	addOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1);

 		btBroadphasePair*	findPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1);
 			
--- a/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp
+++ b/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp
@@ -44,7 +44,8 @@ btSimpleBroadphase::btSimpleBroadphase(int maxProxies, btOverlappingPairCache* o

 	if (!overlappingPairCache)
 	{
-		m_pairCache = new btOverlappingPairCache();
+		void* mem = btAlignedAlloc(sizeof(btOverlappingPairCache),16);
+		m_pairCache = new (mem)btOverlappingPairCache();
 		m_ownsPairCache = true;
 	}

@@ -76,7 +77,7 @@ btSimpleBroadphase::~btSimpleBroadphase()

 	if (m_ownsPairCache)
 	{
-		delete m_pairCache;
+		btAlignedFree(m_pairCache);
 	}
 }

--- a/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.h
+++ b/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.h
@@ -39,11 +39,11 @@ struct btSimpleBroadphaseProxy : public btBroadphaseProxy
 	}
 	
 	
-	inline void SetNextFree(int next) {m_nextFree = next;}
-	inline int GetNextFree() const {return m_nextFree;}
+	SIMD_FORCE_INLINE void SetNextFree(int next) {m_nextFree = next;}
+	SIMD_FORCE_INLINE int GetNextFree() const {return m_nextFree;}

-	inline void SetNextAllocated(int next) {m_nextAllocated = next;}
-	inline int GetNextAllocated() const {return m_nextAllocated;}
+	SIMD_FORCE_INLINE void SetNextAllocated(int next) {m_nextAllocated = next;}
+	SIMD_FORCE_INLINE int GetNextAllocated() const {return m_nextAllocated;}


 };
--- a/src/BulletCollision/CollisionDispatch/btCollisionObject.h
+++ b/src/BulletCollision/CollisionDispatch/btCollisionObject.h
@@ -101,28 +101,28 @@ public:
 	};


-	inline bool mergesSimulationIslands() const
+	SIMD_FORCE_INLINE bool mergesSimulationIslands() const
 	{
 		///static objects, kinematic and object without contact response don't merge islands
 		return  ((m_collisionFlags & (CF_STATIC_OBJECT | CF_KINEMATIC_OBJECT | CF_NO_CONTACT_RESPONSE) )==0);
 	}


-	inline bool		isStaticObject() const {
+	SIMD_FORCE_INLINE bool		isStaticObject() const {
 		return (m_collisionFlags & CF_STATIC_OBJECT) != 0;
 	}

-	inline bool		isKinematicObject() const
+	SIMD_FORCE_INLINE bool		isKinematicObject() const
 	{
 		return (m_collisionFlags & CF_KINEMATIC_OBJECT) != 0;
 	}

-	inline bool		isStaticOrKinematicObject() const
+	SIMD_FORCE_INLINE bool		isStaticOrKinematicObject() const
 	{
 		return (m_collisionFlags & (CF_KINEMATIC_OBJECT | CF_STATIC_OBJECT)) != 0 ;
 	}

-	inline bool		hasContactResponse() const {
+	SIMD_FORCE_INLINE bool		hasContactResponse() const {
 		return (m_collisionFlags & CF_NO_CONTACT_RESPONSE)==0;
 	}

@@ -136,12 +136,12 @@ public:
 		m_collisionShape = collisionShape;
 	}

-	inline const btCollisionShape*	getCollisionShape() const
+	SIMD_FORCE_INLINE const btCollisionShape*	getCollisionShape() const
 	{
 		return m_collisionShape;
 	}

-	inline btCollisionShape*	getCollisionShape()
+	SIMD_FORCE_INLINE btCollisionShape*	getCollisionShape()
 	{
 		return m_collisionShape;
 	}
--- a/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.h
+++ b/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.h
@@ -55,11 +55,11 @@ int	m_triangleCount;
 	
 	void clearCache();

-	inline const btVector3& getAabbMin() const
+	SIMD_FORCE_INLINE const btVector3& getAabbMin() const
 	{
 		return m_aabbMin;
 	}
-	inline const btVector3& getAabbMax() const
+	SIMD_FORCE_INLINE const btVector3& getAabbMax() const
 	{
 		return m_aabbMax;
 	}
--- a/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.cpp
+++ b/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.cpp
@@ -109,8 +109,8 @@ btScalar btSphereBoxCollisionAlgorithm::getSphereDistance(btCollisionObject* box
 	btVector3 bounds[2];
 	btBoxShape* boxShape= (btBoxShape*)boxObj->getCollisionShape();
 	
-	bounds[0] = -boxShape->getHalfExtents();
-	bounds[1] = boxShape->getHalfExtents();
+	bounds[0] = -boxShape->getHalfExtentsWithoutMargin();
+	bounds[1] = boxShape->getHalfExtentsWithoutMargin();

 	margins = boxShape->getMargin();//also add sphereShape margin?

--- a/src/BulletCollision/CollisionDispatch/btUnionFind.h
+++ b/src/BulletCollision/CollisionDispatch/btUnionFind.h
@@ -46,11 +46,11 @@ class btUnionFind

 	  void	reset(int N);

-	  inline int	getNumElements() const
+	  SIMD_FORCE_INLINE int	getNumElements() const
 	  {
 		  return int(m_elements.size());
 	  }
-	  inline bool  isRoot(int x) const
+	  SIMD_FORCE_INLINE bool  isRoot(int x) const
 	  {
 		  return (x == m_elements[x].m_id);
 	  }
--- a/src/BulletCollision/CollisionShapes/btBoxShape.cpp
+++ b/src/BulletCollision/CollisionShapes/btBoxShape.cpp
@@ -15,16 +15,13 @@ subject to the following restrictions:

 #include "btBoxShape.h"

-btVector3 btBoxShape::getHalfExtents() const
-{
-	return m_implicitShapeDimensions * m_localScaling;
-}
+
 //{ 


 void btBoxShape::getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const
 {
-	btVector3 halfExtents = getHalfExtents();
+	const btVector3& halfExtents = getHalfExtentsWithoutMargin();

 	btMatrix3x3 abs_b = t.getBasis().absolute();  
 	btPoint3 center = t.getOrigin();
@@ -43,7 +40,7 @@ void btBoxShape::getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabb
 void	btBoxShape::calculateLocalInertia(btScalar mass,btVector3& inertia)
 {
 	//btScalar margin = btScalar(0.);
-	btVector3 halfExtents = getHalfExtents();
+	btVector3 halfExtents = getHalfExtentsWithMargin();

 	btScalar lx=btScalar(2.)*(halfExtents.x());
 	btScalar ly=btScalar(2.)*(halfExtents.y());
--- a/src/BulletCollision/CollisionShapes/btBoxShape.h
+++ b/src/BulletCollision/CollisionShapes/btBoxShape.h
@@ -31,25 +31,37 @@ class btBoxShape: public btPolyhedralConvexShape

 public:

-	btVector3 getHalfExtents() const;
-		
+	btVector3 getHalfExtentsWithMargin() const
+	{
+		btVector3 halfExtents = getHalfExtentsWithoutMargin();
+		btVector3 margin(getMargin(),getMargin(),getMargin());
+		halfExtents += margin;
+		return halfExtents;
+	}
+	
+	const btVector3& getHalfExtentsWithoutMargin() const
+	{
+		return m_implicitShapeDimensions;//changed in Bullet 2.63: assume the scaling and margin are included
+	}
+	
+
 	virtual int	getShapeType() const { return BOX_SHAPE_PROXYTYPE;}

 	virtual btVector3	localGetSupportingVertex(const btVector3& vec) const
 	{
-		btVector3 halfExtents = getHalfExtents();
+		btVector3 halfExtents = getHalfExtentsWithoutMargin();
+		btVector3 margin(getMargin(),getMargin(),getMargin());
+		halfExtents += margin;
 		
 		return btVector3(btFsels(vec.x(), halfExtents.x(), -halfExtents.x()),
 			btFsels(vec.y(), halfExtents.y(), -halfExtents.y()),
 			btFsels(vec.z(), halfExtents.z(), -halfExtents.z()));
 	}

-	virtual inline btVector3	localGetSupportingVertexWithoutMargin(const btVector3& vec)const
+	SIMD_FORCE_INLINE  btVector3	localGetSupportingVertexWithoutMargin(const btVector3& vec)const
 	{
-		btVector3 halfExtents = getHalfExtents();
-		btVector3 margin(getMargin(),getMargin(),getMargin());
-		halfExtents -= margin;
-
+		const btVector3& halfExtents = getHalfExtentsWithoutMargin();
+		
 		return btVector3(btFsels(vec.x(), halfExtents.x(), -halfExtents.x()),
 			btFsels(vec.y(), halfExtents.y(), -halfExtents.y()),
 			btFsels(vec.z(), halfExtents.z(), -halfExtents.z()));
@@ -57,11 +69,8 @@ public:

 	virtual void	batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const
 	{
-		btVector3 halfExtents = getHalfExtents();
-		btVector3 margin(getMargin(),getMargin(),getMargin());
-		halfExtents -= margin;
-
-
+		const btVector3& halfExtents = getHalfExtentsWithoutMargin();
+	
 		for (int i=0;i<numVectors;i++)
 		{
 			const btVector3& vec = vectors[i];
@@ -75,9 +84,33 @@ public:

 	btBoxShape( const btVector3& boxHalfExtents)
 	{
-		m_implicitShapeDimensions = boxHalfExtents;
+		btVector3 margin(getMargin(),getMargin(),getMargin());
+		m_implicitShapeDimensions = (boxHalfExtents * m_localScaling) - margin;
 	};
-	
+
+	virtual void setMargin(btScalar collisionMargin)
+	{
+		//correct the m_implicitShapeDimensions for the margin
+		btVector3 oldMargin(getMargin(),getMargin(),getMargin());
+		btVector3 implicitShapeDimensionsWithMargin = m_implicitShapeDimensions+oldMargin;
+		
+		btConvexInternalShape::setMargin(collisionMargin);
+		btVector3 newMargin(getMargin(),getMargin(),getMargin());
+		m_implicitShapeDimensions = implicitShapeDimensionsWithMargin - newMargin;
+
+	}
+	virtual void	setLocalScaling(const btVector3& scaling)
+	{
+		btVector3 oldMargin(getMargin(),getMargin(),getMargin());
+		btVector3 implicitShapeDimensionsWithMargin = m_implicitShapeDimensions+oldMargin;
+		btVector3 unScaledImplicitShapeDimensionsWithMargin = implicitShapeDimensionsWithMargin / m_localScaling;
+
+		btConvexInternalShape::setLocalScaling(scaling);
+
+		m_implicitShapeDimensions = (unScaledImplicitShapeDimensionsWithMargin * m_localScaling) - oldMargin;
+
+	}
+
 	virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const;

 	
@@ -112,7 +145,7 @@ public:

 	virtual void getVertex(int i,btVector3& vtx) const
 	{
-		btVector3 halfExtents = getHalfExtents();
+		btVector3 halfExtents = getHalfExtentsWithoutMargin();

 		vtx = btVector3(
 				halfExtents.x() * (1-(i&1)) - halfExtents.x() * (i&1),
@@ -123,7 +156,7 @@ public:

 	virtual void	getPlaneEquation(btVector4& plane,int i) const
 	{
-		btVector3 halfExtents = getHalfExtents();
+		btVector3 halfExtents = getHalfExtentsWithoutMargin();

 		switch (i)
 		{
@@ -230,7 +263,7 @@ public:
 	
 	virtual	bool isInside(const btPoint3& pt,btScalar tolerance) const
 	{
-		btVector3 halfExtents = getHalfExtents();
+		btVector3 halfExtents = getHalfExtentsWithoutMargin();

 		//btScalar minDist = 2*tolerance;
 		
--- a/src/BulletCollision/CollisionShapes/btCollisionShape.h
+++ b/src/BulletCollision/CollisionShapes/btCollisionShape.h
@@ -49,26 +49,26 @@ public:

 #ifndef __SPU__

-	inline bool	isPolyhedral() const
+	SIMD_FORCE_INLINE bool	isPolyhedral() const
 	{
 		return btBroadphaseProxy::isPolyhedral(getShapeType());
 	}

-	inline bool	isConvex() const
+	SIMD_FORCE_INLINE bool	isConvex() const
 	{
 		return btBroadphaseProxy::isConvex(getShapeType());
 	}
-	inline bool	isConcave() const
+	SIMD_FORCE_INLINE bool	isConcave() const
 	{
 		return btBroadphaseProxy::isConcave(getShapeType());
 	}
-	inline bool	isCompound() const
+	SIMD_FORCE_INLINE bool	isCompound() const
 	{
 		return btBroadphaseProxy::isCompound(getShapeType());
 	}

 	///isInfinite is used to catch simulation error (aabb check)
-	inline bool isInfinite() const
+	SIMD_FORCE_INLINE bool isInfinite() const
 	{
 		return btBroadphaseProxy::isInfinite(getShapeType());
 	}
--- a/src/BulletCollision/CollisionShapes/btCylinderShape.cpp
+++ b/src/BulletCollision/CollisionShapes/btCylinderShape.cpp
@@ -45,7 +45,7 @@ void btCylinderShape::getAabb(const btTransform& t,btVector3& aabbMin,btVector3&
 }


-inline btVector3 CylinderLocalSupportX(const btVector3& halfExtents,const btVector3& v) 
+SIMD_FORCE_INLINE btVector3 CylinderLocalSupportX(const btVector3& halfExtents,const btVector3& v) 
 {
 const int cylinderUpAxis = 0;
 const int XX = 1;
@@ -163,24 +163,24 @@ const int ZZ = 1;

 btVector3	btCylinderShapeX::localGetSupportingVertexWithoutMargin(const btVector3& vec)const
 {
-	return CylinderLocalSupportX(getHalfExtents(),vec);
+	return CylinderLocalSupportX(getHalfExtentsWithoutMargin(),vec);
 }


 btVector3	btCylinderShapeZ::localGetSupportingVertexWithoutMargin(const btVector3& vec)const
 {
-	return CylinderLocalSupportZ(getHalfExtents(),vec);
+	return CylinderLocalSupportZ(getHalfExtentsWithoutMargin(),vec);
 }
 btVector3	btCylinderShape::localGetSupportingVertexWithoutMargin(const btVector3& vec)const
 {
-	return CylinderLocalSupportY(getHalfExtents(),vec);
+	return CylinderLocalSupportY(getHalfExtentsWithoutMargin(),vec);
 }

 void	btCylinderShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const
 {
 	for (int i=0;i<numVectors;i++)
 	{
-		supportVerticesOut[i] = CylinderLocalSupportY(getHalfExtents(),vectors[i]);
+		supportVerticesOut[i] = CylinderLocalSupportY(getHalfExtentsWithoutMargin(),vectors[i]);
 	}
 }

@@ -188,7 +188,7 @@ void	btCylinderShapeZ::batchedUnitVectorGetSupportingVertexWithoutMargin(const b
 {
 	for (int i=0;i<numVectors;i++)
 	{
-		supportVerticesOut[i] = CylinderLocalSupportZ(getHalfExtents(),vectors[i]);
+		supportVerticesOut[i] = CylinderLocalSupportZ(getHalfExtentsWithoutMargin(),vectors[i]);
 	}
 }

@@ -199,7 +199,7 @@ void	btCylinderShapeX::batchedUnitVectorGetSupportingVertexWithoutMargin(const b
 {
 	for (int i=0;i<numVectors;i++)
 	{
-		supportVerticesOut[i] = CylinderLocalSupportX(getHalfExtents(),vectors[i]);
+		supportVerticesOut[i] = CylinderLocalSupportX(getHalfExtentsWithoutMargin(),vectors[i]);
 	}
 }

--- a/src/BulletCollision/CollisionShapes/btCylinderShape.h
+++ b/src/BulletCollision/CollisionShapes/btCylinderShape.h
@@ -74,7 +74,7 @@ public:

 	virtual btScalar getRadius() const
 	{
-		return getHalfExtents().getX();
+		return getHalfExtentsWithMargin().getX();
 	}

 	//debugging
@@ -103,7 +103,7 @@ public:

 	virtual btScalar getRadius() const
 	{
-		return getHalfExtents().getY();
+		return getHalfExtentsWithMargin().getY();
 	}

 };
@@ -128,7 +128,7 @@ public:

 	virtual btScalar getRadius() const
 	{
-		return getHalfExtents().getX();
+		return getHalfExtentsWithMargin().getX();
 	}

 };
--- a/src/BulletCollision/CollisionShapes/btOptimizedBvh.h
+++ b/src/BulletCollision/CollisionShapes/btOptimizedBvh.h
@@ -285,7 +285,7 @@ protected:
 #define USE_BANCHLESS 1
 #ifdef USE_BANCHLESS
 	//This block replaces the block below and uses no branches, and replaces the 8 bit return with a 32 bit return for improved performance (~3x on XBox 360)
-	inline unsigned testQuantizedAabbAgainstQuantizedAabb(unsigned short int* aabbMin1,unsigned short int* aabbMax1,const unsigned short int* aabbMin2,const unsigned short int* aabbMax2) const
+	SIMD_FORCE_INLINE unsigned testQuantizedAabbAgainstQuantizedAabb(unsigned short int* aabbMin1,unsigned short int* aabbMax1,const unsigned short int* aabbMin2,const unsigned short int* aabbMax2) const
 	{		
 		return btSelect((unsigned)((aabbMin1[0] <= aabbMax2[0]) & (aabbMax1[0] >= aabbMin2[0])
 			& (aabbMin1[2] <= aabbMax2[2]) & (aabbMax1[2] >= aabbMin2[2])
@@ -293,7 +293,7 @@ protected:
 			1, 0);
 	}
 #else
-	inline bool testQuantizedAabbAgainstQuantizedAabb(unsigned short int* aabbMin1,unsigned short int* aabbMax1,const unsigned short int* aabbMin2,const unsigned short int* aabbMax2) const
+	SIMD_FORCE_INLINE bool testQuantizedAabbAgainstQuantizedAabb(unsigned short int* aabbMin1,unsigned short int* aabbMax1,const unsigned short int* aabbMin2,const unsigned short int* aabbMax2) const
 	{
 		bool overlap = true;
 		overlap = (aabbMin1[0] > aabbMax2[0] || aabbMax1[0] < aabbMin2[0]) ? false : overlap;
@@ -316,7 +316,7 @@ public:

 	void	reportSphereOverlappingNodex(btNodeOverlapCallback* nodeCallback,const btVector3& aabbMin,const btVector3& aabbMax) const;
 	
-	inline void quantizeWithClamp(unsigned short* out, const btVector3& point) const
+	SIMD_FORCE_INLINE void quantizeWithClamp(unsigned short* out, const btVector3& point) const
 	{

 		btAssert(m_useQuantization);
@@ -347,12 +347,12 @@ public:
 	void	updateBvhNodes(btStridingMeshInterface* meshInterface,int firstNode,int endNode,int index);


-	inline QuantizedNodeArray&	getQuantizedNodeArray()
+	SIMD_FORCE_INLINE QuantizedNodeArray&	getQuantizedNodeArray()
 	{	
 		return	m_quantizedContiguousNodes;
 	}

-	inline BvhSubtreeInfoArray&	getSubtreeInfoArray()
+	SIMD_FORCE_INLINE BvhSubtreeInfoArray&	getSubtreeInfoArray()
 	{
 		return m_SubtreeHeaders;
 	}
@@ -368,7 +368,7 @@ public:

 	static unsigned int getAlignmentSerializationPadding();

-	inline bool isQuantized()
+	SIMD_FORCE_INLINE bool isQuantized()
 	{
 		return m_useQuantization;
 	}
--- a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp
+++ b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp
@@ -158,64 +158,62 @@ btScalar restitutionCurve(btScalar rel_vel, btScalar restitution)

 //velocity + friction
 //response  between two dynamic objects with friction
-SIMD_FORCE_INLINE btScalar resolveSingleCollisionCombinedCacheFriendly(
+//SIMD_FORCE_INLINE 
+btScalar resolveSingleCollisionCombinedCacheFriendly(
 	btSolverBody& body1,
 	btSolverBody& body2,
-	btSolverConstraint& contactConstraint,
+	const btSolverConstraint& contactConstraint,
 	const btContactSolverInfo& solverInfo)
 {
 	(void)solverInfo;

-	btScalar normalImpulse(0.f);
-	{
-		if (contactConstraint.m_penetration < 0.f)
-			return 0.f;
-
+	btScalar normalImpulse;
+	
 	//  Optimized version of projected relative velocity, use precomputed cross products with normal
 	//	body1.getVelocityInLocalPoint(contactConstraint.m_rel_posA,vel1);
 	//	body2.getVelocityInLocalPoint(contactConstraint.m_rel_posB,vel2);
 	//	btVector3 vel = vel1 - vel2;
 	//	btScalar  rel_vel = contactConstraint.m_contactNormal.dot(vel);

-		btScalar rel_vel;
-		btScalar vel1Dotn = contactConstraint.m_contactNormal.dot(body1.m_linearVelocity) 
-					+ contactConstraint.m_relpos1CrossNormal.dot(body1.m_angularVelocity);
-		btScalar vel2Dotn = contactConstraint.m_contactNormal.dot(body2.m_linearVelocity) 
-					+ contactConstraint.m_relpos2CrossNormal.dot(body2.m_angularVelocity);
+	btScalar rel_vel;
+	btScalar vel1Dotn = contactConstraint.m_contactNormal.dot(body1.m_linearVelocity) 
+				+ contactConstraint.m_relpos1CrossNormal.dot(body1.m_angularVelocity);
+	btScalar vel2Dotn = contactConstraint.m_contactNormal.dot(body2.m_linearVelocity) 
+				+ contactConstraint.m_relpos2CrossNormal.dot(body2.m_angularVelocity);

-		rel_vel = vel1Dotn-vel2Dotn;
+	rel_vel = vel1Dotn-vel2Dotn;


-		btScalar positionalError = contactConstraint.m_penetration;
-		btScalar velocityError = contactConstraint.m_restitution - rel_vel;// * damping;
+	btScalar positionalError = contactConstraint.m_penetration;
+	btScalar velocityError = contactConstraint.m_restitution - rel_vel;// * damping;

-		btScalar penetrationImpulse = positionalError * contactConstraint.m_jacDiagABInv;
-		btScalar	velocityImpulse = velocityError * contactConstraint.m_jacDiagABInv;
-		btScalar normalImpulse = penetrationImpulse+velocityImpulse;
-		
-		// See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse
-		btScalar oldNormalImpulse = contactConstraint.m_appliedImpulse;
-		btScalar sum = oldNormalImpulse + normalImpulse;
-		contactConstraint.m_appliedImpulse = btScalar(0.) > sum ? btScalar(0.): sum;
+	btScalar penetrationImpulse = positionalError * contactConstraint.m_jacDiagABInv;
+	btScalar	velocityImpulse = velocityError * contactConstraint.m_jacDiagABInv;
+	normalImpulse = penetrationImpulse+velocityImpulse;
+	
+	// See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse
+	btScalar oldNormalImpulse = contactConstraint.m_appliedImpulse;
+	btScalar sum = oldNormalImpulse + normalImpulse;
+	contactConstraint.m_appliedImpulse = btScalar(0.) > sum ? btScalar(0.): sum;

-		btScalar oldVelocityImpulse = contactConstraint.m_appliedVelocityImpulse;
-		btScalar velocitySum = oldVelocityImpulse + velocityImpulse;
-		contactConstraint.m_appliedVelocityImpulse = btScalar(0.) > velocitySum ? btScalar(0.): velocitySum;
+	btScalar oldVelocityImpulse = contactConstraint.m_appliedVelocityImpulse;
+	btScalar velocitySum = oldVelocityImpulse + velocityImpulse;
+	contactConstraint.m_appliedVelocityImpulse = btScalar(0.) > velocitySum ? btScalar(0.): velocitySum;

-		normalImpulse = contactConstraint.m_appliedImpulse - oldNormalImpulse;
-
-		if (body1.m_invMass)
-		{
-			body1.internalApplyImpulse(contactConstraint.m_contactNormal*body1.m_invMass,
-				contactConstraint.m_angularComponentA,normalImpulse);
-		}
-		if (body2.m_invMass)
-		{
-			body2.internalApplyImpulse(contactConstraint.m_contactNormal*body2.m_invMass,
-				contactConstraint.m_angularComponentB,-normalImpulse);
-		}
+	normalImpulse = contactConstraint.m_appliedImpulse - oldNormalImpulse;

+	if (body1.m_invMass)
+	{
+		body1.internalApplyImpulse(contactConstraint.m_contactNormal*body1.m_invMass,
+			contactConstraint.m_angularComponentA,normalImpulse);
 	}
+	if (body2.m_invMass)
+	{
+		body2.internalApplyImpulse(contactConstraint.m_contactNormal*body2.m_invMass,
+			contactConstraint.m_angularComponentB,-normalImpulse);
+	}
+
+

 	

@@ -225,10 +223,11 @@ SIMD_FORCE_INLINE btScalar resolveSingleCollisionCombinedCacheFriendly(

 #ifndef NO_FRICTION_TANGENTIALS

-SIMD_FORCE_INLINE btScalar resolveSingleFrictionCacheFriendly(
+//SIMD_FORCE_INLINE 
+btScalar resolveSingleFrictionCacheFriendly(
 	btSolverBody& body1,
 	btSolverBody& body2,
-	btSolverConstraint& contactConstraint,
+	const btSolverConstraint& contactConstraint,
 	const btContactSolverInfo& solverInfo,
 	btScalar appliedNormalImpulse)
 {
@@ -255,11 +254,36 @@ SIMD_FORCE_INLINE btScalar resolveSingleFrictionCacheFriendly(

 			// calculate j that moves us to zero relative velocity
 			j1 = -rel_vel * contactConstraint.m_jacDiagABInv;
+#define CLAMP_ACCUMULATED_FRICTION_IMPULSE 1
+#ifdef CLAMP_ACCUMULATED_FRICTION_IMPULSE
 			btScalar oldTangentImpulse = contactConstraint.m_appliedImpulse;
 			contactConstraint.m_appliedImpulse = oldTangentImpulse + j1;
-			GEN_set_min(contactConstraint.m_appliedImpulse, limit);
-			GEN_set_max(contactConstraint.m_appliedImpulse, -limit);
+			
+			if (limit < contactConstraint.m_appliedImpulse)
+			{
+				contactConstraint.m_appliedImpulse = limit;
+			} else
+			{
+				if (contactConstraint.m_appliedImpulse < -limit)
+					contactConstraint.m_appliedImpulse = -limit;
+			}
 			j1 = contactConstraint.m_appliedImpulse - oldTangentImpulse;
+#else
+			if (limit < j1)
+			{
+				j1 = limit;
+			} else
+			{
+				if (j1 < -limit)
+					j1 = -limit;
+			}
+
+#endif //CLAMP_ACCUMULATED_FRICTION_IMPULSE
+
+			//GEN_set_min(contactConstraint.m_appliedImpulse, limit);
+			//GEN_set_max(contactConstraint.m_appliedImpulse, -limit);
+
+			

 		}
 	
@@ -446,6 +470,8 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol

 		tmpSolverBodyPool.reserve(minReservation);

+		//don't convert all bodies, only the one we need so solver the constraints
+/*
 		{
 			for (int i=0;i<numBodies;i++)
 			{
@@ -460,7 +486,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
 				} 
 			}
 		}
-
+*/
 		
 		tmpSolverConstraintPool.reserve(minReservation);
 		tmpSolverFrictionConstraintPool.reserve(minReservation);
@@ -485,7 +511,17 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol

 					if (rb0->getIslandTag() >= 0)
 					{
-						solverBodyIdA = rb0->getCompanionId();
+						if (rb0->getCompanionId() >= 0)
+						{
+							//body has already been converted
+							solverBodyIdA = rb0->getCompanionId();
+						} else
+						{
+							solverBodyIdA = tmpSolverBodyPool.size();
+							btSolverBody& solverBody = tmpSolverBodyPool.expand();
+							initSolverBody(&solverBody,rb0);
+							rb0->setCompanionId(solverBodyIdA);
+						}
 					} else
 					{
 						//create a static body
@@ -496,7 +532,16 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol

 					if (rb1->getIslandTag() >= 0)
 					{
-						solverBodyIdB = rb1->getCompanionId();
+						if (rb1->getCompanionId() >= 0)
+						{
+							solverBodyIdB = rb1->getCompanionId();
+						} else
+						{
+							solverBodyIdB = tmpSolverBodyPool.size();
+							btSolverBody& solverBody = tmpSolverBodyPool.expand();
+							initSolverBody(&solverBody,rb1);
+							rb1->setCompanionId(solverBodyIdB);
+						}
 					} else
 					{
 						//create a static body
@@ -729,7 +774,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(
 				int numPoolConstraints = tmpSolverConstraintPool.size();
 				for (j=0;j<numPoolConstraints;j++)
 				{
-					btSolverConstraint& solveManifold = tmpSolverConstraintPool[gOrderTmpConstraintPool[j]];
+					const btSolverConstraint& solveManifold = tmpSolverConstraintPool[gOrderTmpConstraintPool[j]];
 					resolveSingleCollisionCombinedCacheFriendly(tmpSolverBodyPool[solveManifold.m_solverBodyIdA],
 						tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold,infoGlobal);
 				}
@@ -737,13 +782,15 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(

 			{
 				 int numFrictionPoolConstraints = tmpSolverFrictionConstraintPool.size();
-				for (j=0;j<numFrictionPoolConstraints;j++)
+				
+				 for (j=0;j<numFrictionPoolConstraints;j++)
 				{
-					btSolverConstraint& solveManifold = tmpSolverFrictionConstraintPool[gOrderFrictionConstraintPool[j]];
-						btScalar appliedNormalImpulse = tmpSolverConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
+					const btSolverConstraint& solveManifold = tmpSolverFrictionConstraintPool[gOrderFrictionConstraintPool[j]];
+						

 						resolveSingleFrictionCacheFriendly(tmpSolverBodyPool[solveManifold.m_solverBodyIdA],
-							tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold,infoGlobal,appliedNormalImpulse);
+							tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold,infoGlobal,
+							tmpSolverConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse);
 				}
 			}
 			
--- a/src/BulletDynamics/ConstraintSolver/btSolverBody.h
+++ b/src/BulletDynamics/ConstraintSolver/btSolverBody.h
@@ -27,22 +27,22 @@ class	btRigidBody;
 ATTRIBUTE_ALIGNED16 (struct)	btSolverBody
 {
 	BT_DECLARE_ALIGNED_ALLOCATOR();
-
-	btVector3		m_centerOfMassPosition;
-	btVector3		m_linearVelocity;
+	
 	btVector3		m_angularVelocity;
-	btRigidBody*	m_originalBody;
+	float			m_angularFactor;
 	float			m_invMass;
 	float			m_friction;
-	float			m_angularFactor;
-
-	inline void	getVelocityInLocalPoint(const btVector3& rel_pos, btVector3& velocity ) const
+	btRigidBody*	m_originalBody;
+	btVector3		m_linearVelocity;
+	btVector3		m_centerOfMassPosition;
+	
+	SIMD_FORCE_INLINE void	getVelocityInLocalPoint(const btVector3& rel_pos, btVector3& velocity ) const
 	{
 		velocity = m_linearVelocity + m_angularVelocity.cross(rel_pos);
 	}

 	//Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position
-	inline void internalApplyImpulse(const btVector3& linearComponent, const btVector3& angularComponent,btScalar impulseMagnitude)
+	SIMD_FORCE_INLINE void internalApplyImpulse(const btVector3& linearComponent, const btVector3& angularComponent,btScalar impulseMagnitude)
 	{
 		m_linearVelocity += linearComponent*impulseMagnitude;
 		m_angularVelocity += angularComponent*(impulseMagnitude*m_angularFactor);
@@ -54,6 +54,7 @@ ATTRIBUTE_ALIGNED16 (struct)	btSolverBody
 		{
 			m_originalBody->setLinearVelocity(m_linearVelocity);
 			m_originalBody->setAngularVelocity(m_angularVelocity);
+			//m_originalBody->setCompanionId(-1);
 		}
 	}

--- a/src/BulletDynamics/ConstraintSolver/btSolverConstraint.h
+++ b/src/BulletDynamics/ConstraintSolver/btSolverConstraint.h
@@ -30,20 +30,24 @@ ATTRIBUTE_ALIGNED16 (struct)	btSolverConstraint
 	BT_DECLARE_ALIGNED_ALLOCATOR();

 	btVector3	m_relpos1CrossNormal;
-	btVector3	m_relpos2CrossNormal;
 	btVector3	m_contactNormal;
-	btVector3	m_angularComponentA;
-	btVector3	m_angularComponentB;

-	btScalar	m_appliedVelocityImpulse;
+	btVector3	m_relpos2CrossNormal;
+	btVector3	m_angularComponentA;
+
+	btVector3	m_angularComponentB;
+	mutable btScalar	m_appliedVelocityImpulse;
+	mutable btScalar	m_appliedImpulse;
 	int			m_solverBodyIdA;
 	int			m_solverBodyIdB;
+	
 	btScalar	m_friction;
 	btScalar	m_restitution;
 	btScalar	m_jacDiagABInv;
 	btScalar	m_penetration;
-	btScalar	m_appliedImpulse;
+	

+	
 	int			m_constraintType;
 	int			m_frictionIndex;
 	int			m_unusedPadding[2];
--- a/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp
+++ b/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp
@@ -412,7 +412,7 @@ void	btDiscreteDynamicsWorld::removeVehicle(btRaycastVehicle* vehicle)
 	m_vehicles.remove(vehicle);
 }

-inline	int	btGetConstraintIslandId(const btTypedConstraint* lhs)
+SIMD_FORCE_INLINE	int	btGetConstraintIslandId(const btTypedConstraint* lhs)
 {
 	int islandId;
 	
@@ -866,10 +866,24 @@ void btDiscreteDynamicsWorld::debugDrawObject(const btTransform& worldTransform,
 				btScalar radius = coneShape->getRadius();//+coneShape->getMargin();
 				btScalar height = coneShape->getHeight();//+coneShape->getMargin();
 				btVector3 start = worldTransform.getOrigin();
-				getDebugDrawer()->drawLine(start+worldTransform.getBasis() * btVector3(btScalar(0.),btScalar(0.),btScalar(0.5)*height),start+worldTransform.getBasis() * btVector3(radius,btScalar(0.),btScalar(-0.5)*height),color);
-				getDebugDrawer()->drawLine(start+worldTransform.getBasis() * btVector3(btScalar(0.),btScalar(0.),btScalar(0.5)*height),start+worldTransform.getBasis() * btVector3(-radius,btScalar(0.),btScalar(-0.5)*height),color);
-				getDebugDrawer()->drawLine(start+worldTransform.getBasis() * btVector3(btScalar(0.),btScalar(0.),btScalar(0.5)*height),start+worldTransform.getBasis() * btVector3(btScalar(0.),radius,btScalar(-0.5)*height),color);
-				getDebugDrawer()->drawLine(start+worldTransform.getBasis() * btVector3(btScalar(0.),btScalar(0.),btScalar(0.5)*height),start+worldTransform.getBasis() * btVector3(btScalar(0.),-radius,btScalar(-0.5)*height),color);
+
+				int upAxis= coneShape->getConeUpIndex();
+				
+
+				btVector3	offsetHeight(0,0,0);
+				offsetHeight[upAxis] = height * btScalar(0.5);
+				btVector3	offsetRadius(0,0,0);
+				offsetRadius[(upAxis+1)%3] = radius;
+				btVector3	offset2Radius(0,0,0);
+				offset2Radius[(upAxis+2)%3] = radius;
+
+				getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight+offsetRadius),color);
+				getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight-offsetRadius),color);
+				getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight+offset2Radius),color);
+				getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight-offset2Radius),color);
+
+
+
 				break;

 			}
@@ -878,7 +892,7 @@ void btDiscreteDynamicsWorld::debugDrawObject(const btTransform& worldTransform,
 				const btCylinderShape* cylinder = static_cast<const btCylinderShape*>(shape);
 				int upAxis = cylinder->getUpAxis();
 				btScalar radius = cylinder->getRadius();
-				btScalar halfHeight = cylinder->getHalfExtents()[upAxis];
+				btScalar halfHeight = cylinder->getHalfExtentsWithMargin()[upAxis];
 				btVector3 start = worldTransform.getOrigin();
 				btVector3	offsetHeight(0,0,0);
 				offsetHeight[upAxis] = halfHeight;
--- a/src/BulletDynamics/Dynamics/btRigidBody.h
+++ b/src/BulletDynamics/Dynamics/btRigidBody.h
@@ -111,11 +111,11 @@ public:

 	void			setDamping(btScalar lin_damping, btScalar ang_damping);
 	
-	inline const btCollisionShape*	getCollisionShape() const {
+	SIMD_FORCE_INLINE const btCollisionShape*	getCollisionShape() const {
 		return m_collisionShape;
 	}

-	inline btCollisionShape*	getCollisionShape() {
+	SIMD_FORCE_INLINE btCollisionShape*	getCollisionShape() {
 			return m_collisionShape;
 	}
 	
@@ -185,7 +185,7 @@ public:
 	}

 	//Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position
-	inline void internalApplyImpulse(const btVector3& linearComponent, const btVector3& angularComponent,btScalar impulseMagnitude)
+	SIMD_FORCE_INLINE void internalApplyImpulse(const btVector3& linearComponent, const btVector3& angularComponent,btScalar impulseMagnitude)
 	{
 		if (m_inverseMass != btScalar(0.))
 		{
@@ -255,7 +255,7 @@ public:


 	
-	inline btScalar computeImpulseDenominator(const btPoint3& pos, const btVector3& normal) const
+	SIMD_FORCE_INLINE btScalar computeImpulseDenominator(const btPoint3& pos, const btVector3& normal) const
 	{
 		btVector3 r0 = pos - getCenterOfMassPosition();

@@ -267,13 +267,13 @@ public:

 	}

-	inline btScalar computeAngularImpulseDenominator(const btVector3& axis) const
+	SIMD_FORCE_INLINE btScalar computeAngularImpulseDenominator(const btVector3& axis) const
 	{
 		btVector3 vec = axis * getInvInertiaTensorWorld();
 		return axis.dot(vec);
 	}

-	inline void	updateDeactivation(btScalar timeStep)
+	SIMD_FORCE_INLINE void	updateDeactivation(btScalar timeStep)
 	{
 		if ( (getActivationState() == ISLAND_SLEEPING) || (getActivationState() == DISABLE_DEACTIVATION))
 			return;
@@ -290,7 +290,7 @@ public:

 	}

-	inline bool	wantsSleeping()
+	SIMD_FORCE_INLINE bool	wantsSleeping()
 	{

 		if (getActivationState() == DISABLE_DEACTIVATION)
--- a/src/LinearMath/btAlignedAllocator.cpp
+++ b/src/LinearMath/btAlignedAllocator.cpp
@@ -16,7 +16,7 @@ subject to the following restrictions:
 #include "btAlignedAllocator.h"


-#if defined (BT_HAS_ALIGNED_ALOCATOR)
+#if defined (BT_HAS_ALIGNED_ALLOCATOR)

 #include <malloc.h>
 void*	btAlignedAlloc	(size_t size, int alignment)
--- a/src/LinearMath/btScalar.h
+++ b/src/LinearMath/btScalar.h
@@ -33,7 +33,7 @@ subject to the following restrictions:
 			#define ATTRIBUTE_ALIGNED16(a) a
 			#define ATTRIBUTE_ALIGNED128(a) a
 		#else
-			#define BT_HAS_ALIGNED_ALOCATOR
+			#define BT_HAS_ALIGNED_ALLOCATOR
 			#pragma warning(disable:4530)
 			#pragma warning(disable:4996)
 			#pragma warning(disable:4786)
@@ -288,7 +288,7 @@ SIMD_FORCE_INLINE float btSelect(unsigned condition, float valueIfConditionNonZe
 #endif
 }

-template<typename T> inline void btSwap(T& a, T& b)
+template<typename T> SIMD_FORCE_INLINE void btSwap(T& a, T& b)
 {
 	T tmp = a;
 	a = b;
--- a/src/LinearMath/btStackAlloc.h
+++ b/src/LinearMath/btStackAlloc.h
@@ -77,7 +77,7 @@ public:
 		
 		return(0);
 	}
-	inline btBlock*		beginBlock()
+	SIMD_FORCE_INLINE btBlock*		beginBlock()
 	{
 		btBlock*	pb = (btBlock*)allocate(sizeof(btBlock));
 		pb->previous	=	current;
@@ -85,7 +85,7 @@ public:
 		current			=	pb;
 		return(pb);
 	}
-	inline void		endBlock(btBlock* block)
+	SIMD_FORCE_INLINE void		endBlock(btBlock* block)
 	{
 		btAssert(block==current);
 		//Raise(L"Unmatched blocks");
--- a/src/LinearMath/btTransformUtil.h
+++ b/src/LinearMath/btTransformUtil.h
@@ -25,7 +25,7 @@ subject to the following restrictions:

 #define btRecipSqrt(x) ((btScalar)(btScalar(1.0)/btSqrt(btScalar(x))))		/* reciprocal square root */

-inline btVector3 btAabbSupport(const btVector3& halfExtents,const btVector3& supportDir)
+SIMD_FORCE_INLINE btVector3 btAabbSupport(const btVector3& halfExtents,const btVector3& supportDir)
 {
 	return btVector3(supportDir.x() < btScalar(0.0) ? -halfExtents.x() : halfExtents.x(),
      supportDir.y() < btScalar(0.0) ? -halfExtents.y() : halfExtents.y(),
@@ -33,7 +33,7 @@ inline btVector3 btAabbSupport(const btVector3& halfExtents,const btVector3& sup
 }


-inline void btPlaneSpace1 (const btVector3& n, btVector3& p, btVector3& q)
+SIMD_FORCE_INLINE void btPlaneSpace1 (const btVector3& n, btVector3& p, btVector3& q)
 {
  if (btFabs(n.z()) > SIMDSQRT12) {
    // choose p in y-z plane