Add EPA penetration depth solver support to bullet multithreaded.

Update SubSimplexConvexCast algorithm used in bullet multithreaded.
2008-05-06 00:44:18 +00:00
parent 25c5d0d57a
commit 3268cab6d7
15 changed files with 1079 additions and 51 deletions
--- a/Extras/BulletMultiThreaded/SpuCollisionTaskProcess.cpp
+++ b/Extras/BulletMultiThreaded/SpuCollisionTaskProcess.cpp
@@ -71,7 +71,7 @@ SpuCollisionTaskProcess::~SpuCollisionTaskProcess()



-void SpuCollisionTaskProcess::initialize2()
+void SpuCollisionTaskProcess::initialize2(bool useEpa)
 {

 #ifdef DEBUG_SPU_TASK_SCHEDULING
@@ -82,6 +82,7 @@ void SpuCollisionTaskProcess::initialize2()
 		m_workUnitTaskBuffers = (unsigned char *)btAlignedAlloc(MIDPHASE_WORKUNIT_TASK_SIZE*m_maxNumOutstandingTasks, 128);
 	}

+	
 	for (int i = 0; i < m_maxNumOutstandingTasks; i++)
 	{
 		m_taskBusy[i] = false;
@@ -90,6 +91,7 @@ void SpuCollisionTaskProcess::initialize2()
 	m_currentTask = 0;
 	m_currentPage = 0;
 	m_currentPageEntry = 0;
+	m_useEpa = useEpa;

 #ifdef DEBUG_SpuCollisionTaskProcess
 	m_initialized = true;
@@ -110,6 +112,8 @@ void SpuCollisionTaskProcess::issueTask2()


 	SpuGatherAndProcessPairsTaskDesc& taskDesc = m_spuGatherTaskDesc[m_currentTask];
+	taskDesc.m_useEpa = m_useEpa;
+
 	{
 		// send task description in event message
 		// no error checking here...
--- a/Extras/BulletMultiThreaded/SpuCollisionTaskProcess.h
+++ b/Extras/BulletMultiThreaded/SpuCollisionTaskProcess.h
@@ -111,6 +111,8 @@ class SpuCollisionTaskProcess
 	unsigned int   m_currentPage;
 	unsigned int   m_currentPageEntry;

+	bool m_useEpa;
+
 #ifdef DEBUG_SpuCollisionTaskProcess
 	bool m_initialized;
 #endif
@@ -123,7 +125,7 @@ public:
 	~SpuCollisionTaskProcess();
 	
 	///call initialize in the beginning of the frame, before addCollisionPairToTask
-	void initialize2();
+	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);
--- a/Extras/BulletMultiThreaded/SpuGatheringCollisionDispatcher.cpp
+++ b/Extras/BulletMultiThreaded/SpuGatheringCollisionDispatcher.cpp
@@ -149,7 +149,7 @@ void	SpuGatheringCollisionDispatcher::dispatchAllCollisionPairs(btOverlappingPai
 		if (!m_spuCollisionTaskProcess)
 			m_spuCollisionTaskProcess = new SpuCollisionTaskProcess(m_threadInterface,m_maxNumOutstandingTasks);
 	
-		m_spuCollisionTaskProcess->initialize2();
+		m_spuCollisionTaskProcess->initialize2(dispatchInfo.m_useEpa);
 	
 		///modified version of btCollisionDispatcher::dispatchAllCollisionPairs:
 		{
--- a/Extras/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuCollisionShapes.cpp
+++ b/Extras/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuCollisionShapes.cpp
@@ -16,7 +16,7 @@ subject to the following restrictions:

 #include "SpuCollisionShapes.h"

-btPoint3 localGetSupportingVertexWithoutMargin(int shapeType, void* shape, btVector3& localDir,struct	SpuConvexPolyhedronVertexData* convexVertexData)//, int *featureIndex)
+btPoint3 localGetSupportingVertexWithoutMargin(int shapeType, void* shape, const btVector3& localDir,struct	SpuConvexPolyhedronVertexData* convexVertexData)//, int *featureIndex)
 {
    switch (shapeType)
    {
@@ -250,6 +250,12 @@ void computeAabb (btVector3& aabbMin, btVector3& aabbMax, btConvexInternalShape*
 		//add the radius to y-axis to get full height
 		btScalar radius = halfExtents[0];
 		halfExtents[1] += radius;
+#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();
@@ -298,7 +304,12 @@ void dmaBvhShapeData (bvhMeshShape_LocalStoreMemory* bvhMeshShape, btBvhTriangle
 	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));
 	
--- a/Extras/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuCollisionShapes.h
+++ b/Extras/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuCollisionShapes.h
@@ -63,8 +63,7 @@ struct bvhMeshShape_LocalStoreMemory
 };


-
-btPoint3 localGetSupportingVertexWithoutMargin(int shapeType, void* shape, btVector3& localDir,struct	SpuConvexPolyhedronVertexData* convexVertexData);//, int *featureIndex)
+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);
--- a/Extras/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuContactResult.h
+++ b/Extras/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuContactResult.h
@@ -50,6 +50,7 @@ struct SpuCollisionPairInput
 	btTransform m_worldTransform1;
 	
 	bool	m_isSwapped;
+	bool    m_useEpa;
 };


--- a/Extras/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuEpaPenetrationDepthSolver.cpp
+++ b/Extras/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuEpaPenetrationDepthSolver.cpp
@@ -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;
+}
--- a/Extras/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuEpaPenetrationDepthSolver.h
+++ b/Extras/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuEpaPenetrationDepthSolver.h
@@ -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
+
--- a/Extras/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuGatheringCollisionTask.cpp
+++ b/Extras/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuGatheringCollisionTask.cpp
@@ -23,6 +23,7 @@
 #include "BulletCollision/CollisionShapes/btCompoundShape.h"

 #include "SpuMinkowskiPenetrationDepthSolver.h"
+#include "SpuEpaPenetrationDepthSolver.h"
 #include "SpuGjkPairDetector.h"
 #include "SpuVoronoiSimplexSolver.h"

@@ -82,6 +83,7 @@ int g_CacheHits=0;

 #endif // USE_SOFTWARE_CACHE

+bool gUseEpa = false;

 #ifdef USE_SN_TUNER
 #include <LibSN_SPU.h>
@@ -196,18 +198,30 @@ SIMD_FORCE_INLINE void small_cache_read_triple(	void* ls0, ppu_address_t ea0,
 		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) );
