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

   1 #ifndef B3_FIND_SEPARATING_AXIS_H
   2 #define B3_FIND_SEPARATING_AXIS_H
   3
   4 inline void b3ProjectAxis(const b3ConvexPolyhedronData& hull, const b3Float4& pos, const b3Quaternion& orn, const b3Float4& dir, const b3AlignedObjectArray<b3Vector3>& vertices, b3Scalar& min, b3Scalar& max)
   5 {
   6         min = FLT_MAX;
   7         max = -FLT_MAX;
   8         int numVerts = hull.m_numVertices;
   9
  10         const b3Float4 localDir = b3QuatRotate(orn.inverse(), dir);
  11
  12         b3Scalar offset = b3Dot3F4(pos, dir);
  13
  14         for (int i = 0; i < numVerts; i++)
  15         {
  16                 //b3Vector3 pt = trans * vertices[m_vertexOffset+i];
  17                 //b3Scalar dp = pt.dot(dir);
  18                 //b3Vector3 vertex = vertices[hull.m_vertexOffset+i];
  19                 b3Scalar dp = b3Dot3F4((b3Float4&)vertices[hull.m_vertexOffset + i], localDir);
  20                 //b3Assert(dp==dpL);
  21                 if (dp < min) min = dp;
  22                 if (dp > max) max = dp;
  23         }
  24         if (min > max)
  25         {
  26                 b3Scalar tmp = min;
  27                 min = max;
  28                 max = tmp;
  29         }
  30         min += offset;
  31         max += offset;
  32 }
  33
  34 inline bool b3TestSepAxis(const b3ConvexPolyhedronData& hullA, const b3ConvexPolyhedronData& hullB,
  35                                                   const b3Float4& posA, const b3Quaternion& ornA,
  36                                                   const b3Float4& posB, const b3Quaternion& ornB,
  37                                                   const b3Float4& sep_axis, const b3AlignedObjectArray<b3Vector3>& verticesA, const b3AlignedObjectArray<b3Vector3>& verticesB, b3Scalar& depth)
  38 {
  39         b3Scalar Min0, Max0;
  40         b3Scalar Min1, Max1;
  41         b3ProjectAxis(hullA, posA, ornA, sep_axis, verticesA, Min0, Max0);
  42         b3ProjectAxis(hullB, posB, ornB, sep_axis, verticesB, Min1, Max1);
  43
  44         if (Max0 < Min1 || Max1 < Min0)
  45                 return false;
  46
  47         b3Scalar d0 = Max0 - Min1;
  48         b3Assert(d0 >= 0.0f);
  49         b3Scalar d1 = Max1 - Min0;
  50         b3Assert(d1 >= 0.0f);
  51         depth = d0 < d1 ? d0 : d1;
  52         return true;
  53 }
  54
  55 inline bool b3FindSeparatingAxis(const b3ConvexPolyhedronData& hullA, const b3ConvexPolyhedronData& hullB,
  56                                                                  const b3Float4& posA1,
  57                                                                  const b3Quaternion& ornA,
  58                                                                  const b3Float4& posB1,
  59                                                                  const b3Quaternion& ornB,
  60                                                                  const b3AlignedObjectArray<b3Vector3>& verticesA,
  61                                                                  const b3AlignedObjectArray<b3Vector3>& uniqueEdgesA,
  62                                                                  const b3AlignedObjectArray<b3GpuFace>& facesA,
  63                                                                  const b3AlignedObjectArray<int>& indicesA,
  64                                                                  const b3AlignedObjectArray<b3Vector3>& verticesB,
  65                                                                  const b3AlignedObjectArray<b3Vector3>& uniqueEdgesB,
  66                                                                  const b3AlignedObjectArray<b3GpuFace>& facesB,
  67                                                                  const b3AlignedObjectArray<int>& indicesB,
  68
  69                                                                  b3Vector3& sep)
  70 {
  71         B3_PROFILE("findSeparatingAxis");
  72
  73         b3Float4 posA = posA1;
  74         posA.w = 0.f;
  75         b3Float4 posB = posB1;
  76         posB.w = 0.f;
  77         //#ifdef TEST_INTERNAL_OBJECTS
  78         b3Float4 c0local = (b3Float4&)hullA.m_localCenter;
  79
  80         b3Float4 c0 = b3TransformPoint(c0local, posA, ornA);
  81         b3Float4 c1local = (b3Float4&)hullB.m_localCenter;
  82         b3Float4 c1 = b3TransformPoint(c1local, posB, ornB);
  83         const b3Float4 deltaC2 = c0 - c1;
  84         //#endif
  85
  86         b3Scalar dmin = FLT_MAX;
  87         int curPlaneTests = 0;
  88
  89         int numFacesA = hullA.m_numFaces;
  90         // Test normals from hullA
  91         for (int i = 0; i < numFacesA; i++)
  92         {
  93                 const b3Float4& normal = (b3Float4&)facesA[hullA.m_faceOffset + i].m_plane;
  94                 b3Float4 faceANormalWS = b3QuatRotate(ornA, normal);
  95
  96                 if (b3Dot3F4(deltaC2, faceANormalWS) < 0)
  97                         faceANormalWS *= -1.f;
  98
  99                 curPlaneTests++;
 100 #ifdef TEST_INTERNAL_OBJECTS
 101                 gExpectedNbTests++;
 102                 if (gUseInternalObject && !TestInternalObjects(transA, transB, DeltaC2, faceANormalWS, hullA, hullB, dmin))
 103                         continue;
 104                 gActualNbTests++;
 105 #endif
 106
 107                 b3Scalar d;
 108                 if (!b3TestSepAxis(hullA, hullB, posA, ornA, posB, ornB, faceANormalWS, verticesA, verticesB, d))
 109                         return false;
 110
 111                 if (d < dmin)
 112                 {
 113                         dmin = d;
 114                         sep = (b3Vector3&)faceANormalWS;
 115                 }
 116         }
 117
 118         int numFacesB = hullB.m_numFaces;
 119         // Test normals from hullB
 120         for (int i = 0; i < numFacesB; i++)
 121         {
 122                 b3Float4 normal = (b3Float4&)facesB[hullB.m_faceOffset + i].m_plane;
 123                 b3Float4 WorldNormal = b3QuatRotate(ornB, normal);
 124
 125                 if (b3Dot3F4(deltaC2, WorldNormal) < 0)
 126                 {
 127                         WorldNormal *= -1.f;
 128                 }
 129                 curPlaneTests++;
 130 #ifdef TEST_INTERNAL_OBJECTS
 131                 gExpectedNbTests++;
 132                 if (gUseInternalObject && !TestInternalObjects(transA, transB, DeltaC2, WorldNormal, hullA, hullB, dmin))
 133                         continue;
 134                 gActualNbTests++;
 135 #endif
 136
 137                 b3Scalar d;
 138                 if (!b3TestSepAxis(hullA, hullB, posA, ornA, posB, ornB, WorldNormal, verticesA, verticesB, d))
 139                         return false;
 140
 141                 if (d < dmin)
 142                 {
 143                         dmin = d;
 144                         sep = (b3Vector3&)WorldNormal;
 145                 }
 146         }
 147
 148         //      b3Vector3 edgeAstart,edgeAend,edgeBstart,edgeBend;
 149
 150         int curEdgeEdge = 0;
 151         // Test edges
 152         for (int e0 = 0; e0 < hullA.m_numUniqueEdges; e0++)
 153         {
 154                 const b3Float4& edge0 = (b3Float4&)uniqueEdgesA[hullA.m_uniqueEdgesOffset + e0];
 155                 b3Float4 edge0World = b3QuatRotate(ornA, (b3Float4&)edge0);
 156
 157                 for (int e1 = 0; e1 < hullB.m_numUniqueEdges; e1++)
 158                 {
 159                         const b3Vector3 edge1 = uniqueEdgesB[hullB.m_uniqueEdgesOffset + e1];
 160                         b3Float4 edge1World = b3QuatRotate(ornB, (b3Float4&)edge1);
 161
 162                         b3Float4 crossje = b3Cross3(edge0World, edge1World);
 163
 164                         curEdgeEdge++;
 165                         if (!b3IsAlmostZero((b3Vector3&)crossje))
 166                         {
 167                                 crossje = b3FastNormalized3(crossje);
 168                                 if (b3Dot3F4(deltaC2, crossje) < 0)
 169                                         crossje *= -1.f;
 170
 171 #ifdef TEST_INTERNAL_OBJECTS
 172                                 gExpectedNbTests++;
 173                                 if (gUseInternalObject && !TestInternalObjects(transA, transB, DeltaC2, Cross, hullA, hullB, dmin))
 174                                         continue;
 175                                 gActualNbTests++;
 176 #endif
 177
 178                                 b3Scalar dist;
 179                                 if (!b3TestSepAxis(hullA, hullB, posA, ornA, posB, ornB, crossje, verticesA, verticesB, dist))
 180                                         return false;
 181
 182                                 if (dist < dmin)
 183                                 {
 184                                         dmin = dist;
 185                                         sep = (b3Vector3&)crossje;
 186                                 }
 187                         }
 188                 }
 189         }
 190
 191         if ((b3Dot3F4(-deltaC2, (b3Float4&)sep)) > 0.0f)
 192                 sep = -sep;
 193
 194         return true;
 195 }
 196
 197 #endif  //B3_FIND_SEPARATING_AXIS_H