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

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"