dali-physics/third-party/bullet3/src/Bullet3OpenCL/NarrowphaseCollision/kernels/primitiveContacts.cl

   1 #include "Bullet3Collision/NarrowPhaseCollision/shared/b3Contact4Data.h"
   2
   3 #define SHAPE_CONVEX_HULL 3
   4 #define SHAPE_PLANE 4
   5 #define SHAPE_CONCAVE_TRIMESH 5
   6 #define SHAPE_COMPOUND_OF_CONVEX_HULLS 6
   7 #define SHAPE_SPHERE 7
   8
   9
  10 #pragma OPENCL EXTENSION cl_amd_printf : enable
  11 #pragma OPENCL EXTENSION cl_khr_local_int32_base_atomics : enable
  12 #pragma OPENCL EXTENSION cl_khr_global_int32_base_atomics : enable
  13 #pragma OPENCL EXTENSION cl_khr_local_int32_extended_atomics : enable
  14 #pragma OPENCL EXTENSION cl_khr_global_int32_extended_atomics : enable
  15
  16 #ifdef cl_ext_atomic_counters_32
  17 #pragma OPENCL EXTENSION cl_ext_atomic_counters_32 : enable
  18 #else
  19 #define counter32_t volatile __global int*
  20 #endif
  21
  22 #define GET_GROUP_IDX get_group_id(0)
  23 #define GET_LOCAL_IDX get_local_id(0)
  24 #define GET_GLOBAL_IDX get_global_id(0)
  25 #define GET_GROUP_SIZE get_local_size(0)
  26 #define GET_NUM_GROUPS get_num_groups(0)
  27 #define GROUP_LDS_BARRIER barrier(CLK_LOCAL_MEM_FENCE)
  28 #define GROUP_MEM_FENCE mem_fence(CLK_LOCAL_MEM_FENCE)
  29 #define AtomInc(x) atom_inc(&(x))
  30 #define AtomInc1(x, out) out = atom_inc(&(x))
  31 #define AppendInc(x, out) out = atomic_inc(x)
  32 #define AtomAdd(x, value) atom_add(&(x), value)
  33 #define AtomCmpxhg(x, cmp, value) atom_cmpxchg( &(x), cmp, value )
  34 #define AtomXhg(x, value) atom_xchg ( &(x), value )
  35
  36 #define max2 max
  37 #define min2 min
  38
  39 typedef unsigned int u32;
  40
  41
  42
  43
  44 typedef struct
  45 {
  46         union
  47         {
  48                 float4  m_min;
  49                 float   m_minElems[4];
  50                 int                     m_minIndices[4];
  51         };
  52         union
  53         {
  54                 float4  m_max;
  55                 float   m_maxElems[4];
  56                 int                     m_maxIndices[4];
  57         };
  58 } btAabbCL;
  59
  60 ///keep this in sync with btCollidable.h
  61 typedef struct
  62 {
  63         int m_numChildShapes;
  64         float m_radius;
  65         int m_shapeType;
  66         int m_shapeIndex;
  67
  68 } btCollidableGpu;
  69
  70 typedef struct
  71 {
  72         float4  m_childPosition;
  73         float4  m_childOrientation;
  74         int m_shapeIndex;
  75         int m_unused0;
  76         int m_unused1;
  77         int m_unused2;
  78 } btGpuChildShape;
  79
  80 #define GET_NPOINTS(x) (x).m_worldNormalOnB.w
  81
  82 typedef struct
  83 {
  84         float4 m_pos;
  85         float4 m_quat;
  86         float4 m_linVel;
  87         float4 m_angVel;
  88
  89         u32 m_collidableIdx;
  90         float m_invMass;
  91         float m_restituitionCoeff;
  92         float m_frictionCoeff;
  93 } BodyData;
  94
  95
  96 typedef struct
  97 {
  98         float4          m_localCenter;
  99         float4          m_extents;
 100         float4          mC;
 101         float4          mE;
 102
 103         float                   m_radius;
 104         int     m_faceOffset;
 105         int m_numFaces;
 106         int     m_numVertices;
 107
 108         int m_vertexOffset;
 109         int     m_uniqueEdgesOffset;
 110         int     m_numUniqueEdges;
 111         int m_unused;
 112
 113 } ConvexPolyhedronCL;
 114
 115 typedef struct
 116 {
 117         float4 m_plane;
 118         int m_indexOffset;
 119         int m_numIndices;
 120 } btGpuFace;
 121
 122 #define SELECT_UINT4( b, a, condition ) select( b,a,condition )
 123
 124 #define make_float4 (float4)
 125 #define make_float2 (float2)
 126 #define make_uint4 (uint4)
 127 #define make_int4 (int4)
 128 #define make_uint2 (uint2)
 129 #define make_int2 (int2)
 130
 131
 132 __inline
 133 float fastDiv(float numerator, float denominator)
 134 {
 135         return native_divide(numerator, denominator);
 136 //      return numerator/denominator;
 137 }
 138
 139 __inline
 140 float4 fastDiv4(float4 numerator, float4 denominator)
 141 {
 142         return native_divide(numerator, denominator);
 143 }
 144
 145
 146 __inline
 147 float4 cross3(float4 a, float4 b)
 148 {
 149         return cross(a,b);
 150 }
 151
 152 //#define dot3F4 dot
 153
 154 __inline
 155 float dot3F4(float4 a, float4 b)
 156 {
 157         float4 a1 = make_float4(a.xyz,0.f);
 158         float4 b1 = make_float4(b.xyz,0.f);
 159         return dot(a1, b1);
 160 }
 161
 162 __inline
 163 float4 fastNormalize4(float4 v)
 164 {
 165         return fast_normalize(v);
 166 }
 167
 168
 169 ///////////////////////////////////////
 170 //      Quaternion
 171 ///////////////////////////////////////
 172
 173 typedef float4 Quaternion;
 174
 175 __inline
 176 Quaternion qtMul(Quaternion a, Quaternion b);
 177
 178 __inline
 179 Quaternion qtNormalize(Quaternion in);
 180
 181 __inline
 182 float4 qtRotate(Quaternion q, float4 vec);
 183
 184 __inline
 185 Quaternion qtInvert(Quaternion q);
 186
 187
 188
 189
 190 __inline
 191 Quaternion qtMul(Quaternion a, Quaternion b)
 192 {
 193         Quaternion ans;
 194         ans = cross3( a, b );
 195         ans += a.w*b+b.w*a;
 196 //      ans.w = a.w*b.w - (a.x*b.x+a.y*b.y+a.z*b.z);
 197         ans.w = a.w*b.w - dot3F4(a, b);
 198         return ans;
 199 }
 200
 201 __inline
 202 Quaternion qtNormalize(Quaternion in)
 203 {
 204         return fastNormalize4(in);
 205 //      in /= length( in );
 206 //      return in;
 207 }
 208 __inline
 209 float4 qtRotate(Quaternion q, float4 vec)
 210 {
 211         Quaternion qInv = qtInvert( q );
 212         float4 vcpy = vec;
 213         vcpy.w = 0.f;
 214         float4 out = qtMul(qtMul(q,vcpy),qInv);
 215         return out;
 216 }
 217
 218 __inline
 219 Quaternion qtInvert(Quaternion q)
 220 {
 221         return (Quaternion)(-q.xyz, q.w);
 222 }
 223
 224 __inline
 225 float4 qtInvRotate(const Quaternion q, float4 vec)
 226 {
 227         return qtRotate( qtInvert( q ), vec );
 228 }
 229
 230 __inline
 231 float4 transform(const float4* p, const float4* translation, const Quaternion* orientation)
 232 {
 233         return qtRotate( *orientation, *p ) + (*translation);
 234 }
 235
 236 void    trInverse(float4 translationIn, Quaternion orientationIn,
 237                 float4* translationOut, Quaternion* orientationOut)
 238 {
 239         *orientationOut = qtInvert(orientationIn);
 240         *translationOut = qtRotate(*orientationOut, -translationIn);
 241 }
 242
 243 void    trMul(float4 translationA, Quaternion orientationA,
 244                                                 float4 translationB, Quaternion orientationB,
 245                 float4* translationOut, Quaternion* orientationOut)
 246 {
 247         *orientationOut = qtMul(orientationA,orientationB);
 248         *translationOut = transform(&translationB,&translationA,&orientationA);
 249 }
 250
 251
 252
 253 __inline
 254 float4 normalize3(const float4 a)
 255 {
 256         float4 n = make_float4(a.x, a.y, a.z, 0.f);
 257         return fastNormalize4( n );
 258 }
 259
 260
 261 __inline float4 lerp3(const float4 a,const float4 b, float  t)
 262 {
 263         return make_float4(     a.x + (b.x - a.x) * t,
 264                                                 a.y + (b.y - a.y) * t,
 265                                                 a.z + (b.z - a.z) * t,
 266                                                 0.f);
 267 }
 268
 269
 270 float signedDistanceFromPointToPlane(float4 point, float4 planeEqn, float4* closestPointOnFace)
 271 {
 272         float4 n = (float4)(planeEqn.x, planeEqn.y, planeEqn.z, 0);
 273         float dist = dot3F4(n, point) + planeEqn.w;
 274         *closestPointOnFace = point - dist * n;
 275         return dist;
 276 }
 277
 278
 279
 280 inline bool IsPointInPolygon(float4 p,
 281                                                         const btGpuFace* face,
 282                                                         __global const float4* baseVertex,
 283                                                         __global const  int* convexIndices,
 284                                                         float4* out)
 285 {
 286     float4 a;
 287     float4 b;
 288     float4 ab;
 289     float4 ap;
 290     float4 v;
 291
 292         float4 plane = make_float4(face->m_plane.x,face->m_plane.y,face->m_plane.z,0.f);
 293
 294         if (face->m_numIndices<2)
 295                 return false;
 296
 297
 298         float4 v0 = baseVertex[convexIndices[face->m_indexOffset + face->m_numIndices-1]];
 299
 300         b = v0;
 301
 302     for(unsigned i=0; i != face->m_numIndices; ++i)
 303     {
 304                 a = b;
 305                 float4 vi = baseVertex[convexIndices[face->m_indexOffset + i]];
 306                 b = vi;
 307         ab = b-a;
 308         ap = p-a;
 309         v = cross3(ab,plane);
 310
 311         if (dot(ap, v) > 0.f)
 312         {
 313             float ab_m2 = dot(ab, ab);
 314             float rt = ab_m2 != 0.f ? dot(ab, ap) / ab_m2 : 0.f;
 315             if (rt <= 0.f)
 316             {
 317                 *out = a;
 318             }
 319             else if (rt >= 1.f)
 320             {
 321                 *out = b;
 322             }
 323             else
 324             {
 325                 float s = 1.f - rt;
 326                                 out[0].x = s * a.x + rt * b.x;
 327                                 out[0].y = s * a.y + rt * b.y;
 328                                 out[0].z = s * a.z + rt * b.z;
 329             }
 330             return false;
 331         }
 332     }
 333     return true;
 334 }
 335
 336
 337
 338
 339 void    computeContactSphereConvex(int pairIndex,
 340                                                                                                                                 int bodyIndexA, int bodyIndexB,
 341                                                                                                                                 int collidableIndexA, int collidableIndexB,
 342                                                                                                                                 __global const BodyData* rigidBodies,
 343                                                                                                                                 __global const btCollidableGpu* collidables,
 344                                                                                                                                 __global const ConvexPolyhedronCL* convexShapes,
 345                                                                                                                                 __global const float4* convexVertices,
 346                                                                                                                                 __global const int* convexIndices,
 347                                                                                                                                 __global const btGpuFace* faces,
 348                                                                                                                                 __global struct b3Contact4Data* restrict globalContactsOut,
 349                                                                                                                                 counter32_t nGlobalContactsOut,
 350                                                                                                                                 int maxContactCapacity,
 351                                                                                                                                 float4 spherePos2,
 352                                                                                                                                 float radius,
 353                                                                                                                                 float4 pos,
 354                                                                                                                                 float4 quat
 355                                                                                                                                 )
 356 {
 357
 358         float4 invPos;
 359         float4 invOrn;
 360
 361         trInverse(pos,quat, &invPos,&invOrn);
 362
 363         float4 spherePos = transform(&spherePos2,&invPos,&invOrn);
 364
 365         int shapeIndex = collidables[collidableIndexB].m_shapeIndex;
 366         int numFaces = convexShapes[shapeIndex].m_numFaces;
 367         float4 closestPnt = (float4)(0, 0, 0, 0);
 368         float4 hitNormalWorld = (float4)(0, 0, 0, 0);
 369         float minDist = -1000000.f;
 370         bool bCollide = true;
 371
 372         for ( int f = 0; f < numFaces; f++ )
 373         {
 374                 btGpuFace face = faces[convexShapes[shapeIndex].m_faceOffset+f];
 375
 376                 // set up a plane equation
 377                 float4 planeEqn;
 378                 float4 n1 = face.m_plane;
 379                 n1.w = 0.f;
 380                 planeEqn = n1;
 381                 planeEqn.w = face.m_plane.w;
 382
 383
 384                 // compute a signed distance from the vertex in cloth to the face of rigidbody.
 385                 float4 pntReturn;
 386                 float dist = signedDistanceFromPointToPlane(spherePos, planeEqn, &pntReturn);
 387
 388                 // If the distance is positive, the plane is a separating plane.
 389                 if ( dist > radius )
 390                 {
 391                         bCollide = false;
 392                         break;
 393                 }
 394
 395
 396                 if (dist>0)
 397                 {
 398                         //might hit an edge or vertex
 399                         float4 out;
 400                         float4 zeroPos = make_float4(0,0,0,0);
 401
 402                         bool isInPoly = IsPointInPolygon(spherePos,
 403                                         &face,
 404                                         &convexVertices[convexShapes[shapeIndex].m_vertexOffset],
 405                                         convexIndices,
 406            &out);
 407                         if (isInPoly)
 408                         {
 409                                 if (dist>minDist)
 410                                 {
 411                                         minDist = dist;
 412                                         closestPnt = pntReturn;
 413                                         hitNormalWorld = planeEqn;
 414
 415                                 }
 416                         } else
 417                         {
 418                                 float4 tmp = spherePos-out;
 419                                 float l2 = dot(tmp,tmp);
 420                                 if (l2<radius*radius)
 421                                 {
 422                                         dist  = sqrt(l2);
 423                                         if (dist>minDist)
 424                                         {
 425                                                 minDist = dist;
 426                                                 closestPnt = out;
 427                                                 hitNormalWorld = tmp/dist;
 428
 429                                         }
 430
 431                                 } else
 432                                 {
 433                                         bCollide = false;
 434                                         break;
 435                                 }
 436                         }
 437                 } else
 438                 {
 439                         if ( dist > minDist )
 440                         {
 441                                 minDist = dist;
 442                                 closestPnt = pntReturn;
 443                                 hitNormalWorld.xyz = planeEqn.xyz;
 444                         }
 445                 }
 446
 447         }
 448
 449
 450
 451         if (bCollide && minDist > -10000)
 452         {
 453                 float4 normalOnSurfaceB1 = qtRotate(quat,-hitNormalWorld);
 454                 float4 pOnB1 = transform(&closestPnt,&pos,&quat);
 455
 456                 float actualDepth = minDist-radius;
 457                 if (actualDepth<=0.f)
 458                 {
 459
 460
 461                         pOnB1.w = actualDepth;
 462
 463                         int dstIdx;
 464                         AppendInc( nGlobalContactsOut, dstIdx );
 465
 466
 467                         if (1)//dstIdx < maxContactCapacity)
 468                         {
 469                                 __global struct b3Contact4Data* c = &globalContactsOut[dstIdx];
 470                                 c->m_worldNormalOnB = -normalOnSurfaceB1;
 471                                 c->m_restituitionCoeffCmp = (0.f*0xffff);c->m_frictionCoeffCmp = (0.7f*0xffff);
 472                                 c->m_batchIdx = pairIndex;
 473                                 c->m_bodyAPtrAndSignBit = rigidBodies[bodyIndexA].m_invMass==0?-bodyIndexA:bodyIndexA;
 474                                 c->m_bodyBPtrAndSignBit = rigidBodies[bodyIndexB].m_invMass==0?-bodyIndexB:bodyIndexB;
 475                                 c->m_worldPosB[0] = pOnB1;
 476                                 c->m_childIndexA = -1;
 477                                 c->m_childIndexB = -1;
 478
 479                                 GET_NPOINTS(*c) = 1;
 480                         }
 481
 482                 }
 483         }//if (hasCollision)
 484
 485 }
 486
 487
 488
 489 int extractManifoldSequential(const float4* p, int nPoints, float4 nearNormal, int4* contactIdx)
 490 {
 491         if( nPoints == 0 )
 492         return 0;
 493
 494     if (nPoints <=4)
 495         return nPoints;
 496
 497
 498     if (nPoints >64)
 499         nPoints = 64;
 500
 501         float4 center = make_float4(0.f);
 502         {
 503
 504                 for (int i=0;i<nPoints;i++)
 505                         center += p[i];
 506                 center /= (float)nPoints;
 507         }
 508
 509
 510
 511         //      sample 4 directions
 512
 513     float4 aVector = p[0] - center;
 514     float4 u = cross3( nearNormal, aVector );
 515     float4 v = cross3( nearNormal, u );
 516     u = normalize3( u );
 517     v = normalize3( v );
 518
 519
 520     //keep point with deepest penetration
 521     float minW= FLT_MAX;
 522
 523     int minIndex=-1;
 524
 525     float4 maxDots;
 526     maxDots.x = FLT_MIN;
 527     maxDots.y = FLT_MIN;
 528     maxDots.z = FLT_MIN;
 529     maxDots.w = FLT_MIN;
 530
 531     //  idx, distance
 532     for(int ie = 0; ie<nPoints; ie++ )
 533     {
 534         if (p[ie].w<minW)
 535         {
 536             minW = p[ie].w;
 537             minIndex=ie;
 538         }
 539         float f;
 540         float4 r = p[ie]-center;
 541         f = dot3F4( u, r );
 542         if (f<maxDots.x)
 543         {
 544             maxDots.x = f;
 545             contactIdx[0].x = ie;
 546         }
 547
 548         f = dot3F4( -u, r );
 549         if (f<maxDots.y)
 550         {
 551             maxDots.y = f;
 552             contactIdx[0].y = ie;
 553         }
 554
 555
 556         f = dot3F4( v, r );
 557         if (f<maxDots.z)
 558         {
 559             maxDots.z = f;
 560             contactIdx[0].z = ie;
 561         }
 562
 563         f = dot3F4( -v, r );
 564         if (f<maxDots.w)
 565         {
 566             maxDots.w = f;
 567             contactIdx[0].w = ie;
 568         }
 569
 570     }
 571
 572     if (contactIdx[0].x != minIndex && contactIdx[0].y != minIndex && contactIdx[0].z != minIndex && contactIdx[0].w != minIndex)
 573     {
 574         //replace the first contact with minimum (todo: replace contact with least penetration)
 575         contactIdx[0].x = minIndex;
 576     }
 577
 578     return 4;
 579
 580 }
 581
 582 #define MAX_PLANE_CONVEX_POINTS 64
 583
 584 int computeContactPlaneConvex(int pairIndex,
 585                                                                 int bodyIndexA, int bodyIndexB,
 586                                                                 int collidableIndexA, int collidableIndexB,
 587                                                                 __global const BodyData* rigidBodies,
 588                                                                 __global const btCollidableGpu*collidables,
 589                                                                 __global const ConvexPolyhedronCL* convexShapes,
 590                                                                 __global const float4* convexVertices,
 591                                                                 __global const int* convexIndices,
 592                                                                 __global const btGpuFace* faces,
 593                                                                 __global struct b3Contact4Data* restrict globalContactsOut,
 594                                                                 counter32_t nGlobalContactsOut,
 595                                                                 int maxContactCapacity,
 596                                                                 float4 posB,
 597                                                                 Quaternion ornB
 598                                                                 )
 599 {
 600         int resultIndex=-1;
 601
 602                 int shapeIndex = collidables[collidableIndexB].m_shapeIndex;
 603         __global const ConvexPolyhedronCL* hullB = &convexShapes[shapeIndex];
 604
 605         float4 posA;
 606         posA = rigidBodies[bodyIndexA].m_pos;
 607         Quaternion ornA;
 608         ornA = rigidBodies[bodyIndexA].m_quat;
 609
 610         int numContactsOut = 0;
 611         int numWorldVertsB1= 0;
 612
 613         float4 planeEq;
 614          planeEq = faces[collidables[collidableIndexA].m_shapeIndex].m_plane;
 615         float4 planeNormal = make_float4(planeEq.x,planeEq.y,planeEq.z,0.f);
 616         float4 planeNormalWorld;
 617         planeNormalWorld = qtRotate(ornA,planeNormal);
 618         float planeConstant = planeEq.w;
 619
 620         float4 invPosA;Quaternion invOrnA;
 621         float4 convexInPlaneTransPos1; Quaternion convexInPlaneTransOrn1;
 622         {
 623
 624                 trInverse(posA,ornA,&invPosA,&invOrnA);
 625                 trMul(invPosA,invOrnA,posB,ornB,&convexInPlaneTransPos1,&convexInPlaneTransOrn1);
 626         }
 627         float4 invPosB;Quaternion invOrnB;
 628         float4 planeInConvexPos1;       Quaternion planeInConvexOrn1;
 629         {
 630
 631                 trInverse(posB,ornB,&invPosB,&invOrnB);
 632                 trMul(invPosB,invOrnB,posA,ornA,&planeInConvexPos1,&planeInConvexOrn1);
 633         }
 634
 635
 636         float4 planeNormalInConvex = qtRotate(planeInConvexOrn1,-planeNormal);
 637         float maxDot = -1e30;
 638         int hitVertex=-1;
 639         float4 hitVtx;
 640
 641
 642
 643         float4 contactPoints[MAX_PLANE_CONVEX_POINTS];
 644         int numPoints = 0;
 645
 646         int4 contactIdx;
 647         contactIdx=make_int4(0,1,2,3);
 648
 649
 650         for (int i=0;i<hullB->m_numVertices;i++)
 651         {
 652                 float4 vtx = convexVertices[hullB->m_vertexOffset+i];
 653                 float curDot = dot(vtx,planeNormalInConvex);
 654
 655
 656                 if (curDot>maxDot)
 657                 {
 658                         hitVertex=i;
 659                         maxDot=curDot;
 660                         hitVtx = vtx;
 661                         //make sure the deepest points is always included
 662                         if (numPoints==MAX_PLANE_CONVEX_POINTS)
 663                                 numPoints--;
 664                 }
 665
 666                 if (numPoints<MAX_PLANE_CONVEX_POINTS)
 667                 {
 668                         float4 vtxWorld = transform(&vtx, &posB, &ornB);
 669                         float4 vtxInPlane = transform(&vtxWorld, &invPosA, &invOrnA);//oplaneTransform.inverse()*vtxWorld;
 670                         float dist = dot(planeNormal,vtxInPlane)-planeConstant;
 671                         if (dist<0.f)
 672                         {
 673                                 vtxWorld.w = dist;
 674                                 contactPoints[numPoints] = vtxWorld;
 675                                 numPoints++;
 676                         }
 677                 }
 678
 679         }
 680
 681         int numReducedPoints  = numPoints;
 682         if (numPoints>4)
 683         {
 684                 numReducedPoints = extractManifoldSequential( contactPoints, numPoints, planeNormalInConvex, &contactIdx);
 685         }
 686
 687         if (numReducedPoints>0)
 688         {
 689                 int dstIdx;
 690             AppendInc( nGlobalContactsOut, dstIdx );
 691
 692                 if (dstIdx < maxContactCapacity)
 693                 {
 694                         resultIndex = dstIdx;
 695                         __global struct b3Contact4Data* c = &globalContactsOut[dstIdx];
 696                         c->m_worldNormalOnB = -planeNormalWorld;
 697                         //c->setFrictionCoeff(0.7);
 698                         //c->setRestituitionCoeff(0.f);
 699                         c->m_restituitionCoeffCmp = (0.f*0xffff);c->m_frictionCoeffCmp = (0.7f*0xffff);
 700                         c->m_batchIdx = pairIndex;
 701                         c->m_bodyAPtrAndSignBit = rigidBodies[bodyIndexA].m_invMass==0?-bodyIndexA:bodyIndexA;
 702                         c->m_bodyBPtrAndSignBit = rigidBodies[bodyIndexB].m_invMass==0?-bodyIndexB:bodyIndexB;
 703                         c->m_childIndexA = -1;
 704                         c->m_childIndexB = -1;
 705
 706                         switch (numReducedPoints)
 707             {
 708                 case 4:
 709                     c->m_worldPosB[3] = contactPoints[contactIdx.w];
 710                 case 3:
 711                     c->m_worldPosB[2] = contactPoints[contactIdx.z];
 712                 case 2:
 713                     c->m_worldPosB[1] = contactPoints[contactIdx.y];
 714                 case 1:
 715                     c->m_worldPosB[0] = contactPoints[contactIdx.x];
 716                 default:
 717                 {
 718                 }
 719             };
 720
 721                         GET_NPOINTS(*c) = numReducedPoints;
 722                 }//if (dstIdx < numPairs)
 723         }
 724
 725         return resultIndex;
 726 }
 727
 728
 729 void    computeContactPlaneSphere(int pairIndex,
 730                                                                                                                                 int bodyIndexA, int bodyIndexB,
 731                                                                                                                                 int collidableIndexA, int collidableIndexB,
 732                                                                                                                                 __global const BodyData* rigidBodies,
 733                                                                                                                                 __global const btCollidableGpu* collidables,
 734                                                                                                                                 __global const btGpuFace* faces,
 735                                                                                                                                 __global struct b3Contact4Data* restrict globalContactsOut,
 736                                                                                                                                 counter32_t nGlobalContactsOut,
 737                                                                                                                                 int maxContactCapacity)
 738 {
 739         float4 planeEq = faces[collidables[collidableIndexA].m_shapeIndex].m_plane;
 740         float radius = collidables[collidableIndexB].m_radius;
 741         float4 posA1 = rigidBodies[bodyIndexA].m_pos;
 742         float4 ornA1 = rigidBodies[bodyIndexA].m_quat;
 743         float4 posB1 = rigidBodies[bodyIndexB].m_pos;
 744         float4 ornB1 = rigidBodies[bodyIndexB].m_quat;
 745
 746         bool hasCollision = false;
 747         float4 planeNormal1 = make_float4(planeEq.x,planeEq.y,planeEq.z,0.f);
 748         float planeConstant = planeEq.w;
 749         float4 convexInPlaneTransPos1; Quaternion convexInPlaneTransOrn1;
 750         {
 751                 float4 invPosA;Quaternion invOrnA;
 752                 trInverse(posA1,ornA1,&invPosA,&invOrnA);
 753                 trMul(invPosA,invOrnA,posB1,ornB1,&convexInPlaneTransPos1,&convexInPlaneTransOrn1);
 754         }
 755         float4 planeInConvexPos1;       Quaternion planeInConvexOrn1;
 756         {
 757                 float4 invPosB;Quaternion invOrnB;
 758                 trInverse(posB1,ornB1,&invPosB,&invOrnB);
 759                 trMul(invPosB,invOrnB,posA1,ornA1,&planeInConvexPos1,&planeInConvexOrn1);
 760         }
 761         float4 vtx1 = qtRotate(planeInConvexOrn1,-planeNormal1)*radius;
 762         float4 vtxInPlane1 = transform(&vtx1,&convexInPlaneTransPos1,&convexInPlaneTransOrn1);
 763         float distance = dot3F4(planeNormal1,vtxInPlane1) - planeConstant;
 764         hasCollision = distance < 0.f;//m_manifoldPtr->getContactBreakingThreshold();
 765         if (hasCollision)
 766         {
 767                 float4 vtxInPlaneProjected1 = vtxInPlane1 -   distance*planeNormal1;
 768                 float4 vtxInPlaneWorld1 = transform(&vtxInPlaneProjected1,&posA1,&ornA1);
 769                 float4 normalOnSurfaceB1 = qtRotate(ornA1,planeNormal1);
 770                 float4 pOnB1 = vtxInPlaneWorld1+normalOnSurfaceB1*distance;
 771                 pOnB1.w = distance;
 772
 773                 int dstIdx;
 774     AppendInc( nGlobalContactsOut, dstIdx );
 775
 776                 if (dstIdx < maxContactCapacity)
 777                 {
 778                         __global struct b3Contact4Data* c = &globalContactsOut[dstIdx];
 779                         c->m_worldNormalOnB = -normalOnSurfaceB1;
 780                         c->m_restituitionCoeffCmp = (0.f*0xffff);c->m_frictionCoeffCmp = (0.7f*0xffff);
 781                         c->m_batchIdx = pairIndex;
 782                         c->m_bodyAPtrAndSignBit = rigidBodies[bodyIndexA].m_invMass==0?-bodyIndexA:bodyIndexA;
 783                         c->m_bodyBPtrAndSignBit = rigidBodies[bodyIndexB].m_invMass==0?-bodyIndexB:bodyIndexB;
 784                         c->m_worldPosB[0] = pOnB1;
 785                         c->m_childIndexA = -1;
 786                         c->m_childIndexB = -1;
 787                         GET_NPOINTS(*c) = 1;
 788                 }//if (dstIdx < numPairs)
 789         }//if (hasCollision)
 790 }
 791
 792
 793 __kernel void   primitiveContactsKernel( __global int4* pairs,
 794                                                                                                                                                                         __global const BodyData* rigidBodies,
 795                                                                                                                                                                         __global const btCollidableGpu* collidables,
 796                                                                                                                                                                         __global const ConvexPolyhedronCL* convexShapes,
 797                                                                                                                                                                         __global const float4* vertices,
 798                                                                                                                                                                         __global const float4* uniqueEdges,
 799                                                                                                                                                                         __global const btGpuFace* faces,
 800                                                                                                                                                                         __global const int* indices,
 801                                                                                                                                                                         __global struct b3Contact4Data* restrict globalContactsOut,
 802                                                                                                                                                                         counter32_t nGlobalContactsOut,
 803                                                                                                                                                                         int numPairs, int maxContactCapacity)
 804 {
 805
 806         int i = get_global_id(0);
 807         int pairIndex = i;
 808
 809         float4 worldVertsB1[64];
 810         float4 worldVertsB2[64];
 811         int capacityWorldVerts = 64;
 812
 813         float4 localContactsOut[64];
 814         int localContactCapacity=64;
 815
 816         float minDist = -1e30f;
 817         float maxDist = 0.02f;
 818
 819         if (i<numPairs)
 820         {
 821
 822                 int bodyIndexA = pairs[i].x;
 823                 int bodyIndexB = pairs[i].y;
 824
 825                 int collidableIndexA = rigidBodies[bodyIndexA].m_collidableIdx;
 826                 int collidableIndexB = rigidBodies[bodyIndexB].m_collidableIdx;
 827
 828                 if (collidables[collidableIndexA].m_shapeType == SHAPE_PLANE &&
 829                         collidables[collidableIndexB].m_shapeType == SHAPE_CONVEX_HULL)
 830                 {
 831
 832                         float4 posB;
 833                         posB = rigidBodies[bodyIndexB].m_pos;
 834                         Quaternion ornB;
 835                         ornB = rigidBodies[bodyIndexB].m_quat;
 836                         int contactIndex = computeContactPlaneConvex(pairIndex, bodyIndexA, bodyIndexB, collidableIndexA, collidableIndexB,
 837                                                                                                                                 rigidBodies,collidables,convexShapes,vertices,indices,
 838                                                                                                                                 faces,  globalContactsOut, nGlobalContactsOut,maxContactCapacity, posB,ornB);
 839                         if (contactIndex>=0)
 840                                 pairs[pairIndex].z = contactIndex;
 841
 842                         return;
 843                 }
 844
 845
 846                 if (collidables[collidableIndexA].m_shapeType == SHAPE_CONVEX_HULL &&
 847                         collidables[collidableIndexB].m_shapeType == SHAPE_PLANE)
 848                 {
 849
 850                         float4 posA;
 851                         posA = rigidBodies[bodyIndexA].m_pos;
 852                         Quaternion ornA;
 853                         ornA = rigidBodies[bodyIndexA].m_quat;
 854
 855
 856                         int contactIndex = computeContactPlaneConvex( pairIndex, bodyIndexB,bodyIndexA,  collidableIndexB,collidableIndexA,
 857                                                                                                                                 rigidBodies,collidables,convexShapes,vertices,indices,
 858                                                                                                                                 faces,  globalContactsOut, nGlobalContactsOut,maxContactCapacity,posA,ornA);
 859
 860                         if (contactIndex>=0)
 861                                 pairs[pairIndex].z = contactIndex;
 862
 863                         return;
 864                 }
 865
 866                 if (collidables[collidableIndexA].m_shapeType == SHAPE_PLANE &&
 867                         collidables[collidableIndexB].m_shapeType == SHAPE_SPHERE)
 868                 {
 869                         computeContactPlaneSphere(pairIndex, bodyIndexA, bodyIndexB, collidableIndexA, collidableIndexB,
 870                                                                                                                                 rigidBodies,collidables,faces,  globalContactsOut, nGlobalContactsOut,maxContactCapacity);
 871                         return;
 872                 }
 873
 874
 875                 if (collidables[collidableIndexA].m_shapeType == SHAPE_SPHERE &&
 876                         collidables[collidableIndexB].m_shapeType == SHAPE_PLANE)
 877                 {
 878
 879
 880                         computeContactPlaneSphere( pairIndex, bodyIndexB,bodyIndexA,  collidableIndexB,collidableIndexA,
 881                                                                                                                                 rigidBodies,collidables,
 882                                                                                                                                 faces,  globalContactsOut, nGlobalContactsOut,maxContactCapacity);
 883
 884                         return;
 885                 }
 886
 887
 888
 889
 890                 if (collidables[collidableIndexA].m_shapeType == SHAPE_SPHERE &&
 891                         collidables[collidableIndexB].m_shapeType == SHAPE_CONVEX_HULL)
 892                 {
 893
 894                         float4 spherePos = rigidBodies[bodyIndexA].m_pos;
 895                         float sphereRadius = collidables[collidableIndexA].m_radius;
 896                         float4 convexPos = rigidBodies[bodyIndexB].m_pos;
 897                         float4 convexOrn = rigidBodies[bodyIndexB].m_quat;
 898
 899                         computeContactSphereConvex(pairIndex, bodyIndexA, bodyIndexB, collidableIndexA, collidableIndexB,
 900                                                                                                                                 rigidBodies,collidables,convexShapes,vertices,indices,faces, globalContactsOut, nGlobalContactsOut,maxContactCapacity,
 901                                                                                                                                 spherePos,sphereRadius,convexPos,convexOrn);
 902
 903                         return;
 904                 }
 905
 906                 if (collidables[collidableIndexA].m_shapeType == SHAPE_CONVEX_HULL &&
 907                         collidables[collidableIndexB].m_shapeType == SHAPE_SPHERE)
 908                 {
 909
 910                         float4 spherePos = rigidBodies[bodyIndexB].m_pos;
 911                         float sphereRadius = collidables[collidableIndexB].m_radius;
 912                         float4 convexPos = rigidBodies[bodyIndexA].m_pos;
 913                         float4 convexOrn = rigidBodies[bodyIndexA].m_quat;
 914
 915                         computeContactSphereConvex(pairIndex, bodyIndexB, bodyIndexA, collidableIndexB, collidableIndexA,
 916                                                                                                                                 rigidBodies,collidables,convexShapes,vertices,indices,faces, globalContactsOut, nGlobalContactsOut,maxContactCapacity,
 917                                                                                                                                 spherePos,sphereRadius,convexPos,convexOrn);
 918                         return;
 919                 }
 920
 921
 922
 923
 924
 925
 926                 if (collidables[collidableIndexA].m_shapeType == SHAPE_SPHERE &&
 927                         collidables[collidableIndexB].m_shapeType == SHAPE_SPHERE)
 928                 {
 929                         //sphere-sphere
 930                         float radiusA = collidables[collidableIndexA].m_radius;
 931                         float radiusB = collidables[collidableIndexB].m_radius;
 932                         float4 posA = rigidBodies[bodyIndexA].m_pos;
 933                         float4 posB = rigidBodies[bodyIndexB].m_pos;
 934
 935                         float4 diff = posA-posB;
 936                         float len = length(diff);
 937
 938                         ///iff distance positive, don't generate a new contact
 939                         if ( len <= (radiusA+radiusB))
 940                         {
 941                                 ///distance (negative means penetration)
 942                                 float dist = len - (radiusA+radiusB);
 943                                 float4 normalOnSurfaceB = make_float4(1.f,0.f,0.f,0.f);
 944                                 if (len > 0.00001)
 945                                 {
 946                                         normalOnSurfaceB = diff / len;
 947                                 }
 948                                 float4 contactPosB = posB + normalOnSurfaceB*radiusB;
 949                                 contactPosB.w = dist;
 950
 951                                 int dstIdx;
 952                                  AppendInc( nGlobalContactsOut, dstIdx );
 953
 954                                 if (dstIdx < maxContactCapacity)
 955                                 {
 956                                         __global struct b3Contact4Data* c = &globalContactsOut[dstIdx];
 957                                         c->m_worldNormalOnB = normalOnSurfaceB;
 958                                         c->m_restituitionCoeffCmp = (0.f*0xffff);c->m_frictionCoeffCmp = (0.7f*0xffff);
 959                                         c->m_batchIdx = pairIndex;
 960                                         int bodyA = pairs[pairIndex].x;
 961                                         int bodyB = pairs[pairIndex].y;
 962                                         c->m_bodyAPtrAndSignBit = rigidBodies[bodyA].m_invMass==0?-bodyA:bodyA;
 963                                         c->m_bodyBPtrAndSignBit = rigidBodies[bodyB].m_invMass==0?-bodyB:bodyB;
 964                                         c->m_worldPosB[0] = contactPosB;
 965                                         c->m_childIndexA = -1;
 966                                         c->m_childIndexB = -1;
 967                                         GET_NPOINTS(*c) = 1;
 968                                 }//if (dstIdx < numPairs)
 969                         }//if ( len <= (radiusA+radiusB))
 970
 971                         return;
 972                 }//SHAPE_SPHERE SHAPE_SPHERE
 973
 974         }//     if (i<numPairs)
 975
 976 }
 977
 978
 979 // work-in-progress
 980 __kernel void   processCompoundPairsPrimitivesKernel( __global const int4* gpuCompoundPairs,
 981                                                                                                         __global const BodyData* rigidBodies,
 982                                                                                                         __global const btCollidableGpu* collidables,
 983                                                                                                         __global const ConvexPolyhedronCL* convexShapes,
 984                                                                                                         __global const float4* vertices,
 985                                                                                                         __global const float4* uniqueEdges,
 986                                                                                                         __global const btGpuFace* faces,
 987                                                                                                         __global const int* indices,
 988                                                                                                         __global btAabbCL* aabbs,
 989                                                                                                         __global const btGpuChildShape* gpuChildShapes,
 990                                                                                                         __global struct b3Contact4Data* restrict globalContactsOut,
 991                                                                                                         counter32_t nGlobalContactsOut,
 992                                                                                                         int numCompoundPairs, int maxContactCapacity
 993                                                                                                         )
 994 {
 995
 996         int i = get_global_id(0);
 997         if (i<numCompoundPairs)
 998         {
 999                 int bodyIndexA = gpuCompoundPairs[i].x;
1000                 int bodyIndexB = gpuCompoundPairs[i].y;
1001
1002                 int childShapeIndexA = gpuCompoundPairs[i].z;
1003                 int childShapeIndexB = gpuCompoundPairs[i].w;
1004
1005                 int collidableIndexA = -1;
1006                 int collidableIndexB = -1;
1007
1008                 float4 ornA = rigidBodies[bodyIndexA].m_quat;
1009                 float4 posA = rigidBodies[bodyIndexA].m_pos;
1010
1011                 float4 ornB = rigidBodies[bodyIndexB].m_quat;
1012                 float4 posB = rigidBodies[bodyIndexB].m_pos;
1013
1014                 if (childShapeIndexA >= 0)
1015                 {
1016                         collidableIndexA = gpuChildShapes[childShapeIndexA].m_shapeIndex;
1017                         float4 childPosA = gpuChildShapes[childShapeIndexA].m_childPosition;
1018                         float4 childOrnA = gpuChildShapes[childShapeIndexA].m_childOrientation;
1019                         float4 newPosA = qtRotate(ornA,childPosA)+posA;
1020                         float4 newOrnA = qtMul(ornA,childOrnA);
1021                         posA = newPosA;
1022                         ornA = newOrnA;
1023                 } else
1024                 {
1025                         collidableIndexA = rigidBodies[bodyIndexA].m_collidableIdx;
1026                 }
1027
1028                 if (childShapeIndexB>=0)
1029                 {
1030                         collidableIndexB = gpuChildShapes[childShapeIndexB].m_shapeIndex;
1031                         float4 childPosB = gpuChildShapes[childShapeIndexB].m_childPosition;
1032                         float4 childOrnB = gpuChildShapes[childShapeIndexB].m_childOrientation;
1033                         float4 newPosB = transform(&childPosB,&posB,&ornB);
1034                         float4 newOrnB = qtMul(ornB,childOrnB);
1035                         posB = newPosB;
1036                         ornB = newOrnB;
1037                 } else
1038                 {
1039                         collidableIndexB = rigidBodies[bodyIndexB].m_collidableIdx;
1040                 }
1041
1042                 int shapeIndexA = collidables[collidableIndexA].m_shapeIndex;
1043                 int shapeIndexB = collidables[collidableIndexB].m_shapeIndex;
1044
1045                 int shapeTypeA = collidables[collidableIndexA].m_shapeType;
1046                 int shapeTypeB = collidables[collidableIndexB].m_shapeType;
1047
1048                 int pairIndex = i;
1049                 if ((shapeTypeA == SHAPE_PLANE) && (shapeTypeB==SHAPE_CONVEX_HULL))
1050                 {
1051
1052                         computeContactPlaneConvex( pairIndex, bodyIndexA,bodyIndexB,  collidableIndexA,collidableIndexB,
1053                                                                                                                                 rigidBodies,collidables,convexShapes,vertices,indices,
1054                                                                                                                                 faces,  globalContactsOut, nGlobalContactsOut,maxContactCapacity,posB,ornB);
1055                         return;
1056                 }
1057
1058                 if ((shapeTypeA == SHAPE_CONVEX_HULL) && (shapeTypeB==SHAPE_PLANE))
1059                 {
1060
1061                         computeContactPlaneConvex( pairIndex, bodyIndexB,bodyIndexA,  collidableIndexB,collidableIndexA,
1062                                                                                                                                 rigidBodies,collidables,convexShapes,vertices,indices,
1063                                                                                                                                 faces,  globalContactsOut, nGlobalContactsOut,maxContactCapacity,posA,ornA);
1064                         return;
1065                 }
1066
1067                 if ((shapeTypeA == SHAPE_CONVEX_HULL) && (shapeTypeB == SHAPE_SPHERE))
1068                 {
1069                         float4 spherePos = rigidBodies[bodyIndexB].m_pos;
1070                         float sphereRadius = collidables[collidableIndexB].m_radius;
1071                         float4 convexPos = posA;
1072                         float4 convexOrn = ornA;
1073
1074                         computeContactSphereConvex(pairIndex, bodyIndexB, bodyIndexA , collidableIndexB,collidableIndexA,
1075                                                                                 rigidBodies,collidables,convexShapes,vertices,indices,faces, globalContactsOut, nGlobalContactsOut,maxContactCapacity,
1076                                                                                 spherePos,sphereRadius,convexPos,convexOrn);
1077
1078                         return;
1079                 }
1080
1081                 if ((shapeTypeA == SHAPE_SPHERE) && (shapeTypeB == SHAPE_CONVEX_HULL))
1082                 {
1083
1084                         float4 spherePos = rigidBodies[bodyIndexA].m_pos;
1085                         float sphereRadius = collidables[collidableIndexA].m_radius;
1086                         float4 convexPos = posB;
1087                         float4 convexOrn = ornB;
1088
1089
1090                         computeContactSphereConvex(pairIndex, bodyIndexA, bodyIndexB, collidableIndexA, collidableIndexB,
1091                                                                                 rigidBodies,collidables,convexShapes,vertices,indices,faces, globalContactsOut, nGlobalContactsOut,maxContactCapacity,
1092                                                                                 spherePos,sphereRadius,convexPos,convexOrn);
1093
1094                         return;
1095                 }
1096         }//     if (i<numCompoundPairs)
1097 }
1098
1099
1100 bool pointInTriangle(const float4* vertices, const float4* normal, float4 *p )
1101 {
1102
1103         const float4* p1 = &vertices[0];
1104         const float4* p2 = &vertices[1];
1105         const float4* p3 = &vertices[2];
1106
1107         float4 edge1;   edge1 = (*p2 - *p1);
1108         float4 edge2;   edge2 = ( *p3 - *p2 );
1109         float4 edge3;   edge3 = ( *p1 - *p3 );
1110
1111
1112         float4 p1_to_p; p1_to_p = ( *p - *p1 );
1113         float4 p2_to_p; p2_to_p = ( *p - *p2 );
1114         float4 p3_to_p; p3_to_p = ( *p - *p3 );
1115
1116         float4 edge1_normal; edge1_normal = ( cross(edge1,*normal));
1117         float4 edge2_normal; edge2_normal = ( cross(edge2,*normal));
1118         float4 edge3_normal; edge3_normal = ( cross(edge3,*normal));
1119
1120
1121
1122         float r1, r2, r3;
1123         r1 = dot(edge1_normal,p1_to_p );
1124         r2 = dot(edge2_normal,p2_to_p );
1125         r3 = dot(edge3_normal,p3_to_p );
1126
1127         if ( r1 > 0 && r2 > 0 && r3 > 0 )
1128                 return true;
1129     if ( r1 <= 0 && r2 <= 0 && r3 <= 0 )
1130                 return true;
1131         return false;
1132
1133 }
1134
1135
1136 float segmentSqrDistance(float4 from, float4 to,float4 p, float4* nearest)
1137 {
1138         float4 diff = p - from;
1139         float4 v = to - from;
1140         float t = dot(v,diff);
1141
1142         if (t > 0)
1143         {
1144                 float dotVV = dot(v,v);
1145                 if (t < dotVV)
1146                 {
1147                         t /= dotVV;
1148                         diff -= t*v;
1149                 } else
1150                 {
1151                         t = 1;
1152                         diff -= v;
1153                 }
1154         } else
1155         {
1156                 t = 0;
1157         }
1158         *nearest = from + t*v;
1159         return dot(diff,diff);
1160 }
1161
1162
1163 void    computeContactSphereTriangle(int pairIndex,
1164                                                                         int bodyIndexA, int bodyIndexB,
1165                                                                         int collidableIndexA, int collidableIndexB,
1166                                                                         __global const BodyData* rigidBodies,
1167                                                                         __global const btCollidableGpu* collidables,
1168                                                                         const float4* triangleVertices,
1169                                                                         __global struct b3Contact4Data* restrict globalContactsOut,
1170                                                                         counter32_t nGlobalContactsOut,
1171                                                                         int maxContactCapacity,
1172                                                                         float4 spherePos2,
1173                                                                         float radius,
1174                                                                         float4 pos,
1175                                                                         float4 quat,
1176                                                                         int faceIndex
1177                                                                         )
1178 {
1179
1180         float4 invPos;
1181         float4 invOrn;
1182
1183         trInverse(pos,quat, &invPos,&invOrn);
1184         float4 spherePos = transform(&spherePos2,&invPos,&invOrn);
1185         int numFaces = 3;
1186         float4 closestPnt = (float4)(0, 0, 0, 0);
1187         float4 hitNormalWorld = (float4)(0, 0, 0, 0);
1188         float minDist = -1000000.f;
1189         bool bCollide = false;
1190
1191
1192         //////////////////////////////////////
1193
1194         float4 sphereCenter;
1195         sphereCenter = spherePos;
1196
1197         const float4* vertices = triangleVertices;
1198         float contactBreakingThreshold = 0.f;//todo?
1199         float radiusWithThreshold = radius + contactBreakingThreshold;
1200         float4 edge10;
1201         edge10 = vertices[1]-vertices[0];
1202         edge10.w = 0.f;//is this needed?
1203         float4 edge20;
1204         edge20 = vertices[2]-vertices[0];
1205         edge20.w = 0.f;//is this needed?
1206         float4 normal = cross3(edge10,edge20);
1207         normal = normalize(normal);
1208         float4 p1ToCenter;
1209         p1ToCenter = sphereCenter - vertices[0];
1210
1211         float distanceFromPlane = dot(p1ToCenter,normal);
1212
1213         if (distanceFromPlane < 0.f)
1214         {
1215                 //triangle facing the other way
1216                 distanceFromPlane *= -1.f;
1217                 normal *= -1.f;
1218         }
1219         hitNormalWorld = normal;
1220
1221         bool isInsideContactPlane = distanceFromPlane < radiusWithThreshold;
1222
1223         // Check for contact / intersection
1224         bool hasContact = false;
1225         float4 contactPoint;
1226         if (isInsideContactPlane)
1227         {
1228
1229                 if (pointInTriangle(vertices,&normal, &sphereCenter))
1230                 {
1231                         // Inside the contact wedge - touches a point on the shell plane
1232                         hasContact = true;
1233                         contactPoint = sphereCenter - normal*distanceFromPlane;
1234
1235                 } else {
1236                         // Could be inside one of the contact capsules
1237                         float contactCapsuleRadiusSqr = radiusWithThreshold*radiusWithThreshold;
1238                         float4 nearestOnEdge;
1239                         int numEdges = 3;
1240                         for (int i = 0; i < numEdges; i++)
1241                         {
1242                                 float4 pa =vertices[i];
1243                                 float4 pb = vertices[(i+1)%3];
1244
1245                                 float distanceSqr = segmentSqrDistance(pa,pb,sphereCenter, &nearestOnEdge);
1246                                 if (distanceSqr < contactCapsuleRadiusSqr)
1247                                 {
1248                                         // Yep, we're inside a capsule
1249                                         hasContact = true;
1250                                         contactPoint = nearestOnEdge;
1251
1252                                 }
1253
1254                         }
1255                 }
1256         }
1257
1258         if (hasContact)
1259         {
1260
1261                 closestPnt = contactPoint;
1262                 float4 contactToCenter = sphereCenter - contactPoint;
1263                 minDist = length(contactToCenter);
1264                 if (minDist>FLT_EPSILON)
1265                 {
1266                         hitNormalWorld = normalize(contactToCenter);//*(1./minDist);
1267                         bCollide  = true;
1268                 }
1269
1270         }
1271
1272
1273         /////////////////////////////////////
1274
1275         if (bCollide && minDist > -10000)
1276         {
1277
1278                 float4 normalOnSurfaceB1 = qtRotate(quat,-hitNormalWorld);
1279                 float4 pOnB1 = transform(&closestPnt,&pos,&quat);
1280                 float actualDepth = minDist-radius;
1281
1282
1283                 if (actualDepth<=0.f)
1284                 {
1285                         pOnB1.w = actualDepth;
1286                         int dstIdx;
1287
1288
1289                         float lenSqr = dot3F4(normalOnSurfaceB1,normalOnSurfaceB1);
1290                         if (lenSqr>FLT_EPSILON)
1291                         {
1292                                 AppendInc( nGlobalContactsOut, dstIdx );
1293
1294                                 if (dstIdx < maxContactCapacity)
1295                                 {
1296                                         __global struct b3Contact4Data* c = &globalContactsOut[dstIdx];
1297                                         c->m_worldNormalOnB = -normalOnSurfaceB1;
1298                                         c->m_restituitionCoeffCmp = (0.f*0xffff);c->m_frictionCoeffCmp = (0.7f*0xffff);
1299                                         c->m_batchIdx = pairIndex;
1300                                         c->m_bodyAPtrAndSignBit = rigidBodies[bodyIndexA].m_invMass==0?-bodyIndexA:bodyIndexA;
1301                                         c->m_bodyBPtrAndSignBit = rigidBodies[bodyIndexB].m_invMass==0?-bodyIndexB:bodyIndexB;
1302                                         c->m_worldPosB[0] = pOnB1;
1303
1304                                         c->m_childIndexA = -1;
1305                                         c->m_childIndexB = faceIndex;
1306
1307                                         GET_NPOINTS(*c) = 1;
1308                                 }
1309                         }
1310
1311                 }
1312         }//if (hasCollision)
1313
1314 }
1315
1316
1317
1318 // work-in-progress
1319 __kernel void   findConcaveSphereContactsKernel( __global int4* concavePairs,
1320                                                                                                 __global const BodyData* rigidBodies,
1321                                                                                                 __global const btCollidableGpu* collidables,
1322                                                                                                 __global const ConvexPolyhedronCL* convexShapes,
1323                                                                                                 __global const float4* vertices,
1324                                                                                                 __global const float4* uniqueEdges,
1325                                                                                                 __global const btGpuFace* faces,
1326                                                                                                 __global const int* indices,
1327                                                                                                 __global btAabbCL* aabbs,
1328                                                                                                 __global struct b3Contact4Data* restrict globalContactsOut,
1329                                                                                                 counter32_t nGlobalContactsOut,
1330                                                                                                         int numConcavePairs, int maxContactCapacity
1331                                                                                                 )
1332 {
1333
1334         int i = get_global_id(0);
1335         if (i>=numConcavePairs)
1336                 return;
1337         int pairIdx = i;
1338
1339         int bodyIndexA = concavePairs[i].x;
1340         int bodyIndexB = concavePairs[i].y;
1341
1342         int collidableIndexA = rigidBodies[bodyIndexA].m_collidableIdx;
1343         int collidableIndexB = rigidBodies[bodyIndexB].m_collidableIdx;
1344
1345         int shapeIndexA = collidables[collidableIndexA].m_shapeIndex;
1346         int shapeIndexB = collidables[collidableIndexB].m_shapeIndex;
1347
1348         if (collidables[collidableIndexB].m_shapeType==SHAPE_SPHERE)
1349         {
1350                 int f = concavePairs[i].z;
1351                 btGpuFace face = faces[convexShapes[shapeIndexA].m_faceOffset+f];
1352
1353                 float4 verticesA[3];
1354                 for (int i=0;i<3;i++)
1355                 {
1356                         int index = indices[face.m_indexOffset+i];
1357                         float4 vert = vertices[convexShapes[shapeIndexA].m_vertexOffset+index];
1358                         verticesA[i] = vert;
1359                 }
1360
1361                 float4 spherePos = rigidBodies[bodyIndexB].m_pos;
1362                 float sphereRadius = collidables[collidableIndexB].m_radius;
1363                 float4 convexPos = rigidBodies[bodyIndexA].m_pos;
1364                 float4 convexOrn = rigidBodies[bodyIndexA].m_quat;
1365
1366                 computeContactSphereTriangle(i, bodyIndexB, bodyIndexA, collidableIndexB, collidableIndexA,
1367                                                                                                                                 rigidBodies,collidables,
1368                                                                                                                                 verticesA,
1369                                                                                                                                 globalContactsOut, nGlobalContactsOut,maxContactCapacity,
1370                                                                                                                                 spherePos,sphereRadius,convexPos,convexOrn, f);
1371
1372                 return;
1373         }
1374 }