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 182 183 SIMD_FORCE_INLINE void btSegmentsClosestPoints( 184 btVector3& ptsVector, 185 btVector3& offsetA, 186 btVector3& offsetB, 187 btScalar& tA, btScalar& tB, 188 const btVector3& translation, 189 const btVector3& dirA, btScalar hlenA, 190 const btVector3& dirB, btScalar hlenB ) 191 { 192 // compute the parameters of the closest points on each line segment 193 194 btScalar dirA_dot_dirB = btDot(dirA,dirB); 195 btScalar dirA_dot_trans = btDot(dirA,translation); 196 btScalar dirB_dot_trans = btDot(dirB,translation); 197 198 btScalar denom = 1.0f - dirA_dot_dirB * dirA_dot_dirB; 199 200 if ( denom == 0.0f ) { 201 tA = 0.0f; 202 } else { 203 tA = ( dirA_dot_trans - dirB_dot_trans * dirA_dot_dirB ) / denom; 204 if ( tA < -hlenA ) 205 tA = -hlenA; 206 else if ( tA > hlenA ) 207 tA = hlenA; 208 } 209 210 tB = tA * dirA_dot_dirB - dirB_dot_trans; 211 212 if ( tB < -hlenB ) { 213 tB = -hlenB; 214 tA = tB * dirA_dot_dirB + dirA_dot_trans; 215 216 if ( tA < -hlenA ) 217 tA = -hlenA; 218 else if ( tA > hlenA ) 219 tA = hlenA; 220 } else if ( tB > hlenB ) { 221 tB = hlenB; 222 tA = tB * dirA_dot_dirB + dirA_dot_trans; 223 224 if ( tA < -hlenA ) 225 tA = -hlenA; 226 else if ( tA > hlenA ) 227 tA = hlenA; 228 } 229 230 // compute the closest points relative to segment centers. 231 232 offsetA = dirA * tA; 233 offsetB = dirB * tB; 234 235 ptsVector = translation - offsetA + offsetB; 236 } 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(0,0,0),witnessPointB(0,0,0); 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; 373 btVector3 offsetA; 374 btVector3 offsetB; 375 btScalar tA; 376 btScalar tB; 377 378 btVector3 translation = witnessPointB-witnessPointA; 379 380 btVector3 dirA = worldEdgeA; 381 btVector3 dirB = worldEdgeB; 382 383 btScalar hlenB = 1e30f; 384 btScalar hlenA = 1e30f; 385 386 btSegmentsClosestPoints(ptsVector,offsetA,offsetB,tA,tB, 387 translation, 388 dirA, hlenA, 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 } 571
