Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

add support for compound hull against plane

Browse Source
This commit is contained in:
erwin coumans
2013-04-08 18:42:32 -07:00
parent ce5652c26a
commit e1a4400037
6 changed files with 573 additions and 123 deletions
Download Patch File Download Diff File Expand all files Collapse all files

171
opencl/gpu_sat/host/ConvexHullContact.cpp
View File

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

43
opencl/gpu_sat/kernels/primitiveContacts.cl
View File

@@ -507,18 +507,17 @@ void computeContactPlaneConvex(int pairIndex,
__global const btGpuFace* faces,
__global Contact4* restrict globalContactsOut,
counter32_t nGlobalContactsOut,
int maxContactCapacity)
int maxContactCapacity,
float4 posB,
Quaternion ornB
)
{
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;
posA = rigidBodies[bodyIndexA].m_pos;
Quaternion ornA;
ornA = rigidBodies[bodyIndexA].m_quat;
@@ -736,9 +735,14 @@ __kernel void primitiveContactsKernel( __global const int2* pairs,
if (collidables[collidableIndexA].m_shapeType == SHAPE_PLANE &&
collidables[collidableIndexB].m_shapeType == SHAPE_CONVEX_HULL)
{
float4 posB;
posB = rigidBodies[bodyIndexB].m_pos;
Quaternion ornB;
ornB = rigidBodies[bodyIndexB].m_quat;
computeContactPlaneConvex(pairIndex, bodyIndexA, bodyIndexB, collidableIndexA, collidableIndexB,
rigidBodies,collidables,convexShapes,vertices,indices,
faces, globalContactsOut, nGlobalContactsOut,maxContactCapacity);
faces, globalContactsOut, nGlobalContactsOut,maxContactCapacity, posB,ornB);
return;
}
@@ -747,10 +751,15 @@ __kernel void primitiveContactsKernel( __global const int2* pairs,
collidables[collidableIndexB].m_shapeType == SHAPE_PLANE)
{
float4 posA;
posA = rigidBodies[bodyIndexA].m_pos;
Quaternion ornA;
ornA = rigidBodies[bodyIndexA].m_quat;
computeContactPlaneConvex( pairIndex, bodyIndexB,bodyIndexA, collidableIndexB,collidableIndexA,
rigidBodies,collidables,convexShapes,vertices,indices,
faces, globalContactsOut, nGlobalContactsOut,maxContactCapacity);
faces, globalContactsOut, nGlobalContactsOut,maxContactCapacity,posA,ornA);
return;
}
@@ -936,6 +945,24 @@ __kernel void processCompoundPairsPrimitivesKernel( __global const int4* gpuCo
int shapeTypeB = collidables[collidableIndexB].m_shapeType;
int pairIndex = i;
if ((shapeTypeA == SHAPE_PLANE) && (shapeTypeB==SHAPE_CONVEX_HULL))
{
computeContactPlaneConvex( pairIndex, bodyIndexA,bodyIndexB, collidableIndexA,collidableIndexB,
rigidBodies,collidables,convexShapes,vertices,indices,
faces, globalContactsOut, nGlobalContactsOut,maxContactCapacity,posB,ornB);
return;
}
if ((shapeTypeA == SHAPE_CONVEX_HULL) && (shapeTypeB==SHAPE_PLANE))
{
computeContactPlaneConvex( pairIndex, bodyIndexB,bodyIndexA, collidableIndexB,collidableIndexA,
rigidBodies,collidables,convexShapes,vertices,indices,
faces, globalContactsOut, nGlobalContactsOut,maxContactCapacity,posA,ornA);
return;
}
if ((shapeTypeA == SHAPE_CONVEX_HULL) && (shapeTypeB == SHAPE_SPHERE))
{
float4 spherePos = rigidBodies[bodyIndexB].m_pos;

213
opencl/gpu_sat/kernels/primitiveContacts.h
View File

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