@@ -458,7 +472,17 @@ void	ProcessSpuConvexConvexCollision(SpuCollisionPairInput* wuInput, CollisionTa
 		//try generic GJK

 		SpuVoronoiSimplexSolver vsSolver;
-		SpuMinkowskiPenetrationDepthSolver	penetrationSolver;
+		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)]);
@@ -537,7 +561,7 @@ void	ProcessSpuConvexConvexCollision(SpuCollisionPairInput* wuInput, CollisionTa
 			lsMemPtr->getColObj0()->getFriction(),lsMemPtr->getColObj1()->getFriction(),
 			wuInput->m_isSwapped);

-		SpuGjkPairDetector gjk(shape0Ptr,shape1Ptr,shapeType0,shapeType1,marginA,marginB,&vsSolver,&penetrationSolver);
+		SpuGjkPairDetector gjk(shape0Ptr,shape1Ptr,shapeType0,shapeType1,marginA,marginB,&vsSolver,penetrationSolver);
 		gjk.getClosestPoints(cpInput,spuContacts);//,debugDraw);
 	}

@@ -785,6 +809,8 @@ void	processCollisionTask(void* userPtr, void* lsMemPtr)
 	CollisionTask_LocalStoreMemory*	colMemPtr = (CollisionTask_LocalStoreMemory*)lsMemPtr;
 	CollisionTask_LocalStoreMemory& lsMem = *(colMemPtr);

+	gUseEpa = taskDesc.m_useEpa;
+
 	//	spu_printf("taskDescPtr=%llx\n",taskDescPtr);

 	SpuContactResult spuContacts;
