Bullet3OpenCL/NarrowphaseCollision/b3ConvexHullContact.cpp

/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2011 Advanced Micro Devices, Inc.  http://bulletphysics.org

This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:

1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/

bool findSeparatingAxisOnGpu = true;
bool splitSearchSepAxisConcave = false;
bool splitSearchSepAxisConvex = true;
bool useMprGpu = true;  //use mpr for edge-edge  (+contact point) or sat. Needs testing on main OpenCL platforms, before enabling...
bool bvhTraversalKernelGPU = true;
bool findConcaveSeparatingAxisKernelGPU = true;
bool clipConcaveFacesAndFindContactsCPU = false;  //false;//true;
bool clipConvexFacesAndFindContactsCPU = false;   //false;//true;
bool reduceConcaveContactsOnGPU = true;           //false;
bool reduceConvexContactsOnGPU = true;            //false;
bool findConvexClippingFacesGPU = true;
bool useGjk = false;          ///option for CPU/host testing, when findSeparatingAxisOnGpu = false
bool useGjkContacts = false;  //////option for CPU/host testing when findSeparatingAxisOnGpu = false

static int myframecount = 0;  ///for testing

///This file was written by Erwin Coumans
///Separating axis rest based on work from Pierre Terdiman, see
///And contact clipping based on work from Simon Hobbs

//#define B3_DEBUG_SAT_FACE

//#define CHECK_ON_HOST

#ifdef CHECK_ON_HOST
//#define PERSISTENT_CONTACTS_HOST
#endif

int b3g_actualSATPairTests = 0;

#include "b3ConvexHullContact.h"
#include <string.h>  //memcpy
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3ConvexPolyhedronData.h"
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3MprPenetration.h"

#include "Bullet3OpenCL/NarrowphaseCollision/b3ContactCache.h"
#include "Bullet3Geometry/b3AabbUtil.h"

typedef b3AlignedObjectArray<b3Vector3> b3VertexArray;

#include <float.h>  //for FLT_MAX
#include "Bullet3OpenCL/Initialize/b3OpenCLUtils.h"
#include "Bullet3OpenCL/ParallelPrimitives/b3LauncherCL.h"
//#include "AdlQuaternion.h"

#include "kernels/satKernels.h"
#include "kernels/mprKernels.h"

#include "kernels/satConcaveKernels.h"

#include "kernels/satClipHullContacts.h"
#include "kernels/bvhTraversal.h"
#include "kernels/primitiveContacts.h"

#include "Bullet3Geometry/b3AabbUtil.h"

#define BT_NARROWPHASE_SAT_PATH "src/Bullet3OpenCL/NarrowphaseCollision/kernels/sat.cl"
#define BT_NARROWPHASE_SAT_CONCAVE_PATH "src/Bullet3OpenCL/NarrowphaseCollision/kernels/satConcave.cl"

#define BT_NARROWPHASE_MPR_PATH "src/Bullet3OpenCL/NarrowphaseCollision/kernels/mpr.cl"

#define BT_NARROWPHASE_CLIPHULL_PATH "src/Bullet3OpenCL/NarrowphaseCollision/kernels/satClipHullContacts.cl"
#define BT_NARROWPHASE_BVH_TRAVERSAL_PATH "src/Bullet3OpenCL/NarrowphaseCollision/kernels/bvhTraversal.cl"
#define BT_NARROWPHASE_PRIMITIVE_CONTACT_PATH "src/Bullet3OpenCL/NarrowphaseCollision/kernels/primitiveContacts.cl"

#ifndef __global
#define __global
#endif

#ifndef __kernel
#define __kernel
#endif

#include "Bullet3Collision/NarrowPhaseCollision/shared/b3BvhTraversal.h"
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3FindConcaveSatAxis.h"
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3ClipFaces.h"
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3NewContactReduction.h"

#define dot3F4 b3Dot

GpuSatCollision::GpuSatCollision(cl_context ctx, cl_device_id device, cl_command_queue q)
	: m_context(ctx),
	  m_device(device),
	  m_queue(q),

	  m_findSeparatingAxisKernel(0),
	  m_findSeparatingAxisVertexFaceKernel(0),
	  m_findSeparatingAxisEdgeEdgeKernel(0),
	  m_unitSphereDirections(m_context, m_queue),

	  m_totalContactsOut(m_context, m_queue),
	  m_sepNormals(m_context, m_queue),
	  m_dmins(m_context, m_queue),

	  m_hasSeparatingNormals(m_context, m_queue),
	  m_concaveSepNormals(m_context, m_queue),
	  m_concaveHasSeparatingNormals(m_context, m_queue),
	  m_numConcavePairsOut(m_context, m_queue),

	  m_gpuCompoundPairs(m_context, m_queue),

	  m_gpuCompoundSepNormals(m_context, m_queue),
	  m_gpuHasCompoundSepNormals(m_context, m_queue),

	  m_numCompoundPairsOut(m_context, m_queue)
{
	m_totalContactsOut.push_back(0);

	cl_int errNum = 0;

	if (1)
	{
		const char* mprSrc = mprKernelsCL;

		const char* srcConcave = satConcaveKernelsCL;
		char flags[1024] = {0};
		//#ifdef CL_PLATFORM_INTEL
		//		sprintf(flags,"-g -s \"%s\"","C:/develop/bullet3_experiments2/opencl/gpu_narrowphase/kernels/sat.cl");
		//#endif
		m_mprPenetrationKernel = 0;
		m_findSeparatingAxisUnitSphereKernel = 0;

		if (useMprGpu)
		{
			cl_program mprProg = b3OpenCLUtils::compileCLProgramFromString(m_context, m_device, mprSrc, &errNum, flags, BT_NARROWPHASE_MPR_PATH);
			b3Assert(errNum == CL_SUCCESS);

			m_mprPenetrationKernel = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device, mprSrc, "mprPenetrationKernel", &errNum, mprProg);
			b3Assert(m_mprPenetrationKernel);
			b3Assert(errNum == CL_SUCCESS);

			m_findSeparatingAxisUnitSphereKernel = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device, mprSrc, "findSeparatingAxisUnitSphereKernel", &errNum, mprProg);
			b3Assert(m_findSeparatingAxisUnitSphereKernel);
			b3Assert(errNum == CL_SUCCESS);

			int numDirections = sizeof(unitSphere162) / sizeof(b3Vector3);
			m_unitSphereDirections.resize(numDirections);
			m_unitSphereDirections.copyFromHostPointer(unitSphere162, numDirections, 0, true);
		}

		cl_program satProg = b3OpenCLUtils::compileCLProgramFromString(m_context, m_device, satKernelsCL, &errNum, flags, BT_NARROWPHASE_SAT_PATH);
		b3Assert(errNum == CL_SUCCESS);

		cl_program satConcaveProg = b3OpenCLUtils::compileCLProgramFromString(m_context, m_device, srcConcave, &errNum, flags, BT_NARROWPHASE_SAT_CONCAVE_PATH);
		b3Assert(errNum == CL_SUCCESS);

		m_findSeparatingAxisKernel = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device, satKernelsCL, "findSeparatingAxisKernel", &errNum, satProg);
		b3Assert(m_findSeparatingAxisKernel);
		b3Assert(errNum == CL_SUCCESS);

		m_findSeparatingAxisVertexFaceKernel = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device, satKernelsCL, "findSeparatingAxisVertexFaceKernel", &errNum, satProg);
		b3Assert(m_findSeparatingAxisVertexFaceKernel);

		m_findSeparatingAxisEdgeEdgeKernel = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device, satKernelsCL, "findSeparatingAxisEdgeEdgeKernel", &errNum, satProg);
		b3Assert(m_findSeparatingAxisVertexFaceKernel);

		m_findConcaveSeparatingAxisKernel = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device, satKernelsCL, "findConcaveSeparatingAxisKernel", &errNum, satProg);
		b3Assert(m_findConcaveSeparatingAxisKernel);
		b3Assert(errNum == CL_SUCCESS);

		m_findConcaveSeparatingAxisVertexFaceKernel = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device, srcConcave, "findConcaveSeparatingAxisVertexFaceKernel", &errNum, satConcaveProg);
		b3Assert(m_findConcaveSeparatingAxisVertexFaceKernel);
		b3Assert(errNum == CL_SUCCESS);

		m_findConcaveSeparatingAxisEdgeEdgeKernel = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device, srcConcave, "findConcaveSeparatingAxisEdgeEdgeKernel", &errNum, satConcaveProg);
		b3Assert(m_findConcaveSeparatingAxisEdgeEdgeKernel);
		b3Assert(errNum == CL_SUCCESS);

		m_findCompoundPairsKernel = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device, satKernelsCL, "findCompoundPairsKernel", &errNum, satProg);
		b3Assert(m_findCompoundPairsKernel);
		b3Assert(errNum == CL_SUCCESS);
		m_processCompoundPairsKernel = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device, satKernelsCL, "processCompoundPairsKernel", &errNum, satProg);
		b3Assert(m_processCompoundPairsKernel);
		b3Assert(errNum == CL_SUCCESS);
	}

	if (1)
	{
		const char* srcClip = satClipKernelsCL;

		char flags[1024] = {0};
		//#ifdef CL_PLATFORM_INTEL
		//		sprintf(flags,"-g -s \"%s\"","C:/develop/bullet3_experiments2/opencl/gpu_narrowphase/kernels/satClipHullContacts.cl");
		//#endif

		cl_program satClipContactsProg = b3OpenCLUtils::compileCLProgramFromString(m_context, m_device, srcClip, &errNum, flags, BT_NARROWPHASE_CLIPHULL_PATH);
		b3Assert(errNum == CL_SUCCESS);

		m_clipHullHullKernel = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device, srcClip, "clipHullHullKernel", &errNum, satClipContactsProg);
		b3Assert(errNum == CL_SUCCESS);

		m_clipCompoundsHullHullKernel = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device, srcClip, "clipCompoundsHullHullKernel", &errNum, satClipContactsProg);
		b3Assert(errNum == CL_SUCCESS);

		m_findClippingFacesKernel = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device, srcClip, "findClippingFacesKernel", &errNum, satClipContactsProg);
		b3Assert(errNum == CL_SUCCESS);

		m_clipFacesAndFindContacts = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device, srcClip, "clipFacesAndFindContactsKernel", &errNum, satClipContactsProg);
		b3Assert(errNum == CL_SUCCESS);

		m_clipHullHullConcaveConvexKernel = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device, srcClip, "clipHullHullConcaveConvexKernel", &errNum, satClipContactsProg);
		b3Assert(errNum == CL_SUCCESS);

		//		m_extractManifoldAndAddContactKernel = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device,srcClip, "extractManifoldAndAddContactKernel",&errNum,satClipContactsProg);
		//	b3Assert(errNum==CL_SUCCESS);

		m_newContactReductionKernel = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device, srcClip,
																			   "newContactReductionKernel", &errNum, satClipContactsProg);
		b3Assert(errNum == CL_SUCCESS);
	}
	else
	{
		m_clipHullHullKernel = 0;
		m_clipCompoundsHullHullKernel = 0;
		m_findClippingFacesKernel = 0;
		m_newContactReductionKernel = 0;
		m_clipFacesAndFindContacts = 0;
		m_clipHullHullConcaveConvexKernel = 0;
		//		m_extractManifoldAndAddContactKernel = 0;
	}

	if (1)
	{
		const char* srcBvh = bvhTraversalKernelCL;
		cl_program bvhTraversalProg = b3OpenCLUtils::compileCLProgramFromString(m_context, m_device, srcBvh, &errNum, "", BT_NARROWPHASE_BVH_TRAVERSAL_PATH);
		b3Assert(errNum == CL_SUCCESS);

		m_bvhTraversalKernel = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device, srcBvh, "bvhTraversalKernel", &errNum, bvhTraversalProg, "");
		b3Assert(errNum == CL_SUCCESS);
	}

	{
		const char* primitiveContactsSrc = primitiveContactsKernelsCL;
		cl_program primitiveContactsProg = b3OpenCLUtils::compileCLProgramFromString(m_context, m_device, primitiveContactsSrc, &errNum, "", BT_NARROWPHASE_PRIMITIVE_CONTACT_PATH);
		b3Assert(errNum == CL_SUCCESS);

		m_primitiveContactsKernel = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device, primitiveContactsSrc, "primitiveContactsKernel", &errNum, primitiveContactsProg, "");
		b3Assert(errNum == CL_SUCCESS);

		m_findConcaveSphereContactsKernel = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device, primitiveContactsSrc, "findConcaveSphereContactsKernel", &errNum, primitiveContactsProg);
		b3Assert(errNum == CL_SUCCESS);
		b3Assert(m_findConcaveSphereContactsKernel);

		m_processCompoundPairsPrimitivesKernel = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device, primitiveContactsSrc, "processCompoundPairsPrimitivesKernel", &errNum, primitiveContactsProg, "");
		b3Assert(errNum == CL_SUCCESS);
		b3Assert(m_processCompoundPairsPrimitivesKernel);
	}
}

GpuSatCollision::~GpuSatCollision()
{
	if (m_findSeparatingAxisVertexFaceKernel)
		clReleaseKernel(m_findSeparatingAxisVertexFaceKernel);

	if (m_findSeparatingAxisEdgeEdgeKernel)
		clReleaseKernel(m_findSeparatingAxisEdgeEdgeKernel);

	if (m_findSeparatingAxisUnitSphereKernel)
		clReleaseKernel(m_findSeparatingAxisUnitSphereKernel);

	if (m_mprPenetrationKernel)
		clReleaseKernel(m_mprPenetrationKernel);

	if (m_findSeparatingAxisKernel)
		clReleaseKernel(m_findSeparatingAxisKernel);

	if (m_findConcaveSeparatingAxisVertexFaceKernel)
		clReleaseKernel(m_findConcaveSeparatingAxisVertexFaceKernel);

	if (m_findConcaveSeparatingAxisEdgeEdgeKernel)
		clReleaseKernel(m_findConcaveSeparatingAxisEdgeEdgeKernel);

	if (m_findConcaveSeparatingAxisKernel)
		clReleaseKernel(m_findConcaveSeparatingAxisKernel);

	if (m_findCompoundPairsKernel)
		clReleaseKernel(m_findCompoundPairsKernel);

	if (m_processCompoundPairsKernel)
		clReleaseKernel(m_processCompoundPairsKernel);

	if (m_findClippingFacesKernel)
		clReleaseKernel(m_findClippingFacesKernel);

	if (m_clipFacesAndFindContacts)
		clReleaseKernel(m_clipFacesAndFindContacts);
	if (m_newContactReductionKernel)
		clReleaseKernel(m_newContactReductionKernel);
	if (m_primitiveContactsKernel)
		clReleaseKernel(m_primitiveContactsKernel);

	if (m_findConcaveSphereContactsKernel)
		clReleaseKernel(m_findConcaveSphereContactsKernel);

	if (m_processCompoundPairsPrimitivesKernel)
		clReleaseKernel(m_processCompoundPairsPrimitivesKernel);

	if (m_clipHullHullKernel)
		clReleaseKernel(m_clipHullHullKernel);
	if (m_clipCompoundsHullHullKernel)
		clReleaseKernel(m_clipCompoundsHullHullKernel);

	if (m_clipHullHullConcaveConvexKernel)
		clReleaseKernel(m_clipHullHullConcaveConvexKernel);
	//	if (m_extractManifoldAndAddContactKernel)
	//	clReleaseKernel(m_extractManifoldAndAddContactKernel);

	if (m_bvhTraversalKernel)
		clReleaseKernel(m_bvhTraversalKernel);
}

struct MyTriangleCallback : public b3NodeOverlapCallback
{
	int m_bodyIndexA;
	int m_bodyIndexB;

	virtual void processNode(int subPart, int triangleIndex)
	{
		printf("bodyIndexA %d, bodyIndexB %d\n", m_bodyIndexA, m_bodyIndexB);
		printf("triangleIndex %d\n", triangleIndex);
	}
};

#define float4 b3Vector3
#define make_float4(x, y, z, w) b3MakeVector3(x, y, z, w)

float signedDistanceFromPointToPlane(const float4& point, const float4& planeEqn, float4* closestPointOnFace)
{
	float4 n = planeEqn;
	n[3] = 0.f;
	float dist = dot3F4(n, point) + planeEqn[3];
	*closestPointOnFace = point - dist * n;
	return dist;
}

