1 2 /*** 3 * --------------------------------- 4 * Copyright (c)2012 Daniel Fiser <danfis (at) danfis.cz> 5 * 6 * This file was ported from mpr.c file, part of libccd. 7 * The Minkoski Portal Refinement implementation was ported 8 * to OpenCL by Erwin Coumans for the Bullet 3 Physics library. 9 * The original MPR idea and implementation is by Gary Snethen 10 * in XenoCollide, see http://github.com/erwincoumans/xenocollide 11 * 12 * Distributed under the OSI-approved BSD License (the "License"); 13 * see <http://www.opensource.org/licenses/bsd-license.php>. 14 * This software is distributed WITHOUT ANY WARRANTY; without even the 15 * implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. 16 * See the License for more information. 17 */ 18 19 ///2014 Oct, Erwin Coumans, Use templates to avoid void* casts 20 21 #ifndef BT_MPR_PENETRATION_H 22 #define BT_MPR_PENETRATION_H 23 24 #define BT_DEBUG_MPR1 25 26 #include "LinearMath/btTransform.h" 27 #include "LinearMath/btAlignedObjectArray.h" 28 29 //#define MPR_AVERAGE_CONTACT_POSITIONS 30 31 32 struct btMprCollisionDescription 33 { 34 btVector3 m_firstDir; 35 int m_maxGjkIterations; 36 btScalar m_maximumDistanceSquared; 37 btScalar m_gjkRelError2; 38 39 btMprCollisionDescription() 40 : m_firstDir(0,1,0), 41 m_maxGjkIterations(1000), 42 m_maximumDistanceSquared(1e30f), 43 m_gjkRelError2(1.0e-6) 44 { 45 } 46 virtual ~btMprCollisionDescription() 47 { 48 } 49 }; 50 51 struct btMprDistanceInfo 52 { 53 btVector3 m_pointOnA; 54 btVector3 m_pointOnB; 55 btVector3 m_normalBtoA; 56 btScalar m_distance; 57 }; 58 59 #ifdef __cplusplus 60 #define BT_MPR_SQRT sqrtf 61 #else 62 #define BT_MPR_SQRT sqrt 63 #endif 64 #define BT_MPR_FMIN(x, y) ((x) < (y) ? (x) : (y)) 65 #define BT_MPR_FABS fabs 66 67 #define BT_MPR_TOLERANCE 1E-6f 68 #define BT_MPR_MAX_ITERATIONS 1000 69 70 struct _btMprSupport_t 71 { 72 btVector3 v; //!< Support point in minkowski sum 73 btVector3 v1; //!< Support point in obj1 74 btVector3 v2; //!< Support point in obj2 75 }; 76 typedef struct _btMprSupport_t btMprSupport_t; 77 78 struct _btMprSimplex_t 79 { 80 btMprSupport_t ps[4]; 81 int last; //!< index of last added point 82 }; 83 typedef struct _btMprSimplex_t btMprSimplex_t; 84 85 inline btMprSupport_t* btMprSimplexPointW(btMprSimplex_t *s, int idx) 86 { 87 return &s->ps[idx]; 88 } 89 90 inline void btMprSimplexSetSize(btMprSimplex_t *s, int size) 91 { 92 s->last = size - 1; 93 } 94 95 #ifdef DEBUG_MPR 96 inline void btPrintPortalVertex(_btMprSimplex_t* portal, int index) 97 { 98 printf("portal[%d].v = %f,%f,%f, v1=%f,%f,%f, v2=%f,%f,%f\n", index, portal->ps[index].v.x(),portal->ps[index].v.y(),portal->ps[index].v.z(), 99 portal->ps[index].v1.x(),portal->ps[index].v1.y(),portal->ps[index].v1.z(), 100 portal->ps[index].v2.x(),portal->ps[index].v2.y(),portal->ps[index].v2.z()); 101 } 102 #endif //DEBUG_MPR 103 104 105 106 107 inline int btMprSimplexSize(const btMprSimplex_t *s) 108 { 109 return s->last + 1; 110 } 111 112 113 inline const btMprSupport_t* btMprSimplexPoint(const btMprSimplex_t* s, int idx) 114 { 115 // here is no check on boundaries 116 return &s->ps[idx]; 117 } 118 119 inline void btMprSupportCopy(btMprSupport_t *d, const btMprSupport_t *s) 120 { 121 *d = *s; 122 } 123 124 inline void btMprSimplexSet(btMprSimplex_t *s, size_t pos, const btMprSupport_t *a) 125 { 126 btMprSupportCopy(s->ps + pos, a); 127 } 128 129 130 inline void btMprSimplexSwap(btMprSimplex_t *s, size_t pos1, size_t pos2) 131 { 132 btMprSupport_t supp; 133 134 btMprSupportCopy(&supp, &s->ps[pos1]); 135 btMprSupportCopy(&s->ps[pos1], &s->ps[pos2]); 136 btMprSupportCopy(&s->ps[pos2], &supp); 137 } 138 139 140 inline int btMprIsZero(float val) 141 { 142 return BT_MPR_FABS(val) < FLT_EPSILON; 143 } 144 145 146 147 inline int btMprEq(float _a, float _b) 148 { 149 float ab; 150 float a, b; 151 152 ab = BT_MPR_FABS(_a - _b); 153 if (BT_MPR_FABS(ab) < FLT_EPSILON) 154 return 1; 155 156 a = BT_MPR_FABS(_a); 157 b = BT_MPR_FABS(_b); 158 if (b > a){ 159 return ab < FLT_EPSILON * b; 160 }else{ 161 return ab < FLT_EPSILON * a; 162 } 163 } 164 165 166 inline int btMprVec3Eq(const btVector3* a, const btVector3 *b) 167 { 168 return btMprEq((*a).x(), (*b).x()) 169 && btMprEq((*a).y(), (*b).y()) 170 && btMprEq((*a).z(), (*b).z()); 171 } 172 173 174 175 176 177 178 179 180 181 182 183 template <typename btConvexTemplate> 184 inline void btFindOrigin(const btConvexTemplate& a, const btConvexTemplate& b, const btMprCollisionDescription& colDesc,btMprSupport_t *center) 185 { 186 187 center->v1 = a.getObjectCenterInWorld(); 188 center->v2 = b.getObjectCenterInWorld(); 189 center->v = center->v1 - center->v2; 190 } 191 192 inline void btMprVec3Set(btVector3 *v, float x, float y, float z) 193 { 194 v->setValue(x,y,z); 195 } 196 197 inline void btMprVec3Add(btVector3 *v, const btVector3 *w) 198 { 199 *v += *w; 200 } 201 202 inline void btMprVec3Copy(btVector3 *v, const btVector3 *w) 203 { 204 *v = *w; 205 } 206 207 inline void btMprVec3Scale(btVector3 *d, float k) 208 { 209 *d *= k; 210 } 211 212 inline float btMprVec3Dot(const btVector3 *a, const btVector3 *b) 213 { 214 float dot; 215 216 dot = btDot(*a,*b); 217 return dot; 218 } 219 220 221 inline float btMprVec3Len2(const btVector3 *v) 222 { 223 return btMprVec3Dot(v, v); 224 } 225 226 inline void btMprVec3Normalize(btVector3 *d) 227 { 228 float k = 1.f / BT_MPR_SQRT(btMprVec3Len2(d)); 229 btMprVec3Scale(d, k); 230 } 231 232 inline void btMprVec3Cross(btVector3 *d, const btVector3 *a, const btVector3 *b) 233 { 234 *d = btCross(*a,*b); 235 236 } 237 238 239 inline void btMprVec3Sub2(btVector3 *d, const btVector3 *v, const btVector3 *w) 240 { 241 *d = *v - *w; 242 } 243 244 inline void btPortalDir(const btMprSimplex_t *portal, btVector3 *dir) 245 { 246 btVector3 v2v1, v3v1; 247 248 btMprVec3Sub2(&v2v1, &btMprSimplexPoint(portal, 2)->v, 249 &btMprSimplexPoint(portal, 1)->v); 250 btMprVec3Sub2(&v3v1, &btMprSimplexPoint(portal, 3)->v, 251 &btMprSimplexPoint(portal, 1)->v); 252 btMprVec3Cross(dir, &v2v1, &v3v1); 253 btMprVec3Normalize(dir); 254 } 255 256 257 inline int portalEncapsulesOrigin(const btMprSimplex_t *portal, 258 const btVector3 *dir) 259 { 260 float dot; 261 dot = btMprVec3Dot(dir, &btMprSimplexPoint(portal, 1)->v); 262 return btMprIsZero(dot) || dot > 0.f; 263 } 264 265 inline int portalReachTolerance(const btMprSimplex_t *portal, 266 const btMprSupport_t *v4, 267 const btVector3 *dir) 268 { 269 float dv1, dv2, dv3, dv4; 270 float dot1, dot2, dot3; 271 272 // find the smallest dot product of dir and {v1-v4, v2-v4, v3-v4} 273 274 dv1 = btMprVec3Dot(&btMprSimplexPoint(portal, 1)->v, dir); 275 dv2 = btMprVec3Dot(&btMprSimplexPoint(portal, 2)->v, dir); 276 dv3 = btMprVec3Dot(&btMprSimplexPoint(portal, 3)->v, dir); 277 dv4 = btMprVec3Dot(&v4->v, dir); 278 279 dot1 = dv4 - dv1; 280 dot2 = dv4 - dv2; 281 dot3 = dv4 - dv3; 282 283 dot1 = BT_MPR_FMIN(dot1, dot2); 284 dot1 = BT_MPR_FMIN(dot1, dot3); 285 286 return btMprEq(dot1, BT_MPR_TOLERANCE) || dot1 < BT_MPR_TOLERANCE; 287 } 288 289 inline int portalCanEncapsuleOrigin(const btMprSimplex_t *portal, 290 const btMprSupport_t *v4, 291 const btVector3 *dir) 292 { 293 float dot; 294 dot = btMprVec3Dot(&v4->v, dir); 295 return btMprIsZero(dot) || dot > 0.f; 296 } 297 298 inline void btExpandPortal(btMprSimplex_t *portal, 299 const btMprSupport_t *v4) 300 { 301 float dot; 302 btVector3 v4v0; 303 304 btMprVec3Cross(&v4v0, &v4->v, &btMprSimplexPoint(portal, 0)->v); 305 dot = btMprVec3Dot(&btMprSimplexPoint(portal, 1)->v, &v4v0); 306 if (dot > 0.f){ 307 dot = btMprVec3Dot(&btMprSimplexPoint(portal, 2)->v, &v4v0); 308 if (dot > 0.f){ 309 btMprSimplexSet(portal, 1, v4); 310 }else{ 311 btMprSimplexSet(portal, 3, v4); 312 } 313 }else{ 314 dot = btMprVec3Dot(&btMprSimplexPoint(portal, 3)->v, &v4v0); 315 if (dot > 0.f){ 316 btMprSimplexSet(portal, 2, v4); 317 }else{ 318 btMprSimplexSet(portal, 1, v4); 319 } 320 } 321 } 322 template <typename btConvexTemplate> 323 inline void btMprSupport(const btConvexTemplate& a, const btConvexTemplate& b, 324 const btMprCollisionDescription& colDesc, 325 const btVector3& dir, btMprSupport_t *supp) 326 { 327 btVector3 seperatingAxisInA = dir* a.getWorldTransform().getBasis(); 328 btVector3 seperatingAxisInB = -dir* b.getWorldTransform().getBasis(); 329 330 btVector3 pInA = a.getLocalSupportWithMargin(seperatingAxisInA); 331 btVector3 qInB = b.getLocalSupportWithMargin(seperatingAxisInB); 332 333 supp->v1 = a.getWorldTransform()(pInA); 334 supp->v2 = b.getWorldTransform()(qInB); 335 supp->v = supp->v1 - supp->v2; 336 } 337 338 339 template <typename btConvexTemplate> 340 static int btDiscoverPortal(const btConvexTemplate& a, const btConvexTemplate& b, 341 const btMprCollisionDescription& colDesc, 342 btMprSimplex_t *portal) 343 { 344 btVector3 dir, va, vb; 345 float dot; 346 int cont; 347 348 349 350 // vertex 0 is center of portal 351 btFindOrigin(a,b,colDesc, btMprSimplexPointW(portal, 0)); 352 353 354 // vertex 0 is center of portal 355 btMprSimplexSetSize(portal, 1); 356 357 358 359 btVector3 zero = btVector3(0,0,0); 360 btVector3* org = &zero; 361 362 if (btMprVec3Eq(&btMprSimplexPoint(portal, 0)->v, org)){ 363 // Portal's center lies on origin (0,0,0) => we know that objects 364 // intersect but we would need to know penetration info. 365 // So move center little bit... 366 btMprVec3Set(&va, FLT_EPSILON * 10.f, 0.f, 0.f); 367 btMprVec3Add(&btMprSimplexPointW(portal, 0)->v, &va); 368 } 369 370 371 // vertex 1 = support in direction of origin 372 btMprVec3Copy(&dir, &btMprSimplexPoint(portal, 0)->v); 373 btMprVec3Scale(&dir, -1.f); 374 btMprVec3Normalize(&dir); 375 376 377 btMprSupport(a,b,colDesc, dir, btMprSimplexPointW(portal, 1)); 378 379 btMprSimplexSetSize(portal, 2); 380 381 // test if origin isn't outside of v1 382 dot = btMprVec3Dot(&btMprSimplexPoint(portal, 1)->v, &dir); 383 384 385 if (btMprIsZero(dot) || dot < 0.f) 386 return -1; 387 388 389 // vertex 2 390 btMprVec3Cross(&dir, &btMprSimplexPoint(portal, 0)->v, 391 &btMprSimplexPoint(portal, 1)->v); 392 if (btMprIsZero(btMprVec3Len2(&dir))){ 393 if (btMprVec3Eq(&btMprSimplexPoint(portal, 1)->v, org)){ 394 // origin lies on v1 395 return 1; 396 }else{ 397 // origin lies on v0-v1 segment 398 return 2; 399 } 400 } 401 402 btMprVec3Normalize(&dir); 403 btMprSupport(a,b,colDesc, dir, btMprSimplexPointW(portal, 2)); 404 405 406 407 dot = btMprVec3Dot(&btMprSimplexPoint(portal, 2)->v, &dir); 408 if (btMprIsZero(dot) || dot < 0.f) 409 return -1; 410 411 btMprSimplexSetSize(portal, 3); 412 413 // vertex 3 direction 414 btMprVec3Sub2(&va, &btMprSimplexPoint(portal, 1)->v, 415 &btMprSimplexPoint(portal, 0)->v); 416 btMprVec3Sub2(&vb, &btMprSimplexPoint(portal, 2)->v, 417 &btMprSimplexPoint(portal, 0)->v); 418 btMprVec3Cross(&dir, &va, &vb); 419 btMprVec3Normalize(&dir); 420 421 // it is better to form portal faces to be oriented "outside" origin 422 dot = btMprVec3Dot(&dir, &btMprSimplexPoint(portal, 0)->v); 423 if (dot > 0.f){ 424 btMprSimplexSwap(portal, 1, 2); 425 btMprVec3Scale(&dir, -1.f); 426 } 427 428 while (btMprSimplexSize(portal) < 4){ 429 btMprSupport(a,b,colDesc, dir, btMprSimplexPointW(portal, 3)); 430 431 dot = btMprVec3Dot(&btMprSimplexPoint(portal, 3)->v, &dir); 432 if (btMprIsZero(dot) || dot < 0.f) 433 return -1; 434 435 cont = 0; 436 437 // test if origin is outside (v1, v0, v3) - set v2 as v3 and 438 // continue 439 btMprVec3Cross(&va, &btMprSimplexPoint(portal, 1)->v, 440 &btMprSimplexPoint(portal, 3)->v); 441 dot = btMprVec3Dot(&va, &btMprSimplexPoint(portal, 0)->v); 442 if (dot < 0.f && !btMprIsZero(dot)){ 443 btMprSimplexSet(portal, 2, btMprSimplexPoint(portal, 3)); 444 cont = 1; 445 } 446 447 if (!cont){ 448 // test if origin is outside (v3, v0, v2) - set v1 as v3 and 449 // continue 450 btMprVec3Cross(&va, &btMprSimplexPoint(portal, 3)->v, 451 &btMprSimplexPoint(portal, 2)->v); 452 dot = btMprVec3Dot(&va, &btMprSimplexPoint(portal, 0)->v); 453 if (dot < 0.f && !btMprIsZero(dot)){ 454 btMprSimplexSet(portal, 1, btMprSimplexPoint(portal, 3)); 455 cont = 1; 456 } 457 } 458 459 if (cont){ 460 btMprVec3Sub2(&va, &btMprSimplexPoint(portal, 1)->v, 461 &btMprSimplexPoint(portal, 0)->v); 462 btMprVec3Sub2(&vb, &btMprSimplexPoint(portal, 2)->v, 463 &btMprSimplexPoint(portal, 0)->v); 464 btMprVec3Cross(&dir, &va, &vb); 465 btMprVec3Normalize(&dir); 466 }else{ 467 btMprSimplexSetSize(portal, 4); 468 } 469 } 470 471 return 0; 472 } 473 474 template <typename btConvexTemplate> 475 static int btRefinePortal(const btConvexTemplate& a, const btConvexTemplate& b,const btMprCollisionDescription& colDesc, 476 btMprSimplex_t *portal) 477 { 478 btVector3 dir; 479 btMprSupport_t v4; 480 481 for (int i=0;i<BT_MPR_MAX_ITERATIONS;i++) 482 //while (1) 483 { 484 // compute direction outside the portal (from v0 throught v1,v2,v3 485 // face) 486 btPortalDir(portal, &dir); 487 488 // test if origin is inside the portal 489 if (portalEncapsulesOrigin(portal, &dir)) 490 return 0; 491 492 // get next support point 493 494 btMprSupport(a,b,colDesc, dir, &v4); 495 496 497 // test if v4 can expand portal to contain origin and if portal 498 // expanding doesn't reach given tolerance 499 if (!portalCanEncapsuleOrigin(portal, &v4, &dir) 500 || portalReachTolerance(portal, &v4, &dir)) 501 { 502 return -1; 503 } 504 505 // v1-v2-v3 triangle must be rearranged to face outside Minkowski 506 // difference (direction from v0). 507 btExpandPortal(portal, &v4); 508 } 509 510 return -1; 511 } 512 513 static void btFindPos(const btMprSimplex_t *portal, btVector3 *pos) 514 { 515 516 btVector3 zero = btVector3(0,0,0); 517 btVector3* origin = &zero; 518 519 btVector3 dir; 520 size_t i; 521 float b[4], sum, inv; 522 btVector3 vec, p1, p2; 523 524 btPortalDir(portal, &dir); 525 526 // use barycentric coordinates of tetrahedron to find origin 527 btMprVec3Cross(&vec, &btMprSimplexPoint(portal, 1)->v, 528 &btMprSimplexPoint(portal, 2)->v); 529 b[0] = btMprVec3Dot(&vec, &btMprSimplexPoint(portal, 3)->v); 530 531 btMprVec3Cross(&vec, &btMprSimplexPoint(portal, 3)->v, 532 &btMprSimplexPoint(portal, 2)->v); 533 b[1] = btMprVec3Dot(&vec, &btMprSimplexPoint(portal, 0)->v); 534 535 btMprVec3Cross(&vec, &btMprSimplexPoint(portal, 0)->v, 536 &btMprSimplexPoint(portal, 1)->v); 537 b[2] = btMprVec3Dot(&vec, &btMprSimplexPoint(portal, 3)->v); 538 539 btMprVec3Cross(&vec, &btMprSimplexPoint(portal, 2)->v, 540 &btMprSimplexPoint(portal, 1)->v); 541 b[3] = btMprVec3Dot(&vec, &btMprSimplexPoint(portal, 0)->v); 542 543 sum = b[0] + b[1] + b[2] + b[3]; 544 545 if (btMprIsZero(sum) || sum < 0.f){ 546 b[0] = 0.f; 547 548 btMprVec3Cross(&vec, &btMprSimplexPoint(portal, 2)->v, 549 &btMprSimplexPoint(portal, 3)->v); 550 b[1] = btMprVec3Dot(&vec, &dir); 551 btMprVec3Cross(&vec, &btMprSimplexPoint(portal, 3)->v, 552 &btMprSimplexPoint(portal, 1)->v); 553 b[2] = btMprVec3Dot(&vec, &dir); 554 btMprVec3Cross(&vec, &btMprSimplexPoint(portal, 1)->v, 555 &btMprSimplexPoint(portal, 2)->v); 556 b[3] = btMprVec3Dot(&vec, &dir); 557 558 sum = b[1] + b[2] + b[3]; 559 } 560 561 inv = 1.f / sum; 562 563 btMprVec3Copy(&p1, origin); 564 btMprVec3Copy(&p2, origin); 565 for (i = 0; i < 4; i++){ 566 btMprVec3Copy(&vec, &btMprSimplexPoint(portal, i)->v1); 567 btMprVec3Scale(&vec, b[i]); 568 btMprVec3Add(&p1, &vec); 569 570 btMprVec3Copy(&vec, &btMprSimplexPoint(portal, i)->v2); 571 btMprVec3Scale(&vec, b[i]); 572 btMprVec3Add(&p2, &vec); 573 } 574 btMprVec3Scale(&p1, inv); 575 btMprVec3Scale(&p2, inv); 576 #ifdef MPR_AVERAGE_CONTACT_POSITIONS 577 btMprVec3Copy(pos, &p1); 578 btMprVec3Add(pos, &p2); 579 btMprVec3Scale(pos, 0.5); 580 #else 581 btMprVec3Copy(pos, &p2); 582 #endif//MPR_AVERAGE_CONTACT_POSITIONS 583 } 584 585 inline float btMprVec3Dist2(const btVector3 *a, const btVector3 *b) 586 { 587 btVector3 ab; 588 btMprVec3Sub2(&ab, a, b); 589 return btMprVec3Len2(&ab); 590 } 591 592 inline float _btMprVec3PointSegmentDist2(const btVector3 *P, 593 const btVector3 *x0, 594 const btVector3 *b, 595 btVector3 *witness) 596 { 597 // The computation comes from solving equation of segment: 598 // S(t) = x0 + t.d 599 // where - x0 is initial point of segment 600 // - d is direction of segment from x0 (|d| > 0) 601 // - t belongs to <0, 1> interval 602 // 603 // Than, distance from a segment to some point P can be expressed: 604 // D(t) = |x0 + t.d - P|^2 605 // which is distance from any point on segment. Minimization 606 // of this function brings distance from P to segment. 607 // Minimization of D(t) leads to simple quadratic equation that's 608 // solving is straightforward. 609 // 610 // Bonus of this method is witness point for free. 611 612 float dist, t; 613 btVector3 d, a; 614 615 // direction of segment 616 btMprVec3Sub2(&d, b, x0); 617 618 // precompute vector from P to x0 619 btMprVec3Sub2(&a, x0, P); 620 621 t = -1.f * btMprVec3Dot(&a, &d); 622 t /= btMprVec3Len2(&d); 623 624 if (t < 0.f || btMprIsZero(t)){ 625 dist = btMprVec3Dist2(x0, P); 626 if (witness) 627 btMprVec3Copy(witness, x0); 628 }else if (t > 1.f || btMprEq(t, 1.f)){ 629 dist = btMprVec3Dist2(b, P); 630 if (witness) 631 btMprVec3Copy(witness, b); 632 }else{ 633 if (witness){ 634 btMprVec3Copy(witness, &d); 635 btMprVec3Scale(witness, t); 636 btMprVec3Add(witness, x0); 637 dist = btMprVec3Dist2(witness, P); 638 }else{ 639 // recycling variables 640 btMprVec3Scale(&d, t); 641 btMprVec3Add(&d, &a); 642 dist = btMprVec3Len2(&d); 643 } 644 } 645 646 return dist; 647 } 648 649 650 651 inline float btMprVec3PointTriDist2(const btVector3 *P, 652 const btVector3 *x0, const btVector3 *B, 653 const btVector3 *C, 654 btVector3 *witness) 655 { 656 // Computation comes from analytic expression for triangle (x0, B, C) 657 // T(s, t) = x0 + s.d1 + t.d2, where d1 = B - x0 and d2 = C - x0 and 658 // Then equation for distance is: 659 // D(s, t) = | T(s, t) - P |^2 660 // This leads to minimization of quadratic function of two variables. 661 // The solution from is taken only if s is between 0 and 1, t is 662 // between 0 and 1 and t + s < 1, otherwise distance from segment is 663 // computed. 664 665 btVector3 d1, d2, a; 666 float u, v, w, p, q, r; 667 float s, t, dist, dist2; 668 btVector3 witness2; 669 670 btMprVec3Sub2(&d1, B, x0); 671 btMprVec3Sub2(&d2, C, x0); 672 btMprVec3Sub2(&a, x0, P); 673 674 u = btMprVec3Dot(&a, &a); 675 v = btMprVec3Dot(&d1, &d1); 676 w = btMprVec3Dot(&d2, &d2); 677 p = btMprVec3Dot(&a, &d1); 678 q = btMprVec3Dot(&a, &d2); 679 r = btMprVec3Dot(&d1, &d2); 680 681 btScalar div = (w * v - r * r); 682 if (btMprIsZero(div)) 683 { 684 s=-1; 685 } else 686 { 687 s = (q * r - w * p) / div; 688 t = (-s * r - q) / w; 689 } 690 691 if ((btMprIsZero(s) || s > 0.f) 692 && (btMprEq(s, 1.f) || s < 1.f) 693 && (btMprIsZero(t) || t > 0.f) 694 && (btMprEq(t, 1.f) || t < 1.f) 695 && (btMprEq(t + s, 1.f) || t + s < 1.f)){ 696 697 if (witness){ 698 btMprVec3Scale(&d1, s); 699 btMprVec3Scale(&d2, t); 700 btMprVec3Copy(witness, x0); 701 btMprVec3Add(witness, &d1); 702 btMprVec3Add(witness, &d2); 703 704 dist = btMprVec3Dist2(witness, P); 705 }else{ 706 dist = s * s * v; 707 dist += t * t * w; 708 dist += 2.f * s * t * r; 709 dist += 2.f * s * p; 710 dist += 2.f * t * q; 711 dist += u; 712 } 713 }else{ 714 dist = _btMprVec3PointSegmentDist2(P, x0, B, witness); 715 716 dist2 = _btMprVec3PointSegmentDist2(P, x0, C, &witness2); 717 if (dist2 < dist){ 718 dist = dist2; 719 if (witness) 720 btMprVec3Copy(witness, &witness2); 721 } 722 723 dist2 = _btMprVec3PointSegmentDist2(P, B, C, &witness2); 724 if (dist2 < dist){ 725 dist = dist2; 726 if (witness) 727 btMprVec3Copy(witness, &witness2); 728 } 729 } 730 731 return dist; 732 } 733 734 template <typename btConvexTemplate> 735 static void btFindPenetr(const btConvexTemplate& a, const btConvexTemplate& b, 736 const btMprCollisionDescription& colDesc, 737 btMprSimplex_t *portal, 738 float *depth, btVector3 *pdir, btVector3 *pos) 739 { 740 btVector3 dir; 741 btMprSupport_t v4; 742 unsigned long iterations; 743 744 btVector3 zero = btVector3(0,0,0); 745 btVector3* origin = &zero; 746 747 748 iterations = 1UL; 749 for (int i=0;i<BT_MPR_MAX_ITERATIONS;i++) 750 //while (1) 751 { 752 // compute portal direction and obtain next support point 753 btPortalDir(portal, &dir); 754 755 btMprSupport(a,b,colDesc, dir, &v4); 756 757 758 // reached tolerance -> find penetration info 759 if (portalReachTolerance(portal, &v4, &dir) 760 || iterations ==BT_MPR_MAX_ITERATIONS) 761 { 762 *depth = btMprVec3PointTriDist2(origin,&btMprSimplexPoint(portal, 1)->v,&btMprSimplexPoint(portal, 2)->v,&btMprSimplexPoint(portal, 3)->v,pdir); 763 *depth = BT_MPR_SQRT(*depth); 764 765 if (btMprIsZero((*pdir).x()) && btMprIsZero((*pdir).y()) && btMprIsZero((*pdir).z())) 766 { 767 768 *pdir = dir; 769 } 770 btMprVec3Normalize(pdir); 771 772 // barycentric coordinates: 773 btFindPos(portal, pos); 774 775 776 return; 777 } 778 779 btExpandPortal(portal, &v4); 780 781 iterations++; 782 } 783 } 784 785 static void btFindPenetrTouch(btMprSimplex_t *portal,float *depth, btVector3 *dir, btVector3 *pos) 786 { 787 // Touching contact on portal's v1 - so depth is zero and direction 788 // is unimportant and pos can be guessed 789 *depth = 0.f; 790 btVector3 zero = btVector3(0,0,0); 791 btVector3* origin = &zero; 792 793 794 btMprVec3Copy(dir, origin); 795 #ifdef MPR_AVERAGE_CONTACT_POSITIONS 796 btMprVec3Copy(pos, &btMprSimplexPoint(portal, 1)->v1); 797 btMprVec3Add(pos, &btMprSimplexPoint(portal, 1)->v2); 798 btMprVec3Scale(pos, 0.5); 799 #else 800 btMprVec3Copy(pos, &btMprSimplexPoint(portal, 1)->v2); 801 #endif 802 } 803 804 static void btFindPenetrSegment(btMprSimplex_t *portal, 805 float *depth, btVector3 *dir, btVector3 *pos) 806 { 807 808 // Origin lies on v0-v1 segment. 809 // Depth is distance to v1, direction also and position must be 810 // computed 811 #ifdef MPR_AVERAGE_CONTACT_POSITIONS 812 btMprVec3Copy(pos, &btMprSimplexPoint(portal, 1)->v1); 813 btMprVec3Add(pos, &btMprSimplexPoint(portal, 1)->v2); 814 btMprVec3Scale(pos, 0.5f); 815 #else 816 btMprVec3Copy(pos, &btMprSimplexPoint(portal, 1)->v2); 817 #endif//MPR_AVERAGE_CONTACT_POSITIONS 818 819 btMprVec3Copy(dir, &btMprSimplexPoint(portal, 1)->v); 820 *depth = BT_MPR_SQRT(btMprVec3Len2(dir)); 821 btMprVec3Normalize(dir); 822 823 824 } 825 826 827 template <typename btConvexTemplate> 828 inline int btMprPenetration( const btConvexTemplate& a, const btConvexTemplate& b, 829 const btMprCollisionDescription& colDesc, 830 float *depthOut, btVector3* dirOut, btVector3* posOut) 831 { 832 833 btMprSimplex_t portal; 834 835 836 // Phase 1: Portal discovery 837 int result = btDiscoverPortal(a,b,colDesc, &portal); 838 839 840 //sepAxis[pairIndex] = *pdir;//or -dir? 841 842 switch (result) 843 { 844 case 0: 845 { 846 // Phase 2: Portal refinement 847 848 result = btRefinePortal(a,b,colDesc, &portal); 849 if (result < 0) 850 return -1; 851 852 // Phase 3. Penetration info 853 btFindPenetr(a,b,colDesc, &portal, depthOut, dirOut, posOut); 854 855 856 break; 857 } 858 case 1: 859 { 860 // Touching contact on portal's v1. 861 btFindPenetrTouch(&portal, depthOut, dirOut, posOut); 862 result=0; 863 break; 864 } 865 case 2: 866 { 867 868 btFindPenetrSegment( &portal, depthOut, dirOut, posOut); 869 result=0; 870 break; 871 } 872 default: 873 { 874 //if (res < 0) 875 //{ 876 // Origin isn't inside portal - no collision. 877 result = -1; 878 //} 879 } 880 }; 881 882 return result; 883 }; 884 885 886 template<typename btConvexTemplate, typename btMprDistanceTemplate> 887 inline int btComputeMprPenetration( const btConvexTemplate& a, const btConvexTemplate& b, const 888 btMprCollisionDescription& colDesc, btMprDistanceTemplate* distInfo) 889 { 890 btVector3 dir,pos; 891 float depth; 892 893 int res = btMprPenetration(a,b,colDesc,&depth, &dir, &pos); 894 if (res==0) 895 { 896 distInfo->m_distance = -depth; 897 distInfo->m_pointOnB = pos; 898 distInfo->m_normalBtoA = -dir; 899 distInfo->m_pointOnA = pos-distInfo->m_distance*dir; 900 return 0; 901 } 902 903 return -1; 904 } 905 906 907 908 #endif //BT_MPR_PENETRATION_H 909
