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

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