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

   1
   2 #include "Bullet3Collision/NarrowPhaseCollision/shared/b3MprPenetration.h"
   3 #include "Bullet3Collision/NarrowPhaseCollision/shared/b3Contact4Data.h"
   4
   5 #define AppendInc(x, out) out = atomic_inc(x)
   6 #define GET_NPOINTS(x) (x).m_worldNormalOnB.w
   7 #ifdef cl_ext_atomic_counters_32
   8         #pragma OPENCL EXTENSION cl_ext_atomic_counters_32 : enable
   9 #else
  10         #define counter32_t volatile __global int*
  11 #endif
  12
  13
  14 __kernel void   mprPenetrationKernel( __global int4* pairs,
  15                                                                                                                                                                         __global const b3RigidBodyData_t* rigidBodies,
  16                                                                                                                                                                         __global const b3Collidable_t* collidables,
  17                                                                                                                                                                         __global const b3ConvexPolyhedronData_t* convexShapes,
  18                                                                                                                                                                         __global const float4* vertices,
  19                                                                                                                                                                         __global float4* separatingNormals,
  20                                                                                                                                                                         __global int* hasSeparatingAxis,
  21                                                                                                                                                                         __global struct b3Contact4Data* restrict globalContactsOut,
  22                                                                                                                                                                         counter32_t nGlobalContactsOut,
  23                                                                                                                                                                         int contactCapacity,
  24                                                                                                                                                                         int numPairs)
  25 {
  26         int i = get_global_id(0);
  27         int pairIndex = i;
  28         if (i<numPairs)
  29         {
  30                 int bodyIndexA = pairs[i].x;
  31                 int bodyIndexB = pairs[i].y;
  32
  33                 int collidableIndexA = rigidBodies[bodyIndexA].m_collidableIdx;
  34                 int collidableIndexB = rigidBodies[bodyIndexB].m_collidableIdx;
  35
  36                 int shapeIndexA = collidables[collidableIndexA].m_shapeIndex;
  37                 int shapeIndexB = collidables[collidableIndexB].m_shapeIndex;
  38
  39
  40                 //once the broadphase avoids static-static pairs, we can remove this test
  41                 if ((rigidBodies[bodyIndexA].m_invMass==0) &&(rigidBodies[bodyIndexB].m_invMass==0))
  42                 {
  43                         return;
  44                 }
  45
  46
  47                 if ((collidables[collidableIndexA].m_shapeType!=SHAPE_CONVEX_HULL) ||(collidables[collidableIndexB].m_shapeType!=SHAPE_CONVEX_HULL))
  48                 {
  49                         return;
  50                 }
  51
  52                 float depthOut;
  53                 b3Float4 dirOut;
  54                 b3Float4 posOut;
  55
  56
  57                 int res = b3MprPenetration(pairIndex, bodyIndexA, bodyIndexB,rigidBodies,convexShapes,collidables,vertices,separatingNormals,hasSeparatingAxis,&depthOut, &dirOut, &posOut);
  58
  59
  60
  61
  62
  63                 if (res==0)
  64                 {
  65                         //add a contact
  66
  67                         int dstIdx;
  68                         AppendInc( nGlobalContactsOut, dstIdx );
  69                         if (dstIdx<contactCapacity)
  70                         {
  71                                 pairs[pairIndex].z = dstIdx;
  72                                 __global struct b3Contact4Data* c = globalContactsOut + dstIdx;
  73                                 c->m_worldNormalOnB = -dirOut;//normal;
  74                                 c->m_restituitionCoeffCmp = (0.f*0xffff);c->m_frictionCoeffCmp = (0.7f*0xffff);
  75                                 c->m_batchIdx = pairIndex;
  76                                 int bodyA = pairs[pairIndex].x;
  77                                 int bodyB = pairs[pairIndex].y;
  78                                 c->m_bodyAPtrAndSignBit = rigidBodies[bodyA].m_invMass==0 ? -bodyA:bodyA;
  79                                 c->m_bodyBPtrAndSignBit = rigidBodies[bodyB].m_invMass==0 ? -bodyB:bodyB;
  80                                 c->m_childIndexA = -1;
  81                                 c->m_childIndexB = -1;
  82                                 //for (int i=0;i<nContacts;i++)
  83                                 posOut.w = -depthOut;
  84                                 c->m_worldPosB[0] = posOut;//localPoints[contactIdx[i]];
  85                                 GET_NPOINTS(*c) = 1;//nContacts;
  86                         }
  87                 }
  88
  89         }
  90 }
  91
  92 typedef float4 Quaternion;
  93 #define make_float4 (float4)
  94
  95 __inline
  96 float dot3F4(float4 a, float4 b)
  97 {
  98         float4 a1 = make_float4(a.xyz,0.f);
  99         float4 b1 = make_float4(b.xyz,0.f);
 100         return dot(a1, b1);
 101 }
 102
 103
 104
 105
 106 __inline
 107 float4 cross3(float4 a, float4 b)
 108 {
 109         return cross(a,b);
 110 }
 111 __inline
 112 Quaternion qtMul(Quaternion a, Quaternion b)
 113 {
 114         Quaternion ans;
 115         ans = cross3( a, b );
 116         ans += a.w*b+b.w*a;
 117 //      ans.w = a.w*b.w - (a.x*b.x+a.y*b.y+a.z*b.z);
 118         ans.w = a.w*b.w - dot3F4(a, b);
 119         return ans;
 120 }
 121
 122 __inline
 123 Quaternion qtInvert(Quaternion q)
 124 {
 125         return (Quaternion)(-q.xyz, q.w);
 126 }
 127
 128 __inline
 129 float4 qtRotate(Quaternion q, float4 vec)
 130 {
 131         Quaternion qInv = qtInvert( q );
 132         float4 vcpy = vec;
 133         vcpy.w = 0.f;
 134         float4 out = qtMul(qtMul(q,vcpy),qInv);
 135         return out;
 136 }
 137
 138 __inline
 139 float4 transform(const float4* p, const float4* translation, const Quaternion* orientation)
 140 {
 141         return qtRotate( *orientation, *p ) + (*translation);
 142 }
 143
 144
 145 __inline
 146 float4 qtInvRotate(const Quaternion q, float4 vec)
 147 {
 148         return qtRotate( qtInvert( q ), vec );
 149 }
 150
 151
 152 inline void project(__global const b3ConvexPolyhedronData_t* hull,  const float4 pos, const float4 orn,
 153 const float4* dir, __global const float4* vertices, float* min, float* max)
 154 {
 155         min[0] = FLT_MAX;
 156         max[0] = -FLT_MAX;
 157         int numVerts = hull->m_numVertices;
 158
 159         const float4 localDir = qtInvRotate(orn,*dir);
 160         float offset = dot(pos,*dir);
 161         for(int i=0;i<numVerts;i++)
 162         {
 163                 float dp = dot(vertices[hull->m_vertexOffset+i],localDir);
 164                 if(dp < min[0])
 165                         min[0] = dp;
 166                 if(dp > max[0])
 167                         max[0] = dp;
 168         }
 169         if(min[0]>max[0])
 170         {
 171                 float tmp = min[0];
 172                 min[0] = max[0];
 173                 max[0] = tmp;
 174         }
 175         min[0] += offset;
 176         max[0] += offset;
 177 }
 178
 179
 180 bool findSeparatingAxisUnitSphere(      __global const b3ConvexPolyhedronData_t* hullA, __global const b3ConvexPolyhedronData_t* hullB,
 181         const float4 posA1,
 182         const float4 ornA,
 183         const float4 posB1,
 184         const float4 ornB,
 185         const float4 DeltaC2,
 186         __global const float4* vertices,
 187         __global const float4* unitSphereDirections,
 188         int numUnitSphereDirections,
 189         float4* sep,
 190         float* dmin)
 191 {
 192
 193         float4 posA = posA1;
 194         posA.w = 0.f;
 195         float4 posB = posB1;
 196         posB.w = 0.f;
 197
 198         int curPlaneTests=0;
 199
 200         int curEdgeEdge = 0;
 201         // Test unit sphere directions
 202         for (int i=0;i<numUnitSphereDirections;i++)
 203         {
 204
 205                 float4 crossje;
 206                 crossje = unitSphereDirections[i];
 207
 208                 if (dot3F4(DeltaC2,crossje)>0)
 209                         crossje *= -1.f;
 210                 {
 211                         float dist;
 212                         bool result = true;
 213                         float Min0,Max0;
 214                         float Min1,Max1;
 215                         project(hullA,posA,ornA,&crossje,vertices, &Min0, &Max0);
 216                         project(hullB,posB,ornB,&crossje,vertices, &Min1, &Max1);
 217
 218                         if(Max0<Min1 || Max1<Min0)
 219                                 return false;
 220
 221                         float d0 = Max0 - Min1;
 222                         float d1 = Max1 - Min0;
 223                         dist = d0<d1 ? d0:d1;
 224                         result = true;
 225
 226                         if(dist<*dmin)
 227                         {
 228                                 *dmin = dist;
 229                                 *sep = crossje;
 230                         }
 231                 }
 232         }
 233
 234
 235         if((dot3F4(-DeltaC2,*sep))>0.0f)
 236         {
 237                 *sep = -(*sep);
 238         }
 239         return true;
 240 }
 241
 242
 243
 244 __kernel void   findSeparatingAxisUnitSphereKernel( __global const int4* pairs,
 245                                                                                                                                                                         __global const b3RigidBodyData_t* rigidBodies,
 246                                                                                                                                                                         __global const b3Collidable_t* collidables,
 247                                                                                                                                                                         __global const b3ConvexPolyhedronData_t* convexShapes,
 248                                                                                                                                                                         __global const float4* vertices,
 249                                                                                                                                                                         __global const float4* unitSphereDirections,
 250                                                                                                                                                                         __global  float4* separatingNormals,
 251                                                                                                                                                                         __global  int* hasSeparatingAxis,
 252                                                                                                                                                                         __global  float* dmins,
 253                                                                                                                                                                         int numUnitSphereDirections,
 254                                                                                                                                                                         int numPairs
 255                                                                                                                                                                         )
 256 {
 257
 258         int i = get_global_id(0);
 259
 260         if (i<numPairs)
 261         {
 262
 263                 if (hasSeparatingAxis[i])
 264                 {
 265
 266                         int bodyIndexA = pairs[i].x;
 267                         int bodyIndexB = pairs[i].y;
 268
 269                         int collidableIndexA = rigidBodies[bodyIndexA].m_collidableIdx;
 270                         int collidableIndexB = rigidBodies[bodyIndexB].m_collidableIdx;
 271
 272                         int shapeIndexA = collidables[collidableIndexA].m_shapeIndex;
 273                         int shapeIndexB = collidables[collidableIndexB].m_shapeIndex;
 274
 275
 276                         int numFacesA = convexShapes[shapeIndexA].m_numFaces;
 277
 278                         float dmin = dmins[i];
 279
 280                         float4 posA = rigidBodies[bodyIndexA].m_pos;
 281                         posA.w = 0.f;
 282                         float4 posB = rigidBodies[bodyIndexB].m_pos;
 283                         posB.w = 0.f;
 284                         float4 c0local = convexShapes[shapeIndexA].m_localCenter;
 285                         float4 ornA = rigidBodies[bodyIndexA].m_quat;
 286                         float4 c0 = transform(&c0local, &posA, &ornA);
 287                         float4 c1local = convexShapes[shapeIndexB].m_localCenter;
 288                         float4 ornB =rigidBodies[bodyIndexB].m_quat;
 289                         float4 c1 = transform(&c1local,&posB,&ornB);
 290                         const float4 DeltaC2 = c0 - c1;
 291                         float4 sepNormal = separatingNormals[i];
 292
 293                         int numEdgeEdgeDirections = convexShapes[shapeIndexA].m_numUniqueEdges*convexShapes[shapeIndexB].m_numUniqueEdges;
 294                         if (numEdgeEdgeDirections>numUnitSphereDirections)
 295                         {
 296                                 bool sepEE = findSeparatingAxisUnitSphere(      &convexShapes[shapeIndexA], &convexShapes[shapeIndexB],posA,ornA,
 297                                                                                                                                                                                                                 posB,ornB,
 298                                                                                                                                                                                                                 DeltaC2,
 299                                                                                                                                                                                                                 vertices,unitSphereDirections,numUnitSphereDirections,&sepNormal,&dmin);
 300                                 if (!sepEE)
 301                                 {
 302                                         hasSeparatingAxis[i] = 0;
 303                                 } else
 304                                 {
 305                                         hasSeparatingAxis[i] = 1;
 306                                         separatingNormals[i] = sepNormal;
 307                                 }
 308                         }
 309                 }               //if (hasSeparatingAxis[i])
 310         }//(i<numPairs)
 311 }