Index: /code/branches/physics/src/bullet/BulletMultiThreaded/SpuBatchRaycaster.cpp
===================================================================
--- /code/branches/physics/src/bullet/BulletMultiThreaded/SpuBatchRaycaster.cpp	(revision 1966)
+++ /code/branches/physics/src/bullet/BulletMultiThreaded/SpuBatchRaycaster.cpp	(revision 1966)
@@ -0,0 +1,151 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2007 Erwin Coumans  http://bulletphysics.com
+
+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.
+*/
+
+#include <new>
+#include "BulletCollision/CollisionShapes/btCollisionShape.h"
+#include "LinearMath/btAlignedAllocator.h"
+#include "SpuBatchRaycaster.h"
+
+SpuBatchRaycaster::SpuBatchRaycaster (class	btThreadSupportInterface* threadInterface, int maxNumOutstandingTasks)
+{
+	m_threadInterface = threadInterface;
+
+	castUponObjectWrappers = NULL;
+	numCastUponObjectWrappers = 0;
+
+	m_spuRaycastTaskProcess = new SpuRaycastTaskProcess(m_threadInterface,maxNumOutstandingTasks); // FIXME non constant
+}
+
+SpuBatchRaycaster::~SpuBatchRaycaster ()
+{
+	if (castUponObjectWrappers)
+	{
+		btAlignedFree (castUponObjectWrappers);
+		castUponObjectWrappers = NULL;
+	}
+}
+
+void
+SpuBatchRaycaster::setCollisionObjects (btCollisionObjectArray& castUponObjects, int numCastUponObjects)
+{
+	if (castUponObjectWrappers)
+	{
+		btAlignedFree (castUponObjectWrappers);
+		castUponObjectWrappers = NULL;
+	}
+
+	castUponObjectWrappers = (SpuCollisionObjectWrapper*)btAlignedAlloc (sizeof(SpuCollisionObjectWrapper) * numCastUponObjects,16);
+	numCastUponObjectWrappers = numCastUponObjects;
+
+	for (int i = 0; i < numCastUponObjectWrappers; i++)
+	{
+		castUponObjectWrappers[i] = SpuCollisionObjectWrapper(castUponObjects[i]);
+	}
+}
+
+void
+SpuBatchRaycaster::setCollisionObjectsSkipPE (btCollisionObjectArray& castUponObjects, int numCastUponObjects)
+{
+	if (castUponObjectWrappers)
+	{
+		btAlignedFree (castUponObjectWrappers);
+		castUponObjectWrappers = NULL;
+	}
+
+	int numNonPEShapes = 0;
+	for (int i = 0; i < numCastUponObjects; i++)
+	{
+		const btCollisionShape* shape = castUponObjects[i]->getCollisionShape();
+
+		if (shape->getShapeType () == BOX_SHAPE_PROXYTYPE ||
+			shape->getShapeType () == SPHERE_SHAPE_PROXYTYPE ||
+			shape->getShapeType () == CAPSULE_SHAPE_PROXYTYPE)
+		{
+			continue;
+		}
+
+		numNonPEShapes++;
+	}
+
+	castUponObjectWrappers = (SpuCollisionObjectWrapper*)btAlignedAlloc (sizeof(SpuCollisionObjectWrapper) * numNonPEShapes,16);
+	numCastUponObjectWrappers = numNonPEShapes;
+
+	int index = 0;
+	for (int i = 0; i < numCastUponObjects; i++)
+	{
+		const btCollisionShape* shape = castUponObjects[i]->getCollisionShape();
+
+		if (shape->getShapeType () == BOX_SHAPE_PROXYTYPE ||
+			shape->getShapeType () == SPHERE_SHAPE_PROXYTYPE ||
+			shape->getShapeType () == CAPSULE_SHAPE_PROXYTYPE)
+		{
+			continue;
+		}
+
+		castUponObjectWrappers[index] = SpuCollisionObjectWrapper(castUponObjects[i]);
+		index++;
+	}
+
+//	printf("Number of shapes bullet is casting against: %d\n", numNonPEShapes);
+	btAssert (index == numNonPEShapes);
+}
+
+void
+SpuBatchRaycaster::addRay (const btVector3& rayFrom, const btVector3& rayTo, const btScalar hitFraction)
+{
+	SpuRaycastTaskWorkUnitOut workUnitOut;
+	workUnitOut.hitFraction = hitFraction;
+	workUnitOut.hitNormal = btVector3(0.0, 1.0, 0.0);
+
+	rayBatchOutput.push_back (workUnitOut);
+
+	SpuRaycastTaskWorkUnit workUnit;
+	workUnit.rayFrom = rayFrom;
+	workUnit.rayTo = rayTo;
+	rayBatch.push_back (workUnit);
+}
+
+void
+SpuBatchRaycaster::clearRays ()
+{
+	rayBatch.clear ();
+	rayBatchOutput.clear ();
+}
+
+void
+SpuBatchRaycaster::performBatchRaycast ()
+{
+	m_spuRaycastTaskProcess->initialize2 (castUponObjectWrappers, numCastUponObjectWrappers);
+
+	for (int i = 0; i < rayBatch.size(); i++)
+	{
+		rayBatch[i].output = &rayBatchOutput[i]; // assign output memory location
+		m_spuRaycastTaskProcess->addWorkToTask(rayBatch[i]);
+	}
+
+	m_spuRaycastTaskProcess->flush2 ();
+}
+
+const SpuRaycastTaskWorkUnitOut&
+SpuBatchRaycaster::operator [] (int i) const
+{
+	return rayBatchOutput[i];
+}
+
+int
+SpuBatchRaycaster::getNumRays () const
+{
+	return rayBatchOutput.size();
+}
Index: /code/branches/physics/src/bullet/BulletMultiThreaded/SpuBatchRaycaster.h
===================================================================
--- /code/branches/physics/src/bullet/BulletMultiThreaded/SpuBatchRaycaster.h	(revision 1966)
+++ /code/branches/physics/src/bullet/BulletMultiThreaded/SpuBatchRaycaster.h	(revision 1966)
@@ -0,0 +1,49 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2007 Erwin Coumans  http://bulletphysics.com
+
+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.
+*/
+
+#ifndef SPU_BATCH_RAYCASTER_H
+#define SPU_BATCH_RAYCASTER_H
+
+#include "LinearMath/btAlignedObjectArray.h"
+#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
+#include "SpuRaycastTaskProcess.h"
+#include "SpuRaycastTask/SpuRaycastTask.h"
+#include "SpuCollisionObjectWrapper.h"
+
+/* FIXME:
+ * Need to decide how callbacks are performed...
+ */
+class SpuBatchRaycaster 
+{
+protected:
+	SpuCollisionObjectWrapper* castUponObjectWrappers;
+	int numCastUponObjectWrappers;
+	btAlignedObjectArray<SpuRaycastTaskWorkUnit> rayBatch;
+	btAlignedObjectArray<SpuRaycastTaskWorkUnitOut> rayBatchOutput;
+	SpuRaycastTaskProcess* m_spuRaycastTaskProcess;
+	class	btThreadSupportInterface*	m_threadInterface;
+public:
+	SpuBatchRaycaster (class btThreadSupportInterface* threadInterface, int maxNumOutstandingTasks);
+	~SpuBatchRaycaster ();
+	void setCollisionObjects (btCollisionObjectArray& castUponObjects, int numCastUponObjects);
+	void setCollisionObjectsSkipPE (btCollisionObjectArray& castUponObjects, int numCastUponObjects);
+	void addRay (const btVector3& rayFrom, const btVector3& rayTo, const btScalar hitFraction = 1.0);
+	void clearRays ();
+	void performBatchRaycast ();
+	const SpuRaycastTaskWorkUnitOut& operator [] (int i) const;
+	int getNumRays () const;
+};
+
+#endif
Index: /code/branches/physics/src/bullet/BulletMultiThreaded/SpuCollisionObjectWrapper.cpp
===================================================================
--- /code/branches/physics/src/bullet/BulletMultiThreaded/SpuCollisionObjectWrapper.cpp	(revision 1966)
+++ /code/branches/physics/src/bullet/BulletMultiThreaded/SpuCollisionObjectWrapper.cpp	(revision 1966)
@@ -0,0 +1,48 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2007 Erwin Coumans  http://bulletphysics.com
+
+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.
+*/
+
+#include "SpuCollisionObjectWrapper.h"
+#include "BulletCollision/CollisionShapes/btCollisionShape.h"
+
+SpuCollisionObjectWrapper::SpuCollisionObjectWrapper ()
+{
+}
+
+#ifndef __SPU__
+SpuCollisionObjectWrapper::SpuCollisionObjectWrapper (const btCollisionObject* collisionObject)
+{
+	m_shapeType = collisionObject->getCollisionShape()->getShapeType ();
+	m_collisionObjectPtr = (ppu_address_t)collisionObject;
+	m_margin = collisionObject->getCollisionShape()->getMargin ();
+}
+#endif
+
+int
+SpuCollisionObjectWrapper::getShapeType () const
+{
+	return m_shapeType;
+}
+
+float
+SpuCollisionObjectWrapper::getCollisionMargin () const
+{
+	return m_margin;
+}
+
+ppu_address_t
+SpuCollisionObjectWrapper::getCollisionObjectPtr () const
+{
+	return m_collisionObjectPtr;
+}
Index: /code/branches/physics/src/bullet/BulletMultiThreaded/SpuCollisionObjectWrapper.h
===================================================================
--- /code/branches/physics/src/bullet/BulletMultiThreaded/SpuCollisionObjectWrapper.h	(revision 1966)
+++ /code/branches/physics/src/bullet/BulletMultiThreaded/SpuCollisionObjectWrapper.h	(revision 1966)
@@ -0,0 +1,35 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2007 Erwin Coumans  http://bulletphysics.com
+
+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.
+*/
+
+#include "PlatformDefinitions.h"
+#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
+
+ATTRIBUTE_ALIGNED16(class) SpuCollisionObjectWrapper
+{
+protected:
+	int m_shapeType;
+	float m_margin;
+	ppu_address_t m_collisionObjectPtr;
+
+public:
+	SpuCollisionObjectWrapper ();
+
+	SpuCollisionObjectWrapper (const btCollisionObject* collisionObject);
+
+	int           getShapeType () const;
+	float         getCollisionMargin () const;
+	ppu_address_t getCollisionObjectPtr () const;
+};
+
Index: /code/branches/physics/src/bullet/BulletMultiThreaded/SpuCollisionTaskProcess.cpp
===================================================================
--- /code/branches/physics/src/bullet/BulletMultiThreaded/SpuCollisionTaskProcess.cpp	(revision 1966)
+++ /code/branches/physics/src/bullet/BulletMultiThreaded/SpuCollisionTaskProcess.cpp	(revision 1966)
@@ -0,0 +1,304 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2007 Erwin Coumans  http://bulletphysics.com
+
+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.
+*/
+
+
+//#define DEBUG_SPU_TASK_SCHEDULING 1
+
+
+//class OptimizedBvhNode;
+
+#include "SpuCollisionTaskProcess.h"
+
+
+
+
+
+
+SpuCollisionTaskProcess::SpuCollisionTaskProcess(class	btThreadSupportInterface*	threadInterface, unsigned int	maxNumOutstandingTasks)
+:m_threadInterface(threadInterface),
+m_maxNumOutstandingTasks(maxNumOutstandingTasks)
+{
+	m_workUnitTaskBuffers = (unsigned char *)0;
+	m_taskBusy.resize(m_maxNumOutstandingTasks);
+	m_spuGatherTaskDesc.resize(m_maxNumOutstandingTasks);
+
+	for (int i = 0; i < m_maxNumOutstandingTasks; i++)
+	{
+		m_taskBusy[i] = false;
+	}
+	m_numBusyTasks = 0;
+	m_currentTask = 0;
+	m_currentPage = 0;
+	m_currentPageEntry = 0;
+
+#ifdef DEBUG_SpuCollisionTaskProcess
+	m_initialized = false;
+#endif
+
+	m_threadInterface->startSPU();
+
+	//printf("sizeof vec_float4: %d\n", sizeof(vec_float4));
+	printf("sizeof SpuGatherAndProcessWorkUnitInput: %d\n", sizeof(SpuGatherAndProcessWorkUnitInput));
+
+}
+
+SpuCollisionTaskProcess::~SpuCollisionTaskProcess()
+{
+	
+	if (m_workUnitTaskBuffers != 0)
+	{
+		btAlignedFree(m_workUnitTaskBuffers);
+		m_workUnitTaskBuffers = 0;
+	}
+	
+
+
+	m_threadInterface->stopSPU();
+	
+}
+
+
+
+void SpuCollisionTaskProcess::initialize2(bool useEpa)
+{
+
+#ifdef DEBUG_SPU_TASK_SCHEDULING
+	printf("SpuCollisionTaskProcess::initialize()\n");
+#endif //DEBUG_SPU_TASK_SCHEDULING
+	if (!m_workUnitTaskBuffers)
+	{
+		m_workUnitTaskBuffers = (unsigned char *)btAlignedAlloc(MIDPHASE_WORKUNIT_TASK_SIZE*m_maxNumOutstandingTasks, 128);
+	}
+
+	
+	for (int i = 0; i < m_maxNumOutstandingTasks; i++)
+	{
+		m_taskBusy[i] = false;
+	}
+	m_numBusyTasks = 0;
+	m_currentTask = 0;
+	m_currentPage = 0;
+	m_currentPageEntry = 0;
+	m_useEpa = useEpa;
+
+#ifdef DEBUG_SpuCollisionTaskProcess
+	m_initialized = true;
+	assert(MIDPHASE_NUM_WORKUNITS_PER_TASK*sizeof(SpuGatherAndProcessWorkUnitInput) <= MIDPHASE_WORKUNIT_TASK_SIZE);
+#endif
+}
+
+
+void SpuCollisionTaskProcess::issueTask2()
+{
+
+#ifdef DEBUG_SPU_TASK_SCHEDULING
+	printf("SpuCollisionTaskProcess::issueTask (m_currentTask= %d\n)", m_currentTask);
+#endif //DEBUG_SPU_TASK_SCHEDULING
+
+	m_taskBusy[m_currentTask] = true;
+	m_numBusyTasks++;
+
+
+	SpuGatherAndProcessPairsTaskDesc& taskDesc = m_spuGatherTaskDesc[m_currentTask];
+	taskDesc.m_useEpa = m_useEpa;
+
+	{
+		// send task description in event message
+		// no error checking here...
+		// but, currently, event queue can be no larger than NUM_WORKUNIT_TASKS.
+	
+		taskDesc.inPtr = reinterpret_cast<uint64_t>(MIDPHASE_TASK_PTR(m_currentTask));
+	
+		taskDesc.taskId = m_currentTask;
+		taskDesc.numPages = m_currentPage+1;
+		taskDesc.numOnLastPage = m_currentPageEntry;
+	}
+
+
+
+	m_threadInterface->sendRequest(CMD_GATHER_AND_PROCESS_PAIRLIST, (uint32_t) &taskDesc,m_currentTask);
+
+	// if all tasks busy, wait for spu event to clear the task.
+	
+
+	if (m_numBusyTasks >= m_maxNumOutstandingTasks)
+	{
+		unsigned int taskId;
+		unsigned int outputSize;
+
+		
+		for (int i=0;i<m_maxNumOutstandingTasks;i++)
+		  {
+			  if (m_taskBusy[i])
+			  {
+				  taskId = i;
+				  break;
+			  }
+		  }
+
+	  btAssert(taskId>=0);
+
+	  
+		m_threadInterface->waitForResponse(&taskId, &outputSize);
+
+//		printf("issueTask taskId %d completed, numBusy=%d\n",taskId,m_numBusyTasks);
+
+		//printf("PPU: after issue, received event: %u %d\n", taskId, outputSize);
+
+		//postProcess(taskId, outputSize);
+
+		m_taskBusy[taskId] = false;
+
+		m_numBusyTasks--;
+	}
+	
+}
+
+void SpuCollisionTaskProcess::addWorkToTask(void* pairArrayPtr,int startIndex,int endIndex)
+{
+#ifdef DEBUG_SPU_TASK_SCHEDULING
+	printf("#");
+#endif //DEBUG_SPU_TASK_SCHEDULING
+	
+#ifdef DEBUG_SpuCollisionTaskProcess
+	assert(m_initialized);
+	assert(m_workUnitTaskBuffers);
+
+#endif
+
+	bool batch = true;
+
+	if (batch)
+	{
+		if (m_currentPageEntry == MIDPHASE_NUM_WORKUNITS_PER_PAGE)
+		{
+			if (m_currentPage == MIDPHASE_NUM_WORKUNIT_PAGES-1)
+			{
+				// task buffer is full, issue current task.
+				// if all task buffers busy, this waits until SPU is done.
+				issueTask2();
+
+				// find new task buffer
+				for (unsigned int i = 0; i < m_maxNumOutstandingTasks; i++)
+				{
+					if (!m_taskBusy[i])
+					{
+						m_currentTask = i;
+						//init the task data
+
+						break;
+					}
+				}
+
+				m_currentPage = 0;
+			}
+			else
+			{
+				m_currentPage++;
+			}
+
+			m_currentPageEntry = 0;
+		}
+	}
+
+	{
+
+
+
+		SpuGatherAndProcessWorkUnitInput &wuInput = 
+			*(reinterpret_cast<SpuGatherAndProcessWorkUnitInput*>
+			(MIDPHASE_ENTRY_PTR(m_currentTask, m_currentPage, m_currentPageEntry)));
+		
+		wuInput.m_pairArrayPtr = reinterpret_cast<uint64_t>(pairArrayPtr);
+		wuInput.m_startIndex = startIndex;
+		wuInput.m_endIndex = endIndex;
+
+		
+	
+		m_currentPageEntry++;
+
+		if (!batch)
+		{
+			issueTask2();
+
+			// find new task buffer
+			for (unsigned int i = 0; i < m_maxNumOutstandingTasks; i++)
+			{
+				if (!m_taskBusy[i])
+				{
+					m_currentTask = i;
+					//init the task data
+
+					break;
+				}
+			}
+
+			m_currentPage = 0;
+			m_currentPageEntry =0;
+		}
+	}
+}
+
+
+void 
+SpuCollisionTaskProcess::flush2()
+{
+#ifdef DEBUG_SPU_TASK_SCHEDULING
+	printf("\nSpuCollisionTaskProcess::flush()\n");
+#endif //DEBUG_SPU_TASK_SCHEDULING
+	
+	// if there's a partially filled task buffer, submit that task
+	if (m_currentPage > 0 || m_currentPageEntry > 0)
+	{
+		issueTask2();
+	}
+
+
+	// all tasks are issued, wait for all tasks to be complete
+	while(m_numBusyTasks > 0)
+	{
+	  // Consolidating SPU code
+	  unsigned int taskId=-1;
+	  unsigned int outputSize;
+	  
+	  for (int i=0;i<m_maxNumOutstandingTasks;i++)
+	  {
+		  if (m_taskBusy[i])
+		  {
+			  taskId = i;
+			  break;
+		  }
+	  }
+
+	  btAssert(taskId>=0);
+
+	
+	  {
+			
+		// SPURS support.
+		  m_threadInterface->waitForResponse(&taskId, &outputSize);
+	  }
+//		 printf("flush2 taskId %d completed, numBusy =%d \n",taskId,m_numBusyTasks);
+		//printf("PPU: flushing, received event: %u %d\n", taskId, outputSize);
+
+		//postProcess(taskId, outputSize);
+
+		m_taskBusy[taskId] = false;
+
+		m_numBusyTasks--;
+	}
+
+
+}
Index: /code/branches/physics/src/bullet/BulletMultiThreaded/SpuCollisionTaskProcess.h
===================================================================
--- /code/branches/physics/src/bullet/BulletMultiThreaded/SpuCollisionTaskProcess.h	(revision 1966)
+++ /code/branches/physics/src/bullet/BulletMultiThreaded/SpuCollisionTaskProcess.h	(revision 1966)
@@ -0,0 +1,154 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2007 Erwin Coumans  http://bulletphysics.com
+
+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.
+*/
+
+#ifndef SPU_COLLISION_TASK_PROCESS_H
+#define SPU_COLLISION_TASK_PROCESS_H
+
+#include <assert.h>
+
+#include <LinearMath/btScalar.h>
+
+#include "PlatformDefinitions.h"
+#include "LinearMath/btAlignedObjectArray.h"
+#include "SpuNarrowPhaseCollisionTask/SpuGatheringCollisionTask.h" // for definitions processCollisionTask and createCollisionLocalStoreMemory
+
+#include "btThreadSupportInterface.h"
+
+
+//#include "SPUAssert.h"
+#include <string.h>
+
+
+#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
+#include "BulletCollision/CollisionShapes/btCollisionShape.h"
+#include "BulletCollision/CollisionShapes/btConvexShape.h"
+
+#include <LinearMath/btAlignedAllocator.h>
+
+#include <stdio.h>
+
+
+#define DEBUG_SpuCollisionTaskProcess 1
+
+
+#define CMD_GATHER_AND_PROCESS_PAIRLIST	1
+
+class btCollisionObject;
+class btPersistentManifold;
+class btDispatcher;
+
+
+/////Task Description for SPU collision detection
+//struct SpuGatherAndProcessPairsTaskDesc
+//{
+//	uint64_t	inPtr;//m_pairArrayPtr;
+//	//mutex variable
+//	uint32_t	m_someMutexVariableInMainMemory;
+//
+//	uint64_t	m_dispatcher;
+//
+//	uint32_t	numOnLastPage;
+//
+//	uint16_t numPages;
+//	uint16_t taskId;
+//
+//	struct	CollisionTask_LocalStoreMemory*	m_lsMemory; 
+//}
+//
+//#if  defined(__CELLOS_LV2__) || defined(USE_LIBSPE2)
+//__attribute__ ((aligned (16)))
+//#endif
+//;
+
+
+///MidphaseWorkUnitInput stores individual primitive versus mesh collision detection input, to be processed by the SPU.
+struct SpuGatherAndProcessWorkUnitInput
+{
+	uint64_t m_pairArrayPtr;
+	int		m_startIndex;
+	int		m_endIndex;
+};
+
+
+
+
+/// SpuCollisionTaskProcess handles SPU processing of collision pairs.
+/// Maintains a set of task buffers.
+/// When the task is full, the task is issued for SPUs to process.  Contact output goes into btPersistentManifold
+/// associated with each task.
+/// When PPU issues a task, it will look for completed task buffers
+/// PPU will do postprocessing, dependent on workunit output (not likely)
+class SpuCollisionTaskProcess
+{
+
+  unsigned char  *m_workUnitTaskBuffers;
+
+
+	// track task buffers that are being used, and total busy tasks
+	btAlignedObjectArray<bool>	m_taskBusy;
+	btAlignedObjectArray<SpuGatherAndProcessPairsTaskDesc>	m_spuGatherTaskDesc;
+
+	class	btThreadSupportInterface*	m_threadInterface;
+
+	unsigned int	m_maxNumOutstandingTasks;
+
+	unsigned int   m_numBusyTasks;
+
+	// the current task and the current entry to insert a new work unit
+	unsigned int   m_currentTask;
+	unsigned int   m_currentPage;
+	unsigned int   m_currentPageEntry;
+
+	bool m_useEpa;
+
+#ifdef DEBUG_SpuCollisionTaskProcess
+	bool m_initialized;
+#endif
+	void issueTask2();
+	//void postProcess(unsigned int taskId, int outputSize);
+
+public:
+	SpuCollisionTaskProcess(btThreadSupportInterface*	threadInterface, unsigned int maxNumOutstandingTasks);
+	
+	~SpuCollisionTaskProcess();
+	
+	///call initialize in the beginning of the frame, before addCollisionPairToTask
+	void initialize2(bool useEpa = false);
+
+	///batch up additional work to a current task for SPU processing. When batch is full, it issues the task.
+	void addWorkToTask(void* pairArrayPtr,int startIndex,int endIndex);
+
+	///call flush to submit potential outstanding work to SPUs and wait for all involved SPUs to be finished
+	void flush2();
+};
+
+
+
+#define MIDPHASE_TASK_PTR(task) (&m_workUnitTaskBuffers[0] + MIDPHASE_WORKUNIT_TASK_SIZE*task)
+#define MIDPHASE_ENTRY_PTR(task,page,entry) (MIDPHASE_TASK_PTR(task) + MIDPHASE_WORKUNIT_PAGE_SIZE*page + sizeof(SpuGatherAndProcessWorkUnitInput)*entry)
+#define MIDPHASE_OUTPUT_PTR(task) (&m_contactOutputBuffers[0] + MIDPHASE_MAX_CONTACT_BUFFER_SIZE*task)
+#define MIDPHASE_TREENODES_PTR(task) (&m_complexShapeBuffers[0] + MIDPHASE_COMPLEX_SHAPE_BUFFER_SIZE*task)
+
+
+#define MIDPHASE_WORKUNIT_PAGE_SIZE (16)
+
+#define MIDPHASE_NUM_WORKUNIT_PAGES 1
+#define MIDPHASE_WORKUNIT_TASK_SIZE (MIDPHASE_WORKUNIT_PAGE_SIZE*MIDPHASE_NUM_WORKUNIT_PAGES)
+#define MIDPHASE_NUM_WORKUNITS_PER_PAGE (MIDPHASE_WORKUNIT_PAGE_SIZE / sizeof(SpuGatherAndProcessWorkUnitInput))
+#define MIDPHASE_NUM_WORKUNITS_PER_TASK (MIDPHASE_NUM_WORKUNITS_PER_PAGE*MIDPHASE_NUM_WORKUNIT_PAGES)
+
+
+#endif // SPU_COLLISION_TASK_PROCESS_H
+
Index: /code/branches/physics/src/bullet/BulletMultiThreaded/SpuContactManifoldCollisionAlgorithm.cpp
===================================================================
--- /code/branches/physics/src/bullet/BulletMultiThreaded/SpuContactManifoldCollisionAlgorithm.cpp	(revision 1966)
+++ /code/branches/physics/src/bullet/BulletMultiThreaded/SpuContactManifoldCollisionAlgorithm.cpp	(revision 1966)
@@ -0,0 +1,58 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2007 Erwin Coumans  http://bulletphysics.com
+
+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.
+*/
+
+#include "SpuContactManifoldCollisionAlgorithm.h"
+#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
+#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
+#include "BulletCollision/CollisionShapes/btCollisionShape.h"
+
+
+SpuContactManifoldCollisionAlgorithm::SpuContactManifoldCollisionAlgorithm()
+:m_manifoldPtr(0)
+{
+	
+}
+
+
+void SpuContactManifoldCollisionAlgorithm::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+{
+	btAssert(0);
+}
+
+btScalar SpuContactManifoldCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+{
+	btAssert(0);
+	return 1.f;
+}
+
+#ifndef __SPU__
+SpuContactManifoldCollisionAlgorithm::SpuContactManifoldCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1)
+:btCollisionAlgorithm(ci)
+{
+	m_manifoldPtr = m_dispatcher->getNewManifold(body0,body1);
+	m_shapeType0 = body0->getCollisionShape()->getShapeType();
+	m_shapeType1 = body1->getCollisionShape()->getShapeType();
+	m_collisionMargin0 = body0->getCollisionShape()->getMargin();
+	m_collisionMargin1 = body1->getCollisionShape()->getMargin();
+
+}
+#endif //__SPU__
+
+
+SpuContactManifoldCollisionAlgorithm::~SpuContactManifoldCollisionAlgorithm()
+{
+	if (m_manifoldPtr)
+			m_dispatcher->releaseManifold(m_manifoldPtr);
+}
Index: /code/branches/physics/src/bullet/BulletMultiThreaded/SpuContactManifoldCollisionAlgorithm.h
===================================================================
--- /code/branches/physics/src/bullet/BulletMultiThreaded/SpuContactManifoldCollisionAlgorithm.h	(revision 1966)
+++ /code/branches/physics/src/bullet/BulletMultiThreaded/SpuContactManifoldCollisionAlgorithm.h	(revision 1966)
@@ -0,0 +1,89 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2007 Erwin Coumans  http://bulletphysics.com
+
+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.
+*/
+
+#ifndef SPU_CONTACTMANIFOLD_COLLISION_ALGORITHM_H
+#define SPU_CONTACTMANIFOLD_COLLISION_ALGORITHM_H
+
+#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h"
+#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
+#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h"
+#include "BulletCollision/BroadphaseCollision/btDispatcher.h"
+
+class btPersistentManifold;
+
+/// SpuContactManifoldCollisionAlgorithm  provides contact manifold and should be processed on SPU.
+ATTRIBUTE_ALIGNED16(class) SpuContactManifoldCollisionAlgorithm : public btCollisionAlgorithm
+{
+
+	btPersistentManifold*	m_manifoldPtr;
+	int		m_shapeType0;
+	int		m_shapeType1;
+	float	m_collisionMargin0;
+	float	m_collisionMargin1;
+
+	
+public:
+	
+	virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+
+	virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+
+	SpuContactManifoldCollisionAlgorithm();
+
+	SpuContactManifoldCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1);
+
+	virtual ~SpuContactManifoldCollisionAlgorithm();
+
+	virtual	void	getAllContactManifolds(btManifoldArray&	manifoldArray)
+	{
+		if (m_manifoldPtr)
+			manifoldArray.push_back(m_manifoldPtr);
+	}
+
+	btPersistentManifold*	getContactManifoldPtr()
+	{
+		return m_manifoldPtr;
+	}
+
+	int		getShapeType0() const
+	{
+		return m_shapeType0;
+	}
+
+	int		getShapeType1() const
+	{
+		return m_shapeType1;
+	}
+	float	getCollisionMargin0() const
+	{
+		return m_collisionMargin0;
+	}
+	float	getCollisionMargin1() const
+	{
+		return m_collisionMargin1;
+	}
+
+	struct CreateFunc :public 	btCollisionAlgorithmCreateFunc
+	{
+		virtual	btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1)
+		{
+			void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(SpuContactManifoldCollisionAlgorithm));
+			return new(mem) SpuContactManifoldCollisionAlgorithm(ci,body0,body1);
+		}
+	};
+
+};
+
+#endif //SPU_CONTACTMANIFOLD_COLLISION_ALGORITHM_H
Index: /code/branches/physics/src/bullet/BulletMultiThreaded/SpuDoubleBuffer.h
===================================================================
--- /code/branches/physics/src/bullet/BulletMultiThreaded/SpuDoubleBuffer.h	(revision 1966)
+++ /code/branches/physics/src/bullet/BulletMultiThreaded/SpuDoubleBuffer.h	(revision 1966)
@@ -0,0 +1,107 @@
+#ifndef DOUBLE_BUFFER_H
+#define DOUBLE_BUFFER_H
+
+#include "SpuFakeDma.h"
+#include <LinearMath/btScalar.h>
+
+
+///DoubleBuffer
+template<class T, int size>
+class DoubleBuffer
+{
+#if defined(__SPU__) || defined(USE_LIBSPE2)
+	ATTRIBUTE_ALIGNED128( T m_buffer0[size] ) ;
+	ATTRIBUTE_ALIGNED128( T m_buffer1[size] ) ;
+#else
+	T m_buffer0[size];
+	T m_buffer1[size];
+#endif
+	
+	T *m_frontBuffer;
+	T *m_backBuffer;
+
+	unsigned int m_dmaTag;
+	bool m_dmaPending;
+public:
+	bool	isPending() const { return m_dmaPending;}
+	DoubleBuffer();
+
+	void init ();
+
+	// dma get and put commands
+	void backBufferDmaGet(uint64_t ea, unsigned int numBytes, unsigned int tag);
+	void backBufferDmaPut(uint64_t ea, unsigned int numBytes, unsigned int tag);
+
+	// gets pointer to a buffer
+	T *getFront();
+	T *getBack();
+
+	// if back buffer dma was started, wait for it to complete
+	// then move back to front and vice versa
+	T *swapBuffers();
+};
+
+template<class T, int size>
+DoubleBuffer<T,size>::DoubleBuffer()
+{
+	init ();
+}
+
+template<class T, int size>
+void DoubleBuffer<T,size>::init()
+{
+	this->m_dmaPending = false;
+	this->m_frontBuffer = &this->m_buffer0[0];
+	this->m_backBuffer = &this->m_buffer1[0];
+}
+
+template<class T, int size>
+void
+DoubleBuffer<T,size>::backBufferDmaGet(uint64_t ea, unsigned int numBytes, unsigned int tag)
+{
+	m_dmaPending = true;
+	m_dmaTag = tag;
+	cellDmaLargeGet(m_backBuffer, ea, numBytes, tag, 0, 0);
+}
+
+template<class T, int size>
+void
+DoubleBuffer<T,size>::backBufferDmaPut(uint64_t ea, unsigned int numBytes, unsigned int tag)
+{
+	m_dmaPending = true;
+	m_dmaTag = tag;
+	cellDmaLargePut(m_backBuffer, ea, numBytes, tag, 0, 0);
+}
+
+template<class T, int size>
+T *
+DoubleBuffer<T,size>::getFront()
+{
+	return m_frontBuffer;
+}
+
+template<class T, int size>
+T *
+DoubleBuffer<T,size>::getBack()
+{
+	return m_backBuffer;
+}
+
+template<class T, int size>
+T *
+DoubleBuffer<T,size>::swapBuffers()
+{
+	if (m_dmaPending)
+	{
+		cellDmaWaitTagStatusAll(1<<m_dmaTag);
+		m_dmaPending = false;
+	}
+
+	T *tmp = m_backBuffer;
+	m_backBuffer = m_frontBuffer;
+	m_frontBuffer = tmp;
+
+	return m_frontBuffer;
+}
+
+#endif
Index: /code/branches/physics/src/bullet/BulletMultiThreaded/SpuFakeDma.cpp
===================================================================
--- /code/branches/physics/src/bullet/BulletMultiThreaded/SpuFakeDma.cpp	(revision 1966)
+++ /code/branches/physics/src/bullet/BulletMultiThreaded/SpuFakeDma.cpp	(revision 1966)
@@ -0,0 +1,195 @@
+
+#include "SpuFakeDma.h"
+#include <LinearMath/btScalar.h> //for btAssert
+//Disabling memcpy sometimes helps debugging DMA
+
+#define USE_MEMCPY 1
+#ifdef USE_MEMCPY
+
+#endif
+
+
+void*	cellDmaLargeGetReadOnly(void *ls, uint64_t ea, uint32_t size, uint32_t tag, uint32_t tid, uint32_t rid)
+{
+
+#if defined (__CELLOS_LV2__) || defined (USE_LIBSPE2)
+	cellDmaLargeGet(ls,ea,size,tag,tid,rid);
+	return ls;
+#else
+	return (void*)(uint32_t)ea;
+#endif
+}
+
+void*	cellDmaSmallGetReadOnly(void *ls, uint64_t ea, uint32_t size, uint32_t tag, uint32_t tid, uint32_t rid)
+{
+#if defined (__SPU__) || defined (USE_LIBSPE2)
+	mfc_get(ls,ea,size,tag,0,0);
+	return ls;
+#else
+	return (void*)(uint32_t)ea;
+#endif
+}
+
+
+
+
+void*	cellDmaGetReadOnly(void *ls, uint64_t ea, uint32_t size, uint32_t tag, uint32_t tid, uint32_t rid)
+{
+#if defined (__SPU__) || defined (USE_LIBSPE2)
+	cellDmaGet(ls,ea,size,tag,tid,rid);
+	return ls;
+#else
+	return (void*)(uint32_t)ea;
+#endif
+}
+
+
+///this unalignedDma should not be frequently used, only for small data. It handles alignment and performs check on size (<16 bytes)
+int stallingUnalignedDmaSmallGet(void *ls, uint64_t ea, uint32_t size)
+{
+	
+	btAssert(size<32);
+	
+	ATTRIBUTE_ALIGNED16(char	tmpBuffer[32]);
+
+	char* mainMem = (char*)ea;
+	char* localStore = (char*)ls;
+	uint32_t i;
+	
+
+	///make sure last 4 bits are the same, for cellDmaSmallGet
+	uint32_t last4BitsOffset = ea & 0x0f;
+	char* tmpTarget = tmpBuffer + last4BitsOffset;
+	
+#if defined (__SPU__) || defined (USE_LIBSPE2)
+	
+	int remainingSize = size;
+
+//#define FORCE_cellDmaUnalignedGet 1
+#ifdef FORCE_cellDmaUnalignedGet
+	cellDmaUnalignedGet(tmpTarget,ea,size,DMA_TAG(1),0,0);
+#else
+	char* remainingTmpTarget = tmpTarget;
+	uint64_t remainingEa = ea;
+
+	while (remainingSize)
+	{
+		switch (remainingSize)
+		{
+		case 1:
+		case 2:
+		case 4:
+		case 8:
+		case 16:
+			{
+				mfc_get(remainingTmpTarget,remainingEa,remainingSize,DMA_TAG(1),0,0);
+				remainingSize=0;
+				break;
+			}
+		default:
+			{
+				//spu_printf("unaligned DMA with non-natural size:%d\n",remainingSize);
+				int actualSize = 0;
+
+				if (remainingSize > 16)
+					actualSize = 16;
+				else
+					if (remainingSize >8)
+						actualSize=8;
+					else
+						if (remainingSize >4)
+							actualSize=4;
+						else
+							if (remainingSize >2)
+								actualSize=2;
+				mfc_get(remainingTmpTarget,remainingEa,actualSize,DMA_TAG(1),0,0);
+				remainingSize-=actualSize;
+				remainingTmpTarget+=actualSize;
+				remainingEa += actualSize;
+			}
+		}
+	}
+#endif//FORCE_cellDmaUnalignedGet
+
+#else
+	//copy into final destination
+#ifdef USE_MEMCPY
+		memcpy(tmpTarget,mainMem,size);
+#else
+		for ( i=0;i<size;i++)
+		{
+			tmpTarget[i] = mainMem[i];
+		}
+#endif //USE_MEMCPY
+
+#endif
+
+	cellDmaWaitTagStatusAll(DMA_MASK(1));
+
+	//this is slowish, perhaps memcpy on SPU is smarter?
+	for (i=0; btLikely( i<size );i++)
+	{
+		localStore[i] = tmpTarget[i];
+	}
+
+	return 0;
+}
+
+#if defined (__SPU__) || defined (USE_LIBSPE2)
+#else
+
+int	cellDmaLargeGet(void *ls, uint64_t ea, uint32_t size, uint32_t tag, uint32_t tid, uint32_t rid)
+{
+	char* mainMem = (char*)ea;
+	char* localStore = (char*)ls;
+
+#ifdef USE_MEMCPY
+	memcpy(localStore,mainMem,size);
+#else
+	for (uint32_t i=0;i<size;i++)
+	{
+		localStore[i] = mainMem[i];
+	}
+#endif
+	return 0;
+}
+
+int	cellDmaGet(void *ls, uint64_t ea, uint32_t size, uint32_t tag, uint32_t tid, uint32_t rid)
+{
+	char* mainMem = (char*)ea;
+	char* localStore = (char*)ls;
+#ifdef USE_MEMCPY
+	memcpy(localStore,mainMem,size);
+#else
+	for (uint32_t i=0;i<size;i++)
+	{
+		localStore[i] = mainMem[i];
+	}	
+#endif //#ifdef USE_MEMCPY
+	return 0;
+}
+
+int cellDmaLargePut(const void *ls, uint64_t ea, uint32_t size, uint32_t tag, uint32_t tid, uint32_t rid)
+{
+	char* mainMem = (char*)ea;
+	const char* localStore = (const char*)ls;
+#ifdef USE_MEMCPY
+	memcpy(mainMem,localStore,size);
+#else
+	for (uint32_t i=0;i<size;i++)
+	{
+		mainMem[i] = localStore[i];
+	}	
+#endif //#ifdef USE_MEMCPY
+
+	return 0;
+}
+
+
+
+void	cellDmaWaitTagStatusAll(int ignore)
+{
+
+}
+
+#endif
Index: /code/branches/physics/src/bullet/BulletMultiThreaded/SpuFakeDma.h
===================================================================
--- /code/branches/physics/src/bullet/BulletMultiThreaded/SpuFakeDma.h	(revision 1966)
+++ /code/branches/physics/src/bullet/BulletMultiThreaded/SpuFakeDma.h	(revision 1966)
@@ -0,0 +1,121 @@
+
+#ifndef FAKE_DMA_H
+#define FAKE_DMA_H
+
+
+#include "PlatformDefinitions.h"
+#include "LinearMath/btScalar.h"
+
+
+#ifdef __SPU__
+
+#ifndef USE_LIBSPE2
+
+#include <cell/dma.h>
+#include <stdint.h>
+
+#define DMA_TAG(xfer) (xfer + 1)
+#define DMA_MASK(xfer) (1 << DMA_TAG(xfer))
+
+#else // !USE_LIBSPE2
+
+#define DMA_TAG(xfer) (xfer + 1)
+#define DMA_MASK(xfer) (1 << DMA_TAG(xfer))
+		
+#include <spu_mfcio.h>		
+		
+#define DEBUG_DMA		
+#ifdef DEBUG_DMA
+#define dUASSERT(a,b) if (!(a)) { printf(b);}
+#define uintsize ppu_address_t
+		
+#define cellDmaLargeGet(ls, ea, size, tag, tid, rid) if (  (((uintsize)ls%16) != ((uintsize)ea%16)) || ((((uintsize)ea%16) || ((uintsize)ls%16)) && (( ((uintsize)ls%16) != ((uintsize)size%16) ) || ( ((uintsize)ea%16) != ((uintsize)size%16) ) ) ) || ( ((uintsize)size%16) && ((uintsize)size!=1) && ((uintsize)size!=2) && ((uintsize)size!=4) && ((uintsize)size!=8) ) || (size >= 16384) || !(uintsize)ls || !(uintsize)ea) { \
+															dUASSERT( (((uintsize)ea % 16) == 0) || (size < 16), "XDR Address not aligned: "); \
+															dUASSERT( (((uintsize)ls % 16) == 0) || (size < 16), "LS Address not aligned: "); \
+															dUASSERT( ((((uintsize)ls % size) == 0) && (((uintsize)ea % size) == 0))  || (size > 16), "Not naturally aligned: "); \
+															dUASSERT((size == 1) || (size == 2) || (size == 4) || (size == 8) || ((size % 16) == 0), "size not a multiple of 16byte: "); \
+															dUASSERT(size < 16384, "size too big: "); \
+															dUASSERT( ((uintsize)ea%16)==((uintsize)ls%16), "wrong Quadword alignment of LS and EA: "); \
+	    													dUASSERT(ea != 0, "Nullpointer EA: "); dUASSERT(ls != 0, "Nullpointer LS: ");\
+															printf("GET %s:%d from: 0x%x, to: 0x%x - %d bytes\n", __FILE__, __LINE__, (unsigned int)ea,(unsigned int)ls,(unsigned int)size);\
+															} \
+															mfc_get(ls, ea, size, tag, tid, rid)
+#define cellDmaGet(ls, ea, size, tag, tid, rid) if (  (((uintsize)ls%16) != ((uintsize)ea%16)) || ((((uintsize)ea%16) || ((uintsize)ls%16)) && (( ((uintsize)ls%16) != ((uintsize)size%16) ) || ( ((uintsize)ea%16) != ((uintsize)size%16) ) ) ) || ( ((uintsize)size%16) && ((uintsize)size!=1) && ((uintsize)size!=2) && ((uintsize)size!=4) && ((uintsize)size!=8) ) || (size >= 16384) || !(uintsize)ls || !(uintsize)ea) { \
+														dUASSERT( (((uintsize)ea % 16) == 0) || (size < 16), "XDR Address not aligned: "); \
+														dUASSERT( (((uintsize)ls % 16) == 0) || (size < 16), "LS Address not aligned: "); \
+														dUASSERT( ((((uintsize)ls % size) == 0) && (((uintsize)ea % size) == 0))  || (size > 16), "Not naturally aligned: "); \
+														dUASSERT((size == 1) || (size == 2) || (size == 4) || (size == 8) || ((size % 16) == 0), "size not a multiple of 16byte: "); \
+    													dUASSERT(size < 16384, "size too big: "); \
+														dUASSERT( ((uintsize)ea%16)==((uintsize)ls%16), "wrong Quadword alignment of LS and EA: "); \
+    													dUASSERT(ea != 0, "Nullpointer EA: "); dUASSERT(ls != 0, "Nullpointer LS: ");\
+    													printf("GET %s:%d from: 0x%x, to: 0x%x - %d bytes\n", __FILE__, __LINE__, (unsigned int)ea,(unsigned int)ls,(unsigned int)size);\
+														} \
+														mfc_get(ls, ea, size, tag, tid, rid)
+#define cellDmaLargePut(ls, ea, size, tag, tid, rid) if (  (((uintsize)ls%16) != ((uintsize)ea%16)) || ((((uintsize)ea%16) || ((uintsize)ls%16)) && (( ((uintsize)ls%16) != ((uintsize)size%16) ) || ( ((uintsize)ea%16) != ((uintsize)size%16) ) ) ) || ( ((uintsize)size%16) && ((uintsize)size!=1) && ((uintsize)size!=2) && ((uintsize)size!=4) && ((uintsize)size!=8) ) || (size >= 16384) || !(uintsize)ls || !(uintsize)ea) { \
+															dUASSERT( (((uintsize)ea % 16) == 0) || (size < 16), "XDR Address not aligned: "); \
+															dUASSERT( (((uintsize)ls % 16) == 0) || (size < 16), "LS Address not aligned: "); \
+															dUASSERT( ((((uintsize)ls % size) == 0) && (((uintsize)ea % size) == 0))  || (size > 16), "Not naturally aligned: "); \
+															dUASSERT((size == 1) || (size == 2) || (size == 4) || (size == 8) || ((size % 16) == 0), "size not a multiple of 16byte: "); \
+        													dUASSERT(size < 16384, "size too big: "); \
+															dUASSERT( ((uintsize)ea%16)==((uintsize)ls%16), "wrong Quadword alignment of LS and EA: "); \
+        													dUASSERT(ea != 0, "Nullpointer EA: "); dUASSERT(ls != 0, "Nullpointer LS: ");\
+    														printf("PUT %s:%d from: 0x%x, to: 0x%x - %d bytes\n", __FILE__, __LINE__, (unsigned int)ls,(unsigned int)ea,(unsigned int)size); \
+															} \
+															mfc_put(ls, ea, size, tag, tid, rid)
+#define cellDmaSmallGet(ls, ea, size, tag, tid, rid) if (  (((uintsize)ls%16) != ((uintsize)ea%16)) || ((((uintsize)ea%16) || ((uintsize)ls%16)) && (( ((uintsize)ls%16) != ((uintsize)size%16) ) || ( ((uintsize)ea%16) != ((uintsize)size%16) ) ) ) || ( ((uintsize)size%16) && ((uintsize)size!=1) && ((uintsize)size!=2) && ((uintsize)size!=4) && ((uintsize)size!=8) ) || (size >= 16384) || !(uintsize)ls || !(uintsize)ea) { \
+																dUASSERT( (((uintsize)ea % 16) == 0) || (size < 16), "XDR Address not aligned: "); \
+																dUASSERT( (((uintsize)ls % 16) == 0) || (size < 16), "LS Address not aligned: "); \
+																dUASSERT( ((((uintsize)ls % size) == 0) && (((uintsize)ea % size) == 0))  || (size > 16), "Not naturally aligned: "); \
+    															dUASSERT((size == 1) || (size == 2) || (size == 4) || (size == 8) || ((size % 16) == 0), "size not a multiple of 16byte: "); \
+    															dUASSERT(size < 16384, "size too big: "); \
+    															dUASSERT( ((uintsize)ea%16)==((uintsize)ls%16), "wrong Quadword alignment of LS and EA: "); \
+    	    													dUASSERT(ea != 0, "Nullpointer EA: "); dUASSERT(ls != 0, "Nullpointer LS: ");\
+    															printf("GET %s:%d from: 0x%x, to: 0x%x - %d bytes\n", __FILE__, __LINE__, (unsigned int)ea,(unsigned int)ls,(unsigned int)size);\
+																} \
+																mfc_get(ls, ea, size, tag, tid, rid)
+#define cellDmaWaitTagStatusAll(ignore) mfc_write_tag_mask(ignore) ; mfc_read_tag_status_all()
+
+#else
+#define cellDmaLargeGet(ls, ea, size, tag, tid, rid) mfc_get(ls, ea, size, tag, tid, rid)
+#define cellDmaGet(ls, ea, size, tag, tid, rid) mfc_get(ls, ea, size, tag, tid, rid)
+#define cellDmaLargePut(ls, ea, size, tag, tid, rid) mfc_put(ls, ea, size, tag, tid, rid)
+#define cellDmaSmallGet(ls, ea, size, tag, tid, rid) mfc_get(ls, ea, size, tag, tid, rid)
+#define cellDmaWaitTagStatusAll(ignore) mfc_write_tag_mask(ignore) ; mfc_read_tag_status_all()
+#endif // DEBUG_DMA
+
+		
+		
+		
+		
+		
+		
+		
+#endif // USE_LIBSPE2
+#else // !__SPU__
+//Simulate DMA using memcpy or direct access on non-CELL platforms that don't have DMAs and SPUs (Win32, Mac, Linux etc)
+//Potential to add networked simulation using this interface
+
+#define DMA_TAG(a) (a)
+#define DMA_MASK(a) (a)
+
+		/// cellDmaLargeGet Win32 replacements for Cell DMA to allow simulating most of the SPU code (just memcpy)
+		int	cellDmaLargeGet(void *ls, uint64_t ea, uint32_t size, uint32_t tag, uint32_t tid, uint32_t rid);
+		int	cellDmaGet(void *ls, uint64_t ea, uint32_t size, uint32_t tag, uint32_t tid, uint32_t rid);
+		/// cellDmaLargePut Win32 replacements for Cell DMA to allow simulating most of the SPU code (just memcpy)
+		int cellDmaLargePut(const void *ls, uint64_t ea, uint32_t size, uint32_t tag, uint32_t tid, uint32_t rid);
+		/// cellDmaWaitTagStatusAll Win32 replacements for Cell DMA to allow simulating most of the SPU code (just memcpy)
+		void	cellDmaWaitTagStatusAll(int ignore);
+
+
+#endif //__CELLOS_LV2__
+
+///stallingUnalignedDmaSmallGet internally uses DMA_TAG(1)
+int	stallingUnalignedDmaSmallGet(void *ls, uint64_t ea, uint32_t size);
+
+
+void*	cellDmaLargeGetReadOnly(void *ls, uint64_t ea, uint32_t size, uint32_t tag, uint32_t tid, uint32_t rid);
+void*	cellDmaGetReadOnly(void *ls, uint64_t ea, uint32_t size, uint32_t tag, uint32_t tid, uint32_t rid);
+void*	cellDmaSmallGetReadOnly(void *ls, uint64_t ea, uint32_t size, uint32_t tag, uint32_t tid, uint32_t rid);
+
+
+#endif //FAKE_DMA_H
Index: /code/branches/physics/src/bullet/BulletMultiThreaded/SpuGatheringCollisionDispatcher.cpp
===================================================================
--- /code/branches/physics/src/bullet/BulletMultiThreaded/SpuGatheringCollisionDispatcher.cpp	(revision 1966)
+++ /code/branches/physics/src/bullet/BulletMultiThreaded/SpuGatheringCollisionDispatcher.cpp	(revision 1966)
@@ -0,0 +1,215 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2007 Erwin Coumans  http://bulletphysics.com
+
+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.
+*/
+
+#include "SpuGatheringCollisionDispatcher.h"
+#include "SpuCollisionTaskProcess.h"
+
+
+#include "BulletCollision/BroadphaseCollision/btOverlappingPairCache.h"
+#include "BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.h"
+#include "SpuContactManifoldCollisionAlgorithm.h"
+#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
+#include "BulletCollision/CollisionShapes/btCollisionShape.h"
+
+
+
+
+SpuGatheringCollisionDispatcher::SpuGatheringCollisionDispatcher(class	btThreadSupportInterface*	threadInterface, unsigned int	maxNumOutstandingTasks,btCollisionConfiguration* collisionConfiguration)
+:btCollisionDispatcher(collisionConfiguration),
+m_spuCollisionTaskProcess(0),
+m_threadInterface(threadInterface),
+m_maxNumOutstandingTasks(maxNumOutstandingTasks)
+{
+	
+}
+
+
+bool	SpuGatheringCollisionDispatcher::supportsDispatchPairOnSpu(int proxyType0,int proxyType1)
+{
+	bool supported0 = (
+		(proxyType0 == BOX_SHAPE_PROXYTYPE) ||
+		(proxyType0 == TRIANGLE_SHAPE_PROXYTYPE) ||
+		(proxyType0 == SPHERE_SHAPE_PROXYTYPE) ||
+		(proxyType0 == CAPSULE_SHAPE_PROXYTYPE) ||
+		(proxyType0 == CYLINDER_SHAPE_PROXYTYPE) ||
+//		(proxyType0 == CONE_SHAPE_PROXYTYPE) ||
+		(proxyType0 == TRIANGLE_MESH_SHAPE_PROXYTYPE) ||
+		(proxyType0 == CONVEX_HULL_SHAPE_PROXYTYPE)||
+		(proxyType0 == COMPOUND_SHAPE_PROXYTYPE)
+		);
+
+	bool supported1 = (
+		(proxyType1 == BOX_SHAPE_PROXYTYPE) ||
+		(proxyType1 == TRIANGLE_SHAPE_PROXYTYPE) ||
+		(proxyType1 == SPHERE_SHAPE_PROXYTYPE) ||
+		(proxyType1 == CAPSULE_SHAPE_PROXYTYPE) ||
+		(proxyType1 == CYLINDER_SHAPE_PROXYTYPE) ||
+//		(proxyType1 == CONE_SHAPE_PROXYTYPE) ||
+		(proxyType1 == TRIANGLE_MESH_SHAPE_PROXYTYPE) ||
+		(proxyType1 == CONVEX_HULL_SHAPE_PROXYTYPE) ||
+		(proxyType1 == COMPOUND_SHAPE_PROXYTYPE)
+		);
+
+	return supported0 && supported1;
+}
+
+
+
+SpuGatheringCollisionDispatcher::~SpuGatheringCollisionDispatcher()
+{
+	if (m_spuCollisionTaskProcess)
+		delete m_spuCollisionTaskProcess;
+	
+}
+
+#include "stdio.h"
+
+
+
+///interface for iterating all overlapping collision pairs, no matter how those pairs are stored (array, set, map etc)
+///this is useful for the collision dispatcher.
+class btSpuCollisionPairCallback : public btOverlapCallback
+{
+	const btDispatcherInfo& m_dispatchInfo;
+	SpuGatheringCollisionDispatcher*	m_dispatcher;
+
+public:
+
+	btSpuCollisionPairCallback(const btDispatcherInfo& dispatchInfo, SpuGatheringCollisionDispatcher*	dispatcher)
+	:m_dispatchInfo(dispatchInfo),
+	m_dispatcher(dispatcher)
+	{
+	}
+
+	virtual bool	processOverlap(btBroadphasePair& collisionPair)
+	{
+
+
+		//PPU version
+		//(*m_dispatcher->getNearCallback())(collisionPair,*m_dispatcher,m_dispatchInfo);
+
+		//only support discrete collision detection for now, we could fallback on PPU/unoptimized version for TOI/CCD
+		btAssert(m_dispatchInfo.m_dispatchFunc == btDispatcherInfo::DISPATCH_DISCRETE);
+
+		//by default, Bullet will use this near callback
+		{
+			///userInfo is used to determine if the SPU has to handle this case or not (skip PPU tasks)
+			if (!collisionPair.m_userInfo)
+			{
+				collisionPair.m_userInfo = (void*) 1;
+			}
+			if (!collisionPair.m_algorithm)
+			{
+				btCollisionObject* colObj0 = (btCollisionObject*)collisionPair.m_pProxy0->m_clientObject;
+				btCollisionObject* colObj1 = (btCollisionObject*)collisionPair.m_pProxy1->m_clientObject;
+
+				btCollisionAlgorithmConstructionInfo ci;
+				ci.m_dispatcher1 = m_dispatcher;
+				ci.m_manifold = 0;
+
+				if (m_dispatcher->needsCollision(colObj0,colObj1))
+				{
+					int	proxyType0 = colObj0->getCollisionShape()->getShapeType();
+					int	proxyType1 = colObj1->getCollisionShape()->getShapeType();
+					if (m_dispatcher->supportsDispatchPairOnSpu(proxyType0,proxyType1))
+					{
+						int so = sizeof(SpuContactManifoldCollisionAlgorithm);
+						void* mem = m_dispatcher->allocateCollisionAlgorithm(so);
+						collisionPair.m_algorithm = new(mem) SpuContactManifoldCollisionAlgorithm(ci,colObj0,colObj1);
+						collisionPair.m_userInfo = (void*) 2;
+					} else
+					{
+						collisionPair.m_algorithm = m_dispatcher->findAlgorithm(colObj0,colObj1);
+						collisionPair.m_userInfo = (void*)3;
+					}
+				} 
+			}
+		}
+		return false;
+	}
+};
+
+void	SpuGatheringCollisionDispatcher::dispatchAllCollisionPairs(btOverlappingPairCache* pairCache,const btDispatcherInfo& dispatchInfo, btDispatcher* dispatcher) 
+{
+
+	if (dispatchInfo.m_enableSPU)
+	{
+		if (!m_spuCollisionTaskProcess)
+			m_spuCollisionTaskProcess = new SpuCollisionTaskProcess(m_threadInterface,m_maxNumOutstandingTasks);
+	
+		m_spuCollisionTaskProcess->initialize2(dispatchInfo.m_useEpa);
+	
+		///modified version of btCollisionDispatcher::dispatchAllCollisionPairs:
+		{
+			btSpuCollisionPairCallback	collisionCallback(dispatchInfo,this);
+
+			pairCache->processAllOverlappingPairs(&collisionCallback,dispatcher);
+		}
+
+		//send one big batch
+		int numTotalPairs = pairCache->getNumOverlappingPairs();
+		btBroadphasePair* pairPtr = pairCache->getOverlappingPairArrayPtr();
+		int i;
+		for (i=0;i<numTotalPairs;)
+		{
+			//Performance Hint: tweak this number during benchmarking
+			static const int pairRange = SPU_BATCHSIZE_BROADPHASE_PAIRS;
+			int endIndex = (i+pairRange) < numTotalPairs ? i+pairRange : numTotalPairs;
+			m_spuCollisionTaskProcess->addWorkToTask(pairPtr,i,endIndex);
+			i = endIndex;
+		}
+
+		//handle PPU fallback pairs
+		for (i=0;i<numTotalPairs;i++)
+		{
+			btBroadphasePair& collisionPair = pairPtr[i];
+			if (collisionPair.m_userInfo == (void*)3)
+			{
+				if (collisionPair.m_algorithm)
+				{
+					btCollisionObject* colObj0 = (btCollisionObject*)collisionPair.m_pProxy0->m_clientObject;
+					btCollisionObject* colObj1 = (btCollisionObject*)collisionPair.m_pProxy1->m_clientObject;
+
+					if (dispatcher->needsCollision(colObj0,colObj1))
+					{
+						btManifoldResult contactPointResult(colObj0,colObj1);
+						
+						if (dispatchInfo.m_dispatchFunc == 		btDispatcherInfo::DISPATCH_DISCRETE)
+						{
+							//discrete collision detection query
+							collisionPair.m_algorithm->processCollision(colObj0,colObj1,dispatchInfo,&contactPointResult);
+						} else
+						{
+							//continuous collision detection query, time of impact (toi)
+							btScalar toi = collisionPair.m_algorithm->calculateTimeOfImpact(colObj0,colObj1,dispatchInfo,&contactPointResult);
+							if (dispatchInfo.m_timeOfImpact > toi)
+								dispatchInfo.m_timeOfImpact = toi;
+
+						}
+					}
+				}
+			}
+		}
+
+		//make sure all SPU work is done
+		m_spuCollisionTaskProcess->flush2();
+
+	} else
+	{
+		///PPU fallback
+		///!Need to make sure to clear all 'algorithms' when switching between SPU and PPU
+		btCollisionDispatcher::dispatchAllCollisionPairs(pairCache,dispatchInfo,dispatcher);
+	}
+}
Index: /code/branches/physics/src/bullet/BulletMultiThreaded/SpuGatheringCollisionDispatcher.h
===================================================================
--- /code/branches/physics/src/bullet/BulletMultiThreaded/SpuGatheringCollisionDispatcher.h	(revision 1966)
+++ /code/branches/physics/src/bullet/BulletMultiThreaded/SpuGatheringCollisionDispatcher.h	(revision 1966)
@@ -0,0 +1,64 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2007 Erwin Coumans  http://bulletphysics.com
+
+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.
+*/
+#ifndef SPU_GATHERING_COLLISION__DISPATCHER_H
+#define SPU_GATHERING_COLLISION__DISPATCHER_H
+
+#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
+
+
+///Tuning value to optimized SPU utilization 
+///Too small value means Task overhead is large compared to computation (too fine granularity)
+///Too big value might render some SPUs are idle, while a few other SPUs are doing all work.
+#define SPU_BATCHSIZE_BROADPHASE_PAIRS 16
+//#define SPU_BATCHSIZE_BROADPHASE_PAIRS 256
+
+
+class SpuCollisionTaskProcess;
+
+///SpuGatheringCollisionDispatcher can use SPU to gather and calculate collision detection
+///Time of Impact, Closest Points and Penetration Depth.
+class SpuGatheringCollisionDispatcher : public btCollisionDispatcher
+{
+	
+	SpuCollisionTaskProcess*	m_spuCollisionTaskProcess;
+	
+protected:
+
+	class	btThreadSupportInterface*	m_threadInterface;
+
+	unsigned int	m_maxNumOutstandingTasks;
+	
+
+public:
+
+	//can be used by SPU collision algorithms	
+	SpuCollisionTaskProcess*	getSpuCollisionTaskProcess()
+	{
+			return m_spuCollisionTaskProcess;
+	}
+	
+	SpuGatheringCollisionDispatcher (class	btThreadSupportInterface*	threadInterface, unsigned int	maxNumOutstandingTasks,btCollisionConfiguration* collisionConfiguration);
+	
+	virtual ~SpuGatheringCollisionDispatcher();
+
+	bool	supportsDispatchPairOnSpu(int proxyType0,int proxyType1);
+
+	virtual void	dispatchAllCollisionPairs(btOverlappingPairCache* pairCache,const btDispatcherInfo& dispatchInfo,btDispatcher* dispatcher) ;
+
+};
+
+
+
+#endif //SPU_GATHERING_COLLISION__DISPATCHER_H
Index: /code/branches/physics/src/bullet/BulletMultiThreaded/SpuLibspe2Support.cpp
===================================================================
--- /code/branches/physics/src/bullet/BulletMultiThreaded/SpuLibspe2Support.cpp	(revision 1966)
+++ /code/branches/physics/src/bullet/BulletMultiThreaded/SpuLibspe2Support.cpp	(revision 1966)
@@ -0,0 +1,257 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2007 Erwin Coumans  http://bulletphysics.com
+
+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.
+*/
+
+#ifdef USE_LIBSPE2
+
+#include "SpuLibspe2Support.h"
+
+
+
+
+//SpuLibspe2Support helps to initialize/shutdown libspe2, start/stop SPU tasks and communication
+///Setup and initialize SPU/CELL/Libspe2
+SpuLibspe2Support::SpuLibspe2Support(spe_program_handle_t *speprog, int numThreads)
+{
+	this->program = speprog;
+	this->numThreads =  ((numThreads <= spe_cpu_info_get(SPE_COUNT_PHYSICAL_SPES, -1)) ? numThreads : spe_cpu_info_get(SPE_COUNT_PHYSICAL_SPES, -1));
+}
+
+///cleanup/shutdown Libspe2
+SpuLibspe2Support::~SpuLibspe2Support()
+{
+	
+	stopSPU();
+}
+
+
+
+///send messages to SPUs
+void SpuLibspe2Support::sendRequest(uint32_t uiCommand, uint32_t uiArgument0, uint32_t uiArgument1)
+{
+	spe_context_ptr_t context;
+	
+	switch (uiCommand)
+	{
+	case CMD_SAMPLE_TASK_COMMAND:
+	{
+		//get taskdescription
+		SpuSampleTaskDesc* taskDesc = (SpuSampleTaskDesc*) uiArgument0;
+
+		btAssert(taskDesc->m_taskId<m_activeSpuStatus.size());
+
+		//get status of SPU on which task should run
+		btSpuStatus&	spuStatus = m_activeSpuStatus[taskDesc->m_taskId];
+
+		//set data for spuStatus
+		spuStatus.m_commandId = uiCommand;
+		spuStatus.m_status = Spu_Status_Occupied; //set SPU as "occupied"
+		spuStatus.m_taskDesc.p = taskDesc; 
+		
+		//get context
+		context = data[taskDesc->m_taskId].context;
+		
+		
+		taskDesc->m_mainMemoryPtr = reinterpret_cast<uint64_t> (spuStatus.m_lsMemory.p);
+		
+
+		break;
+	}
+	case CMD_GATHER_AND_PROCESS_PAIRLIST:
+		{
+			//get taskdescription
+			SpuGatherAndProcessPairsTaskDesc* taskDesc = (SpuGatherAndProcessPairsTaskDesc*) uiArgument0;
+
+			btAssert(taskDesc->taskId<m_activeSpuStatus.size());
+
+			//get status of SPU on which task should run
+			btSpuStatus&	spuStatus = m_activeSpuStatus[taskDesc->taskId];
+
+			//set data for spuStatus
+			spuStatus.m_commandId = uiCommand;
+			spuStatus.m_status = Spu_Status_Occupied; //set SPU as "occupied"
+			spuStatus.m_taskDesc.p = taskDesc; 
+			
+			//get context
+			context = data[taskDesc->taskId].context;
+			
+			
+			taskDesc->m_lsMemory = (CollisionTask_LocalStoreMemory*)spuStatus.m_lsMemory.p;
+			
+			break;
+		}
+	default:
+		{
+			///not implemented
+			btAssert(0);
+		}
+
+	};
+
+	
+	//write taskdescription in mailbox
+	unsigned int event = Spu_Mailbox_Event_Task;
+	spe_in_mbox_write(context, &event, 1, SPE_MBOX_ANY_NONBLOCKING);
+
+}
+
+///check for messages from SPUs
+void SpuLibspe2Support::waitForResponse(unsigned int *puiArgument0, unsigned int *puiArgument1)
+{
+	///We should wait for (one of) the first tasks to finish (or other SPU messages), and report its response
+	
+	///A possible response can be 'yes, SPU handled it', or 'no, please do a PPU fallback'
+	
+	btAssert(m_activeSpuStatus.size());
+
+	
+	int last = -1;
+	
+	//find an active spu/thread
+	while(last < 0)
+	{
+		for (int i=0;i<m_activeSpuStatus.size();i++)
+		{
+			if ( m_activeSpuStatus[i].m_status == Spu_Status_Free)
+			{
+				last = i;
+				break;
+			}
+		}
+		if(last < 0)
+			sched_yield();
+	}
+
+
+
+	btSpuStatus& spuStatus = m_activeSpuStatus[last];
+
+	///need to find an active spu
+	btAssert(last>=0);
+
+	
+
+	*puiArgument0 = spuStatus.m_taskId;
+	*puiArgument1 = spuStatus.m_status;
+
+
+}
+
+
+void SpuLibspe2Support::startSPU()
+{
+	this->internal_startSPU();
+}
+
+
+
+///start the spus group (can be called at the beginning of each frame, to make sure that the right SPU program is loaded)
+void SpuLibspe2Support::internal_startSPU()
+{
+	m_activeSpuStatus.resize(numThreads);
+	
+	
+	for (int i=0; i < numThreads; i++)
+	{
+		
+		if(data[i].context == NULL) 
+		{
+					
+			 /* Create context */
+			if ((data[i].context = spe_context_create(0, NULL)) == NULL)
+			{
+			      perror ("Failed creating context");
+		          exit(1);
+			}
+	
+			/* Load program into context */
+			if(spe_program_load(data[i].context, this->program))
+			{
+			      perror ("Failed loading program");
+		          exit(1);
+			}
+			
+			m_activeSpuStatus[i].m_status = Spu_Status_Startup; 
+			m_activeSpuStatus[i].m_taskId = i; 
+			m_activeSpuStatus[i].m_commandId = 0; 
+			m_activeSpuStatus[i].m_lsMemory.p = NULL; 
+			
+			
+			data[i].entry = SPE_DEFAULT_ENTRY;
+			data[i].flags = 0;
+			data[i].argp.p = &m_activeSpuStatus[i];
+			data[i].envp.p = NULL;
+			
+		    /* Create thread for each SPE context */
+			if (pthread_create(&data[i].pthread, NULL, &ppu_pthread_function, &(data[i]) ))
+			{
+			      perror ("Failed creating thread");
+		          exit(1);
+			}
+			/*
+			else
+			{
+				printf("started thread %d\n",i);
+			}*/
+		}		
+	}
+	
+	
+	for (int i=0; i < numThreads; i++)
+	{
+		if(data[i].context != NULL) 
+		{
+			while( m_activeSpuStatus[i].m_status == Spu_Status_Startup)
+			{
+				// wait for spu to set up
+				sched_yield();
+			}
+			printf("Spu %d is ready\n", i);
+		}
+	}
+}
+
+///tell the task scheduler we are done with the SPU tasks
+void SpuLibspe2Support::stopSPU()
+{
+	// wait for all threads to finish 
+	int i;
+	for ( i = 0; i < this->numThreads; i++ ) 
+	{ 
+		
+		unsigned int event = Spu_Mailbox_Event_Shutdown;
+		spe_context_ptr_t context = data[i].context;
+		spe_in_mbox_write(context, &event, 1, SPE_MBOX_ALL_BLOCKING);
+		pthread_join (data[i].pthread, NULL); 
+		
+	} 
+	// close SPE program 
+	spe_image_close(program); 
+	// destroy SPE contexts 
+	for ( i = 0; i < this->numThreads; i++ ) 
+	{ 
+		if(data[i].context != NULL)
+		{
+			spe_context_destroy (data[i].context);
+		}
+	} 
+	
+	m_activeSpuStatus.clear();
+	
+}
+
+
+
+#endif //USE_LIBSPE2
+
Index: /code/branches/physics/src/bullet/BulletMultiThreaded/SpuLibspe2Support.h
===================================================================
--- /code/branches/physics/src/bullet/BulletMultiThreaded/SpuLibspe2Support.h	(revision 1966)
+++ /code/branches/physics/src/bullet/BulletMultiThreaded/SpuLibspe2Support.h	(revision 1966)
@@ -0,0 +1,173 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2007 Erwin Coumans  http://bulletphysics.com
+
+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.
+*/
+
+
+#ifndef SPU_LIBSPE2_SUPPORT_H
+#define SPU_LIBSPE2_SUPPORT_H
+
+#include <LinearMath/btScalar.h> //for uint32_t etc.
+
+#ifdef USE_LIBSPE2
+
+#include <stdlib.h>
+#include <stdio.h>
+//#include "SpuNarrowPhaseCollisionTask/SpuGatheringCollisionTask.h"
+#include "PlatformDefinitions.h"
+
+
+//extern struct SpuGatherAndProcessPairsTaskDesc;
+
+enum
+{
+	Spu_Mailbox_Event_Nothing = 0,
+	Spu_Mailbox_Event_Task = 1,
+	Spu_Mailbox_Event_Shutdown = 2,
+	
+	Spu_Mailbox_Event_ForceDword = 0xFFFFFFFF
+	
+};
+
+enum
+{
+	Spu_Status_Free = 0,
+	Spu_Status_Occupied = 1,
+	Spu_Status_Startup = 2,
+	
+	Spu_Status_ForceDword = 0xFFFFFFFF
+	
+};
+
+
+struct btSpuStatus
+{
+	uint32_t	m_taskId;
+	uint32_t	m_commandId;
+	uint32_t	m_status;
+
+	addr64 m_taskDesc;
+	addr64 m_lsMemory;
+	
+}
+__attribute__ ((aligned (128)))
+;
+
+
+
+#ifndef __SPU__
+
+#include "LinearMath/btAlignedObjectArray.h"
+#include "SpuCollisionTaskProcess.h"
+#include "SpuSampleTaskProcess.h"
+#include "btThreadSupportInterface.h"
+#include <libspe2.h>
+#include <pthread.h>
+#include <sched.h>
+
+#define MAX_SPUS 4 
+
+typedef struct ppu_pthread_data 
+{
+	spe_context_ptr_t context;
+	pthread_t pthread;
+	unsigned int entry;
+	unsigned int flags;
+	addr64 argp;
+	addr64 envp;
+	spe_stop_info_t stopinfo;
+} ppu_pthread_data_t;
+
+
+static void *ppu_pthread_function(void *arg)
+{
+    ppu_pthread_data_t * datap = (ppu_pthread_data_t *)arg;
+    /*
+    int rc;
+    do 
+    {*/
+        spe_context_run(datap->context, &datap->entry, datap->flags, datap->argp.p, datap->envp.p, &datap->stopinfo);
+        if (datap->stopinfo.stop_reason == SPE_EXIT) 
+        {
+           if (datap->stopinfo.result.spe_exit_code != 0) 
+           {
+             perror("FAILED: SPE returned a non-zero exit status: \n");
+             exit(1);
+           }
+         } 
+        else 
+         {
+           perror("FAILED: SPE abnormally terminated\n");
+           exit(1);
+         }
+        
+        
+    //} while (rc > 0); // loop until exit or error, and while any stop & signal
+    pthread_exit(NULL);
+}
+
+
+
+
+
+
+///SpuLibspe2Support helps to initialize/shutdown libspe2, start/stop SPU tasks and communication
+class SpuLibspe2Support : public btThreadSupportInterface
+{
+
+	btAlignedObjectArray<btSpuStatus>	m_activeSpuStatus;
+	
+public:
+	//Setup and initialize SPU/CELL/Libspe2
+	SpuLibspe2Support(spe_program_handle_t *speprog,int numThreads);
+	
+	// SPE program handle ptr.
+	spe_program_handle_t *program;
+	
+	// SPE program data
+	ppu_pthread_data_t data[MAX_SPUS];
+	
+	//cleanup/shutdown Libspe2
+	~SpuLibspe2Support();
+
+	///send messages to SPUs
+	void sendRequest(uint32_t uiCommand, uint32_t uiArgument0, uint32_t uiArgument1=0);
+
+	//check for messages from SPUs
+	void waitForResponse(unsigned int *puiArgument0, unsigned int *puiArgument1);
+
+	//start the spus (can be called at the beginning of each frame, to make sure that the right SPU program is loaded)
+	virtual void startSPU();
+
+	//tell the task scheduler we are done with the SPU tasks
+	virtual void stopSPU();
+
+private:
+	
+	///start the spus (can be called at the beginning of each frame, to make sure that the right SPU program is loaded)
+	void internal_startSPU();
+	
+	
+	int numThreads;
+
+};
+
+#endif // NOT __SPU__
+
+#endif //USE_LIBSPE2
+
+#endif //SPU_LIBSPE2_SUPPORT_H
+
+
+
+
Index: /code/branches/physics/src/bullet/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuCollisionShapes.cpp
===================================================================
--- /code/branches/physics/src/bullet/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuCollisionShapes.cpp	(revision 1966)
+++ /code/branches/physics/src/bullet/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuCollisionShapes.cpp	(revision 1966)
@@ -0,0 +1,487 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+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.
+*/
+
+
+#include "SpuCollisionShapes.h"
+
+btPoint3 localGetSupportingVertexWithoutMargin(int shapeType, void* shape, const btVector3& localDir,struct	SpuConvexPolyhedronVertexData* convexVertexData)//, int *featureIndex)
+{
+    switch (shapeType)
+    {
+    case SPHERE_SHAPE_PROXYTYPE:
+        {
+            return btPoint3(0,0,0);
+        }
+	case BOX_SHAPE_PROXYTYPE:
+		{
+//			spu_printf("SPU: getSupport BOX_SHAPE_PROXYTYPE\n");
+			btConvexInternalShape* convexShape = (btConvexInternalShape*)shape;
+			const btVector3& halfExtents = convexShape->getImplicitShapeDimensions();
+			
+			return btPoint3(
+				localDir.getX() < 0.0f ? -halfExtents.x() : halfExtents.x(),
+							localDir.getY() < 0.0f ? -halfExtents.y() : halfExtents.y(),
+							localDir.getZ() < 0.0f ? -halfExtents.z() : halfExtents.z());
+		}
+
+	case TRIANGLE_SHAPE_PROXYTYPE:
+		{
+
+			btVector3 dir(localDir.getX(),localDir.getY(),localDir.getZ());
+			btVector3* vertices = (btVector3*)shape;
+			btVector3 dots(dir.dot(vertices[0]), dir.dot(vertices[1]), dir.dot(vertices[2]));
+	  		btVector3 sup = vertices[dots.maxAxis()];
+			return btPoint3(sup.getX(),sup.getY(),sup.getZ());
+			break;
+		}
+
+	case CYLINDER_SHAPE_PROXYTYPE:
+		{
+			btCylinderShape* cylShape = (btCylinderShape*)shape;
+
+			//mapping of halfextents/dimension onto radius/height depends on how cylinder local orientation is (upAxis)
+
+			btVector3 halfExtents = cylShape->getImplicitShapeDimensions();
+			btVector3 v(localDir.getX(),localDir.getY(),localDir.getZ());
+			
+			int cylinderUpAxis = cylShape->getUpAxis();
+			int XX(1),YY(0),ZZ(2);
+
+			switch (cylinderUpAxis)
+			{
+			case 0:
+				{
+					XX = 1;
+					YY = 0;
+					ZZ = 2;
+					break;
+				}
+			case 1:
+				{
+					XX = 0;
+					YY = 1;
+					ZZ = 2;
+				break;
+				}
+			case 2:
+				{
+					XX = 0;
+					YY = 2;
+					ZZ = 1;
+					break;
+				}
+			default:
+				btAssert(0);
+				//printf("SPU:localGetSupportingVertexWithoutMargin unknown Cylinder up-axis\n");
+			};
+
+			btScalar radius = halfExtents[XX];
+			btScalar halfHeight = halfExtents[cylinderUpAxis];
+
+			btVector3 tmp;
+			btScalar d ;
+
+			btScalar s = btSqrt(v[XX] * v[XX] + v[ZZ] * v[ZZ]);
+			if (s != btScalar(0.0))
+			{
+				d = radius / s;  
+				tmp[XX] = v[XX] * d;
+				tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight;
+				tmp[ZZ] = v[ZZ] * d;
+				return btPoint3(tmp.getX(),tmp.getY(),tmp.getZ());
+			}
+			else
+			{
+				tmp[XX] = radius;
+				tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight;
+				tmp[ZZ] = btScalar(0.0);
+				return btPoint3(tmp.getX(),tmp.getY(),tmp.getZ());
+			}
+		}
+
+	case CAPSULE_SHAPE_PROXYTYPE:
+	{
+		//spu_printf("SPU: todo: getSupport CAPSULE_SHAPE_PROXYTYPE\n");
+		btVector3 vec0(localDir.getX(),localDir.getY(),localDir.getZ());
+
+		btCapsuleShape* capsuleShape = (btCapsuleShape*)shape;
+		btVector3 halfExtents = capsuleShape->getImplicitShapeDimensions();
+		btScalar halfHeight = capsuleShape->getHalfHeight();
+		int capsuleUpAxis = capsuleShape->getUpAxis();
+
+		btScalar radius = capsuleShape->getRadius();
+		btVector3 supVec(0,0,0);
+
+		btScalar maxDot(btScalar(-1e30));
+
+		btVector3 vec = vec0;
+		btScalar lenSqr = vec.length2();
+		if (lenSqr < btScalar(0.0001))
+		{
+			vec.setValue(1,0,0);
+		} else
+		{
+			btScalar rlen = btScalar(1.) / btSqrt(lenSqr );
+			vec *= rlen;
+		}
+		btVector3 vtx;
+		btScalar newDot;
+		{
+			btVector3 pos(0,0,0);
+			pos[capsuleUpAxis] = halfHeight;
+
+			vtx = pos +vec*(radius);
+			newDot = vec.dot(vtx);
+			if (newDot > maxDot)
+			{
+				maxDot = newDot;
+				supVec = vtx;
+			}
+		}
+		{
+			btVector3 pos(0,0,0);
+			pos[capsuleUpAxis] = -halfHeight;
+
+			vtx = pos +vec*(radius);
+			newDot = vec.dot(vtx);
+			if (newDot > maxDot)
+			{
+				maxDot = newDot;
+				supVec = vtx;
+			}
+		}
+		return btPoint3(supVec.getX(),supVec.getY(),supVec.getZ());
+		break;
+	};
+
+	case CONVEX_HULL_SHAPE_PROXYTYPE:
+		{
+			//spu_printf("SPU: todo: getSupport CONVEX_HULL_SHAPE_PROXYTYPE\n");
+
+		
+
+			btPoint3* points = 0;
+			int numPoints = 0;
+			points = convexVertexData->gConvexPoints;
+			numPoints = convexVertexData->gNumConvexPoints;
+
+		//	spu_printf("numPoints = %d\n",numPoints);
+
+			btVector3 supVec(btScalar(0.),btScalar(0.),btScalar(0.));
+			btScalar newDot,maxDot = btScalar(-1e30);
+
+			btVector3 vec0(localDir.getX(),localDir.getY(),localDir.getZ());
+			btVector3 vec = vec0;
+			btScalar lenSqr = vec.length2();
+			if (lenSqr < btScalar(0.0001))
+			{
+				vec.setValue(1,0,0);
+			} else
+			{
+				btScalar rlen = btScalar(1.) / btSqrt(lenSqr );
+				vec *= rlen;
+			}
+
+
+			for (int i=0;i<numPoints;i++)
+			{
+				btPoint3 vtx = points[i];// * m_localScaling;
+
+				newDot = vec.dot(vtx);
+				if (newDot > maxDot)
+				{
+					maxDot = newDot;
+					supVec = vtx;
+				}
+			}
+			return btPoint3(supVec.getX(),supVec.getY(),supVec.getZ());
+
+			break;
+		};
+
+    default:
+
+		//spu_printf("SPU:(type %i) missing support function\n",shapeType);
+
+		
+#if __ASSERT
+       // spu_printf("localGetSupportingVertexWithoutMargin() - Unsupported bound type: %d.\n", shapeType);
+#endif // __ASSERT
+        return btPoint3(0.f, 0.f, 0.f);
+    }
+}
+
+void computeAabb (btVector3& aabbMin, btVector3& aabbMax, btConvexInternalShape* convexShape, ppu_address_t convexShapePtr, int shapeType, btTransform xform)
+{
+	//calculate the aabb, given the types...
+	switch (shapeType)
+	{
+	case CYLINDER_SHAPE_PROXYTYPE:
+		/* fall through */
+	case BOX_SHAPE_PROXYTYPE:
+	{
+		float margin=convexShape->getMarginNV();
+		btVector3 halfExtents = convexShape->getImplicitShapeDimensions();
+		halfExtents += btVector3(margin,margin,margin);
+		btTransform& t = xform;
+		btMatrix3x3 abs_b = t.getBasis().absolute();  
+		btPoint3 center = t.getOrigin();
+		btVector3 extent = btVector3(abs_b[0].dot(halfExtents),abs_b[1].dot(halfExtents),abs_b[2].dot(halfExtents));
+		
+		aabbMin = center - extent;
+		aabbMax = center + extent;
+		break;
+	}
+	case CAPSULE_SHAPE_PROXYTYPE:
+	{
+		float margin=convexShape->getMarginNV();
+		btVector3 halfExtents = convexShape->getImplicitShapeDimensions();
+		//add the radius to y-axis to get full height
+		btScalar radius = halfExtents[0];
+		halfExtents[1] += radius;
+		halfExtents += btVector3(margin,margin,margin);
+#if 0
+		int capsuleUpAxis = convexShape->getUpAxis();
+		btScalar halfHeight = convexShape->getHalfHeight();
+		btScalar radius = convexShape->getRadius();
+		halfExtents[capsuleUpAxis] = radius + halfHeight;
+#endif
+		btTransform& t = xform;
+		btMatrix3x3 abs_b = t.getBasis().absolute();  
+		btPoint3 center = t.getOrigin();
+		btVector3 extent = btVector3(abs_b[0].dot(halfExtents),abs_b[1].dot(halfExtents),abs_b[2].dot(halfExtents));
+		
+		aabbMin = center - extent;
+		aabbMax = center + extent;
+		break;
+	}
+	case SPHERE_SHAPE_PROXYTYPE:
+	{
+		float radius = convexShape->getImplicitShapeDimensions().getX();// * convexShape->getLocalScaling().getX();
+		float margin = radius + convexShape->getMarginNV();
+		btTransform& t = xform;
+		const btVector3& center = t.getOrigin();
+		btVector3 extent(margin,margin,margin);
+		aabbMin = center - extent;
+		aabbMax = center + extent;
+		break;
+	}
+	case CONVEX_HULL_SHAPE_PROXYTYPE:
+	{
+		ATTRIBUTE_ALIGNED16(char convexHullShape0[sizeof(btConvexHullShape)]);
+		cellDmaGet(&convexHullShape0, convexShapePtr  , sizeof(btConvexHullShape), DMA_TAG(1), 0, 0);
+		cellDmaWaitTagStatusAll(DMA_MASK(1));
+		btConvexHullShape* localPtr = (btConvexHullShape*)&convexHullShape0;
+		btTransform& t = xform;
+		btScalar margin = convexShape->getMarginNV();
+		localPtr->getNonvirtualAabb(t,aabbMin,aabbMax,margin);
+		//spu_printf("SPU convex aabbMin=%f,%f,%f=\n",aabbMin.getX(),aabbMin.getY(),aabbMin.getZ());
+		//spu_printf("SPU convex aabbMax=%f,%f,%f=\n",aabbMax.getX(),aabbMax.getY(),aabbMax.getZ());
+		break;
+	}
+	default:
+		{
+	//	spu_printf("SPU: unsupported shapetype %d in AABB calculation\n");
+		}
+	};
+}
+
+void dmaBvhShapeData (bvhMeshShape_LocalStoreMemory* bvhMeshShape, btBvhTriangleMeshShape* triMeshShape)
+{
+	register int dmaSize;
+	register ppu_address_t	dmaPpuAddress2;
+
+	dmaSize = sizeof(btTriangleIndexVertexArray);
+	dmaPpuAddress2 = reinterpret_cast<ppu_address_t>(triMeshShape->getMeshInterface());
+	//	spu_printf("trimeshShape->getMeshInterface() == %llx\n",dmaPpuAddress2);
+#ifdef __SPU__
+	cellDmaGet(&bvhMeshShape->gTriangleMeshInterfaceStorage, dmaPpuAddress2  , dmaSize, DMA_TAG(1), 0, 0);
+	bvhMeshShape->gTriangleMeshInterfacePtr = &bvhMeshShape->gTriangleMeshInterfaceStorage;
+#else
+	bvhMeshShape->gTriangleMeshInterfacePtr = (btTriangleIndexVertexArray*)cellDmaGetReadOnly(&bvhMeshShape->gTriangleMeshInterfaceStorage, dmaPpuAddress2  , dmaSize, DMA_TAG(1), 0, 0);
+#endif
+
+	//cellDmaWaitTagStatusAll(DMA_MASK(1));
+	
+	///now DMA over the BVH
+	
+	dmaSize = sizeof(btOptimizedBvh);
+	dmaPpuAddress2 = reinterpret_cast<ppu_address_t>(triMeshShape->getOptimizedBvh());
+	//spu_printf("trimeshShape->getOptimizedBvh() == %llx\n",dmaPpuAddress2);
+	cellDmaGet(&bvhMeshShape->gOptimizedBvh, dmaPpuAddress2  , dmaSize, DMA_TAG(2), 0, 0);
+	//cellDmaWaitTagStatusAll(DMA_MASK(2));
+	cellDmaWaitTagStatusAll(DMA_MASK(1) | DMA_MASK(2));
+}
+
+void dmaBvhIndexedMesh (btIndexedMesh* IndexMesh, IndexedMeshArray& indexArray, int index, uint32_t dmaTag)
+{		
+	cellDmaGet(IndexMesh, (ppu_address_t)&indexArray[index]  , sizeof(btIndexedMesh), DMA_TAG(dmaTag), 0, 0);
+	
+}
+
+void dmaBvhSubTreeHeaders (btBvhSubtreeInfo* subTreeHeaders, ppu_address_t subTreePtr, int batchSize, uint32_t dmaTag)
+{
+	cellDmaGet(subTreeHeaders, subTreePtr, batchSize * sizeof(btBvhSubtreeInfo), DMA_TAG(dmaTag), 0, 0);
+}
+
+void dmaBvhSubTreeNodes (btQuantizedBvhNode* nodes, const btBvhSubtreeInfo& subtree, QuantizedNodeArray&	nodeArray, int dmaTag)
+{
+	cellDmaGet(nodes, reinterpret_cast<ppu_address_t>(&nodeArray[subtree.m_rootNodeIndex]) , subtree.m_subtreeSize* sizeof(btQuantizedBvhNode), DMA_TAG(2), 0, 0);
+}
+
+///getShapeTypeSize could easily be optimized, but it is not likely a bottleneck
+int		getShapeTypeSize(int shapeType)
+{
+
+
+	switch (shapeType)
+	{
+	case CYLINDER_SHAPE_PROXYTYPE:
+		{
+			int shapeSize = sizeof(btCylinderShape);
+			btAssert(shapeSize < MAX_SHAPE_SIZE);
+			return shapeSize;
+		}
+	case BOX_SHAPE_PROXYTYPE:
+		{
+			int shapeSize = sizeof(btBoxShape);
+			btAssert(shapeSize < MAX_SHAPE_SIZE);
+			return shapeSize;
+		}
+	case SPHERE_SHAPE_PROXYTYPE:
+		{
+			int shapeSize = sizeof(btSphereShape);
+			btAssert(shapeSize < MAX_SHAPE_SIZE);
+			return shapeSize;
+		}
+	case TRIANGLE_MESH_SHAPE_PROXYTYPE:
+		{
+			int shapeSize = sizeof(btBvhTriangleMeshShape);
+			btAssert(shapeSize < MAX_SHAPE_SIZE);
+			return shapeSize;
+		}
+	case CAPSULE_SHAPE_PROXYTYPE:
+		{
+			int shapeSize = sizeof(btCapsuleShape);
+			btAssert(shapeSize < MAX_SHAPE_SIZE);
+			return shapeSize;
+		}
+
+	case CONVEX_HULL_SHAPE_PROXYTYPE:
+		{
+			int shapeSize = sizeof(btConvexHullShape);
+			btAssert(shapeSize < MAX_SHAPE_SIZE);
+			return shapeSize;
+		}
+
+	case COMPOUND_SHAPE_PROXYTYPE:
+		{
+			int shapeSize = sizeof(btCompoundShape);
+			btAssert(shapeSize < MAX_SHAPE_SIZE);
+			return shapeSize;
+		}
+
+	default:
+		btAssert(0);
+		//unsupported shapetype, please add here
+		return 0;
+	}
+}
+
+void dmaConvexVertexData (SpuConvexPolyhedronVertexData* convexVertexData, btConvexHullShape* convexShapeSPU)
+{
+	convexVertexData->gNumConvexPoints = convexShapeSPU->getNumPoints();
+	if (convexVertexData->gNumConvexPoints>MAX_NUM_SPU_CONVEX_POINTS)
+	{
+		btAssert(0);
+	//	spu_printf("SPU: Error: MAX_NUM_SPU_CONVEX_POINTS(%d) exceeded: %d\n",MAX_NUM_SPU_CONVEX_POINTS,convexVertexData->gNumConvexPoints);
+		return;
+	}
+			
+	register int dmaSize = convexVertexData->gNumConvexPoints*sizeof(btPoint3);
+	ppu_address_t pointsPPU = (ppu_address_t) convexShapeSPU->getPoints();
+	cellDmaGet(&convexVertexData->g_convexPointBuffer[0], pointsPPU  , dmaSize, DMA_TAG(2), 0, 0);
+}
+
+void dmaCollisionShape (void* collisionShapeLocation, ppu_address_t collisionShapePtr, uint32_t dmaTag, int shapeType)
+{
+	register int dmaSize = getShapeTypeSize(shapeType);
+	cellDmaGet(collisionShapeLocation, collisionShapePtr  , dmaSize, DMA_TAG(dmaTag), 0, 0);
+	//cellDmaWaitTagStatusAll(DMA_MASK(dmaTag));
+}
+
+void dmaCompoundShapeInfo (CompoundShape_LocalStoreMemory* compoundShapeLocation, btCompoundShape* spuCompoundShape, uint32_t dmaTag)
+{
+	register int dmaSize;
+	register	ppu_address_t	dmaPpuAddress2;
+	int childShapeCount = spuCompoundShape->getNumChildShapes();
+	dmaSize = childShapeCount * sizeof(btCompoundShapeChild);
+	dmaPpuAddress2 = (ppu_address_t)spuCompoundShape->getChildList();
+	cellDmaGet(&compoundShapeLocation->gSubshapes[0], dmaPpuAddress2, dmaSize, DMA_TAG(dmaTag), 0, 0);
+}
+
+void dmaCompoundSubShapes (CompoundShape_LocalStoreMemory* compoundShapeLocation, btCompoundShape* spuCompoundShape, uint32_t dmaTag)
+{
+	int childShapeCount = spuCompoundShape->getNumChildShapes();
+	int i;
+	// DMA all the subshapes 
+	for ( i = 0; i < childShapeCount; ++i)
+	{
+		btCompoundShapeChild& childShape = compoundShapeLocation->gSubshapes[i];
+		dmaCollisionShape (&compoundShapeLocation->gSubshapeShape[i],(ppu_address_t)childShape.m_childShape, dmaTag, childShape.m_childShapeType);
+	}
+}
+
+
+void	spuWalkStacklessQuantizedTree(btNodeOverlapCallback* nodeCallback,unsigned short int* quantizedQueryAabbMin,unsigned short int* quantizedQueryAabbMax,const btQuantizedBvhNode* rootNode,int startNodeIndex,int endNodeIndex)
+{
+
+	int curIndex = startNodeIndex;
+	int walkIterations = 0;
+	int subTreeSize = endNodeIndex - startNodeIndex;
+
+	int escapeIndex;
+
+	unsigned int aabbOverlap, isLeafNode;
+
+	while (curIndex < endNodeIndex)
+	{
+		//catch bugs in tree data
+		assert (walkIterations < subTreeSize);
+
+		walkIterations++;
+		aabbOverlap = spuTestQuantizedAabbAgainstQuantizedAabb(quantizedQueryAabbMin,quantizedQueryAabbMax,rootNode->m_quantizedAabbMin,rootNode->m_quantizedAabbMax);
+		isLeafNode = rootNode->isLeafNode();
+
+		if (isLeafNode && aabbOverlap)
+		{
+			//printf("overlap with node %d\n",rootNode->getTriangleIndex());
+			nodeCallback->processNode(0,rootNode->getTriangleIndex());
+			//			spu_printf("SPU: overlap detected with triangleIndex:%d\n",rootNode->getTriangleIndex());
+		} 
+
+		if (aabbOverlap || isLeafNode)
+		{
+			rootNode++;
+			curIndex++;
+		} else
+		{
+			escapeIndex = rootNode->getEscapeIndex();
+			rootNode += escapeIndex;
+			curIndex += escapeIndex;
+		}
+	}
+
+}
Index: /code/branches/physics/src/bullet/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuCollisionShapes.h
===================================================================
--- /code/branches/physics/src/bullet/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuCollisionShapes.h	(revision 1966)
+++ /code/branches/physics/src/bullet/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuCollisionShapes.h	(revision 1966)
@@ -0,0 +1,102 @@
+#ifndef __SPU_COLLISION_SHAPES_H
+#define __SPU_COLLISION_SHAPES_H
+
+#include "../SpuDoubleBuffer.h"
+
+#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
+#include "BulletCollision/CollisionShapes/btConvexInternalShape.h"
+#include "BulletCollision/CollisionShapes/btCylinderShape.h"
+
+#include "BulletCollision/CollisionShapes/btOptimizedBvh.h"
+#include "BulletCollision/CollisionShapes/btTriangleIndexVertexArray.h"
+#include "BulletCollision/CollisionShapes/btSphereShape.h"
+
+#include "BulletCollision/CollisionShapes/btCapsuleShape.h"
+
+#include "BulletCollision/CollisionShapes/btConvexShape.h"
+#include "BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h"
+#include "BulletCollision/CollisionShapes/btConvexHullShape.h"
+#include "BulletCollision/CollisionShapes/btCompoundShape.h"
+
+#define MAX_NUM_SPU_CONVEX_POINTS 128
+
+struct	SpuConvexPolyhedronVertexData
+{
+	void*	gSpuConvexShapePtr;
+	btPoint3* gConvexPoints;
+	int gNumConvexPoints;
+	ATTRIBUTE_ALIGNED16(btPoint3 g_convexPointBuffer[MAX_NUM_SPU_CONVEX_POINTS]);
+};
+
+#define MAX_SHAPE_SIZE 256
+
+struct CollisionShape_LocalStoreMemory
+{
+	ATTRIBUTE_ALIGNED16(char collisionShape[MAX_SHAPE_SIZE]);
+};
+
+struct CompoundShape_LocalStoreMemory
+{
+	// Compound data
+#define MAX_SPU_COMPOUND_SUBSHAPES 16
+	ATTRIBUTE_ALIGNED16(btCompoundShapeChild gSubshapes[MAX_SPU_COMPOUND_SUBSHAPES]);
+	ATTRIBUTE_ALIGNED16(char gSubshapeShape[MAX_SPU_COMPOUND_SUBSHAPES][MAX_SHAPE_SIZE]);
+};
+
+struct bvhMeshShape_LocalStoreMemory
+{
+	//ATTRIBUTE_ALIGNED16(btOptimizedBvh	gOptimizedBvh);
+	ATTRIBUTE_ALIGNED16(char gOptimizedBvh[sizeof(btOptimizedBvh)+16]);
+	btOptimizedBvh*	getOptimizedBvh()
+	{
+		return (btOptimizedBvh*) gOptimizedBvh;
+	}
+
+	ATTRIBUTE_ALIGNED16(btTriangleIndexVertexArray	gTriangleMeshInterfaceStorage);
+	btTriangleIndexVertexArray*	gTriangleMeshInterfacePtr;
+	///only a single mesh part for now, we can add support for multiple parts, but quantized trees don't support this at the moment 
+	ATTRIBUTE_ALIGNED16(btIndexedMesh	gIndexMesh);
+	#define MAX_SPU_SUBTREE_HEADERS 32
+	//1024
+	ATTRIBUTE_ALIGNED16(btBvhSubtreeInfo	gSubtreeHeaders[MAX_SPU_SUBTREE_HEADERS]);
+	ATTRIBUTE_ALIGNED16(btQuantizedBvhNode	gSubtreeNodes[MAX_SUBTREE_SIZE_IN_BYTES/sizeof(btQuantizedBvhNode)]);
+};
+
+
+btPoint3 localGetSupportingVertexWithoutMargin(int shapeType, void* shape, const btVector3& localDir,struct	SpuConvexPolyhedronVertexData* convexVertexData);//, int *featureIndex)
+void computeAabb (btVector3& aabbMin, btVector3& aabbMax, btConvexInternalShape* convexShape, ppu_address_t convexShapePtr, int shapeType, btTransform xform);
+void dmaBvhShapeData (bvhMeshShape_LocalStoreMemory* bvhMeshShape, btBvhTriangleMeshShape* triMeshShape);
+void dmaBvhIndexedMesh (btIndexedMesh* IndexMesh, IndexedMeshArray& indexArray, int index, uint32_t dmaTag);
+void dmaBvhSubTreeHeaders (btBvhSubtreeInfo* subTreeHeaders, ppu_address_t subTreePtr, int batchSize, uint32_t dmaTag);
+void dmaBvhSubTreeNodes (btQuantizedBvhNode* nodes, const btBvhSubtreeInfo& subtree, QuantizedNodeArray&	nodeArray, int dmaTag);
+
+int  getShapeTypeSize(int shapeType);
+void dmaConvexVertexData (SpuConvexPolyhedronVertexData* convexVertexData, btConvexHullShape* convexShapeSPU);
+void dmaCollisionShape (void* collisionShapeLocation, ppu_address_t collisionShapePtr, uint32_t dmaTag, int shapeType);
+void dmaCompoundShapeInfo (CompoundShape_LocalStoreMemory* compoundShapeLocation, btCompoundShape* spuCompoundShape, uint32_t dmaTag);
+void dmaCompoundSubShapes (CompoundShape_LocalStoreMemory* compoundShapeLocation, btCompoundShape* spuCompoundShape, uint32_t dmaTag);
+
+#define USE_BRANCHFREE_TEST 1
+#ifdef USE_BRANCHFREE_TEST
+SIMD_FORCE_INLINE unsigned int spuTestQuantizedAabbAgainstQuantizedAabb(unsigned short int* aabbMin1,unsigned short int* aabbMax1,const unsigned short int* aabbMin2,const unsigned short int* aabbMax2)
+{		
+	return btSelect((unsigned)((aabbMin1[0] <= aabbMax2[0]) & (aabbMax1[0] >= aabbMin2[0])
+		& (aabbMin1[2] <= aabbMax2[2]) & (aabbMax1[2] >= aabbMin2[2])
+		& (aabbMin1[1] <= aabbMax2[1]) & (aabbMax1[1] >= aabbMin2[1])),
+		1, 0);
+}
+#else
+
+unsigned int spuTestQuantizedAabbAgainstQuantizedAabb(const unsigned short int* aabbMin1,const unsigned short int* aabbMax1,const unsigned short int* aabbMin2,const unsigned short int*  aabbMax2)
+{
+	unsigned int overlap = 1;
+	overlap = (aabbMin1[0] > aabbMax2[0] || aabbMax1[0] < aabbMin2[0]) ? 0 : overlap;
+	overlap = (aabbMin1[2] > aabbMax2[2] || aabbMax1[2] < aabbMin2[2]) ? 0 : overlap;
+	overlap = (aabbMin1[1] > aabbMax2[1] || aabbMax1[1] < aabbMin2[1]) ? 0 : overlap;
+	return overlap;
+}
+#endif
+
+void	spuWalkStacklessQuantizedTree(btNodeOverlapCallback* nodeCallback,unsigned short int* quantizedQueryAabbMin,unsigned short int* quantizedQueryAabbMax,const btQuantizedBvhNode* rootNode,int startNodeIndex,int endNodeIndex);
+
+#endif
Index: /code/branches/physics/src/bullet/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuContactResult.cpp
===================================================================
--- /code/branches/physics/src/bullet/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuContactResult.cpp	(revision 1966)
+++ /code/branches/physics/src/bullet/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuContactResult.cpp	(revision 1966)
@@ -0,0 +1,227 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+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.
+*/
+
+#include "SpuContactResult.h"
+
+//#define DEBUG_SPU_COLLISION_DETECTION 1
+
+
+SpuContactResult::SpuContactResult()
+{
+	m_manifoldAddress = 0;
+	m_spuManifold = NULL;
+	m_RequiresWriteBack = false;
+}
+
+ SpuContactResult::~SpuContactResult()
+{
+	g_manifoldDmaExport.swapBuffers();
+}
+
+ 	///User can override this material combiner by implementing gContactAddedCallback and setting body0->m_collisionFlags |= btCollisionObject::customMaterialCallback;
+inline btScalar	calculateCombinedFriction(btScalar friction0,btScalar friction1)
+{
+	btScalar friction = friction0*friction1;
+
+	const btScalar MAX_FRICTION  = btScalar(10.);
+
+	if (friction < -MAX_FRICTION)
+		friction = -MAX_FRICTION;
+	if (friction > MAX_FRICTION)
+		friction = MAX_FRICTION;
+	return friction;
+
+}
+
+inline btScalar	calculateCombinedRestitution(btScalar restitution0,btScalar restitution1)
+{
+	return restitution0*restitution1;
+}
+
+
+
+ void	SpuContactResult::setContactInfo(btPersistentManifold* spuManifold, ppu_address_t	manifoldAddress,const btTransform& worldTrans0,const btTransform& worldTrans1, btScalar restitution0,btScalar restitution1, btScalar friction0,btScalar friction1, bool isSwapped)
+ {
+	//spu_printf("SpuContactResult::setContactInfo ManifoldAddress: %lu\n", manifoldAddress);
+	m_rootWorldTransform0 = worldTrans0;
+	m_rootWorldTransform1 = worldTrans1;
+	m_manifoldAddress = manifoldAddress;    
+	m_spuManifold = spuManifold;
+
+	m_combinedFriction = calculateCombinedFriction(friction0,friction1);
+	m_combinedRestitution = calculateCombinedRestitution(restitution0,restitution1);
+	m_isSwapped = isSwapped;
+ }
+
+ void SpuContactResult::setShapeIdentifiers(int partId0,int index0,	int partId1,int index1)
+ {
+	
+ }
+	
+
+
+ ///return true if it requires a dma transfer back
+bool ManifoldResultAddContactPoint(const btVector3& normalOnBInWorld,
+								   const btVector3& pointInWorld,
+								   float depth,
+								   btPersistentManifold* manifoldPtr,
+								   btTransform& transA,
+								   btTransform& transB,
+									btScalar	combinedFriction,
+									btScalar	combinedRestitution,
+								   bool isSwapped)
+{
+	
+	float contactTreshold = manifoldPtr->getContactBreakingThreshold();
+
+	//spu_printf("SPU: add contactpoint, depth:%f, contactTreshold %f, manifoldPtr %llx\n",depth,contactTreshold,manifoldPtr);
+
+#ifdef DEBUG_SPU_COLLISION_DETECTION
+	spu_printf("SPU: contactTreshold %f\n",contactTreshold);
+#endif //DEBUG_SPU_COLLISION_DETECTION
+	if (depth > manifoldPtr->getContactBreakingThreshold())
+		return false;
+
+	//provide inverses or just calculate?
+	btTransform transAInv = transA.inverse();//m_body0->m_cachedInvertedWorldTransform;
+	btTransform transBInv= transB.inverse();//m_body1->m_cachedInvertedWorldTransform;
+
+	btVector3 pointA;
+	btVector3 localA;
+	btVector3 localB;
+	btVector3 normal;
+
+	if (isSwapped)
+	{
+		normal = normalOnBInWorld * -1;
+		pointA = pointInWorld + normal * depth;
+		localA = transAInv(pointA );
+		localB = transBInv(pointInWorld);
+		/*localA = transBInv(pointA );
+		localB = transAInv(pointInWorld);*/
+	}
+	else
+	{
+		normal = normalOnBInWorld;
+		pointA = pointInWorld + normal * depth;
+		localA = transAInv(pointA );
+		localB = transBInv(pointInWorld);
+	}
+
+	btManifoldPoint newPt(localA,localB,normal,depth);
+
+	int insertIndex = manifoldPtr->getCacheEntry(newPt);
+	if (insertIndex >= 0)
+	{
+//		manifoldPtr->replaceContactPoint(newPt,insertIndex);
+//		return true;
+
+#ifdef DEBUG_SPU_COLLISION_DETECTION
+		spu_printf("SPU: same contact detected, nothing done\n");
+#endif //DEBUG_SPU_COLLISION_DETECTION
+		// This is not needed, just use the old info! saves a DMA transfer as well
+	} else
+	{
+
+		newPt.m_combinedFriction = combinedFriction;
+		newPt.m_combinedRestitution = combinedRestitution;
+
+		/*
+		//potential TODO: SPU callbacks, either immediate (local on the SPU), or deferred
+		//User can override friction and/or restitution
+		if (gContactAddedCallback &&
+			//and if either of the two bodies requires custom material
+			 ((m_body0->m_collisionFlags & btCollisionObject::customMaterialCallback) ||
+			   (m_body1->m_collisionFlags & btCollisionObject::customMaterialCallback)))
+		{
+			//experimental feature info, for per-triangle material etc.
+			(*gContactAddedCallback)(newPt,m_body0,m_partId0,m_index0,m_body1,m_partId1,m_index1);
+		}
+		*/
+		manifoldPtr->addManifoldPoint(newPt);
+		return true;
+
+	}
+	return false;
+	
+}
+
+
+void SpuContactResult::writeDoubleBufferedManifold(btPersistentManifold* lsManifold, btPersistentManifold* mmManifold)
+{
+    memcpy(g_manifoldDmaExport.getFront(),lsManifold,sizeof(btPersistentManifold));
+
+    g_manifoldDmaExport.swapBuffers();
+    uint64_t mmAddr = (uint32_t)mmManifold;
+    g_manifoldDmaExport.backBufferDmaPut(mmAddr, sizeof(btPersistentManifold), DMA_TAG(9));
+	// Should there be any kind of wait here?  What if somebody tries to use this tag again?  What if we call this function again really soon?
+	//no, the swapBuffers does the wait
+}
+
+void SpuContactResult::addContactPoint(const btVector3& normalOnBInWorld,const btPoint3& pointInWorld,float depth)
+{
+	//spu_printf("*** SpuContactResult::addContactPoint: depth = %f\n",depth);
+
+#ifdef DEBUG_SPU_COLLISION_DETECTION
+ //   int sman = sizeof(rage::phManifold);
+//	spu_printf("sizeof_manifold = %i\n",sman);
+#endif //DEBUG_SPU_COLLISION_DETECTION
+
+	btPersistentManifold* localManifold = m_spuManifold;
+
+	btVector3	normalB(normalOnBInWorld.getX(),normalOnBInWorld.getY(),normalOnBInWorld.getZ());
+	btVector3	pointWrld(pointInWorld.getX(),pointInWorld.getY(),pointInWorld.getZ());
+
+	//process the contact point
+	const bool retVal = ManifoldResultAddContactPoint(normalB,
+		pointWrld,
+		depth,
+		localManifold,
+		m_rootWorldTransform0,
+		m_rootWorldTransform1,
+		m_combinedFriction,
+		m_combinedRestitution,
+		m_isSwapped);
+	m_RequiresWriteBack = m_RequiresWriteBack || retVal;
+}
+
+void SpuContactResult::flush()
+{
+
+	if (m_spuManifold && m_spuManifold->getNumContacts())
+	{
+		m_spuManifold->refreshContactPoints(m_rootWorldTransform0,m_rootWorldTransform1);
+		m_RequiresWriteBack = true;
+	}
+
+
+	if (m_RequiresWriteBack)
+	{
+#ifdef DEBUG_SPU_COLLISION_DETECTION
+		spu_printf("SPU: Start SpuContactResult::flush (Put) DMA\n");
+		spu_printf("Num contacts:%d\n", m_spuManifold->getNumContacts());
+		spu_printf("Manifold address: %llu\n", m_manifoldAddress);
+#endif //DEBUG_SPU_COLLISION_DETECTION
+	//	spu_printf("writeDoubleBufferedManifold\n");
+		writeDoubleBufferedManifold(m_spuManifold, (btPersistentManifold*)m_manifoldAddress);
+#ifdef DEBUG_SPU_COLLISION_DETECTION
+		spu_printf("SPU: Finished (Put) DMA\n");
+#endif //DEBUG_SPU_COLLISION_DETECTION
+	}
+	m_spuManifold = NULL;
+	m_RequiresWriteBack = false;
+}
+
+
Index: /code/branches/physics/src/bullet/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuContactResult.h
===================================================================
--- /code/branches/physics/src/bullet/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuContactResult.h	(revision 1966)
+++ /code/branches/physics/src/bullet/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuContactResult.h	(revision 1966)
@@ -0,0 +1,113 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+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.
+*/
+
+#ifndef SPU_CONTACT_RESULT2_H
+#define SPU_CONTACT_RESULT2_H
+
+
+#ifndef WIN32
+#include <stdint.h>
+#endif
+
+
+
+#include "../SpuDoubleBuffer.h"
+
+
+#include "LinearMath/btTransform.h"
+#include "LinearMath/btPoint3.h"
+
+
+#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
+
+
+struct SpuCollisionPairInput
+{
+	ppu_address_t m_collisionShapes[2];
+	void*	m_spuCollisionShapes[2];
+
+	ppu_address_t m_persistentManifoldPtr;
+	btVector3	m_primitiveDimensions0;
+	btVector3	m_primitiveDimensions1;
+	int		m_shapeType0;
+	int		m_shapeType1;	
+	float	m_collisionMargin0;
+	float	m_collisionMargin1;
+
+	btTransform	m_worldTransform0;
+	btTransform m_worldTransform1;
+	
+	bool	m_isSwapped;
+	bool    m_useEpa;
+};
+
+
+struct SpuClosestPointInput
+{
+    SpuClosestPointInput()
+        :m_maximumDistanceSquared(float(1e30)),
+        m_stackAlloc(0)
+    {
+    }
+
+    btTransform m_transformA;
+    btTransform m_transformB;
+    float	m_maximumDistanceSquared;
+    class	btStackAlloc* m_stackAlloc;
+	struct SpuConvexPolyhedronVertexData* m_convexVertexData[2];
+};
+
+///SpuContactResult exports the contact points using double-buffered DMA transfers, only when needed
+///So when an existing contact point is duplicated, no transfer/refresh is performed.
+class SpuContactResult
+{
+    btTransform		m_rootWorldTransform0;
+	btTransform		m_rootWorldTransform1;
+	ppu_address_t	m_manifoldAddress;
+
+    btPersistentManifold* m_spuManifold;
+	bool m_RequiresWriteBack;
+	btScalar	m_combinedFriction;
+	btScalar	m_combinedRestitution;
+	
+	bool m_isSwapped;
+
+	DoubleBuffer<btPersistentManifold, 1> g_manifoldDmaExport;
+
+	public:
+		SpuContactResult();
+		virtual ~SpuContactResult();
+
+		btPersistentManifold*	GetSpuManifold() const
+		{
+			return m_spuManifold;
+		}
+
+		virtual void setShapeIdentifiers(int partId0,int index0,	int partId1,int index1);
+
+		void	setContactInfo(btPersistentManifold* spuManifold, ppu_address_t	manifoldAddress,const btTransform& worldTrans0,const btTransform& worldTrans1, btScalar restitution0,btScalar restitution1, btScalar friction0,btScalar friction01, bool isSwapped);
+
+
+        void writeDoubleBufferedManifold(btPersistentManifold* lsManifold, btPersistentManifold* mmManifold);
+
+        virtual void addContactPoint(const btVector3& normalOnBInWorld,const btPoint3& pointInWorld,float depth);
+
+		void flush();
+};
+
+
+
+#endif //SPU_CONTACT_RESULT2_H
+
Index: /code/branches/physics/src/bullet/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuConvexPenetrationDepthSolver.h
===================================================================
--- /code/branches/physics/src/bullet/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuConvexPenetrationDepthSolver.h	(revision 1966)
+++ /code/branches/physics/src/bullet/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuConvexPenetrationDepthSolver.h	(revision 1966)
@@ -0,0 +1,52 @@
+
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+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.
+*/
+
+
+#ifndef CONVEX_PENETRATION_DEPTH_H
+#define CONVEX_PENETRATION_DEPTH_H
+
+
+
+class btStackAlloc;
+class btIDebugDraw;
+class SpuVoronoiSimplexSolver;
+
+#include <LinearMath/btTransform.h>
+#include <LinearMath/btPoint3.h>
+
+
+///ConvexPenetrationDepthSolver provides an interface for penetration depth calculation.
+class SpuConvexPenetrationDepthSolver
+{
+public:	
+	
+	virtual ~SpuConvexPenetrationDepthSolver() {};
+	virtual bool calcPenDepth( SpuVoronoiSimplexSolver& simplexSolver,
+	        void* convexA,void* convexB,int shapeTypeA, int shapeTypeB, float marginA, float marginB,
+            btTransform& transA,const btTransform& transB,
+			btVector3& v, btPoint3& pa, btPoint3& pb,
+			class btIDebugDraw* debugDraw,btStackAlloc* stackAlloc,
+			struct SpuConvexPolyhedronVertexData* convexVertexDataA,
+			struct SpuConvexPolyhedronVertexData* convexVertexDataB
+			) const = 0;
+
+
+};
+
+
+
+#endif //CONVEX_PENETRATION_DEPTH_H
+
Index: /code/branches/physics/src/bullet/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuEpaPenetrationDepthSolver.cpp
===================================================================
--- /code/branches/physics/src/bullet/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuEpaPenetrationDepthSolver.cpp	(revision 1966)
+++ /code/branches/physics/src/bullet/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuEpaPenetrationDepthSolver.cpp	(revision 1966)
@@ -0,0 +1,37 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+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.
+*/
+
+#include "SpuEpaPenetrationDepthSolver.h"
+#include "SpuVoronoiSimplexSolver.h"
+#include "SpuGjkPairDetector.h"
+#include "SpuContactResult.h"
+#include "SpuGjkEpa2.h"
+
+bool SpuEpaPenetrationDepthSolver::calcPenDepth( SpuVoronoiSimplexSolver& simplexSolver,
+	        void* convexA,void* convexB,int shapeTypeA, int shapeTypeB, float marginA, float marginB,
+		btTransform& transA,const btTransform& transB,
+			btVector3& v, btPoint3& pa, btPoint3& pb,
+			class btIDebugDraw* debugDraw,btStackAlloc* stackAlloc,
+			struct SpuConvexPolyhedronVertexData* convexVertexDataA,
+			struct SpuConvexPolyhedronVertexData* convexVertexDataB
+			) const
+{
+	bool r;
+	SpuGjkEpaSolver2::sResults results;
+	r = SpuGjkEpaSolver2::Penetration (convexA, convexVertexDataA, shapeTypeA, marginA, transA, convexB, convexVertexDataB, shapeTypeB, marginB, transB, btVector3(1.0f, 0.0f, 0.0f), results);
+	pa = results.witnesses[0];
+	pb = results.witnesses[1];
+	return r;
+}
Index: /code/branches/physics/src/bullet/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuEpaPenetrationDepthSolver.h
===================================================================
--- /code/branches/physics/src/bullet/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuEpaPenetrationDepthSolver.h	(revision 1966)
+++ /code/branches/physics/src/bullet/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuEpaPenetrationDepthSolver.h	(revision 1966)
@@ -0,0 +1,47 @@
+
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+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.
+*/
+
+#ifndef SPU_EPA_PENETRATION_DEPTH_SOLVER_H
+#define SPU_EPA_PENETRATION_DEPTH_SOLVER_H
+
+
+#include "SpuConvexPenetrationDepthSolver.h"
+
+class btStackAlloc;
+class btIDebugDraw;
+class SpuVoronoiSimplexSolver;
+
+///MinkowskiPenetrationDepthSolver implements bruteforce penetration depth estimation.
+///Implementation is based on sampling the depth using support mapping, and using GJK step to get the witness points.
+class SpuEpaPenetrationDepthSolver : public SpuConvexPenetrationDepthSolver
+{
+public:
+
+	virtual bool calcPenDepth( SpuVoronoiSimplexSolver& simplexSolver,
+	        void* convexA,void* convexB,int shapeTypeA, int shapeTypeB, float marginA, float marginB,
+            btTransform& transA,const btTransform& transB,
+			btVector3& v, btPoint3& pa, btPoint3& pb,
+			class btIDebugDraw* debugDraw,btStackAlloc* stackAlloc,
+			struct SpuConvexPolyhedronVertexData* convexVertexDataA,
+			struct SpuConvexPolyhedronVertexData* convexVertexDataB
+			) const;
+
+
+};
+
+
+#endif //SPU_EPA_PENETRATION_DEPTH_SOLVER_H
+
Index: /code/branches/physics/src/bullet/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuGatheringCollisionTask.cpp
===================================================================
--- /code/branches/physics/src/bullet/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuGatheringCollisionTask.cpp	(revision 1966)
+++ /code/branches/physics/src/bullet/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuGatheringCollisionTask.cpp	(revision 1966)
@@ -0,0 +1,985 @@
+
+#include "SpuGatheringCollisionTask.h"
+
+//#define DEBUG_SPU_COLLISION_DETECTION 1
+#include "../SpuDoubleBuffer.h"
+
+#include "../SpuCollisionTaskProcess.h"
+#include "../SpuGatheringCollisionDispatcher.h" //for SPU_BATCHSIZE_BROADPHASE_PAIRS
+
+#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
+#include "../SpuContactManifoldCollisionAlgorithm.h"
+#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
+#include "SpuContactResult.h"
+#include "BulletCollision/CollisionShapes/btOptimizedBvh.h"
+#include "BulletCollision/CollisionShapes/btTriangleIndexVertexArray.h"
+#include "BulletCollision/CollisionShapes/btSphereShape.h"
+
+#include "BulletCollision/CollisionShapes/btCapsuleShape.h"
+
+#include "BulletCollision/CollisionShapes/btConvexShape.h"
+#include "BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h"
+#include "BulletCollision/CollisionShapes/btConvexHullShape.h"
+#include "BulletCollision/CollisionShapes/btCompoundShape.h"
+
+#include "SpuMinkowskiPenetrationDepthSolver.h"
+#include "SpuEpaPenetrationDepthSolver.h"
+#include "SpuGjkPairDetector.h"
+#include "SpuVoronoiSimplexSolver.h"
+
+#include "SpuCollisionShapes.h" //definition of SpuConvexPolyhedronVertexData
+
+#ifdef __SPU__
+///Software caching from the IBM Cell SDK, it reduces 25% SPU time for our test cases
+#ifndef USE_LIBSPE2
+#define USE_SOFTWARE_CACHE 1
+#endif
+#endif //__SPU__
+
+////////////////////////////////////////////////
+/// software caching
+#if USE_SOFTWARE_CACHE
+#include <spu_intrinsics.h>
+#include <sys/spu_thread.h>
+#include <sys/spu_event.h>
+#include <stdint.h>
+#define SPE_CACHE_NWAY   		4
+//#define SPE_CACHE_NSETS 		32, 16
+#define SPE_CACHE_NSETS 		8
+//#define SPE_CACHELINE_SIZE 		512
+#define SPE_CACHELINE_SIZE 		128
+#define SPE_CACHE_SET_TAGID(set) 	15
+///make sure that spe_cache.h is below those defines!
+#include "software_cache/cache/include/spe_cache.h"
+
+
+int g_CacheMisses=0;
+int g_CacheHits=0;
+
+#if 0 // Added to allow cache misses and hits to be tracked, change this to 1 to restore unmodified version
+#define spe_cache_read(ea)		_spe_cache_lookup_xfer_wait_(ea, 0, 1)
+#else
+#define spe_cache_read(ea)		\
+({								\
+    int set, idx, line, byte;					\
+    _spe_cache_nway_lookup_(ea, set, idx);			\
+								\
+    if (btUnlikely(idx < 0)) {					\
+        ++g_CacheMisses;                        \
+	    idx = _spe_cache_miss_(ea, set, -1);			\
+        spu_writech(22, SPE_CACHE_SET_TAGMASK(set));		\
+        spu_mfcstat(MFC_TAG_UPDATE_ALL);			\
+    } 								\
+    else                            \
+    {                               \
+        ++g_CacheHits;              \
+    }                               \
+    line = _spe_cacheline_num_(set, idx);			\
+    byte = _spe_cacheline_byte_offset_(ea);			\
+    (void *) &spe_cache_mem[line + byte];			\
+})
+
+#endif
+
+#endif // USE_SOFTWARE_CACHE
+
+bool gUseEpa = false;
+
+#ifdef USE_SN_TUNER
+#include <LibSN_SPU.h>
+#endif //USE_SN_TUNER
+
+#if defined (__CELLOS_LV2__) || defined (USE_LIBSPE2)
+#else
+#define IGNORE_ALIGNMENT 1
+#define spu_printf printf
+#include <stdio.h>
+#endif
+
+//int gNumConvexPoints0=0;
+
+///Make sure no destructors are called on this memory
+struct	CollisionTask_LocalStoreMemory
+{
+	ATTRIBUTE_ALIGNED16(char	bufferProxy0[sizeof(btBroadphaseProxy)+16]);
+	ATTRIBUTE_ALIGNED16(char	bufferProxy1[sizeof(btBroadphaseProxy)+16]);
+
+	ATTRIBUTE_ALIGNED16(btBroadphaseProxy*	gProxyPtr0);
+	ATTRIBUTE_ALIGNED16(btBroadphaseProxy*	gProxyPtr1);
+
+	//ATTRIBUTE_ALIGNED16(btCollisionObject	gColObj0);
+	//ATTRIBUTE_ALIGNED16(btCollisionObject	gColObj1);
+	ATTRIBUTE_ALIGNED16(char gColObj0 [sizeof(btCollisionObject)+16]);
+	ATTRIBUTE_ALIGNED16(char gColObj1 [sizeof(btCollisionObject)+16]);
+	
+	btCollisionObject* getColObj0()
+	{
+		return (btCollisionObject*) gColObj0;
+	}
+	btCollisionObject* getColObj1()
+	{
+		return (btCollisionObject*) gColObj1;
+	}
+
+	DoubleBuffer<unsigned char, MIDPHASE_WORKUNIT_PAGE_SIZE> g_workUnitTaskBuffers;
+	ATTRIBUTE_ALIGNED16(btBroadphasePair	gBroadphasePairs[SPU_BATCHSIZE_BROADPHASE_PAIRS]);
+
+
+	//SpuContactManifoldCollisionAlgorithm	gSpuContactManifoldAlgo;
+	//ATTRIBUTE_ALIGNED16(char	gSpuContactManifoldAlgo[sizeof(SpuContactManifoldCollisionAlgorithm)+128]);
+
+	SpuContactManifoldCollisionAlgorithm	gSpuContactManifoldAlgo;
+
+	SpuContactManifoldCollisionAlgorithm*	getlocalCollisionAlgorithm()
+	{
+		return (SpuContactManifoldCollisionAlgorithm*)&gSpuContactManifoldAlgo;
+
+	}
+	btPersistentManifold	gPersistentManifold;
+
+	CollisionShape_LocalStoreMemory gCollisionShapes[2];
+
+	///we reserve 32bit integer indices, even though they might be 16bit
+	ATTRIBUTE_ALIGNED16(int	spuIndices[16]);
+
+	bvhMeshShape_LocalStoreMemory bvhShapeData;
+	SpuConvexPolyhedronVertexData convexVertexData[2];
+	CompoundShape_LocalStoreMemory compoundShapeData[2];
+};
+
+
+#if defined(__CELLOS_LV2__) || defined(USE_LIBSPE2) 
+
+ATTRIBUTE_ALIGNED16(CollisionTask_LocalStoreMemory	gLocalStoreMemory);
+
+void* createCollisionLocalStoreMemory()
+{
+	return &gLocalStoreMemory;
+}
+#else
+void* createCollisionLocalStoreMemory()
+{
+        return new CollisionTask_LocalStoreMemory;
+};
+
+#endif
+
+void	ProcessSpuConvexConvexCollision(SpuCollisionPairInput* wuInput, CollisionTask_LocalStoreMemory* lsMemPtr, SpuContactResult& spuContacts);
+
+
+SIMD_FORCE_INLINE void small_cache_read(void* buffer, ppu_address_t ea, size_t size)
+{
+#if USE_SOFTWARE_CACHE
+	// Check for alignment requirements. We need to make sure the entire request fits within one cache line,
+	// so the first and last bytes should fall on the same cache line
+	btAssert((ea & ~SPE_CACHELINE_MASK) == ((ea + size - 1) & ~SPE_CACHELINE_MASK));
+
+	void* ls = spe_cache_read(ea);
+	memcpy(buffer, ls, size);
+#else
+	stallingUnalignedDmaSmallGet(buffer,ea,size);
+#endif
+}
+
+SIMD_FORCE_INLINE void small_cache_read_triple(	void* ls0, ppu_address_t ea0,
+												void* ls1, ppu_address_t ea1,
+												void* ls2, ppu_address_t ea2,
+												size_t size)
+{
+		btAssert(size<16);
+		ATTRIBUTE_ALIGNED16(char	tmpBuffer0[32]);
+		ATTRIBUTE_ALIGNED16(char	tmpBuffer1[32]);
+		ATTRIBUTE_ALIGNED16(char	tmpBuffer2[32]);
+
+		uint32_t i;
+		
+
+		///make sure last 4 bits are the same, for cellDmaSmallGet
+		char* localStore0 = (char*)ls0;
+		uint32_t last4BitsOffset = ea0 & 0x0f;
+		char* tmpTarget0 = tmpBuffer0 + last4BitsOffset;
+#ifdef __SPU__
+		cellDmaSmallGet(tmpTarget0,ea0,size,DMA_TAG(1),0,0);
+#else
+		tmpTarget0 = (char*)cellDmaSmallGetReadOnly(tmpTarget0,ea0,size,DMA_TAG(1),0,0);
+#endif
+
+
+		char* localStore1 = (char*)ls1;
+		last4BitsOffset = ea1 & 0x0f;
+		char* tmpTarget1 = tmpBuffer1 + last4BitsOffset;
+#ifdef __SPU__
+		cellDmaSmallGet(tmpTarget1,ea1,size,DMA_TAG(1),0,0);
+#else
+		tmpTarget1 = (char*)cellDmaSmallGetReadOnly(tmpTarget1,ea1,size,DMA_TAG(1),0,0);
+#endif
+		
+		char* localStore2 = (char*)ls2;
+		last4BitsOffset = ea2 & 0x0f;
+		char* tmpTarget2 = tmpBuffer2 + last4BitsOffset;
+#ifdef __SPU__
+		cellDmaSmallGet(tmpTarget2,ea2,size,DMA_TAG(1),0,0);
+#else
+		tmpTarget2 = (char*)cellDmaSmallGetReadOnly(tmpTarget2,ea2,size,DMA_TAG(1),0,0);
+#endif
+		
+		
+		cellDmaWaitTagStatusAll( DMA_MASK(1) );
+
+		//this is slowish, perhaps memcpy on SPU is smarter?
+		for (i=0; btLikely( i<size );i++)
+		{
+			localStore0[i] = tmpTarget0[i];
+			localStore1[i] = tmpTarget1[i];
+			localStore2[i] = tmpTarget2[i];
+		}
+
+		
+}
+
+
+
+class spuNodeCallback : public btNodeOverlapCallback
+{
+	SpuCollisionPairInput* m_wuInput;
+	SpuContactResult&		m_spuContacts;
+	CollisionTask_LocalStoreMemory*	m_lsMemPtr;
+
+	ATTRIBUTE_ALIGNED16(btVector3	spuTriangleVertices[3]);
+	ATTRIBUTE_ALIGNED16(btScalar	spuUnscaledVertex[4]);
+	
+
+
+public:
+	spuNodeCallback(SpuCollisionPairInput* wuInput, CollisionTask_LocalStoreMemory*	lsMemPtr,SpuContactResult& spuContacts)
+		:	m_wuInput(wuInput),
+		m_lsMemPtr(lsMemPtr),
+		m_spuContacts(spuContacts)
+	{
+	}
+
+	virtual void processNode(int subPart, int triangleIndex)
+	{
+		///Create a triangle on the stack, call process collision, with GJK
+		///DMA the vertices, can benefit from software caching
+
+		//		spu_printf("processNode with triangleIndex %d\n",triangleIndex);
+
+///TODO: add switch between short int, and int indices, based on indexType
+
+		// ugly solution to support both 16bit and 32bit indices
+		if (m_lsMemPtr->bvhShapeData.gIndexMesh.m_indexType == PHY_SHORT)
+		{
+			unsigned short int* indexBasePtr = (unsigned short int*)(m_lsMemPtr->bvhShapeData.gIndexMesh.m_triangleIndexBase+triangleIndex*m_lsMemPtr->bvhShapeData.gIndexMesh.m_triangleIndexStride);
+			ATTRIBUTE_ALIGNED16(unsigned short int tmpIndices[3]);
+
+			small_cache_read_triple(&tmpIndices[0],(ppu_address_t)&indexBasePtr[0],
+									&tmpIndices[1],(ppu_address_t)&indexBasePtr[1],
+									&tmpIndices[2],(ppu_address_t)&indexBasePtr[2],
+									sizeof(unsigned short int));
+
+			m_lsMemPtr->spuIndices[0] = int(tmpIndices[0]);
+			m_lsMemPtr->spuIndices[1] = int(tmpIndices[1]);
+			m_lsMemPtr->spuIndices[2] = int(tmpIndices[2]);
+		} else
+		{
+			unsigned int* indexBasePtr = (unsigned int*)(m_lsMemPtr->bvhShapeData.gIndexMesh.m_triangleIndexBase+triangleIndex*m_lsMemPtr->bvhShapeData.gIndexMesh.m_triangleIndexStride);
+
+			small_cache_read_triple(&m_lsMemPtr->spuIndices[0],(ppu_address_t)&indexBasePtr[0],
+								&m_lsMemPtr->spuIndices[1],(ppu_address_t)&indexBasePtr[1],
+								&m_lsMemPtr->spuIndices[2],(ppu_address_t)&indexBasePtr[2],
+								sizeof(int));
+		}
+		
+		//		spu_printf("SPU index0=%d ,",spuIndices[0]);
+		//		spu_printf("SPU index1=%d ,",spuIndices[1]);
+		//		spu_printf("SPU index2=%d ,",spuIndices[2]);
+		//		spu_printf("SPU: indexBasePtr=%llx\n",indexBasePtr);
+
+		const btVector3& meshScaling = m_lsMemPtr->bvhShapeData.gTriangleMeshInterfacePtr->getScaling();
+		for (int j=2;btLikely( j>=0 );j--)
+		{
+			int graphicsindex = m_lsMemPtr->spuIndices[j];
+
+			//			spu_printf("SPU index=%d ,",graphicsindex);
+			btScalar* graphicsbasePtr = (btScalar*)(m_lsMemPtr->bvhShapeData.gIndexMesh.m_vertexBase+graphicsindex*m_lsMemPtr->bvhShapeData.gIndexMesh.m_vertexStride);
+			//			spu_printf("SPU graphicsbasePtr=%llx\n",graphicsbasePtr);
+
+
+			///handle un-aligned vertices...
+
+			//another DMA for each vertex
+			small_cache_read_triple(&spuUnscaledVertex[0],(ppu_address_t)&graphicsbasePtr[0],
+									&spuUnscaledVertex[1],(ppu_address_t)&graphicsbasePtr[1],
+									&spuUnscaledVertex[2],(ppu_address_t)&graphicsbasePtr[2],
+									sizeof(btScalar));
+			
+			spuTriangleVertices[j] = btVector3(
+				spuUnscaledVertex[0]*meshScaling.getX(),
+				spuUnscaledVertex[1]*meshScaling.getY(),
+				spuUnscaledVertex[2]*meshScaling.getZ());
+
+			//			spu_printf("SPU:triangle vertices:%f,%f,%f\n",spuTriangleVertices[j].x(),spuTriangleVertices[j].y(),spuTriangleVertices[j].z());
+		}
+
+
+
+		//btTriangleShape	tmpTriangleShape(spuTriangleVertices[0],spuTriangleVertices[1],spuTriangleVertices[2]);
+
+
+		SpuCollisionPairInput triangleConcaveInput(*m_wuInput);
+		triangleConcaveInput.m_spuCollisionShapes[1] = &spuTriangleVertices[0];
+		triangleConcaveInput.m_shapeType1 = TRIANGLE_SHAPE_PROXYTYPE;
+
+		m_spuContacts.setShapeIdentifiers(-1,-1,subPart,triangleIndex);
+
+		//		m_spuContacts.flush();
+
+		ProcessSpuConvexConvexCollision(&triangleConcaveInput, m_lsMemPtr,m_spuContacts);
+		///this flush should be automatic
+		//	m_spuContacts.flush();
+	}
+
+};
+
+
+////////////////////////
+/// Convex versus Concave triangle mesh collision detection (handles concave triangle mesh versus sphere, box, cylinder, triangle, cone, convex polyhedron etc)
+///////////////////
+void	ProcessConvexConcaveSpuCollision(SpuCollisionPairInput* wuInput, CollisionTask_LocalStoreMemory* lsMemPtr, SpuContactResult& spuContacts)
+{
+	//order: first collision shape is convex, second concave. m_isSwapped is true, if the original order was opposite
+	register int dmaSize;
+	register ppu_address_t	dmaPpuAddress2;
+
+	btBvhTriangleMeshShape*	trimeshShape = (btBvhTriangleMeshShape*)wuInput->m_spuCollisionShapes[1];
+	//need the mesh interface, for access to triangle vertices
+	dmaBvhShapeData (&lsMemPtr->bvhShapeData, trimeshShape);
+
+	btVector3 aabbMin(-1,-400,-1);
+	btVector3 aabbMax(1,400,1);
+
+
+	//recalc aabbs
+	btTransform convexInTriangleSpace;
+	convexInTriangleSpace = wuInput->m_worldTransform1.inverse() * wuInput->m_worldTransform0;
+	btConvexInternalShape* convexShape = (btConvexInternalShape*)wuInput->m_spuCollisionShapes[0];
+
+	computeAabb (aabbMin, aabbMax, convexShape, wuInput->m_collisionShapes[0], wuInput->m_shapeType0, convexInTriangleSpace);
+
+
+	//CollisionShape* triangleShape = static_cast<btCollisionShape*>(triBody->m_collisionShape);
+	//convexShape->getAabb(convexInTriangleSpace,m_aabbMin,m_aabbMax);
+
+	//	btScalar extraMargin = collisionMarginTriangle;
+	//	btVector3 extra(extraMargin,extraMargin,extraMargin);
+	//	aabbMax += extra;
+	//	aabbMin -= extra;
+
+	///quantize query AABB
+	unsigned short int quantizedQueryAabbMin[3];
+	unsigned short int quantizedQueryAabbMax[3];
+	lsMemPtr->bvhShapeData.getOptimizedBvh()->quantizeWithClamp(quantizedQueryAabbMin,aabbMin,0);
+	lsMemPtr->bvhShapeData.getOptimizedBvh()->quantizeWithClamp(quantizedQueryAabbMax,aabbMax,1);
+
+	QuantizedNodeArray&	nodeArray = lsMemPtr->bvhShapeData.getOptimizedBvh()->getQuantizedNodeArray();
+	//spu_printf("SPU: numNodes = %d\n",nodeArray.size());
+
+	BvhSubtreeInfoArray& subTrees = lsMemPtr->bvhShapeData.getOptimizedBvh()->getSubtreeInfoArray();
+
+
+	spuNodeCallback	nodeCallback(wuInput,lsMemPtr,spuContacts);
+	IndexedMeshArray&	indexArray = lsMemPtr->bvhShapeData.gTriangleMeshInterfacePtr->getIndexedMeshArray();
+	//spu_printf("SPU:indexArray.size() = %d\n",indexArray.size());
+
+	//	spu_printf("SPU: numSubTrees = %d\n",subTrees.size());
+	//not likely to happen
+	if (subTrees.size() && indexArray.size() == 1)
+	{
+		///DMA in the index info
+		dmaBvhIndexedMesh (&lsMemPtr->bvhShapeData.gIndexMesh, indexArray, 0 /* index into indexArray */, 1 /* dmaTag */);
+		cellDmaWaitTagStatusAll(DMA_MASK(1));
+		
+		//display the headers
+		int numBatch = subTrees.size();
+		for (int i=0;i<numBatch;)
+		{
+// BEN: TODO - can reorder DMA transfers for less stall
+			int remaining = subTrees.size() - i;
+			int nextBatch = remaining < MAX_SPU_SUBTREE_HEADERS ? remaining : MAX_SPU_SUBTREE_HEADERS;
+			
+			dmaBvhSubTreeHeaders (&lsMemPtr->bvhShapeData.gSubtreeHeaders[0], (ppu_address_t)(&subTrees[i]), nextBatch, 1);
+			cellDmaWaitTagStatusAll(DMA_MASK(1));
+			
+
+			//			spu_printf("nextBatch = %d\n",nextBatch);
+
+			for (int j=0;j<nextBatch;j++)
+			{
+				const btBvhSubtreeInfo& subtree = lsMemPtr->bvhShapeData.gSubtreeHeaders[j];
+
+				unsigned int overlap = spuTestQuantizedAabbAgainstQuantizedAabb(quantizedQueryAabbMin,quantizedQueryAabbMax,subtree.m_quantizedAabbMin,subtree.m_quantizedAabbMax);
+				if (overlap)
+				{
+					btAssert(subtree.m_subtreeSize);
+
+					//dma the actual nodes of this subtree
+					dmaBvhSubTreeNodes (&lsMemPtr->bvhShapeData.gSubtreeNodes[0], subtree, nodeArray, 2);
+					cellDmaWaitTagStatusAll(DMA_MASK(2));
+
+					/* Walk this subtree */
+					spuWalkStacklessQuantizedTree(&nodeCallback,quantizedQueryAabbMin,quantizedQueryAabbMax,
+						&lsMemPtr->bvhShapeData.gSubtreeNodes[0],
+						0,
+						subtree.m_subtreeSize);
+				}
+				//				spu_printf("subtreeSize = %d\n",gSubtreeHeaders[j].m_subtreeSize);
+			}
+
+			//	unsigned short int	m_quantizedAabbMin[3];
+			//	unsigned short int	m_quantizedAabbMax[3];
+			//	int			m_rootNodeIndex;
+			//	int			m_subtreeSize;
+			i+=nextBatch;
+		}
+
+		//pre-fetch first tree, then loop and double buffer
+	}
+
+}
+
+
+
+////////////////////////
+/// Convex versus Convex collision detection (handles collision between sphere, box, cylinder, triangle, cone, convex polyhedron etc)
+///////////////////
+void	ProcessSpuConvexConvexCollision(SpuCollisionPairInput* wuInput, CollisionTask_LocalStoreMemory* lsMemPtr, SpuContactResult& spuContacts)
+{
+	register int dmaSize;
+	register ppu_address_t	dmaPpuAddress2;
+	
+#ifdef DEBUG_SPU_COLLISION_DETECTION
+	//spu_printf("SPU: ProcessSpuConvexConvexCollision\n");
+#endif //DEBUG_SPU_COLLISION_DETECTION
+	//CollisionShape* shape0 = (CollisionShape*)wuInput->m_collisionShapes[0];
+	//CollisionShape* shape1 = (CollisionShape*)wuInput->m_collisionShapes[1];
+	btPersistentManifold* manifold = (btPersistentManifold*)wuInput->m_persistentManifoldPtr;
+
+	bool genericGjk = true;
+
+	if (genericGjk)
+	{
+		//try generic GJK
+
+		SpuVoronoiSimplexSolver vsSolver;
+		SpuEpaPenetrationDepthSolver epaPenetrationSolver;
+		SpuMinkowskiPenetrationDepthSolver	minkowskiPenetrationSolver;
+		SpuConvexPenetrationDepthSolver* penetrationSolver;
+
+		if (gUseEpa)
+		{
+			penetrationSolver = &epaPenetrationSolver;
+		} else {
+			penetrationSolver = &minkowskiPenetrationSolver;
+		}
+
+
+		///DMA in the vertices for convex shapes
+		ATTRIBUTE_ALIGNED16(char convexHullShape0[sizeof(btConvexHullShape)]);
+		ATTRIBUTE_ALIGNED16(char convexHullShape1[sizeof(btConvexHullShape)]);
+
+		if ( btLikely( wuInput->m_shapeType0== CONVEX_HULL_SHAPE_PROXYTYPE ) )
+		{
+			//	spu_printf("SPU: DMA btConvexHullShape\n");
+			
+			dmaSize = sizeof(btConvexHullShape);
+			dmaPpuAddress2 = wuInput->m_collisionShapes[0];
+
+			cellDmaGet(&convexHullShape0, dmaPpuAddress2  , dmaSize, DMA_TAG(1), 0, 0);
+			//cellDmaWaitTagStatusAll(DMA_MASK(1));
+		}
+
+		if ( btLikely( wuInput->m_shapeType1 == CONVEX_HULL_SHAPE_PROXYTYPE ) )
+		{
+			//	spu_printf("SPU: DMA btConvexHullShape\n");
+			dmaSize = sizeof(btConvexHullShape);
+			dmaPpuAddress2 = wuInput->m_collisionShapes[1];
+			cellDmaGet(&convexHullShape1, dmaPpuAddress2  , dmaSize, DMA_TAG(1), 0, 0);
+			//cellDmaWaitTagStatusAll(DMA_MASK(1));
+		}
+		
+		if ( btLikely( wuInput->m_shapeType0 == CONVEX_HULL_SHAPE_PROXYTYPE ) )
+		{		
+			cellDmaWaitTagStatusAll(DMA_MASK(1));
+			dmaConvexVertexData (&lsMemPtr->convexVertexData[0], (btConvexHullShape*)&convexHullShape0);
+			lsMemPtr->convexVertexData[0].gSpuConvexShapePtr = wuInput->m_spuCollisionShapes[0];
+		}
+
+			
+		if ( btLikely( wuInput->m_shapeType1 == CONVEX_HULL_SHAPE_PROXYTYPE ) )
+		{
+			cellDmaWaitTagStatusAll(DMA_MASK(1));
+			dmaConvexVertexData (&lsMemPtr->convexVertexData[1], (btConvexHullShape*)&convexHullShape1);
+			lsMemPtr->convexVertexData[1].gSpuConvexShapePtr = wuInput->m_spuCollisionShapes[1];
+		}
+
+		if ( btLikely( wuInput->m_shapeType0 == CONVEX_HULL_SHAPE_PROXYTYPE ) )
+		{		
+			cellDmaWaitTagStatusAll(DMA_MASK(2));
+			lsMemPtr->convexVertexData[0].gConvexPoints = &lsMemPtr->convexVertexData[0].g_convexPointBuffer[0];
+		}
+
+		if ( btLikely( wuInput->m_shapeType1 == CONVEX_HULL_SHAPE_PROXYTYPE ) )
+		{
+			cellDmaWaitTagStatusAll(DMA_MASK(2));		
+			lsMemPtr->convexVertexData[1].gConvexPoints = &lsMemPtr->convexVertexData[1].g_convexPointBuffer[0];
+		}
+
+
+		void* shape0Ptr = wuInput->m_spuCollisionShapes[0];
+		void* shape1Ptr = wuInput->m_spuCollisionShapes[1];
+		int shapeType0 = wuInput->m_shapeType0;
+		int shapeType1 = wuInput->m_shapeType1;
+		float marginA = wuInput->m_collisionMargin0;
+		float marginB = wuInput->m_collisionMargin1;
+
+		SpuClosestPointInput	cpInput;
+		cpInput.m_convexVertexData[0] = &lsMemPtr->convexVertexData[0];
+		cpInput.m_convexVertexData[1] = &lsMemPtr->convexVertexData[1];
+		cpInput.m_transformA = wuInput->m_worldTransform0;
+		cpInput.m_transformB = wuInput->m_worldTransform1;
+		float sumMargin = (marginA+marginB+lsMemPtr->gPersistentManifold.getContactBreakingThreshold());
+		cpInput.m_maximumDistanceSquared = sumMargin * sumMargin;
+
+		ppu_address_t manifoldAddress = (ppu_address_t)manifold;
+
+		btPersistentManifold* spuManifold=&lsMemPtr->gPersistentManifold;
+		//spuContacts.setContactInfo(spuManifold,manifoldAddress,wuInput->m_worldTransform0,wuInput->m_worldTransform1,wuInput->m_isSwapped);
+		spuContacts.setContactInfo(spuManifold,manifoldAddress,lsMemPtr->getColObj0()->getWorldTransform(),
+			lsMemPtr->getColObj1()->getWorldTransform(),
+			lsMemPtr->getColObj0()->getRestitution(),lsMemPtr->getColObj1()->getRestitution(),
+			lsMemPtr->getColObj0()->getFriction(),lsMemPtr->getColObj1()->getFriction(),
+			wuInput->m_isSwapped);
+
+		SpuGjkPairDetector gjk(shape0Ptr,shape1Ptr,shapeType0,shapeType1,marginA,marginB,&vsSolver,penetrationSolver);
+		gjk.getClosestPoints(cpInput,spuContacts);//,debugDraw);
+	}
+
+
+}
+
+
+template<typename T> void DoSwap(T& a, T& b)
+{
+	char tmp[sizeof(T)];
+	memcpy(tmp, &a, sizeof(T));
+	memcpy(&a, &b, sizeof(T));
+	memcpy(&b, tmp, sizeof(T));
+}
+
+SIMD_FORCE_INLINE void	dmaAndSetupCollisionObjects(SpuCollisionPairInput& collisionPairInput, CollisionTask_LocalStoreMemory& lsMem)
+{
+	register int dmaSize;
+	register ppu_address_t	dmaPpuAddress2;
+		
+	dmaSize = sizeof(btCollisionObject);
+	dmaPpuAddress2 = /*collisionPairInput.m_isSwapped ? (ppu_address_t)lsMem.gProxyPtr1->m_clientObject :*/ (ppu_address_t)lsMem.gProxyPtr0->m_clientObject;
+	cellDmaGet(&lsMem.gColObj0, dmaPpuAddress2  , dmaSize, DMA_TAG(1), 0, 0);		
+
+	dmaSize = sizeof(btCollisionObject);
+	dmaPpuAddress2 = /*collisionPairInput.m_isSwapped ? (ppu_address_t)lsMem.gProxyPtr0->m_clientObject :*/ (ppu_address_t)lsMem.gProxyPtr1->m_clientObject;
+	cellDmaGet(&lsMem.gColObj1, dmaPpuAddress2  , dmaSize, DMA_TAG(2), 0, 0);		
+	
+	cellDmaWaitTagStatusAll(DMA_MASK(1) | DMA_MASK(2));
+
+	collisionPairInput.m_worldTransform0 = lsMem.getColObj0()->getWorldTransform();
+	collisionPairInput.m_worldTransform1 = lsMem.getColObj1()->getWorldTransform();
+}
+
+
+
+void	handleCollisionPair(SpuCollisionPairInput& collisionPairInput, CollisionTask_LocalStoreMemory& lsMem,
+							SpuContactResult &spuContacts,
+							ppu_address_t collisionShape0Ptr, void* collisionShape0Loc,
+							ppu_address_t collisionShape1Ptr, void* collisionShape1Loc, bool dmaShapes = true)
+{
+	register int dmaSize;
+	register	ppu_address_t	dmaPpuAddress2;
+	
+	if (btBroadphaseProxy::isConvex(collisionPairInput.m_shapeType0) 
+		&& btBroadphaseProxy::isConvex(collisionPairInput.m_shapeType1))
+	{
+		if (dmaShapes)
+		{
+			dmaCollisionShape (collisionShape0Loc, collisionShape0Ptr, 1, collisionPairInput.m_shapeType0);
+			dmaCollisionShape (collisionShape1Loc, collisionShape1Ptr, 2, collisionPairInput.m_shapeType1);
+			cellDmaWaitTagStatusAll(DMA_MASK(1) | DMA_MASK(2));
+		}
+
+		btConvexInternalShape* spuConvexShape0 = (btConvexInternalShape*)collisionShape0Loc;
+		btConvexInternalShape* spuConvexShape1 = (btConvexInternalShape*)collisionShape1Loc;
+
+		btVector3 dim0 = spuConvexShape0->getImplicitShapeDimensions();
+		btVector3 dim1 = spuConvexShape1->getImplicitShapeDimensions();
+
+		collisionPairInput.m_primitiveDimensions0 = dim0;
+		collisionPairInput.m_primitiveDimensions1 = dim1;
+		collisionPairInput.m_collisionShapes[0] = collisionShape0Ptr;
+		collisionPairInput.m_collisionShapes[1] = collisionShape1Ptr;
+		collisionPairInput.m_spuCollisionShapes[0] = spuConvexShape0;
+		collisionPairInput.m_spuCollisionShapes[1] = spuConvexShape1;
+		ProcessSpuConvexConvexCollision(&collisionPairInput,&lsMem,spuContacts);
+	} 
+	else if (btBroadphaseProxy::isCompound(collisionPairInput.m_shapeType0) && 
+			btBroadphaseProxy::isCompound(collisionPairInput.m_shapeType1))
+	{
+		//snPause();
+
+		dmaCollisionShape (collisionShape0Loc, collisionShape0Ptr, 1, collisionPairInput.m_shapeType0);
+		dmaCollisionShape (collisionShape1Loc, collisionShape1Ptr, 2, collisionPairInput.m_shapeType1);
+		cellDmaWaitTagStatusAll(DMA_MASK(1) | DMA_MASK(2));
+
+		// Both are compounds, do N^2 CD for now
+		// TODO: add some AABB-based pruning
+	
+		btCompoundShape* spuCompoundShape0 = (btCompoundShape*)collisionShape0Loc;
+		btCompoundShape* spuCompoundShape1 = (btCompoundShape*)collisionShape1Loc;
+
+		dmaCompoundShapeInfo (&lsMem.compoundShapeData[0], spuCompoundShape0, 1);
+		dmaCompoundShapeInfo (&lsMem.compoundShapeData[1], spuCompoundShape1, 2);
+		cellDmaWaitTagStatusAll(DMA_MASK(1) | DMA_MASK(2));
+		
+
+		dmaCompoundSubShapes (&lsMem.compoundShapeData[0], spuCompoundShape0, 1);
+		cellDmaWaitTagStatusAll(DMA_MASK(1));
+		dmaCompoundSubShapes (&lsMem.compoundShapeData[1], spuCompoundShape1, 1);
+		cellDmaWaitTagStatusAll(DMA_MASK(1));
+
+		int childShapeCount0 = spuCompoundShape0->getNumChildShapes();
+		int childShapeCount1 = spuCompoundShape1->getNumChildShapes();
+
+		// Start the N^2
+		for (int i = 0; i < childShapeCount0; ++i)
+		{
+			btCompoundShapeChild& childShape0 = lsMem.compoundShapeData[0].gSubshapes[i];
+
+			for (int j = 0; j < childShapeCount1; ++j)
+			{
+				btCompoundShapeChild& childShape1 = lsMem.compoundShapeData[1].gSubshapes[j];
+
+				/* Create a new collision pair input struct using the two child shapes */
+				SpuCollisionPairInput cinput (collisionPairInput);
+
+				cinput.m_worldTransform0 = collisionPairInput.m_worldTransform0 * childShape0.m_transform;
+				cinput.m_shapeType0 = childShape0.m_childShapeType;
+				cinput.m_collisionMargin0 = childShape0.m_childMargin;
+
+				cinput.m_worldTransform1 = collisionPairInput.m_worldTransform1 * childShape1.m_transform;
+				cinput.m_shapeType1 = childShape1.m_childShapeType;
+				cinput.m_collisionMargin1 = childShape1.m_childMargin;
+				/* Recursively call handleCollisionPair () with new collision pair input */
+				handleCollisionPair(cinput, lsMem, spuContacts,			
+					(ppu_address_t)childShape0.m_childShape, lsMem.compoundShapeData[0].gSubshapeShape[i], 
+					(ppu_address_t)childShape1.m_childShape, lsMem.compoundShapeData[1].gSubshapeShape[j], false); // bug fix: changed index to j.
+			}
+		}
+	}
+	else if (btBroadphaseProxy::isCompound(collisionPairInput.m_shapeType0) )
+	{
+		//snPause();
+		
+		dmaCollisionShape (collisionShape0Loc, collisionShape0Ptr, 1, collisionPairInput.m_shapeType0);
+		dmaCollisionShape (collisionShape1Loc, collisionShape1Ptr, 2, collisionPairInput.m_shapeType1);
+		cellDmaWaitTagStatusAll(DMA_MASK(1) | DMA_MASK(2));
+
+		// object 0 compound, object 1 non-compound
+		btCompoundShape* spuCompoundShape = (btCompoundShape*)collisionShape0Loc;
+		dmaCompoundShapeInfo (&lsMem.compoundShapeData[0], spuCompoundShape, 1);
+		cellDmaWaitTagStatusAll(DMA_MASK(1));
+
+		int childShapeCount = spuCompoundShape->getNumChildShapes();
+
+		for (int i = 0; i < childShapeCount; ++i)
+		{
+			btCompoundShapeChild& childShape = lsMem.compoundShapeData[0].gSubshapes[i];
+
+			// Dma the child shape
+			dmaCollisionShape (&lsMem.compoundShapeData[0].gSubshapeShape[i], (ppu_address_t)childShape.m_childShape, 1, childShape.m_childShapeType);
+			cellDmaWaitTagStatusAll(DMA_MASK(1));
+			
+			SpuCollisionPairInput cinput (collisionPairInput);
+			cinput.m_worldTransform0 = collisionPairInput.m_worldTransform0 * childShape.m_transform;
+			cinput.m_shapeType0 = childShape.m_childShapeType;
+			cinput.m_collisionMargin0 = childShape.m_childMargin;
+
+			handleCollisionPair(cinput, lsMem, spuContacts,			
+				(ppu_address_t)childShape.m_childShape, lsMem.compoundShapeData[0].gSubshapeShape[i], 
+				collisionShape1Ptr, collisionShape1Loc, false);
+		}
+	}
+	else if (btBroadphaseProxy::isCompound(collisionPairInput.m_shapeType1) )
+	{
+		//snPause();
+		
+		dmaCollisionShape (collisionShape0Loc, collisionShape0Ptr, 1, collisionPairInput.m_shapeType0);
+		dmaCollisionShape (collisionShape1Loc, collisionShape1Ptr, 2, collisionPairInput.m_shapeType1);
+		cellDmaWaitTagStatusAll(DMA_MASK(1) | DMA_MASK(2));
+		// object 0 non-compound, object 1 compound
+		btCompoundShape* spuCompoundShape = (btCompoundShape*)collisionShape1Loc;
+		dmaCompoundShapeInfo (&lsMem.compoundShapeData[0], spuCompoundShape, 1);
+		cellDmaWaitTagStatusAll(DMA_MASK(1));
+		
+		int childShapeCount = spuCompoundShape->getNumChildShapes();
+
+		for (int i = 0; i < childShapeCount; ++i)
+		{
+			btCompoundShapeChild& childShape = lsMem.compoundShapeData[0].gSubshapes[i];
+			// Dma the child shape
+			dmaCollisionShape (&lsMem.compoundShapeData[0].gSubshapeShape[i], (ppu_address_t)childShape.m_childShape, 1, childShape.m_childShapeType);
+			cellDmaWaitTagStatusAll(DMA_MASK(1));
+
+			SpuCollisionPairInput cinput (collisionPairInput);
+			cinput.m_worldTransform1 = collisionPairInput.m_worldTransform1 * childShape.m_transform;
+			cinput.m_shapeType1 = childShape.m_childShapeType;
+			cinput.m_collisionMargin1 = childShape.m_childMargin;
+			handleCollisionPair(cinput, lsMem, spuContacts,
+				collisionShape0Ptr, collisionShape0Loc, 
+				(ppu_address_t)childShape.m_childShape, lsMem.compoundShapeData[0].gSubshapeShape[i], false);
+		}
+		
+	}
+	else
+	{
+		//a non-convex shape is involved									
+		bool handleConvexConcave = false;
+
+		//snPause();
+
+		if (btBroadphaseProxy::isConcave(collisionPairInput.m_shapeType0) &&
+			btBroadphaseProxy::isConvex(collisionPairInput.m_shapeType1))
+		{
+			// Swap stuff
+			DoSwap(collisionShape0Ptr, collisionShape1Ptr);
+			DoSwap(collisionShape0Loc, collisionShape1Loc);
+			DoSwap(collisionPairInput.m_shapeType0, collisionPairInput.m_shapeType1);
+			DoSwap(collisionPairInput.m_worldTransform0, collisionPairInput.m_worldTransform1);
+			DoSwap(collisionPairInput.m_collisionMargin0, collisionPairInput.m_collisionMargin1);
+			
+			collisionPairInput.m_isSwapped = true;
+		}
+		
+		if (btBroadphaseProxy::isConvex(collisionPairInput.m_shapeType0)&&
+			btBroadphaseProxy::isConcave(collisionPairInput.m_shapeType1))
+		{
+			handleConvexConcave = true;
+		}
+		if (handleConvexConcave)
+		{
+			if (dmaShapes)
+			{
+				dmaCollisionShape (collisionShape0Loc, collisionShape0Ptr, 1, collisionPairInput.m_shapeType0);
+				dmaCollisionShape (collisionShape1Loc, collisionShape1Ptr, 2, collisionPairInput.m_shapeType1);
+				cellDmaWaitTagStatusAll(DMA_MASK(1) | DMA_MASK(2));
+			}
+			
+			btConvexInternalShape* spuConvexShape0 = (btConvexInternalShape*)collisionShape0Loc;
+			btBvhTriangleMeshShape* trimeshShape = (btBvhTriangleMeshShape*)collisionShape1Loc;
+
+			btVector3 dim0 = spuConvexShape0->getImplicitShapeDimensions();
+			collisionPairInput.m_primitiveDimensions0 = dim0;
+			collisionPairInput.m_collisionShapes[0] = collisionShape0Ptr;
+			collisionPairInput.m_collisionShapes[1] = collisionShape1Ptr;
+			collisionPairInput.m_spuCollisionShapes[0] = spuConvexShape0;
+			collisionPairInput.m_spuCollisionShapes[1] = trimeshShape;
+
+			ProcessConvexConcaveSpuCollision(&collisionPairInput,&lsMem,spuContacts);
+		}
+
+	}
+
+	spuContacts.flush();
+}
+
+
+void	processCollisionTask(void* userPtr, void* lsMemPtr)
+{
+
+	SpuGatherAndProcessPairsTaskDesc* taskDescPtr = (SpuGatherAndProcessPairsTaskDesc*)userPtr;
+	SpuGatherAndProcessPairsTaskDesc& taskDesc = *taskDescPtr;
+	CollisionTask_LocalStoreMemory*	colMemPtr = (CollisionTask_LocalStoreMemory*)lsMemPtr;
+	CollisionTask_LocalStoreMemory& lsMem = *(colMemPtr);
+
+	gUseEpa = taskDesc.m_useEpa;
+
+	//	spu_printf("taskDescPtr=%llx\n",taskDescPtr);
+
+	SpuContactResult spuContacts;
+
+	////////////////////
+
+	ppu_address_t dmaInPtr = taskDesc.inPtr;
+	unsigned int numPages = taskDesc.numPages;
+	unsigned int numOnLastPage = taskDesc.numOnLastPage;
+
+	// prefetch first set of inputs and wait
+	lsMem.g_workUnitTaskBuffers.init();
+
+	unsigned int nextNumOnPage = (numPages > 1)? MIDPHASE_NUM_WORKUNITS_PER_PAGE : numOnLastPage;
+	lsMem.g_workUnitTaskBuffers.backBufferDmaGet(dmaInPtr, nextNumOnPage*sizeof(SpuGatherAndProcessWorkUnitInput), DMA_TAG(3));
+	dmaInPtr += MIDPHASE_WORKUNIT_PAGE_SIZE;
+
+	
+	register unsigned char *inputPtr;
+	register unsigned int numOnPage;
+	register unsigned int j;
+	SpuGatherAndProcessWorkUnitInput* wuInputs;	
+	register int dmaSize;
+	register ppu_address_t	dmaPpuAddress;
+	register ppu_address_t	dmaPpuAddress2;
+
+	int userInfo;
+	int numPairs;
+	register int p;
+	SpuCollisionPairInput collisionPairInput;
+	
+	for (unsigned int i = 0; btLikely(i < numPages); i++)
+	{
+
+		// wait for back buffer dma and swap buffers
+		inputPtr = lsMem.g_workUnitTaskBuffers.swapBuffers();
+
+		// number on current page is number prefetched last iteration
+		numOnPage = nextNumOnPage;
+
+
+		// prefetch next set of inputs
+#if MIDPHASE_NUM_WORKUNIT_PAGES > 2
+		if ( btLikely( i < numPages-1 ) )
+#else
+		if ( btUnlikely( i < numPages-1 ) )
+#endif
+		{
+			nextNumOnPage = (i == numPages-2)? numOnLastPage : MIDPHASE_NUM_WORKUNITS_PER_PAGE;
+			lsMem.g_workUnitTaskBuffers.backBufferDmaGet(dmaInPtr, nextNumOnPage*sizeof(SpuGatherAndProcessWorkUnitInput), DMA_TAG(3));
+			dmaInPtr += MIDPHASE_WORKUNIT_PAGE_SIZE;
+		}
+
+		wuInputs = reinterpret_cast<SpuGatherAndProcessWorkUnitInput *>(inputPtr);
+		
+		
+		for (j = 0; btLikely( j < numOnPage ); j++)
+		{
+#ifdef DEBUG_SPU_COLLISION_DETECTION
+		//	printMidphaseInput(&wuInputs[j]);
+#endif //DEBUG_SPU_COLLISION_DETECTION
+
+
+			numPairs = wuInputs[j].m_endIndex - wuInputs[j].m_startIndex;
+			
+			if ( btLikely( numPairs ) )
+			{
+					dmaSize = numPairs*sizeof(btBroadphasePair);
+					dmaPpuAddress = wuInputs[j].m_pairArrayPtr+wuInputs[j].m_startIndex * sizeof(btBroadphasePair);
+					cellDmaGet(&lsMem.gBroadphasePairs, dmaPpuAddress  , dmaSize, DMA_TAG(1), 0, 0);
+					cellDmaWaitTagStatusAll(DMA_MASK(1));
+				
+
+				for (p=0;p<numPairs;p++)
+				{
+
+					//for each broadphase pair, do something
+
+					btBroadphasePair& pair = lsMem.gBroadphasePairs[p];
+#ifdef DEBUG_SPU_COLLISION_DETECTION
+					spu_printf("pair->m_userInfo = %d\n",pair.m_userInfo);
+					spu_printf("pair->m_algorithm = %d\n",pair.m_algorithm);
+					spu_printf("pair->m_pProxy0 = %d\n",pair.m_pProxy0);
+					spu_printf("pair->m_pProxy1 = %d\n",pair.m_pProxy1);
+#endif //DEBUG_SPU_COLLISION_DETECTION
+
+					userInfo = int(pair.m_userInfo);
+
+					if (userInfo == 2 && pair.m_algorithm && pair.m_pProxy0 && pair.m_pProxy1)
+					{
+						dmaSize = sizeof(SpuContactManifoldCollisionAlgorithm);
+						dmaPpuAddress2 = (ppu_address_t)pair.m_algorithm;
+						cellDmaGet(&lsMem.gSpuContactManifoldAlgo, dmaPpuAddress2  , dmaSize, DMA_TAG(1), 0, 0);
+
+						//snPause();
+
+#ifdef DEBUG_SPU_COLLISION_DETECTION
+						//spu_printf("SPU: manifoldPtr: %llx",collisionPairInput->m_persistentManifoldPtr);
+#endif //DEBUG_SPU_COLLISION_DETECTION
+
+						
+						dmaSize = sizeof(btBroadphaseProxy);
+						dmaPpuAddress2 = (ppu_address_t)pair.m_pProxy0;
+						//stallingUnalignedDmaSmallGet(lsMem.gProxyPtr0, dmaPpuAddress2  , dmaSize);
+						void* tmpPtr = cellDmaSmallGetReadOnly(&lsMem.bufferProxy0, dmaPpuAddress2  , dmaSize,DMA_TAG(1), 0, 0);
+						lsMem.gProxyPtr0 = (btBroadphaseProxy*) tmpPtr;
+
+						dmaSize = sizeof(btBroadphaseProxy);
+						dmaPpuAddress2 = (ppu_address_t)pair.m_pProxy1;
+						tmpPtr  = cellDmaSmallGetReadOnly(&lsMem.bufferProxy1, dmaPpuAddress2  , dmaSize,DMA_TAG(1), 0, 0);
+
+						lsMem.gProxyPtr1 = (btBroadphaseProxy*)tmpPtr;
+
+						cellDmaWaitTagStatusAll(DMA_MASK(1));
+
+						collisionPairInput.m_persistentManifoldPtr = (ppu_address_t) lsMem.gSpuContactManifoldAlgo.getContactManifoldPtr();
+						collisionPairInput.m_isSwapped = false;
+
+						if (1)
+						{
+
+							///can wait on the combined DMA_MASK, or dma on the same tag
+
+
+#ifdef DEBUG_SPU_COLLISION_DETECTION
+					//		spu_printf("SPU collisionPairInput->m_shapeType0 = %d\n",collisionPairInput->m_shapeType0);
+					//		spu_printf("SPU collisionPairInput->m_shapeType1 = %d\n",collisionPairInput->m_shapeType1);
+#endif //DEBUG_SPU_COLLISION_DETECTION
+
+							
+							dmaSize = sizeof(btPersistentManifold);
+
+							dmaPpuAddress2 = collisionPairInput.m_persistentManifoldPtr;
+							cellDmaGet(&lsMem.gPersistentManifold, dmaPpuAddress2  , dmaSize, DMA_TAG(1), 0, 0);
+
+							collisionPairInput.m_shapeType0 = lsMem.gSpuContactManifoldAlgo.getShapeType0();
+							collisionPairInput.m_shapeType1 = lsMem.gSpuContactManifoldAlgo.getShapeType1();
+							collisionPairInput.m_collisionMargin0 = lsMem.gSpuContactManifoldAlgo.getCollisionMargin0();
+							collisionPairInput.m_collisionMargin1 = lsMem.gSpuContactManifoldAlgo.getCollisionMargin1();
+							
+							
+							
+							//??cellDmaWaitTagStatusAll(DMA_MASK(1));
+							
+
+							if (1)
+							{
+								//snPause();
+
+								// Get the collision objects
+								dmaAndSetupCollisionObjects(collisionPairInput, lsMem);
+
+								if (lsMem.getColObj0()->isActive() || lsMem.getColObj1()->isActive())
+								{
+									handleCollisionPair(collisionPairInput, lsMem, spuContacts, 
+										(ppu_address_t)lsMem.getColObj0()->getCollisionShape(), &lsMem.gCollisionShapes[0].collisionShape,
+										(ppu_address_t)lsMem.getColObj1()->getCollisionShape(), &lsMem.gCollisionShapes[1].collisionShape);
+								}
+
+							}
+						}
+
+					}
+				}
+			}
+		} //end for (j = 0; j < numOnPage; j++)
+
+	}//	for 
+
+
+	return;
+}
Index: /code/branches/physics/src/bullet/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuGatheringCollisionTask.h
===================================================================
--- /code/branches/physics/src/bullet/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuGatheringCollisionTask.h	(revision 1966)
+++ /code/branches/physics/src/bullet/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuGatheringCollisionTask.h	(revision 1966)
@@ -0,0 +1,140 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+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.
+*/
+
+#ifndef SPU_GATHERING_COLLISION_TASK_H
+#define SPU_GATHERING_COLLISION_TASK_H
+
+#include "../PlatformDefinitions.h"
+//#define DEBUG_SPU_COLLISION_DETECTION 1
+
+
+///Task Description for SPU collision detection
+struct SpuGatherAndProcessPairsTaskDesc 
+{
+	ppu_address_t	inPtr;//m_pairArrayPtr;
+	//mutex variable
+	uint32_t	m_someMutexVariableInMainMemory;
+
+	ppu_address_t	m_dispatcher;
+
+	uint32_t	numOnLastPage;
+
+	uint16_t numPages;
+	uint16_t taskId;
+	bool m_useEpa;
+
+	struct	CollisionTask_LocalStoreMemory*	m_lsMemory; 
+}
+
+#if  defined(__CELLOS_LV2__) || defined(USE_LIBSPE2)
+__attribute__ ((aligned (128)))
+#endif
+;
+
+
+void	processCollisionTask(void* userPtr, void* lsMemory);
+
+void*	createCollisionLocalStoreMemory();
+
+
+#if defined(USE_LIBSPE2) && defined(__SPU__)
+#include "../SpuLibspe2Support.h"
+#include <spu_intrinsics.h>
+#include <spu_mfcio.h>
+#include <SpuFakeDma.h>
+
+//#define DEBUG_LIBSPE2_SPU_TASK
+
+
+
+int main(unsigned long long speid, addr64 argp, addr64 envp)
+{
+	printf("SPU: hello \n");
+	
+	ATTRIBUTE_ALIGNED128(btSpuStatus status);
+	ATTRIBUTE_ALIGNED16( SpuGatherAndProcessPairsTaskDesc taskDesc ) ;
+	unsigned int received_message = Spu_Mailbox_Event_Nothing;
+    bool shutdown = false;
+
+	cellDmaGet(&status, argp.ull, sizeof(btSpuStatus), DMA_TAG(3), 0, 0);
+	cellDmaWaitTagStatusAll(DMA_MASK(3));
+
+	status.m_status = Spu_Status_Free;
+	status.m_lsMemory.p = createCollisionLocalStoreMemory();
+
+	cellDmaLargePut(&status, argp.ull, sizeof(btSpuStatus), DMA_TAG(3), 0, 0);
+	cellDmaWaitTagStatusAll(DMA_MASK(3));
+	
+	
+	while ( btLikely( !shutdown ) )
+	{
+		
+		received_message = spu_read_in_mbox();
+		
+		if( btLikely( received_message == Spu_Mailbox_Event_Task ))
+		{
+#ifdef DEBUG_LIBSPE2_SPU_TASK
+			printf("SPU: received Spu_Mailbox_Event_Task\n");
+#endif //DEBUG_LIBSPE2_SPU_TASK
+
+			// refresh the status
+			cellDmaGet(&status, argp.ull, sizeof(btSpuStatus), DMA_TAG(3), 0, 0);
+			cellDmaWaitTagStatusAll(DMA_MASK(3));
+		
+			btAssert(status.m_status==Spu_Status_Occupied);
+			
+			cellDmaGet(&taskDesc, status.m_taskDesc.p, sizeof(SpuGatherAndProcessPairsTaskDesc), DMA_TAG(3), 0, 0);
+			cellDmaWaitTagStatusAll(DMA_MASK(3));
+#ifdef DEBUG_LIBSPE2_SPU_TASK		
+			printf("SPU:processCollisionTask\n");	
+#endif //DEBUG_LIBSPE2_SPU_TASK
+			processCollisionTask((void*)&taskDesc, taskDesc.m_lsMemory);
+			
+#ifdef DEBUG_LIBSPE2_SPU_TASK
+			printf("SPU:finished processCollisionTask\n");
+#endif //DEBUG_LIBSPE2_SPU_TASK
+		}
+		else
+		{
+#ifdef DEBUG_LIBSPE2_SPU_TASK
+			printf("SPU: received ShutDown\n");
+#endif //DEBUG_LIBSPE2_SPU_TASK
+			if( btLikely( received_message == Spu_Mailbox_Event_Shutdown ) )
+			{
+				shutdown = true;
+			}
+			else
+			{
+				//printf("SPU - Sth. recieved\n");
+			}
+		}
+
+		// set to status free and wait for next task
+		status.m_status = Spu_Status_Free;
+		cellDmaLargePut(&status, argp.ull, sizeof(btSpuStatus), DMA_TAG(3), 0, 0);
+		cellDmaWaitTagStatusAll(DMA_MASK(3));		
+				
+		
+  	}
+
+	printf("SPU: shutdown\n");
+  	return 0;
+}
+#endif // USE_LIBSPE2
+
+
+#endif //SPU_GATHERING_COLLISION_TASK_H
+
+
Index: /code/branches/physics/src/bullet/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuGjkEpa2.cpp
===================================================================
--- /code/branches/physics/src/bullet/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuGjkEpa2.cpp	(revision 1966)
+++ /code/branches/physics/src/bullet/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuGjkEpa2.cpp	(revision 1966)
@@ -0,0 +1,850 @@
+#include "BulletCollision/CollisionShapes/btConvexInternalShape.h"
+#include "BulletCollision/CollisionShapes/btSphereShape.h"
+#include "SpuCollisionShapes.h"
+#include "SpuGjkEpa2.h"
+
+#if defined(DEBUG) || defined (_DEBUG)
+#include <stdio.h> //for debug printf
+#ifdef __SPU__
+#include <spu_printf.h>
+#define printf spu_printf
+#endif //__SPU__
+#endif
+
+namespace gjkepa2_spu_impl
+{
+
+// Config
+
+	/* GJK	*/ 
+#define GJK_MAX_ITERATIONS	128
+#define GJK_ACCURARY		((btScalar)0.0001)
+#define GJK_MIN_DISTANCE	((btScalar)0.0001)
+#define GJK_DUPLICATED_EPS	((btScalar)0.0001)
+#define GJK_SIMPLEX2_EPS	((btScalar)0.0)
+#define GJK_SIMPLEX3_EPS	((btScalar)0.0)
+#define GJK_SIMPLEX4_EPS	((btScalar)0.0)
+
+	/* EPA	*/ 
+#define EPA_MAX_VERTICES	64
+#define EPA_MAX_FACES		(EPA_MAX_VERTICES*2)
+#define EPA_MAX_ITERATIONS	255
+#define EPA_ACCURACY		((btScalar)0.0001)
+#define EPA_FALLBACK		(10*EPA_ACCURACY)
+#define EPA_PLANE_EPS		((btScalar)0.00001)
+#define EPA_INSIDE_EPS		((btScalar)0.01)
+
+
+// Shorthands
+typedef unsigned int	U;
+typedef unsigned char	U1;
+
+struct convexShape
+{
+	void* shape;
+	SpuConvexPolyhedronVertexData* convexData;
+	int shapeType;
+	float margin;
+};
+
+// MinkowskiDiff
+struct	MinkowskiDiff
+	{
+	convexShape m_shapes[2];
+	btMatrix3x3				m_toshape1;
+	btTransform				m_toshape0;
+	btVector3				(btConvexShape::*Ls)(const btVector3&) const;
+	void					EnableMargin(bool enable)
+		{
+#if 0
+		if(enable)
+			Ls=&btConvexShape::localGetSupportingVertex;
+			else
+			Ls=&btConvexShape::localGetSupportingVertexWithoutMargin;
+#endif
+		}	
+	inline btVector3		Support0(const btVector3& d) const
+		{
+			btVector3 sp = localGetSupportingVertexWithoutMargin (m_shapes[0].shapeType, m_shapes[0].shape, d, m_shapes[0].convexData);
+			btVector3 ud = d;
+			ud.normalize();
+			sp += ud * m_shapes[0].margin;
+			return sp;
+	//	return(((m_shapes[0])->*(Ls))(d));
+		}
+	inline btVector3		Support1(const btVector3& d) const
+		{
+			btVector3 nd = m_toshape1*d;
+			btVector3 ud = nd;
+			ud.normalize ();
+			btVector3 sp = localGetSupportingVertexWithoutMargin (m_shapes[1].shapeType, m_shapes[1].shape, nd, m_shapes[1].convexData);
+			sp += ud * m_shapes[1].margin;
+			return m_toshape0 * sp;
+	//	return(m_toshape0*((m_shapes[1])->*(Ls))(m_toshape1*d));
+		}
+	inline btVector3		Support(const btVector3& d) const
+		{
+			return(Support0(d)-Support1(-d));
+		}
+	btVector3				Support(const btVector3& d,U index) const
+		{
+
+		if(index)
+			return(Support1(d));
+			else
+			return(Support0(d));
+
+		}
+	};
+
+typedef	MinkowskiDiff	tShape;
+
+// GJK
+struct	GJK
+{
+/* Types		*/ 
+struct	sSV
+	{
+	btVector3	d,w;
+	};
+struct	sSimplex
+	{
+	sSV*		c[4];
+	btScalar	p[4];
+	U			rank;
+	};
+struct	eStatus	{ enum _ {
+	Valid,
+	Inside,
+	Failed		};};
+/* Fields		*/ 
+tShape			m_shape;
+btVector3		m_ray;
+btScalar		m_distance;
+sSimplex		m_simplices[2];
+sSV				m_store[4];
+sSV*			m_free[4];
+U				m_nfree;
+U				m_current;
+sSimplex*		m_simplex;
+eStatus::_		m_status;
+/* Methods		*/ 
+					GJK()
+	{
+	Initialize();
+	}
+void				Initialize()
+	{
+	m_ray		=	btVector3(0,0,0);
+	m_nfree		=	0;
+	m_status	=	eStatus::Failed;
+	m_current	=	0;
+	m_distance	=	0;
+	}
+eStatus::_			Evaluate(const tShape& shapearg,const btVector3& guess)
+	{
+	U			iterations=0;
+	btScalar	sqdist=0;
+	btScalar	alpha=0;
+	btVector3	lastw[4];
+	U			clastw=0;
+	/* Initialize solver		*/ 
+	m_free[0]			=	&m_store[0];
+	m_free[1]			=	&m_store[1];
+	m_free[2]			=	&m_store[2];
+	m_free[3]			=	&m_store[3];
+	m_nfree				=	4;
+	m_current			=	0;
+	m_status			=	eStatus::Valid;
+	m_shape				=	shapearg;
+	m_distance			=	0;
+	/* Initialize simplex		*/ 
+	m_simplices[0].rank	=	0;
+	m_ray				=	guess;
+	const btScalar	sqrl=	m_ray.length2();
+	appendvertice(m_simplices[0],sqrl>0?-m_ray:btVector3(1,0,0));
+	m_simplices[0].p[0]	=	1;
+	m_ray				=	m_simplices[0].c[0]->w;	
+	sqdist				=	sqrl;
+	lastw[0]			=
+	lastw[1]			=
+	lastw[2]			=
+	lastw[3]			=	m_ray;
+	/* Loop						*/ 
+	do	{
+		const U		next=1-m_current;
+		sSimplex&	cs=m_simplices[m_current];
+		sSimplex&	ns=m_simplices[next];
+		/* Check zero							*/ 
+		const btScalar	rl=m_ray.length();
+		if(rl<GJK_MIN_DISTANCE)
+			{/* Touching or inside				*/ 
+			m_status=eStatus::Inside;
+			break;
+			}
+		/* Append new vertice in -'v' direction	*/ 
+		appendvertice(cs,-m_ray);
+		const btVector3&	w=cs.c[cs.rank-1]->w;
+		bool				found=false;
+		for(U i=0;i<4;++i)
+			{
+			if((w-lastw[i]).length2()<GJK_DUPLICATED_EPS)
+				{ found=true;break; }
+			}
+		if(found)
+			{/* Return old simplex				*/ 
+			removevertice(m_simplices[m_current]);
+			break;
+			}
+			else
+			{/* Update lastw					*/ 
+			lastw[clastw=(clastw+1)&3]=w;
+			}
+		/* Check for termination				*/ 
+		const btScalar	omega=dot(m_ray,w)/rl;
+		alpha=btMax(omega,alpha);
+		if(((rl-alpha)-(GJK_ACCURARY*rl))<=0)
+			{/* Return old simplex				*/ 
+			removevertice(m_simplices[m_current]);
+			break;
+			}		
+		/* Reduce simplex						*/ 
+		btScalar	weights[4];
+		U			mask=0;
+		switch(cs.rank)
+			{
+			case	2:	sqdist=projectorigin(	cs.c[0]->w,
+												cs.c[1]->w,
+												weights,mask);break;
+			case	3:	sqdist=projectorigin(	cs.c[0]->w,
+												cs.c[1]->w,
+												cs.c[2]->w,
+												weights,mask);break;
+			case	4:	sqdist=projectorigin(	cs.c[0]->w,
+												cs.c[1]->w,
+												cs.c[2]->w,
+												cs.c[3]->w,
+												weights,mask);break;
+			}
+		if(sqdist>=0)
+			{/* Valid	*/ 
+			ns.rank		=	0;
+			m_ray		=	btVector3(0,0,0);
+			m_current	=	next;
+			for(U i=0,ni=cs.rank;i<ni;++i)
+				{
+				if(mask&(1<<i))
+					{
+					ns.c[ns.rank]		=	cs.c[i];
+					ns.p[ns.rank++]		=	weights[i];
+					m_ray				+=	cs.c[i]->w*weights[i];
+					}
+					else
+					{
+					m_free[m_nfree++]	=	cs.c[i];
+					}
+				}
+			if(mask==15) m_status=eStatus::Inside;
+			}
+			else
+			{/* Return old simplex				*/ 
+			removevertice(m_simplices[m_current]);
+			break;
+			}
+		m_status=((++iterations)<GJK_MAX_ITERATIONS)?m_status:eStatus::Failed;
+		} while(m_status==eStatus::Valid);
+	m_simplex=&m_simplices[m_current];
+	switch(m_status)
+		{
+		case	eStatus::Valid:		m_distance=m_ray.length();break;
+		case	eStatus::Inside:	m_distance=0;break;
+		}	
+	return(m_status);
+	}
+bool					EncloseOrigin()
+	{
+	switch(m_simplex->rank)
+		{
+		case	1:
+			{
+			for(U i=0;i<3;++i)
+				{
+				btVector3		axis=btVector3(0,0,0);
+				axis[i]=1;
+				appendvertice(*m_simplex, axis);
+				if(EncloseOrigin())	return(true);
+				removevertice(*m_simplex);
+				appendvertice(*m_simplex,-axis);
+				if(EncloseOrigin())	return(true);
+				removevertice(*m_simplex);
+				}
+			}
+		break;
+		case	2:
+			{
+			const btVector3	d=m_simplex->c[1]->w-m_simplex->c[0]->w;
+			for(U i=0;i<3;++i)
+				{
+				btVector3		axis=btVector3(0,0,0);
+				axis[i]=1;
+				if(btFabs(dot(axis,d))>0)
+					{
+					const btVector3	p=cross(d,axis);
+					appendvertice(*m_simplex, p);
+					if(EncloseOrigin())	return(true);
+					removevertice(*m_simplex);
+					appendvertice(*m_simplex,-p);
+					if(EncloseOrigin())	return(true);
+					removevertice(*m_simplex);
+					}
+				}
+			}
+		break;
+		case	3:
+			{
+			const btVector3	n=cross(m_simplex->c[1]->w-m_simplex->c[0]->w,
+									m_simplex->c[2]->w-m_simplex->c[0]->w);
+			const btScalar	l=n.length();
+			if(l>0)
+				{
+				appendvertice(*m_simplex,n);
+				if(EncloseOrigin())	return(true);
+				removevertice(*m_simplex);
+				appendvertice(*m_simplex,-n);
+				if(EncloseOrigin())	return(true);
+				removevertice(*m_simplex);
+				}
+			}
+		break;
+		case	4:
+			{
+			if(btFabs(det(	m_simplex->c[0]->w-m_simplex->c[3]->w,
+							m_simplex->c[1]->w-m_simplex->c[3]->w,
+							m_simplex->c[2]->w-m_simplex->c[3]->w))>0)
+				return(true);
+			}
+		break;
+		}
+	return(false);
+	}
+/* Internals	*/ 
+void				getsupport(const btVector3& d,sSV& sv) const
+	{
+	sv.d	=	d/d.length();
+	sv.w	=	m_shape.Support(sv.d);
+	}
+void				removevertice(sSimplex& simplex)
+	{
+	m_free[m_nfree++]=simplex.c[--simplex.rank];
+	}
+void				appendvertice(sSimplex& simplex,const btVector3& v)
+	{
+	simplex.p[simplex.rank]=0;
+	simplex.c[simplex.rank]=m_free[--m_nfree];
+	getsupport(v,*simplex.c[simplex.rank++]);
+	}
+static btScalar		det(const btVector3& a,const btVector3& b,const btVector3& c)
+	{
+	return(	a.y()*b.z()*c.x()+a.z()*b.x()*c.y()-
+			a.x()*b.z()*c.y()-a.y()*b.x()*c.z()+
+			a.x()*b.y()*c.z()-a.z()*b.y()*c.x());
+	}
+static btScalar		projectorigin(	const btVector3& a,
+									const btVector3& b,
+									btScalar* w,U& m)
+	{
+	const btVector3	d=b-a;
+	const btScalar	l=d.length2();
+	if(l>GJK_SIMPLEX2_EPS)
+		{
+		const btScalar	t(l>0?-dot(a,d)/l:0);
+		if(t>=1)		{ w[0]=0;w[1]=1;m=2;return(b.length2()); }
+		else if(t<=0)	{ w[0]=1;w[1]=0;m=1;return(a.length2()); }
+		else			{ w[0]=1-(w[1]=t);m=3;return((a+d*t).length2()); }
+		}
+	return(-1);
+	}
+static btScalar		projectorigin(	const btVector3& a,
+									const btVector3& b,
+									const btVector3& c,
+									btScalar* w,U& m)
+	{
+	static const U		imd3[]={1,2,0};
+	const btVector3*	vt[]={&a,&b,&c};
+	const btVector3		dl[]={a-b,b-c,c-a};
+	const btVector3		n=cross(dl[0],dl[1]);
+	const btScalar		l=n.length2();
+	if(l>GJK_SIMPLEX3_EPS)
+		{
+		btScalar	mindist=-1;
+		btScalar	subw[2] = { btScalar(0.0f), btScalar(0.0f) };
+		U			subm;
+		for(U i=0;i<3;++i)
+			{
+			if(dot(*vt[i],cross(dl[i],n))>0)
+				{
+				const U			j=imd3[i];
+				const btScalar	subd(projectorigin(*vt[i],*vt[j],subw,subm));
+				if((mindist<0)||(subd<mindist))
+					{
+					mindist		=	subd;
+					m			=	((subm&1)?1<<i:0)+((subm&2)?1<<j:0);
+					w[i]		=	subw[0];
+					w[j]		=	subw[1];
+					w[imd3[j]]	=	0;				
+					}
+				}
+			}
+		if(mindist<0)
+			{
+			const btScalar	d=dot(a,n);	
+			const btScalar	s=btSqrt(l);
+			const btVector3	p=n*(d/l);
+			mindist	=	p.length2();
+			m		=	7;
+			w[0]	=	(cross(dl[1],b-p)).length()/s;
+			w[1]	=	(cross(dl[2],c-p)).length()/s;
+			w[2]	=	1-(w[0]+w[1]);
+			}
+		return(mindist);
+		}
+	return(-1);
+	}
+static btScalar		projectorigin(	const btVector3& a,
+									const btVector3& b,
+									const btVector3& c,
+									const btVector3& d,
+									btScalar* w,U& m)
+	{
+	static const U		imd3[]={1,2,0};
+	const btVector3*	vt[]={&a,&b,&c,&d};
+	const btVector3		dl[]={a-d,b-d,c-d};
+	const btScalar		vl=det(dl[0],dl[1],dl[2]);
+	const bool			ng=(vl*dot(a,cross(b-c,a-b)))<=0;
+	if(ng&&(btFabs(vl)>GJK_SIMPLEX4_EPS))
+		{
+		btScalar	mindist=-1;
+		btScalar	subw[3];
+		U			subm;
+		for(U i=0;i<3;++i)
+			{
+			const U			j=imd3[i];
+			const btScalar	s=vl*dot(d,cross(dl[i],dl[j]));
+			if(s>0)
+				{
+				const btScalar	subd=projectorigin(*vt[i],*vt[j],d,subw,subm);
+				if((mindist<0)||(subd<mindist))
+					{
+					mindist		=	subd;
+					m			=	(subm&1?1<<i:0)+
+									(subm&2?1<<j:0)+
+									(subm&4?8:0);
+					w[i]		=	subw[0];
+					w[j]		=	subw[1];
+					w[imd3[j]]	=	0;
+					w[3]		=	subw[2];
+					}
+				}
+			}
+		if(mindist<0)
+			{
+			mindist	=	0;
+			m		=	15;
+			w[0]	=	det(c,b,d)/vl;
+			w[1]	=	det(a,c,d)/vl;
+			w[2]	=	det(b,a,d)/vl;
+			w[3]	=	1-(w[0]+w[1]+w[2]);
+			}
+		return(mindist);
+		}
+	return(-1);
+	}
+};
+
+// EPA
+struct	EPA
+{
+/* Types		*/ 
+typedef	GJK::sSV	sSV;
+struct	sFace
+	{
+	btVector3	n;
+	btScalar	d;
+	btScalar	p;
+	sSV*		c[3];
+	sFace*		f[3];
+	sFace*		l[2];
+	U1			e[3];
+	U1			pass;
+	};
+struct	sList
+	{
+	sFace*		root;
+	U			count;
+				sList() : root(0),count(0)	{}
+	};
+struct	sHorizon
+	{
+	sFace*		cf;
+	sFace*		ff;
+	U			nf;
+				sHorizon() : cf(0),ff(0),nf(0)	{}
+	};
+struct	eStatus { enum _ {
+	Valid,
+	Touching,
+	Degenerated,
+	NonConvex,
+	InvalidHull,		
+	OutOfFaces,
+	OutOfVertices,
+	AccuraryReached,
+	FallBack,
+	Failed,		};};
+/* Fields		*/ 
+eStatus::_		m_status;
+GJK::sSimplex	m_result;
+btVector3		m_normal;
+btScalar		m_depth;
+sSV				m_sv_store[EPA_MAX_VERTICES];
+sFace			m_fc_store[EPA_MAX_FACES];
+U				m_nextsv;
+sList			m_hull;
+sList			m_stock;
+/* Methods		*/ 
+					EPA()
+	{
+	Initialize();	
+	}
+void				Initialize()
+	{
+	m_status	=	eStatus::Failed;
+	m_normal	=	btVector3(0,0,0);
+	m_depth		=	0;
+	m_nextsv	=	0;
+	for(U i=0;i<EPA_MAX_FACES;++i)
+		{
+		append(m_stock,&m_fc_store[EPA_MAX_FACES-i-1]);
+		}
+	}
+eStatus::_			Evaluate(GJK& gjk,const btVector3& guess)
+	{
+	GJK::sSimplex&	simplex=*gjk.m_simplex;
+	if((simplex.rank>1)&&gjk.EncloseOrigin())
+		{
+		/* Clean up				*/ 
+		while(m_hull.root)
+			{
+			sFace*	f(m_hull.root);
+			remove(m_hull,f);
+			append(m_stock,f);
+			}
+		m_status	=	eStatus::Valid;
+		m_nextsv	=	0;
+		/* Orient simplex		*/ 
+		if(gjk.det(	simplex.c[0]->w-simplex.c[3]->w,
+					simplex.c[1]->w-simplex.c[3]->w,
+					simplex.c[2]->w-simplex.c[3]->w)<0)
+			{
+			btSwap(simplex.c[0],simplex.c[1]);
+			btSwap(simplex.p[0],simplex.p[1]);
+			}
+		/* Build initial hull	*/ 
+		sFace*	tetra[]={newface(simplex.c[0],simplex.c[1],simplex.c[2],true),
+						newface(simplex.c[1],simplex.c[0],simplex.c[3],true),
+						newface(simplex.c[2],simplex.c[1],simplex.c[3],true),
+						newface(simplex.c[0],simplex.c[2],simplex.c[3],true)};
+		if(m_hull.count==4)
+			{
+			sFace*		best=findbest();
+			sFace		outer=*best;
+			U			pass=0;
+			U			iterations=0;
+			bind(tetra[0],0,tetra[1],0);
+			bind(tetra[0],1,tetra[2],0);
+			bind(tetra[0],2,tetra[3],0);
+			bind(tetra[1],1,tetra[3],2);
+			bind(tetra[1],2,tetra[2],1);
+			bind(tetra[2],2,tetra[3],1);
+			m_status=eStatus::Valid;
+			for(;iterations<EPA_MAX_ITERATIONS;++iterations)
+				{
+				if(m_nextsv<EPA_MAX_VERTICES)
+					{	
+					sHorizon		horizon;
+					sSV*			w=&m_sv_store[m_nextsv++];
+					bool			valid=true;					
+					best->pass	=	(U1)(++pass);
+					gjk.getsupport(best->n,*w);
+					const btScalar	wdist=dot(best->n,w->w)-best->d;
+					if(wdist>EPA_ACCURACY)
+						{
+						for(U j=0;(j<3)&&valid;++j)
+							{
+							valid&=expand(	pass,w,
+											best->f[j],best->e[j],
+											horizon);
+							}
+						if(valid&&(horizon.nf>=3))
+							{
+							bind(horizon.cf,1,horizon.ff,2);
+							remove(m_hull,best);
+							append(m_stock,best);
+							best=findbest();
+							if(best->p>=outer.p) outer=*best;
+							} else { m_status=eStatus::InvalidHull;break; }
+						} else { m_status=eStatus::AccuraryReached;break; }
+					} else { m_status=eStatus::OutOfVertices;break; }
+				}
+			const btVector3	projection=outer.n*outer.d;
+			m_normal	=	outer.n;
+			m_depth		=	outer.d;
+			m_result.rank	=	3;
+			m_result.c[0]	=	outer.c[0];
+			m_result.c[1]	=	outer.c[1];
+			m_result.c[2]	=	outer.c[2];
+			m_result.p[0]	=	cross(	outer.c[1]->w-projection,
+										outer.c[2]->w-projection).length();
+			m_result.p[1]	=	cross(	outer.c[2]->w-projection,
+										outer.c[0]->w-projection).length();
+			m_result.p[2]	=	cross(	outer.c[0]->w-projection,
+										outer.c[1]->w-projection).length();
+			const btScalar	sum=m_result.p[0]+m_result.p[1]+m_result.p[2];
+			m_result.p[0]	/=	sum;
+			m_result.p[1]	/=	sum;
+			m_result.p[2]	/=	sum;
+			return(m_status);
+			}
+		}
+	/* Fallback		*/ 
+	m_status	=	eStatus::FallBack;
+	m_normal	=	-guess;
+	const btScalar	nl=m_normal.length();
+	if(nl>0)
+		m_normal	=	m_normal/nl;
+		else
+		m_normal	=	btVector3(1,0,0);
+	m_depth	=	0;
+	m_result.rank=1;
+	m_result.c[0]=simplex.c[0];
+	m_result.p[0]=1;	
+	return(m_status);
+	}
+sFace*				newface(sSV* a,sSV* b,sSV* c,bool forced)
+	{
+	if(m_stock.root)
+		{
+		sFace*	face=m_stock.root;
+		remove(m_stock,face);
+		append(m_hull,face);
+		face->pass	=	0;
+		face->c[0]	=	a;
+		face->c[1]	=	b;
+		face->c[2]	=	c;
+		face->n		=	cross(b->w-a->w,c->w-a->w);
+		const btScalar	l=face->n.length();
+		const bool		v=l>EPA_ACCURACY;
+		face->p		=	btMin(btMin(
+							dot(a->w,cross(face->n,a->w-b->w)),
+							dot(b->w,cross(face->n,b->w-c->w))),
+							dot(c->w,cross(face->n,c->w-a->w)))	/
+							(v?l:1);
+		face->p		=	face->p>=-EPA_INSIDE_EPS?0:face->p;
+		if(v)
+			{
+			face->d		=	dot(a->w,face->n)/l;
+			face->n		/=	l;
+			if(forced||(face->d>=-EPA_PLANE_EPS))
+				{
+				return(face);
+				} else m_status=eStatus::NonConvex;
+			} else m_status=eStatus::Degenerated;
+		remove(m_hull,face);
+		append(m_stock,face);
+		return(0);
+		}
+	m_status=m_stock.root?eStatus::OutOfVertices:eStatus::OutOfFaces;
+	return(0);
+	}
+sFace*				findbest()
+	{
+	sFace*		minf=m_hull.root;
+	btScalar	mind=minf->d*minf->d;
+	btScalar	maxp=minf->p;
+	for(sFace* f=minf->l[1];f;f=f->l[1])
+		{
+		const btScalar	sqd=f->d*f->d;
+		if((f->p>=maxp)&&(sqd<mind))
+			{
+			minf=f;
+			mind=sqd;
+			maxp=f->p;
+			}
+		}
+	return(minf);
+	}
+bool				expand(U pass,sSV* w,sFace* f,U e,sHorizon& horizon)
+	{
+	static const U	i1m3[]={1,2,0};
+	static const U	i2m3[]={2,0,1};
+	if(f->pass!=pass)
+		{
+		const U	e1=i1m3[e];
+		if((dot(f->n,w->w)-f->d)<-EPA_PLANE_EPS)
+			{
+			sFace*	nf=newface(f->c[e1],f->c[e],w,false);
+			if(nf)
+				{
+				bind(nf,0,f,e);
+				if(horizon.cf) bind(horizon.cf,1,nf,2); else horizon.ff=nf;
+				horizon.cf=nf;
+				++horizon.nf;
+				return(true);
+				}
+			}
+			else
+			{
+			const U	e2=i2m3[e];
+			f->pass		=	(U1)pass;
+			if(	expand(pass,w,f->f[e1],f->e[e1],horizon)&&
+				expand(pass,w,f->f[e2],f->e[e2],horizon))
+				{
+				remove(m_hull,f);
+				append(m_stock,f);
+				return(true);
+				}
+			}
+		}
+	return(false);
+	}
+static inline void		bind(sFace* fa,U ea,sFace* fb,U eb)
+	{
+	fa->e[ea]=(U1)eb;fa->f[ea]=fb;
+	fb->e[eb]=(U1)ea;fb->f[eb]=fa;
+	}
+static inline void		append(sList& list,sFace* face)
+	{
+	face->l[0]	=	0;
+	face->l[1]	=	list.root;
+	if(list.root) list.root->l[0]=face;
+	list.root	=	face;
+	++list.count;
+	}
+static inline void		remove(sList& list,sFace* face)
+	{
+	if(face->l[1]) face->l[1]->l[0]=face->l[0];
+	if(face->l[0]) face->l[0]->l[1]=face->l[1];
+	if(face==list.root) list.root=face->l[1];
+	--list.count;
+	}
+};
+
+//
+static void	Initialize(void* shapeA,
+						SpuConvexPolyhedronVertexData* convexDataA,
+						int shapeTypeA,
+						float marginA,
+						const btTransform& wtrs0,
+						void* shapeB,
+						SpuConvexPolyhedronVertexData* convexDataB,
+						int shapeTypeB,
+						float marginB,
+						const btTransform& wtrs1,	
+						SpuGjkEpaSolver2::sResults& results,
+						tShape& shape,
+						bool withmargins)
+{
+/* Results		*/ 
+results.witnesses[0]	=
+results.witnesses[1]	=	btVector3(0,0,0);
+results.status			=	SpuGjkEpaSolver2::sResults::Separated;
+/* Shape		*/ 
+shape.m_shapes[0].margin = marginA;
+shape.m_shapes[0].shape = shapeA;
+shape.m_shapes[0].shapeType = shapeTypeA;
+shape.m_shapes[0].convexData = convexDataA;
+shape.m_shapes[1].margin = marginB;
+shape.m_shapes[1].shape = shapeB;
+shape.m_shapes[1].shapeType = shapeTypeB;
+shape.m_shapes[1].convexData = convexDataB;
+shape.m_toshape1		=	wtrs1.getBasis().transposeTimes(wtrs0.getBasis());
+shape.m_toshape0		=	wtrs0.inverseTimes(wtrs1);
+shape.EnableMargin(withmargins);
+}
+
+}
+
+//
+// Api
+//
+
+using namespace	gjkepa2_spu_impl;
+
+//
+int			SpuGjkEpaSolver2::StackSizeRequirement()
+{
+return(sizeof(GJK)+sizeof(EPA));
+}
+
+//
+bool   SpuGjkEpaSolver2::Penetration(void* shapeA,
+									SpuConvexPolyhedronVertexData* convexDataA,
+									int shapeTypeA,
+									float marginA,
+									const btTransform& wtrs0,
+									void* shapeB,
+									SpuConvexPolyhedronVertexData* convexDataB,
+									int shapeTypeB,
+									float marginB,
+									const btTransform& wtrs1,
+									const btVector3&		guess,
+									sResults&				results)
+{
+tShape			shape;
+Initialize(shapeA, convexDataA, shapeTypeA, marginA, wtrs0, shapeB, convexDataB, shapeTypeB, marginB, wtrs1, results,shape,true);
+GJK				gjk;	
+GJK::eStatus::_	gjk_status=gjk.Evaluate(shape,-guess);
+switch(gjk_status)
+	{
+	case	GJK::eStatus::Inside:
+		{
+		EPA				epa;
+		EPA::eStatus::_	epa_status=epa.Evaluate(gjk,-guess);
+		if(epa_status!=EPA::eStatus::Failed)
+			{
+			btVector3	w0=btVector3(0,0,0);
+			for(U i=0;i<epa.m_result.rank;++i)
+				{
+				w0+=shape.Support(epa.m_result.c[i]->d,0)*epa.m_result.p[i];
+				}
+			results.status			=	sResults::Penetrating;
+			results.witnesses[0]	=	wtrs0*w0;
+			results.witnesses[1]	=	wtrs0*(w0-epa.m_normal*epa.m_depth);
+			return(true);
+			} else results.status=sResults::EPA_Failed;
+		}
+	break;
+	case	GJK::eStatus::Failed:
+	results.status=sResults::GJK_Failed;
+	break;
+	}
+return(false);
+}
+
+/* Symbols cleanup		*/ 
+
+#undef GJK_MAX_ITERATIONS
+#undef GJK_ACCURARY
+#undef GJK_MIN_DISTANCE
+#undef GJK_DUPLICATED_EPS
+#undef GJK_SIMPLEX2_EPS
+#undef GJK_SIMPLEX3_EPS
+#undef GJK_SIMPLEX4_EPS
+
+#undef EPA_MAX_VERTICES
+#undef EPA_MAX_FACES
+#undef EPA_MAX_ITERATIONS
+#undef EPA_ACCURACY
+#undef EPA_FALLBACK
+#undef EPA_PLANE_EPS
+#undef EPA_INSIDE_EPS
Index: /code/branches/physics/src/bullet/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuGjkEpa2.h
===================================================================
--- /code/branches/physics/src/bullet/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuGjkEpa2.h	(revision 1966)
+++ /code/branches/physics/src/bullet/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuGjkEpa2.h	(revision 1966)
@@ -0,0 +1,38 @@
+#ifndef _68DA1F85_90B7_4bb0_A705_83B4040A75C6_
+#define _68DA1F85_90B7_4bb0_A705_83B4040A75C6_
+#include "BulletCollision/CollisionShapes/btConvexShape.h"
+
+///btGjkEpaSolver contributed under zlib by Nathanael Presson
+struct	SpuGjkEpaSolver2
+{
+struct	sResults
+	{
+	enum eStatus
+		{
+		Separated,		/* Shapes doesnt penetrate												*/ 
+		Penetrating,	/* Shapes are penetrating												*/ 
+		GJK_Failed,		/* GJK phase fail, no big issue, shapes are probably just 'touching'	*/ 
+		EPA_Failed,		/* EPA phase fail, bigger problem, need to save parameters, and debug	*/ 
+		}		status;
+	btVector3	witnesses[2];
+	btVector3	normal;
+	};
+
+static int		StackSizeRequirement();
+
+
+static bool		Penetration(void* shapeA,
+							SpuConvexPolyhedronVertexData* convexDataA,
+							int shapeTypeA,
+							float marginA,
+							const btTransform& xformA,
+							void* shapeB,
+							SpuConvexPolyhedronVertexData* convexDataB,
+							int shapeTypeB,
+							float marginB,
+							const btTransform& xformB,
+							const btVector3& guess,
+							sResults&	results);
+};
+
+#endif
Index: /code/branches/physics/src/bullet/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuGjkPairDetector.cpp
===================================================================
--- /code/branches/physics/src/bullet/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuGjkPairDetector.cpp	(revision 1966)
+++ /code/branches/physics/src/bullet/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuGjkPairDetector.cpp	(revision 1966)
@@ -0,0 +1,311 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+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.
+*/
+
+#include "SpuGjkPairDetector.h"
+#include "SpuConvexPenetrationDepthSolver.h"
+#include "SpuCollisionShapes.h"
+
+
+
+#if defined(DEBUG) || defined (_DEBUG)
+#include <stdio.h> //for debug printf
+#ifdef __SPU__
+#include <spu_printf.h>
+#define printf spu_printf
+#endif //__SPU__
+#endif
+
+//must be above the machine epsilon
+#define REL_ERROR2 btScalar(1.0e-6)
+
+//temp globals, to improve GJK/EPA/penetration calculations
+int gSpuNumDeepPenetrationChecks = 0;
+int gSpuNumGjkChecks = 0;
+
+
+
+SpuGjkPairDetector::SpuGjkPairDetector(void* objectA,void* objectB,int shapeTypeA, int shapeTypeB, float marginA,float marginB,SpuVoronoiSimplexSolver* simplexSolver, const SpuConvexPenetrationDepthSolver*	penetrationDepthSolver)
+:m_cachedSeparatingAxis(float(0.),float(0.),float(1.)),
+m_penetrationDepthSolver(penetrationDepthSolver),
+m_simplexSolver(simplexSolver),
+m_minkowskiA(objectA),
+m_minkowskiB(objectB),
+m_shapeTypeA(shapeTypeA),
+m_shapeTypeB(shapeTypeB),
+m_marginA(marginA),
+m_marginB(marginB),
+m_ignoreMargin(false),
+m_lastUsedMethod(-1),
+m_catchDegeneracies(1)
+{
+}
+
+void SpuGjkPairDetector::getClosestPoints(const SpuClosestPointInput& input,SpuContactResult& output)
+{
+	btScalar distance=btScalar(0.);
+	btVector3	normalInB(btScalar(0.),btScalar(0.),btScalar(0.));
+	btVector3 pointOnA,pointOnB;
+	btTransform	localTransA = input.m_transformA;
+	btTransform localTransB = input.m_transformB;
+	btVector3 positionOffset = (localTransA.getOrigin() + localTransB.getOrigin()) * btScalar(0.5);
+	localTransA.getOrigin() -= positionOffset;
+	localTransB.getOrigin() -= positionOffset;
+
+	btScalar marginA = m_marginA;
+	btScalar marginB = m_marginB;
+
+	gSpuNumGjkChecks++;
+
+	//for CCD we don't use margins
+	if (m_ignoreMargin)
+	{
+		marginA = btScalar(0.);
+		marginB = btScalar(0.);
+	}
+
+	m_curIter = 0;
+	int gGjkMaxIter = 1000;//this is to catch invalid input, perhaps check for #NaN?
+	m_cachedSeparatingAxis.setValue(0,1,0);
+
+	bool isValid = false;
+	bool checkSimplex = false;
+	bool checkPenetration = true;
+	m_degenerateSimplex = 0;
+
+	m_lastUsedMethod = -1;
+
+	{
+		btScalar squaredDistance = SIMD_INFINITY;
+		btScalar delta = btScalar(0.);
+		
+		btScalar margin = marginA + marginB;
+		
+		
+
+		m_simplexSolver->reset();
+		
+		for ( ; ; )
+		//while (true)
+		{
+
+			btVector3 seperatingAxisInA = (-m_cachedSeparatingAxis)* input.m_transformA.getBasis();
+			btVector3 seperatingAxisInB = m_cachedSeparatingAxis* input.m_transformB.getBasis();
+
+//			btVector3 pInA = m_minkowskiA->localGetSupportingVertexWithoutMargin(seperatingAxisInA);
+//			btVector3 qInB = m_minkowskiB->localGetSupportingVertexWithoutMargin(seperatingAxisInB);
+
+			btVector3 pInA  = localGetSupportingVertexWithoutMargin(m_shapeTypeA, m_minkowskiA, seperatingAxisInA,input.m_convexVertexData[0]);//, &featureIndexA);
+			btVector3 qInB  = localGetSupportingVertexWithoutMargin(m_shapeTypeB, m_minkowskiB, seperatingAxisInB,input.m_convexVertexData[1]);//, &featureIndexB);
+
+
+			btPoint3  pWorld = localTransA(pInA);	
+			btPoint3  qWorld = localTransB(qInB);
+			
+			btVector3 w	= pWorld - qWorld;
+			delta = m_cachedSeparatingAxis.dot(w);
+
+			// potential exit, they don't overlap
+			if ((delta > btScalar(0.0)) && (delta * delta > squaredDistance * input.m_maximumDistanceSquared)) 
+			{
+				checkPenetration = false;
+				break;
+			}
+
+			//exit 0: the new point is already in the simplex, or we didn't come any closer
+			if (m_simplexSolver->inSimplex(w))
+			{
+				m_degenerateSimplex = 1;
+				checkSimplex = true;
+				break;
+			}
+			// are we getting any closer ?
+			btScalar f0 = squaredDistance - delta;
+			btScalar f1 = squaredDistance * REL_ERROR2;
+
+			if (f0 <= f1)
+			{
+				if (f0 <= btScalar(0.))
+				{
+					m_degenerateSimplex = 2;
+				}
+				checkSimplex = true;
+				break;
+			}
+			//add current vertex to simplex
+			m_simplexSolver->addVertex(w, pWorld, qWorld);
+
+			//calculate the closest point to the origin (update vector v)
+			if (!m_simplexSolver->closest(m_cachedSeparatingAxis))
+			{
+				m_degenerateSimplex = 3;
+				checkSimplex = true;
+				break;
+			}
+
+			btScalar previousSquaredDistance = squaredDistance;
+			squaredDistance = m_cachedSeparatingAxis.length2();
+			
+			//redundant m_simplexSolver->compute_points(pointOnA, pointOnB);
+
+			//are we getting any closer ?
+			if (previousSquaredDistance - squaredDistance <= SIMD_EPSILON * previousSquaredDistance) 
+			{ 
+				m_simplexSolver->backup_closest(m_cachedSeparatingAxis);
+				checkSimplex = true;
+				break;
+			}
+
+			  //degeneracy, this is typically due to invalid/uninitialized worldtransforms for a btCollisionObject   
+              if (m_curIter++ > gGjkMaxIter)   
+              {   
+                      #if defined(DEBUG) || defined (_DEBUG)   
+
+                              printf("SpuGjkPairDetector maxIter exceeded:%i\n",m_curIter);   
+                              printf("sepAxis=(%f,%f,%f), squaredDistance = %f, shapeTypeA=%i,shapeTypeB=%i\n",   
+                              m_cachedSeparatingAxis.getX(),   
+                              m_cachedSeparatingAxis.getY(),   
+                              m_cachedSeparatingAxis.getZ(),   
+                              squaredDistance,   
+							  m_shapeTypeA,   
+                              m_shapeTypeB);
+
+                      #endif   
+                      break;   
+
+              } 
+
+
+			bool check = (!m_simplexSolver->fullSimplex());
+			//bool check = (!m_simplexSolver->fullSimplex() && squaredDistance > SIMD_EPSILON * m_simplexSolver->maxVertex());
+
+			if (!check)
+			{
+				//do we need this backup_closest here ?
+				m_simplexSolver->backup_closest(m_cachedSeparatingAxis);
+				break;
+			}
+		}
+
+		if (checkSimplex)
+		{
+			m_simplexSolver->compute_points(pointOnA, pointOnB);
+			normalInB = pointOnA-pointOnB;
+			btScalar lenSqr = m_cachedSeparatingAxis.length2();
+			//valid normal
+			if (lenSqr < 0.0001)
+			{
+				m_degenerateSimplex = 5;
+			} 
+			if (lenSqr > SIMD_EPSILON*SIMD_EPSILON)
+			{
+				btScalar rlen = btScalar(1.) / btSqrt(lenSqr );
+				normalInB *= rlen; //normalize
+				btScalar s = btSqrt(squaredDistance);
+			
+				btAssert(s > btScalar(0.0));
+				pointOnA -= m_cachedSeparatingAxis * (marginA / s);
+				pointOnB += m_cachedSeparatingAxis * (marginB / s);
+				distance = ((btScalar(1.)/rlen) - margin);
+				isValid = true;
+				
+				m_lastUsedMethod = 1;
+			} else
+			{
+				m_lastUsedMethod = 2;
+			}
+		}
+
+		bool catchDegeneratePenetrationCase = 
+			(m_catchDegeneracies && m_penetrationDepthSolver && m_degenerateSimplex && ((distance+margin) < 0.01));
+
+		//if (checkPenetration && !isValid)
+		if (checkPenetration && (!isValid || catchDegeneratePenetrationCase ))
+		{
+			//penetration case
+		
+			//if there is no way to handle penetrations, bail out
+			if (m_penetrationDepthSolver)
+			{
+				// Penetration depth case.
+				btVector3 tmpPointOnA,tmpPointOnB;
+				
+				gSpuNumDeepPenetrationChecks++;
+
+				bool isValid2 = m_penetrationDepthSolver->calcPenDepth( 
+					*m_simplexSolver, 
+					m_minkowskiA,m_minkowskiB,
+                    m_shapeTypeA, m_shapeTypeB,
+                    marginA, marginB,
+					localTransA,localTransB,
+					m_cachedSeparatingAxis, tmpPointOnA, tmpPointOnB,
+					0,input.m_stackAlloc,input.m_convexVertexData[0], input.m_convexVertexData[1]
+					);
+
+				if (isValid2)
+				{
+					btVector3 tmpNormalInB = tmpPointOnB-tmpPointOnA;
+					btScalar lenSqr = tmpNormalInB.length2();
+					if (lenSqr > (SIMD_EPSILON*SIMD_EPSILON))
+					{
+						tmpNormalInB /= btSqrt(lenSqr);
+						btScalar distance2 = -(tmpPointOnA-tmpPointOnB).length();
+						//only replace valid penetrations when the result is deeper (check)
+						if (!isValid || (distance2 < distance))
+						{
+							distance = distance2;
+							pointOnA = tmpPointOnA;
+							pointOnB = tmpPointOnB;
+							normalInB = tmpNormalInB;
+							isValid = true;
+							m_lastUsedMethod = 3;
+						} else
+						{
+							
+						}
+					} else
+					{
+						//isValid = false;
+						m_lastUsedMethod = 4;
+					}
+				} else
+				{
+					m_lastUsedMethod = 5;
+				}
+				
+			}
+		}
+	}
+
+	if (isValid)
+	{
+#ifdef __SPU__
+		//spu_printf("distance\n");
+#endif //__SPU__
+
+
+		output.addContactPoint(
+			normalInB,
+			pointOnB+positionOffset,
+			distance);
+		//printf("gjk add:%f",distance);
+	}
+
+
+}
+
+
+
+
+
Index: /code/branches/physics/src/bullet/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuGjkPairDetector.h
===================================================================
--- /code/branches/physics/src/bullet/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuGjkPairDetector.h	(revision 1966)
+++ /code/branches/physics/src/bullet/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuGjkPairDetector.h	(revision 1966)
@@ -0,0 +1,93 @@
+
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+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.
+*/
+
+
+
+
+#ifndef SPU_GJK_PAIR_DETECTOR_H
+#define SPU_GJK_PAIR_DETECTOR_H
+
+
+
+#include "SpuContactResult.h"
+
+
+#include "SpuVoronoiSimplexSolver.h"
+class SpuConvexPenetrationDepthSolver;
+
+/// btGjkPairDetector uses GJK to implement the btDiscreteCollisionDetectorInterface
+class SpuGjkPairDetector 
+{
+	
+
+	btVector3	m_cachedSeparatingAxis;
+	const SpuConvexPenetrationDepthSolver*	m_penetrationDepthSolver;
+	SpuVoronoiSimplexSolver* m_simplexSolver;
+	void* m_minkowskiA;
+	void* m_minkowskiB;
+    int m_shapeTypeA;
+    int m_shapeTypeB;
+    float m_marginA;
+    float m_marginB;
+	bool		m_ignoreMargin;
+	
+
+public:
+
+	//some debugging to fix degeneracy problems
+	int			m_lastUsedMethod;
+	int			m_curIter;
+	int			m_degenerateSimplex;
+	int			m_catchDegeneracies;
+
+
+	SpuGjkPairDetector(void* objectA,void* objectB,int m_shapeTypeA, int m_shapeTypeB, float marginA, float marginB, SpuVoronoiSimplexSolver* simplexSolver, const SpuConvexPenetrationDepthSolver*	penetrationDepthSolver);
+	virtual ~SpuGjkPairDetector() {};
+
+	virtual void	getClosestPoints(const SpuClosestPointInput& input,SpuContactResult& output);
+
+	void setMinkowskiA(void* minkA)
+	{
+		m_minkowskiA = minkA;
+	}
+
+	void setMinkowskiB(void* minkB)
+	{
+		m_minkowskiB = minkB;
+	}
+
+	void setCachedSeperatingAxis(const btVector3& seperatingAxis)
+	{
+		m_cachedSeparatingAxis = seperatingAxis;
+	}
+
+	void	setPenetrationDepthSolver(SpuConvexPenetrationDepthSolver*	penetrationDepthSolver)
+	{
+		m_penetrationDepthSolver = penetrationDepthSolver;
+	}
+
+	///don't use setIgnoreMargin, it's for Bullet's internal use
+	void	setIgnoreMargin(bool ignoreMargin)
+	{
+		m_ignoreMargin = ignoreMargin;
+	}
+
+
+};
+
+
+
+#endif //SPU_GJK_PAIR_DETECTOR_H
Index: /code/branches/physics/src/bullet/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuLocalSupport.h
===================================================================
--- /code/branches/physics/src/bullet/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuLocalSupport.h	(revision 1966)
+++ /code/branches/physics/src/bullet/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuLocalSupport.h	(revision 1966)
@@ -0,0 +1,19 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+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.
+*/
+
+
+
+
+
Index: /code/branches/physics/src/bullet/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuMinkowskiPenetrationDepthSolver.cpp
===================================================================
--- /code/branches/physics/src/bullet/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuMinkowskiPenetrationDepthSolver.cpp	(revision 1966)
+++ /code/branches/physics/src/bullet/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuMinkowskiPenetrationDepthSolver.cpp	(revision 1966)
@@ -0,0 +1,347 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+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.
+*/
+
+#include "SpuMinkowskiPenetrationDepthSolver.h"
+#include "SpuVoronoiSimplexSolver.h"
+#include "SpuGjkPairDetector.h"
+#include "SpuContactResult.h"
+#include "SpuPreferredPenetrationDirections.h"
+
+
+#include "SpuCollisionShapes.h"
+
+#define NUM_UNITSPHERE_POINTS 42
+static btVector3	sPenetrationDirections[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2] = 
+{
+btVector3(btScalar(0.000000) , btScalar(-0.000000),btScalar(-1.000000)),
+btVector3(btScalar(0.723608) , btScalar(-0.525725),btScalar(-0.447219)),
+btVector3(btScalar(-0.276388) , btScalar(-0.850649),btScalar(-0.447219)),
+btVector3(btScalar(-0.894426) , btScalar(-0.000000),btScalar(-0.447216)),
+btVector3(btScalar(-0.276388) , btScalar(0.850649),btScalar(-0.447220)),
+btVector3(btScalar(0.723608) , btScalar(0.525725),btScalar(-0.447219)),
+btVector3(btScalar(0.276388) , btScalar(-0.850649),btScalar(0.447220)),
+btVector3(btScalar(-0.723608) , btScalar(-0.525725),btScalar(0.447219)),
+btVector3(btScalar(-0.723608) , btScalar(0.525725),btScalar(0.447219)),
+btVector3(btScalar(0.276388) , btScalar(0.850649),btScalar(0.447219)),
+btVector3(btScalar(0.894426) , btScalar(0.000000),btScalar(0.447216)),
+btVector3(btScalar(-0.000000) , btScalar(0.000000),btScalar(1.000000)),
+btVector3(btScalar(0.425323) , btScalar(-0.309011),btScalar(-0.850654)),
+btVector3(btScalar(-0.162456) , btScalar(-0.499995),btScalar(-0.850654)),
+btVector3(btScalar(0.262869) , btScalar(-0.809012),btScalar(-0.525738)),
+btVector3(btScalar(0.425323) , btScalar(0.309011),btScalar(-0.850654)),
+btVector3(btScalar(0.850648) , btScalar(-0.000000),btScalar(-0.525736)),
+btVector3(btScalar(-0.525730) , btScalar(-0.000000),btScalar(-0.850652)),
+btVector3(btScalar(-0.688190) , btScalar(-0.499997),btScalar(-0.525736)),
+btVector3(btScalar(-0.162456) , btScalar(0.499995),btScalar(-0.850654)),
+btVector3(btScalar(-0.688190) , btScalar(0.499997),btScalar(-0.525736)),
+btVector3(btScalar(0.262869) , btScalar(0.809012),btScalar(-0.525738)),
+btVector3(btScalar(0.951058) , btScalar(0.309013),btScalar(0.000000)),
+btVector3(btScalar(0.951058) , btScalar(-0.309013),btScalar(0.000000)),
+btVector3(btScalar(0.587786) , btScalar(-0.809017),btScalar(0.000000)),
+btVector3(btScalar(0.000000) , btScalar(-1.000000),btScalar(0.000000)),
+btVector3(btScalar(-0.587786) , btScalar(-0.809017),btScalar(0.000000)),
+btVector3(btScalar(-0.951058) , btScalar(-0.309013),btScalar(-0.000000)),
+btVector3(btScalar(-0.951058) , btScalar(0.309013),btScalar(-0.000000)),
+btVector3(btScalar(-0.587786) , btScalar(0.809017),btScalar(-0.000000)),
+btVector3(btScalar(-0.000000) , btScalar(1.000000),btScalar(-0.000000)),
+btVector3(btScalar(0.587786) , btScalar(0.809017),btScalar(-0.000000)),
+btVector3(btScalar(0.688190) , btScalar(-0.499997),btScalar(0.525736)),
+btVector3(btScalar(-0.262869) , btScalar(-0.809012),btScalar(0.525738)),
+btVector3(btScalar(-0.850648) , btScalar(0.000000),btScalar(0.525736)),
+btVector3(btScalar(-0.262869) , btScalar(0.809012),btScalar(0.525738)),
+btVector3(btScalar(0.688190) , btScalar(0.499997),btScalar(0.525736)),
+btVector3(btScalar(0.525730) , btScalar(0.000000),btScalar(0.850652)),
+btVector3(btScalar(0.162456) , btScalar(-0.499995),btScalar(0.850654)),
+btVector3(btScalar(-0.425323) , btScalar(-0.309011),btScalar(0.850654)),
+btVector3(btScalar(-0.425323) , btScalar(0.309011),btScalar(0.850654)),
+btVector3(btScalar(0.162456) , btScalar(0.499995),btScalar(0.850654))
+};
+
+bool SpuMinkowskiPenetrationDepthSolver::calcPenDepth( SpuVoronoiSimplexSolver& simplexSolver,
+	        void* convexA,void* convexB,int shapeTypeA, int shapeTypeB, float marginA, float marginB,
+            btTransform& transA,const btTransform& transB,
+			btVector3& v, btPoint3& pa, btPoint3& pb,
+			class btIDebugDraw* debugDraw,btStackAlloc* stackAlloc,
+			struct SpuConvexPolyhedronVertexData* convexVertexDataA,
+			struct SpuConvexPolyhedronVertexData* convexVertexDataB
+			) const
+{
+
+	(void)stackAlloc;
+	(void)v;
+	
+
+	struct btIntermediateResult : public SpuContactResult
+	{
+
+		btIntermediateResult():m_hasResult(false)
+		{
+		}
+		
+		btVector3 m_normalOnBInWorld;
+		btVector3 m_pointInWorld;
+		btScalar m_depth;
+		bool	m_hasResult;
+
+		virtual void setShapeIdentifiers(int partId0,int index0,	int partId1,int index1)
+		{
+			(void)partId0;
+			(void)index0;
+			(void)partId1;
+			(void)index1;
+		}
+		void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth)
+		{
+			m_normalOnBInWorld = normalOnBInWorld;
+			m_pointInWorld = pointInWorld;
+			m_depth = depth;
+			m_hasResult = true;
+		}
+	};
+
+	//just take fixed number of orientation, and sample the penetration depth in that direction
+	btScalar minProj = btScalar(1e30);
+	btVector3 minNorm;
+	btVector3 minVertex;
+	btVector3 minA,minB;
+	btVector3 seperatingAxisInA,seperatingAxisInB;
+	btVector3 pInA,qInB,pWorld,qWorld,w;
+
+//#define USE_BATCHED_SUPPORT 1
+#ifdef USE_BATCHED_SUPPORT
+
+	btVector3	supportVerticesABatch[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2];
+	btVector3	supportVerticesBBatch[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2];
+	btVector3	seperatingAxisInABatch[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2];
+	btVector3	seperatingAxisInBBatch[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2];
+	int i;
+
+	int numSampleDirections = NUM_UNITSPHERE_POINTS;
+
+	for (i=0;i<numSampleDirections;i++)
+	{
+		const btVector3& norm = sPenetrationDirections[i];
+		seperatingAxisInABatch[i] =  (-norm) * transA.getBasis() ;
+		seperatingAxisInBBatch[i] =  norm   * transB.getBasis() ;
+	}
+
+	{
+		int numPDA = convexA->getNumPreferredPenetrationDirections();
+		if (numPDA)
+		{
+			for (int i=0;i<numPDA;i++)
+			{
+				btVector3 norm;
+				convexA->getPreferredPenetrationDirection(i,norm);
+				norm  = transA.getBasis() * norm;
+				sPenetrationDirections[numSampleDirections] = norm;
+				seperatingAxisInABatch[numSampleDirections] = (-norm) * transA.getBasis();
+				seperatingAxisInBBatch[numSampleDirections] = norm * transB.getBasis();
+				numSampleDirections++;
+			}
+		}
+	}
+
+	{
+		int numPDB = convexB->getNumPreferredPenetrationDirections();
+		if (numPDB)
+		{
+			for (int i=0;i<numPDB;i++)
+			{
+				btVector3 norm;
+				convexB->getPreferredPenetrationDirection(i,norm);
+				norm  = transB.getBasis() * norm;
+				sPenetrationDirections[numSampleDirections] = norm;
+				seperatingAxisInABatch[numSampleDirections] = (-norm) * transA.getBasis();
+				seperatingAxisInBBatch[numSampleDirections] = norm * transB.getBasis();
+				numSampleDirections++;
+			}
+		}
+	}
+
+
+
+	convexA->batchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInABatch,supportVerticesABatch,numSampleDirections);
+	convexB->batchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInBBatch,supportVerticesBBatch,numSampleDirections);
+
+	for (i=0;i<numSampleDirections;i++)
+	{
+		const btVector3& norm = sPenetrationDirections[i];
+		seperatingAxisInA = seperatingAxisInABatch[i];
+		seperatingAxisInB = seperatingAxisInBBatch[i];
+
+		pInA = supportVerticesABatch[i];
+		qInB = supportVerticesBBatch[i];
+
+		pWorld = transA(pInA);	
+		qWorld = transB(qInB);
+		w	= qWorld - pWorld;
+		btScalar delta = norm.dot(w);
+		//find smallest delta
+		if (delta < minProj)
+		{
+			minProj = delta;
+			minNorm = norm;
+			minA = pWorld;
+			minB = qWorld;
+		}
+	}	
+#else
+
+	int numSampleDirections = NUM_UNITSPHERE_POINTS;
+
+///this is necessary, otherwise the normal is not correct, and sphere will rotate forever on a sloped triangle mesh
+#define DO_PREFERRED_DIRECTIONS 1
+#ifdef DO_PREFERRED_DIRECTIONS
+	{
+		int numPDA = spuGetNumPreferredPenetrationDirections(shapeTypeA,convexA);
+		if (numPDA)
+		{
+			for (int i=0;i<numPDA;i++)
+			{
+				btVector3 norm;
+				spuGetPreferredPenetrationDirection(shapeTypeA,convexA,i,norm);
+				norm  = transA.getBasis() * norm;
+				sPenetrationDirections[numSampleDirections] = norm;
+				numSampleDirections++;
+			}
+		}
+	}
+
+	{
+		int numPDB = spuGetNumPreferredPenetrationDirections(shapeTypeB,convexB);
+		if (numPDB)
+		{
+			for (int i=0;i<numPDB;i++)
+			{
+				btVector3 norm;
+				spuGetPreferredPenetrationDirection(shapeTypeB,convexB,i,norm);
+				norm  = transB.getBasis() * norm;
+				sPenetrationDirections[numSampleDirections] = norm;
+				numSampleDirections++;
+			}
+		}
+	}
+#endif //DO_PREFERRED_DIRECTIONS
+
+	for (int i=0;i<numSampleDirections;i++)
+	{
+		const btVector3& norm = sPenetrationDirections[i];
+		seperatingAxisInA = (-norm)* transA.getBasis();
+		seperatingAxisInB = norm* transB.getBasis();
+
+		pInA = localGetSupportingVertexWithoutMargin(shapeTypeA, convexA, seperatingAxisInA,convexVertexDataA);//, NULL);
+		qInB = localGetSupportingVertexWithoutMargin(shapeTypeB, convexB, seperatingAxisInB,convexVertexDataB);//, NULL);
+
+	//	pInA = convexA->localGetSupportingVertexWithoutMargin(seperatingAxisInA);
+	//	qInB = convexB->localGetSupportingVertexWithoutMargin(seperatingAxisInB);
+
+		pWorld = transA(pInA);	
+		qWorld = transB(qInB);
+		w	= qWorld - pWorld;
+		btScalar delta = norm.dot(w);
+		//find smallest delta
+		if (delta < minProj)
+		{
+			minProj = delta;
+			minNorm = norm;
+			minA = pWorld;
+			minB = qWorld;
+		}
+	}
+#endif //USE_BATCHED_SUPPORT
+
+	//add the margins
+
+	minA += minNorm*marginA;
+	minB -= minNorm*marginB;
+	//no penetration
+	if (minProj < btScalar(0.))
+		return false;
+
+	minProj += (marginA + marginB) + btScalar(1.00);
+
+
+
+
+
+//#define DEBUG_DRAW 1
+#ifdef DEBUG_DRAW
+	if (debugDraw)
+	{
+		btVector3 color(0,1,0);
+		debugDraw->drawLine(minA,minB,color);
+		color = btVector3 (1,1,1);
+		btVector3 vec = minB-minA;
+		btScalar prj2 = minNorm.dot(vec);
+		debugDraw->drawLine(minA,minA+(minNorm*minProj),color);
+
+	}
+#endif //DEBUG_DRAW
+
+	
+
+	SpuGjkPairDetector gjkdet(convexA,convexB,shapeTypeA,shapeTypeB,marginA,marginB,&simplexSolver,0);
+
+	btScalar offsetDist = minProj;
+	btVector3 offset = minNorm * offsetDist;
+	
+
+	SpuClosestPointInput input;
+	input.m_convexVertexData[0] = convexVertexDataA;
+	input.m_convexVertexData[1] = convexVertexDataB;
+	btVector3 newOrg = transA.getOrigin() + offset;
+
+	btTransform displacedTrans = transA;
+	displacedTrans.setOrigin(newOrg);
+
+	input.m_transformA = displacedTrans;
+	input.m_transformB = transB;
+	input.m_maximumDistanceSquared = btScalar(1e30);//minProj;
+	
+	btIntermediateResult res;
+	gjkdet.getClosestPoints(input,res);
+
+	btScalar correctedMinNorm = minProj - res.m_depth;
+
+
+	//the penetration depth is over-estimated, relax it
+	btScalar penetration_relaxation= btScalar(1.);
+	minNorm*=penetration_relaxation;
+
+	if (res.m_hasResult)
+	{
+
+		pa = res.m_pointInWorld - minNorm * correctedMinNorm;
+		pb = res.m_pointInWorld;
+		
+#ifdef DEBUG_DRAW
+		if (debugDraw)
+		{
+			btVector3 color(1,0,0);
+			debugDraw->drawLine(pa,pb,color);
+		}
+#endif//DEBUG_DRAW
+
+
+	} else {
+		// could not seperate shapes
+		btAssert (false);
+	}
+	return res.m_hasResult;
+}
+
+
+
Index: /code/branches/physics/src/bullet/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuMinkowskiPenetrationDepthSolver.h
===================================================================
--- /code/branches/physics/src/bullet/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuMinkowskiPenetrationDepthSolver.h	(revision 1966)
+++ /code/branches/physics/src/bullet/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuMinkowskiPenetrationDepthSolver.h	(revision 1966)
@@ -0,0 +1,47 @@
+
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+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.
+*/
+
+#ifndef MINKOWSKI_PENETRATION_DEPTH_SOLVER_H
+#define MINKOWSKI_PENETRATION_DEPTH_SOLVER_H
+
+
+#include "SpuConvexPenetrationDepthSolver.h"
+
+class btStackAlloc;
+class btIDebugDraw;
+class SpuVoronoiSimplexSolver;
+
+///MinkowskiPenetrationDepthSolver implements bruteforce penetration depth estimation.
+///Implementation is based on sampling the depth using support mapping, and using GJK step to get the witness points.
+class SpuMinkowskiPenetrationDepthSolver : public SpuConvexPenetrationDepthSolver
+{
+public:
+
+	virtual bool calcPenDepth( SpuVoronoiSimplexSolver& simplexSolver,
+	        void* convexA,void* convexB,int shapeTypeA, int shapeTypeB, float marginA, float marginB,
+            btTransform& transA,const btTransform& transB,
+			btVector3& v, btPoint3& pa, btPoint3& pb,
+			class btIDebugDraw* debugDraw,btStackAlloc* stackAlloc,
+			struct SpuConvexPolyhedronVertexData* convexVertexDataA,
+			struct SpuConvexPolyhedronVertexData* convexVertexDataB
+			) const;
+
+
+};
+
+
+#endif //MINKOWSKI_PENETRATION_DEPTH_SOLVER_H
+
Index: /code/branches/physics/src/bullet/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuPreferredPenetrationDirections.h
===================================================================
--- /code/branches/physics/src/bullet/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuPreferredPenetrationDirections.h	(revision 1966)
+++ /code/branches/physics/src/bullet/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuPreferredPenetrationDirections.h	(revision 1966)
@@ -0,0 +1,70 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2007 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+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.
+*/
+
+#ifndef _SPU_PREFERRED_PENETRATION_DIRECTIONS_H
+#define _SPU_PREFERRED_PENETRATION_DIRECTIONS_H
+
+
+#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
+
+int		spuGetNumPreferredPenetrationDirections(int shapeType, void* shape)
+{
+	switch (shapeType)
+    {
+		case TRIANGLE_SHAPE_PROXYTYPE:
+		{
+			return 2;
+			//spu_printf("2\n");
+			break;
+		}
+		default:
+			{
+#if __ASSERT
+        spu_printf("spuGetNumPreferredPenetrationDirections() - Unsupported bound type: %d.\n", shapeType);
+#endif // __ASSERT
+			}
+	}
+
+	return 0;	
+}	
+
+void	spuGetPreferredPenetrationDirection(int shapeType, void* shape, int index, btVector3& penetrationVector)
+{
+
+
+	switch (shapeType)
+    {
+		case TRIANGLE_SHAPE_PROXYTYPE:
+		{
+			btVector3* vertices = (btVector3*)shape;
+			///calcNormal
+			penetrationVector = (vertices[1]-vertices[0]).cross(vertices[2]-vertices[0]);
+			penetrationVector.normalize();
+			if (index)
+				penetrationVector *= btScalar(-1.);
+			break;
+		}
+		default:
+			{
+					
+#if __ASSERT
+        spu_printf("spuGetNumPreferredPenetrationDirections() - Unsupported bound type: %d.\n", shapeType);
+#endif // __ASSERT
+			}
+	}
+		
+}
+
+#endif //_SPU_PREFERRED_PENETRATION_DIRECTIONS_H
Index: /code/branches/physics/src/bullet/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuVoronoiSimplexSolver.cpp
===================================================================
--- /code/branches/physics/src/bullet/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuVoronoiSimplexSolver.cpp	(revision 1966)
+++ /code/branches/physics/src/bullet/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuVoronoiSimplexSolver.cpp	(revision 1966)
@@ -0,0 +1,606 @@
+
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+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.
+	
+	Elsevier CDROM license agreements grants nonexclusive license to use the software
+	for any purpose, commercial or non-commercial as long as the following credit is included
+	identifying the original source of the software:
+
+	Parts of the source are "from the book Real-Time Collision Detection by
+	Christer Ericson, published by Morgan Kaufmann Publishers,
+	(c) 2005 Elsevier Inc."
+		
+*/
+
+
+#include "SpuVoronoiSimplexSolver.h"
+#include <assert.h>
+#include <stdio.h>
+
+#define VERTA  0
+#define VERTB  1
+#define VERTC  2
+#define VERTD  3
+
+#define CATCH_DEGENERATE_TETRAHEDRON 1
+void	SpuVoronoiSimplexSolver::removeVertex(int index)
+{
+	
+	assert(m_numVertices>0);
+	m_numVertices--;
+	m_simplexVectorW[index] = m_simplexVectorW[m_numVertices];
+	m_simplexPointsP[index] = m_simplexPointsP[m_numVertices];
+	m_simplexPointsQ[index] = m_simplexPointsQ[m_numVertices];
+}
+
+void	SpuVoronoiSimplexSolver::reduceVertices (const SpuUsageBitfield& usedVerts)
+{
+	if ((numVertices() >= 4) && (!usedVerts.usedVertexD))
+		removeVertex(3);
+
+	if ((numVertices() >= 3) && (!usedVerts.usedVertexC))
+		removeVertex(2);
+
+	if ((numVertices() >= 2) && (!usedVerts.usedVertexB))
+		removeVertex(1);
+	
+	if ((numVertices() >= 1) && (!usedVerts.usedVertexA))
+		removeVertex(0);
+
+}
+
+
+
+
+
+//clear the simplex, remove all the vertices
+void SpuVoronoiSimplexSolver::reset()
+{
+	m_cachedValidClosest = false;
+	m_numVertices = 0;
+	m_needsUpdate = true;
+	m_lastW = btVector3(btScalar(1e30),btScalar(1e30),btScalar(1e30));
+	m_cachedBC.reset();
+}
+
+
+
+	//add a vertex
+void SpuVoronoiSimplexSolver::addVertex(const btVector3& w, const btPoint3& p, const btPoint3& q)
+{
+	m_lastW = w;
+	m_needsUpdate = true;
+
+	m_simplexVectorW[m_numVertices] = w;
+	m_simplexPointsP[m_numVertices] = p;
+	m_simplexPointsQ[m_numVertices] = q;
+
+	m_numVertices++;
+}
+
+bool	SpuVoronoiSimplexSolver::updateClosestVectorAndPoints()
+{
+	
+	if (m_needsUpdate)
+	{
+		m_cachedBC.reset();
+
+		m_needsUpdate = false;
+
+		switch (numVertices())
+		{
+		case 0:
+				m_cachedValidClosest = false;
+				break;
+		case 1:
+			{
+				m_cachedP1 = m_simplexPointsP[0];
+				m_cachedP2 = m_simplexPointsQ[0];
+				m_cachedV = m_cachedP1-m_cachedP2; //== m_simplexVectorW[0]
+				m_cachedBC.reset();
+				m_cachedBC.setBarycentricCoordinates(btScalar(1.),btScalar(0.),btScalar(0.),btScalar(0.));
+				m_cachedValidClosest = m_cachedBC.isValid();
+				break;
+			};
+		case 2:
+			{
+			//closest point origin from line segment
+					const btVector3& from = m_simplexVectorW[0];
+					const btVector3& to = m_simplexVectorW[1];
+					btVector3 nearest;
+
+					btVector3 p (btScalar(0.),btScalar(0.),btScalar(0.));
+					btVector3 diff = p - from;
+					btVector3 v = to - from;
+					btScalar t = v.dot(diff);
+					
+					if (t > 0) {
+						btScalar dotVV = v.dot(v);
+						if (t < dotVV) {
+							t /= dotVV;
+							diff -= t*v;
+							m_cachedBC.m_usedVertices.usedVertexA = true;
+							m_cachedBC.m_usedVertices.usedVertexB = true;
+						} else {
+							t = 1;
+							diff -= v;
+							//reduce to 1 point
+							m_cachedBC.m_usedVertices.usedVertexB = true;
+						}
+					} else
+					{
+						t = 0;
+						//reduce to 1 point
+						m_cachedBC.m_usedVertices.usedVertexA = true;
+					}
+					m_cachedBC.setBarycentricCoordinates(1-t,t);
+					nearest = from + t*v;
+
+					m_cachedP1 = m_simplexPointsP[0] + t * (m_simplexPointsP[1] - m_simplexPointsP[0]);
+					m_cachedP2 = m_simplexPointsQ[0] + t * (m_simplexPointsQ[1] - m_simplexPointsQ[0]);
+					m_cachedV = m_cachedP1 - m_cachedP2;
+					
+					reduceVertices(m_cachedBC.m_usedVertices);
+
+					m_cachedValidClosest = m_cachedBC.isValid();
+					break;
+			}
+		case 3:
+			{
+				//closest point origin from triangle
+				btVector3 p (btScalar(0.),btScalar(0.),btScalar(0.));
+				
+				const btVector3& a = m_simplexVectorW[0];
+				const btVector3& b = m_simplexVectorW[1];
+				const btVector3& c = m_simplexVectorW[2];
+
+				closestPtPointTriangle(p,a,b,c,m_cachedBC);
+				m_cachedP1 = m_simplexPointsP[0] * m_cachedBC.m_barycentricCoords[0] +
+								m_simplexPointsP[1] * m_cachedBC.m_barycentricCoords[1] +
+								m_simplexPointsP[2] * m_cachedBC.m_barycentricCoords[2] +
+								m_simplexPointsP[3] * m_cachedBC.m_barycentricCoords[3];
+
+				m_cachedP2 = m_simplexPointsQ[0] * m_cachedBC.m_barycentricCoords[0] +
+					m_simplexPointsQ[1] * m_cachedBC.m_barycentricCoords[1] +
+					m_simplexPointsQ[2] * m_cachedBC.m_barycentricCoords[2] +
+					m_simplexPointsQ[3] * m_cachedBC.m_barycentricCoords[3];
+
+				m_cachedV = m_cachedP1-m_cachedP2;
+
+				reduceVertices (m_cachedBC.m_usedVertices);
+				m_cachedValidClosest =  m_cachedBC.isValid();
+
+				break;
+			}
+		case 4:
+			{
+
+				
+				btVector3 p (btScalar(0.),btScalar(0.),btScalar(0.));
+				
+				const btVector3& a = m_simplexVectorW[0];
+				const btVector3& b = m_simplexVectorW[1];
+				const btVector3& c = m_simplexVectorW[2];
+				const btVector3& d = m_simplexVectorW[3];
+
+				bool hasSeperation = closestPtPointTetrahedron(p,a,b,c,d,m_cachedBC);
+
+				if (hasSeperation)
+				{
+
+					m_cachedP1 = m_simplexPointsP[0] * m_cachedBC.m_barycentricCoords[0] +
+						m_simplexPointsP[1] * m_cachedBC.m_barycentricCoords[1] +
+						m_simplexPointsP[2] * m_cachedBC.m_barycentricCoords[2] +
+						m_simplexPointsP[3] * m_cachedBC.m_barycentricCoords[3];
+
+					m_cachedP2 = m_simplexPointsQ[0] * m_cachedBC.m_barycentricCoords[0] +
+						m_simplexPointsQ[1] * m_cachedBC.m_barycentricCoords[1] +
+						m_simplexPointsQ[2] * m_cachedBC.m_barycentricCoords[2] +
+						m_simplexPointsQ[3] * m_cachedBC.m_barycentricCoords[3];
+
+					m_cachedV = m_cachedP1-m_cachedP2;
+					reduceVertices (m_cachedBC.m_usedVertices);
+				} else
+				{
+//					printf("sub distance got penetration\n");
+
+					if (m_cachedBC.m_degenerate)
+					{
+						m_cachedValidClosest = false;
+					} else
+					{
+						m_cachedValidClosest = true;
+						//degenerate case == false, penetration = true + zero
+						m_cachedV.setValue(btScalar(0.),btScalar(0.),btScalar(0.));
+					}
+					break;
+				}
+
+				m_cachedValidClosest = m_cachedBC.isValid();
+
+				//closest point origin from tetrahedron
+				break;
+			}
+		default:
+			{
+				m_cachedValidClosest = false;
+			}
+		};
+	}
+
+	return m_cachedValidClosest;
+
+}
+
+//return/calculate the closest vertex
+bool SpuVoronoiSimplexSolver::closest(btVector3& v)
+{
+	bool succes = updateClosestVectorAndPoints();
+	v = m_cachedV;
+	return succes;
+}
+
+
+
+btScalar SpuVoronoiSimplexSolver::maxVertex()
+{
+	int i, numverts = numVertices();
+	btScalar maxV = btScalar(0.);
+	for (i=0;i<numverts;i++)
+	{
+		btScalar curLen2 = m_simplexVectorW[i].length2();
+		if (maxV < curLen2)
+			maxV = curLen2;
+	}
+	return maxV;
+}
+
+
+
+	//return the current simplex
+int SpuVoronoiSimplexSolver::getSimplex(btPoint3 *pBuf, btPoint3 *qBuf, btVector3 *yBuf) const
+{
+	int i;
+	for (i=0;i<numVertices();i++)
+	{
+		yBuf[i] = m_simplexVectorW[i];
+		pBuf[i] = m_simplexPointsP[i];
+		qBuf[i] = m_simplexPointsQ[i];
+	}
+	return numVertices();
+}
+
+
+
+
+bool SpuVoronoiSimplexSolver::inSimplex(const btVector3& w)
+{
+	bool found = false;
+	int i, numverts = numVertices();
+	//btScalar maxV = btScalar(0.);
+	
+	//w is in the current (reduced) simplex
+	for (i=0;i<numverts;i++)
+	{
+		if (m_simplexVectorW[i] == w)
+			found = true;
+	}
+
+	//check in case lastW is already removed
+	if (w == m_lastW)
+		return true;
+    	
+	return found;
+}
+
+void SpuVoronoiSimplexSolver::backup_closest(btVector3& v) 
+{
+	v = m_cachedV;
+}
+
+
+bool SpuVoronoiSimplexSolver::emptySimplex() const 
+{
+	return (numVertices() == 0);
+
+}
+
+void SpuVoronoiSimplexSolver::compute_points(btPoint3& p1, btPoint3& p2) 
+{
+	updateClosestVectorAndPoints();
+	p1 = m_cachedP1;
+	p2 = m_cachedP2;
+
+}
+
+
+
+
+bool	SpuVoronoiSimplexSolver::closestPtPointTriangle(const btPoint3& p, const btPoint3& a, const btPoint3& b, const btPoint3& c,SpuSubSimplexClosestResult& result)
+{
+	result.m_usedVertices.reset();
+
+    // Check if P in vertex region outside A
+    btVector3 ab = b - a;
+    btVector3 ac = c - a;
+    btVector3 ap = p - a;
+    btScalar d1 = ab.dot(ap);
+    btScalar d2 = ac.dot(ap);
+    if (d1 <= btScalar(0.0) && d2 <= btScalar(0.0)) 
+	{
+		result.m_closestPointOnSimplex = a;
+		result.m_usedVertices.usedVertexA = true;
+		result.setBarycentricCoordinates(1,0,0);
+		return true;// a; // barycentric coordinates (1,0,0)
+	}
+
+    // Check if P in vertex region outside B
+    btVector3 bp = p - b;
+    btScalar d3 = ab.dot(bp);
+    btScalar d4 = ac.dot(bp);
+    if (d3 >= btScalar(0.0) && d4 <= d3) 
+	{
+		result.m_closestPointOnSimplex = b;
+		result.m_usedVertices.usedVertexB = true;
+		result.setBarycentricCoordinates(0,1,0);
+
+		return true; // b; // barycentric coordinates (0,1,0)
+	}
+    // Check if P in edge region of AB, if so return projection of P onto AB
+    btScalar vc = d1*d4 - d3*d2;
+    if (vc <= btScalar(0.0) && d1 >= btScalar(0.0) && d3 <= btScalar(0.0)) {
+        btScalar v = d1 / (d1 - d3);
+		result.m_closestPointOnSimplex = a + v * ab;
+		result.m_usedVertices.usedVertexA = true;
+		result.m_usedVertices.usedVertexB = true;
+		result.setBarycentricCoordinates(1-v,v,0);
+		return true;
+        //return a + v * ab; // barycentric coordinates (1-v,v,0)
+    }
+
+    // Check if P in vertex region outside C
+    btVector3 cp = p - c;
+    btScalar d5 = ab.dot(cp);
+    btScalar d6 = ac.dot(cp);
+    if (d6 >= btScalar(0.0) && d5 <= d6) 
+	{
+		result.m_closestPointOnSimplex = c;
+		result.m_usedVertices.usedVertexC = true;
+		result.setBarycentricCoordinates(0,0,1);
+		return true;//c; // barycentric coordinates (0,0,1)
+	}
+
+    // Check if P in edge region of AC, if so return projection of P onto AC
+    btScalar vb = d5*d2 - d1*d6;
+    if (vb <= btScalar(0.0) && d2 >= btScalar(0.0) && d6 <= btScalar(0.0)) {
+        btScalar w = d2 / (d2 - d6);
+		result.m_closestPointOnSimplex = a + w * ac;
+		result.m_usedVertices.usedVertexA = true;
+		result.m_usedVertices.usedVertexC = true;
+		result.setBarycentricCoordinates(1-w,0,w);
+		return true;
+        //return a + w * ac; // barycentric coordinates (1-w,0,w)
+    }
+
+    // Check if P in edge region of BC, if so return projection of P onto BC
+    btScalar va = d3*d6 - d5*d4;
+    if (va <= btScalar(0.0) && (d4 - d3) >= btScalar(0.0) && (d5 - d6) >= btScalar(0.0)) {
+        btScalar w = (d4 - d3) / ((d4 - d3) + (d5 - d6));
+		
+		result.m_closestPointOnSimplex = b + w * (c - b);
+		result.m_usedVertices.usedVertexB = true;
+		result.m_usedVertices.usedVertexC = true;
+		result.setBarycentricCoordinates(0,1-w,w);
+		return true;		
+       // return b + w * (c - b); // barycentric coordinates (0,1-w,w)
+    }
+
+    // P inside face region. Compute Q through its barycentric coordinates (u,v,w)
+    btScalar denom = btScalar(1.0) / (va + vb + vc);
+    btScalar v = vb * denom;
+    btScalar w = vc * denom;
+    
+	result.m_closestPointOnSimplex = a + ab * v + ac * w;
+	result.m_usedVertices.usedVertexA = true;
+	result.m_usedVertices.usedVertexB = true;
+	result.m_usedVertices.usedVertexC = true;
+	result.setBarycentricCoordinates(1-v-w,v,w);
+	
+	return true;
+//	return a + ab * v + ac * w; // = u*a + v*b + w*c, u = va * denom = btScalar(1.0) - v - w
+
+}
+
+
+
+
+
+/// Test if point p and d lie on opposite sides of plane through abc
+int SpuVoronoiSimplexSolver::pointOutsideOfPlane(const btPoint3& p, const btPoint3& a, const btPoint3& b, const btPoint3& c, const btPoint3& d)
+{
+	btVector3 normal = (b-a).cross(c-a);
+
+    btScalar signp = (p - a).dot(normal); // [AP AB AC]
+    btScalar signd = (d - a).dot( normal); // [AD AB AC]
+
+#ifdef CATCH_DEGENERATE_TETRAHEDRON
+#ifdef BT_USE_DOUBLE_PRECISION
+if (signd * signd < (btScalar(1e-8) * btScalar(1e-8)))
+	{
+		return -1;
+	}
+#else
+	if (signd * signd < (btScalar(1e-4) * btScalar(1e-4)))
+	{
+//		printf("affine dependent/degenerate\n");//
+		return -1;
+	}
+#endif
+
+#endif
+	// Points on opposite sides if expression signs are opposite
+    return signp * signd < btScalar(0.);
+}
+
+
+bool	SpuVoronoiSimplexSolver::closestPtPointTetrahedron(const btPoint3& p, const btPoint3& a, const btPoint3& b, const btPoint3& c, const btPoint3& d, SpuSubSimplexClosestResult& finalResult)
+{
+	SpuSubSimplexClosestResult tempResult;
+
+    // Start out assuming point inside all halfspaces, so closest to itself
+	finalResult.m_closestPointOnSimplex = p;
+	finalResult.m_usedVertices.reset();
+    finalResult.m_usedVertices.usedVertexA = true;
+	finalResult.m_usedVertices.usedVertexB = true;
+	finalResult.m_usedVertices.usedVertexC = true;
+	finalResult.m_usedVertices.usedVertexD = true;
+
+    int pointOutsideABC = pointOutsideOfPlane(p, a, b, c, d);
+	int pointOutsideACD = pointOutsideOfPlane(p, a, c, d, b);
+  	int	pointOutsideADB = pointOutsideOfPlane(p, a, d, b, c);
+	int	pointOutsideBDC = pointOutsideOfPlane(p, b, d, c, a);
+
+   if (pointOutsideABC < 0 || pointOutsideACD < 0 || pointOutsideADB < 0 || pointOutsideBDC < 0)
+   {
+	   finalResult.m_degenerate = true;
+	   return false;
+   }
+
+   if (!pointOutsideABC  && !pointOutsideACD && !pointOutsideADB && !pointOutsideBDC)
+	 {
+		 return false;
+	 }
+
+
+    btScalar bestSqDist = FLT_MAX;
+    // If point outside face abc then compute closest point on abc
+	if (pointOutsideABC) 
+	{
+        closestPtPointTriangle(p, a, b, c,tempResult);
+		btPoint3 q = tempResult.m_closestPointOnSimplex;
+		
+        btScalar sqDist = (q - p).dot( q - p);
+        // Update best closest point if (squared) distance is less than current best
+        if (sqDist < bestSqDist) {
+			bestSqDist = sqDist;
+			finalResult.m_closestPointOnSimplex = q;
+			//convert result bitmask!
+			finalResult.m_usedVertices.reset();
+			finalResult.m_usedVertices.usedVertexA = tempResult.m_usedVertices.usedVertexA;
+			finalResult.m_usedVertices.usedVertexB = tempResult.m_usedVertices.usedVertexB;
+			finalResult.m_usedVertices.usedVertexC = tempResult.m_usedVertices.usedVertexC;
+			finalResult.setBarycentricCoordinates(
+					tempResult.m_barycentricCoords[VERTA],
+					tempResult.m_barycentricCoords[VERTB],
+					tempResult.m_barycentricCoords[VERTC],
+					0
+			);
+
+		}
+    }
+  
+
+	// Repeat test for face acd
+	if (pointOutsideACD) 
+	{
+        closestPtPointTriangle(p, a, c, d,tempResult);
+		btPoint3 q = tempResult.m_closestPointOnSimplex;
+		//convert result bitmask!
+
+        btScalar sqDist = (q - p).dot( q - p);
+        if (sqDist < bestSqDist) 
+		{
+			bestSqDist = sqDist;
+			finalResult.m_closestPointOnSimplex = q;
+			finalResult.m_usedVertices.reset();
+			finalResult.m_usedVertices.usedVertexA = tempResult.m_usedVertices.usedVertexA;
+			finalResult.m_usedVertices.usedVertexC = tempResult.m_usedVertices.usedVertexB;
+			finalResult.m_usedVertices.usedVertexD = tempResult.m_usedVertices.usedVertexC;
+			finalResult.setBarycentricCoordinates(
+					tempResult.m_barycentricCoords[VERTA],
+					0,
+					tempResult.m_barycentricCoords[VERTB],
+					tempResult.m_barycentricCoords[VERTC]
+			);
+
+		}
+    }
+    // Repeat test for face adb
+
+	
+	if (pointOutsideADB)
+	{
+		closestPtPointTriangle(p, a, d, b,tempResult);
+		btPoint3 q = tempResult.m_closestPointOnSimplex;
+		//convert result bitmask!
+
+        btScalar sqDist = (q - p).dot( q - p);
+        if (sqDist < bestSqDist) 
+		{
+			bestSqDist = sqDist;
+			finalResult.m_closestPointOnSimplex = q;
+			finalResult.m_usedVertices.reset();
+			finalResult.m_usedVertices.usedVertexA = tempResult.m_usedVertices.usedVertexA;
+			finalResult.m_usedVertices.usedVertexD = tempResult.m_usedVertices.usedVertexB;
+			finalResult.m_usedVertices.usedVertexB = tempResult.m_usedVertices.usedVertexC;
+			finalResult.setBarycentricCoordinates(
+					tempResult.m_barycentricCoords[VERTA],
+					tempResult.m_barycentricCoords[VERTC],
+					0,
+					tempResult.m_barycentricCoords[VERTB]
+			);
+
+		}
+    }
+    // Repeat test for face bdc
+    
+
+	if (pointOutsideBDC)
+	{
+        closestPtPointTriangle(p, b, d, c,tempResult);
+		btPoint3 q = tempResult.m_closestPointOnSimplex;
+		//convert result bitmask!
+        btScalar sqDist = (q - p).dot( q - p);
+        if (sqDist < bestSqDist) 
+		{
+			bestSqDist = sqDist;
+			finalResult.m_closestPointOnSimplex = q;
+			finalResult.m_usedVertices.reset();
+			finalResult.m_usedVertices.usedVertexB = tempResult.m_usedVertices.usedVertexA;
+			finalResult.m_usedVertices.usedVertexD = tempResult.m_usedVertices.usedVertexB;
+			finalResult.m_usedVertices.usedVertexC = tempResult.m_usedVertices.usedVertexC;
+
+			finalResult.setBarycentricCoordinates(
+					0,
+					tempResult.m_barycentricCoords[VERTA],
+					tempResult.m_barycentricCoords[VERTC],
+					tempResult.m_barycentricCoords[VERTB]
+			);
+
+		}
+    }
+
+	//help! we ended up full !
+	
+	if (finalResult.m_usedVertices.usedVertexA &&
+		finalResult.m_usedVertices.usedVertexB &&
+		finalResult.m_usedVertices.usedVertexC &&
+		finalResult.m_usedVertices.usedVertexD) 
+	{
+		return true;
+	}
+
+    return true;
+}
+
Index: /code/branches/physics/src/bullet/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuVoronoiSimplexSolver.h
===================================================================
--- /code/branches/physics/src/bullet/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuVoronoiSimplexSolver.h	(revision 1966)
+++ /code/branches/physics/src/bullet/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuVoronoiSimplexSolver.h	(revision 1966)
@@ -0,0 +1,156 @@
+
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+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.
+*/
+
+
+
+#ifndef SPUVoronoiSimplexSolver_H
+#define SPUVoronoiSimplexSolver_H
+
+#include <LinearMath/btTransform.h>
+#include <LinearMath/btPoint3.h>
+
+#define VORONOI_SIMPLEX_MAX_VERTS 5
+
+struct SpuUsageBitfield{
+	SpuUsageBitfield()
+	{
+		reset();
+	}
+
+	void reset()
+	{
+		usedVertexA = false;
+		usedVertexB = false;
+		usedVertexC = false;
+		usedVertexD = false;
+	}
+	unsigned short usedVertexA	: 1;
+	unsigned short usedVertexB	: 1;
+	unsigned short usedVertexC	: 1;
+	unsigned short usedVertexD	: 1;
+	unsigned short unused1		: 1;
+	unsigned short unused2		: 1;
+	unsigned short unused3		: 1;
+	unsigned short unused4		: 1;
+};
+
+
+struct	SpuSubSimplexClosestResult
+{
+	btVector3	m_closestPointOnSimplex;
+	//MASK for m_usedVertices
+	//stores the simplex vertex-usage, using the MASK, 
+	// if m_usedVertices & MASK then the related vertex is used
+	SpuUsageBitfield	m_usedVertices;
+	float	m_barycentricCoords[4];
+	bool m_degenerate;
+
+	void	reset()
+	{
+		m_degenerate = false;
+		setBarycentricCoordinates();
+		m_usedVertices.reset();
+	}
+	bool	isValid()
+	{
+		bool valid = (m_barycentricCoords[0] >= float(0.)) &&
+			(m_barycentricCoords[1] >= float(0.)) &&
+			(m_barycentricCoords[2] >= float(0.)) &&
+			(m_barycentricCoords[3] >= float(0.));
+
+
+		return valid;
+	}
+	void	setBarycentricCoordinates(float a=float(0.),float b=float(0.),float c=float(0.),float d=float(0.))
+	{
+		m_barycentricCoords[0] = a;
+		m_barycentricCoords[1] = b;
+		m_barycentricCoords[2] = c;
+		m_barycentricCoords[3] = d;
+	}
+
+};
+
+/// SpuVoronoiSimplexSolver is an implementation of the closest point distance algorithm from a 1-4 points simplex to the origin.
+/// Can be used with GJK, as an alternative to Johnson distance algorithm.
+class SpuVoronoiSimplexSolver
+{
+public:
+
+	int	m_numVertices;
+
+	btVector3	m_simplexVectorW[VORONOI_SIMPLEX_MAX_VERTS];
+	btVector3	m_simplexPointsP[VORONOI_SIMPLEX_MAX_VERTS];
+	btVector3	m_simplexPointsQ[VORONOI_SIMPLEX_MAX_VERTS];
+
+	int m_VertexIndexA[VORONOI_SIMPLEX_MAX_VERTS];
+	int m_VertexIndexB[VORONOI_SIMPLEX_MAX_VERTS];
+
+	btVector3	m_cachedP1;
+	btVector3	m_cachedP2;
+	btVector3	m_cachedV;
+	btVector3	m_lastW;
+	bool		m_cachedValidClosest;
+
+	SpuSubSimplexClosestResult m_cachedBC;
+
+	bool	m_needsUpdate;
+
+	void	removeVertex(int index);
+	void	reduceVertices (const SpuUsageBitfield& usedVerts);
+	bool	updateClosestVectorAndPoints();
+
+	bool	closestPtPointTetrahedron(const btVector3& p, const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& d, SpuSubSimplexClosestResult& finalResult);
+	int		pointOutsideOfPlane(const btVector3& p, const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& d);
+	bool	closestPtPointTriangle(const btVector3& p, const btVector3& a, const btVector3& b, const btVector3& c,SpuSubSimplexClosestResult& result);
+
+	int RemoveDegenerateIndices (const int *inArray, int numIndices, int *outArray) const;
+
+public:
+
+	void reset();
+
+	void addVertex(const btVector3& w, const btPoint3& p, const btPoint3& q);
+
+
+	bool closest(btVector3& v);
+
+	btScalar maxVertex();
+
+	bool fullSimplex() const
+	{
+		return (m_numVertices == 4);
+	}
+
+	int getSimplex(btVector3 *pBuf, btVector3 *qBuf, btVector3 *yBuf) const;
+
+	bool inSimplex(const btVector3& w);
+
+	void backup_closest(btVector3& v) ;
+
+	bool emptySimplex() const ;
+
+	void compute_points(btVector3& p1, btVector3& p2) ;
+
+	int numVertices() const 
+	{
+		return m_numVertices;
+	}
+};
+
+
+
+#endif //SpuVoronoiSimplexSolver
Index: /code/branches/physics/src/bullet/BulletMultiThreaded/SpuParallelSolver.cpp
===================================================================
--- /code/branches/physics/src/bullet/BulletMultiThreaded/SpuParallelSolver.cpp	(revision 1966)
+++ /code/branches/physics/src/bullet/BulletMultiThreaded/SpuParallelSolver.cpp	(revision 1966)
@@ -0,0 +1,604 @@
+/*
+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 "LinearMath/btQuickprof.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);
+	int i;
+
+	// Setup basic dependency
+	for ( i = 0; i < SPU_HASH_NUMCELLS; ++i)
+	{
+		hash->m_dependencyMatrix[i][i >> 5] |= (1 << (i & 31));
+	}
+
+	// Set some ones to "unused cells"
+	for ( 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,btDispatcher* dispatcher)
+{
+	BT_PROFILE("parallel_solveGroup");
+
+	if (!numManifolds && !numConstraints)
+		return 0;
+	int i;
+
+///refresh contact points is not needed anymore, it has been moved into the processCollision detection part.
+#ifdef FORCE_REFESH_CONTACT_MANIFOLDS
+	for ( 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());
+	}
+#endif //FORCE_REFESH_CONTACT_MANIFOLDS
+
+	// Record and mark the manifolds to the cells
+	for ( 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 ( 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 ( 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)
+{
+	BT_PROFILE("parallel_allSolved");
+
+	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.quickSort(CellHolderPredicate<ManifoldCellHolder>());
+
+	// Sort the constraint list
+	int numConstraints = m_sortedConstraints.size();
+	m_sortedConstraints.quickSort(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
+	{
+		BT_PROFILE("setup moving rigidbodies");
+
+		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();
+	}
+
+	{
+		BT_PROFILE("parallel_solve_iterations");
+
+		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 (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;
+
+			for (int i=0;i<m_maxNumOutstandingTasks;i++)
+			  {
+				  if (m_taskBusy[i])
+				  {
+					  taskId = i;
+					  break;
+				  }
+			  }
+
+			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;
+		for (int i=0;i<m_maxNumOutstandingTasks;i++)
+	  {
+		  if (m_taskBusy[i])
+		  {
+			  taskId = i;
+			  break;
+		  }
+	  }
+
+		m_threadInterface->waitForResponse(&taskId, &outputSize);
+
+		m_taskBusy[taskId] = false;
+		m_numBusyTasks--;
+	}
+}
Index: /code/branches/physics/src/bullet/BulletMultiThreaded/SpuParallelSolver.h
===================================================================
--- /code/branches/physics/src/bullet/BulletMultiThreaded/SpuParallelSolver.h	(revision 1966)
+++ /code/branches/physics/src/bullet/BulletMultiThreaded/SpuParallelSolver.h	(revision 1966)
@@ -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;
+	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();
+
+	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,btDispatcher* dispatcher);
+	virtual void allSolved (const btContactSolverInfo& info,class btIDebugDraw* debugDrawer, btStackAlloc* stackAlloc);
+	virtual void reset ();
+};
+
+#endif
Index: /code/branches/physics/src/bullet/BulletMultiThreaded/SpuRaycastTask/SpuRaycastTask.cpp
===================================================================
--- /code/branches/physics/src/bullet/BulletMultiThreaded/SpuRaycastTask/SpuRaycastTask.cpp	(revision 1966)
+++ /code/branches/physics/src/bullet/BulletMultiThreaded/SpuRaycastTask/SpuRaycastTask.cpp	(revision 1966)
@@ -0,0 +1,786 @@
+
+
+#include "../PlatformDefinitions.h"
+#include "SpuRaycastTask.h"
+#include "../SpuCollisionObjectWrapper.h"
+#include "../SpuNarrowPhaseCollisionTask/SpuCollisionShapes.h"
+#include "SpuSubSimplexConvexCast.h"
+#include "LinearMath/btAabbUtil2.h"
+
+
+/* Future optimization strategies: 
+1. BBOX prune before loading shape data
+2. Could reduce number of dmas for ray output data to a single read and write.
+   By sharing the temporary work unit output structures across objects.
+3. The reason SpuRaycastNodeCallback1 is slower is because the triangle data isn't
+   being cached across calls. Fix that by doing the final ray pruning inside the callback.
+*/
+
+/* Future work:
+1. support first hit, closest hit, etc rather than just closest hit.
+2. support compound objects
+*/
+
+#define CALLBACK_ALL
+
+struct RaycastTask_LocalStoreMemory
+{
+	ATTRIBUTE_ALIGNED16(char gColObj [sizeof(btCollisionObject)+16]);
+	btCollisionObject* getColObj()
+	{
+		return (btCollisionObject*) gColObj;
+	}
+
+	ATTRIBUTE_ALIGNED16(SpuCollisionObjectWrapper gCollisionObjectWrapper);
+	SpuCollisionObjectWrapper* getCollisionObjectWrapper ()
+	{
+		return &gCollisionObjectWrapper;
+	}
+
+	CollisionShape_LocalStoreMemory gCollisionShape;
+	ATTRIBUTE_ALIGNED16(int	spuIndices[16]);
+
+	bvhMeshShape_LocalStoreMemory bvhShapeData;
+	SpuConvexPolyhedronVertexData convexVertexData;
+	CompoundShape_LocalStoreMemory compoundShapeData;
+};
+
+#ifdef WIN32
+void* createRaycastLocalStoreMemory()
+{
+	return new RaycastTask_LocalStoreMemory;
+};
+#elif defined(__CELLOS_LV2__)
+ATTRIBUTE_ALIGNED16(RaycastTask_LocalStoreMemory gLocalStoreMemory);
+void* createRaycastLocalStoreMemory()
+{
+	return &gLocalStoreMemory;
+}
+#endif
+
+void GatherCollisionObjectAndShapeData (RaycastGatheredObjectData* gatheredObjectData, RaycastTask_LocalStoreMemory* lsMemPtr, ppu_address_t objectWrapper)
+{
+	register int dmaSize;
+	register ppu_address_t	dmaPpuAddress2;
+	/* DMA Collision object wrapper into local store */
+	dmaSize = sizeof(SpuCollisionObjectWrapper);
+	dmaPpuAddress2 = objectWrapper;
+	cellDmaGet(&lsMemPtr->gCollisionObjectWrapper, dmaPpuAddress2, dmaSize, DMA_TAG(1), 0, 0);
+	cellDmaWaitTagStatusAll(DMA_MASK(1));
+
+	/* DMA Collision object into local store */
+	dmaSize = sizeof(btCollisionObject);
+	dmaPpuAddress2 = lsMemPtr->getCollisionObjectWrapper()->getCollisionObjectPtr();
+	cellDmaGet(&lsMemPtr->gColObj, dmaPpuAddress2  , dmaSize, DMA_TAG(2), 0, 0);
+	cellDmaWaitTagStatusAll(DMA_MASK(2));
+	
+	/* Gather information about collision object and shape */
+	gatheredObjectData->m_worldTransform = lsMemPtr->getColObj()->getWorldTransform();
+	gatheredObjectData->m_collisionMargin = lsMemPtr->getCollisionObjectWrapper()->getCollisionMargin ();
+	gatheredObjectData->m_shapeType = lsMemPtr->getCollisionObjectWrapper()->getShapeType ();
+	gatheredObjectData->m_collisionShape = (ppu_address_t)lsMemPtr->getColObj()->getCollisionShape();
+	gatheredObjectData->m_spuCollisionShape = (void*)&lsMemPtr->gCollisionShape.collisionShape;
+
+	/* DMA shape data */
+	dmaCollisionShape (gatheredObjectData->m_spuCollisionShape, gatheredObjectData->m_collisionShape, 1, gatheredObjectData->m_shapeType);
+	cellDmaWaitTagStatusAll(DMA_MASK(1));
+	if (btBroadphaseProxy::isConvex (gatheredObjectData->m_shapeType))
+	{
+		btConvexInternalShape* spuConvexShape = (btConvexInternalShape*)gatheredObjectData->m_spuCollisionShape;
+		gatheredObjectData->m_primitiveDimensions = spuConvexShape->getImplicitShapeDimensions ();
+	} else {
+		gatheredObjectData->m_primitiveDimensions = btVector3(1.0, 1.0, 1.0);
+	}
+
+}
+
+void dmaLoadRayOutput (ppu_address_t rayOutputAddr, SpuRaycastTaskWorkUnitOut* rayOutput, uint32_t dmaTag)
+{
+	cellDmaGet(rayOutput, rayOutputAddr, sizeof(*rayOutput), DMA_TAG(dmaTag), 0, 0);
+}
+
+void dmaStoreRayOutput (ppu_address_t rayOutputAddr, const SpuRaycastTaskWorkUnitOut* rayOutput, uint32_t dmaTag)
+{
+	cellDmaLargePut (rayOutput, rayOutputAddr, sizeof(*rayOutput), DMA_TAG(dmaTag), 0, 0);
+}
+
+#if 0
+SIMD_FORCE_INLINE void small_cache_read(void* buffer, ppu_address_t ea, size_t size)
+{
+#if USE_SOFTWARE_CACHE
+	// Check for alignment requirements. We need to make sure the entire request fits within one cache line,
+	// so the first and last bytes should fall on the same cache line
+	btAssert((ea & ~SPE_CACHELINE_MASK) == ((ea + size - 1) & ~SPE_CACHELINE_MASK));
+
+	void* ls = spe_cache_read(ea);
+	memcpy(buffer, ls, size);
+#else
+	stallingUnalignedDmaSmallGet(buffer,ea,size);
+#endif
+}
+#endif
+
+void small_cache_read_triple(	void* ls0, ppu_address_t ea0,
+												void* ls1, ppu_address_t ea1,
+												void* ls2, ppu_address_t ea2,
+												size_t size)
+{
+		btAssert(size<16);
+		ATTRIBUTE_ALIGNED16(char	tmpBuffer0[32]);
+		ATTRIBUTE_ALIGNED16(char	tmpBuffer1[32]);
+		ATTRIBUTE_ALIGNED16(char	tmpBuffer2[32]);
+
+		uint32_t i;
+		
+
+		///make sure last 4 bits are the same, for cellDmaSmallGet
+		char* localStore0 = (char*)ls0;
+		uint32_t last4BitsOffset = ea0 & 0x0f;
+		char* tmpTarget0 = tmpBuffer0 + last4BitsOffset;
+		tmpTarget0 = (char*)cellDmaSmallGetReadOnly(tmpTarget0,ea0,size,DMA_TAG(1),0,0);
+
+
+		char* localStore1 = (char*)ls1;
+		last4BitsOffset = ea1 & 0x0f;
+		char* tmpTarget1 = tmpBuffer1 + last4BitsOffset;
+		tmpTarget1 = (char*)cellDmaSmallGetReadOnly(tmpTarget1,ea1,size,DMA_TAG(1),0,0);
+		
+		char* localStore2 = (char*)ls2;
+		last4BitsOffset = ea2 & 0x0f;
+		char* tmpTarget2 = tmpBuffer2 + last4BitsOffset;
+		tmpTarget2 = (char*)cellDmaSmallGetReadOnly(tmpTarget2,ea2,size,DMA_TAG(1),0,0);
+		
+		
+		cellDmaWaitTagStatusAll( DMA_MASK(1) );
+
+		//this is slowish, perhaps memcpy on SPU is smarter?
+		for (i=0; btLikely( i<size );i++)
+		{
+			localStore0[i] = tmpTarget0[i];
+			localStore1[i] = tmpTarget1[i];
+			localStore2[i] = tmpTarget2[i];
+		}
+}
+
+void performRaycastAgainstConvex (RaycastGatheredObjectData* gatheredObjectData, const SpuRaycastTaskWorkUnit& workUnit, SpuRaycastTaskWorkUnitOut* workUnitOut, RaycastTask_LocalStoreMemory* lsMemPtr);
+
+class spuRaycastNodeCallback1 : public btNodeOverlapCallback
+{
+	RaycastGatheredObjectData* m_gatheredObjectData;
+	const SpuRaycastTaskWorkUnit* m_workUnits;
+	SpuRaycastTaskWorkUnitOut* m_workUnitsOut;
+	int m_workUnit;
+	RaycastTask_LocalStoreMemory* m_lsMemPtr;
+
+	ATTRIBUTE_ALIGNED16(btVector3	spuTriangleVertices[3]);
+	ATTRIBUTE_ALIGNED16(btScalar	spuUnscaledVertex[4]);
+	//ATTRIBUTE_ALIGNED16(int	spuIndices[16]);
+public:
+	spuRaycastNodeCallback1(RaycastGatheredObjectData* gatheredObjectData,const SpuRaycastTaskWorkUnit* workUnits, SpuRaycastTaskWorkUnitOut* workUnitsOut, RaycastTask_LocalStoreMemory* lsMemPtr)
+		: m_gatheredObjectData(gatheredObjectData),
+		  m_workUnits(workUnits),
+		  m_workUnitsOut(workUnitsOut),
+		  m_workUnit(0),
+		  m_lsMemPtr (lsMemPtr)
+	{
+	}
+
+	void setWorkUnit (int workUnit) { m_workUnit = workUnit; }
+	virtual void processNode(int subPart, int triangleIndex)
+	{
+		///Create a triangle on the stack, call process collision, with GJK
+		///DMA the vertices, can benefit from software caching
+
+		//		spu_printf("processNode with triangleIndex %d\n",triangleIndex);
+
+			// ugly solution to support both 16bit and 32bit indices
+		if (m_lsMemPtr->bvhShapeData.gIndexMesh.m_indexType == PHY_SHORT)
+		{
+			short int* indexBasePtr = (short int*)(m_lsMemPtr->bvhShapeData.gIndexMesh.m_triangleIndexBase+triangleIndex*m_lsMemPtr->bvhShapeData.gIndexMesh.m_triangleIndexStride);
+			ATTRIBUTE_ALIGNED16(short int tmpIndices[3]);
+
+			small_cache_read_triple(&tmpIndices[0],(ppu_address_t)&indexBasePtr[0],
+									&tmpIndices[1],(ppu_address_t)&indexBasePtr[1],
+									&tmpIndices[2],(ppu_address_t)&indexBasePtr[2],
+									sizeof(short int));
+
+			m_lsMemPtr->spuIndices[0] = int(tmpIndices[0]);
+			m_lsMemPtr->spuIndices[1] = int(tmpIndices[1]);
+			m_lsMemPtr->spuIndices[2] = int(tmpIndices[2]);
+		} else
+		{
+			int* indexBasePtr = (int*)(m_lsMemPtr->bvhShapeData.gIndexMesh.m_triangleIndexBase+triangleIndex*m_lsMemPtr->bvhShapeData.gIndexMesh.m_triangleIndexStride);
+
+			small_cache_read_triple(&m_lsMemPtr->spuIndices[0],(ppu_address_t)&indexBasePtr[0],
+								&m_lsMemPtr->spuIndices[1],(ppu_address_t)&indexBasePtr[1],
+								&m_lsMemPtr->spuIndices[2],(ppu_address_t)&indexBasePtr[2],
+								sizeof(int));
+		}
+
+		//printf("%d %d %d\n", m_lsMemPtr->spuIndices[0], m_lsMemPtr->spuIndices[1], m_lsMemPtr->spuIndices[2]);
+		//		spu_printf("SPU index0=%d ,",spuIndices[0]);
+		//		spu_printf("SPU index1=%d ,",spuIndices[1]);
+		//		spu_printf("SPU index2=%d ,",spuIndices[2]);
+		//		spu_printf("SPU: indexBasePtr=%llx\n",indexBasePtr);
+
+		const btVector3& meshScaling = m_lsMemPtr->bvhShapeData.gTriangleMeshInterfacePtr->getScaling();
+	
+		for (int j=2;btLikely( j>=0 );j--)
+		{
+			int graphicsindex = m_lsMemPtr->spuIndices[j];
+
+						//spu_printf("SPU index=%d ,",graphicsindex);
+			btScalar* graphicsbasePtr = (btScalar*)(m_lsMemPtr->bvhShapeData.gIndexMesh.m_vertexBase+graphicsindex*m_lsMemPtr->bvhShapeData.gIndexMesh.m_vertexStride);
+			
+			//			spu_printf("SPU graphicsbasePtr=%llx\n",graphicsbasePtr);
+
+
+			///handle un-aligned vertices...
+
+			//another DMA for each vertex
+			small_cache_read_triple(&spuUnscaledVertex[0],(ppu_address_t)&graphicsbasePtr[0],
+									&spuUnscaledVertex[1],(ppu_address_t)&graphicsbasePtr[1],
+									&spuUnscaledVertex[2],(ppu_address_t)&graphicsbasePtr[2],
+									sizeof(btScalar));
+			
+			//printf("%f %f %f\n", spuUnscaledVertex[0],spuUnscaledVertex[1],spuUnscaledVertex[2]);
+			spuTriangleVertices[j] = btVector3(
+				spuUnscaledVertex[0]*meshScaling.getX(),
+				spuUnscaledVertex[1]*meshScaling.getY(),
+				spuUnscaledVertex[2]*meshScaling.getZ());
+
+				//spu_printf("SPU:triangle vertices:%f,%f,%f\n",spuTriangleVertices[j].x(),spuTriangleVertices[j].y(),spuTriangleVertices[j].z());
+		}
+		
+		RaycastGatheredObjectData triangleGatheredObjectData (*m_gatheredObjectData);
+		triangleGatheredObjectData.m_shapeType = TRIANGLE_SHAPE_PROXYTYPE;
+		triangleGatheredObjectData.m_spuCollisionShape = &spuTriangleVertices[0];
+
+		//printf("%f %f %f\n", spuTriangleVertices[0][0],spuTriangleVertices[0][1],spuTriangleVertices[0][2]);
+		//printf("%f %f %f\n", spuTriangleVertices[1][0],spuTriangleVertices[1][1],spuTriangleVertices[1][2]);
+		//printf("%f %f %f\n", spuTriangleVertices[2][0],spuTriangleVertices[2][1],spuTriangleVertices[2][2]);
+		SpuRaycastTaskWorkUnitOut out;
+		out.hitFraction = 1.0;
+		performRaycastAgainstConvex (&triangleGatheredObjectData, m_workUnits[m_workUnit], &out, m_lsMemPtr);
+		/* XXX: For now only take the closest hit */
+		if (out.hitFraction < m_workUnitsOut[m_workUnit].hitFraction)
+		{
+			m_workUnitsOut[m_workUnit].hitFraction = out.hitFraction;
+			m_workUnitsOut[m_workUnit].hitNormal = out.hitNormal;
+		}
+	}
+
+};
+
+class spuRaycastNodeCallback : public btNodeOverlapCallback
+{
+	RaycastGatheredObjectData* m_gatheredObjectData;
+	const SpuRaycastTaskWorkUnit* m_workUnits;
+	SpuRaycastTaskWorkUnitOut* m_workUnitsOut;
+	int m_numWorkUnits;
+	RaycastTask_LocalStoreMemory* m_lsMemPtr;
+
+	ATTRIBUTE_ALIGNED16(btVector3	spuTriangleVertices[3]);
+	ATTRIBUTE_ALIGNED16(btScalar	spuUnscaledVertex[4]);
+	//ATTRIBUTE_ALIGNED16(int	spuIndices[16]);
+public:
+	spuRaycastNodeCallback(RaycastGatheredObjectData* gatheredObjectData,const SpuRaycastTaskWorkUnit* workUnits, SpuRaycastTaskWorkUnitOut* workUnitsOut, int numWorkUnits, RaycastTask_LocalStoreMemory* lsMemPtr)
+		: m_gatheredObjectData(gatheredObjectData),
+		  m_workUnits(workUnits),
+		  m_workUnitsOut(workUnitsOut),
+		  m_numWorkUnits(numWorkUnits),
+		  m_lsMemPtr (lsMemPtr)
+	{
+	}
+
+	virtual void processNode(int subPart, int triangleIndex)
+	{
+		///Create a triangle on the stack, call process collision, with GJK
+		///DMA the vertices, can benefit from software caching
+
+		//		spu_printf("processNode with triangleIndex %d\n",triangleIndex);
+
+			// ugly solution to support both 16bit and 32bit indices
+		if (m_lsMemPtr->bvhShapeData.gIndexMesh.m_indexType == PHY_SHORT)
+		{
+			short int* indexBasePtr = (short int*)(m_lsMemPtr->bvhShapeData.gIndexMesh.m_triangleIndexBase+triangleIndex*m_lsMemPtr->bvhShapeData.gIndexMesh.m_triangleIndexStride);
+			ATTRIBUTE_ALIGNED16(short int tmpIndices[3]);
+
+			small_cache_read_triple(&tmpIndices[0],(ppu_address_t)&indexBasePtr[0],
+									&tmpIndices[1],(ppu_address_t)&indexBasePtr[1],
+									&tmpIndices[2],(ppu_address_t)&indexBasePtr[2],
+									sizeof(short int));
+
+			m_lsMemPtr->spuIndices[0] = int(tmpIndices[0]);
+			m_lsMemPtr->spuIndices[1] = int(tmpIndices[1]);
+			m_lsMemPtr->spuIndices[2] = int(tmpIndices[2]);
+		} else
+		{
+			int* indexBasePtr = (int*)(m_lsMemPtr->bvhShapeData.gIndexMesh.m_triangleIndexBase+triangleIndex*m_lsMemPtr->bvhShapeData.gIndexMesh.m_triangleIndexStride);
+
+			small_cache_read_triple(&m_lsMemPtr->spuIndices[0],(ppu_address_t)&indexBasePtr[0],
+								&m_lsMemPtr->spuIndices[1],(ppu_address_t)&indexBasePtr[1],
+								&m_lsMemPtr->spuIndices[2],(ppu_address_t)&indexBasePtr[2],
+								sizeof(int));
+		}
+
+		//printf("%d %d %d\n", m_lsMemPtr->spuIndices[0], m_lsMemPtr->spuIndices[1], m_lsMemPtr->spuIndices[2]);
+		//		spu_printf("SPU index0=%d ,",spuIndices[0]);
+		//		spu_printf("SPU index1=%d ,",spuIndices[1]);
+		//		spu_printf("SPU index2=%d ,",spuIndices[2]);
+		//		spu_printf("SPU: indexBasePtr=%llx\n",indexBasePtr);
+
+		const btVector3& meshScaling = m_lsMemPtr->bvhShapeData.gTriangleMeshInterfacePtr->getScaling();
+	
+		for (int j=2;btLikely( j>=0 );j--)
+		{
+			int graphicsindex = m_lsMemPtr->spuIndices[j];
+
+						//spu_printf("SPU index=%d ,",graphicsindex);
+			btScalar* graphicsbasePtr = (btScalar*)(m_lsMemPtr->bvhShapeData.gIndexMesh.m_vertexBase+graphicsindex*m_lsMemPtr->bvhShapeData.gIndexMesh.m_vertexStride);
+			
+			//			spu_printf("SPU graphicsbasePtr=%llx\n",graphicsbasePtr);
+
+
+			///handle un-aligned vertices...
+
+			//another DMA for each vertex
+			small_cache_read_triple(&spuUnscaledVertex[0],(ppu_address_t)&graphicsbasePtr[0],
+									&spuUnscaledVertex[1],(ppu_address_t)&graphicsbasePtr[1],
+									&spuUnscaledVertex[2],(ppu_address_t)&graphicsbasePtr[2],
+									sizeof(btScalar));
+			
+			//printf("%f %f %f\n", spuUnscaledVertex[0],spuUnscaledVertex[1],spuUnscaledVertex[2]);
+			spuTriangleVertices[j] = btVector3(
+				spuUnscaledVertex[0]*meshScaling.getX(),
+				spuUnscaledVertex[1]*meshScaling.getY(),
+				spuUnscaledVertex[2]*meshScaling.getZ());
+
+				//spu_printf("SPU:triangle vertices:%f,%f,%f\n",spuTriangleVertices[j].x(),spuTriangleVertices[j].y(),spuTriangleVertices[j].z());
+		}
+		
+		RaycastGatheredObjectData triangleGatheredObjectData (*m_gatheredObjectData);
+		triangleGatheredObjectData.m_shapeType = TRIANGLE_SHAPE_PROXYTYPE;
+		triangleGatheredObjectData.m_spuCollisionShape = &spuTriangleVertices[0];
+
+		//printf("%f %f %f\n", spuTriangleVertices[0][0],spuTriangleVertices[0][1],spuTriangleVertices[0][2]);
+		//printf("%f %f %f\n", spuTriangleVertices[1][0],spuTriangleVertices[1][1],spuTriangleVertices[1][2]);
+		//printf("%f %f %f\n", spuTriangleVertices[2][0],spuTriangleVertices[2][1],spuTriangleVertices[2][2]);
+		for (int i = 0; i < m_numWorkUnits; i++)
+		{
+			SpuRaycastTaskWorkUnitOut out;
+			out.hitFraction = 1.0;
+			performRaycastAgainstConvex (&triangleGatheredObjectData, m_workUnits[i], &out, m_lsMemPtr);
+			/* XXX: For now only take the closest hit */
+			if (out.hitFraction < m_workUnitsOut[i].hitFraction)
+			{
+				m_workUnitsOut[i].hitFraction = out.hitFraction;
+				m_workUnitsOut[i].hitNormal = out.hitNormal;
+			}
+		}
+	}
+
+};
+
+
+void	spuWalkStacklessQuantizedTreeAgainstRays(RaycastTask_LocalStoreMemory* lsMemPtr, 
+						 btNodeOverlapCallback* nodeCallback,
+						 const btVector3* rayFrom,
+						 const btVector3* rayTo,
+						 int numWorkUnits,
+						 unsigned short int* quantizedQueryAabbMin,
+						 unsigned short int* quantizedQueryAabbMax,
+						 const btQuantizedBvhNode* rootNode,
+						 int startNodeIndex,int endNodeIndex)
+{
+	int curIndex = startNodeIndex;
+	int walkIterations = 0;
+	int subTreeSize = endNodeIndex - startNodeIndex;
+
+	int escapeIndex;
+
+	unsigned int boxBoxOverlap, rayBoxOverlap, anyRayBoxOverlap;
+	unsigned int isLeafNode;
+
+#define RAYAABB2
+#ifdef RAYAABB2
+	unsigned int sign[SPU_RAYCAST_WORK_UNITS_PER_TASK][3];
+	btVector3 rayInvDirection[SPU_RAYCAST_WORK_UNITS_PER_TASK];
+	btScalar lambda_max[SPU_RAYCAST_WORK_UNITS_PER_TASK];
+	for (int i = 0; i < numWorkUnits; i++)
+	{
+		btVector3 rayDirection = (rayTo[i]-rayFrom[i]);
+		rayDirection.normalize ();
+		lambda_max[i] = rayDirection.dot(rayTo[i]-rayFrom[i]);
+		rayInvDirection[i][0] = btScalar(1.0) / rayDirection[0];
+		rayInvDirection[i][1] = btScalar(1.0) / rayDirection[1];
+		rayInvDirection[i][2] = btScalar(1.0) / rayDirection[2];
+		sign[i][0] = rayDirection[0] < 0.0;
+		sign[i][1] = rayDirection[1] < 0.0;
+		sign[i][2] = rayDirection[2] < 0.0;
+	}
+#endif
+
+	while (curIndex < endNodeIndex)
+	{
+		//catch bugs in tree data
+		assert (walkIterations < subTreeSize);
+
+		walkIterations++;
+
+		isLeafNode = rootNode->isLeafNode();
+
+		anyRayBoxOverlap = 0;
+
+		for (int i = 0; i < numWorkUnits; i++)
+		{
+			unsigned short int* quamin = (quantizedQueryAabbMin + 3 * i);
+			unsigned short int* quamax = (quantizedQueryAabbMax + 3 * i);
+			boxBoxOverlap = spuTestQuantizedAabbAgainstQuantizedAabb(quamin,quamax,rootNode->m_quantizedAabbMin,rootNode->m_quantizedAabbMax);
+			if (!boxBoxOverlap)
+				continue;
+
+			rayBoxOverlap = 0;
+			btScalar param = 1.0;
+			btVector3 normal;
+			btVector3 bounds[2];
+			bounds[0] = lsMemPtr->bvhShapeData.getOptimizedBvh()->unQuantize(rootNode->m_quantizedAabbMin);
+			bounds[1] = lsMemPtr->bvhShapeData.getOptimizedBvh()->unQuantize(rootNode->m_quantizedAabbMax);
+#ifdef RAYAABB2
+			rayBoxOverlap = btRayAabb2 (rayFrom[i], rayInvDirection[i], sign[i], bounds, param, 0.0, lambda_max[i]);
+#else
+			rayBoxOverlap = btRayAabb(rayFrom[i], rayTo[i], bounds[0], bounds[1], param, normal);
+#endif
+
+#ifndef CALLBACK_ALL
+			anyRayBoxOverlap = rayBoxOverlap || anyRayBoxOverlap;
+			/* If we have any ray vs. box overlap and this isn't a leaf node
+			   we know that we need to dig deeper
+			*/
+			if (!isLeafNode && anyRayBoxOverlap)
+				break;
+
+			if (isLeafNode && rayBoxOverlap)
+			{
+				spuRaycastNodeCallback1* callback = (spuRaycastNodeCallback1*)nodeCallback;
+				callback->setWorkUnit (i);
+				nodeCallback->processNode (0, rootNode->getTriangleIndex());
+			}
+#else
+			/* If we have any ray vs. box overlap and this isn't a leaf node
+			   we know that we need to dig deeper
+			*/
+			if (rayBoxOverlap)
+			{
+				anyRayBoxOverlap = 1;
+				break;
+			}
+#endif
+		}
+
+#ifdef CALLBACK_ALL
+		if (isLeafNode && anyRayBoxOverlap)
+		{
+			nodeCallback->processNode (0, rootNode->getTriangleIndex());
+		}
+#endif
+
+		if (anyRayBoxOverlap || isLeafNode)
+		{
+			rootNode++;
+			curIndex++;
+		} else
+		{
+			escapeIndex = rootNode->getEscapeIndex();
+			rootNode += escapeIndex;
+			curIndex += escapeIndex;
+		}
+	}
+
+}
+
+
+void performRaycastAgainstConcave (RaycastGatheredObjectData* gatheredObjectData, const SpuRaycastTaskWorkUnit* workUnits, SpuRaycastTaskWorkUnitOut* workUnitsOut, int numWorkUnits, RaycastTask_LocalStoreMemory* lsMemPtr)
+{
+	//order: first collision shape is convex, second concave. m_isSwapped is true, if the original order was opposite
+	register int dmaSize;
+	register ppu_address_t	dmaPpuAddress2;
+
+	
+	btBvhTriangleMeshShape*	trimeshShape = (btBvhTriangleMeshShape*)gatheredObjectData->m_spuCollisionShape;
+
+	//need the mesh interface, for access to triangle vertices
+	dmaBvhShapeData (&(lsMemPtr->bvhShapeData), trimeshShape);
+
+	unsigned short int quantizedQueryAabbMin[SPU_RAYCAST_WORK_UNITS_PER_TASK][3];
+	unsigned short int quantizedQueryAabbMax[SPU_RAYCAST_WORK_UNITS_PER_TASK][3];
+	btVector3 rayFromInTriangleSpace[SPU_RAYCAST_WORK_UNITS_PER_TASK];
+	btVector3 rayToInTriangleSpace[SPU_RAYCAST_WORK_UNITS_PER_TASK];
+
+	/* Calculate the AABB for the ray in the triangle mesh shape */
+	btTransform rayInTriangleSpace;
+	rayInTriangleSpace = gatheredObjectData->m_worldTransform.inverse();
+
+	for (int i = 0; i < numWorkUnits; i++)
+	{
+		btVector3 aabbMin;
+		btVector3 aabbMax;
+
+		rayFromInTriangleSpace[i] = rayInTriangleSpace(workUnits[i].rayFrom);
+		rayToInTriangleSpace[i] = rayInTriangleSpace(workUnits[i].rayTo);
+
+		aabbMin = rayFromInTriangleSpace[i];
+		aabbMin.setMin (rayToInTriangleSpace[i]);
+		aabbMax = rayFromInTriangleSpace[i];
+		aabbMax.setMax (rayToInTriangleSpace[i]);
+
+		lsMemPtr->bvhShapeData.getOptimizedBvh()->quantizeWithClamp(quantizedQueryAabbMin[i],aabbMin,0);
+		lsMemPtr->bvhShapeData.getOptimizedBvh()->quantizeWithClamp(quantizedQueryAabbMax[i],aabbMax,1);
+	}
+
+	QuantizedNodeArray&	nodeArray = lsMemPtr->bvhShapeData.getOptimizedBvh()->getQuantizedNodeArray();
+	//spu_printf("SPU: numNodes = %d\n",nodeArray.size());
+
+	BvhSubtreeInfoArray& subTrees = lsMemPtr->bvhShapeData.getOptimizedBvh()->getSubtreeInfoArray();	
+
+#ifdef CALLBACK_ALL
+	spuRaycastNodeCallback nodeCallback (gatheredObjectData, workUnits, workUnitsOut, numWorkUnits, lsMemPtr);
+#else
+	spuRaycastNodeCallback1 nodeCallback (gatheredObjectData, workUnits, workUnitsOut, lsMemPtr);
+#endif
+	
+	IndexedMeshArray&	indexArray = lsMemPtr->bvhShapeData.gTriangleMeshInterfacePtr->getIndexedMeshArray();
+
+	//spu_printf("SPU:indexArray.size() = %d\n",indexArray.size());
+	//	spu_printf("SPU: numSubTrees = %d\n",subTrees.size());
+	//not likely to happen
+	if (subTrees.size() && indexArray.size() == 1)
+	{
+		///DMA in the index info
+		dmaBvhIndexedMesh (&lsMemPtr->bvhShapeData.gIndexMesh, indexArray, 0 /* index into indexArray */, 1 /* dmaTag */);
+		cellDmaWaitTagStatusAll(DMA_MASK(1));
+		
+		//display the headers
+		int numBatch = subTrees.size();
+		for (int i=0;i<numBatch;)
+		{
+// BEN: TODO - can reorder DMA transfers for less stall
+			int remaining = subTrees.size() - i;
+			int nextBatch = remaining < MAX_SPU_SUBTREE_HEADERS ? remaining : MAX_SPU_SUBTREE_HEADERS;
+			
+			dmaBvhSubTreeHeaders (&lsMemPtr->bvhShapeData.gSubtreeHeaders[0], (ppu_address_t)(&subTrees[i]), nextBatch, 1);
+			cellDmaWaitTagStatusAll(DMA_MASK(1));
+			
+
+			//			spu_printf("nextBatch = %d\n",nextBatch);
+
+			
+			for (int j=0;j<nextBatch;j++)
+			{
+				const btBvhSubtreeInfo& subtree = lsMemPtr->bvhShapeData.gSubtreeHeaders[j];
+				
+				unsigned int overlap = 1;
+				for (int boxId = 0; boxId < numWorkUnits; boxId++)
+				{
+					overlap = spuTestQuantizedAabbAgainstQuantizedAabb(quantizedQueryAabbMin[boxId],quantizedQueryAabbMax[boxId],subtree.m_quantizedAabbMin,subtree.m_quantizedAabbMax);
+					if (overlap)
+						break;
+				}
+
+				if (overlap)
+				{
+					btAssert(subtree.m_subtreeSize);
+
+					//dma the actual nodes of this subtree
+					dmaBvhSubTreeNodes (&lsMemPtr->bvhShapeData.gSubtreeNodes[0], subtree, nodeArray, 2);
+
+					cellDmaWaitTagStatusAll(DMA_MASK(2));
+
+					/* Walk this subtree */
+					
+					{
+
+						spuWalkStacklessQuantizedTreeAgainstRays(lsMemPtr,
+										        &nodeCallback,
+										        &rayFromInTriangleSpace[0],
+											&rayToInTriangleSpace[0],
+											numWorkUnits,
+											&quantizedQueryAabbMin[0][0],&quantizedQueryAabbMax[0][0],
+											&lsMemPtr->bvhShapeData.gSubtreeNodes[0], 0, subtree.m_subtreeSize);
+					}
+				}
+				//				spu_printf("subtreeSize = %d\n",gSubtreeHeaders[j].m_subtreeSize);
+			}
+
+			//	unsigned short int	m_quantizedAabbMin[3];
+			//	unsigned short int	m_quantizedAabbMax[3];
+			//	int			m_rootNodeIndex;
+			//	int			m_subtreeSize;
+			i+=nextBatch;
+		}
+
+		//pre-fetch first tree, then loop and double buffer
+	}
+	
+}
+
+void performRaycastAgainstCompound (RaycastGatheredObjectData* gatheredObjectData, const SpuRaycastTaskWorkUnit& workUnit, SpuRaycastTaskWorkUnitOut* workUnitOut, RaycastTask_LocalStoreMemory* lsMemPtr)
+{
+	//XXX spu_printf ("Currently no support for ray. vs compound objects. Support coming soon.\n");
+}
+
+void
+performRaycastAgainstConvex (RaycastGatheredObjectData* gatheredObjectData, const SpuRaycastTaskWorkUnit& workUnit, SpuRaycastTaskWorkUnitOut* workUnitOut, RaycastTask_LocalStoreMemory* lsMemPtr)
+{
+	SpuVoronoiSimplexSolver simplexSolver;
+
+	btTransform rayFromTrans, rayToTrans;
+	rayFromTrans.setIdentity ();
+	rayFromTrans.setOrigin (workUnit.rayFrom);
+	rayToTrans.setIdentity ();
+	rayToTrans.setOrigin (workUnit.rayTo);
+
+	SpuCastResult result;
+
+	/* Load the vertex data if the shape is a convex hull */
+	/* XXX: We might be loading the shape twice */
+	ATTRIBUTE_ALIGNED16(char convexHullShape[sizeof(btConvexHullShape)]);
+	if (gatheredObjectData->m_shapeType == CONVEX_HULL_SHAPE_PROXYTYPE)
+	{
+		register int dmaSize;
+		register ppu_address_t	dmaPpuAddress2;
+		dmaSize = sizeof(btConvexHullShape);
+		dmaPpuAddress2 = gatheredObjectData->m_collisionShape;
+		cellDmaGet(&convexHullShape, dmaPpuAddress2, dmaSize, DMA_TAG(1), 0, 0);
+		cellDmaWaitTagStatusAll(DMA_MASK(1));
+		dmaConvexVertexData (&lsMemPtr->convexVertexData, (btConvexHullShape*)&convexHullShape);
+		cellDmaWaitTagStatusAll(DMA_MASK(2)); // dmaConvexVertexData uses dma channel 2!
+		lsMemPtr->convexVertexData.gSpuConvexShapePtr = gatheredObjectData->m_spuCollisionShape;
+		lsMemPtr->convexVertexData.gConvexPoints = &lsMemPtr->convexVertexData.g_convexPointBuffer[0];
+	}
+
+	/* performRaycast */
+	SpuSubsimplexRayCast caster (gatheredObjectData->m_spuCollisionShape, &lsMemPtr->convexVertexData, gatheredObjectData->m_shapeType, gatheredObjectData->m_collisionMargin, &simplexSolver);
+	bool r = caster.calcTimeOfImpact (rayFromTrans, rayToTrans, gatheredObjectData->m_worldTransform, gatheredObjectData->m_worldTransform,result);
+
+	if (r)
+	{
+		workUnitOut->hitFraction = result.m_fraction;
+		workUnitOut->hitNormal = result.m_normal;
+	}
+}
+
+void	processRaycastTask(void* userPtr, void* lsMemory)
+{
+	RaycastTask_LocalStoreMemory* localMemory = (RaycastTask_LocalStoreMemory*)lsMemory;
+
+	SpuRaycastTaskDesc* taskDescPtr = (SpuRaycastTaskDesc*)userPtr;
+	SpuRaycastTaskDesc& taskDesc = *taskDescPtr;
+
+	SpuCollisionObjectWrapper* cows = (SpuCollisionObjectWrapper*)taskDesc.spuCollisionObjectsWrappers;
+
+	//spu_printf("in processRaycastTask %d\n", taskDesc.numSpuCollisionObjectWrappers);
+	/* for each object */
+	RaycastGatheredObjectData gatheredObjectData;
+	for (int objectId = 0; objectId < taskDesc.numSpuCollisionObjectWrappers; objectId++)
+	{
+		//spu_printf("%d / %d\n", objectId, taskDesc.numSpuCollisionObjectWrappers);
+		
+		/* load initial collision shape */
+		GatherCollisionObjectAndShapeData (&gatheredObjectData, localMemory, (ppu_address_t)&cows[objectId]);
+
+		if (btBroadphaseProxy::isConcave (gatheredObjectData.m_shapeType))
+		{
+			SpuRaycastTaskWorkUnitOut tWorkUnitsOut[SPU_RAYCAST_WORK_UNITS_PER_TASK];
+			for (int rayId = 0; rayId < taskDesc.numWorkUnits; rayId++)
+			{
+				tWorkUnitsOut[rayId].hitFraction = 1.0;
+			}
+
+			performRaycastAgainstConcave (&gatheredObjectData, &taskDesc.workUnits[0], &tWorkUnitsOut[0], taskDesc.numWorkUnits, localMemory);
+
+			for (int rayId = 0; rayId < taskDesc.numWorkUnits; rayId++)
+			{
+				const SpuRaycastTaskWorkUnit& workUnit = taskDesc.workUnits[rayId];
+				if (tWorkUnitsOut[rayId].hitFraction == 1.0)
+					continue;
+
+				ATTRIBUTE_ALIGNED16(SpuRaycastTaskWorkUnitOut workUnitOut);
+				dmaLoadRayOutput ((ppu_address_t)workUnit.output, &workUnitOut, 1);
+				cellDmaWaitTagStatusAll(DMA_MASK(1));
+				
+				
+				/* XXX Only support taking the closest hit for now */
+				if (tWorkUnitsOut[rayId].hitFraction < workUnitOut.hitFraction)
+				{
+					workUnitOut.hitFraction = tWorkUnitsOut[rayId].hitFraction;
+					workUnitOut.hitNormal = tWorkUnitsOut[rayId].hitNormal;
+				}
+
+				/* write ray cast data back */
+				dmaStoreRayOutput ((ppu_address_t)workUnit.output, &workUnitOut, 1);
+				cellDmaWaitTagStatusAll(DMA_MASK(1));
+			}
+		} else if (btBroadphaseProxy::isConvex (gatheredObjectData.m_shapeType)) {
+
+			btVector3 objectBoxMin, objectBoxMax;
+			computeAabb (objectBoxMin, objectBoxMax, (btConvexInternalShape*)gatheredObjectData.m_spuCollisionShape, gatheredObjectData.m_collisionShape, gatheredObjectData.m_shapeType, gatheredObjectData.m_worldTransform);
+			for (unsigned int rayId = 0; rayId < taskDesc.numWorkUnits; rayId++)
+			{
+				const SpuRaycastTaskWorkUnit& workUnit = taskDesc.workUnits[rayId];
+			
+				btScalar ignored_param = 1.0;
+				btVector3 ignored_normal;
+				if (btRayAabb(workUnit.rayFrom, workUnit.rayTo, objectBoxMin, objectBoxMax, ignored_param, ignored_normal))
+				{
+					ATTRIBUTE_ALIGNED16(SpuRaycastTaskWorkUnitOut workUnitOut);
+					SpuRaycastTaskWorkUnitOut tWorkUnitOut;
+					tWorkUnitOut.hitFraction = 1.0;
+
+					performRaycastAgainstConvex (&gatheredObjectData, workUnit, &tWorkUnitOut, localMemory);
+					if (tWorkUnitOut.hitFraction == 1.0)
+						continue;
+	
+					dmaLoadRayOutput ((ppu_address_t)workUnit.output, &workUnitOut, 1);
+					cellDmaWaitTagStatusAll(DMA_MASK(1));
+
+					/* XXX Only support taking the closest hit for now */
+					if (tWorkUnitOut.hitFraction < workUnitOut.hitFraction)
+					{
+						workUnitOut.hitFraction = tWorkUnitOut.hitFraction;
+						workUnitOut.hitNormal = tWorkUnitOut.hitNormal;
+						/* write ray cast data back */
+						dmaStoreRayOutput ((ppu_address_t)workUnit.output, &workUnitOut, 1);
+						cellDmaWaitTagStatusAll(DMA_MASK(1));
+					}
+				}
+			}
+
+		} else if (btBroadphaseProxy::isCompound (gatheredObjectData.m_shapeType)) {
+			for (unsigned int rayId = 0; rayId < taskDesc.numWorkUnits; rayId++)
+			{
+				const SpuRaycastTaskWorkUnit& workUnit = taskDesc.workUnits[rayId];
+				ATTRIBUTE_ALIGNED16(SpuRaycastTaskWorkUnitOut workUnitOut);
+				SpuRaycastTaskWorkUnitOut tWorkUnitOut;
+				tWorkUnitOut.hitFraction = 1.0;
+
+				performRaycastAgainstCompound (&gatheredObjectData, workUnit, &tWorkUnitOut, localMemory);
+				if (tWorkUnitOut.hitFraction == 1.0)
+					continue;
+
+				dmaLoadRayOutput ((ppu_address_t)workUnit.output, &workUnitOut, 1);
+				cellDmaWaitTagStatusAll(DMA_MASK(1));
+				/* XXX Only support taking the closest hit for now */
+				if (tWorkUnitOut.hitFraction < workUnitOut.hitFraction)
+				{
+					workUnitOut.hitFraction = tWorkUnitOut.hitFraction;
+					workUnitOut.hitNormal = tWorkUnitOut.hitNormal;
+				}
+
+				/* write ray cast data back */
+				dmaStoreRayOutput ((ppu_address_t)workUnit.output, &workUnitOut, 1);
+				cellDmaWaitTagStatusAll(DMA_MASK(1));
+			}
+		}
+	}
+}
Index: /code/branches/physics/src/bullet/BulletMultiThreaded/SpuRaycastTask/SpuRaycastTask.h
===================================================================
--- /code/branches/physics/src/bullet/BulletMultiThreaded/SpuRaycastTask/SpuRaycastTask.h	(revision 1966)
+++ /code/branches/physics/src/bullet/BulletMultiThreaded/SpuRaycastTask/SpuRaycastTask.h	(revision 1966)
@@ -0,0 +1,50 @@
+#ifndef __SPU_RAYCAST_TASK_H
+#define __SPU_RAYCAST_TASK_H
+
+#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
+#include "BulletCollision/CollisionDispatch/btCollisionWorld.h"
+#include "LinearMath/btVector3.h"
+#include "../PlatformDefinitions.h"
+
+ATTRIBUTE_ALIGNED16(struct) RaycastGatheredObjectData
+{
+	ppu_address_t m_collisionShape;
+	void* m_spuCollisionShape;
+	btVector3	m_primitiveDimensions;
+	int		m_shapeType;
+	float	m_collisionMargin;
+	btTransform	m_worldTransform;
+};
+
+
+ATTRIBUTE_ALIGNED16(struct) SpuRaycastTaskWorkUnitOut
+{
+	btVector3 hitNormal; /* out */
+	btScalar hitFraction; /* out */
+	btCollisionWorld::LocalShapeInfo shapeInfo; /* out */
+};
+
+/* Perform a raycast on collision object */
+ATTRIBUTE_ALIGNED16(struct) SpuRaycastTaskWorkUnit
+{
+	btVector3 rayFrom; /* in */
+	btVector3 rayTo; /* in */
+	SpuRaycastTaskWorkUnitOut* output; /* out */
+};
+
+#define SPU_RAYCAST_WORK_UNITS_PER_TASK 16
+
+ATTRIBUTE_ALIGNED128(struct) SpuRaycastTaskDesc
+{
+	SpuRaycastTaskWorkUnit workUnits[SPU_RAYCAST_WORK_UNITS_PER_TASK];
+	unsigned int numWorkUnits;
+	void* spuCollisionObjectsWrappers;
+	unsigned int numSpuCollisionObjectWrappers;
+	int taskId;
+};
+
+
+void	processRaycastTask (void* userPtr, void* lsMemory);
+void*	createRaycastLocalStoreMemory ();
+
+#endif
Index: /code/branches/physics/src/bullet/BulletMultiThreaded/SpuRaycastTask/SpuSubSimplexConvexCast.cpp
===================================================================
--- /code/branches/physics/src/bullet/BulletMultiThreaded/SpuRaycastTask/SpuSubSimplexConvexCast.cpp	(revision 1966)
+++ /code/branches/physics/src/bullet/BulletMultiThreaded/SpuRaycastTask/SpuSubSimplexConvexCast.cpp	(revision 1966)
@@ -0,0 +1,152 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+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.
+*/
+
+#include "SpuSubSimplexConvexCast.h"
+
+
+#include "BulletCollision/CollisionShapes/btConvexShape.h"
+#include "BulletCollision/CollisionShapes/btMinkowskiSumShape.h"
+#include "BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h"
+
+
+SpuSubsimplexRayCast::SpuSubsimplexRayCast (void* shapeB, SpuConvexPolyhedronVertexData* convexDataB, int shapeTypeB, float marginB,
+										    SpuVoronoiSimplexSolver* simplexSolver)
+	:m_simplexSolver(simplexSolver), m_shapeB(shapeB), m_convexDataB(convexDataB), m_shapeTypeB(shapeTypeB), m_marginB(marginB)
+{
+}
+
+///Typically the conservative advancement reaches solution in a few iterations, clip it to 32 for degenerate cases.
+///See discussion about this here http://continuousphysics.com/Bullet/phpBB2/viewtopic.php?t=565
+#ifdef BT_USE_DOUBLE_PRECISION
+#define MAX_ITERATIONS 64
+#else
+#define MAX_ITERATIONS 32
+#endif
+
+/* Returns the support point of the minkowski sum:
+ * MSUM(Pellet, ConvexShape)
+ *
+ */
+void supportPoints (const btTransform xformRay,
+		    const btTransform xformB,
+		    const int shapeType,
+		    const void* shape,
+		    SpuConvexPolyhedronVertexData* convexVertexData,
+		    const btScalar marginB,
+		    const btVector3& seperatingAxis,
+		    btVector3& w,
+		    btVector3& supVertexRay,
+		    btVector3& supVertexB)
+{
+	btVector3 saUnit = seperatingAxis;
+	saUnit.normalize();
+	btVector3 SupportPellet = xformRay(0.0001 * -saUnit);
+	btVector3 rotatedSeperatingAxis = seperatingAxis * xformB.getBasis();
+	btVector3 SupportShape = xformB(localGetSupportingVertexWithoutMargin(shapeType, (void*)shape, rotatedSeperatingAxis, convexVertexData));
+	SupportShape += saUnit * marginB;
+	w = SupportPellet - SupportShape;
+	supVertexRay = SupportPellet;
+	supVertexB = SupportShape;
+}
+
+bool	SpuSubsimplexRayCast::calcTimeOfImpact(const btTransform& fromRay,
+											   const btTransform& toRay,
+											   const btTransform& fromB,
+											   const btTransform& toB,
+											   SpuCastResult& result)
+{
+	m_simplexSolver->reset();
+
+	btVector3 linVelRay, linVelB;
+	linVelRay = toRay.getOrigin() - fromRay.getOrigin();
+	linVelB = toB.getOrigin() - fromB.getOrigin ();
+
+	btScalar lambda = btScalar(0.);
+	
+	btTransform interpolatedTransRay = fromRay;
+	btTransform interpolatedTransB = fromB;
+
+	btVector3 r = (linVelRay-linVelB);
+	btVector3 supVertexRay;
+	btVector3 supVertexB;
+	btVector3 v;
+	supportPoints (fromRay, fromB, m_shapeTypeB, m_shapeB, m_convexDataB, m_marginB, r, v, supVertexRay, supVertexB);
+
+	btVector3 n;
+	n.setValue(btScalar(0.), btScalar(0.), btScalar(0.));
+	bool hasResult = false;
+	btVector3 c;
+	int maxIter = MAX_ITERATIONS;
+
+	btScalar lastLambda = lambda;
+
+	btScalar dist2 = v.length2();
+
+#ifdef BT_USE_DOUBLE_PRECISION
+	btScalar epsilon = btScalar(0.0001);
+#else
+	btScalar epsilon = btScalar(0.0001);
+#endif //BT_USE_DOUBLE_PRECISION
+	btVector3 w,p;
+	btScalar VdotR;
+	
+	while ( (dist2 > epsilon) && maxIter--)
+	{
+		supportPoints (interpolatedTransRay, interpolatedTransB, m_shapeTypeB, m_shapeB, m_convexDataB, m_marginB, v, w, supVertexRay, supVertexB);
+
+		btScalar VdotW = v.dot(w);
+
+		if (lambda > btScalar(1.0))
+		{
+			return false;
+		}
+
+		if ( VdotW > btScalar(0.))
+		{
+			VdotR = v.dot(r);
+
+			if (VdotR >= -(SIMD_EPSILON*SIMD_EPSILON))
+				return false;
+			else
+			{
+				lambda = lambda - VdotW / VdotR;
+				interpolatedTransRay.getOrigin().setInterpolate3(fromRay.getOrigin(), toRay.getOrigin(), lambda);
+				interpolatedTransB.getOrigin().setInterpolate3(fromB.getOrigin(), toB.getOrigin(), lambda);
+				lastLambda = lambda;
+				n = v;
+				hasResult = true;
+			}
+		} 
+		m_simplexSolver->addVertex(w, supVertexRay, supVertexB);
+		if (m_simplexSolver->closest(v))
+		{
+			dist2 = v.length2();
+			hasResult = true;
+			//printf("V=%f , %f, %f\n",v[0],v[1],v[2]);
+			//printf("DIST2=%f\n",dist2);
+			//printf("numverts = %i\n",m_simplexSolver->numVertices());
+		} else
+		{
+			dist2 = btScalar(0.);
+		} 
+	}
+
+	result.m_fraction = lambda;
+	result.m_normal = n;
+	btVector3 hitRay, hitB;
+	m_simplexSolver->compute_points (hitRay, hitB);
+	/* TODO: We could output hit point here (hitB) */
+	return true;
+}
Index: /code/branches/physics/src/bullet/BulletMultiThreaded/SpuRaycastTask/SpuSubSimplexConvexCast.h
===================================================================
--- /code/branches/physics/src/bullet/BulletMultiThreaded/SpuRaycastTask/SpuSubSimplexConvexCast.h	(revision 1966)
+++ /code/branches/physics/src/bullet/BulletMultiThreaded/SpuRaycastTask/SpuSubSimplexConvexCast.h	(revision 1966)
@@ -0,0 +1,60 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+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.
+*/
+
+
+#ifndef SPU_SUBSIMPLEX_RAY_CAST_H
+#define SPU_SUBSIMPLEX_RAY_CAST_H
+
+#include "../SpuNarrowPhaseCollisionTask/SpuVoronoiSimplexSolver.h"
+#include "../SpuNarrowPhaseCollisionTask/SpuCollisionShapes.h"
+#include "SpuRaycastTask.h"
+
+class btConvexShape;
+
+struct SpuCastResult
+{
+	float m_fraction;
+	btVector3 m_normal;
+};
+
+/// btSubsimplexConvexCast implements Gino van den Bergens' paper
+///"Ray Casting against bteral Convex Objects with Application to Continuous Collision Detection"
+/// GJK based Ray Cast, optimized version
+/// Objects should not start in overlap, otherwise results are not defined.
+class SpuSubsimplexRayCast
+{
+	SpuVoronoiSimplexSolver* m_simplexSolver;
+	void* m_shapeB;
+	SpuConvexPolyhedronVertexData* m_convexDataB;
+	int m_shapeTypeB;
+	float m_marginB;
+
+public:
+	SpuSubsimplexRayCast (void* shapeB, SpuConvexPolyhedronVertexData* convexDataB, int shapeTypeB, float marginB,
+						  SpuVoronoiSimplexSolver* simplexSolver);
+
+	//virtual ~btSubsimplexConvexCast();
+
+	///SimsimplexConvexCast calculateTimeOfImpact calculates the time of impact+normal for the linear cast (sweep) between two moving objects.
+	///Precondition is that objects should not penetration/overlap at the start from the interval. Overlap can be tested using btGjkPairDetector.
+	bool calcTimeOfImpact(const btTransform& fromRay,
+						  const btTransform& toRay,
+						  const btTransform& fromB,
+						  const btTransform& toB,
+						  SpuCastResult& result);
+
+};
+
+#endif //SUBSIMPLEX_RAY_CAST_H
Index: /code/branches/physics/src/bullet/BulletMultiThreaded/SpuRaycastTaskProcess.cpp
===================================================================
--- /code/branches/physics/src/bullet/BulletMultiThreaded/SpuRaycastTaskProcess.cpp	(revision 1966)
+++ /code/branches/physics/src/bullet/BulletMultiThreaded/SpuRaycastTaskProcess.cpp	(revision 1966)
@@ -0,0 +1,189 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2007 Erwin Coumans  http://bulletphysics.com
+
+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.
+*/
+
+#include "SpuRaycastTaskProcess.h"
+
+
+SpuRaycastTaskProcess::SpuRaycastTaskProcess(class	btThreadSupportInterface*	threadInterface,  int	maxNumOutstandingTasks)
+:m_threadInterface(threadInterface),
+m_maxNumOutstandingTasks(maxNumOutstandingTasks)
+{
+	m_workUnitTaskBuffers = (unsigned char *)0;
+	m_taskBusy.resize(m_maxNumOutstandingTasks);
+	m_spuRaycastTaskDesc.resize(m_maxNumOutstandingTasks);
+
+	for (int i = 0; i < m_maxNumOutstandingTasks; i++)
+	{
+		m_taskBusy[i] = false;
+	}
+	m_numBusyTasks = 0;
+	m_currentTask = 0;
+	m_currentWorkUnitInTask = 0;
+
+	m_threadInterface->startSPU();
+
+	//printf("sizeof vec_float4: %d\n", sizeof(vec_float4));
+	//printf("sizeof SpuGatherAndProcessWorkUnitInput: %d\n", sizeof(SpuGatherAndProcessWorkUnitInput));
+
+}
+
+SpuRaycastTaskProcess::~SpuRaycastTaskProcess()
+{
+	
+	if (m_workUnitTaskBuffers != 0)
+	{
+		btAlignedFree(m_workUnitTaskBuffers);
+		m_workUnitTaskBuffers = 0;
+	}
+	
+	m_threadInterface->stopSPU();	
+}
+
+
+
+void SpuRaycastTaskProcess::initialize2(void* spuCollisionObjectsWrappers, int numSpuCollisionObjectWrappers)
+{
+	m_spuCollisionObjectWrappers = spuCollisionObjectsWrappers;
+	m_numSpuCollisionObjectWrappers = numSpuCollisionObjectWrappers;
+	for (int i = 0; i < m_maxNumOutstandingTasks; i++)
+	{
+		m_taskBusy[i] = false;
+	}
+	m_numBusyTasks = 0;
+	m_currentTask = 0;
+	m_currentWorkUnitInTask = 0;
+
+#ifdef DEBUG_SpuRaycastTaskProcess
+	m_initialized = true;
+#endif
+}
+
+
+void SpuRaycastTaskProcess::issueTask2()
+{
+	m_taskBusy[m_currentTask] = true;
+	m_numBusyTasks++;
+
+	SpuRaycastTaskDesc& taskDesc = m_spuRaycastTaskDesc[m_currentTask];
+
+	taskDesc.taskId = m_currentTask;
+	m_threadInterface->sendRequest(1, (uint32_t) &taskDesc,m_currentTask);
+	//printf("send thread requested for task %d\n", m_currentTask);
+	// if all tasks busy, wait for spu event to clear the task.
+	if (m_numBusyTasks >= m_maxNumOutstandingTasks)
+	{
+		unsigned int taskId;
+		unsigned int outputSize;
+
+		for (int i=0;i<m_maxNumOutstandingTasks;i++)
+	  {
+		  if (m_taskBusy[i])
+		  {
+			  taskId = i;
+			  break;
+		  }
+	  }
+		m_threadInterface->waitForResponse(&taskId, &outputSize);
+
+		//printf("PPU: after issue, received event: %u %d\n", taskId, outputSize);
+
+		m_taskBusy[taskId] = false;
+
+		m_numBusyTasks--;
+	} else {
+		//printf("Sent request, not enough busy tasks\n");
+	}
+}
+
+void SpuRaycastTaskProcess::addWorkToTask(SpuRaycastTaskWorkUnit& workunit)
+{
+	m_spuRaycastTaskDesc[m_currentTask].workUnits[m_currentWorkUnitInTask] = workunit;
+	m_currentWorkUnitInTask++;
+	if (m_currentWorkUnitInTask == SPU_RAYCAST_WORK_UNITS_PER_TASK)
+	{
+		m_spuRaycastTaskDesc[m_currentTask].numWorkUnits = m_currentWorkUnitInTask;
+		m_spuRaycastTaskDesc[m_currentTask].numSpuCollisionObjectWrappers = m_numSpuCollisionObjectWrappers;
+		m_spuRaycastTaskDesc[m_currentTask].spuCollisionObjectsWrappers = m_spuCollisionObjectWrappers;
+		//printf("Task buffer full, issuing\n");
+		issueTask2 ();
+		//printf("Returned from issueTask2()\n");
+		m_currentWorkUnitInTask = 0;
+
+		// find new task buffer
+		for (int i = 0; i < m_maxNumOutstandingTasks; i++)
+		{
+			if (!m_taskBusy[i])
+			{
+				m_currentTask = i;
+				//init the task data
+				break;
+			}
+		}
+		//printf("next task = %d\n", m_currentTask);
+	}
+}
+
+
+void 
+SpuRaycastTaskProcess::flush2()
+{
+#ifdef DEBUG_SPU_TASK_SCHEDULING
+	printf("\nSpuRaycastTaskProcess::flush()\n");
+#endif //DEBUG_SPU_TASK_SCHEDULING
+	
+	// if there's a partially filled task buffer, submit that task
+	//printf("Flushing... %d remaining\n", m_currentWorkUnitInTask);
+	if (m_currentWorkUnitInTask > 0)
+	{
+		m_spuRaycastTaskDesc[m_currentTask].numWorkUnits = m_currentWorkUnitInTask;
+		m_spuRaycastTaskDesc[m_currentTask].numSpuCollisionObjectWrappers = m_numSpuCollisionObjectWrappers;
+		m_spuRaycastTaskDesc[m_currentTask].spuCollisionObjectsWrappers = m_spuCollisionObjectWrappers;
+		issueTask2();
+		m_currentWorkUnitInTask = 0;
+	}
+
+
+	// all tasks are issued, wait for all tasks to be complete
+	while(m_numBusyTasks > 0)
+	{
+	  // Consolidating SPU code
+	  unsigned int taskId;
+	  unsigned int outputSize;
+	  
+	  for (int i=0;i<m_maxNumOutstandingTasks;i++)
+	  {
+		  if (m_taskBusy[i])
+		  {
+			  taskId = i;
+			  break;
+		  }
+	  }
+
+	  //printf("Busy tasks... %d\n", m_numBusyTasks);
+
+	  {
+			// SPURS support.
+			m_threadInterface->waitForResponse(&taskId, &outputSize);
+		}
+
+		//printf("PPU: flushing, received event: %u %d\n", taskId, outputSize);
+
+		//postProcess(taskId, outputSize);
+
+		m_taskBusy[taskId] = false;
+
+		m_numBusyTasks--;
+	}
+}
Index: /code/branches/physics/src/bullet/BulletMultiThreaded/SpuRaycastTaskProcess.h
===================================================================
--- /code/branches/physics/src/bullet/BulletMultiThreaded/SpuRaycastTaskProcess.h	(revision 1966)
+++ /code/branches/physics/src/bullet/BulletMultiThreaded/SpuRaycastTaskProcess.h	(revision 1966)
@@ -0,0 +1,72 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2007 Erwin Coumans  http://bulletphysics.com
+
+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.
+*/
+
+#ifndef SPU_RAY_TASK_PROCESS_H
+#define SPU_RAY_TASK_PROCESS_H
+
+#include <assert.h>
+#include <string.h>
+
+#include <LinearMath/btScalar.h>
+#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
+#include <LinearMath/btAlignedAllocator.h>
+
+#include "PlatformDefinitions.h"
+#include "LinearMath/btAlignedObjectArray.h"
+#include "SpuRaycastTask/SpuRaycastTask.h"
+
+#include "btThreadSupportInterface.h"
+
+/// SpuRaycastTaskProcess handles SPU processing of raycast requests
+class SpuRaycastTaskProcess
+{
+	unsigned char  *m_workUnitTaskBuffers;
+
+	// track task buffers that are being used, and total busy tasks
+	btAlignedObjectArray<bool>	m_taskBusy;
+	btAlignedObjectArray<SpuRaycastTaskDesc>	m_spuRaycastTaskDesc;
+
+	btThreadSupportInterface*	m_threadInterface;
+
+	int	m_maxNumOutstandingTasks;
+
+	int	m_numBusyTasks;
+
+	// the current task and the current entry to insert a new work unit
+	int m_currentTask;
+	int m_currentWorkUnitInTask;
+	int m_numSpuCollisionObjectWrappers;
+	void* m_spuCollisionObjectWrappers;
+	void issueTask2();
+	//void postProcess(unsigned int taskId, int outputSize);
+
+public:
+	SpuRaycastTaskProcess(btThreadSupportInterface*	threadInterface, int maxNumOutstandingTasks);
+	
+	~SpuRaycastTaskProcess();
+	
+	/// call initialize in the beginning of the frame, before addCollisionPairToTask
+	void initialize2(void* spuCollisionObjectsWrappers, int numSpuCollisionObjectWrappers);
+
+	/// batch up additional work to a current task for SPU processing. When batch is full, it issues the task.
+	void addWorkToTask(struct SpuRaycastTaskWorkUnit&);
+
+	/// call flush to submit potential outstanding work to SPUs and wait for all involved SPUs to be finished
+	void flush2();
+};
+
+
+#endif // SPU_COLLISION_TASK_PROCESS_H
+
Index: /code/branches/physics/src/bullet/BulletMultiThreaded/SpuSampleTask/SpuSampleTask.cpp
===================================================================
--- /code/branches/physics/src/bullet/BulletMultiThreaded/SpuSampleTask/SpuSampleTask.cpp	(revision 1966)
+++ /code/branches/physics/src/bullet/BulletMultiThreaded/SpuSampleTask/SpuSampleTask.cpp	(revision 1966)
@@ -0,0 +1,214 @@
+/*
+Bullet Continuous Collision Detection and Physics Library, Copyright (c) 2007 Erwin Coumans
+
+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.
+
+*/
+
+
+#include "SpuSampleTask.h"
+#include "BulletDynamics/Dynamics/btRigidBody.h"
+#include "../PlatformDefinitions.h"
+#include "../SpuFakeDma.h"
+#include "LinearMath/btMinMax.h"
+
+#ifdef __SPU__
+#include <spu_printf.h>
+#else
+#include <stdio.h>
+#define spu_printf printf
+#endif
+
+#define MAX_NUM_BODIES 8192
+
+struct SampleTask_LocalStoreMemory
+{
+	ATTRIBUTE_ALIGNED16(char gLocalRigidBody [sizeof(btRigidBody)+16]);
+	ATTRIBUTE_ALIGNED16(void* gPointerArray[MAX_NUM_BODIES]);
+
+};
+
+
+
+
+//-- MAIN METHOD
+void processSampleTask(void* userPtr, void* lsMemory)
+{
+	//	BT_PROFILE("processSampleTask");
+
+	SampleTask_LocalStoreMemory* localMemory = (SampleTask_LocalStoreMemory*)lsMemory;
+
+	SpuSampleTaskDesc* taskDescPtr = (SpuSampleTaskDesc*)userPtr;
+	SpuSampleTaskDesc& taskDesc = *taskDescPtr;
+
+	switch (taskDesc.m_sampleCommand)
+	{
+	case CMD_SAMPLE_INTEGRATE_BODIES:
+		{
+			btTransform predictedTrans;
+			btCollisionObject** eaPtr = (btCollisionObject**)taskDesc.m_mainMemoryPtr;
+
+			int batchSize = taskDesc.m_sampleValue;
+			if (batchSize>MAX_NUM_BODIES)
+			{
+				spu_printf("SPU Error: exceed number of bodies, see MAX_NUM_BODIES in SpuSampleTask.cpp\n");
+				break;
+			}
+			int dmaArraySize = batchSize*sizeof(void*);
+
+			uint64_t ppuArrayAddress = reinterpret_cast<uint64_t>(eaPtr);
+
+			//			spu_printf("array location is at %llx, batchSize = %d, DMA size = %d\n",ppuArrayAddress,batchSize,dmaArraySize);
+
+			if (dmaArraySize>=16)
+			{
+				cellDmaLargeGet((void*)&localMemory->gPointerArray[0], ppuArrayAddress  , dmaArraySize, DMA_TAG(1), 0, 0);	
+				cellDmaWaitTagStatusAll(DMA_MASK(1));
+			} else
+			{
+				stallingUnalignedDmaSmallGet((void*)&localMemory->gPointerArray[0], ppuArrayAddress  , dmaArraySize);
+			}
+
+
+			for ( int i=0;i<batchSize;i++)
+			{
+				///DMA rigid body
+
+				void* localPtr = &localMemory->gLocalRigidBody[0];
+				void* shortAdd = localMemory->gPointerArray[i];
+				uint64_t ppuRigidBodyAddress = reinterpret_cast<uint64_t>(shortAdd);
+
+				//	spu_printf("cellDmaGet at CMD_SAMPLE_INTEGRATE_BODIES from %llx to %llx\n",ppuRigidBodyAddress,localPtr);
+
+				int dmaBodySize = sizeof(btRigidBody);
+
+				cellDmaGet((void*)localPtr, ppuRigidBodyAddress  , dmaBodySize, DMA_TAG(1), 0, 0);	
+				cellDmaWaitTagStatusAll(DMA_MASK(1));
+
+
+				float timeStep = 1.f/60.f;
+
+				btRigidBody* body = (btRigidBody*) localPtr;//btRigidBody::upcast(colObj);
+				if (body)
+				{
+					if (body->isActive() && (!body->isStaticOrKinematicObject()))
+					{
+						body->predictIntegratedTransform(timeStep, predictedTrans);
+						body->proceedToTransform( predictedTrans);
+						void* ptr = (void*)localPtr;
+						//	spu_printf("cellDmaLargePut from %llx to LS %llx\n",ptr,ppuRigidBodyAddress);
+
+						cellDmaLargePut(ptr, ppuRigidBodyAddress  , dmaBodySize, DMA_TAG(1), 0, 0);
+						cellDmaWaitTagStatusAll(DMA_MASK(1));
+
+					}
+				}
+
+			}
+			break;
+		}
+
+
+	case CMD_SAMPLE_PREDICT_MOTION_BODIES:
+		{
+			btTransform predictedTrans;
+			btCollisionObject** eaPtr = (btCollisionObject**)taskDesc.m_mainMemoryPtr;
+
+			int batchSize = taskDesc.m_sampleValue;
+			int dmaArraySize = batchSize*sizeof(void*);
+
+			if (batchSize>MAX_NUM_BODIES)
+			{
+				spu_printf("SPU Error: exceed number of bodies, see MAX_NUM_BODIES in SpuSampleTask.cpp\n");
+				break;
+			}
+
+			uint64_t ppuArrayAddress = reinterpret_cast<uint64_t>(eaPtr);
+
+			//			spu_printf("array location is at %llx, batchSize = %d, DMA size = %d\n",ppuArrayAddress,batchSize,dmaArraySize);
+
+			if (dmaArraySize>=16)
+			{
+				cellDmaLargeGet((void*)&localMemory->gPointerArray[0], ppuArrayAddress  , dmaArraySize, DMA_TAG(1), 0, 0);	
+				cellDmaWaitTagStatusAll(DMA_MASK(1));
+			} else
+			{
+				stallingUnalignedDmaSmallGet((void*)&localMemory->gPointerArray[0], ppuArrayAddress  , dmaArraySize);
+			}
+
+
+			for ( int i=0;i<batchSize;i++)
+			{
+				///DMA rigid body
+
+				void* localPtr = &localMemory->gLocalRigidBody[0];
+				void* shortAdd = localMemory->gPointerArray[i];
+				uint64_t ppuRigidBodyAddress = reinterpret_cast<uint64_t>(shortAdd);
+
+				//	spu_printf("cellDmaGet at CMD_SAMPLE_INTEGRATE_BODIES from %llx to %llx\n",ppuRigidBodyAddress,localPtr);
+
+				int dmaBodySize = sizeof(btRigidBody);
+
+				cellDmaGet((void*)localPtr, ppuRigidBodyAddress  , dmaBodySize, DMA_TAG(1), 0, 0);	
+				cellDmaWaitTagStatusAll(DMA_MASK(1));
+
+
+				float timeStep = 1.f/60.f;
+
+				btRigidBody* body = (btRigidBody*) localPtr;//btRigidBody::upcast(colObj);
+				if (body)
+				{
+					if (!body->isStaticOrKinematicObject())
+					{
+						if (body->isActive())
+						{
+							body->integrateVelocities( timeStep);
+							//damping
+							body->applyDamping(timeStep);
+
+							body->predictIntegratedTransform(timeStep,body->getInterpolationWorldTransform());
+
+							void* ptr = (void*)localPtr;
+							cellDmaLargePut(ptr, ppuRigidBodyAddress  , dmaBodySize, DMA_TAG(1), 0, 0);
+							cellDmaWaitTagStatusAll(DMA_MASK(1));
+						}
+					}
+				}
+
+			}
+			break;
+		}
+	
+
+
+	default:
+		{
+
+		}
+	};
+}
+
+
+#if defined(__CELLOS_LV2__) || defined (LIBSPE2)
+
+ATTRIBUTE_ALIGNED16(SampleTask_LocalStoreMemory	gLocalStoreMemory);
+
+void* createSampleLocalStoreMemory()
+{
+	return &gLocalStoreMemory;
+}
+#else
+void* createSampleLocalStoreMemory()
+{
+	return new SampleTask_LocalStoreMemory;
+};
+
+#endif
Index: /code/branches/physics/src/bullet/BulletMultiThreaded/SpuSampleTask/SpuSampleTask.h
===================================================================
--- /code/branches/physics/src/bullet/BulletMultiThreaded/SpuSampleTask/SpuSampleTask.h	(revision 1966)
+++ /code/branches/physics/src/bullet/BulletMultiThreaded/SpuSampleTask/SpuSampleTask.h	(revision 1966)
@@ -0,0 +1,54 @@
+/*
+Bullet Continuous Collision Detection and Physics Library, Copyright (c) 2007 Erwin Coumans
+
+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.
+
+*/
+
+#ifndef SPU_SAMPLE_TASK_H
+#define SPU_SAMPLE_TASK_H
+
+#include "../PlatformDefinitions.h"
+#include "LinearMath/btScalar.h"
+#include "LinearMath/btVector3.h"
+#include "LinearMath/btMatrix3x3.h"
+
+#include "LinearMath/btAlignedAllocator.h"
+
+
+enum
+{
+	CMD_SAMPLE_INTEGRATE_BODIES = 1,
+	CMD_SAMPLE_PREDICT_MOTION_BODIES
+};
+
+
+
+ATTRIBUTE_ALIGNED16(struct) SpuSampleTaskDesc
+{
+	BT_DECLARE_ALIGNED_ALLOCATOR();
+
+	uint32_t						m_sampleCommand;
+	uint32_t						m_taskId;
+
+	uint64_t 	m_mainMemoryPtr;
+	int			m_sampleValue;
+	
+
+};
+
+
+void	processSampleTask(void* userPtr, void* lsMemory);
+void*	createSampleLocalStoreMemory();
+
+
+#endif //SPU_SAMPLE_TASK_H
+
Index: /code/branches/physics/src/bullet/BulletMultiThreaded/SpuSampleTaskProcess.cpp
===================================================================
--- /code/branches/physics/src/bullet/BulletMultiThreaded/SpuSampleTaskProcess.cpp	(revision 1966)
+++ /code/branches/physics/src/bullet/BulletMultiThreaded/SpuSampleTaskProcess.cpp	(revision 1966)
@@ -0,0 +1,222 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2007 Erwin Coumans  http://bulletphysics.com
+
+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.
+*/
+
+//#define __CELLOS_LV2__ 1
+
+#define USE_SAMPLE_PROCESS 1
+#ifdef USE_SAMPLE_PROCESS
+
+
+#include "SpuSampleTaskProcess.h"
+#include <stdio.h>
+
+#ifdef __SPU__
+
+
+
+void	SampleThreadFunc(void* userPtr,void* lsMemory)
+{
+	//do nothing
+	printf("hello world\n");
+}
+
+
+void*	SamplelsMemoryFunc()
+{
+	//don't create local store memory, just return 0
+	return 0;
+}
+
+
+#else
+
+
+#include "btThreadSupportInterface.h"
+
+//#	include "SPUAssert.h"
+#include <string.h>
+
+
+
+extern "C" {
+	extern char SPU_SAMPLE_ELF_SYMBOL[];
+};
+
+
+
+
+
+SpuSampleTaskProcess::SpuSampleTaskProcess(btThreadSupportInterface*	threadInterface, unsigned int maxNumOutstandingTasks)
+:m_threadInterface(threadInterface),
+m_maxNumOutstandingTasks(maxNumOutstandingTasks)
+{
+
+	m_taskBusy.resize(m_maxNumOutstandingTasks);
+	m_spuSampleTaskDesc.resize(m_maxNumOutstandingTasks);
+
+	for (int i = 0; i < m_maxNumOutstandingTasks; i++)
+	{
+		m_taskBusy[i] = false;
+	}
+	m_numBusyTasks = 0;
+	m_currentTask = 0;
+
+	m_initialized = false;
+
+	m_threadInterface->startSPU();
+
+
+}
+
+SpuSampleTaskProcess::~SpuSampleTaskProcess()
+{
+	m_threadInterface->stopSPU();
+	
+}
+
+
+
+void	SpuSampleTaskProcess::initialize()
+{
+#ifdef DEBUG_SPU_TASK_SCHEDULING
+	printf("SpuSampleTaskProcess::initialize()\n");
+#endif //DEBUG_SPU_TASK_SCHEDULING
+	
+	for (int i = 0; i < m_maxNumOutstandingTasks; i++)
+	{
+		m_taskBusy[i] = false;
+	}
+	m_numBusyTasks = 0;
+	m_currentTask = 0;
+	m_initialized = true;
+
+}
+
+
+void SpuSampleTaskProcess::issueTask(void* sampleMainMemPtr,int sampleValue,int sampleCommand)
+{
+
+#ifdef DEBUG_SPU_TASK_SCHEDULING
+	printf("SpuSampleTaskProcess::issueTask (m_currentTask= %d\)n", m_currentTask);
+#endif //DEBUG_SPU_TASK_SCHEDULING
+
+	m_taskBusy[m_currentTask] = true;
+	m_numBusyTasks++;
+
+	SpuSampleTaskDesc& taskDesc = m_spuSampleTaskDesc[m_currentTask];
+	{
+		// send task description in event message
+		// no error checking here...
+		// but, currently, event queue can be no larger than NUM_WORKUNIT_TASKS.
+	
+		taskDesc.m_mainMemoryPtr = reinterpret_cast<uint64_t>(sampleMainMemPtr);
+		taskDesc.m_sampleValue = sampleValue;
+		taskDesc.m_sampleCommand = sampleCommand;
+
+		//some bookkeeping to recognize finished tasks
+		taskDesc.m_taskId = m_currentTask;
+	}
+
+
+	m_threadInterface->sendRequest(1, (uint32_t) &taskDesc, m_currentTask);
+
+	// if all tasks busy, wait for spu event to clear the task.
+	
+	if (m_numBusyTasks >= m_maxNumOutstandingTasks)
+	{
+		unsigned int taskId;
+		unsigned int outputSize;
+
+		for (int i=0;i<m_maxNumOutstandingTasks;i++)
+	  {
+		  if (m_taskBusy[i])
+		  {
+			  taskId = i;
+			  break;
+		  }
+	  }
+		m_threadInterface->waitForResponse(&taskId, &outputSize);
+
+		//printf("PPU: after issue, received event: %u %d\n", taskId, outputSize);
+
+		postProcess(taskId, outputSize);
+
+		m_taskBusy[taskId] = false;
+
+		m_numBusyTasks--;
+	}
+
+	// find new task buffer
+	for (unsigned int i = 0; i < m_maxNumOutstandingTasks; i++)
+	{
+		if (!m_taskBusy[i])
+		{
+			m_currentTask = i;
+			break;
+		}
+	}
+}
+
+
+///Optional PPU-size post processing for each task
+void SpuSampleTaskProcess::postProcess(int taskId, int outputSize)
+{
+
+}
+
+
+void SpuSampleTaskProcess::flush()
+{
+#ifdef DEBUG_SPU_TASK_SCHEDULING
+	printf("\nSpuCollisionTaskProcess::flush()\n");
+#endif //DEBUG_SPU_TASK_SCHEDULING
+	
+
+	// all tasks are issued, wait for all tasks to be complete
+	while(m_numBusyTasks > 0)
+	{
+// Consolidating SPU code
+	  unsigned int taskId;
+	  unsigned int outputSize;
+	  
+	  for (int i=0;i<m_maxNumOutstandingTasks;i++)
+	  {
+		  if (m_taskBusy[i])
+		  {
+			  taskId = i;
+			  break;
+		  }
+	  }
+	  {
+			
+		  m_threadInterface->waitForResponse(&taskId, &outputSize);
+	  }
+
+		//printf("PPU: flushing, received event: %u %d\n", taskId, outputSize);
+
+		postProcess(taskId, outputSize);
+
+		m_taskBusy[taskId] = false;
+
+		m_numBusyTasks--;
+	}
+
+
+}
+
+#endif
+
+
+#endif //USE_SAMPLE_PROCESS
Index: /code/branches/physics/src/bullet/BulletMultiThreaded/SpuSampleTaskProcess.h
===================================================================
--- /code/branches/physics/src/bullet/BulletMultiThreaded/SpuSampleTaskProcess.h	(revision 1966)
+++ /code/branches/physics/src/bullet/BulletMultiThreaded/SpuSampleTaskProcess.h	(revision 1966)
@@ -0,0 +1,153 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2007 Erwin Coumans  http://bulletphysics.com
+
+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.
+*/
+
+#ifndef SPU_SAMPLE_TASK_PROCESS_H
+#define SPU_SAMPLE_TASK_PROCESS_H
+
+#include <assert.h>
+
+
+#include "PlatformDefinitions.h"
+
+#include <stdlib.h>
+
+#include "LinearMath/btAlignedObjectArray.h"
+
+
+#include "SpuSampleTask/SpuSampleTask.h"
+
+
+//just add your commands here, try to keep them globally unique for debugging purposes
+#define CMD_SAMPLE_TASK_COMMAND 10
+
+
+
+/// SpuSampleTaskProcess handles SPU processing of collision pairs.
+/// When PPU issues a task, it will look for completed task buffers
+/// PPU will do postprocessing, dependent on workunit output (not likely)
+class SpuSampleTaskProcess
+{
+	// track task buffers that are being used, and total busy tasks
+	btAlignedObjectArray<bool>	m_taskBusy;
+	btAlignedObjectArray<SpuSampleTaskDesc>m_spuSampleTaskDesc;
+	
+	unsigned int   m_numBusyTasks;
+
+	// the current task and the current entry to insert a new work unit
+	unsigned int   m_currentTask;
+
+	bool m_initialized;
+
+	void postProcess(int taskId, int outputSize);
+	
+	class	btThreadSupportInterface*	m_threadInterface;
+
+	unsigned int	m_maxNumOutstandingTasks;
+
+
+
+public:
+	SpuSampleTaskProcess(btThreadSupportInterface*	threadInterface, unsigned int maxNumOutstandingTasks);
+	
+	~SpuSampleTaskProcess();
+	
+	///call initialize in the beginning of the frame, before addCollisionPairToTask
+	void initialize();
+
+	void issueTask(void* sampleMainMemPtr,int sampleValue,int sampleCommand);
+
+	///call flush to submit potential outstanding work to SPUs and wait for all involved SPUs to be finished
+	void flush();
+};
+
+
+#if defined(USE_LIBSPE2) && defined(__SPU__)
+////////////////////MAIN/////////////////////////////
+#include "../SpuLibspe2Support.h"
+#include <spu_intrinsics.h>
+#include <spu_mfcio.h>
+#include <SpuFakeDma.h>
+
+void * SamplelsMemoryFunc();
+void SampleThreadFunc(void* userPtr,void* lsMemory);
+
+//#define DEBUG_LIBSPE2_MAINLOOP
+
+int main(unsigned long long speid, addr64 argp, addr64 envp)
+{
+	printf("SPU is up \n");
+	
+	ATTRIBUTE_ALIGNED128(btSpuStatus status);
+	ATTRIBUTE_ALIGNED16( SpuSampleTaskDesc taskDesc ) ;
+	unsigned int received_message = Spu_Mailbox_Event_Nothing;
+        bool shutdown = false;
+
+	cellDmaGet(&status, argp.ull, sizeof(btSpuStatus), DMA_TAG(3), 0, 0);
+	cellDmaWaitTagStatusAll(DMA_MASK(3));
+
+	status.m_status = Spu_Status_Free;
+	status.m_lsMemory.p = SamplelsMemoryFunc();
+
+	cellDmaLargePut(&status, argp.ull, sizeof(btSpuStatus), DMA_TAG(3), 0, 0);
+	cellDmaWaitTagStatusAll(DMA_MASK(3));
+	
+	
+	while (!shutdown)
+	{
+		received_message = spu_read_in_mbox();
+		
+
+		
+		switch(received_message)
+		{
+		case Spu_Mailbox_Event_Shutdown:
+			shutdown = true;
+			break; 
+		case Spu_Mailbox_Event_Task:
+			// refresh the status
+#ifdef DEBUG_LIBSPE2_MAINLOOP
+			printf("SPU recieved Task \n");
+#endif //DEBUG_LIBSPE2_MAINLOOP
+			cellDmaGet(&status, argp.ull, sizeof(btSpuStatus), DMA_TAG(3), 0, 0);
+			cellDmaWaitTagStatusAll(DMA_MASK(3));
+		
+			btAssert(status.m_status==Spu_Status_Occupied);
+			
+			cellDmaGet(&taskDesc, status.m_taskDesc.p, sizeof(SpuSampleTaskDesc), DMA_TAG(3), 0, 0);
+			cellDmaWaitTagStatusAll(DMA_MASK(3));
+			
+			SampleThreadFunc((void*)&taskDesc, reinterpret_cast<void*> (taskDesc.m_mainMemoryPtr) );
+			break;
+		case Spu_Mailbox_Event_Nothing:
+		default:
+			break;
+		}
+
+		// set to status free and wait for next task
+		status.m_status = Spu_Status_Free;
+		cellDmaLargePut(&status, argp.ull, sizeof(btSpuStatus), DMA_TAG(3), 0, 0);
+		cellDmaWaitTagStatusAll(DMA_MASK(3));		
+				
+		
+  	}
+  	return 0;
+}
+//////////////////////////////////////////////////////
+#endif
+
+
+
+#endif // SPU_SAMPLE_TASK_PROCESS_H
+
Index: /code/branches/physics/src/bullet/BulletMultiThreaded/SpuSolverTask/SpuParallellSolverTask.cpp
===================================================================
--- /code/branches/physics/src/bullet/BulletMultiThreaded/SpuSolverTask/SpuParallellSolverTask.cpp	(revision 1966)
+++ /code/branches/physics/src/bullet/BulletMultiThreaded/SpuSolverTask/SpuParallellSolverTask.cpp	(revision 1966)
@@ -0,0 +1,1789 @@
+/*
+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
+*/
+
+#define IN_PARALLELL_SOLVER 1
+
+
+#include "SpuParallellSolverTask.h"
+#include "BulletDynamics/Dynamics/btRigidBody.h"
+#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
+#include "../PlatformDefinitions.h"
+#include "../SpuFakeDma.h"
+#include "LinearMath/btMinMax.h"
+
+// To setup constraints
+#include "BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h"
+#include "BulletDynamics/ConstraintSolver/btHingeConstraint.h"
+#include "BulletDynamics/ConstraintSolver/btConeTwistConstraint.h"
+#include "BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h"
+
+#ifndef offsetof
+#define offsetof(s,m)   (size_t)&reinterpret_cast<const volatile char&>((((s *)0)->m))
+#endif
+
+//NOTE! When changing this, make sure the package sizes etc below are updated
+#define TEMP_STORAGE_SIZE (150*1024)
+#define CONSTRAINT_MAX_SIZE (46*16)
+
+struct SolverTask_LocalStoreMemory
+{
+	ATTRIBUTE_ALIGNED16(SpuSolverHash			m_localHash);
+
+	// Data for temporary storage in situations where we just need very few
+	ATTRIBUTE_ALIGNED16(SpuSolverInternalConstraint	m_tempInternalConstr[4]);
+	ATTRIBUTE_ALIGNED16(SpuSolverConstraint		m_tempConstraint[1]);
+	ATTRIBUTE_ALIGNED16(SpuSolverBody			m_tempSPUBodies[2]);
+	ATTRIBUTE_ALIGNED16(char					m_tempRBs[2][sizeof(btRigidBody)]);
+	ATTRIBUTE_ALIGNED16(char					m_externalConstraint[CONSTRAINT_MAX_SIZE]);
+
+	// The general temporary storage, "dynamically" allocated
+	ATTRIBUTE_ALIGNED16(uint8_t					m_temporaryStorage[TEMP_STORAGE_SIZE]);
+	size_t										m_temporaryStorageUsed;
+};
+
+
+
+#if defined(__CELLOS_LV2__) || defined (LIBSPE2)
+
+ATTRIBUTE_ALIGNED16(SolverTask_LocalStoreMemory	gLocalStoreMemory);
+
+void* createSolverLocalStoreMemory()
+{
+	return &gLocalStoreMemory;
+}
+#else
+void* createSolverLocalStoreMemory()
+{
+        return new SolverTask_LocalStoreMemory;
+};
+
+#endif
+
+
+
+
+
+
+//-- MEMORY MANAGEMENT HELPERS
+size_t memTemporaryStorage (SolverTask_LocalStoreMemory* lsmem)
+{
+	return TEMP_STORAGE_SIZE - lsmem->m_temporaryStorageUsed;
+}
+
+void setupTemporaryStorage (SolverTask_LocalStoreMemory* lsmem)
+{
+	lsmem->m_temporaryStorageUsed = 0;
+}
+
+void* allocTemporaryStorage (SolverTask_LocalStoreMemory* lsmem, size_t size)
+{
+	// Align size to even 16-byte interval to make it DMA-able
+	size = (size+0xf)&~0xf;
+
+	//btAssert(lsmem->m_temporaryStorageUsed + size <= TEMP_STORAGE_SIZE);
+
+	void *res = &lsmem->m_temporaryStorage[lsmem->m_temporaryStorageUsed];
+	lsmem->m_temporaryStorageUsed += size;
+	return res;
+}
+
+void freeTemporaryStorage (SolverTask_LocalStoreMemory* lsmem, void* ptr, size_t size)
+{
+	// Align size to even 16-byte interval to make it DMA-able
+	size = (size+0xf)&~0xf;
+
+	// Only works if we free the last gotten allocation
+	//btAssert(&lsmem->m_temporaryStorage[lsmem->m_temporaryStorageUsed - size] == ptr);
+
+	lsmem->m_temporaryStorageUsed -= size;
+}
+
+SpuSolverBody* allocBodyStorage (SolverTask_LocalStoreMemory* lsmem, size_t numBodies)
+{
+	return static_cast<SpuSolverBody*> (allocTemporaryStorage(lsmem, sizeof(SpuSolverBody)*numBodies));
+}
+
+void freeBodyStorage (SolverTask_LocalStoreMemory* lsmem, SpuSolverBody* ptr, size_t numBodies)
+{
+	freeTemporaryStorage(lsmem, ptr, sizeof(SpuSolverBody)*numBodies);
+}
+
+SpuSolverInternalConstraint* allocInternalConstraintStorage (SolverTask_LocalStoreMemory* lsmem, size_t numConstr)
+{
+	return static_cast<SpuSolverInternalConstraint*> (allocTemporaryStorage(lsmem, sizeof(SpuSolverInternalConstraint)*numConstr));
+}
+
+void freeInternalConstraintStorage (SolverTask_LocalStoreMemory* lsmem, SpuSolverInternalConstraint* ptr, size_t numConstr)
+{
+	freeTemporaryStorage(lsmem, ptr, sizeof(SpuSolverInternalConstraint)*numConstr);
+}
+
+SpuSolverConstraint* allocConstraintStorage (SolverTask_LocalStoreMemory* lsmem, size_t numConstr)
+{
+	return static_cast<SpuSolverConstraint*> (allocTemporaryStorage(lsmem, sizeof(SpuSolverConstraint)*numConstr));
+}
+
+void freeConstraintStorage (SolverTask_LocalStoreMemory* lsmem, SpuSolverConstraint* ptr, size_t numConstr)
+{
+	freeTemporaryStorage(lsmem, ptr, sizeof(SpuSolverConstraint)*numConstr);
+}
+//-- MEMORY MANAGEMENT HELPERS END
+
+
+
+
+
+
+
+
+
+
+//-- INDEX SET HELPER
+class SpuIndexSet
+{
+public:
+
+	SIMD_FORCE_INLINE SpuIndexSet (uint32_t* a)
+		: m_backingArray (a), m_size (0)
+	{}
+
+	SIMD_FORCE_INLINE int insert (uint32_t data)
+	{
+		int pos = 0;
+		for (pos = 0; pos < m_size; ++pos)
+		{
+			if (m_backingArray[pos] == data)
+			{
+				return pos;
+			}
+		}
+		//btAssert(m_size < SPU_MAX_BODIES_PER_CELL);
+
+		m_backingArray[m_size] = data;
+		return m_size++;
+	}
+
+	SIMD_FORCE_INLINE void clear ()
+	{
+		m_size = 0;
+	}
+
+	SIMD_FORCE_INLINE const uint32_t& operator[](int n) const
+	{
+		return m_backingArray[n];
+	}
+
+	SIMD_FORCE_INLINE uint32_t& operator[](int n)
+	{
+		return m_backingArray[n];
+	}
+
+	SIMD_FORCE_INLINE	int size() const
+	{	// return length of sequence
+		return m_size;
+	}
+
+private:
+	uint32_t*	m_backingArray;
+	int			m_size;
+};
+//-- INDEX SET HELPER END
+
+
+
+
+
+
+
+
+
+//-- RB HANDLING
+static void setupSpuBody (btRigidBody* rb, SpuSolverBody* spuBody)
+{
+	spuBody->m_linearVelocity = rb->getLinearVelocity();
+	spuBody->m_angularVelocity = rb->getAngularVelocity();
+	spuBody->m_worldInvInertiaTensor = rb->getInvInertiaTensorWorld();
+	spuBody->m_invertedMass = rb->getInvMass();
+	spuBody->m_angularFactor = rb->getAngularFactor ();
+}
+//-- RB HANDLING END
+
+
+
+
+
+
+
+//-- HASH HANDLING
+static void writeTaskFlag(SpuSolverHash* hashRemote, uint32_t taskId, uint32_t* flags)
+{
+	int dmaSize = sizeof(uint32_t)*SPU_HASH_NUMCELLDWORDS;
+	uint64_t dmaPpuAddress2 = reinterpret_cast<uint64_t> (hashRemote);
+	dmaPpuAddress2 += offsetof(SpuSolverHash, m_currentMask);
+	dmaPpuAddress2 += sizeof(uint32_t) * SPU_HASH_NUMCELLDWORDS * (taskId + 1);
+	cellDmaLargePut(flags, dmaPpuAddress2, dmaSize, DMA_TAG(1), 0, 0);
+	cellDmaWaitTagStatusAll(DMA_MASK(1));
+
+}
+
+static void updateLocalMask(SolverTask_LocalStoreMemory* localMemory, SpuSolverTaskDesc& taskDesc)
+{
+	int dmaSize = sizeof(uint32_t)*(SPU_MAX_SPUS+1)*SPU_HASH_NUMCELLDWORDS;
+	uint64_t dmaPpuAddress2 = reinterpret_cast<uint64_t> (taskDesc.m_solverData.m_solverHash);
+	dmaPpuAddress2 += offsetof(SpuSolverHash, m_currentMask);
+
+	cellDmaLargeGet(&localMemory->m_localHash.m_currentMask, dmaPpuAddress2, dmaSize, DMA_TAG(1), 0, 0);
+	cellDmaWaitTagStatusAll(DMA_MASK(1));
+}
+
+static unsigned int getZeroIndex(unsigned int start, uint32_t* mask, uint32_t* finished, unsigned int numRegs)
+{
+	// Find the index of some zero within mask|finished 
+	unsigned int index = start;
+
+	int reg = (start >> 5);
+	{
+		unsigned int bit = 1 << (start & 31);
+
+		uint32_t combinedMask = mask[reg] | finished[reg];
+		for (int bitCnt = (start & 31); bitCnt < SPU_HASH_WORDWIDTH; ++bitCnt, ++index, bit <<= 1)
+		{
+			if ((combinedMask & bit) == 0)
+			{
+				return index;
+			}
+		}
+
+		reg++;
+	}
+
+	for (; reg < numRegs; ++reg)
+	{
+		unsigned int bit = 1;
+		uint32_t combinedMask = mask[reg] | finished[reg];
+
+		for (int bitCnt = 0; bitCnt < SPU_HASH_WORDWIDTH; ++bitCnt, ++index, bit <<= 1)
+		{
+			if ((combinedMask & bit) == 0)
+			{
+				return index;
+			}
+		}
+	}
+
+	return SPU_HASH_NUMCELLS;
+}
+
+static bool isAllOne (uint32_t* mask, unsigned int numRegs)
+{
+	uint32_t totalMask = ~0;
+	for (int reg = 0; reg < numRegs; ++reg)
+	{
+		totalMask &= mask[reg];
+	}
+
+	return totalMask == ~0;
+}
+
+static bool checkDependency(unsigned int tryIndex, uint32_t* mask, uint32_t matrix[SPU_HASH_NUMCELLS][SPU_HASH_NUMCELLDWORDS], unsigned int numRegs)
+{
+	for (int reg = 0; reg < numRegs; ++reg)
+	{
+		if ((mask[reg] & matrix[tryIndex][reg]) != 0)
+		{
+			//Dependency conflict, no-go
+			return false;
+		}
+	}
+
+	return true;
+}
+
+static unsigned int getNextFreeCell(SolverTask_LocalStoreMemory* localMemory, SpuSolverTaskDesc& taskDesc, btSpinlock& lock)
+{
+	unsigned int cellIndex = SPU_HASH_NUMCELLS;
+
+	uint32_t myMask[SPU_HASH_NUMCELLDWORDS] = {0};
+	
+	writeTaskFlag(taskDesc.m_solverData.m_solverHash, taskDesc.m_taskId, myMask);
+	SpuSolverHash* hash = &localMemory->m_localHash;
+
+	// locking
+	lock.Lock();		
+
+	bool stopLoop = false;
+	while (!stopLoop)
+	{
+
+		// Try to find a free cell
+		uint32_t tmpMask[SPU_HASH_NUMCELLDWORDS] = {0};
+
+		updateLocalMask(localMemory, taskDesc);
+		
+
+		// Or together the masks of finished cells and all currently locked cells
+		for (int row = 1; row <= SPU_MAX_SPUS; ++row)
+		{
+			for (int reg = 0; reg < SPU_HASH_NUMCELLDWORDS; ++reg)
+			{
+				tmpMask[reg] |= hash->m_currentMask[row][reg];
+			}
+		}
+
+		// Find first zero, starting with offset
+		unsigned int tryIndex;
+		unsigned int start = 0;
+		bool haveTry = false;
+		while (!haveTry)
+		{
+			tryIndex = getZeroIndex(start, tmpMask, hash->m_currentMask[0], SPU_HASH_NUMCELLDWORDS);
+
+			if (tryIndex >= SPU_HASH_NUMCELLS)
+				break;
+
+			haveTry = checkDependency(tryIndex, tmpMask, hash->m_dependencyMatrix, SPU_HASH_NUMCELLDWORDS);
+			start = tryIndex+1;
+		}
+		
+		if (tryIndex < SPU_HASH_NUMCELLS)
+		{
+			// If we get here there is no dependency conflict, so lets use it
+			cellIndex = tryIndex;
+			writeTaskFlag(taskDesc.m_solverData.m_solverHash, taskDesc.m_taskId, hash->m_dependencyMatrix[cellIndex]);
+
+			hash->m_currentMask[0][cellIndex >> 5] |= (1 << (cellIndex & 31));
+
+			{
+				int dmaSize = sizeof(uint32_t)*SPU_HASH_NUMCELLDWORDS;
+				uint64_t dmaPpuAddress2 = reinterpret_cast<uint64_t> (taskDesc.m_solverData.m_solverHash);
+				dmaPpuAddress2 += offsetof(SpuSolverHash, m_currentMask);
+
+				cellDmaLargePut(&hash->m_currentMask, dmaPpuAddress2, dmaSize, DMA_TAG(1), 0, 0);
+				cellDmaWaitTagStatusAll(DMA_MASK(1));
+			}
+
+			stopLoop = true;
+		}
+
+		// Check if there are at all any cells left
+		if (isAllOne (hash->m_currentMask[0], SPU_HASH_NUMCELLDWORDS))
+		{
+			//lock.Unlock();
+			break;
+		}
+		
+	}
+
+	// unlock
+	lock.Unlock();
+	
+
+	return cellIndex;
+}
+//-- HASH HANDLING END
+
+
+
+
+//-- SOLVER METHODS
+// Contact solve method
+static void solveContact (SpuSolverInternalConstraint& constraint, SpuSolverBody& bodyA, SpuSolverBody& bodyB)
+{
+	float normalImpulse(0.f);
+	{
+		if (constraint.m_penetration < 0.f)
+			return;
+
+		//  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;
+		//	float  rel_vel = contactConstraint.m_contactNormal.dot(vel);
+
+		float rel_vel;
+		float vel1Dotn = constraint.m_normal.dot(bodyA.m_linearVelocity) 
+			+ constraint.m_relpos1CrossNormal.dot(bodyA.m_angularVelocity);
+		float vel2Dotn = constraint.m_normal.dot(bodyB.m_linearVelocity) 
+			+ constraint.m_relpos2CrossNormal.dot(bodyB.m_angularVelocity);
+
+		rel_vel = vel1Dotn-vel2Dotn;
+
+
+		float positionalError = constraint.m_penetration;
+		float velocityError = constraint.m_restitution - rel_vel;// * damping;
+
+		float penetrationImpulse = positionalError * constraint.m_jacDiagABInv;
+		float	velocityImpulse = velocityError * constraint.m_jacDiagABInv;
+		float normalImpulse = penetrationImpulse+velocityImpulse;
+
+		// See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse
+		float oldNormalImpulse = constraint.m_appliedImpulse;
+		float sum = oldNormalImpulse + normalImpulse;
+		constraint.m_appliedImpulse = float(0.) > sum ? float(0.): sum;
+
+		normalImpulse = constraint.m_appliedImpulse - oldNormalImpulse;
+
+		if (bodyA.m_invertedMass > 0)
+		{
+			bodyA.m_linearVelocity += constraint.m_normal*bodyA.m_invertedMass*normalImpulse;
+			bodyA.m_angularVelocity += bodyA.m_angularFactor * constraint.m_angularComponentA*normalImpulse;
+		}
+		if (bodyB.m_invertedMass > 0)
+		{
+			bodyB.m_linearVelocity -= constraint.m_normal*bodyB.m_invertedMass*normalImpulse;
+			bodyB.m_angularVelocity -= bodyB.m_angularFactor * constraint.m_angularComponentB*normalImpulse;
+		}
+
+	}
+}
+
+// Friction solve method
+static void solveFriction (SpuSolverInternalConstraint& constraint, SpuSolverBody& bodyA, SpuSolverBody& bodyB, btScalar normalImpulse)
+{
+	const btScalar combinedFriction = constraint.m_friction;
+
+	const btScalar limit = normalImpulse * combinedFriction;
+
+	if (normalImpulse>btScalar(0.))
+	{
+
+		btScalar j1;
+		{
+
+			btScalar rel_vel;
+			const btScalar vel1Dotn = constraint.m_normal.dot(bodyA.m_linearVelocity) 
+				+ constraint.m_relpos1CrossNormal.dot(bodyA.m_angularVelocity);
+			const btScalar vel2Dotn = constraint.m_normal.dot(bodyB.m_linearVelocity) 
+				+ constraint.m_relpos2CrossNormal.dot(bodyB.m_angularVelocity);
+			rel_vel = vel1Dotn-vel2Dotn;
+
+			// calculate j that moves us to zero relative velocity
+			j1 = -rel_vel * constraint.m_jacDiagABInv;
+			btScalar oldTangentImpulse = constraint.m_appliedImpulse;
+			constraint.m_appliedImpulse = oldTangentImpulse + j1;
+			btSetMin(constraint.m_appliedImpulse, limit);
+			btSetMax(constraint.m_appliedImpulse, -limit);
+			j1 = constraint.m_appliedImpulse - oldTangentImpulse;
+
+		}
+
+		if (bodyA.m_invertedMass > 0)
+		{
+			bodyA.m_linearVelocity += constraint.m_normal*bodyA.m_invertedMass*j1;
+			bodyA.m_angularVelocity += bodyA.m_angularFactor * constraint.m_angularComponentA*j1;
+		}
+		if (bodyB.m_invertedMass > 0)
+		{
+			bodyB.m_linearVelocity -= constraint.m_normal*bodyB.m_invertedMass*j1;
+			bodyB.m_angularVelocity -= bodyB.m_angularFactor * constraint.m_angularComponentB*j1;
+		}
+
+	} 
+}
+
+// Constraint solving
+static void solveConstraint (SpuSolverConstraint& constraint, SpuSolverBody& bodyA, SpuSolverBody& bodyB)
+{
+	// All but D6 use worldspace normals, use same code
+	if (constraint.m_flags.m_useLinear)
+	{
+		if (constraint.m_constraintType == POINT2POINT_CONSTRAINT_TYPE ||
+			constraint.m_constraintType == HINGE_CONSTRAINT_TYPE ||
+			constraint.m_constraintType == CONETWIST_CONSTRAINT_TYPE)
+		{
+			btVector3 normal (0,0,0);
+
+			const btVector3& bias =constraint.m_linearBias;
+			const btVector3& jacInv =constraint.m_jacdiagABInv;
+
+			for (int i = 0; i < 3; ++i)
+			{
+				normal[i] = 1;
+
+				// Compute relative velocity
+				btVector3 vel1 = bodyA.m_linearVelocity + bodyA.m_angularVelocity.cross(constraint.m_relPos1);
+				btVector3 vel2 = bodyB.m_linearVelocity + bodyB.m_angularVelocity.cross(constraint.m_relPos2);
+				btVector3 vel = vel1 - vel2;
+
+				float relVelNormal = normal.dot(vel);
+
+				// Compute impulse
+				float impulse = (bias[i] - relVelNormal) * jacInv[i];
+
+				btVector3 impNormal = normal*impulse;
+
+				// Apply
+				if (bodyA.m_invertedMass > 0)
+				{
+					bodyA.m_linearVelocity += impNormal*bodyA.m_invertedMass;
+					bodyA.m_angularVelocity += bodyA.m_angularFactor * (bodyA.m_worldInvInertiaTensor * (btVector3(constraint.m_relPos1).cross(impNormal)));
+				}
+				if (bodyB.m_invertedMass > 0)
+				{
+					bodyB.m_linearVelocity -= impNormal*bodyB.m_invertedMass;
+					bodyB.m_angularVelocity -=  bodyB.m_angularFactor * (bodyB.m_worldInvInertiaTensor * (btVector3(constraint.m_relPos2).cross(impNormal)));
+				}
+
+				normal[i] = 0;
+			}
+		}
+		else
+		{
+			//D6
+		}
+
+	}
+
+	switch (constraint.m_constraintType)
+	{
+	case POINT2POINT_CONSTRAINT_TYPE:
+		break; // Nothing special to do
+	case HINGE_CONSTRAINT_TYPE:
+		{
+			// Angular solving for the two first axes
+			const btVector3& bias =constraint.hinge.m_angularBias;
+			const btVector3& jacInv =constraint.hinge.m_angJacdiagABInv;
+
+			for (int i = 0; i < 2; ++i)
+			{
+				const btVector3& axis =constraint.hinge.m_frameAinW[i];
+				
+				// Compute relative velocity
+				btVector3 relVel = bodyA.m_angularVelocity - bodyB.m_angularVelocity;
+
+				float relVelAxis = axis.dot(relVel);
+
+				// Compute impulse
+				float impulse = (bias[i] - relVelAxis) * jacInv[i];
+				btVector3 impAxis = axis*impulse;
+
+				// Apply
+				if (bodyA.m_invertedMass > 0)
+				{
+					bodyA.m_angularVelocity +=  bodyA.m_angularFactor * (bodyA.m_worldInvInertiaTensor * impAxis);
+				}
+				if (bodyB.m_invertedMass > 0)
+				{
+					bodyB.m_angularVelocity -= bodyB.m_angularFactor * (bodyB.m_worldInvInertiaTensor * impAxis);
+				}
+			}
+
+			// Limit
+			if (constraint.m_flags.m_limit1)
+			{
+				const btVector3& axis =constraint.hinge.m_frameAinW[2];
+
+				// Compute relative velocity
+				btVector3 relVel = bodyA.m_angularVelocity - bodyB.m_angularVelocity;
+				float relVelAxis = axis.dot(relVel);
+				
+				// Compute impulse
+				float impulse = (bias[2] - relVelAxis) * jacInv[2] * constraint.hinge.m_limitJacFactor;
+
+				// Clamp it
+				float temp = constraint.hinge.m_limitAccumulatedImpulse;
+				constraint.hinge.m_limitAccumulatedImpulse = btMax (constraint.hinge.m_limitAccumulatedImpulse + impulse, 0.0f);
+				impulse = constraint.hinge.m_limitAccumulatedImpulse - temp;
+
+				btVector3 impAxis = axis*impulse* (constraint.hinge.m_limitJacFactor/btFabs (constraint.hinge.m_limitJacFactor));
+
+				// Apply
+				if (bodyA.m_invertedMass > 0)
+				{
+					bodyA.m_angularVelocity += bodyA.m_angularFactor * (bodyA.m_worldInvInertiaTensor * impAxis);
+				}
+				if (bodyB.m_invertedMass > 0)
+				{
+					bodyB.m_angularVelocity -= bodyB.m_angularFactor * (bodyB.m_worldInvInertiaTensor * impAxis);
+				}				
+			}
+
+			// Motor
+			if (constraint.m_flags.m_motor1)
+			{
+				const btVector3& axis =constraint.hinge.m_frameAinW[2];
+
+				// Compute relative velocity
+				btVector3 relVel = bodyA.m_angularVelocity - bodyB.m_angularVelocity;
+				float relVelAxis = axis.dot(relVel);
+
+				// Compute impulse
+				float impulse = (constraint.hinge.m_motorVelocity - relVelAxis) * jacInv[2];
+
+				// Clamp it
+				float clampedImpulse = impulse > constraint.hinge.m_motorImpulse ? constraint.hinge.m_motorImpulse : impulse;
+				clampedImpulse = impulse < -constraint.hinge.m_motorImpulse ? -constraint.hinge.m_motorImpulse : clampedImpulse;
+				
+
+				btVector3 impAxis = axis*clampedImpulse;
+
+				// Apply
+				if (bodyA.m_invertedMass > 0)
+				{
+					bodyA.m_angularVelocity += bodyA.m_angularFactor * (bodyA.m_worldInvInertiaTensor * impAxis);
+				}
+				if (bodyB.m_invertedMass > 0)
+				{
+					bodyB.m_angularVelocity -= bodyB.m_angularFactor * (bodyB.m_worldInvInertiaTensor * impAxis);
+				}
+			}
+		}
+		break;
+	case CONETWIST_CONSTRAINT_TYPE:
+		{
+			// Swing
+			if (constraint.m_flags.m_limit1)
+			{
+				const btVector3& axis =constraint.conetwist.m_swingAxis;
+
+				// Compute relative velocity
+				btVector3 relVel = bodyA.m_angularVelocity - bodyB.m_angularVelocity;
+				float relVelAxis = axis.dot(relVel);
+
+				// Compute impulse
+				float impulse = (constraint.conetwist.m_swingError - relVelAxis) * constraint.conetwist.m_swingJacInv;
+
+				// Clamp it
+				float temp = constraint.conetwist.m_swingLimitImpulse;
+				constraint.conetwist.m_swingLimitImpulse = btMax (constraint.conetwist.m_swingLimitImpulse + impulse, 0.0f);
+				impulse = constraint.conetwist.m_swingLimitImpulse - temp;
+
+				btVector3 impAxis = axis*impulse;
+
+				// Apply
+				if (bodyA.m_invertedMass > 0)
+				{
+					bodyA.m_angularVelocity += bodyA.m_angularFactor * (bodyA.m_worldInvInertiaTensor * impAxis);
+				}
+				if (bodyB.m_invertedMass > 0)
+				{
+					bodyB.m_angularVelocity -= bodyB.m_angularFactor * (bodyB.m_worldInvInertiaTensor * impAxis);
+				}
+			}
+
+			// Twist
+			if (constraint.m_flags.m_limit2)
+			{
+				const btVector3& axis =constraint.conetwist.m_twistAxis;
+
+				// Compute relative velocity
+				btVector3 relVel = bodyA.m_angularVelocity - bodyB.m_angularVelocity;
+				float relVelAxis = axis.dot(relVel);
+
+				// Compute impulse
+				float impulse = (constraint.conetwist.m_twistError - relVelAxis) * constraint.conetwist.m_twistJacInv;
+
+				// Clamp it
+				float temp = constraint.conetwist.m_twistLimitImpulse;
+				constraint.conetwist.m_twistLimitImpulse = btMax (constraint.conetwist.m_twistLimitImpulse + impulse, 0.0f);
+				impulse = constraint.conetwist.m_twistLimitImpulse - temp;
+
+				btVector3 impAxis = axis*impulse;
+
+				// Apply
+				if (bodyA.m_invertedMass > 0)
+				{
+					bodyA.m_angularVelocity += bodyA.m_angularFactor * (bodyA.m_worldInvInertiaTensor * impAxis);
+				}
+				if (bodyB.m_invertedMass > 0)
+				{
+					bodyB.m_angularVelocity -= bodyB.m_angularFactor * (bodyB.m_worldInvInertiaTensor * impAxis);
+				}	
+			}
+		}
+		break;
+	default:
+		;
+	}
+}
+//-- SOLVER METHODS END
+
+
+
+
+
+
+
+
+//-- CONSTRAINT SETUP METHODS
+// Compute the jacobian inverse @@TODO: Optimize
+static float computeJacobianInverse (const btRigidBody* rb0, const btRigidBody* rb1,
+							  const btVector3& anchorAinW, const btVector3& anchorBinW, const btVector3& normal)
+{
+	float jacobian = rb0->computeImpulseDenominator(anchorAinW, normal);
+	jacobian += rb1->computeImpulseDenominator(anchorBinW, normal);
+
+	return 1.0f/jacobian;
+}
+
+static float computeAngularJacobianInverse (const btRigidBody* rb0, const btRigidBody* rb1,
+											const btVector3& normal)
+{
+	float jacobian = rb0->computeAngularImpulseDenominator(normal);
+	jacobian += rb1->computeAngularImpulseDenominator(normal);
+
+	return 1.0f/jacobian;
+}
+
+static void setupLinearConstraintWorld (SpuSolverConstraint& constraint, const btRigidBody* rb0, const btRigidBody* rb1,
+										const btVector3& anchorAinW, const btVector3& anchorBinW, const btContactSolverInfoData& solverInfo)
+{
+	btVector3 relPos1 = anchorAinW - rb0->getCenterOfMassPosition();
+	btVector3 relPos2 = anchorBinW - rb1->getCenterOfMassPosition();
+
+	constraint.m_relPos1 = relPos1;
+	constraint.m_relPos2 = relPos2;
+
+	btVector3 error = anchorAinW - anchorBinW;
+
+	// Setup the three axes
+	btVector3 normal (0,0,0);
+	btVector3 jacInv, bias;
+	const float errorFactor = solverInfo.m_tau / (solverInfo.m_timeStep * solverInfo.m_damping);
+
+	for (int i = 0; i < 3; ++i)
+	{
+		normal[i] = 1;
+
+		jacInv[i] = solverInfo.m_damping * computeJacobianInverse (rb0, rb1, anchorAinW, anchorBinW, normal);
+
+		// Compute the depth
+		float depth = -error[i]*errorFactor;
+		bias[i] = depth;
+
+		normal[i] = 0;
+	}
+
+	constraint.m_jacdiagABInv = jacInv;
+	constraint.m_linearBias = bias;
+	constraint.m_flags.m_useLinear = 1;
+}
+//-- CONSTRAINT SETUP METHODS END
+
+
+
+
+
+
+static int getConstraintSize (btTypedConstraintType type)
+{
+	switch (type)
+	{
+	case POINT2POINT_CONSTRAINT_TYPE:
+		return sizeof(btPoint2PointConstraint);
+	case HINGE_CONSTRAINT_TYPE:
+		return sizeof(btHingeConstraint);
+	case CONETWIST_CONSTRAINT_TYPE:
+		return sizeof(btConeTwistConstraint);
+	case D6_CONSTRAINT_TYPE:
+		return sizeof(btGeneric6DofConstraint);
+	default:
+		;
+		//btAssert(0);
+	}
+
+	return 0;
+}
+
+
+
+
+
+
+
+
+
+//-- MAIN METHOD
+void processSolverTask(void* userPtr, void* lsMemory)
+{
+//	BT_PROFILE("processSolverTask");
+
+	SolverTask_LocalStoreMemory* localMemory = (SolverTask_LocalStoreMemory*)lsMemory;
+
+	SpuSolverTaskDesc* taskDescPtr = (SpuSolverTaskDesc*)userPtr;
+	SpuSolverTaskDesc& taskDesc = *taskDescPtr;
+
+	setupTemporaryStorage(localMemory);
+	
+	switch (taskDesc.m_solverCommand)
+	{
+	case CMD_SOLVER_SETUP_BODIES:
+		{
+			int bodiesToProcess = taskDesc.m_commandData.m_bodySetup.m_numBodies;
+			int bodyPackageOffset = taskDesc.m_commandData.m_bodySetup.m_startBody;
+			const int bodiesPerPackage = 256;
+
+			btRigidBody** bodyPtrList = (btRigidBody**)allocTemporaryStorage(localMemory, bodiesPerPackage*sizeof(btRigidBody*));
+			btRigidBody* bodyList = (btRigidBody*)allocTemporaryStorage(localMemory, bodiesPerPackage*sizeof(btRigidBody));
+			SpuSolverBody* spuBodyList = allocBodyStorage(localMemory, bodiesPerPackage);
+
+
+			while (bodiesToProcess > 0)
+			{
+				const int packageSize = bodiesToProcess > bodiesPerPackage ? bodiesPerPackage : bodiesToProcess;
+
+				// DMA the body pointers
+				{
+					int dmaSize = sizeof(btRigidBody*)*packageSize;
+					uint64_t dmaPpuAddress2 = reinterpret_cast<uint64_t> (taskDesc.m_commandData.m_bodySetup.m_rbList + bodyPackageOffset);
+					cellDmaLargeGet(bodyPtrList, dmaPpuAddress2, dmaSize, DMA_TAG(1), 0, 0);
+					cellDmaWaitTagStatusAll(DMA_MASK(1));
+				}
+
+				int b;
+				// DMA the rigid bodies
+				for ( b = 0; b < packageSize; ++b)
+				{
+					btRigidBody* body = bodyPtrList[b];
+					int dmaSize = sizeof(btRigidBody);
+					uint64_t dmaPpuAddress2 = reinterpret_cast<uint64_t> (body);
+					cellDmaLargeGet(&bodyList[b], dmaPpuAddress2, dmaSize, DMA_TAG(1), 0, 0);					
+				}
+				cellDmaWaitTagStatusAll(DMA_MASK(1));
+
+				for ( b = 0; b < packageSize; ++b)
+				{					
+					btRigidBody* localBody = bodyList+b;
+					SpuSolverBody* spuBody = spuBodyList + b;
+					//Set it up solver body
+					setupSpuBody(localBody, spuBody);
+
+					int spuBodyIndex = bodyPackageOffset + b;
+					localBody->setCompanionId(spuBodyIndex);
+				}
+
+				// DMA the rigid bodies back
+				for ( b = 0; b < packageSize; ++b)
+				{
+					btRigidBody* body = bodyPtrList[b];
+					int dmaSize = sizeof(btRigidBody);
+					uint64_t dmaPpuAddress2 = reinterpret_cast<uint64_t> (body);
+					cellDmaLargePut(&bodyList[b], dmaPpuAddress2, dmaSize, DMA_TAG(1), 0, 0);					
+				}
+
+				// DMA the list of SPU bodies
+				{
+					int dmaSize = sizeof(SpuSolverBody)*packageSize;
+					uint64_t dmaPpuAddress2 = reinterpret_cast<uint64_t> (taskDesc.m_solverData.m_solverBodyList + bodyPackageOffset);
+					cellDmaLargePut(spuBodyList, dmaPpuAddress2, dmaSize, DMA_TAG(2), 0, 0);
+				}
+
+
+				cellDmaWaitTagStatusAll(DMA_MASK(1) | DMA_MASK(2));
+				bodiesToProcess -= packageSize;
+				bodyPackageOffset += packageSize;				
+			}
+
+		}
+		break;
+	case CMD_SOLVER_MANIFOLD_SETUP:
+		{			
+			// DMA the hash
+			{
+				int dmaSize = sizeof(SpuSolverHash);
+				uint64_t dmaPpuAddress2 = reinterpret_cast<uint64_t> (taskDesc.m_solverData.m_solverHash);
+				cellDmaLargeGet(&localMemory->m_localHash, dmaPpuAddress2, dmaSize, DMA_TAG(1), 0, 0);
+				cellDmaWaitTagStatusAll(DMA_MASK(1));
+			}
+
+			// Iterate over our cells
+			const int manifoldsPerPackage = 8;
+			const int constraintsPerPackage = 8;
+
+			ManifoldCellHolder* manifoldHolderList = (ManifoldCellHolder*)allocTemporaryStorage(localMemory, sizeof(ManifoldCellHolder)*manifoldsPerPackage);
+			btPersistentManifold* manifoldList = (btPersistentManifold*)allocTemporaryStorage(localMemory, sizeof(btPersistentManifold)*manifoldsPerPackage);			
+
+			ConstraintCellHolder* constraintHolderList = (ConstraintCellHolder*)allocTemporaryStorage(localMemory, sizeof(ConstraintCellHolder)*constraintsPerPackage);
+			uint8_t* constraintList = (uint8_t*)allocTemporaryStorage(localMemory, CONSTRAINT_MAX_SIZE*constraintsPerPackage);
+
+			uint32_t* indexArray = (uint32_t*)allocTemporaryStorage(localMemory, sizeof(uint32_t)*SPU_MAX_BODIES_PER_CELL);						
+
+			for (unsigned int c = 0; c < taskDesc.m_commandData.m_manifoldSetup.m_numCells; ++c)
+			{
+				int cellIdx = taskDesc.m_commandData.m_manifoldSetup.m_startCell + c;
+				SpuSolverHashCell& hashCell = localMemory->m_localHash.m_Hash[cellIdx];
+				
+				SpuIndexSet localRBs (indexArray);
+
+				{
+					int constraintIndex = hashCell.m_internalConstraintListOffset;
+					int manifoldsToProcess = hashCell.m_numManifolds;
+					int manifoldPackageOffset = hashCell.m_manifoldListOffset;								
+
+					while (manifoldsToProcess > 0)
+					{
+						const int packageSize = manifoldsToProcess > manifoldsPerPackage ? manifoldsPerPackage : manifoldsToProcess;
+
+						// DMA the holder list
+						{						
+							int dmaSize = sizeof(ManifoldCellHolder)*packageSize;						
+							uint64_t dmaPpuAddress2 = reinterpret_cast<uint64_t> (taskDesc.m_commandData.m_manifoldSetup.m_manifoldHolders + manifoldPackageOffset);
+							cellDmaLargeGet(manifoldHolderList, dmaPpuAddress2, dmaSize, DMA_TAG(1), 0, 0);
+							cellDmaWaitTagStatusAll(DMA_MASK(1));
+						}
+
+						int m;
+						// DMA the manifold list
+						for ( m = 0; m < packageSize; ++m)
+						{
+							int dmaSize = sizeof(btPersistentManifold);
+							uint64_t dmaPpuAddress2 = reinterpret_cast<uint64_t> (manifoldHolderList[m].m_manifold);
+							cellDmaLargeGet(manifoldList + m, dmaPpuAddress2, dmaSize, DMA_TAG(1), 0, 0);						
+						}
+						cellDmaWaitTagStatusAll(DMA_MASK(1));
+
+						for ( m = 0; m < packageSize; ++m)
+						{
+							btPersistentManifold* currManifold = manifoldList + m;
+
+							btRigidBody* rb0Ptr = (btRigidBody*)currManifold->getBody0();
+							btRigidBody* rb1Ptr = (btRigidBody*)currManifold->getBody1();
+
+							int numContacts = currManifold->getNumContacts();
+
+							if (!numContacts)
+							{
+								// No need to DMA anything more or so, so quit							
+								continue;
+							}
+
+							unsigned int solverBodyIdA = ~0, solverBodyIdB = ~0;
+
+							// DMA the bodies
+							{
+								int dmaSize = sizeof(btRigidBody);
+								uint64_t dmaPpuAddress2 = reinterpret_cast<uint64_t> (rb0Ptr);
+								cellDmaLargeGet(&localMemory->m_tempRBs[0], dmaPpuAddress2, dmaSize, DMA_TAG(1), 0, 0);
+							}
+							{
+								int dmaSize = sizeof(btRigidBody);
+								uint64_t dmaPpuAddress2 = reinterpret_cast<uint64_t> (rb1Ptr);
+								cellDmaLargeGet(&localMemory->m_tempRBs[1], dmaPpuAddress2, dmaSize, DMA_TAG(2), 0, 0);							
+							}
+							cellDmaWaitTagStatusAll(DMA_MASK(1) | DMA_MASK(2));
+
+
+							btRigidBody* rb0 = (btRigidBody*)&localMemory->m_tempRBs[0];
+							btRigidBody* rb1 = (btRigidBody*)&localMemory->m_tempRBs[1];
+
+							if (rb0->getIslandTag() >= 0)
+							{
+								solverBodyIdA = rb0->getCompanionId();
+							} 
+							else
+							{
+								//create a static body
+								solverBodyIdA = taskDesc.m_commandData.m_manifoldSetup.m_numBodies + hashCell.m_manifoldListOffset;
+								setupSpuBody(rb0, &localMemory->m_tempSPUBodies[0]);
+								{
+									int dmaSize = sizeof(SpuSolverBody);
+									uint64_t dmaPpuAddress2 = reinterpret_cast<uint64_t> (taskDesc.m_solverData.m_solverBodyList + solverBodyIdA);
+									cellDmaLargePut(&localMemory->m_tempSPUBodies[0], dmaPpuAddress2, dmaSize, DMA_TAG(1), 0, 0);
+									cellDmaWaitTagStatusAll(DMA_MASK(1));
+								}							
+							}
+
+							if (rb1->getIslandTag() >= 0)
+							{
+								solverBodyIdB = rb1->getCompanionId();
+							} 
+							else
+							{
+								//create a static body
+								solverBodyIdB = taskDesc.m_commandData.m_manifoldSetup.m_numBodies + hashCell.m_manifoldListOffset;					
+								setupSpuBody(rb1, &localMemory->m_tempSPUBodies[0]);
+								{
+									int dmaSize = sizeof(SpuSolverBody);
+									uint64_t dmaPpuAddress2 = reinterpret_cast<uint64_t> (taskDesc.m_solverData.m_solverBodyList + solverBodyIdB);
+									cellDmaLargePut(&localMemory->m_tempSPUBodies[0], dmaPpuAddress2, dmaSize, DMA_TAG(1), 0, 0);
+									cellDmaWaitTagStatusAll(DMA_MASK(1));
+								}
+							}
+
+							// Setup the pointer table
+							int offsA = localRBs.insert(solverBodyIdA);		
+							int offsB = localRBs.insert(solverBodyIdB);
+
+							// Setup all the contacts
+							for (int c = 0; c < numContacts; ++c)
+							{
+								btManifoldPoint& cp = currManifold->getContactPoint(c);
+
+								btVector3 pos1 = cp.getPositionWorldOnA();
+								btVector3 pos2 = cp.getPositionWorldOnB();
+
+								btVector3 rel_pos1 = pos1 - rb0->getCenterOfMassPosition(); 
+								btVector3 rel_pos2 = pos2 - rb1->getCenterOfMassPosition();
+
+								btScalar rel_vel;
+								btVector3 vel;
+
+								// De-penetration
+								{
+									SpuSolverInternalConstraint& constraint = localMemory->m_tempInternalConstr[0];
+
+									constraint.m_localOffsetBodyA = offsA;
+									constraint.m_localOffsetBodyB = offsB;
+
+									constraint.m_normal = cp.m_normalWorldOnB;
+									{
+										//can be optimized, the cross products are already calculated										
+										constraint.m_jacDiagABInv = computeJacobianInverse (rb0, rb1, pos1, pos2, cp.m_normalWorldOnB);
+									}
+
+									constraint.m_relpos1CrossNormal = rel_pos1.cross(cp.m_normalWorldOnB);
+									constraint.m_relpos2CrossNormal = rel_pos2.cross(cp.m_normalWorldOnB);
+
+									btVector3 vel1 = rb0->getVelocityInLocalPoint(rel_pos1);
+									btVector3 vel2 = rb1->getVelocityInLocalPoint(rel_pos2);
+
+									vel = vel1 - vel2;
+									rel_vel = cp.m_normalWorldOnB.dot(vel);
+
+									constraint.m_penetration = cp.getDistance();///btScalar(infoGlobal.m_numIterations);
+									constraint.m_friction = cp.m_combinedFriction;
+									float rest =  - rel_vel * cp.m_combinedRestitution;
+									if (rest <= btScalar(0.))
+									{
+										rest = 0.f;
+									};
+
+									btScalar penVel = -constraint.m_penetration/taskDesc.m_commandData.m_manifoldSetup.m_solverInfo.m_timeStep;
+									constraint.m_penetration *= 
+										-(taskDesc.m_commandData.m_manifoldSetup.m_solverInfo.m_erp/taskDesc.m_commandData.m_manifoldSetup.m_solverInfo.m_timeStep);
+								
+									if (rest > penVel)
+									{
+										constraint.m_penetration = btScalar(0.);
+									}
+									constraint.m_restitution = rest;
+									constraint.m_appliedImpulse = 0.f;
+
+
+									btVector3 torqueAxis0 = rel_pos1.cross(cp.m_normalWorldOnB);
+									constraint.m_angularComponentA = rb0->getInvInertiaTensorWorld()*torqueAxis0;
+									btVector3 torqueAxis1 = rel_pos2.cross(cp.m_normalWorldOnB);		
+									constraint.m_angularComponentB = rb1->getInvInertiaTensorWorld()*torqueAxis1;
+								}
+
+								// Friction
+
+								btVector3 frictionTangential0a, frictionTangential1b;
+
+								frictionTangential0a = vel - cp.m_normalWorldOnB * rel_vel;
+								btScalar lat_rel_vel = frictionTangential0a.length2();
+								if (lat_rel_vel > SIMD_EPSILON)//0.0f)
+								{
+									frictionTangential0a /= btSqrt(lat_rel_vel);
+									frictionTangential1b = frictionTangential0a.cross(cp.m_normalWorldOnB);
+								} else
+								{
+									btPlaneSpace1(cp.m_normalWorldOnB,frictionTangential0a,frictionTangential1b);
+								}
+
+								{
+									SpuSolverInternalConstraint& constraint = localMemory->m_tempInternalConstr[1];
+
+									constraint.m_normal = frictionTangential0a;
+
+									constraint.m_localOffsetBodyA = offsA;
+									constraint.m_localOffsetBodyB = offsB;
+
+									constraint.m_friction = cp.m_combinedFriction;
+
+									constraint.m_appliedImpulse = btScalar(0.);
+
+									constraint.m_jacDiagABInv = computeJacobianInverse (rb0, rb1, pos1, pos2, constraint.m_normal);
+
+									{
+										btVector3 ftorqueAxis0 = rel_pos1.cross(constraint.m_normal);
+										constraint.m_relpos1CrossNormal = ftorqueAxis0;
+										constraint.m_angularComponentA = rb0->getInvInertiaTensorWorld()*ftorqueAxis0;
+									}
+									{
+										btVector3 ftorqueAxis0 = rel_pos2.cross(constraint.m_normal);
+										constraint.m_relpos2CrossNormal = ftorqueAxis0;
+										constraint.m_angularComponentB = rb1->getInvInertiaTensorWorld()*ftorqueAxis0;
+									}
+								}
+
+								{
+									SpuSolverInternalConstraint& constraint = localMemory->m_tempInternalConstr[2];
+
+									constraint.m_normal = frictionTangential1b;
+
+									constraint.m_localOffsetBodyA = offsA;
+									constraint.m_localOffsetBodyB = offsB;
+
+									constraint.m_friction = cp.m_combinedFriction;
+
+									constraint.m_appliedImpulse = btScalar(0.);
+
+									constraint.m_jacDiagABInv = computeJacobianInverse (rb0, rb1, pos1, pos2, constraint.m_normal);
+
+									{
+										btVector3 ftorqueAxis0 = rel_pos1.cross(constraint.m_normal);
+										constraint.m_relpos1CrossNormal = ftorqueAxis0;
+										constraint.m_angularComponentA = rb0->getInvInertiaTensorWorld()*ftorqueAxis0;
+									}
+									{
+										btVector3 ftorqueAxis0 = rel_pos2.cross(constraint.m_normal);
+										constraint.m_relpos2CrossNormal = ftorqueAxis0;
+										constraint.m_angularComponentB = rb1->getInvInertiaTensorWorld()*ftorqueAxis0;
+									}
+								}
+
+								// DMA the three constraints
+								{
+									int dmaSize = sizeof(SpuSolverInternalConstraint)*3;
+									uint64_t dmaPpuAddress2 = reinterpret_cast<uint64_t> (taskDesc.m_solverData.m_solverInternalConstraintList + constraintIndex);
+									cellDmaLargePut(&localMemory->m_tempInternalConstr, dmaPpuAddress2, dmaSize, DMA_TAG(1), 0, 0);		
+									cellDmaWaitTagStatusAll(DMA_MASK(1));
+								}
+
+								constraintIndex += 3;
+
+							}
+
+						}
+
+						manifoldsToProcess -= packageSize;
+						manifoldPackageOffset += packageSize;
+					}
+				}
+				int numOutConstraints = 0;
+				// Setup constraints
+				{
+					const btContactSolverInfoData& solverInfo = taskDesc.m_commandData.m_manifoldSetup.m_solverInfo;
+
+					int constraintIndex = hashCell.m_constraintListOffset;
+					
+					int constraintsToProcess = hashCell.m_numConstraints;
+					int constraintPackageOffset = hashCell.m_constraintListOffset;
+
+					while (constraintsToProcess)
+					{
+						const int packageSize = constraintsToProcess > constraintsPerPackage ? constraintsPerPackage : constraintsToProcess;
+
+						// DMA the holder list
+						{
+							int dmaSize = sizeof(ConstraintCellHolder)*packageSize;						
+							uint64_t dmaPpuAddress2 = reinterpret_cast<uint64_t> (taskDesc.m_commandData.m_manifoldSetup.m_constraintHolders + constraintPackageOffset);
+							cellDmaLargeGet(constraintHolderList, dmaPpuAddress2, dmaSize, DMA_TAG(1), 0, 0);
+							cellDmaWaitTagStatusAll(DMA_MASK(1));
+						}
+
+						int c;
+						// DMA the constraint list
+						for ( c = 0; c < packageSize; ++c)
+						{
+							//int dmaSize = CONSTRAINT_MAX_SIZE;
+							int dmaSize = getConstraintSize((btTypedConstraintType)constraintHolderList[c].m_constraintType);
+							uint64_t dmaPpuAddress2 = reinterpret_cast<uint64_t> (constraintHolderList[c].m_constraint);
+							cellDmaLargeGet(constraintList + CONSTRAINT_MAX_SIZE*c, dmaPpuAddress2, dmaSize, DMA_TAG(1), 0, 0);
+						}
+						cellDmaWaitTagStatusAll(DMA_MASK(1));
+
+						for ( c = 0; c < packageSize; ++c)
+						{
+							btTypedConstraint* currConstraint = (btTypedConstraint*)(constraintList + CONSTRAINT_MAX_SIZE*c);
+							btTypedConstraintType type = currConstraint->getConstraintType();
+
+							btRigidBody* rb0Ptr = (btRigidBody*)&currConstraint->getRigidBodyA();
+							btRigidBody* rb1Ptr = (btRigidBody*)&currConstraint->getRigidBodyB();
+
+							// DMA the bodies
+							{
+								int dmaSize = sizeof(btRigidBody);
+								uint64_t dmaPpuAddress2 = reinterpret_cast<uint64_t> (rb0Ptr);
+								cellDmaLargeGet(&localMemory->m_tempRBs[0], dmaPpuAddress2, dmaSize, DMA_TAG(1), 0, 0);
+							}
+							{
+								int dmaSize = sizeof(btRigidBody);
+								uint64_t dmaPpuAddress2 = reinterpret_cast<uint64_t> (rb1Ptr);
+								cellDmaLargeGet(&localMemory->m_tempRBs[1], dmaPpuAddress2, dmaSize, DMA_TAG(2), 0, 0);							
+							}
+							cellDmaWaitTagStatusAll(DMA_MASK(1) | DMA_MASK(2));
+
+
+							btRigidBody* rb0 = (btRigidBody*)&localMemory->m_tempRBs[0];
+							btRigidBody* rb1 = (btRigidBody*)&localMemory->m_tempRBs[1];
+
+							unsigned int solverBodyIdA = ~0, solverBodyIdB = ~0;
+							if (rb0->getIslandTag() >= 0)
+							{
+								solverBodyIdA = rb0->getCompanionId();
+							} 
+							else
+							{
+								//create a static body
+								solverBodyIdA = taskDesc.m_commandData.m_manifoldSetup.m_numBodies + taskDesc.m_commandData.m_manifoldSetup.m_numBodies + 
+									hashCell.m_constraintListOffset;
+								setupSpuBody(rb0, &localMemory->m_tempSPUBodies[0]);
+								{
+									int dmaSize = sizeof(SpuSolverBody);
+									uint64_t dmaPpuAddress2 = reinterpret_cast<uint64_t> (taskDesc.m_solverData.m_solverBodyList + solverBodyIdA);
+									cellDmaLargePut(&localMemory->m_tempSPUBodies[0], dmaPpuAddress2, dmaSize, DMA_TAG(1), 0, 0);
+									cellDmaWaitTagStatusAll(DMA_MASK(1));
+								}							
+							}
+
+							if (rb1->getIslandTag() >= 0)
+							{
+								solverBodyIdB = rb1->getCompanionId();
+							} 
+							else
+							{
+								//create a static body
+								solverBodyIdB = taskDesc.m_commandData.m_manifoldSetup.m_numBodies + taskDesc.m_commandData.m_manifoldSetup.m_numManifolds + 
+									hashCell.m_constraintListOffset;					
+								setupSpuBody(rb1, &localMemory->m_tempSPUBodies[0]);
+								{
+									int dmaSize = sizeof(SpuSolverBody);
+									uint64_t dmaPpuAddress2 = reinterpret_cast<uint64_t> (taskDesc.m_solverData.m_solverBodyList + solverBodyIdB);
+									cellDmaLargePut(&localMemory->m_tempSPUBodies[0], dmaPpuAddress2, dmaSize, DMA_TAG(1), 0, 0);
+									cellDmaWaitTagStatusAll(DMA_MASK(1));
+								}							
+							}
+
+							// Setup the pointer table
+							int offsA = localRBs.insert(solverBodyIdA);		
+							int offsB = localRBs.insert(solverBodyIdB);
+
+							bool haveConstraint = false;
+
+							// Setup the constraint
+							switch (type)
+							{
+							case POINT2POINT_CONSTRAINT_TYPE:
+								{
+									SpuSolverConstraint& spuConstraint = localMemory->m_tempConstraint[0];
+									btPoint2PointConstraint* p2pC = (btPoint2PointConstraint*)currConstraint;
+
+									spuConstraint.m_localOffsetBodyA = offsA;
+									spuConstraint.m_localOffsetBodyB = offsB;
+									spuConstraint.m_constraintType = type;
+
+									// Compute the anchor positions
+									btVector3 pivotAinW = rb0->getCenterOfMassTransform()*p2pC->m_pivotInA;
+									btVector3 pivotBinW = rb1->getCenterOfMassTransform()*p2pC->m_pivotInB;
+	
+									setupLinearConstraintWorld(spuConstraint, rb0, rb1, pivotAinW, pivotBinW, solverInfo);
+
+									haveConstraint = true; //We have one constraint
+								}
+								break;
+							case HINGE_CONSTRAINT_TYPE:
+								{
+									SpuSolverConstraint& spuConstraint = localMemory->m_tempConstraint[0];
+									btHingeConstraint* hC = (btHingeConstraint*)currConstraint;
+
+									spuConstraint.m_localOffsetBodyA = offsA;
+									spuConstraint.m_localOffsetBodyB = offsB;
+									spuConstraint.m_constraintType = type;									
+
+									// Compute the transforms
+									btTransform frameAinW = rb0->getCenterOfMassTransform()*hC->m_rbAFrame;
+									btTransform frameBinW = rb1->getCenterOfMassTransform()*hC->m_rbBFrame;
+
+									// Setup the linear part
+									setupLinearConstraintWorld(spuConstraint, rb0, rb1, frameAinW.getOrigin(), frameBinW.getOrigin(), solverInfo);
+
+									// Setup angular part
+									btVector3 jacInv; 
+
+									// Setup the jacobian inverses
+									for (int i = 0; i < 3; ++i)
+									{
+										const btVector3 axisA = frameAinW.getBasis().getColumn(i);
+										const btVector3 axisB = frameBinW.getBasis().getColumn(i);
+										
+										spuConstraint.hinge.m_frameAinW[i] = axisA;
+										spuConstraint.hinge.m_frameBinW[i] = axisB;									
+
+										jacInv[i] = computeAngularJacobianInverse(rb0, rb1, axisA);
+									}
+
+									// Compute position error along the two secondary axes & limit
+									{
+										btVector3 angularBias (0,0,0);
+
+										const btVector3 axisA = frameAinW.getBasis().getColumn(2);
+										const btVector3 axisB = frameBinW.getBasis().getColumn(2); 
+										
+										btVector3 error = -axisA.cross(axisB) / solverInfo.m_timeStep;
+										
+										angularBias[0] = error.dot(frameAinW.getBasis().getColumn(0));
+										angularBias[1] = error.dot(frameAinW.getBasis().getColumn(1));
+
+										spuConstraint.m_flags.m_limit1 = 0;
+										
+										if (hC->m_lowerLimit < hC->m_upperLimit)
+										{
+											// Compute hinge axis
+											const btVector3& refAxis0 = frameAinW.getBasis().getColumn(0);
+											const btVector3& refAxis1 = frameAinW.getBasis().getColumn(1);
+											const btVector3& swingAxis = frameBinW.getBasis().getColumn(1);
+
+											float hingeAngle = btAtan2Fast(swingAxis.dot(refAxis0), swingAxis.dot(refAxis1));
+											float correction, sign;
+
+											spuConstraint.hinge.m_limitAccumulatedImpulse = 0;
+
+											if (hingeAngle <= hC->m_lowerLimit*hC->m_limitSoftness)
+											{
+												correction = (hC->m_lowerLimit - hingeAngle);
+												sign = 1.0f;
+												spuConstraint.m_flags.m_limit1 = 1;
+											} 
+											else if (hingeAngle >= hC->m_upperLimit*hC->m_limitSoftness)
+											{
+												correction = (hC->m_upperLimit - hingeAngle);
+												sign = -1.0f;
+												spuConstraint.m_flags.m_limit1 = 1;
+											}
+
+											angularBias[2] = correction * hC->m_biasFactor / (solverInfo.m_timeStep * hC->m_relaxationFactor);
+											spuConstraint.hinge.m_limitJacFactor = hC->m_relaxationFactor * sign;
+										}										
+
+										spuConstraint.hinge.m_angularBias = angularBias;
+									}
+									
+									// Setup motor
+									spuConstraint.m_flags.m_motor1 = 0;
+									if (hC->m_enableAngularMotor)
+									{
+										spuConstraint.m_flags.m_motor1 = 1;
+										spuConstraint.hinge.m_motorVelocity = hC->m_motorTargetVelocity;
+										spuConstraint.hinge.m_motorImpulse = hC->m_maxMotorImpulse;
+									}
+
+									spuConstraint.hinge.m_angJacdiagABInv = jacInv;
+
+									haveConstraint = true;
+								}
+								break;
+							case CONETWIST_CONSTRAINT_TYPE:
+								{
+									SpuSolverConstraint& spuConstraint = localMemory->m_tempConstraint[0];
+									btConeTwistConstraint* ctC = (btConeTwistConstraint*)currConstraint;
+
+									spuConstraint.m_localOffsetBodyA = offsA;
+									spuConstraint.m_localOffsetBodyB = offsB;
+									spuConstraint.m_constraintType = type;
+
+									// Compute the transforms
+									btTransform frameAinW = rb0->getCenterOfMassTransform()*ctC->m_rbAFrame;
+									btTransform frameBinW = rb1->getCenterOfMassTransform()*ctC->m_rbBFrame;
+
+									// Setup the linear part
+									setupLinearConstraintWorld(spuConstraint, rb0, rb1, frameAinW.getOrigin(), frameBinW.getOrigin(), solverInfo);
+
+									// Setup the swing limits
+									const btVector3& b1Axis1 = frameAinW.getBasis().getColumn(0);
+									const btVector3& b2Axis1 = frameBinW.getBasis().getColumn(0);
+									const btVector3& b1Axis2 = frameAinW.getBasis().getColumn(1);
+									const btVector3& b1Axis3 = frameAinW.getBasis().getColumn(2);
+
+									float swing1 = 0.0f, swing2 = 0.0f;
+
+									if (ctC->m_swingSpan1 >= 0.05f)
+									{
+										swing1 = btAtan2Fast(b2Axis1.dot(b1Axis2),b2Axis1.dot(b1Axis1));
+									}
+									if (ctC->m_swingSpan2 >= 0.05f)
+									{
+										swing2 = btAtan2Fast(b2Axis1.dot(b1Axis3),b2Axis1.dot(b1Axis1));
+									}
+
+									float rMaxAngle1Sq = 1.0f / (ctC->m_swingSpan1*ctC->m_swingSpan1);		
+									float rMaxAngle2Sq = 1.0f / (ctC->m_swingSpan2*ctC->m_swingSpan2);	
+									float ellipseAngle = btFabs(swing1)* rMaxAngle1Sq + btFabs(swing2) * rMaxAngle2Sq;
+
+									spuConstraint.m_flags.m_limit1 = 0;
+									spuConstraint.m_flags.m_limit2 = 0;
+
+									spuConstraint.conetwist.m_swingLimitImpulse = 0;
+									spuConstraint.conetwist.m_twistLimitImpulse = 0;
+
+									float relFactorSq = ctC->m_relaxationFactor*ctC->m_relaxationFactor;
+	
+									if (ellipseAngle > 1.0f)
+									{
+										spuConstraint.conetwist.m_swingError = ellipseAngle - 1.0f;
+										spuConstraint.conetwist.m_swingError *= ctC->m_biasFactor;
+										spuConstraint.conetwist.m_swingError /= solverInfo.m_timeStep * relFactorSq;
+
+										spuConstraint.m_flags.m_limit1 = 1;
+
+										btVector3 axis = b2Axis1.cross(b1Axis2* b2Axis1.dot(b1Axis2) + b1Axis3* b2Axis1.dot(b1Axis3));
+										axis.normalize();
+
+										float swingAxisSign = (b2Axis1.dot(b1Axis1) >= 0.0f) ? 1.0f : -1.0f;
+										axis *= swingAxisSign;
+
+										spuConstraint.conetwist.m_swingAxis = axis;
+
+										float jacobian = computeAngularJacobianInverse(rb0, rb1, axis);
+										spuConstraint.conetwist.m_swingJacInv = relFactorSq	 * jacobian;
+									}
+
+									// Setup twist limits
+									if (ctC->m_twistSpan >= 0.0f)										
+									{
+										const btVector3& b2Axis2 = frameBinW.getBasis().getColumn(1);
+
+										btQuaternion rotationArc = shortestArcQuat(b2Axis1,b1Axis1);
+										btVector3 TwistRef = quatRotate(rotationArc,b2Axis2); 
+										float twist = btAtan2Fast(TwistRef.dot(b1Axis3), TwistRef.dot(b1Axis2));
+
+										float lockedFreeFactor = (ctC->m_twistSpan > btScalar(0.05f)) ? ctC->m_limitSoftness : btScalar(0.);
+										if (twist <= -ctC->m_twistSpan*lockedFreeFactor)
+										{
+											spuConstraint.conetwist.m_twistError = -(twist + ctC->m_twistSpan);
+											spuConstraint.conetwist.m_twistError *= ctC->m_biasFactor;
+											spuConstraint.conetwist.m_twistError /= solverInfo.m_timeStep * relFactorSq;
+
+											spuConstraint.m_flags.m_limit2 = 1;
+
+											btVector3 axis = -(b1Axis1 + b2Axis1);
+											axis.normalize();
+											spuConstraint.conetwist.m_twistAxis = axis;
+
+											float jacobian = computeAngularJacobianInverse(rb0, rb1, axis);
+											spuConstraint.conetwist.m_twistJacInv = relFactorSq * jacobian;
+										}
+										else if (twist >= ctC->m_twistSpan*lockedFreeFactor)
+										{
+											spuConstraint.conetwist.m_twistError = twist - ctC->m_twistSpan;
+											spuConstraint.conetwist.m_twistError *= ctC->m_biasFactor;
+											spuConstraint.conetwist.m_twistError /= solverInfo.m_timeStep * relFactorSq;
+
+											spuConstraint.m_flags.m_limit2 = 1;
+
+											btVector3 axis = b1Axis1 + b2Axis1;
+											axis.normalize();
+											spuConstraint.conetwist.m_twistAxis = axis;
+
+											float jacobian = computeAngularJacobianInverse(rb0, rb1, axis);
+											spuConstraint.conetwist.m_twistJacInv = relFactorSq * jacobian;
+										}
+
+									}
+
+									haveConstraint = true; //We have one constraint
+								}
+								break;
+							default:
+								;
+							}
+							
+
+							if (haveConstraint)
+							{
+								//DMA it
+								int dmaSize = sizeof(SpuSolverConstraint);
+								uint64_t dmaPpuAddress2 = reinterpret_cast<uint64_t> (taskDesc.m_solverData.m_solverConstraintList + 
+									hashCell.m_constraintListOffset + numOutConstraints);
+								cellDmaLargePut(&localMemory->m_tempConstraint[0], dmaPpuAddress2, dmaSize, DMA_TAG(1), 0, 0);
+								cellDmaWaitTagStatusAll(DMA_MASK(1));
+
+								numOutConstraints++;
+							}
+
+						}
+
+						constraintsToProcess -= packageSize;
+						constraintPackageOffset += packageSize;
+					}
+				}
+
+				// Write back some data, if needed
+				if (localRBs.size() > 0)
+				{
+					{
+						// DMA the local body list
+						int dmaSize = sizeof(uint32_t)*localRBs.size();
+						uint64_t dmaPpuAddress2 = reinterpret_cast<uint64_t> (taskDesc.m_solverData.m_solverBodyOffsetList + hashCell.m_solverBodyOffsetListOffset);
+						cellDmaLargePut(indexArray, dmaPpuAddress2, dmaSize, DMA_TAG(1), 0, 0);								
+					}
+					hashCell.m_numLocalBodies = localRBs.size();
+					hashCell.m_numConstraints = numOutConstraints;
+					{
+						// DMA the hash cell
+						int dmaSize = sizeof(SpuSolverHashCell);
+						uint64_t dmaPpuAddress2 = reinterpret_cast<uint64_t> (taskDesc.m_solverData.m_solverHash);
+						dmaPpuAddress2 += offsetof(SpuSolverHash,m_Hash);
+						dmaPpuAddress2 += sizeof(SpuSolverHashCell) * cellIdx;
+
+						cellDmaLargePut(&hashCell, dmaPpuAddress2, dmaSize, DMA_TAG(2), 0, 0);
+					}
+					cellDmaWaitTagStatusAll(DMA_MASK(1) | DMA_MASK(2));
+				}				
+			}
+		}
+		break;
+	case CMD_SOLVER_SOLVE_ITERATE:
+		{			
+			// DMA the hash
+			{
+				int dmaSize = sizeof(SpuSolverHash);
+				uint64_t dmaPpuAddress2 = reinterpret_cast<uint64_t> (taskDesc.m_solverData.m_solverHash);
+				cellDmaLargeGet(&localMemory->m_localHash, dmaPpuAddress2, dmaSize, DMA_TAG(1), 0, 0);
+				cellDmaWaitTagStatusAll(DMA_MASK(1));
+			}
+			
+			btSpinlock hashLock (taskDesc.m_commandData.m_iterate.m_spinLockVar);			
+
+			unsigned int cellToProcess;
+			while (1)
+			{
+				cellToProcess = getNextFreeCell(localMemory, taskDesc, hashLock);
+
+				if (cellToProcess >= SPU_HASH_NUMCELLS)
+					break;
+
+				// Now process that one cell
+				SpuSolverHashCell& hashCell = localMemory->m_localHash.m_Hash[cellToProcess];
+				
+				if (hashCell.m_numContacts == 0 && hashCell.m_numConstraints == 0)
+					continue;
+
+				// DMA the local bodies and constraints
+
+				// Get the body list
+				uint32_t* indexList = (uint32_t*)allocTemporaryStorage(localMemory, sizeof(uint32_t)*hashCell.m_numLocalBodies);
+				SpuSolverBody* bodyList = allocBodyStorage(localMemory, hashCell.m_numLocalBodies);
+				int b;
+				{
+					int dmaSize = sizeof(uint32_t)*hashCell.m_numLocalBodies;
+					uint64_t dmaPpuAddress2 = reinterpret_cast<uint64_t> (taskDesc.m_solverData.m_solverBodyOffsetList + hashCell.m_solverBodyOffsetListOffset);
+					cellDmaLargeGet(indexList, dmaPpuAddress2, dmaSize, DMA_TAG(1), 0, 0);		
+					cellDmaWaitTagStatusAll(DMA_MASK(1));
+				}
+
+				// DMA the bodies
+				for ( b = 0; b < hashCell.m_numLocalBodies; ++b)
+				{
+					int dmaSize = sizeof(SpuSolverBody);
+					uint64_t dmaPpuAddress2 = reinterpret_cast<uint64_t> (taskDesc.m_solverData.m_solverBodyList + indexList[b]);
+					cellDmaLargeGet(bodyList+b, dmaPpuAddress2, dmaSize, DMA_TAG(1), 0, 0);						
+				}
+				cellDmaWaitTagStatusAll(DMA_MASK(1));
+
+				// Process the constraints in packets
+				if (hashCell.m_numConstraints)
+				{
+					const size_t maxConstraintsPerPacket = memTemporaryStorage(localMemory) / sizeof(SpuSolverConstraint);
+					size_t constraintsToProcess = hashCell.m_numConstraints;
+					size_t constraintListOffset = hashCell.m_constraintListOffset;
+
+					SpuSolverConstraint* constraints = allocConstraintStorage(localMemory, maxConstraintsPerPacket);
+
+					while (constraintsToProcess > 0)
+					{
+						size_t packetSize = constraintsToProcess > maxConstraintsPerPacket ? maxConstraintsPerPacket : constraintsToProcess;
+
+						// DMA the constraints
+						{
+							int dmaSize = sizeof(SpuSolverConstraint)*packetSize;
+							uint64_t dmaPpuAddress2 = reinterpret_cast<uint64_t> (taskDesc.m_solverData.m_solverConstraintList + constraintListOffset);
+							cellDmaLargeGet(constraints, dmaPpuAddress2, dmaSize, DMA_TAG(1), 0, 0);
+						}
+						cellDmaWaitTagStatusAll(DMA_MASK(1));
+
+
+						// Solve
+						for (int j = 0; j < packetSize; ++j)
+						{
+							SpuSolverConstraint& constraint = constraints[j];
+							SpuSolverBody& bodyA = bodyList[constraint.m_localOffsetBodyA];
+							SpuSolverBody& bodyB = bodyList[constraint.m_localOffsetBodyB];
+
+							solveConstraint(constraint, bodyA, bodyB);
+						}
+						
+						// Write back the constraints for accumulated stuff
+						{
+							int dmaSize = sizeof(SpuSolverConstraint)*packetSize;
+							uint64_t dmaPpuAddress2 = reinterpret_cast<uint64_t> (taskDesc.m_solverData.m_solverConstraintList + constraintListOffset);					
+							cellDmaLargePut(constraints, dmaPpuAddress2, dmaSize, DMA_TAG(1), 0, 0);
+						}
+						cellDmaWaitTagStatusAll(DMA_MASK(1));
+
+						constraintListOffset += packetSize;
+						constraintsToProcess -= packetSize;
+					}
+
+					freeConstraintStorage (localMemory, constraints, maxConstraintsPerPacket);
+				}
+
+				// Now process the contacts
+				if (hashCell.m_numContacts)
+				{
+					const size_t maxContactsPerPacket = memTemporaryStorage(localMemory) / (sizeof(SpuSolverInternalConstraint)*3);
+					size_t contactsToProcess = hashCell.m_numContacts;
+					size_t constraintListOffset = hashCell.m_internalConstraintListOffset;
+
+					SpuSolverInternalConstraint* internalConstraints = allocInternalConstraintStorage(localMemory, maxContactsPerPacket*3);
+
+					while (contactsToProcess > 0)
+					{
+						size_t packetSize = contactsToProcess > maxContactsPerPacket ? maxContactsPerPacket : contactsToProcess;
+
+						// DMA the constraints
+						{
+							int dmaSize = sizeof(SpuSolverInternalConstraint)*packetSize*3;
+							uint64_t dmaPpuAddress2 = reinterpret_cast<uint64_t> (taskDesc.m_solverData.m_solverInternalConstraintList + constraintListOffset);
+							cellDmaLargeGet(internalConstraints, dmaPpuAddress2, dmaSize, DMA_TAG(1), 0, 0);
+						}
+						cellDmaWaitTagStatusAll(DMA_MASK(1));
+
+
+						int j;
+						// Solve
+						for ( j = 0; j < packetSize*3; j += 3)
+						{
+							SpuSolverInternalConstraint& contact = internalConstraints[j];
+							SpuSolverBody& bodyA = bodyList[contact.m_localOffsetBodyA];
+							SpuSolverBody& bodyB = bodyList[contact.m_localOffsetBodyB];
+
+							solveContact(contact, bodyA, bodyB);
+						}
+
+						for ( j = 0; j < packetSize*3; j += 3)
+						{
+							SpuSolverInternalConstraint& contact = internalConstraints[j];
+							SpuSolverBody& bodyA = bodyList[contact.m_localOffsetBodyA];
+							SpuSolverBody& bodyB = bodyList[contact.m_localOffsetBodyB];
+
+							SpuSolverInternalConstraint& frictionConstraint1 = internalConstraints[j + 1];
+							solveFriction(frictionConstraint1, bodyA, bodyB, contact.m_appliedImpulse);
+
+							SpuSolverInternalConstraint& frictionConstraint2 = internalConstraints[j + 2];
+							solveFriction(frictionConstraint2, bodyA, bodyB, contact.m_appliedImpulse);
+						}
+
+
+						// Write back the constraints for accumulated stuff
+						{
+							int dmaSize = sizeof(SpuSolverInternalConstraint)*packetSize*3;
+							uint64_t dmaPpuAddress2 = reinterpret_cast<uint64_t> (taskDesc.m_solverData.m_solverInternalConstraintList + constraintListOffset);					
+							cellDmaLargePut(internalConstraints, dmaPpuAddress2, dmaSize, DMA_TAG(1), 0, 0);
+						}
+						cellDmaWaitTagStatusAll(DMA_MASK(1));
+
+						constraintListOffset += packetSize*3;
+						contactsToProcess -= packetSize;
+					}
+
+					freeInternalConstraintStorage (localMemory, internalConstraints, maxContactsPerPacket*3);
+				}
+
+								
+				// DMA the bodies back to main memory
+				for ( b = 0; b < hashCell.m_numLocalBodies; ++b)
+				{					
+					int dmaSize = sizeof(SpuSolverBody);
+					uint64_t dmaPpuAddress2 = reinterpret_cast<uint64_t> (taskDesc.m_solverData.m_solverBodyList + indexList[b]);
+					cellDmaLargePut(bodyList + b, dmaPpuAddress2, dmaSize, DMA_TAG(1), 0, 0);						
+				}
+				cellDmaWaitTagStatusAll(DMA_MASK(1));
+
+				freeBodyStorage(localMemory, bodyList, hashCell.m_numLocalBodies);
+				freeTemporaryStorage(localMemory, indexList, sizeof(uint32_t)*hashCell.m_numLocalBodies);
+
+			};
+		}
+		break;
+	case CMD_SOLVER_COPYBACK_BODIES:
+		{
+			int bodiesToProcess = taskDesc.m_commandData.m_bodyCopyback.m_numBodies;
+			int bodyPackageOffset = taskDesc.m_commandData.m_bodyCopyback.m_startBody;
+			const int bodiesPerPackage = 256;
+
+			btRigidBody** bodyPtrList = (btRigidBody**)allocTemporaryStorage(localMemory, bodiesPerPackage*sizeof(btRigidBody*));
+			btRigidBody* bodyList = (btRigidBody*)allocTemporaryStorage(localMemory, bodiesPerPackage*sizeof(btRigidBody));
+			SpuSolverBody* spuBodyList = allocBodyStorage(localMemory, bodiesPerPackage);
+
+			while (bodiesToProcess > 0)
+			{
+				const int packageSize = bodiesToProcess > bodiesPerPackage ? bodiesPerPackage : bodiesToProcess;
+
+				// DMA the body pointers
+				{
+					int dmaSize = sizeof(btRigidBody*)*packageSize;
+					uint64_t dmaPpuAddress2 = reinterpret_cast<uint64_t> (taskDesc.m_commandData.m_bodySetup.m_rbList + bodyPackageOffset);
+					cellDmaLargeGet(bodyPtrList, dmaPpuAddress2, dmaSize, DMA_TAG(1), 0, 0);
+					cellDmaWaitTagStatusAll(DMA_MASK(1));
+				}
+
+				int b;
+				// DMA the rigid bodies
+				for ( b = 0; b < packageSize; ++b)
+				{
+					btRigidBody* body = bodyPtrList[b];
+					int dmaSize = sizeof(btRigidBody);
+					uint64_t dmaPpuAddress2 = reinterpret_cast<uint64_t> (body);
+					cellDmaLargeGet(&bodyList[b], dmaPpuAddress2, dmaSize, DMA_TAG(1), 0, 0);					
+				}
+
+				// DMA the list of SPU bodies
+				{
+					int dmaSize = sizeof(SpuSolverBody)*packageSize;
+					uint64_t dmaPpuAddress2 = reinterpret_cast<uint64_t> (taskDesc.m_solverData.m_solverBodyList + bodyPackageOffset);
+					cellDmaLargeGet(spuBodyList, dmaPpuAddress2, dmaSize, DMA_TAG(2), 0, 0);
+				}
+				cellDmaWaitTagStatusAll(DMA_MASK(1) | DMA_MASK(2));
+
+
+				for ( b = 0; b < packageSize; ++b)
+				{
+					btRigidBody* localBody = bodyList + b;
+					SpuSolverBody* solverBody = spuBodyList + b;
+				
+					if (solverBody->m_invertedMass > 0)
+					{
+						localBody->setLinearVelocity(solverBody->m_linearVelocity);
+						localBody->setAngularVelocity(solverBody->m_angularVelocity);
+					}
+					localBody->setCompanionId(-1);
+				}
+
+				// DMA the rigid bodies
+				for ( b = 0; b < packageSize; ++b)
+				{
+					btRigidBody* body = bodyPtrList[b];
+					int dmaSize = sizeof(btRigidBody);
+					uint64_t dmaPpuAddress2 = reinterpret_cast<uint64_t> (body);
+					cellDmaLargePut(&bodyList[b], dmaPpuAddress2, dmaSize, DMA_TAG(1), 0, 0);					
+				}
+
+
+				bodiesToProcess -= packageSize;
+				bodyPackageOffset += packageSize;				
+			}
+
+		}
+		break;
+	default:
+		//.. nothing
+		;
+//		btAssert(0);
+	}
+}
Index: /code/branches/physics/src/bullet/BulletMultiThreaded/SpuSolverTask/SpuParallellSolverTask.h
===================================================================
--- /code/branches/physics/src/bullet/BulletMultiThreaded/SpuSolverTask/SpuParallellSolverTask.h	(revision 1966)
+++ /code/branches/physics/src/bullet/BulletMultiThreaded/SpuSolverTask/SpuParallellSolverTask.h	(revision 1966)
@@ -0,0 +1,279 @@
+/*
+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_PARALLELSOLVERTASK_H
+#define SPU_PARALLELSOLVERTASK_H
+
+#include "../PlatformDefinitions.h"
+#include "LinearMath/btScalar.h"
+#include "LinearMath/btVector3.h"
+#include "LinearMath/btMatrix3x3.h"
+#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h"
+#include "../SpuSync.h"
+#include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"
+#include "LinearMath/btAlignedAllocator.h"
+
+
+ATTRIBUTE_ALIGNED16(struct) ManifoldCellHolder
+{
+	BT_DECLARE_ALIGNED_ALLOCATOR();
+
+	uint32_t					m_hashCellIndex;		
+	class btPersistentManifold*	m_manifold;
+};
+
+ATTRIBUTE_ALIGNED16(struct) ConstraintCellHolder
+{
+	BT_DECLARE_ALIGNED_ALLOCATOR();
+
+	uint32_t					m_hashCellIndex;		
+	uint32_t					m_constraintType;
+	class btTypedConstraint*	m_constraint;
+};
+
+enum
+{
+	SPU_HASH_NUMCELLS = 128,
+	SPU_HASH_WORDWIDTH = sizeof(uint32_t)*8,
+	SPU_HASH_NUMCELLDWORDS = ((SPU_HASH_NUMCELLS + SPU_HASH_WORDWIDTH - 1) / SPU_HASH_WORDWIDTH),
+	SPU_HASH_NUMUNUSEDBITS = (SPU_HASH_NUMCELLDWORDS * SPU_HASH_WORDWIDTH) - SPU_HASH_NUMCELLS, 
+	SPU_HASH_PHYSSIZE = 4, //TODO: MAKE CONFIGURABLE
+
+	SPU_MAX_BODIES_PER_CELL = 1024,
+
+	SPU_MAX_SPUS = 6
+};
+
+enum
+{
+	CMD_SOLVER_SETUP_BODIES = 1,
+	CMD_SOLVER_MANIFOLD_SETUP = 2,
+	CMD_SOLVER_CONSTRAINT_SETUP = 3,
+	CMD_SOLVER_SOLVE_ITERATE = 4,
+	CMD_SOLVER_COPYBACK_BODIES = 5
+};
+
+struct SpuSolverHashCell
+{
+	uint16_t						m_numLocalBodies;
+	uint16_t						m_solverBodyOffsetListOffset;
+
+	uint16_t						m_numManifolds;
+	uint16_t						m_manifoldListOffset;
+
+	uint16_t						m_numContacts;
+	uint16_t						m_internalConstraintListOffset;
+
+	uint16_t						m_numConstraints;
+	uint16_t						m_constraintListOffset;
+};
+
+// Shared data structures
+struct SpuSolverHash
+{
+	// Dependency matrix
+	ATTRIBUTE_ALIGNED16(uint32_t m_dependencyMatrix[SPU_HASH_NUMCELLS][SPU_HASH_NUMCELLDWORDS]);
+	ATTRIBUTE_ALIGNED16(uint32_t m_currentMask[SPU_MAX_SPUS+1][SPU_HASH_NUMCELLDWORDS]);
+
+	// The hash itself
+	ATTRIBUTE_ALIGNED16(SpuSolverHashCell m_Hash[SPU_HASH_NUMCELLS]);
+
+	// Hash meta-data	
+};
+
+inline unsigned int spuHash(unsigned int k)  { return k*2654435769u; };
+inline unsigned int spuGetHashCellIndex(int x, int y, int z)
+{
+	//int n = 0x8da6b343 * x + 0xd8163841 * y + 0xcb1ab31f * z;
+
+	int n = x ^ spuHash(y ^ spuHash (z));
+
+	return ((unsigned int)n) & (SPU_HASH_NUMCELLS-1);
+}
+
+
+ATTRIBUTE_ALIGNED16(struct) SpuSolverBody
+{
+	BT_DECLARE_ALIGNED_ALLOCATOR();
+
+	btVector3			m_linearVelocity;
+	btVector3			m_angularVelocity;
+
+	btMatrix3x3			m_worldInvInertiaTensor;
+	btScalar            m_angularFactor;
+	btScalar				m_invertedMass;
+};
+
+ATTRIBUTE_ALIGNED16(struct) SpuSolverInternalConstraint
+{
+	BT_DECLARE_ALIGNED_ALLOCATOR();
+
+	uint32_t			m_localOffsetBodyA;
+	uint32_t			m_localOffsetBodyB;
+
+	btScalar				m_appliedImpulse;
+
+	btScalar				m_friction;
+	btScalar				m_restitution;
+	btScalar				m_jacDiagABInv;
+	btScalar				m_penetration;
+
+	btVector3			m_normal;
+
+	btVector3			m_relpos1CrossNormal;
+	btVector3			m_relpos2CrossNormal;
+	btVector3			m_angularComponentA;
+	btVector3			m_angularComponentB;
+};
+
+
+ATTRIBUTE_ALIGNED16(struct) SpuSolverConstraint
+{
+	BT_DECLARE_ALIGNED_ALLOCATOR();
+
+	uint16_t			m_localOffsetBodyA;
+	uint16_t			m_localOffsetBodyB;
+
+	uint16_t			m_constraintType;
+	struct 
+	{
+		uint16_t		m_useLinear : 1;
+		
+		uint16_t		m_limit1	: 1;
+		uint16_t		m_limit2	: 1;
+		uint16_t		m_limit3	: 1;
+		uint16_t		m_limit4	: 1;
+		uint16_t		m_limit5	: 1;
+		uint16_t		m_limit6	: 1;
+
+		uint16_t		m_motor1	: 1;
+		uint16_t		m_motor2	: 1;
+		uint16_t		m_motor3	: 1;
+		uint16_t		m_motor4	: 1;
+		uint16_t		m_motor5	: 1;
+		uint16_t		m_motor6	: 1;
+	}					m_flags;
+
+	// Linear parts, used by all constraints
+	btQuadWordStorage	m_relPos1;
+	btQuadWordStorage	m_relPos2;
+	btQuadWordStorage	m_jacdiagABInv;		//Jacobian inverse multiplied by gamma (damping) for each axis
+	btQuadWordStorage	m_linearBias;		//depth*tau/(dt*gamma) along each axis
+
+	// Joint-specific parts
+	union
+	{
+		struct 
+		{
+			btQuadWordStorage	m_frameAinW[3];
+			btQuadWordStorage	m_frameBinW[3];
+
+			// For angular
+			btQuadWordStorage	m_angJacdiagABInv;		//1/j 
+			btQuadWordStorage	m_angularBias;			//error/dt, in x/y.		limit error*bias factor / (dt * relaxation factor) in z
+			
+			// For limit
+			float				m_limitAccumulatedImpulse;
+			float				m_limitJacFactor;		//limitSign*relaxation factor
+
+			// For motor
+			float				m_motorVelocity;
+			float				m_motorImpulse;
+		} hinge;
+		
+		struct  
+		{
+			btQuadWordStorage	m_swingAxis;
+			btQuadWordStorage	m_twistAxis;
+
+			float				m_swingError;
+			float				m_swingJacInv;
+			float				m_swingLimitImpulse;
+
+			float				m_twistError;
+			float				m_twistJacInv;
+			float				m_twistLimitImpulse;
+		} conetwist;
+	};
+};
+
+
+ATTRIBUTE_ALIGNED16(struct) SpuSolverDataDesc
+{
+	BT_DECLARE_ALIGNED_ALLOCATOR();
+
+	SpuSolverHash*					m_solverHash;
+	SpuSolverBody*					m_solverBodyList;
+	SpuSolverInternalConstraint*	m_solverInternalConstraintList;
+	SpuSolverConstraint*			m_solverConstraintList;
+	uint32_t*						m_solverBodyOffsetList;
+};
+
+
+ATTRIBUTE_ALIGNED16(struct) SpuSolverTaskDesc
+{
+	BT_DECLARE_ALIGNED_ALLOCATOR();
+
+	uint32_t						m_solverCommand;
+	uint32_t						m_taskId;
+	SpuSolverDataDesc				m_solverData;
+
+	// command specific data
+	union
+	{
+		// Body setup
+		struct 
+		{
+			uint32_t				m_startBody;
+			uint32_t				m_numBodies;
+
+			class btRigidBody**		m_rbList;
+		} m_bodySetup, m_bodyCopyback;
+
+		struct 
+		{
+			uint32_t				m_startCell;
+			uint32_t				m_numCells;
+
+			uint32_t				m_numBodies;
+			uint32_t				m_numManifolds;
+
+			ManifoldCellHolder*		m_manifoldHolders;
+			ConstraintCellHolder*	m_constraintHolders;
+			btContactSolverInfoData	m_solverInfo;
+		} m_manifoldSetup;
+
+		struct  
+		{
+			btSpinlock::SpinVariable*	m_spinLockVar;
+		} m_iterate;
+	}								m_commandData;
+};
+
+void	processSolverTask(void* userPtr, void* lsMemory);
+void*	createSolverLocalStoreMemory();
+
+// Helper
+inline bool constraintTypeSupported(btTypedConstraintType type)
+{
+	return type == POINT2POINT_CONSTRAINT_TYPE ||
+		type == HINGE_CONSTRAINT_TYPE ||
+		type == CONETWIST_CONSTRAINT_TYPE ||
+		type == D6_CONSTRAINT_TYPE;
+}
+
+#endif
Index: /code/branches/physics/src/bullet/BulletMultiThreaded/SpuSync.h
===================================================================
--- /code/branches/physics/src/bullet/BulletMultiThreaded/SpuSync.h	(revision 1966)
+++ /code/branches/physics/src/bullet/BulletMultiThreaded/SpuSync.h	(revision 1966)
@@ -0,0 +1,146 @@
+/*
+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)
+	{}
+
+	void Init ()
+	{
+#ifndef __SPU__
+		//*spinVariable = 1;
+		cellSyncMutexInitialize(spinVariable);
+#endif
+	}
+
+
+
+	void Lock ()
+	{
+#ifdef __SPU__
+		// lock semaphore
+		/*while (cellAtomicTestAndDecr32(atomic_buf, (uint64_t)spinVariable) == 0) 
+		{
+
+		};*/
+		cellSyncMutexLock((uint64_t)spinVariable);
+#endif
+	}
+
+	void Unlock ()
+	{
+#ifdef __SPU__
+		//cellAtomicIncr32(atomic_buf, (uint64_t)spinVariable);
+		cellSyncMutexUnlock((uint64_t)spinVariable);
+#endif 
+	}
+
+
+private:
+	SpinVariable*	spinVariable;
+	ATTRIBUTE_ALIGNED128(uint32_t		atomic_buf[32]);
+};
+
+#else
+//create a dummy implementation (without any locking) useful for serial processing
+class btSpinlock
+{
+public:
+	typedef int  SpinVariable;
+
+	btSpinlock (SpinVariable* var)
+		: spinVariable (var)
+	{}
+
+	void Init ()
+	{
+	}
+
+	void Lock ()
+	{
+	}
+
+	void Unlock ()
+	{
+	}
+
+private:
+	SpinVariable* spinVariable;
+};
+
+
+#endif
+
+
+#endif
