add support for compound hull against plane

2013-04-08 18:42:32 -07:00
parent ce5652c26a
commit e1a4400037
6 changed files with 573 additions and 123 deletions
--- a/opencl/gpu_sat/host/ConvexHullContact.cpp
+++ b/opencl/gpu_sat/host/ConvexHullContact.cpp
@@ -387,6 +387,14 @@ int extractManifoldSequentialGlobal( const float4* p, int nPoints, const float4&
    
 }

+
+
+
+
+	
+
+
+
 void computeContactPlaneConvex(int pairIndex,
 																int bodyIndexA, int bodyIndexB, 
 																int collidableIndexA, int collidableIndexB, 
@@ -517,6 +525,148 @@ void computeContactPlaneConvex(int pairIndex,
 //	printf("computeContactPlaneConvex\n");
 }

+
+
+
+void computeContactPlaneCompound(int pairIndex,
+																int bodyIndexA, int bodyIndexB, 
+																int collidableIndexA, int collidableIndexB, 
+																const btRigidBodyCL* rigidBodies, 
+																const btCollidable* collidables,
+																const btConvexPolyhedronCL* convexShapes,
+																const btVector3* convexVertices,
+																const int* convexIndices,
+																const btGpuFace* faces,
+																btContact4* globalContactsOut,
+																int& nGlobalContactsOut,
+																int maxContactCapacity)
+{
+
+	int shapeTypeB = collidables[collidableIndexB].m_shapeType;
+	btAssert(shapeTypeB == SHAPE_COMPOUND_OF_CONVEX_HULLS);
+
+
+
+	int shapeIndex = collidables[collidableIndexB].m_shapeIndex;
+	const btConvexPolyhedronCL* hullB = &convexShapes[shapeIndex];
+	
+	btVector3 posB = rigidBodies[bodyIndexB].m_pos;
+	btQuaternion ornB = rigidBodies[bodyIndexB].m_quat;
+	btVector3 posA = rigidBodies[bodyIndexA].m_pos;
+	btQuaternion ornA = rigidBodies[bodyIndexA].m_quat;
+
+	int numContactsOut = 0;
+	int numWorldVertsB1= 0;
+
+	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();
+	convexWorldTransform.setOrigin(posB);
+	convexWorldTransform.setRotation(ornB);
+	btTransform planeTransform;
+	planeTransform.setIdentity();
+	planeTransform.setOrigin(posA);
+	planeTransform.setRotation(ornA);
+
+	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++;
+			}
+		}
+
+	}
+
+	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++;
+
+			btContact4* c = &globalContactsOut[dstIdx];
+			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++)
+			{
+				btVector3 pOnB1 = contactPoints[contactIdx.s[i]];
+				c->m_worldPos[i] = pOnB1;
+			}
+			c->m_worldNormal[3] = numReducedPoints;
+		}//if (dstIdx < numPairs)
+	}	
+		
+
+
+//	printf("computeContactPlaneConvex\n");
+}
+
+
+
+
+
 void	computeContactSphereConvex(int pairIndex,
 																int bodyIndexA, int bodyIndexB, 
 																int collidableIndexA, int collidableIndexB, 
@@ -785,7 +935,26 @@ void GpuSatCollision::computeConvexConvexContactsGPUSAT( const btOpenCLArray<btI
 			
 		}

-		
+
+				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],&hostVertices[0],&hostIndices[0],&hostFaces[0],&hostContacts[0],nContacts,nPairs);
+//			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],&hostVertices[0],&hostIndices[0],&hostFaces[0],&hostContacts[0],nContacts,nPairs);
+//			printf("plane-convex\n");
+			
+		}
+
+
 	}

 	if (nContacts)