dali-physics/third-party/bullet3/src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.cpp

   1 /*
   2 Bullet Continuous Collision Detection and Physics Library
   3 Copyright (c) 2011 Advanced Micro Devices, Inc.  http://bulletphysics.org
   4
   5 This software is provided 'as-is', without any express or implied warranty.
   6 In no event will the authors be held liable for any damages arising from the use of this software.
   7 Permission is granted to anyone to use this software for any purpose,
   8 including commercial applications, and to alter it and redistribute it freely,
   9 subject to the following restrictions:
  10
  11 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
  12 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
  13 3. This notice may not be removed or altered from any source distribution.
  14 */
  15
  16 ///This file was written by Erwin Coumans
  17 ///Separating axis rest based on work from Pierre Terdiman, see
  18 ///And contact clipping based on work from Simon Hobbs
  19
  20 #include "btPolyhedralContactClipping.h"
  21 #include "BulletCollision/CollisionShapes/btConvexPolyhedron.h"
  22
  23 #include <float.h>  //for FLT_MAX
  24
  25 int gExpectedNbTests = 0;
  26 int gActualNbTests = 0;
  27 bool gUseInternalObject = true;
  28
  29 // Clips a face to the back of a plane
  30 void btPolyhedralContactClipping::clipFace(const btVertexArray& pVtxIn, btVertexArray& ppVtxOut, const btVector3& planeNormalWS, btScalar planeEqWS)
  31 {
  32         int ve;
  33         btScalar ds, de;
  34         int numVerts = pVtxIn.size();
  35         if (numVerts < 2)
  36                 return;
  37
  38         btVector3 firstVertex = pVtxIn[pVtxIn.size() - 1];
  39         btVector3 endVertex = pVtxIn[0];
  40
  41         ds = planeNormalWS.dot(firstVertex) + planeEqWS;
  42
  43         for (ve = 0; ve < numVerts; ve++)
  44         {
  45                 endVertex = pVtxIn[ve];
  46
  47                 de = planeNormalWS.dot(endVertex) + planeEqWS;
  48
  49                 if (ds < 0)
  50                 {
  51                         if (de < 0)
  52                         {
  53                                 // Start < 0, end < 0, so output endVertex
  54                                 ppVtxOut.push_back(endVertex);
  55                         }
  56                         else
  57                         {
  58                                 // Start < 0, end >= 0, so output intersection
  59                                 ppVtxOut.push_back(firstVertex.lerp(endVertex, btScalar(ds * 1.f / (ds - de))));
  60                         }
  61                 }
  62                 else
  63                 {
  64                         if (de < 0)
  65                         {
  66                                 // Start >= 0, end < 0 so output intersection and end
  67                                 ppVtxOut.push_back(firstVertex.lerp(endVertex, btScalar(ds * 1.f / (ds - de))));
  68                                 ppVtxOut.push_back(endVertex);
  69                         }
  70                 }
  71                 firstVertex = endVertex;
  72                 ds = de;
  73         }
  74 }
  75
  76 static bool TestSepAxis(const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA, const btTransform& transB, const btVector3& sep_axis, btScalar& depth, btVector3& witnessPointA, btVector3& witnessPointB)
  77 {
  78         btScalar Min0, Max0;
  79         btScalar Min1, Max1;
  80         btVector3 witnesPtMinA, witnesPtMaxA;
  81         btVector3 witnesPtMinB, witnesPtMaxB;
  82
  83         hullA.project(transA, sep_axis, Min0, Max0, witnesPtMinA, witnesPtMaxA);
  84         hullB.project(transB, sep_axis, Min1, Max1, witnesPtMinB, witnesPtMaxB);
  85
  86         if (Max0 < Min1 || Max1 < Min0)
  87                 return false;
  88
  89         btScalar d0 = Max0 - Min1;
  90         btAssert(d0 >= 0.0f);
  91         btScalar d1 = Max1 - Min0;
  92         btAssert(d1 >= 0.0f);
  93         if (d0 < d1)
  94         {
  95                 depth = d0;
  96                 witnessPointA = witnesPtMaxA;
  97                 witnessPointB = witnesPtMinB;
  98         }
  99         else
 100         {
 101                 depth = d1;
 102                 witnessPointA = witnesPtMinA;
 103                 witnessPointB = witnesPtMaxB;
 104         }
 105
 106         return true;
 107 }
 108
 109 static int gActualSATPairTests = 0;
 110
 111 inline bool IsAlmostZero(const btVector3& v)
 112 {
 113         if (btFabs(v.x()) > 1e-6 || btFabs(v.y()) > 1e-6 || btFabs(v.z()) > 1e-6) return false;
 114         return true;
 115 }
 116
 117 #ifdef TEST_INTERNAL_OBJECTS
 118
 119 inline void BoxSupport(const btScalar extents[3], const btScalar sv[3], btScalar p[3])
 120 {
 121         // This version is ~11.000 cycles (4%) faster overall in one of the tests.
 122         //      IR(p[0]) = IR(extents[0])|(IR(sv[0])&SIGN_BITMASK);
 123         //      IR(p[1]) = IR(extents[1])|(IR(sv[1])&SIGN_BITMASK);
 124         //      IR(p[2]) = IR(extents[2])|(IR(sv[2])&SIGN_BITMASK);
 125         p[0] = sv[0] < 0.0f ? -extents[0] : extents[0];
 126         p[1] = sv[1] < 0.0f ? -extents[1] : extents[1];
 127         p[2] = sv[2] < 0.0f ? -extents[2] : extents[2];
 128 }
 129
 130 void InverseTransformPoint3x3(btVector3& out, const btVector3& in, const btTransform& tr)
 131 {
 132         const btMatrix3x3& rot = tr.getBasis();
 133         const btVector3& r0 = rot[0];
 134         const btVector3& r1 = rot[1];
 135         const btVector3& r2 = rot[2];
 136
 137         const btScalar x = r0.x() * in.x() + r1.x() * in.y() + r2.x() * in.z();
 138         const btScalar y = r0.y() * in.x() + r1.y() * in.y() + r2.y() * in.z();
 139         const btScalar z = r0.z() * in.x() + r1.z() * in.y() + r2.z() * in.z();
 140
 141         out.setValue(x, y, z);
 142 }
 143
 144 bool TestInternalObjects(const btTransform& trans0, const btTransform& trans1, const btVector3& delta_c, const btVector3& axis, const btConvexPolyhedron& convex0, const btConvexPolyhedron& convex1, btScalar dmin)
 145 {
 146         const btScalar dp = delta_c.dot(axis);
 147
 148         btVector3 localAxis0;
 149         InverseTransformPoint3x3(localAxis0, axis, trans0);
 150         btVector3 localAxis1;
 151         InverseTransformPoint3x3(localAxis1, axis, trans1);
 152
 153         btScalar p0[3];
 154         BoxSupport(convex0.m_extents, localAxis0, p0);
 155         btScalar p1[3];
 156         BoxSupport(convex1.m_extents, localAxis1, p1);
 157
 158         const btScalar Radius0 = p0[0] * localAxis0.x() + p0[1] * localAxis0.y() + p0[2] * localAxis0.z();
 159         const btScalar Radius1 = p1[0] * localAxis1.x() + p1[1] * localAxis1.y() + p1[2] * localAxis1.z();
 160
 161         const btScalar MinRadius = Radius0 > convex0.m_radius ? Radius0 : convex0.m_radius;
 162         const btScalar MaxRadius = Radius1 > convex1.m_radius ? Radius1 : convex1.m_radius;
 163
 164         const btScalar MinMaxRadius = MaxRadius + MinRadius;
 165         const btScalar d0 = MinMaxRadius + dp;
 166         const btScalar d1 = MinMaxRadius - dp;
 167
 168         const btScalar depth = d0 < d1 ? d0 : d1;
 169         if (depth > dmin)
 170                 return false;
 171         return true;
 172 }
 173 #endif  //TEST_INTERNAL_OBJECTS
 174
 175 SIMD_FORCE_INLINE void btSegmentsClosestPoints(
 176         btVector3& ptsVector,
 177         btVector3& offsetA,
 178         btVector3& offsetB,
 179         btScalar& tA, btScalar& tB,
 180         const btVector3& translation,
 181         const btVector3& dirA, btScalar hlenA,
 182         const btVector3& dirB, btScalar hlenB)
 183 {
 184         // compute the parameters of the closest points on each line segment
 185
 186         btScalar dirA_dot_dirB = btDot(dirA, dirB);
 187         btScalar dirA_dot_trans = btDot(dirA, translation);
 188         btScalar dirB_dot_trans = btDot(dirB, translation);
 189
 190         btScalar denom = 1.0f - dirA_dot_dirB * dirA_dot_dirB;
 191
 192         if (denom == 0.0f)
 193         {
 194                 tA = 0.0f;
 195         }
 196         else
 197         {
 198                 tA = (dirA_dot_trans - dirB_dot_trans * dirA_dot_dirB) / denom;
 199                 if (tA < -hlenA)
 200                         tA = -hlenA;
 201                 else if (tA > hlenA)
 202                         tA = hlenA;
 203         }
 204
 205         tB = tA * dirA_dot_dirB - dirB_dot_trans;
 206
 207         if (tB < -hlenB)
 208         {
 209                 tB = -hlenB;
 210                 tA = tB * dirA_dot_dirB + dirA_dot_trans;
 211
 212                 if (tA < -hlenA)
 213                         tA = -hlenA;
 214                 else if (tA > hlenA)
 215                         tA = hlenA;
 216         }
 217         else if (tB > hlenB)
 218         {
 219                 tB = hlenB;
 220                 tA = tB * dirA_dot_dirB + dirA_dot_trans;
 221
 222                 if (tA < -hlenA)
 223                         tA = -hlenA;
 224                 else if (tA > hlenA)
 225                         tA = hlenA;
 226         }
 227
 228         // compute the closest points relative to segment centers.
 229
 230         offsetA = dirA * tA;
 231         offsetB = dirB * tB;
 232
 233         ptsVector = translation - offsetA + offsetB;
 234 }
 235
 236 bool btPolyhedralContactClipping::findSeparatingAxis(const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA, const btTransform& transB, btVector3& sep, btDiscreteCollisionDetectorInterface::Result& resultOut)
 237 {
 238         gActualSATPairTests++;
 239
 240         //#ifdef TEST_INTERNAL_OBJECTS
 241         const btVector3 c0 = transA * hullA.m_localCenter;
 242         const btVector3 c1 = transB * hullB.m_localCenter;
 243         const btVector3 DeltaC2 = c0 - c1;
 244         //#endif
 245
 246         btScalar dmin = FLT_MAX;
 247         int curPlaneTests = 0;
 248
 249         int numFacesA = hullA.m_faces.size();
 250         // Test normals from hullA
 251         for (int i = 0; i < numFacesA; i++)
 252         {
 253                 const btVector3 Normal(hullA.m_faces[i].m_plane[0], hullA.m_faces[i].m_plane[1], hullA.m_faces[i].m_plane[2]);
 254                 btVector3 faceANormalWS = transA.getBasis() * Normal;
 255                 if (DeltaC2.dot(faceANormalWS) < 0)
 256                         faceANormalWS *= -1.f;
 257
 258                 curPlaneTests++;
 259 #ifdef TEST_INTERNAL_OBJECTS
 260                 gExpectedNbTests++;
 261                 if (gUseInternalObject && !TestInternalObjects(transA, transB, DeltaC2, faceANormalWS, hullA, hullB, dmin))
 262                         continue;
 263                 gActualNbTests++;
 264 #endif
 265
 266                 btScalar d;
 267                 btVector3 wA, wB;
 268                 if (!TestSepAxis(hullA, hullB, transA, transB, faceANormalWS, d, wA, wB))
 269                         return false;
 270
 271                 if (d < dmin)
 272                 {
 273                         dmin = d;
 274                         sep = faceANormalWS;
 275                 }
 276         }
 277
 278         int numFacesB = hullB.m_faces.size();
 279         // Test normals from hullB
 280         for (int i = 0; i < numFacesB; i++)
 281         {
 282                 const btVector3 Normal(hullB.m_faces[i].m_plane[0], hullB.m_faces[i].m_plane[1], hullB.m_faces[i].m_plane[2]);
 283                 btVector3 WorldNormal = transB.getBasis() * Normal;
 284                 if (DeltaC2.dot(WorldNormal) < 0)
 285                         WorldNormal *= -1.f;
 286
 287                 curPlaneTests++;
 288 #ifdef TEST_INTERNAL_OBJECTS
 289                 gExpectedNbTests++;
 290                 if (gUseInternalObject && !TestInternalObjects(transA, transB, DeltaC2, WorldNormal, hullA, hullB, dmin))
 291                         continue;
 292                 gActualNbTests++;
 293 #endif
 294
 295                 btScalar d;
 296                 btVector3 wA, wB;
 297                 if (!TestSepAxis(hullA, hullB, transA, transB, WorldNormal, d, wA, wB))
 298                         return false;
 299
 300                 if (d < dmin)
 301                 {
 302                         dmin = d;
 303                         sep = WorldNormal;
 304                 }
 305         }
 306
 307         btVector3 edgeAstart, edgeAend, edgeBstart, edgeBend;
 308         int edgeA = -1;
 309         int edgeB = -1;
 310         btVector3 worldEdgeA;
 311         btVector3 worldEdgeB;
 312         btVector3 witnessPointA(0, 0, 0), witnessPointB(0, 0, 0);
 313
 314         int curEdgeEdge = 0;
 315         // Test edges
 316         for (int e0 = 0; e0 < hullA.m_uniqueEdges.size(); e0++)
 317         {
 318                 const btVector3 edge0 = hullA.m_uniqueEdges[e0];
 319                 const btVector3 WorldEdge0 = transA.getBasis() * edge0;
 320                 for (int e1 = 0; e1 < hullB.m_uniqueEdges.size(); e1++)
 321                 {
 322                         const btVector3 edge1 = hullB.m_uniqueEdges[e1];
 323                         const btVector3 WorldEdge1 = transB.getBasis() * edge1;
 324
 325                         btVector3 Cross = WorldEdge0.cross(WorldEdge1);
 326                         curEdgeEdge++;
 327                         if (!IsAlmostZero(Cross))
 328                         {
 329                                 Cross = Cross.normalize();
 330                                 if (DeltaC2.dot(Cross) < 0)
 331                                         Cross *= -1.f;
 332
 333 #ifdef TEST_INTERNAL_OBJECTS
 334                                 gExpectedNbTests++;
 335                                 if (gUseInternalObject && !TestInternalObjects(transA, transB, DeltaC2, Cross, hullA, hullB, dmin))
 336                                         continue;
 337                                 gActualNbTests++;
 338 #endif
 339
 340                                 btScalar dist;
 341                                 btVector3 wA, wB;
 342                                 if (!TestSepAxis(hullA, hullB, transA, transB, Cross, dist, wA, wB))
 343                                         return false;
 344
 345                                 if (dist < dmin)
 346                                 {
 347                                         dmin = dist;
 348                                         sep = Cross;
 349                                         edgeA = e0;
 350                                         edgeB = e1;
 351                                         worldEdgeA = WorldEdge0;
 352                                         worldEdgeB = WorldEdge1;
 353                                         witnessPointA = wA;
 354                                         witnessPointB = wB;
 355                                 }
 356                         }
 357                 }
 358         }
 359
 360         if (edgeA >= 0 && edgeB >= 0)
 361         {
 362                 //              printf("edge-edge\n");
 363                 //add an edge-edge contact
 364
 365                 btVector3 ptsVector;
 366                 btVector3 offsetA;
 367                 btVector3 offsetB;
 368                 btScalar tA;
 369                 btScalar tB;
 370
 371                 btVector3 translation = witnessPointB - witnessPointA;
 372
 373                 btVector3 dirA = worldEdgeA;
 374                 btVector3 dirB = worldEdgeB;
 375
 376                 btScalar hlenB = 1e30f;
 377                 btScalar hlenA = 1e30f;
 378
 379                 btSegmentsClosestPoints(ptsVector, offsetA, offsetB, tA, tB,
 380                                                                 translation,
 381                                                                 dirA, hlenA,
 382                                                                 dirB, hlenB);
 383
 384                 btScalar nlSqrt = ptsVector.length2();
 385                 if (nlSqrt > SIMD_EPSILON)
 386                 {
 387                         btScalar nl = btSqrt(nlSqrt);
 388                         ptsVector *= 1.f / nl;
 389                         if (ptsVector.dot(DeltaC2) < 0.f)
 390                         {
 391                                 ptsVector *= -1.f;
 392                         }
 393                         btVector3 ptOnB = witnessPointB + offsetB;
 394                         btScalar distance = nl;
 395                         resultOut.addContactPoint(ptsVector, ptOnB, -distance);
 396                 }
 397         }
 398
 399         if ((DeltaC2.dot(sep)) < 0.0f)
 400                 sep = -sep;
 401
 402         return true;
 403 }
 404
 405 void btPolyhedralContactClipping::clipFaceAgainstHull(const btVector3& separatingNormal, const btConvexPolyhedron& hullA, const btTransform& transA, btVertexArray& worldVertsB1, btVertexArray& worldVertsB2, const btScalar minDist, btScalar maxDist, btDiscreteCollisionDetectorInterface::Result& resultOut)
 406 {
 407         worldVertsB2.resize(0);
 408         btVertexArray* pVtxIn = &worldVertsB1;
 409         btVertexArray* pVtxOut = &worldVertsB2;
 410         pVtxOut->reserve(pVtxIn->size());
 411
 412         int closestFaceA = -1;
 413         {
 414                 btScalar dmin = FLT_MAX;
 415                 for (int face = 0; face < hullA.m_faces.size(); face++)
 416                 {
 417                         const btVector3 Normal(hullA.m_faces[face].m_plane[0], hullA.m_faces[face].m_plane[1], hullA.m_faces[face].m_plane[2]);
 418                         const btVector3 faceANormalWS = transA.getBasis() * Normal;
 419
 420                         btScalar d = faceANormalWS.dot(separatingNormal);
 421                         if (d < dmin)
 422                         {
 423                                 dmin = d;
 424                                 closestFaceA = face;
 425                         }
 426                 }
 427         }
 428         if (closestFaceA < 0)
 429                 return;
 430
 431         const btFace& polyA = hullA.m_faces[closestFaceA];
 432
 433         // clip polygon to back of planes of all faces of hull A that are adjacent to witness face
 434         int numVerticesA = polyA.m_indices.size();
 435         for (int e0 = 0; e0 < numVerticesA; e0++)
 436         {
 437                 const btVector3& a = hullA.m_vertices[polyA.m_indices[e0]];
 438                 const btVector3& b = hullA.m_vertices[polyA.m_indices[(e0 + 1) % numVerticesA]];
 439                 const btVector3 edge0 = a - b;
 440                 const btVector3 WorldEdge0 = transA.getBasis() * edge0;
 441                 btVector3 worldPlaneAnormal1 = transA.getBasis() * btVector3(polyA.m_plane[0], polyA.m_plane[1], polyA.m_plane[2]);
 442
 443                 btVector3 planeNormalWS1 = -WorldEdge0.cross(worldPlaneAnormal1);  //.cross(WorldEdge0);
 444                 btVector3 worldA1 = transA * a;
 445                 btScalar planeEqWS1 = -worldA1.dot(planeNormalWS1);
 446
 447 //int otherFace=0;
 448 #ifdef BLA1
 449                 int otherFace = polyA.m_connectedFaces[e0];
 450                 btVector3 localPlaneNormal(hullA.m_faces[otherFace].m_plane[0], hullA.m_faces[otherFace].m_plane[1], hullA.m_faces[otherFace].m_plane[2]);
 451                 btScalar localPlaneEq = hullA.m_faces[otherFace].m_plane[3];
 452
 453                 btVector3 planeNormalWS = transA.getBasis() * localPlaneNormal;
 454                 btScalar planeEqWS = localPlaneEq - planeNormalWS.dot(transA.getOrigin());
 455 #else
 456                 btVector3 planeNormalWS = planeNormalWS1;
 457                 btScalar planeEqWS = planeEqWS1;
 458
 459 #endif
 460                 //clip face
 461
 462                 clipFace(*pVtxIn, *pVtxOut, planeNormalWS, planeEqWS);
 463                 btSwap(pVtxIn, pVtxOut);
 464                 pVtxOut->resize(0);
 465         }
 466
 467         //#define ONLY_REPORT_DEEPEST_POINT
 468
 469         btVector3 point;
 470
 471         // only keep points that are behind the witness face
 472         {
 473                 btVector3 localPlaneNormal(polyA.m_plane[0], polyA.m_plane[1], polyA.m_plane[2]);
 474                 btScalar localPlaneEq = polyA.m_plane[3];
 475                 btVector3 planeNormalWS = transA.getBasis() * localPlaneNormal;
 476                 btScalar planeEqWS = localPlaneEq - planeNormalWS.dot(transA.getOrigin());
 477                 for (int i = 0; i < pVtxIn->size(); i++)
 478                 {
 479                         btVector3 vtx = pVtxIn->at(i);
 480                         btScalar depth = planeNormalWS.dot(vtx) + planeEqWS;
 481                         if (depth <= minDist)
 482                         {
 483                                 //                              printf("clamped: depth=%f to minDist=%f\n",depth,minDist);
 484                                 depth = minDist;
 485                         }
 486
 487                         if (depth <= maxDist)
 488                         {
 489                                 btVector3 point = pVtxIn->at(i);
 490 #ifdef ONLY_REPORT_DEEPEST_POINT
 491                                 curMaxDist = depth;
 492 #else
 493 #if 0
 494                                 if (depth<-3)
 495                                 {
 496                                         printf("error in btPolyhedralContactClipping depth = %f\n", depth);
 497                                         printf("likely wrong separatingNormal passed in\n");
 498                                 }
 499 #endif
 500                                 resultOut.addContactPoint(separatingNormal, point, depth);
 501 #endif
 502                         }
 503                 }
 504         }
 505 #ifdef ONLY_REPORT_DEEPEST_POINT
 506         if (curMaxDist < maxDist)
 507         {
 508                 resultOut.addContactPoint(separatingNormal, point, curMaxDist);
 509         }
 510 #endif  //ONLY_REPORT_DEEPEST_POINT
 511 }
 512
 513 void btPolyhedralContactClipping::clipHullAgainstHull(const btVector3& separatingNormal1, const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA, const btTransform& transB, const btScalar minDist, btScalar maxDist, btVertexArray& worldVertsB1, btVertexArray& worldVertsB2, btDiscreteCollisionDetectorInterface::Result& resultOut)
 514 {
 515         btVector3 separatingNormal = separatingNormal1.normalized();
 516         //      const btVector3 c0 = transA * hullA.m_localCenter;
 517         //      const btVector3 c1 = transB * hullB.m_localCenter;
 518         //const btVector3 DeltaC2 = c0 - c1;
 519
 520         int closestFaceB = -1;
 521         btScalar dmax = -FLT_MAX;
 522         {
 523                 for (int face = 0; face < hullB.m_faces.size(); face++)
 524                 {
 525                         const btVector3 Normal(hullB.m_faces[face].m_plane[0], hullB.m_faces[face].m_plane[1], hullB.m_faces[face].m_plane[2]);
 526                         const btVector3 WorldNormal = transB.getBasis() * Normal;
 527                         btScalar d = WorldNormal.dot(separatingNormal);
 528                         if (d > dmax)
 529                         {
 530                                 dmax = d;
 531                                 closestFaceB = face;
 532                         }
 533                 }
 534         }
 535         worldVertsB1.resize(0);
 536         {
 537                 const btFace& polyB = hullB.m_faces[closestFaceB];
 538                 const int numVertices = polyB.m_indices.size();
 539                 for (int e0 = 0; e0 < numVertices; e0++)
 540                 {
 541                         const btVector3& b = hullB.m_vertices[polyB.m_indices[e0]];
 542                         worldVertsB1.push_back(transB * b);
 543                 }
 544         }
 545
 546         if (closestFaceB >= 0)
 547                 clipFaceAgainstHull(separatingNormal, hullA, transA, worldVertsB1, worldVertsB2, minDist, maxDist, resultOut);
 548 }