added parallel solver (work in progress) and made modifications to demo/constraints to allow for getting the type without using virtual functions (needed on CELL SPU processors)

Demos/CcdPhysicsDemo/CcdPhysicsDemo.cpp

@@ -26,6 +26,9 @@ subject to the following restrictions:
 //#define USE_CUSTOM_NEAR_CALLBACK 1
 //#define CENTER_OF_MASS_SHIFT 1
 
+//#define USE_PARALLEL_SOLVER 1 //experimental parallel solver
+//#define USE_PARALLEL_DISPATCHER 1
+
 //following define allows to compare/replace Bullet's constraint solver with ODE quickstep
 //this define requires to either add the libquickstep library (win32, see msvc/8/libquickstep.vcproj) or manually add the files from Extras/quickstep
 //#define COMPARE_WITH_QUICKSTEP 1
@@ -38,6 +41,15 @@ subject to the following restrictions:
 #endif //REGISTER_BOX_BOX
 #include "BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.h"
 
+#ifdef USE_PARALLEL_DISPATCHER
+#include "../../Extras/BulletMultiThreaded/SpuGatheringCollisionDispatcher.h"
+#include "../../Extras/BulletMultiThreaded/Win32ThreadSupport.h"
+#include "../../Extras/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuGatheringCollisionTask.h"
+#include "../../Extras/BulletMultiThreaded/SpuParallelSolver.h"
+#include "../../Extras/BulletMultiThreaded/SpuSolverTask/SpuParallellSolverTask.h"
+#endif//USE_PARALLEL_DISPATCHER
+
+
 #ifdef COMPARE_WITH_QUICKSTEP
 #include "../Extras/quickstep/OdeConstraintSolver.h"
 #endif //COMPARE_WITH_QUICKSTEP
@@ -351,7 +363,33 @@ void	CcdPhysicsDemo::initPhysics()
 	m_azi = 90.f;
 #endif //DO_BENCHMARK_PYRAMIDS
 
-	btCollisionDispatcher* dispatcher = new	btCollisionDispatcher();
+	btCollisionDispatcher* dispatcher=0;
+	
+	
+#ifdef USE_PARALLEL_DISPATCHER
+
+#ifdef USE_WIN32_THREADING
+
+int maxNumOutstandingTasks = 4;//number of maximum outstanding tasks
+	Win32ThreadSupport* threadSupportCollision = new Win32ThreadSupport(Win32ThreadSupport::Win32ThreadConstructionInfo(
+								"collision",
+								processCollisionTask,
+								createCollisionLocalStoreMemory,
+								maxNumOutstandingTasks));
+#else
+///todo other platform threading
+///Playstation 3 SPU (SPURS)  version is available through PS3 Devnet
+///Libspe2 SPU support will be available soon
+///pthreads version
+///you can hook it up to your custom task scheduler by deriving from btThreadSupportInterface
+#endif
+
+
+	dispatcher = new	SpuGatheringCollisionDispatcher(threadSupportCollision,maxNumOutstandingTasks);
+//	dispatcher = new	btCollisionDispatcher();
+#else
+	dispatcher = new	btCollisionDispatcher();
+#endif //USE_PARALLEL_DISPATCHER
 
 #ifdef USE_CUSTOM_NEAR_CALLBACK
 	//this is optional
@@ -379,11 +417,24 @@ void	CcdPhysicsDemo::initPhysics()
 #ifdef COMPARE_WITH_QUICKSTEP
 	btConstraintSolver* solver = new OdeConstraintSolver();
 #else
-	//default constraint solver
+
+	
+#ifdef USE_PARALLEL_SOLVER
+
+	Win32ThreadSupport* threadSupportSolver = new Win32ThreadSupport(Win32ThreadSupport::Win32ThreadConstructionInfo(
+								"solver",
+								processSolverTask,
+								createSolverLocalStoreMemory,
+								maxNumOutstandingTasks));
+
+	btConstraintSolver* solver = new btParallelSequentialImpulseSolver(threadSupportSolver,maxNumOutstandingTasks);
+#else
 	btSequentialImpulseConstraintSolver* solver = new btSequentialImpulseConstraintSolver;
 	//default solverMode is SOLVER_RANDMIZE_ORDER. Warmstarting seems not to improve convergence, see 
 	//solver->setSolverMode(btSequentialImpulseConstraintSolver::SOLVER_USE_WARMSTARTING | btSequentialImpulseConstraintSolver::SOLVER_RANDMIZE_ORDER);
 	
+#endif //USE_PARALLEL_SOLVER
+
 #endif
 		
 #ifdef	USER_DEFINED_FRICTION_MODEL
@@ -392,6 +443,7 @@ void	CcdPhysicsDemo::initPhysics()
 #endif //USER_DEFINED_FRICTION_MODEL
 
 		m_dynamicsWorld = new btDiscreteDynamicsWorld(dispatcher,broadphase,solver);
+		m_dynamicsWorld->getDispatchInfo().m_enableSPU = true;
 		m_dynamicsWorld->setGravity(btVector3(0,-10,0));
 
 		m_dynamicsWorld->setDebugDrawer(&debugDrawer);

Extras/BulletMultiThreaded/SpuParallelSolver.cpp

