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