1 2 /* 3 Bullet Continuous Collision Detection and Physics Library 4 Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ 5 6 This software is provided 'as-is', without any express or implied warranty. 7 In no event will the authors be held liable for any damages arising from the use of this software. 8 Permission is granted to anyone to use this software for any purpose, 9 including commercial applications, and to alter it and redistribute it freely, 10 subject to the following restrictions: 11 12 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. 13 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. 14 3. This notice may not be removed or altered from any source distribution. 15 16 Elsevier CDROM license agreements grants nonexclusive license to use the software 17 for any purpose, commercial or non-commercial as long as the following credit is included 18 identifying the original source of the software: 19 20 Parts of the source are "from the book Real-Time Collision Detection by 21 Christer Ericson, published by Morgan Kaufmann Publishers, 22 (c) 2005 Elsevier Inc." 23 24 */ 25 26 27 #include "btVoronoiSimplexSolver.h" 28 29 #define VERTA 0 30 #define VERTB 1 31 #define VERTC 2 32 #define VERTD 3 33 34 #define CATCH_DEGENERATE_TETRAHEDRON 1 35 void btVoronoiSimplexSolver::removeVertex(int index) 36 { 37 38 btAssert(m_numVertices>0); 39 m_numVertices--; 40 m_simplexVectorW[index] = m_simplexVectorW[m_numVertices]; 41 m_simplexPointsP[index] = m_simplexPointsP[m_numVertices]; 42 m_simplexPointsQ[index] = m_simplexPointsQ[m_numVertices]; 43 } 44 45 void btVoronoiSimplexSolver::reduceVertices (const btUsageBitfield& usedVerts) 46 { 47 if ((numVertices() >= 4) && (!usedVerts.usedVertexD)) 48 removeVertex(3); 49 50 if ((numVertices() >= 3) && (!usedVerts.usedVertexC)) 51 removeVertex(2); 52 53 if ((numVertices() >= 2) && (!usedVerts.usedVertexB)) 54 removeVertex(1); 55 56 if ((numVertices() >= 1) && (!usedVerts.usedVertexA)) 57 removeVertex(0); 58 59 } 60 61 62 63 64 65 //clear the simplex, remove all the vertices 66 void btVoronoiSimplexSolver::reset() 67 { 68 m_cachedValidClosest = false; 69 m_numVertices = 0; 70 m_needsUpdate = true; 71 m_lastW = btVector3(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT)); 72 m_cachedBC.reset(); 73 } 74 75 76 77 //add a vertex 78 void btVoronoiSimplexSolver::addVertex(const btVector3& w, const btVector3& p, const btVector3& q) 79 { 80 m_lastW = w; 81 m_needsUpdate = true; 82 83 m_simplexVectorW[m_numVertices] = w; 84 m_simplexPointsP[m_numVertices] = p; 85 m_simplexPointsQ[m_numVertices] = q; 86 87 m_numVertices++; 88 } 89 90 bool btVoronoiSimplexSolver::updateClosestVectorAndPoints() 91 { 92 93 if (m_needsUpdate) 94 { 95 m_cachedBC.reset(); 96 97 m_needsUpdate = false; 98 99 switch (numVertices()) 100 { 101 case 0: 102 m_cachedValidClosest = false; 103 break; 104 case 1: 105 { 106 m_cachedP1 = m_simplexPointsP[0]; 107 m_cachedP2 = m_simplexPointsQ[0]; 108 m_cachedV = m_cachedP1-m_cachedP2; //== m_simplexVectorW[0] 109 m_cachedBC.reset(); 110 m_cachedBC.setBarycentricCoordinates(btScalar(1.),btScalar(0.),btScalar(0.),btScalar(0.)); 111 m_cachedValidClosest = m_cachedBC.isValid(); 112 break; 113 }; 114 case 2: 115 { 116 //closest point origin from line segment 117 const btVector3& from = m_simplexVectorW[0]; 118 const btVector3& to = m_simplexVectorW[1]; 119 btVector3 nearest; 120 121 btVector3 p (btScalar(0.),btScalar(0.),btScalar(0.)); 122 btVector3 diff = p - from; 123 btVector3 v = to - from; 124 btScalar t = v.dot(diff); 125 126 if (t > 0) { 127 btScalar dotVV = v.dot(v); 128 if (t < dotVV) { 129 t /= dotVV; 130 diff -= t*v; 131 m_cachedBC.m_usedVertices.usedVertexA = true; 132 m_cachedBC.m_usedVertices.usedVertexB = true; 133 } else { 134 t = 1; 135 diff -= v; 136 //reduce to 1 point 137 m_cachedBC.m_usedVertices.usedVertexB = true; 138 } 139 } else 140 { 141 t = 0; 142 //reduce to 1 point 143 m_cachedBC.m_usedVertices.usedVertexA = true; 144 } 145 m_cachedBC.setBarycentricCoordinates(1-t,t); 146 nearest = from + t*v; 147 148 m_cachedP1 = m_simplexPointsP[0] + t * (m_simplexPointsP[1] - m_simplexPointsP[0]); 149 m_cachedP2 = m_simplexPointsQ[0] + t * (m_simplexPointsQ[1] - m_simplexPointsQ[0]); 150 m_cachedV = m_cachedP1 - m_cachedP2; 151 152 reduceVertices(m_cachedBC.m_usedVertices); 153 154 m_cachedValidClosest = m_cachedBC.isValid(); 155 break; 156 } 157 case 3: 158 { 159 //closest point origin from triangle 160 btVector3 p (btScalar(0.),btScalar(0.),btScalar(0.)); 161 162 const btVector3& a = m_simplexVectorW[0]; 163 const btVector3& b = m_simplexVectorW[1]; 164 const btVector3& c = m_simplexVectorW[2]; 165 166 closestPtPointTriangle(p,a,b,c,m_cachedBC); 167 m_cachedP1 = m_simplexPointsP[0] * m_cachedBC.m_barycentricCoords[0] + 168 m_simplexPointsP[1] * m_cachedBC.m_barycentricCoords[1] + 169 m_simplexPointsP[2] * m_cachedBC.m_barycentricCoords[2]; 170 171 m_cachedP2 = m_simplexPointsQ[0] * m_cachedBC.m_barycentricCoords[0] + 172 m_simplexPointsQ[1] * m_cachedBC.m_barycentricCoords[1] + 173 m_simplexPointsQ[2] * m_cachedBC.m_barycentricCoords[2]; 174 175 m_cachedV = m_cachedP1-m_cachedP2; 176 177 reduceVertices (m_cachedBC.m_usedVertices); 178 m_cachedValidClosest = m_cachedBC.isValid(); 179 180 break; 181 } 182 case 4: 183 { 184 185 186 btVector3 p (btScalar(0.),btScalar(0.),btScalar(0.)); 187 188 const btVector3& a = m_simplexVectorW[0]; 189 const btVector3& b = m_simplexVectorW[1]; 190 const btVector3& c = m_simplexVectorW[2]; 191 const btVector3& d = m_simplexVectorW[3]; 192 193 bool hasSeperation = closestPtPointTetrahedron(p,a,b,c,d,m_cachedBC); 194 195 if (hasSeperation) 196 { 197 198 m_cachedP1 = m_simplexPointsP[0] * m_cachedBC.m_barycentricCoords[0] + 199 m_simplexPointsP[1] * m_cachedBC.m_barycentricCoords[1] + 200 m_simplexPointsP[2] * m_cachedBC.m_barycentricCoords[2] + 201 m_simplexPointsP[3] * m_cachedBC.m_barycentricCoords[3]; 202 203 m_cachedP2 = m_simplexPointsQ[0] * m_cachedBC.m_barycentricCoords[0] + 204 m_simplexPointsQ[1] * m_cachedBC.m_barycentricCoords[1] + 205 m_simplexPointsQ[2] * m_cachedBC.m_barycentricCoords[2] + 206 m_simplexPointsQ[3] * m_cachedBC.m_barycentricCoords[3]; 207 208 m_cachedV = m_cachedP1-m_cachedP2; 209 reduceVertices (m_cachedBC.m_usedVertices); 210 } else 211 { 212 // printf("sub distance got penetration\n"); 213 214 if (m_cachedBC.m_degenerate) 215 { 216 m_cachedValidClosest = false; 217 } else 218 { 219 m_cachedValidClosest = true; 220 //degenerate case == false, penetration = true + zero 221 m_cachedV.setValue(btScalar(0.),btScalar(0.),btScalar(0.)); 222 } 223 break; 224 } 225 226 m_cachedValidClosest = m_cachedBC.isValid(); 227 228 //closest point origin from tetrahedron 229 break; 230 } 231 default: 232 { 233 m_cachedValidClosest = false; 234 } 235 }; 236 } 237 238 return m_cachedValidClosest; 239 240 } 241 242 //return/calculate the closest vertex 243 bool btVoronoiSimplexSolver::closest(btVector3& v) 244 { 245 bool succes = updateClosestVectorAndPoints(); 246 v = m_cachedV; 247 return succes; 248 } 249 250 251 252 btScalar btVoronoiSimplexSolver::maxVertex() 253 { 254 int i, numverts = numVertices(); 255 btScalar maxV = btScalar(0.); 256 for (i=0;i<numverts;i++) 257 { 258 btScalar curLen2 = m_simplexVectorW[i].length2(); 259 if (maxV < curLen2) 260 maxV = curLen2; 261 } 262 return maxV; 263 } 264 265 266 267 //return the current simplex 268 int btVoronoiSimplexSolver::getSimplex(btVector3 *pBuf, btVector3 *qBuf, btVector3 *yBuf) const 269 { 270 int i; 271 for (i=0;i<numVertices();i++) 272 { 273 yBuf[i] = m_simplexVectorW[i]; 274 pBuf[i] = m_simplexPointsP[i]; 275 qBuf[i] = m_simplexPointsQ[i]; 276 } 277 return numVertices(); 278 } 279 280 281 282 283 bool btVoronoiSimplexSolver::inSimplex(const btVector3& w) 284 { 285 bool found = false; 286 int i, numverts = numVertices(); 287 //btScalar maxV = btScalar(0.); 288 289 //w is in the current (reduced) simplex 290 for (i=0;i<numverts;i++) 291 { 292 #ifdef BT_USE_EQUAL_VERTEX_THRESHOLD 293 if ( m_simplexVectorW[i].distance2(w) <= m_equalVertexThreshold) 294 #else 295 if (m_simplexVectorW[i] == w) 296 #endif 297 found = true; 298 } 299 300 //check in case lastW is already removed 301 if (w == m_lastW) 302 return true; 303 304 return found; 305 } 306 307 void btVoronoiSimplexSolver::backup_closest(btVector3& v) 308 { 309 v = m_cachedV; 310 } 311 312 313 bool btVoronoiSimplexSolver::emptySimplex() const 314 { 315 return (numVertices() == 0); 316 317 } 318 319 void btVoronoiSimplexSolver::compute_points(btVector3& p1, btVector3& p2) 320 { 321 updateClosestVectorAndPoints(); 322 p1 = m_cachedP1; 323 p2 = m_cachedP2; 324 325 } 326 327 328 329 330 bool btVoronoiSimplexSolver::closestPtPointTriangle(const btVector3& p, const btVector3& a, const btVector3& b, const btVector3& c,btSubSimplexClosestResult& result) 331 { 332 result.m_usedVertices.reset(); 333 334 // Check if P in vertex region outside A 335 btVector3 ab = b - a; 336 btVector3 ac = c - a; 337 btVector3 ap = p - a; 338 btScalar d1 = ab.dot(ap); 339 btScalar d2 = ac.dot(ap); 340 if (d1 <= btScalar(0.0) && d2 <= btScalar(0.0)) 341 { 342 result.m_closestPointOnSimplex = a; 343 result.m_usedVertices.usedVertexA = true; 344 result.setBarycentricCoordinates(1,0,0); 345 return true;// a; // barycentric coordinates (1,0,0) 346 } 347 348 // Check if P in vertex region outside B 349 btVector3 bp = p - b; 350 btScalar d3 = ab.dot(bp); 351 btScalar d4 = ac.dot(bp); 352 if (d3 >= btScalar(0.0) && d4 <= d3) 353 { 354 result.m_closestPointOnSimplex = b; 355 result.m_usedVertices.usedVertexB = true; 356 result.setBarycentricCoordinates(0,1,0); 357 358 return true; // b; // barycentric coordinates (0,1,0) 359 } 360 // Check if P in edge region of AB, if so return projection of P onto AB 361 btScalar vc = d1*d4 - d3*d2; 362 if (vc <= btScalar(0.0) && d1 >= btScalar(0.0) && d3 <= btScalar(0.0)) { 363 btScalar v = d1 / (d1 - d3); 364 result.m_closestPointOnSimplex = a + v * ab; 365 result.m_usedVertices.usedVertexA = true; 366 result.m_usedVertices.usedVertexB = true; 367 result.setBarycentricCoordinates(1-v,v,0); 368 return true; 369 //return a + v * ab; // barycentric coordinates (1-v,v,0) 370 } 371 372 // Check if P in vertex region outside C 373 btVector3 cp = p - c; 374 btScalar d5 = ab.dot(cp); 375 btScalar d6 = ac.dot(cp); 376 if (d6 >= btScalar(0.0) && d5 <= d6) 377 { 378 result.m_closestPointOnSimplex = c; 379 result.m_usedVertices.usedVertexC = true; 380 result.setBarycentricCoordinates(0,0,1); 381 return true;//c; // barycentric coordinates (0,0,1) 382 } 383 384 // Check if P in edge region of AC, if so return projection of P onto AC 385 btScalar vb = d5*d2 - d1*d6; 386 if (vb <= btScalar(0.0) && d2 >= btScalar(0.0) && d6 <= btScalar(0.0)) { 387 btScalar w = d2 / (d2 - d6); 388 result.m_closestPointOnSimplex = a + w * ac; 389 result.m_usedVertices.usedVertexA = true; 390 result.m_usedVertices.usedVertexC = true; 391 result.setBarycentricCoordinates(1-w,0,w); 392 return true; 393 //return a + w * ac; // barycentric coordinates (1-w,0,w) 394 } 395 396 // Check if P in edge region of BC, if so return projection of P onto BC 397 btScalar va = d3*d6 - d5*d4; 398 if (va <= btScalar(0.0) && (d4 - d3) >= btScalar(0.0) && (d5 - d6) >= btScalar(0.0)) { 399 btScalar w = (d4 - d3) / ((d4 - d3) + (d5 - d6)); 400 401 result.m_closestPointOnSimplex = b + w * (c - b); 402 result.m_usedVertices.usedVertexB = true; 403 result.m_usedVertices.usedVertexC = true; 404 result.setBarycentricCoordinates(0,1-w,w); 405 return true; 406 // return b + w * (c - b); // barycentric coordinates (0,1-w,w) 407 } 408 409 // P inside face region. Compute Q through its barycentric coordinates (u,v,w) 410 btScalar denom = btScalar(1.0) / (va + vb + vc); 411 btScalar v = vb * denom; 412 btScalar w = vc * denom; 413 414 result.m_closestPointOnSimplex = a + ab * v + ac * w; 415 result.m_usedVertices.usedVertexA = true; 416 result.m_usedVertices.usedVertexB = true; 417 result.m_usedVertices.usedVertexC = true; 418 result.setBarycentricCoordinates(1-v-w,v,w); 419 420 return true; 421 // return a + ab * v + ac * w; // = u*a + v*b + w*c, u = va * denom = btScalar(1.0) - v - w 422 423 } 424 425 426 427 428 429 /// Test if point p and d lie on opposite sides of plane through abc 430 int btVoronoiSimplexSolver::pointOutsideOfPlane(const btVector3& p, const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& d) 431 { 432 btVector3 normal = (b-a).cross(c-a); 433 434 btScalar signp = (p - a).dot(normal); // [AP AB AC] 435 btScalar signd = (d - a).dot( normal); // [AD AB AC] 436 437 #ifdef CATCH_DEGENERATE_TETRAHEDRON 438 #ifdef BT_USE_DOUBLE_PRECISION 439 if (signd * signd < (btScalar(1e-8) * btScalar(1e-8))) 440 { 441 return -1; 442 } 443 #else 444 if (signd * signd < (btScalar(1e-4) * btScalar(1e-4))) 445 { 446 // printf("affine dependent/degenerate\n");// 447 return -1; 448 } 449 #endif 450 451 #endif 452 // Points on opposite sides if expression signs are opposite 453 return signp * signd < btScalar(0.); 454 } 455 456 457 bool btVoronoiSimplexSolver::closestPtPointTetrahedron(const btVector3& p, const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& d, btSubSimplexClosestResult& finalResult) 458 { 459 btSubSimplexClosestResult tempResult; 460 461 // Start out assuming point inside all halfspaces, so closest to itself 462 finalResult.m_closestPointOnSimplex = p; 463 finalResult.m_usedVertices.reset(); 464 finalResult.m_usedVertices.usedVertexA = true; 465 finalResult.m_usedVertices.usedVertexB = true; 466 finalResult.m_usedVertices.usedVertexC = true; 467 finalResult.m_usedVertices.usedVertexD = true; 468 469 int pointOutsideABC = pointOutsideOfPlane(p, a, b, c, d); 470 int pointOutsideACD = pointOutsideOfPlane(p, a, c, d, b); 471 int pointOutsideADB = pointOutsideOfPlane(p, a, d, b, c); 472 int pointOutsideBDC = pointOutsideOfPlane(p, b, d, c, a); 473 474 if (pointOutsideABC < 0 || pointOutsideACD < 0 || pointOutsideADB < 0 || pointOutsideBDC < 0) 475 { 476 finalResult.m_degenerate = true; 477 return false; 478 } 479 480 if (!pointOutsideABC && !pointOutsideACD && !pointOutsideADB && !pointOutsideBDC) 481 { 482 return false; 483 } 484 485 486 btScalar bestSqDist = FLT_MAX; 487 // If point outside face abc then compute closest point on abc 488 if (pointOutsideABC) 489 { 490 closestPtPointTriangle(p, a, b, c,tempResult); 491 btVector3 q = tempResult.m_closestPointOnSimplex; 492 493 btScalar sqDist = (q - p).dot( q - p); 494 // Update best closest point if (squared) distance is less than current best 495 if (sqDist < bestSqDist) { 496 bestSqDist = sqDist; 497 finalResult.m_closestPointOnSimplex = q; 498 //convert result bitmask! 499 finalResult.m_usedVertices.reset(); 500 finalResult.m_usedVertices.usedVertexA = tempResult.m_usedVertices.usedVertexA; 501 finalResult.m_usedVertices.usedVertexB = tempResult.m_usedVertices.usedVertexB; 502 finalResult.m_usedVertices.usedVertexC = tempResult.m_usedVertices.usedVertexC; 503 finalResult.setBarycentricCoordinates( 504 tempResult.m_barycentricCoords[VERTA], 505 tempResult.m_barycentricCoords[VERTB], 506 tempResult.m_barycentricCoords[VERTC], 507 0 508 ); 509 510 } 511 } 512 513 514 // Repeat test for face acd 515 if (pointOutsideACD) 516 { 517 closestPtPointTriangle(p, a, c, d,tempResult); 518 btVector3 q = tempResult.m_closestPointOnSimplex; 519 //convert result bitmask! 520 521 btScalar sqDist = (q - p).dot( q - p); 522 if (sqDist < bestSqDist) 523 { 524 bestSqDist = sqDist; 525 finalResult.m_closestPointOnSimplex = q; 526 finalResult.m_usedVertices.reset(); 527 finalResult.m_usedVertices.usedVertexA = tempResult.m_usedVertices.usedVertexA; 528 529 finalResult.m_usedVertices.usedVertexC = tempResult.m_usedVertices.usedVertexB; 530 finalResult.m_usedVertices.usedVertexD = tempResult.m_usedVertices.usedVertexC; 531 finalResult.setBarycentricCoordinates( 532 tempResult.m_barycentricCoords[VERTA], 533 0, 534 tempResult.m_barycentricCoords[VERTB], 535 tempResult.m_barycentricCoords[VERTC] 536 ); 537 538 } 539 } 540 // Repeat test for face adb 541 542 543 if (pointOutsideADB) 544 { 545 closestPtPointTriangle(p, a, d, b,tempResult); 546 btVector3 q = tempResult.m_closestPointOnSimplex; 547 //convert result bitmask! 548 549 btScalar sqDist = (q - p).dot( q - p); 550 if (sqDist < bestSqDist) 551 { 552 bestSqDist = sqDist; 553 finalResult.m_closestPointOnSimplex = q; 554 finalResult.m_usedVertices.reset(); 555 finalResult.m_usedVertices.usedVertexA = tempResult.m_usedVertices.usedVertexA; 556 finalResult.m_usedVertices.usedVertexB = tempResult.m_usedVertices.usedVertexC; 557 558 finalResult.m_usedVertices.usedVertexD = tempResult.m_usedVertices.usedVertexB; 559 finalResult.setBarycentricCoordinates( 560 tempResult.m_barycentricCoords[VERTA], 561 tempResult.m_barycentricCoords[VERTC], 562 0, 563 tempResult.m_barycentricCoords[VERTB] 564 ); 565 566 } 567 } 568 // Repeat test for face bdc 569 570 571 if (pointOutsideBDC) 572 { 573 closestPtPointTriangle(p, b, d, c,tempResult); 574 btVector3 q = tempResult.m_closestPointOnSimplex; 575 //convert result bitmask! 576 btScalar sqDist = (q - p).dot( q - p); 577 if (sqDist < bestSqDist) 578 { 579 bestSqDist = sqDist; 580 finalResult.m_closestPointOnSimplex = q; 581 finalResult.m_usedVertices.reset(); 582 // 583 finalResult.m_usedVertices.usedVertexB = tempResult.m_usedVertices.usedVertexA; 584 finalResult.m_usedVertices.usedVertexC = tempResult.m_usedVertices.usedVertexC; 585 finalResult.m_usedVertices.usedVertexD = tempResult.m_usedVertices.usedVertexB; 586 587 finalResult.setBarycentricCoordinates( 588 0, 589 tempResult.m_barycentricCoords[VERTA], 590 tempResult.m_barycentricCoords[VERTC], 591 tempResult.m_barycentricCoords[VERTB] 592 ); 593 594 } 595 } 596 597 //help! we ended up full ! 598 599 if (finalResult.m_usedVertices.usedVertexA && 600 finalResult.m_usedVertices.usedVertexB && 601 finalResult.m_usedVertices.usedVertexC && 602 finalResult.m_usedVertices.usedVertexD) 603 { 604 return true; 605 } 606 607 return true; 608 } 609 610