@@ -0,0 +1,568 @@
+/*
+Bullet Continuous Collision Detection and Physics Library - Parallel solver
+Copyright (c) 2007 Starbreeze Studios
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+
+Written by: Marten Svanfeldt
+*/
+
+#include "SpuParallelSolver.h"
+
+//#include "SpuFakeDma.h"
+#include "SpuSync.h"
+
+#include "LinearMath/btVector3.h"
+#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
+#include "BulletDynamics/Dynamics/btRigidBody.h"
+#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h"
+#include "LinearMath/btMinMax.h"
+#include "BulletCollision/CollisionShapes/btCollisionShape.h"
+#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
+#include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"
+
+#include "SpuSolverTask/SpuParallellSolverTask.h"
+
+#include <stdio.h>
+
+enum
+{
+	PARALLEL_SOLVER_BODIES_PER_TASK = 64,
+	PARALLEL_SOLVER_CELLS_PER_TASK = SPU_HASH_NUMCELLS >> 3
+};
+
+
+//-- Hash handling
+static void recordDependency(SpuSolverHash* hash, unsigned int i, unsigned int j)
+{
+	hash->m_dependencyMatrix[i][j >> 5] |= (1 << (j & 31));
+	hash->m_dependencyMatrix[j][i >> 5] |= (1 << (i & 31));
+}
+
+
+// Clear the given hash
+static void clearHash (SpuSolverHash* hash)
+{
+	size_t hashSize = sizeof(SpuSolverHash);
+	memset(hash, 0, hashSize);
+
+	// Setup basic dependency
+	for (int i = 0; i < SPU_HASH_NUMCELLS; ++i)
+	{
+		hash->m_dependencyMatrix[i][i >> 5] |= (1 << (i & 31));
+	}
+
+	// Set some ones to "unused cells"
+	for (int i = SPU_HASH_WORDWIDTH-SPU_HASH_NUMUNUSEDBITS; i < SPU_HASH_WORDWIDTH; ++i)
+	{
+		hash->m_currentMask[0][SPU_HASH_NUMCELLDWORDS-1] |= (1 << i);
+	}
+}
+
+static bool getDependency(SpuSolverHash* hash, unsigned int i, unsigned int j)
+{
+	return (hash->m_dependencyMatrix[i][j >> 5] & (1 << (j & 31))) != 0;
+}
+
+static unsigned int getObjectIndex (btCollisionObject* object)
+{
+	btVector3 center = object->getWorldTransform().getOrigin();
+	int cx = (int)floorf(center.x() / SPU_HASH_PHYSSIZE);
+	int cy = (int)floorf(center.y() / SPU_HASH_PHYSSIZE);
+	int cz = (int)floorf(center.z() / SPU_HASH_PHYSSIZE);
+
+	return spuGetHashCellIndex(cx, cy, cz);
+};
+
+
+
+
+
+btParallelSequentialImpulseSolver::btParallelSequentialImpulseSolver (btThreadSupportInterface* threadIf, int maxOutstandingTasks)
+: m_numberOfContacts(0), m_taskScheduler (threadIf, maxOutstandingTasks)
+{
+	m_solverHash = new SpuSolverHash;
+	clearHash(m_solverHash);
+}
+
+btParallelSequentialImpulseSolver::~btParallelSequentialImpulseSolver ()
+{
+	delete m_solverHash;
+}
+
+
+void btParallelSequentialImpulseSolver::prepareSolve(int numBodies, int numManifolds)
+{
+	m_sortedManifolds.reserve(numManifolds);
+	m_allObjects.reserve(numBodies);
+}
+
+btScalar btParallelSequentialImpulseSolver::solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints, const btContactSolverInfo& info,class btIDebugDraw* debugDrawer, btStackAlloc* stackAlloc)
+{
+	if (!numManifolds && !numConstraints)
+		return 0;
+
+	for (int i = 0; i < numManifolds; ++i)
+	{
+		btPersistentManifold* currManifold = manifold[i];
+		btRigidBody* rb0 = (btRigidBody*)currManifold->getBody0();
+		btRigidBody* rb1 = (btRigidBody*)currManifold->getBody1();
+
+		currManifold->refreshContactPoints(rb0->getCenterOfMassTransform(),rb1->getCenterOfMassTransform());
+	}
+
+	// Record and mark the manifolds to the cells
+	for (int i = 0; i < numManifolds; ++i)
+	{
+		// Compute a hash cell for this manifold
+		btPersistentManifold* currManifold = manifold[i];
+
+		btCollisionObject *ownerObject, *otherObject;
+
+		btRigidBody* rb0 = (btRigidBody*)currManifold->getBody0();
+		btRigidBody* rb1 = (btRigidBody*)currManifold->getBody1();
+
+		if (rb0->getIslandTag() >= 0)
+		{
+			ownerObject = rb0;
+			otherObject = rb1;
+		}
+		else
+		{
+			ownerObject = rb1;
+			otherObject = rb0;
+		}
+
+		// Save the cell
+		unsigned int ownerCellIdx = getObjectIndex(ownerObject);
+		ManifoldCellHolder holder = {ownerCellIdx, currManifold};
+		m_sortedManifolds.push_back(holder);
+		m_solverHash->m_Hash[ownerCellIdx].m_numManifolds++;
+
+		// Record dependency
+		if (rb0->getIslandTag() >= 0 && rb1->getIslandTag() >= 0)
+		{
+			unsigned int otherCellIdx = getObjectIndex(otherObject);
+			recordDependency(m_solverHash, ownerCellIdx, otherCellIdx);
+		}
+		
+		// Save statistics
+		int numContacts = currManifold->getNumContacts();
+		m_solverHash->m_Hash[ownerCellIdx].m_numContacts += numContacts;
+		m_numberOfContacts += numContacts;
+	}
+
+	// Record and mark constraints to the cells
+	for (int i = 0; i < numConstraints; ++i)
+	{
+		// Compute a hash cell for this manifold
+		btTypedConstraint* currConstraint = constraints[i];
+
+		if (!constraintTypeSupported(currConstraint->getConstraintType()))
+			continue;
+
+		btCollisionObject *ownerObject, *otherObject;
+
+		btRigidBody* rb0 = &currConstraint->getRigidBodyA();
+		btRigidBody* rb1 = &currConstraint->getRigidBodyB();
+
+		if (rb0->getIslandTag() >= 0)
+		{
+			ownerObject = rb0;
+			otherObject = rb1;
+		}
+		else
+		{
+			ownerObject = rb1;
+			otherObject = rb0;
+		}
+
+		// Save the cell
+		unsigned int ownerCellIdx = getObjectIndex(ownerObject);
+		ConstraintCellHolder holder = {ownerCellIdx, currConstraint->getConstraintType(), currConstraint};
+		m_sortedConstraints.push_back(holder);
+		m_solverHash->m_Hash[ownerCellIdx].m_numConstraints++;
+
+		// Record dependency
+		if (rb0 && rb1 && rb0->getIslandTag() >= 0 && rb1->getIslandTag() >= 0)
+		{
+			unsigned int otherCellIdx = getObjectIndex(otherObject);
+			recordDependency(m_solverHash, ownerCellIdx, otherCellIdx);
+		}
+	}
+
+	// Save all RBs
+	for (int i = 0; i < numBodies; ++i)
+	{
+		btCollisionObject* obj = bodies[i];
+		//unsigned int cellIdx = getObjectIndex(obj);
+
+		btRigidBody* rb = btRigidBody::upcast(obj);
+		m_allObjects.push_back(rb);
+	}
+
+	return 0;
+}
+
+template<typename T>
+class CellHolderPredicate
+{
+public:
+	SIMD_FORCE_INLINE bool operator() ( const T& lhs, const T& rhs )
+	{
+		return lhs.m_hashCellIndex < rhs.m_hashCellIndex;
+	}
+};
+
+
+static void printDependencyMatrix(SpuSolverHash* hash)
+{
+	for (int r = 0; r < SPU_HASH_NUMCELLS; ++r)
+	{
+		for (int c = 0; c < SPU_HASH_NUMCELLS; ++c)
+		{
+			if (getDependency(hash, r, c))
+			{
+				printf("1");
+			}
+			else
+			{
+				printf("0");
+			}
+		}
+
+		printf("\n");
+	}
+	printf("\n");
+	fflush(stdout);
+}
+
+// Solver caches
+btAlignedObjectArray<SpuSolverBody> solverBodyPool_persist;
+btAlignedObjectArray<uint32_t> solverBodyOffsetList_persist;
+btAlignedObjectArray<SpuSolverInternalConstraint> solverInternalConstraintPool_persist;
+btAlignedObjectArray<SpuSolverConstraint> solverConstraintPool_persist;
+
+
+void btParallelSequentialImpulseSolver::allSolved (const btContactSolverInfo& info,class btIDebugDraw* debugDrawer, btStackAlloc* stackAlloc)
+{
+	if (!m_numberOfContacts && !m_sortedConstraints.size())
+	{
+		m_sortedManifolds.clear();
+		m_sortedConstraints.clear();
+		m_allObjects.clear();
+		clearHash(m_solverHash);
+		return;
+	}
+
+
+	//printDependencyMatrix(m_solverHash);
+
+	// Sort the manifolds list
+	int numManifolds = m_sortedManifolds.size();
+	m_sortedManifolds.heapSort(CellHolderPredicate<ManifoldCellHolder>());
+
+	// Sort the constraint list
+	int numConstraints = m_sortedConstraints.size();
+	m_sortedConstraints.heapSort(CellHolderPredicate<ConstraintCellHolder>());
+
+
+	// Sort the body list
+	int numBodies = m_allObjects.size();
+	
+	// Reassign the hash offset
+	uint32_t emptyCellMask[SPU_HASH_NUMCELLDWORDS] = {0};
+	int numBodyOffsets = 0;
+	{
+		int manifoldRunner = 0;
+		int bodyOffsetRunner = 0;
+		int internalConstraintRunner = 0;
+		int constraintRunner = 0;
+		
+		for (int i = 0; i < SPU_HASH_NUMCELLS; ++i)
+		{
+			bool empty = true;
+
+			SpuSolverHashCell& hashCell = m_solverHash->m_Hash[i];
+			hashCell.m_solverBodyOffsetListOffset = bodyOffsetRunner;
+
+			if (hashCell.m_numManifolds)
+			{
+				hashCell.m_manifoldListOffset = manifoldRunner;
+				manifoldRunner += hashCell.m_numManifolds;
+				
+				bodyOffsetRunner += hashCell.m_numManifolds*2;
+			}			
+			if (hashCell.m_numContacts)
+			{
+				hashCell.m_internalConstraintListOffset = internalConstraintRunner*3;
+				internalConstraintRunner += hashCell.m_numContacts;
+				empty = false;
+			}
+
+			if (hashCell.m_numConstraints)
+			{
+				hashCell.m_constraintListOffset = constraintRunner;
+				constraintRunner += hashCell.m_numConstraints;
+
+				bodyOffsetRunner += hashCell.m_numConstraints*2;
+
+				empty = false;
+			}
+			
+
+			emptyCellMask[i >> 5] |= (empty ? (1 << (i&31)) : 0);
+			// Align the bodyOffsetRunner to a whole number of 4 for right alignment in the list
+			bodyOffsetRunner = (bodyOffsetRunner+3)&~0x3;
+		}
+
+		numBodyOffsets = bodyOffsetRunner;
+	}
+
+	// Setup rigid bodies
+	// Allocate temporary data
+	solverBodyPool_persist.resize(numBodies + numManifolds + numConstraints);
+	SpuSolverBody* solverBodyPool = &solverBodyPool_persist[0];
+
+	solverBodyOffsetList_persist.resize(numBodyOffsets);
+	uint32_t* solverBodyOffsetList = &solverBodyOffsetList_persist[0];
+
+	solverInternalConstraintPool_persist.resize(m_numberOfContacts*3);
+	SpuSolverInternalConstraint* solverInternalConstraintPool = &solverInternalConstraintPool_persist[0];
+	
+	solverConstraintPool_persist.resize(numConstraints);
+	SpuSolverConstraint* solverConstraintPool = &solverConstraintPool_persist[0];
+
+	// Setup all the moving rigid bodies
+	{
+		int bodiesPerTask = PARALLEL_SOLVER_BODIES_PER_TASK;
+		int bodiesToSchedule = numBodies;
+		int startBody = 0;
+
+		while (bodiesToSchedule > 0)
+		{
+			// Schedule a bunch of hash cells
+			int numBodiesInTask = bodiesToSchedule > bodiesPerTask ? bodiesPerTask : bodiesToSchedule;
+
+			SpuSolverTaskDesc* desc = m_taskScheduler.getTask();
+
+			desc->m_solverCommand = CMD_SOLVER_SETUP_BODIES;
+			desc->m_solverData.m_solverHash = m_solverHash;
+			desc->m_solverData.m_solverBodyList = solverBodyPool;
+
+			desc->m_commandData.m_bodySetup.m_startBody = startBody;
+			desc->m_commandData.m_bodySetup.m_numBodies = numBodiesInTask;
+			desc->m_commandData.m_bodySetup.m_rbList = &m_allObjects[0];
+
+			m_taskScheduler.issueTask();
+			bodiesToSchedule -= numBodiesInTask;
+			startBody += numBodiesInTask;
+		}
+		
+		m_taskScheduler.flushTasks();
+	}
+
+	// Manifold setup
+	{
+		int cellsPerTask = PARALLEL_SOLVER_CELLS_PER_TASK;
+		int cellsToSchedule = SPU_HASH_NUMCELLS;
+		int startCell = 0;
+
+		while (cellsToSchedule > 0)
+		{
+			int numCellsInTask = cellsToSchedule > cellsPerTask ? cellsPerTask : cellsToSchedule;
+			
+			SpuSolverTaskDesc* desc = m_taskScheduler.getTask();
+
+			desc->m_solverCommand = CMD_SOLVER_MANIFOLD_SETUP;
+			desc->m_solverData.m_solverHash = m_solverHash;
+			desc->m_solverData.m_solverBodyList = solverBodyPool;
+			desc->m_solverData.m_solverBodyOffsetList = solverBodyOffsetList;
+			desc->m_solverData.m_solverInternalConstraintList = solverInternalConstraintPool;
+			desc->m_solverData.m_solverConstraintList = solverConstraintPool;
+
+			desc->m_commandData.m_manifoldSetup.m_startCell = startCell;
+			desc->m_commandData.m_manifoldSetup.m_numCells = numCellsInTask;
+			desc->m_commandData.m_manifoldSetup.m_numBodies = numBodies;
+			desc->m_commandData.m_manifoldSetup.m_numManifolds = numManifolds;
+			desc->m_commandData.m_manifoldSetup.m_manifoldHolders = &m_sortedManifolds[0];
+			desc->m_commandData.m_manifoldSetup.m_constraintHolders = &m_sortedConstraints[0];
+			desc->m_commandData.m_manifoldSetup.m_solverInfo = info;
+
+			m_taskScheduler.issueTask();
+			cellsToSchedule -= numCellsInTask;
+			startCell += numCellsInTask;
+		}
+		m_taskScheduler.flushTasks();
+	}
+
+	btSpinlock::SpinVariable* spinVar = (btSpinlock::SpinVariable*)btAlignedAlloc(sizeof(btSpinlock::SpinVariable), 128);
+	for (int iter = 0; iter < info.m_numIterations; ++iter)
+	{
+		btSpinlock lock (spinVar);
+		lock.Init();
+
+		// Clear the "processed cells" part of the hash
+		memcpy(m_solverHash->m_currentMask[0], emptyCellMask, sizeof(uint32_t)*SPU_HASH_NUMCELLDWORDS);
+
+		for (int task = 0; task < m_taskScheduler.getMaxOutstandingTasks(); ++task)
+		{
+			SpuSolverTaskDesc* desc = m_taskScheduler.getTask();
+			desc->m_solverCommand = CMD_SOLVER_SOLVE_ITERATE;
+
+			desc->m_solverData.m_solverHash = m_solverHash;
+			desc->m_solverData.m_solverBodyList = solverBodyPool;
+			desc->m_solverData.m_solverBodyOffsetList = solverBodyOffsetList;
+			desc->m_solverData.m_solverInternalConstraintList = solverInternalConstraintPool;
+			desc->m_solverData.m_solverConstraintList = solverConstraintPool;
+
+			desc->m_commandData.m_iterate.m_spinLockVar = spinVar;
+
+			m_taskScheduler.issueTask();
+		} 
+		m_taskScheduler.flushTasks();		
+	}
+	btAlignedFree((void*)spinVar);
+	
+	// Write back velocity
+	{
+		int bodiesPerTask = PARALLEL_SOLVER_BODIES_PER_TASK;
+		int bodiesToSchedule = numBodies;
+		int startBody = 0;
+
+		while (bodiesToSchedule > 0)
+		{
+			// Schedule a bunch of hash cells
+			int numBodiesInTask = bodiesToSchedule > bodiesPerTask ? bodiesPerTask : bodiesToSchedule;
+
+			SpuSolverTaskDesc* desc = m_taskScheduler.getTask();
+
+			desc->m_solverCommand = CMD_SOLVER_COPYBACK_BODIES;
+			desc->m_solverData.m_solverHash = m_solverHash;
+			desc->m_solverData.m_solverBodyList = solverBodyPool;
+
+			desc->m_commandData.m_bodyCopyback.m_startBody = startBody;
+			desc->m_commandData.m_bodyCopyback.m_numBodies = numBodiesInTask;
+			desc->m_commandData.m_bodyCopyback.m_rbList = &m_allObjects[0];
+
+			m_taskScheduler.issueTask();
+			bodiesToSchedule -= numBodiesInTask;
+			startBody += numBodiesInTask;
+		}
+
+		m_taskScheduler.flushTasks();
+	}
+
+
+	// Clean up
+	m_sortedManifolds.resize(0);
+	m_sortedConstraints.resize(0);
+	m_allObjects.resize(0);
+	clearHash(m_solverHash);
+
+
+	m_numberOfContacts = 0;
+}
+
+void btParallelSequentialImpulseSolver::reset()
+{
+	m_sortedManifolds.clear();
+	m_allObjects.clear();
+	m_numberOfContacts = 0;
+	clearHash(m_solverHash);
+
+	solverBodyPool_persist.clear();
+	solverBodyOffsetList_persist.clear();
+	solverConstraintPool_persist.clear();
+	solverInternalConstraintPool_persist.clear();
+}
+
+
+SolverTaskScheduler::SolverTaskScheduler(btThreadSupportInterface* threadIf, int maxOutstandingTasks)
+: m_threadInterface (threadIf), m_maxNumOutstandingTasks (maxOutstandingTasks > SPU_MAX_SPUS ? SPU_MAX_SPUS : maxOutstandingTasks), 
+m_currentTask (0), m_numBusyTasks (0)
+{
+	m_taskDescriptors.resize(m_maxNumOutstandingTasks);
+	m_taskBusy.resize(m_maxNumOutstandingTasks);
+
+	m_threadInterface->startSPU();
+}
+
+
+SolverTaskScheduler::~SolverTaskScheduler()
+{
+	m_threadInterface->stopSPU();
+}
+
+SpuSolverTaskDesc* SolverTaskScheduler::getTask()
+{
+	int taskIdx = -1;
+
+	if (m_taskBusy[m_currentTask])
+	{
+		//try to find a new one
+		for (unsigned int i = 0; i < m_maxNumOutstandingTasks; ++i)
+		{
+			if (!m_taskBusy[i])
+			{
+				taskIdx = i;
+				break;
+			}
+		}
+
+		if (taskIdx < 0)
+		{
+			// Have to wait
+			unsigned int taskId;
+			unsigned int outputSize;
+
+			m_threadInterface->waitForResponse(&taskId, &outputSize);
+
+			m_taskBusy[taskId] = false;
+			m_numBusyTasks--;
+
+			taskIdx = taskId;
+		}
+
+		m_currentTask = taskIdx;
+	}
+
+
+	SpuSolverTaskDesc* result = &m_taskDescriptors[m_currentTask];
+	memset(result, 0, sizeof(SpuSolverTaskDesc));
+	result->m_taskId = m_currentTask;
+
+	return result;
+}
+
+void SolverTaskScheduler::issueTask()
+{
+	m_taskBusy[m_currentTask] = true;
+	m_numBusyTasks++;
+
+	SpuSolverTaskDesc& desc = m_taskDescriptors[m_currentTask];
+	
+	m_threadInterface->sendRequest(1, (uint32_t)&desc, m_currentTask);
+}
+
+void SolverTaskScheduler::flushTasks()
+{
+	while (m_numBusyTasks > 0)
+	{
+		unsigned int taskId;
+		unsigned int outputSize;
+
+		m_threadInterface->waitForResponse(&taskId, &outputSize);
+
+		m_taskBusy[taskId] = false;
+		m_numBusyTasks--;
+	}
+}

