add gjk/epa (host only), possibly improve convex-convex with many edge-edge tests

more preparation towards persistent/incremental contact cache
2013-07-31 23:22:43 -07:00
parent 7992ff816b
commit 34de49d8a4
24 changed files with 3020 additions and 118 deletions
--- a/Demos3/GpuDemos/rigidbody/GpuConvexScene.h
+++ b/Demos3/GpuDemos/rigidbody/GpuConvexScene.h
@@ -72,7 +72,7 @@ public:
 	virtual ~GpuBoxPlaneScene(){}
 	virtual const char* getName()
 	{
-		return "BoxPlane";
+		return "BoxBox";
 	}

 	static GpuDemo* MyCreateFunc()
--- a/src/Bullet3Collision/BroadPhaseCollision/b3OverlappingPair.h
+++ b/src/Bullet3Collision/BroadPhaseCollision/b3OverlappingPair.h
@@ -18,6 +18,9 @@ subject to the following restrictions:

 #include "Bullet3Common/b3Int4.h"

+#define B3_NEW_PAIR_MARKER -1
+#define B3_REMOVED_PAIR_MARKER -2
+
 //typedef b3Int2 b3BroadphasePair;
 struct b3BroadphasePair : public b3Int4
 {
@@ -34,6 +37,8 @@ struct b3BroadphasePair : public b3Int4
 			x = yy;
            y = xx;
        }
+		z = B3_NEW_PAIR_MARKER;
+		w = B3_NEW_PAIR_MARKER;
 	}
 };

--- a/src/Bullet3Common/b3Vector3.h
+++ b/src/Bullet3Common/b3Vector3.h
@@ -992,7 +992,7 @@ B3_FORCE_INLINE   long    b3Vector3::maxDot( const b3Vector3 *array, long array_
 #if defined (B3_USE_SSE) || defined (B3_USE_NEON)
    #if defined _WIN32 || defined (B3_USE_SSE)
        const long scalar_cutoff = 10;
-        long _maxdot_large( const float *array, const float *vec, unsigned long array_count, float *dotOut );
+        long b3_maxdot_large( const float *array, const float *vec, unsigned long array_count, float *dotOut );
    #elif defined B3_USE_NEON
        const long scalar_cutoff = 4;
        extern long (*_maxdot_large)( const float *array, const float *vec, unsigned long array_count, float *dotOut );
@@ -1020,7 +1020,7 @@ B3_FORCE_INLINE   long    b3Vector3::maxDot( const b3Vector3 *array, long array_
        return ptIndex;
    }
 #if defined (B3_USE_SSE) || defined (B3_USE_NEON)
-    return _maxdot_large( (float*) array, (float*) &m_floats[0], array_count, &dotOut );
+    return b3_maxdot_large( (float*) array, (float*) &m_floats[0], array_count, &dotOut );
 #endif
 }

@@ -1029,10 +1029,10 @@ B3_FORCE_INLINE   long    b3Vector3::minDot( const b3Vector3 *array, long array_
 #if defined (B3_USE_SSE) || defined (B3_USE_NEON)
    #if defined B3_USE_SSE
        const long scalar_cutoff = 10;
-        long _mindot_large( const float *array, const float *vec, unsigned long array_count, float *dotOut );
+        long b3_mindot_large( const float *array, const float *vec, unsigned long array_count, float *dotOut );
    #elif defined B3_USE_NEON
        const long scalar_cutoff = 4;
-        extern long (*_mindot_large)( const float *array, const float *vec, unsigned long array_count, float *dotOut );
+        extern long (*b3_mindot_large)( const float *array, const float *vec, unsigned long array_count, float *dotOut );
    #else
        #error unhandled arch!
    #endif
@@ -1060,7 +1060,7 @@ B3_FORCE_INLINE   long    b3Vector3::minDot( const b3Vector3 *array, long array_
        return ptIndex;
    }
 #if defined (B3_USE_SSE) || defined (B3_USE_NEON)
-    return _mindot_large( (float*) array, (float*) &m_floats[0], array_count, &dotOut );
+    return b3_mindot_large( (float*) array, (float*) &m_floats[0], array_count, &dotOut );
 #endif
 }

--- a/src/Bullet3OpenCL/BroadphaseCollision/kernels/sap.cl
+++ b/src/Bullet3OpenCL/BroadphaseCollision/kernels/sap.cl
@@ -13,6 +13,7 @@ subject to the following restrictions:
 */
 //Originally written by Erwin Coumans