#define cross3(a, b) (a.cross(b))
b3Vector3 transform(const b3Vector3* v, const b3Vector3* pos, const b3Quaternion* orn)
{
	b3Transform tr;
	tr.setIdentity();
	tr.setOrigin(*pos);
	tr.setRotation(*orn);
	b3Vector3 res = tr(*v);
	return res;
}

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

	float4 plane = b3MakeVector3(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 (b3Dot(ap, v) > 0.f)
		{
			float ab_m2 = b3Dot(ab, ab);
			float rt = ab_m2 != 0.f ? b3Dot(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;
}

#define normalize3(a) (a.normalize())

int extractManifoldSequentialGlobal(const float4* p, int nPoints, const float4& nearNormal, b3Int4* contactIdx)
{
	if (nPoints == 0)
		return 0;

	if (nPoints <= 4)
		return nPoints;

	if (nPoints > 64)
		nPoints = 64;

	float4 center = b3MakeVector3(0, 0, 0, 0);
	{
		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_VERTS 1024

inline void project(const b3ConvexPolyhedronData& hull, const float4& pos, const b3Quaternion& orn, const float4& dir, const b3AlignedObjectArray<b3Vector3>& vertices, b3Scalar& min, b3Scalar& max)
{
	min = FLT_MAX;
	max = -FLT_MAX;
	int numVerts = hull.m_numVertices;

	const float4 localDir = b3QuatRotate(orn.inverse(), dir);

	b3Scalar offset = dot3F4(pos, dir);

	for (int i = 0; i < numVerts; i++)
	{
		//b3Vector3 pt = trans * vertices[m_vertexOffset+i];
		//b3Scalar dp = pt.dot(dir);
		//b3Vector3 vertex = vertices[hull.m_vertexOffset+i];
		b3Scalar dp = dot3F4((float4&)vertices[hull.m_vertexOffset + i], localDir);
		//b3Assert(dp==dpL);
		if (dp < min) min = dp;
		if (dp > max) max = dp;
	}
	if (min > max)
	{
		b3Scalar tmp = min;
		min = max;
		max = tmp;
	}
	min += offset;
	max += offset;
}

static bool TestSepAxis(const b3ConvexPolyhedronData& hullA, const b3ConvexPolyhedronData& hullB,
						const float4& posA, const b3Quaternion& ornA,
						const float4& posB, const b3Quaternion& ornB,
						const float4& sep_axis, const b3AlignedObjectArray<b3Vector3>& verticesA, const b3AlignedObjectArray<b3Vector3>& verticesB, b3Scalar& depth)
{
	b3Scalar Min0, Max0;
	b3Scalar Min1, Max1;
	project(hullA, posA, ornA, sep_axis, verticesA, Min0, Max0);
	project(hullB, posB, ornB, sep_axis, verticesB, Min1, Max1);

	if (Max0 < Min1 || Max1 < Min0)
		return false;

	b3Scalar d0 = Max0 - Min1;
	assert(d0 >= 0.0f);
	b3Scalar d1 = Max1 - Min0;
	assert(d1 >= 0.0f);
	depth = d0 < d1 ? d0 : d1;
	return true;
}

inline bool IsAlmostZero(const b3Vector3& v)
{
	if (fabsf(v.x) > 1e-6 || fabsf(v.y) > 1e-6 || fabsf(v.z) > 1e-6) return false;
	return true;
}

static bool findSeparatingAxis(const b3ConvexPolyhedronData& hullA, const b3ConvexPolyhedronData& hullB,
							   const float4& posA1,
							   const b3Quaternion& ornA,
							   const float4& 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");

	b3g_actualSATPairTests++;
	float4 posA = posA1;
	posA.w = 0.f;
	float4 posB = posB1;
	posB.w = 0.f;
	//#ifdef TEST_INTERNAL_OBJECTS
	float4 c0local = (float4&)hullA.m_localCenter;
	float4 c0 = transform(&c0local, &posA, &ornA);
	float4 c1local = (float4&)hullB.m_localCenter;
	float4 c1 = transform(&c1local, &posB, &ornB);
	const float4 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 float4& normal = (float4&)facesA[hullA.m_faceOffset + i].m_plane;
		float4 faceANormalWS = b3QuatRotate(ornA, normal);

		if (dot3F4(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 (!TestSepAxis(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++)
	{
		float4 normal = (float4&)facesB[hullB.m_faceOffset + i].m_plane;
		float4 WorldNormal = b3QuatRotate(ornB, normal);

		if (dot3F4(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 (!TestSepAxis(hullA, hullB, posA, ornA, posB, ornB, WorldNormal, verticesA, verticesB, d))
			return false;

		if (d < dmin)
		{
			dmin = d;
			sep = (b3Vector3&)WorldNormal;
		}
	}

	int curEdgeEdge = 0;
	// Test edges
	for (int e0 = 0; e0 < hullA.m_numUniqueEdges; e0++)
	{
		const float4& edge0 = (float4&)uniqueEdgesA[hullA.m_uniqueEdgesOffset + e0];
		float4 edge0World = b3QuatRotate(ornA, (float4&)edge0);

		for (int e1 = 0; e1 < hullB.m_numUniqueEdges; e1++)
		{
			const b3Vector3 edge1 = uniqueEdgesB[hullB.m_uniqueEdgesOffset + e1];
			float4 edge1World = b3QuatRotate(ornB, (float4&)edge1);

			float4 crossje = cross3(edge0World, edge1World);

			curEdgeEdge++;
			if (!IsAlmostZero((b3Vector3&)crossje))
			{
				crossje = normalize3(crossje);
				if (dot3F4(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 (!TestSepAxis(hullA, hullB, posA, ornA, posB, ornB, crossje, verticesA, verticesB, dist))
					return false;

				if (dist < dmin)
				{
					dmin = dist;
					sep = (b3Vector3&)crossje;
				}
			}
		}
	}

	if ((dot3F4(-deltaC2, (float4&)sep)) > 0.0f)
		sep = -sep;

	return true;
}

bool findSeparatingAxisEdgeEdge(__global const b3ConvexPolyhedronData* hullA, __global const b3ConvexPolyhedronData* hullB,
								const b3Float4& posA1,
								const b3Quat& ornA,
								const b3Float4& posB1,
								const b3Quat& ornB,
								const b3Float4& DeltaC2,
								__global const b3AlignedObjectArray<float4>& vertices,
								__global const b3AlignedObjectArray<float4>& uniqueEdges,
								__global const b3AlignedObjectArray<b3GpuFace>& faces,
								__global const b3AlignedObjectArray<int>& indices,
								float4* sep,
								float* dmin)
{
	//	int i = get_global_id(0);

	float4 posA = posA1;
	posA.w = 0.f;
	float4 posB = posB1;
	posB.w = 0.f;

	//int curPlaneTests=0;

	int curEdgeEdge = 0;
	// Test edges
	for (int e0 = 0; e0 < hullA->m_numUniqueEdges; e0++)
	{
		const float4 edge0 = uniqueEdges[hullA->m_uniqueEdgesOffset + e0];
		float4 edge0World = b3QuatRotate(ornA, edge0);

		for (int e1 = 0; e1 < hullB->m_numUniqueEdges; e1++)
		{
			const float4 edge1 = uniqueEdges[hullB->m_uniqueEdgesOffset + e1];
			float4 edge1World = b3QuatRotate(ornB, edge1);

			float4 crossje = cross3(edge0World, edge1World);

			curEdgeEdge++;
			if (!IsAlmostZero(crossje))
			{
				crossje = normalize3(crossje);
				if (dot3F4(DeltaC2, crossje) < 0)
					crossje *= -1.f;

				float dist;
				bool result = true;
				{
					float Min0, Max0;
					float Min1, Max1;
					project(*hullA, posA, ornA, crossje, vertices, Min0, Max0);
					project(*hullB, posB, ornB, crossje, vertices, Min1, Max1);

					if (Max0 < Min1 || Max1 < Min0)
						result = false;

					float d0 = Max0 - Min1;
					float d1 = Max1 - Min0;
					dist = d0 < d1 ? d0 : d1;
					result = true;
				}

				if (dist < *dmin)
				{
					*dmin = dist;
					*sep = crossje;
				}
			}
		}
	}

	if ((dot3F4(-DeltaC2, *sep)) > 0.0f)
	{
		*sep = -(*sep);
	}
	return true;
}

__inline float4 lerp3(const float4& a, const float4& 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
int clipFace(const float4* pVtxIn, int numVertsIn, float4& planeNormalWS, float planeEqWS, float4* ppVtxOut)
{
	int ve;
	float ds, de;
	int numVertsOut = 0;
	if (numVertsIn < 2)
		return 0;

	float4 firstVertex = pVtxIn[numVertsIn - 1];
	float4 endVertex = pVtxIn[0];

	ds = dot3F4(planeNormalWS, firstVertex) + planeEqWS;

	for (ve = 0; ve < numVertsIn; ve++)
	{
		endVertex = pVtxIn[ve];

		de = dot3F4(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++] = lerp3(firstVertex, endVertex, (ds * 1.f / (ds - de)));
			}
		}
		else
		{
			if (de < 0)
			{
				// Start >= 0, end < 0 so output intersection and end
				ppVtxOut[numVertsOut++] = lerp3(firstVertex, endVertex, (ds * 1.f / (ds - de)));
				ppVtxOut[numVertsOut++] = endVertex;
			}
		}
		firstVertex = endVertex;
		ds = de;
	}
	return numVertsOut;
}

int clipFaceAgainstHull(const float4& separatingNormal, const b3ConvexPolyhedronData* hullA,
						const float4& posA, const b3Quaternion& ornA, float4* worldVertsB1, int numWorldVertsB1,
						float4* worldVertsB2, int capacityWorldVertsB2,
						const float minDist, float maxDist,
						const b3AlignedObjectArray<float4>& verticesA, const b3AlignedObjectArray<b3GpuFace>& facesA, const b3AlignedObjectArray<int>& indicesA,
						//const float4* verticesB,	const b3GpuFace* facesB,	const int* indicesB,
						float4* contactsOut,
						int contactCapacity)
{
	int numContactsOut = 0;

	float4* pVtxIn = worldVertsB1;
	float4* 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 float4 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 float4 faceANormalWS = b3QuatRotate(ornA, Normal);

			float d = dot3F4(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 float4 a = verticesA[hullA->m_vertexOffset + indicesA[polyA.m_indexOffset + e0]];
		const float4 b = verticesA[hullA->m_vertexOffset + indicesA[polyA.m_indexOffset + ((e0 + 1) % numVerticesA)]];
		const float4 edge0 = a - b;
		const float4 WorldEdge0 = b3QuatRotate(ornA, edge0);
		float4 planeNormalA = make_float4(polyA.m_plane.x, polyA.m_plane.y, polyA.m_plane.z, 0.f);
		float4 worldPlaneAnormal1 = b3QuatRotate(ornA, planeNormalA);

		float4 planeNormalWS1 = -cross3(WorldEdge0, worldPlaneAnormal1);
		float4 worldA1 = transform(&a, &posA, &ornA);
		float planeEqWS1 = -dot3F4(worldA1, planeNormalWS1);

		float4 planeNormalWS = planeNormalWS1;
		float planeEqWS = planeEqWS1;

		//clip face
		//clipFace(*pVtxIn, *pVtxOut,planeNormalWS,planeEqWS);
		numVertsOut = clipFace(pVtxIn, numVertsIn, planeNormalWS, planeEqWS, pVtxOut);

		//btSwap(pVtxIn,pVtxOut);
		float4* tmp = pVtxOut;
		pVtxOut = pVtxIn;
		pVtxIn = tmp;
		numVertsIn = numVertsOut;
		numVertsOut = 0;
	}

	// only keep points that are behind the witness face
	{
		float4 localPlaneNormal = make_float4(polyA.m_plane.x, polyA.m_plane.y, polyA.m_plane.z, 0.f);
		float localPlaneEq = polyA.m_plane.w;
		float4 planeNormalWS = b3QuatRotate(ornA, localPlaneNormal);
		float planeEqWS = localPlaneEq - dot3F4(planeNormalWS, posA);
		for (int i = 0; i < numVertsIn; i++)
		{
			float depth = dot3F4(planeNormalWS, pVtxIn[i]) + planeEqWS;
			if (depth <= minDist)
			{
				depth = minDist;
			}
			if (numContactsOut < contactCapacity)
			{
				if (depth <= maxDist)
				{
					float4 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;
}

static int clipHullAgainstHull(const float4& separatingNormal,
							   const b3ConvexPolyhedronData& hullA, const b3ConvexPolyhedronData& hullB,
							   const float4& posA, const b3Quaternion& ornA, const float4& posB, const b3Quaternion& ornB,
							   float4* worldVertsB1, float4* worldVertsB2, int capacityWorldVerts,
							   const float minDist, float maxDist,
							   const b3AlignedObjectArray<float4>& verticesA, const b3AlignedObjectArray<b3GpuFace>& facesA, const b3AlignedObjectArray<int>& indicesA,
							   const b3AlignedObjectArray<float4>& verticesB, const b3AlignedObjectArray<b3GpuFace>& facesB, const b3AlignedObjectArray<int>& indicesB,

							   float4* 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++)
				{
					float4 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 float4 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 float4 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 = dot3F4(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 float4& b = verticesB[hullB.m_vertexOffset + indicesB[polyB.m_indexOffset + e0]];
			worldVertsB1[numWorldVertsB1++] = transform(&b, &posB, &ornB);
		}
	}

	if (closestFaceB >= 0)
	{
		//B3_PROFILE("clipFaceAgainstHull");
		numContactsOut = clipFaceAgainstHull((float4&)separatingNormal, &hullA,
											 posA, ornA,
											 worldVertsB1, numWorldVertsB1, worldVertsB2, capacityWorldVerts, minDist, maxDist,
											 verticesA, facesA, indicesA,
											 contactsOut, contactCapacity);
	}

	return numContactsOut;
}

#define PARALLEL_SUM(v, n) \
	for (int j = 1; j < n; j++) v[0] += v[j];
#define PARALLEL_DO(execution, n)  \
	for (int ie = 0; ie < n; ie++) \
	{                              \
		execution;                 \
	}
#define REDUCE_MAX(v, n)                                                                                     \
	{                                                                                                        \
		int i = 0;                                                                                           \
		for (int offset = 0; offset < n; offset++) v[i] = (v[i].y > v[i + offset].y) ? v[i] : v[i + offset]; \
	}
#define REDUCE_MIN(v, n)                                                                                     \
	{                                                                                                        \
		int i = 0;                                                                                           \
		for (int offset = 0; offset < n; offset++) v[i] = (v[i].y < v[i + offset].y) ? v[i] : v[i + offset]; \
	}

int extractManifold(const float4* p, int nPoints, const float4& nearNormal, b3Int4* contactIdx)
{
	if (nPoints == 0)
		return 0;

	if (nPoints <= 4)
		return nPoints;

	if (nPoints > 64)
		nPoints = 64;

	float4 center = make_float4(0, 0, 0, 0);
	{
		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;
}

int clipHullHullSingle(
	int bodyIndexA, int bodyIndexB,
	const float4& posA,
	const b3Quaternion& ornA,
	const float4& posB,
	const b3Quaternion& ornB,

	int collidableIndexA, int collidableIndexB,

	const b3AlignedObjectArray<b3RigidBodyData>* bodyBuf,
	b3AlignedObjectArray<b3Contact4>* 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);

	float4 contactsOut[MAX_VERTS];
	int localContactCapacity = MAX_VERTS;

#ifdef _WIN32
	b3Assert(_finite(bodyBuf->at(bodyIndexA).m_pos.x));
	b3Assert(_finite(bodyBuf->at(bodyIndexB).m_pos.x));
#endif

	{
		float4 worldVertsB1[MAX_VERTS];
		float4 worldVertsB2[MAX_VERTS];
		int capacityWorldVerts = MAX_VERTS;

		float4 hostNormal = make_float4(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("transform 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 = clipHullAgainstHull(hostNormal,
												 hostConvexDataA.at(shapeA),
												 hostConvexDataB.at(shapeB),
												 (float4&)trA.getOrigin(), (b3Quaternion&)trAorn,
												 (float4&)trB.getOrigin(), (b3Quaternion&)trBorn,
												 worldVertsB1, worldVertsB2, capacityWorldVerts,
												 minDist, maxDist,
												 verticesA, facesA, indicesA,
												 verticesB, facesB, indicesB,

												 contactsOut, localContactCapacity);

		if (numContactsOut > 0)
		{
			B3_PROFILE("overlap");

			float4 normalOnSurfaceB = (float4&)hostNormal;

			b3Int4 contactIdx;
			contactIdx.x = 0;
			contactIdx.y = 1;
			contactIdx.z = 2;
			contactIdx.w = 3;

			int numPoints = 0;

			{
				//	B3_PROFILE("extractManifold");
				numPoints = extractManifold(contactsOut, numContactsOut, normalOnSurfaceB, &contactIdx);
			}

			b3Assert(numPoints);

			if (nContacts < maxContactCapacity)
			{
				contactIndex = nContacts;
				globalContactOut->expand();
				b3Contact4& contact = globalContactOut->at(nContacts);
				contact.m_batchIdx = 0;  //i;
				contact.m_bodyAPtrAndSignBit = (bodyBuf->at(bodyIndexA).m_invMass == 0) ? -bodyIndexA : bodyIndexA;
				contact.m_bodyBPtrAndSignBit = (bodyBuf->at(bodyIndexB).m_invMass == 0) ? -bodyIndexB : bodyIndexB;

				contact.m_frictionCoeffCmp = 45874;
				contact.m_restituitionCoeffCmp = 0;

				//			float distance = 0.f;
				for (int p = 0; p < numPoints; p++)
				{
					contact.m_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;
}

void computeContactPlaneConvex(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)
{
	int shapeIndex = collidables[collidableIndexB].m_shapeIndex;
	const b3ConvexPolyhedronData* hullB = &convexShapes[shapeIndex];

	b3Vector3 posB = rigidBodies[bodyIndexB].m_pos;
	b3Quaternion ornB = rigidBodies[bodyIndexB].m_quat;
	b3Vector3 posA = rigidBodies[bodyIndexA].m_pos;
	b3Quaternion ornA = rigidBodies[bodyIndexA].m_quat;

	//	int numContactsOut = 0;
	//	int numWorldVertsB1= 0;

	b3Vector3 planeEq = faces[collidables[collidableIndexA].m_shapeIndex].m_plane;
	b3Vector3 planeNormal = b3MakeVector3(planeEq.x, planeEq.y, planeEq.z);
	b3Vector3 planeNormalWorld = b3QuatRotate(ornA, planeNormal);
	float planeConstant = planeEq.w;
	b3Transform convexWorldTransform;
	convexWorldTransform.setIdentity();
	convexWorldTransform.setOrigin(posB);
	convexWorldTransform.setRotation(ornB);
	b3Transform planeTransform;
	planeTransform.setIdentity();
	planeTransform.setOrigin(posA);
	planeTransform.setRotation(ornA);

	b3Transform planeInConvex;
	planeInConvex = convexWorldTransform.inverse() * planeTransform;
	b3Transform convexInPlane;
	convexInPlane = planeTransform.inverse() * convexWorldTransform;

	b3Vector3 planeNormalInConvex = planeInConvex.getBasis() * -planeNormal;
	float maxDot = -1e30;
	int hitVertex = -1;
	b3Vector3 hitVtx;

#define MAX_PLANE_CONVEX_POINTS 64

	b3Vector3 contactPoints[MAX_PLANE_CONVEX_POINTS];
	int numPoints = 0;

	b3Int4 contactIdx;
	contactIdx.s[0] = 0;
	contactIdx.s[1] = 1;
	contactIdx.s[2] = 2;
	contactIdx.s[3] = 3;

	for (int i = 0; i < hullB->m_numVertices; i++)
	{
		b3Vector3 vtx = convexVertices[hullB->m_vertexOffset + i];
		float curDot = vtx.dot(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)
		{
			b3Vector3 vtxWorld = convexWorldTransform * vtx;
			b3Vector3 vtxInPlane = planeTransform.inverse() * vtxWorld;
			float dist = planeNormal.dot(vtxInPlane) - planeConstant;
			if (dist < 0.f)
			{
				vtxWorld.w = dist;
				contactPoints[numPoints] = vtxWorld;
				numPoints++;
			}
		}
	}

	int numReducedPoints = 0;

	numReducedPoints = numPoints;

	if (numPoints > 4)
	{
		numReducedPoints = extractManifoldSequentialGlobal(contactPoints, numPoints, planeNormalInConvex, &contactIdx);
	}
	int dstIdx;
	//    dstIdx = nGlobalContactsOut++;//AppendInc( nGlobalContactsOut, dstIdx );

	if (numReducedPoints > 0)
	{
		if (nGlobalContactsOut < maxContactCapacity)
		{
			dstIdx = nGlobalContactsOut;
			nGlobalContactsOut++;

			b3Contact4* c = &globalContactsOut[dstIdx];
			c->m_worldNormalOnB = -planeNormalWorld;
			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;
			for (int i = 0; i < numReducedPoints; i++)
			{
				b3Vector3 pOnB1 = contactPoints[contactIdx.s[i]];
				c->m_worldPosB[i] = pOnB1;
			}
			c->m_worldNormalOnB.w = (b3Scalar)numReducedPoints;
		}  //if (dstIdx < numPairs)
	}

	//	printf("computeContactPlaneConvex\n");
}

B3_FORCE_INLINE b3Vector3 MyUnQuantize(const unsigned short* vecIn, const b3Vector3& quantization, const b3Vector3& bvhAabbMin)
{
	b3Vector3 vecOut;
	vecOut.setValue(
		(b3Scalar)(vecIn[0]) / (quantization.x),
		(b3Scalar)(vecIn[1]) / (quantization.y),
		(b3Scalar)(vecIn[2]) / (quantization.z));
	vecOut += bvhAabbMin;
	return vecOut;
}

void traverseTreeTree()
{
}

#include "Bullet3Common/shared/b3Mat3x3.h"

int numAabbChecks = 0;
int maxNumAabbChecks = 0;
int maxDepth = 0;

// work-in-progress
__kernel void findCompoundPairsKernel(
	int pairIndex,
	int bodyIndexA,
	int bodyIndexB,
	int collidableIndexA,
	int collidableIndexB,
	__global const b3RigidBodyData* rigidBodies,
	__global const b3Collidable* collidables,
	__global const b3ConvexPolyhedronData* convexShapes,
	__global const b3AlignedObjectArray<b3Float4>& vertices,
	__global const b3AlignedObjectArray<b3Aabb>& aabbsWorldSpace,
	__global const b3AlignedObjectArray<b3Aabb>& aabbsLocalSpace,
	__global const b3GpuChildShape* gpuChildShapes,
	__global b3Int4* gpuCompoundPairsOut,
	__global int* numCompoundPairsOut,
	int maxNumCompoundPairsCapacity,
	b3AlignedObjectArray<b3QuantizedBvhNode>& treeNodesCPU,
	b3AlignedObjectArray<b3BvhSubtreeInfo>& subTreesCPU,
	b3AlignedObjectArray<b3BvhInfo>& bvhInfoCPU)
{
	numAabbChecks = 0;
	maxNumAabbChecks = 0;
	//	int i = pairIndex;
	{
		int shapeIndexA = collidables[collidableIndexA].m_shapeIndex;
		int shapeIndexB = collidables[collidableIndexB].m_shapeIndex;

		//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_COMPOUND_OF_CONVEX_HULLS) && (collidables[collidableIndexB].m_shapeType == SHAPE_COMPOUND_OF_CONVEX_HULLS))
		{
			int bvhA = collidables[collidableIndexA].m_compoundBvhIndex;
			int bvhB = collidables[collidableIndexB].m_compoundBvhIndex;
			int numSubTreesA = bvhInfoCPU[bvhA].m_numSubTrees;
			int subTreesOffsetA = bvhInfoCPU[bvhA].m_subTreeOffset;
			int subTreesOffsetB = bvhInfoCPU[bvhB].m_subTreeOffset;

			int numSubTreesB = bvhInfoCPU[bvhB].m_numSubTrees;

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

			b3Transform transA;
			transA.setIdentity();
			transA.setOrigin(posA);
			transA.setRotation(ornA);

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

			b3Transform transB;
			transB.setIdentity();
			transB.setOrigin(posB);
			transB.setRotation(ornB);

			for (int p = 0; p < numSubTreesA; p++)
			{
				b3BvhSubtreeInfo subtreeA = subTreesCPU[subTreesOffsetA + p];
				//bvhInfoCPU[bvhA].m_quantization
				b3Vector3 treeAminLocal = MyUnQuantize(subtreeA.m_quantizedAabbMin, bvhInfoCPU[bvhA].m_quantization, bvhInfoCPU[bvhA].m_aabbMin);
				b3Vector3 treeAmaxLocal = MyUnQuantize(subtreeA.m_quantizedAabbMax, bvhInfoCPU[bvhA].m_quantization, bvhInfoCPU[bvhA].m_aabbMin);

				b3Vector3 aabbAMinOut, aabbAMaxOut;
				float margin = 0.f;
				b3TransformAabb2(treeAminLocal, treeAmaxLocal, margin, transA.getOrigin(), transA.getRotation(), &aabbAMinOut, &aabbAMaxOut);

				for (int q = 0; q < numSubTreesB; q++)
				{
					b3BvhSubtreeInfo subtreeB = subTreesCPU[subTreesOffsetB + q];

					b3Vector3 treeBminLocal = MyUnQuantize(subtreeB.m_quantizedAabbMin, bvhInfoCPU[bvhB].m_quantization, bvhInfoCPU[bvhB].m_aabbMin);
					b3Vector3 treeBmaxLocal = MyUnQuantize(subtreeB.m_quantizedAabbMax, bvhInfoCPU[bvhB].m_quantization, bvhInfoCPU[bvhB].m_aabbMin);

					b3Vector3 aabbBMinOut, aabbBMaxOut;
					float margin = 0.f;
					b3TransformAabb2(treeBminLocal, treeBmaxLocal, margin, transB.getOrigin(), transB.getRotation(), &aabbBMinOut, &aabbBMaxOut);

					numAabbChecks = 0;
					bool aabbOverlap = b3TestAabbAgainstAabb(aabbAMinOut, aabbAMaxOut, aabbBMinOut, aabbBMaxOut);
					if (aabbOverlap)
					{
						int startNodeIndexA = subtreeA.m_rootNodeIndex + bvhInfoCPU[bvhA].m_nodeOffset;
						//				int endNodeIndexA = startNodeIndexA+subtreeA.m_subtreeSize;

						int startNodeIndexB = subtreeB.m_rootNodeIndex + bvhInfoCPU[bvhB].m_nodeOffset;
						//				int endNodeIndexB = startNodeIndexB+subtreeB.m_subtreeSize;

						b3AlignedObjectArray<b3Int2> nodeStack;
						b3Int2 node0;
						node0.x = startNodeIndexA;
						node0.y = startNodeIndexB;

						int maxStackDepth = 1024;
						nodeStack.resize(maxStackDepth);
						int depth = 0;
						nodeStack[depth++] = node0;

						do
						{
							if (depth > maxDepth)
							{
								maxDepth = depth;
								printf("maxDepth=%d\n", maxDepth);
							}
							b3Int2 node = nodeStack[--depth];

							b3Vector3 aMinLocal = MyUnQuantize(treeNodesCPU[node.x].m_quantizedAabbMin, bvhInfoCPU[bvhA].m_quantization, bvhInfoCPU[bvhA].m_aabbMin);
							b3Vector3 aMaxLocal = MyUnQuantize(treeNodesCPU[node.x].m_quantizedAabbMax, bvhInfoCPU[bvhA].m_quantization, bvhInfoCPU[bvhA].m_aabbMin);

							b3Vector3 bMinLocal = MyUnQuantize(treeNodesCPU[node.y].m_quantizedAabbMin, bvhInfoCPU[bvhB].m_quantization, bvhInfoCPU[bvhB].m_aabbMin);
							b3Vector3 bMaxLocal = MyUnQuantize(treeNodesCPU[node.y].m_quantizedAabbMax, bvhInfoCPU[bvhB].m_quantization, bvhInfoCPU[bvhB].m_aabbMin);

							float margin = 0.f;
							b3Vector3 aabbAMinOut, aabbAMaxOut;
							b3TransformAabb2(aMinLocal, aMaxLocal, margin, transA.getOrigin(), transA.getRotation(), &aabbAMinOut, &aabbAMaxOut);

							b3Vector3 aabbBMinOut, aabbBMaxOut;
							b3TransformAabb2(bMinLocal, bMaxLocal, margin, transB.getOrigin(), transB.getRotation(), &aabbBMinOut, &aabbBMaxOut);

							numAabbChecks++;
							bool nodeOverlap = b3TestAabbAgainstAabb(aabbAMinOut, aabbAMaxOut, aabbBMinOut, aabbBMaxOut);
							if (nodeOverlap)
							{
								bool isLeafA = treeNodesCPU[node.x].isLeafNode();
								bool isLeafB = treeNodesCPU[node.y].isLeafNode();
								bool isInternalA = !isLeafA;
								bool isInternalB = !isLeafB;

								//fail, even though it might hit two leaf nodes
								if (depth + 4 > maxStackDepth && !(isLeafA && isLeafB))
								{
									b3Error("Error: traversal exceeded maxStackDepth\n");
									continue;
								}

								if (isInternalA)
								{
									int nodeAleftChild = node.x + 1;
									bool isNodeALeftChildLeaf = treeNodesCPU[node.x + 1].isLeafNode();
									int nodeArightChild = isNodeALeftChildLeaf ? node.x + 2 : node.x + 1 + treeNodesCPU[node.x + 1].getEscapeIndex();

									if (isInternalB)
									{
										int nodeBleftChild = node.y + 1;
										bool isNodeBLeftChildLeaf = treeNodesCPU[node.y + 1].isLeafNode();
										int nodeBrightChild = isNodeBLeftChildLeaf ? node.y + 2 : node.y + 1 + treeNodesCPU[node.y + 1].getEscapeIndex();

										nodeStack[depth++] = b3MakeInt2(nodeAleftChild, nodeBleftChild);
										nodeStack[depth++] = b3MakeInt2(nodeArightChild, nodeBleftChild);
										nodeStack[depth++] = b3MakeInt2(nodeAleftChild, nodeBrightChild);
										nodeStack[depth++] = b3MakeInt2(nodeArightChild, nodeBrightChild);
									}
									else
									{
										nodeStack[depth++] = b3MakeInt2(nodeAleftChild, node.y);
										nodeStack[depth++] = b3MakeInt2(nodeArightChild, node.y);
									}
								}
								else
								{
									if (isInternalB)
									{
										int nodeBleftChild = node.y + 1;
										bool isNodeBLeftChildLeaf = treeNodesCPU[node.y + 1].isLeafNode();
										int nodeBrightChild = isNodeBLeftChildLeaf ? node.y + 2 : node.y + 1 + treeNodesCPU[node.y + 1].getEscapeIndex();
										nodeStack[depth++] = b3MakeInt2(node.x, nodeBleftChild);
										nodeStack[depth++] = b3MakeInt2(node.x, nodeBrightChild);
									}
									else
									{
										int compoundPairIdx = b3AtomicInc(numCompoundPairsOut);
										if (compoundPairIdx < maxNumCompoundPairsCapacity)
										{
											int childShapeIndexA = treeNodesCPU[node.x].getTriangleIndex();
											int childShapeIndexB = treeNodesCPU[node.y].getTriangleIndex();
											gpuCompoundPairsOut[compoundPairIdx] = b3MakeInt4(bodyIndexA, bodyIndexB, childShapeIndexA, childShapeIndexB);
										}
									}
								}
							}
						} while (depth);
						maxNumAabbChecks = b3Max(numAabbChecks, maxNumAabbChecks);
					}
				}
			}

			return;
		}

		if ((collidables[collidableIndexA].m_shapeType == SHAPE_COMPOUND_OF_CONVEX_HULLS) || (collidables[collidableIndexB].m_shapeType == SHAPE_COMPOUND_OF_CONVEX_HULLS))
		{
			if (collidables[collidableIndexA].m_shapeType == SHAPE_COMPOUND_OF_CONVEX_HULLS)
			{
				int numChildrenA = collidables[collidableIndexA].m_numChildShapes;
				for (int c = 0; c < numChildrenA; c++)
				{
					int childShapeIndexA = collidables[collidableIndexA].m_shapeIndex + c;
					int childColIndexA = gpuChildShapes[childShapeIndexA].m_shapeIndex;

					float4 posA = rigidBodies[bodyIndexA].m_pos;
					b3Quat ornA = rigidBodies[bodyIndexA].m_quat;
					float4 childPosA = gpuChildShapes[childShapeIndexA].m_childPosition;
					b3Quat childOrnA = gpuChildShapes[childShapeIndexA].m_childOrientation;
					float4 newPosA = b3QuatRotate(ornA, childPosA) + posA;
					b3Quat newOrnA = b3QuatMul(ornA, childOrnA);

					b3Aabb aabbA = aabbsLocalSpace[childColIndexA];

					b3Transform transA;
					transA.setIdentity();
					transA.setOrigin(newPosA);
					transA.setRotation(newOrnA);
					b3Scalar margin = 0.0f;

					b3Vector3 aabbAMinOut, aabbAMaxOut;

					b3TransformAabb2((const b3Float4&)aabbA.m_min, (const b3Float4&)aabbA.m_max, margin, transA.getOrigin(), transA.getRotation(), &aabbAMinOut, &aabbAMaxOut);

					if (collidables[collidableIndexB].m_shapeType == SHAPE_COMPOUND_OF_CONVEX_HULLS)
					{
						int numChildrenB = collidables[collidableIndexB].m_numChildShapes;
						for (int b = 0; b < numChildrenB; b++)
						{
							int childShapeIndexB = collidables[collidableIndexB].m_shapeIndex + b;
							int childColIndexB = gpuChildShapes[childShapeIndexB].m_shapeIndex;
							b3Quat ornB = rigidBodies[bodyIndexB].m_quat;
							float4 posB = rigidBodies[bodyIndexB].m_pos;
							float4 childPosB = gpuChildShapes[childShapeIndexB].m_childPosition;
							b3Quat childOrnB = gpuChildShapes[childShapeIndexB].m_childOrientation;
							float4 newPosB = transform(&childPosB, &posB, &ornB);
							b3Quat newOrnB = b3QuatMul(ornB, childOrnB);

							b3Aabb aabbB = aabbsLocalSpace[childColIndexB];

							b3Transform transB;
							transB.setIdentity();
							transB.setOrigin(newPosB);
							transB.setRotation(newOrnB);

							b3Vector3 aabbBMinOut, aabbBMaxOut;
							b3TransformAabb2((const b3Float4&)aabbB.m_min, (const b3Float4&)aabbB.m_max, margin, transB.getOrigin(), transB.getRotation(), &aabbBMinOut, &aabbBMaxOut);

							numAabbChecks++;
							bool aabbOverlap = b3TestAabbAgainstAabb(aabbAMinOut, aabbAMaxOut, aabbBMinOut, aabbBMaxOut);
							if (aabbOverlap)
							{
								/*
								int numFacesA = convexShapes[shapeIndexA].m_numFaces;
								float dmin = FLT_MAX;
								float4 posA = newPosA;
								posA.w = 0.f;
								float4 posB = newPosB;
								posB.w = 0.f;
								float4 c0local = convexShapes[shapeIndexA].m_localCenter;
								b3Quat ornA = newOrnA;
								float4 c0 = transform(&c0local, &posA, &ornA);
								float4 c1local = convexShapes[shapeIndexB].m_localCenter;
								b3Quat ornB =newOrnB;
								float4 c1 = transform(&c1local,&posB,&ornB);
								const float4 DeltaC2 = c0 - c1;
								*/
								{  //
									int compoundPairIdx = b3AtomicInc(numCompoundPairsOut);
									if (compoundPairIdx < maxNumCompoundPairsCapacity)
									{
										gpuCompoundPairsOut[compoundPairIdx] = b3MakeInt4(bodyIndexA, bodyIndexB, childShapeIndexA, childShapeIndexB);
									}
								}  //
							}      //fi(1)
						}          //for (int b=0
					}              //if (collidables[collidableIndexB].
					else           //if (collidables[collidableIndexB].m_shapeType==SHAPE_COMPOUND_OF_CONVEX_HULLS)
					{
						if (1)
						{
							//	int numFacesA = convexShapes[shapeIndexA].m_numFaces;
							//	float dmin = FLT_MAX;
							float4 posA = newPosA;
							posA.w = 0.f;
							float4 posB = rigidBodies[bodyIndexB].m_pos;
							posB.w = 0.f;
							float4 c0local = convexShapes[shapeIndexA].m_localCenter;
							b3Quat ornA = newOrnA;
							float4 c0;
							c0 = transform(&c0local, &posA, &ornA);
							float4 c1local = convexShapes[shapeIndexB].m_localCenter;
							b3Quat ornB = rigidBodies[bodyIndexB].m_quat;
							float4 c1;
							c1 = transform(&c1local, &posB, &ornB);
							//	const float4 DeltaC2 = c0 - c1;

							{
								int compoundPairIdx = b3AtomicInc(numCompoundPairsOut);
								if (compoundPairIdx < maxNumCompoundPairsCapacity)
								{
									gpuCompoundPairsOut[compoundPairIdx] = b3MakeInt4(bodyIndexA, bodyIndexB, childShapeIndexA, -1);
								}  //if (compoundPairIdx<maxNumCompoundPairsCapacity)
							}      //
						}          //fi (1)
					}              //if (collidables[collidableIndexB].m_shapeType==SHAPE_COMPOUND_OF_CONVEX_HULLS)
				}                  //for (int b=0;b<numChildrenB;b++)
				return;
			}  //if (collidables[collidableIndexB].m_shapeType==SHAPE_COMPOUND_OF_CONVEX_HULLS)
			if ((collidables[collidableIndexA].m_shapeType != SHAPE_CONCAVE_TRIMESH) && (collidables[collidableIndexB].m_shapeType == SHAPE_COMPOUND_OF_CONVEX_HULLS))
			{
				int numChildrenB = collidables[collidableIndexB].m_numChildShapes;
				for (int b = 0; b < numChildrenB; b++)
				{
					int childShapeIndexB = collidables[collidableIndexB].m_shapeIndex + b;
					int childColIndexB = gpuChildShapes[childShapeIndexB].m_shapeIndex;
					b3Quat ornB = rigidBodies[bodyIndexB].m_quat;
					float4 posB = rigidBodies[bodyIndexB].m_pos;
					float4 childPosB = gpuChildShapes[childShapeIndexB].m_childPosition;
					b3Quat childOrnB = gpuChildShapes[childShapeIndexB].m_childOrientation;
					float4 newPosB = b3QuatRotate(ornB, childPosB) + posB;
					b3Quat newOrnB = b3QuatMul(ornB, childOrnB);

					int shapeIndexB = collidables[childColIndexB].m_shapeIndex;

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

					if (1)
					{
						//	int numFacesA = convexShapes[shapeIndexA].m_numFaces;
						//	float dmin = FLT_MAX;
						float4 posA = rigidBodies[bodyIndexA].m_pos;
						posA.w = 0.f;
						float4 posB = newPosB;
						posB.w = 0.f;
						float4 c0local = convexShapes[shapeIndexA].m_localCenter;
						b3Quat ornA = rigidBodies[bodyIndexA].m_quat;
						float4 c0;
						c0 = transform(&c0local, &posA, &ornA);
						float4 c1local = convexShapes[shapeIndexB].m_localCenter;
						b3Quat ornB = newOrnB;
						float4 c1;
						c1 = transform(&c1local, &posB, &ornB);
						//	const float4 DeltaC2 = c0 - c1;
						{  //
							int compoundPairIdx = b3AtomicInc(numCompoundPairsOut);
							if (compoundPairIdx < maxNumCompoundPairsCapacity)
							{
								gpuCompoundPairsOut[compoundPairIdx] = b3MakeInt4(bodyIndexA, bodyIndexB, -1, childShapeIndexB);
							}  //fi (compoundPairIdx<maxNumCompoundPairsCapacity)
						}      //
					}          //fi (1)
				}              //for (int b=0;b<numChildrenB;b++)
				return;
			}  //if (collidables[collidableIndexB].m_shapeType==SHAPE_COMPOUND_OF_CONVEX_HULLS)
			return;
		}  //fi ((collidables[collidableIndexA].m_shapeType==SHAPE_COMPOUND_OF_CONVEX_HULLS) ||(collidables[collidableIndexB].m_shapeType==SHAPE_COMPOUND_OF_CONVEX_HULLS))
	}      //i<numPairs
}

__kernel void processCompoundPairsKernel(__global const b3Int4* gpuCompoundPairs,
										 __global const b3RigidBodyData* rigidBodies,
										 __global const b3Collidable* collidables,
										 __global const b3ConvexPolyhedronData* convexShapes,
										 __global const b3AlignedObjectArray<b3Float4>& vertices,
										 __global const b3AlignedObjectArray<b3Float4>& uniqueEdges,
										 __global const b3AlignedObjectArray<b3GpuFace>& faces,
										 __global const b3AlignedObjectArray<int>& indices,
										 __global b3Aabb* aabbs,
										 __global const b3GpuChildShape* gpuChildShapes,
										 __global b3AlignedObjectArray<b3Float4>& gpuCompoundSepNormalsOut,
										 __global b3AlignedObjectArray<int>& gpuHasCompoundSepNormalsOut,
										 int numCompoundPairs,
										 int i)
{
	//	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;

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

		b3Quat 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;
			b3Quat childOrnA = gpuChildShapes[childShapeIndexA].m_childOrientation;
			float4 newPosA = b3QuatRotate(ornA, childPosA) + posA;
			b3Quat newOrnA = b3QuatMul(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;
			b3Quat childOrnB = gpuChildShapes[childShapeIndexB].m_childOrientation;
			float4 newPosB = b3QuatRotate(ornB, childPosB) + posB;
			b3Quat newOrnB = b3QuatMul(ornB, childOrnB);
			posB = newPosB;
			ornB = newOrnB;
		}
		else
		{
			collidableIndexB = rigidBodies[bodyIndexB].m_collidableIdx;
		}

		gpuHasCompoundSepNormalsOut[i] = 0;

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

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

		if ((shapeTypeA != SHAPE_CONVEX_HULL) || (shapeTypeB != SHAPE_CONVEX_HULL))
		{
			return;
		}

		int hasSeparatingAxis = 5;

		//	int numFacesA = convexShapes[shapeIndexA].m_numFaces;
		float dmin = FLT_MAX;
		posA.w = 0.f;
		posB.w = 0.f;
		float4 c0local = convexShapes[shapeIndexA].m_localCenter;
		float4 c0 = transform(&c0local, &posA, &ornA);
		float4 c1local = convexShapes[shapeIndexB].m_localCenter;
		float4 c1 = transform(&c1local, &posB, &ornB);
		const float4 DeltaC2 = c0 - c1;
		float4 sepNormal = make_float4(1, 0, 0, 0);
		//		bool sepA = findSeparatingAxis(	convexShapes[shapeIndexA], convexShapes[shapeIndexB],posA,ornA,posB,ornB,DeltaC2,vertices,uniqueEdges,faces,indices,&sepNormal,&dmin);
		bool sepA = findSeparatingAxis(convexShapes[shapeIndexA], convexShapes[shapeIndexB], posA, ornA, posB, ornB, vertices, uniqueEdges, faces, indices, vertices, uniqueEdges, faces, indices, sepNormal);  //,&dmin);

		hasSeparatingAxis = 4;
		if (!sepA)
		{
			hasSeparatingAxis = 0;
		}
		else
		{
			bool sepB = findSeparatingAxis(convexShapes[shapeIndexB], convexShapes[shapeIndexA], posB, ornB, posA, ornA, vertices, uniqueEdges, faces, indices, vertices, uniqueEdges, faces, indices, sepNormal);  //,&dmin);

			if (!sepB)
			{
				hasSeparatingAxis = 0;
			}
			else  //(!sepB)
			{
				bool sepEE = findSeparatingAxisEdgeEdge(&convexShapes[shapeIndexA], &convexShapes[shapeIndexB], posA, ornA, posB, ornB, DeltaC2, vertices, uniqueEdges, faces, indices, &sepNormal, &dmin);
				if (sepEE)
				{
					gpuCompoundSepNormalsOut[i] = sepNormal;  //fastNormalize4(sepNormal);
					gpuHasCompoundSepNormalsOut[i] = 1;
				}  //sepEE
			}      //(!sepB)
		}          //(!sepA)
	}
}

__kernel void clipCompoundsHullHullKernel(__global const b3Int4* gpuCompoundPairs,
										  __global const b3RigidBodyData* rigidBodies,
										  __global const b3Collidable* collidables,
										  __global const b3ConvexPolyhedronData* convexShapes,
										  __global const b3AlignedObjectArray<b3Float4>& vertices,
										  __global const b3AlignedObjectArray<b3Float4>& uniqueEdges,
										  __global const b3AlignedObjectArray<b3GpuFace>& faces,
										  __global const b3AlignedObjectArray<int>& indices,
										  __global const b3GpuChildShape* gpuChildShapes,
										  __global const b3AlignedObjectArray<b3Float4>& gpuCompoundSepNormalsOut,
										  __global const b3AlignedObjectArray<int>& gpuHasCompoundSepNormalsOut,
										  __global struct b3Contact4Data* globalContactsOut,
										  int* nGlobalContactsOut,
										  int numCompoundPairs, int maxContactCapacity, int i)
{
	//	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.0f;

	if (i < numCompoundPairs)
	{
		if (gpuHasCompoundSepNormalsOut[i])
		{
			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;

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

			b3Quat 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;
				b3Quat childOrnA = gpuChildShapes[childShapeIndexA].m_childOrientation;
				float4 newPosA = b3QuatRotate(ornA, childPosA) + posA;
				b3Quat newOrnA = b3QuatMul(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;
				b3Quat childOrnB = gpuChildShapes[childShapeIndexB].m_childOrientation;
				float4 newPosB = b3QuatRotate(ornB, childPosB) + posB;
				b3Quat newOrnB = b3QuatMul(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 numLocalContactsOut = clipHullAgainstHull(gpuCompoundSepNormalsOut[i],
														  convexShapes[shapeIndexA], convexShapes[shapeIndexB],
														  posA, ornA,
														  posB, ornB,
														  worldVertsB1, worldVertsB2, capacityWorldVerts,
														  minDist, maxDist,
														  vertices, faces, indices,
														  vertices, faces, indices,
														  localContactsOut, localContactCapacity);

			if (numLocalContactsOut > 0)
			{
				float4 normal = -gpuCompoundSepNormalsOut[i];
				int nPoints = numLocalContactsOut;
				float4* pointsIn = localContactsOut;
				b3Int4 contactIdx;  // = {-1,-1,-1,-1};

				contactIdx.s[0] = 0;
				contactIdx.s[1] = 1;
				contactIdx.s[2] = 2;
				contactIdx.s[3] = 3;

				int nReducedContacts = extractManifoldSequentialGlobal(pointsIn, nPoints, normal, &contactIdx);

				int dstIdx;
				dstIdx = b3AtomicInc(nGlobalContactsOut);
				if ((dstIdx + nReducedContacts) < maxContactCapacity)
				{
					__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 = gpuCompoundPairs[pairIndex].x;
					int bodyB = gpuCompoundPairs[pairIndex].y;
					c->m_bodyAPtrAndSignBit = rigidBodies[bodyA].m_invMass == 0 ? -bodyA : bodyA;
					c->m_bodyBPtrAndSignBit = rigidBodies[bodyB].m_invMass == 0 ? -bodyB : bodyB;
					c->m_childIndexA = childShapeIndexA;
					c->m_childIndexB = childShapeIndexB;
					for (int i = 0; i < nReducedContacts; i++)
					{
						c->m_worldPosB[i] = pointsIn[contactIdx.s[i]];
					}
					b3Contact4Data_setNumPoints(c, nReducedContacts);
				}

			}  //		if (numContactsOut>0)
		}      //		if (gpuHasCompoundSepNormalsOut[i])
	}          //	if (i<numCompoundPairs)
}

void computeContactCompoundCompound(int pairIndex,
									int bodyIndexA, int bodyIndexB,
									int collidableIndexA, int collidableIndexB,
									const b3RigidBodyData* rigidBodies,
									const b3Collidable* collidables,
									const b3ConvexPolyhedronData* convexShapes,
									const b3GpuChildShape* cpuChildShapes,
									const b3AlignedObjectArray<b3Aabb>& hostAabbsWorldSpace,
									const b3AlignedObjectArray<b3Aabb>& hostAabbsLocalSpace,

									const b3AlignedObjectArray<b3Vector3>& convexVertices,
									const b3AlignedObjectArray<b3Vector3>& hostUniqueEdges,
									const b3AlignedObjectArray<int>& convexIndices,
									const b3AlignedObjectArray<b3GpuFace>& faces,

									b3Contact4* globalContactsOut,
									int& nGlobalContactsOut,
									int maxContactCapacity,
									b3AlignedObjectArray<b3QuantizedBvhNode>& treeNodesCPU,
									b3AlignedObjectArray<b3BvhSubtreeInfo>& subTreesCPU,
									b3AlignedObjectArray<b3BvhInfo>& bvhInfoCPU)
{
	int shapeTypeB = collidables[collidableIndexB].m_shapeType;
	b3Assert(shapeTypeB == SHAPE_COMPOUND_OF_CONVEX_HULLS);

	b3AlignedObjectArray<b3Int4> cpuCompoundPairsOut;
	int numCompoundPairsOut = 0;
	int maxNumCompoundPairsCapacity = 8192;  //1024;
	cpuCompoundPairsOut.resize(maxNumCompoundPairsCapacity);

	// work-in-progress
	findCompoundPairsKernel(
		pairIndex,
		bodyIndexA, bodyIndexB,
		collidableIndexA, collidableIndexB,
		rigidBodies,
		collidables,
		convexShapes,
		convexVertices,
		hostAabbsWorldSpace,
		hostAabbsLocalSpace,
		cpuChildShapes,
		&cpuCompoundPairsOut[0],
		&numCompoundPairsOut,
		maxNumCompoundPairsCapacity,
		treeNodesCPU,
		subTreesCPU,
		bvhInfoCPU);

	printf("maxNumAabbChecks=%d\n", maxNumAabbChecks);
	if (numCompoundPairsOut > maxNumCompoundPairsCapacity)
	{
		b3Error("numCompoundPairsOut exceeded maxNumCompoundPairsCapacity (%d)\n", maxNumCompoundPairsCapacity);
		numCompoundPairsOut = maxNumCompoundPairsCapacity;
	}
	b3AlignedObjectArray<b3Float4> cpuCompoundSepNormalsOut;
	b3AlignedObjectArray<int> cpuHasCompoundSepNormalsOut;
	cpuCompoundSepNormalsOut.resize(numCompoundPairsOut);
	cpuHasCompoundSepNormalsOut.resize(numCompoundPairsOut);

	for (int i = 0; i < numCompoundPairsOut; i++)
	{
		processCompoundPairsKernel(&cpuCompoundPairsOut[0], rigidBodies, collidables, convexShapes, convexVertices, hostUniqueEdges, faces, convexIndices, 0, cpuChildShapes,
								   cpuCompoundSepNormalsOut, cpuHasCompoundSepNormalsOut, numCompoundPairsOut, i);
	}

	for (int i = 0; i < numCompoundPairsOut; i++)
	{
		clipCompoundsHullHullKernel(&cpuCompoundPairsOut[0], rigidBodies, collidables, convexShapes, convexVertices, hostUniqueEdges, faces, convexIndices, cpuChildShapes,
									cpuCompoundSepNormalsOut, cpuHasCompoundSepNormalsOut, globalContactsOut, &nGlobalContactsOut, numCompoundPairsOut, maxContactCapacity, i);
	}
	/*
		int childColIndexA = gpuChildShapes[childShapeIndexA].m_shapeIndex;

					float4 posA = rigidBodies[bodyIndexA].m_pos;
					b3Quat ornA = rigidBodies[bodyIndexA].m_quat;
					float4 childPosA = gpuChildShapes[childShapeIndexA].m_childPosition;
					b3Quat childOrnA = gpuChildShapes[childShapeIndexA].m_childOrientation;
					float4 newPosA = b3QuatRotate(ornA,childPosA)+posA;
					b3Quat newOrnA = b3QuatMul(ornA,childOrnA);

					int shapeIndexA = collidables[childColIndexA].m_shapeIndex;


			bool foundSepAxis = findSeparatingAxis(hullA,hullB,
							posA,
							ornA,
							posB,
							ornB,

							convexVertices,uniqueEdges,faces,convexIndices,
							convexVertices,uniqueEdges,faces,convexIndices,

							sepNormalWorldSpace
							);
							*/

	/*
	if (foundSepAxis)
	{


		contactIndex = clipHullHullSingle(
			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;

	/*

	int numChildrenB = collidables[collidableIndexB].m_numChildShapes;
	for (int c=0;c<numChildrenB;c++)
	{
		int childShapeIndexB = collidables[collidableIndexB].m_shapeIndex+c;
		int childColIndexB = cpuChildShapes[childShapeIndexB].m_shapeIndex;

		float4 rootPosB = rigidBodies[bodyIndexB].m_pos;
		b3Quaternion rootOrnB = rigidBodies[bodyIndexB].m_quat;
		b3Vector3 childPosB = cpuChildShapes[childShapeIndexB].m_childPosition;
		b3Quaternion childOrnB = cpuChildShapes[childShapeIndexB].m_childOrientation;
		float4  posB = b3QuatRotate(rootOrnB,childPosB)+rootPosB;
		b3Quaternion ornB = b3QuatMul(rootOrnB,childOrnB);//b3QuatMul(ornB,childOrnB);

		int shapeIndexB = collidables[childColIndexB].m_shapeIndex;

		const b3ConvexPolyhedronData* hullB = &convexShapes[shapeIndexB];

	}
	*/
}

void computeContactPlaneCompound(int pairIndex,
								 int bodyIndexA, int bodyIndexB,
								 int collidableIndexA, int collidableIndexB,
								 const b3RigidBodyData* rigidBodies,
								 const b3Collidable* collidables,
								 const b3ConvexPolyhedronData* convexShapes,
								 const b3GpuChildShape* cpuChildShapes,
								 const b3Vector3* convexVertices,
								 const int* convexIndices,
								 const b3GpuFace* faces,

								 b3Contact4* globalContactsOut,
								 int& nGlobalContactsOut,
								 int maxContactCapacity)
{
	int shapeTypeB = collidables[collidableIndexB].m_shapeType;
	b3Assert(shapeTypeB == SHAPE_COMPOUND_OF_CONVEX_HULLS);

	int numChildrenB = collidables[collidableIndexB].m_numChildShapes;
	for (int c = 0; c < numChildrenB; c++)
	{
		int childShapeIndexB = collidables[collidableIndexB].m_shapeIndex + c;
		int childColIndexB = cpuChildShapes[childShapeIndexB].m_shapeIndex;

		float4 rootPosB = rigidBodies[bodyIndexB].m_pos;
		b3Quaternion rootOrnB = rigidBodies[bodyIndexB].m_quat;
		b3Vector3 childPosB = cpuChildShapes[childShapeIndexB].m_childPosition;
		b3Quaternion childOrnB = cpuChildShapes[childShapeIndexB].m_childOrientation;
		float4 posB = b3QuatRotate(rootOrnB, childPosB) + rootPosB;
		b3Quaternion ornB = rootOrnB * childOrnB;  //b3QuatMul(ornB,childOrnB);

		int shapeIndexB = collidables[childColIndexB].m_shapeIndex;

		const b3ConvexPolyhedronData* hullB = &convexShapes[shapeIndexB];

		b3Vector3 posA = rigidBodies[bodyIndexA].m_pos;
		b3Quaternion ornA = rigidBodies[bodyIndexA].m_quat;

		//	int numContactsOut = 0;
		//	int numWorldVertsB1= 0;

		b3Vector3 planeEq = faces[collidables[collidableIndexA].m_shapeIndex].m_plane;
		b3Vector3 planeNormal = b3MakeVector3(planeEq.x, planeEq.y, planeEq.z);
		b3Vector3 planeNormalWorld = b3QuatRotate(ornA, planeNormal);
		float planeConstant = planeEq.w;
		b3Transform convexWorldTransform;
		convexWorldTransform.setIdentity();
		convexWorldTransform.setOrigin(posB);
		convexWorldTransform.setRotation(ornB);
		b3Transform planeTransform;
		planeTransform.setIdentity();
		planeTransform.setOrigin(posA);
		planeTransform.setRotation(ornA);

		b3Transform planeInConvex;
		planeInConvex = convexWorldTransform.inverse() * planeTransform;
		b3Transform convexInPlane;
		convexInPlane = planeTransform.inverse() * convexWorldTransform;

		b3Vector3 planeNormalInConvex = planeInConvex.getBasis() * -planeNormal;
		float maxDot = -1e30;
		int hitVertex = -1;
		b3Vector3 hitVtx;

#define MAX_PLANE_CONVEX_POINTS 64

		b3Vector3 contactPoints[MAX_PLANE_CONVEX_POINTS];
		int numPoints = 0;

		b3Int4 contactIdx;
		contactIdx.s[0] = 0;
		contactIdx.s[1] = 1;
		contactIdx.s[2] = 2;
		contactIdx.s[3] = 3;

		for (int i = 0; i < hullB->m_numVertices; i++)
		{
			b3Vector3 vtx = convexVertices[hullB->m_vertexOffset + i];
			float curDot = vtx.dot(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)
			{
				b3Vector3 vtxWorld = convexWorldTransform * vtx;
				b3Vector3 vtxInPlane = planeTransform.inverse() * vtxWorld;
				float dist = planeNormal.dot(vtxInPlane) - planeConstant;
				if (dist < 0.f)
				{
					vtxWorld.w = dist;
					contactPoints[numPoints] = vtxWorld;
					numPoints++;
				}
			}
		}

		int numReducedPoints = 0;

		numReducedPoints = numPoints;

		if (numPoints > 4)
		{
			numReducedPoints = extractManifoldSequentialGlobal(contactPoints, numPoints, planeNormalInConvex, &contactIdx);
		}
		int dstIdx;
		//    dstIdx = nGlobalContactsOut++;//AppendInc( nGlobalContactsOut, dstIdx );

		if (numReducedPoints > 0)
		{
			if (nGlobalContactsOut < maxContactCapacity)
			{
				dstIdx = nGlobalContactsOut;
				nGlobalContactsOut++;

				b3Contact4* c = &globalContactsOut[dstIdx];
				c->m_worldNormalOnB = -planeNormalWorld;
				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;
				for (int i = 0; i < numReducedPoints; i++)
				{
					b3Vector3 pOnB1 = contactPoints[contactIdx.s[i]];
					c->m_worldPosB[i] = pOnB1;
				}
				c->m_worldNormalOnB.w = (b3Scalar)numReducedPoints;
			}  //if (dstIdx < numPairs)
		}
	}
}

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)
}

int computeContactConvexConvex2(
	int pairIndex,
	int bodyIndexA, int bodyIndexB,
	int collidableIndexA, int collidableIndexB,
	const b3AlignedObjectArray<b3RigidBodyData>& rigidBodies,
	const b3AlignedObjectArray<b3Collidable>& collidables,
	const b3AlignedObjectArray<b3ConvexPolyhedronData>& convexShapes,
	const b3AlignedObjectArray<b3Vector3>& convexVertices,
	const b3AlignedObjectArray<b3Vector3>& uniqueEdges,
	const b3AlignedObjectArray<int>& convexIndices,
	const b3AlignedObjectArray<b3GpuFace>& faces,
	b3AlignedObjectArray<b3Contact4>& globalContactsOut,
	int& nGlobalContactsOut,
	int maxContactCapacity,
	const b3AlignedObjectArray<b3Contact4>& oldContacts)
{
	int contactIndex = -1;
	b3Vector3 posA = rigidBodies[bodyIndexA].m_pos;
	b3Quaternion ornA = rigidBodies[bodyIndexA].m_quat;
	b3Vector3 posB = rigidBodies[bodyIndexB].m_pos;
	b3Quaternion ornB = rigidBodies[bodyIndexB].m_quat;

	b3ConvexPolyhedronData hullA, hullB;

	b3Vector3 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);

	//	int contactCapacity = MAX_VERTS;
	//int numContactsOut=0;

#ifdef _WIN32
	b3Assert(_finite(rigidBodies[bodyIndexA].m_pos.x));
	b3Assert(_finite(rigidBodies[bodyIndexB].m_pos.x));
#endif

	bool foundSepAxis = findSeparatingAxis(hullA, hullB,
										   posA,
										   ornA,
										   posB,
										   ornB,

										   convexVertices, uniqueEdges, faces, convexIndices,
										   convexVertices, uniqueEdges, faces, convexIndices,

										   sepNormalWorldSpace);

	if (foundSepAxis)
	{
		contactIndex = clipHullHullSingle(
			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;
}

void GpuSatCollision::computeConvexConvexContactsGPUSAT(b3OpenCLArray<b3Int4>* pairs, int nPairs,
														const b3OpenCLArray<b3RigidBodyData>* bodyBuf,
														b3OpenCLArray<b3Contact4>* contactOut, int& nContacts,
														const b3OpenCLArray<b3Contact4>* oldContacts,
														int maxContactCapacity,
														int compoundPairCapacity,
														const b3OpenCLArray<b3ConvexPolyhedronData>& convexData,
														const b3OpenCLArray<b3Vector3>& gpuVertices,
														const b3OpenCLArray<b3Vector3>& gpuUniqueEdges,
														const b3OpenCLArray<b3GpuFace>& gpuFaces,
														const b3OpenCLArray<int>& gpuIndices,
														const b3OpenCLArray<b3Collidable>& gpuCollidables,
														const b3OpenCLArray<b3GpuChildShape>& gpuChildShapes,

														const b3OpenCLArray<b3Aabb>& clAabbsWorldSpace,
														const b3OpenCLArray<b3Aabb>& clAabbsLocalSpace,

														b3OpenCLArray<b3Vector3>& worldVertsB1GPU,
														b3OpenCLArray<b3Int4>& clippingFacesOutGPU,
														b3OpenCLArray<b3Vector3>& worldNormalsAGPU,
														b3OpenCLArray<b3Vector3>& worldVertsA1GPU,
														b3OpenCLArray<b3Vector3>& worldVertsB2GPU,
														b3AlignedObjectArray<class b3OptimizedBvh*>& bvhDataUnused,
														b3OpenCLArray<b3QuantizedBvhNode>* treeNodesGPU,
														b3OpenCLArray<b3BvhSubtreeInfo>* subTreesGPU,
														b3OpenCLArray<b3BvhInfo>* bvhInfo,

														int numObjects,
														int maxTriConvexPairCapacity,
														b3OpenCLArray<b3Int4>& triangleConvexPairsOut,
														int& numTriConvexPairsOut)
{
	myframecount++;

	if (!nPairs)
		return;

#ifdef CHECK_ON_HOST

	b3AlignedObjectArray<b3QuantizedBvhNode> treeNodesCPU;
	treeNodesGPU->copyToHost(treeNodesCPU);

	b3AlignedObjectArray<b3BvhSubtreeInfo> subTreesCPU;
	subTreesGPU->copyToHost(subTreesCPU);

	b3AlignedObjectArray<b3BvhInfo> bvhInfoCPU;
	bvhInfo->copyToHost(bvhInfoCPU);

	b3AlignedObjectArray<b3Aabb> hostAabbsWorldSpace;
	clAabbsWorldSpace.copyToHost(hostAabbsWorldSpace);

	b3AlignedObjectArray<b3Aabb> hostAabbsLocalSpace;
	clAabbsLocalSpace.copyToHost(hostAabbsLocalSpace);

	b3AlignedObjectArray<b3Int4> hostPairs;
	pairs->copyToHost(hostPairs);

	b3AlignedObjectArray<b3RigidBodyData> hostBodyBuf;
	bodyBuf->copyToHost(hostBodyBuf);

	b3AlignedObjectArray<b3ConvexPolyhedronData> hostConvexData;
	convexData.copyToHost(hostConvexData);

	b3AlignedObjectArray<b3Vector3> hostVertices;
	gpuVertices.copyToHost(hostVertices);

	b3AlignedObjectArray<b3Vector3> hostUniqueEdges;
	gpuUniqueEdges.copyToHost(hostUniqueEdges);
	b3AlignedObjectArray<b3GpuFace> hostFaces;
	gpuFaces.copyToHost(hostFaces);
	b3AlignedObjectArray<int> hostIndices;
	gpuIndices.copyToHost(hostIndices);
	b3AlignedObjectArray<b3Collidable> hostCollidables;
	gpuCollidables.copyToHost(hostCollidables);

	b3AlignedObjectArray<b3GpuChildShape> cpuChildShapes;
	gpuChildShapes.copyToHost(cpuChildShapes);

	b3AlignedObjectArray<b3Int4> hostTriangleConvexPairs;

	b3AlignedObjectArray<b3Contact4> hostContacts;
	if (nContacts)
	{
		contactOut->copyToHost(hostContacts);
	}

	b3AlignedObjectArray<b3Contact4> oldHostContacts;

	if (oldContacts->size())
	{
		oldContacts->copyToHost(oldHostContacts);
	}

	hostContacts.resize(maxContactCapacity);

	for (int i = 0; i < nPairs; i++)
	{
		int bodyIndexA = hostPairs[i].x;
		int bodyIndexB = hostPairs[i].y;
		int collidableIndexA = hostBodyBuf[bodyIndexA].m_collidableIdx;
		int collidableIndexB = hostBodyBuf[bodyIndexB].m_collidableIdx;

		if (hostCollidables[collidableIndexA].m_shapeType == SHAPE_SPHERE &&
			hostCollidables[collidableIndexB].m_shapeType == SHAPE_CONVEX_HULL)
		{
			computeContactSphereConvex(i, bodyIndexA, bodyIndexB, collidableIndexA, collidableIndexB, &hostBodyBuf[0],
									   &hostCollidables[0], &hostConvexData[0], &hostVertices[0], &hostIndices[0], &hostFaces[0], &hostContacts[0], nContacts, maxContactCapacity);
		}

		if (hostCollidables[collidableIndexA].m_shapeType == SHAPE_CONVEX_HULL &&
			hostCollidables[collidableIndexB].m_shapeType == SHAPE_SPHERE)
		{
			computeContactSphereConvex(i, bodyIndexB, bodyIndexA, collidableIndexB, collidableIndexA, &hostBodyBuf[0],
									   &hostCollidables[0], &hostConvexData[0], &hostVertices[0], &hostIndices[0], &hostFaces[0], &hostContacts[0], nContacts, maxContactCapacity);
			//printf("convex-sphere\n");
		}

		if (hostCollidables[collidableIndexA].m_shapeType == SHAPE_CONVEX_HULL &&
			hostCollidables[collidableIndexB].m_shapeType == SHAPE_PLANE)
		{
			computeContactPlaneConvex(i, bodyIndexB, bodyIndexA, collidableIndexB, collidableIndexA, &hostBodyBuf[0],
									  &hostCollidables[0], &hostConvexData[0], &hostVertices[0], &hostIndices[0], &hostFaces[0], &hostContacts[0], nContacts, maxContactCapacity);
			//			printf("convex-plane\n");
		}

		if (hostCollidables[collidableIndexA].m_shapeType == SHAPE_PLANE &&
			hostCollidables[collidableIndexB].m_shapeType == SHAPE_CONVEX_HULL)
		{
			computeContactPlaneConvex(i, bodyIndexA, bodyIndexB, collidableIndexA, collidableIndexB, &hostBodyBuf[0],
									  &hostCollidables[0], &hostConvexData[0], &hostVertices[0], &hostIndices[0], &hostFaces[0], &hostContacts[0], nContacts, maxContactCapacity);
			//			printf("plane-convex\n");
		}

		if (hostCollidables[collidableIndexA].m_shapeType == SHAPE_COMPOUND_OF_CONVEX_HULLS &&
			hostCollidables[collidableIndexB].m_shapeType == SHAPE_COMPOUND_OF_CONVEX_HULLS)
		{
			computeContactCompoundCompound(i, bodyIndexB, bodyIndexA, collidableIndexB, collidableIndexA, &hostBodyBuf[0],
										   &hostCollidables[0], &hostConvexData[0], &cpuChildShapes[0], hostAabbsWorldSpace, hostAabbsLocalSpace, hostVertices, hostUniqueEdges, hostIndices, hostFaces, &hostContacts[0],
										   nContacts, maxContactCapacity, treeNodesCPU, subTreesCPU, bvhInfoCPU);
			//			printf("convex-plane\n");
		}

		if (hostCollidables[collidableIndexA].m_shapeType == SHAPE_COMPOUND_OF_CONVEX_HULLS &&
			hostCollidables[collidableIndexB].m_shapeType == SHAPE_PLANE)
		{
			computeContactPlaneCompound(i, bodyIndexB, bodyIndexA, collidableIndexB, collidableIndexA, &hostBodyBuf[0],
										&hostCollidables[0], &hostConvexData[0], &cpuChildShapes[0], &hostVertices[0], &hostIndices[0], &hostFaces[0], &hostContacts[0], nContacts, maxContactCapacity);
			//			printf("convex-plane\n");
		}

		if (hostCollidables[collidableIndexA].m_shapeType == SHAPE_PLANE &&
			hostCollidables[collidableIndexB].m_shapeType == SHAPE_COMPOUND_OF_CONVEX_HULLS)
		{
			computeContactPlaneCompound(i, bodyIndexA, bodyIndexB, collidableIndexA, collidableIndexB, &hostBodyBuf[0],
										&hostCollidables[0], &hostConvexData[0], &cpuChildShapes[0], &hostVertices[0], &hostIndices[0], &hostFaces[0], &hostContacts[0], nContacts, maxContactCapacity);
			//			printf("plane-convex\n");
		}

		if (hostCollidables[collidableIndexA].m_shapeType == SHAPE_CONVEX_HULL &&
			hostCollidables[collidableIndexB].m_shapeType == SHAPE_CONVEX_HULL)
		{
			//printf("hostPairs[i].z=%d\n",hostPairs[i].z);
			int contactIndex = computeContactConvexConvex2(i, bodyIndexA, bodyIndexB, collidableIndexA, collidableIndexB, hostBodyBuf, hostCollidables, hostConvexData, hostVertices, hostUniqueEdges, hostIndices, hostFaces, hostContacts, nContacts, maxContactCapacity, oldHostContacts);
			//int contactIndex = computeContactConvexConvex(hostPairs,i,bodyIndexA,bodyIndexB,collidableIndexA,collidableIndexB,hostBodyBuf,hostCollidables,hostConvexData,hostVertices,hostUniqueEdges,hostIndices,hostFaces,hostContacts,nContacts,maxContactCapacity,oldHostContacts);

			if (contactIndex >= 0)
			{
				//				printf("convex convex contactIndex = %d\n",contactIndex);
				hostPairs[i].z = contactIndex;
			}
			//			printf("plane-convex\n");
		}
	}

	if (hostPairs.size())
	{
		pairs->copyFromHost(hostPairs);
	}

	hostContacts.resize(nContacts);
	if (nContacts)
	{
		contactOut->copyFromHost(hostContacts);
	}
	else
	{
		contactOut->resize(0);
	}

	m_totalContactsOut.copyFromHostPointer(&nContacts, 1, 0, true);
	//printf("(HOST) nContacts = %d\n",nContacts);

#else

	{
		if (nPairs)
		{
			m_totalContactsOut.copyFromHostPointer(&nContacts, 1, 0, true);

			B3_PROFILE("primitiveContactsKernel");
			b3BufferInfoCL bInfo[] = {
				b3BufferInfoCL(pairs->getBufferCL(), true),
				b3BufferInfoCL(bodyBuf->getBufferCL(), true),
				b3BufferInfoCL(gpuCollidables.getBufferCL(), true),
				b3BufferInfoCL(convexData.getBufferCL(), true),
				b3BufferInfoCL(gpuVertices.getBufferCL(), true),
				b3BufferInfoCL(gpuUniqueEdges.getBufferCL(), true),
				b3BufferInfoCL(gpuFaces.getBufferCL(), true),
				b3BufferInfoCL(gpuIndices.getBufferCL(), true),
				b3BufferInfoCL(contactOut->getBufferCL()),
				b3BufferInfoCL(m_totalContactsOut.getBufferCL())};

			b3LauncherCL launcher(m_queue, m_primitiveContactsKernel, "m_primitiveContactsKernel");
			launcher.setBuffers(bInfo, sizeof(bInfo) / sizeof(b3BufferInfoCL));
			launcher.setConst(nPairs);
			launcher.setConst(maxContactCapacity);
			int num = nPairs;
			launcher.launch1D(num);
			clFinish(m_queue);

			nContacts = m_totalContactsOut.at(0);
			contactOut->resize(nContacts);
		}
	}

#endif  //CHECK_ON_HOST

	B3_PROFILE("computeConvexConvexContactsGPUSAT");
	// printf("nContacts = %d\n",nContacts);

	m_sepNormals.resize(nPairs);
	m_hasSeparatingNormals.resize(nPairs);

	int concaveCapacity = maxTriConvexPairCapacity;
	m_concaveSepNormals.resize(concaveCapacity);
	m_concaveHasSeparatingNormals.resize(concaveCapacity);
	m_numConcavePairsOut.resize(0);
	m_numConcavePairsOut.push_back(0);

	m_gpuCompoundPairs.resize(compoundPairCapacity);

	m_gpuCompoundSepNormals.resize(compoundPairCapacity);

	m_gpuHasCompoundSepNormals.resize(compoundPairCapacity);

	m_numCompoundPairsOut.resize(0);
	m_numCompoundPairsOut.push_back(0);

	int numCompoundPairs = 0;

	int numConcavePairs = 0;

	{
		clFinish(m_queue);
		if (findSeparatingAxisOnGpu)
		{
			m_dmins.resize(nPairs);
			if (splitSearchSepAxisConvex)
			{
				if (useMprGpu)
				{
					nContacts = m_totalContactsOut.at(0);
					{
						B3_PROFILE("mprPenetrationKernel");
						b3BufferInfoCL bInfo[] = {
							b3BufferInfoCL(pairs->getBufferCL(), true),
							b3BufferInfoCL(bodyBuf->getBufferCL(), true),
							b3BufferInfoCL(gpuCollidables.getBufferCL(), true),
							b3BufferInfoCL(convexData.getBufferCL(), true),
							b3BufferInfoCL(gpuVertices.getBufferCL(), true),
							b3BufferInfoCL(m_sepNormals.getBufferCL()),
							b3BufferInfoCL(m_hasSeparatingNormals.getBufferCL()),
							b3BufferInfoCL(contactOut->getBufferCL()),
							b3BufferInfoCL(m_totalContactsOut.getBufferCL())};

						b3LauncherCL launcher(m_queue, m_mprPenetrationKernel, "mprPenetrationKernel");
						launcher.setBuffers(bInfo, sizeof(bInfo) / sizeof(b3BufferInfoCL));

						launcher.setConst(maxContactCapacity);
						launcher.setConst(nPairs);

						int num = nPairs;
						launcher.launch1D(num);
						clFinish(m_queue);
						/*
						b3AlignedObjectArray<int>hostHasSepAxis;
						m_hasSeparatingNormals.copyToHost(hostHasSepAxis);
						b3AlignedObjectArray<b3Vector3>hostSepAxis;
						m_sepNormals.copyToHost(hostSepAxis);
						*/
						nContacts = m_totalContactsOut.at(0);
						contactOut->resize(nContacts);
						//	printf("nContacts (after mprPenetrationKernel) = %d\n",nContacts);
						if (nContacts > maxContactCapacity)
						{
							b3Error("Error: contacts exceeds capacity (%d/%d)\n", nContacts, maxContactCapacity);
							nContacts = maxContactCapacity;
						}
					}
				}

				if (1)
				{
					if (1)
					{
						{
							B3_PROFILE("findSeparatingAxisVertexFaceKernel");
							b3BufferInfoCL bInfo[] = {
								b3BufferInfoCL(pairs->getBufferCL(), true),
								b3BufferInfoCL(bodyBuf->getBufferCL(), true),
								b3BufferInfoCL(gpuCollidables.getBufferCL(), true),
								b3BufferInfoCL(convexData.getBufferCL(), true),
								b3BufferInfoCL(gpuVertices.getBufferCL(), true),
								b3BufferInfoCL(gpuUniqueEdges.getBufferCL(), true),
								b3BufferInfoCL(gpuFaces.getBufferCL(), true),
								b3BufferInfoCL(gpuIndices.getBufferCL(), true),
								b3BufferInfoCL(clAabbsWorldSpace.getBufferCL(), true),
								b3BufferInfoCL(m_sepNormals.getBufferCL()),
								b3BufferInfoCL(m_hasSeparatingNormals.getBufferCL()),
								b3BufferInfoCL(m_dmins.getBufferCL())};

							b3LauncherCL launcher(m_queue, m_findSeparatingAxisVertexFaceKernel, "findSeparatingAxisVertexFaceKernel");
							launcher.setBuffers(bInfo, sizeof(bInfo) / sizeof(b3BufferInfoCL));
							launcher.setConst(nPairs);

							int num = nPairs;
							launcher.launch1D(num);
							clFinish(m_queue);
						}

						int numDirections = sizeof(unitSphere162) / sizeof(b3Vector3);

						{
							B3_PROFILE("findSeparatingAxisEdgeEdgeKernel");
							b3BufferInfoCL bInfo[] = {
								b3BufferInfoCL(pairs->getBufferCL(), true),
								b3BufferInfoCL(bodyBuf->getBufferCL(), true),
								b3BufferInfoCL(gpuCollidables.getBufferCL(), true),
								b3BufferInfoCL(convexData.getBufferCL(), true),
								b3BufferInfoCL(gpuVertices.getBufferCL(), true),
								b3BufferInfoCL(gpuUniqueEdges.getBufferCL(), true),
								b3BufferInfoCL(gpuFaces.getBufferCL(), true),
								b3BufferInfoCL(gpuIndices.getBufferCL(), true),
								b3BufferInfoCL(clAabbsWorldSpace.getBufferCL(), true),
								b3BufferInfoCL(m_sepNormals.getBufferCL()),
								b3BufferInfoCL(m_hasSeparatingNormals.getBufferCL()),
								b3BufferInfoCL(m_dmins.getBufferCL()),
								b3BufferInfoCL(m_unitSphereDirections.getBufferCL(), true)

							};

							b3LauncherCL launcher(m_queue, m_findSeparatingAxisEdgeEdgeKernel, "findSeparatingAxisEdgeEdgeKernel");
							launcher.setBuffers(bInfo, sizeof(bInfo) / sizeof(b3BufferInfoCL));
							launcher.setConst(numDirections);
							launcher.setConst(nPairs);
							int num = nPairs;
							launcher.launch1D(num);
							clFinish(m_queue);
						}
					}
					if (useMprGpu)
					{
						B3_PROFILE("findSeparatingAxisUnitSphereKernel");
						b3BufferInfoCL bInfo[] = {
							b3BufferInfoCL(pairs->getBufferCL(), true),
							b3BufferInfoCL(bodyBuf->getBufferCL(), true),
							b3BufferInfoCL(gpuCollidables.getBufferCL(), true),
							b3BufferInfoCL(convexData.getBufferCL(), true),
							b3BufferInfoCL(gpuVertices.getBufferCL(), true),
							b3BufferInfoCL(m_unitSphereDirections.getBufferCL(), true),
							b3BufferInfoCL(m_sepNormals.getBufferCL()),
							b3BufferInfoCL(m_hasSeparatingNormals.getBufferCL()),
							b3BufferInfoCL(m_dmins.getBufferCL())};

						b3LauncherCL launcher(m_queue, m_findSeparatingAxisUnitSphereKernel, "findSeparatingAxisUnitSphereKernel");
						launcher.setBuffers(bInfo, sizeof(bInfo) / sizeof(b3BufferInfoCL));
						int numDirections = sizeof(unitSphere162) / sizeof(b3Vector3);
						launcher.setConst(numDirections);

						launcher.setConst(nPairs);

						int num = nPairs;
						launcher.launch1D(num);
						clFinish(m_queue);
					}
				}
			}
			else
			{
				B3_PROFILE("findSeparatingAxisKernel");
				b3BufferInfoCL bInfo[] = {
					b3BufferInfoCL(pairs->getBufferCL(), true),
					b3BufferInfoCL(bodyBuf->getBufferCL(), true),
					b3BufferInfoCL(gpuCollidables.getBufferCL(), true),
					b3BufferInfoCL(convexData.getBufferCL(), true),
					b3BufferInfoCL(gpuVertices.getBufferCL(), true),
					b3BufferInfoCL(gpuUniqueEdges.getBufferCL(), true),
					b3BufferInfoCL(gpuFaces.getBufferCL(), true),
					b3BufferInfoCL(gpuIndices.getBufferCL(), true),
					b3BufferInfoCL(clAabbsWorldSpace.getBufferCL(), true),
					b3BufferInfoCL(m_sepNormals.getBufferCL()),
					b3BufferInfoCL(m_hasSeparatingNormals.getBufferCL())};

				b3LauncherCL launcher(m_queue, m_findSeparatingAxisKernel, "m_findSeparatingAxisKernel");
				launcher.setBuffers(bInfo, sizeof(bInfo) / sizeof(b3BufferInfoCL));
				launcher.setConst(nPairs);

				int num = nPairs;
				launcher.launch1D(num);
				clFinish(m_queue);
			}
		}
		else
		{
			B3_PROFILE("findSeparatingAxisKernel CPU");

			b3AlignedObjectArray<b3Int4> hostPairs;
			pairs->copyToHost(hostPairs);
			b3AlignedObjectArray<b3RigidBodyData> hostBodyBuf;
			bodyBuf->copyToHost(hostBodyBuf);

			b3AlignedObjectArray<b3Collidable> hostCollidables;
			gpuCollidables.copyToHost(hostCollidables);

			b3AlignedObjectArray<b3GpuChildShape> cpuChildShapes;
			gpuChildShapes.copyToHost(cpuChildShapes);

			b3AlignedObjectArray<b3ConvexPolyhedronData> hostConvexShapeData;
			convexData.copyToHost(hostConvexShapeData);

			b3AlignedObjectArray<b3Vector3> hostVertices;
			gpuVertices.copyToHost(hostVertices);

			b3AlignedObjectArray<int> hostHasSepAxis;
			hostHasSepAxis.resize(nPairs);
			b3AlignedObjectArray<b3Vector3> hostSepAxis;
			hostSepAxis.resize(nPairs);

			b3AlignedObjectArray<b3Vector3> hostUniqueEdges;
			gpuUniqueEdges.copyToHost(hostUniqueEdges);
			b3AlignedObjectArray<b3GpuFace> hostFaces;
			gpuFaces.copyToHost(hostFaces);

			b3AlignedObjectArray<int> hostIndices;
			gpuIndices.copyToHost(hostIndices);

			b3AlignedObjectArray<b3Contact4> hostContacts;
			if (nContacts)
			{
				contactOut->copyToHost(hostContacts);
			}
			hostContacts.resize(maxContactCapacity);
			int nGlobalContactsOut = nContacts;

			for (int i = 0; i < nPairs; i++)
			{
				int bodyIndexA = hostPairs[i].x;
				int bodyIndexB = hostPairs[i].y;
				int collidableIndexA = hostBodyBuf[bodyIndexA].m_collidableIdx;
				int collidableIndexB = hostBodyBuf[bodyIndexB].m_collidableIdx;

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

				hostHasSepAxis[i] = 0;

				//once the broadphase avoids static-static pairs, we can remove this test
				if ((hostBodyBuf[bodyIndexA].m_invMass == 0) && (hostBodyBuf[bodyIndexB].m_invMass == 0))
				{
					continue;
				}

				if ((hostCollidables[collidableIndexA].m_shapeType != SHAPE_CONVEX_HULL) || (hostCollidables[collidableIndexB].m_shapeType != SHAPE_CONVEX_HULL))
				{
					continue;
				}

				float dmin = FLT_MAX;

				b3ConvexPolyhedronData* convexShapeA = &hostConvexShapeData[shapeIndexA];
				b3ConvexPolyhedronData* convexShapeB = &hostConvexShapeData[shapeIndexB];
				b3Vector3 posA = hostBodyBuf[bodyIndexA].m_pos;
				b3Vector3 posB = hostBodyBuf[bodyIndexB].m_pos;
				b3Quaternion ornA = hostBodyBuf[bodyIndexA].m_quat;
				b3Quaternion ornB = hostBodyBuf[bodyIndexB].m_quat;

				if (useGjk)
				{
					//first approximate the separating axis, to 'fail-proof' GJK+EPA or MPR
					{
						b3Vector3 c0local = hostConvexShapeData[shapeIndexA].m_localCenter;
						b3Vector3 c0 = b3TransformPoint(c0local, posA, ornA);
						b3Vector3 c1local = hostConvexShapeData[shapeIndexB].m_localCenter;
						b3Vector3 c1 = b3TransformPoint(c1local, posB, ornB);
						b3Vector3 DeltaC2 = c0 - c1;

						b3Vector3 sepAxis;

						bool hasSepAxisA = b3FindSeparatingAxis(convexShapeA, convexShapeB, posA, ornA, posB, ornB, DeltaC2,
																&hostVertices.at(0), &hostUniqueEdges.at(0), &hostFaces.at(0), &hostIndices.at(0),
																&hostVertices.at(0), &hostUniqueEdges.at(0), &hostFaces.at(0), &hostIndices.at(0),
																&sepAxis, &dmin);

						if (hasSepAxisA)
						{
							bool hasSepAxisB = b3FindSeparatingAxis(convexShapeB, convexShapeA, posB, ornB, posA, ornA, DeltaC2,
																	&hostVertices.at(0), &hostUniqueEdges.at(0), &hostFaces.at(0), &hostIndices.at(0),
																	&hostVertices.at(0), &hostUniqueEdges.at(0), &hostFaces.at(0), &hostIndices.at(0),
																	&sepAxis, &dmin);
							if (hasSepAxisB)
							{
								bool hasEdgeEdge = b3FindSeparatingAxisEdgeEdge(convexShapeA, convexShapeB, posA, ornA, posB, ornB, DeltaC2,
																				&hostVertices.at(0), &hostUniqueEdges.at(0), &hostFaces.at(0), &hostIndices.at(0),
																				&hostVertices.at(0), &hostUniqueEdges.at(0), &hostFaces.at(0), &hostIndices.at(0),
																				&sepAxis, &dmin, false);

								if (hasEdgeEdge)
								{
									hostHasSepAxis[i] = 1;
									hostSepAxis[i] = sepAxis;
									hostSepAxis[i].w = dmin;
								}
							}
						}
					}

					if (hostHasSepAxis[i])
					{
						int pairIndex = i;

						bool useMpr = true;
						if (useMpr)
						{
							int res = 0;
							float depth = 0.f;
							b3Vector3 sepAxis2 = b3MakeVector3(1, 0, 0);
							b3Vector3 resultPointOnBWorld = b3MakeVector3(0, 0, 0);

							float depthOut;
							b3Vector3 dirOut;
							b3Vector3 posOut;

							//res = b3MprPenetration(bodyIndexA,bodyIndexB,hostBodyBuf,hostConvexShapeData,hostCollidables,hostVertices,&mprConfig,&depthOut,&dirOut,&posOut);
							res = b3MprPenetration(pairIndex, bodyIndexA, bodyIndexB, &hostBodyBuf[0], &hostConvexShapeData[0], &hostCollidables[0], &hostVertices[0], &hostSepAxis[0], &hostHasSepAxis[0], &depthOut, &dirOut, &posOut);
							depth = depthOut;
							sepAxis2 = b3MakeVector3(-dirOut.x, -dirOut.y, -dirOut.z);
							resultPointOnBWorld = posOut;
							//hostHasSepAxis[i] = 0;

							if (res == 0)
							{
								//add point?
								//printf("depth = %f\n",depth);
								//printf("normal = %f,%f,%f\n",dir.v[0],dir.v[1],dir.v[2]);
								//qprintf("pos = %f,%f,%f\n",pos.v[0],pos.v[1],pos.v[2]);

								float dist = 0.f;

								const b3ConvexPolyhedronData& hullA = hostConvexShapeData[hostCollidables[hostBodyBuf[bodyIndexA].m_collidableIdx].m_shapeIndex];
								const b3ConvexPolyhedronData& hullB = hostConvexShapeData[hostCollidables[hostBodyBuf[bodyIndexB].m_collidableIdx].m_shapeIndex];

								if (b3TestSepAxis(&hullA, &hullB, posA, ornA, posB, ornB, &sepAxis2, &hostVertices[0], &hostVertices[0], &dist))
								{
									if (depth > dist)
									{
										float diff = depth - dist;

										static float maxdiff = 0.f;
										if (maxdiff < diff)
										{
											maxdiff = diff;
											printf("maxdiff = %20.10f\n", maxdiff);
										}
									}
								}
								if (depth > dmin)
								{
									b3Vector3 oldAxis = hostSepAxis[i];
									depth = dmin;
									sepAxis2 = oldAxis;
								}

								if (b3TestSepAxis(&hullA, &hullB, posA, ornA, posB, ornB, &sepAxis2, &hostVertices[0], &hostVertices[0], &dist))
								{
									if (depth > dist)
									{
										float diff = depth - dist;
										//printf("?diff  = %f\n",diff );
										static float maxdiff = 0.f;
										if (maxdiff < diff)
										{
											maxdiff = diff;
											printf("maxdiff = %20.10f\n", maxdiff);
										}
									}
									//this is used for SAT
									//hostHasSepAxis[i] = 1;
									//hostSepAxis[i] = sepAxis2;

									//add contact point

									//int contactIndex = nGlobalContactsOut;
									b3Contact4& newContact = hostContacts.at(nGlobalContactsOut);
									nGlobalContactsOut++;
									newContact.m_batchIdx = 0;  //i;
									newContact.m_bodyAPtrAndSignBit = (hostBodyBuf.at(bodyIndexA).m_invMass == 0) ? -bodyIndexA : bodyIndexA;
									newContact.m_bodyBPtrAndSignBit = (hostBodyBuf.at(bodyIndexB).m_invMass == 0) ? -bodyIndexB : bodyIndexB;

									newContact.m_frictionCoeffCmp = 45874;
									newContact.m_restituitionCoeffCmp = 0;

									static float maxDepth = 0.f;

									if (depth > maxDepth)
									{
										maxDepth = depth;
										printf("MPR maxdepth = %f\n", maxDepth);
									}

									resultPointOnBWorld.w = -depth;
									newContact.m_worldPosB[0] = resultPointOnBWorld;
									//b3Vector3 resultPointOnAWorld = resultPointOnBWorld+depth*sepAxis2;
									newContact.m_worldNormalOnB = sepAxis2;
									newContact.m_worldNormalOnB.w = (b3Scalar)1;
								}
								else
								{
									printf("rejected\n");
								}
							}
						}
						else
						{
							//int contactIndex = computeContactConvexConvex2(           i,bodyIndexA,bodyIndexB,collidableIndexA,collidableIndexB,hostBodyBuf, hostCollidables,hostConvexData,hostVertices,hostUniqueEdges,hostIndices,hostFaces,hostContacts,nContacts,maxContactCapacity,oldHostContacts);
							b3AlignedObjectArray<b3Contact4> oldHostContacts;
							int result;
							result = computeContactConvexConvex2(  //hostPairs,
								pairIndex,
								bodyIndexA, bodyIndexB,
								collidableIndexA, collidableIndexB,
								hostBodyBuf,
								hostCollidables,
								hostConvexShapeData,
								hostVertices,
								hostUniqueEdges,
								hostIndices,
								hostFaces,
								hostContacts,
								nGlobalContactsOut,
								maxContactCapacity,
								oldHostContacts
								//hostHasSepAxis,
								//hostSepAxis

							);
						}  //mpr
					}      //hostHasSepAxis[i] = 1;
				}
				else
				{
					b3Vector3 c0local = hostConvexShapeData[shapeIndexA].m_localCenter;
					b3Vector3 c0 = b3TransformPoint(c0local, posA, ornA);
					b3Vector3 c1local = hostConvexShapeData[shapeIndexB].m_localCenter;
					b3Vector3 c1 = b3TransformPoint(c1local, posB, ornB);
					b3Vector3 DeltaC2 = c0 - c1;

					b3Vector3 sepAxis;

					bool hasSepAxisA = b3FindSeparatingAxis(convexShapeA, convexShapeB, posA, ornA, posB, ornB, DeltaC2,
															&hostVertices.at(0), &hostUniqueEdges.at(0), &hostFaces.at(0), &hostIndices.at(0),
															&hostVertices.at(0), &hostUniqueEdges.at(0), &hostFaces.at(0), &hostIndices.at(0),
															&sepAxis, &dmin);

					if (hasSepAxisA)
					{
						bool hasSepAxisB = b3FindSeparatingAxis(convexShapeB, convexShapeA, posB, ornB, posA, ornA, DeltaC2,
																&hostVertices.at(0), &hostUniqueEdges.at(0), &hostFaces.at(0), &hostIndices.at(0),
																&hostVertices.at(0), &hostUniqueEdges.at(0), &hostFaces.at(0), &hostIndices.at(0),
																&sepAxis, &dmin);
						if (hasSepAxisB)
						{
							bool hasEdgeEdge = b3FindSeparatingAxisEdgeEdge(convexShapeA, convexShapeB, posA, ornA, posB, ornB, DeltaC2,
																			&hostVertices.at(0), &hostUniqueEdges.at(0), &hostFaces.at(0), &hostIndices.at(0),
																			&hostVertices.at(0), &hostUniqueEdges.at(0), &hostFaces.at(0), &hostIndices.at(0),
																			&sepAxis, &dmin, true);

							if (hasEdgeEdge)
							{
								hostHasSepAxis[i] = 1;
								hostSepAxis[i] = sepAxis;
							}
						}
					}
				}
			}

			if (useGjkContacts)  //nGlobalContactsOut>0)
			{
				//printf("nGlobalContactsOut=%d\n",nGlobalContactsOut);
				nContacts = nGlobalContactsOut;
				contactOut->copyFromHost(hostContacts);

				m_totalContactsOut.copyFromHostPointer(&nContacts, 1, 0, true);
			}

			m_hasSeparatingNormals.copyFromHost(hostHasSepAxis);
			m_sepNormals.copyFromHost(hostSepAxis);

			/*
             //double-check results from GPU (comment-out the 'else' so both paths are executed
            b3AlignedObjectArray<int> checkHasSepAxis;
            m_hasSeparatingNormals.copyToHost(checkHasSepAxis);
            static int frameCount = 0;
            frameCount++;
            for (int i=0;i<nPairs;i++)
            {
                if (hostHasSepAxis[i] != checkHasSepAxis[i])
                {
                    printf("at frameCount %d hostHasSepAxis[%d] = %d but checkHasSepAxis[i] = %d\n",
                           frameCount,i,hostHasSepAxis[i],checkHasSepAxis[i]);
                }
            }
            //m_hasSeparatingNormals.copyFromHost(hostHasSepAxis);
            //    m_sepNormals.copyFromHost(hostSepAxis);
            */
		}

		numCompoundPairs = m_numCompoundPairsOut.at(0);
		bool useGpuFindCompoundPairs = true;
		if (useGpuFindCompoundPairs)
		{
			B3_PROFILE("findCompoundPairsKernel");
			b3BufferInfoCL bInfo[] =
				{
					b3BufferInfoCL(pairs->getBufferCL(), true),
					b3BufferInfoCL(bodyBuf->getBufferCL(), true),
					b3BufferInfoCL(gpuCollidables.getBufferCL(), true),
					b3BufferInfoCL(convexData.getBufferCL(), true),
					b3BufferInfoCL(gpuVertices.getBufferCL(), true),
					b3BufferInfoCL(gpuUniqueEdges.getBufferCL(), true),
					b3BufferInfoCL(gpuFaces.getBufferCL(), true),
					b3BufferInfoCL(gpuIndices.getBufferCL(), true),
					b3BufferInfoCL(clAabbsLocalSpace.getBufferCL(), true),
					b3BufferInfoCL(gpuChildShapes.getBufferCL(), true),
					b3BufferInfoCL(m_gpuCompoundPairs.getBufferCL()),
					b3BufferInfoCL(m_numCompoundPairsOut.getBufferCL()),
					b3BufferInfoCL(subTreesGPU->getBufferCL()),
					b3BufferInfoCL(treeNodesGPU->getBufferCL()),
					b3BufferInfoCL(bvhInfo->getBufferCL())};

			b3LauncherCL launcher(m_queue, m_findCompoundPairsKernel, "m_findCompoundPairsKernel");
			launcher.setBuffers(bInfo, sizeof(bInfo) / sizeof(b3BufferInfoCL));
			launcher.setConst(nPairs);
			launcher.setConst(compoundPairCapacity);

			int num = nPairs;
			launcher.launch1D(num);
			clFinish(m_queue);

			numCompoundPairs = m_numCompoundPairsOut.at(0);
			//printf("numCompoundPairs =%d\n",numCompoundPairs );
			if (numCompoundPairs)
			{
				//printf("numCompoundPairs=%d\n",numCompoundPairs);
			}
		}
		else
		{
			b3AlignedObjectArray<b3QuantizedBvhNode> treeNodesCPU;
			treeNodesGPU->copyToHost(treeNodesCPU);

			b3AlignedObjectArray<b3BvhSubtreeInfo> subTreesCPU;
			subTreesGPU->copyToHost(subTreesCPU);

			b3AlignedObjectArray<b3BvhInfo> bvhInfoCPU;
			bvhInfo->copyToHost(bvhInfoCPU);

			b3AlignedObjectArray<b3Aabb> hostAabbsWorldSpace;
			clAabbsWorldSpace.copyToHost(hostAabbsWorldSpace);

			b3AlignedObjectArray<b3Aabb> hostAabbsLocalSpace;
			clAabbsLocalSpace.copyToHost(hostAabbsLocalSpace);

			b3AlignedObjectArray<b3Int4> hostPairs;
			pairs->copyToHost(hostPairs);

			b3AlignedObjectArray<b3RigidBodyData> hostBodyBuf;
			bodyBuf->copyToHost(hostBodyBuf);

			b3AlignedObjectArray<b3Int4> cpuCompoundPairsOut;
			cpuCompoundPairsOut.resize(compoundPairCapacity);

			b3AlignedObjectArray<b3Collidable> hostCollidables;
			gpuCollidables.copyToHost(hostCollidables);

			b3AlignedObjectArray<b3GpuChildShape> cpuChildShapes;
			gpuChildShapes.copyToHost(cpuChildShapes);

			b3AlignedObjectArray<b3ConvexPolyhedronData> hostConvexData;
			convexData.copyToHost(hostConvexData);

			b3AlignedObjectArray<b3Vector3> hostVertices;
			gpuVertices.copyToHost(hostVertices);

			for (int pairIndex = 0; pairIndex < nPairs; pairIndex++)
			{
				int bodyIndexA = hostPairs[pairIndex].x;
				int bodyIndexB = hostPairs[pairIndex].y;
				int collidableIndexA = hostBodyBuf[bodyIndexA].m_collidableIdx;
				int collidableIndexB = hostBodyBuf[bodyIndexB].m_collidableIdx;
				if (cpuChildShapes.size())
				{
					findCompoundPairsKernel(
						pairIndex,
						bodyIndexA,
						bodyIndexB,
						collidableIndexA,
						collidableIndexB,
						&hostBodyBuf[0],
						&hostCollidables[0],
						&hostConvexData[0],
						hostVertices,
						hostAabbsWorldSpace,
						hostAabbsLocalSpace,
						&cpuChildShapes[0],
						&cpuCompoundPairsOut[0],
						&numCompoundPairs,
						compoundPairCapacity,
						treeNodesCPU,
						subTreesCPU,
						bvhInfoCPU);
				}
			}

			m_numCompoundPairsOut.copyFromHostPointer(&numCompoundPairs, 1, 0, true);
			if (numCompoundPairs)
			{
				b3CompoundOverlappingPair* ptr = (b3CompoundOverlappingPair*)&cpuCompoundPairsOut[0];
				m_gpuCompoundPairs.copyFromHostPointer(ptr, numCompoundPairs, 0, true);
			}
			//cpuCompoundPairsOut
		}
		if (numCompoundPairs)
		{
			printf("numCompoundPairs=%d\n", numCompoundPairs);
		}

		if (numCompoundPairs > compoundPairCapacity)
		{
			b3Error("Exceeded compound pair capacity (%d/%d)\n", numCompoundPairs, compoundPairCapacity);
			numCompoundPairs = compoundPairCapacity;
		}

		m_gpuCompoundPairs.resize(numCompoundPairs);
		m_gpuHasCompoundSepNormals.resize(numCompoundPairs);
		m_gpuCompoundSepNormals.resize(numCompoundPairs);

		if (numCompoundPairs)
		{
			B3_PROFILE("processCompoundPairsPrimitivesKernel");
			b3BufferInfoCL bInfo[] =
				{
					b3BufferInfoCL(m_gpuCompoundPairs.getBufferCL(), true),
					b3BufferInfoCL(bodyBuf->getBufferCL(), true),
					b3BufferInfoCL(gpuCollidables.getBufferCL(), true),
					b3BufferInfoCL(convexData.getBufferCL(), true),
					b3BufferInfoCL(gpuVertices.getBufferCL(), true),
					b3BufferInfoCL(gpuUniqueEdges.getBufferCL(), true),
					b3BufferInfoCL(gpuFaces.getBufferCL(), true),
					b3BufferInfoCL(gpuIndices.getBufferCL(), true),
					b3BufferInfoCL(clAabbsWorldSpace.getBufferCL(), true),
					b3BufferInfoCL(gpuChildShapes.getBufferCL(), true),
					b3BufferInfoCL(contactOut->getBufferCL()),
					b3BufferInfoCL(m_totalContactsOut.getBufferCL())};

			b3LauncherCL launcher(m_queue, m_processCompoundPairsPrimitivesKernel, "m_processCompoundPairsPrimitivesKernel");
			launcher.setBuffers(bInfo, sizeof(bInfo) / sizeof(b3BufferInfoCL));
			launcher.setConst(numCompoundPairs);
			launcher.setConst(maxContactCapacity);

			int num = numCompoundPairs;
			launcher.launch1D(num);
			clFinish(m_queue);
			nContacts = m_totalContactsOut.at(0);
			//printf("nContacts (after processCompoundPairsPrimitivesKernel) = %d\n",nContacts);
			if (nContacts > maxContactCapacity)
			{
				b3Error("Error: contacts exceeds capacity (%d/%d)\n", nContacts, maxContactCapacity);
				nContacts = maxContactCapacity;
			}
		}

		if (numCompoundPairs)
		{
			B3_PROFILE("processCompoundPairsKernel");
			b3BufferInfoCL bInfo[] =
				{
					b3BufferInfoCL(m_gpuCompoundPairs.getBufferCL(), true),
					b3BufferInfoCL(bodyBuf->getBufferCL(), true),
					b3BufferInfoCL(gpuCollidables.getBufferCL(), true),
					b3BufferInfoCL(convexData.getBufferCL(), true),
					b3BufferInfoCL(gpuVertices.getBufferCL(), true),
					b3BufferInfoCL(gpuUniqueEdges.getBufferCL(), true),
					b3BufferInfoCL(gpuFaces.getBufferCL(), true),
					b3BufferInfoCL(gpuIndices.getBufferCL(), true),
					b3BufferInfoCL(clAabbsWorldSpace.getBufferCL(), true),
					b3BufferInfoCL(gpuChildShapes.getBufferCL(), true),
					b3BufferInfoCL(m_gpuCompoundSepNormals.getBufferCL()),
					b3BufferInfoCL(m_gpuHasCompoundSepNormals.getBufferCL())};

			b3LauncherCL launcher(m_queue, m_processCompoundPairsKernel, "m_processCompoundPairsKernel");
			launcher.setBuffers(bInfo, sizeof(bInfo) / sizeof(b3BufferInfoCL));
			launcher.setConst(numCompoundPairs);

			int num = numCompoundPairs;
			launcher.launch1D(num);
			clFinish(m_queue);
		}

		//printf("numConcave  = %d\n",numConcave);

		//		printf("hostNormals.size()=%d\n",hostNormals.size());
		//int numPairs = pairCount.at(0);
	}
	int vertexFaceCapacity = 64;

	{
		//now perform the tree query on GPU

		if (treeNodesGPU->size() && treeNodesGPU->size())
		{
			if (bvhTraversalKernelGPU)
			{
				B3_PROFILE("m_bvhTraversalKernel");

				numConcavePairs = m_numConcavePairsOut.at(0);

				b3LauncherCL launcher(m_queue, m_bvhTraversalKernel, "m_bvhTraversalKernel");
				launcher.setBuffer(pairs->getBufferCL());
				launcher.setBuffer(bodyBuf->getBufferCL());
				launcher.setBuffer(gpuCollidables.getBufferCL());
				launcher.setBuffer(clAabbsWorldSpace.getBufferCL());
				launcher.setBuffer(triangleConvexPairsOut.getBufferCL());
				launcher.setBuffer(m_numConcavePairsOut.getBufferCL());
				launcher.setBuffer(subTreesGPU->getBufferCL());
				launcher.setBuffer(treeNodesGPU->getBufferCL());
				launcher.setBuffer(bvhInfo->getBufferCL());

				launcher.setConst(nPairs);
				launcher.setConst(maxTriConvexPairCapacity);
				int num = nPairs;
				launcher.launch1D(num);
				clFinish(m_queue);
				numConcavePairs = m_numConcavePairsOut.at(0);
			}
			else
			{
				b3AlignedObjectArray<b3Int4> hostPairs;
				pairs->copyToHost(hostPairs);
				b3AlignedObjectArray<b3RigidBodyData> hostBodyBuf;
				bodyBuf->copyToHost(hostBodyBuf);
				b3AlignedObjectArray<b3Collidable> hostCollidables;
				gpuCollidables.copyToHost(hostCollidables);
				b3AlignedObjectArray<b3Aabb> hostAabbsWorldSpace;
				clAabbsWorldSpace.copyToHost(hostAabbsWorldSpace);

				//int maxTriConvexPairCapacity,
				b3AlignedObjectArray<b3Int4> triangleConvexPairsOutHost;
				triangleConvexPairsOutHost.resize(maxTriConvexPairCapacity);

				//int numTriConvexPairsOutHost=0;
				numConcavePairs = 0;
				//m_numConcavePairsOut

				b3AlignedObjectArray<b3QuantizedBvhNode> treeNodesCPU;
				treeNodesGPU->copyToHost(treeNodesCPU);
				b3AlignedObjectArray<b3BvhSubtreeInfo> subTreesCPU;
				subTreesGPU->copyToHost(subTreesCPU);
				b3AlignedObjectArray<b3BvhInfo> bvhInfoCPU;
				bvhInfo->copyToHost(bvhInfoCPU);
				//compute it...

				volatile int hostNumConcavePairsOut = 0;

				//
				for (int i = 0; i < nPairs; i++)
				{
					b3BvhTraversal(&hostPairs.at(0),
								   &hostBodyBuf.at(0),
								   &hostCollidables.at(0),
								   &hostAabbsWorldSpace.at(0),
								   &triangleConvexPairsOutHost.at(0),
								   &hostNumConcavePairsOut,
								   &subTreesCPU.at(0),
								   &treeNodesCPU.at(0),
								   &bvhInfoCPU.at(0),
								   nPairs,
								   maxTriConvexPairCapacity,
								   i);
				}
				numConcavePairs = hostNumConcavePairsOut;

				if (hostNumConcavePairsOut)
				{
					triangleConvexPairsOutHost.resize(hostNumConcavePairsOut);
					triangleConvexPairsOut.copyFromHost(triangleConvexPairsOutHost);
				}
				//

				m_numConcavePairsOut.resize(0);
				m_numConcavePairsOut.push_back(numConcavePairs);
			}

			//printf("numConcavePairs=%d (max = %d\n",numConcavePairs,maxTriConvexPairCapacity);

			if (numConcavePairs > maxTriConvexPairCapacity)
			{
				static int exceeded_maxTriConvexPairCapacity_count = 0;
				b3Error("Exceeded the maxTriConvexPairCapacity (found %d but max is %d, it happened %d times)\n",
						numConcavePairs, maxTriConvexPairCapacity, exceeded_maxTriConvexPairCapacity_count++);
				numConcavePairs = maxTriConvexPairCapacity;
			}
			triangleConvexPairsOut.resize(numConcavePairs);

			if (numConcavePairs)
			{
				clippingFacesOutGPU.resize(numConcavePairs);
				worldNormalsAGPU.resize(numConcavePairs);
				worldVertsA1GPU.resize(vertexFaceCapacity * (numConcavePairs));
				worldVertsB1GPU.resize(vertexFaceCapacity * (numConcavePairs));

				if (findConcaveSeparatingAxisKernelGPU)
				{
					/*
					m_concaveHasSeparatingNormals.copyFromHost(concaveHasSeparatingNormalsCPU);
						clippingFacesOutGPU.copyFromHost(clippingFacesOutCPU);
						worldVertsA1GPU.copyFromHost(worldVertsA1CPU);
						worldNormalsAGPU.copyFromHost(worldNormalsACPU);
						worldVertsB1GPU.copyFromHost(worldVertsB1CPU);
					*/

					//now perform a SAT test for each triangle-convex element (stored in triangleConvexPairsOut)
					if (splitSearchSepAxisConcave)
					{
						//printf("numConcavePairs = %d\n",numConcavePairs);
						m_dmins.resize(numConcavePairs);
						{
							B3_PROFILE("findConcaveSeparatingAxisVertexFaceKernel");
							b3BufferInfoCL bInfo[] = {
								b3BufferInfoCL(triangleConvexPairsOut.getBufferCL()),
								b3BufferInfoCL(bodyBuf->getBufferCL(), true),
								b3BufferInfoCL(gpuCollidables.getBufferCL(), true),
								b3BufferInfoCL(convexData.getBufferCL(), true),
								b3BufferInfoCL(gpuVertices.getBufferCL(), true),
								b3BufferInfoCL(gpuUniqueEdges.getBufferCL(), true),
								b3BufferInfoCL(gpuFaces.getBufferCL(), true),
								b3BufferInfoCL(gpuIndices.getBufferCL(), true),
								b3BufferInfoCL(gpuChildShapes.getBufferCL(), true),
								b3BufferInfoCL(clAabbsWorldSpace.getBufferCL(), true),
								b3BufferInfoCL(m_concaveSepNormals.getBufferCL()),
								b3BufferInfoCL(m_concaveHasSeparatingNormals.getBufferCL()),
								b3BufferInfoCL(clippingFacesOutGPU.getBufferCL()),
								b3BufferInfoCL(worldVertsA1GPU.getBufferCL()),
								b3BufferInfoCL(worldNormalsAGPU.getBufferCL()),
								b3BufferInfoCL(worldVertsB1GPU.getBufferCL()),
								b3BufferInfoCL(m_dmins.getBufferCL())};

							b3LauncherCL launcher(m_queue, m_findConcaveSeparatingAxisVertexFaceKernel, "m_findConcaveSeparatingAxisVertexFaceKernel");
							launcher.setBuffers(bInfo, sizeof(bInfo) / sizeof(b3BufferInfoCL));
							launcher.setConst(vertexFaceCapacity);
							launcher.setConst(numConcavePairs);

							int num = numConcavePairs;
							launcher.launch1D(num);
							clFinish(m_queue);
						}
						//                        numConcavePairs = 0;
						if (1)
						{
							B3_PROFILE("findConcaveSeparatingAxisEdgeEdgeKernel");
							b3BufferInfoCL bInfo[] = {
								b3BufferInfoCL(triangleConvexPairsOut.getBufferCL()),
								b3BufferInfoCL(bodyBuf->getBufferCL(), true),
								b3BufferInfoCL(gpuCollidables.getBufferCL(), true),
								b3BufferInfoCL(convexData.getBufferCL(), true),
								b3BufferInfoCL(gpuVertices.getBufferCL(), true),
								b3BufferInfoCL(gpuUniqueEdges.getBufferCL(), true),
								b3BufferInfoCL(gpuFaces.getBufferCL(), true),
								b3BufferInfoCL(gpuIndices.getBufferCL(), true),
								b3BufferInfoCL(gpuChildShapes.getBufferCL(), true),
								b3BufferInfoCL(clAabbsWorldSpace.getBufferCL(), true),
								b3BufferInfoCL(m_concaveSepNormals.getBufferCL()),
								b3BufferInfoCL(m_concaveHasSeparatingNormals.getBufferCL()),
								b3BufferInfoCL(clippingFacesOutGPU.getBufferCL()),
								b3BufferInfoCL(worldVertsA1GPU.getBufferCL()),
								b3BufferInfoCL(worldNormalsAGPU.getBufferCL()),
								b3BufferInfoCL(worldVertsB1GPU.getBufferCL()),
								b3BufferInfoCL(m_dmins.getBufferCL())};

							b3LauncherCL launcher(m_queue, m_findConcaveSeparatingAxisEdgeEdgeKernel, "m_findConcaveSeparatingAxisEdgeEdgeKernel");
							launcher.setBuffers(bInfo, sizeof(bInfo) / sizeof(b3BufferInfoCL));
							launcher.setConst(vertexFaceCapacity);
							launcher.setConst(numConcavePairs);

							int num = numConcavePairs;
							launcher.launch1D(num);
							clFinish(m_queue);
						}

						// numConcavePairs = 0;
					}
					else
					{
						B3_PROFILE("findConcaveSeparatingAxisKernel");
						b3BufferInfoCL bInfo[] = {
							b3BufferInfoCL(triangleConvexPairsOut.getBufferCL()),
							b3BufferInfoCL(bodyBuf->getBufferCL(), true),
							b3BufferInfoCL(gpuCollidables.getBufferCL(), true),
							b3BufferInfoCL(convexData.getBufferCL(), true),
							b3BufferInfoCL(gpuVertices.getBufferCL(), true),
							b3BufferInfoCL(gpuUniqueEdges.getBufferCL(), true),
							b3BufferInfoCL(gpuFaces.getBufferCL(), true),
							b3BufferInfoCL(gpuIndices.getBufferCL(), true),
							b3BufferInfoCL(gpuChildShapes.getBufferCL(), true),
							b3BufferInfoCL(clAabbsWorldSpace.getBufferCL(), true),
							b3BufferInfoCL(m_concaveSepNormals.getBufferCL()),
							b3BufferInfoCL(m_concaveHasSeparatingNormals.getBufferCL()),
							b3BufferInfoCL(clippingFacesOutGPU.getBufferCL()),
							b3BufferInfoCL(worldVertsA1GPU.getBufferCL()),
							b3BufferInfoCL(worldNormalsAGPU.getBufferCL()),
							b3BufferInfoCL(worldVertsB1GPU.getBufferCL())};

						b3LauncherCL launcher(m_queue, m_findConcaveSeparatingAxisKernel, "m_findConcaveSeparatingAxisKernel");
						launcher.setBuffers(bInfo, sizeof(bInfo) / sizeof(b3BufferInfoCL));
						launcher.setConst(vertexFaceCapacity);
						launcher.setConst(numConcavePairs);

						int num = numConcavePairs;
						launcher.launch1D(num);
						clFinish(m_queue);
					}
				}
				else
				{
					b3AlignedObjectArray<b3Int4> clippingFacesOutCPU;
					b3AlignedObjectArray<b3Vector3> worldVertsA1CPU;
					b3AlignedObjectArray<b3Vector3> worldNormalsACPU;
					b3AlignedObjectArray<b3Vector3> worldVertsB1CPU;
					b3AlignedObjectArray<int> concaveHasSeparatingNormalsCPU;

					b3AlignedObjectArray<b3Int4> triangleConvexPairsOutHost;
					triangleConvexPairsOut.copyToHost(triangleConvexPairsOutHost);
					//triangleConvexPairsOutHost.resize(maxTriConvexPairCapacity);
					b3AlignedObjectArray<b3RigidBodyData> hostBodyBuf;
					bodyBuf->copyToHost(hostBodyBuf);
					b3AlignedObjectArray<b3Collidable> hostCollidables;
					gpuCollidables.copyToHost(hostCollidables);
					b3AlignedObjectArray<b3Aabb> hostAabbsWorldSpace;
					clAabbsWorldSpace.copyToHost(hostAabbsWorldSpace);

					b3AlignedObjectArray<b3ConvexPolyhedronData> hostConvexData;
					convexData.copyToHost(hostConvexData);

					b3AlignedObjectArray<b3Vector3> hostVertices;
					gpuVertices.copyToHost(hostVertices);

					b3AlignedObjectArray<b3Vector3> hostUniqueEdges;
					gpuUniqueEdges.copyToHost(hostUniqueEdges);
					b3AlignedObjectArray<b3GpuFace> hostFaces;
					gpuFaces.copyToHost(hostFaces);
					b3AlignedObjectArray<int> hostIndices;
					gpuIndices.copyToHost(hostIndices);
					b3AlignedObjectArray<b3GpuChildShape> cpuChildShapes;
					gpuChildShapes.copyToHost(cpuChildShapes);

					b3AlignedObjectArray<b3Vector3> concaveSepNormalsHost;
					m_concaveSepNormals.copyToHost(concaveSepNormalsHost);
					concaveHasSeparatingNormalsCPU.resize(concaveSepNormalsHost.size());

					b3GpuChildShape* childShapePointerCPU = 0;
					if (cpuChildShapes.size())
						childShapePointerCPU = &cpuChildShapes.at(0);

					clippingFacesOutCPU.resize(clippingFacesOutGPU.size());
					worldVertsA1CPU.resize(worldVertsA1GPU.size());
					worldNormalsACPU.resize(worldNormalsAGPU.size());
					worldVertsB1CPU.resize(worldVertsB1GPU.size());

					for (int i = 0; i < numConcavePairs; i++)
					{
						b3FindConcaveSeparatingAxisKernel(&triangleConvexPairsOutHost.at(0),
														  &hostBodyBuf.at(0),
														  &hostCollidables.at(0),
														  &hostConvexData.at(0), &hostVertices.at(0), &hostUniqueEdges.at(0),
														  &hostFaces.at(0), &hostIndices.at(0), childShapePointerCPU,
														  &hostAabbsWorldSpace.at(0),
														  &concaveSepNormalsHost.at(0),
														  &clippingFacesOutCPU.at(0),
														  &worldVertsA1CPU.at(0),
														  &worldNormalsACPU.at(0),
														  &worldVertsB1CPU.at(0),
														  &concaveHasSeparatingNormalsCPU.at(0),
														  vertexFaceCapacity,
														  numConcavePairs, i);
					};

					m_concaveSepNormals.copyFromHost(concaveSepNormalsHost);
					m_concaveHasSeparatingNormals.copyFromHost(concaveHasSeparatingNormalsCPU);
					clippingFacesOutGPU.copyFromHost(clippingFacesOutCPU);
					worldVertsA1GPU.copyFromHost(worldVertsA1CPU);
					worldNormalsAGPU.copyFromHost(worldNormalsACPU);
					worldVertsB1GPU.copyFromHost(worldVertsB1CPU);
				}
				//							b3AlignedObjectArray<b3Vector3> cpuCompoundSepNormals;
				//						m_concaveSepNormals.copyToHost(cpuCompoundSepNormals);
				//					b3AlignedObjectArray<b3Int4> cpuConcavePairs;
				//				triangleConvexPairsOut.copyToHost(cpuConcavePairs);
			}
		}
	}

	if (numConcavePairs)
	{
		if (numConcavePairs)
		{
			B3_PROFILE("findConcaveSphereContactsKernel");
			nContacts = m_totalContactsOut.at(0);
			//				printf("nContacts1 = %d\n",nContacts);
			b3BufferInfoCL bInfo[] = {
				b3BufferInfoCL(triangleConvexPairsOut.getBufferCL()),
				b3BufferInfoCL(bodyBuf->getBufferCL(), true),
				b3BufferInfoCL(gpuCollidables.getBufferCL(), true),
				b3BufferInfoCL(convexData.getBufferCL(), true),
				b3BufferInfoCL(gpuVertices.getBufferCL(), true),
				b3BufferInfoCL(gpuUniqueEdges.getBufferCL(), true),
				b3BufferInfoCL(gpuFaces.getBufferCL(), true),
				b3BufferInfoCL(gpuIndices.getBufferCL(), true),
				b3BufferInfoCL(clAabbsWorldSpace.getBufferCL(), true),
				b3BufferInfoCL(contactOut->getBufferCL()),
				b3BufferInfoCL(m_totalContactsOut.getBufferCL())};

			b3LauncherCL launcher(m_queue, m_findConcaveSphereContactsKernel, "m_findConcaveSphereContactsKernel");
			launcher.setBuffers(bInfo, sizeof(bInfo) / sizeof(b3BufferInfoCL));

			launcher.setConst(numConcavePairs);
			launcher.setConst(maxContactCapacity);

			int num = numConcavePairs;
			launcher.launch1D(num);
			clFinish(m_queue);
			nContacts = m_totalContactsOut.at(0);
			//printf("nContacts (after findConcaveSphereContactsKernel) = %d\n",nContacts);

			//printf("nContacts2 = %d\n",nContacts);

			if (nContacts >= maxContactCapacity)
			{
				b3Error("Error: contacts exceeds capacity (%d/%d)\n", nContacts, maxContactCapacity);
				nContacts = maxContactCapacity;
			}
		}
	}

#ifdef __APPLE__
	bool contactClippingOnGpu = true;
#else
	bool contactClippingOnGpu = true;
#endif

	if (contactClippingOnGpu)
	{
		m_totalContactsOut.copyFromHostPointer(&nContacts, 1, 0, true);
		//		printf("nContacts3 = %d\n",nContacts);

		//B3_PROFILE("clipHullHullKernel");

		bool breakupConcaveConvexKernel = true;

#ifdef __APPLE__
		//actually, some Apple OpenCL platform/device combinations work fine...
		breakupConcaveConvexKernel = true;
#endif
		//concave-convex contact clipping
		if (numConcavePairs)
		{
			//			printf("numConcavePairs = %d\n", numConcavePairs);
			//		nContacts = m_totalContactsOut.at(0);
			//	printf("nContacts before = %d\n", nContacts);

			if (breakupConcaveConvexKernel)
			{
				worldVertsB2GPU.resize(vertexFaceCapacity * numConcavePairs);

				//clipFacesAndFindContacts

				if (clipConcaveFacesAndFindContactsCPU)
				{
					b3AlignedObjectArray<b3Int4> clippingFacesOutCPU;
					b3AlignedObjectArray<b3Vector3> worldVertsA1CPU;
					b3AlignedObjectArray<b3Vector3> worldNormalsACPU;
					b3AlignedObjectArray<b3Vector3> worldVertsB1CPU;

					clippingFacesOutGPU.copyToHost(clippingFacesOutCPU);
					worldVertsA1GPU.copyToHost(worldVertsA1CPU);
					worldNormalsAGPU.copyToHost(worldNormalsACPU);
					worldVertsB1GPU.copyToHost(worldVertsB1CPU);

					b3AlignedObjectArray<int> concaveHasSeparatingNormalsCPU;
					m_concaveHasSeparatingNormals.copyToHost(concaveHasSeparatingNormalsCPU);

					b3AlignedObjectArray<b3Vector3> concaveSepNormalsHost;
					m_concaveSepNormals.copyToHost(concaveSepNormalsHost);

					b3AlignedObjectArray<b3Vector3> worldVertsB2CPU;
					worldVertsB2CPU.resize(worldVertsB2GPU.size());

					for (int i = 0; i < numConcavePairs; i++)
					{
						clipFacesAndFindContactsKernel(&concaveSepNormalsHost.at(0),
													   &concaveHasSeparatingNormalsCPU.at(0),
													   &clippingFacesOutCPU.at(0),
													   &worldVertsA1CPU.at(0),
													   &worldNormalsACPU.at(0),
													   &worldVertsB1CPU.at(0),
													   &worldVertsB2CPU.at(0),
													   vertexFaceCapacity,
													   i);
					}

					clippingFacesOutGPU.copyFromHost(clippingFacesOutCPU);
					worldVertsB2GPU.copyFromHost(worldVertsB2CPU);
				}
				else
				{
					if (1)
					{
						B3_PROFILE("clipFacesAndFindContacts");
						//nContacts = m_totalContactsOut.at(0);
						//int h = m_hasSeparatingNormals.at(0);
						//int4 p = clippingFacesOutGPU.at(0);
						b3BufferInfoCL bInfo[] = {
							b3BufferInfoCL(m_concaveSepNormals.getBufferCL()),
							b3BufferInfoCL(m_concaveHasSeparatingNormals.getBufferCL()),
							b3BufferInfoCL(clippingFacesOutGPU.getBufferCL()),
							b3BufferInfoCL(worldVertsA1GPU.getBufferCL()),
							b3BufferInfoCL(worldNormalsAGPU.getBufferCL()),
							b3BufferInfoCL(worldVertsB1GPU.getBufferCL()),
							b3BufferInfoCL(worldVertsB2GPU.getBufferCL())};
						b3LauncherCL launcher(m_queue, m_clipFacesAndFindContacts, "m_clipFacesAndFindContacts");
						launcher.setBuffers(bInfo, sizeof(bInfo) / sizeof(b3BufferInfoCL));
						launcher.setConst(vertexFaceCapacity);

						launcher.setConst(numConcavePairs);
						int debugMode = 0;
						launcher.setConst(debugMode);
						int num = numConcavePairs;
						launcher.launch1D(num);
						clFinish(m_queue);
						//int bla = m_totalContactsOut.at(0);
					}
				}
				//contactReduction
				{
					int newContactCapacity = nContacts + numConcavePairs;
					contactOut->reserve(newContactCapacity);
					if (reduceConcaveContactsOnGPU)
					{
						//						printf("newReservation = %d\n",newReservation);
						{
							B3_PROFILE("newContactReductionKernel");
							b3BufferInfoCL bInfo[] =
								{
									b3BufferInfoCL(triangleConvexPairsOut.getBufferCL(), true),
									b3BufferInfoCL(bodyBuf->getBufferCL(), true),
									b3BufferInfoCL(m_concaveSepNormals.getBufferCL()),
									b3BufferInfoCL(m_concaveHasSeparatingNormals.getBufferCL()),
									b3BufferInfoCL(contactOut->getBufferCL()),
									b3BufferInfoCL(clippingFacesOutGPU.getBufferCL()),
									b3BufferInfoCL(worldVertsB2GPU.getBufferCL()),
									b3BufferInfoCL(m_totalContactsOut.getBufferCL())};

							b3LauncherCL launcher(m_queue, m_newContactReductionKernel, "m_newContactReductionKernel");
							launcher.setBuffers(bInfo, sizeof(bInfo) / sizeof(b3BufferInfoCL));
							launcher.setConst(vertexFaceCapacity);
							launcher.setConst(newContactCapacity);
							launcher.setConst(numConcavePairs);
							int num = numConcavePairs;

							launcher.launch1D(num);
						}
						nContacts = m_totalContactsOut.at(0);
						contactOut->resize(nContacts);

						//printf("contactOut4 (after newContactReductionKernel) = %d\n",nContacts);
					}
					else
					{
						volatile int nGlobalContactsOut = nContacts;
						b3AlignedObjectArray<b3Int4> triangleConvexPairsOutHost;
						triangleConvexPairsOut.copyToHost(triangleConvexPairsOutHost);
						b3AlignedObjectArray<b3RigidBodyData> hostBodyBuf;
						bodyBuf->copyToHost(hostBodyBuf);

						b3AlignedObjectArray<int> concaveHasSeparatingNormalsCPU;
						m_concaveHasSeparatingNormals.copyToHost(concaveHasSeparatingNormalsCPU);

						b3AlignedObjectArray<b3Vector3> concaveSepNormalsHost;
						m_concaveSepNormals.copyToHost(concaveSepNormalsHost);

						b3AlignedObjectArray<b3Contact4> hostContacts;
						if (nContacts)
						{
							contactOut->copyToHost(hostContacts);
						}
						hostContacts.resize(newContactCapacity);

						b3AlignedObjectArray<b3Int4> clippingFacesOutCPU;
						b3AlignedObjectArray<b3Vector3> worldVertsB2CPU;

						clippingFacesOutGPU.copyToHost(clippingFacesOutCPU);
						worldVertsB2GPU.copyToHost(worldVertsB2CPU);

						for (int i = 0; i < numConcavePairs; i++)
						{
							b3NewContactReductionKernel(&triangleConvexPairsOutHost.at(0),
														&hostBodyBuf.at(0),
														&concaveSepNormalsHost.at(0),
														&concaveHasSeparatingNormalsCPU.at(0),
														&hostContacts.at(0),
														&clippingFacesOutCPU.at(0),
														&worldVertsB2CPU.at(0),
														&nGlobalContactsOut,
														vertexFaceCapacity,
														newContactCapacity,
														numConcavePairs,
														i);
						}

						nContacts = nGlobalContactsOut;
						m_totalContactsOut.copyFromHostPointer(&nContacts, 1, 0, true);
						//						nContacts = m_totalContactsOut.at(0);
						//contactOut->resize(nContacts);
						hostContacts.resize(nContacts);
						//printf("contactOut4 (after newContactReductionKernel) = %d\n",nContacts);
						contactOut->copyFromHost(hostContacts);
					}
				}
				//re-use?
			}
			else
			{
				B3_PROFILE("clipHullHullConcaveConvexKernel");
				nContacts = m_totalContactsOut.at(0);
				int newContactCapacity = contactOut->capacity();

				//printf("contactOut5 = %d\n",nContacts);
				b3BufferInfoCL bInfo[] = {
					b3BufferInfoCL(triangleConvexPairsOut.getBufferCL(), true),
					b3BufferInfoCL(bodyBuf->getBufferCL(), true),
					b3BufferInfoCL(gpuCollidables.getBufferCL(), true),
					b3BufferInfoCL(convexData.getBufferCL(), true),
					b3BufferInfoCL(gpuVertices.getBufferCL(), true),
					b3BufferInfoCL(gpuUniqueEdges.getBufferCL(), true),
					b3BufferInfoCL(gpuFaces.getBufferCL(), true),
					b3BufferInfoCL(gpuIndices.getBufferCL(), true),
					b3BufferInfoCL(gpuChildShapes.getBufferCL(), true),
					b3BufferInfoCL(m_concaveSepNormals.getBufferCL()),
					b3BufferInfoCL(contactOut->getBufferCL()),
					b3BufferInfoCL(m_totalContactsOut.getBufferCL())};
				b3LauncherCL launcher(m_queue, m_clipHullHullConcaveConvexKernel, "m_clipHullHullConcaveConvexKernel");
				launcher.setBuffers(bInfo, sizeof(bInfo) / sizeof(b3BufferInfoCL));
				launcher.setConst(newContactCapacity);
				launcher.setConst(numConcavePairs);
				int num = numConcavePairs;
				launcher.launch1D(num);
				clFinish(m_queue);
				nContacts = m_totalContactsOut.at(0);
				contactOut->resize(nContacts);
				//printf("contactOut6 = %d\n",nContacts);
				b3AlignedObjectArray<b3Contact4> cpuContacts;
				contactOut->copyToHost(cpuContacts);
			}
			//			printf("nContacts after = %d\n", nContacts);
		}  //numConcavePairs

		//convex-convex contact clipping

		bool breakupKernel = false;

#ifdef __APPLE__
		breakupKernel = true;
#endif

#ifdef CHECK_ON_HOST
		bool computeConvexConvex = false;
#else
		bool computeConvexConvex = true;
#endif  //CHECK_ON_HOST
		if (computeConvexConvex)
		{
			B3_PROFILE("clipHullHullKernel");
			if (breakupKernel)
			{
				worldVertsB1GPU.resize(vertexFaceCapacity * nPairs);
				clippingFacesOutGPU.resize(nPairs);
				worldNormalsAGPU.resize(nPairs);
				worldVertsA1GPU.resize(vertexFaceCapacity * nPairs);
				worldVertsB2GPU.resize(vertexFaceCapacity * nPairs);

				if (findConvexClippingFacesGPU)
				{
					B3_PROFILE("findClippingFacesKernel");
					b3BufferInfoCL bInfo[] = {
						b3BufferInfoCL(pairs->getBufferCL(), true),
						b3BufferInfoCL(bodyBuf->getBufferCL(), true),
						b3BufferInfoCL(gpuCollidables.getBufferCL(), true),
						b3BufferInfoCL(convexData.getBufferCL(), true),
						b3BufferInfoCL(gpuVertices.getBufferCL(), true),
						b3BufferInfoCL(gpuUniqueEdges.getBufferCL(), true),
						b3BufferInfoCL(gpuFaces.getBufferCL(), true),
						b3BufferInfoCL(gpuIndices.getBufferCL(), true),
						b3BufferInfoCL(m_sepNormals.getBufferCL()),
						b3BufferInfoCL(m_hasSeparatingNormals.getBufferCL()),
						b3BufferInfoCL(clippingFacesOutGPU.getBufferCL()),
						b3BufferInfoCL(worldVertsA1GPU.getBufferCL()),
						b3BufferInfoCL(worldNormalsAGPU.getBufferCL()),
						b3BufferInfoCL(worldVertsB1GPU.getBufferCL())};

					b3LauncherCL launcher(m_queue, m_findClippingFacesKernel, "m_findClippingFacesKernel");
					launcher.setBuffers(bInfo, sizeof(bInfo) / sizeof(b3BufferInfoCL));
					launcher.setConst(vertexFaceCapacity);
					launcher.setConst(nPairs);
					int num = nPairs;
					launcher.launch1D(num);
					clFinish(m_queue);
				}
				else
				{
					float minDist = -1e30f;
					float maxDist = 0.02f;

					b3AlignedObjectArray<b3ConvexPolyhedronData> hostConvexData;
					convexData.copyToHost(hostConvexData);
					b3AlignedObjectArray<b3Collidable> hostCollidables;
					gpuCollidables.copyToHost(hostCollidables);

					b3AlignedObjectArray<int> hostHasSepNormals;
					m_hasSeparatingNormals.copyToHost(hostHasSepNormals);
					b3AlignedObjectArray<b3Vector3> cpuSepNormals;
					m_sepNormals.copyToHost(cpuSepNormals);

					b3AlignedObjectArray<b3Int4> hostPairs;
					pairs->copyToHost(hostPairs);
					b3AlignedObjectArray<b3RigidBodyData> hostBodyBuf;
					bodyBuf->copyToHost(hostBodyBuf);

					//worldVertsB1GPU.resize(vertexFaceCapacity*nPairs);
					b3AlignedObjectArray<b3Vector3> worldVertsB1CPU;
					worldVertsB1GPU.copyToHost(worldVertsB1CPU);

					b3AlignedObjectArray<b3Int4> clippingFacesOutCPU;
					clippingFacesOutGPU.copyToHost(clippingFacesOutCPU);

					b3AlignedObjectArray<b3Vector3> worldNormalsACPU;
					worldNormalsACPU.resize(nPairs);

					b3AlignedObjectArray<b3Vector3> worldVertsA1CPU;
					worldVertsA1CPU.resize(worldVertsA1GPU.size());

					b3AlignedObjectArray<b3Vector3> hostVertices;
					gpuVertices.copyToHost(hostVertices);
					b3AlignedObjectArray<b3GpuFace> hostFaces;
					gpuFaces.copyToHost(hostFaces);
					b3AlignedObjectArray<int> hostIndices;
					gpuIndices.copyToHost(hostIndices);

					for (int i = 0; i < nPairs; i++)
					{
						int bodyIndexA = hostPairs[i].x;
						int bodyIndexB = hostPairs[i].y;

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

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

						if (hostHasSepNormals[i])
						{
							b3FindClippingFaces(cpuSepNormals[i],
												&hostConvexData[shapeIndexA],
												&hostConvexData[shapeIndexB],
												hostBodyBuf[bodyIndexA].m_pos, hostBodyBuf[bodyIndexA].m_quat,
												hostBodyBuf[bodyIndexB].m_pos, hostBodyBuf[bodyIndexB].m_quat,
												&worldVertsA1CPU.at(0), &worldNormalsACPU.at(0),
												&worldVertsB1CPU.at(0),
												vertexFaceCapacity, minDist, maxDist,
												&hostVertices.at(0), &hostFaces.at(0),
												&hostIndices.at(0),
												&hostVertices.at(0), &hostFaces.at(0),
												&hostIndices.at(0), &clippingFacesOutCPU.at(0), i);
						}
					}

					clippingFacesOutGPU.copyFromHost(clippingFacesOutCPU);
					worldVertsA1GPU.copyFromHost(worldVertsA1CPU);
					worldNormalsAGPU.copyFromHost(worldNormalsACPU);
					worldVertsB1GPU.copyFromHost(worldVertsB1CPU);
				}

				///clip face B against face A, reduce contacts and append them to a global contact array
				if (1)
				{
					if (clipConvexFacesAndFindContactsCPU)
					{
						//b3AlignedObjectArray<b3Int4> hostPairs;
						//pairs->copyToHost(hostPairs);

						b3AlignedObjectArray<b3Vector3> hostSepNormals;
						m_sepNormals.copyToHost(hostSepNormals);
						b3AlignedObjectArray<int> hostHasSepAxis;
						m_hasSeparatingNormals.copyToHost(hostHasSepAxis);

						b3AlignedObjectArray<b3Int4> hostClippingFaces;
						clippingFacesOutGPU.copyToHost(hostClippingFaces);
						b3AlignedObjectArray<b3Vector3> worldVertsB2CPU;
						worldVertsB2CPU.resize(vertexFaceCapacity * nPairs);

						b3AlignedObjectArray<b3Vector3> worldVertsA1CPU;
						worldVertsA1GPU.copyToHost(worldVertsA1CPU);
						b3AlignedObjectArray<b3Vector3> worldNormalsACPU;
						worldNormalsAGPU.copyToHost(worldNormalsACPU);

						b3AlignedObjectArray<b3Vector3> worldVertsB1CPU;
						worldVertsB1GPU.copyToHost(worldVertsB1CPU);

						/*
					  __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
					*/
						for (int i = 0; i < nPairs; i++)
						{
							clipFacesAndFindContactsKernel(
								&hostSepNormals.at(0),
								&hostHasSepAxis.at(0),
								&hostClippingFaces.at(0),
								&worldVertsA1CPU.at(0),
								&worldNormalsACPU.at(0),
								&worldVertsB1CPU.at(0),
								&worldVertsB2CPU.at(0),

								vertexFaceCapacity,
								i);
						}

						clippingFacesOutGPU.copyFromHost(hostClippingFaces);
						worldVertsB2GPU.copyFromHost(worldVertsB2CPU);
					}
					else
					{
						B3_PROFILE("clipFacesAndFindContacts");
						//nContacts = m_totalContactsOut.at(0);
						//int h = m_hasSeparatingNormals.at(0);
						//int4 p = clippingFacesOutGPU.at(0);
						b3BufferInfoCL bInfo[] = {
							b3BufferInfoCL(m_sepNormals.getBufferCL()),
							b3BufferInfoCL(m_hasSeparatingNormals.getBufferCL()),
							b3BufferInfoCL(clippingFacesOutGPU.getBufferCL()),
							b3BufferInfoCL(worldVertsA1GPU.getBufferCL()),
							b3BufferInfoCL(worldNormalsAGPU.getBufferCL()),
							b3BufferInfoCL(worldVertsB1GPU.getBufferCL()),
							b3BufferInfoCL(worldVertsB2GPU.getBufferCL())};

						b3LauncherCL launcher(m_queue, m_clipFacesAndFindContacts, "m_clipFacesAndFindContacts");
						launcher.setBuffers(bInfo, sizeof(bInfo) / sizeof(b3BufferInfoCL));
						launcher.setConst(vertexFaceCapacity);

						launcher.setConst(nPairs);
						int debugMode = 0;
						launcher.setConst(debugMode);
						int num = nPairs;
						launcher.launch1D(num);
						clFinish(m_queue);
					}

					{
						nContacts = m_totalContactsOut.at(0);
						//printf("nContacts = %d\n",nContacts);

						int newContactCapacity = nContacts + nPairs;
						contactOut->reserve(newContactCapacity);

						if (reduceConvexContactsOnGPU)
						{
							{
								B3_PROFILE("newContactReductionKernel");
								b3BufferInfoCL bInfo[] =
									{
										b3BufferInfoCL(pairs->getBufferCL(), true),
										b3BufferInfoCL(bodyBuf->getBufferCL(), true),
										b3BufferInfoCL(m_sepNormals.getBufferCL()),
										b3BufferInfoCL(m_hasSeparatingNormals.getBufferCL()),
										b3BufferInfoCL(contactOut->getBufferCL()),
										b3BufferInfoCL(clippingFacesOutGPU.getBufferCL()),
										b3BufferInfoCL(worldVertsB2GPU.getBufferCL()),
										b3BufferInfoCL(m_totalContactsOut.getBufferCL())};

								b3LauncherCL launcher(m_queue, m_newContactReductionKernel, "m_newContactReductionKernel");
								launcher.setBuffers(bInfo, sizeof(bInfo) / sizeof(b3BufferInfoCL));
								launcher.setConst(vertexFaceCapacity);
								launcher.setConst(newContactCapacity);
								launcher.setConst(nPairs);
								int num = nPairs;

								launcher.launch1D(num);
							}
							nContacts = m_totalContactsOut.at(0);
							contactOut->resize(nContacts);
						}
						else
						{
							volatile int nGlobalContactsOut = nContacts;
							b3AlignedObjectArray<b3Int4> hostPairs;
							pairs->copyToHost(hostPairs);
							b3AlignedObjectArray<b3RigidBodyData> hostBodyBuf;
							bodyBuf->copyToHost(hostBodyBuf);
							b3AlignedObjectArray<b3Vector3> hostSepNormals;
							m_sepNormals.copyToHost(hostSepNormals);
							b3AlignedObjectArray<int> hostHasSepAxis;
							m_hasSeparatingNormals.copyToHost(hostHasSepAxis);
							b3AlignedObjectArray<b3Contact4> hostContactsOut;
							contactOut->copyToHost(hostContactsOut);
							hostContactsOut.resize(newContactCapacity);

							b3AlignedObjectArray<b3Int4> hostClippingFaces;
							clippingFacesOutGPU.copyToHost(hostClippingFaces);
							b3AlignedObjectArray<b3Vector3> worldVertsB2CPU;
							worldVertsB2GPU.copyToHost(worldVertsB2CPU);

							for (int i = 0; i < nPairs; i++)
							{
								b3NewContactReductionKernel(&hostPairs.at(0),
															&hostBodyBuf.at(0),
															&hostSepNormals.at(0),
															&hostHasSepAxis.at(0),
															&hostContactsOut.at(0),
															&hostClippingFaces.at(0),
															&worldVertsB2CPU.at(0),
															&nGlobalContactsOut,
															vertexFaceCapacity,
															newContactCapacity,
															nPairs,
															i);
							}

							nContacts = nGlobalContactsOut;
							m_totalContactsOut.copyFromHostPointer(&nContacts, 1, 0, true);
							hostContactsOut.resize(nContacts);
							//printf("contactOut4 (after newContactReductionKernel) = %d\n",nContacts);
							contactOut->copyFromHost(hostContactsOut);
						}
						//                    b3Contact4 pt = contactOut->at(0);
						//                  printf("nContacts = %d\n",nContacts);
					}
				}
			}
			else  //breakupKernel
			{
				if (nPairs)
				{
					b3BufferInfoCL bInfo[] = {
						b3BufferInfoCL(pairs->getBufferCL(), true),
						b3BufferInfoCL(bodyBuf->getBufferCL(), true),
						b3BufferInfoCL(gpuCollidables.getBufferCL(), true),
						b3BufferInfoCL(convexData.getBufferCL(), true),
						b3BufferInfoCL(gpuVertices.getBufferCL(), true),
						b3BufferInfoCL(gpuUniqueEdges.getBufferCL(), true),
						b3BufferInfoCL(gpuFaces.getBufferCL(), true),
						b3BufferInfoCL(gpuIndices.getBufferCL(), true),
						b3BufferInfoCL(m_sepNormals.getBufferCL()),
						b3BufferInfoCL(m_hasSeparatingNormals.getBufferCL()),
						b3BufferInfoCL(contactOut->getBufferCL()),
						b3BufferInfoCL(m_totalContactsOut.getBufferCL())};
					b3LauncherCL launcher(m_queue, m_clipHullHullKernel, "m_clipHullHullKernel");
					launcher.setBuffers(bInfo, sizeof(bInfo) / sizeof(b3BufferInfoCL));
					launcher.setConst(nPairs);
					launcher.setConst(maxContactCapacity);

					int num = nPairs;
					launcher.launch1D(num);
					clFinish(m_queue);

					nContacts = m_totalContactsOut.at(0);
					if (nContacts >= maxContactCapacity)
					{
						b3Error("Exceeded contact capacity (%d/%d)\n", nContacts, maxContactCapacity);
						nContacts = maxContactCapacity;
					}
					contactOut->resize(nContacts);
				}
			}

			int nCompoundsPairs = m_gpuCompoundPairs.size();

			if (nCompoundsPairs)
			{
				b3BufferInfoCL bInfo[] = {
					b3BufferInfoCL(m_gpuCompoundPairs.getBufferCL(), true),
					b3BufferInfoCL(bodyBuf->getBufferCL(), true),
					b3BufferInfoCL(gpuCollidables.getBufferCL(), true),
					b3BufferInfoCL(convexData.getBufferCL(), true),
					b3BufferInfoCL(gpuVertices.getBufferCL(), true),
					b3BufferInfoCL(gpuUniqueEdges.getBufferCL(), true),
					b3BufferInfoCL(gpuFaces.getBufferCL(), true),
					b3BufferInfoCL(gpuIndices.getBufferCL(), true),
					b3BufferInfoCL(gpuChildShapes.getBufferCL(), true),
					b3BufferInfoCL(m_gpuCompoundSepNormals.getBufferCL(), true),
					b3BufferInfoCL(m_gpuHasCompoundSepNormals.getBufferCL(), true),
					b3BufferInfoCL(contactOut->getBufferCL()),
					b3BufferInfoCL(m_totalContactsOut.getBufferCL())};
				b3LauncherCL launcher(m_queue, m_clipCompoundsHullHullKernel, "m_clipCompoundsHullHullKernel");
				launcher.setBuffers(bInfo, sizeof(bInfo) / sizeof(b3BufferInfoCL));
				launcher.setConst(nCompoundsPairs);
				launcher.setConst(maxContactCapacity);

				int num = nCompoundsPairs;
				launcher.launch1D(num);
				clFinish(m_queue);

				nContacts = m_totalContactsOut.at(0);
				if (nContacts > maxContactCapacity)
				{
					b3Error("Error: contacts exceeds capacity (%d/%d)\n", nContacts, maxContactCapacity);
					nContacts = maxContactCapacity;
				}
				contactOut->resize(nContacts);
			}  //if nCompoundsPairs
		}
	}  //contactClippingOnGpu

	//printf("nContacts end = %d\n",nContacts);

	//printf("frameCount = %d\n",frameCount++);
}