Extras/BulletMultiThreaded/SpuParallelSolver.h

@@ -0,0 +1,75 @@
+/*
+Bullet Continuous Collision Detection and Physics Library - Parallel solver
+Copyright (c) 2007 Starbreeze Studios
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+
+Written by: Marten Svanfeldt
+*/
+
+#ifndef SPU_PARALLELSOLVER_H
+#define SPU_PARALLELSOLVER_H
+
+#include "BulletDynamics/ConstraintSolver/btConstraintSolver.h"
+#include "btThreadSupportInterface.h"
+#include "LinearMath/btAlignedObjectArray.h"
+
+class SolverTaskScheduler
+{
+protected:
+	class	btThreadSupportInterface*	m_threadInterface;
+	unsigned int						m_maxNumOutstandingTasks;
+
+	unsigned int						m_currentTask;
+	unsigned int						m_numBusyTasks;
+
+	btAlignedObjectArray<struct SpuSolverTaskDesc>	m_taskDescriptors;
+	btAlignedObjectArray<bool>						m_taskBusy;
+
+public:
+	SolverTaskScheduler (btThreadSupportInterface* threadIf, int maxOutstandingTasks);
+	~SolverTaskScheduler ();
+
+	struct SpuSolverTaskDesc* getTask ();
+
+	void issueTask();
+	void flushTasks();
+
+	unsigned int getMaxOutstandingTasks()
+	{
+		return m_maxNumOutstandingTasks;
+	}
+};
+
+class btParallelSequentialImpulseSolver : public btConstraintSolver
+{
+protected:
+
+	struct SpuSolverHash*						m_solverHash;
+	btAlignedObjectArray<struct ManifoldCellHolder>				m_sortedManifolds;
+	btAlignedObjectArray<struct ConstraintCellHolder>				m_sortedConstraints;
+	btAlignedObjectArray<class btRigidBody*>	m_allObjects;
+
+	int											m_numberOfContacts;	
+
+	SolverTaskScheduler							m_taskScheduler;
+
+public:
+	btParallelSequentialImpulseSolver (btThreadSupportInterface* threadIf, int maxOutstandingTasks);
+	virtual ~btParallelSequentialImpulseSolver();
+
+	virtual void prepareSolve (int numBodies, int numManifolds);
+	virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints, const btContactSolverInfo& info,class btIDebugDraw* debugDrawer, btStackAlloc* stackAlloc);
+	virtual void allSolved (const btContactSolverInfo& info,class btIDebugDraw* debugDrawer, btStackAlloc* stackAlloc);
+	virtual void reset ();
+};
+
+#endif

