1 /* 2 Bullet Continuous Collision Detection and Physics Library 3 Copyright (c) 2003-2014 Erwin Coumans http://continuousphysics.com/Bullet/ 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 7 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 10 freely, 11 subject to the following restrictions: 12 13 1. The origin of this software must not be misrepresented; you must not 14 claim that you wrote the original software. If you use this software in a 15 product, an acknowledgment in the product documentation would be appreciated 16 but is not required. 17 2. Altered source versions must be plainly marked as such, and must not be 18 misrepresented as being the original software. 19 3. This notice may not be removed or altered from any source distribution. 20 */ 21 22 /* 23 Initial GJK-EPA collision solver by Nathanael Presson, 2008 24 Improvements and refactoring by Erwin Coumans, 2008-2014 25 */ 26 #ifndef BT_GJK_EPA3_H 27 #define BT_GJK_EPA3_H 28 29 #include "LinearMath/btTransform.h" 30 #include "btGjkCollisionDescription.h" 31 32 33 34 struct btGjkEpaSolver3 35 { 36 struct sResults 37 { 38 enum eStatus 39 { 40 Separated, /* Shapes doesnt penetrate */ 41 Penetrating, /* Shapes are penetrating */ 42 GJK_Failed, /* GJK phase fail, no big issue, shapes are probably just 'touching' */ 43 EPA_Failed /* EPA phase fail, bigger problem, need to save parameters, and debug */ 44 } status; 45 btVector3 witnesses[2]; 46 btVector3 normal; 47 btScalar distance; 48 }; 49 50 51 }; 52 53 54 55 #if defined(DEBUG) || defined (_DEBUG) 56 #include <stdio.h> //for debug printf 57 #ifdef __SPU__ 58 #include <spu_printf.h> 59 #define printf spu_printf 60 #endif //__SPU__ 61 #endif 62 63 64 65 // Config 66 67 /* GJK */ 68 #define GJK_MAX_ITERATIONS 128 69 #define GJK_ACCURARY ((btScalar)0.0001) 70 #define GJK_MIN_DISTANCE ((btScalar)0.0001) 71 #define GJK_DUPLICATED_EPS ((btScalar)0.0001) 72 #define GJK_SIMPLEX2_EPS ((btScalar)0.0) 73 #define GJK_SIMPLEX3_EPS ((btScalar)0.0) 74 #define GJK_SIMPLEX4_EPS ((btScalar)0.0) 75 76 /* EPA */ 77 #define EPA_MAX_VERTICES 64 78 #define EPA_MAX_FACES (EPA_MAX_VERTICES*2) 79 #define EPA_MAX_ITERATIONS 255 80 #define EPA_ACCURACY ((btScalar)0.0001) 81 #define EPA_FALLBACK (10*EPA_ACCURACY) 82 #define EPA_PLANE_EPS ((btScalar)0.00001) 83 #define EPA_INSIDE_EPS ((btScalar)0.01) 84 85 86 // Shorthands 87 typedef unsigned int U; 88 typedef unsigned char U1; 89 90 // MinkowskiDiff 91 template <typename btConvexTemplate> 92 struct MinkowskiDiff 93 { 94 const btConvexTemplate* m_convexAPtr; 95 const btConvexTemplate* m_convexBPtr; 96 97 btMatrix3x3 m_toshape1; 98 btTransform m_toshape0; 99 100 bool m_enableMargin; 101 102 103 MinkowskiDiff(const btConvexTemplate& a, const btConvexTemplate& b) 104 :m_convexAPtr(&a), 105 m_convexBPtr(&b) 106 { 107 } 108 109 void EnableMargin(bool enable) 110 { 111 m_enableMargin = enable; 112 } 113 inline btVector3 Support0(const btVector3& d) const 114 { 115 return m_convexAPtr->getLocalSupportWithMargin(d); 116 } 117 inline btVector3 Support1(const btVector3& d) const 118 { 119 return m_toshape0*m_convexBPtr->getLocalSupportWithMargin(m_toshape1*d); 120 } 121 122 123 inline btVector3 Support(const btVector3& d) const 124 { 125 return(Support0(d)-Support1(-d)); 126 } 127 btVector3 Support(const btVector3& d,U index) const 128 { 129 if(index) 130 return(Support1(d)); 131 else 132 return(Support0(d)); 133 } 134 }; 135 136 enum eGjkStatus 137 { 138 eGjkValid, 139 eGjkInside, 140 eGjkFailed 141 }; 142 143 // GJK 144 template <typename btConvexTemplate> 145 struct GJK 146 { 147 /* Types */ 148 struct sSV 149 { 150 btVector3 d,w; 151 }; 152 struct sSimplex 153 { 154 sSV* c[4]; 155 btScalar p[4]; 156 U rank; 157 }; 158 159 /* Fields */ 160 161 MinkowskiDiff<btConvexTemplate> m_shape; 162 btVector3 m_ray; 163 btScalar m_distance; 164 sSimplex m_simplices[2]; 165 sSV m_store[4]; 166 sSV* m_free[4]; 167 U m_nfree; 168 U m_current; 169 sSimplex* m_simplex; 170 eGjkStatus m_status; 171 /* Methods */ 172 173 GJK(const btConvexTemplate& a, const btConvexTemplate& b) 174 :m_shape(a,b) 175 { 176 Initialize(); 177 } 178 void Initialize() 179 { 180 m_ray = btVector3(0,0,0); 181 m_nfree = 0; 182 m_status = eGjkFailed; 183 m_current = 0; 184 m_distance = 0; 185 } 186 eGjkStatus Evaluate(const MinkowskiDiff<btConvexTemplate>& shapearg,const btVector3& guess) 187 { 188 U iterations=0; 189 btScalar sqdist=0; 190 btScalar alpha=0; 191 btVector3 lastw[4]; 192 U clastw=0; 193 /* Initialize solver */ 194 m_free[0] = &m_store[0]; 195 m_free[1] = &m_store[1]; 196 m_free[2] = &m_store[2]; 197 m_free[3] = &m_store[3]; 198 m_nfree = 4; 199 m_current = 0; 200 m_status = eGjkValid; 201 m_shape = shapearg; 202 m_distance = 0; 203 /* Initialize simplex */ 204 m_simplices[0].rank = 0; 205 m_ray = guess; 206 const btScalar sqrl= m_ray.length2(); 207 appendvertice(m_simplices[0],sqrl>0?-m_ray:btVector3(1,0,0)); 208 m_simplices[0].p[0] = 1; 209 m_ray = m_simplices[0].c[0]->w; 210 sqdist = sqrl; 211 lastw[0] = 212 lastw[1] = 213 lastw[2] = 214 lastw[3] = m_ray; 215 /* Loop */ 216 do { 217 const U next=1-m_current; 218 sSimplex& cs=m_simplices[m_current]; 219 sSimplex& ns=m_simplices[next]; 220 /* Check zero */ 221 const btScalar rl=m_ray.length(); 222 if(rl<GJK_MIN_DISTANCE) 223 {/* Touching or inside */ 224 m_status=eGjkInside; 225 break; 226 } 227 /* Append new vertice in -'v' direction */ 228 appendvertice(cs,-m_ray); 229 const btVector3& w=cs.c[cs.rank-1]->w; 230 bool found=false; 231 for(U i=0;i<4;++i) 232 { 233 if((w-lastw[i]).length2()<GJK_DUPLICATED_EPS) 234 { found=true;break; } 235 } 236 if(found) 237 {/* Return old simplex */ 238 removevertice(m_simplices[m_current]); 239 break; 240 } 241 else 242 {/* Update lastw */ 243 lastw[clastw=(clastw+1)&3]=w; 244 } 245 /* Check for termination */ 246 const btScalar omega=btDot(m_ray,w)/rl; 247 alpha=btMax(omega,alpha); 248 if(((rl-alpha)-(GJK_ACCURARY*rl))<=0) 249 {/* Return old simplex */ 250 removevertice(m_simplices[m_current]); 251 break; 252 } 253 /* Reduce simplex */ 254 btScalar weights[4]; 255 U mask=0; 256 switch(cs.rank) 257 { 258 case 2: sqdist=projectorigin( cs.c[0]->w, 259 cs.c[1]->w, 260 weights,mask);break; 261 case 3: sqdist=projectorigin( cs.c[0]->w, 262 cs.c[1]->w, 263 cs.c[2]->w, 264 weights,mask);break; 265 case 4: sqdist=projectorigin( cs.c[0]->w, 266 cs.c[1]->w, 267 cs.c[2]->w, 268 cs.c[3]->w, 269 weights,mask);break; 270 } 271 if(sqdist>=0) 272 {/* Valid */ 273 ns.rank = 0; 274 m_ray = btVector3(0,0,0); 275 m_current = next; 276 for(U i=0,ni=cs.rank;i<ni;++i) 277 { 278 if(mask&(1<<i)) 279 { 280 ns.c[ns.rank] = cs.c[i]; 281 ns.p[ns.rank++] = weights[i]; 282 m_ray += cs.c[i]->w*weights[i]; 283 } 284 else 285 { 286 m_free[m_nfree++] = cs.c[i]; 287 } 288 } 289 if(mask==15) m_status=eGjkInside; 290 } 291 else 292 {/* Return old simplex */ 293 removevertice(m_simplices[m_current]); 294 break; 295 } 296 m_status=((++iterations)<GJK_MAX_ITERATIONS)?m_status:eGjkFailed; 297 } while(m_status==eGjkValid); 298 m_simplex=&m_simplices[m_current]; 299 switch(m_status) 300 { 301 case eGjkValid: m_distance=m_ray.length();break; 302 case eGjkInside: m_distance=0;break; 303 default: 304 { 305 } 306 } 307 return(m_status); 308 } 309 bool EncloseOrigin() 310 { 311 switch(m_simplex->rank) 312 { 313 case 1: 314 { 315 for(U i=0;i<3;++i) 316 { 317 btVector3 axis=btVector3(0,0,0); 318 axis[i]=1; 319 appendvertice(*m_simplex, axis); 320 if(EncloseOrigin()) return(true); 321 removevertice(*m_simplex); 322 appendvertice(*m_simplex,-axis); 323 if(EncloseOrigin()) return(true); 324 removevertice(*m_simplex); 325 } 326 } 327 break; 328 case 2: 329 { 330 const btVector3 d=m_simplex->c[1]->w-m_simplex->c[0]->w; 331 for(U i=0;i<3;++i) 332 { 333 btVector3 axis=btVector3(0,0,0); 334 axis[i]=1; 335 const btVector3 p=btCross(d,axis); 336 if(p.length2()>0) 337 { 338 appendvertice(*m_simplex, p); 339 if(EncloseOrigin()) return(true); 340 removevertice(*m_simplex); 341 appendvertice(*m_simplex,-p); 342 if(EncloseOrigin()) return(true); 343 removevertice(*m_simplex); 344 } 345 } 346 } 347 break; 348 case 3: 349 { 350 const btVector3 n=btCross(m_simplex->c[1]->w-m_simplex->c[0]->w, 351 m_simplex->c[2]->w-m_simplex->c[0]->w); 352 if(n.length2()>0) 353 { 354 appendvertice(*m_simplex,n); 355 if(EncloseOrigin()) return(true); 356 removevertice(*m_simplex); 357 appendvertice(*m_simplex,-n); 358 if(EncloseOrigin()) return(true); 359 removevertice(*m_simplex); 360 } 361 } 362 break; 363 case 4: 364 { 365 if(btFabs(det( m_simplex->c[0]->w-m_simplex->c[3]->w, 366 m_simplex->c[1]->w-m_simplex->c[3]->w, 367 m_simplex->c[2]->w-m_simplex->c[3]->w))>0) 368 return(true); 369 } 370 break; 371 } 372 return(false); 373 } 374 /* Internals */ 375 void getsupport(const btVector3& d,sSV& sv) const 376 { 377 sv.d = d/d.length(); 378 sv.w = m_shape.Support(sv.d); 379 } 380 void removevertice(sSimplex& simplex) 381 { 382 m_free[m_nfree++]=simplex.c[--simplex.rank]; 383 } 384 void appendvertice(sSimplex& simplex,const btVector3& v) 385 { 386 simplex.p[simplex.rank]=0; 387 simplex.c[simplex.rank]=m_free[--m_nfree]; 388 getsupport(v,*simplex.c[simplex.rank++]); 389 } 390 static btScalar det(const btVector3& a,const btVector3& b,const btVector3& c) 391 { 392 return( a.y()*b.z()*c.x()+a.z()*b.x()*c.y()- 393 a.x()*b.z()*c.y()-a.y()*b.x()*c.z()+ 394 a.x()*b.y()*c.z()-a.z()*b.y()*c.x()); 395 } 396 static btScalar projectorigin( const btVector3& a, 397 const btVector3& b, 398 btScalar* w,U& m) 399 { 400 const btVector3 d=b-a; 401 const btScalar l=d.length2(); 402 if(l>GJK_SIMPLEX2_EPS) 403 { 404 const btScalar t(l>0?-btDot(a,d)/l:0); 405 if(t>=1) { w[0]=0;w[1]=1;m=2;return(b.length2()); } 406 else if(t<=0) { w[0]=1;w[1]=0;m=1;return(a.length2()); } 407 else { w[0]=1-(w[1]=t);m=3;return((a+d*t).length2()); } 408 } 409 return(-1); 410 } 411 static btScalar projectorigin( const btVector3& a, 412 const btVector3& b, 413 const btVector3& c, 414 btScalar* w,U& m) 415 { 416 static const U imd3[]={1,2,0}; 417 const btVector3* vt[]={&a,&b,&c}; 418 const btVector3 dl[]={a-b,b-c,c-a}; 419 const btVector3 n=btCross(dl[0],dl[1]); 420 const btScalar l=n.length2(); 421 if(l>GJK_SIMPLEX3_EPS) 422 { 423 btScalar mindist=-1; 424 btScalar subw[2]={0.f,0.f}; 425 U subm(0); 426 for(U i=0;i<3;++i) 427 { 428 if(btDot(*vt[i],btCross(dl[i],n))>0) 429 { 430 const U j=imd3[i]; 431 const btScalar subd(projectorigin(*vt[i],*vt[j],subw,subm)); 432 if((mindist<0)||(subd<mindist)) 433 { 434 mindist = subd; 435 m = static_cast<U>(((subm&1)?1<<i:0)+((subm&2)?1<<j:0)); 436 w[i] = subw[0]; 437 w[j] = subw[1]; 438 w[imd3[j]] = 0; 439 } 440 } 441 } 442 if(mindist<0) 443 { 444 const btScalar d=btDot(a,n); 445 const btScalar s=btSqrt(l); 446 const btVector3 p=n*(d/l); 447 mindist = p.length2(); 448 m = 7; 449 w[0] = (btCross(dl[1],b-p)).length()/s; 450 w[1] = (btCross(dl[2],c-p)).length()/s; 451 w[2] = 1-(w[0]+w[1]); 452 } 453 return(mindist); 454 } 455 return(-1); 456 } 457 static btScalar projectorigin( const btVector3& a, 458 const btVector3& b, 459 const btVector3& c, 460 const btVector3& d, 461 btScalar* w,U& m) 462 { 463 static const U imd3[]={1,2,0}; 464 const btVector3* vt[]={&a,&b,&c,&d}; 465 const btVector3 dl[]={a-d,b-d,c-d}; 466 const btScalar vl=det(dl[0],dl[1],dl[2]); 467 const bool ng=(vl*btDot(a,btCross(b-c,a-b)))<=0; 468 if(ng&&(btFabs(vl)>GJK_SIMPLEX4_EPS)) 469 { 470 btScalar mindist=-1; 471 btScalar subw[3]={0.f,0.f,0.f}; 472 U subm(0); 473 for(U i=0;i<3;++i) 474 { 475 const U j=imd3[i]; 476 const btScalar s=vl*btDot(d,btCross(dl[i],dl[j])); 477 if(s>0) 478 { 479 const btScalar subd=projectorigin(*vt[i],*vt[j],d,subw,subm); 480 if((mindist<0)||(subd<mindist)) 481 { 482 mindist = subd; 483 m = static_cast<U>((subm&1?1<<i:0)+ 484 (subm&2?1<<j:0)+ 485 (subm&4?8:0)); 486 w[i] = subw[0]; 487 w[j] = subw[1]; 488 w[imd3[j]] = 0; 489 w[3] = subw[2]; 490 } 491 } 492 } 493 if(mindist<0) 494 { 495 mindist = 0; 496 m = 15; 497 w[0] = det(c,b,d)/vl; 498 w[1] = det(a,c,d)/vl; 499 w[2] = det(b,a,d)/vl; 500 w[3] = 1-(w[0]+w[1]+w[2]); 501 } 502 return(mindist); 503 } 504 return(-1); 505 } 506 }; 507 508 509 enum eEpaStatus 510 { 511 eEpaValid, 512 eEpaTouching, 513 eEpaDegenerated, 514 eEpaNonConvex, 515 eEpaInvalidHull, 516 eEpaOutOfFaces, 517 eEpaOutOfVertices, 518 eEpaAccuraryReached, 519 eEpaFallBack, 520 eEpaFailed 521 }; 522 523 524 // EPA 525 template <typename btConvexTemplate> 526 struct EPA 527 { 528 /* Types */ 529 530 struct sFace 531 { 532 btVector3 n; 533 btScalar d; 534 typename GJK<btConvexTemplate>::sSV* c[3]; 535 sFace* f[3]; 536 sFace* l[2]; 537 U1 e[3]; 538 U1 pass; 539 }; 540 struct sList 541 { 542 sFace* root; 543 U count; 544 sList() : root(0),count(0) {} 545 }; 546 struct sHorizon 547 { 548 sFace* cf; 549 sFace* ff; 550 U nf; 551 sHorizon() : cf(0),ff(0),nf(0) {} 552 }; 553 554 /* Fields */ 555 eEpaStatus m_status; 556 typename GJK<btConvexTemplate>::sSimplex m_result; 557 btVector3 m_normal; 558 btScalar m_depth; 559 typename GJK<btConvexTemplate>::sSV m_sv_store[EPA_MAX_VERTICES]; 560 sFace m_fc_store[EPA_MAX_FACES]; 561 U m_nextsv; 562 sList m_hull; 563 sList m_stock; 564 /* Methods */ 565 EPA() 566 { 567 Initialize(); 568 } 569 570 571 static inline void bind(sFace* fa,U ea,sFace* fb,U eb) 572 { 573 fa->e[ea]=(U1)eb;fa->f[ea]=fb; 574 fb->e[eb]=(U1)ea;fb->f[eb]=fa; 575 } 576 static inline void append(sList& list,sFace* face) 577 { 578 face->l[0] = 0; 579 face->l[1] = list.root; 580 if(list.root) list.root->l[0]=face; 581 list.root = face; 582 ++list.count; 583 } 584 static inline void remove(sList& list,sFace* face) 585 { 586 if(face->l[1]) face->l[1]->l[0]=face->l[0]; 587 if(face->l[0]) face->l[0]->l[1]=face->l[1]; 588 if(face==list.root) list.root=face->l[1]; 589 --list.count; 590 } 591 592 593 void Initialize() 594 { 595 m_status = eEpaFailed; 596 m_normal = btVector3(0,0,0); 597 m_depth = 0; 598 m_nextsv = 0; 599 for(U i=0;i<EPA_MAX_FACES;++i) 600 { 601 append(m_stock,&m_fc_store[EPA_MAX_FACES-i-1]); 602 } 603 } 604 eEpaStatus Evaluate(GJK<btConvexTemplate>& gjk,const btVector3& guess) 605 { 606 typename GJK<btConvexTemplate>::sSimplex& simplex=*gjk.m_simplex; 607 if((simplex.rank>1)&&gjk.EncloseOrigin()) 608 { 609 610 /* Clean up */ 611 while(m_hull.root) 612 { 613 sFace* f = m_hull.root; 614 remove(m_hull,f); 615 append(m_stock,f); 616 } 617 m_status = eEpaValid; 618 m_nextsv = 0; 619 /* Orient simplex */ 620 if(gjk.det( simplex.c[0]->w-simplex.c[3]->w, 621 simplex.c[1]->w-simplex.c[3]->w, 622 simplex.c[2]->w-simplex.c[3]->w)<0) 623 { 624 btSwap(simplex.c[0],simplex.c[1]); 625 btSwap(simplex.p[0],simplex.p[1]); 626 } 627 /* Build initial hull */ 628 sFace* tetra[]={newface(simplex.c[0],simplex.c[1],simplex.c[2],true), 629 newface(simplex.c[1],simplex.c[0],simplex.c[3],true), 630 newface(simplex.c[2],simplex.c[1],simplex.c[3],true), 631 newface(simplex.c[0],simplex.c[2],simplex.c[3],true)}; 632 if(m_hull.count==4) 633 { 634 sFace* best=findbest(); 635 sFace outer=*best; 636 U pass=0; 637 U iterations=0; 638 bind(tetra[0],0,tetra[1],0); 639 bind(tetra[0],1,tetra[2],0); 640 bind(tetra[0],2,tetra[3],0); 641 bind(tetra[1],1,tetra[3],2); 642 bind(tetra[1],2,tetra[2],1); 643 bind(tetra[2],2,tetra[3],1); 644 m_status=eEpaValid; 645 for(;iterations<EPA_MAX_ITERATIONS;++iterations) 646 { 647 if(m_nextsv<EPA_MAX_VERTICES) 648 { 649 sHorizon horizon; 650 typename GJK<btConvexTemplate>::sSV* w=&m_sv_store[m_nextsv++]; 651 bool valid=true; 652 best->pass = (U1)(++pass); 653 gjk.getsupport(best->n,*w); 654 const btScalar wdist=btDot(best->n,w->w)-best->d; 655 if(wdist>EPA_ACCURACY) 656 { 657 for(U j=0;(j<3)&&valid;++j) 658 { 659 valid&=expand( pass,w, 660 best->f[j],best->e[j], 661 horizon); 662 } 663 if(valid&&(horizon.nf>=3)) 664 { 665 bind(horizon.cf,1,horizon.ff,2); 666 remove(m_hull,best); 667 append(m_stock,best); 668 best=findbest(); 669 outer=*best; 670 } else { m_status=eEpaInvalidHull;break; } 671 } else { m_status=eEpaAccuraryReached;break; } 672 } else { m_status=eEpaOutOfVertices;break; } 673 } 674 const btVector3 projection=outer.n*outer.d; 675 m_normal = outer.n; 676 m_depth = outer.d; 677 m_result.rank = 3; 678 m_result.c[0] = outer.c[0]; 679 m_result.c[1] = outer.c[1]; 680 m_result.c[2] = outer.c[2]; 681 m_result.p[0] = btCross( outer.c[1]->w-projection, 682 outer.c[2]->w-projection).length(); 683 m_result.p[1] = btCross( outer.c[2]->w-projection, 684 outer.c[0]->w-projection).length(); 685 m_result.p[2] = btCross( outer.c[0]->w-projection, 686 outer.c[1]->w-projection).length(); 687 const btScalar sum=m_result.p[0]+m_result.p[1]+m_result.p[2]; 688 m_result.p[0] /= sum; 689 m_result.p[1] /= sum; 690 m_result.p[2] /= sum; 691 return(m_status); 692 } 693 } 694 /* Fallback */ 695 m_status = eEpaFallBack; 696 m_normal = -guess; 697 const btScalar nl=m_normal.length(); 698 if(nl>0) 699 m_normal = m_normal/nl; 700 else 701 m_normal = btVector3(1,0,0); 702 m_depth = 0; 703 m_result.rank=1; 704 m_result.c[0]=simplex.c[0]; 705 m_result.p[0]=1; 706 return(m_status); 707 } 708 bool getedgedist(sFace* face, typename GJK<btConvexTemplate>::sSV* a, typename GJK<btConvexTemplate>::sSV* b, btScalar& dist) 709 { 710 const btVector3 ba = b->w - a->w; 711 const btVector3 n_ab = btCross(ba, face->n); // Outward facing edge normal direction, on triangle plane 712 const btScalar a_dot_nab = btDot(a->w, n_ab); // Only care about the sign to determine inside/outside, so not normalization required 713 714 if(a_dot_nab < 0) 715 { 716 // Outside of edge a->b 717 718 const btScalar ba_l2 = ba.length2(); 719 const btScalar a_dot_ba = btDot(a->w, ba); 720 const btScalar b_dot_ba = btDot(b->w, ba); 721 722 if(a_dot_ba > 0) 723 { 724 // Pick distance vertex a 725 dist = a->w.length(); 726 } 727 else if(b_dot_ba < 0) 728 { 729 // Pick distance vertex b 730 dist = b->w.length(); 731 } 732 else 733 { 734 // Pick distance to edge a->b 735 const btScalar a_dot_b = btDot(a->w, b->w); 736 dist = btSqrt(btMax((a->w.length2() * b->w.length2() - a_dot_b * a_dot_b) / ba_l2, (btScalar)0)); 737 } 738 739 return true; 740 } 741 742 return false; 743 } 744 sFace* newface(typename GJK<btConvexTemplate>::sSV* a,typename GJK<btConvexTemplate>::sSV* b,typename GJK<btConvexTemplate>::sSV* c,bool forced) 745 { 746 if(m_stock.root) 747 { 748 sFace* face=m_stock.root; 749 remove(m_stock,face); 750 append(m_hull,face); 751 face->pass = 0; 752 face->c[0] = a; 753 face->c[1] = b; 754 face->c[2] = c; 755 face->n = btCross(b->w-a->w,c->w-a->w); 756 const btScalar l=face->n.length(); 757 const bool v=l>EPA_ACCURACY; 758 759 if(v) 760 { 761 if(!(getedgedist(face, a, b, face->d) || 762 getedgedist(face, b, c, face->d) || 763 getedgedist(face, c, a, face->d))) 764 { 765 // Origin projects to the interior of the triangle 766 // Use distance to triangle plane 767 face->d = btDot(a->w, face->n) / l; 768 } 769 770 face->n /= l; 771 if(forced || (face->d >= -EPA_PLANE_EPS)) 772 { 773 return face; 774 } 775 else 776 m_status=eEpaNonConvex; 777 } 778 else 779 m_status=eEpaDegenerated; 780 781 remove(m_hull, face); 782 append(m_stock, face); 783 return 0; 784 785 } 786 m_status = m_stock.root ? eEpaOutOfVertices : eEpaOutOfFaces; 787 return 0; 788 } 789 sFace* findbest() 790 { 791 sFace* minf=m_hull.root; 792 btScalar mind=minf->d*minf->d; 793 for(sFace* f=minf->l[1];f;f=f->l[1]) 794 { 795 const btScalar sqd=f->d*f->d; 796 if(sqd<mind) 797 { 798 minf=f; 799 mind=sqd; 800 } 801 } 802 return(minf); 803 } 804 bool expand(U pass,typename GJK<btConvexTemplate>::sSV* w,sFace* f,U e,sHorizon& horizon) 805 { 806 static const U i1m3[]={1,2,0}; 807 static const U i2m3[]={2,0,1}; 808 if(f->pass!=pass) 809 { 810 const U e1=i1m3[e]; 811 if((btDot(f->n,w->w)-f->d)<-EPA_PLANE_EPS) 812 { 813 sFace* nf=newface(f->c[e1],f->c[e],w,false); 814 if(nf) 815 { 816 bind(nf,0,f,e); 817 if(horizon.cf) bind(horizon.cf,1,nf,2); else horizon.ff=nf; 818 horizon.cf=nf; 819 ++horizon.nf; 820 return(true); 821 } 822 } 823 else 824 { 825 const U e2=i2m3[e]; 826 f->pass = (U1)pass; 827 if( expand(pass,w,f->f[e1],f->e[e1],horizon)&& 828 expand(pass,w,f->f[e2],f->e[e2],horizon)) 829 { 830 remove(m_hull,f); 831 append(m_stock,f); 832 return(true); 833 } 834 } 835 } 836 return(false); 837 } 838 839 }; 840 841 template <typename btConvexTemplate> 842 static void Initialize( const btConvexTemplate& a, const btConvexTemplate& b, 843 btGjkEpaSolver3::sResults& results, 844 MinkowskiDiff<btConvexTemplate>& shape) 845 { 846 /* Results */ 847 results.witnesses[0] = 848 results.witnesses[1] = btVector3(0,0,0); 849 results.status = btGjkEpaSolver3::sResults::Separated; 850 /* Shape */ 851 852 shape.m_toshape1 = b.getWorldTransform().getBasis().transposeTimes(a.getWorldTransform().getBasis()); 853 shape.m_toshape0 = a.getWorldTransform().inverseTimes(b.getWorldTransform()); 854 855 } 856 857 858 // 859 // Api 860 // 861 862 863 864 // 865 template <typename btConvexTemplate> 866 bool btGjkEpaSolver3_Distance(const btConvexTemplate& a, const btConvexTemplate& b, 867 const btVector3& guess, 868 btGjkEpaSolver3::sResults& results) 869 { 870 MinkowskiDiff<btConvexTemplate> shape(a,b); 871 Initialize(a,b,results,shape); 872 GJK<btConvexTemplate> gjk(a,b); 873 eGjkStatus gjk_status=gjk.Evaluate(shape,guess); 874 if(gjk_status==eGjkValid) 875 { 876 btVector3 w0=btVector3(0,0,0); 877 btVector3 w1=btVector3(0,0,0); 878 for(U i=0;i<gjk.m_simplex->rank;++i) 879 { 880 const btScalar p=gjk.m_simplex->p[i]; 881 w0+=shape.Support( gjk.m_simplex->c[i]->d,0)*p; 882 w1+=shape.Support(-gjk.m_simplex->c[i]->d,1)*p; 883 } 884 results.witnesses[0] = a.getWorldTransform()*w0; 885 results.witnesses[1] = a.getWorldTransform()*w1; 886 results.normal = w0-w1; 887 results.distance = results.normal.length(); 888 results.normal /= results.distance>GJK_MIN_DISTANCE?results.distance:1; 889 return(true); 890 } 891 else 892 { 893 results.status = gjk_status==eGjkInside? 894 btGjkEpaSolver3::sResults::Penetrating : 895 btGjkEpaSolver3::sResults::GJK_Failed ; 896 return(false); 897 } 898 } 899 900 901 template <typename btConvexTemplate> 902 bool btGjkEpaSolver3_Penetration(const btConvexTemplate& a, 903 const btConvexTemplate& b, 904 const btVector3& guess, 905 btGjkEpaSolver3::sResults& results) 906 { 907 MinkowskiDiff<btConvexTemplate> shape(a,b); 908 Initialize(a,b,results,shape); 909 GJK<btConvexTemplate> gjk(a,b); 910 eGjkStatus gjk_status=gjk.Evaluate(shape,-guess); 911 switch(gjk_status) 912 { 913 case eGjkInside: 914 { 915 EPA<btConvexTemplate> epa; 916 eEpaStatus epa_status=epa.Evaluate(gjk,-guess); 917 if(epa_status!=eEpaFailed) 918 { 919 btVector3 w0=btVector3(0,0,0); 920 for(U i=0;i<epa.m_result.rank;++i) 921 { 922 w0+=shape.Support(epa.m_result.c[i]->d,0)*epa.m_result.p[i]; 923 } 924 results.status = btGjkEpaSolver3::sResults::Penetrating; 925 results.witnesses[0] = a.getWorldTransform()*w0; 926 results.witnesses[1] = a.getWorldTransform()*(w0-epa.m_normal*epa.m_depth); 927 results.normal = -epa.m_normal; 928 results.distance = -epa.m_depth; 929 return(true); 930 } else results.status=btGjkEpaSolver3::sResults::EPA_Failed; 931 } 932 break; 933 case eGjkFailed: 934 results.status=btGjkEpaSolver3::sResults::GJK_Failed; 935 break; 936 default: 937 { 938 } 939 } 940 return(false); 941 } 942 943 #if 0 944 int btComputeGjkEpaPenetration2(const btCollisionDescription& colDesc, btDistanceInfo* distInfo) 945 { 946 btGjkEpaSolver3::sResults results; 947 btVector3 guess = colDesc.m_firstDir; 948 949 bool res = btGjkEpaSolver3::Penetration(colDesc.m_objA,colDesc.m_objB, 950 colDesc.m_transformA,colDesc.m_transformB, 951 colDesc.m_localSupportFuncA,colDesc.m_localSupportFuncB, 952 guess, 953 results); 954 if (res) 955 { 956 if ((results.status==btGjkEpaSolver3::sResults::Penetrating) || results.status==GJK::eStatus::Inside) 957 { 958 //normal could be 'swapped' 959 960 distInfo->m_distance = results.distance; 961 distInfo->m_normalBtoA = results.normal; 962 btVector3 tmpNormalInB = results.witnesses[1]-results.witnesses[0]; 963 btScalar lenSqr = tmpNormalInB.length2(); 964 if (lenSqr <= (SIMD_EPSILON*SIMD_EPSILON)) 965 { 966 tmpNormalInB = results.normal; 967 lenSqr = results.normal.length2(); 968 } 969 970 if (lenSqr > (SIMD_EPSILON*SIMD_EPSILON)) 971 { 972 tmpNormalInB /= btSqrt(lenSqr); 973 btScalar distance2 = -(results.witnesses[0]-results.witnesses[1]).length(); 974 //only replace valid penetrations when the result is deeper (check) 975 //if ((distance2 < results.distance)) 976 { 977 distInfo->m_distance = distance2; 978 distInfo->m_pointOnA= results.witnesses[0]; 979 distInfo->m_pointOnB= results.witnesses[1]; 980 distInfo->m_normalBtoA= tmpNormalInB; 981 return 0; 982 } 983 } 984 } 985 986 } 987 988 return -1; 989 } 990 #endif 991 992 template <typename btConvexTemplate, typename btDistanceInfoTemplate> 993 int btComputeGjkDistance(const btConvexTemplate& a, const btConvexTemplate& b, 994 const btGjkCollisionDescription& colDesc, btDistanceInfoTemplate* distInfo) 995 { 996 btGjkEpaSolver3::sResults results; 997 btVector3 guess = colDesc.m_firstDir; 998 999 bool isSeparated = btGjkEpaSolver3_Distance( a,b, 1000 guess, 1001 results); 1002 if (isSeparated) 1003 { 1004 distInfo->m_distance = results.distance; 1005 distInfo->m_pointOnA= results.witnesses[0]; 1006 distInfo->m_pointOnB= results.witnesses[1]; 1007 distInfo->m_normalBtoA= results.normal; 1008 return 0; 1009 } 1010 1011 return -1; 1012 } 1013 1014 /* Symbols cleanup */ 1015 1016 #undef GJK_MAX_ITERATIONS 1017 #undef GJK_ACCURARY 1018 #undef GJK_MIN_DISTANCE 1019 #undef GJK_DUPLICATED_EPS 1020 #undef GJK_SIMPLEX2_EPS 1021 #undef GJK_SIMPLEX3_EPS 1022 #undef GJK_SIMPLEX4_EPS 1023 1024 #undef EPA_MAX_VERTICES 1025 #undef EPA_MAX_FACES 1026 #undef EPA_MAX_ITERATIONS 1027 #undef EPA_ACCURACY 1028 #undef EPA_FALLBACK 1029 #undef EPA_PLANE_EPS 1030 #undef EPA_INSIDE_EPS 1031 1032 1033 1034 #endif //BT_GJK_EPA3_H 1035 1036
