From 330bf3ea094700b8d5fa3740767b357f81ad9a04 Mon Sep 17 00:00:00 2001 From: erwin coumans Date: Thu, 20 Jun 2013 11:33:00 -0700 Subject: [PATCH] start implementing ray-convex on GPU (work-in-progress) --- src/Bullet3OpenCL/Raycast/b3GpuRaycast.cpp | 22 +- .../Raycast/kernels/rayCastKernels.cl | 211 +++++++++++++++++- .../Raycast/kernels/rayCastKernels.h | 211 +++++++++++++++++- .../RigidBody/b3GpuRigidBodyPipeline.cpp | 2 +- 4 files changed, 421 insertions(+), 25 deletions(-) diff --git a/src/Bullet3OpenCL/Raycast/b3GpuRaycast.cpp b/src/Bullet3OpenCL/Raycast/b3GpuRaycast.cpp index 92dc65ba6..9dff7fc89 100644 --- a/src/Bullet3OpenCL/Raycast/b3GpuRaycast.cpp +++ b/src/Bullet3OpenCL/Raycast/b3GpuRaycast.cpp @@ -76,14 +76,14 @@ bool sphere_intersect(const b3Vector3& spherePos, b3Scalar radius, const b3Vect } bool rayConvex(const b3Vector3& rayFromLocal, const b3Vector3& rayToLocal, const b3ConvexPolyhedronCL& poly, - const struct b3GpuNarrowPhaseInternalData* narrowphaseData, float& hitFraction, b3Vector3& hitNormal) + const b3AlignedObjectArray& faces, float& hitFraction, b3Vector3& hitNormal) { float exitFraction = hitFraction; float enterFraction = -0.1f; b3Vector3 curHitNormal(0,0,0); for (int i=0;im_convexFaces[poly.m_faceOffset+i]; + const b3GpuFace& face = faces[poly.m_faceOffset+i]; float fromPlaneDist = b3Dot(rayFromLocal,face.m_plane)+face.m_plane.w; float toPlaneDist = b3Dot(rayToLocal,face.m_plane)+face.m_plane.w; if (fromPlaneDist<0.f) @@ -174,7 +174,7 @@ void b3GpuRaycast::castRaysHost(const b3AlignedObjectArray& rays, b3A int shapeIndex = collidables[bodies[b].m_collidableIdx].m_shapeIndex; const b3ConvexPolyhedronCL& poly = narrowphaseData->m_convexPolyhedra[shapeIndex]; - if (rayConvex(rayFromLocal, rayToLocal,poly,narrowphaseData, hitFraction, hitNormal)) + if (rayConvex(rayFromLocal, rayToLocal,poly,narrowphaseData->m_convexFaces, hitFraction, hitNormal)) { hitBodyIndex = b; } @@ -218,14 +218,6 @@ void b3GpuRaycast::castRays(const b3AlignedObjectArray& rays, b3Align gpuHitResults.resize(hitResults.size()); gpuHitResults.copyFromHost(hitResults); - b3OpenCLArray gpuBodies(m_data->m_context,m_data->m_q); - gpuBodies.resize(numBodies); - gpuBodies.copyFromHostPointer(bodies,numBodies); - - b3OpenCLArray gpuCollidables(m_data->m_context,m_data->m_q); - gpuCollidables.resize(numCollidables); - gpuCollidables.copyFromHostPointer(collidables,numCollidables); - //run kernel { @@ -239,9 +231,11 @@ void b3GpuRaycast::castRays(const b3AlignedObjectArray& rays, b3Align launcher.setBuffer(gpuHitResults.getBufferCL()); launcher.setConst(numBodies); - launcher.setBuffer(gpuBodies.getBufferCL()); - launcher.setBuffer(gpuCollidables.getBufferCL()); - + launcher.setBuffer(narrowphaseData->m_bodyBufferGPU->getBufferCL()); + launcher.setBuffer(narrowphaseData->m_collidablesGPU->getBufferCL()); + launcher.setBuffer(narrowphaseData->m_convexFacesGPU->getBufferCL()); + launcher.setBuffer(narrowphaseData->m_convexPolyhedraGPU->getBufferCL()); + launcher.launch1D(numRays); clFinish(m_data->m_q); } diff --git a/src/Bullet3OpenCL/Raycast/kernels/rayCastKernels.cl b/src/Bullet3OpenCL/Raycast/kernels/rayCastKernels.cl index 9a627c141..4cf493d1f 100644 --- a/src/Bullet3OpenCL/Raycast/kernels/rayCastKernels.cl +++ b/src/Bullet3OpenCL/Raycast/kernels/rayCastKernels.cl @@ -47,6 +47,181 @@ typedef struct Collidable } Collidable; +typedef struct +{ + float4 m_localCenter; + float4 m_extents; + float4 mC; + float4 mE; + + float m_radius; + int m_faceOffset; + int m_numFaces; + int m_numVertices; + + int m_vertexOffset; + int m_uniqueEdgesOffset; + int m_numUniqueEdges; + int m_unused; + +} ConvexPolyhedronCL; + +typedef struct +{ + float4 m_plane; + int m_indexOffset; + int m_numIndices; +} b3GpuFace; + + + +/////////////////////////////////////// +// Quaternion +/////////////////////////////////////// + +typedef float4 Quaternion; + +__inline +Quaternion qtMul(Quaternion a, Quaternion b); + +__inline +Quaternion qtNormalize(Quaternion in); + +__inline +float4 qtRotate(Quaternion q, float4 vec); + +__inline +Quaternion qtInvert(Quaternion q); + + +__inline +float dot3F4(float4 a, float4 b) +{ + float4 a1 = (float4)(a.xyz,0.f); + float4 b1 = (float4)(b.xyz,0.f); + return dot(a1, b1); +} + + +__inline +Quaternion qtMul(Quaternion a, Quaternion b) +{ + Quaternion ans; + ans = cross( a, b ); + ans += a.w*b+b.w*a; +// ans.w = a.w*b.w - (a.x*b.x+a.y*b.y+a.z*b.z); + ans.w = a.w*b.w - dot3F4(a, b); + return ans; +} + +__inline +Quaternion qtNormalize(Quaternion in) +{ + return fast_normalize(in); +// in /= length( in ); +// return in; +} +__inline +float4 qtRotate(Quaternion q, float4 vec) +{ + Quaternion qInv = qtInvert( q ); + float4 vcpy = vec; + vcpy.w = 0.f; + float4 out = qtMul(qtMul(q,vcpy),qInv); + return out; +} + +__inline +Quaternion qtInvert(Quaternion q) +{ + return (Quaternion)(-q.xyz, q.w); +} + +__inline +float4 qtInvRotate(const Quaternion q, float4 vec) +{ + return qtRotate( qtInvert( q ), vec ); +} + +__inline +float4 transform(const float4* p, const float4* translation, const Quaternion* orientation) +{ + return qtRotate( *orientation, *p ) + (*translation); +} + +void trInverse(float4 translationIn, Quaternion orientationIn, + float4* translationOut, Quaternion* orientationOut) +{ + *orientationOut = qtInvert(orientationIn); + *translationOut = qtRotate(*orientationOut, -translationIn); +} + +void trMul(float4 translationA, Quaternion orientationA, + float4 translationB, Quaternion orientationB, + float4* translationOut, Quaternion* orientationOut) +{ + *orientationOut = qtMul(orientationA,orientationB); + *translationOut = transform(&translationB,&translationA,&orientationA); +} + + + +bool rayConvex(float4 rayFromLocal, float4 rayToLocal, int numFaces, int faceOffset, + __global const b3GpuFace* faces, float* hitFraction, float4* hitNormal) +{ + rayFromLocal.w = 0.f; + rayToLocal.w = 0.f; + + float exitFraction = *hitFraction; + float enterFraction = -0.1f; + float4 curHitNormal = (float4)(0,0,0,0); + for (int i=0;i= 0.f) + { + float fraction = fromPlaneDist / (fromPlaneDist-toPlaneDist); + if (exitFraction>fraction) + { + exitFraction = fraction; + } + } + } else + { + if (toPlaneDist<0.f) + { + float fraction = fromPlaneDist / (fromPlaneDist-toPlaneDist); + if (enterFraction <= fraction) + { + enterFraction = fraction; + curHitNormal = face.m_plane; + curHitNormal.w = 0.f; + } + } else + { + return false; + } + } + if (exitFraction <= enterFraction) + return false; + } + + if (enterFraction < 0.f) + return false; + + *hitFraction = enterFraction; + *hitNormal = curHitNormal; + return true; +} + + + + + bool sphere_intersect(float4 spherePos, float radius, float4 rayFrom, float4 rayTo, float* hitFraction) { @@ -88,10 +263,11 @@ __kernel void rayCastKernel( __global b3RayHit* hitResults, const int numBodies, __global Body* bodies, - __global Collidable* collidables) + __global Collidable* collidables, + __global const b3GpuFace* faces, + __global const ConvexPolyhedronCL* convexShapes ) { - int i = get_global_id(0); if (i=0) { hitResults[i].m_hitFraction = hitFraction; - hitResults[i].m_hitPoint = setInterpolate3(rayFrom, rayTo,hitFraction); - float4 hitNormal = (float4) (hitResults[i].m_hitPoint-bodies[hitBodyIndex].m_pos); + hitResults[i].m_hitPoint = hitPoint; hitResults[i].m_hitNormal = normalize(hitNormal); hitResults[i].m_hitResult0 = hitBodyIndex; } diff --git a/src/Bullet3OpenCL/Raycast/kernels/rayCastKernels.h b/src/Bullet3OpenCL/Raycast/kernels/rayCastKernels.h index 3d1268c19..6f47345ef 100644 --- a/src/Bullet3OpenCL/Raycast/kernels/rayCastKernels.h +++ b/src/Bullet3OpenCL/Raycast/kernels/rayCastKernels.h @@ -49,6 +49,181 @@ static const char* rayCastKernelCL= \ "} Collidable;\n" "\n" "\n" +"typedef struct \n" +"{\n" +" float4 m_localCenter;\n" +" float4 m_extents;\n" +" float4 mC;\n" +" float4 mE;\n" +" \n" +" float m_radius;\n" +" int m_faceOffset;\n" +" int m_numFaces;\n" +" int m_numVertices;\n" +" \n" +" int m_vertexOffset;\n" +" int m_uniqueEdgesOffset;\n" +" int m_numUniqueEdges;\n" +" int m_unused;\n" +"\n" +"} ConvexPolyhedronCL;\n" +"\n" +"typedef struct\n" +"{\n" +" float4 m_plane;\n" +" int m_indexOffset;\n" +" int m_numIndices;\n" +"} b3GpuFace;\n" +"\n" +"\n" +"\n" +"///////////////////////////////////////\n" +"// Quaternion\n" +"///////////////////////////////////////\n" +"\n" +"typedef float4 Quaternion;\n" +"\n" +"__inline\n" +"Quaternion qtMul(Quaternion a, Quaternion b);\n" +"\n" +"__inline\n" +"Quaternion qtNormalize(Quaternion in);\n" +"\n" +"__inline\n" +"float4 qtRotate(Quaternion q, float4 vec);\n" +"\n" +"__inline\n" +"Quaternion qtInvert(Quaternion q);\n" +"\n" +"\n" +"__inline\n" +"float dot3F4(float4 a, float4 b)\n" +"{\n" +" float4 a1 = (float4)(a.xyz,0.f);\n" +" float4 b1 = (float4)(b.xyz,0.f);\n" +" return dot(a1, b1);\n" +"}\n" +"\n" +"\n" +"__inline\n" +"Quaternion qtMul(Quaternion a, Quaternion b)\n" +"{\n" +" Quaternion ans;\n" +" ans = cross( a, b );\n" +" ans += a.w*b+b.w*a;\n" +"// ans.w = a.w*b.w - (a.x*b.x+a.y*b.y+a.z*b.z);\n" +" ans.w = a.w*b.w - dot3F4(a, b);\n" +" return ans;\n" +"}\n" +"\n" +"__inline\n" +"Quaternion qtNormalize(Quaternion in)\n" +"{\n" +" return fast_normalize(in);\n" +"// in /= length( in );\n" +"// return in;\n" +"}\n" +"__inline\n" +"float4 qtRotate(Quaternion q, float4 vec)\n" +"{\n" +" Quaternion qInv = qtInvert( q );\n" +" float4 vcpy = vec;\n" +" vcpy.w = 0.f;\n" +" float4 out = qtMul(qtMul(q,vcpy),qInv);\n" +" return out;\n" +"}\n" +"\n" +"__inline\n" +"Quaternion qtInvert(Quaternion q)\n" +"{\n" +" return (Quaternion)(-q.xyz, q.w);\n" +"}\n" +"\n" +"__inline\n" +"float4 qtInvRotate(const Quaternion q, float4 vec)\n" +"{\n" +" return qtRotate( qtInvert( q ), vec );\n" +"}\n" +"\n" +"__inline\n" +"float4 transform(const float4* p, const float4* translation, const Quaternion* orientation)\n" +"{\n" +" return qtRotate( *orientation, *p ) + (*translation);\n" +"}\n" +"\n" +"void trInverse(float4 translationIn, Quaternion orientationIn,\n" +" float4* translationOut, Quaternion* orientationOut)\n" +"{\n" +" *orientationOut = qtInvert(orientationIn);\n" +" *translationOut = qtRotate(*orientationOut, -translationIn);\n" +"}\n" +"\n" +"void trMul(float4 translationA, Quaternion orientationA,\n" +" float4 translationB, Quaternion orientationB,\n" +" float4* translationOut, Quaternion* orientationOut)\n" +"{\n" +" *orientationOut = qtMul(orientationA,orientationB);\n" +" *translationOut = transform(&translationB,&translationA,&orientationA);\n" +"}\n" +"\n" +"\n" +"\n" +"bool rayConvex(float4 rayFromLocal, float4 rayToLocal, int numFaces, int faceOffset,\n" +" __global const b3GpuFace* faces, float* hitFraction, float4* hitNormal)\n" +"{\n" +" rayFromLocal.w = 0.f;\n" +" rayToLocal.w = 0.f;\n" +"\n" +" float exitFraction = *hitFraction;\n" +" float enterFraction = -0.1f;\n" +" float4 curHitNormal = (float4)(0,0,0,0);\n" +" for (int i=0;i= 0.f)\n" +" {\n" +" float fraction = fromPlaneDist / (fromPlaneDist-toPlaneDist);\n" +" if (exitFraction>fraction)\n" +" {\n" +" exitFraction = fraction;\n" +" }\n" +" } \n" +" } else\n" +" {\n" +" if (toPlaneDist<0.f)\n" +" {\n" +" float fraction = fromPlaneDist / (fromPlaneDist-toPlaneDist);\n" +" if (enterFraction <= fraction)\n" +" {\n" +" enterFraction = fraction;\n" +" curHitNormal = face.m_plane;\n" +" curHitNormal.w = 0.f;\n" +" }\n" +" } else\n" +" {\n" +" return false;\n" +" }\n" +" }\n" +" if (exitFraction <= enterFraction)\n" +" return false;\n" +" }\n" +"\n" +" if (enterFraction < 0.f)\n" +" return false;\n" +"\n" +" *hitFraction = enterFraction;\n" +" *hitNormal = curHitNormal;\n" +" return true;\n" +"}\n" +"\n" +"\n" +"\n" +"\n" +"\n" "\n" "bool sphere_intersect(float4 spherePos, float radius, float4 rayFrom, float4 rayTo, float* hitFraction)\n" "{\n" @@ -90,10 +265,11 @@ static const char* rayCastKernelCL= \ " __global b3RayHit* hitResults, \n" " const int numBodies, \n" " __global Body* bodies,\n" -" __global Collidable* collidables)\n" +" __global Collidable* collidables,\n" +" __global const b3GpuFace* faces,\n" +" __global const ConvexPolyhedronCL* convexShapes )\n" "{\n" "\n" -"\n" " int i = get_global_id(0);\n" " if (i=0)\n" " {\n" " hitResults[i].m_hitFraction = hitFraction;\n" -" hitResults[i].m_hitPoint = setInterpolate3(rayFrom, rayTo,hitFraction);\n" -" float4 hitNormal = (float4) (hitResults[i].m_hitPoint-bodies[hitBodyIndex].m_pos);\n" +" hitResults[i].m_hitPoint = hitPoint;\n" " hitResults[i].m_hitNormal = normalize(hitNormal);\n" " hitResults[i].m_hitResult0 = hitBodyIndex;\n" " }\n" diff --git a/src/Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipeline.cpp b/src/Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipeline.cpp index e7bc308b3..a77de1fb4 100644 --- a/src/Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipeline.cpp +++ b/src/Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipeline.cpp @@ -457,7 +457,7 @@ int b3GpuRigidBodyPipeline::registerPhysicsInstance(float mass, const float* po void b3GpuRigidBodyPipeline::castRays(const b3AlignedObjectArray& rays, b3AlignedObjectArray& hitResults) { - this->m_data->m_raycaster->castRaysHost(rays,hitResults, + this->m_data->m_raycaster->castRays(rays,hitResults, getNumBodies(),this->m_data->m_narrowphase->getBodiesCpu(), m_data->m_narrowphase->getNumCollidablesGpu(), m_data->m_narrowphase->getCollidablesCpu(), m_data->m_narrowphase->getInternalData() );