Extras/BulletMultiThreaded/SpuSync.h

@@ -0,0 +1,112 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2007 Starbreeze Studios
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+
+Written by: Marten Svanfeldt
+*/
+
+#ifndef SPU_SYNC_H
+#define	SPU_SYNC_H
+
+
+#include "PlatformDefinitions.h"
+
+
+#if defined(WIN32)
+
+#define WIN32_LEAN_AND_MEAN
+#ifdef _XBOX
+#include <Xtl.h>
+#else
+#include <Windows.h>
+#endif
+
+class btSpinlock
+{
+public:
+	//typedef volatile LONG SpinVariable;
+	typedef CRITICAL_SECTION SpinVariable;
+
+	btSpinlock (SpinVariable* var)
+		: spinVariable (var)
+	{}
+
+	void Init ()
+	{
+		//*spinVariable = 0;
+		InitializeCriticalSection(spinVariable);
+	}
+
+	void Lock ()
+	{
+		EnterCriticalSection(spinVariable);
+	}
+
+	void Unlock ()
+	{
+		LeaveCriticalSection(spinVariable);
+	}
+
+private:
+	SpinVariable* spinVariable;
+};
+
+
+#elif defined (__CELLOS_LV2__)
+
+//#include <cell/atomic.h>
+#include <cell/sync/mutex.h>
+
+class btSpinlock
+{
+public:
+	typedef CellSyncMutex SpinVariable;
+
+	btSpinlock (SpinVariable* var)
+		: spinVariable (var)
+	{}
+#ifndef SPU
+	void Init ()
+	{
+		//*spinVariable = 1;
+		cellSyncMutexInitialize(spinVariable);
+	}
+#endif
+
+#ifdef SPU
+	void Lock ()
+	{
+		// lock semaphore
+		/*while (cellAtomicTestAndDecr32(atomic_buf, (uint64_t)spinVariable) == 0) 
+		{
+
+		};*/
+		cellSyncMutexLock((uint64_t)spinVariable);
+	}
+
+	void Unlock ()
+	{
+		//cellAtomicIncr32(atomic_buf, (uint64_t)spinVariable);
+		cellSyncMutexUnlock((uint64_t)spinVariable);
+	}
+#endif
+
+private:
+	SpinVariable*	spinVariable;
+	uint32_t		atomic_buf[32] __attribute__((aligned(128)));
+};
+
+#endif
+
+
+#endif

