00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025 #include "BulletCollision/CollisionShapes/btConvexInternalShape.h"
00026 #include "BulletCollision/CollisionShapes/btSphereShape.h"
00027 #include "btGjkEpa2.h"
00028
00029 #if defined(DEBUG) || defined (_DEBUG)
00030 #include <stdio.h>
00031 #ifdef __SPU__
00032 #include <spu_printf.h>
00033 #define printf spu_printf
00034 #endif //__SPU__
00035 #endif
00036
00037 namespace gjkepa2_impl
00038 {
00039
00040
00041
00042
00043 #define GJK_MAX_ITERATIONS 128
00044 #define GJK_ACCURARY ((btScalar)0.0001)
00045 #define GJK_MIN_DISTANCE ((btScalar)0.0001)
00046 #define GJK_DUPLICATED_EPS ((btScalar)0.0001)
00047 #define GJK_SIMPLEX2_EPS ((btScalar)0.0)
00048 #define GJK_SIMPLEX3_EPS ((btScalar)0.0)
00049 #define GJK_SIMPLEX4_EPS ((btScalar)0.0)
00050
00051
00052 #define EPA_MAX_VERTICES 64
00053 #define EPA_MAX_FACES (EPA_MAX_VERTICES*2)
00054 #define EPA_MAX_ITERATIONS 255
00055 #define EPA_ACCURACY ((btScalar)0.0001)
00056 #define EPA_FALLBACK (10*EPA_ACCURACY)
00057 #define EPA_PLANE_EPS ((btScalar)0.00001)
00058 #define EPA_INSIDE_EPS ((btScalar)0.01)
00059
00060
00061
00062 typedef unsigned int U;
00063 typedef unsigned char U1;
00064
00065
00066 struct MinkowskiDiff
00067 {
00068 const btConvexShape* m_shapes[2];
00069 btMatrix3x3 m_toshape1;
00070 btTransform m_toshape0;
00071 #ifdef __SPU__
00072 bool m_enableMargin;
00073 #else
00074 btVector3 (btConvexShape::*Ls)(const btVector3&) const;
00075 #endif//__SPU__
00076
00077
00078 MinkowskiDiff()
00079 {
00080
00081 }
00082 #ifdef __SPU__
00083 void EnableMargin(bool enable)
00084 {
00085 m_enableMargin = enable;
00086 }
00087 inline btVector3 Support0(const btVector3& d) const
00088 {
00089 if (m_enableMargin)
00090 {
00091 return m_shapes[0]->localGetSupportVertexNonVirtual(d);
00092 } else
00093 {
00094 return m_shapes[0]->localGetSupportVertexWithoutMarginNonVirtual(d);
00095 }
00096 }
00097 inline btVector3 Support1(const btVector3& d) const
00098 {
00099 if (m_enableMargin)
00100 {
00101 return m_toshape0*(m_shapes[1]->localGetSupportVertexNonVirtual(m_toshape1*d));
00102 } else
00103 {
00104 return m_toshape0*(m_shapes[1]->localGetSupportVertexWithoutMarginNonVirtual(m_toshape1*d));
00105 }
00106 }
00107 #else
00108 void EnableMargin(bool enable)
00109 {
00110 if(enable)
00111 Ls=&btConvexShape::localGetSupportVertexNonVirtual;
00112 else
00113 Ls=&btConvexShape::localGetSupportVertexWithoutMarginNonVirtual;
00114 }
00115 inline btVector3 Support0(const btVector3& d) const
00116 {
00117 return(((m_shapes[0])->*(Ls))(d));
00118 }
00119 inline btVector3 Support1(const btVector3& d) const
00120 {
00121 return(m_toshape0*((m_shapes[1])->*(Ls))(m_toshape1*d));
00122 }
00123 #endif //__SPU__
00124
00125 inline btVector3 Support(const btVector3& d) const
00126 {
00127 return(Support0(d)-Support1(-d));
00128 }
00129 btVector3 Support(const btVector3& d,U index) const
00130 {
00131 if(index)
00132 return(Support1(d));
00133 else
00134 return(Support0(d));
00135 }
00136 };
00137
00138 typedef MinkowskiDiff tShape;
00139
00140
00141
00142 struct GJK
00143 {
00144
00145 struct sSV
00146 {
00147 btVector3 d,w;
00148 };
00149 struct sSimplex
00150 {
00151 sSV* c[4];
00152 btScalar p[4];
00153 U rank;
00154 };
00155 struct eStatus { enum _ {
00156 Valid,
00157 Inside,
00158 Failed };};
00159
00160 tShape m_shape;
00161 btVector3 m_ray;
00162 btScalar m_distance;
00163 sSimplex m_simplices[2];
00164 sSV m_store[4];
00165 sSV* m_free[4];
00166 U m_nfree;
00167 U m_current;
00168 sSimplex* m_simplex;
00169 eStatus::_ m_status;
00170
00171 GJK()
00172 {
00173 Initialize();
00174 }
00175 void Initialize()
00176 {
00177 m_ray = btVector3(0,0,0);
00178 m_nfree = 0;
00179 m_status = eStatus::Failed;
00180 m_current = 0;
00181 m_distance = 0;
00182 }
00183 eStatus::_ Evaluate(const tShape& shapearg,const btVector3& guess)
00184 {
00185 U iterations=0;
00186 btScalar sqdist=0;
00187 btScalar alpha=0;
00188 btVector3 lastw[4];
00189 U clastw=0;
00190
00191 m_free[0] = &m_store[0];
00192 m_free[1] = &m_store[1];
00193 m_free[2] = &m_store[2];
00194 m_free[3] = &m_store[3];
00195 m_nfree = 4;
00196 m_current = 0;
00197 m_status = eStatus::Valid;
00198 m_shape = shapearg;
00199 m_distance = 0;
00200
00201 m_simplices[0].rank = 0;
00202 m_ray = guess;
00203 const btScalar sqrl= m_ray.length2();
00204 appendvertice(m_simplices[0],sqrl>0?-m_ray:btVector3(1,0,0));
00205 m_simplices[0].p[0] = 1;
00206 m_ray = m_simplices[0].c[0]->w;
00207 sqdist = sqrl;
00208 lastw[0] =
00209 lastw[1] =
00210 lastw[2] =
00211 lastw[3] = m_ray;
00212
00213 do {
00214 const U next=1-m_current;
00215 sSimplex& cs=m_simplices[m_current];
00216 sSimplex& ns=m_simplices[next];
00217
00218 const btScalar rl=m_ray.length();
00219 if(rl<GJK_MIN_DISTANCE)
00220 {
00221 m_status=eStatus::Inside;
00222 break;
00223 }
00224
00225 appendvertice(cs,-m_ray);
00226 const btVector3& w=cs.c[cs.rank-1]->w;
00227 bool found=false;
00228 for(U i=0;i<4;++i)
00229 {
00230 if((w-lastw[i]).length2()<GJK_DUPLICATED_EPS)
00231 { found=true;break; }
00232 }
00233 if(found)
00234 {
00235 removevertice(m_simplices[m_current]);
00236 break;
00237 }
00238 else
00239 {
00240 lastw[clastw=(clastw+1)&3]=w;
00241 }
00242
00243 const btScalar omega=btDot(m_ray,w)/rl;
00244 alpha=btMax(omega,alpha);
00245 if(((rl-alpha)-(GJK_ACCURARY*rl))<=0)
00246 {
00247 removevertice(m_simplices[m_current]);
00248 break;
00249 }
00250
00251 btScalar weights[4];
00252 U mask=0;
00253 switch(cs.rank)
00254 {
00255 case 2: sqdist=projectorigin( cs.c[0]->w,
00256 cs.c[1]->w,
00257 weights,mask);break;
00258 case 3: sqdist=projectorigin( cs.c[0]->w,
00259 cs.c[1]->w,
00260 cs.c[2]->w,
00261 weights,mask);break;
00262 case 4: sqdist=projectorigin( cs.c[0]->w,
00263 cs.c[1]->w,
00264 cs.c[2]->w,
00265 cs.c[3]->w,
00266 weights,mask);break;
00267 }
00268 if(sqdist>=0)
00269 {
00270 ns.rank = 0;
00271 m_ray = btVector3(0,0,0);
00272 m_current = next;
00273 for(U i=0,ni=cs.rank;i<ni;++i)
00274 {
00275 if(mask&(1<<i))
00276 {
00277 ns.c[ns.rank] = cs.c[i];
00278 ns.p[ns.rank++] = weights[i];
00279 m_ray += cs.c[i]->w*weights[i];
00280 }
00281 else
00282 {
00283 m_free[m_nfree++] = cs.c[i];
00284 }
00285 }
00286 if(mask==15) m_status=eStatus::Inside;
00287 }
00288 else
00289 {
00290 removevertice(m_simplices[m_current]);
00291 break;
00292 }
00293 m_status=((++iterations)<GJK_MAX_ITERATIONS)?m_status:eStatus::Failed;
00294 } while(m_status==eStatus::Valid);
00295 m_simplex=&m_simplices[m_current];
00296 switch(m_status)
00297 {
00298 case eStatus::Valid: m_distance=m_ray.length();break;
00299 case eStatus::Inside: m_distance=0;break;
00300 default:
00301 {
00302 }
00303 }
00304 return(m_status);
00305 }
00306 bool EncloseOrigin()
00307 {
00308 switch(m_simplex->rank)
00309 {
00310 case 1:
00311 {
00312 for(U i=0;i<3;++i)
00313 {
00314 btVector3 axis=btVector3(0,0,0);
00315 axis[i]=1;
00316 appendvertice(*m_simplex, axis);
00317 if(EncloseOrigin()) return(true);
00318 removevertice(*m_simplex);
00319 appendvertice(*m_simplex,-axis);
00320 if(EncloseOrigin()) return(true);
00321 removevertice(*m_simplex);
00322 }
00323 }
00324 break;
00325 case 2:
00326 {
00327 const btVector3 d=m_simplex->c[1]->w-m_simplex->c[0]->w;
00328 for(U i=0;i<3;++i)
00329 {
00330 btVector3 axis=btVector3(0,0,0);
00331 axis[i]=1;
00332 const btVector3 p=btCross(d,axis);
00333 if(p.length2()>0)
00334 {
00335 appendvertice(*m_simplex, p);
00336 if(EncloseOrigin()) return(true);
00337 removevertice(*m_simplex);
00338 appendvertice(*m_simplex,-p);
00339 if(EncloseOrigin()) return(true);
00340 removevertice(*m_simplex);
00341 }
00342 }
00343 }
00344 break;
00345 case 3:
00346 {
00347 const btVector3 n=btCross(m_simplex->c[1]->w-m_simplex->c[0]->w,
00348 m_simplex->c[2]->w-m_simplex->c[0]->w);
00349 if(n.length2()>0)
00350 {
00351 appendvertice(*m_simplex,n);
00352 if(EncloseOrigin()) return(true);
00353 removevertice(*m_simplex);
00354 appendvertice(*m_simplex,-n);
00355 if(EncloseOrigin()) return(true);
00356 removevertice(*m_simplex);
00357 }
00358 }
00359 break;
00360 case 4:
00361 {
00362 if(btFabs(det( m_simplex->c[0]->w-m_simplex->c[3]->w,
00363 m_simplex->c[1]->w-m_simplex->c[3]->w,
00364 m_simplex->c[2]->w-m_simplex->c[3]->w))>0)
00365 return(true);
00366 }
00367 break;
00368 }
00369 return(false);
00370 }
00371
00372 void getsupport(const btVector3& d,sSV& sv) const
00373 {
00374 sv.d = d/d.length();
00375 sv.w = m_shape.Support(sv.d);
00376 }
00377 void removevertice(sSimplex& simplex)
00378 {
00379 m_free[m_nfree++]=simplex.c[--simplex.rank];
00380 }
00381 void appendvertice(sSimplex& simplex,const btVector3& v)
00382 {
00383 simplex.p[simplex.rank]=0;
00384 simplex.c[simplex.rank]=m_free[--m_nfree];
00385 getsupport(v,*simplex.c[simplex.rank++]);
00386 }
00387 static btScalar det(const btVector3& a,const btVector3& b,const btVector3& c)
00388 {
00389 return( a.y()*b.z()*c.x()+a.z()*b.x()*c.y()-
00390 a.x()*b.z()*c.y()-a.y()*b.x()*c.z()+
00391 a.x()*b.y()*c.z()-a.z()*b.y()*c.x());
00392 }
00393 static btScalar projectorigin( const btVector3& a,
00394 const btVector3& b,
00395 btScalar* w,U& m)
00396 {
00397 const btVector3 d=b-a;
00398 const btScalar l=d.length2();
00399 if(l>GJK_SIMPLEX2_EPS)
00400 {
00401 const btScalar t(l>0?-btDot(a,d)/l:0);
00402 if(t>=1) { w[0]=0;w[1]=1;m=2;return(b.length2()); }
00403 else if(t<=0) { w[0]=1;w[1]=0;m=1;return(a.length2()); }
00404 else { w[0]=1-(w[1]=t);m=3;return((a+d*t).length2()); }
00405 }
00406 return(-1);
00407 }
00408 static btScalar projectorigin( const btVector3& a,
00409 const btVector3& b,
00410 const btVector3& c,
00411 btScalar* w,U& m)
00412 {
00413 static const U imd3[]={1,2,0};
00414 const btVector3* vt[]={&a,&b,&c};
00415 const btVector3 dl[]={a-b,b-c,c-a};
00416 const btVector3 n=btCross(dl[0],dl[1]);
00417 const btScalar l=n.length2();
00418 if(l>GJK_SIMPLEX3_EPS)
00419 {
00420 btScalar mindist=-1;
00421 btScalar subw[2]={0.f,0.f};
00422 U subm(0);
00423 for(U i=0;i<3;++i)
00424 {
00425 if(btDot(*vt[i],btCross(dl[i],n))>0)
00426 {
00427 const U j=imd3[i];
00428 const btScalar subd(projectorigin(*vt[i],*vt[j],subw,subm));
00429 if((mindist<0)||(subd<mindist))
00430 {
00431 mindist = subd;
00432 m = static_cast<U>(((subm&1)?1<<i:0)+((subm&2)?1<<j:0));
00433 w[i] = subw[0];
00434 w[j] = subw[1];
00435 w[imd3[j]] = 0;
00436 }
00437 }
00438 }
00439 if(mindist<0)
00440 {
00441 const btScalar d=btDot(a,n);
00442 const btScalar s=btSqrt(l);
00443 const btVector3 p=n*(d/l);
00444 mindist = p.length2();
00445 m = 7;
00446 w[0] = (btCross(dl[1],b-p)).length()/s;
00447 w[1] = (btCross(dl[2],c-p)).length()/s;
00448 w[2] = 1-(w[0]+w[1]);
00449 }
00450 return(mindist);
00451 }
00452 return(-1);
00453 }
00454 static btScalar projectorigin( const btVector3& a,
00455 const btVector3& b,
00456 const btVector3& c,
00457 const btVector3& d,
00458 btScalar* w,U& m)
00459 {
00460 static const U imd3[]={1,2,0};
00461 const btVector3* vt[]={&a,&b,&c,&d};
00462 const btVector3 dl[]={a-d,b-d,c-d};
00463 const btScalar vl=det(dl[0],dl[1],dl[2]);
00464 const bool ng=(vl*btDot(a,btCross(b-c,a-b)))<=0;
00465 if(ng&&(btFabs(vl)>GJK_SIMPLEX4_EPS))
00466 {
00467 btScalar mindist=-1;
00468 btScalar subw[3]={0.f,0.f,0.f};
00469 U subm(0);
00470 for(U i=0;i<3;++i)
00471 {
00472 const U j=imd3[i];
00473 const btScalar s=vl*btDot(d,btCross(dl[i],dl[j]));
00474 if(s>0)
00475 {
00476 const btScalar subd=projectorigin(*vt[i],*vt[j],d,subw,subm);
00477 if((mindist<0)||(subd<mindist))
00478 {
00479 mindist = subd;
00480 m = static_cast<U>((subm&1?1<<i:0)+
00481 (subm&2?1<<j:0)+
00482 (subm&4?8:0));
00483 w[i] = subw[0];
00484 w[j] = subw[1];
00485 w[imd3[j]] = 0;
00486 w[3] = subw[2];
00487 }
00488 }
00489 }
00490 if(mindist<0)
00491 {
00492 mindist = 0;
00493 m = 15;
00494 w[0] = det(c,b,d)/vl;
00495 w[1] = det(a,c,d)/vl;
00496 w[2] = det(b,a,d)/vl;
00497 w[3] = 1-(w[0]+w[1]+w[2]);
00498 }
00499 return(mindist);
00500 }
00501 return(-1);
00502 }
00503 };
00504
00505
00506 struct EPA
00507 {
00508
00509 typedef GJK::sSV sSV;
00510 struct sFace
00511 {
00512 btVector3 n;
00513 btScalar d;
00514 btScalar p;
00515 sSV* c[3];
00516 sFace* f[3];
00517 sFace* l[2];
00518 U1 e[3];
00519 U1 pass;
00520 };
00521 struct sList
00522 {
00523 sFace* root;
00524 U count;
00525 sList() : root(0),count(0) {}
00526 };
00527 struct sHorizon
00528 {
00529 sFace* cf;
00530 sFace* ff;
00531 U nf;
00532 sHorizon() : cf(0),ff(0),nf(0) {}
00533 };
00534 struct eStatus { enum _ {
00535 Valid,
00536 Touching,
00537 Degenerated,
00538 NonConvex,
00539 InvalidHull,
00540 OutOfFaces,
00541 OutOfVertices,
00542 AccuraryReached,
00543 FallBack,
00544 Failed };};
00545
00546 eStatus::_ m_status;
00547 GJK::sSimplex m_result;
00548 btVector3 m_normal;
00549 btScalar m_depth;
00550 sSV m_sv_store[EPA_MAX_VERTICES];
00551 sFace m_fc_store[EPA_MAX_FACES];
00552 U m_nextsv;
00553 sList m_hull;
00554 sList m_stock;
00555
00556 EPA()
00557 {
00558 Initialize();
00559 }
00560
00561
00562 static inline void bind(sFace* fa,U ea,sFace* fb,U eb)
00563 {
00564 fa->e[ea]=(U1)eb;fa->f[ea]=fb;
00565 fb->e[eb]=(U1)ea;fb->f[eb]=fa;
00566 }
00567 static inline void append(sList& list,sFace* face)
00568 {
00569 face->l[0] = 0;
00570 face->l[1] = list.root;
00571 if(list.root) list.root->l[0]=face;
00572 list.root = face;
00573 ++list.count;
00574 }
00575 static inline void remove(sList& list,sFace* face)
00576 {
00577 if(face->l[1]) face->l[1]->l[0]=face->l[0];
00578 if(face->l[0]) face->l[0]->l[1]=face->l[1];
00579 if(face==list.root) list.root=face->l[1];
00580 --list.count;
00581 }
00582
00583
00584 void Initialize()
00585 {
00586 m_status = eStatus::Failed;
00587 m_normal = btVector3(0,0,0);
00588 m_depth = 0;
00589 m_nextsv = 0;
00590 for(U i=0;i<EPA_MAX_FACES;++i)
00591 {
00592 append(m_stock,&m_fc_store[EPA_MAX_FACES-i-1]);
00593 }
00594 }
00595 eStatus::_ Evaluate(GJK& gjk,const btVector3& guess)
00596 {
00597 GJK::sSimplex& simplex=*gjk.m_simplex;
00598 if((simplex.rank>1)&&gjk.EncloseOrigin())
00599 {
00600
00601
00602 while(m_hull.root)
00603 {
00604 sFace* f = m_hull.root;
00605 remove(m_hull,f);
00606 append(m_stock,f);
00607 }
00608 m_status = eStatus::Valid;
00609 m_nextsv = 0;
00610
00611 if(gjk.det( simplex.c[0]->w-simplex.c[3]->w,
00612 simplex.c[1]->w-simplex.c[3]->w,
00613 simplex.c[2]->w-simplex.c[3]->w)<0)
00614 {
00615 btSwap(simplex.c[0],simplex.c[1]);
00616 btSwap(simplex.p[0],simplex.p[1]);
00617 }
00618
00619 sFace* tetra[]={newface(simplex.c[0],simplex.c[1],simplex.c[2],true),
00620 newface(simplex.c[1],simplex.c[0],simplex.c[3],true),
00621 newface(simplex.c[2],simplex.c[1],simplex.c[3],true),
00622 newface(simplex.c[0],simplex.c[2],simplex.c[3],true)};
00623 if(m_hull.count==4)
00624 {
00625 sFace* best=findbest();
00626 sFace outer=*best;
00627 U pass=0;
00628 U iterations=0;
00629 bind(tetra[0],0,tetra[1],0);
00630 bind(tetra[0],1,tetra[2],0);
00631 bind(tetra[0],2,tetra[3],0);
00632 bind(tetra[1],1,tetra[3],2);
00633 bind(tetra[1],2,tetra[2],1);
00634 bind(tetra[2],2,tetra[3],1);
00635 m_status=eStatus::Valid;
00636 for(;iterations<EPA_MAX_ITERATIONS;++iterations)
00637 {
00638 if(m_nextsv<EPA_MAX_VERTICES)
00639 {
00640 sHorizon horizon;
00641 sSV* w=&m_sv_store[m_nextsv++];
00642 bool valid=true;
00643 best->pass = (U1)(++pass);
00644 gjk.getsupport(best->n,*w);
00645 const btScalar wdist=btDot(best->n,w->w)-best->d;
00646 if(wdist>EPA_ACCURACY)
00647 {
00648 for(U j=0;(j<3)&&valid;++j)
00649 {
00650 valid&=expand( pass,w,
00651 best->f[j],best->e[j],
00652 horizon);
00653 }
00654 if(valid&&(horizon.nf>=3))
00655 {
00656 bind(horizon.cf,1,horizon.ff,2);
00657 remove(m_hull,best);
00658 append(m_stock,best);
00659 best=findbest();
00660 if(best->p>=outer.p) outer=*best;
00661 } else { m_status=eStatus::InvalidHull;break; }
00662 } else { m_status=eStatus::AccuraryReached;break; }
00663 } else { m_status=eStatus::OutOfVertices;break; }
00664 }
00665 const btVector3 projection=outer.n*outer.d;
00666 m_normal = outer.n;
00667 m_depth = outer.d;
00668 m_result.rank = 3;
00669 m_result.c[0] = outer.c[0];
00670 m_result.c[1] = outer.c[1];
00671 m_result.c[2] = outer.c[2];
00672 m_result.p[0] = btCross( outer.c[1]->w-projection,
00673 outer.c[2]->w-projection).length();
00674 m_result.p[1] = btCross( outer.c[2]->w-projection,
00675 outer.c[0]->w-projection).length();
00676 m_result.p[2] = btCross( outer.c[0]->w-projection,
00677 outer.c[1]->w-projection).length();
00678 const btScalar sum=m_result.p[0]+m_result.p[1]+m_result.p[2];
00679 m_result.p[0] /= sum;
00680 m_result.p[1] /= sum;
00681 m_result.p[2] /= sum;
00682 return(m_status);
00683 }
00684 }
00685
00686 m_status = eStatus::FallBack;
00687 m_normal = -guess;
00688 const btScalar nl=m_normal.length();
00689 if(nl>0)
00690 m_normal = m_normal/nl;
00691 else
00692 m_normal = btVector3(1,0,0);
00693 m_depth = 0;
00694 m_result.rank=1;
00695 m_result.c[0]=simplex.c[0];
00696 m_result.p[0]=1;
00697 return(m_status);
00698 }
00699 sFace* newface(sSV* a,sSV* b,sSV* c,bool forced)
00700 {
00701 if(m_stock.root)
00702 {
00703 sFace* face=m_stock.root;
00704 remove(m_stock,face);
00705 append(m_hull,face);
00706 face->pass = 0;
00707 face->c[0] = a;
00708 face->c[1] = b;
00709 face->c[2] = c;
00710 face->n = btCross(b->w-a->w,c->w-a->w);
00711 const btScalar l=face->n.length();
00712 const bool v=l>EPA_ACCURACY;
00713 face->p = btMin(btMin(
00714 btDot(a->w,btCross(face->n,a->w-b->w)),
00715 btDot(b->w,btCross(face->n,b->w-c->w))),
00716 btDot(c->w,btCross(face->n,c->w-a->w))) /
00717 (v?l:1);
00718 face->p = face->p>=-EPA_INSIDE_EPS?0:face->p;
00719 if(v)
00720 {
00721 face->d = btDot(a->w,face->n)/l;
00722 face->n /= l;
00723 if(forced||(face->d>=-EPA_PLANE_EPS))
00724 {
00725 return(face);
00726 } else m_status=eStatus::NonConvex;
00727 } else m_status=eStatus::Degenerated;
00728 remove(m_hull,face);
00729 append(m_stock,face);
00730 return(0);
00731 }
00732 m_status=m_stock.root?eStatus::OutOfVertices:eStatus::OutOfFaces;
00733 return(0);
00734 }
00735 sFace* findbest()
00736 {
00737 sFace* minf=m_hull.root;
00738 btScalar mind=minf->d*minf->d;
00739 btScalar maxp=minf->p;
00740 for(sFace* f=minf->l[1];f;f=f->l[1])
00741 {
00742 const btScalar sqd=f->d*f->d;
00743 if((f->p>=maxp)&&(sqd<mind))
00744 {
00745 minf=f;
00746 mind=sqd;
00747 maxp=f->p;
00748 }
00749 }
00750 return(minf);
00751 }
00752 bool expand(U pass,sSV* w,sFace* f,U e,sHorizon& horizon)
00753 {
00754 static const U i1m3[]={1,2,0};
00755 static const U i2m3[]={2,0,1};
00756 if(f->pass!=pass)
00757 {
00758 const U e1=i1m3[e];
00759 if((btDot(f->n,w->w)-f->d)<-EPA_PLANE_EPS)
00760 {
00761 sFace* nf=newface(f->c[e1],f->c[e],w,false);
00762 if(nf)
00763 {
00764 bind(nf,0,f,e);
00765 if(horizon.cf) bind(horizon.cf,1,nf,2); else horizon.ff=nf;
00766 horizon.cf=nf;
00767 ++horizon.nf;
00768 return(true);
00769 }
00770 }
00771 else
00772 {
00773 const U e2=i2m3[e];
00774 f->pass = (U1)pass;
00775 if( expand(pass,w,f->f[e1],f->e[e1],horizon)&&
00776 expand(pass,w,f->f[e2],f->e[e2],horizon))
00777 {
00778 remove(m_hull,f);
00779 append(m_stock,f);
00780 return(true);
00781 }
00782 }
00783 }
00784 return(false);
00785 }
00786
00787 };
00788
00789
00790 static void Initialize( const btConvexShape* shape0,const btTransform& wtrs0,
00791 const btConvexShape* shape1,const btTransform& wtrs1,
00792 btGjkEpaSolver2::sResults& results,
00793 tShape& shape,
00794 bool withmargins)
00795 {
00796
00797 results.witnesses[0] =
00798 results.witnesses[1] = btVector3(0,0,0);
00799 results.status = btGjkEpaSolver2::sResults::Separated;
00800
00801 shape.m_shapes[0] = shape0;
00802 shape.m_shapes[1] = shape1;
00803 shape.m_toshape1 = wtrs1.getBasis().transposeTimes(wtrs0.getBasis());
00804 shape.m_toshape0 = wtrs0.inverseTimes(wtrs1);
00805 shape.EnableMargin(withmargins);
00806 }
00807
00808 }
00809
00810
00811
00812
00813
00814 using namespace gjkepa2_impl;
00815
00816
00817 int btGjkEpaSolver2::StackSizeRequirement()
00818 {
00819 return(sizeof(GJK)+sizeof(EPA));
00820 }
00821
00822
00823 bool btGjkEpaSolver2::Distance( const btConvexShape* shape0,
00824 const btTransform& wtrs0,
00825 const btConvexShape* shape1,
00826 const btTransform& wtrs1,
00827 const btVector3& guess,
00828 sResults& results)
00829 {
00830 tShape shape;
00831 Initialize(shape0,wtrs0,shape1,wtrs1,results,shape,false);
00832 GJK gjk;
00833 GJK::eStatus::_ gjk_status=gjk.Evaluate(shape,guess);
00834 if(gjk_status==GJK::eStatus::Valid)
00835 {
00836 btVector3 w0=btVector3(0,0,0);
00837 btVector3 w1=btVector3(0,0,0);
00838 for(U i=0;i<gjk.m_simplex->rank;++i)
00839 {
00840 const btScalar p=gjk.m_simplex->p[i];
00841 w0+=shape.Support( gjk.m_simplex->c[i]->d,0)*p;
00842 w1+=shape.Support(-gjk.m_simplex->c[i]->d,1)*p;
00843 }
00844 results.witnesses[0] = wtrs0*w0;
00845 results.witnesses[1] = wtrs0*w1;
00846 results.normal = w0-w1;
00847 results.distance = results.normal.length();
00848 results.normal /= results.distance>GJK_MIN_DISTANCE?results.distance:1;
00849 return(true);
00850 }
00851 else
00852 {
00853 results.status = gjk_status==GJK::eStatus::Inside?
00854 sResults::Penetrating :
00855 sResults::GJK_Failed ;
00856 return(false);
00857 }
00858 }
00859
00860
00861 bool btGjkEpaSolver2::Penetration( const btConvexShape* shape0,
00862 const btTransform& wtrs0,
00863 const btConvexShape* shape1,
00864 const btTransform& wtrs1,
00865 const btVector3& guess,
00866 sResults& results,
00867 bool usemargins)
00868 {
00869 tShape shape;
00870 Initialize(shape0,wtrs0,shape1,wtrs1,results,shape,usemargins);
00871 GJK gjk;
00872 GJK::eStatus::_ gjk_status=gjk.Evaluate(shape,-guess);
00873 switch(gjk_status)
00874 {
00875 case GJK::eStatus::Inside:
00876 {
00877 EPA epa;
00878 EPA::eStatus::_ epa_status=epa.Evaluate(gjk,-guess);
00879 if(epa_status!=EPA::eStatus::Failed)
00880 {
00881 btVector3 w0=btVector3(0,0,0);
00882 for(U i=0;i<epa.m_result.rank;++i)
00883 {
00884 w0+=shape.Support(epa.m_result.c[i]->d,0)*epa.m_result.p[i];
00885 }
00886 results.status = sResults::Penetrating;
00887 results.witnesses[0] = wtrs0*w0;
00888 results.witnesses[1] = wtrs0*(w0-epa.m_normal*epa.m_depth);
00889 results.normal = -epa.m_normal;
00890 results.distance = -epa.m_depth;
00891 return(true);
00892 } else results.status=sResults::EPA_Failed;
00893 }
00894 break;
00895 case GJK::eStatus::Failed:
00896 results.status=sResults::GJK_Failed;
00897 break;
00898 default:
00899 {
00900 }
00901 }
00902 return(false);
00903 }
00904
00905 #ifndef __SPU__
00906
00907 btScalar btGjkEpaSolver2::SignedDistance(const btVector3& position,
00908 btScalar margin,
00909 const btConvexShape* shape0,
00910 const btTransform& wtrs0,
00911 sResults& results)
00912 {
00913 tShape shape;
00914 btSphereShape shape1(margin);
00915 btTransform wtrs1(btQuaternion(0,0,0,1),position);
00916 Initialize(shape0,wtrs0,&shape1,wtrs1,results,shape,false);
00917 GJK gjk;
00918 GJK::eStatus::_ gjk_status=gjk.Evaluate(shape,btVector3(1,1,1));
00919 if(gjk_status==GJK::eStatus::Valid)
00920 {
00921 btVector3 w0=btVector3(0,0,0);
00922 btVector3 w1=btVector3(0,0,0);
00923 for(U i=0;i<gjk.m_simplex->rank;++i)
00924 {
00925 const btScalar p=gjk.m_simplex->p[i];
00926 w0+=shape.Support( gjk.m_simplex->c[i]->d,0)*p;
00927 w1+=shape.Support(-gjk.m_simplex->c[i]->d,1)*p;
00928 }
00929 results.witnesses[0] = wtrs0*w0;
00930 results.witnesses[1] = wtrs0*w1;
00931 const btVector3 delta= results.witnesses[1]-
00932 results.witnesses[0];
00933 const btScalar margin= shape0->getMarginNonVirtual()+
00934 shape1.getMarginNonVirtual();
00935 const btScalar length= delta.length();
00936 results.normal = delta/length;
00937 results.witnesses[0] += results.normal*margin;
00938 return(length-margin);
00939 }
00940 else
00941 {
00942 if(gjk_status==GJK::eStatus::Inside)
00943 {
00944 if(Penetration(shape0,wtrs0,&shape1,wtrs1,gjk.m_ray,results))
00945 {
00946 const btVector3 delta= results.witnesses[0]-
00947 results.witnesses[1];
00948 const btScalar length= delta.length();
00949 if (length >= SIMD_EPSILON)
00950 results.normal = delta/length;
00951 return(-length);
00952 }
00953 }
00954 }
00955 return(SIMD_INFINITY);
00956 }
00957
00958
00959 bool btGjkEpaSolver2::SignedDistance(const btConvexShape* shape0,
00960 const btTransform& wtrs0,
00961 const btConvexShape* shape1,
00962 const btTransform& wtrs1,
00963 const btVector3& guess,
00964 sResults& results)
00965 {
00966 if(!Distance(shape0,wtrs0,shape1,wtrs1,guess,results))
00967 return(Penetration(shape0,wtrs0,shape1,wtrs1,guess,results,false));
00968 else
00969 return(true);
00970 }
00971 #endif //__SPU__
00972
00973
00974
00975 #undef GJK_MAX_ITERATIONS
00976 #undef GJK_ACCURARY
00977 #undef GJK_MIN_DISTANCE
00978 #undef GJK_DUPLICATED_EPS
00979 #undef GJK_SIMPLEX2_EPS
00980 #undef GJK_SIMPLEX3_EPS
00981 #undef GJK_SIMPLEX4_EPS
00982
00983 #undef EPA_MAX_VERTICES
00984 #undef EPA_MAX_FACES
00985 #undef EPA_MAX_ITERATIONS
00986 #undef EPA_ACCURACY
00987 #undef EPA_FALLBACK
00988 #undef EPA_PLANE_EPS
00989 #undef EPA_INSIDE_EPS
