add re-usable createGraphicsSphere method in GpuDemo.

introduce and use maxContactCapacity (needs to be fixed in various other contact kernels)
implement sphere versus trimesh
disable new/sequential GPU batching (only uses 1 thread in a warp, slow but works on NVIDIA/Apple OpenCL)

demo/gpudemo/GpuDemo.cpp

@@ -2,7 +2,8 @@
 #include "GpuDemoInternalData.h"
 #include "BulletCommon/btScalar.h"
 #include "basic_initialize/btOpenCLUtils.h"
-
+#include "OpenGLWindow/ShapeData.h"
+#include "OpenGLWindow/GLInstancingRenderer.h"
 
 GpuDemo::GpuDemo()
 :m_clData(0)
@@ -78,3 +79,40 @@ void GpuDemo::initCL(int preferredDeviceIndex, int preferredPlatformIndex)
 }
 
 
+int	GpuDemo::registerGraphicsSphereShape(const ConstructionInfo& ci, float radius, bool usePointSprites, int largeSphereThreshold, int mediumSphereThreshold)
+{
+	
+	int strideInBytes = 9*sizeof(float);
+	
+	int graphicsShapeIndex = -1;
+	
+	if (radius>=largeSphereThreshold)
+	{
+		int numVertices = sizeof(detailed_sphere_vertices)/strideInBytes;
+		int numIndices = sizeof(detailed_sphere_indices)/sizeof(int);
+		graphicsShapeIndex = ci.m_instancingRenderer->registerShape(&detailed_sphere_vertices[0],numVertices,detailed_sphere_indices,numIndices);
+	} else
+	{
+		
+		if (usePointSprites)
+		{
+			int numVertices = sizeof(point_sphere_vertices)/strideInBytes;
+			int numIndices = sizeof(point_sphere_indices)/sizeof(int);
+			graphicsShapeIndex = ci.m_instancingRenderer->registerShape(&point_sphere_vertices[0],numVertices,point_sphere_indices,numIndices,BT_GL_POINTS);
+		} else
+		{
+			if (radius>=mediumSphereThreshold)
+			{
+				int numVertices = sizeof(medium_sphere_vertices)/strideInBytes;
+				int numIndices = sizeof(medium_sphere_indices)/sizeof(int);
+				graphicsShapeIndex = ci.m_instancingRenderer->registerShape(&medium_sphere_vertices[0],numVertices,medium_sphere_indices,numIndices);
+			} else
+			{
+				int numVertices = sizeof(low_sphere_vertices)/strideInBytes;
+				int numIndices = sizeof(low_sphere_indices)/sizeof(int);
+				graphicsShapeIndex = ci.m_instancingRenderer->registerShape(&low_sphere_vertices[0],numVertices,low_sphere_indices,numIndices);
+			}
+		}
+	}
+	return graphicsShapeIndex;
+}

demo/gpudemo/GpuDemo.h

@@ -38,9 +38,9 @@ public:
                     :useOpenCL(true),
                     preferredOpenCLPlatformIndex(-1),
                     preferredOpenCLDeviceIndex(-1),
-					arraySizeX(15),
+					arraySizeX(41),
-		arraySizeY(15),
+		arraySizeY(41),
-		arraySizeZ(15),
+		arraySizeZ(41),
 		m_useConcaveMesh(false),
 		gapX(14.3),
 		gapY(14.0),
@@ -65,6 +65,7 @@ public:
 	
 	virtual void clientMoveAndDisplay()=0;
 
+	int	registerGraphicsSphereShape(const ConstructionInfo& ci, float radius, bool usePointSprites=true, int largeSphereThreshold=100, int mediumSphereThreshold=10);
 
 };
 

demo/gpudemo/main_opengl3core.cpp

@@ -66,15 +66,15 @@ int selectedDemo = 0;
 GpuDemo::CreateFunc* allDemos[]=
 {
 
-	GpuSphereScene::MyCreateFunc,
-	GpuCompoundScene::MyCreateFunc,
 	
+	GpuSphereScene::MyCreateFunc,
 	
 	
 	GpuConvexScene::MyCreateFunc,
 	ConcaveScene::MyCreateFunc,
+	ConcaveCompoundScene::MyCreateFunc,
 	
-	
+	GpuCompoundScene::MyCreateFunc,
 	PairBench::MyCreateFunc,	
 
 

demo/gpudemo/rigidbody/ConcaveScene.cpp

@@ -154,17 +154,20 @@ GLInstanceGraphicsShape* createGraphicsShapeFromWavefrontObj(objLoader* obj)
 	}
 }
 
-void ConcaveScene::setupScene(const ConstructionInfo& ci)
+
+void ConcaveScene::createConcaveMesh(const ConstructionInfo& ci)
 {
 	objLoader* objData = new objLoader();
+	//char* fileName = "data/slopedPlane100.obj";
 	//char* fileName = "data/plane100.obj";
+
 	//char* fileName = "data/teddy.obj";//"plane.obj";
 //	char* fileName = "data/sponza_closed.obj";//"plane.obj";
 	//char* fileName = "data/leoTest1.obj";
 	char* fileName = "data/samurai_monastry.obj";
 	
-	btVector3 shift(0,0,0);//150,-100,-120);
+	btVector3 shift(0,0,0);//0,230,80);//150,-100,-120);
-	btVector4 scaling(10,10,10,1);//4,4,4,1);
+	btVector4 scaling(4,4,4,1);
 	FILE* f = 0;
 
 	char relativeFileName[1024];
@@ -235,9 +238,30 @@ void ConcaveScene::setupScene(const ConstructionInfo& ci)
 
 
 
+		
+
 		}
 	}
 
+}
+
+void ConcaveScene::setupScene(const ConstructionInfo& ci)
+{
+
+	createConcaveMesh(ci);
+
+	createDynamicObjects(ci);
+	
+	float camPos[4]={0,0,0,0};//65.5,4.5,65.5,0};
+	//float camPos[4]={1,12.5,1.5,0};
+	m_instancingRenderer->setCameraPitch(45);
+	m_instancingRenderer->setCameraTargetPosition(camPos);
+	m_instancingRenderer->setCameraDistance(370);
+
+}
+
+void ConcaveScene::createDynamicObjects(const ConstructionInfo& ci)
+{
 		int strideInBytes = 9*sizeof(float);
 	int numVertices = sizeof(cube_vertices)/strideInBytes;
 	int numIndices = sizeof(cube_indices)/sizeof(int);
@@ -247,7 +271,7 @@ void ConcaveScene::setupScene(const ConstructionInfo& ci)
 	int mask=1;
 	
 	
-
+	int index=0;
 	
 
 	if (1)
@@ -287,10 +311,63 @@ void ConcaveScene::setupScene(const ConstructionInfo& ci)
 			}
 		}
 	}
-	float camPos[4]={0,0,0,0};//65.5,4.5,65.5,0};
+
+}
+
+void ConcaveCompoundScene::setupScene(const ConstructionInfo& ci)
+{
+	ConcaveScene::setupScene(ci);
+
+	float camPos[4]={0,50,0,0};//65.5,4.5,65.5,0};
 	//float camPos[4]={1,12.5,1.5,0};
 	m_instancingRenderer->setCameraPitch(45);
 	m_instancingRenderer->setCameraTargetPosition(camPos);
-	m_instancingRenderer->setCameraDistance(370);
+	m_instancingRenderer->setCameraDistance(40);
+
 
 }
+
+void ConcaveCompoundScene::createDynamicObjects(const ConstructionInfo& ci)
+{
+		btVector4 colors[4] = 
+	{
+		btVector4(1,0,0,1),
+		btVector4(0,1,0,1),
+		btVector4(0,1,1,1),
+		btVector4(1,1,0,1),
+	};
+
+	int index=0;
+	int curColor = 0;
+	float radius = 1;
+	//int colIndex = m_data->m_np->registerConvexHullShape(&cube_vertices[0],strideInBytes,numVertices, scaling);
+	int colIndex = m_data->m_np->registerSphereShape(radius);//>registerConvexHullShape(&cube_vertices[0],strideInBytes,numVertices, scaling);
+	int prevGraphicsShapeIndex = registerGraphicsSphereShape(ci,radius,false);
+
+	for (int i=0;i<ci.arraySizeX;i++)
+	{
+		for (int j=0;j<ci.arraySizeY;j++)
+		{
+			for (int k=0;k<ci.arraySizeZ;k++)
+			{
+				float mass = 1.f;
+
+				
+				btVector3 position(-(ci.arraySizeX/2)*8+i*8,50+j*8,-(ci.arraySizeZ/2)*8+k*8);
+					
+				//btVector3 position(0,-41,0);//0,0,0);//i*radius*3,-41+j*radius*3,k*radius*3);
+					
+				btQuaternion orn(0,0,0,1);
+				
+				btVector4 color = colors[curColor];
+				curColor++;
+				curColor&=3;
+				btVector4 scaling(radius,radius,radius,1);
+				int id = ci.m_instancingRenderer->registerGraphicsInstance(prevGraphicsShapeIndex,position,orn,color,scaling);
+				int pid = m_data->m_rigidBodyPipeline->registerPhysicsInstance(mass,position,orn,colIndex,index);
+				
+				index++;
+			}
+		}
+	}
+}