src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp

@@ -23,13 +23,14 @@ Written by: Marcus Hennix
 #include <new>
 
 btConeTwistConstraint::btConeTwistConstraint()
+:btTypedConstraint(CONETWIST_CONSTRAINT_TYPE)
 {
 }
 
 
 btConeTwistConstraint::btConeTwistConstraint(btRigidBody& rbA,btRigidBody& rbB, 
 											 const btTransform& rbAFrame,const btTransform& rbBFrame)
-											 :btTypedConstraint(rbA,rbB),m_rbAFrame(rbAFrame),m_rbBFrame(rbBFrame),
+											 :btTypedConstraint(CONETWIST_CONSTRAINT_TYPE, rbA,rbB),m_rbAFrame(rbAFrame),m_rbBFrame(rbBFrame),
 											 m_angularOnly(false)
 {
 	// flip axis for correct angles
@@ -49,7 +50,7 @@ btConeTwistConstraint::btConeTwistConstraint(btRigidBody& rbA,btRigidBody& rbB,
 }
 
 btConeTwistConstraint::btConeTwistConstraint(btRigidBody& rbA,const btTransform& rbAFrame)
-											:btTypedConstraint(rbA),m_rbAFrame(rbAFrame),
+											:btTypedConstraint(CONETWIST_CONSTRAINT_TYPE,rbA),m_rbAFrame(rbAFrame),
 											 m_angularOnly(false)
 {
 	m_rbBFrame = m_rbAFrame;

src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h

@@ -30,6 +30,9 @@ class btRigidBody;
 ///btConeTwistConstraint can be used to simulate ragdoll joints (upper arm, leg etc)
 class btConeTwistConstraint : public btTypedConstraint
 {
+#ifdef IN_PARALLELL_SOLVER
+public:
+#endif
 	btJacobianEntry	m_jac[3]; //3 orthogonal linear constraints
 
 	btTransform m_rbAFrame; 

src/BulletDynamics/ConstraintSolver/btConstraintSolver.h

@@ -35,12 +35,15 @@ public:
 
 	virtual ~btConstraintSolver() {}
 	
+	virtual void prepareSolve (int numBodies, int numManifolds) {;}
+
 	///solve a group of constraints
 	virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints, const btContactSolverInfo& info,class btIDebugDraw* debugDrawer, btStackAlloc* stackAlloc) = 0;
 
+	virtual void allSolved (const btContactSolverInfo& info,class btIDebugDraw* debugDrawer, btStackAlloc* stackAlloc) {;}
+
 	///clear internal cached data and reset random seed
 	virtual	void	reset() = 0;
-
 };
 
 

src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h

@@ -16,8 +16,21 @@ subject to the following restrictions:
 #ifndef CONTACT_SOLVER_INFO
 #define CONTACT_SOLVER_INFO
 
+struct btContactSolverInfoData
+{
+	btScalar	m_tau;
+	btScalar	m_damping;
+	btScalar	m_friction;
+	btScalar	m_timeStep;
+	btScalar	m_restitution;
+	int		m_numIterations;
+	btScalar	m_maxErrorReduction;
+	btScalar	m_sor;
+	btScalar	m_erp;
 
-struct btContactSolverInfo
+};
+
+struct btContactSolverInfo : public btContactSolverInfoData
 {
 
 	inline btContactSolverInfo()
@@ -32,16 +45,7 @@ struct btContactSolverInfo
 		m_sor = btScalar(1.3);
 	}
 
-	btScalar	m_tau;
-	btScalar	m_damping;
-	btScalar	m_friction;
-	btScalar	m_timeStep;
-	btScalar	m_restitution;
-	int		m_numIterations;
-	btScalar	m_maxErrorReduction;
-	btScalar	m_sor;
-	btScalar	m_erp;
-
+	
 };
 
 #endif //CONTACT_SOLVER_INFO

