Merge pull request #12641 from AndreaCatania/bullet

Bullet physics wrapper

16 files changed, 3587 insertions, 0 deletions

diff --git a/thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3BvhSubtreeInfoData.h b/thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3BvhSubtreeInfoData.h
new file mode 100644
index 0000000000..8788ccbb47
--- /dev/null
+++ b/thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3BvhSubtreeInfoData.h
@@ -0,0 +1,20 @@
+
+#ifndef B3_BVH_SUBTREE_INFO_DATA_H
+#define B3_BVH_SUBTREE_INFO_DATA_H
+
+typedef struct b3BvhSubtreeInfoData b3BvhSubtreeInfoData_t;
+
+struct b3BvhSubtreeInfoData
+{
+	//12 bytes
+	unsigned short int	m_quantizedAabbMin[3];
+	unsigned short int	m_quantizedAabbMax[3];
+	//4 bytes, points to the root of the subtree
+	int			m_rootNodeIndex;
+	//4 bytes
+	int			m_subtreeSize;
+	int			m_padding[3];
+};
+
+#endif //B3_BVH_SUBTREE_INFO_DATA_H
+
diff --git a/thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3BvhTraversal.h b/thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3BvhTraversal.h
new file mode 100644
index 0000000000..2618da24bc
--- /dev/null
+++ b/thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3BvhTraversal.h
@@ -0,0 +1,126 @@
+
+
+#include "Bullet3Common/shared/b3Int4.h"
+#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h"
+#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Collidable.h"
+#include "Bullet3Collision/BroadPhaseCollision/shared/b3Aabb.h"
+#include "Bullet3Collision/NarrowPhaseCollision/shared/b3BvhSubtreeInfoData.h"
+#include "Bullet3Collision/NarrowPhaseCollision/shared/b3QuantizedBvhNodeData.h"
+
+
+
+// work-in-progress
+void   b3BvhTraversal( __global const b3Int4* pairs, 
+									__global const b3RigidBodyData* rigidBodies, 
+									__global const b3Collidable* collidables,
+									__global b3Aabb* aabbs,
+									__global b3Int4* concavePairsOut,
+									__global volatile int* numConcavePairsOut,
+									__global const b3BvhSubtreeInfo* subtreeHeadersRoot,
+									__global const b3QuantizedBvhNode* quantizedNodesRoot,
+									__global const b3BvhInfo* bvhInfos,
+									int numPairs,
+									int maxNumConcavePairsCapacity,
+									int id)
+{
+	
+	int bodyIndexA = pairs[id].x;
+	int bodyIndexB = pairs[id].y;
+	int collidableIndexA = rigidBodies[bodyIndexA].m_collidableIdx;
+	int collidableIndexB = rigidBodies[bodyIndexB].m_collidableIdx;
+	
+	//once the broadphase avoids static-static pairs, we can remove this test
+	if ((rigidBodies[bodyIndexA].m_invMass==0) &&(rigidBodies[bodyIndexB].m_invMass==0))
+	{
+		return;
+	}
+		
+	if (collidables[collidableIndexA].m_shapeType!=SHAPE_CONCAVE_TRIMESH)
+		return;
+
+	int shapeTypeB = collidables[collidableIndexB].m_shapeType;
+		
+	if (shapeTypeB!=SHAPE_CONVEX_HULL &&
+		shapeTypeB!=SHAPE_SPHERE	&&
+		shapeTypeB!=SHAPE_COMPOUND_OF_CONVEX_HULLS
+		)
+		return;
+
+	b3BvhInfo bvhInfo = bvhInfos[collidables[collidableIndexA].m_numChildShapes];
+
+	b3Float4	bvhAabbMin = bvhInfo.m_aabbMin;
+	b3Float4	bvhAabbMax = bvhInfo.m_aabbMax;
+	b3Float4	bvhQuantization = bvhInfo.m_quantization;
+	int numSubtreeHeaders = bvhInfo.m_numSubTrees;
+	__global const b3BvhSubtreeInfoData* subtreeHeaders = &subtreeHeadersRoot[bvhInfo.m_subTreeOffset];
+	__global const b3QuantizedBvhNodeData* quantizedNodes = &quantizedNodesRoot[bvhInfo.m_nodeOffset];
+	
+
+	unsigned short int quantizedQueryAabbMin[3];
+	unsigned short int quantizedQueryAabbMax[3];
+	b3QuantizeWithClamp(quantizedQueryAabbMin,aabbs[bodyIndexB].m_minVec,false,bvhAabbMin, bvhAabbMax,bvhQuantization);
+	b3QuantizeWithClamp(quantizedQueryAabbMax,aabbs[bodyIndexB].m_maxVec,true ,bvhAabbMin, bvhAabbMax,bvhQuantization);
+	
+	for (int i=0;i<numSubtreeHeaders;i++)
+	{
+		b3BvhSubtreeInfoData subtree = subtreeHeaders[i];
+				
+		int overlap = b3TestQuantizedAabbAgainstQuantizedAabbSlow(quantizedQueryAabbMin,quantizedQueryAabbMax,subtree.m_quantizedAabbMin,subtree.m_quantizedAabbMax);
+		if (overlap != 0)
+		{
+			int startNodeIndex = subtree.m_rootNodeIndex;
+			int endNodeIndex = subtree.m_rootNodeIndex+subtree.m_subtreeSize;
+			int curIndex = startNodeIndex;
+			int escapeIndex;
+			int isLeafNode;
+			int aabbOverlap;
+			while (curIndex < endNodeIndex)
+			{
+				b3QuantizedBvhNodeData rootNode = quantizedNodes[curIndex];
+				aabbOverlap = b3TestQuantizedAabbAgainstQuantizedAabbSlow(quantizedQueryAabbMin,quantizedQueryAabbMax,rootNode.m_quantizedAabbMin,rootNode.m_quantizedAabbMax);
+				isLeafNode = b3IsLeaf(&rootNode);
+				if (aabbOverlap)
+				{
+					if (isLeafNode)
+					{
+						int triangleIndex = b3GetTriangleIndex(&rootNode);
+						if (shapeTypeB==SHAPE_COMPOUND_OF_CONVEX_HULLS)
+						{
+								int numChildrenB = collidables[collidableIndexB].m_numChildShapes;
+								int pairIdx = b3AtomicAdd (numConcavePairsOut,numChildrenB);
+								for (int b=0;b<numChildrenB;b++)
+								{
+									if ((pairIdx+b)<maxNumConcavePairsCapacity)
+									{
+										int childShapeIndexB = collidables[collidableIndexB].m_shapeIndex+b;
+										b3Int4 newPair = b3MakeInt4(bodyIndexA,bodyIndexB,triangleIndex,childShapeIndexB);
+										concavePairsOut[pairIdx+b] = newPair;
+									}
+								}
+						} else
+						{
+							int pairIdx = b3AtomicInc(numConcavePairsOut);
+							if (pairIdx<maxNumConcavePairsCapacity)
+							{
+								b3Int4 newPair = b3MakeInt4(bodyIndexA,bodyIndexB,triangleIndex,0);
+								concavePairsOut[pairIdx] = newPair;
+							}
+						}
+					} 
+					curIndex++;
+				} else
+				{
+					if (isLeafNode)
+					{
+						curIndex++;
+					} else
+					{
+						escapeIndex = b3GetEscapeIndex(&rootNode);
+						curIndex += escapeIndex;
+					}
+				}
+			}
+		}
+	}
+
+}
\ No newline at end of file
diff --git a/thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3ClipFaces.h b/thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3ClipFaces.h
new file mode 100644
index 0000000000..8009e7d6e0
--- /dev/null
+++ b/thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3ClipFaces.h
@@ -0,0 +1,188 @@
+#ifndef B3_CLIP_FACES_H
+#define B3_CLIP_FACES_H
+
+
+#include "Bullet3Common/shared/b3Int4.h"
+#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h"
+#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Collidable.h"
+#include "Bullet3Collision/BroadPhaseCollision/shared/b3Aabb.h"
+#include "Bullet3Collision/NarrowPhaseCollision/shared/b3BvhSubtreeInfoData.h"
+#include "Bullet3Collision/NarrowPhaseCollision/shared/b3QuantizedBvhNodeData.h"
+#include "Bullet3Collision/NarrowPhaseCollision/shared/b3ConvexPolyhedronData.h"
+
+
+inline b3Float4 b3Lerp3(b3Float4ConstArg a,b3Float4ConstArg b, float  t)
+{
+	return b3MakeFloat4(	a.x + (b.x - a.x) * t,
+						a.y + (b.y - a.y) * t,
+						a.z + (b.z - a.z) * t,
+						0.f);
+}
+
+// Clips a face to the back of a plane, return the number of vertices out, stored in ppVtxOut
+int clipFaceGlobal(__global const b3Float4* pVtxIn, int numVertsIn, b3Float4ConstArg planeNormalWS,float planeEqWS, __global b3Float4* ppVtxOut)
+{
+	
+	int ve;
+	float ds, de;
+	int numVertsOut = 0;
+    //double-check next test
+    //	if (numVertsIn < 2)
+    //		return 0;
+    
+	b3Float4 firstVertex=pVtxIn[numVertsIn-1];
+	b3Float4 endVertex = pVtxIn[0];
+	
+	ds = b3Dot(planeNormalWS,firstVertex)+planeEqWS;
+    
+	for (ve = 0; ve < numVertsIn; ve++)
+	{
+		endVertex=pVtxIn[ve];
+		de = b3Dot(planeNormalWS,endVertex)+planeEqWS;
+		if (ds<0)
+		{
+			if (de<0)
+			{
+				// Start < 0, end < 0, so output endVertex
+				ppVtxOut[numVertsOut++] = endVertex;
+			}
+			else
+			{
+				// Start < 0, end >= 0, so output intersection
+				ppVtxOut[numVertsOut++] = b3Lerp3(firstVertex, endVertex,(ds * 1.f/(ds - de)) );
+			}
+		}
+		else
+		{
+			if (de<0)
+			{
+				// Start >= 0, end < 0 so output intersection and end
+				ppVtxOut[numVertsOut++] = b3Lerp3(firstVertex, endVertex,(ds * 1.f/(ds - de)) );
+				ppVtxOut[numVertsOut++] = endVertex;
+			}
+		}
+		firstVertex = endVertex;
+		ds = de;
+	}
+	return numVertsOut;
+}
+
+
+__kernel void   clipFacesAndFindContactsKernel(    __global const b3Float4* separatingNormals,
+                                                   __global const int* hasSeparatingAxis,
+                                                   __global b3Int4* clippingFacesOut,
+                                                   __global b3Float4* worldVertsA1,
+                                                   __global b3Float4* worldNormalsA1,
+                                                   __global b3Float4* worldVertsB1,
+                                                   __global b3Float4* worldVertsB2,
+                                                    int vertexFaceCapacity,
+															int pairIndex
+                                                   )
+{
+//    int i = get_global_id(0);
+	//int pairIndex = i;
+	int i = pairIndex;
+    
+	float minDist = -1e30f;
+	float maxDist = 0.02f;
+    
+//	if (i<numPairs)
+	{
+        
+		if (hasSeparatingAxis[i])
+		{
+            
+//			int bodyIndexA = pairs[i].x;
+	//		int bodyIndexB = pairs[i].y;
+		    
+            int numLocalContactsOut = 0;
+
+            int capacityWorldVertsB2 = vertexFaceCapacity;
+            
+            __global b3Float4* pVtxIn = &worldVertsB1[pairIndex*capacityWorldVertsB2];
+            __global b3Float4* pVtxOut = &worldVertsB2[pairIndex*capacityWorldVertsB2];
+            
+
+            {
+                __global b3Int4* clippingFaces = clippingFacesOut;
+            
+                
+                int closestFaceA = clippingFaces[pairIndex].x;
+               // int closestFaceB = clippingFaces[pairIndex].y;
+                int numVertsInA = clippingFaces[pairIndex].z;
+                int numVertsInB = clippingFaces[pairIndex].w;
+                
+                int numVertsOut = 0;
+                
+                if (closestFaceA>=0)
+                {
+                    
+                    
+                    
+                    // clip polygon to back of planes of all faces of hull A that are adjacent to witness face
+                    
+                    for(int e0=0;e0<numVertsInA;e0++)
+                    {
+                        const b3Float4 aw = worldVertsA1[pairIndex*capacityWorldVertsB2+e0];
+                        const b3Float4 bw = worldVertsA1[pairIndex*capacityWorldVertsB2+((e0+1)%numVertsInA)];
+                        const b3Float4 WorldEdge0 = aw - bw;
+                        b3Float4 worldPlaneAnormal1 = worldNormalsA1[pairIndex];
+                        b3Float4 planeNormalWS1 = -b3Cross(WorldEdge0,worldPlaneAnormal1);
+                        b3Float4 worldA1 = aw;
+                        float planeEqWS1 = -b3Dot(worldA1,planeNormalWS1);
+                        b3Float4 planeNormalWS = planeNormalWS1;
+                        float planeEqWS=planeEqWS1;
+                        numVertsOut = clipFaceGlobal(pVtxIn, numVertsInB, planeNormalWS,planeEqWS, pVtxOut);
+                        __global b3Float4* tmp = pVtxOut;
+                        pVtxOut = pVtxIn;
+                        pVtxIn = tmp;
+                        numVertsInB = numVertsOut;
+                        numVertsOut = 0;
+                    }
+                    
+                    b3Float4 planeNormalWS = worldNormalsA1[pairIndex];
+                    float planeEqWS=-b3Dot(planeNormalWS,worldVertsA1[pairIndex*capacityWorldVertsB2]);
+                    
+                    for (int i=0;i<numVertsInB;i++)
+                    {
+                        float depth = b3Dot(planeNormalWS,pVtxIn[i])+planeEqWS;
+                        if (depth <=minDist)
+                        {
+                            depth = minDist;
+                        }
+/*
+						static float maxDepth = 0.f;
+						if (depth < maxDepth)
+						{
+							maxDepth = depth;
+							if (maxDepth < -10)
+							{
+								printf("error at framecount %d?\n",myframecount);
+							}
+							printf("maxDepth = %f\n", maxDepth);
+
+						}
+*/
+                        if (depth <=maxDist)
+                        {
+                            b3Float4 pointInWorld = pVtxIn[i];
+                            pVtxOut[numLocalContactsOut++] = b3MakeFloat4(pointInWorld.x,pointInWorld.y,pointInWorld.z,depth);
+                        }
+                    }
+                    
+                }
+                clippingFaces[pairIndex].w =numLocalContactsOut;
+                
+
+            }
+            
+            for (int i=0;i<numLocalContactsOut;i++)
+                pVtxIn[i] = pVtxOut[i];
+                
+		}//		if (hasSeparatingAxis[i])
+	}//	if (i<numPairs)
+    
+}
+
+#endif //B3_CLIP_FACES_H
+
diff --git a/thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3Collidable.h b/thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3Collidable.h
new file mode 100644
index 0000000000..77cdc7b7a9
--- /dev/null
+++ b/thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3Collidable.h
@@ -0,0 +1,76 @@
+
+#ifndef B3_COLLIDABLE_H
+#define B3_COLLIDABLE_H
+
+
+#include "Bullet3Common/shared/b3Float4.h"
+#include "Bullet3Common/shared/b3Quat.h"
+
+enum b3ShapeTypes
+{
+	SHAPE_HEIGHT_FIELD=1,
+
+	SHAPE_CONVEX_HULL=3,
+	SHAPE_PLANE=4,
+	SHAPE_CONCAVE_TRIMESH=5,
+	SHAPE_COMPOUND_OF_CONVEX_HULLS=6,
+	SHAPE_SPHERE=7,
+	MAX_NUM_SHAPE_TYPES,
+};
+
+typedef struct b3Collidable b3Collidable_t;
+
+
+struct b3Collidable
+{
+	union {
+		int m_numChildShapes;
+		int m_bvhIndex;
+	};
+	union
+	{
+		float m_radius;
+		int	m_compoundBvhIndex;
+	};
+
+	int m_shapeType;
+	union
+	{
+		int m_shapeIndex;
+		float m_height;
+	};
+};
+
+typedef struct b3GpuChildShape b3GpuChildShape_t;
+struct b3GpuChildShape
+{
+	b3Float4	m_childPosition;
+	b3Quat		m_childOrientation;
+	union
+	{
+		int			m_shapeIndex;//used for SHAPE_COMPOUND_OF_CONVEX_HULLS
+		int			m_capsuleAxis;
+	};
+	union 
+	{
+		float		m_radius;//used for childshape of SHAPE_COMPOUND_OF_SPHERES or SHAPE_COMPOUND_OF_CAPSULES
+		int			m_numChildShapes;//used for compound shape
+	};
+	union 
+	{
+		float		m_height;//used for childshape of SHAPE_COMPOUND_OF_CAPSULES
+		int	m_collidableShapeIndex;
+	};
+	int			m_shapeType;
+};
+
+struct b3CompoundOverlappingPair
+{
+	int m_bodyIndexA;
+	int m_bodyIndexB;
+//	int	m_pairType;
+	int m_childShapeIndexA;
+	int m_childShapeIndexB;
+};
+
+#endif //B3_COLLIDABLE_H
diff --git a/thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3Contact4Data.h b/thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3Contact4Data.h
new file mode 100644
index 0000000000..dfd45cc566
--- /dev/null
+++ b/thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3Contact4Data.h
@@ -0,0 +1,40 @@
+#ifndef B3_CONTACT4DATA_H
+#define B3_CONTACT4DATA_H
+
+#include "Bullet3Common/shared/b3Float4.h"
+
+typedef  struct b3Contact4Data b3Contact4Data_t;
+
+struct b3Contact4Data
+{
+	b3Float4	m_worldPosB[4];
+//	b3Float4	m_localPosA[4];
+//	b3Float4	m_localPosB[4];
+	b3Float4	m_worldNormalOnB;	//	w: m_nPoints
+	unsigned short  m_restituitionCoeffCmp;
+	unsigned short  m_frictionCoeffCmp;
+	int m_batchIdx;
+	int m_bodyAPtrAndSignBit;//x:m_bodyAPtr, y:m_bodyBPtr
+	int m_bodyBPtrAndSignBit;
+
+	int	m_childIndexA;
+	int	m_childIndexB;
+	int m_unused1;
+	int m_unused2;
+
+
+};
+
+inline int b3Contact4Data_getNumPoints(const struct b3Contact4Data* contact)
+{
+	return (int)contact->m_worldNormalOnB.w;
+};
+
+inline void b3Contact4Data_setNumPoints(struct b3Contact4Data* contact, int numPoints)
+{
+	contact->m_worldNormalOnB.w = (float)numPoints;
+};
+
+
+
+#endif //B3_CONTACT4DATA_H
\ No newline at end of file
diff --git a/thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3ContactConvexConvexSAT.h b/thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3ContactConvexConvexSAT.h
new file mode 100644
index 0000000000..f295f01a6c
--- /dev/null
+++ b/thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3ContactConvexConvexSAT.h
@@ -0,0 +1,520 @@
+
+#ifndef B3_CONTACT_CONVEX_CONVEX_SAT_H
+#define B3_CONTACT_CONVEX_CONVEX_SAT_H
+
+
+#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Contact4Data.h"
+#include "Bullet3Collision/NarrowPhaseCollision/shared/b3FindSeparatingAxis.h"
+#include "Bullet3Collision/NarrowPhaseCollision/shared/b3ReduceContacts.h"
+
+#define B3_MAX_VERTS 1024
+
+
+
+inline b3Float4 b3Lerp3(const b3Float4& a,const b3Float4& b, float  t)
+{
+	return b3MakeVector3(	a.x + (b.x - a.x) * t,
+						a.y + (b.y - a.y) * t,
+						a.z + (b.z - a.z) * t,
+						0.f);
+}
+
+
+// Clips a face to the back of a plane, return the number of vertices out, stored in ppVtxOut
+inline int b3ClipFace(const b3Float4* pVtxIn, int numVertsIn, b3Float4& planeNormalWS,float planeEqWS, b3Float4* ppVtxOut)
+{
+	
+	int ve;
+	float ds, de;
+	int numVertsOut = 0;
+	if (numVertsIn < 2)
+		return 0;
+
+	b3Float4 firstVertex=pVtxIn[numVertsIn-1];
+	b3Float4 endVertex = pVtxIn[0];
+	
+	ds = b3Dot3F4(planeNormalWS,firstVertex)+planeEqWS;
+
+	for (ve = 0; ve < numVertsIn; ve++)
+	{
+		endVertex=pVtxIn[ve];
+
+		de = b3Dot3F4(planeNormalWS,endVertex)+planeEqWS;
+
+		if (ds<0)
+		{
+			if (de<0)
+			{
+				// Start < 0, end < 0, so output endVertex
+				ppVtxOut[numVertsOut++] = endVertex;
+			}
+			else
+			{
+				// Start < 0, end >= 0, so output intersection
+				ppVtxOut[numVertsOut++] = b3Lerp3(firstVertex, endVertex,(ds * 1.f/(ds - de)) );
+			}
+		}
+		else
+		{
+			if (de<0)
+			{
+				// Start >= 0, end < 0 so output intersection and end
+				ppVtxOut[numVertsOut++] = b3Lerp3(firstVertex, endVertex,(ds * 1.f/(ds - de)) );
+				ppVtxOut[numVertsOut++] = endVertex;
+			}
+		}
+		firstVertex = endVertex;
+		ds = de;
+	}
+	return numVertsOut;
+}
+
+
+inline int b3ClipFaceAgainstHull(const b3Float4& separatingNormal, const b3ConvexPolyhedronData* hullA,  
+	const b3Float4& posA, const b3Quaternion& ornA, b3Float4* worldVertsB1, int numWorldVertsB1,
+	b3Float4* worldVertsB2, int capacityWorldVertsB2,
+	const float minDist, float maxDist,
+	const b3AlignedObjectArray<b3Float4>& verticesA,	const b3AlignedObjectArray<b3GpuFace>& facesA,	const b3AlignedObjectArray<int>& indicesA,
+	//const b3Float4* verticesB,	const b3GpuFace* facesB,	const int* indicesB,
+	b3Float4* contactsOut,
+	int contactCapacity)
+{
+	int numContactsOut = 0;
+
+	b3Float4* pVtxIn = worldVertsB1;
+	b3Float4* pVtxOut = worldVertsB2;
+	
+	int numVertsIn = numWorldVertsB1;
+	int numVertsOut = 0;
+
+	int closestFaceA=-1;
+	{
+		float dmin = FLT_MAX;
+		for(int face=0;face<hullA->m_numFaces;face++)
+		{
+			const b3Float4 Normal = b3MakeVector3(
+				facesA[hullA->m_faceOffset+face].m_plane.x, 
+				facesA[hullA->m_faceOffset+face].m_plane.y, 
+				facesA[hullA->m_faceOffset+face].m_plane.z,0.f);
+			const b3Float4 faceANormalWS = b3QuatRotate(ornA,Normal);
+		
+			float d = b3Dot3F4(faceANormalWS,separatingNormal);
+			if (d < dmin)
+			{
+				dmin = d;
+				closestFaceA = face;
+			}
+		}
+	}
+	if (closestFaceA<0)
+		return numContactsOut;
+
+	b3GpuFace polyA = facesA[hullA->m_faceOffset+closestFaceA];
+
+	// clip polygon to back of planes of all faces of hull A that are adjacent to witness face
+	//int numContacts = numWorldVertsB1;
+	int numVerticesA = polyA.m_numIndices;
+	for(int e0=0;e0<numVerticesA;e0++)
+	{
+		const b3Float4 a = verticesA[hullA->m_vertexOffset+indicesA[polyA.m_indexOffset+e0]];
+		const b3Float4 b = verticesA[hullA->m_vertexOffset+indicesA[polyA.m_indexOffset+((e0+1)%numVerticesA)]];
+		const b3Float4 edge0 = a - b;
+		const b3Float4 WorldEdge0 = b3QuatRotate(ornA,edge0);
+		b3Float4 planeNormalA = b3MakeFloat4(polyA.m_plane.x,polyA.m_plane.y,polyA.m_plane.z,0.f);
+		b3Float4 worldPlaneAnormal1 = b3QuatRotate(ornA,planeNormalA);
+
+		b3Float4 planeNormalWS1 = -b3Cross3(WorldEdge0,worldPlaneAnormal1);
+		b3Float4 worldA1 = b3TransformPoint(a,posA,ornA);
+		float planeEqWS1 = -b3Dot3F4(worldA1,planeNormalWS1);
+		
+		b3Float4 planeNormalWS = planeNormalWS1;
+		float planeEqWS=planeEqWS1;
+		
+		//clip face
+		//clipFace(*pVtxIn, *pVtxOut,planeNormalWS,planeEqWS);
+		numVertsOut = b3ClipFace(pVtxIn, numVertsIn, planeNormalWS,planeEqWS, pVtxOut);
+
+		//btSwap(pVtxIn,pVtxOut);
+		b3Float4* tmp = pVtxOut;
+		pVtxOut = pVtxIn;
+		pVtxIn = tmp;
+		numVertsIn = numVertsOut;
+		numVertsOut = 0;
+	}
+
+	
+	// only keep points that are behind the witness face
+	{
+		b3Float4 localPlaneNormal  = b3MakeFloat4(polyA.m_plane.x,polyA.m_plane.y,polyA.m_plane.z,0.f);
+		float localPlaneEq = polyA.m_plane.w;
+		b3Float4 planeNormalWS = b3QuatRotate(ornA,localPlaneNormal);
+		float planeEqWS=localPlaneEq-b3Dot3F4(planeNormalWS,posA);
+		for (int i=0;i<numVertsIn;i++)
+		{
+			float depth = b3Dot3F4(planeNormalWS,pVtxIn[i])+planeEqWS;
+			if (depth <=minDist)
+			{
+				depth = minDist;
+			}
+			if (numContactsOut<contactCapacity)
+			{
+				if (depth <=maxDist)
+				{
+					b3Float4 pointInWorld = pVtxIn[i];
+					//resultOut.addContactPoint(separatingNormal,point,depth);
+					contactsOut[numContactsOut++] = b3MakeVector3(pointInWorld.x,pointInWorld.y,pointInWorld.z,depth);
+					//printf("depth=%f\n",depth);
+				}
+			} else
+			{
+				b3Error("exceeding contact capacity (%d,%df)\n", numContactsOut,contactCapacity);
+			}
+		}
+	}
+
+	return numContactsOut;
+}
+
+
+
+inline int	b3ClipHullAgainstHull(const b3Float4& separatingNormal, 
+	const b3ConvexPolyhedronData& hullA, const b3ConvexPolyhedronData& hullB, 
+	const b3Float4& posA, const b3Quaternion& ornA,const b3Float4& posB, const b3Quaternion& ornB, 
+	b3Float4* worldVertsB1, b3Float4* worldVertsB2, int capacityWorldVerts,
+	const float minDist, float maxDist,
+	const b3AlignedObjectArray<b3Float4>& verticesA,	const b3AlignedObjectArray<b3GpuFace>& facesA,	const b3AlignedObjectArray<int>& indicesA,
+	const b3AlignedObjectArray<b3Float4>& verticesB,	const b3AlignedObjectArray<b3GpuFace>& facesB,	const b3AlignedObjectArray<int>& indicesB,
+
+	b3Float4*	contactsOut,
+	int contactCapacity)
+{
+	int numContactsOut = 0;
+	int numWorldVertsB1= 0;
+	
+	B3_PROFILE("clipHullAgainstHull");
+
+	//float curMaxDist=maxDist;
+	int closestFaceB=-1;
+	float dmax = -FLT_MAX;
+
+	{
+		//B3_PROFILE("closestFaceB");
+		if (hullB.m_numFaces!=1)
+		{
+			//printf("wtf\n");
+		}
+		static bool once = true;
+		//printf("separatingNormal=%f,%f,%f\n",separatingNormal.x,separatingNormal.y,separatingNormal.z);
+		
+		for(int face=0;face<hullB.m_numFaces;face++)
+		{
+#ifdef BT_DEBUG_SAT_FACE
+			if (once)
+				printf("face %d\n",face);
+			const b3GpuFace* faceB = &facesB[hullB.m_faceOffset+face];
+			if (once)
+			{
+				for (int i=0;i<faceB->m_numIndices;i++)
+				{
+					b3Float4 vert = verticesB[hullB.m_vertexOffset+indicesB[faceB->m_indexOffset+i]];
+					printf("vert[%d] = %f,%f,%f\n",i,vert.x,vert.y,vert.z);
+				}
+			}
+#endif //BT_DEBUG_SAT_FACE
+			//if (facesB[hullB.m_faceOffset+face].m_numIndices>2)
+			{
+				const b3Float4 Normal = b3MakeVector3(facesB[hullB.m_faceOffset+face].m_plane.x, 
+					facesB[hullB.m_faceOffset+face].m_plane.y, facesB[hullB.m_faceOffset+face].m_plane.z,0.f);
+				const b3Float4 WorldNormal = b3QuatRotate(ornB, Normal);
+#ifdef BT_DEBUG_SAT_FACE
+				if (once)
+					printf("faceNormal = %f,%f,%f\n",Normal.x,Normal.y,Normal.z);
+#endif
+				float d = b3Dot3F4(WorldNormal,separatingNormal);
+				if (d > dmax)
+				{
+					dmax = d;
+					closestFaceB = face;
+				}
+			}
+		}
+		once = false;
+	}
+
+	
+	b3Assert(closestFaceB>=0);
+	{
+		//B3_PROFILE("worldVertsB1");
+		const b3GpuFace& polyB = facesB[hullB.m_faceOffset+closestFaceB];
+		const int numVertices = polyB.m_numIndices;
+		for(int e0=0;e0<numVertices;e0++)
+		{
+			const b3Float4& b = verticesB[hullB.m_vertexOffset+indicesB[polyB.m_indexOffset+e0]];
+			worldVertsB1[numWorldVertsB1++] = b3TransformPoint(b,posB,ornB);
+		}
+	}
+
+	if (closestFaceB>=0)
+	{
+		//B3_PROFILE("clipFaceAgainstHull");
+		numContactsOut = b3ClipFaceAgainstHull((b3Float4&)separatingNormal, &hullA, 
+				posA,ornA,
+				worldVertsB1,numWorldVertsB1,worldVertsB2,capacityWorldVerts, minDist, maxDist,
+				verticesA,				facesA,				indicesA,
+				contactsOut,contactCapacity);
+	}
+
+	return numContactsOut;
+}
+
+
+
+
+inline int b3ClipHullHullSingle(
+			int bodyIndexA, int bodyIndexB,
+										 const b3Float4& posA,
+										 const b3Quaternion& ornA,
+										 const b3Float4& posB,
+										 const b3Quaternion& ornB,
+
+			int collidableIndexA, int collidableIndexB,
+
+			const b3AlignedObjectArray<b3RigidBodyData>* bodyBuf, 
+			b3AlignedObjectArray<b3Contact4Data>* globalContactOut, 
+			int& nContacts,
+			
+			const b3AlignedObjectArray<b3ConvexPolyhedronData>& hostConvexDataA,
+			const b3AlignedObjectArray<b3ConvexPolyhedronData>& 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,
+			const b3Vector3& sepNormalWorldSpace,
+			int maxContactCapacity			)
+{
+	int contactIndex = -1;
+	b3ConvexPolyhedronData hullA, hullB;
+    
+    b3Collidable colA = hostCollidablesA[collidableIndexA];
+    hullA = hostConvexDataA[colA.m_shapeIndex];
+    //printf("numvertsA = %d\n",hullA.m_numVertices);
+    
+    
+    b3Collidable colB = hostCollidablesB[collidableIndexB];
+    hullB = hostConvexDataB[colB.m_shapeIndex];
+    //printf("numvertsB = %d\n",hullB.m_numVertices);
+    
+	
+	b3Float4 contactsOut[B3_MAX_VERTS];
+	int localContactCapacity = B3_MAX_VERTS;
+
+#ifdef _WIN32
+	b3Assert(_finite(bodyBuf->at(bodyIndexA).m_pos.x));
+	b3Assert(_finite(bodyBuf->at(bodyIndexB).m_pos.x));
+#endif
+	
+	
+	{
+		
+		b3Float4 worldVertsB1[B3_MAX_VERTS];
+		b3Float4 worldVertsB2[B3_MAX_VERTS];
+		int capacityWorldVerts = B3_MAX_VERTS;
+
+		b3Float4 hostNormal = b3MakeFloat4(sepNormalWorldSpace.x,sepNormalWorldSpace.y,sepNormalWorldSpace.z,0.f);
+		int shapeA = hostCollidablesA[collidableIndexA].m_shapeIndex;
+		int shapeB = hostCollidablesB[collidableIndexB].m_shapeIndex;
+
+		b3Scalar minDist = -1;
+		b3Scalar maxDist = 0.;
+
+		        
+
+		b3Transform trA,trB;
+		{
+		//B3_PROFILE("b3TransformPoint computation");
+		//trA.setIdentity();
+		trA.setOrigin(b3MakeVector3(posA.x,posA.y,posA.z));
+		trA.setRotation(b3Quaternion(ornA.x,ornA.y,ornA.z,ornA.w));
+				
+		//trB.setIdentity();
+		trB.setOrigin(b3MakeVector3(posB.x,posB.y,posB.z));
+		trB.setRotation(b3Quaternion(ornB.x,ornB.y,ornB.z,ornB.w));
+		}
+
+		b3Quaternion trAorn = trA.getRotation();
+        b3Quaternion trBorn = trB.getRotation();
+        
+		int numContactsOut = b3ClipHullAgainstHull(hostNormal, 
+						hostConvexDataA.at(shapeA), 
+						hostConvexDataB.at(shapeB),
+								(b3Float4&)trA.getOrigin(), (b3Quaternion&)trAorn,
+								(b3Float4&)trB.getOrigin(), (b3Quaternion&)trBorn,
+								worldVertsB1,worldVertsB2,capacityWorldVerts,
+								minDist, maxDist,
+								verticesA,	facesA,indicesA,
+								verticesB,	facesB,indicesB,
+								
+								contactsOut,localContactCapacity);
+
+		if (numContactsOut>0)
+		{
+			B3_PROFILE("overlap");
+
+			b3Float4 normalOnSurfaceB = (b3Float4&)hostNormal;
+//			b3Float4 centerOut;
+			
+			b3Int4 contactIdx;
+			contactIdx.x = 0;
+			contactIdx.y = 1;
+			contactIdx.z = 2;
+			contactIdx.w = 3;
+			
+			int numPoints = 0;
+					
+			{
+				B3_PROFILE("extractManifold");
+				numPoints = b3ReduceContacts(contactsOut, numContactsOut, normalOnSurfaceB,  &contactIdx);
+			}
+					
+			b3Assert(numPoints);
+					
+			if (nContacts<maxContactCapacity)
+			{
+				contactIndex = nContacts;
+				globalContactOut->expand();
+				b3Contact4Data& 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_worldPosB[p] = contactsOut[contactIdx.s[p]];//check if it is actually on B
+					contact.m_worldNormalOnB = normalOnSurfaceB; 
+				}
+				//printf("bodyIndexA %d,bodyIndexB %d,normal=%f,%f,%f numPoints %d\n",bodyIndexA,bodyIndexB,normalOnSurfaceB.x,normalOnSurfaceB.y,normalOnSurfaceB.z,numPoints);
+				contact.m_worldNormalOnB.w = (b3Scalar)numPoints;
+				nContacts++;
+			} else
+			{
+				b3Error("Error: exceeding contact capacity (%d/%d)\n", nContacts,maxContactCapacity);
+			}
+		}
+	}
+	return contactIndex;
+}
+
+	
+
+
+
+inline int b3ContactConvexConvexSAT(
+																int pairIndex,
+																int bodyIndexA, int bodyIndexB, 
+																int collidableIndexA, int collidableIndexB, 
+																const b3AlignedObjectArray<b3RigidBodyData>& rigidBodies, 
+																const b3AlignedObjectArray<b3Collidable>& collidables,
+																const b3AlignedObjectArray<b3ConvexPolyhedronData>& convexShapes,
+																const b3AlignedObjectArray<b3Float4>& convexVertices,
+																const b3AlignedObjectArray<b3Float4>& uniqueEdges,
+																const b3AlignedObjectArray<int>& convexIndices,
+																const b3AlignedObjectArray<b3GpuFace>& faces,
+																b3AlignedObjectArray<b3Contact4Data>& globalContactsOut,
+																int& nGlobalContactsOut,
+																int maxContactCapacity)
+{
+	int contactIndex = -1;
+
+
+	b3Float4 posA = rigidBodies[bodyIndexA].m_pos;
+	b3Quaternion ornA = rigidBodies[bodyIndexA].m_quat;
+	b3Float4 posB = rigidBodies[bodyIndexB].m_pos;
+	b3Quaternion ornB = rigidBodies[bodyIndexB].m_quat;
+	
+
+	b3ConvexPolyhedronData hullA, hullB;
+    
+	b3Float4 sepNormalWorldSpace;
+
+	
+
+    b3Collidable colA = collidables[collidableIndexA];
+    hullA = convexShapes[colA.m_shapeIndex];
+    //printf("numvertsA = %d\n",hullA.m_numVertices);
+    
+    
+    b3Collidable colB = collidables[collidableIndexB];
+    hullB = convexShapes[colB.m_shapeIndex];
+    //printf("numvertsB = %d\n",hullB.m_numVertices);
+    
+	
+
+
+#ifdef _WIN32
+	b3Assert(_finite(rigidBodies[bodyIndexA].m_pos.x));
+	b3Assert(_finite(rigidBodies[bodyIndexB].m_pos.x));
+#endif
+	
+		bool foundSepAxis = b3FindSeparatingAxis(hullA,hullB,
+							posA,
+							ornA,
+							posB,
+							ornB,
+
+							convexVertices,uniqueEdges,faces,convexIndices,
+							convexVertices,uniqueEdges,faces,convexIndices,
+							
+							sepNormalWorldSpace
+							);
+
+	
+	if (foundSepAxis)
+	{
+		
+		
+		contactIndex = b3ClipHullHullSingle(
+			bodyIndexA, bodyIndexB,
+						   posA,ornA,
+						   posB,ornB,
+			collidableIndexA, collidableIndexB,
+			&rigidBodies, 
+			&globalContactsOut,
+			nGlobalContactsOut,
+			
+			convexShapes,
+			convexShapes,
+	
+			convexVertices, 
+			uniqueEdges, 
+			faces,
+			convexIndices,
+	
+			convexVertices,
+			uniqueEdges,
+			faces,
+			convexIndices,
+
+			collidables,
+			collidables,
+			sepNormalWorldSpace,
+			maxContactCapacity);
+			
+	}
+
+	return contactIndex;
+}
+
+#endif //B3_CONTACT_CONVEX_CONVEX_SAT_H
diff --git a/thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3ContactSphereSphere.h b/thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3ContactSphereSphere.h
new file mode 100644
index 0000000000..a3fa82287b
--- /dev/null
+++ b/thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3ContactSphereSphere.h
@@ -0,0 +1,162 @@
+
+#ifndef B3_CONTACT_SPHERE_SPHERE_H
+#define B3_CONTACT_SPHERE_SPHERE_H
+
+
+
+
+
+void	computeContactSphereConvex(int pairIndex,
+																int bodyIndexA, int bodyIndexB, 
+																int collidableIndexA, int collidableIndexB, 
+																const b3RigidBodyData* rigidBodies, 
+																const b3Collidable* collidables,
+																const b3ConvexPolyhedronData* convexShapes,
+																const b3Vector3* convexVertices,
+																const int* convexIndices,
+																const b3GpuFace* faces,
+																b3Contact4* globalContactsOut,
+																int& nGlobalContactsOut,
+																int maxContactCapacity)
+{
+
+	float radius = collidables[collidableIndexA].m_radius;
+	float4 spherePos1 = rigidBodies[bodyIndexA].m_pos;
+	b3Quaternion sphereOrn = rigidBodies[bodyIndexA].m_quat;
+
+
+
+	float4 pos = rigidBodies[bodyIndexB].m_pos;
+	
+
+	b3Quaternion quat = rigidBodies[bodyIndexB].m_quat;
+
+	b3Transform tr;
+	tr.setIdentity();
+	tr.setOrigin(pos);
+	tr.setRotation(quat);
+	b3Transform trInv = tr.inverse();
+
+	float4 spherePos = trInv(spherePos1);
+
+	int collidableIndex = rigidBodies[bodyIndexB].m_collidableIdx;
+	int shapeIndex = collidables[collidableIndex].m_shapeIndex;
+	int numFaces = convexShapes[shapeIndex].m_numFaces;
+	float4 closestPnt = b3MakeVector3(0, 0, 0, 0);
+	float4 hitNormalWorld = b3MakeVector3(0, 0, 0, 0);
+	float minDist = -1000000.f; // TODO: What is the largest/smallest float?
+	bool bCollide = true;
+	int region = -1;
+	float4 localHitNormal;
+	for ( int f = 0; f < numFaces; f++ )
+	{
+		b3GpuFace face = faces[convexShapes[shapeIndex].m_faceOffset+f];
+		float4 planeEqn;
+		float4 localPlaneNormal = b3MakeVector3(face.m_plane.x,face.m_plane.y,face.m_plane.z,0.f);
+		float4 n1 = localPlaneNormal;//quatRotate(quat,localPlaneNormal);
+		planeEqn = n1;
+		planeEqn[3] = face.m_plane.w;
+
+		float4 pntReturn;
+		float dist = signedDistanceFromPointToPlane(spherePos, planeEqn, &pntReturn);
+
+		if ( dist > radius)
+		{
+			bCollide = false;
+			break;
+		}
+
+		if ( dist > 0 )
+		{
+			//might hit an edge or vertex
+			b3Vector3 out;
+
+			bool isInPoly = IsPointInPolygon(spherePos,
+					&face,
+					&convexVertices[convexShapes[shapeIndex].m_vertexOffset],
+					convexIndices,
+                    &out);
+			if (isInPoly)
+			{
+				if (dist>minDist)
+				{
+					minDist = dist;
+					closestPnt = pntReturn;
+					localHitNormal = planeEqn;
+					region=1;
+				}
+			} else
+			{
+				b3Vector3 tmp = spherePos-out;
+				b3Scalar l2 = tmp.length2();
+				if (l2<radius*radius)
+				{
+					dist  = b3Sqrt(l2);
+					if (dist>minDist)
+					{
+						minDist = dist;
+						closestPnt = out;
+						localHitNormal = tmp/dist;
+						region=2;
+					}
+					
+				} else
+				{
+					bCollide = false;
+					break;
+				}
+			}
+		}
+		else
+		{
+			if ( dist > minDist )
+			{
+				minDist = dist;
+				closestPnt = pntReturn;
+				localHitNormal = planeEqn;
+				region=3;
+			}
+		}
+	}
+	static int numChecks = 0;
+	numChecks++;
+
+	if (bCollide && minDist > -10000)
+	{
+		
+		float4 normalOnSurfaceB1 = tr.getBasis()*localHitNormal;//-hitNormalWorld;
+		float4 pOnB1 = tr(closestPnt);
+		//printf("dist ,%f,",minDist);
+		float actualDepth = minDist-radius;
+		if (actualDepth<0)
+		{
+		//printf("actualDepth = ,%f,", actualDepth);
+		//printf("normalOnSurfaceB1 = ,%f,%f,%f,", normalOnSurfaceB1.x,normalOnSurfaceB1.y,normalOnSurfaceB1.z);
+		//printf("region=,%d,\n", region);
+		pOnB1[3] = actualDepth;
+
+		int dstIdx;
+//    dstIdx = nGlobalContactsOut++;//AppendInc( nGlobalContactsOut, dstIdx );
+		
+		if (nGlobalContactsOut < maxContactCapacity)
+		{
+			dstIdx=nGlobalContactsOut;
+			nGlobalContactsOut++;
+
+			b3Contact4* c = &globalContactsOut[dstIdx];
+			c->m_worldNormalOnB = normalOnSurfaceB1;
+			c->setFrictionCoeff(0.7);
+			c->setRestituitionCoeff(0.f);
+
+			c->m_batchIdx = pairIndex;
+			c->m_bodyAPtrAndSignBit = rigidBodies[bodyIndexA].m_invMass==0?-bodyIndexA:bodyIndexA;
+			c->m_bodyBPtrAndSignBit = rigidBodies[bodyIndexB].m_invMass==0?-bodyIndexB:bodyIndexB;
+			c->m_worldPosB[0] = pOnB1;
+			int numPoints = 1;
+			c->m_worldNormalOnB.w = (b3Scalar)numPoints;
+		}//if (dstIdx < numPairs)
+		}
+	}//if (hasCollision)
+	
+}
+#endif //B3_CONTACT_SPHERE_SPHERE_H
diff --git a/thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3ConvexPolyhedronData.h b/thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3ConvexPolyhedronData.h
new file mode 100644
index 0000000000..5c5f4e297f
--- /dev/null
+++ b/thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3ConvexPolyhedronData.h
@@ -0,0 +1,40 @@
+
+#ifndef B3_CONVEX_POLYHEDRON_DATA_H
+#define B3_CONVEX_POLYHEDRON_DATA_H
+
+
+
+#include "Bullet3Common/shared/b3Float4.h"
+#include "Bullet3Common/shared/b3Quat.h"
+
+typedef struct b3GpuFace b3GpuFace_t;
+struct b3GpuFace
+{
+	b3Float4 m_plane;
+	int m_indexOffset;
+	int m_numIndices;
+	int m_unusedPadding1;
+	int m_unusedPadding2;
+};
+
+typedef struct b3ConvexPolyhedronData b3ConvexPolyhedronData_t;
+
+struct b3ConvexPolyhedronData
+{
+	b3Float4		m_localCenter;
+	b3Float4		m_extents;
+	b3Float4		mC;
+	b3Float4		mE;
+
+	float			m_radius;
+	int	m_faceOffset;
+	int m_numFaces;
+	int	m_numVertices;
+
+	int m_vertexOffset;
+	int	m_uniqueEdgesOffset;
+	int	m_numUniqueEdges;
+	int m_unused;
+};
+
+#endif //B3_CONVEX_POLYHEDRON_DATA_H
diff --git a/thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3FindConcaveSatAxis.h b/thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3FindConcaveSatAxis.h
new file mode 100644
index 0000000000..89993f3565
--- /dev/null
+++ b/thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3FindConcaveSatAxis.h
@@ -0,0 +1,832 @@
+#ifndef B3_FIND_CONCAVE_SEPARATING_AXIS_H
+#define B3_FIND_CONCAVE_SEPARATING_AXIS_H
+
+#define B3_TRIANGLE_NUM_CONVEX_FACES 5
+
+
+#include "Bullet3Common/shared/b3Int4.h"
+#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h"
+#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Collidable.h"
+#include "Bullet3Collision/BroadPhaseCollision/shared/b3Aabb.h"
+#include "Bullet3Collision/NarrowPhaseCollision/shared/b3BvhSubtreeInfoData.h"
+#include "Bullet3Collision/NarrowPhaseCollision/shared/b3QuantizedBvhNodeData.h"
+#include "Bullet3Collision/NarrowPhaseCollision/shared/b3ConvexPolyhedronData.h"
+
+
+inline void b3Project(__global const b3ConvexPolyhedronData* hull,  b3Float4ConstArg pos, b3QuatConstArg orn, 
+const b3Float4* dir, __global const b3Float4* vertices, float* min, float* max)
+{
+	min[0] = FLT_MAX;
+	max[0] = -FLT_MAX;
+	int numVerts = hull->m_numVertices;
+
+	const b3Float4 localDir = b3QuatRotate(b3QuatInverse(orn),*dir);
+	float offset = b3Dot(pos,*dir);
+	for(int i=0;i<numVerts;i++)
+	{
+		float dp = b3Dot(vertices[hull->m_vertexOffset+i],localDir);
+		if(dp < min[0])	
+			min[0] = dp;
+		if(dp > max[0])	
+			max[0] = dp;
+	}
+	if(min[0]>max[0])
+	{
+		float tmp = min[0];
+		min[0] = max[0];
+		max[0] = tmp;
+	}
+	min[0] += offset;
+	max[0] += offset;
+}
+
+
+inline bool b3TestSepAxis(const b3ConvexPolyhedronData* hullA, __global const b3ConvexPolyhedronData* hullB, 
+	b3Float4ConstArg posA,b3QuatConstArg ornA,
+	b3Float4ConstArg posB,b3QuatConstArg ornB,
+	b3Float4* sep_axis, const b3Float4* verticesA, __global const b3Float4* verticesB,float* depth)
+{
+	float Min0,Max0;
+	float Min1,Max1;
+	b3Project(hullA,posA,ornA,sep_axis,verticesA, &Min0, &Max0);
+	b3Project(hullB,posB,ornB, sep_axis,verticesB, &Min1, &Max1);
+
+	if(Max0<Min1 || Max1<Min0)
+		return false;
+
+	float d0 = Max0 - Min1;
+	float d1 = Max1 - Min0;
+	*depth = d0<d1 ? d0:d1;
+	return true;
+}
+
+
+bool b3FindSeparatingAxis(	const b3ConvexPolyhedronData* hullA, __global const b3ConvexPolyhedronData* hullB, 
+	b3Float4ConstArg posA1,
+	b3QuatConstArg ornA,
+	b3Float4ConstArg posB1,
+	b3QuatConstArg ornB,
+	b3Float4ConstArg DeltaC2,
+	
+	const b3Float4* verticesA, 
+	const b3Float4* uniqueEdgesA, 
+	const b3GpuFace* facesA,
+	const int*  indicesA,
+
+	__global const b3Float4* verticesB, 
+	__global const b3Float4* uniqueEdgesB, 
+	__global const b3GpuFace* facesB,
+	__global const int*  indicesB,
+	b3Float4* sep,
+	float* dmin)
+{
+	
+
+	b3Float4 posA = posA1;
+	posA.w = 0.f;
+	b3Float4 posB = posB1;
+	posB.w = 0.f;
+/*
+	static int maxFaceVertex = 0;
+
+	int curFaceVertexAB = hullA->m_numFaces*hullB->m_numVertices;
+	curFaceVertexAB+= hullB->m_numFaces*hullA->m_numVertices;
+
+	if (curFaceVertexAB>maxFaceVertex)
+	{
+		maxFaceVertex = curFaceVertexAB;
+		printf("curFaceVertexAB = %d\n",curFaceVertexAB);
+		printf("hullA->m_numFaces = %d\n",hullA->m_numFaces);
+		printf("hullA->m_numVertices = %d\n",hullA->m_numVertices);
+		printf("hullB->m_numVertices = %d\n",hullB->m_numVertices);
+	}
+*/
+
+	int curPlaneTests=0;
+	{
+		int numFacesA = hullA->m_numFaces;
+		// Test normals from hullA
+		for(int i=0;i<numFacesA;i++)
+		{
+			const b3Float4 normal = facesA[hullA->m_faceOffset+i].m_plane;
+			b3Float4 faceANormalWS = b3QuatRotate(ornA,normal);
+			if (b3Dot(DeltaC2,faceANormalWS)<0)
+				faceANormalWS*=-1.f;
+			curPlaneTests++;
+			float d;
+			if(!b3TestSepAxis( hullA, hullB, posA,ornA,posB,ornB,&faceANormalWS, verticesA, verticesB,&d))
+				return false;
+			if(d<*dmin)
+			{
+				*dmin = d;
+				*sep = faceANormalWS;
+			}
+		}
+	}
+	if((b3Dot(-DeltaC2,*sep))>0.0f)
+	{
+		*sep = -(*sep);
+	}
+	return true;
+}
+
+
+b3Vector3 unitSphere162[]=
+{
+	b3MakeVector3(0.000000,-1.000000,0.000000),
+b3MakeVector3(0.203181,-0.967950,0.147618),
+b3MakeVector3(-0.077607,-0.967950,0.238853),
+b3MakeVector3(0.723607,-0.447220,0.525725),
+b3MakeVector3(0.609547,-0.657519,0.442856),
+b3MakeVector3(0.812729,-0.502301,0.295238),
+b3MakeVector3(-0.251147,-0.967949,0.000000),
+b3MakeVector3(-0.077607,-0.967950,-0.238853),
+b3MakeVector3(0.203181,-0.967950,-0.147618),
+b3MakeVector3(0.860698,-0.251151,0.442858),
+b3MakeVector3(-0.276388,-0.447220,0.850649),
+b3MakeVector3(-0.029639,-0.502302,0.864184),
+b3MakeVector3(-0.155215,-0.251152,0.955422),
+b3MakeVector3(-0.894426,-0.447216,0.000000),
+b3MakeVector3(-0.831051,-0.502299,0.238853),
+b3MakeVector3(-0.956626,-0.251149,0.147618),
+b3MakeVector3(-0.276388,-0.447220,-0.850649),
+b3MakeVector3(-0.483971,-0.502302,-0.716565),
+b3MakeVector3(-0.436007,-0.251152,-0.864188),
+b3MakeVector3(0.723607,-0.447220,-0.525725),
+b3MakeVector3(0.531941,-0.502302,-0.681712),
+b3MakeVector3(0.687159,-0.251152,-0.681715),
+b3MakeVector3(0.687159,-0.251152,0.681715),
+b3MakeVector3(-0.436007,-0.251152,0.864188),
+b3MakeVector3(-0.956626,-0.251149,-0.147618),
+b3MakeVector3(-0.155215,-0.251152,-0.955422),
+b3MakeVector3(0.860698,-0.251151,-0.442858),
+b3MakeVector3(0.276388,0.447220,0.850649),
+b3MakeVector3(0.483971,0.502302,0.716565),
+b3MakeVector3(0.232822,0.657519,0.716563),
+b3MakeVector3(-0.723607,0.447220,0.525725),
+b3MakeVector3(-0.531941,0.502302,0.681712),
+b3MakeVector3(-0.609547,0.657519,0.442856),
+b3MakeVector3(-0.723607,0.447220,-0.525725),
+b3MakeVector3(-0.812729,0.502301,-0.295238),
+b3MakeVector3(-0.609547,0.657519,-0.442856),
+b3MakeVector3(0.276388,0.447220,-0.850649),
+b3MakeVector3(0.029639,0.502302,-0.864184),
+b3MakeVector3(0.232822,0.657519,-0.716563),
+b3MakeVector3(0.894426,0.447216,0.000000),
+b3MakeVector3(0.831051,0.502299,-0.238853),
+b3MakeVector3(0.753442,0.657515,0.000000),
+b3MakeVector3(-0.232822,-0.657519,0.716563),
+b3MakeVector3(-0.162456,-0.850654,0.499995),
+b3MakeVector3(0.052790,-0.723612,0.688185),
+b3MakeVector3(0.138199,-0.894429,0.425321),
+b3MakeVector3(0.262869,-0.525738,0.809012),
+b3MakeVector3(0.361805,-0.723611,0.587779),
+b3MakeVector3(0.531941,-0.502302,0.681712),
+b3MakeVector3(0.425323,-0.850654,0.309011),
+b3MakeVector3(0.812729,-0.502301,-0.295238),
+b3MakeVector3(0.609547,-0.657519,-0.442856),
+b3MakeVector3(0.850648,-0.525736,0.000000),
+b3MakeVector3(0.670817,-0.723611,-0.162457),
+b3MakeVector3(0.670817,-0.723610,0.162458),
+b3MakeVector3(0.425323,-0.850654,-0.309011),
+b3MakeVector3(0.447211,-0.894428,0.000001),
+b3MakeVector3(-0.753442,-0.657515,0.000000),
+b3MakeVector3(-0.525730,-0.850652,0.000000),
+b3MakeVector3(-0.638195,-0.723609,0.262864),
+b3MakeVector3(-0.361801,-0.894428,0.262864),
+b3MakeVector3(-0.688189,-0.525736,0.499997),
+b3MakeVector3(-0.447211,-0.723610,0.525729),
+b3MakeVector3(-0.483971,-0.502302,0.716565),
+b3MakeVector3(-0.232822,-0.657519,-0.716563),
+b3MakeVector3(-0.162456,-0.850654,-0.499995),
+b3MakeVector3(-0.447211,-0.723611,-0.525727),
+b3MakeVector3(-0.361801,-0.894429,-0.262863),
+b3MakeVector3(-0.688189,-0.525736,-0.499997),
+b3MakeVector3(-0.638195,-0.723609,-0.262863),
+b3MakeVector3(-0.831051,-0.502299,-0.238853),
+b3MakeVector3(0.361804,-0.723612,-0.587779),
+b3MakeVector3(0.138197,-0.894429,-0.425321),
+b3MakeVector3(0.262869,-0.525738,-0.809012),
+b3MakeVector3(0.052789,-0.723611,-0.688186),
+b3MakeVector3(-0.029639,-0.502302,-0.864184),
+b3MakeVector3(0.956626,0.251149,0.147618),
+b3MakeVector3(0.956626,0.251149,-0.147618),
+b3MakeVector3(0.951058,-0.000000,0.309013),
+b3MakeVector3(1.000000,0.000000,0.000000),
+b3MakeVector3(0.947213,-0.276396,0.162458),
+b3MakeVector3(0.951058,0.000000,-0.309013),
+b3MakeVector3(0.947213,-0.276396,-0.162458),
+b3MakeVector3(0.155215,0.251152,0.955422),
+b3MakeVector3(0.436007,0.251152,0.864188),
+b3MakeVector3(-0.000000,-0.000000,1.000000),
+b3MakeVector3(0.309017,0.000000,0.951056),
+b3MakeVector3(0.138199,-0.276398,0.951055),
+b3MakeVector3(0.587786,0.000000,0.809017),
+b3MakeVector3(0.447216,-0.276398,0.850648),
+b3MakeVector3(-0.860698,0.251151,0.442858),
+b3MakeVector3(-0.687159,0.251152,0.681715),
+b3MakeVector3(-0.951058,-0.000000,0.309013),
+b3MakeVector3(-0.809018,0.000000,0.587783),
+b3MakeVector3(-0.861803,-0.276396,0.425324),
+b3MakeVector3(-0.587786,0.000000,0.809017),
+b3MakeVector3(-0.670819,-0.276397,0.688191),
+b3MakeVector3(-0.687159,0.251152,-0.681715),
+b3MakeVector3(-0.860698,0.251151,-0.442858),
+b3MakeVector3(-0.587786,-0.000000,-0.809017),
+b3MakeVector3(-0.809018,-0.000000,-0.587783),
+b3MakeVector3(-0.670819,-0.276397,-0.688191),
+b3MakeVector3(-0.951058,0.000000,-0.309013),
+b3MakeVector3(-0.861803,-0.276396,-0.425324),
+b3MakeVector3(0.436007,0.251152,-0.864188),
+b3MakeVector3(0.155215,0.251152,-0.955422),
+b3MakeVector3(0.587786,-0.000000,-0.809017),
+b3MakeVector3(0.309017,-0.000000,-0.951056),
+b3MakeVector3(0.447216,-0.276398,-0.850648),
+b3MakeVector3(0.000000,0.000000,-1.000000),
+b3MakeVector3(0.138199,-0.276398,-0.951055),
+b3MakeVector3(0.670820,0.276396,0.688190),
+b3MakeVector3(0.809019,-0.000002,0.587783),
+b3MakeVector3(0.688189,0.525736,0.499997),
+b3MakeVector3(0.861804,0.276394,0.425323),
+b3MakeVector3(0.831051,0.502299,0.238853),
+b3MakeVector3(-0.447216,0.276397,0.850649),
+b3MakeVector3(-0.309017,-0.000001,0.951056),
+b3MakeVector3(-0.262869,0.525738,0.809012),
+b3MakeVector3(-0.138199,0.276397,0.951055),
+b3MakeVector3(0.029639,0.502302,0.864184),
+b3MakeVector3(-0.947213,0.276396,-0.162458),
+b3MakeVector3(-1.000000,0.000001,0.000000),
+b3MakeVector3(-0.850648,0.525736,-0.000000),
+b3MakeVector3(-0.947213,0.276397,0.162458),
+b3MakeVector3(-0.812729,0.502301,0.295238),
+b3MakeVector3(-0.138199,0.276397,-0.951055),
+b3MakeVector3(-0.309016,-0.000000,-0.951057),
+b3MakeVector3(-0.262869,0.525738,-0.809012),
+b3MakeVector3(-0.447215,0.276397,-0.850649),
+b3MakeVector3(-0.531941,0.502302,-0.681712),
+b3MakeVector3(0.861804,0.276396,-0.425322),
+b3MakeVector3(0.809019,0.000000,-0.587782),
+b3MakeVector3(0.688189,0.525736,-0.499997),
+b3MakeVector3(0.670821,0.276397,-0.688189),
+b3MakeVector3(0.483971,0.502302,-0.716565),
+b3MakeVector3(0.077607,0.967950,0.238853),
+b3MakeVector3(0.251147,0.967949,0.000000),
+b3MakeVector3(0.000000,1.000000,0.000000),
+b3MakeVector3(0.162456,0.850654,0.499995),
+b3MakeVector3(0.361800,0.894429,0.262863),
+b3MakeVector3(0.447209,0.723612,0.525728),
+b3MakeVector3(0.525730,0.850652,0.000000),
+b3MakeVector3(0.638194,0.723610,0.262864),
+b3MakeVector3(-0.203181,0.967950,0.147618),
+b3MakeVector3(-0.425323,0.850654,0.309011),
+b3MakeVector3(-0.138197,0.894430,0.425320),
+b3MakeVector3(-0.361804,0.723612,0.587778),
+b3MakeVector3(-0.052790,0.723612,0.688185),
+b3MakeVector3(-0.203181,0.967950,-0.147618),
+b3MakeVector3(-0.425323,0.850654,-0.309011),
+b3MakeVector3(-0.447210,0.894429,0.000000),
+b3MakeVector3(-0.670817,0.723611,-0.162457),
+b3MakeVector3(-0.670817,0.723611,0.162457),
+b3MakeVector3(0.077607,0.967950,-0.238853),
+b3MakeVector3(0.162456,0.850654,-0.499995),
+b3MakeVector3(-0.138197,0.894430,-0.425320),
+b3MakeVector3(-0.052790,0.723612,-0.688185),
+b3MakeVector3(-0.361804,0.723612,-0.587778),
+b3MakeVector3(0.361800,0.894429,-0.262863),
+b3MakeVector3(0.638194,0.723610,-0.262864),
+b3MakeVector3(0.447209,0.723612,-0.525728)
+};
+
+
+bool b3FindSeparatingAxisEdgeEdge(	const b3ConvexPolyhedronData* hullA, __global const b3ConvexPolyhedronData* hullB, 
+	b3Float4ConstArg posA1,
+	b3QuatConstArg ornA,
+	b3Float4ConstArg posB1,
+	b3QuatConstArg ornB,
+	b3Float4ConstArg DeltaC2,
+	const b3Float4* verticesA, 
+	const b3Float4* uniqueEdgesA, 
+	const b3GpuFace* facesA,
+	const int*  indicesA,
+	__global const b3Float4* verticesB, 
+	__global const b3Float4* uniqueEdgesB, 
+	__global const b3GpuFace* facesB,
+	__global const int*  indicesB,
+		b3Float4* sep,
+	float* dmin,
+	bool searchAllEdgeEdge)
+{
+
+
+	b3Float4 posA = posA1;
+	posA.w = 0.f;
+	b3Float4 posB = posB1;
+	posB.w = 0.f;
+
+//	int curPlaneTests=0;
+
+	int curEdgeEdge = 0;
+	// Test edges
+	static int maxEdgeTests = 0;
+	int curEdgeTests = hullA->m_numUniqueEdges * hullB->m_numUniqueEdges;
+	if (curEdgeTests >maxEdgeTests )
+	{
+		maxEdgeTests  = curEdgeTests ;
+		printf("maxEdgeTests = %d\n",maxEdgeTests );
+		printf("hullA->m_numUniqueEdges = %d\n",hullA->m_numUniqueEdges);
+		printf("hullB->m_numUniqueEdges = %d\n",hullB->m_numUniqueEdges);
+
+	}
+
+	
+	if (searchAllEdgeEdge)
+	{
+		for(int e0=0;e0<hullA->m_numUniqueEdges;e0++)
+		{
+			const b3Float4 edge0 = uniqueEdgesA[hullA->m_uniqueEdgesOffset+e0];
+			b3Float4 edge0World = b3QuatRotate(ornA,edge0);
+
+			for(int e1=0;e1<hullB->m_numUniqueEdges;e1++)
+			{
+				const b3Float4 edge1 = uniqueEdgesB[hullB->m_uniqueEdgesOffset+e1];
+				b3Float4 edge1World = b3QuatRotate(ornB,edge1);
+
+
+				b3Float4 crossje = b3Cross(edge0World,edge1World);
+
+				curEdgeEdge++;
+				if(!b3IsAlmostZero(crossje))
+				{
+					crossje = b3Normalized(crossje);
+					if (b3Dot(DeltaC2,crossje)<0)
+						crossje *= -1.f;
+
+					float dist;
+					bool result = true;
+					{
+						float Min0,Max0;
+						float Min1,Max1;
+						b3Project(hullA,posA,ornA,&crossje,verticesA, &Min0, &Max0);
+						b3Project(hullB,posB,ornB,&crossje,verticesB, &Min1, &Max1);
+				
+						if(Max0<Min1 || Max1<Min0)
+							return false;
+                    
+						float d0 = Max0 - Min1;
+						float d1 = Max1 - Min0;
+						dist = d0<d1 ? d0:d1;
+						result = true;
+
+					}
+				
+
+					if(dist<*dmin)
+					{
+						*dmin = dist;
+						*sep = crossje;
+					}
+				}
+			}
+
+		}
+	} else
+	{
+		int numDirections = sizeof(unitSphere162)/sizeof(b3Vector3);
+		//printf("numDirections =%d\n",numDirections );
+
+
+		for(int i=0;i<numDirections;i++)
+		{
+			b3Float4 crossje = unitSphere162[i];
+			{
+				//if (b3Dot(DeltaC2,crossje)>0)
+				{
+					float dist;
+					bool result = true;
+					{
+						float Min0,Max0;
+						float Min1,Max1;
+						b3Project(hullA,posA,ornA,&crossje,verticesA, &Min0, &Max0);
+						b3Project(hullB,posB,ornB,&crossje,verticesB, &Min1, &Max1);
+				
+						if(Max0<Min1 || Max1<Min0)
+							return false;
+                    
+						float d0 = Max0 - Min1;
+						float d1 = Max1 - Min0;
+						dist = d0<d1 ? d0:d1;
+						result = true;
+
+					}
+				
+
+					if(dist<*dmin)
+					{
+						*dmin = dist;
+						*sep = crossje;
+					}
+				}
+			}
+		}
+
+	}
+
+	
+	if((b3Dot(-DeltaC2,*sep))>0.0f)
+	{
+		*sep = -(*sep);
+	}
+	return true;
+}
+
+
+
+inline int	b3FindClippingFaces(b3Float4ConstArg separatingNormal,
+                      __global const b3ConvexPolyhedronData_t* hullA, __global const b3ConvexPolyhedronData_t* hullB,
+                      b3Float4ConstArg posA, b3QuatConstArg ornA,b3Float4ConstArg posB, b3QuatConstArg ornB,
+                       __global b3Float4* worldVertsA1,
+                      __global b3Float4* worldNormalsA1,
+                      __global b3Float4* worldVertsB1,
+                      int capacityWorldVerts,
+                      const float minDist, float maxDist,
+                      __global const b3Float4* verticesA,
+                      __global const b3GpuFace_t* facesA,
+                      __global const int* indicesA,
+						__global const b3Float4* verticesB,
+                      __global const b3GpuFace_t* facesB,
+                      __global const int* indicesB,
+
+                      __global b3Int4* clippingFaces, int pairIndex)
+{
+	int numContactsOut = 0;
+	int numWorldVertsB1= 0;
+    
+    
+	int closestFaceB=-1;
+	float dmax = -FLT_MAX;
+    
+	{
+		for(int face=0;face<hullB->m_numFaces;face++)
+		{
+			const b3Float4 Normal = b3MakeFloat4(facesB[hullB->m_faceOffset+face].m_plane.x,
+                                              facesB[hullB->m_faceOffset+face].m_plane.y, facesB[hullB->m_faceOffset+face].m_plane.z,0.f);
+			const b3Float4 WorldNormal = b3QuatRotate(ornB, Normal);
+			float d = b3Dot(WorldNormal,separatingNormal);
+			if (d > dmax)
+			{
+				dmax = d;
+				closestFaceB = face;
+			}
+		}
+	}
+    
+	{
+		const b3GpuFace_t polyB = facesB[hullB->m_faceOffset+closestFaceB];
+		const int numVertices = polyB.m_numIndices;
+		for(int e0=0;e0<numVertices;e0++)
+		{
+			const b3Float4 b = verticesB[hullB->m_vertexOffset+indicesB[polyB.m_indexOffset+e0]];
+			worldVertsB1[pairIndex*capacityWorldVerts+numWorldVertsB1++] = b3TransformPoint(b,posB,ornB);
+		}
+	}
+    
+    int closestFaceA=-1;
+	{
+		float dmin = FLT_MAX;
+		for(int face=0;face<hullA->m_numFaces;face++)
+		{
+			const b3Float4 Normal = b3MakeFloat4(
+                                              facesA[hullA->m_faceOffset+face].m_plane.x,
+                                              facesA[hullA->m_faceOffset+face].m_plane.y,
+                                              facesA[hullA->m_faceOffset+face].m_plane.z,
+                                              0.f);
+			const b3Float4 faceANormalWS = b3QuatRotate(ornA,Normal);
+            
+			float d = b3Dot(faceANormalWS,separatingNormal);
+			if (d < dmin)
+			{
+				dmin = d;
+				closestFaceA = face;
+                worldNormalsA1[pairIndex] = faceANormalWS;
+			}
+		}
+	}
+    
+    int numVerticesA = facesA[hullA->m_faceOffset+closestFaceA].m_numIndices;
+	for(int e0=0;e0<numVerticesA;e0++)
+	{
+        const b3Float4 a = verticesA[hullA->m_vertexOffset+indicesA[facesA[hullA->m_faceOffset+closestFaceA].m_indexOffset+e0]];
+        worldVertsA1[pairIndex*capacityWorldVerts+e0] = b3TransformPoint(a, posA,ornA);
+    }
+    
+    clippingFaces[pairIndex].x = closestFaceA;
+    clippingFaces[pairIndex].y = closestFaceB;
+    clippingFaces[pairIndex].z = numVerticesA;
+    clippingFaces[pairIndex].w = numWorldVertsB1;
+    
+    
+	return numContactsOut;
+}
+
+
+        
+
+__kernel void   b3FindConcaveSeparatingAxisKernel( __global b3Int4* concavePairs,
+																					__global const b3RigidBodyData* rigidBodies,
+																					__global const b3Collidable* collidables,
+																					__global const b3ConvexPolyhedronData* convexShapes, 
+																					__global const b3Float4* vertices,
+																					__global const b3Float4* uniqueEdges,
+																					__global const b3GpuFace* faces,
+																					__global const int* indices,
+																					__global const b3GpuChildShape* gpuChildShapes,
+																					__global b3Aabb* aabbs,
+																					__global b3Float4* concaveSeparatingNormalsOut,
+																					__global b3Int4* clippingFacesOut,
+																					__global b3Vector3* worldVertsA1Out,
+																					__global b3Vector3* worldNormalsA1Out,
+																					__global b3Vector3* worldVertsB1Out,
+																					__global int* hasSeparatingNormals,
+																					int vertexFaceCapacity,
+																					int numConcavePairs,
+																					int pairIdx
+																					)
+{
+	int i = pairIdx;
+/*	int i = get_global_id(0);
+	if (i>=numConcavePairs)
+		return;
+	int pairIdx = i;
+	*/
+
+	int bodyIndexA = concavePairs[i].x;
+	int bodyIndexB = concavePairs[i].y;
+
+	int collidableIndexA = rigidBodies[bodyIndexA].m_collidableIdx;
+	int collidableIndexB = rigidBodies[bodyIndexB].m_collidableIdx;
+
+	int shapeIndexA = collidables[collidableIndexA].m_shapeIndex;
+	int shapeIndexB = collidables[collidableIndexB].m_shapeIndex;
+
+	if (collidables[collidableIndexB].m_shapeType!=SHAPE_CONVEX_HULL&&
+		collidables[collidableIndexB].m_shapeType!=SHAPE_COMPOUND_OF_CONVEX_HULLS)
+	{
+		concavePairs[pairIdx].w = -1;
+		return;
+	}
+
+	hasSeparatingNormals[i] = 0;
+
+//	int numFacesA = convexShapes[shapeIndexA].m_numFaces;
+	int numActualConcaveConvexTests = 0;
+	
+	int f = concavePairs[i].z;
+	
+	bool overlap = false;
+	
+	b3ConvexPolyhedronData convexPolyhedronA;
+
+	//add 3 vertices of the triangle
+	convexPolyhedronA.m_numVertices = 3;
+	convexPolyhedronA.m_vertexOffset = 0;
+	b3Float4	localCenter = b3MakeFloat4(0.f,0.f,0.f,0.f);
+
+	b3GpuFace face = faces[convexShapes[shapeIndexA].m_faceOffset+f];
+	b3Aabb triAabb;
+	triAabb.m_minVec = b3MakeFloat4(1e30f,1e30f,1e30f,0.f);
+	triAabb.m_maxVec = b3MakeFloat4(-1e30f,-1e30f,-1e30f,0.f);
+	
+	b3Float4 verticesA[3];
+	for (int i=0;i<3;i++)
+	{
+		int index = indices[face.m_indexOffset+i];
+		b3Float4 vert = vertices[convexShapes[shapeIndexA].m_vertexOffset+index];
+		verticesA[i] = vert;
+		localCenter += vert;
+			
+		triAabb.m_minVec = b3MinFloat4(triAabb.m_minVec,vert);		
+		triAabb.m_maxVec = b3MaxFloat4(triAabb.m_maxVec,vert);		
+
+	}
+
+	overlap = true;
+	overlap = (triAabb.m_minVec.x > aabbs[bodyIndexB].m_maxVec.x || triAabb.m_maxVec.x < aabbs[bodyIndexB].m_minVec.x) ? false : overlap;
+	overlap = (triAabb.m_minVec.z > aabbs[bodyIndexB].m_maxVec.z || triAabb.m_maxVec.z < aabbs[bodyIndexB].m_minVec.z) ? false : overlap;
+	overlap = (triAabb.m_minVec.y > aabbs[bodyIndexB].m_maxVec.y || triAabb.m_maxVec.y < aabbs[bodyIndexB].m_minVec.y) ? false : overlap;
+		
+	if (overlap)
+	{
+		float dmin = FLT_MAX;
+		int hasSeparatingAxis=5;
+		b3Float4 sepAxis=b3MakeFloat4(1,2,3,4);
+
+	//	int localCC=0;
+		numActualConcaveConvexTests++;
+
+		//a triangle has 3 unique edges
+		convexPolyhedronA.m_numUniqueEdges = 3;
+		convexPolyhedronA.m_uniqueEdgesOffset = 0;
+		b3Float4 uniqueEdgesA[3];
+		
+		uniqueEdgesA[0] = (verticesA[1]-verticesA[0]);
+		uniqueEdgesA[1] = (verticesA[2]-verticesA[1]);
+		uniqueEdgesA[2] = (verticesA[0]-verticesA[2]);
+
+
+		convexPolyhedronA.m_faceOffset = 0;
+                                  
+		b3Float4 normal = b3MakeFloat4(face.m_plane.x,face.m_plane.y,face.m_plane.z,0.f);
+                             
+		b3GpuFace facesA[B3_TRIANGLE_NUM_CONVEX_FACES];
+		int indicesA[3+3+2+2+2];
+		int curUsedIndices=0;
+		int fidx=0;
+
+		//front size of triangle
+		{
+			facesA[fidx].m_indexOffset=curUsedIndices;
+			indicesA[0] = 0;
+			indicesA[1] = 1;
+			indicesA[2] = 2;
+			curUsedIndices+=3;
+			float c = face.m_plane.w;
+			facesA[fidx].m_plane.x = normal.x;
+			facesA[fidx].m_plane.y = normal.y;
+			facesA[fidx].m_plane.z = normal.z;
+			facesA[fidx].m_plane.w = c;
+			facesA[fidx].m_numIndices=3;
+		}
+		fidx++;
+		//back size of triangle
+		{
+			facesA[fidx].m_indexOffset=curUsedIndices;
+			indicesA[3]=2;
+			indicesA[4]=1;
+			indicesA[5]=0;
+			curUsedIndices+=3;
+			float c = b3Dot(normal,verticesA[0]);
+		//	float c1 = -face.m_plane.w;
+			facesA[fidx].m_plane.x = -normal.x;
+			facesA[fidx].m_plane.y = -normal.y;
+			facesA[fidx].m_plane.z = -normal.z;
+			facesA[fidx].m_plane.w = c;
+			facesA[fidx].m_numIndices=3;
+		}
+		fidx++;
+
+		bool addEdgePlanes = true;
+		if (addEdgePlanes)
+		{
+			int numVertices=3;
+			int prevVertex = numVertices-1;
+			for (int i=0;i<numVertices;i++)
+			{
+				b3Float4 v0 = verticesA[i];
+				b3Float4 v1 = verticesA[prevVertex];
+                                            
+				b3Float4 edgeNormal = b3Normalized(b3Cross(normal,v1-v0));
+				float c = -b3Dot(edgeNormal,v0);
+
+				facesA[fidx].m_numIndices = 2;
+				facesA[fidx].m_indexOffset=curUsedIndices;
+				indicesA[curUsedIndices++]=i;
+				indicesA[curUsedIndices++]=prevVertex;
+                                            
+				facesA[fidx].m_plane.x = edgeNormal.x;
+				facesA[fidx].m_plane.y = edgeNormal.y;
+				facesA[fidx].m_plane.z = edgeNormal.z;
+				facesA[fidx].m_plane.w = c;
+				fidx++;
+				prevVertex = i;
+			}
+		}
+		convexPolyhedronA.m_numFaces = B3_TRIANGLE_NUM_CONVEX_FACES;
+		convexPolyhedronA.m_localCenter = localCenter*(1.f/3.f);
+
+
+		b3Float4 posA = rigidBodies[bodyIndexA].m_pos;
+		posA.w = 0.f;
+		b3Float4 posB = rigidBodies[bodyIndexB].m_pos;
+		posB.w = 0.f;
+
+		b3Quaternion ornA = rigidBodies[bodyIndexA].m_quat;
+		b3Quaternion ornB =rigidBodies[bodyIndexB].m_quat;
+
+		
+
+
+		///////////////////
+		///compound shape support
+
+		if (collidables[collidableIndexB].m_shapeType==SHAPE_COMPOUND_OF_CONVEX_HULLS)
+		{
+			int compoundChild = concavePairs[pairIdx].w;
+			int childShapeIndexB = compoundChild;//collidables[collidableIndexB].m_shapeIndex+compoundChild;
+			int childColIndexB = gpuChildShapes[childShapeIndexB].m_shapeIndex;
+			b3Float4 childPosB = gpuChildShapes[childShapeIndexB].m_childPosition;
+			b3Quaternion childOrnB = gpuChildShapes[childShapeIndexB].m_childOrientation;
+			b3Float4 newPosB = b3TransformPoint(childPosB,posB,ornB);
+			b3Quaternion newOrnB = b3QuatMul(ornB,childOrnB);
+			posB = newPosB;
+			ornB = newOrnB;
+			shapeIndexB = collidables[childColIndexB].m_shapeIndex;
+		}
+		//////////////////
+
+		b3Float4 c0local = convexPolyhedronA.m_localCenter;
+		b3Float4 c0 = b3TransformPoint(c0local, posA, ornA);
+		b3Float4 c1local = convexShapes[shapeIndexB].m_localCenter;
+		b3Float4 c1 = b3TransformPoint(c1local,posB,ornB);
+		const b3Float4 DeltaC2 = c0 - c1;
+
+
+		bool sepA = b3FindSeparatingAxis(	&convexPolyhedronA, &convexShapes[shapeIndexB],
+												posA,ornA,
+												posB,ornB,
+												DeltaC2,
+												verticesA,uniqueEdgesA,facesA,indicesA,
+												vertices,uniqueEdges,faces,indices,
+												&sepAxis,&dmin);
+		hasSeparatingAxis = 4;
+		if (!sepA)
+		{
+			hasSeparatingAxis = 0;
+		} else
+		{
+			bool sepB = b3FindSeparatingAxis(	&convexShapes[shapeIndexB],&convexPolyhedronA,
+												posB,ornB,
+												posA,ornA,
+												DeltaC2,
+												vertices,uniqueEdges,faces,indices,
+												verticesA,uniqueEdgesA,facesA,indicesA,
+												&sepAxis,&dmin);
+
+			if (!sepB)
+			{
+				hasSeparatingAxis = 0;
+			} else
+			{
+				bool sepEE = b3FindSeparatingAxisEdgeEdge(	&convexPolyhedronA, &convexShapes[shapeIndexB],
+															posA,ornA,
+															posB,ornB,
+															DeltaC2,
+															verticesA,uniqueEdgesA,facesA,indicesA,
+															vertices,uniqueEdges,faces,indices,
+															&sepAxis,&dmin,true);
+	
+				if (!sepEE)
+				{
+					hasSeparatingAxis = 0;
+				} else
+				{
+					hasSeparatingAxis = 1;
+				}
+			}
+		}	
+		
+		if (hasSeparatingAxis)
+		{
+			hasSeparatingNormals[i]=1;
+			sepAxis.w = dmin;
+			concaveSeparatingNormalsOut[pairIdx]=sepAxis;
+
+			//now compute clipping faces A and B, and world-space clipping vertices A and B...
+
+			float minDist = -1e30f;
+			float maxDist = 0.02f;
+
+			b3FindClippingFaces(sepAxis,
+                     &convexPolyhedronA,
+					 &convexShapes[shapeIndexB],
+					 posA,ornA,
+					 posB,ornB,
+                       worldVertsA1Out,
+                      worldNormalsA1Out,
+                      worldVertsB1Out,
+					  vertexFaceCapacity,
+                      minDist, maxDist,
+                      verticesA,
+                      facesA,
+                      indicesA,
+ 
+					  vertices,
+                      faces,
+                      indices,
+                      clippingFacesOut, pairIdx);
+
+		} else
+		{	
+			//mark this pair as in-active
+			concavePairs[pairIdx].w = -1;
+		}
+	}
+	else
+	{	
+		//mark this pair as in-active
+		concavePairs[pairIdx].w = -1;
+	}
+}
+
+
+#endif //B3_FIND_CONCAVE_SEPARATING_AXIS_H
+
diff --git a/thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3FindSeparatingAxis.h b/thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3FindSeparatingAxis.h
new file mode 100644
index 0000000000..332dbc278c
--- /dev/null
+++ b/thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3FindSeparatingAxis.h
@@ -0,0 +1,206 @@
+#ifndef B3_FIND_SEPARATING_AXIS_H
+#define B3_FIND_SEPARATING_AXIS_H
+
+
+inline void b3ProjectAxis(const b3ConvexPolyhedronData& hull,  const b3Float4& pos, const b3Quaternion& orn, const b3Float4& dir, const b3AlignedObjectArray<b3Vector3>& vertices, b3Scalar& min, b3Scalar& max)
+{
+	min = FLT_MAX;
+	max = -FLT_MAX;
+	int numVerts = hull.m_numVertices;
+
+	const b3Float4 localDir = b3QuatRotate(orn.inverse(),dir);
+
+	b3Scalar offset = b3Dot3F4(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 = b3Dot3F4((b3Float4&)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;
+}
+
+
+inline bool b3TestSepAxis(const b3ConvexPolyhedronData& hullA, const b3ConvexPolyhedronData& hullB, 
+	const b3Float4& posA,const b3Quaternion& ornA,
+	const b3Float4& posB,const b3Quaternion& ornB,
+	const b3Float4& sep_axis, const b3AlignedObjectArray<b3Vector3>& verticesA,const b3AlignedObjectArray<b3Vector3>& verticesB,b3Scalar& depth)
+{
+	b3Scalar Min0,Max0;
+	b3Scalar Min1,Max1;
+	b3ProjectAxis(hullA,posA,ornA,sep_axis,verticesA, Min0, Max0);
+	b3ProjectAxis(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);
+	depth = d0<d1 ? d0:d1;
+	return true;
+}
+
+
+inline bool b3FindSeparatingAxis(	const b3ConvexPolyhedronData& hullA, const b3ConvexPolyhedronData& hullB, 
+	const b3Float4& posA1,
+	const b3Quaternion& ornA,
+	const b3Float4& posB1,
+	const b3Quaternion& ornB,
+	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,
+
+	b3Vector3& sep)
+{
+	B3_PROFILE("findSeparatingAxis");
+
+	b3Float4 posA = posA1;
+	posA.w = 0.f;
+	b3Float4 posB = posB1;
+	posB.w = 0.f;
+//#ifdef TEST_INTERNAL_OBJECTS
+	b3Float4 c0local = (b3Float4&)hullA.m_localCenter;
+
+	b3Float4 c0 = b3TransformPoint(c0local, posA, ornA);
+	b3Float4 c1local = (b3Float4&)hullB.m_localCenter;
+	b3Float4 c1 = b3TransformPoint(c1local,posB,ornB);
+	const b3Float4 deltaC2 = c0 - c1;
+//#endif
+
+	b3Scalar dmin = FLT_MAX;
+	int curPlaneTests=0;
+
+	int numFacesA = hullA.m_numFaces;
+	// Test normals from hullA
+	for(int i=0;i<numFacesA;i++)
+	{
+		const b3Float4& normal = (b3Float4&)facesA[hullA.m_faceOffset+i].m_plane;
+		b3Float4 faceANormalWS = b3QuatRotate(ornA,normal);
+
+		if (b3Dot3F4(deltaC2,faceANormalWS)<0)
+			faceANormalWS*=-1.f;
+
+		curPlaneTests++;
+#ifdef TEST_INTERNAL_OBJECTS
+		gExpectedNbTests++;
+		if(gUseInternalObject && !TestInternalObjects(transA,transB, DeltaC2, faceANormalWS, hullA, hullB, dmin))
+			continue;
+		gActualNbTests++;
+#endif
+
+		
+		b3Scalar d;
+		if(!b3TestSepAxis( hullA, hullB, posA,ornA,posB,ornB,faceANormalWS, verticesA, verticesB,d))
+			return false;
+
+		if(d<dmin)
+		{
+			dmin = d;
+			sep = (b3Vector3&)faceANormalWS;
+		}
+	}
+
+	int numFacesB = hullB.m_numFaces;
+	// Test normals from hullB
+	for(int i=0;i<numFacesB;i++)
+	{
+		b3Float4 normal = (b3Float4&)facesB[hullB.m_faceOffset+i].m_plane;
+		b3Float4 WorldNormal = b3QuatRotate(ornB, normal);
+
+		if (b3Dot3F4(deltaC2,WorldNormal)<0)
+		{
+			WorldNormal*=-1.f;
+		}
+		curPlaneTests++;
+#ifdef TEST_INTERNAL_OBJECTS
+		gExpectedNbTests++;
+		if(gUseInternalObject && !TestInternalObjects(transA,transB,DeltaC2, WorldNormal, hullA, hullB, dmin))
+			continue;
+		gActualNbTests++;
+#endif
+
+		b3Scalar d;
+		if(!b3TestSepAxis(hullA, hullB,posA,ornA,posB,ornB,WorldNormal,verticesA,verticesB,d))
+			return false;
+
+		if(d<dmin)
+		{
+			dmin = d;
+			sep = (b3Vector3&)WorldNormal;
+		}
+	}
+
+//	b3Vector3 edgeAstart,edgeAend,edgeBstart,edgeBend;
+
+	int curEdgeEdge = 0;
+	// Test edges
+	for(int e0=0;e0<hullA.m_numUniqueEdges;e0++)
+	{
+		const b3Float4& edge0 = (b3Float4&) uniqueEdgesA[hullA.m_uniqueEdgesOffset+e0];
+		b3Float4 edge0World = b3QuatRotate(ornA,(b3Float4&)edge0);
+
+		for(int e1=0;e1<hullB.m_numUniqueEdges;e1++)
+		{
+			const b3Vector3 edge1 = uniqueEdgesB[hullB.m_uniqueEdgesOffset+e1];
+			b3Float4 edge1World = b3QuatRotate(ornB,(b3Float4&)edge1);
+
+
+			b3Float4 crossje = b3Cross3(edge0World,edge1World);
+
+			curEdgeEdge++;
+			if(!b3IsAlmostZero((b3Vector3&)crossje))
+			{
+				crossje = b3FastNormalized3(crossje);
+				if (b3Dot3F4(deltaC2,crossje)<0)
+					crossje*=-1.f;
+
+
+#ifdef TEST_INTERNAL_OBJECTS
+				gExpectedNbTests++;
+				if(gUseInternalObject && !TestInternalObjects(transA,transB,DeltaC2, Cross, hullA, hullB, dmin))
+					continue;
+				gActualNbTests++;
+#endif
+
+				b3Scalar dist;
+				if(!b3TestSepAxis( hullA, hullB, posA,ornA,posB,ornB,crossje, verticesA,verticesB,dist))
+					return false;
+
+				if(dist<dmin)
+				{
+					dmin = dist;
+					sep = (b3Vector3&)crossje;
+				}
+			}
+		}
+
+	}
+
+	
+	if((b3Dot3F4(-deltaC2,(b3Float4&)sep))>0.0f)
+		sep = -sep;
+
+	return true;
+}
+
+#endif //B3_FIND_SEPARATING_AXIS_H
+
diff --git a/thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3MprPenetration.h b/thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3MprPenetration.h
new file mode 100644
index 0000000000..6c3ad7c9dd
--- /dev/null
+++ b/thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3MprPenetration.h
@@ -0,0 +1,920 @@
+
+/***
+ * ---------------------------------
+ * Copyright (c)2012 Daniel Fiser <danfis@danfis.cz>
+ *
+ *  This file was ported from mpr.c file, part of libccd.
+ *  The Minkoski Portal Refinement implementation was ported 
+ *  to OpenCL by Erwin Coumans for the Bullet 3 Physics library.
+ *  at http://github.com/erwincoumans/bullet3
+ *
+ *  Distributed under the OSI-approved BSD License (the "License");
+ *  see <http://www.opensource.org/licenses/bsd-license.php>.
+ *  This software is distributed WITHOUT ANY WARRANTY; without even the
+ *  implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ *  See the License for more information.
+ */
+
+
+
+
+#ifndef B3_MPR_PENETRATION_H
+#define B3_MPR_PENETRATION_H
+
+#include "Bullet3Common/shared/b3PlatformDefinitions.h"
+#include "Bullet3Common/shared/b3Float4.h"
+#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h"
+#include "Bullet3Collision/NarrowPhaseCollision/shared/b3ConvexPolyhedronData.h"
+#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Collidable.h"
+
+
+
+
+#ifdef __cplusplus
+#define B3_MPR_SQRT sqrtf
+#else
+#define B3_MPR_SQRT sqrt
+#endif
+#define B3_MPR_FMIN(x, y) ((x) < (y) ? (x) : (y))
+#define B3_MPR_FABS fabs
+
+#define B3_MPR_TOLERANCE 1E-6f
+#define B3_MPR_MAX_ITERATIONS 1000
+
+struct _b3MprSupport_t 
+{
+    b3Float4 v;  //!< Support point in minkowski sum
+    b3Float4 v1; //!< Support point in obj1
+    b3Float4 v2; //!< Support point in obj2
+};
+typedef struct _b3MprSupport_t b3MprSupport_t;
+
+struct _b3MprSimplex_t 
+{
+    b3MprSupport_t ps[4];
+    int last; //!< index of last added point
+};
+typedef struct _b3MprSimplex_t b3MprSimplex_t;
+
+inline b3MprSupport_t* b3MprSimplexPointW(b3MprSimplex_t *s, int idx)
+{
+    return &s->ps[idx];
+}
+
+inline void b3MprSimplexSetSize(b3MprSimplex_t *s, int size)
+{
+    s->last = size - 1;
+}
+
+
+inline int b3MprSimplexSize(const b3MprSimplex_t *s)
+{
+    return s->last + 1;
+}
+
+
+inline const b3MprSupport_t* b3MprSimplexPoint(const b3MprSimplex_t* s, int idx)
+{
+    // here is no check on boundaries
+    return &s->ps[idx];
+}
+
+inline void b3MprSupportCopy(b3MprSupport_t *d, const b3MprSupport_t *s)
+{
+    *d = *s;
+}
+
+inline void b3MprSimplexSet(b3MprSimplex_t *s, size_t pos, const b3MprSupport_t *a)
+{
+    b3MprSupportCopy(s->ps + pos, a);
+}
+
+
+inline void b3MprSimplexSwap(b3MprSimplex_t *s, size_t pos1, size_t pos2)
+{
+    b3MprSupport_t supp;
+
+    b3MprSupportCopy(&supp, &s->ps[pos1]);
+    b3MprSupportCopy(&s->ps[pos1], &s->ps[pos2]);
+    b3MprSupportCopy(&s->ps[pos2], &supp);
+}
+
+
+inline int b3MprIsZero(float val)
+{
+    return B3_MPR_FABS(val) < FLT_EPSILON;
+}
+
+
+
+inline int b3MprEq(float _a, float _b)
+{
+    float ab;
+    float a, b;
+
+    ab = B3_MPR_FABS(_a - _b);
+    if (B3_MPR_FABS(ab) < FLT_EPSILON)
+        return 1;
+
+    a = B3_MPR_FABS(_a);
+    b = B3_MPR_FABS(_b);
+    if (b > a){
+        return ab < FLT_EPSILON * b;
+    }else{
+        return ab < FLT_EPSILON * a;
+    }
+}
+
+
+inline int b3MprVec3Eq(const b3Float4* a, const b3Float4 *b)
+{
+    return b3MprEq((*a).x, (*b).x)
+            && b3MprEq((*a).y, (*b).y)
+            && b3MprEq((*a).z, (*b).z);
+}
+
+
+
+inline b3Float4 b3LocalGetSupportVertex(b3Float4ConstArg supportVec,__global const b3ConvexPolyhedronData_t* hull, 	b3ConstArray(b3Float4) verticesA)
+{
+	b3Float4 supVec = b3MakeFloat4(0,0,0,0);
+	float maxDot = -B3_LARGE_FLOAT;
+
+    if( 0 < hull->m_numVertices )
+    {
+        const b3Float4 scaled = supportVec;
+		int index = b3MaxDot(scaled, &verticesA[hull->m_vertexOffset], hull->m_numVertices, &maxDot);
+        return verticesA[hull->m_vertexOffset+index];
+    }
+
+    return supVec;
+
+}
+
+
+B3_STATIC void b3MprConvexSupport(int pairIndex,int bodyIndex,  b3ConstArray(b3RigidBodyData_t) cpuBodyBuf, 
+													b3ConstArray(b3ConvexPolyhedronData_t) cpuConvexData, 
+													b3ConstArray(b3Collidable_t)				cpuCollidables,
+													b3ConstArray(b3Float4)					cpuVertices,
+													__global b3Float4* sepAxis,
+														const b3Float4* _dir, b3Float4* outp, int logme)
+{
+	//dir is in worldspace, move to local space
+	
+	b3Float4 pos = cpuBodyBuf[bodyIndex].m_pos;
+	b3Quat orn = cpuBodyBuf[bodyIndex].m_quat;
+	
+	b3Float4 dir = b3MakeFloat4((*_dir).x,(*_dir).y,(*_dir).z,0.f);
+	
+	const b3Float4 localDir = b3QuatRotate(b3QuatInverse(orn),dir);
+	
+
+	//find local support vertex
+	int colIndex = cpuBodyBuf[bodyIndex].m_collidableIdx;
+	
+	b3Assert(cpuCollidables[colIndex].m_shapeType==SHAPE_CONVEX_HULL);
+	__global const b3ConvexPolyhedronData_t* hull = &cpuConvexData[cpuCollidables[colIndex].m_shapeIndex];
+	
+	b3Float4 pInA;
+	if (logme)
+	{
+
+
+	//	b3Float4 supVec = b3MakeFloat4(0,0,0,0);
+		float maxDot = -B3_LARGE_FLOAT;
+
+		if( 0 < hull->m_numVertices )
+		{
+			const b3Float4 scaled = localDir;
+			int index = b3MaxDot(scaled, &cpuVertices[hull->m_vertexOffset], hull->m_numVertices, &maxDot);
+			pInA = cpuVertices[hull->m_vertexOffset+index];
+			
+		}
+
+
+	} else
+	{
+		pInA = b3LocalGetSupportVertex(localDir,hull,cpuVertices);
+	}
+
+	//move vertex to world space
+	*outp = b3TransformPoint(pInA,pos,orn);
+	
+}
+
+inline void b3MprSupport(int pairIndex,int bodyIndexA, int bodyIndexB,   b3ConstArray(b3RigidBodyData_t) cpuBodyBuf, 
+													b3ConstArray(b3ConvexPolyhedronData_t) cpuConvexData, 
+													b3ConstArray(b3Collidable_t)				cpuCollidables,
+													b3ConstArray(b3Float4)					cpuVertices,
+													__global b3Float4* sepAxis,
+													const b3Float4* _dir, b3MprSupport_t *supp)
+{
+    b3Float4 dir;
+	dir = *_dir;
+	b3MprConvexSupport(pairIndex,bodyIndexA,cpuBodyBuf,cpuConvexData,cpuCollidables,cpuVertices,sepAxis,&dir, &supp->v1,0);
+    dir = *_dir*-1.f;
+	b3MprConvexSupport(pairIndex,bodyIndexB,cpuBodyBuf,cpuConvexData,cpuCollidables,cpuVertices,sepAxis,&dir, &supp->v2,0);
+    supp->v = supp->v1 - supp->v2;
+}
+
+
+
+
+
+
+
+
+
+inline void b3FindOrigin(int bodyIndexA, int bodyIndexB, b3ConstArray(b3RigidBodyData_t) cpuBodyBuf, b3MprSupport_t *center)
+{
+
+    center->v1 = cpuBodyBuf[bodyIndexA].m_pos;
+	center->v2 = cpuBodyBuf[bodyIndexB].m_pos;
+    center->v = center->v1 - center->v2;
+}
+
+inline void b3MprVec3Set(b3Float4 *v, float x, float y, float z)
+{
+	(*v).x = x;
+	(*v).y = y;
+	(*v).z = z;
+	(*v).w = 0.f;
+}
+
+inline void b3MprVec3Add(b3Float4 *v, const b3Float4 *w)
+{
+    (*v).x += (*w).x;
+    (*v).y += (*w).y;
+    (*v).z += (*w).z;
+}
+
+inline void b3MprVec3Copy(b3Float4 *v, const b3Float4 *w)
+{
+    *v = *w;
+}
+
+inline void b3MprVec3Scale(b3Float4 *d, float k)
+{
+    *d *= k;
+}
+
+inline float b3MprVec3Dot(const b3Float4 *a, const b3Float4 *b)
+{
+    float dot;
+
+	dot = b3Dot3F4(*a,*b);
+    return dot;
+}
+
+
+inline float b3MprVec3Len2(const b3Float4 *v)
+{
+    return b3MprVec3Dot(v, v);
+}
+
+inline void b3MprVec3Normalize(b3Float4 *d)
+{
+    float k = 1.f / B3_MPR_SQRT(b3MprVec3Len2(d));
+    b3MprVec3Scale(d, k);
+}
+
+inline void b3MprVec3Cross(b3Float4 *d, const b3Float4 *a, const b3Float4 *b)
+{
+	*d = b3Cross3(*a,*b);
+	
+}
+
+
+inline void b3MprVec3Sub2(b3Float4 *d, const b3Float4 *v, const b3Float4 *w)
+{
+	*d = *v - *w;
+}
+
+inline void b3PortalDir(const b3MprSimplex_t *portal, b3Float4 *dir)
+{
+    b3Float4 v2v1, v3v1;
+
+    b3MprVec3Sub2(&v2v1, &b3MprSimplexPoint(portal, 2)->v,
+                       &b3MprSimplexPoint(portal, 1)->v);
+    b3MprVec3Sub2(&v3v1, &b3MprSimplexPoint(portal, 3)->v,
+                       &b3MprSimplexPoint(portal, 1)->v);
+    b3MprVec3Cross(dir, &v2v1, &v3v1);
+    b3MprVec3Normalize(dir);
+}
+
+
+inline int portalEncapsulesOrigin(const b3MprSimplex_t *portal,
+                                       const b3Float4 *dir)
+{
+    float dot;
+    dot = b3MprVec3Dot(dir, &b3MprSimplexPoint(portal, 1)->v);
+    return b3MprIsZero(dot) || dot > 0.f;
+}
+
+inline int portalReachTolerance(const b3MprSimplex_t *portal,
+                                     const b3MprSupport_t *v4,
+                                     const b3Float4 *dir)
+{
+    float dv1, dv2, dv3, dv4;
+    float dot1, dot2, dot3;
+
+    // find the smallest dot product of dir and {v1-v4, v2-v4, v3-v4}
+
+    dv1 = b3MprVec3Dot(&b3MprSimplexPoint(portal, 1)->v, dir);
+    dv2 = b3MprVec3Dot(&b3MprSimplexPoint(portal, 2)->v, dir);
+    dv3 = b3MprVec3Dot(&b3MprSimplexPoint(portal, 3)->v, dir);
+    dv4 = b3MprVec3Dot(&v4->v, dir);
+
+    dot1 = dv4 - dv1;
+    dot2 = dv4 - dv2;
+    dot3 = dv4 - dv3;
+
+    dot1 = B3_MPR_FMIN(dot1, dot2);
+    dot1 = B3_MPR_FMIN(dot1, dot3);
+
+    return b3MprEq(dot1, B3_MPR_TOLERANCE) || dot1 < B3_MPR_TOLERANCE;
+}
+
+inline int portalCanEncapsuleOrigin(const b3MprSimplex_t *portal,   
+                                         const b3MprSupport_t *v4,
+                                         const b3Float4 *dir)
+{
+    float dot;
+    dot = b3MprVec3Dot(&v4->v, dir);
+    return b3MprIsZero(dot) || dot > 0.f;
+}
+
+inline void b3ExpandPortal(b3MprSimplex_t *portal,
+                              const b3MprSupport_t *v4)
+{
+    float dot;
+    b3Float4 v4v0;
+
+    b3MprVec3Cross(&v4v0, &v4->v, &b3MprSimplexPoint(portal, 0)->v);
+    dot = b3MprVec3Dot(&b3MprSimplexPoint(portal, 1)->v, &v4v0);
+    if (dot > 0.f){
+        dot = b3MprVec3Dot(&b3MprSimplexPoint(portal, 2)->v, &v4v0);
+        if (dot > 0.f){
+            b3MprSimplexSet(portal, 1, v4);
+        }else{
+            b3MprSimplexSet(portal, 3, v4);
+        }
+    }else{
+        dot = b3MprVec3Dot(&b3MprSimplexPoint(portal, 3)->v, &v4v0);
+        if (dot > 0.f){
+            b3MprSimplexSet(portal, 2, v4);
+        }else{
+            b3MprSimplexSet(portal, 1, v4);
+        }
+    }
+}
+
+
+
+B3_STATIC int b3DiscoverPortal(int pairIndex, int bodyIndexA, int bodyIndexB,  b3ConstArray(b3RigidBodyData_t) cpuBodyBuf, 
+													b3ConstArray(b3ConvexPolyhedronData_t) cpuConvexData, 
+													b3ConstArray(b3Collidable_t)				cpuCollidables,
+													b3ConstArray(b3Float4)					cpuVertices,
+													__global b3Float4* sepAxis,
+													__global int*	hasSepAxis,
+													b3MprSimplex_t *portal)
+{
+    b3Float4 dir, va, vb;
+    float dot;
+    int cont;
+	
+	
+
+    // vertex 0 is center of portal
+    b3FindOrigin(bodyIndexA,bodyIndexB,cpuBodyBuf, b3MprSimplexPointW(portal, 0));
+    // vertex 0 is center of portal
+    b3MprSimplexSetSize(portal, 1);
+	
+
+
+	b3Float4 zero = b3MakeFloat4(0,0,0,0);
+	b3Float4* b3mpr_vec3_origin = &zero;
+
+    if (b3MprVec3Eq(&b3MprSimplexPoint(portal, 0)->v, b3mpr_vec3_origin)){
+        // Portal's center lies on origin (0,0,0) => we know that objects
+        // intersect but we would need to know penetration info.
+        // So move center little bit...
+        b3MprVec3Set(&va, FLT_EPSILON * 10.f, 0.f, 0.f);
+        b3MprVec3Add(&b3MprSimplexPointW(portal, 0)->v, &va);
+    }
+
+
+    // vertex 1 = support in direction of origin
+    b3MprVec3Copy(&dir, &b3MprSimplexPoint(portal, 0)->v);
+    b3MprVec3Scale(&dir, -1.f);
+    b3MprVec3Normalize(&dir);
+
+
+    b3MprSupport(pairIndex,bodyIndexA,bodyIndexB,cpuBodyBuf,cpuConvexData,cpuCollidables,cpuVertices, sepAxis,&dir, b3MprSimplexPointW(portal, 1));
+
+    b3MprSimplexSetSize(portal, 2);
+
+    // test if origin isn't outside of v1
+    dot = b3MprVec3Dot(&b3MprSimplexPoint(portal, 1)->v, &dir);
+	
+
+    if (b3MprIsZero(dot) || dot < 0.f)
+        return -1;
+
+
+    // vertex 2
+    b3MprVec3Cross(&dir, &b3MprSimplexPoint(portal, 0)->v,
+                       &b3MprSimplexPoint(portal, 1)->v);
+    if (b3MprIsZero(b3MprVec3Len2(&dir))){
+        if (b3MprVec3Eq(&b3MprSimplexPoint(portal, 1)->v, b3mpr_vec3_origin)){
+            // origin lies on v1
+            return 1;
+        }else{
+            // origin lies on v0-v1 segment
+            return 2;
+        }
+    }
+
+    b3MprVec3Normalize(&dir);
+	 b3MprSupport(pairIndex,bodyIndexA,bodyIndexB,cpuBodyBuf,cpuConvexData,cpuCollidables,cpuVertices, sepAxis,&dir, b3MprSimplexPointW(portal, 2));
+    
+    dot = b3MprVec3Dot(&b3MprSimplexPoint(portal, 2)->v, &dir);
+    if (b3MprIsZero(dot) || dot < 0.f)
+        return -1;
+
+    b3MprSimplexSetSize(portal, 3);
+
+    // vertex 3 direction
+    b3MprVec3Sub2(&va, &b3MprSimplexPoint(portal, 1)->v,
+                     &b3MprSimplexPoint(portal, 0)->v);
+    b3MprVec3Sub2(&vb, &b3MprSimplexPoint(portal, 2)->v,
+                     &b3MprSimplexPoint(portal, 0)->v);
+    b3MprVec3Cross(&dir, &va, &vb);
+    b3MprVec3Normalize(&dir);
+
+    // it is better to form portal faces to be oriented "outside" origin
+    dot = b3MprVec3Dot(&dir, &b3MprSimplexPoint(portal, 0)->v);
+    if (dot > 0.f){
+        b3MprSimplexSwap(portal, 1, 2);
+        b3MprVec3Scale(&dir, -1.f);
+    }
+
+    while (b3MprSimplexSize(portal) < 4){
+		 b3MprSupport(pairIndex,bodyIndexA,bodyIndexB,cpuBodyBuf,cpuConvexData,cpuCollidables,cpuVertices, sepAxis,&dir, b3MprSimplexPointW(portal, 3));
+        
+        dot = b3MprVec3Dot(&b3MprSimplexPoint(portal, 3)->v, &dir);
+        if (b3MprIsZero(dot) || dot < 0.f)
+            return -1;
+
+        cont = 0;
+
+        // test if origin is outside (v1, v0, v3) - set v2 as v3 and
+        // continue
+        b3MprVec3Cross(&va, &b3MprSimplexPoint(portal, 1)->v,
+                          &b3MprSimplexPoint(portal, 3)->v);
+        dot = b3MprVec3Dot(&va, &b3MprSimplexPoint(portal, 0)->v);
+        if (dot < 0.f && !b3MprIsZero(dot)){
+            b3MprSimplexSet(portal, 2, b3MprSimplexPoint(portal, 3));
+            cont = 1;
+        }
+
+        if (!cont){
+            // test if origin is outside (v3, v0, v2) - set v1 as v3 and
+            // continue
+            b3MprVec3Cross(&va, &b3MprSimplexPoint(portal, 3)->v,
+                              &b3MprSimplexPoint(portal, 2)->v);
+            dot = b3MprVec3Dot(&va, &b3MprSimplexPoint(portal, 0)->v);
+            if (dot < 0.f && !b3MprIsZero(dot)){
+                b3MprSimplexSet(portal, 1, b3MprSimplexPoint(portal, 3));
+                cont = 1;
+            }
+        }
+
+        if (cont){
+            b3MprVec3Sub2(&va, &b3MprSimplexPoint(portal, 1)->v,
+                             &b3MprSimplexPoint(portal, 0)->v);
+            b3MprVec3Sub2(&vb, &b3MprSimplexPoint(portal, 2)->v,
+                             &b3MprSimplexPoint(portal, 0)->v);
+            b3MprVec3Cross(&dir, &va, &vb);
+            b3MprVec3Normalize(&dir);
+        }else{
+            b3MprSimplexSetSize(portal, 4);
+        }
+    }
+
+    return 0;
+}
+
+
+B3_STATIC int b3RefinePortal(int pairIndex,int bodyIndexA, int bodyIndexB,  b3ConstArray(b3RigidBodyData_t) cpuBodyBuf, 
+													b3ConstArray(b3ConvexPolyhedronData_t) cpuConvexData, 
+													b3ConstArray(b3Collidable_t)				cpuCollidables,
+													b3ConstArray(b3Float4)					cpuVertices,
+													__global b3Float4* sepAxis,
+													b3MprSimplex_t *portal)
+{
+    b3Float4 dir;
+    b3MprSupport_t v4;
+
+	for (int i=0;i<B3_MPR_MAX_ITERATIONS;i++)
+    //while (1)
+	{
+        // compute direction outside the portal (from v0 throught v1,v2,v3
+        // face)
+        b3PortalDir(portal, &dir);
+
+        // test if origin is inside the portal
+        if (portalEncapsulesOrigin(portal, &dir))
+            return 0;
+
+        // get next support point
+        
+		 b3MprSupport(pairIndex,bodyIndexA,bodyIndexB,cpuBodyBuf,cpuConvexData,cpuCollidables,cpuVertices, sepAxis,&dir, &v4);
+
+
+        // test if v4 can expand portal to contain origin and if portal
+        // expanding doesn't reach given tolerance
+        if (!portalCanEncapsuleOrigin(portal, &v4, &dir)
+                || portalReachTolerance(portal, &v4, &dir))
+		{
+            return -1;
+        }
+
+        // v1-v2-v3 triangle must be rearranged to face outside Minkowski
+        // difference (direction from v0).
+        b3ExpandPortal(portal, &v4);
+    }
+
+    return -1;
+}
+
+B3_STATIC void b3FindPos(const b3MprSimplex_t *portal, b3Float4 *pos)
+{
+
+	b3Float4 zero = b3MakeFloat4(0,0,0,0);
+	b3Float4* b3mpr_vec3_origin = &zero;
+
+    b3Float4 dir;
+    size_t i;
+    float b[4], sum, inv;
+    b3Float4 vec, p1, p2;
+
+    b3PortalDir(portal, &dir);
+
+    // use barycentric coordinates of tetrahedron to find origin
+    b3MprVec3Cross(&vec, &b3MprSimplexPoint(portal, 1)->v,
+                       &b3MprSimplexPoint(portal, 2)->v);
+    b[0] = b3MprVec3Dot(&vec, &b3MprSimplexPoint(portal, 3)->v);
+
+    b3MprVec3Cross(&vec, &b3MprSimplexPoint(portal, 3)->v,
+                       &b3MprSimplexPoint(portal, 2)->v);
+    b[1] = b3MprVec3Dot(&vec, &b3MprSimplexPoint(portal, 0)->v);
+
+    b3MprVec3Cross(&vec, &b3MprSimplexPoint(portal, 0)->v,
+                       &b3MprSimplexPoint(portal, 1)->v);
+    b[2] = b3MprVec3Dot(&vec, &b3MprSimplexPoint(portal, 3)->v);
+
+    b3MprVec3Cross(&vec, &b3MprSimplexPoint(portal, 2)->v,
+                       &b3MprSimplexPoint(portal, 1)->v);
+    b[3] = b3MprVec3Dot(&vec, &b3MprSimplexPoint(portal, 0)->v);
+
+	sum = b[0] + b[1] + b[2] + b[3];
+
+    if (b3MprIsZero(sum) || sum < 0.f){
+		b[0] = 0.f;
+
+        b3MprVec3Cross(&vec, &b3MprSimplexPoint(portal, 2)->v,
+                           &b3MprSimplexPoint(portal, 3)->v);
+        b[1] = b3MprVec3Dot(&vec, &dir);
+        b3MprVec3Cross(&vec, &b3MprSimplexPoint(portal, 3)->v,
+                           &b3MprSimplexPoint(portal, 1)->v);
+        b[2] = b3MprVec3Dot(&vec, &dir);
+        b3MprVec3Cross(&vec, &b3MprSimplexPoint(portal, 1)->v,
+                           &b3MprSimplexPoint(portal, 2)->v);
+        b[3] = b3MprVec3Dot(&vec, &dir);
+
+		sum = b[1] + b[2] + b[3];
+	}
+
+	inv = 1.f / sum;
+
+    b3MprVec3Copy(&p1, b3mpr_vec3_origin);
+    b3MprVec3Copy(&p2, b3mpr_vec3_origin);
+    for (i = 0; i < 4; i++){
+        b3MprVec3Copy(&vec, &b3MprSimplexPoint(portal, i)->v1);
+        b3MprVec3Scale(&vec, b[i]);
+        b3MprVec3Add(&p1, &vec);
+
+        b3MprVec3Copy(&vec, &b3MprSimplexPoint(portal, i)->v2);
+        b3MprVec3Scale(&vec, b[i]);
+        b3MprVec3Add(&p2, &vec);
+    }
+    b3MprVec3Scale(&p1, inv);
+    b3MprVec3Scale(&p2, inv);
+
+    b3MprVec3Copy(pos, &p1);
+    b3MprVec3Add(pos, &p2);
+    b3MprVec3Scale(pos, 0.5);
+}
+
+inline float b3MprVec3Dist2(const b3Float4 *a, const b3Float4 *b)
+{
+    b3Float4 ab;
+    b3MprVec3Sub2(&ab, a, b);
+    return b3MprVec3Len2(&ab);
+}
+
+inline float _b3MprVec3PointSegmentDist2(const b3Float4 *P,
+                                                  const b3Float4 *x0,
+                                                  const b3Float4 *b,
+                                                  b3Float4 *witness)
+{
+    // The computation comes from solving equation of segment:
+    //      S(t) = x0 + t.d
+    //          where - x0 is initial point of segment
+    //                - d is direction of segment from x0 (|d| > 0)
+    //                - t belongs to <0, 1> interval
+    // 
+    // Than, distance from a segment to some point P can be expressed:
+    //      D(t) = |x0 + t.d - P|^2
+    //          which is distance from any point on segment. Minimization
+    //          of this function brings distance from P to segment.
+    // Minimization of D(t) leads to simple quadratic equation that's
+    // solving is straightforward.
+    //
+    // Bonus of this method is witness point for free.
+
+    float dist, t;
+    b3Float4 d, a;
+
+    // direction of segment
+    b3MprVec3Sub2(&d, b, x0);
+
+    // precompute vector from P to x0
+    b3MprVec3Sub2(&a, x0, P);
+
+    t  = -1.f * b3MprVec3Dot(&a, &d);
+    t /= b3MprVec3Len2(&d);
+
+    if (t < 0.f || b3MprIsZero(t)){
+        dist = b3MprVec3Dist2(x0, P);
+        if (witness)
+            b3MprVec3Copy(witness, x0);
+    }else if (t > 1.f || b3MprEq(t, 1.f)){
+        dist = b3MprVec3Dist2(b, P);
+        if (witness)
+            b3MprVec3Copy(witness, b);
+    }else{
+        if (witness){
+            b3MprVec3Copy(witness, &d);
+            b3MprVec3Scale(witness, t);
+            b3MprVec3Add(witness, x0);
+            dist = b3MprVec3Dist2(witness, P);
+        }else{
+            // recycling variables
+            b3MprVec3Scale(&d, t);
+            b3MprVec3Add(&d, &a);
+            dist = b3MprVec3Len2(&d);
+        }
+    }
+
+    return dist;
+}
+
+
+inline float b3MprVec3PointTriDist2(const b3Float4 *P,
+                                const b3Float4 *x0, const b3Float4 *B,
+                                const b3Float4 *C,
+                                b3Float4 *witness)
+{
+    // Computation comes from analytic expression for triangle (x0, B, C)
+    //      T(s, t) = x0 + s.d1 + t.d2, where d1 = B - x0 and d2 = C - x0 and
+    // Then equation for distance is:
+    //      D(s, t) = | T(s, t) - P |^2
+    // This leads to minimization of quadratic function of two variables.
+    // The solution from is taken only if s is between 0 and 1, t is
+    // between 0 and 1 and t + s < 1, otherwise distance from segment is
+    // computed.
+
+    b3Float4 d1, d2, a;
+    float u, v, w, p, q, r;
+    float s, t, dist, dist2;
+    b3Float4 witness2;
+
+    b3MprVec3Sub2(&d1, B, x0);
+    b3MprVec3Sub2(&d2, C, x0);
+    b3MprVec3Sub2(&a, x0, P);
+
+    u = b3MprVec3Dot(&a, &a);
+    v = b3MprVec3Dot(&d1, &d1);
+    w = b3MprVec3Dot(&d2, &d2);
+    p = b3MprVec3Dot(&a, &d1);
+    q = b3MprVec3Dot(&a, &d2);
+    r = b3MprVec3Dot(&d1, &d2);
+
+    s = (q * r - w * p) / (w * v - r * r);
+    t = (-s * r - q) / w;
+
+    if ((b3MprIsZero(s) || s > 0.f)
+            && (b3MprEq(s, 1.f) || s < 1.f)
+            && (b3MprIsZero(t) || t > 0.f)
+            && (b3MprEq(t, 1.f) || t < 1.f)
+            && (b3MprEq(t + s, 1.f) || t + s < 1.f)){
+
+        if (witness){
+            b3MprVec3Scale(&d1, s);
+            b3MprVec3Scale(&d2, t);
+            b3MprVec3Copy(witness, x0);
+            b3MprVec3Add(witness, &d1);
+            b3MprVec3Add(witness, &d2);
+
+            dist = b3MprVec3Dist2(witness, P);
+        }else{
+            dist  = s * s * v;
+            dist += t * t * w;
+            dist += 2.f * s * t * r;
+            dist += 2.f * s * p;
+            dist += 2.f * t * q;
+            dist += u;
+        }
+    }else{
+        dist = _b3MprVec3PointSegmentDist2(P, x0, B, witness);
+
+        dist2 = _b3MprVec3PointSegmentDist2(P, x0, C, &witness2);
+        if (dist2 < dist){
+            dist = dist2;
+            if (witness)
+                b3MprVec3Copy(witness, &witness2);
+        }
+
+        dist2 = _b3MprVec3PointSegmentDist2(P, B, C, &witness2);
+        if (dist2 < dist){
+            dist = dist2;
+            if (witness)
+                b3MprVec3Copy(witness, &witness2);
+        }
+    }
+
+    return dist;
+}
+
+
+B3_STATIC void b3FindPenetr(int pairIndex,int bodyIndexA, int bodyIndexB,  b3ConstArray(b3RigidBodyData_t) cpuBodyBuf, 
+													b3ConstArray(b3ConvexPolyhedronData_t) cpuConvexData, 
+													b3ConstArray(b3Collidable_t)				cpuCollidables,
+													b3ConstArray(b3Float4)					cpuVertices,
+													__global b3Float4* sepAxis,
+                       b3MprSimplex_t *portal,
+                       float *depth, b3Float4 *pdir, b3Float4 *pos)
+{
+    b3Float4 dir;
+    b3MprSupport_t v4;
+    unsigned long iterations;
+
+	b3Float4 zero = b3MakeFloat4(0,0,0,0);
+	b3Float4* b3mpr_vec3_origin = &zero;
+
+
+    iterations = 1UL;
+	for (int i=0;i<B3_MPR_MAX_ITERATIONS;i++)
+    //while (1)
+	{
+        // compute portal direction and obtain next support point
+        b3PortalDir(portal, &dir);
+        
+		 b3MprSupport(pairIndex,bodyIndexA,bodyIndexB,cpuBodyBuf,cpuConvexData,cpuCollidables,cpuVertices, sepAxis,&dir, &v4);
+
+
+        // reached tolerance -> find penetration info
+        if (portalReachTolerance(portal, &v4, &dir)
+                || iterations ==B3_MPR_MAX_ITERATIONS)
+		{
+            *depth = b3MprVec3PointTriDist2(b3mpr_vec3_origin,&b3MprSimplexPoint(portal, 1)->v,&b3MprSimplexPoint(portal, 2)->v,&b3MprSimplexPoint(portal, 3)->v,pdir);
+            *depth = B3_MPR_SQRT(*depth);
+			
+			if (b3MprIsZero((*pdir).x) && b3MprIsZero((*pdir).y) && b3MprIsZero((*pdir).z))
+			{
+				
+				*pdir = dir;
+			} 
+			b3MprVec3Normalize(pdir);
+			
+            // barycentric coordinates:
+            b3FindPos(portal, pos);
+
+
+            return;
+        }
+
+        b3ExpandPortal(portal, &v4);
+
+        iterations++;
+    }
+}
+
+B3_STATIC void b3FindPenetrTouch(b3MprSimplex_t *portal,float *depth, b3Float4 *dir, b3Float4 *pos)
+{
+    // Touching contact on portal's v1 - so depth is zero and direction
+    // is unimportant and pos can be guessed
+    *depth = 0.f;
+    b3Float4 zero = b3MakeFloat4(0,0,0,0);
+	b3Float4* b3mpr_vec3_origin = &zero;
+
+
+	b3MprVec3Copy(dir, b3mpr_vec3_origin);
+
+    b3MprVec3Copy(pos, &b3MprSimplexPoint(portal, 1)->v1);
+    b3MprVec3Add(pos, &b3MprSimplexPoint(portal, 1)->v2);
+    b3MprVec3Scale(pos, 0.5);
+}
+
+B3_STATIC void b3FindPenetrSegment(b3MprSimplex_t *portal,
+                              float *depth, b3Float4 *dir, b3Float4 *pos)
+{
+    
+    // Origin lies on v0-v1 segment.
+    // Depth is distance to v1, direction also and position must be
+    // computed
+
+    b3MprVec3Copy(pos, &b3MprSimplexPoint(portal, 1)->v1);
+    b3MprVec3Add(pos, &b3MprSimplexPoint(portal, 1)->v2);
+    b3MprVec3Scale(pos, 0.5f);
+
+    
+    b3MprVec3Copy(dir, &b3MprSimplexPoint(portal, 1)->v);
+    *depth = B3_MPR_SQRT(b3MprVec3Len2(dir));
+    b3MprVec3Normalize(dir);
+}
+
+
+
+inline int b3MprPenetration(int pairIndex, int bodyIndexA, int bodyIndexB,
+					b3ConstArray(b3RigidBodyData_t) cpuBodyBuf,
+					b3ConstArray(b3ConvexPolyhedronData_t) cpuConvexData, 
+					b3ConstArray(b3Collidable_t)	cpuCollidables,
+					b3ConstArray(b3Float4)	cpuVertices,
+					__global b3Float4* sepAxis,
+					__global int*	hasSepAxis,
+					float *depthOut, b3Float4* dirOut, b3Float4* posOut)
+{
+	
+	 b3MprSimplex_t portal;
+
+	 
+//	if (!hasSepAxis[pairIndex])
+	//	return -1;
+	
+	hasSepAxis[pairIndex] = 0;
+	 int res;
+
+    // Phase 1: Portal discovery
+    res = b3DiscoverPortal(pairIndex,bodyIndexA,bodyIndexB,cpuBodyBuf,cpuConvexData,cpuCollidables,cpuVertices,sepAxis,hasSepAxis, &portal);
+	
+	  
+	//sepAxis[pairIndex] = *pdir;//or -dir?
+
+	switch (res)
+	{
+	case 0:
+		{
+			// Phase 2: Portal refinement
+		
+			res = b3RefinePortal(pairIndex,bodyIndexA,bodyIndexB,cpuBodyBuf,cpuConvexData,cpuCollidables,cpuVertices, sepAxis,&portal);
+			if (res < 0)
+				return -1;
+
+			// Phase 3. Penetration info
+			b3FindPenetr(pairIndex,bodyIndexA,bodyIndexB,cpuBodyBuf,cpuConvexData,cpuCollidables,cpuVertices, sepAxis,&portal, depthOut, dirOut, posOut);
+			hasSepAxis[pairIndex] = 1;
+			sepAxis[pairIndex] = -*dirOut;
+			break;
+		}
+	case 1:
+		{
+			 // Touching contact on portal's v1.
+			b3FindPenetrTouch(&portal, depthOut, dirOut, posOut);
+			break;
+		}
+	case 2:
+		{
+			
+			b3FindPenetrSegment( &portal, depthOut, dirOut, posOut);
+			break;
+		}
+	default:
+		{
+			hasSepAxis[pairIndex]=0;
+			//if (res < 0)
+			//{
+				// Origin isn't inside portal - no collision.
+				return -1;
+			//}
+		}
+	};
+	
+	return 0;
+};
+
+
+
+#endif //B3_MPR_PENETRATION_H
diff --git a/thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3NewContactReduction.h b/thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3NewContactReduction.h
new file mode 100644
index 0000000000..718222ebca
--- /dev/null
+++ b/thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3NewContactReduction.h
@@ -0,0 +1,196 @@
+
+#ifndef B3_NEW_CONTACT_REDUCTION_H
+#define B3_NEW_CONTACT_REDUCTION_H
+
+#include "Bullet3Common/shared/b3Float4.h"
+#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h"
+#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Contact4Data.h"
+
+#define GET_NPOINTS(x) (x).m_worldNormalOnB.w
+
+
+int b3ExtractManifoldSequentialGlobal(__global const b3Float4* p, int nPoints, b3Float4ConstArg nearNormal, b3Int4* contactIdx)
+{
+	if( nPoints == 0 )
+        return 0;
+    
+    if (nPoints <=4)
+        return nPoints;
+    
+    
+    if (nPoints >64)
+        nPoints = 64;
+    
+	b3Float4 center = b3MakeFloat4(0,0,0,0);
+	{
+		
+		for (int i=0;i<nPoints;i++)
+			center += p[i];
+		center /= (float)nPoints;
+	}
+    
+	
+    
+	//	sample 4 directions
+    
+    b3Float4 aVector = p[0] - center;
+    b3Float4 u = b3Cross( nearNormal, aVector );
+    b3Float4 v = b3Cross( nearNormal, u );
+    u = b3Normalized( u );
+    v = b3Normalized( v );
+    
+    
+    //keep point with deepest penetration
+    float minW= FLT_MAX;
+    
+    int minIndex=-1;
+    
+    b3Float4 maxDots;
+    maxDots.x = FLT_MIN;
+    maxDots.y = FLT_MIN;
+    maxDots.z = FLT_MIN;
+    maxDots.w = FLT_MIN;
+    
+    //	idx, distance
+    for(int ie = 0; ie<nPoints; ie++ )
+    {
+        if (p[ie].w<minW)
+        {
+            minW = p[ie].w;
+            minIndex=ie;
+        }
+        float f;
+        b3Float4 r = p[ie]-center;
+        f = b3Dot( u, r );
+        if (f<maxDots.x)
+        {
+            maxDots.x = f;
+            contactIdx[0].x = ie;
+        }
+        
+        f = b3Dot( -u, r );
+        if (f<maxDots.y)
+        {
+            maxDots.y = f;
+            contactIdx[0].y = ie;
+        }
+        
+        
+        f = b3Dot( v, r );
+        if (f<maxDots.z)
+        {
+            maxDots.z = f;
+            contactIdx[0].z = ie;
+        }
+        
+        f = b3Dot( -v, r );
+        if (f<maxDots.w)
+        {
+            maxDots.w = f;
+            contactIdx[0].w = ie;
+        }
+        
+    }
+    
+    if (contactIdx[0].x != minIndex && contactIdx[0].y != minIndex && contactIdx[0].z != minIndex && contactIdx[0].w != minIndex)
+    {
+        //replace the first contact with minimum (todo: replace contact with least penetration)
+        contactIdx[0].x = minIndex;
+    }
+    
+    return 4;
+    
+}
+
+__kernel void   b3NewContactReductionKernel( __global b3Int4* pairs,
+                                                   __global const b3RigidBodyData_t* rigidBodies,
+                                                   __global const b3Float4* separatingNormals,
+                                                   __global const int* hasSeparatingAxis,
+                                                   __global struct b3Contact4Data* globalContactsOut,
+                                                   __global b3Int4* clippingFaces,
+                                                   __global b3Float4* worldVertsB2,
+                                                   volatile __global int* nGlobalContactsOut,
+                                                   int vertexFaceCapacity,
+												   int contactCapacity,
+                                                   int numPairs,
+												   int pairIndex
+                                                   )
+{
+//    int i = get_global_id(0);
+	//int pairIndex = i;
+	int i = pairIndex;
+
+    b3Int4 contactIdx;
+    contactIdx=b3MakeInt4(0,1,2,3);
+    
+	if (i<numPairs)
+	{
+        
+		if (hasSeparatingAxis[i])
+		{
+            
+			
+            
+            
+			int nPoints = clippingFaces[pairIndex].w;
+           
+            if (nPoints>0)
+            {
+
+                 __global b3Float4* pointsIn = &worldVertsB2[pairIndex*vertexFaceCapacity];
+                b3Float4 normal = -separatingNormals[i];
+                
+                int nReducedContacts = b3ExtractManifoldSequentialGlobal(pointsIn, nPoints, normal, &contactIdx);
+            
+                int dstIdx;
+                dstIdx = b3AtomicInc( nGlobalContactsOut);
+				
+//#if 0
+                b3Assert(dstIdx < contactCapacity);
+				if (dstIdx < contactCapacity)
+				{
+
+					__global struct b3Contact4Data* c = &globalContactsOut[dstIdx];
+					c->m_worldNormalOnB = -normal;
+					c->m_restituitionCoeffCmp = (0.f*0xffff);c->m_frictionCoeffCmp = (0.7f*0xffff);
+					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;
+					c->m_childIndexB =-1;
+
+                    switch (nReducedContacts)
+                    {
+                        case 4:
+                            c->m_worldPosB[3] = pointsIn[contactIdx.w];
+                        case 3:
+                            c->m_worldPosB[2] = pointsIn[contactIdx.z];
+                        case 2:
+                            c->m_worldPosB[1] = pointsIn[contactIdx.y];
+                        case 1:
+                            c->m_worldPosB[0] = pointsIn[contactIdx.x];
+                        default:
+                        {
+                        }
+                    };
+                    
+					GET_NPOINTS(*c) = nReducedContacts;
+                    
+                 }
+                 
+                
+//#endif
+				
+			}//		if (numContactsOut>0)
+		}//		if (hasSeparatingAxis[i])
+	}//	if (i<numPairs)
+
+    
+    
+}
+#endif
diff --git a/thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3QuantizedBvhNodeData.h b/thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3QuantizedBvhNodeData.h
new file mode 100644
index 0000000000..3661e43cf1
--- /dev/null
+++ b/thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3QuantizedBvhNodeData.h
@@ -0,0 +1,90 @@
+
+
+#ifndef B3_QUANTIZED_BVH_NODE_H
+#define B3_QUANTIZED_BVH_NODE_H
+
+#include "Bullet3Common/shared/b3Float4.h"
+
+#define B3_MAX_NUM_PARTS_IN_BITS 10
+
+///b3QuantizedBvhNodeData is a compressed aabb node, 16 bytes.
+///Node can be used for leafnode or internal node. Leafnodes can point to 32-bit triangle index (non-negative range).
+typedef struct b3QuantizedBvhNodeData b3QuantizedBvhNodeData_t;
+
+struct b3QuantizedBvhNodeData
+{
+	//12 bytes
+	unsigned short int	m_quantizedAabbMin[3];
+	unsigned short int	m_quantizedAabbMax[3];
+	//4 bytes
+	int	m_escapeIndexOrTriangleIndex;
+};
+
+inline int	b3GetTriangleIndex(const b3QuantizedBvhNodeData* rootNode)
+{
+	unsigned int x=0;
+	unsigned int y = (~(x&0))<<(31-B3_MAX_NUM_PARTS_IN_BITS);
+	// Get only the lower bits where the triangle index is stored
+	return (rootNode->m_escapeIndexOrTriangleIndex&~(y));
+}
+
+inline int b3IsLeaf(const b3QuantizedBvhNodeData* rootNode)
+{
+	//skipindex is negative (internal node), triangleindex >=0 (leafnode)
+	return (rootNode->m_escapeIndexOrTriangleIndex >= 0)? 1 : 0;
+}
+	
+inline int b3GetEscapeIndex(const b3QuantizedBvhNodeData* rootNode)
+{
+	return -rootNode->m_escapeIndexOrTriangleIndex;
+}
+
+inline void b3QuantizeWithClamp(unsigned short* out, b3Float4ConstArg point2,int isMax, b3Float4ConstArg bvhAabbMin, b3Float4ConstArg bvhAabbMax, b3Float4ConstArg bvhQuantization)
+{
+	b3Float4 clampedPoint = b3MaxFloat4(point2,bvhAabbMin);
+	clampedPoint = b3MinFloat4 (clampedPoint, bvhAabbMax);
+
+	b3Float4 v = (clampedPoint - bvhAabbMin) * bvhQuantization;
+	if (isMax)
+	{
+		out[0] = (unsigned short) (((unsigned short)(v.x+1.f) | 1));
+		out[1] = (unsigned short) (((unsigned short)(v.y+1.f) | 1));
+		out[2] = (unsigned short) (((unsigned short)(v.z+1.f) | 1));
+	} else
+	{
+		out[0] = (unsigned short) (((unsigned short)(v.x) & 0xfffe));
+		out[1] = (unsigned short) (((unsigned short)(v.y) & 0xfffe));
+		out[2] = (unsigned short) (((unsigned short)(v.z) & 0xfffe));
+	}
+
+}
+
+
+inline int b3TestQuantizedAabbAgainstQuantizedAabbSlow(
+								const unsigned short int* aabbMin1,
+								const unsigned short int* aabbMax1,
+								const unsigned short int* aabbMin2,
+								const unsigned short int* aabbMax2)
+{
+	//int overlap = 1;
+	if (aabbMin1[0] > aabbMax2[0])
+		return 0;
+	if (aabbMax1[0] < aabbMin2[0])
+		return 0;
+	if (aabbMin1[1] > aabbMax2[1])
+		return 0;
+	if (aabbMax1[1] < aabbMin2[1])
+		return 0;
+	if (aabbMin1[2] > aabbMax2[2])
+		return 0;
+	if (aabbMax1[2] < aabbMin2[2])
+		return 0;
+	return 1;
+	//overlap = ((aabbMin1[0] > aabbMax2[0]) || (aabbMax1[0] < aabbMin2[0])) ? 0 : overlap;
+	//overlap = ((aabbMin1[2] > aabbMax2[2]) || (aabbMax1[2] < aabbMin2[2])) ? 0 : overlap;
+	//overlap = ((aabbMin1[1] > aabbMax2[1]) || (aabbMax1[1] < aabbMin2[1])) ? 0 : overlap;
+	//return overlap;
+}
+
+
+#endif //B3_QUANTIZED_BVH_NODE_H
diff --git a/thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3ReduceContacts.h b/thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3ReduceContacts.h
new file mode 100644
index 0000000000..35b5197006
--- /dev/null
+++ b/thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3ReduceContacts.h
@@ -0,0 +1,97 @@
+#ifndef B3_REDUCE_CONTACTS_H
+#define B3_REDUCE_CONTACTS_H
+
+inline int b3ReduceContacts(const b3Float4* p, int nPoints, const b3Float4& nearNormal, b3Int4* contactIdx)
+{
+	if( nPoints == 0 )
+        return 0;
+    
+    if (nPoints <=4)
+        return nPoints;
+    
+    
+    if (nPoints >64)
+        nPoints = 64;
+    
+	b3Float4 center = b3MakeFloat4(0,0,0,0);
+	{
+		
+		for (int i=0;i<nPoints;i++)
+			center += p[i];
+		center /= (float)nPoints;
+	}
+    
+	
+    
+	//	sample 4 directions
+    
+    b3Float4 aVector = p[0] - center;
+    b3Float4 u = b3Cross3( nearNormal, aVector );
+    b3Float4 v = b3Cross3( nearNormal, u );
+    u = b3FastNormalized3( u );
+    v = b3FastNormalized3( v );
+    
+    
+    //keep point with deepest penetration
+    float minW= FLT_MAX;
+    
+    int minIndex=-1;
+    
+    b3Float4 maxDots;
+    maxDots.x = FLT_MIN;
+    maxDots.y = FLT_MIN;
+    maxDots.z = FLT_MIN;
+    maxDots.w = FLT_MIN;
+    
+    //	idx, distance
+    for(int ie = 0; ie<nPoints; ie++ )
+    {
+        if (p[ie].w<minW)
+        {
+            minW = p[ie].w;
+            minIndex=ie;
+        }
+        float f;
+        b3Float4 r = p[ie]-center;
+        f = b3Dot3F4( u, r );
+        if (f<maxDots.x)
+        {
+            maxDots.x = f;
+            contactIdx[0].x = ie;
+        }
+        
+        f = b3Dot3F4( -u, r );
+        if (f<maxDots.y)
+        {
+            maxDots.y = f;
+            contactIdx[0].y = ie;
+        }
+        
+        
+        f = b3Dot3F4( v, r );
+        if (f<maxDots.z)
+        {
+            maxDots.z = f;
+            contactIdx[0].z = ie;
+        }
+        
+        f = b3Dot3F4( -v, r );
+        if (f<maxDots.w)
+        {
+            maxDots.w = f;
+            contactIdx[0].w = ie;
+        }
+        
+    }
+    
+    if (contactIdx[0].x != minIndex && contactIdx[0].y != minIndex && contactIdx[0].z != minIndex && contactIdx[0].w != minIndex)
+    {
+        //replace the first contact with minimum (todo: replace contact with least penetration)
+        contactIdx[0].x = minIndex;
+    }
+    
+    return 4;
+    
+}
+
+#endif //B3_REDUCE_CONTACTS_H
diff --git a/thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h b/thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h
new file mode 100644
index 0000000000..50632c871f
--- /dev/null
+++ b/thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h
@@ -0,0 +1,34 @@
+#ifndef B3_RIGIDBODY_DATA_H
+#define B3_RIGIDBODY_DATA_H
+
+#include "Bullet3Common/shared/b3Float4.h"
+#include "Bullet3Common/shared/b3Quat.h"
+#include "Bullet3Common/shared/b3Mat3x3.h"
+
+typedef struct b3RigidBodyData b3RigidBodyData_t;
+
+
+struct b3RigidBodyData
+{
+	b3Float4				m_pos;
+	b3Quat					m_quat;
+	b3Float4				m_linVel;
+	b3Float4				m_angVel;
+
+	int 					m_collidableIdx;
+	float 				m_invMass;
+	float 				m_restituitionCoeff;
+	float 				m_frictionCoeff;
+};
+
+typedef struct b3InertiaData b3InertiaData_t;
+
+struct b3InertiaData
+{
+	b3Mat3x3 m_invInertiaWorld;
+	b3Mat3x3 m_initInvInertia;
+};
+
+
+#endif //B3_RIGIDBODY_DATA_H
+	
\ No newline at end of file
diff --git a/thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3UpdateAabbs.h b/thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3UpdateAabbs.h
new file mode 100644
index 0000000000..8d40d19a03
--- /dev/null
+++ b/thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3UpdateAabbs.h
@@ -0,0 +1,40 @@
+#ifndef B3_UPDATE_AABBS_H
+#define B3_UPDATE_AABBS_H
+
+
+
+#include "Bullet3Collision/BroadPhaseCollision/shared/b3Aabb.h"
+#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Collidable.h"
+#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h"
+
+
+
+void b3ComputeWorldAabb(  int bodyId, __global const b3RigidBodyData_t* bodies, __global const  b3Collidable_t* collidables, __global const  b3Aabb_t* localShapeAABB, __global b3Aabb_t* worldAabbs)
+{
+	__global const b3RigidBodyData_t* body = &bodies[bodyId];
+
+	b3Float4 position = body->m_pos;
+	b3Quat	orientation = body->m_quat;
+	
+	int collidableIndex = body->m_collidableIdx;
+	int shapeIndex = collidables[collidableIndex].m_shapeIndex;
+		
+	if (shapeIndex>=0)
+	{
+				
+		b3Aabb_t localAabb = localShapeAABB[collidableIndex];
+		b3Aabb_t worldAabb;
+		
+		b3Float4 aabbAMinOut,aabbAMaxOut;	
+		float margin = 0.f;
+		b3TransformAabb2(localAabb.m_minVec,localAabb.m_maxVec,margin,position,orientation,&aabbAMinOut,&aabbAMaxOut);
+		
+		worldAabb.m_minVec =aabbAMinOut;
+		worldAabb.m_minIndices[3] = bodyId;
+		worldAabb.m_maxVec = aabbAMaxOut;
+		worldAabb.m_signedMaxIndices[3] = body[bodyId].m_invMass==0.f? 0 : 1;
+		worldAabbs[bodyId] = worldAabb;
+	}
+}
+
+#endif //B3_UPDATE_AABBS_H