dali-physics/third-party/bullet3/src/Bullet3Collision/NarrowPhaseCollision/shared/b3NewContactReduction.h

   1
   2 #ifndef B3_NEW_CONTACT_REDUCTION_H
   3 #define B3_NEW_CONTACT_REDUCTION_H
   4
   5 #include "Bullet3Common/shared/b3Float4.h"
   6 #include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h"
   7 #include "Bullet3Collision/NarrowPhaseCollision/shared/b3Contact4Data.h"
   8
   9 #define GET_NPOINTS(x) (x).m_worldNormalOnB.w
  10
  11 int b3ExtractManifoldSequentialGlobal(__global const b3Float4* p, int nPoints, b3Float4ConstArg nearNormal, b3Int4* contactIdx)
  12 {
  13         if (nPoints == 0)
  14                 return 0;
  15
  16         if (nPoints <= 4)
  17                 return nPoints;
  18
  19         if (nPoints > 64)
  20                 nPoints = 64;
  21
  22         b3Float4 center = b3MakeFloat4(0, 0, 0, 0);
  23         {
  24                 for (int i = 0; i < nPoints; i++)
  25                         center += p[i];
  26                 center /= (float)nPoints;
  27         }
  28
  29         //      sample 4 directions
  30
  31         b3Float4 aVector = p[0] - center;
  32         b3Float4 u = b3Cross(nearNormal, aVector);
  33         b3Float4 v = b3Cross(nearNormal, u);
  34         u = b3Normalized(u);
  35         v = b3Normalized(v);
  36
  37         //keep point with deepest penetration
  38         float minW = FLT_MAX;
  39
  40         int minIndex = -1;
  41
  42         b3Float4 maxDots;
  43         maxDots.x = FLT_MIN;
  44         maxDots.y = FLT_MIN;
  45         maxDots.z = FLT_MIN;
  46         maxDots.w = FLT_MIN;
  47
  48         //      idx, distance
  49         for (int ie = 0; ie < nPoints; ie++)
  50         {
  51                 if (p[ie].w < minW)
  52                 {
  53                         minW = p[ie].w;
  54                         minIndex = ie;
  55                 }
  56                 float f;
  57                 b3Float4 r = p[ie] - center;
  58                 f = b3Dot(u, r);
  59                 if (f < maxDots.x)
  60                 {
  61                         maxDots.x = f;
  62                         contactIdx[0].x = ie;
  63                 }
  64
  65                 f = b3Dot(-u, r);
  66                 if (f < maxDots.y)
  67                 {
  68                         maxDots.y = f;
  69                         contactIdx[0].y = ie;
  70                 }
  71
  72                 f = b3Dot(v, r);
  73                 if (f < maxDots.z)
  74                 {
  75                         maxDots.z = f;
  76                         contactIdx[0].z = ie;
  77                 }
  78
  79                 f = b3Dot(-v, r);
  80                 if (f < maxDots.w)
  81                 {
  82                         maxDots.w = f;
  83                         contactIdx[0].w = ie;
  84                 }
  85         }
  86
  87         if (contactIdx[0].x != minIndex && contactIdx[0].y != minIndex && contactIdx[0].z != minIndex && contactIdx[0].w != minIndex)
  88         {
  89                 //replace the first contact with minimum (todo: replace contact with least penetration)
  90                 contactIdx[0].x = minIndex;
  91         }
  92
  93         return 4;
  94 }
  95
  96 __kernel void b3NewContactReductionKernel(__global b3Int4* pairs,
  97                                                                                   __global const b3RigidBodyData_t* rigidBodies,
  98                                                                                   __global const b3Float4* separatingNormals,
  99                                                                                   __global const int* hasSeparatingAxis,
 100                                                                                   __global struct b3Contact4Data* globalContactsOut,
 101                                                                                   __global b3Int4* clippingFaces,
 102                                                                                   __global b3Float4* worldVertsB2,
 103                                                                                   volatile __global int* nGlobalContactsOut,
 104                                                                                   int vertexFaceCapacity,
 105                                                                                   int contactCapacity,
 106                                                                                   int numPairs,
 107                                                                                   int pairIndex)
 108 {
 109         //    int i = get_global_id(0);
 110         //int pairIndex = i;
 111         int i = pairIndex;
 112
 113         b3Int4 contactIdx;
 114         contactIdx = b3MakeInt4(0, 1, 2, 3);
 115
 116         if (i < numPairs)
 117         {
 118                 if (hasSeparatingAxis[i])
 119                 {
 120                         int nPoints = clippingFaces[pairIndex].w;
 121
 122                         if (nPoints > 0)
 123                         {
 124                                 __global b3Float4* pointsIn = &worldVertsB2[pairIndex * vertexFaceCapacity];
 125                                 b3Float4 normal = -separatingNormals[i];
 126
 127                                 int nReducedContacts = b3ExtractManifoldSequentialGlobal(pointsIn, nPoints, normal, &contactIdx);
 128
 129                                 int dstIdx;
 130                                 dstIdx = b3AtomicInc(nGlobalContactsOut);
 131
 132                                 //#if 0
 133                                 b3Assert(dstIdx < contactCapacity);
 134                                 if (dstIdx < contactCapacity)
 135                                 {
 136                                         __global struct b3Contact4Data* c = &globalContactsOut[dstIdx];
 137                                         c->m_worldNormalOnB = -normal;
 138                                         c->m_restituitionCoeffCmp = (0.f * 0xffff);
 139                                         c->m_frictionCoeffCmp = (0.7f * 0xffff);
 140                                         c->m_batchIdx = pairIndex;
 141                                         int bodyA = pairs[pairIndex].x;
 142                                         int bodyB = pairs[pairIndex].y;
 143
 144                                         pairs[pairIndex].w = dstIdx;
 145
 146                                         c->m_bodyAPtrAndSignBit = rigidBodies[bodyA].m_invMass == 0 ? -bodyA : bodyA;
 147                                         c->m_bodyBPtrAndSignBit = rigidBodies[bodyB].m_invMass == 0 ? -bodyB : bodyB;
 148                                         c->m_childIndexA = -1;
 149                                         c->m_childIndexB = -1;
 150
 151                                         switch (nReducedContacts)
 152                                         {
 153                                                 case 4:
 154                                                         c->m_worldPosB[3] = pointsIn[contactIdx.w];
 155                                                 case 3:
 156                                                         c->m_worldPosB[2] = pointsIn[contactIdx.z];
 157                                                 case 2:
 158                                                         c->m_worldPosB[1] = pointsIn[contactIdx.y];
 159                                                 case 1:
 160                                                         c->m_worldPosB[0] = pointsIn[contactIdx.x];
 161                                                 default:
 162                                                 {
 163                                                 }
 164                                         };
 165
 166                                         GET_NPOINTS(*c) = nReducedContacts;
 167                                 }
 168
 169                                 //#endif
 170
 171                         }  //           if (numContactsOut>0)
 172                 }      //               if (hasSeparatingAxis[i])
 173         }          //   if (i<numPairs)
 174 }
 175 #endif