src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp

@@ -25,11 +25,12 @@ static const int kAxisB[] = { 2, 2, 1 };
 #define GENERIC_D6_DISABLE_WARMSTARTING 1
 
 btGeneric6DofConstraint::btGeneric6DofConstraint()
+:btTypedConstraint(D6_CONSTRAINT_TYPE)
 {
 }
 
 btGeneric6DofConstraint::btGeneric6DofConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB)
-: btTypedConstraint(rbA, rbB)
+: btTypedConstraint(D6_CONSTRAINT_TYPE, rbA, rbB)
 , m_frameInA(frameInA)
 , m_frameInB(frameInB)
 {

src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp

@@ -21,14 +21,15 @@ subject to the following restrictions:
 #include <new>
 
 
-btHingeConstraint::btHingeConstraint():
+btHingeConstraint::btHingeConstraint()
+: btTypedConstraint (HINGE_CONSTRAINT_TYPE),
 m_enableAngularMotor(false)
 {
 }
 
 btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB,
 									 btVector3& axisInA,btVector3& axisInB)
-									 :btTypedConstraint(rbA,rbB),
+									 :btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA,rbB),
 									 m_angularOnly(false),
 									 m_enableAngularMotor(false)
 {
@@ -70,7 +71,7 @@ btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const bt
 
 
 btHingeConstraint::btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,btVector3& axisInA)
-:btTypedConstraint(rbA), m_angularOnly(false), m_enableAngularMotor(false)
+:btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA), m_angularOnly(false), m_enableAngularMotor(false)
 {
 
 	// since no frame is given, assume this to be zero angle and just pick rb transform axis
@@ -113,7 +114,7 @@ btHingeConstraint::btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,
 
 btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, 
 								     const btTransform& rbAFrame, const btTransform& rbBFrame)
