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