From 358f4f97a2e7d2b9aa543d1fd7b5127704a425bb Mon Sep 17 00:00:00 2001 From: erwin coumans Date: Thu, 4 Apr 2013 17:54:45 -0700 Subject: [PATCH] add re-usable createGraphicsSphere method in GpuDemo. introduce and use maxContactCapacity (needs to be fixed in various other contact kernels) implement sphere versus trimesh disable new/sequential GPU batching (only uses 1 thread in a warp, slow but works on NVIDIA/Apple OpenCL) --- demo/gpudemo/GpuDemo.cpp | 40 +- demo/gpudemo/GpuDemo.h | 7 +- demo/gpudemo/main_opengl3core.cpp | 6 +- demo/gpudemo/rigidbody/ConcaveScene.cpp | 93 +++- demo/gpudemo/rigidbody/ConcaveScene.h | 28 ++ demo/gpudemo/rigidbody/GpuSphereScene.cpp | 92 ++-- opencl/gpu_rigidbody/host/Solver.cpp | 2 +- opencl/gpu_sat/host/ConvexHullContact.cpp | 60 ++- opencl/gpu_sat/host/ConvexHullContact.h | 2 + opencl/gpu_sat/kernels/bvhTraversal.cl | 7 +- opencl/gpu_sat/kernels/bvhTraversal.h | 7 +- opencl/gpu_sat/kernels/primitiveContacts.cl | 278 ++++++++++- opencl/gpu_sat/kernels/primitiveContacts.h | 502 ++++++++++++++++++-- opencl/gpu_sat/kernels/sat.cl | 7 + opencl/gpu_sat/kernels/satKernels.h | 16 + 15 files changed, 1008 insertions(+), 139 deletions(-) diff --git a/demo/gpudemo/GpuDemo.cpp b/demo/gpudemo/GpuDemo.cpp index f9c2a21c7..bf97de3fa 100644 --- a/demo/gpudemo/GpuDemo.cpp +++ b/demo/gpudemo/GpuDemo.cpp @@ -2,7 +2,8 @@ #include "GpuDemoInternalData.h" #include "BulletCommon/btScalar.h" #include "basic_initialize/btOpenCLUtils.h" - +#include "OpenGLWindow/ShapeData.h" +#include "OpenGLWindow/GLInstancingRenderer.h" GpuDemo::GpuDemo() :m_clData(0) @@ -78,3 +79,40 @@ void GpuDemo::initCL(int preferredDeviceIndex, int preferredPlatformIndex) } +int GpuDemo::registerGraphicsSphereShape(const ConstructionInfo& ci, float radius, bool usePointSprites, int largeSphereThreshold, int mediumSphereThreshold) +{ + + int strideInBytes = 9*sizeof(float); + + int graphicsShapeIndex = -1; + + if (radius>=largeSphereThreshold) + { + int numVertices = sizeof(detailed_sphere_vertices)/strideInBytes; + int numIndices = sizeof(detailed_sphere_indices)/sizeof(int); + graphicsShapeIndex = ci.m_instancingRenderer->registerShape(&detailed_sphere_vertices[0],numVertices,detailed_sphere_indices,numIndices); + } else + { + + if (usePointSprites) + { + int numVertices = sizeof(point_sphere_vertices)/strideInBytes; + int numIndices = sizeof(point_sphere_indices)/sizeof(int); + graphicsShapeIndex = ci.m_instancingRenderer->registerShape(&point_sphere_vertices[0],numVertices,point_sphere_indices,numIndices,BT_GL_POINTS); + } else + { + if (radius>=mediumSphereThreshold) + { + int numVertices = sizeof(medium_sphere_vertices)/strideInBytes; + int numIndices = sizeof(medium_sphere_indices)/sizeof(int); + graphicsShapeIndex = ci.m_instancingRenderer->registerShape(&medium_sphere_vertices[0],numVertices,medium_sphere_indices,numIndices); + } else + { + int numVertices = sizeof(low_sphere_vertices)/strideInBytes; + int numIndices = sizeof(low_sphere_indices)/sizeof(int); + graphicsShapeIndex = ci.m_instancingRenderer->registerShape(&low_sphere_vertices[0],numVertices,low_sphere_indices,numIndices); + } + } + } + return graphicsShapeIndex; +} \ No newline at end of file diff --git a/demo/gpudemo/GpuDemo.h b/demo/gpudemo/GpuDemo.h index bb8563f21..77af3c89d 100644 --- a/demo/gpudemo/GpuDemo.h +++ b/demo/gpudemo/GpuDemo.h @@ -38,9 +38,9 @@ public: :useOpenCL(true), preferredOpenCLPlatformIndex(-1), preferredOpenCLDeviceIndex(-1), - arraySizeX(15), - arraySizeY(15), - arraySizeZ(15), + arraySizeX(41), + arraySizeY(41), + arraySizeZ(41), m_useConcaveMesh(false), gapX(14.3), gapY(14.0), @@ -65,6 +65,7 @@ public: virtual void clientMoveAndDisplay()=0; + int registerGraphicsSphereShape(const ConstructionInfo& ci, float radius, bool usePointSprites=true, int largeSphereThreshold=100, int mediumSphereThreshold=10); }; diff --git a/demo/gpudemo/main_opengl3core.cpp b/demo/gpudemo/main_opengl3core.cpp index 97b633646..08c5fadd3 100644 --- a/demo/gpudemo/main_opengl3core.cpp +++ b/demo/gpudemo/main_opengl3core.cpp @@ -65,16 +65,16 @@ btAlignedObjectArray demoNames; int selectedDemo = 0; GpuDemo::CreateFunc* allDemos[]= { + GpuSphereScene::MyCreateFunc, - GpuCompoundScene::MyCreateFunc, - GpuConvexScene::MyCreateFunc, ConcaveScene::MyCreateFunc, - + ConcaveCompoundScene::MyCreateFunc, + GpuCompoundScene::MyCreateFunc, PairBench::MyCreateFunc, diff --git a/demo/gpudemo/rigidbody/ConcaveScene.cpp b/demo/gpudemo/rigidbody/ConcaveScene.cpp index d4a66c73a..17962a411 100644 --- a/demo/gpudemo/rigidbody/ConcaveScene.cpp +++ b/demo/gpudemo/rigidbody/ConcaveScene.cpp @@ -154,17 +154,20 @@ GLInstanceGraphicsShape* createGraphicsShapeFromWavefrontObj(objLoader* obj) } } -void ConcaveScene::setupScene(const ConstructionInfo& ci) + +void ConcaveScene::createConcaveMesh(const ConstructionInfo& ci) { objLoader* objData = new objLoader(); + //char* fileName = "data/slopedPlane100.obj"; //char* fileName = "data/plane100.obj"; + //char* fileName = "data/teddy.obj";//"plane.obj"; // char* fileName = "data/sponza_closed.obj";//"plane.obj"; //char* fileName = "data/leoTest1.obj"; char* fileName = "data/samurai_monastry.obj"; - btVector3 shift(0,0,0);//150,-100,-120); - btVector4 scaling(10,10,10,1);//4,4,4,1); + btVector3 shift(0,0,0);//0,230,80);//150,-100,-120); + btVector4 scaling(4,4,4,1); FILE* f = 0; char relativeFileName[1024]; @@ -233,12 +236,33 @@ void ConcaveScene::setupScene(const ConstructionInfo& ci) index++; } + + } } - int strideInBytes = 9*sizeof(float); +} + +void ConcaveScene::setupScene(const ConstructionInfo& ci) +{ + + createConcaveMesh(ci); + + createDynamicObjects(ci); + + float camPos[4]={0,0,0,0};//65.5,4.5,65.5,0}; + //float camPos[4]={1,12.5,1.5,0}; + m_instancingRenderer->setCameraPitch(45); + m_instancingRenderer->setCameraTargetPosition(camPos); + m_instancingRenderer->setCameraDistance(370); + +} + +void ConcaveScene::createDynamicObjects(const ConstructionInfo& ci) +{ + int strideInBytes = 9*sizeof(float); int numVertices = sizeof(cube_vertices)/strideInBytes; int numIndices = sizeof(cube_indices)/sizeof(int); //int shapeId = ci.m_instancingRenderer->registerShape(&cube_vertices[0],numVertices,cube_indices,numIndices); @@ -247,7 +271,7 @@ void ConcaveScene::setupScene(const ConstructionInfo& ci) int mask=1; - + int index=0; if (1) @@ -287,10 +311,63 @@ void ConcaveScene::setupScene(const ConstructionInfo& ci) } } } - float camPos[4]={0,0,0,0};//65.5,4.5,65.5,0}; + +} + +void ConcaveCompoundScene::setupScene(const ConstructionInfo& ci) +{ + ConcaveScene::setupScene(ci); + + float camPos[4]={0,50,0,0};//65.5,4.5,65.5,0}; //float camPos[4]={1,12.5,1.5,0}; m_instancingRenderer->setCameraPitch(45); m_instancingRenderer->setCameraTargetPosition(camPos); - m_instancingRenderer->setCameraDistance(370); + m_instancingRenderer->setCameraDistance(40); -} \ No newline at end of file + +} + +void ConcaveCompoundScene::createDynamicObjects(const ConstructionInfo& ci) +{ + btVector4 colors[4] = + { + btVector4(1,0,0,1), + btVector4(0,1,0,1), + btVector4(0,1,1,1), + btVector4(1,1,0,1), + }; + + int index=0; + int curColor = 0; + float radius = 1; + //int colIndex = m_data->m_np->registerConvexHullShape(&cube_vertices[0],strideInBytes,numVertices, scaling); + int colIndex = m_data->m_np->registerSphereShape(radius);//>registerConvexHullShape(&cube_vertices[0],strideInBytes,numVertices, scaling); + int prevGraphicsShapeIndex = registerGraphicsSphereShape(ci,radius,false); + + for (int i=0;iregisterGraphicsInstance(prevGraphicsShapeIndex,position,orn,color,scaling); + int pid = m_data->m_rigidBodyPipeline->registerPhysicsInstance(mass,position,orn,colIndex,index); + + index++; + } + } + } +} diff --git a/demo/gpudemo/rigidbody/ConcaveScene.h b/demo/gpudemo/rigidbody/ConcaveScene.h index a2af06e6b..1fab1f74f 100644 --- a/demo/gpudemo/rigidbody/ConcaveScene.h +++ b/demo/gpudemo/rigidbody/ConcaveScene.h @@ -22,6 +22,34 @@ public: virtual void setupScene(const ConstructionInfo& ci); + virtual void createDynamicObjects(const ConstructionInfo& ci); + + virtual void createConcaveMesh(const ConstructionInfo& ci); + + +}; + +class ConcaveCompoundScene : public ConcaveScene +{ +public: + + ConcaveCompoundScene(){} + virtual ~ConcaveCompoundScene(){} + virtual const char* getName() + { + return "GRBConcaveCompound"; + } + + static GpuDemo* MyCreateFunc() + { + GpuDemo* demo = new ConcaveCompoundScene; + return demo; + } + + virtual void setupScene(const ConstructionInfo& ci); + + virtual void createDynamicObjects(const ConstructionInfo& ci); + }; #endif //CONCAVE_SCENE_H diff --git a/demo/gpudemo/rigidbody/GpuSphereScene.cpp b/demo/gpudemo/rigidbody/GpuSphereScene.cpp index c0d6b61d2..320335ad1 100644 --- a/demo/gpudemo/rigidbody/GpuSphereScene.cpp +++ b/demo/gpudemo/rigidbody/GpuSphereScene.cpp @@ -35,7 +35,7 @@ void GpuSphereScene::setupScene(const ConstructionInfo& ci) if (0) { - float radius = 40; + float radius = 60; int prevGraphicsShapeIndex = -1; { @@ -86,7 +86,7 @@ void GpuSphereScene::setupScene(const ConstructionInfo& ci) float mass = 0.f; //btVector3 position((j&1)+i*2.2,1+j*2.,(j&1)+k*2.2); - btVector3 position(0,-40,0); + btVector3 position(0,0,0); btQuaternion orn(0,0,0,1); @@ -106,41 +106,7 @@ void GpuSphereScene::setupScene(const ConstructionInfo& ci) - { - - - - int prevGraphicsShapeIndex = -1; - float radius = 41; - if (1)//radius>=100) - { - int numVertices = sizeof(detailed_sphere_vertices)/strideInBytes; - int numIndices = sizeof(detailed_sphere_indices)/sizeof(int); - prevGraphicsShapeIndex = ci.m_instancingRenderer->registerShape(&detailed_sphere_vertices[0],numVertices,detailed_sphere_indices,numIndices); - } else - { - bool usePointSprites = true; - if (usePointSprites) - { - int numVertices = sizeof(point_sphere_vertices)/strideInBytes; - int numIndices = sizeof(point_sphere_indices)/sizeof(int); - prevGraphicsShapeIndex = ci.m_instancingRenderer->registerShape(&point_sphere_vertices[0],numVertices,point_sphere_indices,numIndices,BT_GL_POINTS); - } else - { - if (radius>=10) - { - int numVertices = sizeof(medium_sphere_vertices)/strideInBytes; - int numIndices = sizeof(medium_sphere_indices)/sizeof(int); - prevGraphicsShapeIndex = ci.m_instancingRenderer->registerShape(&medium_sphere_vertices[0],numVertices,medium_sphere_indices,numIndices); - } else - { - int numVertices = sizeof(low_sphere_vertices)/strideInBytes; - int numIndices = sizeof(low_sphere_indices)/sizeof(int); - prevGraphicsShapeIndex = ci.m_instancingRenderer->registerShape(&low_sphere_vertices[0],numVertices,low_sphere_indices,numIndices); - } - } - } - + @@ -160,35 +126,39 @@ void GpuSphereScene::setupScene(const ConstructionInfo& ci) - int curColor = 0; - - //int colIndex = m_data->m_np->registerConvexHullShape(&cube_vertices[0],strideInBytes,numVertices, scaling); - int colIndex = m_data->m_np->registerSphereShape(radius);//>registerConvexHullShape(&cube_vertices[0],strideInBytes,numVertices, scaling); - for (int i=0;im_np->registerConvexHullShape(&cube_vertices[0],strideInBytes,numVertices, scaling); + int colIndex = m_data->m_np->registerSphereShape(radius);//>registerConvexHullShape(&cube_vertices[0],strideInBytes,numVertices, scaling); + int prevGraphicsShapeIndex = registerGraphicsSphereShape(ci,radius,false); - btVector3 position((j&1)+i*142.2,-51+j*142.,(j&1)+k*142.2); - //btVector3 position(0,-41,0);//0,0,0);//i*radius*3,-41+j*radius*3,k*radius*3); + //for (int i=0;iregisterGraphicsInstance(prevGraphicsShapeIndex,position,orn,color,scaling); - int pid = m_data->m_rigidBodyPipeline->registerPhysicsInstance(mass,position,orn,colIndex,index); + btVector4 color = colors[curColor]; + curColor++; + curColor&=3; + btVector4 scaling(radius,radius,radius,1); + int id = ci.m_instancingRenderer->registerGraphicsInstance(prevGraphicsShapeIndex,position,orn,color,scaling); + int pid = m_data->m_rigidBodyPipeline->registerPhysicsInstance(mass,position,orn,colIndex,index); - index++; - } + index++; } } } + if (1) { @@ -205,7 +175,7 @@ void GpuSphereScene::setupScene(const ConstructionInfo& ci) //int i=0;int j=0; { //int colIndex = m_data->m_np->registerPlaneShape(normal,constant);//>registerConvexHullShape(&cube_vertices[0],strideInBytes,numVertices, scaling); - btVector4 position(2*i,k*2,2*j+8,0); + btVector4 position(2*i,70+k*2,2*j+8,0); //btQuaternion orn(0,0,0,1); btQuaternion orn(btVector3(1,0,0),0.3); @@ -221,7 +191,7 @@ void GpuSphereScene::setupScene(const ConstructionInfo& ci) float camPos[4]={ci.arraySizeX,ci.arraySizeY/2,ci.arraySizeZ,0}; //float camPos[4]={1,12.5,1.5,0}; m_instancingRenderer->setCameraTargetPosition(camPos); - m_instancingRenderer->setCameraDistance(30); + m_instancingRenderer->setCameraDistance(130); char msg[1024]; diff --git a/opencl/gpu_rigidbody/host/Solver.cpp b/opencl/gpu_rigidbody/host/Solver.cpp index 3ce7c2a3d..9c22c68b4 100644 --- a/opencl/gpu_rigidbody/host/Solver.cpp +++ b/opencl/gpu_rigidbody/host/Solver.cpp @@ -17,7 +17,7 @@ subject to the following restrictions: #include "Solver.h" ///useNewBatchingKernel is a rewritten kernel using just a single thread of the warp, for experiments -bool useNewBatchingKernel = true; +bool useNewBatchingKernel = false; #define SOLVER_SETUP_KERNEL_PATH "opencl/gpu_rigidbody/kernels/solverSetup.cl" #define SOLVER_SETUP2_KERNEL_PATH "opencl/gpu_rigidbody/kernels/solverSetup2.cl" diff --git a/opencl/gpu_sat/host/ConvexHullContact.cpp b/opencl/gpu_sat/host/ConvexHullContact.cpp index 584c77c9a..679b118f8 100644 --- a/opencl/gpu_sat/host/ConvexHullContact.cpp +++ b/opencl/gpu_sat/host/ConvexHullContact.cpp @@ -62,13 +62,18 @@ m_totalContactsOut(m_context, m_queue) btAssert(errNum==CL_SUCCESS); m_findSeparatingAxisKernel = btOpenCLUtils::compileCLKernelFromString(m_context, m_device,src, "findSeparatingAxisKernel",&errNum,satProg ); + btAssert(m_findSeparatingAxisKernel); + btAssert(errNum==CL_SUCCESS); m_findConcaveSeparatingAxisKernel = btOpenCLUtils::compileCLKernelFromString(m_context, m_device,src, "findConcaveSeparatingAxisKernel",&errNum,satProg ); - - + btAssert(m_findConcaveSeparatingAxisKernel); + btAssert(errNum==CL_SUCCESS); + m_findCompoundPairsKernel = btOpenCLUtils::compileCLKernelFromString(m_context, m_device,src, "findCompoundPairsKernel",&errNum,satProg ); - + btAssert(m_findCompoundPairsKernel); + btAssert(errNum==CL_SUCCESS); m_processCompoundPairsKernel = btOpenCLUtils::compileCLKernelFromString(m_context, m_device,src, "processCompoundPairsKernel",&errNum,satProg ); + btAssert(m_processCompoundPairsKernel); btAssert(errNum==CL_SUCCESS); } @@ -126,12 +131,16 @@ 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",disableKernelCaching); + cl_program primitiveContactsProg = btOpenCLUtils::compileCLProgramFromString(m_context,m_device,primitiveContactsSrc,&errNum,"","opencl/gpu_sat/kernels/primitiveContacts.cl"); btAssert(errNum==CL_SUCCESS); m_primitiveContactsKernel = btOpenCLUtils::compileCLKernelFromString(m_context, m_device,primitiveContactsSrc, "primitiveContactsKernel",&errNum,primitiveContactsProg,""); btAssert(errNum==CL_SUCCESS); + m_findConcaveSphereContactsKernel = btOpenCLUtils::compileCLKernelFromString(m_context, m_device,primitiveContactsSrc, "findConcaveSphereContactsKernel",&errNum,primitiveContactsProg ); + btAssert(errNum==CL_SUCCESS); + btAssert(m_findConcaveSphereContactsKernel); + m_processCompoundPairsPrimitivesKernel = btOpenCLUtils::compileCLKernelFromString(m_context, m_device,primitiveContactsSrc, "processCompoundPairsPrimitivesKernel",&errNum,primitiveContactsProg,""); btAssert(errNum==CL_SUCCESS); btAssert(m_processCompoundPairsPrimitivesKernel); @@ -166,6 +175,9 @@ GpuSatCollision::~GpuSatCollision() if (m_primitiveContactsKernel) clReleaseKernel(m_primitiveContactsKernel); + if (m_findConcaveSphereContactsKernel) + clReleaseKernel(m_findConcaveSphereContactsKernel); + if (m_processCompoundPairsPrimitivesKernel) clReleaseKernel(m_processCompoundPairsPrimitivesKernel); @@ -828,6 +840,45 @@ void GpuSatCollision::computeConvexConvexContactsGPUSAT( const btOpenCLArraygetBufferCL(),true), + btBufferInfoCL( gpuCollidables.getBufferCL(),true), + btBufferInfoCL( convexData.getBufferCL(),true), + btBufferInfoCL( gpuVertices.getBufferCL(),true), + btBufferInfoCL( gpuUniqueEdges.getBufferCL(),true), + btBufferInfoCL( gpuFaces.getBufferCL(),true), + btBufferInfoCL( gpuIndices.getBufferCL(),true), + btBufferInfoCL( clAabbsWS.getBufferCL(),true), + btBufferInfoCL( contactOut->getBufferCL()), + btBufferInfoCL( m_totalContactsOut.getBufferCL()) + }; + + btLauncherCL launcher(m_queue, m_findConcaveSphereContactsKernel); + launcher.setBuffers( bInfo, sizeof(bInfo)/sizeof(btBufferInfoCL) ); + + launcher.setConst( numConcavePairs ); + launcher.setConst(maxContactCapacity); + + int num = numConcavePairs; + launcher.launch1D( num); + clFinish(m_queue); + nContacts = m_totalContactsOut.at(0); + } + + } + + + #ifdef __APPLE__ bool contactClippingOnGpu = true; #else @@ -869,6 +920,7 @@ void GpuSatCollision::computeConvexConvexContactsGPUSAT( const btOpenCLArray 0 && r2 > 0 && r3 > 0 ) + return true; + if ( r1 <= 0 && r2 <= 0 && r3 <= 0 ) + return true; + return false; + +} + + +float segmentSqrDistance(float4 from, float4 to,float4 p, float4* nearest) +{ + float4 diff = p - from; + float4 v = to - from; + float t = dot(v,diff); + + if (t > 0) + { + float dotVV = dot(v,v); + if (t < dotVV) + { + t /= dotVV; + diff -= t*v; + } else + { + t = 1; + diff -= v; + } + } else + { + t = 0; + } + *nearest = from + t*v; + return dot(diff,diff); +} + + +void computeContactSphereTriangle(int pairIndex, + int bodyIndexA, int bodyIndexB, + int collidableIndexA, int collidableIndexB, + __global const BodyData* rigidBodies, + __global const btCollidableGpu* collidables, + const float4* triangleVertices, + __global Contact4* restrict globalContactsOut, + counter32_t nGlobalContactsOut, + int maxContactCapacity, + float4 spherePos2, + float radius, + float4 pos, + float4 quat + ) +{ + + float4 invPos; + float4 invOrn; + + trInverse(pos,quat, &invPos,&invOrn); + float4 spherePos = transform(&spherePos2,&invPos,&invOrn); + int numFaces = 3; + float4 closestPnt = (float4)(0, 0, 0, 0); + float4 hitNormalWorld = (float4)(0, 0, 0, 0); + float minDist = -1000000.f; + bool bCollide = true; + + + ////////////////////////////////////// + + float4 sphereCenter; + sphereCenter = spherePos; + + const float4* vertices = triangleVertices; + float contactBreakingThreshold = 0.f;//todo? + float radiusWithThreshold = radius + contactBreakingThreshold; + float4 edge10; + edge10 = vertices[1]-vertices[0]; + edge10.w = 0.f;//is this needed? + float4 edge20; + edge20 = vertices[2]-vertices[0]; + edge20.w = 0.f;//is this needed? + float4 normal = cross3(edge10,edge20); + normal = normalize(normal); + float4 p1ToCenter; + p1ToCenter = sphereCenter - vertices[0]; + + float distanceFromPlane = dot(p1ToCenter,normal); + + if (distanceFromPlane < 0.f) + { + //triangle facing the other way + distanceFromPlane *= -1.f; + normal *= -1.f; + } + hitNormalWorld = normal; + + bool isInsideContactPlane = distanceFromPlane < radiusWithThreshold; + + // Check for contact / intersection + bool hasContact = false; + float4 contactPoint; + if (isInsideContactPlane) + { + + if (pointInTriangle(vertices,&normal, &sphereCenter)) + { + // Inside the contact wedge - touches a point on the shell plane + hasContact = true; + contactPoint = sphereCenter - normal*distanceFromPlane; + + } else { + // Could be inside one of the contact capsules + float contactCapsuleRadiusSqr = radiusWithThreshold*radiusWithThreshold; + float4 nearestOnEdge; + int numEdges = 3; + for (int i = 0; i < numEdges; i++) + { + float4 pa =vertices[i]; + float4 pb = vertices[(i+1)%3]; + + float distanceSqr = segmentSqrDistance(pa,pb,sphereCenter, &nearestOnEdge); + if (distanceSqr < contactCapsuleRadiusSqr) + { + // Yep, we're inside a capsule + hasContact = true; + contactPoint = nearestOnEdge; + + } + + } + } + } + + if (hasContact) + { + + closestPnt = contactPoint; + float4 contactToCenter = sphereCenter - contactPoint; + minDist = length(contactToCenter); + if (minDist>0.f) + { + hitNormalWorld = normalize(contactToCenter);//*(1./minDist); + } + bCollide = true; + } + + + ///////////////////////////////////// + + if (bCollide && minDist > -10000) + { + + float4 normalOnSurfaceB1 = qtRotate(quat,-hitNormalWorld); + float4 pOnB1 = transform(&closestPnt,&pos,&quat); + float actualDepth = minDist-radius; + + + if (actualDepth<=0.f) + { + pOnB1.w = actualDepth; + int dstIdx; + AppendInc( nGlobalContactsOut, dstIdx ); + + if (dstIdx < maxContactCapacity) + { + __global Contact4* c = &globalContactsOut[dstIdx]; + c->m_worldNormal = normalOnSurfaceB1; + 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; + c->m_worldPos[0] = pOnB1; + GET_NPOINTS(*c) = 1; + } + + } + }//if (hasCollision) + +} + + + +// work-in-progress +__kernel void findConcaveSphereContactsKernel( __global int4* concavePairs, + __global const BodyData* rigidBodies, + __global const btCollidableGpu* collidables, + __global const ConvexPolyhedronCL* convexShapes, + __global const float4* vertices, + __global const float4* uniqueEdges, + __global const btGpuFace* faces, + __global const int* indices, + __global btAabbCL* aabbs, + __global Contact4* restrict globalContactsOut, + counter32_t nGlobalContactsOut, + int numConcavePairs, int maxContactCapacity + ) +{ + + int i = get_global_id(0); + if (i>=numConcavePairs) + return; + int pairIdx = i; + + int bodyIndexA = concavePairs[i].x; + int bodyIndexB = concavePairs[i].y; + + int collidableIndexA = rigidBodies[bodyIndexA].m_collidableIdx; + int collidableIndexB = rigidBodies[bodyIndexB].m_collidableIdx; + + int shapeIndexA = collidables[collidableIndexA].m_shapeIndex; + int shapeIndexB = collidables[collidableIndexB].m_shapeIndex; + + if (collidables[collidableIndexB].m_shapeType==SHAPE_SPHERE) + { + int f = concavePairs[i].z; + btGpuFace face = faces[convexShapes[shapeIndexA].m_faceOffset+f]; + + float4 verticesA[3]; + for (int i=0;i<3;i++) + { + int index = indices[face.m_indexOffset+i]; + float4 vert = vertices[convexShapes[shapeIndexA].m_vertexOffset+index]; + verticesA[i] = vert; + } + + float4 spherePos = rigidBodies[bodyIndexB].m_pos; + float sphereRadius = collidables[collidableIndexB].m_radius; + float4 convexPos = rigidBodies[bodyIndexA].m_pos; + float4 convexOrn = rigidBodies[bodyIndexA].m_quat; + + computeContactSphereTriangle(i, bodyIndexB, bodyIndexA, collidableIndexB, collidableIndexA, + rigidBodies,collidables, + verticesA, + globalContactsOut, nGlobalContactsOut,maxContactCapacity, + spherePos,sphereRadius,convexPos,convexOrn); + + return; + } } \ No newline at end of file diff --git a/opencl/gpu_sat/kernels/primitiveContacts.h b/opencl/gpu_sat/kernels/primitiveContacts.h index d9924ba72..f1711dafd 100644 --- a/opencl/gpu_sat/kernels/primitiveContacts.h +++ b/opencl/gpu_sat/kernels/primitiveContacts.h @@ -53,6 +53,21 @@ static const char* primitiveContactsKernelsCL= \ " int m_bodyBPtrAndSignBit;\n" "} Contact4;\n" "\n" +"typedef struct \n" +"{\n" +" union\n" +" {\n" +" float4 m_min;\n" +" float m_minElems[4];\n" +" int m_minIndices[4];\n" +" };\n" +" union\n" +" {\n" +" float4 m_max;\n" +" float m_maxElems[4];\n" +" int m_maxIndices[4];\n" +" };\n" +"} btAabbCL;\n" "\n" "///keep this in sync with btCollidable.h\n" "typedef struct\n" @@ -275,8 +290,6 @@ static const char* primitiveContactsKernelsCL= \ "\n" "\n" "inline bool IsPointInPolygon(float4 p, \n" -" float4 posConvex,\n" -" float4 ornConvex,\n" " const btGpuFace* face,\n" " __global const float4* baseVertex,\n" " __global const int* convexIndices,\n" @@ -295,16 +308,14 @@ static const char* primitiveContactsKernelsCL= \ "\n" " \n" " float4 v0 = baseVertex[convexIndices[face->m_indexOffset + face->m_numIndices-1]];\n" -" float4 worldV0 = transform(&v0, &posConvex, &ornConvex);\n" " \n" -" b = worldV0;\n" +" b = v0;\n" "\n" " for(unsigned i=0; i != face->m_numIndices; ++i)\n" " {\n" " a = b;\n" " float4 vi = baseVertex[convexIndices[face->m_indexOffset + i]];\n" -" float4 worldVi = transform(&vi, &posConvex, &ornConvex);\n" -" b = worldVi;\n" +" b = vi;\n" " ab = b-a;\n" " ap = p-a;\n" " v = cross3(ab,plane);\n" @@ -324,9 +335,9 @@ static const char* primitiveContactsKernelsCL= \ " else\n" " {\n" " float s = 1.f - rt;\n" -" out[0].x = s * a.x + rt * b.x;\n" -" out[0].y = s * a.y + rt * b.y;\n" -" out[0].z = s * a.z + rt * b.z;\n" +" out[0].x = s * a.x + rt * b.x;\n" +" out[0].y = s * a.y + rt * b.y;\n" +" out[0].z = s * a.z + rt * b.z;\n" " }\n" " return false;\n" " }\n" @@ -348,22 +359,22 @@ static const char* primitiveContactsKernelsCL= \ " __global const btGpuFace* faces,\n" " __global Contact4* restrict globalContactsOut,\n" " counter32_t nGlobalContactsOut,\n" -" int numPairs)\n" +" int maxContactCapacity,\n" +" float4 spherePos2,\n" +" float radius,\n" +" float4 pos,\n" +" float4 quat\n" +" )\n" "{\n" "\n" -" float radius = collidables[collidableIndexA].m_radius;\n" -" float4 spherePos1 = rigidBodies[bodyIndexA].m_pos;\n" -" float4 sphereOrn = rigidBodies[bodyIndexA].m_quat;\n" +" float4 invPos;\n" +" float4 invOrn;\n" "\n" +" trInverse(pos,quat, &invPos,&invOrn);\n" "\n" +" float4 spherePos = transform(&spherePos2,&invPos,&invOrn);\n" "\n" -" float4 pos = rigidBodies[bodyIndexB].m_pos;\n" -" float4 quat = rigidBodies[bodyIndexB].m_quat;\n" -"\n" -" float4 spherePos = spherePos1 - pos;\n" -"\n" -" int collidableIndex = rigidBodies[bodyIndexB].m_collidableIdx;\n" -" int shapeIndex = collidables[collidableIndex].m_shapeIndex;\n" +" int shapeIndex = collidables[collidableIndexB].m_shapeIndex;\n" " int numFaces = convexShapes[shapeIndex].m_numFaces;\n" " float4 closestPnt = (float4)(0, 0, 0, 0);\n" " float4 hitNormalWorld = (float4)(0, 0, 0, 0);\n" @@ -376,7 +387,8 @@ static const char* primitiveContactsKernelsCL= \ "\n" " // set up a plane equation \n" " float4 planeEqn;\n" -" float4 n1 = qtRotate(quat, (float4)(face.m_plane.xyz, 0));\n" +" float4 n1 = face.m_plane;\n" +" n1.w = 0.f;\n" " planeEqn = n1;\n" " planeEqn.w = face.m_plane.w;\n" " \n" @@ -397,9 +409,9 @@ static const char* primitiveContactsKernelsCL= \ " {\n" " //might hit an edge or vertex\n" " float4 out;\n" +" float4 zeroPos = make_float4(0,0,0,0);\n" +"\n" " bool isInPoly = IsPointInPolygon(spherePos,\n" -" pos,\n" -" quat,\n" " &face,\n" " &convexVertices[convexShapes[shapeIndex].m_vertexOffset],\n" " convexIndices,\n" @@ -448,32 +460,42 @@ static const char* primitiveContactsKernelsCL= \ "\n" " \n" "\n" -" if (bCollide)\n" +" if (bCollide && minDist > -10000)\n" " {\n" -" float4 normalOnSurfaceB1 = -hitNormalWorld;\n" -" float4 pOnB1 = closestPnt+pos;\n" -" float actualDepth = minDist-radius;\n" -" pOnB1.w = actualDepth;\n" -"\n" -" int dstIdx;\n" -" AppendInc( nGlobalContactsOut, dstIdx );\n" +" float4 normalOnSurfaceB1 = qtRotate(quat,-hitNormalWorld);\n" +" float4 pOnB1 = transform(&closestPnt,&pos,&quat);\n" " \n" -" if (dstIdx < numPairs)\n" +" float actualDepth = minDist-radius;\n" +" if (actualDepth<=0.f)\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" +" \n" +"\n" +" pOnB1.w = actualDepth;\n" +"\n" +" int dstIdx;\n" +" AppendInc( nGlobalContactsOut, dstIdx );\n" +" \n" +" \n" +" if (1)//dstIdx < maxContactCapacity)\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" +" } \n" +"\n" +" }\n" " }//if (hasCollision)\n" "\n" "}\n" " \n" "\n" +"\n" +"\n" " \n" "void computeContactPlaneConvex(int pairIndex,\n" " int bodyIndexA, int bodyIndexB, \n" @@ -483,7 +505,7 @@ static const char* primitiveContactsKernelsCL= \ " __global const btGpuFace* faces,\n" " __global Contact4* restrict globalContactsOut,\n" " counter32_t nGlobalContactsOut,\n" -" int numPairs)\n" +" int maxContactCapacity)\n" "{\n" " float4 planeEq = faces[collidables[collidableIndexA].m_shapeIndex].m_plane;\n" " float radius = collidables[collidableIndexB].m_radius;\n" @@ -522,7 +544,7 @@ static const char* primitiveContactsKernelsCL= \ " int dstIdx;\n" " AppendInc( nGlobalContactsOut, dstIdx );\n" " \n" -" if (dstIdx < numPairs)\n" +" if (dstIdx < maxContactCapacity)\n" " {\n" " __global Contact4* c = &globalContactsOut[dstIdx];\n" " c->m_worldNormal = normalOnSurfaceB1;\n" @@ -549,7 +571,7 @@ static const char* primitiveContactsKernelsCL= \ " __global const int* indices,\n" " __global Contact4* restrict globalContactsOut,\n" " counter32_t nGlobalContactsOut,\n" -" int numPairs)\n" +" int numPairs, int maxContactCapacity)\n" "{\n" "\n" " int i = get_global_id(0);\n" @@ -581,7 +603,7 @@ static const char* primitiveContactsKernelsCL= \ "\n" "\n" " computeContactPlaneConvex( pairIndex, bodyIndexB,bodyIndexA, collidableIndexB,collidableIndexA, \n" -" rigidBodies,collidables,faces, globalContactsOut, nGlobalContactsOut,numPairs);\n" +" rigidBodies,collidables,faces, globalContactsOut, nGlobalContactsOut,maxContactCapacity);\n" " return;\n" " }\n" "\n" @@ -591,7 +613,7 @@ static const char* primitiveContactsKernelsCL= \ "\n" "\n" " computeContactPlaneConvex(pairIndex, bodyIndexA, bodyIndexB, collidableIndexA, collidableIndexB, \n" -" rigidBodies,collidables,faces, globalContactsOut, nGlobalContactsOut,numPairs);\n" +" rigidBodies,collidables,faces, globalContactsOut, nGlobalContactsOut,maxContactCapacity);\n" " return;\n" " \n" " }\n" @@ -600,8 +622,15 @@ static const char* primitiveContactsKernelsCL= \ " collidables[collidableIndexB].m_shapeType == SHAPE_CONVEX_HULL)\n" " {\n" " \n" +" float4 spherePos = rigidBodies[bodyIndexA].m_pos;\n" +" float sphereRadius = collidables[collidableIndexA].m_radius;\n" +" float4 convexPos = rigidBodies[bodyIndexB].m_pos;\n" +" float4 convexOrn = rigidBodies[bodyIndexB].m_quat;\n" +"\n" " computeContactSphereConvex(pairIndex, bodyIndexA, bodyIndexB, collidableIndexA, collidableIndexB, \n" -" rigidBodies,collidables,convexShapes,vertices,indices,faces, globalContactsOut, nGlobalContactsOut,numPairs);\n" +" rigidBodies,collidables,convexShapes,vertices,indices,faces, globalContactsOut, nGlobalContactsOut,maxContactCapacity,\n" +" spherePos,sphereRadius,convexPos,convexOrn);\n" +"\n" " return;\n" " }\n" "\n" @@ -609,8 +638,14 @@ static const char* primitiveContactsKernelsCL= \ " collidables[collidableIndexB].m_shapeType == SHAPE_SPHERE)\n" " {\n" " \n" +" float4 spherePos = rigidBodies[bodyIndexB].m_pos;\n" +" float sphereRadius = collidables[collidableIndexB].m_radius;\n" +" float4 convexPos = rigidBodies[bodyIndexA].m_pos;\n" +" float4 convexOrn = rigidBodies[bodyIndexA].m_quat;\n" +"\n" " computeContactSphereConvex(pairIndex, bodyIndexB, bodyIndexA, collidableIndexB, collidableIndexA, \n" -" rigidBodies,collidables,convexShapes,vertices,indices,faces, globalContactsOut, nGlobalContactsOut,numPairs);\n" +" rigidBodies,collidables,convexShapes,vertices,indices,faces, globalContactsOut, nGlobalContactsOut,maxContactCapacity,\n" +" spherePos,sphereRadius,convexPos,convexOrn);\n" " return;\n" " }\n" " \n" @@ -644,9 +679,9 @@ static const char* primitiveContactsKernelsCL= \ " contactPosB.w = dist;\n" " \n" " int dstIdx;\n" -" AppendInc( nGlobalContactsOut, dstIdx );\n" +" AppendInc( nGlobalContactsOut, dstIdx );\n" " \n" -" if (dstIdx < numPairs)\n" +" if (dstIdx < maxContactCapacity)\n" " {\n" " __global Contact4* c = &globalContactsOut[dstIdx];\n" " c->m_worldNormal = -normalOnSurfaceB;\n" @@ -668,4 +703,371 @@ static const char* primitiveContactsKernelsCL= \ "\n" "}\n" "\n" +"\n" +"// work-in-progress\n" +"__kernel void processCompoundPairsPrimitivesKernel( __global const int4* gpuCompoundPairs,\n" +" __global const BodyData* rigidBodies, \n" +" __global const btCollidableGpu* collidables,\n" +" __global const ConvexPolyhedronCL* convexShapes, \n" +" __global const float4* vertices,\n" +" __global const float4* uniqueEdges,\n" +" __global const btGpuFace* faces,\n" +" __global const int* indices,\n" +" __global btAabbCL* aabbs,\n" +" __global const btGpuChildShape* gpuChildShapes,\n" +" __global Contact4* restrict globalContactsOut,\n" +" counter32_t nGlobalContactsOut,\n" +" int numCompoundPairs, int maxContactCapacity\n" +" )\n" +"{\n" +"\n" +" int i = get_global_id(0);\n" +" if (i= 0)\n" +" {\n" +" collidableIndexA = gpuChildShapes[childShapeIndexA].m_shapeIndex;\n" +" float4 childPosA = gpuChildShapes[childShapeIndexA].m_childPosition;\n" +" float4 childOrnA = gpuChildShapes[childShapeIndexA].m_childOrientation;\n" +" float4 newPosA = qtRotate(ornA,childPosA)+posA;\n" +" float4 newOrnA = qtMul(ornA,childOrnA);\n" +" posA = newPosA;\n" +" ornA = newOrnA;\n" +" } else\n" +" {\n" +" collidableIndexA = rigidBodies[bodyIndexA].m_collidableIdx;\n" +" }\n" +" \n" +" if (childShapeIndexB>=0)\n" +" {\n" +" collidableIndexB = gpuChildShapes[childShapeIndexB].m_shapeIndex;\n" +" float4 childPosB = gpuChildShapes[childShapeIndexB].m_childPosition;\n" +" float4 childOrnB = gpuChildShapes[childShapeIndexB].m_childOrientation;\n" +" float4 newPosB = transform(&childPosB,&posB,&ornB);\n" +" float4 newOrnB = qtMul(ornB,childOrnB);\n" +" posB = newPosB;\n" +" ornB = newOrnB;\n" +" } else\n" +" {\n" +" collidableIndexB = rigidBodies[bodyIndexB].m_collidableIdx; \n" +" }\n" +" \n" +" int shapeIndexA = collidables[collidableIndexA].m_shapeIndex;\n" +" int shapeIndexB = collidables[collidableIndexB].m_shapeIndex;\n" +" \n" +" int shapeTypeA = collidables[collidableIndexA].m_shapeType;\n" +" int shapeTypeB = collidables[collidableIndexB].m_shapeType;\n" +"\n" +" int pairIndex = i;\n" +" if ((shapeTypeA == SHAPE_CONVEX_HULL) && (shapeTypeB == SHAPE_SPHERE))\n" +" {\n" +" float4 spherePos = rigidBodies[bodyIndexB].m_pos;\n" +" float sphereRadius = collidables[collidableIndexB].m_radius;\n" +" float4 convexPos = posA;\n" +" float4 convexOrn = ornA;\n" +" \n" +" computeContactSphereConvex(pairIndex, bodyIndexB, bodyIndexA , collidableIndexB,collidableIndexA, \n" +" rigidBodies,collidables,convexShapes,vertices,indices,faces, globalContactsOut, nGlobalContactsOut,maxContactCapacity,\n" +" spherePos,sphereRadius,convexPos,convexOrn);\n" +" \n" +" return;\n" +" }\n" +"\n" +" if ((shapeTypeA == SHAPE_SPHERE) && (shapeTypeB == SHAPE_CONVEX_HULL))\n" +" {\n" +"\n" +" float4 spherePos = rigidBodies[bodyIndexA].m_pos;\n" +" float sphereRadius = collidables[collidableIndexA].m_radius;\n" +" float4 convexPos = posB;\n" +" float4 convexOrn = ornB;\n" +"\n" +" \n" +" computeContactSphereConvex(pairIndex, bodyIndexA, bodyIndexB, collidableIndexA, collidableIndexB, \n" +" rigidBodies,collidables,convexShapes,vertices,indices,faces, globalContactsOut, nGlobalContactsOut,maxContactCapacity,\n" +" spherePos,sphereRadius,convexPos,convexOrn);\n" +" \n" +" return;\n" +" }\n" +" }// if (i 0 && r2 > 0 && r3 > 0 )\n" +" return true;\n" +" if ( r1 <= 0 && r2 <= 0 && r3 <= 0 ) \n" +" return true;\n" +" return false;\n" +"\n" +"}\n" +"\n" +"\n" +"float segmentSqrDistance(float4 from, float4 to,float4 p, float4* nearest) \n" +"{\n" +" float4 diff = p - from;\n" +" float4 v = to - from;\n" +" float t = dot(v,diff);\n" +" \n" +" if (t > 0) \n" +" {\n" +" float dotVV = dot(v,v);\n" +" if (t < dotVV) \n" +" {\n" +" t /= dotVV;\n" +" diff -= t*v;\n" +" } else \n" +" {\n" +" t = 1;\n" +" diff -= v;\n" +" }\n" +" } else\n" +" {\n" +" t = 0;\n" +" }\n" +" *nearest = from + t*v;\n" +" return dot(diff,diff); \n" +"}\n" +"\n" +"\n" +"void computeContactSphereTriangle(int pairIndex,\n" +" int bodyIndexA, int bodyIndexB,\n" +" int collidableIndexA, int collidableIndexB, \n" +" __global const BodyData* rigidBodies, \n" +" __global const btCollidableGpu* collidables,\n" +" const float4* triangleVertices,\n" +" __global Contact4* restrict globalContactsOut,\n" +" counter32_t nGlobalContactsOut,\n" +" int maxContactCapacity,\n" +" float4 spherePos2,\n" +" float radius,\n" +" float4 pos,\n" +" float4 quat\n" +" )\n" +"{\n" +"\n" +" float4 invPos;\n" +" float4 invOrn;\n" +"\n" +" trInverse(pos,quat, &invPos,&invOrn);\n" +" float4 spherePos = transform(&spherePos2,&invPos,&invOrn);\n" +" int numFaces = 3;\n" +" float4 closestPnt = (float4)(0, 0, 0, 0);\n" +" float4 hitNormalWorld = (float4)(0, 0, 0, 0);\n" +" float minDist = -1000000.f;\n" +" bool bCollide = true;\n" +"\n" +" \n" +" //////////////////////////////////////\n" +"\n" +" float4 sphereCenter;\n" +" sphereCenter = spherePos;\n" +"\n" +" const float4* vertices = triangleVertices;\n" +" float contactBreakingThreshold = 0.f;//todo?\n" +" float radiusWithThreshold = radius + contactBreakingThreshold;\n" +" float4 edge10;\n" +" edge10 = vertices[1]-vertices[0];\n" +" edge10.w = 0.f;//is this needed?\n" +" float4 edge20;\n" +" edge20 = vertices[2]-vertices[0];\n" +" edge20.w = 0.f;//is this needed?\n" +" float4 normal = cross3(edge10,edge20);\n" +" normal = normalize(normal);\n" +" float4 p1ToCenter;\n" +" p1ToCenter = sphereCenter - vertices[0];\n" +" \n" +" float distanceFromPlane = dot(p1ToCenter,normal);\n" +"\n" +" if (distanceFromPlane < 0.f)\n" +" {\n" +" //triangle facing the other way\n" +" distanceFromPlane *= -1.f;\n" +" normal *= -1.f;\n" +" }\n" +" hitNormalWorld = normal;\n" +"\n" +" bool isInsideContactPlane = distanceFromPlane < radiusWithThreshold;\n" +" \n" +" // Check for contact / intersection\n" +" bool hasContact = false;\n" +" float4 contactPoint;\n" +" if (isInsideContactPlane) \n" +" {\n" +" \n" +" if (pointInTriangle(vertices,&normal, &sphereCenter)) \n" +" {\n" +" // Inside the contact wedge - touches a point on the shell plane\n" +" hasContact = true;\n" +" contactPoint = sphereCenter - normal*distanceFromPlane;\n" +" \n" +" } else {\n" +" // Could be inside one of the contact capsules\n" +" float contactCapsuleRadiusSqr = radiusWithThreshold*radiusWithThreshold;\n" +" float4 nearestOnEdge;\n" +" int numEdges = 3;\n" +" for (int i = 0; i < numEdges; i++) \n" +" {\n" +" float4 pa =vertices[i];\n" +" float4 pb = vertices[(i+1)%3];\n" +"\n" +" float distanceSqr = segmentSqrDistance(pa,pb,sphereCenter, &nearestOnEdge);\n" +" if (distanceSqr < contactCapsuleRadiusSqr) \n" +" {\n" +" // Yep, we're inside a capsule\n" +" hasContact = true;\n" +" contactPoint = nearestOnEdge;\n" +" \n" +" }\n" +" \n" +" }\n" +" }\n" +" }\n" +"\n" +" if (hasContact) \n" +" {\n" +"\n" +" closestPnt = contactPoint;\n" +" float4 contactToCenter = sphereCenter - contactPoint;\n" +" minDist = length(contactToCenter);\n" +" if (minDist>0.f)\n" +" {\n" +" hitNormalWorld = normalize(contactToCenter);//*(1./minDist);\n" +" }\n" +" bCollide = true;\n" +" }\n" +"\n" +"\n" +" /////////////////////////////////////\n" +"\n" +" if (bCollide && minDist > -10000)\n" +" {\n" +" \n" +" float4 normalOnSurfaceB1 = qtRotate(quat,-hitNormalWorld);\n" +" float4 pOnB1 = transform(&closestPnt,&pos,&quat);\n" +" float actualDepth = minDist-radius;\n" +"\n" +" \n" +" if (actualDepth<=0.f)\n" +" {\n" +" pOnB1.w = actualDepth;\n" +" int dstIdx;\n" +" AppendInc( nGlobalContactsOut, dstIdx );\n" +" \n" +" if (dstIdx < maxContactCapacity)\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" +" } \n" +"\n" +" }\n" +" }//if (hasCollision)\n" +"\n" +"}\n" +"\n" +"\n" +"\n" +"// work-in-progress\n" +"__kernel void findConcaveSphereContactsKernel( __global int4* concavePairs,\n" +" __global const BodyData* rigidBodies,\n" +" __global const btCollidableGpu* collidables,\n" +" __global const ConvexPolyhedronCL* convexShapes, \n" +" __global const float4* vertices,\n" +" __global const float4* uniqueEdges,\n" +" __global const btGpuFace* faces,\n" +" __global const int* indices,\n" +" __global btAabbCL* aabbs,\n" +" __global Contact4* restrict globalContactsOut,\n" +" counter32_t nGlobalContactsOut,\n" +" int numConcavePairs, int maxContactCapacity\n" +" )\n" +"{\n" +"\n" +" int i = get_global_id(0);\n" +" if (i>=numConcavePairs)\n" +" return;\n" +" int pairIdx = i;\n" +"\n" +" int bodyIndexA = concavePairs[i].x;\n" +" int bodyIndexB = concavePairs[i].y;\n" +"\n" +" int collidableIndexA = rigidBodies[bodyIndexA].m_collidableIdx;\n" +" int collidableIndexB = rigidBodies[bodyIndexB].m_collidableIdx;\n" +"\n" +" int shapeIndexA = collidables[collidableIndexA].m_shapeIndex;\n" +" int shapeIndexB = collidables[collidableIndexB].m_shapeIndex;\n" +"\n" +" if (collidables[collidableIndexB].m_shapeType==SHAPE_SPHERE)\n" +" {\n" +" int f = concavePairs[i].z;\n" +" btGpuFace face = faces[convexShapes[shapeIndexA].m_faceOffset+f];\n" +" \n" +" float4 verticesA[3];\n" +" for (int i=0;i<3;i++)\n" +" {\n" +" int index = indices[face.m_indexOffset+i];\n" +" float4 vert = vertices[convexShapes[shapeIndexA].m_vertexOffset+index];\n" +" verticesA[i] = vert;\n" +" }\n" +"\n" +" float4 spherePos = rigidBodies[bodyIndexB].m_pos;\n" +" float sphereRadius = collidables[collidableIndexB].m_radius;\n" +" float4 convexPos = rigidBodies[bodyIndexA].m_pos;\n" +" float4 convexOrn = rigidBodies[bodyIndexA].m_quat;\n" +"\n" +" computeContactSphereTriangle(i, bodyIndexB, bodyIndexA, collidableIndexB, collidableIndexA, \n" +" rigidBodies,collidables,\n" +" verticesA,\n" +" globalContactsOut, nGlobalContactsOut,maxContactCapacity,\n" +" spherePos,sphereRadius,convexPos,convexOrn);\n" +"\n" +" return;\n" +" }\n" +"}\n" ; diff --git a/opencl/gpu_sat/kernels/sat.cl b/opencl/gpu_sat/kernels/sat.cl index c44fc9dc6..465cdd86e 100644 --- a/opencl/gpu_sat/kernels/sat.cl +++ b/opencl/gpu_sat/kernels/sat.cl @@ -1081,6 +1081,13 @@ __kernel void findConcaveSeparatingAxisKernel( __global int4* concavePairs, int shapeIndexA = collidables[collidableIndexA].m_shapeIndex; int shapeIndexB = collidables[collidableIndexB].m_shapeIndex; + if (collidables[collidableIndexB].m_shapeType!=SHAPE_CONVEX_HULL) + { + concavePairs[pairIdx].w = 0; + return; + } + + int numFacesA = convexShapes[shapeIndexA].m_numFaces; int numActualConcaveConvexTests = 0; diff --git a/opencl/gpu_sat/kernels/satKernels.h b/opencl/gpu_sat/kernels/satKernels.h index 26b378280..6c1cd700f 100644 --- a/opencl/gpu_sat/kernels/satKernels.h +++ b/opencl/gpu_sat/kernels/satKernels.h @@ -708,6 +708,15 @@ static const char* satKernelsCL= \ " int shapeIndexA = collidables[collidableIndexA].m_shapeIndex;\n" " int shapeIndexB = collidables[collidableIndexB].m_shapeIndex;\n" " \n" +" int shapeTypeA = collidables[collidableIndexA].m_shapeType;\n" +" int shapeTypeB = collidables[collidableIndexB].m_shapeType;\n" +" \n" +"\n" +" if ((shapeTypeA != SHAPE_CONVEX_HULL) || (shapeTypeB != SHAPE_CONVEX_HULL))\n" +" {\n" +" return;\n" +" }\n" +"\n" " int hasSeparatingAxis = 5;\n" " \n" " int numFacesA = convexShapes[shapeIndexA].m_numFaces;\n" @@ -1074,6 +1083,13 @@ static const char* satKernelsCL= \ " int shapeIndexA = collidables[collidableIndexA].m_shapeIndex;\n" " int shapeIndexB = collidables[collidableIndexB].m_shapeIndex;\n" "\n" +" if (collidables[collidableIndexB].m_shapeType!=SHAPE_CONVEX_HULL)\n" +" {\n" +" concavePairs[pairIdx].w = 0;\n" +" return;\n" +" }\n" +"\n" +"\n" "\n" " int numFacesA = convexShapes[shapeIndexA].m_numFaces;\n" " int numActualConcaveConvexTests = 0;\n"