-:btTypedConstraint(rbA,rbB),m_rbAFrame(rbAFrame),m_rbBFrame(rbBFrame),
+:btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA,rbB),m_rbAFrame(rbAFrame),m_rbBFrame(rbBFrame),
 m_angularOnly(false),
 m_enableAngularMotor(false)
 {
@@ -134,7 +135,7 @@ m_enableAngularMotor(false)
 
 
 btHingeConstraint::btHingeConstraint(btRigidBody& rbA, const btTransform& rbAFrame)
-:btTypedConstraint(rbA),m_rbAFrame(rbAFrame),m_rbBFrame(rbAFrame),
+:btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA),m_rbAFrame(rbAFrame),m_rbBFrame(rbAFrame),
 m_angularOnly(false),
 m_enableAngularMotor(false)
 {

src/BulletDynamics/ConstraintSolver/btHingeConstraint.h

@@ -28,6 +28,9 @@ class btRigidBody;
 /// axis defines the orientation of the hinge axis
 class btHingeConstraint : public btTypedConstraint
 {
+#ifdef IN_PARALLELL_SOLVER
+public:
+#endif
 	btJacobianEntry	m_jac[3]; //3 orthogonal linear constraints
 	btJacobianEntry	m_jacAng[3]; //2 orthogonal angular constraints+ 1 for limit/motor
 

src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp

@@ -21,18 +21,19 @@ subject to the following restrictions:
 
 
 btPoint2PointConstraint::btPoint2PointConstraint()
+:btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE)
 {
 }
 
 btPoint2PointConstraint::btPoint2PointConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB)
-:btTypedConstraint(rbA,rbB),m_pivotInA(pivotInA),m_pivotInB(pivotInB)
+:btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE,rbA,rbB),m_pivotInA(pivotInA),m_pivotInB(pivotInB)
 {
 
 }
 
 
 btPoint2PointConstraint::btPoint2PointConstraint(btRigidBody& rbA,const btVector3& pivotInA)
-:btTypedConstraint(rbA),m_pivotInA(pivotInA),m_pivotInB(rbA.getCenterOfMassTransform()(pivotInA))
+:btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE,rbA),m_pivotInA(pivotInA),m_pivotInB(rbA.getCenterOfMassTransform()(pivotInA))
 {
 	
 }

src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h

@@ -36,6 +36,9 @@ struct	btConstraintSetting
 /// point to point constraint between two rigidbodies each with a pivotpoint that descibes the 'ballsocket' location in local space
 class btPoint2PointConstraint : public btTypedConstraint
 {
+#ifdef IN_PARALLELL_SOLVER
+public:
+#endif
 	btJacobianEntry	m_jac[3]; //3 orthogonal linear constraints
 	
 	btVector3	m_pivotInA;

src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h

@@ -105,7 +105,9 @@ public:
 
 };
 
-
+#ifndef BT_PREFER_SIMD
+typedef btSequentialImpulseConstraintSolver btSequentialImpulseConstraintSolverPrefered;
+#endif
 
 
 #endif //SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_H

src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp

@@ -19,8 +19,9 @@ subject to the following restrictions:
 
 static btRigidBody s_fixed(0, 0,0);
 
-btTypedConstraint::btTypedConstraint()
-: m_userConstraintType(-1),
+btTypedConstraint::btTypedConstraint(btTypedConstraintType type)
+: m_constraintType (type),
+m_userConstraintType(-1),
 m_userConstraintId(-1),
 m_rbA(s_fixed),
 m_rbB(s_fixed),
@@ -28,8 +29,9 @@ m_appliedImpulse(btScalar(0.))
 {
 	s_fixed.setMassProps(btScalar(0.),btVector3(btScalar(0.),btScalar(0.),btScalar(0.)));
 }
-btTypedConstraint::btTypedConstraint(btRigidBody& rbA)
-: m_userConstraintType(-1),
+btTypedConstraint::btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA)
+: m_constraintType (type),
+m_userConstraintType(-1),
 m_userConstraintId(-1),
 m_rbA(rbA),
 m_rbB(s_fixed),
@@ -40,8 +42,9 @@ m_appliedImpulse(btScalar(0.))
 }
 
 
-btTypedConstraint::btTypedConstraint(btRigidBody& rbA,btRigidBody& rbB)
-: m_userConstraintType(-1),
+btTypedConstraint::btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA,btRigidBody& rbB)
+: m_constraintType (type),
+m_userConstraintType(-1),
 m_userConstraintId(-1),
 m_rbA(rbA),
 m_rbB(rbB),

src/BulletDynamics/ConstraintSolver/btTypedConstraint.h

@@ -19,12 +19,23 @@ subject to the following restrictions:
 class btRigidBody;
 #include "LinearMath/btScalar.h"
 
+enum btTypedConstraintType
+{
+	POINT2POINT_CONSTRAINT_TYPE,
+	HINGE_CONSTRAINT_TYPE,
+	CONETWIST_CONSTRAINT_TYPE,
+	D6_CONSTRAINT_TYPE,
+	VEHICLE_CONSTRAINT_TYPE
+};
+
 ///TypedConstraint is the baseclass for Bullet constraints and vehicles
 class btTypedConstraint
 {
 	int	m_userConstraintType;
 	int	m_userConstraintId;
 
+	btTypedConstraintType m_constraintType;
+
 	btTypedConstraint&	operator=(btTypedConstraint&	other)
 	{
 		btAssert(0);
@@ -40,11 +51,11 @@ protected:
 
 public:
 
-	btTypedConstraint();
+	btTypedConstraint(btTypedConstraintType type);
 	virtual ~btTypedConstraint() {};
-	btTypedConstraint(btRigidBody& rbA);
+	btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA);
 
-	btTypedConstraint(btRigidBody& rbA,btRigidBody& rbB);
+	btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA,btRigidBody& rbB);
 
 	virtual void	buildJacobian() = 0;
 
