diff options
Diffstat (limited to 'thirdparty/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3GjkEpa.cpp')
| -rw-r--r-- | thirdparty/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3GjkEpa.cpp | 1014 |
1 files changed, 1014 insertions, 0 deletions
diff --git a/thirdparty/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3GjkEpa.cpp b/thirdparty/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3GjkEpa.cpp new file mode 100644 index 0000000000..d636f983c6 --- /dev/null +++ b/thirdparty/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3GjkEpa.cpp @@ -0,0 +1,1014 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2008 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, 2008 +*/ + +#include "b3GjkEpa.h" + +#include "b3SupportMappings.h" + +namespace gjkepa2_impl2 +{ + + // Config + + /* GJK */ +#define GJK_MAX_ITERATIONS 128 +#define GJK_ACCURACY ((b3Scalar)0.0001) +#define GJK_MIN_DISTANCE ((b3Scalar)0.0001) +#define GJK_DUPLICATED_EPS ((b3Scalar)0.0001) +#define GJK_SIMPLEX2_EPS ((b3Scalar)0.0) +#define GJK_SIMPLEX3_EPS ((b3Scalar)0.0) +#define GJK_SIMPLEX4_EPS ((b3Scalar)0.0) + + /* EPA */ +#define EPA_MAX_VERTICES 64 +#define EPA_MAX_FACES (EPA_MAX_VERTICES*2) +#define EPA_MAX_ITERATIONS 255 +#define EPA_ACCURACY ((b3Scalar)0.0001) +#define EPA_FALLBACK (10*EPA_ACCURACY) +#define EPA_PLANE_EPS ((b3Scalar)0.00001) +#define EPA_INSIDE_EPS ((b3Scalar)0.01) + + + // Shorthands + + + // MinkowskiDiff + struct b3MinkowskiDiff + { + + + const b3ConvexPolyhedronData* m_shapes[2]; + + + b3Matrix3x3 m_toshape1; + b3Transform m_toshape0; + + bool m_enableMargin; + + + void EnableMargin(bool enable) + { + m_enableMargin = enable; + } + inline b3Vector3 Support0(const b3Vector3& d, const b3AlignedObjectArray<b3Vector3>& verticesA) const + { + if (m_enableMargin) + { + return localGetSupportVertexWithMargin(d,m_shapes[0],verticesA,0.f); + } else + { + return localGetSupportVertexWithoutMargin(d,m_shapes[0],verticesA); + } + } + inline b3Vector3 Support1(const b3Vector3& d, const b3AlignedObjectArray<b3Vector3>& verticesB) const + { + if (m_enableMargin) + { + return m_toshape0*(localGetSupportVertexWithMargin(m_toshape1*d,m_shapes[1],verticesB,0.f)); + } else + { + return m_toshape0*(localGetSupportVertexWithoutMargin(m_toshape1*d,m_shapes[1],verticesB)); + } + } + + inline b3Vector3 Support(const b3Vector3& d, const b3AlignedObjectArray<b3Vector3>& verticesA, const b3AlignedObjectArray<b3Vector3>& verticesB) const + { + return(Support0(d,verticesA)-Support1(-d,verticesB)); + } + b3Vector3 Support(const b3Vector3& d,unsigned int index,const b3AlignedObjectArray<b3Vector3>& verticesA, const b3AlignedObjectArray<b3Vector3>& verticesB) const + { + if(index) + return(Support1(d,verticesA)); + else + return(Support0(d,verticesB)); + } + }; + + typedef b3MinkowskiDiff tShape; + + + // GJK + struct b3GJK + { + /* Types */ + struct sSV + { + b3Vector3 d,w; + }; + struct sSimplex + { + sSV* c[4]; + b3Scalar p[4]; + unsigned int rank; + }; + struct eStatus { enum _ { + Valid, + Inside, + Failed };}; + /* Fields */ + tShape m_shape; + const b3AlignedObjectArray<b3Vector3>& m_verticesA; + const b3AlignedObjectArray<b3Vector3>& m_verticesB; + b3Vector3 m_ray; + b3Scalar m_distance; + sSimplex m_simplices[2]; + sSV m_store[4]; + sSV* m_free[4]; + unsigned int m_nfree; + unsigned int m_current; + sSimplex* m_simplex; + eStatus::_ m_status; + /* Methods */ + b3GJK(const b3AlignedObjectArray<b3Vector3>& verticesA,const b3AlignedObjectArray<b3Vector3>& verticesB) + :m_verticesA(verticesA),m_verticesB(verticesB) + { + Initialize(); + } + void Initialize() + { + m_ray = b3MakeVector3(0,0,0); + m_nfree = 0; + m_status = eStatus::Failed; + m_current = 0; + m_distance = 0; + } + eStatus::_ Evaluate(const tShape& shapearg,const b3Vector3& guess) + { + unsigned int iterations=0; + b3Scalar sqdist=0; + b3Scalar alpha=0; + b3Vector3 lastw[4]; + unsigned int clastw=0; + /* Initialize solver */ + m_free[0] = &m_store[0]; + m_free[1] = &m_store[1]; + m_free[2] = &m_store[2]; + m_free[3] = &m_store[3]; + m_nfree = 4; + m_current = 0; + m_status = eStatus::Valid; + m_shape = shapearg; + m_distance = 0; + /* Initialize simplex */ + m_simplices[0].rank = 0; + m_ray = guess; + const b3Scalar sqrl= m_ray.length2(); + appendvertice(m_simplices[0],sqrl>0?-m_ray:b3MakeVector3(1,0,0)); + m_simplices[0].p[0] = 1; + m_ray = m_simplices[0].c[0]->w; + sqdist = sqrl; + lastw[0] = + lastw[1] = + lastw[2] = + lastw[3] = m_ray; + /* Loop */ + do { + const unsigned int next=1-m_current; + sSimplex& cs=m_simplices[m_current]; + sSimplex& ns=m_simplices[next]; + /* Check zero */ + const b3Scalar rl=m_ray.length(); + if(rl<GJK_MIN_DISTANCE) + {/* Touching or inside */ + m_status=eStatus::Inside; + break; + } + /* Append new vertice in -'v' direction */ + appendvertice(cs,-m_ray); + const b3Vector3& w=cs.c[cs.rank-1]->w; + bool found=false; + for(unsigned int i=0;i<4;++i) + { + if((w-lastw[i]).length2()<GJK_DUPLICATED_EPS) + { found=true;break; } + } + if(found) + {/* Return old simplex */ + removevertice(m_simplices[m_current]); + break; + } + else + {/* Update lastw */ + lastw[clastw=(clastw+1)&3]=w; + } + /* Check for termination */ + const b3Scalar omega=b3Dot(m_ray,w)/rl; + alpha=b3Max(omega,alpha); + if(((rl-alpha)-(GJK_ACCURACY*rl))<=0) + {/* Return old simplex */ + removevertice(m_simplices[m_current]); + break; + } + /* Reduce simplex */ + b3Scalar weights[4]; + unsigned int mask=0; + switch(cs.rank) + { + case 2: sqdist=projectorigin( cs.c[0]->w, + cs.c[1]->w, + weights,mask);break; + case 3: sqdist=projectorigin( cs.c[0]->w, + cs.c[1]->w, + cs.c[2]->w, + weights,mask);break; + case 4: sqdist=projectorigin( cs.c[0]->w, + cs.c[1]->w, + cs.c[2]->w, + cs.c[3]->w, + weights,mask);break; + } + if(sqdist>=0) + {/* Valid */ + ns.rank = 0; + m_ray = b3MakeVector3(0,0,0); + m_current = next; + for(unsigned int i=0,ni=cs.rank;i<ni;++i) + { + if(mask&(1<<i)) + { + ns.c[ns.rank] = cs.c[i]; + ns.p[ns.rank++] = weights[i]; + m_ray += cs.c[i]->w*weights[i]; + } + else + { + m_free[m_nfree++] = cs.c[i]; + } + } + if(mask==15) m_status=eStatus::Inside; + } + else + {/* Return old simplex */ + removevertice(m_simplices[m_current]); + break; + } + m_status=((++iterations)<GJK_MAX_ITERATIONS)?m_status:eStatus::Failed; + } while(m_status==eStatus::Valid); + m_simplex=&m_simplices[m_current]; + switch(m_status) + { + case eStatus::Valid: m_distance=m_ray.length();break; + case eStatus::Inside: m_distance=0;break; + default: + { + } + } + return(m_status); + } + bool EncloseOrigin() + { + switch(m_simplex->rank) + { + case 1: + { + for(unsigned int i=0;i<3;++i) + { + b3Vector3 axis=b3MakeVector3(0,0,0); + axis[i]=1; + appendvertice(*m_simplex, axis); + if(EncloseOrigin()) return(true); + removevertice(*m_simplex); + appendvertice(*m_simplex,-axis); + if(EncloseOrigin()) return(true); + removevertice(*m_simplex); + } + } + break; + case 2: + { + const b3Vector3 d=m_simplex->c[1]->w-m_simplex->c[0]->w; + for(unsigned int i=0;i<3;++i) + { + b3Vector3 axis=b3MakeVector3(0,0,0); + axis[i]=1; + const b3Vector3 p=b3Cross(d,axis); + if(p.length2()>0) + { + appendvertice(*m_simplex, p); + if(EncloseOrigin()) return(true); + removevertice(*m_simplex); + appendvertice(*m_simplex,-p); + if(EncloseOrigin()) return(true); + removevertice(*m_simplex); + } + } + } + break; + case 3: + { + const b3Vector3 n=b3Cross(m_simplex->c[1]->w-m_simplex->c[0]->w, + m_simplex->c[2]->w-m_simplex->c[0]->w); + if(n.length2()>0) + { + appendvertice(*m_simplex,n); + if(EncloseOrigin()) return(true); + removevertice(*m_simplex); + appendvertice(*m_simplex,-n); + if(EncloseOrigin()) return(true); + removevertice(*m_simplex); + } + } + break; + case 4: + { + if(b3Fabs(det( m_simplex->c[0]->w-m_simplex->c[3]->w, + m_simplex->c[1]->w-m_simplex->c[3]->w, + m_simplex->c[2]->w-m_simplex->c[3]->w))>0) + return(true); + } + break; + } + return(false); + } + /* Internals */ + void getsupport(const b3Vector3& d,sSV& sv) const + { + sv.d = d/d.length(); + sv.w = m_shape.Support(sv.d,m_verticesA,m_verticesB); + } + void removevertice(sSimplex& simplex) + { + m_free[m_nfree++]=simplex.c[--simplex.rank]; + } + void appendvertice(sSimplex& simplex,const b3Vector3& v) + { + simplex.p[simplex.rank]=0; + simplex.c[simplex.rank]=m_free[--m_nfree]; + getsupport(v,*simplex.c[simplex.rank++]); + } + static b3Scalar det(const b3Vector3& a,const b3Vector3& b,const b3Vector3& c) + { + return( a.y*b.z*c.x+a.z*b.x*c.y- + a.x*b.z*c.y-a.y*b.x*c.z+ + a.x*b.y*c.z-a.z*b.y*c.x); + } + static b3Scalar projectorigin( const b3Vector3& a, + const b3Vector3& b, + b3Scalar* w,unsigned int& m) + { + const b3Vector3 d=b-a; + const b3Scalar l=d.length2(); + if(l>GJK_SIMPLEX2_EPS) + { + const b3Scalar t(l>0?-b3Dot(a,d)/l:0); + if(t>=1) { w[0]=0;w[1]=1;m=2;return(b.length2()); } + else if(t<=0) { w[0]=1;w[1]=0;m=1;return(a.length2()); } + else { w[0]=1-(w[1]=t);m=3;return((a+d*t).length2()); } + } + return(-1); + } + static b3Scalar projectorigin( const b3Vector3& a, + const b3Vector3& b, + const b3Vector3& c, + b3Scalar* w,unsigned int& m) + { + static const unsigned int imd3[]={1,2,0}; + const b3Vector3* vt[]={&a,&b,&c}; + const b3Vector3 dl[]={a-b,b-c,c-a}; + const b3Vector3 n=b3Cross(dl[0],dl[1]); + const b3Scalar l=n.length2(); + if(l>GJK_SIMPLEX3_EPS) + { + b3Scalar mindist=-1; + b3Scalar subw[2]={0.f,0.f}; + unsigned int subm(0); + for(unsigned int i=0;i<3;++i) + { + if(b3Dot(*vt[i],b3Cross(dl[i],n))>0) + { + const unsigned int j=imd3[i]; + const b3Scalar subd(projectorigin(*vt[i],*vt[j],subw,subm)); + if((mindist<0)||(subd<mindist)) + { + mindist = subd; + m = static_cast<unsigned int>(((subm&1)?1<<i:0)+((subm&2)?1<<j:0)); + w[i] = subw[0]; + w[j] = subw[1]; + w[imd3[j]] = 0; + } + } + } + if(mindist<0) + { + const b3Scalar d=b3Dot(a,n); + const b3Scalar s=b3Sqrt(l); + const b3Vector3 p=n*(d/l); + mindist = p.length2(); + m = 7; + w[0] = (b3Cross(dl[1],b-p)).length()/s; + w[1] = (b3Cross(dl[2],c-p)).length()/s; + w[2] = 1-(w[0]+w[1]); + } + return(mindist); + } + return(-1); + } + static b3Scalar projectorigin( const b3Vector3& a, + const b3Vector3& b, + const b3Vector3& c, + const b3Vector3& d, + b3Scalar* w,unsigned int& m) + { + static const unsigned int imd3[]={1,2,0}; + const b3Vector3* vt[]={&a,&b,&c,&d}; + const b3Vector3 dl[]={a-d,b-d,c-d}; + const b3Scalar vl=det(dl[0],dl[1],dl[2]); + const bool ng=(vl*b3Dot(a,b3Cross(b-c,a-b)))<=0; + if(ng&&(b3Fabs(vl)>GJK_SIMPLEX4_EPS)) + { + b3Scalar mindist=-1; + b3Scalar subw[3]={0.f,0.f,0.f}; + unsigned int subm(0); + for(unsigned int i=0;i<3;++i) + { + const unsigned int j=imd3[i]; + const b3Scalar s=vl*b3Dot(d,b3Cross(dl[i],dl[j])); + if(s>0) + { + const b3Scalar subd=projectorigin(*vt[i],*vt[j],d,subw,subm); + if((mindist<0)||(subd<mindist)) + { + mindist = subd; + m = static_cast<unsigned int>((subm&1?1<<i:0)+ + (subm&2?1<<j:0)+ + (subm&4?8:0)); + w[i] = subw[0]; + w[j] = subw[1]; + w[imd3[j]] = 0; + w[3] = subw[2]; + } + } + } + if(mindist<0) + { + mindist = 0; + m = 15; + w[0] = det(c,b,d)/vl; + w[1] = det(a,c,d)/vl; + w[2] = det(b,a,d)/vl; + w[3] = 1-(w[0]+w[1]+w[2]); + } + return(mindist); + } + return(-1); + } + }; + + // EPA + struct b3EPA + { + /* Types */ + typedef b3GJK::sSV sSV; + struct sFace + { + b3Vector3 n; + b3Scalar d; + sSV* c[3]; + sFace* f[3]; + sFace* l[2]; + unsigned char e[3]; + unsigned char pass; + }; + struct sList + { + sFace* root; + unsigned int count; + sList() : root(0),count(0) {} + }; + struct sHorizon + { + sFace* cf; + sFace* ff; + unsigned int nf; + sHorizon() : cf(0),ff(0),nf(0) {} + }; + struct eStatus { enum _ { + Valid, + Touching, + Degenerated, + NonConvex, + InvalidHull, + OutOfFaces, + OutOfVertices, + AccuraryReached, + FallBack, + Failed };}; + /* Fields */ + eStatus::_ m_status; + b3GJK::sSimplex m_result; + b3Vector3 m_normal; + b3Scalar m_depth; + sSV m_sv_store[EPA_MAX_VERTICES]; + sFace m_fc_store[EPA_MAX_FACES]; + unsigned int m_nextsv; + sList m_hull; + sList m_stock; + /* Methods */ + b3EPA() + { + Initialize(); + } + + + static inline void bind(sFace* fa,unsigned int ea,sFace* fb,unsigned int eb) + { + fa->e[ea]=(unsigned char)eb;fa->f[ea]=fb; + fb->e[eb]=(unsigned char)ea;fb->f[eb]=fa; + } + static inline void append(sList& list,sFace* face) + { + face->l[0] = 0; + face->l[1] = list.root; + if(list.root) list.root->l[0]=face; + list.root = face; + ++list.count; + } + static inline void remove(sList& list,sFace* face) + { + if(face->l[1]) face->l[1]->l[0]=face->l[0]; + if(face->l[0]) face->l[0]->l[1]=face->l[1]; + if(face==list.root) list.root=face->l[1]; + --list.count; + } + + + void Initialize() + { + m_status = eStatus::Failed; + m_normal = b3MakeVector3(0,0,0); + m_depth = 0; + m_nextsv = 0; + for(unsigned int i=0;i<EPA_MAX_FACES;++i) + { + append(m_stock,&m_fc_store[EPA_MAX_FACES-i-1]); + } + } + eStatus::_ Evaluate(b3GJK& gjk,const b3Vector3& guess) + { + b3GJK::sSimplex& simplex=*gjk.m_simplex; + if((simplex.rank>1)&&gjk.EncloseOrigin()) + { + + /* Clean up */ + while(m_hull.root) + { + sFace* f = m_hull.root; + remove(m_hull,f); + append(m_stock,f); + } + m_status = eStatus::Valid; + m_nextsv = 0; + /* Orient simplex */ + if(gjk.det( simplex.c[0]->w-simplex.c[3]->w, + simplex.c[1]->w-simplex.c[3]->w, + simplex.c[2]->w-simplex.c[3]->w)<0) + { + b3Swap(simplex.c[0],simplex.c[1]); + b3Swap(simplex.p[0],simplex.p[1]); + } + /* Build initial hull */ + sFace* tetra[]={newface(simplex.c[0],simplex.c[1],simplex.c[2],true), + newface(simplex.c[1],simplex.c[0],simplex.c[3],true), + newface(simplex.c[2],simplex.c[1],simplex.c[3],true), + newface(simplex.c[0],simplex.c[2],simplex.c[3],true)}; + if(m_hull.count==4) + { + sFace* best=findbest(); + sFace outer=*best; + unsigned int pass=0; + unsigned int iterations=0; + bind(tetra[0],0,tetra[1],0); + bind(tetra[0],1,tetra[2],0); + bind(tetra[0],2,tetra[3],0); + bind(tetra[1],1,tetra[3],2); + bind(tetra[1],2,tetra[2],1); + bind(tetra[2],2,tetra[3],1); + m_status=eStatus::Valid; + for(;iterations<EPA_MAX_ITERATIONS;++iterations) + { + if(m_nextsv<EPA_MAX_VERTICES) + { + sHorizon horizon; + sSV* w=&m_sv_store[m_nextsv++]; + bool valid=true; + best->pass = (unsigned char)(++pass); + gjk.getsupport(best->n,*w); + const b3Scalar wdist=b3Dot(best->n,w->w)-best->d; + if(wdist>EPA_ACCURACY) + { + for(unsigned int j=0;(j<3)&&valid;++j) + { + valid&=expand( pass,w, + best->f[j],best->e[j], + horizon); + } + if(valid&&(horizon.nf>=3)) + { + bind(horizon.cf,1,horizon.ff,2); + remove(m_hull,best); + append(m_stock,best); + best=findbest(); + outer=*best; + } else { + m_status=eStatus::Failed; + //m_status=eStatus::InvalidHull; + break; } + } else { m_status=eStatus::AccuraryReached;break; } + } else { m_status=eStatus::OutOfVertices;break; } + } + const b3Vector3 projection=outer.n*outer.d; + m_normal = outer.n; + m_depth = outer.d; + m_result.rank = 3; + m_result.c[0] = outer.c[0]; + m_result.c[1] = outer.c[1]; + m_result.c[2] = outer.c[2]; + m_result.p[0] = b3Cross( outer.c[1]->w-projection, + outer.c[2]->w-projection).length(); + m_result.p[1] = b3Cross( outer.c[2]->w-projection, + outer.c[0]->w-projection).length(); + m_result.p[2] = b3Cross( outer.c[0]->w-projection, + outer.c[1]->w-projection).length(); + const b3Scalar sum=m_result.p[0]+m_result.p[1]+m_result.p[2]; + m_result.p[0] /= sum; + m_result.p[1] /= sum; + m_result.p[2] /= sum; + return(m_status); + } + } + /* Fallback */ + m_status = eStatus::FallBack; + m_normal = -guess; + const b3Scalar nl=m_normal.length(); + if(nl>0) + m_normal = m_normal/nl; + else + m_normal = b3MakeVector3(1,0,0); + m_depth = 0; + m_result.rank=1; + m_result.c[0]=simplex.c[0]; + m_result.p[0]=1; + return(m_status); + } + bool getedgedist(sFace* face, sSV* a, sSV* b, b3Scalar& dist) + { + const b3Vector3 ba = b->w - a->w; + const b3Vector3 n_ab = b3Cross(ba, face->n); // Outward facing edge normal direction, on triangle plane + const b3Scalar a_dot_nab = b3Dot(a->w, n_ab); // Only care about the sign to determine inside/outside, so not normalization required + + if(a_dot_nab < 0) + { + // Outside of edge a->b + + const b3Scalar ba_l2 = ba.length2(); + const b3Scalar a_dot_ba = b3Dot(a->w, ba); + const b3Scalar b_dot_ba = b3Dot(b->w, ba); + + if(a_dot_ba > 0) + { + // Pick distance vertex a + dist = a->w.length(); + } + else if(b_dot_ba < 0) + { + // Pick distance vertex b + dist = b->w.length(); + } + else + { + // Pick distance to edge a->b + const b3Scalar a_dot_b = b3Dot(a->w, b->w); + dist = b3Sqrt(b3Max((a->w.length2() * b->w.length2() - a_dot_b * a_dot_b) / ba_l2, (b3Scalar)0)); + } + + return true; + } + + return false; + } + sFace* newface(sSV* a,sSV* b,sSV* c,bool forced) + { + if(m_stock.root) + { + sFace* face=m_stock.root; + remove(m_stock,face); + append(m_hull,face); + face->pass = 0; + face->c[0] = a; + face->c[1] = b; + face->c[2] = c; + face->n = b3Cross(b->w-a->w,c->w-a->w); + const b3Scalar l=face->n.length(); + const bool v=l>EPA_ACCURACY; + + if(v) + { + if(!(getedgedist(face, a, b, face->d) || + getedgedist(face, b, c, face->d) || + getedgedist(face, c, a, face->d))) + { + // Origin projects to the interior of the triangle + // Use distance to triangle plane + face->d = b3Dot(a->w, face->n) / l; + } + + face->n /= l; + if(forced || (face->d >= -EPA_PLANE_EPS)) + { + return face; + } + else + m_status=eStatus::NonConvex; + } + else + m_status=eStatus::Degenerated; + + remove(m_hull, face); + append(m_stock, face); + return 0; + + } + m_status = m_stock.root ? eStatus::OutOfVertices : eStatus::OutOfFaces; + return 0; + } + sFace* findbest() + { + sFace* minf=m_hull.root; + b3Scalar mind=minf->d*minf->d; + for(sFace* f=minf->l[1];f;f=f->l[1]) + { + const b3Scalar sqd=f->d*f->d; + if(sqd<mind) + { + minf=f; + mind=sqd; + } + } + return(minf); + } + bool expand(unsigned int pass,sSV* w,sFace* f,unsigned int e,sHorizon& horizon) + { + static const unsigned int i1m3[]={1,2,0}; + static const unsigned int i2m3[]={2,0,1}; + if(f->pass!=pass) + { + const unsigned int e1=i1m3[e]; + if((b3Dot(f->n,w->w)-f->d)<-EPA_PLANE_EPS) + { + sFace* nf=newface(f->c[e1],f->c[e],w,false); + if(nf) + { + bind(nf,0,f,e); + if(horizon.cf) bind(horizon.cf,1,nf,2); else horizon.ff=nf; + horizon.cf=nf; + ++horizon.nf; + return(true); + } + } + else + { + const unsigned int e2=i2m3[e]; + f->pass = (unsigned char)pass; + if( expand(pass,w,f->f[e1],f->e[e1],horizon)&& + expand(pass,w,f->f[e2],f->e[e2],horizon)) + { + remove(m_hull,f); + append(m_stock,f); + return(true); + } + } + } + return(false); + } + + }; + + // + static void Initialize(const b3Transform& transA, const b3Transform& transB, + const b3ConvexPolyhedronData* hullA, const b3ConvexPolyhedronData* hullB, + const b3AlignedObjectArray<b3Vector3>& verticesA, + const b3AlignedObjectArray<b3Vector3>& verticesB, + b3GjkEpaSolver2::sResults& results, + tShape& shape, + bool withmargins) + { + /* Results */ + results.witnesses[0] = + results.witnesses[1] = b3MakeVector3(0,0,0); + results.status = b3GjkEpaSolver2::sResults::Separated; + /* Shape */ + shape.m_shapes[0] = hullA; + shape.m_shapes[1] = hullB; + shape.m_toshape1 = transB.getBasis().transposeTimes(transA.getBasis()); + shape.m_toshape0 = transA.inverseTimes(transB); + shape.EnableMargin(withmargins); + } + +} + +// +// Api +// + +using namespace gjkepa2_impl2; + +// +int b3GjkEpaSolver2::StackSizeRequirement() +{ + return(sizeof(b3GJK)+sizeof(b3EPA)); +} + +// +bool b3GjkEpaSolver2::Distance( const b3Transform& transA, const b3Transform& transB, + const b3ConvexPolyhedronData* hullA, const b3ConvexPolyhedronData* hullB, + const b3AlignedObjectArray<b3Vector3>& verticesA, + const b3AlignedObjectArray<b3Vector3>& verticesB, + const b3Vector3& guess, + sResults& results) +{ + tShape shape; + Initialize(transA,transB,hullA,hullB,verticesA,verticesB,results,shape,false); + b3GJK gjk(verticesA,verticesB); + b3GJK::eStatus::_ gjk_status=gjk.Evaluate(shape,guess); + if(gjk_status==b3GJK::eStatus::Valid) + { + b3Vector3 w0=b3MakeVector3(0,0,0); + b3Vector3 w1=b3MakeVector3(0,0,0); + for(unsigned int i=0;i<gjk.m_simplex->rank;++i) + { + const b3Scalar p=gjk.m_simplex->p[i]; + w0+=shape.Support( gjk.m_simplex->c[i]->d,0,verticesA,verticesB)*p; + w1+=shape.Support(-gjk.m_simplex->c[i]->d,1,verticesA,verticesB)*p; + } + results.witnesses[0] = transA*w0; + results.witnesses[1] = transA*w1; + results.normal = w0-w1; + results.distance = results.normal.length(); + results.normal /= results.distance>GJK_MIN_DISTANCE?results.distance:1; + return(true); + } + else + { + results.status = gjk_status==b3GJK::eStatus::Inside? + sResults::Penetrating : + sResults::GJK_Failed ; + return(false); + } +} + +// +bool b3GjkEpaSolver2::Penetration( const b3Transform& transA, const b3Transform& transB, + const b3ConvexPolyhedronData* hullA, const b3ConvexPolyhedronData* hullB, + const b3AlignedObjectArray<b3Vector3>& verticesA, + const b3AlignedObjectArray<b3Vector3>& verticesB, + const b3Vector3& guess, + sResults& results, + bool usemargins) +{ + + tShape shape; + Initialize(transA,transB,hullA,hullB,verticesA,verticesB,results,shape,usemargins); + b3GJK gjk(verticesA,verticesB); + b3GJK::eStatus::_ gjk_status=gjk.Evaluate(shape,guess); + switch(gjk_status) + { + case b3GJK::eStatus::Inside: + { + b3EPA epa; + b3EPA::eStatus::_ epa_status=epa.Evaluate(gjk,-guess); + if(epa_status!=b3EPA::eStatus::Failed) + { + b3Vector3 w0=b3MakeVector3(0,0,0); + for(unsigned int i=0;i<epa.m_result.rank;++i) + { + w0+=shape.Support(epa.m_result.c[i]->d,0,verticesA,verticesB)*epa.m_result.p[i]; + } + results.status = sResults::Penetrating; + results.witnesses[0] = transA*w0; + results.witnesses[1] = transA*(w0-epa.m_normal*epa.m_depth); + results.normal = -epa.m_normal; + results.distance = -epa.m_depth; + return(true); + } else results.status=sResults::EPA_Failed; + } + break; + case b3GJK::eStatus::Failed: + results.status=sResults::GJK_Failed; + break; + default: + { + } + } + return(false); +} + + +#if 0 +// +b3Scalar b3GjkEpaSolver2::SignedDistance(const b3Vector3& position, + b3Scalar margin, + const b3Transform& transA, + const b3ConvexPolyhedronData& hullA, + const b3AlignedObjectArray<b3Vector3>& verticesA, + sResults& results) +{ + tShape shape; + btSphereShape shape1(margin); + b3Transform wtrs1(b3Quaternion(0,0,0,1),position); + Initialize(shape0,wtrs0,&shape1,wtrs1,results,shape,false); + GJK gjk; + GJK::eStatus::_ gjk_status=gjk.Evaluate(shape,b3Vector3(1,1,1)); + if(gjk_status==GJK::eStatus::Valid) + { + b3Vector3 w0=b3Vector3(0,0,0); + b3Vector3 w1=b3Vector3(0,0,0); + for(unsigned int i=0;i<gjk.m_simplex->rank;++i) + { + const b3Scalar p=gjk.m_simplex->p[i]; + w0+=shape.Support( gjk.m_simplex->c[i]->d,0)*p; + w1+=shape.Support(-gjk.m_simplex->c[i]->d,1)*p; + } + results.witnesses[0] = wtrs0*w0; + results.witnesses[1] = wtrs0*w1; + const b3Vector3 delta= results.witnesses[1]- + results.witnesses[0]; + const b3Scalar margin= shape0->getMarginNonVirtual()+ + shape1.getMarginNonVirtual(); + const b3Scalar length= delta.length(); + results.normal = delta/length; + results.witnesses[0] += results.normal*margin; + return(length-margin); + } + else + { + if(gjk_status==GJK::eStatus::Inside) + { + if(Penetration(shape0,wtrs0,&shape1,wtrs1,gjk.m_ray,results)) + { + const b3Vector3 delta= results.witnesses[0]- + results.witnesses[1]; + const b3Scalar length= delta.length(); + if (length >= B3_EPSILON) + results.normal = delta/length; + return(-length); + } + } + } + return(B3_INFINITY); +} + +// +bool b3GjkEpaSolver2::SignedDistance(const btConvexShape* shape0, + const b3Transform& wtrs0, + const btConvexShape* shape1, + const b3Transform& wtrs1, + const b3Vector3& guess, + sResults& results) +{ + if(!Distance(shape0,wtrs0,shape1,wtrs1,guess,results)) + return(Penetration(shape0,wtrs0,shape1,wtrs1,guess,results,false)); + else + return(true); +} +#endif + + +/* Symbols cleanup */ + +#undef GJK_MAX_ITERATIONS +#undef GJK_ACCURACY +#undef GJK_MIN_DISTANCE +#undef GJK_DUPLICATED_EPS +#undef GJK_SIMPLEX2_EPS +#undef GJK_SIMPLEX3_EPS +#undef GJK_SIMPLEX4_EPS + +#undef EPA_MAX_VERTICES +#undef EPA_MAX_FACES +#undef EPA_MAX_ITERATIONS +#undef EPA_ACCURACY +#undef EPA_FALLBACK +#undef EPA_PLANE_EPS +#undef EPA_INSIDE_EPS |