--- a/Extras/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuGatheringCollisionTask.h
+++ b/Extras/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuGatheringCollisionTask.h
@@ -33,6 +33,7 @@ struct SpuGatherAndProcessPairsTaskDesc

 	uint16_t numPages;
 	uint16_t taskId;
+	bool m_useEpa;

 	struct	CollisionTask_LocalStoreMemory*	m_lsMemory; 
 }
--- a/Extras/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuGjkEpa2.cpp
+++ b/Extras/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuGjkEpa2.cpp
@@ -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_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];
+		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_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
--- a/Extras/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuGjkEpa2.h
+++ b/Extras/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuGjkEpa2.h
@@ -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
--- a/Extras/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuMinkowskiPenetrationDepthSolver.cpp
+++ b/Extras/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuMinkowskiPenetrationDepthSolver.cpp
@@ -271,7 +271,7 @@ bool SpuMinkowskiPenetrationDepthSolver::calcPenDepth( SpuVoronoiSimplexSolver&
 	if (minProj < btScalar(0.))
 		return false;

-	minProj += (marginA + marginB);
+	minProj += (marginA + marginB) + btScalar(1.00);



@@ -336,6 +336,9 @@ bool SpuMinkowskiPenetrationDepthSolver::calcPenDepth( SpuVoronoiSimplexSolver&
 #endif//DEBUG_DRAW


+	} else {
+		// could not seperate shapes
+		btAssert (false);
 	}
 	return res.m_hasResult;
 }
--- a/Extras/BulletMultiThreaded/SpuRaycastTask/SpuRaycastTask.cpp
+++ b/Extras/BulletMultiThreaded/SpuRaycastTask/SpuRaycastTask.cpp
@@ -62,7 +62,6 @@ void GatherCollisionObjectAndShapeData (RaycastGatheredObjectData* gatheredObjec
 {
 	register int dmaSize;
 	register ppu_address_t	dmaPpuAddress2;
-
 	/* DMA Collision object wrapper into local store */
 	dmaSize = sizeof(SpuCollisionObjectWrapper);
 	dmaPpuAddress2 = objectWrapper;
@@ -92,6 +91,7 @@ void GatherCollisionObjectAndShapeData (RaycastGatheredObjectData* gatheredObjec
 	} else {
 		gatheredObjectData->m_primitiveDimensions = btVector3(1.0, 1.0, 1.0);
 	}
+
 }

 void dmaLoadRayOutput (ppu_address_t rayOutputAddr, SpuRaycastTaskWorkUnitOut* rayOutput, uint32_t dmaTag)
@@ -506,6 +506,7 @@ void performRaycastAgainstConcave (RaycastGatheredObjectData* gatheredObjectData
 	register int dmaSize;
 	register ppu_address_t	dmaPpuAddress2;

+	
 	btBvhTriangleMeshShape*	trimeshShape = (btBvhTriangleMeshShape*)gatheredObjectData->m_spuCollisionShape;

 	//need the mesh interface, for access to triangle vertices
@@ -620,6 +621,7 @@ void performRaycastAgainstConcave (RaycastGatheredObjectData* gatheredObjectData

 		//pre-fetch first tree, then loop and double buffer
 	}
+	
 }

 void performRaycastAgainstCompound (RaycastGatheredObjectData* gatheredObjectData, const SpuRaycastTaskWorkUnit& workUnit, SpuRaycastTaskWorkUnitOut* workUnitOut, RaycastTask_LocalStoreMemory* lsMemPtr)
@@ -677,9 +679,11 @@ void	processRaycastTask(void* userPtr, void* lsMemory)

 	SpuCollisionObjectWrapper* cows = (SpuCollisionObjectWrapper*)taskDesc.spuCollisionObjectsWrappers;

+	//spu_printf("in processRaycastTask %d\n", taskDesc.numSpuCollisionObjectWrappers);
 	/* for each object */
 	for (int objectId = 0; objectId < taskDesc.numSpuCollisionObjectWrappers; objectId++)
 	{
+		//spu_printf("%d / %d\n", objectId, taskDesc.numSpuCollisionObjectWrappers);
 		RaycastGatheredObjectData gatheredObjectData;
 		/* load initial collision shape */
 		GatherCollisionObjectAndShapeData (&gatheredObjectData, localMemory, (ppu_address_t)&cows[objectId]);
@@ -777,4 +781,4 @@ void	processRaycastTask(void* userPtr, void* lsMemory)
 			}
 		}
 	}
