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

   1 #ifndef B3_REDUCE_CONTACTS_H
   2 #define B3_REDUCE_CONTACTS_H
   3
   4 inline int b3ReduceContacts(const b3Float4* p, int nPoints, const b3Float4& nearNormal, b3Int4* contactIdx)
   5 {
   6         if (nPoints == 0)
   7                 return 0;
   8
   9         if (nPoints <= 4)
  10                 return nPoints;
  11
  12         if (nPoints > 64)
  13                 nPoints = 64;
  14
  15         b3Float4 center = b3MakeFloat4(0, 0, 0, 0);
  16         {
  17                 for (int i = 0; i < nPoints; i++)
  18                         center += p[i];
  19                 center /= (float)nPoints;
  20         }
  21
  22         //      sample 4 directions
  23
  24         b3Float4 aVector = p[0] - center;
  25         b3Float4 u = b3Cross3(nearNormal, aVector);
  26         b3Float4 v = b3Cross3(nearNormal, u);
  27         u = b3FastNormalized3(u);
  28         v = b3FastNormalized3(v);
  29
  30         //keep point with deepest penetration
  31         float minW = FLT_MAX;
  32
  33         int minIndex = -1;
  34
  35         b3Float4 maxDots;
  36         maxDots.x = FLT_MIN;
  37         maxDots.y = FLT_MIN;
  38         maxDots.z = FLT_MIN;
  39         maxDots.w = FLT_MIN;
  40
  41         //      idx, distance
  42         for (int ie = 0; ie < nPoints; ie++)
  43         {
  44                 if (p[ie].w < minW)
  45                 {
  46                         minW = p[ie].w;
  47                         minIndex = ie;
  48                 }
  49                 float f;
  50                 b3Float4 r = p[ie] - center;
  51                 f = b3Dot3F4(u, r);
  52                 if (f < maxDots.x)
  53                 {
  54                         maxDots.x = f;
  55                         contactIdx[0].x = ie;
  56                 }
  57
  58                 f = b3Dot3F4(-u, r);
  59                 if (f < maxDots.y)
  60                 {
  61                         maxDots.y = f;
  62                         contactIdx[0].y = ie;
  63                 }
  64
  65                 f = b3Dot3F4(v, r);
  66                 if (f < maxDots.z)
  67                 {
  68                         maxDots.z = f;
  69                         contactIdx[0].z = ie;
  70                 }
  71
  72                 f = b3Dot3F4(-v, r);
  73                 if (f < maxDots.w)
  74                 {
  75                         maxDots.w = f;
  76                         contactIdx[0].w = ie;
  77                 }
  78         }
  79
  80         if (contactIdx[0].x != minIndex && contactIdx[0].y != minIndex && contactIdx[0].z != minIndex && contactIdx[0].w != minIndex)
  81         {
  82                 //replace the first contact with minimum (todo: replace contact with least penetration)
  83                 contactIdx[0].x = minIndex;
  84         }
  85
  86         return 4;
  87 }
  88
  89 #endif  //B3_REDUCE_CONTACTS_H