NarrowphaseCollision/kernels/primitiveContacts.cl

#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Contact4Data.h"

#define SHAPE_CONVEX_HULL 3
#define SHAPE_PLANE 4
#define SHAPE_CONCAVE_TRIMESH 5
#define SHAPE_COMPOUND_OF_CONVEX_HULLS 6
#define SHAPE_SPHERE 7


#pragma OPENCL EXTENSION cl_amd_printf : enable
#pragma OPENCL EXTENSION cl_khr_local_int32_base_atomics : enable
#pragma OPENCL EXTENSION cl_khr_global_int32_base_atomics : enable
#pragma OPENCL EXTENSION cl_khr_local_int32_extended_atomics : enable
#pragma OPENCL EXTENSION cl_khr_global_int32_extended_atomics : enable

#ifdef cl_ext_atomic_counters_32
#pragma OPENCL EXTENSION cl_ext_atomic_counters_32 : enable
#else
#define counter32_t volatile __global int*
#endif

#define GET_GROUP_IDX get_group_id(0)
#define GET_LOCAL_IDX get_local_id(0)
#define GET_GLOBAL_IDX get_global_id(0)
#define GET_GROUP_SIZE get_local_size(0)
#define GET_NUM_GROUPS get_num_groups(0)
#define GROUP_LDS_BARRIER barrier(CLK_LOCAL_MEM_FENCE)
#define GROUP_MEM_FENCE mem_fence(CLK_LOCAL_MEM_FENCE)
#define AtomInc(x) atom_inc(&(x))
#define AtomInc1(x, out) out = atom_inc(&(x))
#define AppendInc(x, out) out = atomic_inc(x)
#define AtomAdd(x, value) atom_add(&(x), value)
#define AtomCmpxhg(x, cmp, value) atom_cmpxchg( &(x), cmp, value )
#define AtomXhg(x, value) atom_xchg ( &(x), value )

#define max2 max
#define min2 min

typedef unsigned int u32;


typedef struct
{
	union
	{
		float4	m_min;
		float   m_minElems[4];
		int			m_minIndices[4];
	};
	union
	{
		float4	m_max;
		float   m_maxElems[4];
		int			m_maxIndices[4];
	};
} btAabbCL;

///keep this in sync with btCollidable.h
typedef struct
{
	int m_numChildShapes;
	float m_radius;
	int m_shapeType;
	int m_shapeIndex;

} btCollidableGpu;

typedef struct
{
	float4	m_childPosition;
	float4	m_childOrientation;
	int m_shapeIndex;
	int m_unused0;
	int m_unused1;
	int m_unused2;
} btGpuChildShape;

#define GET_NPOINTS(x) (x).m_worldNormalOnB.w

typedef struct
{
	float4 m_pos;
	float4 m_quat;
	float4 m_linVel;
	float4 m_angVel;

	u32 m_collidableIdx;
	float m_invMass;
	float m_restituitionCoeff;
	float m_frictionCoeff;
} BodyData;


typedef struct
{
	float4		m_localCenter;
	float4		m_extents;
	float4		mC;
	float4		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;

} ConvexPolyhedronCL;

typedef struct
{
	float4 m_plane;
	int m_indexOffset;
	int m_numIndices;
} btGpuFace;

#define SELECT_UINT4( b, a, condition ) select( b,a,condition )

#define make_float4 (float4)
#define make_float2 (float2)
#define make_uint4 (uint4)
#define make_int4 (int4)
#define make_uint2 (uint2)
#define make_int2 (int2)


__inline
float fastDiv(float numerator, float denominator)
{
	return native_divide(numerator, denominator);
//	return numerator/denominator;
}

__inline
float4 fastDiv4(float4 numerator, float4 denominator)
{
	return native_divide(numerator, denominator);
}


__inline
float4 cross3(float4 a, float4 b)
{
	return cross(a,b);
}

//#define dot3F4 dot

__inline
float dot3F4(float4 a, float4 b)
{
	float4 a1 = make_float4(a.xyz,0.f);
	float4 b1 = make_float4(b.xyz,0.f);
	return dot(a1, b1);
}

__inline
float4 fastNormalize4(float4 v)
{
	return fast_normalize(v);
}


///////////////////////////////////////
//	Quaternion
///////////////////////////////////////

typedef float4 Quaternion;

__inline
Quaternion qtMul(Quaternion a, Quaternion b);

__inline
Quaternion qtNormalize(Quaternion in);

__inline
float4 qtRotate(Quaternion q, float4 vec);

__inline
Quaternion qtInvert(Quaternion q);


__inline
Quaternion qtMul(Quaternion a, Quaternion b)
{
	Quaternion ans;
	ans = cross3( a, b );
	ans += a.w*b+b.w*a;
//	ans.w = a.w*b.w - (a.x*b.x+a.y*b.y+a.z*b.z);
	ans.w = a.w*b.w - dot3F4(a, b);
	return ans;
}

__inline
Quaternion qtNormalize(Quaternion in)
{
	return fastNormalize4(in);
//	in /= length( in );
//	return in;
}
__inline
float4 qtRotate(Quaternion q, float4 vec)
{
	Quaternion qInv = qtInvert( q );
	float4 vcpy = vec;
	vcpy.w = 0.f;
	float4 out = qtMul(qtMul(q,vcpy),qInv);
	return out;
}

