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 sSV* c[3];
00515 sFace* f[3];
00516 sFace* l[2];
00517 U1 e[3];
00518 U1 pass;
00519 };
00520 struct sList
00521 {
00522 sFace* root;
00523 U count;
00524 sList() : root(0),count(0) {}
00525 };
00526 struct sHorizon
00527 {
00528 sFace* cf;
00529 sFace* ff;
00530 U nf;
00531 sHorizon() : cf(0),ff(0),nf(0) {}
00532 };
00533 struct eStatus { enum _ {
00534 Valid,
00535 Touching,
00536 Degenerated,
00537 NonConvex,
00538 InvalidHull,
00539 OutOfFaces,
00540 OutOfVertices,
00541 AccuraryReached,
00542 FallBack,
00543 Failed };};
00544
00545 eStatus::_ m_status;
00546 GJK::sSimplex m_result;
00547 btVector3 m_normal;
00548 btScalar m_depth;
00549 sSV m_sv_store[EPA_MAX_VERTICES];
00550 sFace m_fc_store[EPA_MAX_FACES];
00551 U m_nextsv;
00552 sList m_hull;
00553 sList m_stock;
00554
00555 EPA()
00556 {
00557 Initialize();
00558 }
00559
00560
00561 static inline void bind(sFace* fa,U ea,sFace* fb,U eb)
00562 {
00563 fa->e[ea]=(U1)eb;fa->f[ea]=fb;
00564 fb->e[eb]=(U1)ea;fb->f[eb]=fa;
00565 }
00566 static inline void append(sList& list,sFace* face)
00567 {
00568 face->l[0] = 0;
00569 face->l[1] = list.root;
00570 if(list.root) list.root->l[0]=face;
00571 list.root = face;
00572 ++list.count;
00573 }
00574 static inline void remove(sList& list,sFace* face)
00575 {
00576 if(face->l[1]) face->l[1]->l[0]=face->l[0];
00577 if(face->l[0]) face->l[0]->l[1]=face->l[1];
00578 if(face==list.root) list.root=face->l[1];
00579 --list.count;
00580 }
00581
00582
00583 void Initialize()
00584 {
00585 m_status = eStatus::Failed;
00586 m_normal = btVector3(0,0,0);
00587 m_depth = 0;
00588 m_nextsv = 0;
00589 for(U i=0;i<EPA_MAX_FACES;++i)
00590 {
00591 append(m_stock,&m_fc_store[EPA_MAX_FACES-i-1]);
00592 }
00593 }
00594 eStatus::_ Evaluate(GJK& gjk,const btVector3& guess)
00595 {
00596 GJK::sSimplex& simplex=*gjk.m_simplex;
00597 if((simplex.rank>1)&&gjk.EncloseOrigin())
00598 {
00599
00600
00601 while(m_hull.root)
00602 {
00603 sFace* f = m_hull.root;
00604 remove(m_hull,f);
00605 append(m_stock,f);
00606 }
00607 m_status = eStatus::Valid;
00608 m_nextsv = 0;
00609
00610 if(gjk.det( simplex.c[0]->w-simplex.c[3]->w,
00611 simplex.c[1]->w-simplex.c[3]->w,
00612 simplex.c[2]->w-simplex.c[3]->w)<0)
00613 {
00614 btSwap(simplex.c[0],simplex.c[1]);
00615 btSwap(simplex.p[0],simplex.p[1]);
00616 }
00617
00618 sFace* tetra[]={newface(simplex.c[0],simplex.c[1],simplex.c[2],true),
00619 newface(simplex.c[1],simplex.c[0],simplex.c[3],true),
00620 newface(simplex.c[2],simplex.c[1],simplex.c[3],true),
00621 newface(simplex.c[0],simplex.c[2],simplex.c[3],true)};
00622 if(m_hull.count==4)
00623 {
00624 sFace* best=findbest();
00625 sFace outer=*best;
00626 U pass=0;
00627 U iterations=0;
00628 bind(tetra[0],0,tetra[1],0);
00629 bind(tetra[0],1,tetra[2],0);
00630 bind(tetra[0],2,tetra[3],0);
00631 bind(tetra[1],1,tetra[3],2);
00632 bind(tetra[1],2,tetra[2],1);
00633 bind(tetra[2],2,tetra[3],1);
00634 m_status=eStatus::Valid;
00635 for(;iterations<EPA_MAX_ITERATIONS;++iterations)
00636 {
00637 if(m_nextsv<EPA_MAX_VERTICES)
00638 {
00639 sHorizon horizon;
00640 sSV* w=&m_sv_store[m_nextsv++];
00641 bool valid=true;
00642 best->pass = (U1)(++pass);
00643 gjk.getsupport(best->n,*w);
00644 const btScalar wdist=btDot(best->n,w->w)-best->d;
00645 if(wdist>EPA_ACCURACY)
00646 {
00647 for(U j=0;(j<3)&&valid;++j)
00648 {
00649 valid&=expand( pass,w,
00650 best->f[j],best->e[j],
00651 horizon);
00652 }
00653 if(valid&&(horizon.nf>=3))
00654 {
00655 bind(horizon.cf,1,horizon.ff,2);
00656 remove(m_hull,best);
00657 append(m_stock,best);
00658 best=findbest();
00659 outer=*best;
00660 } else { m_status=eStatus::InvalidHull;break; }
00661 } else { m_status=eStatus::AccuraryReached;break; }
00662 } else { m_status=eStatus::OutOfVertices;break; }
00663 }
00664 const btVector3 projection=outer.n*outer.d;
00665 m_normal = outer.n;
00666 m_depth = outer.d;
00667 m_result.rank = 3;
00668 m_result.c[0] = outer.c[0];
00669 m_result.c[1] = outer.c[1];
00670 m_result.c[2] = outer.c[2];
00671 m_result.p[0] = btCross( outer.c[1]->w-projection,
00672 outer.c[2]->w-projection).length();
00673 m_result.p[1] = btCross( outer.c[2]->w-projection,
00674 outer.c[0]->w-projection).length();
00675 m_result.p[2] = btCross( outer.c[0]->w-projection,
00676 outer.c[1]->w-projection).length();
00677 const btScalar sum=m_result.p[0]+m_result.p[1]+m_result.p[2];
00678 m_result.p[0] /= sum;
00679 m_result.p[1] /= sum;
00680 m_result.p[2] /= sum;
00681 return(m_status);
00682 }
00683 }
00684
00685 m_status = eStatus::FallBack;
00686 m_normal = -guess;
00687 const btScalar nl=m_normal.length();
00688 if(nl>0)
00689 m_normal = m_normal/nl;
00690 else
00691 m_normal = btVector3(1,0,0);
00692 m_depth = 0;
00693 m_result.rank=1;
00694 m_result.c[0]=simplex.c[0];
00695 m_result.p[0]=1;
00696 return(m_status);
00697 }
00698 bool getedgedist(sFace* face, sSV* a, sSV* b, btScalar& dist)
00699 {
00700 const btVector3 ba = b->w - a->w;
00701 const btVector3 n_ab = btCross(ba, face->n);
00702 const btScalar a_dot_nab = btDot(a->w, n_ab);
00703
00704 if(a_dot_nab < 0)
00705 {
00706
00707
00708 const btScalar ba_l2 = ba.length2();
00709 const btScalar a_dot_ba = btDot(a->w, ba);
00710 const btScalar b_dot_ba = btDot(b->w, ba);
00711
00712 if(a_dot_ba > 0)
00713 {
00714
00715 dist = a->w.length();
00716 }
00717 else if(b_dot_ba < 0)
00718 {
00719
00720 dist = b->w.length();
00721 }
00722 else
00723 {
00724
00725 const btScalar a_dot_b = btDot(a->w, b->w);
00726 dist = btSqrt(btMax((a->w.length2() * b->w.length2() - a_dot_b * a_dot_b) / ba_l2, (btScalar)0));
00727 }
00728
00729 return true;
00730 }
00731
00732 return false;
00733 }
00734 sFace* newface(sSV* a,sSV* b,sSV* c,bool forced)
00735 {
00736 if(m_stock.root)
00737 {
00738 sFace* face=m_stock.root;
00739 remove(m_stock,face);
00740 append(m_hull,face);
00741 face->pass = 0;
00742 face->c[0] = a;
00743 face->c[1] = b;
00744 face->c[2] = c;
00745 face->n = btCross(b->w-a->w,c->w-a->w);
00746 const btScalar l=face->n.length();
00747 const bool v=l>EPA_ACCURACY;
00748
00749 if(v)
00750 {
00751 if(!(getedgedist(face, a, b, face->d) ||
00752 getedgedist(face, b, c, face->d) ||
00753 getedgedist(face, c, a, face->d)))
00754 {
00755
00756
00757 face->d = btDot(a->w, face->n) / l;
00758 }
00759
00760 face->n /= l;
00761 if(forced || (face->d >= -EPA_PLANE_EPS))
00762 {
00763 return face;
00764 }
00765 else
00766 m_status=eStatus::NonConvex;
00767 }
00768 else
00769 m_status=eStatus::Degenerated;
00770
00771 remove(m_hull, face);
00772 append(m_stock, face);
00773 return 0;
00774
00775 }
00776 m_status = m_stock.root ? eStatus::OutOfVertices : eStatus::OutOfFaces;
00777 return 0;
00778 }
00779 sFace* findbest()
00780 {
00781 sFace* minf=m_hull.root;
00782 btScalar mind=minf->d*minf->d;
00783 for(sFace* f=minf->l[1];f;f=f->l[1])
00784 {
00785 const btScalar sqd=f->d*f->d;
00786 if(sqd<mind)
00787 {
00788 minf=f;
00789 mind=sqd;
00790 }
00791 }
00792 return(minf);
00793 }
00794 bool expand(U pass,sSV* w,sFace* f,U e,sHorizon& horizon)
00795 {
00796 static const U i1m3[]={1,2,0};
00797 static const U i2m3[]={2,0,1};
00798 if(f->pass!=pass)
00799 {
00800 const U e1=i1m3[e];
00801 if((btDot(f->n,w->w)-f->d)<-EPA_PLANE_EPS)
00802 {
00803 sFace* nf=newface(f->c[e1],f->c[e],w,false);
00804 if(nf)
00805 {
00806 bind(nf,0,f,e);
00807 if(horizon.cf) bind(horizon.cf,1,nf,2); else horizon.ff=nf;
00808 horizon.cf=nf;
00809 ++horizon.nf;
00810 return(true);
00811 }
00812 }
00813 else
00814 {
00815 const U e2=i2m3[e];
00816 f->pass = (U1)pass;
00817 if( expand(pass,w,f->f[e1],f->e[e1],horizon)&&
00818 expand(pass,w,f->f[e2],f->e[e2],horizon))
00819 {
00820 remove(m_hull,f);
00821 append(m_stock,f);
00822 return(true);
00823 }
00824 }
00825 }
00826 return(false);
00827 }
00828
00829 };
00830
00831
00832 static void Initialize( const btConvexShape* shape0,const btTransform& wtrs0,
00833 const btConvexShape* shape1,const btTransform& wtrs1,
00834 btGjkEpaSolver2::sResults& results,
00835 tShape& shape,
00836 bool withmargins)
00837 {
00838
00839 results.witnesses[0] =
00840 results.witnesses[1] = btVector3(0,0,0);
00841 results.status = btGjkEpaSolver2::sResults::Separated;
00842
00843 shape.m_shapes[0] = shape0;
00844 shape.m_shapes[1] = shape1;
00845 shape.m_toshape1 = wtrs1.getBasis().transposeTimes(wtrs0.getBasis());
00846 shape.m_toshape0 = wtrs0.inverseTimes(wtrs1);
00847 shape.EnableMargin(withmargins);
00848 }
00849
00850 }
00851
00852
00853
00854
00855
00856 using namespace gjkepa2_impl;
00857
00858
00859 int btGjkEpaSolver2::StackSizeRequirement()
00860 {
00861 return(sizeof(GJK)+sizeof(EPA));
00862 }
00863
00864
00865 bool btGjkEpaSolver2::Distance( const btConvexShape* shape0,
00866 const btTransform& wtrs0,
00867 const btConvexShape* shape1,
00868 const btTransform& wtrs1,
00869 const btVector3& guess,
00870 sResults& results)
00871 {
00872 tShape shape;
00873 Initialize(shape0,wtrs0,shape1,wtrs1,results,shape,false);
00874 GJK gjk;
00875 GJK::eStatus::_ gjk_status=gjk.Evaluate(shape,guess);
00876 if(gjk_status==GJK::eStatus::Valid)
00877 {
00878 btVector3 w0=btVector3(0,0,0);
00879 btVector3 w1=btVector3(0,0,0);
00880 for(U i=0;i<gjk.m_simplex->rank;++i)
00881 {
00882 const btScalar p=gjk.m_simplex->p[i];
00883 w0+=shape.Support( gjk.m_simplex->c[i]->d,0)*p;
00884 w1+=shape.Support(-gjk.m_simplex->c[i]->d,1)*p;
00885 }
00886 results.witnesses[0] = wtrs0*w0;
00887 results.witnesses[1] = wtrs0*w1;
00888 results.normal = w0-w1;
00889 results.distance = results.normal.length();
00890 results.normal /= results.distance>GJK_MIN_DISTANCE?results.distance:1;
00891 return(true);
00892 }
00893 else
00894 {
00895 results.status = gjk_status==GJK::eStatus::Inside?
00896 sResults::Penetrating :
00897 sResults::GJK_Failed ;
00898 return(false);
00899 }
00900 }
00901
00902
00903 bool btGjkEpaSolver2::Penetration( const btConvexShape* shape0,
00904 const btTransform& wtrs0,
00905 const btConvexShape* shape1,
00906 const btTransform& wtrs1,
00907 const btVector3& guess,
00908 sResults& results,
00909 bool usemargins)
00910 {
00911 tShape shape;
00912 Initialize(shape0,wtrs0,shape1,wtrs1,results,shape,usemargins);
00913 GJK gjk;
00914 GJK::eStatus::_ gjk_status=gjk.Evaluate(shape,-guess);
00915 switch(gjk_status)
00916 {
00917 case GJK::eStatus::Inside:
00918 {
00919 EPA epa;
00920 EPA::eStatus::_ epa_status=epa.Evaluate(gjk,-guess);
00921 if(epa_status!=EPA::eStatus::Failed)
00922 {
00923 btVector3 w0=btVector3(0,0,0);
00924 for(U i=0;i<epa.m_result.rank;++i)
00925 {
00926 w0+=shape.Support(epa.m_result.c[i]->d,0)*epa.m_result.p[i];
00927 }
00928 results.status = sResults::Penetrating;
00929 results.witnesses[0] = wtrs0*w0;
00930 results.witnesses[1] = wtrs0*(w0-epa.m_normal*epa.m_depth);
00931 results.normal = -epa.m_normal;
00932 results.distance = -epa.m_depth;
00933 return(true);
00934 } else results.status=sResults::EPA_Failed;
00935 }
00936 break;
00937 case GJK::eStatus::Failed:
00938 results.status=sResults::GJK_Failed;
00939 break;
00940 default:
00941 {
00942 }
00943 }
00944 return(false);
00945 }
00946
00947 #ifndef __SPU__
00948
00949 btScalar btGjkEpaSolver2::SignedDistance(const btVector3& position,
00950 btScalar margin,
00951 const btConvexShape* shape0,
00952 const btTransform& wtrs0,
00953 sResults& results)
00954 {
00955 tShape shape;
00956 btSphereShape shape1(margin);
00957 btTransform wtrs1(btQuaternion(0,0,0,1),position);
00958 Initialize(shape0,wtrs0,&shape1,wtrs1,results,shape,false);
00959 GJK gjk;
00960 GJK::eStatus::_ gjk_status=gjk.Evaluate(shape,btVector3(1,1,1));
00961 if(gjk_status==GJK::eStatus::Valid)
00962 {
00963 btVector3 w0=btVector3(0,0,0);
00964 btVector3 w1=btVector3(0,0,0);
00965 for(U i=0;i<gjk.m_simplex->rank;++i)
00966 {
00967 const btScalar p=gjk.m_simplex->p[i];
00968 w0+=shape.Support( gjk.m_simplex->c[i]->d,0)*p;
00969 w1+=shape.Support(-gjk.m_simplex->c[i]->d,1)*p;
00970 }
00971 results.witnesses[0] = wtrs0*w0;
00972 results.witnesses[1] = wtrs0*w1;
00973 const btVector3 delta= results.witnesses[1]-
00974 results.witnesses[0];
00975 const btScalar margin= shape0->getMarginNonVirtual()+
00976 shape1.getMarginNonVirtual();
00977 const btScalar length= delta.length();
00978 results.normal = delta/length;
00979 results.witnesses[0] += results.normal*margin;
00980 return(length-margin);
00981 }
00982 else
00983 {
00984 if(gjk_status==GJK::eStatus::Inside)
00985 {
00986 if(Penetration(shape0,wtrs0,&shape1,wtrs1,gjk.m_ray,results))
00987 {
00988 const btVector3 delta= results.witnesses[0]-
00989 results.witnesses[1];
00990 const btScalar length= delta.length();
00991 if (length >= SIMD_EPSILON)
00992 results.normal = delta/length;
00993 return(-length);
00994 }
00995 }
00996 }
00997 return(SIMD_INFINITY);
00998 }
00999
01000
01001 bool btGjkEpaSolver2::SignedDistance(const btConvexShape* shape0,
01002 const btTransform& wtrs0,
01003 const btConvexShape* shape1,
01004 const btTransform& wtrs1,
01005 const btVector3& guess,
01006 sResults& results)
01007 {
01008 if(!Distance(shape0,wtrs0,shape1,wtrs1,guess,results))
01009 return(Penetration(shape0,wtrs0,shape1,wtrs1,guess,results,false));
01010 else
01011 return(true);
01012 }
01013 #endif //__SPU__
01014
01015
01016
01017 #undef GJK_MAX_ITERATIONS
01018 #undef GJK_ACCURARY
01019 #undef GJK_MIN_DISTANCE
01020 #undef GJK_DUPLICATED_EPS
01021 #undef GJK_SIMPLEX2_EPS
01022 #undef GJK_SIMPLEX3_EPS
01023 #undef GJK_SIMPLEX4_EPS
01024
01025 #undef EPA_MAX_VERTICES
01026 #undef EPA_MAX_FACES
01027 #undef EPA_MAX_ITERATIONS
01028 #undef EPA_ACCURACY
01029 #undef EPA_FALLBACK
01030 #undef EPA_PLANE_EPS
01031 #undef EPA_INSIDE_EPS