+#define NEW_PAIR_MARKER -1

 typedef struct 
 {
@@ -87,6 +88,9 @@ __kernel void   computePairsKernelTwoArrays( __global const btAabbCL* unsortedAa
 		
 		myPair.x = xIndex;
 		myPair.y = yIndex;
+		myPair.z = NEW_PAIR_MARKER;
+		myPair.w = NEW_PAIR_MARKER;
+

 		int curPair = atomic_inc (pairCount);
 		if (curPair<maxPairs)
@@ -112,7 +116,9 @@ __kernel void   computePairsKernelOriginal( __global const btAabbCL* aabbs, vola
 			int4 myPair;
 			myPair.x = aabbs[i].m_minIndices[3];
 			myPair.y = aabbs[j].m_minIndices[3];
-			
+			myPair.z = NEW_PAIR_MARKER;
+			myPair.w = NEW_PAIR_MARKER;
+
 			int curPair = atomic_inc (pairCount);
 			if (curPair<maxPairs)
 			{
@@ -176,6 +182,9 @@ __kernel void   computePairsKernelBarrier( __global const btAabbCL* aabbs, volat
 				int4 myPair;
 				myPair.x = aabbs[i].m_minIndices[3];
 				myPair.y = aabbs[j].m_minIndices[3];
+				myPair.z = NEW_PAIR_MARKER;
+				myPair.w = NEW_PAIR_MARKER;
+
 				int curPair = atomic_inc (pairCount);
 				if (curPair<maxPairs)
 				{
@@ -251,6 +260,9 @@ __kernel void   computePairsKernelLocalSharedMemory( __global const btAabbCL* aa
 				int4 myPair;
 				myPair.x = myAabb.m_minIndices[3];
 				myPair.y = localAabbs[localCount+localId+1].m_minIndices[3];
+				myPair.z = NEW_PAIR_MARKER;
+				myPair.w = NEW_PAIR_MARKER;
+
 				int curPair = atomic_inc (pairCount);
 				if (curPair<maxPairs)
 				{
--- a/src/Bullet3OpenCL/BroadphaseCollision/kernels/sapFast.cl
+++ b/src/Bullet3OpenCL/BroadphaseCollision/kernels/sapFast.cl
@@ -13,6 +13,8 @@ subject to the following restrictions:
 */
 //Originally written by Erwin Coumans

+#define NEW_PAIR_MARKER -1
+#define REMOVED_PAIR_MARKER -2

 typedef struct 
 {
@@ -177,9 +179,11 @@ __kernel void   computePairsIncremental3dSapKernel( __global const uint2* object
 										int curPair = atomic_inc(addedHostPairsCount);
 										if (curPair<maxCapacity)
 										{
-											
 											addedHostPairsGPU[curPair].x = newPair.x;
 											addedHostPairsGPU[curPair].y = newPair.y;
+											addedHostPairsGPU[curPair].z = NEW_PAIR_MARKER;
+											addedHostPairsGPU[curPair].w = NEW_PAIR_MARKER;
+
 										}
 									}

@@ -208,6 +212,8 @@ __kernel void   computePairsIncremental3dSapKernel( __global const uint2* object
 											
 											removedHostPairsGPU[curPair].x = removedPair.x;
 											removedHostPairsGPU[curPair].y = removedPair.y;
+											removedHostPairsGPU[curPair].z = REMOVED_PAIR_MARKER;
+											removedHostPairsGPU[curPair].w = REMOVED_PAIR_MARKER;

 										}
 									}
@@ -273,6 +279,9 @@ __kernel void   computePairsIncremental3dSapKernel( __global const uint2* object
 											
 											addedHostPairsGPU[curPair].x = newPair.x;
 											addedHostPairsGPU[curPair].y = newPair.y;
+											addedHostPairsGPU[curPair].z = NEW_PAIR_MARKER;
+											addedHostPairsGPU[curPair].w = NEW_PAIR_MARKER;
+
 										}
 									}
 							
@@ -301,6 +310,8 @@ __kernel void   computePairsIncremental3dSapKernel( __global const uint2* object
 											
 											removedHostPairsGPU[curPair].x = removedPair.x;
 											removedHostPairsGPU[curPair].y = removedPair.y;
+											removedHostPairsGPU[curPair].z = REMOVED_PAIR_MARKER;
+											removedHostPairsGPU[curPair].w = REMOVED_PAIR_MARKER;
 										}
 									}
 								
@@ -396,7 +407,9 @@ __kernel void   computePairsKernel( __global const btAabbCL* aabbs, volatile __g
 							int4 tmpPair;
 							tmpPair.x = myPairs[p].x;
 							tmpPair.y = myPairs[p].y;
-							
+							tmpPair.z = NEW_PAIR_MARKER;
+							tmpPair.w = NEW_PAIR_MARKER;
+
 							pairsOut[curPair+p] = tmpPair; //flush to main memory
 						}
 					}
@@ -430,7 +443,8 @@ __kernel void   computePairsKernel( __global const btAabbCL* aabbs, volatile __g
 					int4 tmpPair;
 					tmpPair.x = myPairs[p].x;
 					tmpPair.y = myPairs[p].y;
-					
+					tmpPair.z = NEW_PAIR_MARKER;
+					tmpPair.w = NEW_PAIR_MARKER;
 					pairsOut[curPair+p] = tmpPair; //flush to main memory
 			}
 		}
--- a/src/Bullet3OpenCL/BroadphaseCollision/kernels/sapFastKernels.h
+++ b/src/Bullet3OpenCL/BroadphaseCollision/kernels/sapFastKernels.h
@@ -15,6 +15,8 @@ static const char* sapFastCL= \
 "*/\n"
 "//Originally written by Erwin Coumans\n"
 "\n"
+"#define NEW_PAIR_MARKER -1\n"
+"#define REMOVED_PAIR_MARKER -2\n"
 "\n"
 "typedef struct \n"
 "{\n"
@@ -179,9 +181,11 @@ static const char* sapFastCL= \
 "										int curPair = atomic_inc(addedHostPairsCount);\n"
 "										if (curPair<maxCapacity)\n"
 "										{\n"
-"											\n"
 "											addedHostPairsGPU[curPair].x = newPair.x;\n"
 "											addedHostPairsGPU[curPair].y = newPair.y;\n"
+"											addedHostPairsGPU[curPair].z = NEW_PAIR_MARKER;\n"
+"											addedHostPairsGPU[curPair].w = NEW_PAIR_MARKER;\n"
+"\n"
 "										}\n"
 "									}\n"
 "\n"
@@ -210,6 +214,8 @@ static const char* sapFastCL= \
 "											\n"
 "											removedHostPairsGPU[curPair].x = removedPair.x;\n"
 "											removedHostPairsGPU[curPair].y = removedPair.y;\n"
+"											removedHostPairsGPU[curPair].z = REMOVED_PAIR_MARKER;\n"
+"											removedHostPairsGPU[curPair].w = REMOVED_PAIR_MARKER;\n"
 "\n"
 "										}\n"
 "									}\n"
@@ -275,6 +281,9 @@ static const char* sapFastCL= \
 "											\n"
 "											addedHostPairsGPU[curPair].x = newPair.x;\n"
 "											addedHostPairsGPU[curPair].y = newPair.y;\n"
+"											addedHostPairsGPU[curPair].z = NEW_PAIR_MARKER;\n"
+"											addedHostPairsGPU[curPair].w = NEW_PAIR_MARKER;\n"
+"\n"
 "										}\n"
 "									}\n"
 "							\n"
@@ -303,6 +312,8 @@ static const char* sapFastCL= \
 "											\n"
 "											removedHostPairsGPU[curPair].x = removedPair.x;\n"
 "											removedHostPairsGPU[curPair].y = removedPair.y;\n"
+"											removedHostPairsGPU[curPair].z = REMOVED_PAIR_MARKER;\n"
+"											removedHostPairsGPU[curPair].w = REMOVED_PAIR_MARKER;\n"
 "										}\n"
 "									}\n"
 "								\n"
@@ -398,7 +409,9 @@ static const char* sapFastCL= \
 "							int4 tmpPair;\n"
 "							tmpPair.x = myPairs[p].x;\n"
 "							tmpPair.y = myPairs[p].y;\n"
-"							\n"
+"							tmpPair.z = NEW_PAIR_MARKER;\n"
+"							tmpPair.w = NEW_PAIR_MARKER;\n"
+"\n"
 "							pairsOut[curPair+p] = tmpPair; //flush to main memory\n"
 "						}\n"
 "					}\n"
@@ -432,7 +445,8 @@ static const char* sapFastCL= \
 "					int4 tmpPair;\n"
 "					tmpPair.x = myPairs[p].x;\n"
 "					tmpPair.y = myPairs[p].y;\n"
-"					\n"
+"					tmpPair.z = NEW_PAIR_MARKER;\n"
+"					tmpPair.w = NEW_PAIR_MARKER;\n"
 "					pairsOut[curPair+p] = tmpPair; //flush to main memory\n"
 "			}\n"
 "		}\n"
--- a/src/Bullet3OpenCL/BroadphaseCollision/kernels/sapKernels.h
+++ b/src/Bullet3OpenCL/BroadphaseCollision/kernels/sapKernels.h
@@ -15,6 +15,7 @@ static const char* sapCL= \
 "*/\n"
 "//Originally written by Erwin Coumans\n"
 "\n"
+"#define NEW_PAIR_MARKER -1\n"
 "\n"
 "typedef struct \n"
 "{\n"
@@ -89,6 +90,9 @@ static const char* sapCL= \
 "		\n"
 "		myPair.x = xIndex;\n"
 "		myPair.y = yIndex;\n"
+"		myPair.z = NEW_PAIR_MARKER;\n"
+"		myPair.w = NEW_PAIR_MARKER;\n"
+"\n"
 "\n"
 "		int curPair = atomic_inc (pairCount);\n"
 "		if (curPair<maxPairs)\n"
@@ -114,7 +118,9 @@ static const char* sapCL= \
 "			int4 myPair;\n"
 "			myPair.x = aabbs[i].m_minIndices[3];\n"
 "			myPair.y = aabbs[j].m_minIndices[3];\n"
-"			\n"
+"			myPair.z = NEW_PAIR_MARKER;\n"
+"			myPair.w = NEW_PAIR_MARKER;\n"
+"\n"
 "			int curPair = atomic_inc (pairCount);\n"
 "			if (curPair<maxPairs)\n"
 "			{\n"
@@ -178,6 +184,9 @@ static const char* sapCL= \
 "				int4 myPair;\n"
 "				myPair.x = aabbs[i].m_minIndices[3];\n"
 "				myPair.y = aabbs[j].m_minIndices[3];\n"
+"				myPair.z = NEW_PAIR_MARKER;\n"
+"				myPair.w = NEW_PAIR_MARKER;\n"
+"\n"
 "				int curPair = atomic_inc (pairCount);\n"
 "				if (curPair<maxPairs)\n"
 "				{\n"
@@ -253,6 +262,9 @@ static const char* sapCL= \
 "				int4 myPair;\n"
 "				myPair.x = myAabb.m_minIndices[3];\n"
 "				myPair.y = localAabbs[localCount+localId+1].m_minIndices[3];\n"
+"				myPair.z = NEW_PAIR_MARKER;\n"
+"				myPair.w = NEW_PAIR_MARKER;\n"
+"\n"
 "				int curPair = atomic_inc (pairCount);\n"
 "				if (curPair<maxPairs)\n"
 "				{\n"
--- a/src/Bullet3OpenCL/NarrowphaseCollision/b3ConvexHullContact.cpp
+++ b/src/Bullet3OpenCL/NarrowphaseCollision/b3ConvexHullContact.cpp
@@ -1201,13 +1201,18 @@ int clipFaceAgainstHull(const float4& separatingNormal, const b3ConvexPolyhedron
 			{
 				depth = minDist;
 			}
-
-			if (depth <=maxDist)
+			if (numContactsOut<contactCapacity)
 			{
-				float4 pointInWorld = pVtxIn[i];
-				//resultOut.addContactPoint(separatingNormal,point,depth);
-				contactsOut[numContactsOut++] = make_float4(pointInWorld.x,pointInWorld.y,pointInWorld.z,depth);
-				//printf("depth=%f\n",depth);
+				if (depth <=maxDist)
+				{
+					float4 pointInWorld = pVtxIn[i];
+					//resultOut.addContactPoint(separatingNormal,point,depth);
+					contactsOut[numContactsOut++] = make_float4(pointInWorld.x,pointInWorld.y,pointInWorld.z,depth);
+					//printf("depth=%f\n",depth);
+				}
+			} else
+			{
+				b3Error("exceeding contact capacity (%d,%df)\n", numContactsOut,contactCapacity);
 			}
 		}
 	}
@@ -1415,7 +1420,7 @@ int extractManifold(const float4* p, int nPoints, const float4& nearNormal, b3In



-void clipHullHullSingle(
+int clipHullHullSingle(
 			int bodyIndexA, int bodyIndexB,
 										 const float4& posA,
 										 const b3Quaternion& ornA,
@@ -1425,7 +1430,7 @@ void clipHullHullSingle(
 			int collidableIndexA, int collidableIndexB,

 			const b3AlignedObjectArray<b3RigidBodyCL>* bodyBuf, 
-			b3AlignedObjectArray<b3Contact4>* contactOut, 
+			b3AlignedObjectArray<b3Contact4>* globalContactOut, 
 			int& nContacts,
 			
 			const b3AlignedObjectArray<b3ConvexPolyhedronCL>& hostConvexDataA,
@@ -1443,9 +1448,10 @@ void clipHullHullSingle(

 			const b3AlignedObjectArray<b3Collidable>& hostCollidablesA,
 			const b3AlignedObjectArray<b3Collidable>& hostCollidablesB,
-			const b3Vector3& sepNormalWorldSpace			)
+			const b3Vector3& sepNormalWorldSpace,
+			int maxContactCapacity			)
 {
-
+	int contactIndex = -1;
 	b3ConvexPolyhedronCL hullA, hullB;
    
    b3Collidable colA = hostCollidablesA[collidableIndexA];
@@ -1459,7 +1465,7 @@ void clipHullHullSingle(
    
 	
 	float4 contactsOut[MAX_VERTS];
-	int contactCapacity = MAX_VERTS;
+	int localContactCapacity = MAX_VERTS;

 #ifdef _WIN32
 	b3Assert(_finite(bodyBuf->at(bodyIndexA).m_pos.x));
@@ -1507,7 +1513,7 @@ void clipHullHullSingle(
 								(float4*)&verticesA[0],	&facesA[0],&indicesA[0],
 								(float4*)&verticesB[0],	&facesB[0],&indicesB[0],
 								
-								contactsOut,contactCapacity);
+								contactsOut,localContactCapacity);

 		if (numContactsOut>0)
 		{
@@ -1531,32 +1537,42 @@ void clipHullHullSingle(
 					
 			b3Assert(numPoints);
 					
-			
-			contactOut->expand();
-			b3Contact4& contact = contactOut->at(nContacts);
-			contact.m_batchIdx = 0;//i;
-			contact.m_bodyAPtrAndSignBit = (bodyBuf->at(bodyIndexA).m_invMass==0)? -bodyIndexA:bodyIndexA;
-			contact.m_bodyBPtrAndSignBit = (bodyBuf->at(bodyIndexB).m_invMass==0)? -bodyIndexB:bodyIndexB;
-
-			contact.m_frictionCoeffCmp = 45874;
-			contact.m_restituitionCoeffCmp = 0;
-					
-			float distance = 0.f;
-			for (int p=0;p<numPoints;p++)
+			if (nContacts<maxContactCapacity)
 			{
-				contact.m_worldPos[p] = contactsOut[contactIdx.s[p]];
-				contact.m_worldNormal = normalOnSurfaceB; 
+				contactIndex = nContacts;
+				globalContactOut->expand();
+				b3Contact4& contact = globalContactOut->at(nContacts);
+				contact.m_batchIdx = 0;//i;
+				contact.m_bodyAPtrAndSignBit = (bodyBuf->at(bodyIndexA).m_invMass==0)? -bodyIndexA:bodyIndexA;
+				contact.m_bodyBPtrAndSignBit = (bodyBuf->at(bodyIndexB).m_invMass==0)? -bodyIndexB:bodyIndexB;
+
+				contact.m_frictionCoeffCmp = 45874;
+				contact.m_restituitionCoeffCmp = 0;
+					
+				float distance = 0.f;
+				for (int p=0;p<numPoints;p++)
+				{
+					contact.m_worldPos[p] = contactsOut[contactIdx.s[p]];
+					contact.m_worldNormal = normalOnSurfaceB; 
+				}
+				//printf("bodyIndexA %d,bodyIndexB %d,normal=%f,%f,%f numPoints %d\n",bodyIndexA,bodyIndexB,normalOnSurfaceB.x,normalOnSurfaceB.y,normalOnSurfaceB.z,numPoints);
+				contact.m_worldNormal.w = (b3Scalar)numPoints;
+				nContacts++;
+			} else
+			{
+				b3Error("Error: exceeding contact capacity (%d/%d)\n", nContacts,maxContactCapacity);
 			}
-			//printf("bodyIndexA %d,bodyIndexB %d,normal=%f,%f,%f numPoints %d\n",bodyIndexA,bodyIndexB,normalOnSurfaceB.x,normalOnSurfaceB.y,normalOnSurfaceB.z,numPoints);
-			contact.m_worldNormal.w = (b3Scalar)numPoints;
-			nContacts++;
 		}
 	}
+	return contactIndex;
 }

+#include "b3GjkPairDetector.h"
+#include "b3GjkEpa.h"
+#include "b3VoronoiSimplexSolver.h"

-void computeContactConvexConvex(
-					int pairIndex,
+int computeContactConvexConvex(
+																int pairIndex,
 																int bodyIndexA, int bodyIndexB, 
 																int collidableIndexA, int collidableIndexB, 
 																const b3AlignedObjectArray<b3RigidBodyCL>& rigidBodies, 
@@ -1568,43 +1584,98 @@ void computeContactConvexConvex(
 																const b3AlignedObjectArray<b3GpuFace>& faces,
 																b3AlignedObjectArray<b3Contact4>& globalContactsOut,
 																int& nGlobalContactsOut,
-																int maxContactCapacity)
-{
+																int maxContactCapacity,
+																const b3AlignedObjectArray<b3Contact4>& oldContacts
+																)
+{	
+	int contactIndex = -1;
+	b3VoronoiSimplexSolver simplexSolver;
+	b3GjkEpaSolver2 epaSolver;
+			
+	b3GjkPairDetector gjkDetector(&simplexSolver,&epaSolver);

+	b3Transform transA;
+	transA.setOrigin(rigidBodies[bodyIndexA].m_pos);
+	transA.setRotation(rigidBodies[bodyIndexA].m_quat);
+
+	b3Transform transB;
+	transB.setOrigin(rigidBodies[bodyIndexB].m_pos);
+	transB.setRotation(rigidBodies[bodyIndexB].m_quat);
+	float maximumDistanceSquared = 1e30f;
+					
+	b3Vector3 resultPointOnB;
+	b3Vector3 sepAxis2(0,1,0);
+	b3Scalar distance2 = 1e30f;
+	
+	int shapeIndexA = collidables[collidableIndexA].m_shapeIndex;
+	int shapeIndexB = collidables[collidableIndexB].m_shapeIndex;
+
+	bool result2 = getClosestPoints(&gjkDetector, transA, transB,
+		convexShapes[shapeIndexA], convexShapes[shapeIndexB],
+		convexVertices,convexVertices,
+		maximumDistanceSquared,
+		sepAxis2,
+		distance2,
+		resultPointOnB);
+
+	if (result2)
+	{
+		if (nGlobalContactsOut<maxContactCapacity)
+		{
+			contactIndex = nGlobalContactsOut;
+			globalContactsOut.expand();
+			b3Contact4& contact = globalContactsOut.at(nGlobalContactsOut);
+			contact.m_batchIdx = 0;//i;
+			contact.m_bodyAPtrAndSignBit = (rigidBodies.at(bodyIndexA).m_invMass==0)? -bodyIndexA:bodyIndexA;
+			contact.m_bodyBPtrAndSignBit = (rigidBodies.at(bodyIndexB).m_invMass==0)? -bodyIndexB:bodyIndexB;
+
+			contact.m_frictionCoeffCmp = 45874;
+			contact.m_restituitionCoeffCmp = 0;
+					
+			
+			int numPoints = 1;
+			for (int p=0;p<numPoints;p++)
+			{
+				resultPointOnB.w = distance2;
+				contact.m_worldPos[p] = resultPointOnB;
+				
+				contact.m_worldNormal = -sepAxis2; 
+			}
+			//printf("bodyIndexA %d,bodyIndexB %d,normal=%f,%f,%f numPoints %d\n",bodyIndexA,bodyIndexB,normalOnSurfaceB.x,normalOnSurfaceB.y,normalOnSurfaceB.z,numPoints);
+			contact.m_worldNormal.w = (b3Scalar)numPoints;
+			nGlobalContactsOut++;
+		} else
+		{
+			b3Error("Error: exceeding contact capacity (%d/%d)\n", nGlobalContactsOut,maxContactCapacity);
+		}
+	}
+	
+	return contactIndex;
+}
+
+int computeContactConvexConvex2(
+																int pairIndex,
+																int bodyIndexA, int bodyIndexB, 
+																int collidableIndexA, int collidableIndexB, 
+																const b3AlignedObjectArray<b3RigidBodyCL>& rigidBodies, 
+																const b3AlignedObjectArray<b3Collidable>& collidables,
+																const b3AlignedObjectArray<b3ConvexPolyhedronCL>& convexShapes,
+																const b3AlignedObjectArray<b3Vector3>& convexVertices,
+																const b3AlignedObjectArray<b3Vector3>& uniqueEdges,
+																const b3AlignedObjectArray<int>& convexIndices,
+																const b3AlignedObjectArray<b3GpuFace>& faces,
+																b3AlignedObjectArray<b3Contact4>& globalContactsOut,
+																int& nGlobalContactsOut,
+																int maxContactCapacity,
+																const b3AlignedObjectArray<b3Contact4>& oldContacts
+																)
+{
+	int contactIndex = -1;
 	b3Vector3 posA = rigidBodies[bodyIndexA].m_pos;
 	b3Quaternion ornA = rigidBodies[bodyIndexA].m_quat;
 	b3Vector3 posB = rigidBodies[bodyIndexB].m_pos;
 	b3Quaternion ornB = rigidBodies[bodyIndexB].m_quat;
 	
-			/*int bodyIndexA, int bodyIndexB,
-		  const float4& posA,
-		  const float4& ornA,
-		  const float4& posB,
-		  const float4& ornB,
-			int collidableIndexA, int collidableIndexB,
-
-			const b3AlignedObjectArray<b3RigidBodyCL>* bodyBuf, 
-			b3AlignedObjectArray<b3Contact4>* contactOut,
-															  int& nContacts,
-
-			const b3AlignedObjectArray<b3ConvexPolyhedronCL>& hostConvexDataA,
-			const b3AlignedObjectArray<b3ConvexPolyhedronCL>& hostConvexDataB,
-	
-			const b3AlignedObjectArray<b3Vector3>& verticesA, 
-			const b3AlignedObjectArray<b3Vector3>& uniqueEdgesA, 
-			const b3AlignedObjectArray<b3GpuFace>& facesA,
-			const b3AlignedObjectArray<int>& indicesA,
-	
-			const b3AlignedObjectArray<b3Vector3>& verticesB,
-			const b3AlignedObjectArray<b3Vector3>& uniqueEdgesB,
-			const b3AlignedObjectArray<b3GpuFace>& facesB,
-			const b3AlignedObjectArray<int>& indicesB,
-
-			const b3AlignedObjectArray<b3Collidable>& hostCollidablesA,
-			const b3AlignedObjectArray<b3Collidable>& hostCollidablesB
-			)
-			*/
-

 	b3ConvexPolyhedronCL hullA, hullB;
    
@@ -1649,7 +1720,7 @@ void computeContactConvexConvex(
 	{
 		
 		
-		clipHullHullSingle(
+		contactIndex = clipHullHullSingle(
 			bodyIndexA, bodyIndexB,
 						   posA,ornA,
 						   posB,ornB,
@@ -1673,17 +1744,19 @@ void computeContactConvexConvex(

 			collidables,
 			collidables,
-			sepNormalWorldSpace);
+			sepNormalWorldSpace,
+			maxContactCapacity);
 			
 	}

-
+	return contactIndex;
 }


-void GpuSatCollision::computeConvexConvexContactsGPUSAT( const b3OpenCLArray<b3Int4>* pairs, int nPairs,
+void GpuSatCollision::computeConvexConvexContactsGPUSAT( b3OpenCLArray<b3Int4>* pairs, int nPairs,
 			const b3OpenCLArray<b3RigidBodyCL>* bodyBuf,
 			b3OpenCLArray<b3Contact4>* contactOut, int& nContacts,
+			const b3OpenCLArray<b3Contact4>* oldContacts,
 			int maxContactCapacity,
 			int compoundPairCapacity,
 			const b3OpenCLArray<b3ConvexPolyhedronCL>& convexData,
@@ -1754,6 +1827,13 @@ void GpuSatCollision::computeConvexConvexContactsGPUSAT( const b3OpenCLArray<b3I
 		contactOut->copyToHost(hostContacts);
 	}

+	b3AlignedObjectArray<b3Contact4> oldHostContacts;
+	if (oldContacts->size())
+	{
+		oldContacts->copyToHost(oldHostContacts);
+	}
+
+
 	hostContacts.resize(nPairs);

 	for (int i=0;i<nPairs;i++)
@@ -1819,8 +1899,15 @@ void GpuSatCollision::computeConvexConvexContactsGPUSAT( const b3OpenCLArray<b3I
 		if (hostCollidables[collidableIndexA].m_shapeType == SHAPE_CONVEX_HULL &&
 			hostCollidables[collidableIndexB].m_shapeType == SHAPE_CONVEX_HULL)
 		{
-			computeContactConvexConvex(i,bodyIndexA,bodyIndexB,collidableIndexA,collidableIndexB,hostBodyBuf,
-			hostCollidables,hostConvexData,hostVertices,hostUniqueEdges,hostIndices,hostFaces,hostContacts,nContacts,maxContactCapacity);
+			//printf("hostPairs[i].z=%d\n",hostPairs[i].z);
+			int contactIndex = computeContactConvexConvex(i,bodyIndexA,bodyIndexB,collidableIndexA,collidableIndexB,hostBodyBuf,
+					hostCollidables,hostConvexData,hostVertices,hostUniqueEdges,hostIndices,hostFaces,hostContacts,nContacts,maxContactCapacity,
+					oldHostContacts);
+
+			if (contactIndex>=0)
+			{
+				hostPairs[i].z = contactIndex;
+			}
 //			printf("plane-convex\n");
 			
 		}
@@ -1828,6 +1915,11 @@ void GpuSatCollision::computeConvexConvexContactsGPUSAT( const b3OpenCLArray<b3I

 	}

+	if (hostPairs.size())
+	{
+		pairs->copyFromHost(hostPairs);
+	}
+
 	if (nContacts)
 		{
 			hostContacts.resize(nContacts);
@@ -2380,7 +2472,7 @@ void GpuSatCollision::computeConvexConvexContactsGPUSAT( const b3OpenCLArray<b3I
                }
            }
 	}            
-	else
+	else//breakupKernel
 	{
 	 
 		if (nPairs)
@@ -2402,11 +2494,18 @@ void GpuSatCollision::computeConvexConvexContactsGPUSAT( const b3OpenCLArray<b3I
 			b3LauncherCL launcher(m_queue, m_clipHullHullKernel);
 			launcher.setBuffers( bInfo, sizeof(bInfo)/sizeof(b3BufferInfoCL) );
 			launcher.setConst( nPairs  );
+			launcher.setConst(maxContactCapacity);
+			
 			int num = nPairs;
 			launcher.launch1D( num);
 			clFinish(m_queue);
 		
 			nContacts = m_totalContactsOut.at(0);
+			if (nContacts >= maxContactCapacity)
+			{
+				b3Error("Exceeded contact capacity (%d/%d)\n",nContacts,maxContactCapacity);
+				nContacts = maxContactCapacity;
+			}
 			contactOut->resize(nContacts);
 		}

--- a/src/Bullet3OpenCL/NarrowphaseCollision/b3ConvexHullContact.h
+++ b/src/Bullet3OpenCL/NarrowphaseCollision/b3ConvexHullContact.h
@@ -75,9 +75,10 @@ struct GpuSatCollision
 	virtual ~GpuSatCollision();
 	

-	void computeConvexConvexContactsGPUSAT( const b3OpenCLArray<b3Int4>* pairs, int nPairs, 
+	void computeConvexConvexContactsGPUSAT( b3OpenCLArray<b3Int4>* pairs, int nPairs, 
 			const b3OpenCLArray<b3RigidBodyCL>* bodyBuf,
 			b3OpenCLArray<b3Contact4>* contactOut, int& nContacts,
+			const b3OpenCLArray<b3Contact4>* oldContacts,
 			int maxContactCapacity,
 			int compoundPairCapacity,
 			const b3OpenCLArray<b3ConvexPolyhedronCL>& hostConvexData,
--- a/src/Bullet3OpenCL/NarrowphaseCollision/b3GjkEpa.cpp
+++ b/src/Bullet3OpenCL/NarrowphaseCollision/b3GjkEpa.cpp
--- a/src/Bullet3OpenCL/NarrowphaseCollision/b3GjkEpa.h
+++ b/src/Bullet3OpenCL/NarrowphaseCollision/b3GjkEpa.h
@@ -0,0 +1,85 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2008 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.
+*/
+
+/*
+GJK-EPA collision solver by Nathanael Presson, 2008
+*/
+#ifndef B3_GJK_EPA2_H
+#define B3_GJK_EPA2_H
+
+#include "Bullet3Common/b3AlignedObjectArray.h"
+#include "Bullet3Common/b3Transform.h"
+#include "b3ConvexPolyhedronCL.h"
+
+///btGjkEpaSolver contributed under zlib by Nathanael Presson
+struct	b3GjkEpaSolver2
+{
+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;
+	b3Vector3	witnesses[2];
+	b3Vector3	normal;
+	b3Scalar	distance;
+	};
+
+static int		StackSizeRequirement();
+
+static bool		Distance(	 const b3Transform&	transA, const b3Transform&	transB,
+							const b3ConvexPolyhedronCL* hullA, const b3ConvexPolyhedronCL* hullB, 
+							const b3AlignedObjectArray<b3Vector3>& verticesA,
+							const b3AlignedObjectArray<b3Vector3>& verticesB,
+							const b3Vector3& guess,
+							sResults& results);
+
+static bool		Penetration( const b3Transform&	transA, const b3Transform&	transB,
+							const b3ConvexPolyhedronCL* hullA, const b3ConvexPolyhedronCL* hullB, 
+							const b3AlignedObjectArray<b3Vector3>& verticesA,
+							const b3AlignedObjectArray<b3Vector3>& verticesB,
+							const b3Vector3& guess,
+							sResults& results,
+							bool usemargins=true);
+#if 0
+static b3Scalar	SignedDistance(	const b3Vector3& position,
+								b3Scalar margin,
+								const b3Transform&	transA,
+								const b3ConvexPolyhedronCL& hullA, 
+								const b3AlignedObjectArray<b3Vector3>& verticesA,
+	
+								sResults& results);
+							
+static bool		SignedDistance(	 const b3Transform&	transA, const b3Transform&	transB,
+								const b3ConvexPolyhedronCL& hullA, const b3ConvexPolyhedronCL& hullB, 
+								const b3AlignedObjectArray<b3Vector3>& verticesA,
+								const b3AlignedObjectArray<b3Vector3>& verticesB,
+								const b3Vector3& guess,
+								sResults& results);
+#endif //0
+
+};
+
+#endif //B3_GJK_EPA2_H
+
--- a/src/Bullet3OpenCL/NarrowphaseCollision/b3GjkPairDetector.cpp
+++ b/src/Bullet3OpenCL/NarrowphaseCollision/b3GjkPairDetector.cpp
@@ -0,0 +1,664 @@
+/*
+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 "b3GjkPairDetector.h"
+#include "Bullet3Common/b3Transform.h"
+#include "b3VoronoiSimplexSolver.h"
+#include "b3ConvexPolyhedronCL.h"
+#include "b3VectorFloat4.h"
+#include "b3GjkEpa.h"
+#include "b3SupportMappings.h"
+
+//must be above the machine epsilon
+#define REL_ERROR2 b3Scalar(1.0e-6)
+
+//temp globals, to improve GJK/EPA/penetration calculations
+int gNumDeepPenetrationChecks = 0;
+int gNumGjkChecks = 0;
+int gGjkSeparatingAxis=0;
+int gEpaSeparatingAxis=0;
+
+
+
+b3GjkPairDetector::b3GjkPairDetector(b3VoronoiSimplexSolver* simplexSolver,b3GjkEpaSolver2*	penetrationDepthSolver)
+:m_cachedSeparatingAxis(b3Scalar(0.),b3Scalar(-1.),b3Scalar(0.)),
+m_penetrationDepthSolver(penetrationDepthSolver),
+m_simplexSolver(simplexSolver),
+m_ignoreMargin(false),
+m_lastUsedMethod(-1),
+m_catchDegeneracies(1),
+m_fixContactNormalDirection(1)
+{
+}
+
+
+
+
+bool calcPenDepth( b3VoronoiSimplexSolver& simplexSolver,
+											  const b3Transform&	transformA, const b3Transform&	transformB,
+	const b3ConvexPolyhedronCL& hullA, const b3ConvexPolyhedronCL& hullB, 
+	const b3AlignedObjectArray<b3Vector3>& verticesA,
+	const b3AlignedObjectArray<b3Vector3>& verticesB,
+											  b3Vector3& v, b3Vector3& wWitnessOnA, b3Vector3& wWitnessOnB)
+{
+
+	(void)v;
+	(void)simplexSolver;
+
+	b3Vector3	guessVector(transformB.getOrigin()-transformA.getOrigin());
+	b3GjkEpaSolver2::sResults	results;
+	
+
+	if(b3GjkEpaSolver2::Penetration(transformA,transformB,&hullA,&hullB,verticesA,verticesB,guessVector,results))
+	{
+		wWitnessOnA = results.witnesses[0];
+		wWitnessOnB = results.witnesses[1];
+		v = results.normal;
+		return true;		
+	} 
+	else
+	{
+		if(b3GjkEpaSolver2::Distance(transformA,transformB,&hullA,&hullB,verticesA,verticesB,guessVector,results))
+		{
+			wWitnessOnA = results.witnesses[0];
+			wWitnessOnB = results.witnesses[1];
+			v = results.normal;
+			return false;
+		}
+	}
+
+	return false;
+}
+#define dot3F4 b3Dot
+
+inline void project(const b3ConvexPolyhedronCL& hull,  const float4& pos, const b3Quaternion& orn, const float4& dir, const b3AlignedObjectArray<b3Vector3>& vertices, b3Scalar& min, b3Scalar& max)
+{
+	min = FLT_MAX;
+	max = -FLT_MAX;
+	int numVerts = hull.m_numVertices;
+
+	const float4 localDir = b3QuatRotate(orn.inverse(),dir);
+
+	b3Scalar offset = dot3F4(pos,dir);
+
+	for(int i=0;i<numVerts;i++)
+	{
+		//b3Vector3 pt = trans * vertices[m_vertexOffset+i];
+		//b3Scalar dp = pt.dot(dir);
+		b3Vector3 vertex = vertices[hull.m_vertexOffset+i];
+		b3Scalar dp = dot3F4((float4&)vertices[hull.m_vertexOffset+i],localDir);
+		//b3Assert(dp==dpL);
+		if(dp < min)	min = dp;
+		if(dp > max)	max = dp;
+	}
+	if(min>max)
+	{
+		b3Scalar tmp = min;
+		min = max;
+		max = tmp;
+	}
+	min += offset;
+	max += offset;
+}
+
+
+static bool TestSepAxis(const b3ConvexPolyhedronCL& hullA, const b3ConvexPolyhedronCL& hullB, 
+	const float4& posA,const b3Quaternion& ornA,
+	const float4& posB,const b3Quaternion& ornB,
+	float4& sep_axis, const b3AlignedObjectArray<b3Vector3>& verticesA,const b3AlignedObjectArray<b3Vector3>& verticesB,b3Scalar& depth)
+{
+	b3Scalar Min0,Max0;
+	b3Scalar Min1,Max1;
+	project(hullA,posA,ornA,sep_axis,verticesA, Min0, Max0);
+	project(hullB,posB,ornB, sep_axis,verticesB, Min1, Max1);
+
+	if(Max0<Min1 || Max1<Min0)
+		return false;
+
+	b3Scalar d0 = Max0 - Min1;
+	b3Assert(d0>=0.0f);
+	b3Scalar d1 = Max1 - Min0;
+	b3Assert(d1>=0.0f);
+	if (d0<d1)
+	{
+		depth = d0;
+		sep_axis *=-1;
+	} else
+	{
+		depth = d1;
+		
+	}
+	
+	return true;
+}
+
+
+bool getClosestPoints(b3GjkPairDetector* gjkDetector, const b3Transform&	transA, const b3Transform&	transB,
+	const b3ConvexPolyhedronCL& hullA, const b3ConvexPolyhedronCL& hullB, 
+	const b3AlignedObjectArray<b3Vector3>& verticesA,
+	const b3AlignedObjectArray<b3Vector3>& verticesB,
+	b3Scalar maximumDistanceSquared,
+	b3Vector3& resultSepNormal,
+	float& resultSepDistance,
+	b3Vector3& resultPointOnB)
+{
+	//resultSepDistance = maximumDistanceSquared;
+
+	gjkDetector->m_cachedSeparatingDistance = 0.f;
+
+	b3Scalar distance=b3Scalar(0.);
+	b3Vector3	normalInB(b3Scalar(0.),b3Scalar(0.),b3Scalar(0.));
+	b3Vector3 pointOnA,pointOnB;
+
+	b3Transform localTransA = transA;
+	b3Transform localTransB = transB;
+	
+	b3Vector3 positionOffset(0,0,0);// = (localTransA.getOrigin() + localTransB.getOrigin()) * b3Scalar(0.5);
+	localTransA.getOrigin() -= positionOffset;
+	localTransB.getOrigin() -= positionOffset;
+
+	bool check2d = false;//m_minkowskiA->isConvex2d() && m_minkowskiB->isConvex2d();
+
+	b3Scalar marginA = 0.f;//m_marginA;
+	b3Scalar marginB = 0.f;//m_marginB;
+
+	gNumGjkChecks++;
+
+
+	//for CCD we don't use margins
+	if (gjkDetector->m_ignoreMargin)
+	{
+		marginA = b3Scalar(0.);
+		marginB = b3Scalar(0.);
+	}
+
+	gjkDetector->m_curIter = 0;
+	int gGjkMaxIter = 1000;//this is to catch invalid input, perhaps check for #NaN?
+	gjkDetector->m_cachedSeparatingAxis.setValue(1,1,1);//0,0,0);
+
+	bool isValid = false;
+	bool checkSimplex = false;
+	bool checkPenetration = true;
+	gjkDetector->m_degenerateSimplex = 0;
+
+	gjkDetector->m_lastUsedMethod = -1;
+
+	{
+		b3Scalar squaredDistance = B3_LARGE_FLOAT;
+		b3Scalar delta = -1e30f;//b3Scalar(0.);
+		b3Scalar prevDelta = -1e30f;//b3Scalar(0.);
+		
+		b3Scalar margin = marginA + marginB;
+		b3Scalar bestDeltaN = -1e30f;
+		b3Vector3 bestSepAxis(0,0,0);
+		b3Vector3 bestPointOnA;
+		b3Vector3 bestPointOnB;
+
+		
+
+		gjkDetector->m_simplexSolver->reset();
+		
+		for ( ; ; )
+		//while (true)
+		{
+
+		
+			b3Vector3 seperatingAxisInA = (-gjkDetector->m_cachedSeparatingAxis)* localTransA.getBasis();
+			b3Vector3 seperatingAxisInB = gjkDetector->m_cachedSeparatingAxis* localTransB.getBasis();
+
+			b3Vector3 pInA = localGetSupportVertexWithoutMargin(seperatingAxisInA,&hullA,verticesA);
+			b3Vector3 qInB = localGetSupportVertexWithoutMargin(seperatingAxisInB,&hullB,verticesB);
+
+			b3Vector3  pWorld = localTransA(pInA);	
+			b3Vector3  qWorld = localTransB(qInB);
+
+			{
+				b3Scalar l2 = gjkDetector->m_cachedSeparatingAxis.length2();
+				if (l2>B3_EPSILON*B3_EPSILON)
+				{
+
+					b3Vector3 testAxis = gjkDetector->m_cachedSeparatingAxis*(1./b3Sqrt(l2));
+					float computedDepth=1e30f;
+					if (!TestSepAxis(hullA,hullB,transA.getOrigin(),transA.getRotation(),
+						transB.getOrigin(),transB.getRotation(),testAxis,verticesA,verticesB,computedDepth))
+					{
+						return false;
+					}
+			
+
+				
+					if(computedDepth<resultSepDistance)
+					{
+						if (testAxis.length2()>B3_EPSILON*B3_EPSILON)
+						{
+							resultSepDistance = computedDepth;
+							resultSepNormal = testAxis;
+						}
+					}
+				}
+			}
+
+
+
+			if (check2d)
+			{
+				pWorld[2] = 0.f;
+				qWorld[2] = 0.f;
+			}
+
+			b3Vector3 w	= pWorld - qWorld;
+			delta = gjkDetector->m_cachedSeparatingAxis.dot(w);
+			if (delta>0)
+				return false;
+			b3Scalar deltaN = gjkDetector->m_cachedSeparatingAxis.normalized().dot(w.normalized());
+			
+
+			if (deltaN < bestDeltaN)
+			{
+				bestDeltaN = deltaN;
+				//printf("new solution?\n");
+				bestSepAxis = gjkDetector->m_cachedSeparatingAxis;
+				gjkDetector->m_simplexSolver->compute_points(bestPointOnA, bestPointOnB);
+			}
+			prevDelta = delta;
+			b3Scalar dist = 0;
+			if (delta<0)
+				dist = -b3Sqrt(b3Fabs(delta));
+			else
+				dist = b3Sqrt(delta);
+
+			//printf("gjkDetector->m_cachedSeparatingAxis = %f,%f,%f delta/dist = %f\n",gjkDetector->m_cachedSeparatingAxis.x,gjkDetector->m_cachedSeparatingAxis.y,gjkDetector->m_cachedSeparatingAxis.z,dist);
+			// potential exit, they don't overlap
+			if ((delta > b3Scalar(0.0)) && (delta * delta > squaredDistance * maximumDistanceSquared)) 
+			{
+				gjkDetector->m_degenerateSimplex = 10;
+				checkSimplex=true;
+				//checkPenetration = false;
+				break;
+			}
+
+			//exit 0: the new point is already in the simplex, or we didn't come any closer
+			if (gjkDetector->m_simplexSolver->inSimplex(w))
+			{
+				gjkDetector->m_degenerateSimplex = 1;
+				checkSimplex = true;
+				break;
+			}
+			// are we getting any closer ?
+			b3Scalar f0 = squaredDistance - delta;
+			b3Scalar f1 = squaredDistance * REL_ERROR2;
+
+			if (f0 <= f1)
+			{
+				if (f0 <= b3Scalar(0.))
+				{
+					gjkDetector->m_degenerateSimplex = 2;
+				} else
+				{
+					gjkDetector->m_degenerateSimplex = 11;
+				}
+				checkSimplex = true;
+				break;
+			}
+
+			//add current vertex to simplex
+			gjkDetector->m_simplexSolver->addVertex(w, pWorld, qWorld);
+			b3Vector3 newCachedSeparatingAxis;
+
+			//calculate the closest point to the origin (update vector v)
+			if (!gjkDetector->m_simplexSolver->closest(newCachedSeparatingAxis))
+			{
+				gjkDetector->m_degenerateSimplex = 3;
+				checkSimplex = true;
+				break;
+			}
+
+			if(0)//newCachedSeparatingAxis.length2()<REL_ERROR2)
+            {
+				if (delta<bestDeltaN)
+				{
+					gjkDetector->m_cachedSeparatingAxis = newCachedSeparatingAxis;
+				}
+                gjkDetector->m_degenerateSimplex = 6;
+                checkSimplex = true;
+                break;
+            }
+
+			b3Scalar previousSquaredDistance = squaredDistance;
+			squaredDistance = newCachedSeparatingAxis.length2();
+		
+			b3Vector3 sepAxis=newCachedSeparatingAxis.normalized();
+
+			
+
+
+			//redundant m_simplexSolver->compute_points(pointOnA, pointOnB);
+
+			//are we getting any closer ?
+			if (previousSquaredDistance - squaredDistance <= B3_EPSILON * previousSquaredDistance) 
+			{ 
+				checkSimplex = true;
+				gjkDetector->m_degenerateSimplex = 12;
+				
+				break;
+			}
+
+			gjkDetector->m_cachedSeparatingAxis = newCachedSeparatingAxis;
+
+			{
+				b3Scalar l2 = gjkDetector->m_cachedSeparatingAxis.length2();
+				if (l2>B3_EPSILON*B3_EPSILON)
+				{
+
+					b3Vector3 testAxis = gjkDetector->m_cachedSeparatingAxis*(1./b3Sqrt(l2));
+					float computedDepth=1e30f;
+					if (!TestSepAxis(hullA,hullB,transA.getOrigin(),transA.getRotation(),
+						transB.getOrigin(),transB.getRotation(),testAxis,verticesA,verticesB,computedDepth))
+					{
+						return false;
+					}
+			
+
+				
+					if(computedDepth<resultSepDistance)
+					{
+						if (testAxis.length2()>B3_EPSILON*B3_EPSILON)
+						{
+							resultSepDistance = computedDepth;
+							resultSepNormal = testAxis;
+						}
+					}
+				}
+			}
+
+			//degeneracy, this is typically due to invalid/uninitialized worldtransforms for a btCollisionObject   
+            if (gjkDetector->m_curIter++ > gGjkMaxIter)   
+            {   
+                    break;   
+            } 
+
+
+			bool check = (!gjkDetector->m_simplexSolver->fullSimplex());
+
+			float projectedDepth = 0;
+			
+			if (delta<0)
+			{
+				projectedDepth = -b3Sqrt(b3Fabs(delta));
+			} else
+			{
+				projectedDepth = b3Sqrt(delta);
+			}
+			
+			//printf("dist2 = %f dist= %f projectedDepth = %f\n", squaredDistance,b3Sqrt(squaredDistance),projectedDepth);
+
+			if (!check)
+			{
+				gjkDetector->m_degenerateSimplex = 13;
+				break;
+			}
+		}
+
+		if (checkSimplex)
+		{
+			if (bestSepAxis.length2())
+			{
+				pointOnA = bestPointOnA;
+				pointOnB = bestPointOnB;
+				gjkDetector->m_cachedSeparatingAxis = bestSepAxis;
+			} else
+			{
+				gjkDetector->m_simplexSolver->compute_points(pointOnA, pointOnB);
+			}
+			
+			normalInB = gjkDetector->m_cachedSeparatingAxis;
+			b3Scalar lenSqr =gjkDetector->m_cachedSeparatingAxis.length2();
+			
+			//valid normal
+			if (lenSqr < 0.0001)
+			{
+				gjkDetector->m_degenerateSimplex = 5;
+			} 
+			if (lenSqr > B3_EPSILON*B3_EPSILON)
+			{
+				b3Scalar rlen = b3Scalar(1.) / b3Sqrt(lenSqr );
+				normalInB *= rlen; //normalize
+				b3Scalar s = b3Sqrt(squaredDistance);
+			
+				b3Assert(s > b3Scalar(0.0));
+				pointOnA -= gjkDetector->m_cachedSeparatingAxis * (marginA / s);
+				pointOnB += gjkDetector->m_cachedSeparatingAxis * (marginB / s);
+				distance = ((b3Scalar(1.)/rlen) - margin);
+				isValid = true;
+				
+				gjkDetector->m_lastUsedMethod = 1;
+			} else
+			{
+				gjkDetector->m_lastUsedMethod = 2;
+			}
+		}
+
+		bool catchDegeneratePenetrationCase = (gjkDetector->m_catchDegeneracies && gjkDetector->m_penetrationDepthSolver && gjkDetector->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 (gjkDetector->m_penetrationDepthSolver)
+			{
+				// Penetration depth case.
+				b3Vector3 tmpPointOnA,tmpPointOnB;
+				
+				gNumDeepPenetrationChecks++;
+				gjkDetector->m_cachedSeparatingAxis.setZero();
+
+				bool isValid2 = calcPenDepth( 
+					*gjkDetector->m_simplexSolver, 
+					transA,transB,hullA,hullB,verticesA,verticesB,
+					gjkDetector->m_cachedSeparatingAxis, tmpPointOnA, tmpPointOnB
+					);
+
+
+				if (isValid2)
+				{
+					b3Vector3 tmpNormalInB = tmpPointOnB-tmpPointOnA;
+					b3Scalar lenSqr = tmpNormalInB.length2();
+					if (lenSqr <= (B3_EPSILON*B3_EPSILON))
+					{
+						tmpNormalInB = gjkDetector->m_cachedSeparatingAxis;
+						lenSqr = gjkDetector->m_cachedSeparatingAxis.length2();
+					}
+
+					if (lenSqr > (B3_EPSILON*B3_EPSILON))
+					{
+						tmpNormalInB /= b3Sqrt(lenSqr);
+						b3Scalar 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;
+							gjkDetector->m_lastUsedMethod = 3;
+						} else
+						{
+							gjkDetector->m_lastUsedMethod = 8;
+						}
+					} else
+					{
+						gjkDetector->m_lastUsedMethod = 9;
+					}
+				} else
+
+				{
+					///this is another degenerate case, where the initial GJK calculation reports a degenerate case
+					///EPA reports no penetration, and the second GJK (using the supporting vector without margin)
+					///reports a valid positive distance. Use the results of the second GJK instead of failing.
+					///thanks to Jacob.Langford for the reproduction case
+					///http://code.google.com/p/bullet/issues/detail?id=250
+
+				
+					if (gjkDetector->m_cachedSeparatingAxis.length2() > b3Scalar(0.))
+					{
+						b3Scalar distance2 = (tmpPointOnA-tmpPointOnB).length()-margin;
+						//only replace valid distances when the distance is less
+						if (!isValid || (distance2 < distance))
+						{
+							distance = distance2;
+							pointOnA = tmpPointOnA;
+							pointOnB = tmpPointOnB;
+							pointOnA -= gjkDetector->m_cachedSeparatingAxis * marginA ;
+							pointOnB += gjkDetector->m_cachedSeparatingAxis * marginB ;
+							normalInB = gjkDetector->m_cachedSeparatingAxis;
+							normalInB.normalize();
+							isValid = true;
+							gjkDetector->m_lastUsedMethod = 6;
+						} else
+						{
+							gjkDetector->m_lastUsedMethod = 5;
+						}
+					}
+				}
+				
+			}
+
+		}
+	}
+
+	
+
+	if (isValid && ((distance < 0) || (distance*distance < maximumDistanceSquared)))
+	{
+
+		if (gjkDetector->m_fixContactNormalDirection)
+		{
+			///@workaround for sticky convex collisions
+			//in some degenerate cases (usually when the use uses very small margins) 
+			//the contact normal is pointing the wrong direction
+			//so fix it now (until we can deal with all degenerate cases in GJK and EPA)
+			//contact normals need to point from B to A in all cases, so we can simply check if the contact normal really points from B to A
+			//We like to use a dot product of the normal against the difference of the centroids, 
+			//once the centroid is available in the API
+			//until then we use the center of the aabb to approximate the centroid
+			b3Vector3 aabbMin,aabbMax;
+			//m_minkowskiA->getAabb(localTransA,aabbMin,aabbMax);
+			//b3Vector3 posA  = (aabbMax+aabbMin)*b3Scalar(0.5);
+		
+			//m_minkowskiB->getAabb(localTransB,aabbMin,aabbMax);
+			//b3Vector3 posB = (aabbMin+aabbMax)*b3Scalar(0.5);
+
+
+			b3Vector3 diff = transA.getOrigin()-transB.getOrigin();
+			if (diff.dot(normalInB) < 0.f)
+				normalInB *= -1.f;
+		}
+		gjkDetector->m_cachedSeparatingAxis = normalInB;
+		gjkDetector->m_cachedSeparatingDistance = distance;
+
+		{
+				b3Scalar l2 = gjkDetector->m_cachedSeparatingAxis.length2();
+				if (l2>B3_EPSILON*B3_EPSILON)
+				{
+
+					b3Vector3 testAxis = gjkDetector->m_cachedSeparatingAxis*(1./b3Sqrt(l2));
+					float computedDepth=1e30f;
+					if (!TestSepAxis(hullA,hullB,transA.getOrigin(),transA.getRotation(),
+						transB.getOrigin(),transB.getRotation(),testAxis,verticesA,verticesB,computedDepth))
+					{
+						return false;
+					}
+			
+
+				
+					if(computedDepth<resultSepDistance)
+					{
+						if (testAxis.length2()>B3_EPSILON*B3_EPSILON)
+						{
+							resultSepDistance = computedDepth;
+							resultSepNormal = testAxis;
+						}
+					}
+				}
+			}
+
+		
+		resultSepNormal = normalInB;
+		//printf("normalInB = %f,%f,%f, distance = %f\n",normalInB.x,normalInB.y,normalInB.z,distance);
+		resultSepDistance = distance;
+		
+
+		b3Scalar lsqr = resultSepNormal.length2();
+		b3Assert(lsqr>B3_EPSILON*B3_EPSILON);
+		if (lsqr<B3_EPSILON*B3_EPSILON)
+			return false;
+		resultSepNormal *= 1.f/b3Sqrt(lsqr);
+
+#if 0
+		float dot = resultSepNormal.dot(b3Vector3(0,-1,0));
+		//float dot = b3Vector3(-0.7,-0.6,-0.2).dot(b3Vector3(0,-1,0));
+
+		static float minDot = 1e30f;
+
+		if (dot<minDot)
+		{
+			//printf("minDot = %f\n", dot);
+			minDot=dot;
+		}
+
+		//printf("gNumGjkChecks = %d, gNumDeepPenetrationChecks = %d, minDot = %f\n", gNumGjkChecks,gNumDeepPenetrationChecks,minDot);
+		if (0)//dot<0.64)
+		{
+			{
+				b3Scalar l2 = resultSepNormal.length2();
+				if (l2>B3_EPSILON*B3_EPSILON)
+				{
+
+					b3Vector3 testAxis = gjkDetector->m_cachedSeparatingAxis*(1./b3Sqrt(l2));
+					float computedDepth=1e30f;
+					if (!TestSepAxis(hullA,hullB,transA.getOrigin(),transA.getRotation(),
+						transB.getOrigin(),transB.getRotation(),testAxis,verticesA,verticesB,computedDepth))
+					{
+						return false;
+					}
+			
+
+				
+					if(computedDepth<resultSepDistance)
+					{
+						if (testAxis.length2()>B3_EPSILON*B3_EPSILON)
+						{
+							resultSepDistance = computedDepth;
+							resultSepNormal = testAxis;
+						}
+					}
+				}
+			}
+		}
+#endif
+		resultPointOnB = pointOnB+positionOffset;
+		return true;
+	}
+	return false;
+
+}
+
+
+
+
+
--- a/src/Bullet3OpenCL/NarrowphaseCollision/b3GjkPairDetector.h
+++ b/src/Bullet3OpenCL/NarrowphaseCollision/b3GjkPairDetector.h
@@ -0,0 +1,84 @@
+
+
+
+
+#ifndef B3_GJK_PAIR_DETECTOR_H
+#define B3_GJK_PAIR_DETECTOR_H
+
+
+#include "Bullet3Common/b3Vector3.h"
+#include "Bullet3Common/b3AlignedObjectArray.h"
+
+struct b3Transform;
+struct b3GjkEpaSolver2;
+class b3VoronoiSimplexSolver;
+struct b3ConvexPolyhedronCL;
+
+B3_ATTRIBUTE_ALIGNED16(struct) b3GjkPairDetector
+{
+	
+
+	b3Vector3	m_cachedSeparatingAxis;
+	b3GjkEpaSolver2*	m_penetrationDepthSolver;
+	b3VoronoiSimplexSolver* m_simplexSolver;
+
+
+	bool		m_ignoreMargin;
+	b3Scalar	m_cachedSeparatingDistance;
+	
+
+public:
+
+	//some debugging to fix degeneracy problems
+	int			m_lastUsedMethod;
+	int			m_curIter;
+	int			m_degenerateSimplex;
+	int			m_catchDegeneracies;
+	int			m_fixContactNormalDirection;
+
+	b3GjkPairDetector(b3VoronoiSimplexSolver* simplexSolver,b3GjkEpaSolver2*	penetrationDepthSolver);
+	
+	virtual ~b3GjkPairDetector() {};
+
+	
+	//void	getClosestPoints(,Result& output);
+	
+	void setCachedSeperatingAxis(const b3Vector3& seperatingAxis)
+	{
+		m_cachedSeparatingAxis = seperatingAxis;
+	}
+
+	const b3Vector3& getCachedSeparatingAxis() const
+	{
+		return m_cachedSeparatingAxis;
+	}
+	b3Scalar	getCachedSeparatingDistance() const
+	{
+		return m_cachedSeparatingDistance;
+	}
+
+	void	setPenetrationDepthSolver(b3GjkEpaSolver2*	penetrationDepthSolver)
+	{
+		m_penetrationDepthSolver = penetrationDepthSolver;
+	}
+
+	///don't use setIgnoreMargin, it's for Bullet's internal use
+	void	setIgnoreMargin(bool ignoreMargin)
+	{
+		m_ignoreMargin = ignoreMargin;
+	}
+
+
+};
+
+
+bool getClosestPoints(b3GjkPairDetector* gjkDetector, const b3Transform&	transA, const b3Transform&	transB,
+	const b3ConvexPolyhedronCL& hullA, const b3ConvexPolyhedronCL& hullB, 
+	const b3AlignedObjectArray<b3Vector3>& verticesA,
+	const b3AlignedObjectArray<b3Vector3>& verticesB,
+	b3Scalar maximumDistanceSquared,
+	b3Vector3& resultSepNormal,
+	float& resultSepDistance,
+	b3Vector3& resultPointOnB);
+
+#endif //B3_GJK_PAIR_DETECTOR_H
--- a/src/Bullet3OpenCL/NarrowphaseCollision/b3SupportMappings.h
+++ b/src/Bullet3OpenCL/NarrowphaseCollision/b3SupportMappings.h
@@ -0,0 +1,36 @@
+
+#ifndef B3_SUPPORT_MAPPINGS_H
+#define B3_SUPPORT_MAPPINGS_H
+
+#include "Bullet3Common/b3Transform.h"
+#include "Bullet3Common/b3AlignedObjectArray.h"
+#include "b3VectorFloat4.h"
+
+struct b3ConvexPolyhedronCL;
+struct b3GjkPairDetector;
+
+inline b3Vector3 localGetSupportVertexWithMargin(const float4& supportVec,const struct b3ConvexPolyhedronCL* hull, 
+	const b3AlignedObjectArray<b3Vector3>& verticesA, b3Scalar margin)
+{
+	b3Vector3 supVec(b3Scalar(0.),b3Scalar(0.),b3Scalar(0.));
+	b3Scalar maxDot = b3Scalar(-B3_LARGE_FLOAT);
+
+    // Here we take advantage of dot(a, b*c) = dot(a*b, c).  Note: This is true mathematically, but not numerically. 
+    if( 0 < hull->m_numVertices )
+    {
+        const b3Vector3 scaled = supportVec;
+		int index = (int) scaled.maxDot( &verticesA[hull->m_vertexOffset], hull->m_numVertices, maxDot); 
+        return verticesA[hull->m_vertexOffset+index];
+    }
+
+    return supVec;
+
+}
+
+inline b3Vector3 localGetSupportVertexWithoutMargin(const float4& supportVec,const struct b3ConvexPolyhedronCL* hull, 
+	const b3AlignedObjectArray<b3Vector3>& verticesA)
+{
+	return localGetSupportVertexWithMargin(supportVec,hull,verticesA,0.f);
+}
+
+#endif //B3_SUPPORT_MAPPINGS_H
--- a/src/Bullet3OpenCL/NarrowphaseCollision/b3VectorFloat4.h
+++ b/src/Bullet3OpenCL/NarrowphaseCollision/b3VectorFloat4.h
@@ -0,0 +1,20 @@
+#ifndef B3_VECTOR_FLOAT4_H
+#define B3_VECTOR_FLOAT4_H
+
+#include "Bullet3Common/b3Transform.h"
+
+#define cross3(a,b) (a.cross(b))
+#define float4 b3Vector3
+#define make_float4(x,y,z,w) b3Vector4(x,y,z,w)
+
+inline b3Vector3 transform(const b3Vector3* v, const b3Vector3* pos, const b3Quaternion* orn)
+{
+	b3Transform tr;
+	tr.setIdentity();
+	tr.setOrigin(*pos);
+	tr.setRotation(*orn);
+	b3Vector3 res = tr(*v);
+	return res;
+}
+
+#endif //B3_VECTOR_FLOAT4_H
--- a/src/Bullet3OpenCL/NarrowphaseCollision/b3VoronoiSimplexSolver.cpp
+++ b/src/Bullet3OpenCL/NarrowphaseCollision/b3VoronoiSimplexSolver.cpp
@@ -0,0 +1,609 @@
+
+/*
+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 "b3VoronoiSimplexSolver.h"
+
+#define VERTA  0
+#define VERTB  1
+#define VERTC  2
+#define VERTD  3
+
+#define B3_CATCH_DEGENERATE_TETRAHEDRON 1
+void	b3VoronoiSimplexSolver::removeVertex(int index)
+{
+	
+	b3Assert(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	b3VoronoiSimplexSolver::reduceVertices (const b3UsageBitfield& 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 b3VoronoiSimplexSolver::reset()
+{
+	m_cachedValidClosest = false;
+	m_numVertices = 0;
+	m_needsUpdate = true;
+	m_lastW = b3Vector3(b3Scalar(B3_LARGE_FLOAT),b3Scalar(B3_LARGE_FLOAT),b3Scalar(B3_LARGE_FLOAT));
+	m_cachedBC.reset();
+}
+
+
+
+	//add a vertex
+void b3VoronoiSimplexSolver::addVertex(const b3Vector3& w, const b3Vector3& p, const b3Vector3& 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	b3VoronoiSimplexSolver::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(b3Scalar(1.),b3Scalar(0.),b3Scalar(0.),b3Scalar(0.));
+				m_cachedValidClosest = m_cachedBC.isValid();
+				break;
+			};
+		case 2:
+			{
+			//closest point origin from line segment
+					const b3Vector3& from = m_simplexVectorW[0];
+					const b3Vector3& to = m_simplexVectorW[1];
+					b3Vector3 nearest;
+
+					b3Vector3 p (b3Scalar(0.),b3Scalar(0.),b3Scalar(0.));
+					b3Vector3 diff = p - from;
+					b3Vector3 v = to - from;
+					b3Scalar t = v.dot(diff);
+					
+					if (t > 0) {
+						b3Scalar 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 
+				b3Vector3 p (b3Scalar(0.),b3Scalar(0.),b3Scalar(0.)); 
+
+				const b3Vector3& a = m_simplexVectorW[0]; 
+				const b3Vector3& b = m_simplexVectorW[1]; 
+				const b3Vector3& 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_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_cachedV = m_cachedP1-m_cachedP2; 
+
+				reduceVertices (m_cachedBC.m_usedVertices); 
+				m_cachedValidClosest = m_cachedBC.isValid(); 
+
+				break; 
+			}
+		case 4:
+			{
+
+				
+				b3Vector3 p (b3Scalar(0.),b3Scalar(0.),b3Scalar(0.));
+				
+				const b3Vector3& a = m_simplexVectorW[0];
+				const b3Vector3& b = m_simplexVectorW[1];
+				const b3Vector3& c = m_simplexVectorW[2];
+				const b3Vector3& 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(b3Scalar(0.),b3Scalar(0.),b3Scalar(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 b3VoronoiSimplexSolver::closest(b3Vector3& v)
+{
+	bool succes = updateClosestVectorAndPoints();
+	v = m_cachedV;
+	return succes;
+}
+
+
+
+b3Scalar b3VoronoiSimplexSolver::maxVertex()
+{
+	int i, numverts = numVertices();
+	b3Scalar maxV = b3Scalar(0.);
+	for (i=0;i<numverts;i++)
+	{
+		b3Scalar curLen2 = m_simplexVectorW[i].length2();
+		if (maxV < curLen2)
+			maxV = curLen2;
+	}
+	return maxV;
+}
+
+
+
+	//return the current simplex
+int b3VoronoiSimplexSolver::getSimplex(b3Vector3 *pBuf, b3Vector3 *qBuf, b3Vector3 *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 b3VoronoiSimplexSolver::inSimplex(const b3Vector3& w)
+{
+	bool found = false;
+	int i, numverts = numVertices();
+	//b3Scalar maxV = b3Scalar(0.);
+	
+	//w is in the current (reduced) simplex
+	for (i=0;i<numverts;i++)
+	{
+#ifdef BT_USE_EQUAL_VERTEX_THRESHOLD
+		if ( m_simplexVectorW[i].distance2(w) <= m_equalVertexThreshold)
+#else
+		if (m_simplexVectorW[i] == w)
+#endif
+			found = true;
+	}
+
+	//check in case lastW is already removed
+	if (w == m_lastW)
+		return true;
+    	
+	return found;
+}
+
+void b3VoronoiSimplexSolver::backup_closest(b3Vector3& v) 
+{
+	v = m_cachedV;
+}
+
+
+bool b3VoronoiSimplexSolver::emptySimplex() const 
+{
+	return (numVertices() == 0);
+
+}
+
+void b3VoronoiSimplexSolver::compute_points(b3Vector3& p1, b3Vector3& p2) 
+{
+	updateClosestVectorAndPoints();
+	p1 = m_cachedP1;
+	p2 = m_cachedP2;
+
+}
+
+
+
+
+bool	b3VoronoiSimplexSolver::closestPtPointTriangle(const b3Vector3& p, const b3Vector3& a, const b3Vector3& b, const b3Vector3& c,b3SubSimplexClosestResult& result)
+{
+	result.m_usedVertices.reset();
+
+    // Check if P in vertex region outside A
+    b3Vector3 ab = b - a;
+    b3Vector3 ac = c - a;
+    b3Vector3 ap = p - a;
+    b3Scalar d1 = ab.dot(ap);
+    b3Scalar d2 = ac.dot(ap);
+    if (d1 <= b3Scalar(0.0) && d2 <= b3Scalar(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
+    b3Vector3 bp = p - b;
+    b3Scalar d3 = ab.dot(bp);
+    b3Scalar d4 = ac.dot(bp);
+    if (d3 >= b3Scalar(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
+    b3Scalar vc = d1*d4 - d3*d2;
+    if (vc <= b3Scalar(0.0) && d1 >= b3Scalar(0.0) && d3 <= b3Scalar(0.0)) {
+        b3Scalar 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
+    b3Vector3 cp = p - c;
+    b3Scalar d5 = ab.dot(cp);
+    b3Scalar d6 = ac.dot(cp);
+    if (d6 >= b3Scalar(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
+    b3Scalar vb = d5*d2 - d1*d6;
+    if (vb <= b3Scalar(0.0) && d2 >= b3Scalar(0.0) && d6 <= b3Scalar(0.0)) {
+        b3Scalar 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
+    b3Scalar va = d3*d6 - d5*d4;
+    if (va <= b3Scalar(0.0) && (d4 - d3) >= b3Scalar(0.0) && (d5 - d6) >= b3Scalar(0.0)) {
+        b3Scalar 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)
+    b3Scalar denom = b3Scalar(1.0) / (va + vb + vc);
+    b3Scalar v = vb * denom;
+    b3Scalar 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 = b3Scalar(1.0) - v - w
+
+}
+
+
+
+
+
+/// Test if point p and d lie on opposite sides of plane through abc
+int b3VoronoiSimplexSolver::pointOutsideOfPlane(const b3Vector3& p, const b3Vector3& a, const b3Vector3& b, const b3Vector3& c, const b3Vector3& d)
+{
+	b3Vector3 normal = (b-a).cross(c-a);
+
+    b3Scalar signp = (p - a).dot(normal); // [AP AB AC]
+    b3Scalar signd = (d - a).dot( normal); // [AD AB AC]
+
+#ifdef B3_CATCH_DEGENERATE_TETRAHEDRON
+#ifdef BT_USE_DOUBLE_PRECISION
+if (signd * signd < (b3Scalar(1e-8) * b3Scalar(1e-8)))
+	{
+		return -1;
+	}
+#else
+	if (signd * signd < (b3Scalar(1e-4) * b3Scalar(1e-4)))
+	{
+//		printf("affine dependent/degenerate\n");//
+		return -1;
+	}
+#endif
+
+#endif
+	// Points on opposite sides if expression signs are opposite
+    return signp * signd < b3Scalar(0.);
+}
+
+
+bool	b3VoronoiSimplexSolver::closestPtPointTetrahedron(const b3Vector3& p, const b3Vector3& a, const b3Vector3& b, const b3Vector3& c, const b3Vector3& d, b3SubSimplexClosestResult& finalResult)
+{
+	b3SubSimplexClosestResult 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;
+	 }
+
+
+    b3Scalar bestSqDist = FLT_MAX;
+    // If point outside face abc then compute closest point on abc
+	if (pointOutsideABC) 
+	{
+        closestPtPointTriangle(p, a, b, c,tempResult);
+		b3Vector3 q = tempResult.m_closestPointOnSimplex;
+		
+        b3Scalar 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);
+		b3Vector3 q = tempResult.m_closestPointOnSimplex;
+		//convert result bitmask!
+
+        b3Scalar 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);
+		b3Vector3 q = tempResult.m_closestPointOnSimplex;
+		//convert result bitmask!
+
+        b3Scalar 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.usedVertexB = tempResult.m_usedVertices.usedVertexC;
+			
+			finalResult.m_usedVertices.usedVertexD = tempResult.m_usedVertices.usedVertexB;
+			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);
+		b3Vector3 q = tempResult.m_closestPointOnSimplex;
+		//convert result bitmask!
+        b3Scalar 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.usedVertexC = tempResult.m_usedVertices.usedVertexC;
+			finalResult.m_usedVertices.usedVertexD = tempResult.m_usedVertices.usedVertexB;
+
+			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;
+}
+
--- a/src/Bullet3OpenCL/NarrowphaseCollision/b3VoronoiSimplexSolver.h
+++ b/src/Bullet3OpenCL/NarrowphaseCollision/b3VoronoiSimplexSolver.h
@@ -0,0 +1,177 @@
+/*
+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 B3_VORONOI_SIMPLEX_SOLVER_H
+#define B3_VORONOI_SIMPLEX_SOLVER_H
+
+#include "Bullet3Common/b3Vector3.h"
+
+
+#define VORONOI_SIMPLEX_MAX_VERTS 5
+
+///disable next define, or use defaultCollisionConfiguration->getSimplexSolver()->setEqualVertexThreshold(0.f) to disable/configure
+//#define BT_USE_EQUAL_VERTEX_THRESHOLD
+#define VORONOI_DEFAULT_EQUAL_VERTEX_THRESHOLD 0.0001f
+
+
+struct b3UsageBitfield{
+	b3UsageBitfield()
+	{
+		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	b3SubSimplexClosestResult
+{
+	b3Vector3	m_closestPointOnSimplex;
+	//MASK for m_usedVertices
+	//stores the simplex vertex-usage, using the MASK, 
+	// if m_usedVertices & MASK then the related vertex is used
+	b3UsageBitfield	m_usedVertices;
+	b3Scalar	m_barycentricCoords[4];
+	bool m_degenerate;
+
+	void	reset()
+	{
+		m_degenerate = false;
+		setBarycentricCoordinates();
+		m_usedVertices.reset();
+	}
+	bool	isValid()
+	{
+		bool valid = (m_barycentricCoords[0] >= b3Scalar(0.)) &&
+			(m_barycentricCoords[1] >= b3Scalar(0.)) &&
+			(m_barycentricCoords[2] >= b3Scalar(0.)) &&
+			(m_barycentricCoords[3] >= b3Scalar(0.));
+
+
+		return valid;
+	}
+	void	setBarycentricCoordinates(b3Scalar a=b3Scalar(0.),b3Scalar b=b3Scalar(0.),b3Scalar c=b3Scalar(0.),b3Scalar d=b3Scalar(0.))
+	{
+		m_barycentricCoords[0] = a;
+		m_barycentricCoords[1] = b;
+		m_barycentricCoords[2] = c;
+		m_barycentricCoords[3] = d;
+	}
+
+};
+
+/// b3VoronoiSimplexSolver 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.
+
+B3_ATTRIBUTE_ALIGNED16(class) b3VoronoiSimplexSolver 
+{
+public:
+
+	B3_DECLARE_ALIGNED_ALLOCATOR();
+
+	int	m_numVertices;
+
+	b3Vector3	m_simplexVectorW[VORONOI_SIMPLEX_MAX_VERTS];
+	b3Vector3	m_simplexPointsP[VORONOI_SIMPLEX_MAX_VERTS];
+	b3Vector3	m_simplexPointsQ[VORONOI_SIMPLEX_MAX_VERTS];
+
+	
+
+	b3Vector3	m_cachedP1;
+	b3Vector3	m_cachedP2;
+	b3Vector3	m_cachedV;
+	b3Vector3	m_lastW;
+	
+	b3Scalar	m_equalVertexThreshold;
+	bool		m_cachedValidClosest;
+
+
+	b3SubSimplexClosestResult m_cachedBC;
+
+	bool	m_needsUpdate;
+	
+	void	removeVertex(int index);
+	void	reduceVertices (const b3UsageBitfield& usedVerts);
+	bool	updateClosestVectorAndPoints();
+
+	bool	closestPtPointTetrahedron(const b3Vector3& p, const b3Vector3& a, const b3Vector3& b, const b3Vector3& c, const b3Vector3& d, b3SubSimplexClosestResult& finalResult);
+	int		pointOutsideOfPlane(const b3Vector3& p, const b3Vector3& a, const b3Vector3& b, const b3Vector3& c, const b3Vector3& d);
+	bool	closestPtPointTriangle(const b3Vector3& p, const b3Vector3& a, const b3Vector3& b, const b3Vector3& c,b3SubSimplexClosestResult& result);
+
+public:
+
+	b3VoronoiSimplexSolver()
+		:  m_equalVertexThreshold(VORONOI_DEFAULT_EQUAL_VERTEX_THRESHOLD)
+	{
+	}
+	 void reset();
+
+	 void addVertex(const b3Vector3& w, const b3Vector3& p, const b3Vector3& q);
+
+	 void	setEqualVertexThreshold(b3Scalar threshold)
+	 {
+		 m_equalVertexThreshold = threshold;
+	 }
+
+	 b3Scalar	getEqualVertexThreshold() const
+	 {
+		 return m_equalVertexThreshold;
+	 }
+
+	 bool closest(b3Vector3& v);
+
+	 b3Scalar maxVertex();
+
+	 bool fullSimplex() const
+	 {
+		 return (m_numVertices == 4);
+	 }
+
+	 int getSimplex(b3Vector3 *pBuf, b3Vector3 *qBuf, b3Vector3 *yBuf) const;
+
+	 bool inSimplex(const b3Vector3& w);
+	
+	 void backup_closest(b3Vector3& v) ;
+
+	 bool emptySimplex() const ;
+
+	 void compute_points(b3Vector3& p1, b3Vector3& p2) ;
+
+	 int numVertices() const 
+	 {
+		 return m_numVertices;
+	 }
+
+
+};
+
+#endif //B3_VORONOI_SIMPLEX_SOLVER_H
+
--- a/src/Bullet3OpenCL/NarrowphaseCollision/kernels/primitiveContacts.cl
+++ b/src/Bullet3OpenCL/NarrowphaseCollision/kernels/primitiveContacts.cl
@@ -597,7 +597,7 @@ int extractManifoldSequential(const float4* p, int nPoints, float4 nearNormal, i

 #define MAX_PLANE_CONVEX_POINTS 64

-void computeContactPlaneConvex(int pairIndex,
+int computeContactPlaneConvex(int pairIndex,
 								int bodyIndexA, int bodyIndexB, 
 								int collidableIndexA, int collidableIndexB, 
 								__global const BodyData* rigidBodies, 
@@ -613,6 +613,7 @@ void computeContactPlaneConvex(int pairIndex,
 								Quaternion ornB
 								)
 {
+	int resultIndex=-1;

 		int shapeIndex = collidables[collidableIndexB].m_shapeIndex;
 	__global const ConvexPolyhedronCL* hullB = &convexShapes[shapeIndex];
@@ -706,6 +707,7 @@ void computeContactPlaneConvex(int pairIndex,

 		if (dstIdx < maxContactCapacity)
 		{
+			resultIndex = dstIdx;
 			__global Contact4* c = &globalContactsOut[dstIdx];
 			c->m_worldNormal = planeNormalWorld;
 			//c->setFrictionCoeff(0.7);
@@ -735,6 +737,8 @@ void computeContactPlaneConvex(int pairIndex,
 			GET_NPOINTS(*c) = numReducedPoints;
 		}//if (dstIdx < numPairs)
 	}	
+
+	return resultIndex;
 }


@@ -802,7 +806,7 @@ void	computeContactPlaneSphere(int pairIndex,
 }


-__kernel void   primitiveContactsKernel( __global const int4* pairs, 
+__kernel void   primitiveContactsKernel( __global int4* pairs, 
 																					__global const BodyData* rigidBodies, 
 																					__global const btCollidableGpu* collidables,
 																					__global const ConvexPolyhedronCL* convexShapes, 
@@ -845,9 +849,12 @@ __kernel void   primitiveContactsKernel( __global const int4* pairs,
 			posB = rigidBodies[bodyIndexB].m_pos;
 			Quaternion ornB;
 			ornB = rigidBodies[bodyIndexB].m_quat;
-			computeContactPlaneConvex(pairIndex, bodyIndexA, bodyIndexB, collidableIndexA, collidableIndexB, 
+			int contactIndex = computeContactPlaneConvex(pairIndex, bodyIndexA, bodyIndexB, collidableIndexA, collidableIndexB, 
 																rigidBodies,collidables,convexShapes,vertices,indices,
 																faces,	globalContactsOut, nGlobalContactsOut,maxContactCapacity, posB,ornB);
+			if (contactIndex>=0)
+				pairs[pairIndex].z = contactIndex;
+
 			return;
 		}

@@ -862,10 +869,13 @@ __kernel void   primitiveContactsKernel( __global const int4* pairs,
 			ornA = rigidBodies[bodyIndexA].m_quat;


-			computeContactPlaneConvex( pairIndex, bodyIndexB,bodyIndexA,  collidableIndexB,collidableIndexA, 
+			int contactIndex = computeContactPlaneConvex( pairIndex, bodyIndexB,bodyIndexA,  collidableIndexB,collidableIndexA, 
 																rigidBodies,collidables,convexShapes,vertices,indices,
 																faces,	globalContactsOut, nGlobalContactsOut,maxContactCapacity,posA,ornA);

+			if (contactIndex>=0)
+				pairs[pairIndex].z = contactIndex;
+
 			return;
 		}

--- a/src/Bullet3OpenCL/NarrowphaseCollision/kernels/primitiveContacts.h
+++ b/src/Bullet3OpenCL/NarrowphaseCollision/kernels/primitiveContacts.h
@@ -599,7 +599,7 @@ static const char* primitiveContactsKernelsCL= \
 "\n"
 "#define MAX_PLANE_CONVEX_POINTS 64\n"
 "\n"
-"void computeContactPlaneConvex(int pairIndex,\n"
+"int computeContactPlaneConvex(int pairIndex,\n"
 "								int bodyIndexA, int bodyIndexB, \n"
 "								int collidableIndexA, int collidableIndexB, \n"
 "								__global const BodyData* rigidBodies, \n"
@@ -615,6 +615,7 @@ static const char* primitiveContactsKernelsCL= \
 "								Quaternion ornB\n"
 "								)\n"
 "{\n"
+"	int resultIndex=-1;\n"
 "\n"
 "		int shapeIndex = collidables[collidableIndexB].m_shapeIndex;\n"
 "	__global const ConvexPolyhedronCL* hullB = &convexShapes[shapeIndex];\n"
@@ -708,6 +709,7 @@ static const char* primitiveContactsKernelsCL= \
 "\n"
 "		if (dstIdx < maxContactCapacity)\n"
 "		{\n"
+"			resultIndex = dstIdx;\n"
 "			__global Contact4* c = &globalContactsOut[dstIdx];\n"
 "			c->m_worldNormal = planeNormalWorld;\n"
 "			//c->setFrictionCoeff(0.7);\n"
@@ -737,6 +739,8 @@ static const char* primitiveContactsKernelsCL= \
 "			GET_NPOINTS(*c) = numReducedPoints;\n"
 "		}//if (dstIdx < numPairs)\n"
 "	}	\n"
+"\n"
+"	return resultIndex;\n"
 "}\n"
 "\n"
 "\n"
@@ -804,7 +808,7 @@ static const char* primitiveContactsKernelsCL= \
 "}\n"
 "\n"
 "\n"
-"__kernel void   primitiveContactsKernel( __global const int4* pairs, \n"
+"__kernel void   primitiveContactsKernel( __global int4* pairs, \n"
 "																					__global const BodyData* rigidBodies, \n"
 "																					__global const btCollidableGpu* collidables,\n"
 "																					__global const ConvexPolyhedronCL* convexShapes, \n"
@@ -847,9 +851,12 @@ static const char* primitiveContactsKernelsCL= \
 "			posB = rigidBodies[bodyIndexB].m_pos;\n"
 "			Quaternion ornB;\n"
 "			ornB = rigidBodies[bodyIndexB].m_quat;\n"
-"			computeContactPlaneConvex(pairIndex, bodyIndexA, bodyIndexB, collidableIndexA, collidableIndexB, \n"
+"			int contactIndex = computeContactPlaneConvex(pairIndex, bodyIndexA, bodyIndexB, collidableIndexA, collidableIndexB, \n"
 "																rigidBodies,collidables,convexShapes,vertices,indices,\n"
 "																faces,	globalContactsOut, nGlobalContactsOut,maxContactCapacity, posB,ornB);\n"
+"			if (contactIndex>=0)\n"
+"				pairs[pairIndex].z = contactIndex;\n"
+"\n"
 "			return;\n"
 "		}\n"
 "\n"
@@ -864,10 +871,13 @@ static const char* primitiveContactsKernelsCL= \
 "			ornA = rigidBodies[bodyIndexA].m_quat;\n"
 "\n"
 "\n"
-"			computeContactPlaneConvex( pairIndex, bodyIndexB,bodyIndexA,  collidableIndexB,collidableIndexA, \n"
+"			int contactIndex = computeContactPlaneConvex( pairIndex, bodyIndexB,bodyIndexA,  collidableIndexB,collidableIndexA, \n"
 "																rigidBodies,collidables,convexShapes,vertices,indices,\n"
 "																faces,	globalContactsOut, nGlobalContactsOut,maxContactCapacity,posA,ornA);\n"
 "\n"
+"			if (contactIndex>=0)\n"
+"				pairs[pairIndex].z = contactIndex;\n"
+"\n"
 "			return;\n"
 "		}\n"
 "\n"
--- a/src/Bullet3OpenCL/NarrowphaseCollision/kernels/satClipHullContacts.cl
+++ b/src/Bullet3OpenCL/NarrowphaseCollision/kernels/satClipHullContacts.cl
@@ -45,15 +45,15 @@ typedef struct
 {
 	float4 m_worldPos[4];
 	float4 m_worldNormal;	//	w: m_nPoints
+
 	u32 m_coeffs;
 	u32 m_batchIdx;
-
 	int m_bodyAPtrAndSignBit;//x:m_bodyAPtr, y:m_bodyBPtr
 	int m_bodyBPtrAndSignBit;

 	int	m_childIndexA;
 	int	m_childIndexB;
-	int m_unused1;
+	float m_unused1;
 	int m_unused2;

 } Contact4;
@@ -960,7 +960,7 @@ void	trMul(float4 translationA, Quaternion orientationA,



-__kernel void   clipHullHullKernel( __global const int4* pairs, 
+__kernel void   clipHullHullKernel( __global int4* pairs, 
 																					__global const BodyData* rigidBodies, 
 																					__global const btCollidableGpu* collidables,
 																					__global const ConvexPolyhedronCL* convexShapes, 
@@ -972,7 +972,8 @@ __kernel void   clipHullHullKernel( __global const int4* pairs,
 																					__global const int* hasSeparatingAxis,
 																					__global Contact4* restrict globalContactsOut,
 																					counter32_t nGlobalContactsOut,
-																					int numPairs)
+																					int numPairs,
+																					int contactCapacity)
 {

 	int i = get_global_id(0);
@@ -1032,8 +1033,10 @@ __kernel void   clipHullHullKernel( __global const int4* pairs,
 		
 				int dstIdx;
 				AppendInc( nGlobalContactsOut, dstIdx );
-				//if ((dstIdx+nReducedContacts) < capacity)
+				if (dstIdx<contactCapacity)
 				{
+					pairs[pairIndex].z = dstIdx;
+
 					__global Contact4* c = globalContactsOut+ dstIdx;
 					c->m_worldNormal = normal;
 					c->m_coeffs = (u32)(0.f*0xffff) | ((u32)(0.7f*0xffff)<<16);
@@ -1740,7 +1743,7 @@ __kernel void   findClippingFacesKernel(  __global const int4* pairs,



-__kernel void   clipFacesAndContactReductionKernel( __global const int4* pairs,
+__kernel void   clipFacesAndContactReductionKernel( __global int4* pairs,
                                                   __global const BodyData* rigidBodies,
                                                   __global const float4* separatingNormals,
                                                   __global const int* hasSeparatingAxis,
@@ -1853,7 +1856,7 @@ __kernel void   clipFacesAndContactReductionKernel( __global const int4* pairs,



-__kernel void   newContactReductionKernel( __global const int4* pairs,
+__kernel void   newContactReductionKernel( __global int4* pairs,
                                                   __global const BodyData* rigidBodies,
                                                   __global const float4* separatingNormals,
                                                   __global const int* hasSeparatingAxis,
@@ -1897,12 +1900,16 @@ __kernel void   newContactReductionKernel( __global const int4* pairs,
                
 				if (dstIdx < numPairs)
 				{
+
 					__global Contact4* c = &globalContactsOut[dstIdx];
 					c->m_worldNormal = normal;
 					c->m_coeffs = (u32)(0.f*0xffff) | ((u32)(0.7f*0xffff)<<16);
 					c->m_batchIdx = pairIndex;
 					int bodyA = pairs[pairIndex].x;
 					int bodyB = pairs[pairIndex].y;
+
+					pairs[pairIndex].w = dstIdx;
+
 					c->m_bodyAPtrAndSignBit = rigidBodies[bodyA].m_invMass==0?-bodyA:bodyA;
 					c->m_bodyBPtrAndSignBit = rigidBodies[bodyB].m_invMass==0?-bodyB:bodyB;
                    c->m_childIndexA =-1;
--- a/src/Bullet3OpenCL/NarrowphaseCollision/kernels/satClipHullContacts.h
+++ b/src/Bullet3OpenCL/NarrowphaseCollision/kernels/satClipHullContacts.h
@@ -962,7 +962,7 @@ static const char* satClipKernelsCL= \
 "\n"
 "\n"
 "\n"
-"__kernel void   clipHullHullKernel( __global const int4* pairs, \n"
+"__kernel void   clipHullHullKernel( __global int4* pairs, \n"
 "																					__global const BodyData* rigidBodies, \n"
 "																					__global const btCollidableGpu* collidables,\n"
 "																					__global const ConvexPolyhedronCL* convexShapes, \n"
@@ -974,7 +974,8 @@ static const char* satClipKernelsCL= \
 "																					__global const int* hasSeparatingAxis,\n"
 "																					__global Contact4* restrict globalContactsOut,\n"
 "																					counter32_t nGlobalContactsOut,\n"
-"																					int numPairs)\n"
+"																					int numPairs,\n"
+"																					int contactCapacity)\n"
 "{\n"
 "\n"
 "	int i = get_global_id(0);\n"
@@ -1034,8 +1035,10 @@ static const char* satClipKernelsCL= \
 "		\n"
 "				int dstIdx;\n"
 "				AppendInc( nGlobalContactsOut, dstIdx );\n"
-"				//if ((dstIdx+nReducedContacts) < capacity)\n"
+"				if (dstIdx<contactCapacity)\n"
 "				{\n"
+"					pairs[pairIndex].z = dstIdx;\n"
+"\n"
 "					__global Contact4* c = globalContactsOut+ dstIdx;\n"
 "					c->m_worldNormal = normal;\n"
 "					c->m_coeffs = (u32)(0.f*0xffff) | ((u32)(0.7f*0xffff)<<16);\n"
@@ -1742,7 +1745,7 @@ static const char* satClipKernelsCL= \
 "\n"
 "\n"
 "\n"
-"__kernel void   clipFacesAndContactReductionKernel( __global const int4* pairs,\n"
+"__kernel void   clipFacesAndContactReductionKernel( __global int4* pairs,\n"
 "                                                   __global const BodyData* rigidBodies,\n"
 "                                                   __global const float4* separatingNormals,\n"
 "                                                   __global const int* hasSeparatingAxis,\n"
@@ -1855,7 +1858,7 @@ static const char* satClipKernelsCL= \
 "\n"
 "\n"
 "\n"
-"__kernel void   newContactReductionKernel( __global const int4* pairs,\n"
+"__kernel void   newContactReductionKernel( __global int4* pairs,\n"
 "                                                   __global const BodyData* rigidBodies,\n"
 "                                                   __global const float4* separatingNormals,\n"
 "                                                   __global const int* hasSeparatingAxis,\n"
@@ -1899,12 +1902,16 @@ static const char* satClipKernelsCL= \
 "                \n"
 "				if (dstIdx < numPairs)\n"
 "				{\n"
+"\n"
 "					__global Contact4* c = &globalContactsOut[dstIdx];\n"
 "					c->m_worldNormal = normal;\n"
 "					c->m_coeffs = (u32)(0.f*0xffff) | ((u32)(0.7f*0xffff)<<16);\n"
 "					c->m_batchIdx = pairIndex;\n"
 "					int bodyA = pairs[pairIndex].x;\n"
 "					int bodyB = pairs[pairIndex].y;\n"
+"\n"
+"					pairs[pairIndex].w = dstIdx;\n"
+"\n"
 "					c->m_bodyAPtrAndSignBit = rigidBodies[bodyA].m_invMass==0?-bodyA:bodyA;\n"
 "					c->m_bodyBPtrAndSignBit = rigidBodies[bodyB].m_invMass==0?-bodyB:bodyB;\n"
 "                    c->m_childIndexA =-1;\n"
--- a/src/Bullet3OpenCL/RigidBody/b3GpuNarrowPhase.cpp
+++ b/src/Bullet3OpenCL/RigidBody/b3GpuNarrowPhase.cpp
@@ -25,14 +25,16 @@ m_queue(queue)
 {
    
 	m_data = new b3GpuNarrowPhaseInternalData();
+	m_data->m_currentContactBuffer = 0;
+
 	memset(m_data,0,sizeof(b3GpuNarrowPhaseInternalData));
    

 	m_data->m_config = config;
 	
 	m_data->m_gpuSatCollision = new GpuSatCollision(ctx,device,queue);
-	m_data->m_pBufPairsCPU = new b3AlignedObjectArray<b3Int2>;
-	m_data->m_pBufPairsCPU->resize(config.m_maxBroadphasePairs);
+	
+	
 	m_data->m_triangleConvexPairs = new b3OpenCLArray<b3Int4>(m_context,m_queue, config.m_maxTriConvexPairCapacity);


@@ -47,7 +49,8 @@ m_queue(queue)
 	m_data->m_inertiaBufferCPU = new b3AlignedObjectArray<b3InertiaCL>();
 	m_data->m_inertiaBufferCPU->resize(config.m_maxConvexBodies);
 	
-	m_data->m_pBufContactOutGPU = new b3OpenCLArray<b3Contact4>(ctx,queue, config.m_maxContactCapacity,true);
+	m_data->m_pBufContactBuffersGPU[0] = new b3OpenCLArray<b3Contact4>(ctx,queue, config.m_maxContactCapacity,true);
+	m_data->m_pBufContactBuffersGPU[1] = new b3OpenCLArray<b3Contact4>(ctx,queue, config.m_maxContactCapacity,true);
 	
 	m_data->m_inertiaBufferGPU = new b3OpenCLArray<b3InertiaCL>(ctx,queue,config.m_maxConvexBodies,false);
 	m_data->m_collidablesGPU = new b3OpenCLArray<b3Collidable>(ctx,queue,config.m_maxConvexShapes);
@@ -111,14 +114,17 @@ m_queue(queue)
 b3GpuNarrowPhase::~b3GpuNarrowPhase()
 {
 	delete m_data->m_gpuSatCollision;
-	delete m_data->m_pBufPairsCPU;
+	
 	delete m_data->m_triangleConvexPairs;
 	//delete m_data->m_convexPairsOutGPU;
 	//delete m_data->m_planePairs;
 	delete m_data->m_pBufContactOutCPU;
 	delete m_data->m_bodyBufferCPU;
 	delete m_data->m_inertiaBufferCPU;
-	delete m_data->m_pBufContactOutGPU;
+	delete m_data->m_pBufContactBuffersGPU[0];
+	delete m_data->m_pBufContactBuffersGPU[1];
+
+
 	delete m_data->m_inertiaBufferGPU;
 	delete m_data->m_collidablesGPU;
 	delete m_data->m_localShapeAABBCPU;
@@ -707,16 +713,16 @@ int	b3GpuNarrowPhase::getNumCollidablesGpu() const

 int	b3GpuNarrowPhase::getNumContactsGpu() const
 {
-	return m_data->m_pBufContactOutGPU->size();
+	return m_data->m_pBufContactBuffersGPU[m_data->m_currentContactBuffer]->size();
 }
 cl_mem b3GpuNarrowPhase::getContactsGpu()
 {
-	return m_data->m_pBufContactOutGPU->getBufferCL();
+	return m_data->m_pBufContactBuffersGPU[m_data->m_currentContactBuffer]->getBufferCL();
 }

 const b3Contact4* b3GpuNarrowPhase::getContactsCPU() const
 {
-	m_data->m_pBufContactOutGPU->copyToHost(*m_data->m_pBufContactOutCPU);
+	m_data->m_pBufContactBuffersGPU[m_data->m_currentContactBuffer]->copyToHost(*m_data->m_pBufContactOutCPU);
 	return &m_data->m_pBufContactOutCPU->at(0);
 }

@@ -724,19 +730,29 @@ void b3GpuNarrowPhase::computeContacts(cl_mem broadphasePairs, int numBroadphase
 {
 	int nContactOut = 0;

+	//swap buffer
+	m_data->m_currentContactBuffer=1-m_data->m_currentContactBuffer;
+
+	int curSize = m_data->m_pBufContactBuffersGPU[m_data->m_currentContactBuffer]->size();
+
 	int maxTriConvexPairCapacity = m_data->m_config.m_maxTriConvexPairCapacity;
 	int numTriConvexPairsOut=0;
 	
 	b3OpenCLArray<b3Int4> broadphasePairsGPU(m_context,m_queue);
 	broadphasePairsGPU.setFromOpenCLBuffer(broadphasePairs,numBroadphasePairs);
+
+	
+
+
 	b3OpenCLArray<b3YetAnotherAabb> clAabbArray(this->m_context,this->m_queue);
 	clAabbArray.setFromOpenCLBuffer(aabbsWS,numObjects);

 	m_data->m_gpuSatCollision->computeConvexConvexContactsGPUSAT(
 		&broadphasePairsGPU, numBroadphasePairs,
 		m_data->m_bodyBufferGPU,
-		m_data->m_pBufContactOutGPU,
+		m_data->m_pBufContactBuffersGPU[m_data->m_currentContactBuffer],
 		nContactOut,
+		m_data->m_pBufContactBuffersGPU[1-m_data->m_currentContactBuffer],
 		m_data->m_config.m_maxContactCapacity,
 		m_data->m_config.m_compoundPairCapacity,
 		*m_data->m_convexPolyhedraGPU,
@@ -762,6 +778,10 @@ void b3GpuNarrowPhase::computeContacts(cl_mem broadphasePairs, int numBroadphase
 		numTriConvexPairsOut
 		);

+	/*b3AlignedObjectArray<b3Int4> broadphasePairsCPU;
+	broadphasePairsGPU.copyToHost(broadphasePairsCPU);
+	printf("checking pairs\n");
+	*/
 }

 const b3SapAabb& b3GpuNarrowPhase::getLocalSpaceAabb(int collidableIndex) const
--- a/src/Bullet3OpenCL/RigidBody/b3GpuNarrowPhaseInternalData.h
+++ b/src/Bullet3OpenCL/RigidBody/b3GpuNarrowPhaseInternalData.h
@@ -50,13 +50,12 @@ struct b3GpuNarrowPhaseInternalData
    
 	struct GpuSatCollision*	m_gpuSatCollision;
 	    
-	b3AlignedObjectArray<b3Int2>* m_pBufPairsCPU;
+	
 	b3OpenCLArray<b3Int4>*			m_triangleConvexPairs;
    
-	//b3OpenCLArray<b3Int2>* m_convexPairsOutGPU;
-	//b3OpenCLArray<b3Int2>* m_planePairs;
-    
-	b3OpenCLArray<b3Contact4>* m_pBufContactOutGPU;
+	
+	b3OpenCLArray<b3Contact4>* m_pBufContactBuffersGPU[2];
+	int	m_currentContactBuffer;
 	b3AlignedObjectArray<b3Contact4>* m_pBufContactOutCPU;
 	
    
--- a/src/Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipeline.cpp
+++ b/src/Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipeline.cpp
@@ -276,6 +276,12 @@ void	b3GpuRigidBodyPipeline::stepSimulation(float deltaTime)
 		m_data->m_narrowphase->computeContacts(pairs,numPairs,aabbsWS,numBodies);
 		numContacts = m_data->m_narrowphase->getNumContactsGpu();

+		if (useDbvt)
+		{
+			///store the cached information (contact locations in the 'z' component)
+			B3_PROFILE("m_overlappingPairsGPU->copyToHost");
+			m_data->m_overlappingPairsGPU->copyToHost(m_data->m_broadphaseDbvt->getOverlappingPairCache()->getOverlappingPairArray());
+		}
 		if (dumpContactStats && numContacts)
 		{
 			m_data->m_narrowphase->getContactsGpu();