__inline
Quaternion qtInvert(Quaternion q)
{
	return (Quaternion)(-q.xyz, q.w);
}

__inline
float4 qtInvRotate(const Quaternion q, float4 vec)
{
	return qtRotate( qtInvert( q ), vec );
}

__inline
float4 transform(const float4* p, const float4* translation, const Quaternion* orientation)
{
	return qtRotate( *orientation, *p ) + (*translation);
}

void	trInverse(float4 translationIn, Quaternion orientationIn,
		float4* translationOut, Quaternion* orientationOut)
{
	*orientationOut = qtInvert(orientationIn);
	*translationOut = qtRotate(*orientationOut, -translationIn);
}

void	trMul(float4 translationA, Quaternion orientationA,
						float4 translationB, Quaternion orientationB,
		float4* translationOut, Quaternion* orientationOut)
{
	*orientationOut = qtMul(orientationA,orientationB);
	*translationOut = transform(&translationB,&translationA,&orientationA);
}


__inline
float4 normalize3(const float4 a)
{
	float4 n = make_float4(a.x, a.y, a.z, 0.f);
	return fastNormalize4( n );
}


__inline float4 lerp3(const float4 a,const float4 b, float  t)
{
	return make_float4(	a.x + (b.x - a.x) * t,
						a.y + (b.y - a.y) * t,
						a.z + (b.z - a.z) * t,
						0.f);
}


float signedDistanceFromPointToPlane(float4 point, float4 planeEqn, float4* closestPointOnFace)
{
	float4 n = (float4)(planeEqn.x, planeEqn.y, planeEqn.z, 0);
	float dist = dot3F4(n, point) + planeEqn.w;
	*closestPointOnFace = point - dist * n;
	return dist;
}


inline bool IsPointInPolygon(float4 p,
							const btGpuFace* face,
							__global const float4* baseVertex,
							__global const  int* convexIndices,
							float4* out)
{
    float4 a;
    float4 b;
    float4 ab;
    float4 ap;
    float4 v;

	float4 plane = make_float4(face->m_plane.x,face->m_plane.y,face->m_plane.z,0.f);

	if (face->m_numIndices<2)
		return false;


	float4 v0 = baseVertex[convexIndices[face->m_indexOffset + face->m_numIndices-1]];

	b = v0;

    for(unsigned i=0; i != face->m_numIndices; ++i)
    {
		a = b;
		float4 vi = baseVertex[convexIndices[face->m_indexOffset + i]];
		b = vi;
        ab = b-a;
        ap = p-a;
        v = cross3(ab,plane);

        if (dot(ap, v) > 0.f)
        {
            float ab_m2 = dot(ab, ab);
            float rt = ab_m2 != 0.f ? dot(ab, ap) / ab_m2 : 0.f;
            if (rt <= 0.f)
            {
                *out = a;
            }
            else if (rt >= 1.f)
            {
                *out = b;
            }
            else
            {
            	float s = 1.f - rt;
				out[0].x = s * a.x + rt * b.x;
				out[0].y = s * a.y + rt * b.y;
				out[0].z = s * a.z + rt * b.z;
            }
            return false;
        }
    }
    return true;
}


void	computeContactSphereConvex(int pairIndex,
																int bodyIndexA, int bodyIndexB,
																int collidableIndexA, int collidableIndexB,
																__global const BodyData* rigidBodies,
																__global const btCollidableGpu* collidables,
																__global const ConvexPolyhedronCL* convexShapes,
																__global const float4* convexVertices,
																__global const int* convexIndices,
																__global const btGpuFace* faces,
																__global struct b3Contact4Data* restrict globalContactsOut,
																counter32_t nGlobalContactsOut,
																int maxContactCapacity,
																float4 spherePos2,
																float radius,
																float4 pos,
																float4 quat
																)
{

	float4 invPos;
	float4 invOrn;

	trInverse(pos,quat, &invPos,&invOrn);

	float4 spherePos = transform(&spherePos2,&invPos,&invOrn);

	int shapeIndex = collidables[collidableIndexB].m_shapeIndex;
	int numFaces = convexShapes[shapeIndex].m_numFaces;
	float4 closestPnt = (float4)(0, 0, 0, 0);
	float4 hitNormalWorld = (float4)(0, 0, 0, 0);
	float minDist = -1000000.f;
	bool bCollide = true;

	for ( int f = 0; f < numFaces; f++ )
	{
		btGpuFace face = faces[convexShapes[shapeIndex].m_faceOffset+f];

		// set up a plane equation
		float4 planeEqn;
		float4 n1 = face.m_plane;
		n1.w = 0.f;
		planeEqn = n1;
		planeEqn.w = face.m_plane.w;


		// compute a signed distance from the vertex in cloth to the face of rigidbody.
		float4 pntReturn;
		float dist = signedDistanceFromPointToPlane(spherePos, planeEqn, &pntReturn);

		// If the distance is positive, the plane is a separating plane.
		if ( dist > radius )
		{
			bCollide = false;
			break;
		}


		if (dist>0)
		{
			//might hit an edge or vertex
			float4 out;
			float4 zeroPos = make_float4(0,0,0,0);

			bool isInPoly = IsPointInPolygon(spherePos,
					&face,
					&convexVertices[convexShapes[shapeIndex].m_vertexOffset],
					convexIndices,
           &out);
			if (isInPoly)
			{
				if (dist>minDist)
				{
					minDist = dist;
					closestPnt = pntReturn;
					hitNormalWorld = planeEqn;

				}
			} else
			{
				float4 tmp = spherePos-out;
				float l2 = dot(tmp,tmp);
				if (l2<radius*radius)
				{
					dist  = sqrt(l2);
					if (dist>minDist)
					{
						minDist = dist;
						closestPnt = out;
						hitNormalWorld = tmp/dist;

					}

				} else
				{
					bCollide = false;
					break;
				}
			}
		} else
		{
			if ( dist > minDist )
			{
				minDist = dist;
				closestPnt = pntReturn;
				hitNormalWorld.xyz = planeEqn.xyz;
			}
		}

	}


	if (bCollide && minDist > -10000)
	{
		float4 normalOnSurfaceB1 = qtRotate(quat,-hitNormalWorld);
		float4 pOnB1 = transform(&closestPnt,&pos,&quat);

		float actualDepth = minDist-radius;
		if (actualDepth<=0.f)
		{


			pOnB1.w = actualDepth;

			int dstIdx;
			AppendInc( nGlobalContactsOut, dstIdx );


			if (1)//dstIdx < maxContactCapacity)
			{
				__global struct b3Contact4Data* c = &globalContactsOut[dstIdx];
				c->m_worldNormalOnB = -normalOnSurfaceB1;
				c->m_restituitionCoeffCmp = (0.f*0xffff);c->m_frictionCoeffCmp = (0.7f*0xffff);
				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;
				c->m_childIndexA = -1;
				c->m_childIndexB = -1;

				GET_NPOINTS(*c) = 1;
			}

		}
	}//if (hasCollision)

}