@@ -59,7 +70,7 @@ public:
 		return m_rbB;
 	}
 
-	btRigidBody& getRigidBodyA()
+	btRigidBody& getRigidBodyA() 
 	{
 		return m_rbA;
 	}
@@ -83,14 +94,19 @@ public:
 		m_userConstraintId = uid;
 	}
 	
-	int getUserConstraintId()
+	int getUserConstraintId() const
 	{
 		return m_userConstraintId;
 	}
-	btScalar	getAppliedImpulse()
+	btScalar	getAppliedImpulse() const
 	{
 		return m_appliedImpulse;
 	}
+
+	btTypedConstraintType getConstraintType () const
+	{
+		return m_constraintType;
+	}
 };
 
 #endif //TYPED_CONSTRAINT_H

src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp

@@ -74,8 +74,9 @@ int		btSimpleDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps, b
 		
 		btContactSolverInfo infoGlobal;
 		infoGlobal.m_timeStep = timeStep;
-		
+		m_constraintSolver->prepareSolve(0,numManifolds);
 		m_constraintSolver->solveGroup(0,0,manifoldPtr, numManifolds,0,0,infoGlobal,m_debugDrawer, m_stackAlloc);
+		m_constraintSolver->allSolved(infoGlobal,m_debugDrawer, m_stackAlloc);
 	}
 
 	///integrate transforms
@@ -210,7 +211,7 @@ void	btSimpleDynamicsWorld::setConstraintSolver(btConstraintSolver* solver)
 	m_constraintSolver = solver;
 }
 
-btConstraintSolver* btSimpleDynamicsWorld::getConstraintSolver()
-{
-	return m_constraintSolver;
-}
+btConstraintSolver* btSimpleDynamicsWorld::getConstraintSolver()
+{
+	return m_constraintSolver;
+}

src/BulletDynamics/Vehicle/btRaycastVehicle.cpp

@@ -28,7 +28,8 @@
 static btRigidBody s_fixedObject( 0,0,0);
 
 btRaycastVehicle::btRaycastVehicle(const btVehicleTuning& tuning,btRigidBody* chassis,	btVehicleRaycaster* raycaster )
-:m_vehicleRaycaster(raycaster),
+: btTypedConstraint(VEHICLE_CONSTRAINT_TYPE),
+m_vehicleRaycaster(raycaster),
 m_pitchControl(btScalar(0.))
 {
 	m_chassisBody = chassis;

src/LinearMath/btQuadWord.h

@@ -19,16 +19,19 @@ subject to the following restrictions:
 #include "btScalar.h"
 #include "btSimdMinMax.h"
 
+class btQuadWordStorage
+{
+protected:
+	btScalar	m_x;
+	btScalar	m_y;
+	btScalar	m_z;
+	btScalar	m_unusedW;
+};
+
 
 ///btQuadWord is base-class for vectors, points
-class	btQuadWord
+class	btQuadWord : public btQuadWordStorage
 {
-	protected:
-		btScalar	m_x;
-		btScalar	m_y;
-		btScalar	m_z;
-		btScalar	m_unusedW;
-
 	public:
 	
 //		SIMD_FORCE_INLINE btScalar&       operator[](int i)       { return (&m_x)[i];	}      
@@ -88,16 +91,19 @@ class	btQuadWord
 		{
 		}
 
-		SIMD_FORCE_INLINE btQuadWord(const btScalar& x, const btScalar& y, const btScalar& z) 
-		:m_x(x),m_y(y),m_z(z)
-		//todo, remove this in release/simd ?
-		,m_unusedW(btScalar(0.))
+		SIMD_FORCE_INLINE btQuadWord(const btQuadWordStorage& q)
 		{
+			*((btQuadWordStorage*)this) = q;
+		}
+
+		SIMD_FORCE_INLINE btQuadWord(const btScalar& x, const btScalar& y, const btScalar& z)		
+		{
+			m_x = x, m_y = y, m_z = z, m_unusedW = 0.0f;
 		}
 
 		SIMD_FORCE_INLINE btQuadWord(const btScalar& x, const btScalar& y, const btScalar& z,const btScalar& w) 
-			:m_x(x),m_y(y),m_z(z),m_unusedW(w)
 		{
+			m_x = x, m_y = y, m_z = z, m_unusedW = w;
 		}
 
 

src/LinearMath/btQuaternion.h

@@ -285,7 +285,7 @@ slerp(const btQuaternion& q1, const btQuaternion& q2, const btScalar& t)
 }
 
 SIMD_FORCE_INLINE btVector3 
-quatRotate(btQuaternion& rotation, btVector3& v) 
+quatRotate(const btQuaternion& rotation, const btVector3& v) 
 {
 	btQuaternion q = rotation * v;
 	q *= rotation.inverse();
@@ -293,7 +293,7 @@ quatRotate(btQuaternion& rotation, btVector3& v)
 }
 
 SIMD_FORCE_INLINE btQuaternion 
-shortestArcQuat(btVector3& v0,btVector3& v1) // Game Programming Gems 2.10. make sure v0,v1 are normalized
+shortestArcQuat(const btVector3& v0, const btVector3& v1) // Game Programming Gems 2.10. make sure v0,v1 are normalized
 {
 	btVector3 c = v0.cross(v1);
 	btScalar  d = v0.dot(v1);
@@ -308,7 +308,7 @@ shortestArcQuat(btVector3& v0,btVector3& v1) // Game Programming Gems 2.10. make
 }
 
 SIMD_FORCE_INLINE btQuaternion 
-shortestArcQuatNormalize(btVector3& v0,btVector3& v1)
+shortestArcQuatNormalize(btVector3 v0,btVector3 v1)
 {
 	v0.normalize();
 	v1.normalize();

src/LinearMath/btVector3.h

@@ -27,6 +27,10 @@ class	btVector3 : public btQuadWord {
 public:
 	SIMD_FORCE_INLINE btVector3() {}
 
+	SIMD_FORCE_INLINE btVector3(const btQuadWordStorage& q) 
+		: btQuadWord(q)
+	{
+	}
 	
 
 	SIMD_FORCE_INLINE btVector3(const btScalar& x, const btScalar& y, const btScalar& z)