-}
+}
--- a/Extras/BulletMultiThreaded/SpuRaycastTask/SpuSubSimplexConvexCast.cpp
+++ b/Extras/BulletMultiThreaded/SpuRaycastTask/SpuSubSimplexConvexCast.cpp
@@ -39,12 +39,26 @@ SpuSubsimplexRayCast::SpuSubsimplexRayCast (void* shapeB, SpuConvexPolyhedronVer
 * MSUM(Pellet, ConvexShape)
 *
 */
-btVector3 supportPoint (btTransform xform, int shapeType, const void* shape, SpuConvexPolyhedronVertexData* convexVertexData, btVector3 seperatingAxis)
+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 SupportPellet = btVector3(0.0, 0.0, 0.0);
-	btVector3 rotatedSeperatingAxis = seperatingAxis * xform.getBasis();
-	btVector3 SupportShape = xform(localGetSupportingVertexWithoutMargin(shapeType, (void*)shape, rotatedSeperatingAxis, convexVertexData));
-	return SupportPellet + SupportShape;
+	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,
@@ -53,56 +67,52 @@ bool	SpuSubsimplexRayCast::calcTimeOfImpact(const btTransform& fromRay,
 											   const btTransform& toB,
 											   SpuCastResult& result)
 {
-	btTransform	rayFromLocalA;
-	btTransform	rayToLocalA;
-
-	rayFromLocalA = fromRay.inverse()* fromB;
-	rayToLocalA = toRay.inverse()* toB;
-
 	m_simplexSolver->reset();
-	
-	btTransform bXform = btTransform(rayFromLocalA.getBasis());

-	//btScalar radius = btScalar(0.01);
+	btVector3 linVelRay, linVelB;
+	linVelRay = toRay.getOrigin() - fromRay.getOrigin();
+	linVelB = toB.getOrigin() - fromB.getOrigin ();

 	btScalar lambda = btScalar(0.);
-	//todo: need to verify this:
-	//because of minkowski difference, we need the inverse direction
 	
-	btVector3 s = -rayFromLocalA.getOrigin();
-	btVector3 r = -(rayToLocalA.getOrigin()-rayFromLocalA.getOrigin());
-	btVector3 x = s;
-	btVector3 v;
-	btVector3 arbitraryPoint = supportPoint(bXform, m_shapeTypeB, m_shapeB, m_convexDataB, r);
-	v = x - arbitraryPoint;
+	btTransform interpolatedTransRay = fromRay;
+	btTransform interpolatedTransB = fromB;

-	int maxIter = MAX_ITERATIONS;
+	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.));
+	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;
+	btVector3 w,p;
 	btScalar VdotR;
 	
 	while ( (dist2 > epsilon) && maxIter--)
 	{
-		p = supportPoint(bXform, m_shapeTypeB, m_shapeB, m_convexDataB, v);
-		btVector3 supportunit = v.normalized();
-		p += m_marginB * supportunit;
-		w = x - p;
+		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);
@@ -112,16 +122,14 @@ bool	SpuSubsimplexRayCast::calcTimeOfImpact(const btTransform& fromRay,
 			else
 			{
 				lambda = lambda - VdotW / VdotR;
-				x = s + lambda * r;
-				m_simplexSolver->reset();
-				//check next line
-				w = x-p;
+				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, x , p);
+		m_simplexSolver->addVertex(w, supVertexRay, supVertexB);
 		if (m_simplexSolver->closest(v))
 		{
 			dist2 = v.length2();
@@ -135,13 +143,10 @@ bool	SpuSubsimplexRayCast::calcTimeOfImpact(const btTransform& fromRay,
 		} 
 	}

-	//int numiter = MAX_ITERATIONS - maxIter;
-//	printf("number of iterations: %d", numiter);
 	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;
 }
-
-
-