add convex versus plane contact generation

demo/gpudemo/GpuDemo.h

@@ -38,9 +38,9 @@ public:
                     :useOpenCL(true),
                     preferredOpenCLPlatformIndex(-1),
                     preferredOpenCLDeviceIndex(-1),
-					arraySizeX(1),
+					arraySizeX(10),
-		arraySizeY(1),
+		arraySizeY(31),
-		arraySizeZ(1),
+		arraySizeZ(10),
 		m_useConcaveMesh(false),
 		gapX(14.3),
 		gapY(14.0),

demo/gpudemo/rigidbody/GpuConvexScene.cpp

@@ -59,7 +59,8 @@ void GpuConvexScene::setupScene(const ConstructionInfo& ci)
 				{
 					float mass = 1.f;
 
-					btVector3 position((j&1)+i*2.2,2+j*2.,(j&1)+k*2.2);
+					btVector3 position((j&1)+i*2.2,1+j*2.,(j&1)+k*2.2);
+					//btVector3 position(i*2.2,1+j*2.,k*2.2);
 					
 					btQuaternion orn(0,0,0,1);
 				
@@ -125,7 +126,7 @@ void GpuConvexPlaneScene::createStaticEnvironment(const ConstructionInfo& ci)
 	btQuaternion orn(0,0,0,1);
 	//		btQuaternion orn(btVector3(1,0,0),0.3);
 	btVector4 color(0,0,1,1);
-	btVector4 scaling(100,0.0001,100,1);
+	btVector4 scaling(100,0.1,100,1);
 	int strideInBytes = 9*sizeof(float);
 	int numVertices = sizeof(cube_vertices)/strideInBytes;
 	int numIndices = sizeof(cube_indices)/sizeof(int);

opencl/gpu_rigidbody/host/btGpuBatchingPgsSolver.cpp

@@ -672,10 +672,11 @@ void btGpuBatchingPgsSolver::solveContacts(int numBodies, cl_mem bodyBuf, cl_mem
         
         if (1)
         {
-            BT_PROFILE("GPU solveContactConstraint");
             m_data->m_solverGPU->m_nIterations = 4;//10
 			if (gpuSolveConstraint)
 			{
+				BT_PROFILE("GPU solveContactConstraint");
+
 				m_data->m_solverGPU->solveContactConstraint(
 					m_data->m_bodyBufferGPU, 
 					m_data->m_inertiaBufferGPU,
@@ -685,6 +686,8 @@ void btGpuBatchingPgsSolver::solveContacts(int numBodies, cl_mem bodyBuf, cl_mem
 			}
 			else
 			{
+				BT_PROFILE("Host solveContactConstraint");
+
 				m_data->m_solverGPU->solveContactConstraintHost(m_data->m_bodyBufferGPU, m_data->m_inertiaBufferGPU, m_data->m_contactCGPU,0, nContactOut ,maxNumBatches);
 			}
             

opencl/gpu_sat/host/ConvexHullContact.cpp

@@ -131,7 +131,7 @@ 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");
+		cl_program primitiveContactsProg = btOpenCLUtils::compileCLProgramFromString(m_context,m_device,primitiveContactsSrc,&errNum,"","opencl/gpu_sat/kernels/primitiveContacts.cl",true);
 		btAssert(errNum==CL_SUCCESS);
 
 		m_primitiveContactsKernel = btOpenCLUtils::compileCLKernelFromString(m_context, m_device,primitiveContactsSrc, "primitiveContactsKernel",&errNum,primitiveContactsProg,"");
@@ -291,6 +291,102 @@ inline bool IsPointInPolygon(const float4& p,
     return true;
 }
 
+#define normalize3(a) (a.normalize())
+
+
+int extractManifoldSequentialGlobal( const float4* p, int nPoints, const float4& nearNormal, btInt4* 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;
+    
+}
+
 void computeContactPlaneConvex(int pairIndex,
 																int bodyIndexA, int bodyIndexB, 
 																int collidableIndexA, int collidableIndexB, 
@@ -318,6 +414,7 @@ void computeContactPlaneConvex(int pairIndex,
 
 	btVector3 planeEq = faces[collidables[collidableIndexA].m_shapeIndex].m_plane;
 	btVector3 planeNormal(planeEq.x,planeEq.y,planeEq.z);
+	btVector3 planeNormalWorld = quatRotate(ornA,planeNormal);
 	float planeConstant = planeEq.w;
 	btTransform convexWorldTransform;
 	convexWorldTransform.setIdentity();
@@ -330,85 +427,94 @@ void computeContactPlaneConvex(int pairIndex,
 
 	btTransform planeInConvex;
 	planeInConvex= convexWorldTransform.inverse() * planeTransform;
+	btTransform convexInPlane;
+	convexInPlane = planeTransform.inverse() * convexWorldTransform;
 	
 	btVector3 planeNormalInConvex = planeInConvex.getBasis()*-planeNormal;
 	float maxDot = -1e30;
 	int hitVertex=-1;
 	btVector3 hitVtx;
+
+#define MAX_PLANE_CONVEX_POINTS 64
+
+	btVector3 contactPoints[MAX_PLANE_CONVEX_POINTS];
+	int numPoints = 0;
+
+	btInt4 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++)
 	{
 		btVector3 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)
+		{
+			btVector3 vtxWorld = convexWorldTransform*vtx;
+			btVector3 vtxInPlane = planeTransform.inverse()*vtxWorld;
+			float dist = planeNormal.dot(vtxInPlane)-planeConstant;
+			if (dist<0.f)
+			{
+				vtxWorld.w = dist;
+				contactPoints[numPoints] = vtxWorld;
+				numPoints++;
+			}
+		}
+
 	}
 
-	btVector3 hitVtxWorld = convexWorldTransform*hitVtx;
+	int numReducedPoints  = 0;
-	float dist = 0;
 
+	numReducedPoints = numPoints;
+	
+	if (numPoints>4)
+	{
+		numReducedPoints = extractManifoldSequentialGlobal( contactPoints, numPoints, planeNormalInConvex, &contactIdx);
+	}
 	int dstIdx;
 //    dstIdx = nGlobalContactsOut++;//AppendInc( nGlobalContactsOut, dstIdx );
 		
-	if (nGlobalContactsOut < maxContactCapacity)
+	if (numReducedPoints>0)
 	{
-		dstIdx=nGlobalContactsOut;
+		if (nGlobalContactsOut < maxContactCapacity)
-		nGlobalContactsOut++;
-
-		btContact4* c = &globalContactsOut[dstIdx];
-		c->m_worldNormal = -planeNormalInConvex;
-		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;
-		btVector3 pOnB1 = hitVtxWorld;
-		pOnB1[3] = -0.01f;
-		c->m_worldPos[0] = pOnB1;
-		int numPoints = 1;
-		c->m_worldNormal[3] = numPoints;
-	}//if (dstIdx < numPairs)
-	
-		/*
-
-	int closestFaceB=-1;
-	float dmax = -FLT_MAX;
-    
-	{
-		for(int face=0;face<hullB->m_numFaces;face++)
 		{
-			const float4 Normal = make_float4(faces[hullB->m_faceOffset+face].m_plane.x,
+			dstIdx=nGlobalContactsOut;
-                                              faces[hullB->m_faceOffset+face].m_plane.y, faces[hullB->m_faceOffset+face].m_plane.z,0.f);
+			nGlobalContactsOut++;
-			const float4 WorldNormal = quatRotate(ornB, Normal);
+
-			float d = dot3F4(WorldNormal,separatingNormal);
+			btContact4* c = &globalContactsOut[dstIdx];
-			if (d > dmax)
+			c->m_worldNormal = 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++)
 			{
-				dmax = d;
+				btVector3 pOnB1 = contactPoints[contactIdx.s[i]];
-				closestFaceB = face;
+				c->m_worldPos[i] = pOnB1;
 			}
-		}
+			c->m_worldNormal[3] = numReducedPoints;
-	}
+		}//if (dstIdx < numPairs)
-    
+	}	
-	{
+		
-		const btGpuFace polyB = faces[hullB->m_faceOffset+closestFaceB];
-		const int numVertices = polyB.m_numIndices;
-		for(int e0=0;e0<numVertices;e0++)
-		{
-			const float4 b = vertices[hullB->m_vertexOffset+indices[polyB.m_indexOffset+e0]];
-			worldVertsB1[pairIndex*capacityWorldVerts+numWorldVertsB1++] = transform(b,posB,ornB);
-		}
-	}
 
 
-	*/
+//	printf("computeContactPlaneConvex\n");
-
-
-
-	printf("computeContactPlaneConvex\n");
 }
 
 void	computeContactSphereConvex(int pairIndex,
@@ -597,7 +703,7 @@ void GpuSatCollision::computeConvexConvexContactsGPUSAT( const btOpenCLArray<btI
 		return;
 
 
-#define CHECK_ON_HOST
+//#define CHECK_ON_HOST
 #ifdef CHECK_ON_HOST
 	btAlignedObjectArray<btYetAnotherAabb> hostAabbs;
 	clAabbsWS.copyToHost(hostAabbs);
@@ -666,7 +772,7 @@ void GpuSatCollision::computeConvexConvexContactsGPUSAT( const btOpenCLArray<btI
 		{
 			computeContactPlaneConvex(i,bodyIndexB,bodyIndexA,collidableIndexB,collidableIndexA,&hostBodyBuf[0],
 			&hostCollidables[0],&hostConvexData[0],&hostVertices[0],&hostIndices[0],&hostFaces[0],&hostContacts[0],nContacts,nPairs);
-			printf("convex-plane\n");
+//			printf("convex-plane\n");
 			
 		}
 
@@ -675,7 +781,7 @@ void GpuSatCollision::computeConvexConvexContactsGPUSAT( const btOpenCLArray<btI
 		{
 			computeContactPlaneConvex(i,bodyIndexA,bodyIndexB,collidableIndexA,collidableIndexB,&hostBodyBuf[0],
 			&hostCollidables[0],&hostConvexData[0],&hostVertices[0],&hostIndices[0],&hostFaces[0],&hostContacts[0],nContacts,nPairs);
-			printf("plane-convex\n");
+//			printf("plane-convex\n");
 			
 		}
 

opencl/gpu_sat/kernels/primitiveContacts.cl

@@ -494,8 +494,149 @@ void	computeContactSphereConvex(int pairIndex,
 
 
 
-									
+#define MAX_PLANE_CONVEX_POINTS 64
-void	computeContactPlaneConvex(int pairIndex,
+
+void computeContactPlaneConvex(int pairIndex,
+								int bodyIndexA, int bodyIndexB, 
+								int collidableIndexA, int collidableIndexB, 
+								__global const BodyData* rigidBodies, 
+								__global const btCollidableGpu*collidables,
+								__global const ConvexPolyhedronCL* convexShapes,
+								__global const float4* convexVertices,
+								__global const int* convexIndices,
+								__global const btGpuFace* faces,
+								__global Contact4* restrict globalContactsOut,
+								counter32_t nGlobalContactsOut,
+								int maxContactCapacity)
+{
+
+		int shapeIndex = collidables[collidableIndexB].m_shapeIndex;
+	__global const ConvexPolyhedronCL* hullB = &convexShapes[shapeIndex];
+	
+	float4 posB;
+	posB = rigidBodies[bodyIndexB].m_pos;
+	Quaternion ornB;
+	 ornB = rigidBodies[bodyIndexB].m_quat;
+	float4 posA;
+	 posA = rigidBodies[bodyIndexA].m_pos;
+	Quaternion ornA;
+	ornA = rigidBodies[bodyIndexA].m_quat;
+
+	int numContactsOut = 0;
+	int numWorldVertsB1= 0;
+
+	float4 planeEq;
+	 planeEq = faces[collidables[collidableIndexA].m_shapeIndex].m_plane;
+	float4 planeNormal = make_float4(planeEq.x,planeEq.y,planeEq.z,0.f);
+	float4 planeNormalWorld;
+	planeNormalWorld = qtRotate(ornA,planeNormal);
+	float planeConstant = planeEq.w;
+	
+	float4 invPosA;Quaternion invOrnA;
+	float4 convexInPlaneTransPos1; Quaternion convexInPlaneTransOrn1;
+	{
+		
+		trInverse(posA,ornA,&invPosA,&invOrnA);
+		trMul(invPosA,invOrnA,posB,ornB,&convexInPlaneTransPos1,&convexInPlaneTransOrn1);
+	}
+	float4 invPosB;Quaternion invOrnB;
+	float4 planeInConvexPos1;	Quaternion planeInConvexOrn1;
+	{
+		
+		trInverse(posB,ornB,&invPosB,&invOrnB);
+		trMul(invPosB,invOrnB,posA,ornA,&planeInConvexPos1,&planeInConvexOrn1);	
+	}
+
+	
+	float4 planeNormalInConvex = qtRotate(planeInConvexOrn1,-planeNormal);
+	float maxDot = -1e30;
+	int hitVertex=-1;
+	float4 hitVtx;
+
+
+
+	float4 contactPoints[MAX_PLANE_CONVEX_POINTS];
+	int numPoints = 0;
+
+	int4 contactIdx;
+	contactIdx=make_int4(0,1,2,3);
+    
+	
+	for (int i=0;i<hullB->m_numVertices;i++)
+	{
+		float4 vtx = convexVertices[hullB->m_vertexOffset+i];
+		float curDot = dot(vtx,planeNormalInConvex);
+
+
+		if (curDot>maxDot)
+		{
+			hitVertex=i;
+			maxDot=curDot;
+			hitVtx = vtx;
+			//make sure the deepest points is always included
+			if (numPoints==MAX_PLANE_CONVEX_POINTS)
+				numPoints--;
+		}
+
+		if (numPoints<MAX_PLANE_CONVEX_POINTS)
+		{
+			float4 vtxWorld = transform(&vtx, &posB, &ornB);
+			float4 vtxInPlane = transform(&vtxWorld, &invPosA, &invOrnA);//oplaneTransform.inverse()*vtxWorld;
+			float dist = dot(planeNormal,vtxInPlane)-planeConstant;
+			if (dist<0.f)
+			{
+				vtxWorld.w = dist;
+				contactPoints[numPoints] = vtxWorld;
+				numPoints++;
+			}
+		}
+
+	}
+
+	int numReducedPoints  = numPoints;
+	//if (numPoints>4)
+	//{
+//		numReducedPoints = extractManifoldSequentialGlobal( contactPoints, numPoints, planeNormalInConvex, &contactIdx);
+	//}
+
+	if (numReducedPoints>0)
+	{
+		int dstIdx;
+	    AppendInc( nGlobalContactsOut, dstIdx );
+
+		if (dstIdx < maxContactCapacity)
+		{
+			__global Contact4* c = &globalContactsOut[dstIdx];
+			c->m_worldNormal = planeNormalWorld;
+			//c->setFrictionCoeff(0.7);
+			//c->setRestituitionCoeff(0.f);
+			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;
+
+			switch (numReducedPoints)
+            {
+                case 4:
+                    c->m_worldPos[3] = contactPoints[contactIdx.w];
+                case 3:
+                    c->m_worldPos[2] = contactPoints[contactIdx.z];
+                case 2:
+                    c->m_worldPos[1] = contactPoints[contactIdx.y];
+                case 1:
+                    c->m_worldPos[0] = contactPoints[contactIdx.x];
+                default:
+                {
+                }
+            };
+			
+			GET_NPOINTS(*c) = numReducedPoints;
+		}//if (dstIdx < numPairs)
+	}	
+}
+
+
+void	computeContactPlaneSphere(int pairIndex,
 																int bodyIndexA, int bodyIndexB, 
 																int collidableIndexA, int collidableIndexB, 
 																__global const BodyData* rigidBodies, 
@@ -557,8 +698,6 @@ void	computeContactPlaneConvex(int pairIndex,
 }
 
 
-
-
 __kernel void   primitiveContactsKernel( __global const int2* pairs, 
 																					__global const BodyData* rigidBodies, 
 																					__global const btCollidableGpu* collidables,
@@ -594,27 +733,51 @@ __kernel void   primitiveContactsKernel( __global const int2* pairs,
 		int collidableIndexA = rigidBodies[bodyIndexA].m_collidableIdx;
 		int collidableIndexB = rigidBodies[bodyIndexB].m_collidableIdx;
 	
+		if (collidables[collidableIndexA].m_shapeType == SHAPE_PLANE &&
+			collidables[collidableIndexB].m_shapeType == SHAPE_CONVEX_HULL)
+		{
+			computeContactPlaneConvex(pairIndex, bodyIndexA, bodyIndexB, collidableIndexA, collidableIndexB, 
+																rigidBodies,collidables,convexShapes,vertices,indices,
+																faces,	globalContactsOut, nGlobalContactsOut,maxContactCapacity);
+			return;
+		}
 
-		if (collidables[collidableIndexA].m_shapeType == SHAPE_SPHERE &&
+
+		if (collidables[collidableIndexA].m_shapeType == SHAPE_CONVEX_HULL &&
 			collidables[collidableIndexB].m_shapeType == SHAPE_PLANE)
 		{
 
 
 			computeContactPlaneConvex( pairIndex, bodyIndexB,bodyIndexA,  collidableIndexB,collidableIndexA, 
-																rigidBodies,collidables,faces,	globalContactsOut, nGlobalContactsOut,maxContactCapacity);
+																rigidBodies,collidables,convexShapes,vertices,indices,
+																faces,	globalContactsOut, nGlobalContactsOut,maxContactCapacity);
+
 			return;
 		}
 
 		if (collidables[collidableIndexA].m_shapeType == SHAPE_PLANE &&
 			collidables[collidableIndexB].m_shapeType == SHAPE_SPHERE)
 		{
-
+			computeContactPlaneSphere(pairIndex, bodyIndexA, bodyIndexB, collidableIndexA, collidableIndexB, 
-
+																rigidBodies,collidables,faces,	globalContactsOut, nGlobalContactsOut,maxContactCapacity);
-			computeContactPlaneConvex(pairIndex, bodyIndexA, bodyIndexB, collidableIndexA, collidableIndexB, 
-																rigidBodies,collidables,faces, globalContactsOut, nGlobalContactsOut,maxContactCapacity);
 			return;
-			
 		}
+
+
+		if (collidables[collidableIndexA].m_shapeType == SHAPE_SPHERE &&
+			collidables[collidableIndexB].m_shapeType == SHAPE_PLANE)
+		{
+
+
+			computeContactPlaneSphere( pairIndex, bodyIndexB,bodyIndexA,  collidableIndexB,collidableIndexA, 
+																rigidBodies,collidables,
+																faces,	globalContactsOut, nGlobalContactsOut,maxContactCapacity);
+
+			return;
+		}
+
+		
+
 	
 		if (collidables[collidableIndexA].m_shapeType == SHAPE_SPHERE &&
 			collidables[collidableIndexB].m_shapeType == SHAPE_CONVEX_HULL)
@@ -649,6 +812,7 @@ __kernel void   primitiveContactsKernel( __global const int2* pairs,
 	
 	
 	
+		
 	
 	
 		if (collidables[collidableIndexA].m_shapeType == SHAPE_SPHERE &&