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

   1
   2 #ifndef B3_CONTACT_CONVEX_CONVEX_SAT_H
   3 #define B3_CONTACT_CONVEX_CONVEX_SAT_H
   4
   5 #include "Bullet3Collision/NarrowPhaseCollision/shared/b3Contact4Data.h"
   6 #include "Bullet3Collision/NarrowPhaseCollision/shared/b3FindSeparatingAxis.h"
   7 #include "Bullet3Collision/NarrowPhaseCollision/shared/b3ReduceContacts.h"
   8
   9 #define B3_MAX_VERTS 1024
  10
  11 inline b3Float4 b3Lerp3(const b3Float4& a, const b3Float4& b, float t)
  12 {
  13         return b3MakeVector3(a.x + (b.x - a.x) * t,
  14                                                  a.y + (b.y - a.y) * t,
  15                                                  a.z + (b.z - a.z) * t,
  16                                                  0.f);
  17 }
  18
  19 // Clips a face to the back of a plane, return the number of vertices out, stored in ppVtxOut
  20 inline int b3ClipFace(const b3Float4* pVtxIn, int numVertsIn, b3Float4& planeNormalWS, float planeEqWS, b3Float4* ppVtxOut)
  21 {
  22         int ve;
  23         float ds, de;
  24         int numVertsOut = 0;
  25         if (numVertsIn < 2)
  26                 return 0;
  27
  28         b3Float4 firstVertex = pVtxIn[numVertsIn - 1];
  29         b3Float4 endVertex = pVtxIn[0];
  30
  31         ds = b3Dot3F4(planeNormalWS, firstVertex) + planeEqWS;
  32
  33         for (ve = 0; ve < numVertsIn; ve++)
  34         {
  35                 endVertex = pVtxIn[ve];
  36
  37                 de = b3Dot3F4(planeNormalWS, endVertex) + planeEqWS;
  38
  39                 if (ds < 0)
  40                 {
  41                         if (de < 0)
  42                         {
  43                                 // Start < 0, end < 0, so output endVertex
  44                                 ppVtxOut[numVertsOut++] = endVertex;
  45                         }
  46                         else
  47                         {
  48                                 // Start < 0, end >= 0, so output intersection
  49                                 ppVtxOut[numVertsOut++] = b3Lerp3(firstVertex, endVertex, (ds * 1.f / (ds - de)));
  50                         }
  51                 }
  52                 else
  53                 {
  54                         if (de < 0)
  55                         {
  56                                 // Start >= 0, end < 0 so output intersection and end
  57                                 ppVtxOut[numVertsOut++] = b3Lerp3(firstVertex, endVertex, (ds * 1.f / (ds - de)));
  58                                 ppVtxOut[numVertsOut++] = endVertex;
  59                         }
  60                 }
  61                 firstVertex = endVertex;
  62                 ds = de;
  63         }
  64         return numVertsOut;
  65 }
  66
  67 inline int b3ClipFaceAgainstHull(const b3Float4& separatingNormal, const b3ConvexPolyhedronData* hullA,
  68                                                                  const b3Float4& posA, const b3Quaternion& ornA, b3Float4* worldVertsB1, int numWorldVertsB1,
  69                                                                  b3Float4* worldVertsB2, int capacityWorldVertsB2,
  70                                                                  const float minDist, float maxDist,
  71                                                                  const b3AlignedObjectArray<b3Float4>& verticesA, const b3AlignedObjectArray<b3GpuFace>& facesA, const b3AlignedObjectArray<int>& indicesA,
  72                                                                  //const b3Float4* verticesB,   const b3GpuFace* facesB,        const int* indicesB,
  73                                                                  b3Float4* contactsOut,
  74                                                                  int contactCapacity)
  75 {
  76         int numContactsOut = 0;
  77
  78         b3Float4* pVtxIn = worldVertsB1;
  79         b3Float4* pVtxOut = worldVertsB2;
  80
  81         int numVertsIn = numWorldVertsB1;
  82         int numVertsOut = 0;
  83
  84         int closestFaceA = -1;
  85         {
  86                 float dmin = FLT_MAX;
  87                 for (int face = 0; face < hullA->m_numFaces; face++)
  88                 {
  89                         const b3Float4 Normal = b3MakeVector3(
  90                                 facesA[hullA->m_faceOffset + face].m_plane.x,
  91                                 facesA[hullA->m_faceOffset + face].m_plane.y,
  92                                 facesA[hullA->m_faceOffset + face].m_plane.z, 0.f);
  93                         const b3Float4 faceANormalWS = b3QuatRotate(ornA, Normal);
  94
  95                         float d = b3Dot3F4(faceANormalWS, separatingNormal);
  96                         if (d < dmin)
  97                         {
  98                                 dmin = d;
  99                                 closestFaceA = face;
 100                         }
 101                 }
 102         }
 103         if (closestFaceA < 0)
 104                 return numContactsOut;
 105
 106         b3GpuFace polyA = facesA[hullA->m_faceOffset + closestFaceA];
 107
 108         // clip polygon to back of planes of all faces of hull A that are adjacent to witness face
 109         //int numContacts = numWorldVertsB1;
 110         int numVerticesA = polyA.m_numIndices;
 111         for (int e0 = 0; e0 < numVerticesA; e0++)
 112         {
 113                 const b3Float4 a = verticesA[hullA->m_vertexOffset + indicesA[polyA.m_indexOffset + e0]];
 114                 const b3Float4 b = verticesA[hullA->m_vertexOffset + indicesA[polyA.m_indexOffset + ((e0 + 1) % numVerticesA)]];
 115                 const b3Float4 edge0 = a - b;
 116                 const b3Float4 WorldEdge0 = b3QuatRotate(ornA, edge0);
 117                 b3Float4 planeNormalA = b3MakeFloat4(polyA.m_plane.x, polyA.m_plane.y, polyA.m_plane.z, 0.f);
 118                 b3Float4 worldPlaneAnormal1 = b3QuatRotate(ornA, planeNormalA);
 119
 120                 b3Float4 planeNormalWS1 = -b3Cross3(WorldEdge0, worldPlaneAnormal1);
 121                 b3Float4 worldA1 = b3TransformPoint(a, posA, ornA);
 122                 float planeEqWS1 = -b3Dot3F4(worldA1, planeNormalWS1);
 123
 124                 b3Float4 planeNormalWS = planeNormalWS1;
 125                 float planeEqWS = planeEqWS1;
 126
 127                 //clip face
 128                 //clipFace(*pVtxIn, *pVtxOut,planeNormalWS,planeEqWS);
 129                 numVertsOut = b3ClipFace(pVtxIn, numVertsIn, planeNormalWS, planeEqWS, pVtxOut);
 130
 131                 //btSwap(pVtxIn,pVtxOut);
 132                 b3Float4* tmp = pVtxOut;
 133                 pVtxOut = pVtxIn;
 134                 pVtxIn = tmp;
 135                 numVertsIn = numVertsOut;
 136                 numVertsOut = 0;
 137         }
 138
 139         // only keep points that are behind the witness face
 140         {
 141                 b3Float4 localPlaneNormal = b3MakeFloat4(polyA.m_plane.x, polyA.m_plane.y, polyA.m_plane.z, 0.f);
 142                 float localPlaneEq = polyA.m_plane.w;
 143                 b3Float4 planeNormalWS = b3QuatRotate(ornA, localPlaneNormal);
 144                 float planeEqWS = localPlaneEq - b3Dot3F4(planeNormalWS, posA);
 145                 for (int i = 0; i < numVertsIn; i++)
 146                 {
 147                         float depth = b3Dot3F4(planeNormalWS, pVtxIn[i]) + planeEqWS;
 148                         if (depth <= minDist)
 149                         {
 150                                 depth = minDist;
 151                         }
 152                         if (numContactsOut < contactCapacity)
 153                         {
 154                                 if (depth <= maxDist)
 155                                 {
 156                                         b3Float4 pointInWorld = pVtxIn[i];
 157                                         //resultOut.addContactPoint(separatingNormal,point,depth);
 158                                         contactsOut[numContactsOut++] = b3MakeVector3(pointInWorld.x, pointInWorld.y, pointInWorld.z, depth);
 159                                         //printf("depth=%f\n",depth);
 160                                 }
 161                         }
 162                         else
 163                         {
 164                                 b3Error("exceeding contact capacity (%d,%df)\n", numContactsOut, contactCapacity);
 165                         }
 166                 }
 167         }
 168
 169         return numContactsOut;
 170 }
 171
 172 inline int b3ClipHullAgainstHull(const b3Float4& separatingNormal,
 173                                                                  const b3ConvexPolyhedronData& hullA, const b3ConvexPolyhedronData& hullB,
 174                                                                  const b3Float4& posA, const b3Quaternion& ornA, const b3Float4& posB, const b3Quaternion& ornB,
 175                                                                  b3Float4* worldVertsB1, b3Float4* worldVertsB2, int capacityWorldVerts,
 176                                                                  const float minDist, float maxDist,
 177                                                                  const b3AlignedObjectArray<b3Float4>& verticesA, const b3AlignedObjectArray<b3GpuFace>& facesA, const b3AlignedObjectArray<int>& indicesA,
 178                                                                  const b3AlignedObjectArray<b3Float4>& verticesB, const b3AlignedObjectArray<b3GpuFace>& facesB, const b3AlignedObjectArray<int>& indicesB,
 179
 180                                                                  b3Float4* contactsOut,
 181                                                                  int contactCapacity)
 182 {
 183         int numContactsOut = 0;
 184         int numWorldVertsB1 = 0;
 185
 186         B3_PROFILE("clipHullAgainstHull");
 187
 188         //float curMaxDist=maxDist;
 189         int closestFaceB = -1;
 190         float dmax = -FLT_MAX;
 191
 192         {
 193                 //B3_PROFILE("closestFaceB");
 194                 if (hullB.m_numFaces != 1)
 195                 {
 196                         //printf("wtf\n");
 197                 }
 198                 static bool once = true;
 199                 //printf("separatingNormal=%f,%f,%f\n",separatingNormal.x,separatingNormal.y,separatingNormal.z);
 200
 201                 for (int face = 0; face < hullB.m_numFaces; face++)
 202                 {
 203 #ifdef BT_DEBUG_SAT_FACE
 204                         if (once)
 205                                 printf("face %d\n", face);
 206                         const b3GpuFace* faceB = &facesB[hullB.m_faceOffset + face];
 207                         if (once)
 208                         {
 209                                 for (int i = 0; i < faceB->m_numIndices; i++)
 210                                 {
 211                                         b3Float4 vert = verticesB[hullB.m_vertexOffset + indicesB[faceB->m_indexOffset + i]];
 212                                         printf("vert[%d] = %f,%f,%f\n", i, vert.x, vert.y, vert.z);
 213                                 }
 214                         }
 215 #endif  //BT_DEBUG_SAT_FACE \
 216         //if (facesB[hullB.m_faceOffset+face].m_numIndices>2)
 217                         {
 218                                 const b3Float4 Normal = b3MakeVector3(facesB[hullB.m_faceOffset + face].m_plane.x,
 219                                                                                                           facesB[hullB.m_faceOffset + face].m_plane.y, facesB[hullB.m_faceOffset + face].m_plane.z, 0.f);
 220                                 const b3Float4 WorldNormal = b3QuatRotate(ornB, Normal);
 221 #ifdef BT_DEBUG_SAT_FACE
 222                                 if (once)
 223                                         printf("faceNormal = %f,%f,%f\n", Normal.x, Normal.y, Normal.z);
 224 #endif
 225                                 float d = b3Dot3F4(WorldNormal, separatingNormal);
 226                                 if (d > dmax)
 227                                 {
 228                                         dmax = d;
 229                                         closestFaceB = face;
 230                                 }
 231                         }
 232                 }
 233                 once = false;
 234         }
 235
 236         b3Assert(closestFaceB >= 0);
 237         {
 238                 //B3_PROFILE("worldVertsB1");
 239                 const b3GpuFace& polyB = facesB[hullB.m_faceOffset + closestFaceB];
 240                 const int numVertices = polyB.m_numIndices;
 241                 for (int e0 = 0; e0 < numVertices; e0++)
 242                 {
 243                         const b3Float4& b = verticesB[hullB.m_vertexOffset + indicesB[polyB.m_indexOffset + e0]];
 244                         worldVertsB1[numWorldVertsB1++] = b3TransformPoint(b, posB, ornB);
 245                 }
 246         }
 247
 248         if (closestFaceB >= 0)
 249         {
 250                 //B3_PROFILE("clipFaceAgainstHull");
 251                 numContactsOut = b3ClipFaceAgainstHull((b3Float4&)separatingNormal, &hullA,
 252                                                                                            posA, ornA,
 253                                                                                            worldVertsB1, numWorldVertsB1, worldVertsB2, capacityWorldVerts, minDist, maxDist,
 254                                                                                            verticesA, facesA, indicesA,
 255                                                                                            contactsOut, contactCapacity);
 256         }
 257
 258         return numContactsOut;
 259 }
 260
 261 inline int b3ClipHullHullSingle(
 262         int bodyIndexA, int bodyIndexB,
 263         const b3Float4& posA,
 264         const b3Quaternion& ornA,
 265         const b3Float4& posB,
 266         const b3Quaternion& ornB,
 267
 268         int collidableIndexA, int collidableIndexB,
 269
 270         const b3AlignedObjectArray<b3RigidBodyData>* bodyBuf,
 271         b3AlignedObjectArray<b3Contact4Data>* globalContactOut,
 272         int& nContacts,
 273
 274         const b3AlignedObjectArray<b3ConvexPolyhedronData>& hostConvexDataA,
 275         const b3AlignedObjectArray<b3ConvexPolyhedronData>& hostConvexDataB,
 276
 277         const b3AlignedObjectArray<b3Vector3>& verticesA,
 278         const b3AlignedObjectArray<b3Vector3>& uniqueEdgesA,
 279         const b3AlignedObjectArray<b3GpuFace>& facesA,
 280         const b3AlignedObjectArray<int>& indicesA,
 281
 282         const b3AlignedObjectArray<b3Vector3>& verticesB,
 283         const b3AlignedObjectArray<b3Vector3>& uniqueEdgesB,
 284         const b3AlignedObjectArray<b3GpuFace>& facesB,
 285         const b3AlignedObjectArray<int>& indicesB,
 286
 287         const b3AlignedObjectArray<b3Collidable>& hostCollidablesA,
 288         const b3AlignedObjectArray<b3Collidable>& hostCollidablesB,
 289         const b3Vector3& sepNormalWorldSpace,
 290         int maxContactCapacity)
 291 {
 292         int contactIndex = -1;
 293         b3ConvexPolyhedronData hullA, hullB;
 294
 295         b3Collidable colA = hostCollidablesA[collidableIndexA];
 296         hullA = hostConvexDataA[colA.m_shapeIndex];
 297         //printf("numvertsA = %d\n",hullA.m_numVertices);
 298
 299         b3Collidable colB = hostCollidablesB[collidableIndexB];
 300         hullB = hostConvexDataB[colB.m_shapeIndex];
 301         //printf("numvertsB = %d\n",hullB.m_numVertices);
 302
 303         b3Float4 contactsOut[B3_MAX_VERTS];
 304         int localContactCapacity = B3_MAX_VERTS;
 305
 306 #ifdef _WIN32
 307         b3Assert(_finite(bodyBuf->at(bodyIndexA).m_pos.x));
 308         b3Assert(_finite(bodyBuf->at(bodyIndexB).m_pos.x));
 309 #endif
 310
 311         {
 312                 b3Float4 worldVertsB1[B3_MAX_VERTS];
 313                 b3Float4 worldVertsB2[B3_MAX_VERTS];
 314                 int capacityWorldVerts = B3_MAX_VERTS;
 315
 316                 b3Float4 hostNormal = b3MakeFloat4(sepNormalWorldSpace.x, sepNormalWorldSpace.y, sepNormalWorldSpace.z, 0.f);
 317                 int shapeA = hostCollidablesA[collidableIndexA].m_shapeIndex;
 318                 int shapeB = hostCollidablesB[collidableIndexB].m_shapeIndex;
 319
 320                 b3Scalar minDist = -1;
 321                 b3Scalar maxDist = 0.;
 322
 323                 b3Transform trA, trB;
 324                 {
 325                         //B3_PROFILE("b3TransformPoint computation");
 326                         //trA.setIdentity();
 327                         trA.setOrigin(b3MakeVector3(posA.x, posA.y, posA.z));
 328                         trA.setRotation(b3Quaternion(ornA.x, ornA.y, ornA.z, ornA.w));
 329
 330                         //trB.setIdentity();
 331                         trB.setOrigin(b3MakeVector3(posB.x, posB.y, posB.z));
 332                         trB.setRotation(b3Quaternion(ornB.x, ornB.y, ornB.z, ornB.w));
 333                 }
 334
 335                 b3Quaternion trAorn = trA.getRotation();
 336                 b3Quaternion trBorn = trB.getRotation();
 337
 338                 int numContactsOut = b3ClipHullAgainstHull(hostNormal,
 339                                                                                                    hostConvexDataA.at(shapeA),
 340                                                                                                    hostConvexDataB.at(shapeB),
 341                                                                                                    (b3Float4&)trA.getOrigin(), (b3Quaternion&)trAorn,
 342                                                                                                    (b3Float4&)trB.getOrigin(), (b3Quaternion&)trBorn,
 343                                                                                                    worldVertsB1, worldVertsB2, capacityWorldVerts,
 344                                                                                                    minDist, maxDist,
 345                                                                                                    verticesA, facesA, indicesA,
 346                                                                                                    verticesB, facesB, indicesB,
 347
 348                                                                                                    contactsOut, localContactCapacity);
 349
 350                 if (numContactsOut > 0)
 351                 {
 352                         B3_PROFILE("overlap");
 353
 354                         b3Float4 normalOnSurfaceB = (b3Float4&)hostNormal;
 355                         //                      b3Float4 centerOut;
 356
 357                         b3Int4 contactIdx;
 358                         contactIdx.x = 0;
 359                         contactIdx.y = 1;
 360                         contactIdx.z = 2;
 361                         contactIdx.w = 3;
 362
 363                         int numPoints = 0;
 364
 365                         {
 366                                 B3_PROFILE("extractManifold");
 367                                 numPoints = b3ReduceContacts(contactsOut, numContactsOut, normalOnSurfaceB, &contactIdx);
 368                         }
 369
 370                         b3Assert(numPoints);
 371
 372                         if (nContacts < maxContactCapacity)
 373                         {
 374                                 contactIndex = nContacts;
 375                                 globalContactOut->expand();
 376                                 b3Contact4Data& contact = globalContactOut->at(nContacts);
 377                                 contact.m_batchIdx = 0;  //i;
 378                                 contact.m_bodyAPtrAndSignBit = (bodyBuf->at(bodyIndexA).m_invMass == 0) ? -bodyIndexA : bodyIndexA;
 379                                 contact.m_bodyBPtrAndSignBit = (bodyBuf->at(bodyIndexB).m_invMass == 0) ? -bodyIndexB : bodyIndexB;
 380
 381                                 contact.m_frictionCoeffCmp = 45874;
 382                                 contact.m_restituitionCoeffCmp = 0;
 383
 384                                 //      float distance = 0.f;
 385                                 for (int p = 0; p < numPoints; p++)
 386                                 {
 387                                         contact.m_worldPosB[p] = contactsOut[contactIdx.s[p]];  //check if it is actually on B
 388                                         contact.m_worldNormalOnB = normalOnSurfaceB;
 389                                 }
 390                                 //printf("bodyIndexA %d,bodyIndexB %d,normal=%f,%f,%f numPoints %d\n",bodyIndexA,bodyIndexB,normalOnSurfaceB.x,normalOnSurfaceB.y,normalOnSurfaceB.z,numPoints);
 391                                 contact.m_worldNormalOnB.w = (b3Scalar)numPoints;
 392                                 nContacts++;
 393                         }
 394                         else
 395                         {
 396                                 b3Error("Error: exceeding contact capacity (%d/%d)\n", nContacts, maxContactCapacity);
 397                         }
 398                 }
 399         }
 400         return contactIndex;
 401 }
 402
 403 inline int b3ContactConvexConvexSAT(
 404         int pairIndex,
 405         int bodyIndexA, int bodyIndexB,
 406         int collidableIndexA, int collidableIndexB,
 407         const b3AlignedObjectArray<b3RigidBodyData>& rigidBodies,
 408         const b3AlignedObjectArray<b3Collidable>& collidables,
 409         const b3AlignedObjectArray<b3ConvexPolyhedronData>& convexShapes,
 410         const b3AlignedObjectArray<b3Float4>& convexVertices,
 411         const b3AlignedObjectArray<b3Float4>& uniqueEdges,
 412         const b3AlignedObjectArray<int>& convexIndices,
 413         const b3AlignedObjectArray<b3GpuFace>& faces,
 414         b3AlignedObjectArray<b3Contact4Data>& globalContactsOut,
 415         int& nGlobalContactsOut,
 416         int maxContactCapacity)
 417 {
 418         int contactIndex = -1;
 419
 420         b3Float4 posA = rigidBodies[bodyIndexA].m_pos;
 421         b3Quaternion ornA = rigidBodies[bodyIndexA].m_quat;
 422         b3Float4 posB = rigidBodies[bodyIndexB].m_pos;
 423         b3Quaternion ornB = rigidBodies[bodyIndexB].m_quat;
 424
 425         b3ConvexPolyhedronData hullA, hullB;
 426
 427         b3Float4 sepNormalWorldSpace;
 428
 429         b3Collidable colA = collidables[collidableIndexA];
 430         hullA = convexShapes[colA.m_shapeIndex];
 431         //printf("numvertsA = %d\n",hullA.m_numVertices);
 432
 433         b3Collidable colB = collidables[collidableIndexB];
 434         hullB = convexShapes[colB.m_shapeIndex];
 435         //printf("numvertsB = %d\n",hullB.m_numVertices);
 436
 437 #ifdef _WIN32
 438         b3Assert(_finite(rigidBodies[bodyIndexA].m_pos.x));
 439         b3Assert(_finite(rigidBodies[bodyIndexB].m_pos.x));
 440 #endif
 441
 442         bool foundSepAxis = b3FindSeparatingAxis(hullA, hullB,
 443                                                                                          posA,
 444                                                                                          ornA,
 445                                                                                          posB,
 446                                                                                          ornB,
 447
 448                                                                                          convexVertices, uniqueEdges, faces, convexIndices,
 449                                                                                          convexVertices, uniqueEdges, faces, convexIndices,
 450
 451                                                                                          sepNormalWorldSpace);
 452
 453         if (foundSepAxis)
 454         {
 455                 contactIndex = b3ClipHullHullSingle(
 456                         bodyIndexA, bodyIndexB,
 457                         posA, ornA,
 458                         posB, ornB,
 459                         collidableIndexA, collidableIndexB,
 460                         &rigidBodies,
 461                         &globalContactsOut,
 462                         nGlobalContactsOut,
 463
 464                         convexShapes,
 465                         convexShapes,
 466
 467                         convexVertices,
 468                         uniqueEdges,
 469                         faces,
 470                         convexIndices,
 471
 472                         convexVertices,
 473                         uniqueEdges,
 474                         faces,
 475                         convexIndices,
 476
 477                         collidables,
 478                         collidables,
 479                         sepNormalWorldSpace,
 480                         maxContactCapacity);
 481         }
 482
 483         return contactIndex;
 484 }
 485
 486 #endif  //B3_CONTACT_CONVEX_CONVEX_SAT_H