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

implemented sphere-convex (supports edge and corner-vertex cases)

Browse Source
This commit is contained in:
erwincoumans
2013-04-02 22:09:40 -07:00
parent e38c032280
commit 5c8c8e1cbb
9 changed files with 1708 additions and 303 deletions
Download Patch File Download Diff File Expand all files Collapse all files

146
opencl/gpu_sat/kernels/satClipHullContacts.h
View File

@@ -3,9 +3,13 @@ static const char* satClipKernelsCL= \
"\n"
"#define TRIANGLE_NUM_CONVEX_FACES 5\n"
"\n"
"#define SHAPE_CONVEX_HULL 3\n"
"#define SHAPE_PLANE 4\n"
"#define SHAPE_CONCAVE_TRIMESH 5\n"
"#define SHAPE_COMPOUND_OF_CONVEX_HULLS 6\n"
"#define SHAPE_SPHERE 7\n"
"\n"
"\n"
"#pragma OPENCL EXTENSION cl_amd_printf : enable\n"
"#pragma OPENCL EXTENSION cl_khr_local_int32_base_atomics : enable\n"
"#pragma OPENCL EXTENSION cl_khr_global_int32_base_atomics : enable\n"
@@ -43,12 +47,8 @@ static const char* satClipKernelsCL= \
"{\n"
" float4 m_worldPos[4];\n"
" float4 m_worldNormal; // w: m_nPoints\n"
"// float m_restituitionCoeff;\n"
"// float m_frictionCoeff;\n"
" u32 m_coeffs;\n"
" u32 m_batchIdx;\n"
"// int m_nPoints;\n"
"// int m_padding0;\n"
"\n"
" int m_bodyAPtrAndSignBit;//x:m_bodyAPtr, y:m_bodyBPtr\n"
" int m_bodyBPtrAndSignBit;\n"
@@ -874,11 +874,6 @@ static const char* satClipKernelsCL= \
" contactIdx[2] = idx[2];\n"
" contactIdx[3] = idx[3];\n"
"\n"
"// if( max00.y < 0.0f )\n"
"// contactIdx[0] = (int)max00.x;\n"
"\n"
" //does this sort happen on GPU too?\n"
" //std::sort( contactIdx, contactIdx+4 );\n"
"\n"
" return 4;\n"
" }\n"
@@ -910,7 +905,7 @@ static const char* satClipKernelsCL= \
" {\n"
" localPoints[i] = pointsIn[i];\n"
" }\n"
"// int contactIdx[4] = {-1,-1,-1,-1};\n"
"\n"
" int contactIdx[4];// = {-1,-1,-1,-1};\n"
" contactIdx[0] = -1;\n"
" contactIdx[1] = -1;\n"
@@ -956,66 +951,7 @@ static const char* satClipKernelsCL= \
" *translationOut = transform(&translationB,&translationA,&orientationA);\n"
"}\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 btGpuFace* faces,\n"
" __global Contact4* restrict globalContactsOut,\n"
" counter32_t nGlobalContactsOut,\n"
" int numPairs)\n"
"{\n"
" float4 planeEq = faces[collidables[collidableIndexA].m_shapeIndex].m_plane;\n"
" float radius = collidables[collidableIndexB].m_radius;\n"
" float4 posA1 = rigidBodies[bodyIndexA].m_pos;\n"
" float4 ornA1 = rigidBodies[bodyIndexA].m_quat;\n"
" float4 posB1 = rigidBodies[bodyIndexB].m_pos;\n"
" float4 ornB1 = rigidBodies[bodyIndexB].m_quat;\n"
" \n"
" bool hasCollision = false;\n"
" float4 planeNormal1 = make_float4(planeEq.x,planeEq.y,planeEq.z,0.f);\n"
" float planeConstant = planeEq.w;\n"
" float4 convexInPlaneTransPos1; Quaternion convexInPlaneTransOrn1;\n"
" {\n"
" float4 invPosA;Quaternion invOrnA;\n"
" trInverse(posA1,ornA1,&invPosA,&invOrnA);\n"
" trMul(invPosA,invOrnA,posB1,ornB1,&convexInPlaneTransPos1,&convexInPlaneTransOrn1);\n"
" }\n"
" float4 planeInConvexPos1; Quaternion planeInConvexOrn1;\n"
" {\n"
" float4 invPosB;Quaternion invOrnB;\n"
" trInverse(posB1,ornB1,&invPosB,&invOrnB);\n"
" trMul(invPosB,invOrnB,posA1,ornA1,&planeInConvexPos1,&planeInConvexOrn1); \n"
" }\n"
" float4 vtx1 = qtRotate(planeInConvexOrn1,-planeNormal1)*radius;\n"
" float4 vtxInPlane1 = transform(&vtx1,&convexInPlaneTransPos1,&convexInPlaneTransOrn1);\n"
" float distance = dot3F4(planeNormal1,vtxInPlane1) - planeConstant;\n"
" hasCollision = distance < 0.f;//m_manifoldPtr->getContactBreakingThreshold();\n"
" if (hasCollision)\n"
" {\n"
" float4 vtxInPlaneProjected1 = vtxInPlane1 - distance*planeNormal1;\n"
" float4 vtxInPlaneWorld1 = transform(&vtxInPlaneProjected1,&posA1,&ornA1);\n"
" float4 normalOnSurfaceB1 = qtRotate(ornA1,planeNormal1);\n"
" float4 pOnB1 = vtxInPlaneWorld1+normalOnSurfaceB1*distance;\n"
" pOnB1.w = distance;\n"
"\n"
" int dstIdx;\n"
" AppendInc( nGlobalContactsOut, dstIdx );\n"
" \n"
" if (dstIdx < numPairs)\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"
" }//if (dstIdx < numPairs)\n"
" }//if (hasCollision)\n"
"}\n"
"\n"
"\n"
"__kernel void clipHullHullKernel( __global const int2* pairs, \n"
@@ -1054,74 +990,6 @@ static const char* satClipKernelsCL= \
" \n"
" int collidableIndexA = rigidBodies[bodyIndexA].m_collidableIdx;\n"
" int collidableIndexB = rigidBodies[bodyIndexB].m_collidableIdx;\n"
" \n"
"\n"
" if (collidables[collidableIndexA].m_shapeType == SHAPE_SPHERE &&\n"
" collidables[collidableIndexB].m_shapeType == SHAPE_PLANE)\n"
" {\n"
"\n"
"\n"
" computeContactPlaneConvex( pairIndex, bodyIndexB,bodyIndexA, collidableIndexB,collidableIndexA, \n"
" rigidBodies,collidables,faces, globalContactsOut, nGlobalContactsOut,numPairs);\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,numPairs);\n"
" return;\n"
" \n"
" }\n"
" \n"
" if (collidables[collidableIndexA].m_shapeType == SHAPE_SPHERE &&\n"
" collidables[collidableIndexB].m_shapeType == SHAPE_SPHERE)\n"
" {\n"
" //sphere-sphere\n"
" float radiusA = collidables[collidableIndexA].m_radius;\n"
" float radiusB = collidables[collidableIndexB].m_radius;\n"
" float4 posA = rigidBodies[bodyIndexA].m_pos;\n"
" float4 posB = rigidBodies[bodyIndexB].m_pos;\n"
"\n"
" float4 diff = posA-posB;\n"
" float len = length(diff);\n"
" \n"
" ///iff distance positive, don't generate a new contact\n"
" if ( len <= (radiusA+radiusB))\n"
" {\n"
" ///distance (negative means penetration)\n"
" float dist = len - (radiusA+radiusB);\n"
" float4 normalOnSurfaceB = make_float4(1.f,0.f,0.f,0.f);\n"
" if (len > 0.00001)\n"
" {\n"
" normalOnSurfaceB = diff / len;\n"
" }\n"
" float4 contactPosB = posB + normalOnSurfaceB*radiusB;\n"
" contactPosB.w = dist;\n"
" \n"
" int dstIdx;\n"
" AppendInc( nGlobalContactsOut, dstIdx );\n"
" \n"
" if (dstIdx < numPairs)\n"
" {\n"
" __global Contact4* c = &globalContactsOut[dstIdx];\n"
" c->m_worldNormal = -normalOnSurfaceB;\n"
" c->m_coeffs = (u32)(0.f*0xffff) | ((u32)(0.7f*0xffff)<<16);\n"
" c->m_batchIdx = pairIndex;\n"
" int bodyA = pairs[pairIndex].x;\n"
" int bodyB = pairs[pairIndex].y;\n"
" c->m_bodyAPtrAndSignBit = rigidBodies[bodyA].m_invMass==0?-bodyA:bodyA;\n"
" c->m_bodyBPtrAndSignBit = rigidBodies[bodyB].m_invMass==0?-bodyB:bodyB;\n"
" c->m_worldPos[0] = contactPosB;\n"
" GET_NPOINTS(*c) = 1;\n"
" }//if (dstIdx < numPairs)\n"
" }//if ( len <= (radiusA+radiusB))\n"
"\n"
" return;\n"
" }//SHAPE_SPHERE SHAPE_SPHERE\n"
"\n"
" if (hasSeparatingAxis[i])\n"
" {\n"
@@ -1263,10 +1131,6 @@ static const char* satClipKernelsCL= \
" \n"
" int shapeIndexA = collidables[collidableIndexA].m_shapeIndex;\n"
" int shapeIndexB = collidables[collidableIndexB].m_shapeIndex;\n"
"\n"
" \n"
"\n"
"\n"
" \n"
" int numLocalContactsOut = clipHullAgainstHull(gpuCompoundSepNormalsOut[i],\n"
" &convexShapes[shapeIndexA], &convexShapes[shapeIndexB],\n"