diff options
Diffstat (limited to 'extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa.cpp')
| -rw-r--r-- | extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa.cpp | 628 |
1 files changed, 0 insertions, 628 deletions
diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa.cpp deleted file mode 100644 index 36cdeeaefdb..00000000000 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa.cpp +++ /dev/null @@ -1,628 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the -use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it -freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not -claim that you wrote the original software. If you use this software in a -product, an acknowledgment in the product documentation would be appreciated -but is not required. -2. Altered source versions must be plainly marked as such, and must not be -misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -/* -GJK-EPA collision solver by Nathanael Presson -Nov.2006 -*/ - -#include "btGjkEpa.h" -#include <string.h> //for memset -#include "LinearMath/btStackAlloc.h" - -#if defined(DEBUG) || defined (_DEBUG) -#include <stdio.h> //for debug printf -#ifdef __SPU__ -#include <spu_printf.h> -#define printf spu_printf -#endif //__SPU__ -#endif - -namespace gjkepa_impl -{ - -// -// Port. typedefs -// - -typedef btScalar F; -typedef bool Z; -typedef int I; -typedef unsigned int U; -typedef unsigned char U1; -typedef unsigned short U2; - -typedef btVector3 Vector3; -typedef btMatrix3x3 Rotation; - -// -// Config -// - -#if 0 -#define BTLOCALSUPPORT localGetSupportingVertexWithoutMargin -#else -#define BTLOCALSUPPORT localGetSupportingVertex -#endif - -// -// Const -// - - -#define cstInf SIMD_INFINITY -#define cstPi SIMD_PI -#define cst2Pi SIMD_2_PI -#define GJK_maxiterations (128) -#define GJK_hashsize (1<<6) -#define GJK_hashmask (GJK_hashsize-1) -#define GJK_insimplex_eps F(0.0001) -#define GJK_sqinsimplex_eps (GJK_insimplex_eps*GJK_insimplex_eps) -#define EPA_maxiterations 256 -#define EPA_inface_eps F(0.01) -#define EPA_accuracy F(0.001) - -// -// Utils -// - -static inline F Abs(F v) { return(v<0?-v:v); } -static inline F Sign(F v) { return(F(v<0?-1:1)); } -template <typename T> static inline void Swap(T& a,T& b) { T -t(a);a=b;b=t; } -template <typename T> static inline T Min(const T& a,const T& b) { -return(a<b?a:b); } -template <typename T> static inline T Max(const T& a,const T& b) { -return(a>b?a:b); } -static inline void ClearMemory(void* p,U sz) { memset(p,0,(size_t)sz); -} -#if 0 -template <typename T> static inline void Raise(const T& object) { -throw(object); } -#else -template <typename T> static inline void Raise(const T&) {} -#endif - - - -// -// GJK -// -struct GJK - { - struct Mkv - { - Vector3 w; /* Minkowski vertice */ - Vector3 r; /* Ray */ - }; - struct He - { - Vector3 v; - He* n; - }; - btStackAlloc* sa; - btBlock* sablock; - He* table[GJK_hashsize]; - Rotation wrotations[2]; - Vector3 positions[2]; - const btConvexShape* shapes[2]; - Mkv simplex[5]; - Vector3 ray; - U order; - U iterations; - F margin; - Z failed; - // - GJK(btStackAlloc* psa, - const Rotation& wrot0,const Vector3& pos0,const btConvexShape* shape0, - const Rotation& wrot1,const Vector3& pos1,const btConvexShape* shape1, - F pmargin=0) - { - wrotations[0]=wrot0;positions[0]=pos0;shapes[0]=shape0; - wrotations[1]=wrot1;positions[1]=pos1;shapes[1]=shape1; - sa =psa; - sablock =sa->beginBlock(); - margin =pmargin; - failed =false; - } - // - ~GJK() - { - sa->endBlock(sablock); - } - // vdh : very dumm hash - static inline U Hash(const Vector3& v) - { - //this doesn't compile under GCC 3.3.5, so add the ()... - //const U h(U(v[0]*15461)^U(v[1]*83003)^U(v[2]*15473)); - //return(((*((const U*)&h))*169639)&GJK_hashmask); - const U h((U)(v[0]*15461)^(U)(v[1]*83003)^(U)(v[2]*15473)); - return(((*((const U*)&h))*169639)&GJK_hashmask); - } - // - inline Vector3 LocalSupport(const Vector3& d,U i) const - { - return(wrotations[i]*shapes[i]->BTLOCALSUPPORT(d*wrotations[i])+positions[i]); - } - // - inline void Support(const Vector3& d,Mkv& v) const - { - v.r = d; - v.w = LocalSupport(d,0)-LocalSupport(-d,1)+d*margin; - } - #define SPX(_i_) simplex[_i_] - #define SPXW(_i_) simplex[_i_].w - // - inline Z FetchSupport() - { - const U h(Hash(ray)); - He* e = (He*)(table[h]); - while(e) { if(e->v==ray) { --order;return(false); } else e=e->n; } - e=(He*)sa->allocate(sizeof(He));e->v=ray;e->n=table[h];table[h]=e; - Support(ray,simplex[++order]); - return(ray.dot(SPXW(order))>0); - } - // - inline Z SolveSimplex2(const Vector3& ao,const Vector3& ab) - { - if(ab.dot(ao)>=0) - { - const Vector3 cabo(cross(ab,ao)); - if(cabo.length2()>GJK_sqinsimplex_eps) - { ray=cross(cabo,ab); } - else - { return(true); } - } - else - { order=0;SPX(0)=SPX(1);ray=ao; } - return(false); - } - // - inline Z SolveSimplex3(const Vector3& ao,const Vector3& ab,const Vector3& -ac) - { - return(SolveSimplex3a(ao,ab,ac,cross(ab,ac))); - } - // - inline Z SolveSimplex3a(const Vector3& ao,const Vector3& ab,const Vector3& -ac,const Vector3& cabc) - { - if((cross(cabc,ab)).dot(ao)<-GJK_insimplex_eps) - { order=1;SPX(0)=SPX(1);SPX(1)=SPX(2);return(SolveSimplex2(ao,ab)); } - else if((cross(cabc,ac)).dot(ao)>+GJK_insimplex_eps) - { order=1;SPX(1)=SPX(2);return(SolveSimplex2(ao,ac)); } - else - { - const F d(cabc.dot(ao)); - if(Abs(d)>GJK_insimplex_eps) - { - if(d>0) - { ray=cabc; } - else - { ray=-cabc;Swap(SPX(0),SPX(1)); } - return(false); - } else return(true); - } - } - // - inline Z SolveSimplex4(const Vector3& ao,const Vector3& ab,const Vector3& -ac,const Vector3& ad) - { - Vector3 crs; - if((crs=cross(ab,ac)).dot(ao)>GJK_insimplex_eps) - { -order=2;SPX(0)=SPX(1);SPX(1)=SPX(2);SPX(2)=SPX(3);return(SolveSimplex3a(ao,ab,ac,crs)); -} - else if((crs=cross(ac,ad)).dot(ao)>GJK_insimplex_eps) - { order=2;SPX(2)=SPX(3);return(SolveSimplex3a(ao,ac,ad,crs)); } - else if((crs=cross(ad,ab)).dot(ao)>GJK_insimplex_eps) - { -order=2;SPX(1)=SPX(0);SPX(0)=SPX(2);SPX(2)=SPX(3);return(SolveSimplex3a(ao,ad,ab,crs)); -} - else return(true); - } - // - inline Z SearchOrigin(const Vector3& initray=Vector3(1,0,0)) - { - iterations = 0; - order = (U)-1; - failed = false; - ray = initray.normalized(); - ClearMemory(table,sizeof(void*)*GJK_hashsize); - FetchSupport(); - ray = -SPXW(0); - for(;iterations<GJK_maxiterations;++iterations) - { - const F rl(ray.length()); - ray/=rl>0?rl:1; - if(FetchSupport()) - { - Z found(false); - switch(order) - { - case 1: found=SolveSimplex2(-SPXW(1),SPXW(0)-SPXW(1));break; - case 2: found=SolveSimplex3(-SPXW(2),SPXW(1)-SPXW(2),SPXW(0)-SPXW(2));break; - case 3: found=SolveSimplex4(-SPXW(3),SPXW(2)-SPXW(3),SPXW(1)-SPXW(3),SPXW(0)-SPXW(3));break; - } - if(found) return(true); - } else return(false); - } - failed=true; - return(false); - } - // - inline Z EncloseOrigin() - { - switch(order) - { - /* Point */ - case 0: break; - /* Line */ - case 1: - { - const Vector3 ab(SPXW(1)-SPXW(0)); - const Vector3 b[]={ cross(ab,Vector3(1,0,0)), - cross(ab,Vector3(0,1,0)), - cross(ab,Vector3(0,0,1))}; - const F m[]={b[0].length2(),b[1].length2(),b[2].length2()}; - const Rotation r(btQuaternion(ab.normalized(),cst2Pi/3)); - Vector3 w(b[m[0]>m[1]?m[0]>m[2]?0:2:m[1]>m[2]?1:2]); - Support(w.normalized(),simplex[4]);w=r*w; - Support(w.normalized(),simplex[2]);w=r*w; - Support(w.normalized(),simplex[3]);w=r*w; - order=4; - return(true); - } - break; - /* Triangle */ - case 2: - { - const -Vector3 n(cross((SPXW(1)-SPXW(0)),(SPXW(2)-SPXW(0))).normalized()); - Support( n,simplex[3]); - Support(-n,simplex[4]); - order=4; - return(true); - } - break; - /* Tetrahedron */ - case 3: return(true); - /* Hexahedron */ - case 4: return(true); - } - return(false); - } - #undef SPX - #undef SPXW - }; - -// -// EPA -// -struct EPA - { - // - struct Face - { - const GJK::Mkv* v[3]; - Face* f[3]; - U e[3]; - Vector3 n; - F d; - U mark; - Face* prev; - Face* next; - Face() {} - }; - // - GJK* gjk; - btStackAlloc* sa; - Face* root; - U nfaces; - U iterations; - Vector3 features[2][3]; - Vector3 nearest[2]; - Vector3 normal; - F depth; - Z failed; - // - EPA(GJK* pgjk) - { - gjk = pgjk; - sa = pgjk->sa; - } - // - ~EPA() - { - } - // - inline Vector3 GetCoordinates(const Face* face) const - { - const Vector3 o(face->n*-face->d); - const F a[]={ cross(face->v[0]->w-o,face->v[1]->w-o).length(), - cross(face->v[1]->w-o,face->v[2]->w-o).length(), - cross(face->v[2]->w-o,face->v[0]->w-o).length()}; - const F sm(a[0]+a[1]+a[2]); - return(Vector3(a[1],a[2],a[0])/(sm>0?sm:1)); - } - // - inline Face* FindBest() const - { - Face* bf = 0; - if(root) - { - Face* cf = root; - F bd(cstInf); - do { - if(cf->d<bd) { bd=cf->d;bf=cf; } - } while(0!=(cf=cf->next)); - } - return(bf); - } - // - inline Z Set(Face* f,const GJK::Mkv* a,const GJK::Mkv* b,const GJK::Mkv* -c) const - { - const Vector3 nrm(cross(b->w-a->w,c->w-a->w)); - const F len(nrm.length()); - const Z valid( (cross(a->w,b->w).dot(nrm)>=-EPA_inface_eps)&& - (cross(b->w,c->w).dot(nrm)>=-EPA_inface_eps)&& - (cross(c->w,a->w).dot(nrm)>=-EPA_inface_eps)); - f->v[0] = a; - f->v[1] = b; - f->v[2] = c; - f->mark = 0; - f->n = nrm/(len>0?len:cstInf); - f->d = Max<F>(0,-f->n.dot(a->w)); - return(valid); - } - // - inline Face* NewFace(const GJK::Mkv* a,const GJK::Mkv* b,const GJK::Mkv* c) - { - Face* pf = (Face*)sa->allocate(sizeof(Face)); - if(Set(pf,a,b,c)) - { - if(root) root->prev=pf; - pf->prev=0; - pf->next=root; - root =pf; - ++nfaces; - } - else - { - pf->prev=pf->next=0; - } - return(pf); - } - // - inline void Detach(Face* face) - { - if(face->prev||face->next) - { - --nfaces; - if(face==root) - { root=face->next;root->prev=0; } - else - { - if(face->next==0) - { face->prev->next=0; } - else - { face->prev->next=face->next;face->next->prev=face->prev; } - } - face->prev=face->next=0; - } - } - // - inline void Link(Face* f0,U e0,Face* f1,U e1) const - { - f0->f[e0]=f1;f1->e[e1]=e0; - f1->f[e1]=f0;f0->e[e0]=e1; - } - // - GJK::Mkv* Support(const Vector3& w) const - { - GJK::Mkv* v =(GJK::Mkv*)sa->allocate(sizeof(GJK::Mkv)); - gjk->Support(w,*v); - return(v); - } - // - U BuildHorizon(U markid,const GJK::Mkv* w,Face& f,U e,Face*& cf,Face*& -ff) - { - static const U mod3[]={0,1,2,0,1}; - U ne(0); - if(f.mark!=markid) - { - const U e1(mod3[e+1]); - if((f.n.dot(w->w)+f.d)>0) - { - Face* nf = NewFace(f.v[e1],f.v[e],w); - Link(nf,0,&f,e); - if(cf) Link(cf,1,nf,2); else ff=nf; - cf=nf;ne=1; - } - else - { - const U e2(mod3[e+2]); - Detach(&f); - f.mark = markid; - ne += BuildHorizon(markid,w,*f.f[e1],f.e[e1],cf,ff); - ne += BuildHorizon(markid,w,*f.f[e2],f.e[e2],cf,ff); - } - } - return(ne); - } - // - inline F EvaluatePD(F accuracy=EPA_accuracy) - { - btBlock* sablock = sa->beginBlock(); - Face* bestface = 0; - U markid(1); - depth = -cstInf; - normal = Vector3(0,0,0); - root = 0; - nfaces = 0; - iterations = 0; - failed = false; - /* Prepare hull */ - if(gjk->EncloseOrigin()) - { - const U* pfidx = 0; - U nfidx= 0; - const U* peidx = 0; - U neidx = 0; - GJK::Mkv* basemkv[5]; - Face* basefaces[6]; - switch(gjk->order) - { - /* Tetrahedron */ - case 3: { - static const U fidx[4][3]={{2,1,0},{3,0,1},{3,1,2},{3,2,0}}; - static const -U eidx[6][4]={{0,0,2,1},{0,1,1,1},{0,2,3,1},{1,0,3,2},{2,0,1,2},{3,0,2,2}}; - pfidx=(const U*)fidx;nfidx=4;peidx=(const U*)eidx;neidx=6; - } break; - /* Hexahedron */ - case 4: { - static const -U fidx[6][3]={{2,0,4},{4,1,2},{1,4,0},{0,3,1},{0,2,3},{1,3,2}}; - static const -U eidx[9][4]={{0,0,4,0},{0,1,2,1},{0,2,1,2},{1,1,5,2},{1,0,2,0},{2,2,3,2},{3,1,5,0},{3,0,4,2},{5,1,4,1}}; - pfidx=(const U*)fidx;nfidx=6;peidx=(const U*)eidx;neidx=9; - } break; - } - U i; - - for( i=0;i<=gjk->order;++i) { -basemkv[i]=(GJK::Mkv*)sa->allocate(sizeof(GJK::Mkv));*basemkv[i]=gjk->simplex[i]; -} - for( i=0;i<nfidx;++i,pfidx+=3) { -basefaces[i]=NewFace(basemkv[pfidx[0]],basemkv[pfidx[1]],basemkv[pfidx[2]]); -} - for( i=0;i<neidx;++i,peidx+=4) { -Link(basefaces[peidx[0]],peidx[1],basefaces[peidx[2]],peidx[3]); } - } - if(0==nfaces) - { - sa->endBlock(sablock); - return(depth); - } - /* Expand hull */ - for(;iterations<EPA_maxiterations;++iterations) - { - Face* bf = FindBest(); - if(bf) - { - GJK::Mkv* w = Support(-bf->n); - const F d(bf->n.dot(w->w)+bf->d); - bestface = bf; - if(d<-accuracy) - { - Face* cf =0; - Face* ff =0; - U nf = 0; - Detach(bf); - bf->mark=++markid; - for(U i=0;i<3;++i) { -nf+=BuildHorizon(markid,w,*bf->f[i],bf->e[i],cf,ff); } - if(nf<=2) { break; } - Link(cf,1,ff,2); - } else break; - } else break; - } - /* Extract contact */ - if(bestface) - { - const Vector3 b(GetCoordinates(bestface)); - normal = bestface->n; - depth = Max<F>(0,bestface->d); - for(U i=0;i<2;++i) - { - const F s(F(i?-1:1)); - for(U j=0;j<3;++j) - { - features[i][j]=gjk->LocalSupport(s*bestface->v[j]->r,i); - } - } - nearest[0] = features[0][0]*b.x()+features[0][1]*b.y()+features[0][2]*b.z(); - nearest[1] = features[1][0]*b.x()+features[1][1]*b.y()+features[1][2]*b.z(); - } else failed=true; - sa->endBlock(sablock); - return(depth); - } - }; -} - -// -// Api -// - -using namespace gjkepa_impl; - - - -// -bool btGjkEpaSolver::Collide(const btConvexShape *shape0,const btTransform &wtrs0, - const btConvexShape *shape1,const btTransform &wtrs1, - btScalar radialmargin, - btStackAlloc* stackAlloc, - sResults& results) -{ - - -/* Initialize */ -results.witnesses[0] = -results.witnesses[1] = -results.normal = Vector3(0,0,0); -results.depth = 0; -results.status = sResults::Separated; -results.epa_iterations = 0; -results.gjk_iterations = 0; -/* Use GJK to locate origin */ -GJK gjk(stackAlloc, - wtrs0.getBasis(),wtrs0.getOrigin(),shape0, - wtrs1.getBasis(),wtrs1.getOrigin(),shape1, - radialmargin+EPA_accuracy); -const Z collide(gjk.SearchOrigin()); -results.gjk_iterations = static_cast<int>(gjk.iterations+1); -if(collide) - { - /* Then EPA for penetration depth */ - EPA epa(&gjk); - const F pd(epa.EvaluatePD()); - results.epa_iterations = static_cast<int>(epa.iterations+1); - if(pd>0) - { - results.status = sResults::Penetrating; - results.normal = epa.normal; - results.depth = pd; - results.witnesses[0] = epa.nearest[0]; - results.witnesses[1] = epa.nearest[1]; - return(true); - } else { if(epa.failed) results.status=sResults::EPA_Failed; } - } else { if(gjk.failed) results.status=sResults::GJK_Failed; } -return(false); -} - - - - - |