demo/gpudemo/rigidbody/ConcaveScene.h

@@ -22,6 +22,34 @@ public:
 
 	virtual void setupScene(const ConstructionInfo& ci);
 
+	virtual void createDynamicObjects(const ConstructionInfo& ci);
+
+	virtual void createConcaveMesh(const ConstructionInfo& ci);
+
+	
+};
+
+class ConcaveCompoundScene : public ConcaveScene
+{
+public:
+
+	ConcaveCompoundScene(){}
+	virtual ~ConcaveCompoundScene(){}
+	virtual const char* getName()
+	{
+		return "GRBConcaveCompound";
+	}
+
+	static GpuDemo* MyCreateFunc()
+	{
+		GpuDemo* demo = new ConcaveCompoundScene;
+		return demo;
+	}
+
+	virtual void setupScene(const ConstructionInfo& ci);
+
+	virtual void createDynamicObjects(const ConstructionInfo& ci);
+
 };
 
 #endif //CONCAVE_SCENE_H

demo/gpudemo/rigidbody/GpuSphereScene.cpp

@@ -35,7 +35,7 @@ void GpuSphereScene::setupScene(const ConstructionInfo& ci)
 
 	if (0)
 	{
-			float radius = 40;
+			float radius = 60;
 			int prevGraphicsShapeIndex = -1;
 		{
 
@@ -86,7 +86,7 @@ void GpuSphereScene::setupScene(const ConstructionInfo& ci)
 		float mass = 0.f;
 
 		//btVector3 position((j&1)+i*2.2,1+j*2.,(j&1)+k*2.2);
-		btVector3 position(0,-40,0);
+		btVector3 position(0,0,0);
 
 		btQuaternion orn(0,0,0,1);
 
@@ -106,40 +106,6 @@ void GpuSphereScene::setupScene(const ConstructionInfo& ci)
 
 	
 
-	{
-
-
-
-		int prevGraphicsShapeIndex = -1;
-		float radius = 41;
-		if (1)//radius>=100)
-		{
-			int numVertices = sizeof(detailed_sphere_vertices)/strideInBytes;
-			int numIndices = sizeof(detailed_sphere_indices)/sizeof(int);
-			prevGraphicsShapeIndex = ci.m_instancingRenderer->registerShape(&detailed_sphere_vertices[0],numVertices,detailed_sphere_indices,numIndices);
-		} else
-		{
-			bool usePointSprites = true;
-			if (usePointSprites)
-			{
-				int numVertices = sizeof(point_sphere_vertices)/strideInBytes;
-				int numIndices = sizeof(point_sphere_indices)/sizeof(int);
-				prevGraphicsShapeIndex = ci.m_instancingRenderer->registerShape(&point_sphere_vertices[0],numVertices,point_sphere_indices,numIndices,BT_GL_POINTS);
-			} else
-			{
-				if (radius>=10)
-				{
-					int numVertices = sizeof(medium_sphere_vertices)/strideInBytes;
-					int numIndices = sizeof(medium_sphere_indices)/sizeof(int);
-					prevGraphicsShapeIndex = ci.m_instancingRenderer->registerShape(&medium_sphere_vertices[0],numVertices,medium_sphere_indices,numIndices);
-				} else
-				{
-					int numVertices = sizeof(low_sphere_vertices)/strideInBytes;
-					int numIndices = sizeof(low_sphere_indices)/sizeof(int);
-					prevGraphicsShapeIndex = ci.m_instancingRenderer->registerShape(&low_sphere_vertices[0],numVertices,low_sphere_indices,numIndices);
-				}
-			}
-		}
 	
 
 
@@ -161,18 +127,22 @@ void GpuSphereScene::setupScene(const ConstructionInfo& ci)
 
 
 	int curColor = 0;
-		
+	float radius = 61;
 	//int colIndex = m_data->m_np->registerConvexHullShape(&cube_vertices[0],strideInBytes,numVertices, scaling);
 	int colIndex = m_data->m_np->registerSphereShape(radius);//>registerConvexHullShape(&cube_vertices[0],strideInBytes,numVertices, scaling);
-		for (int i=0;i<ci.arraySizeX;i++)
+	int prevGraphicsShapeIndex = registerGraphicsSphereShape(ci,radius,false);
+
+	//for (int i=0;i<ci.arraySizeX;i++)
 	{
-			for (int j=0;j<ci.arraySizeY;j++)
+	//	for (int j=0;j<ci.arraySizeY;j++)
 		{
-				for (int k=0;k<ci.arraySizeZ;k++)
+	//		for (int k=0;k<ci.arraySizeZ;k++)
 			{
+				int i=0,j=0,k=0;
 				float mass = 0.f;
 
-					btVector3 position((j&1)+i*142.2,-51+j*142.,(j&1)+k*142.2);
+				btVector3 position(0,0,0);
+				//btVector3 position((j&1)+i*142.2,-51+j*142.,(j&1)+k*142.2);
 				//btVector3 position(0,-41,0);//0,0,0);//i*radius*3,-41+j*radius*3,k*radius*3);
 					
 				btQuaternion orn(0,0,0,1);
@@ -188,7 +158,7 @@ void GpuSphereScene::setupScene(const ConstructionInfo& ci)
 			}
 		}
 	}
-	}
+	
 
 	if (1)
 	{
@@ -205,7 +175,7 @@ void GpuSphereScene::setupScene(const ConstructionInfo& ci)
 		//int i=0;int j=0;
 		{
 			//int colIndex = m_data->m_np->registerPlaneShape(normal,constant);//>registerConvexHullShape(&cube_vertices[0],strideInBytes,numVertices, scaling);
-			btVector4 position(2*i,k*2,2*j+8,0);
+			btVector4 position(2*i,70+k*2,2*j+8,0);
 			//btQuaternion orn(0,0,0,1);
 			btQuaternion orn(btVector3(1,0,0),0.3);
 
@@ -221,7 +191,7 @@ void GpuSphereScene::setupScene(const ConstructionInfo& ci)
 	float camPos[4]={ci.arraySizeX,ci.arraySizeY/2,ci.arraySizeZ,0};
 	//float camPos[4]={1,12.5,1.5,0};
 	m_instancingRenderer->setCameraTargetPosition(camPos);
-	m_instancingRenderer->setCameraDistance(30);
+	m_instancingRenderer->setCameraDistance(130);
 
 
 	char msg[1024];

opencl/gpu_rigidbody/host/Solver.cpp

@@ -17,7 +17,7 @@ subject to the following restrictions:
 #include "Solver.h"
 
 ///useNewBatchingKernel  is a rewritten kernel using just a single thread of the warp, for experiments
-bool useNewBatchingKernel = true;
+bool useNewBatchingKernel = false;
 
 #define SOLVER_SETUP_KERNEL_PATH "opencl/gpu_rigidbody/kernels/solverSetup.cl"
 #define SOLVER_SETUP2_KERNEL_PATH "opencl/gpu_rigidbody/kernels/solverSetup2.cl"

opencl/gpu_sat/host/ConvexHullContact.cpp

@@ -62,13 +62,18 @@ m_totalContactsOut(m_context, m_queue)
 		btAssert(errNum==CL_SUCCESS);
 
 		m_findSeparatingAxisKernel = btOpenCLUtils::compileCLKernelFromString(m_context, m_device,src, "findSeparatingAxisKernel",&errNum,satProg );
+		btAssert(m_findSeparatingAxisKernel);
+		btAssert(errNum==CL_SUCCESS);
 
 		m_findConcaveSeparatingAxisKernel = btOpenCLUtils::compileCLKernelFromString(m_context, m_device,src, "findConcaveSeparatingAxisKernel",&errNum,satProg );
-
+		btAssert(m_findConcaveSeparatingAxisKernel);
+		btAssert(errNum==CL_SUCCESS);
 		
 		m_findCompoundPairsKernel = btOpenCLUtils::compileCLKernelFromString(m_context, m_device,src, "findCompoundPairsKernel",&errNum,satProg );
-	
+		btAssert(m_findCompoundPairsKernel);
+		btAssert(errNum==CL_SUCCESS);
 		m_processCompoundPairsKernel = btOpenCLUtils::compileCLKernelFromString(m_context, m_device,src, "processCompoundPairsKernel",&errNum,satProg );
+		btAssert(m_processCompoundPairsKernel);
 		btAssert(errNum==CL_SUCCESS);
 	}
 
@@ -126,12 +131,16 @@ m_totalContactsOut(m_context, m_queue)
         
 	 {
 		 const char* primitiveContactsSrc = primitiveContactsKernelsCL;
-		cl_program primitiveContactsProg = btOpenCLUtils::compileCLProgramFromString(m_context,m_device,primitiveContactsSrc,&errNum,"","opencl/gpu_sat/kernels/primitiveContacts.cl",disableKernelCaching);
+		cl_program primitiveContactsProg = btOpenCLUtils::compileCLProgramFromString(m_context,m_device,primitiveContactsSrc,&errNum,"","opencl/gpu_sat/kernels/primitiveContacts.cl");
 		btAssert(errNum==CL_SUCCESS);
 
 		m_primitiveContactsKernel = btOpenCLUtils::compileCLKernelFromString(m_context, m_device,primitiveContactsSrc, "primitiveContactsKernel",&errNum,primitiveContactsProg,"");
 		btAssert(errNum==CL_SUCCESS);
 
+		m_findConcaveSphereContactsKernel = btOpenCLUtils::compileCLKernelFromString(m_context, m_device,primitiveContactsSrc, "findConcaveSphereContactsKernel",&errNum,primitiveContactsProg );
+		btAssert(errNum==CL_SUCCESS);
+		btAssert(m_findConcaveSphereContactsKernel);
+
 		m_processCompoundPairsPrimitivesKernel = btOpenCLUtils::compileCLKernelFromString(m_context, m_device,primitiveContactsSrc, "processCompoundPairsPrimitivesKernel",&errNum,primitiveContactsProg,"");
 		btAssert(errNum==CL_SUCCESS);
 		btAssert(m_processCompoundPairsPrimitivesKernel);
@@ -166,6 +175,9 @@ GpuSatCollision::~GpuSatCollision()
 	if (m_primitiveContactsKernel)
 		clReleaseKernel(m_primitiveContactsKernel);
     
+	if (m_findConcaveSphereContactsKernel)
+		clReleaseKernel(m_findConcaveSphereContactsKernel);
+
 	if (m_processCompoundPairsPrimitivesKernel)
 		clReleaseKernel(m_processCompoundPairsPrimitivesKernel);
 
@@ -828,6 +840,45 @@ void GpuSatCollision::computeConvexConvexContactsGPUSAT( const btOpenCLArray<btI
 		
 		
 	}
+
+
+
+	if (numConcavePairs)
+	{
+			if (numConcavePairs)
+		{
+			BT_PROFILE("findConcaveSphereContactsKernel");
+
+			btBufferInfoCL bInfo[] = { 
+				btBufferInfoCL( triangleConvexPairsOut.getBufferCL() ), 
+				btBufferInfoCL( bodyBuf->getBufferCL(),true), 
+				btBufferInfoCL( gpuCollidables.getBufferCL(),true), 
+				btBufferInfoCL( convexData.getBufferCL(),true),
+				btBufferInfoCL( gpuVertices.getBufferCL(),true),
+				btBufferInfoCL( gpuUniqueEdges.getBufferCL(),true),
+				btBufferInfoCL( gpuFaces.getBufferCL(),true),
+				btBufferInfoCL( gpuIndices.getBufferCL(),true),
+				btBufferInfoCL( clAabbsWS.getBufferCL(),true),
+				btBufferInfoCL( contactOut->getBufferCL()),
+				btBufferInfoCL( m_totalContactsOut.getBufferCL())
+			};
+
+			btLauncherCL launcher(m_queue, m_findConcaveSphereContactsKernel);
+			launcher.setBuffers( bInfo, sizeof(bInfo)/sizeof(btBufferInfoCL) );
+
+			launcher.setConst( numConcavePairs  );
+			launcher.setConst(maxContactCapacity);
+
+			int num = numConcavePairs;
+			launcher.launch1D( num);
+			clFinish(m_queue);
+			nContacts = m_totalContactsOut.at(0);
+		}
+		
+	}
+
+
+
 #ifdef __APPLE__
 	bool contactClippingOnGpu = true;
 #else
@@ -870,6 +921,7 @@ void GpuSatCollision::computeConvexConvexContactsGPUSAT( const btOpenCLArray<btI
 		}
 
 		
+
 		//convex-convex contact clipping
         if (1)
 		{

opencl/gpu_sat/host/ConvexHullContact.h

@@ -53,6 +53,8 @@ struct GpuSatCollision
 
 	cl_kernel				m_bvhTraversalKernel;
 	cl_kernel				m_primitiveContactsKernel;
+	cl_kernel				m_findConcaveSphereContactsKernel;
+
 	cl_kernel				m_processCompoundPairsPrimitivesKernel;
     
 

opencl/gpu_sat/kernels/bvhTraversal.cl

@@ -5,6 +5,7 @@
 #define SHAPE_CONCAVE_TRIMESH 5
 #define TRIANGLE_NUM_CONVEX_FACES 5
 #define SHAPE_COMPOUND_OF_CONVEX_HULLS 6
+#define SHAPE_SPHERE 7
 
 typedef unsigned int u32;
 
@@ -211,9 +212,13 @@ __kernel void   bvhTraversalKernel( __global const int2* pairs,
 	if (collidables[collidableIndexA].m_shapeType!=SHAPE_CONCAVE_TRIMESH)
 		return;
 
-	if (collidables[collidableIndexB].m_shapeType!=SHAPE_CONVEX_HULL)
+	int shapeTypeB = collidables[collidableIndexB].m_shapeType;
+		
+	if (shapeTypeB!=SHAPE_CONVEX_HULL &&
+		shapeTypeB!=SHAPE_SPHERE	)
 		return;
 
+	
 	unsigned short int quantizedQueryAabbMin[3];
 	unsigned short int quantizedQueryAabbMax[3];
 	quantizeWithClamp(quantizedQueryAabbMin,aabbs[bodyIndexB].m_min,false,bvhAabbMin, bvhAabbMax,bvhQuantization);

opencl/gpu_sat/kernels/bvhTraversal.h

@@ -7,6 +7,7 @@ static const char* bvhTraversalKernelCL= \
 "#define SHAPE_CONCAVE_TRIMESH 5\n"
 "#define TRIANGLE_NUM_CONVEX_FACES 5\n"
 "#define SHAPE_COMPOUND_OF_CONVEX_HULLS 6\n"
+"#define SHAPE_SPHERE 7\n"
 "\n"
 "typedef unsigned int u32;\n"
 "\n"
@@ -213,9 +214,13 @@ static const char* bvhTraversalKernelCL= \
 "	if (collidables[collidableIndexA].m_shapeType!=SHAPE_CONCAVE_TRIMESH)\n"
 "		return;\n"
 "\n"
-"	if (collidables[collidableIndexB].m_shapeType!=SHAPE_CONVEX_HULL)\n"
+"	int shapeTypeB = collidables[collidableIndexB].m_shapeType;\n"
+"		\n"
+"	if (shapeTypeB!=SHAPE_CONVEX_HULL &&\n"
+"		shapeTypeB!=SHAPE_SPHERE	)\n"
 "		return;\n"
 "\n"
+"	\n"
 "	unsigned short int quantizedQueryAabbMin[3];\n"
 "	unsigned short int quantizedQueryAabbMax[3];\n"
 "	quantizeWithClamp(quantizedQueryAabbMin,aabbs[bodyIndexB].m_min,false,bvhAabbMin, bvhAabbMax,bvhQuantization);\n"

opencl/gpu_sat/kernels/primitiveContacts.cl

@@ -462,19 +462,18 @@ void	computeContactSphereConvex(int pairIndex,
 	{
 		float4 normalOnSurfaceB1 = qtRotate(quat,-hitNormalWorld);
 		float4 pOnB1 = transform(&closestPnt,&pos,&quat);
-		//printf("pOnB1=%f,%f,%f\n", pOnB1.x,pOnB1.y,pOnB1.z);
+		
 		float actualDepth = minDist-radius;
 		if (actualDepth<=0.f)
 		{
-			//printf("actualDepth  = %f\n", actualDepth );
+			
-			//printf("normalOnSurfaceB1 = ,%f,%f,%f,", normalOnSurfaceB1.x,normalOnSurfaceB1.y,normalOnSurfaceB1.z);
 
 			pOnB1.w = actualDepth;
 
 			int dstIdx;
 			AppendInc( nGlobalContactsOut, dstIdx );
 		
-			//printf("maxContactCapacity=%d\n", maxContactCapacity);
+			
 			if (1)//dstIdx < maxContactCapacity)
 			{
 				__global Contact4* c = &globalContactsOut[dstIdx];
@@ -494,6 +493,8 @@ void	computeContactSphereConvex(int pairIndex,
 							
 
 
+
+									
 void	computeContactPlaneConvex(int pairIndex,
 																int bodyIndexA, int bodyIndexB, 
 																int collidableIndexA, int collidableIndexB, 
@@ -777,7 +778,7 @@ __kernel void   processCompoundPairsPrimitivesKernel( __global const int4* gpuCo
 			float sphereRadius = collidables[collidableIndexB].m_radius;
 			float4 convexPos = posA;
 			float4 convexOrn = ornA;
-			//printf("convex-sphere with radius %f\n",sphereRadius);
+			
 			computeContactSphereConvex(pairIndex, bodyIndexB, bodyIndexA , collidableIndexB,collidableIndexA, 
 										rigidBodies,collidables,convexShapes,vertices,indices,faces, globalContactsOut, nGlobalContactsOut,maxContactCapacity,
 										spherePos,sphereRadius,convexPos,convexOrn);
@@ -793,7 +794,7 @@ __kernel void   processCompoundPairsPrimitivesKernel( __global const int4* gpuCo
 			float4 convexPos = posB;
 			float4 convexOrn = ornB;
 
-			//printf("sphere-convex with radius %f\n", sphereRadius);
+			
 			computeContactSphereConvex(pairIndex, bodyIndexA, bodyIndexB, collidableIndexA, collidableIndexB, 
 										rigidBodies,collidables,convexShapes,vertices,indices,faces, globalContactsOut, nGlobalContactsOut,maxContactCapacity,
 										spherePos,sphereRadius,convexPos,convexOrn);
@@ -802,3 +803,268 @@ __kernel void   processCompoundPairsPrimitivesKernel( __global const int4* gpuCo
 		}
 	}//	if (i<numCompoundPairs)
 }
+
+
+bool pointInTriangle(const float4* vertices, const float4* normal, float4 *p )
+{
+
+	const float4* p1 = &vertices[0];
+	const float4* p2 = &vertices[1];
+	const float4* p3 = &vertices[2];
+
+	float4 edge1;	edge1 = (*p2 - *p1);
+	float4 edge2;	edge2 = ( *p3 - *p2 );
+	float4 edge3;	edge3 = ( *p1 - *p3 );
+
+	
+	float4 p1_to_p; p1_to_p = ( *p - *p1 );
+	float4 p2_to_p; p2_to_p = ( *p - *p2 );
+	float4 p3_to_p; p3_to_p = ( *p - *p3 );
+
+	float4 edge1_normal; edge1_normal = ( cross(edge1,*normal));
+	float4 edge2_normal; edge2_normal = ( cross(edge2,*normal));
+	float4 edge3_normal; edge3_normal = ( cross(edge3,*normal));
+
+	
+	
+	float r1, r2, r3;
+	r1 = dot(edge1_normal,p1_to_p );
+	r2 = dot(edge2_normal,p2_to_p );
+	r3 = dot(edge3_normal,p3_to_p );
+	
+	if ( r1 > 0 && r2 > 0 && r3 > 0 )
+		return true;
+    if ( r1 <= 0 && r2 <= 0 && r3 <= 0 ) 
+		return true;
+	return false;
+
+}
+
+
+float segmentSqrDistance(float4 from, float4 to,float4 p, float4* nearest) 
+{
+	float4 diff = p - from;
+	float4 v = to - from;
+	float t = dot(v,diff);
+	
+	if (t > 0) 
+	{
+		float dotVV = dot(v,v);
+		if (t < dotVV) 
+		{
+			t /= dotVV;
+			diff -= t*v;
+		} else 
+		{
+			t = 1;
+			diff -= v;
+		}
+	} else
+	{
+		t = 0;
+	}
+	*nearest = from + t*v;
+	return dot(diff,diff);	
+}
+
+
+void	computeContactSphereTriangle(int pairIndex,
+									int bodyIndexA, int bodyIndexB,
+									int collidableIndexA, int collidableIndexB, 
+									__global const BodyData* rigidBodies, 
+									__global const btCollidableGpu* collidables,
+									const float4* triangleVertices,
+									__global Contact4* restrict globalContactsOut,
+									counter32_t nGlobalContactsOut,
+									int maxContactCapacity,
+									float4 spherePos2,
+									float radius,
+									float4 pos,
+									float4 quat
+									)
+{
+
+	float4 invPos;
+	float4 invOrn;
+
+	trInverse(pos,quat, &invPos,&invOrn);
+	float4 spherePos = transform(&spherePos2,&invPos,&invOrn);
+	int numFaces = 3;
+	float4 closestPnt = (float4)(0, 0, 0, 0);
+	float4 hitNormalWorld = (float4)(0, 0, 0, 0);
+	float minDist = -1000000.f;
+	bool bCollide = true;
+
+	
+	//////////////////////////////////////
+
+	float4 sphereCenter;
+	sphereCenter = spherePos;
+
+	const float4* vertices = triangleVertices;
+	float contactBreakingThreshold = 0.f;//todo?
+	float radiusWithThreshold = radius + contactBreakingThreshold;
+	float4 edge10;
+	edge10 = vertices[1]-vertices[0];
+	edge10.w = 0.f;//is this needed?
+	float4 edge20;
+	edge20 = vertices[2]-vertices[0];
+	edge20.w = 0.f;//is this needed?
+	float4 normal = cross3(edge10,edge20);
+	normal = normalize(normal);
+	float4 p1ToCenter;
+	p1ToCenter = sphereCenter - vertices[0];
+	
+	float distanceFromPlane = dot(p1ToCenter,normal);
+
+	if (distanceFromPlane < 0.f)
+	{
+		//triangle facing the other way
+		distanceFromPlane *= -1.f;
+		normal *= -1.f;
+	}
+	hitNormalWorld = normal;
+
+	bool isInsideContactPlane = distanceFromPlane < radiusWithThreshold;
+	
+	// Check for contact / intersection
+	bool hasContact = false;
+	float4 contactPoint;
+	if (isInsideContactPlane) 
+	{
+	
+		if (pointInTriangle(vertices,&normal, &sphereCenter)) 
+		{
+			// Inside the contact wedge - touches a point on the shell plane
+			hasContact = true;
+			contactPoint = sphereCenter - normal*distanceFromPlane;
+			
+		} else {
+			// Could be inside one of the contact capsules
+			float contactCapsuleRadiusSqr = radiusWithThreshold*radiusWithThreshold;
+			float4 nearestOnEdge;
+			int numEdges = 3;
+			for (int i = 0; i < numEdges; i++) 
+			{
+				float4 pa =vertices[i];
+				float4 pb = vertices[(i+1)%3];
+
+				float distanceSqr = segmentSqrDistance(pa,pb,sphereCenter, &nearestOnEdge);
+				if (distanceSqr < contactCapsuleRadiusSqr) 
+				{
+					// Yep, we're inside a capsule
+					hasContact = true;
+					contactPoint = nearestOnEdge;
+					
+				}
+				
+			}
+		}
+	}
+
+	if (hasContact) 
+	{
+
+		closestPnt = contactPoint;
+		float4 contactToCenter = sphereCenter - contactPoint;
+		minDist = length(contactToCenter);
+		if (minDist>0.f)
+		{
+			hitNormalWorld = normalize(contactToCenter);//*(1./minDist);
+		}
+		bCollide  = true;
+	}
+
+
+	/////////////////////////////////////
+
+	if (bCollide && minDist > -10000)
+	{
+		
+		float4 normalOnSurfaceB1 = qtRotate(quat,-hitNormalWorld);
+		float4 pOnB1 = transform(&closestPnt,&pos,&quat);
+		float actualDepth = minDist-radius;
+
+		
+		if (actualDepth<=0.f)
+		{
+			pOnB1.w = actualDepth;
+			int dstIdx;
+			AppendInc( nGlobalContactsOut, dstIdx );
+			
+			if (dstIdx < maxContactCapacity)
+			{
+				__global Contact4* c = &globalContactsOut[dstIdx];
+				c->m_worldNormal = normalOnSurfaceB1;
+				c->m_coeffs = (u32)(0.f*0xffff) | ((u32)(0.7f*0xffff)<<16);
+				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_worldPos[0] = pOnB1;
+				GET_NPOINTS(*c) = 1;
+			} 
+
+		}
+	}//if (hasCollision)
+
+}
+
+
+
+// work-in-progress
+__kernel void   findConcaveSphereContactsKernel( __global int4* concavePairs,
+												__global const BodyData* rigidBodies,
+												__global const btCollidableGpu* collidables,
+												__global const ConvexPolyhedronCL* convexShapes, 
+												__global const float4* vertices,
+												__global const float4* uniqueEdges,
+												__global const btGpuFace* faces,
+												__global const int* indices,
+												__global btAabbCL* aabbs,
+												__global Contact4* restrict globalContactsOut,
+												counter32_t nGlobalContactsOut,
+													int numConcavePairs, int maxContactCapacity
+												)
+{
+
+	int i = get_global_id(0);
+	if (i>=numConcavePairs)
+		return;
+	int pairIdx = i;
+
+	int bodyIndexA = concavePairs[i].x;
+	int bodyIndexB = concavePairs[i].y;
+
+	int collidableIndexA = rigidBodies[bodyIndexA].m_collidableIdx;
+	int collidableIndexB = rigidBodies[bodyIndexB].m_collidableIdx;
+
+	int shapeIndexA = collidables[collidableIndexA].m_shapeIndex;
+	int shapeIndexB = collidables[collidableIndexB].m_shapeIndex;
+
+	if (collidables[collidableIndexB].m_shapeType==SHAPE_SPHERE)
+	{
+		int f = concavePairs[i].z;
+		btGpuFace face = faces[convexShapes[shapeIndexA].m_faceOffset+f];
+		
+		float4 verticesA[3];
+		for (int i=0;i<3;i++)
+		{
+			int index = indices[face.m_indexOffset+i];
+			float4 vert = vertices[convexShapes[shapeIndexA].m_vertexOffset+index];
+			verticesA[i] = vert;
+		}
+
+		float4 spherePos = rigidBodies[bodyIndexB].m_pos;
+		float sphereRadius = collidables[collidableIndexB].m_radius;
+		float4 convexPos = rigidBodies[bodyIndexA].m_pos;
+		float4 convexOrn = rigidBodies[bodyIndexA].m_quat;
+
+		computeContactSphereTriangle(i, bodyIndexB, bodyIndexA, collidableIndexB, collidableIndexA, 
+																rigidBodies,collidables,
+																verticesA,
+																globalContactsOut, nGlobalContactsOut,maxContactCapacity,
+																spherePos,sphereRadius,convexPos,convexOrn);
+
+		return;
+	}
+}

opencl/gpu_sat/kernels/primitiveContacts.h

@@ -53,6 +53,21 @@ static const char* primitiveContactsKernelsCL= \
 "	int m_bodyBPtrAndSignBit;\n"
 "} Contact4;\n"
 "\n"
+"typedef struct \n"
+"{\n"
+"	union\n"
+"	{\n"
+"		float4	m_min;\n"
+"		float   m_minElems[4];\n"
+"		int			m_minIndices[4];\n"
+"	};\n"
+"	union\n"
+"	{\n"
+"		float4	m_max;\n"
+"		float   m_maxElems[4];\n"
+"		int			m_maxIndices[4];\n"
+"	};\n"
+"} btAabbCL;\n"
 "\n"
 "///keep this in sync with btCollidable.h\n"
 "typedef struct\n"
@@ -275,8 +290,6 @@ static const char* primitiveContactsKernelsCL= \
 "\n"
 "\n"
 "inline bool IsPointInPolygon(float4 p, \n"
-"							float4	posConvex,\n"
-"							float4	ornConvex,\n"
 "							const btGpuFace* face,\n"
 "							__global const float4* baseVertex,\n"
 "							__global const  int* convexIndices,\n"
@@ -295,16 +308,14 @@ static const char* primitiveContactsKernelsCL= \
 "\n"
 "	\n"
 "	float4 v0 = baseVertex[convexIndices[face->m_indexOffset + face->m_numIndices-1]];\n"
-"	float4 worldV0 = transform(&v0, &posConvex, &ornConvex);\n"
 "	\n"
-"	b = worldV0;\n"
+"	b = v0;\n"
 "\n"
 "    for(unsigned i=0; i != face->m_numIndices; ++i)\n"
 "    {\n"
 "		a = b;\n"
 "		float4 vi = baseVertex[convexIndices[face->m_indexOffset + i]];\n"
-"		float4 worldVi = transform(&vi, &posConvex, &ornConvex);\n"
+"		b = vi;\n"
-"		b = worldVi;\n"
 "        ab = b-a;\n"
 "        ap = p-a;\n"
 "        v = cross3(ab,plane);\n"
@@ -348,22 +359,22 @@ static const char* primitiveContactsKernelsCL= \
 "																__global const btGpuFace* faces,\n"
 "																__global Contact4* restrict globalContactsOut,\n"
 "																counter32_t nGlobalContactsOut,\n"
-"																int numPairs)\n"
+"																int maxContactCapacity,\n"
+"																float4 spherePos2,\n"
+"																float radius,\n"
+"																float4 pos,\n"
+"																float4 quat\n"
+"																)\n"
 "{\n"
 "\n"
-"	float radius = collidables[collidableIndexA].m_radius;\n"
+"	float4 invPos;\n"
-"	float4 spherePos1 = rigidBodies[bodyIndexA].m_pos;\n"
+"	float4 invOrn;\n"
-"	float4 sphereOrn = rigidBodies[bodyIndexA].m_quat;\n"
 "\n"
+"	trInverse(pos,quat, &invPos,&invOrn);\n"
 "\n"
+"	float4 spherePos = transform(&spherePos2,&invPos,&invOrn);\n"
 "\n"
-"	float4 pos = rigidBodies[bodyIndexB].m_pos;\n"
+"	int shapeIndex = collidables[collidableIndexB].m_shapeIndex;\n"
-"	float4 quat = rigidBodies[bodyIndexB].m_quat;\n"
-"\n"
-"	float4 spherePos = spherePos1 - pos;\n"
-"\n"
-"	int collidableIndex = rigidBodies[bodyIndexB].m_collidableIdx;\n"
-"	int shapeIndex = collidables[collidableIndex].m_shapeIndex;\n"
 "	int numFaces = convexShapes[shapeIndex].m_numFaces;\n"
 "	float4 closestPnt = (float4)(0, 0, 0, 0);\n"
 "	float4 hitNormalWorld = (float4)(0, 0, 0, 0);\n"
@@ -376,7 +387,8 @@ static const char* primitiveContactsKernelsCL= \
 "\n"
 "		// set up a plane equation \n"
 "		float4 planeEqn;\n"
-"		float4 n1 = qtRotate(quat, (float4)(face.m_plane.xyz, 0));\n"
+"		float4 n1 = face.m_plane;\n"
+"		n1.w = 0.f;\n"
 "		planeEqn = n1;\n"
 "		planeEqn.w = face.m_plane.w;\n"
 "		\n"
@@ -397,9 +409,9 @@ static const char* primitiveContactsKernelsCL= \
 "		{\n"
 "			//might hit an edge or vertex\n"
 "			float4 out;\n"
+"			float4 zeroPos = make_float4(0,0,0,0);\n"
+"\n"
 "			bool isInPoly = IsPointInPolygon(spherePos,\n"
-"					pos,\n"
-"					quat,\n"
 "					&face,\n"
 "					&convexVertices[convexShapes[shapeIndex].m_vertexOffset],\n"
 "					convexIndices,\n"
@@ -448,17 +460,23 @@ static const char* primitiveContactsKernelsCL= \
 "\n"
 "	\n"
 "\n"
-"	if (bCollide)\n"
+"	if (bCollide && minDist > -10000)\n"
 "	{\n"
-"		float4 normalOnSurfaceB1 = -hitNormalWorld;\n"
+"		float4 normalOnSurfaceB1 = qtRotate(quat,-hitNormalWorld);\n"
-"		float4 pOnB1 = closestPnt+pos;\n"
+"		float4 pOnB1 = transform(&closestPnt,&pos,&quat);\n"
+"		\n"
 "		float actualDepth = minDist-radius;\n"
+"		if (actualDepth<=0.f)\n"
+"		{\n"
+"			\n"
+"\n"
 "			pOnB1.w = actualDepth;\n"
 "\n"
 "			int dstIdx;\n"
 "			AppendInc( nGlobalContactsOut, dstIdx );\n"
 "		\n"
-"		if (dstIdx < numPairs)\n"
+"			\n"
+"			if (1)//dstIdx < maxContactCapacity)\n"
 "			{\n"
 "				__global Contact4* c = &globalContactsOut[dstIdx];\n"
 "				c->m_worldNormal = normalOnSurfaceB1;\n"
@@ -468,13 +486,17 @@ static const char* primitiveContactsKernelsCL= \
 "				c->m_bodyBPtrAndSignBit = rigidBodies[bodyIndexB].m_invMass==0?-bodyIndexB:bodyIndexB;\n"
 "				c->m_worldPos[0] = pOnB1;\n"
 "				GET_NPOINTS(*c) = 1;\n"
-"		}//if (dstIdx < numPairs)\n"
+"			} \n"
+"\n"
+"		}\n"
 "	}//if (hasCollision)\n"
 "\n"
 "}\n"
 "							\n"
 "\n"
 "\n"
+"\n"
+"									\n"
 "void	computeContactPlaneConvex(int pairIndex,\n"
 "																int bodyIndexA, int bodyIndexB, \n"
 "																int collidableIndexA, int collidableIndexB, \n"
@@ -483,7 +505,7 @@ static const char* primitiveContactsKernelsCL= \
 "																__global const btGpuFace* faces,\n"
 "																__global Contact4* restrict globalContactsOut,\n"
 "																counter32_t nGlobalContactsOut,\n"
-"																int numPairs)\n"
+"																int maxContactCapacity)\n"
 "{\n"
 "	float4 planeEq = faces[collidables[collidableIndexA].m_shapeIndex].m_plane;\n"
 "	float radius = collidables[collidableIndexB].m_radius;\n"
@@ -522,7 +544,7 @@ static const char* primitiveContactsKernelsCL= \
 "		int dstIdx;\n"
 "    AppendInc( nGlobalContactsOut, dstIdx );\n"
 "		\n"
-"		if (dstIdx < numPairs)\n"
+"		if (dstIdx < maxContactCapacity)\n"
 "		{\n"
 "			__global Contact4* c = &globalContactsOut[dstIdx];\n"
 "			c->m_worldNormal = normalOnSurfaceB1;\n"
@@ -549,7 +571,7 @@ static const char* primitiveContactsKernelsCL= \
 "																					__global const int* indices,\n"
 "																					__global Contact4* restrict globalContactsOut,\n"
 "																					counter32_t nGlobalContactsOut,\n"
-"																					int numPairs)\n"
+"																					int numPairs, int maxContactCapacity)\n"
 "{\n"
 "\n"
 "	int i = get_global_id(0);\n"
@@ -581,7 +603,7 @@ static const char* primitiveContactsKernelsCL= \
 "\n"
 "\n"
 "			computeContactPlaneConvex( pairIndex, bodyIndexB,bodyIndexA,  collidableIndexB,collidableIndexA, \n"
-"																rigidBodies,collidables,faces,	globalContactsOut, nGlobalContactsOut,numPairs);\n"
+"																rigidBodies,collidables,faces,	globalContactsOut, nGlobalContactsOut,maxContactCapacity);\n"
 "			return;\n"
 "		}\n"
 "\n"
@@ -591,7 +613,7 @@ static const char* primitiveContactsKernelsCL= \
 "\n"
 "\n"
 "			computeContactPlaneConvex(pairIndex, bodyIndexA, bodyIndexB, collidableIndexA, collidableIndexB, \n"
-"																rigidBodies,collidables,faces, globalContactsOut, nGlobalContactsOut,numPairs);\n"
+"																rigidBodies,collidables,faces, globalContactsOut, nGlobalContactsOut,maxContactCapacity);\n"
 "			return;\n"
 "			\n"
 "		}\n"
@@ -600,8 +622,15 @@ static const char* primitiveContactsKernelsCL= \
 "			collidables[collidableIndexB].m_shapeType == SHAPE_CONVEX_HULL)\n"
 "		{\n"
 "		\n"
+"			float4 spherePos = rigidBodies[bodyIndexA].m_pos;\n"
+"			float sphereRadius = collidables[collidableIndexA].m_radius;\n"
+"			float4 convexPos = rigidBodies[bodyIndexB].m_pos;\n"
+"			float4 convexOrn = rigidBodies[bodyIndexB].m_quat;\n"
+"\n"
 "			computeContactSphereConvex(pairIndex, bodyIndexA, bodyIndexB, collidableIndexA, collidableIndexB, \n"
-"																rigidBodies,collidables,convexShapes,vertices,indices,faces, globalContactsOut, nGlobalContactsOut,numPairs);\n"
+"																rigidBodies,collidables,convexShapes,vertices,indices,faces, globalContactsOut, nGlobalContactsOut,maxContactCapacity,\n"
+"																spherePos,sphereRadius,convexPos,convexOrn);\n"
+"\n"
 "			return;\n"
 "		}\n"
 "\n"
@@ -609,8 +638,14 @@ static const char* primitiveContactsKernelsCL= \
 "			collidables[collidableIndexB].m_shapeType == SHAPE_SPHERE)\n"
 "		{\n"
 "		\n"
+"			float4 spherePos = rigidBodies[bodyIndexB].m_pos;\n"
+"			float sphereRadius = collidables[collidableIndexB].m_radius;\n"
+"			float4 convexPos = rigidBodies[bodyIndexA].m_pos;\n"
+"			float4 convexOrn = rigidBodies[bodyIndexA].m_quat;\n"
+"\n"
 "			computeContactSphereConvex(pairIndex, bodyIndexB, bodyIndexA, collidableIndexB, collidableIndexA, \n"
-"																rigidBodies,collidables,convexShapes,vertices,indices,faces, globalContactsOut, nGlobalContactsOut,numPairs);\n"
+"																rigidBodies,collidables,convexShapes,vertices,indices,faces, globalContactsOut, nGlobalContactsOut,maxContactCapacity,\n"
+"																spherePos,sphereRadius,convexPos,convexOrn);\n"
 "			return;\n"
 "		}\n"
 "	\n"
@@ -646,7 +681,7 @@ static const char* primitiveContactsKernelsCL= \
 "				int dstIdx;\n"
 "				 AppendInc( nGlobalContactsOut, dstIdx );\n"
 "				\n"
-"				if (dstIdx < numPairs)\n"
+"				if (dstIdx < maxContactCapacity)\n"
 "				{\n"
 "					__global Contact4* c = &globalContactsOut[dstIdx];\n"
 "					c->m_worldNormal = -normalOnSurfaceB;\n"
@@ -668,4 +703,371 @@ static const char* primitiveContactsKernelsCL= \
 "\n"
 "}\n"
 "\n"
+"\n"
+"// work-in-progress\n"
+"__kernel void   processCompoundPairsPrimitivesKernel( __global const int4* gpuCompoundPairs,\n"
+"													__global const BodyData* rigidBodies, \n"
+"													__global const btCollidableGpu* collidables,\n"
+"													__global const ConvexPolyhedronCL* convexShapes, \n"
+"													__global const float4* vertices,\n"
+"													__global const float4* uniqueEdges,\n"
+"													__global const btGpuFace* faces,\n"
+"													__global const int* indices,\n"
+"													__global btAabbCL* aabbs,\n"
+"													__global const btGpuChildShape* gpuChildShapes,\n"
+"													__global Contact4* restrict globalContactsOut,\n"
+"													counter32_t nGlobalContactsOut,\n"
+"													int numCompoundPairs, int maxContactCapacity\n"
+"													)\n"
+"{\n"
+"\n"
+"	int i = get_global_id(0);\n"
+"	if (i<numCompoundPairs)\n"
+"	{\n"
+"		int bodyIndexA = gpuCompoundPairs[i].x;\n"
+"		int bodyIndexB = gpuCompoundPairs[i].y;\n"
+"\n"
+"		int childShapeIndexA = gpuCompoundPairs[i].z;\n"
+"		int childShapeIndexB = gpuCompoundPairs[i].w;\n"
+"		\n"
+"		int collidableIndexA = -1;\n"
+"		int collidableIndexB = -1;\n"
+"		\n"
+"		float4 ornA = rigidBodies[bodyIndexA].m_quat;\n"
+"		float4 posA = rigidBodies[bodyIndexA].m_pos;\n"
+"		\n"
+"		float4 ornB = rigidBodies[bodyIndexB].m_quat;\n"
+"		float4 posB = rigidBodies[bodyIndexB].m_pos;\n"
+"							\n"
+"		if (childShapeIndexA >= 0)\n"
+"		{\n"
+"			collidableIndexA = gpuChildShapes[childShapeIndexA].m_shapeIndex;\n"
+"			float4 childPosA = gpuChildShapes[childShapeIndexA].m_childPosition;\n"
+"			float4 childOrnA = gpuChildShapes[childShapeIndexA].m_childOrientation;\n"
+"			float4 newPosA = qtRotate(ornA,childPosA)+posA;\n"
+"			float4 newOrnA = qtMul(ornA,childOrnA);\n"
+"			posA = newPosA;\n"
+"			ornA = newOrnA;\n"
+"		} else\n"
+"		{\n"
+"			collidableIndexA = rigidBodies[bodyIndexA].m_collidableIdx;\n"
+"		}\n"
+"		\n"
+"		if (childShapeIndexB>=0)\n"
+"		{\n"
+"			collidableIndexB = gpuChildShapes[childShapeIndexB].m_shapeIndex;\n"
+"			float4 childPosB = gpuChildShapes[childShapeIndexB].m_childPosition;\n"
+"			float4 childOrnB = gpuChildShapes[childShapeIndexB].m_childOrientation;\n"
+"			float4 newPosB = transform(&childPosB,&posB,&ornB);\n"
+"			float4 newOrnB = qtMul(ornB,childOrnB);\n"
+"			posB = newPosB;\n"
+"			ornB = newOrnB;\n"
+"		} else\n"
+"		{\n"
+"			collidableIndexB = rigidBodies[bodyIndexB].m_collidableIdx;	\n"
+"		}\n"
+"	\n"
+"		int shapeIndexA = collidables[collidableIndexA].m_shapeIndex;\n"
+"		int shapeIndexB = collidables[collidableIndexB].m_shapeIndex;\n"
+"	\n"
+"		int shapeTypeA = collidables[collidableIndexA].m_shapeType;\n"
+"		int shapeTypeB = collidables[collidableIndexB].m_shapeType;\n"
+"\n"
+"		int pairIndex = i;\n"
+"		if ((shapeTypeA == SHAPE_CONVEX_HULL) && (shapeTypeB == SHAPE_SPHERE))\n"
+"		{\n"
+"			float4 spherePos = rigidBodies[bodyIndexB].m_pos;\n"
+"			float sphereRadius = collidables[collidableIndexB].m_radius;\n"
+"			float4 convexPos = posA;\n"
+"			float4 convexOrn = ornA;\n"
+"			\n"
+"			computeContactSphereConvex(pairIndex, bodyIndexB, bodyIndexA , collidableIndexB,collidableIndexA, \n"
+"										rigidBodies,collidables,convexShapes,vertices,indices,faces, globalContactsOut, nGlobalContactsOut,maxContactCapacity,\n"
+"										spherePos,sphereRadius,convexPos,convexOrn);\n"
+"	\n"
+"			return;\n"
+"		}\n"
+"\n"
+"		if ((shapeTypeA == SHAPE_SPHERE) && (shapeTypeB == SHAPE_CONVEX_HULL))\n"
+"		{\n"
+"\n"
+"			float4 spherePos = rigidBodies[bodyIndexA].m_pos;\n"
+"			float sphereRadius = collidables[collidableIndexA].m_radius;\n"
+"			float4 convexPos = posB;\n"
+"			float4 convexOrn = ornB;\n"
+"\n"
+"			\n"
+"			computeContactSphereConvex(pairIndex, bodyIndexA, bodyIndexB, collidableIndexA, collidableIndexB, \n"
+"										rigidBodies,collidables,convexShapes,vertices,indices,faces, globalContactsOut, nGlobalContactsOut,maxContactCapacity,\n"
+"										spherePos,sphereRadius,convexPos,convexOrn);\n"
+"	\n"
+"			return;\n"
+"		}\n"
+"	}//	if (i<numCompoundPairs)\n"
+"}\n"
+"\n"
+"\n"
+"bool pointInTriangle(const float4* vertices, const float4* normal, float4 *p )\n"
+"{\n"
+"\n"
+"	const float4* p1 = &vertices[0];\n"
+"	const float4* p2 = &vertices[1];\n"
+"	const float4* p3 = &vertices[2];\n"
+"\n"
+"	float4 edge1;	edge1 = (*p2 - *p1);\n"
+"	float4 edge2;	edge2 = ( *p3 - *p2 );\n"
+"	float4 edge3;	edge3 = ( *p1 - *p3 );\n"
+"\n"
+"	\n"
+"	float4 p1_to_p; p1_to_p = ( *p - *p1 );\n"
+"	float4 p2_to_p; p2_to_p = ( *p - *p2 );\n"
+"	float4 p3_to_p; p3_to_p = ( *p - *p3 );\n"
+"\n"
+"	float4 edge1_normal; edge1_normal = ( cross(edge1,*normal));\n"
+"	float4 edge2_normal; edge2_normal = ( cross(edge2,*normal));\n"
+"	float4 edge3_normal; edge3_normal = ( cross(edge3,*normal));\n"
+"\n"
+"	\n"
+"	\n"
+"	float r1, r2, r3;\n"
+"	r1 = dot(edge1_normal,p1_to_p );\n"
+"	r2 = dot(edge2_normal,p2_to_p );\n"
+"	r3 = dot(edge3_normal,p3_to_p );\n"
+"	\n"
+"	if ( r1 > 0 && r2 > 0 && r3 > 0 )\n"
+"		return true;\n"
+"    if ( r1 <= 0 && r2 <= 0 && r3 <= 0 ) \n"
+"		return true;\n"
+"	return false;\n"
+"\n"
+"}\n"
+"\n"
+"\n"
+"float segmentSqrDistance(float4 from, float4 to,float4 p, float4* nearest) \n"
+"{\n"
+"	float4 diff = p - from;\n"
+"	float4 v = to - from;\n"
+"	float t = dot(v,diff);\n"
+"	\n"
+"	if (t > 0) \n"
+"	{\n"
+"		float dotVV = dot(v,v);\n"
+"		if (t < dotVV) \n"
+"		{\n"
+"			t /= dotVV;\n"
+"			diff -= t*v;\n"
+"		} else \n"
+"		{\n"
+"			t = 1;\n"
+"			diff -= v;\n"
+"		}\n"
+"	} else\n"
+"	{\n"
+"		t = 0;\n"
+"	}\n"
+"	*nearest = from + t*v;\n"
+"	return dot(diff,diff);	\n"
+"}\n"
+"\n"
+"\n"
+"void	computeContactSphereTriangle(int pairIndex,\n"
+"									int bodyIndexA, int bodyIndexB,\n"
+"									int collidableIndexA, int collidableIndexB, \n"
+"									__global const BodyData* rigidBodies, \n"
+"									__global const btCollidableGpu* collidables,\n"
+"									const float4* triangleVertices,\n"
+"									__global Contact4* restrict globalContactsOut,\n"
+"									counter32_t nGlobalContactsOut,\n"
+"									int maxContactCapacity,\n"
+"									float4 spherePos2,\n"
+"									float radius,\n"
+"									float4 pos,\n"
+"									float4 quat\n"
+"									)\n"
+"{\n"
+"\n"
+"	float4 invPos;\n"
+"	float4 invOrn;\n"
+"\n"
+"	trInverse(pos,quat, &invPos,&invOrn);\n"
+"	float4 spherePos = transform(&spherePos2,&invPos,&invOrn);\n"
+"	int numFaces = 3;\n"
+"	float4 closestPnt = (float4)(0, 0, 0, 0);\n"
+"	float4 hitNormalWorld = (float4)(0, 0, 0, 0);\n"
+"	float minDist = -1000000.f;\n"
+"	bool bCollide = true;\n"
+"\n"
+"	\n"
+"	//////////////////////////////////////\n"
+"\n"
+"	float4 sphereCenter;\n"
+"	sphereCenter = spherePos;\n"
+"\n"
+"	const float4* vertices = triangleVertices;\n"
+"	float contactBreakingThreshold = 0.f;//todo?\n"
+"	float radiusWithThreshold = radius + contactBreakingThreshold;\n"
+"	float4 edge10;\n"
+"	edge10 = vertices[1]-vertices[0];\n"
+"	edge10.w = 0.f;//is this needed?\n"
+"	float4 edge20;\n"
+"	edge20 = vertices[2]-vertices[0];\n"
+"	edge20.w = 0.f;//is this needed?\n"
+"	float4 normal = cross3(edge10,edge20);\n"
+"	normal = normalize(normal);\n"
+"	float4 p1ToCenter;\n"
+"	p1ToCenter = sphereCenter - vertices[0];\n"
+"	\n"
+"	float distanceFromPlane = dot(p1ToCenter,normal);\n"
+"\n"
+"	if (distanceFromPlane < 0.f)\n"
+"	{\n"
+"		//triangle facing the other way\n"
+"		distanceFromPlane *= -1.f;\n"
+"		normal *= -1.f;\n"
+"	}\n"
+"	hitNormalWorld = normal;\n"
+"\n"
+"	bool isInsideContactPlane = distanceFromPlane < radiusWithThreshold;\n"
+"	\n"
+"	// Check for contact / intersection\n"
+"	bool hasContact = false;\n"
+"	float4 contactPoint;\n"
+"	if (isInsideContactPlane) \n"
+"	{\n"
+"	\n"
+"		if (pointInTriangle(vertices,&normal, &sphereCenter)) \n"
+"		{\n"
+"			// Inside the contact wedge - touches a point on the shell plane\n"
+"			hasContact = true;\n"
+"			contactPoint = sphereCenter - normal*distanceFromPlane;\n"
+"			\n"
+"		} else {\n"
+"			// Could be inside one of the contact capsules\n"
+"			float contactCapsuleRadiusSqr = radiusWithThreshold*radiusWithThreshold;\n"
+"			float4 nearestOnEdge;\n"
+"			int numEdges = 3;\n"
+"			for (int i = 0; i < numEdges; i++) \n"
+"			{\n"
+"				float4 pa =vertices[i];\n"
+"				float4 pb = vertices[(i+1)%3];\n"
+"\n"
+"				float distanceSqr = segmentSqrDistance(pa,pb,sphereCenter, &nearestOnEdge);\n"
+"				if (distanceSqr < contactCapsuleRadiusSqr) \n"
+"				{\n"
+"					// Yep, we're inside a capsule\n"
+"					hasContact = true;\n"
+"					contactPoint = nearestOnEdge;\n"
+"					\n"
+"				}\n"
+"				\n"
+"			}\n"
+"		}\n"
+"	}\n"
+"\n"
+"	if (hasContact) \n"
+"	{\n"
+"\n"
+"		closestPnt = contactPoint;\n"
+"		float4 contactToCenter = sphereCenter - contactPoint;\n"
+"		minDist = length(contactToCenter);\n"
+"		if (minDist>0.f)\n"
+"		{\n"
+"			hitNormalWorld = normalize(contactToCenter);//*(1./minDist);\n"
+"		}\n"
+"		bCollide  = true;\n"
+"	}\n"
+"\n"
+"\n"
+"	/////////////////////////////////////\n"
+"\n"
+"	if (bCollide && minDist > -10000)\n"
+"	{\n"
+"		\n"
+"		float4 normalOnSurfaceB1 = qtRotate(quat,-hitNormalWorld);\n"
+"		float4 pOnB1 = transform(&closestPnt,&pos,&quat);\n"
+"		float actualDepth = minDist-radius;\n"
+"\n"
+"		\n"
+"		if (actualDepth<=0.f)\n"
+"		{\n"
+"			pOnB1.w = actualDepth;\n"
+"			int dstIdx;\n"
+"			AppendInc( nGlobalContactsOut, dstIdx );\n"
+"			\n"
+"			if (dstIdx < maxContactCapacity)\n"
+"			{\n"
+"				__global Contact4* c = &globalContactsOut[dstIdx];\n"
+"				c->m_worldNormal = normalOnSurfaceB1;\n"
+"				c->m_coeffs = (u32)(0.f*0xffff) | ((u32)(0.7f*0xffff)<<16);\n"
+"				c->m_batchIdx = pairIndex;\n"
+"				c->m_bodyAPtrAndSignBit = rigidBodies[bodyIndexA].m_invMass==0?-bodyIndexA:bodyIndexA;\n"
+"				c->m_bodyBPtrAndSignBit = rigidBodies[bodyIndexB].m_invMass==0?-bodyIndexB:bodyIndexB;\n"
+"				c->m_worldPos[0] = pOnB1;\n"
+"				GET_NPOINTS(*c) = 1;\n"
+"			} \n"
+"\n"
+"		}\n"
+"	}//if (hasCollision)\n"
+"\n"
+"}\n"
+"\n"
+"\n"
+"\n"
+"// work-in-progress\n"
+"__kernel void   findConcaveSphereContactsKernel( __global int4* concavePairs,\n"
+"												__global const BodyData* rigidBodies,\n"
+"												__global const btCollidableGpu* collidables,\n"
+"												__global const ConvexPolyhedronCL* convexShapes, \n"
+"												__global const float4* vertices,\n"
+"												__global const float4* uniqueEdges,\n"
+"												__global const btGpuFace* faces,\n"
+"												__global const int* indices,\n"
+"												__global btAabbCL* aabbs,\n"
+"												__global Contact4* restrict globalContactsOut,\n"
+"												counter32_t nGlobalContactsOut,\n"
+"													int numConcavePairs, int maxContactCapacity\n"
+"												)\n"
+"{\n"
+"\n"
+"	int i = get_global_id(0);\n"
+"	if (i>=numConcavePairs)\n"
+"		return;\n"
+"	int pairIdx = i;\n"
+"\n"
+"	int bodyIndexA = concavePairs[i].x;\n"
+"	int bodyIndexB = concavePairs[i].y;\n"
+"\n"
+"	int collidableIndexA = rigidBodies[bodyIndexA].m_collidableIdx;\n"
+"	int collidableIndexB = rigidBodies[bodyIndexB].m_collidableIdx;\n"
+"\n"
+"	int shapeIndexA = collidables[collidableIndexA].m_shapeIndex;\n"
+"	int shapeIndexB = collidables[collidableIndexB].m_shapeIndex;\n"
+"\n"
+"	if (collidables[collidableIndexB].m_shapeType==SHAPE_SPHERE)\n"
+"	{\n"
+"		int f = concavePairs[i].z;\n"
+"		btGpuFace face = faces[convexShapes[shapeIndexA].m_faceOffset+f];\n"
+"		\n"
+"		float4 verticesA[3];\n"
+"		for (int i=0;i<3;i++)\n"
+"		{\n"
+"			int index = indices[face.m_indexOffset+i];\n"
+"			float4 vert = vertices[convexShapes[shapeIndexA].m_vertexOffset+index];\n"
+"			verticesA[i] = vert;\n"
+"		}\n"
+"\n"
+"		float4 spherePos = rigidBodies[bodyIndexB].m_pos;\n"
+"		float sphereRadius = collidables[collidableIndexB].m_radius;\n"
+"		float4 convexPos = rigidBodies[bodyIndexA].m_pos;\n"
+"		float4 convexOrn = rigidBodies[bodyIndexA].m_quat;\n"
+"\n"
+"		computeContactSphereTriangle(i, bodyIndexB, bodyIndexA, collidableIndexB, collidableIndexA, \n"
+"																rigidBodies,collidables,\n"
+"																verticesA,\n"
+"																globalContactsOut, nGlobalContactsOut,maxContactCapacity,\n"
+"																spherePos,sphereRadius,convexPos,convexOrn);\n"
+"\n"
+"		return;\n"
+"	}\n"
+"}\n"
 ;

opencl/gpu_sat/kernels/sat.cl

@@ -1081,6 +1081,13 @@ __kernel void   findConcaveSeparatingAxisKernel( __global int4* concavePairs,
 	int shapeIndexA = collidables[collidableIndexA].m_shapeIndex;
 	int shapeIndexB = collidables[collidableIndexB].m_shapeIndex;
 
+	if (collidables[collidableIndexB].m_shapeType!=SHAPE_CONVEX_HULL)
+	{
+		concavePairs[pairIdx].w = 0;
+		return;
+	}
+
+
 
 	int numFacesA = convexShapes[shapeIndexA].m_numFaces;
 	int numActualConcaveConvexTests = 0;

opencl/gpu_sat/kernels/satKernels.h

@@ -708,6 +708,15 @@ static const char* satKernelsCL= \
 "		int shapeIndexA = collidables[collidableIndexA].m_shapeIndex;\n"
 "		int shapeIndexB = collidables[collidableIndexB].m_shapeIndex;\n"
 "	\n"
+"		int shapeTypeA = collidables[collidableIndexA].m_shapeType;\n"
+"		int shapeTypeB = collidables[collidableIndexB].m_shapeType;\n"
+"	\n"
+"\n"
+"		if ((shapeTypeA != SHAPE_CONVEX_HULL) || (shapeTypeB != SHAPE_CONVEX_HULL))\n"
+"		{\n"
+"			return;\n"
+"		}\n"
+"\n"
 "		int hasSeparatingAxis = 5;\n"
 "							\n"
 "		int numFacesA = convexShapes[shapeIndexA].m_numFaces;\n"
@@ -1074,6 +1083,13 @@ static const char* satKernelsCL= \
 "	int shapeIndexA = collidables[collidableIndexA].m_shapeIndex;\n"
 "	int shapeIndexB = collidables[collidableIndexB].m_shapeIndex;\n"
 "\n"
+"	if (collidables[collidableIndexB].m_shapeType!=SHAPE_CONVEX_HULL)\n"
+"	{\n"
+"		concavePairs[pairIdx].w = 0;\n"
+"		return;\n"
+"	}\n"
+"\n"
+"\n"
 "\n"
 "	int numFacesA = convexShapes[shapeIndexA].m_numFaces;\n"
 "	int numActualConcaveConvexTests = 0;\n"