dali-physics/third-party/bullet3/src/Bullet3OpenCL/Raycast/kernels/rayCastKernels.cl

   1
   2 #define SHAPE_CONVEX_HULL 3
   3 #define SHAPE_PLANE 4
   4 #define SHAPE_CONCAVE_TRIMESH 5
   5 #define SHAPE_COMPOUND_OF_CONVEX_HULLS 6
   6 #define SHAPE_SPHERE 7
   7
   8
   9 typedef struct
  10 {
  11         float4 m_from;
  12         float4 m_to;
  13 } b3RayInfo;
  14
  15 typedef struct
  16 {
  17         float m_hitFraction;
  18         int     m_hitResult0;
  19         int     m_hitResult1;
  20         int     m_hitResult2;
  21         float4  m_hitPoint;
  22         float4  m_hitNormal;
  23 } b3RayHit;
  24
  25 typedef struct
  26 {
  27         float4 m_pos;
  28         float4 m_quat;
  29         float4 m_linVel;
  30         float4 m_angVel;
  31
  32         unsigned int m_collidableIdx;
  33         float m_invMass;
  34         float m_restituitionCoeff;
  35         float m_frictionCoeff;
  36 } Body;
  37
  38 typedef struct Collidable
  39 {
  40         union {
  41                 int m_numChildShapes;
  42                 int m_bvhIndex;
  43         };
  44         float m_radius;
  45         int m_shapeType;
  46         int m_shapeIndex;
  47 } Collidable;
  48
  49
  50 typedef struct
  51 {
  52         float4          m_localCenter;
  53         float4          m_extents;
  54         float4          mC;
  55         float4          mE;
  56
  57         float                   m_radius;
  58         int     m_faceOffset;
  59         int m_numFaces;
  60         int     m_numVertices;
  61
  62         int m_vertexOffset;
  63         int     m_uniqueEdgesOffset;
  64         int     m_numUniqueEdges;
  65         int m_unused;
  66
  67 } ConvexPolyhedronCL;
  68
  69 typedef struct
  70 {
  71         float4 m_plane;
  72         int m_indexOffset;
  73         int m_numIndices;
  74 } b3GpuFace;
  75
  76
  77
  78 ///////////////////////////////////////
  79 //      Quaternion
  80 ///////////////////////////////////////
  81
  82 typedef float4 Quaternion;
  83
  84 __inline
  85         Quaternion qtMul(Quaternion a, Quaternion b);
  86
  87 __inline
  88         Quaternion qtNormalize(Quaternion in);
  89
  90
  91 __inline
  92         Quaternion qtInvert(Quaternion q);
  93
  94
  95 __inline
  96         float dot3F4(float4 a, float4 b)
  97 {
  98         float4 a1 = (float4)(a.xyz,0.f);
  99         float4 b1 = (float4)(b.xyz,0.f);
 100         return dot(a1, b1);
 101 }
 102
 103
 104 __inline
 105         Quaternion qtMul(Quaternion a, Quaternion b)
 106 {
 107         Quaternion ans;
 108         ans = cross( a, b );
 109         ans += a.w*b+b.w*a;
 110         //      ans.w = a.w*b.w - (a.x*b.x+a.y*b.y+a.z*b.z);
 111         ans.w = a.w*b.w - dot3F4(a, b);
 112         return ans;
 113 }
 114
 115 __inline
 116         Quaternion qtNormalize(Quaternion in)
 117 {
 118         return fast_normalize(in);
 119         //      in /= length( in );
 120         //      return in;
 121 }
 122 __inline
 123         float4 qtRotate(Quaternion q, float4 vec)
 124 {
 125         Quaternion qInv = qtInvert( q );
 126         float4 vcpy = vec;
 127         vcpy.w = 0.f;
 128         float4 out = qtMul(q,vcpy);
 129         out = qtMul(out,qInv);
 130         return out;
 131 }
 132
 133 __inline
 134         Quaternion qtInvert(Quaternion q)
 135 {
 136         return (Quaternion)(-q.xyz, q.w);
 137 }
 138
 139 __inline
 140         float4 qtInvRotate(const Quaternion q, float4 vec)
 141 {
 142         return qtRotate( qtInvert( q ), vec );
 143 }
 144
 145
 146
 147 void    trInverse(float4 translationIn, Quaternion orientationIn,
 148         float4* translationOut, Quaternion* orientationOut)
 149 {
 150         *orientationOut = qtInvert(orientationIn);
 151         *translationOut = qtRotate(*orientationOut, -translationIn);
 152 }
 153
 154
 155
 156
 157
 158 bool rayConvex(float4 rayFromLocal, float4 rayToLocal, int numFaces, int faceOffset,
 159         __global const b3GpuFace* faces, float* hitFraction, float4* hitNormal)
 160 {
 161         rayFromLocal.w = 0.f;
 162         rayToLocal.w = 0.f;
 163         bool result = true;
 164
 165         float exitFraction = hitFraction[0];
 166         float enterFraction = -0.3f;
 167         float4 curHitNormal = (float4)(0,0,0,0);
 168         for (int i=0;i<numFaces && result;i++)
 169         {
 170                 b3GpuFace face = faces[faceOffset+i];
 171                 float fromPlaneDist = dot(rayFromLocal,face.m_plane)+face.m_plane.w;
 172                 float toPlaneDist = dot(rayToLocal,face.m_plane)+face.m_plane.w;
 173                 if (fromPlaneDist<0.f)
 174                 {
 175                         if (toPlaneDist >= 0.f)
 176                         {
 177                                 float fraction = fromPlaneDist / (fromPlaneDist-toPlaneDist);
 178                                 if (exitFraction>fraction)
 179                                 {
 180                                         exitFraction = fraction;
 181                                 }
 182                         }
 183                 } else
 184                 {
 185                         if (toPlaneDist<0.f)
 186                         {
 187                                 float fraction = fromPlaneDist / (fromPlaneDist-toPlaneDist);
 188                                 if (enterFraction <= fraction)
 189                                 {
 190                                         enterFraction = fraction;
 191                                         curHitNormal = face.m_plane;
 192                                         curHitNormal.w = 0.f;
 193                                 }
 194                         } else
 195                         {
 196                                 result = false;
 197                         }
 198                 }
 199                 if (exitFraction <= enterFraction)
 200                         result = false;
 201         }
 202
 203         if (enterFraction < 0.f)
 204         {
 205                 result = false;
 206         }
 207
 208         if (result)
 209         {
 210                 hitFraction[0] = enterFraction;
 211                 hitNormal[0] = curHitNormal;
 212         }
 213         return result;
 214 }
 215
 216
 217
 218
 219
 220
 221 bool sphere_intersect(float4 spherePos,  float radius, float4 rayFrom, float4 rayTo, float* hitFraction)
 222 {
 223         float4 rs = rayFrom - spherePos;
 224         rs.w = 0.f;
 225         float4 rayDir = rayTo-rayFrom;
 226         rayDir.w = 0.f;
 227         float A = dot(rayDir,rayDir);
 228         float B = dot(rs, rayDir);
 229         float C = dot(rs, rs) - (radius * radius);
 230
 231         float D = B * B - A*C;
 232
 233         if (D > 0.0f)
 234         {
 235                 float t = (-B - sqrt(D))/A;
 236
 237                 if ( (t >= 0.0f) && (t < (*hitFraction)) )
 238                 {
 239                         *hitFraction = t;
 240                         return true;
 241                 }
 242         }
 243         return false;
 244 }
 245
 246 float4 setInterpolate3(float4 from, float4 to, float t)
 247 {
 248         float s = 1.0f - t;
 249         float4 result;
 250         result = s * from + t * to;
 251         result.w = 0.f;
 252         return result;
 253 }
 254
 255 __kernel void rayCastKernel(
 256         int numRays,
 257         const __global b3RayInfo* rays,
 258         __global b3RayHit* hitResults,
 259         const int numBodies,
 260         __global Body* bodies,
 261         __global Collidable* collidables,
 262         __global const b3GpuFace* faces,
 263         __global const ConvexPolyhedronCL* convexShapes )
 264 {
 265
 266         int i = get_global_id(0);
 267         if (i>=numRays)
 268                 return;
 269
 270         hitResults[i].m_hitFraction = 1.f;
 271
 272         float4 rayFrom = rays[i].m_from;
 273         float4 rayTo = rays[i].m_to;
 274         float hitFraction = 1.f;
 275         float4 hitPoint;
 276         float4 hitNormal;
 277         int hitBodyIndex= -1;
 278
 279         int cachedCollidableIndex = -1;
 280         Collidable cachedCollidable;
 281
 282         for (int b=0;b<numBodies;b++)
 283         {
 284                 if (hitResults[i].m_hitResult2==b)
 285                         continue;
 286                 Body body = bodies[b];
 287                 float4 pos = body.m_pos;
 288                 float4 orn = body.m_quat;
 289                 if (cachedCollidableIndex != body.m_collidableIdx)
 290                 {
 291                         cachedCollidableIndex = body.m_collidableIdx;
 292                         cachedCollidable = collidables[cachedCollidableIndex];
 293                 }
 294                 if (cachedCollidable.m_shapeType == SHAPE_CONVEX_HULL)
 295                 {
 296
 297                         float4 invPos = (float4)(0,0,0,0);
 298                         float4 invOrn = (float4)(0,0,0,0);
 299                         float4 rayFromLocal = (float4)(0,0,0,0);
 300                         float4 rayToLocal = (float4)(0,0,0,0);
 301                         invOrn = qtInvert(orn);
 302                         invPos = qtRotate(invOrn, -pos);
 303                         rayFromLocal = qtRotate( invOrn, rayFrom ) + invPos;
 304                         rayToLocal = qtRotate( invOrn, rayTo) + invPos;
 305                         rayFromLocal.w = 0.f;
 306                         rayToLocal.w = 0.f;
 307                         int numFaces = convexShapes[cachedCollidable.m_shapeIndex].m_numFaces;
 308                         int faceOffset = convexShapes[cachedCollidable.m_shapeIndex].m_faceOffset;
 309                         if (numFaces)
 310                         {
 311                                 if (rayConvex(rayFromLocal, rayToLocal, numFaces, faceOffset,faces, &hitFraction, &hitNormal))
 312                                 {
 313                                         hitBodyIndex = b;
 314
 315                                 }
 316                         }
 317                 }
 318                 if (cachedCollidable.m_shapeType == SHAPE_SPHERE)
 319                 {
 320                         float radius = cachedCollidable.m_radius;
 321
 322                         if (sphere_intersect(pos,  radius, rayFrom, rayTo, &hitFraction))
 323                         {
 324                                 hitBodyIndex = b;
 325                                 hitNormal = (float4) (hitPoint-bodies[b].m_pos);
 326                         }
 327                 }
 328         }
 329
 330         if (hitBodyIndex>=0)
 331         {
 332                 hitPoint = setInterpolate3(rayFrom, rayTo,hitFraction);
 333                 hitResults[i].m_hitFraction = hitFraction;
 334                 hitResults[i].m_hitPoint = hitPoint;
 335                 hitResults[i].m_hitNormal = normalize(hitNormal);
 336                 hitResults[i].m_hitResult0 = hitBodyIndex;
 337         }
 338
 339 }
 340
 341
 342 __kernel void findRayRigidPairIndexRanges(__global int2* rayRigidPairs,
 343                                                                                         __global int* out_firstRayRigidPairIndexPerRay,
 344                                                                                         __global int* out_numRayRigidPairsPerRay,
 345                                                                                         int numRayRigidPairs)
 346 {
 347         int rayRigidPairIndex = get_global_id(0);
 348         if (rayRigidPairIndex >= numRayRigidPairs) return;
 349
 350         int rayIndex = rayRigidPairs[rayRigidPairIndex].x;
 351
 352         atomic_min(&out_firstRayRigidPairIndexPerRay[rayIndex], rayRigidPairIndex);
 353         atomic_inc(&out_numRayRigidPairsPerRay[rayIndex]);
 354 }
 355
 356 __kernel void rayCastPairsKernel(const __global b3RayInfo* rays,
 357                                                                 __global b3RayHit* hitResults,
 358                                                                 __global int* firstRayRigidPairIndexPerRay,
 359                                                                 __global int* numRayRigidPairsPerRay,
 360
 361                                                                 __global Body* bodies,
 362                                                                 __global Collidable* collidables,
 363                                                                 __global const b3GpuFace* faces,
 364                                                                 __global const ConvexPolyhedronCL* convexShapes,
 365
 366                                                                 __global int2* rayRigidPairs,
 367                                                                 int numRays)
 368 {
 369         int i = get_global_id(0);
 370         if (i >= numRays) return;
 371
 372         float4 rayFrom = rays[i].m_from;
 373         float4 rayTo = rays[i].m_to;
 374
 375         hitResults[i].m_hitFraction = 1.f;
 376
 377         float hitFraction = 1.f;
 378         float4 hitPoint;
 379         float4 hitNormal;
 380         int hitBodyIndex = -1;
 381
 382         //
 383         for(int pair = 0; pair < numRayRigidPairsPerRay[i]; ++pair)
 384         {
 385                 int rayRigidPairIndex = pair + firstRayRigidPairIndexPerRay[i];
 386                 int b = rayRigidPairs[rayRigidPairIndex].y;
 387
 388                 if (hitResults[i].m_hitResult2 == b) continue;
 389
 390                 Body body = bodies[b];
 391                 Collidable rigidCollidable = collidables[body.m_collidableIdx];
 392
 393                 float4 pos = body.m_pos;
 394                 float4 orn = body.m_quat;
 395
 396                 if (rigidCollidable.m_shapeType == SHAPE_CONVEX_HULL)
 397                 {
 398                         float4 invPos = (float4)(0,0,0,0);
 399                         float4 invOrn = (float4)(0,0,0,0);
 400                         float4 rayFromLocal = (float4)(0,0,0,0);
 401                         float4 rayToLocal = (float4)(0,0,0,0);
 402                         invOrn = qtInvert(orn);
 403                         invPos = qtRotate(invOrn, -pos);
 404                         rayFromLocal = qtRotate( invOrn, rayFrom ) + invPos;
 405                         rayToLocal = qtRotate( invOrn, rayTo) + invPos;
 406                         rayFromLocal.w = 0.f;
 407                         rayToLocal.w = 0.f;
 408                         int numFaces = convexShapes[rigidCollidable.m_shapeIndex].m_numFaces;
 409                         int faceOffset = convexShapes[rigidCollidable.m_shapeIndex].m_faceOffset;
 410
 411                         if (numFaces && rayConvex(rayFromLocal, rayToLocal, numFaces, faceOffset,faces, &hitFraction, &hitNormal))
 412                         {
 413                                 hitBodyIndex = b;
 414                                 hitPoint = setInterpolate3(rayFrom, rayTo, hitFraction);
 415                         }
 416                 }
 417
 418                 if (rigidCollidable.m_shapeType == SHAPE_SPHERE)
 419                 {
 420                         float radius = rigidCollidable.m_radius;
 421
 422                         if (sphere_intersect(pos, radius, rayFrom, rayTo, &hitFraction))
 423                         {
 424                                 hitBodyIndex = b;
 425                                 hitPoint = setInterpolate3(rayFrom, rayTo, hitFraction);
 426                                 hitNormal = (float4) (hitPoint - bodies[b].m_pos);
 427                         }
 428                 }
 429         }
 430
 431         if (hitBodyIndex >= 0)
 432         {
 433                 hitResults[i].m_hitFraction = hitFraction;
 434                 hitResults[i].m_hitPoint = hitPoint;
 435                 hitResults[i].m_hitNormal = normalize(hitNormal);
 436                 hitResults[i].m_hitResult0 = hitBodyIndex;
 437         }
 438
 439 }