#define SHAPE_CONVEX_HULL 3 #define SHAPE_PLANE 4 #define SHAPE_CONCAVE_TRIMESH 5 #define SHAPE_COMPOUND_OF_CONVEX_HULLS 6 #define SHAPE_SPHERE 7 typedef struct { float4 m_from; float4 m_to; } b3RayInfo; typedef struct { float m_hitFraction; int m_hitResult0; int m_hitResult1; int m_hitResult2; float4 m_hitPoint; float4 m_hitNormal; } b3RayHit; typedef struct { float4 m_pos; float4 m_quat; float4 m_linVel; float4 m_angVel; unsigned int m_collidableIdx; float m_invMass; float m_restituitionCoeff; float m_frictionCoeff; } Body; typedef struct Collidable { union { int m_numChildShapes; int m_bvhIndex; }; float m_radius; int m_shapeType; int m_shapeIndex; } 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 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(q,vcpy); out = qtMul(out,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 ); } void trInverse(float4 translationIn, Quaternion orientationIn, float4* translationOut, Quaternion* orientationOut) { *orientationOut = qtInvert(orientationIn); *translationOut = qtRotate(*orientationOut, -translationIn); } 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; bool result = true; float exitFraction = hitFraction[0]; float enterFraction = -0.3f; float4 curHitNormal = (float4)(0,0,0,0); for (int i=0;i<numFaces && result;i++) { b3GpuFace face = faces[faceOffset+i]; float fromPlaneDist = dot(rayFromLocal,face.m_plane)+face.m_plane.w; float toPlaneDist = dot(rayToLocal,face.m_plane)+face.m_plane.w; if (fromPlaneDist<0.f) { if (toPlaneDist >= 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 { result = false; } } if (exitFraction <= enterFraction) result = false; } if (enterFraction < 0.f) { result = false; } if (result) { hitFraction[0] = enterFraction; hitNormal[0] = curHitNormal; } return result; } bool sphere_intersect(float4 spherePos, float radius, float4 rayFrom, float4 rayTo, float* hitFraction) { float4 rs = rayFrom - spherePos; rs.w = 0.f; float4 rayDir = rayTo-rayFrom; rayDir.w = 0.f; float A = dot(rayDir,rayDir); float B = dot(rs, rayDir); float C = dot(rs, rs) - (radius * radius); float D = B * B - A*C; if (D > 0.0f) { float t = (-B - sqrt(D))/A; if ( (t >= 0.0f) && (t < (*hitFraction)) ) { *hitFraction = t; return true; } } return false; } float4 setInterpolate3(float4 from, float4 to, float t) { float s = 1.0f - t; float4 result; result = s * from + t * to; result.w = 0.f; return result; } __kernel void rayCastKernel( int numRays, const __global b3RayInfo* rays, __global b3RayHit* hitResults, const int numBodies, __global Body* bodies, __global Collidable* collidables, __global const b3GpuFace* faces, __global const ConvexPolyhedronCL* convexShapes ) { int i = get_global_id(0); if (i>=numRays) return; hitResults[i].m_hitFraction = 1.f; float4 rayFrom = rays[i].m_from; float4 rayTo = rays[i].m_to; float hitFraction = 1.f; float4 hitPoint; float4 hitNormal; int hitBodyIndex= -1; int cachedCollidableIndex = -1; Collidable cachedCollidable; for (int b=0;b<numBodies;b++) { if (hitResults[i].m_hitResult2==b) continue; Body body = bodies[b]; float4 pos = body.m_pos; float4 orn = body.m_quat; if (cachedCollidableIndex != body.m_collidableIdx) { cachedCollidableIndex = body.m_collidableIdx; cachedCollidable = collidables[cachedCollidableIndex]; } if (cachedCollidable.m_shapeType == SHAPE_CONVEX_HULL) { float4 invPos = (float4)(0,0,0,0); float4 invOrn = (float4)(0,0,0,0); float4 rayFromLocal = (float4)(0,0,0,0); float4 rayToLocal = (float4)(0,0,0,0); invOrn = qtInvert(orn); invPos = qtRotate(invOrn, -pos); rayFromLocal = qtRotate( invOrn, rayFrom ) + invPos; rayToLocal = qtRotate( invOrn, rayTo) + invPos; rayFromLocal.w = 0.f; rayToLocal.w = 0.f; int numFaces = convexShapes[cachedCollidable.m_shapeIndex].m_numFaces; int faceOffset = convexShapes[cachedCollidable.m_shapeIndex].m_faceOffset; if (numFaces) { if (rayConvex(rayFromLocal, rayToLocal, numFaces, faceOffset,faces, &hitFraction, &hitNormal)) { hitBodyIndex = b; } } } if (cachedCollidable.m_shapeType == SHAPE_SPHERE) { float radius = cachedCollidable.m_radius; if (sphere_intersect(pos, radius, rayFrom, rayTo, &hitFraction)) { hitBodyIndex = b; hitNormal = (float4) (hitPoint-bodies[b].m_pos); } } } if (hitBodyIndex>=0) { hitPoint = setInterpolate3(rayFrom, rayTo,hitFraction); hitResults[i].m_hitFraction = hitFraction; hitResults[i].m_hitPoint = hitPoint; hitResults[i].m_hitNormal = normalize(hitNormal); hitResults[i].m_hitResult0 = hitBodyIndex; } } __kernel void findRayRigidPairIndexRanges(__global int2* rayRigidPairs, __global int* out_firstRayRigidPairIndexPerRay, __global int* out_numRayRigidPairsPerRay, int numRayRigidPairs) { int rayRigidPairIndex = get_global_id(0); if (rayRigidPairIndex >= numRayRigidPairs) return; int rayIndex = rayRigidPairs[rayRigidPairIndex].x; atomic_min(&out_firstRayRigidPairIndexPerRay[rayIndex], rayRigidPairIndex); atomic_inc(&out_numRayRigidPairsPerRay[rayIndex]); } __kernel void rayCastPairsKernel(const __global b3RayInfo* rays, __global b3RayHit* hitResults, __global int* firstRayRigidPairIndexPerRay, __global int* numRayRigidPairsPerRay, __global Body* bodies, __global Collidable* collidables, __global const b3GpuFace* faces, __global const ConvexPolyhedronCL* convexShapes, __global int2* rayRigidPairs, int numRays) { int i = get_global_id(0); if (i >= numRays) return; float4 rayFrom = rays[i].m_from; float4 rayTo = rays[i].m_to; hitResults[i].m_hitFraction = 1.f; float hitFraction = 1.f; float4 hitPoint; float4 hitNormal; int hitBodyIndex = -1; // for(int pair = 0; pair < numRayRigidPairsPerRay[i]; ++pair) { int rayRigidPairIndex = pair + firstRayRigidPairIndexPerRay[i]; int b = rayRigidPairs[rayRigidPairIndex].y; if (hitResults[i].m_hitResult2 == b) continue; Body body = bodies[b]; Collidable rigidCollidable = collidables[body.m_collidableIdx]; float4 pos = body.m_pos; float4 orn = body.m_quat; if (rigidCollidable.m_shapeType == SHAPE_CONVEX_HULL) { float4 invPos = (float4)(0,0,0,0); float4 invOrn = (float4)(0,0,0,0); float4 rayFromLocal = (float4)(0,0,0,0); float4 rayToLocal = (float4)(0,0,0,0); invOrn = qtInvert(orn); invPos = qtRotate(invOrn, -pos); rayFromLocal = qtRotate( invOrn, rayFrom ) + invPos; rayToLocal = qtRotate( invOrn, rayTo) + invPos; rayFromLocal.w = 0.f; rayToLocal.w = 0.f; int numFaces = convexShapes[rigidCollidable.m_shapeIndex].m_numFaces; int faceOffset = convexShapes[rigidCollidable.m_shapeIndex].m_faceOffset; if (numFaces && rayConvex(rayFromLocal, rayToLocal, numFaces, faceOffset,faces, &hitFraction, &hitNormal)) { hitBodyIndex = b; hitPoint = setInterpolate3(rayFrom, rayTo, hitFraction); } } if (rigidCollidable.m_shapeType == SHAPE_SPHERE) { float radius = rigidCollidable.m_radius; if (sphere_intersect(pos, radius, rayFrom, rayTo, &hitFraction)) { hitBodyIndex = b; hitPoint = setInterpolate3(rayFrom, rayTo, hitFraction); hitNormal = (float4) (hitPoint - bodies[b].m_pos); } } } if (hitBodyIndex >= 0) { hitResults[i].m_hitFraction = hitFraction; hitResults[i].m_hitPoint = hitPoint; hitResults[i].m_hitNormal = normalize(hitNormal); hitResults[i].m_hitResult0 = hitBodyIndex; } }