int extractManifoldSequential(const float4* p, int nPoints, float4 nearNormal, int4* contactIdx)
{
	if( nPoints == 0 )
        return 0;

    if (nPoints <=4)
        return nPoints;


    if (nPoints >64)
        nPoints = 64;

	float4 center = make_float4(0.f);
	{

		for (int i=0;i<nPoints;i++)
			center += p[i];
		center /= (float)nPoints;
	}


	//	sample 4 directions

    float4 aVector = p[0] - center;
    float4 u = cross3( nearNormal, aVector );
    float4 v = cross3( nearNormal, u );
    u = normalize3( u );
    v = normalize3( v );


    //keep point with deepest penetration
    float minW= FLT_MAX;

    int minIndex=-1;

    float4 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;
        float4 r = p[ie]-center;
        f = dot3F4( u, r );
        if (f<maxDots.x)
        {
            maxDots.x = f;
            contactIdx[0].x = ie;
        }

        f = dot3F4( -u, r );
        if (f<maxDots.y)
        {
            maxDots.y = f;
            contactIdx[0].y = ie;
        }


        f = dot3F4( v, r );
        if (f<maxDots.z)
        {
            maxDots.z = f;
            contactIdx[0].z = ie;
        }

        f = dot3F4( -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;

}

#define MAX_PLANE_CONVEX_POINTS 64

int computeContactPlaneConvex(int pairIndex,
								int bodyIndexA, int bodyIndexB,
								int collidableIndexA, int collidableIndexB,
								__global const BodyData* rigidBodies,
								__global const btCollidableGpu*collidables,
								__global const ConvexPolyhedronCL* convexShapes,
								__global const float4* convexVertices,
								__global const int* convexIndices,
								__global const btGpuFace* faces,
								__global struct b3Contact4Data* restrict globalContactsOut,
								counter32_t nGlobalContactsOut,
								int maxContactCapacity,
								float4 posB,
								Quaternion ornB
								)
{
	int resultIndex=-1;

		int shapeIndex = collidables[collidableIndexB].m_shapeIndex;
	__global const ConvexPolyhedronCL* hullB = &convexShapes[shapeIndex];

	float4 posA;
	posA = rigidBodies[bodyIndexA].m_pos;
	Quaternion ornA;
	ornA = rigidBodies[bodyIndexA].m_quat;

	int numContactsOut = 0;
	int numWorldVertsB1= 0;

	float4 planeEq;
	 planeEq = faces[collidables[collidableIndexA].m_shapeIndex].m_plane;
	float4 planeNormal = make_float4(planeEq.x,planeEq.y,planeEq.z,0.f);
	float4 planeNormalWorld;
	planeNormalWorld = qtRotate(ornA,planeNormal);
	float planeConstant = planeEq.w;

	float4 invPosA;Quaternion invOrnA;
	float4 convexInPlaneTransPos1; Quaternion convexInPlaneTransOrn1;
	{

		trInverse(posA,ornA,&invPosA,&invOrnA);
		trMul(invPosA,invOrnA,posB,ornB,&convexInPlaneTransPos1,&convexInPlaneTransOrn1);
	}
	float4 invPosB;Quaternion invOrnB;
	float4 planeInConvexPos1;	Quaternion planeInConvexOrn1;
	{

		trInverse(posB,ornB,&invPosB,&invOrnB);
		trMul(invPosB,invOrnB,posA,ornA,&planeInConvexPos1,&planeInConvexOrn1);
	}


	float4 planeNormalInConvex = qtRotate(planeInConvexOrn1,-planeNormal);
	float maxDot = -1e30;
	int hitVertex=-1;
	float4 hitVtx;


	float4 contactPoints[MAX_PLANE_CONVEX_POINTS];
	int numPoints = 0;

	int4 contactIdx;
	contactIdx=make_int4(0,1,2,3);


	for (int i=0;i<hullB->m_numVertices;i++)
	{
		float4 vtx = convexVertices[hullB->m_vertexOffset+i];
		float curDot = dot(vtx,planeNormalInConvex);


		if (curDot>maxDot)
		{
			hitVertex=i;
			maxDot=curDot;
			hitVtx = vtx;
			//make sure the deepest points is always included
			if (numPoints==MAX_PLANE_CONVEX_POINTS)
				numPoints--;
		}

		if (numPoints<MAX_PLANE_CONVEX_POINTS)
		{
			float4 vtxWorld = transform(&vtx, &posB, &ornB);
			float4 vtxInPlane = transform(&vtxWorld, &invPosA, &invOrnA);//oplaneTransform.inverse()*vtxWorld;
			float dist = dot(planeNormal,vtxInPlane)-planeConstant;
			if (dist<0.f)
			{
				vtxWorld.w = dist;
				contactPoints[numPoints] = vtxWorld;
				numPoints++;
			}
		}

	}

	int numReducedPoints  = numPoints;
	if (numPoints>4)
	{
		numReducedPoints = extractManifoldSequential( contactPoints, numPoints, planeNormalInConvex, &contactIdx);
	}

	if (numReducedPoints>0)
	{
		int dstIdx;
	    AppendInc( nGlobalContactsOut, dstIdx );

		if (dstIdx < maxContactCapacity)
		{
			resultIndex = dstIdx;
			__global struct b3Contact4Data* c = &globalContactsOut[dstIdx];
			c->m_worldNormalOnB = -planeNormalWorld;
			//c->setFrictionCoeff(0.7);
			//c->setRestituitionCoeff(0.f);
			c->m_restituitionCoeffCmp = (0.f*0xffff);c->m_frictionCoeffCmp = (0.7f*0xffff);
			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_childIndexA = -1;
			c->m_childIndexB = -1;

			switch (numReducedPoints)
            {
                case 4:
                    c->m_worldPosB[3] = contactPoints[contactIdx.w];
                case 3:
                    c->m_worldPosB[2] = contactPoints[contactIdx.z];
                case 2:
                    c->m_worldPosB[1] = contactPoints[contactIdx.y];
                case 1:
                    c->m_worldPosB[0] = contactPoints[contactIdx.x];
                default:
                {
                }
            };

			GET_NPOINTS(*c) = numReducedPoints;
		}//if (dstIdx < numPairs)
	}

	return resultIndex;
}


void	computeContactPlaneSphere(int pairIndex,
																int bodyIndexA, int bodyIndexB,
																int collidableIndexA, int collidableIndexB,
																__global const BodyData* rigidBodies,
																__global const btCollidableGpu* collidables,
																__global const btGpuFace* faces,
																__global struct b3Contact4Data* restrict globalContactsOut,
																counter32_t nGlobalContactsOut,
																int maxContactCapacity)
{
	float4 planeEq = faces[collidables[collidableIndexA].m_shapeIndex].m_plane;
	float radius = collidables[collidableIndexB].m_radius;
	float4 posA1 = rigidBodies[bodyIndexA].m_pos;
	float4 ornA1 = rigidBodies[bodyIndexA].m_quat;
	float4 posB1 = rigidBodies[bodyIndexB].m_pos;
	float4 ornB1 = rigidBodies[bodyIndexB].m_quat;

	bool hasCollision = false;
	float4 planeNormal1 = make_float4(planeEq.x,planeEq.y,planeEq.z,0.f);
	float planeConstant = planeEq.w;
	float4 convexInPlaneTransPos1; Quaternion convexInPlaneTransOrn1;
	{
		float4 invPosA;Quaternion invOrnA;
		trInverse(posA1,ornA1,&invPosA,&invOrnA);
		trMul(invPosA,invOrnA,posB1,ornB1,&convexInPlaneTransPos1,&convexInPlaneTransOrn1);
	}
	float4 planeInConvexPos1;	Quaternion planeInConvexOrn1;
	{
		float4 invPosB;Quaternion invOrnB;
		trInverse(posB1,ornB1,&invPosB,&invOrnB);
		trMul(invPosB,invOrnB,posA1,ornA1,&planeInConvexPos1,&planeInConvexOrn1);
	}
	float4 vtx1 = qtRotate(planeInConvexOrn1,-planeNormal1)*radius;
	float4 vtxInPlane1 = transform(&vtx1,&convexInPlaneTransPos1,&convexInPlaneTransOrn1);
	float distance = dot3F4(planeNormal1,vtxInPlane1) - planeConstant;
	hasCollision = distance < 0.f;//m_manifoldPtr->getContactBreakingThreshold();
	if (hasCollision)
	{
		float4 vtxInPlaneProjected1 = vtxInPlane1 -   distance*planeNormal1;
		float4 vtxInPlaneWorld1 = transform(&vtxInPlaneProjected1,&posA1,&ornA1);
		float4 normalOnSurfaceB1 = qtRotate(ornA1,planeNormal1);
		float4 pOnB1 = vtxInPlaneWorld1+normalOnSurfaceB1*distance;
		pOnB1.w = distance;

		int dstIdx;
    AppendInc( nGlobalContactsOut, dstIdx );

		if (dstIdx < maxContactCapacity)
		{
			__global struct b3Contact4Data* c = &globalContactsOut[dstIdx];
			c->m_worldNormalOnB = -normalOnSurfaceB1;
			c->m_restituitionCoeffCmp = (0.f*0xffff);c->m_frictionCoeffCmp = (0.7f*0xffff);
			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;
			c->m_childIndexA = -1;
			c->m_childIndexB = -1;
			GET_NPOINTS(*c) = 1;
		}//if (dstIdx < numPairs)
	}//if (hasCollision)
}


__kernel void   primitiveContactsKernel( __global int4* pairs,
																					__global const BodyData* rigidBodies,
																					__global const btCollidableGpu* collidables,
																					__global const ConvexPolyhedronCL* convexShapes,
																					__global const float4* vertices,
																					__global const float4* uniqueEdges,
																					__global const btGpuFace* faces,
																					__global const int* indices,
																					__global struct b3Contact4Data* restrict globalContactsOut,
																					counter32_t nGlobalContactsOut,
																					int numPairs, int maxContactCapacity)
{

	int i = get_global_id(0);
	int pairIndex = i;

	float4 worldVertsB1[64];
	float4 worldVertsB2[64];
	int capacityWorldVerts = 64;

	float4 localContactsOut[64];
	int localContactCapacity=64;

	float minDist = -1e30f;
	float maxDist = 0.02f;

	if (i<numPairs)
	{

		int bodyIndexA = pairs[i].x;
		int bodyIndexB = pairs[i].y;

		int collidableIndexA = rigidBodies[bodyIndexA].m_collidableIdx;
		int collidableIndexB = rigidBodies[bodyIndexB].m_collidableIdx;

		if (collidables[collidableIndexA].m_shapeType == SHAPE_PLANE &&
			collidables[collidableIndexB].m_shapeType == SHAPE_CONVEX_HULL)
		{

			float4 posB;
			posB = rigidBodies[bodyIndexB].m_pos;
			Quaternion ornB;
			ornB = rigidBodies[bodyIndexB].m_quat;
			int contactIndex = computeContactPlaneConvex(pairIndex, bodyIndexA, bodyIndexB, collidableIndexA, collidableIndexB,
																rigidBodies,collidables,convexShapes,vertices,indices,
																faces,	globalContactsOut, nGlobalContactsOut,maxContactCapacity, posB,ornB);
			if (contactIndex>=0)
				pairs[pairIndex].z = contactIndex;

			return;
		}


		if (collidables[collidableIndexA].m_shapeType == SHAPE_CONVEX_HULL &&
			collidables[collidableIndexB].m_shapeType == SHAPE_PLANE)
		{

			float4 posA;
			posA = rigidBodies[bodyIndexA].m_pos;
			Quaternion ornA;
			ornA = rigidBodies[bodyIndexA].m_quat;


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

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

			return;
		}

		if (collidables[collidableIndexA].m_shapeType == SHAPE_PLANE &&
			collidables[collidableIndexB].m_shapeType == SHAPE_SPHERE)
		{
			computeContactPlaneSphere(pairIndex, bodyIndexA, bodyIndexB, collidableIndexA, collidableIndexB,
																rigidBodies,collidables,faces,	globalContactsOut, nGlobalContactsOut,maxContactCapacity);
			return;
		}


		if (collidables[collidableIndexA].m_shapeType == SHAPE_SPHERE &&
			collidables[collidableIndexB].m_shapeType == SHAPE_PLANE)
		{


			computeContactPlaneSphere( pairIndex, bodyIndexB,bodyIndexA,  collidableIndexB,collidableIndexA,
																rigidBodies,collidables,
																faces,	globalContactsOut, nGlobalContactsOut,maxContactCapacity);

			return;
		}


		if (collidables[collidableIndexA].m_shapeType == SHAPE_SPHERE &&
			collidables[collidableIndexB].m_shapeType == SHAPE_CONVEX_HULL)
		{

			float4 spherePos = rigidBodies[bodyIndexA].m_pos;
			float sphereRadius = collidables[collidableIndexA].m_radius;
			float4 convexPos = rigidBodies[bodyIndexB].m_pos;
			float4 convexOrn = rigidBodies[bodyIndexB].m_quat;

			computeContactSphereConvex(pairIndex, bodyIndexA, bodyIndexB, collidableIndexA, collidableIndexB,
																rigidBodies,collidables,convexShapes,vertices,indices,faces, globalContactsOut, nGlobalContactsOut,maxContactCapacity,
																spherePos,sphereRadius,convexPos,convexOrn);

			return;
		}

		if (collidables[collidableIndexA].m_shapeType == SHAPE_CONVEX_HULL &&
			collidables[collidableIndexB].m_shapeType == SHAPE_SPHERE)
		{

			float4 spherePos = rigidBodies[bodyIndexB].m_pos;
			float sphereRadius = collidables[collidableIndexB].m_radius;
			float4 convexPos = rigidBodies[bodyIndexA].m_pos;
			float4 convexOrn = rigidBodies[bodyIndexA].m_quat;

			computeContactSphereConvex(pairIndex, bodyIndexB, bodyIndexA, collidableIndexB, collidableIndexA,
																rigidBodies,collidables,convexShapes,vertices,indices,faces, globalContactsOut, nGlobalContactsOut,maxContactCapacity,
																spherePos,sphereRadius,convexPos,convexOrn);
			return;
		}


		if (collidables[collidableIndexA].m_shapeType == SHAPE_SPHERE &&
			collidables[collidableIndexB].m_shapeType == SHAPE_SPHERE)
		{
			//sphere-sphere
			float radiusA = collidables[collidableIndexA].m_radius;
			float radiusB = collidables[collidableIndexB].m_radius;
			float4 posA = rigidBodies[bodyIndexA].m_pos;
			float4 posB = rigidBodies[bodyIndexB].m_pos;

			float4 diff = posA-posB;
			float len = length(diff);

			///iff distance positive, don't generate a new contact
			if ( len <= (radiusA+radiusB))
			{
				///distance (negative means penetration)
				float dist = len - (radiusA+radiusB);
				float4 normalOnSurfaceB = make_float4(1.f,0.f,0.f,0.f);
				if (len > 0.00001)
				{
					normalOnSurfaceB = diff / len;
				}
				float4 contactPosB = posB + normalOnSurfaceB*radiusB;
				contactPosB.w = dist;

				int dstIdx;
				 AppendInc( nGlobalContactsOut, dstIdx );

				if (dstIdx < maxContactCapacity)
				{
					__global struct b3Contact4Data* c = &globalContactsOut[dstIdx];
					c->m_worldNormalOnB = normalOnSurfaceB;
					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;
					c->m_bodyAPtrAndSignBit = rigidBodies[bodyA].m_invMass==0?-bodyA:bodyA;
					c->m_bodyBPtrAndSignBit = rigidBodies[bodyB].m_invMass==0?-bodyB:bodyB;
					c->m_worldPosB[0] = contactPosB;
					c->m_childIndexA = -1;
					c->m_childIndexB = -1;
					GET_NPOINTS(*c) = 1;
				}//if (dstIdx < numPairs)
			}//if ( len <= (radiusA+radiusB))

			return;
		}//SHAPE_SPHERE SHAPE_SPHERE

	}//	if (i<numPairs)

}


// work-in-progress
__kernel void   processCompoundPairsPrimitivesKernel( __global const int4* gpuCompoundPairs,
													__global const BodyData* rigidBodies,
													__global const btCollidableGpu* collidables,
													__global const ConvexPolyhedronCL* convexShapes,
													__global const float4* vertices,
													__global const float4* uniqueEdges,
													__global const btGpuFace* faces,
													__global const int* indices,
													__global btAabbCL* aabbs,
													__global const btGpuChildShape* gpuChildShapes,
													__global struct b3Contact4Data* restrict globalContactsOut,
													counter32_t nGlobalContactsOut,
													int numCompoundPairs, int maxContactCapacity
													)
{

	int i = get_global_id(0);
	if (i<numCompoundPairs)
	{
		int bodyIndexA = gpuCompoundPairs[i].x;
		int bodyIndexB = gpuCompoundPairs[i].y;

		int childShapeIndexA = gpuCompoundPairs[i].z;
		int childShapeIndexB = gpuCompoundPairs[i].w;

		int collidableIndexA = -1;
		int collidableIndexB = -1;

		float4 ornA = rigidBodies[bodyIndexA].m_quat;
		float4 posA = rigidBodies[bodyIndexA].m_pos;

		float4 ornB = rigidBodies[bodyIndexB].m_quat;
		float4 posB = rigidBodies[bodyIndexB].m_pos;

		if (childShapeIndexA >= 0)
		{
			collidableIndexA = gpuChildShapes[childShapeIndexA].m_shapeIndex;
			float4 childPosA = gpuChildShapes[childShapeIndexA].m_childPosition;
			float4 childOrnA = gpuChildShapes[childShapeIndexA].m_childOrientation;
			float4 newPosA = qtRotate(ornA,childPosA)+posA;
			float4 newOrnA = qtMul(ornA,childOrnA);
			posA = newPosA;
			ornA = newOrnA;
		} else
		{
			collidableIndexA = rigidBodies[bodyIndexA].m_collidableIdx;
		}

		if (childShapeIndexB>=0)
		{
			collidableIndexB = gpuChildShapes[childShapeIndexB].m_shapeIndex;
			float4 childPosB = gpuChildShapes[childShapeIndexB].m_childPosition;
			float4 childOrnB = gpuChildShapes[childShapeIndexB].m_childOrientation;
			float4 newPosB = transform(&childPosB,&posB,&ornB);
			float4 newOrnB = qtMul(ornB,childOrnB);
			posB = newPosB;
			ornB = newOrnB;
		} else
		{
			collidableIndexB = rigidBodies[bodyIndexB].m_collidableIdx;
		}

		int shapeIndexA = collidables[collidableIndexA].m_shapeIndex;
		int shapeIndexB = collidables[collidableIndexB].m_shapeIndex;

		int shapeTypeA = collidables[collidableIndexA].m_shapeType;
		int shapeTypeB = collidables[collidableIndexB].m_shapeType;

		int pairIndex = i;
		if ((shapeTypeA == SHAPE_PLANE) && (shapeTypeB==SHAPE_CONVEX_HULL))
		{

			computeContactPlaneConvex( pairIndex, bodyIndexA,bodyIndexB,  collidableIndexA,collidableIndexB,
																rigidBodies,collidables,convexShapes,vertices,indices,
																faces,	globalContactsOut, nGlobalContactsOut,maxContactCapacity,posB,ornB);
			return;
		}

		if ((shapeTypeA == SHAPE_CONVEX_HULL) && (shapeTypeB==SHAPE_PLANE))
		{

			computeContactPlaneConvex( pairIndex, bodyIndexB,bodyIndexA,  collidableIndexB,collidableIndexA,
																rigidBodies,collidables,convexShapes,vertices,indices,
																faces,	globalContactsOut, nGlobalContactsOut,maxContactCapacity,posA,ornA);
			return;
		}

		if ((shapeTypeA == SHAPE_CONVEX_HULL) && (shapeTypeB == SHAPE_SPHERE))
		{
			float4 spherePos = rigidBodies[bodyIndexB].m_pos;
			float sphereRadius = collidables[collidableIndexB].m_radius;
			float4 convexPos = posA;
			float4 convexOrn = ornA;

			computeContactSphereConvex(pairIndex, bodyIndexB, bodyIndexA , collidableIndexB,collidableIndexA,
										rigidBodies,collidables,convexShapes,vertices,indices,faces, globalContactsOut, nGlobalContactsOut,maxContactCapacity,
										spherePos,sphereRadius,convexPos,convexOrn);

			return;
		}

		if ((shapeTypeA == SHAPE_SPHERE) && (shapeTypeB == SHAPE_CONVEX_HULL))
		{

			float4 spherePos = rigidBodies[bodyIndexA].m_pos;
			float sphereRadius = collidables[collidableIndexA].m_radius;
			float4 convexPos = posB;
			float4 convexOrn = ornB;


			computeContactSphereConvex(pairIndex, bodyIndexA, bodyIndexB, collidableIndexA, collidableIndexB,
										rigidBodies,collidables,convexShapes,vertices,indices,faces, globalContactsOut, nGlobalContactsOut,maxContactCapacity,
										spherePos,sphereRadius,convexPos,convexOrn);

			return;
		}
	}//	if (i<numCompoundPairs)
}


bool pointInTriangle(const float4* vertices, const float4* normal, float4 *p )
{

	const float4* p1 = &vertices[0];
	const float4* p2 = &vertices[1];
	const float4* p3 = &vertices[2];

	float4 edge1;	edge1 = (*p2 - *p1);
	float4 edge2;	edge2 = ( *p3 - *p2 );
	float4 edge3;	edge3 = ( *p1 - *p3 );


	float4 p1_to_p; p1_to_p = ( *p - *p1 );
	float4 p2_to_p; p2_to_p = ( *p - *p2 );
	float4 p3_to_p; p3_to_p = ( *p - *p3 );

	float4 edge1_normal; edge1_normal = ( cross(edge1,*normal));
	float4 edge2_normal; edge2_normal = ( cross(edge2,*normal));
	float4 edge3_normal; edge3_normal = ( cross(edge3,*normal));


	float r1, r2, r3;
	r1 = dot(edge1_normal,p1_to_p );
	r2 = dot(edge2_normal,p2_to_p );
	r3 = dot(edge3_normal,p3_to_p );

	if ( r1 > 0 && r2 > 0 && r3 > 0 )
		return true;
    if ( r1 <= 0 && r2 <= 0 && r3 <= 0 )
		return true;
	return false;

}


float segmentSqrDistance(float4 from, float4 to,float4 p, float4* nearest)
{
	float4 diff = p - from;
	float4 v = to - from;
	float t = dot(v,diff);

	if (t > 0)
	{
		float dotVV = dot(v,v);
		if (t < dotVV)
		{
			t /= dotVV;
			diff -= t*v;
		} else
		{
			t = 1;
			diff -= v;
		}
	} else
	{
		t = 0;
	}
	*nearest = from + t*v;
	return dot(diff,diff);
}


void	computeContactSphereTriangle(int pairIndex,
									int bodyIndexA, int bodyIndexB,
									int collidableIndexA, int collidableIndexB,
									__global const BodyData* rigidBodies,
									__global const btCollidableGpu* collidables,
									const float4* triangleVertices,
									__global struct b3Contact4Data* restrict globalContactsOut,
									counter32_t nGlobalContactsOut,
									int maxContactCapacity,
									float4 spherePos2,
									float radius,
									float4 pos,
									float4 quat,
									int faceIndex
									)
{

	float4 invPos;
	float4 invOrn;

	trInverse(pos,quat, &invPos,&invOrn);
	float4 spherePos = transform(&spherePos2,&invPos,&invOrn);
	int numFaces = 3;
	float4 closestPnt = (float4)(0, 0, 0, 0);
	float4 hitNormalWorld = (float4)(0, 0, 0, 0);
	float minDist = -1000000.f;
	bool bCollide = false;


	//////////////////////////////////////

	float4 sphereCenter;
	sphereCenter = spherePos;

	const float4* vertices = triangleVertices;
	float contactBreakingThreshold = 0.f;//todo?
	float radiusWithThreshold = radius + contactBreakingThreshold;
	float4 edge10;
	edge10 = vertices[1]-vertices[0];
	edge10.w = 0.f;//is this needed?
	float4 edge20;
	edge20 = vertices[2]-vertices[0];
	edge20.w = 0.f;//is this needed?
	float4 normal = cross3(edge10,edge20);
	normal = normalize(normal);
	float4 p1ToCenter;
	p1ToCenter = sphereCenter - vertices[0];

	float distanceFromPlane = dot(p1ToCenter,normal);

	if (distanceFromPlane < 0.f)
	{
		//triangle facing the other way
		distanceFromPlane *= -1.f;
		normal *= -1.f;
	}
	hitNormalWorld = normal;

	bool isInsideContactPlane = distanceFromPlane < radiusWithThreshold;

	// Check for contact / intersection
	bool hasContact = false;
	float4 contactPoint;
	if (isInsideContactPlane)
	{

		if (pointInTriangle(vertices,&normal, &sphereCenter))
		{
			// Inside the contact wedge - touches a point on the shell plane
			hasContact = true;
			contactPoint = sphereCenter - normal*distanceFromPlane;

		} else {
			// Could be inside one of the contact capsules
			float contactCapsuleRadiusSqr = radiusWithThreshold*radiusWithThreshold;
			float4 nearestOnEdge;
			int numEdges = 3;
			for (int i = 0; i < numEdges; i++)
			{
				float4 pa =vertices[i];
				float4 pb = vertices[(i+1)%3];

				float distanceSqr = segmentSqrDistance(pa,pb,sphereCenter, &nearestOnEdge);
				if (distanceSqr < contactCapsuleRadiusSqr)
				{
					// Yep, we're inside a capsule
					hasContact = true;
					contactPoint = nearestOnEdge;

				}

			}
		}
	}

	if (hasContact)
	{

		closestPnt = contactPoint;
		float4 contactToCenter = sphereCenter - contactPoint;
		minDist = length(contactToCenter);
		if (minDist>FLT_EPSILON)
		{
			hitNormalWorld = normalize(contactToCenter);//*(1./minDist);
			bCollide  = true;
		}

	}


	/////////////////////////////////////

	if (bCollide && minDist > -10000)
	{

		float4 normalOnSurfaceB1 = qtRotate(quat,-hitNormalWorld);
		float4 pOnB1 = transform(&closestPnt,&pos,&quat);
		float actualDepth = minDist-radius;


		if (actualDepth<=0.f)
		{
			pOnB1.w = actualDepth;
			int dstIdx;


			float lenSqr = dot3F4(normalOnSurfaceB1,normalOnSurfaceB1);
			if (lenSqr>FLT_EPSILON)
			{
				AppendInc( nGlobalContactsOut, dstIdx );

				if (dstIdx < maxContactCapacity)
				{
					__global struct b3Contact4Data* c = &globalContactsOut[dstIdx];
					c->m_worldNormalOnB = -normalOnSurfaceB1;
					c->m_restituitionCoeffCmp = (0.f*0xffff);c->m_frictionCoeffCmp = (0.7f*0xffff);
					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;

					c->m_childIndexA = -1;
					c->m_childIndexB = faceIndex;

					GET_NPOINTS(*c) = 1;
				}
			}

		}
	}//if (hasCollision)

}


// work-in-progress
__kernel void   findConcaveSphereContactsKernel( __global int4* concavePairs,
												__global const BodyData* rigidBodies,
												__global const btCollidableGpu* collidables,
												__global const ConvexPolyhedronCL* convexShapes,
												__global const float4* vertices,
												__global const float4* uniqueEdges,
												__global const btGpuFace* faces,
												__global const int* indices,
												__global btAabbCL* aabbs,
												__global struct b3Contact4Data* restrict globalContactsOut,
												counter32_t nGlobalContactsOut,
													int numConcavePairs, int maxContactCapacity
												)
{

	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_SPHERE)
	{
		int f = concavePairs[i].z;
		btGpuFace face = faces[convexShapes[shapeIndexA].m_faceOffset+f];

		float4 verticesA[3];
		for (int i=0;i<3;i++)
		{
			int index = indices[face.m_indexOffset+i];
			float4 vert = vertices[convexShapes[shapeIndexA].m_vertexOffset+index];
			verticesA[i] = vert;
		}

		float4 spherePos = rigidBodies[bodyIndexB].m_pos;
		float sphereRadius = collidables[collidableIndexB].m_radius;
		float4 convexPos = rigidBodies[bodyIndexA].m_pos;
		float4 convexOrn = rigidBodies[bodyIndexA].m_quat;

		computeContactSphereTriangle(i, bodyIndexB, bodyIndexA, collidableIndexB, collidableIndexA,
																rigidBodies,collidables,
																verticesA,
																globalContactsOut, nGlobalContactsOut,maxContactCapacity,
																spherePos,sphereRadius,convexPos,convexOrn, f);

		return;
	}
}