diff options
Diffstat (limited to 'extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.cpp')
| -rw-r--r-- | extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.cpp | 1703 |
1 files changed, 888 insertions, 815 deletions
diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.cpp index 3268f06c2f9..7d53f8624a8 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.cpp +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.cpp @@ -26,996 +26,1069 @@ GJK-EPA collision solver by Nathanael Presson, 2008 #include "BulletCollision/CollisionShapes/btSphereShape.h" #include "btGjkEpa2.h" -#if defined(DEBUG) || defined (_DEBUG) -#include <stdio.h> //for debug printf +#if defined(DEBUG) || defined(_DEBUG) +#include <stdio.h> //for debug printf #ifdef __SPU__ #include <spu_printf.h> #define printf spu_printf -#endif //__SPU__ +#endif //__SPU__ #endif namespace gjkepa2_impl { +// Config - // Config +/* GJK */ +#define GJK_MAX_ITERATIONS 128 - /* GJK */ -#define GJK_MAX_ITERATIONS 128 -#define GJK_ACCURARY ((btScalar)0.0001) -#define GJK_MIN_DISTANCE ((btScalar)0.0001) -#define GJK_DUPLICATED_EPS ((btScalar)0.0001) -#define GJK_SIMPLEX2_EPS ((btScalar)0.0) -#define GJK_SIMPLEX3_EPS ((btScalar)0.0) -#define GJK_SIMPLEX4_EPS ((btScalar)0.0) - - /* EPA */ -#define EPA_MAX_VERTICES 64 -#define EPA_MAX_FACES (EPA_MAX_VERTICES*2) -#define EPA_MAX_ITERATIONS 255 -#define EPA_ACCURACY ((btScalar)0.0001) -#define EPA_FALLBACK (10*EPA_ACCURACY) -#define EPA_PLANE_EPS ((btScalar)0.00001) -#define EPA_INSIDE_EPS ((btScalar)0.01) +#ifdef BT_USE_DOUBLE_PRECISION +#define GJK_ACCURACY ((btScalar)1e-12) +#define GJK_MIN_DISTANCE ((btScalar)1e-12) +#define GJK_DUPLICATED_EPS ((btScalar)1e-12) +#else +#define GJK_ACCURACY ((btScalar)0.0001) +#define GJK_MIN_DISTANCE ((btScalar)0.0001) +#define GJK_DUPLICATED_EPS ((btScalar)0.0001) +#endif //BT_USE_DOUBLE_PRECISION +#define GJK_SIMPLEX2_EPS ((btScalar)0.0) +#define GJK_SIMPLEX3_EPS ((btScalar)0.0) +#define GJK_SIMPLEX4_EPS ((btScalar)0.0) - // Shorthands - typedef unsigned int U; - typedef unsigned char U1; +/* EPA */ +#define EPA_MAX_VERTICES 128 +#define EPA_MAX_ITERATIONS 255 - // MinkowskiDiff - struct MinkowskiDiff - { - const btConvexShape* m_shapes[2]; - btMatrix3x3 m_toshape1; - btTransform m_toshape0; -#ifdef __SPU__ - bool m_enableMargin; +#ifdef BT_USE_DOUBLE_PRECISION +#define EPA_ACCURACY ((btScalar)1e-12) +#define EPA_PLANE_EPS ((btScalar)1e-14) +#define EPA_INSIDE_EPS ((btScalar)1e-9) #else - btVector3 (btConvexShape::*Ls)(const btVector3&) const; -#endif//__SPU__ - +#define EPA_ACCURACY ((btScalar)0.0001) +#define EPA_PLANE_EPS ((btScalar)0.00001) +#define EPA_INSIDE_EPS ((btScalar)0.01) +#endif - MinkowskiDiff() - { +#define EPA_FALLBACK (10 * EPA_ACCURACY) +#define EPA_MAX_FACES (EPA_MAX_VERTICES * 2) - } +// Shorthands +typedef unsigned int U; +typedef unsigned char U1; + +// MinkowskiDiff +struct MinkowskiDiff +{ + const btConvexShape* m_shapes[2]; + btMatrix3x3 m_toshape1; + btTransform m_toshape0; #ifdef __SPU__ - void EnableMargin(bool enable) - { - m_enableMargin = enable; - } - inline btVector3 Support0(const btVector3& d) const - { - if (m_enableMargin) - { - return m_shapes[0]->localGetSupportVertexNonVirtual(d); - } else - { - return m_shapes[0]->localGetSupportVertexWithoutMarginNonVirtual(d); - } - } - inline btVector3 Support1(const btVector3& d) const - { - if (m_enableMargin) - { - return m_toshape0*(m_shapes[1]->localGetSupportVertexNonVirtual(m_toshape1*d)); - } else - { - return m_toshape0*(m_shapes[1]->localGetSupportVertexWithoutMarginNonVirtual(m_toshape1*d)); - } - } + bool m_enableMargin; #else - void EnableMargin(bool enable) - { - if(enable) - Ls=&btConvexShape::localGetSupportVertexNonVirtual; - else - Ls=&btConvexShape::localGetSupportVertexWithoutMarginNonVirtual; - } - inline btVector3 Support0(const btVector3& d) const + btVector3 (btConvexShape::*Ls)(const btVector3&) const; +#endif //__SPU__ + + MinkowskiDiff() + { + } +#ifdef __SPU__ + void EnableMargin(bool enable) + { + m_enableMargin = enable; + } + inline btVector3 Support0(const btVector3& d) const + { + if (m_enableMargin) { - return(((m_shapes[0])->*(Ls))(d)); + return m_shapes[0]->localGetSupportVertexNonVirtual(d); } - inline btVector3 Support1(const btVector3& d) const + else { - return(m_toshape0*((m_shapes[1])->*(Ls))(m_toshape1*d)); + return m_shapes[0]->localGetSupportVertexWithoutMarginNonVirtual(d); } -#endif //__SPU__ - - inline btVector3 Support(const btVector3& d) const + } + inline btVector3 Support1(const btVector3& d) const + { + if (m_enableMargin) { - return(Support0(d)-Support1(-d)); + return m_toshape0 * (m_shapes[1]->localGetSupportVertexNonVirtual(m_toshape1 * d)); } - btVector3 Support(const btVector3& d,U index) const + else { - if(index) - return(Support1(d)); - else - return(Support0(d)); + return m_toshape0 * (m_shapes[1]->localGetSupportVertexWithoutMarginNonVirtual(m_toshape1 * d)); } - }; + } +#else + void EnableMargin(bool enable) + { + if (enable) + Ls = &btConvexShape::localGetSupportVertexNonVirtual; + else + Ls = &btConvexShape::localGetSupportVertexWithoutMarginNonVirtual; + } + inline btVector3 Support0(const btVector3& d) const + { + return (((m_shapes[0])->*(Ls))(d)); + } + inline btVector3 Support1(const btVector3& d) const + { + return (m_toshape0 * ((m_shapes[1])->*(Ls))(m_toshape1 * d)); + } +#endif //__SPU__ - typedef MinkowskiDiff tShape; + inline btVector3 Support(const btVector3& d) const + { + return (Support0(d) - Support1(-d)); + } + btVector3 Support(const btVector3& d, U index) const + { + if (index) + return (Support1(d)); + else + return (Support0(d)); + } +}; +typedef MinkowskiDiff tShape; - // GJK - struct GJK +// GJK +struct GJK +{ + /* Types */ + struct sSV { - /* Types */ - struct sSV - { - btVector3 d,w; - }; - struct sSimplex + btVector3 d, w; + }; + struct sSimplex + { + sSV* c[4]; + btScalar p[4]; + U rank; + }; + struct eStatus + { + enum _ { - sSV* c[4]; - btScalar p[4]; - U rank; - }; - struct eStatus { enum _ { Valid, Inside, - Failed };}; - /* Fields */ - tShape m_shape; - btVector3 m_ray; - btScalar m_distance; - sSimplex m_simplices[2]; - sSV m_store[4]; - sSV* m_free[4]; - U m_nfree; - U m_current; - sSimplex* m_simplex; - eStatus::_ m_status; - /* Methods */ - GJK() - { - Initialize(); + Failed + }; + }; + /* Fields */ + tShape m_shape; + btVector3 m_ray; + btScalar m_distance; + sSimplex m_simplices[2]; + sSV m_store[4]; + sSV* m_free[4]; + U m_nfree; + U m_current; + sSimplex* m_simplex; + eStatus::_ m_status; + /* Methods */ + GJK() + { + Initialize(); + } + void Initialize() + { + m_ray = btVector3(0, 0, 0); + m_nfree = 0; + m_status = eStatus::Failed; + m_current = 0; + m_distance = 0; + } + eStatus::_ Evaluate(const tShape& shapearg, const btVector3& guess) + { + U iterations = 0; + btScalar sqdist = 0; + btScalar alpha = 0; + btVector3 lastw[4]; + U 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 btScalar sqrl = m_ray.length2(); + appendvertice(m_simplices[0], sqrl > 0 ? -m_ray : btVector3(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 U next = 1 - m_current; + sSimplex& cs = m_simplices[m_current]; + sSimplex& ns = m_simplices[next]; + /* Check zero */ + const btScalar rl = m_ray.length(); + if (rl < GJK_MIN_DISTANCE) + { /* Touching or inside */ + m_status = eStatus::Inside; + break; } - void Initialize() + /* Append new vertice in -'v' direction */ + appendvertice(cs, -m_ray); + const btVector3& w = cs.c[cs.rank - 1]->w; + bool found = false; + for (U i = 0; i < 4; ++i) { - m_ray = btVector3(0,0,0); - m_nfree = 0; - m_status = eStatus::Failed; - m_current = 0; - m_distance = 0; + if ((w - lastw[i]).length2() < GJK_DUPLICATED_EPS) + { + found = true; + break; + } } - eStatus::_ Evaluate(const tShape& shapearg,const btVector3& guess) + if (found) + { /* Return old simplex */ + removevertice(m_simplices[m_current]); + break; + } + else + { /* Update lastw */ + lastw[clastw = (clastw + 1) & 3] = w; + } + /* Check for termination */ + const btScalar omega = btDot(m_ray, w) / rl; + alpha = btMax(omega, alpha); + if (((rl - alpha) - (GJK_ACCURACY * rl)) <= 0) + { /* Return old simplex */ + removevertice(m_simplices[m_current]); + break; + } + /* Reduce simplex */ + btScalar weights[4]; + U mask = 0; + switch (cs.rank) { - U iterations=0; - btScalar sqdist=0; - btScalar alpha=0; - btVector3 lastw[4]; - U 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 btScalar sqrl= m_ray.length2(); - appendvertice(m_simplices[0],sqrl>0?-m_ray:btVector3(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 U next=1-m_current; - sSimplex& cs=m_simplices[m_current]; - sSimplex& ns=m_simplices[next]; - /* Check zero */ - const btScalar 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 btVector3& w=cs.c[cs.rank-1]->w; - bool found=false; - for(U i=0;i<4;++i) + 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 = btVector3(0, 0, 0); + m_current = next; + for (U i = 0, ni = cs.rank; i < ni; ++i) + { + if (mask & (1 << i)) { - if((w-lastw[i]).length2()<GJK_DUPLICATED_EPS) - { found=true;break; } - } - if(found) - {/* Return old simplex */ - removevertice(m_simplices[m_current]); - break; + ns.c[ns.rank] = cs.c[i]; + ns.p[ns.rank++] = weights[i]; + m_ray += cs.c[i]->w * weights[i]; } else - {/* Update lastw */ - lastw[clastw=(clastw+1)&3]=w; - } - /* Check for termination */ - const btScalar omega=btDot(m_ray,w)/rl; - alpha=btMax(omega,alpha); - if(((rl-alpha)-(GJK_ACCURARY*rl))<=0) - {/* Return old simplex */ - removevertice(m_simplices[m_current]); - break; - } - /* Reduce simplex */ - btScalar weights[4]; - U 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 = btVector3(0,0,0); - m_current = next; - for(U 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_free[m_nfree++] = cs.c[i]; } - m_status=((++iterations)<GJK_MAX_ITERATIONS)?m_status:eStatus::Failed; - } while(m_status==eStatus::Valid); - m_simplex=&m_simplices[m_current]; - switch(m_status) + } + 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 (U i = 0; i < 3; ++i) { - case eStatus::Valid: m_distance=m_ray.length();break; - case eStatus::Inside: m_distance=0;break; - default: - { - } - } - return(m_status); + btVector3 axis = btVector3(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); + } } - bool EncloseOrigin() + break; + case 2: { - switch(m_simplex->rank) + const btVector3 d = m_simplex->c[1]->w - m_simplex->c[0]->w; + for (U i = 0; i < 3; ++i) { - case 1: + btVector3 axis = btVector3(0, 0, 0); + axis[i] = 1; + const btVector3 p = btCross(d, axis); + if (p.length2() > 0) { - for(U i=0;i<3;++i) - { - btVector3 axis=btVector3(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 btVector3 d=m_simplex->c[1]->w-m_simplex->c[0]->w; - for(U i=0;i<3;++i) - { - btVector3 axis=btVector3(0,0,0); - axis[i]=1; - const btVector3 p=btCross(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); - } - } + 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 btVector3 n=btCross(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(btFabs(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 btVector3& d,sSV& sv) const + break; + case 3: { - sv.d = d/d.length(); - sv.w = m_shape.Support(sv.d); + const btVector3 n = btCross(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); + } } - void removevertice(sSimplex& simplex) + break; + case 4: { - m_free[m_nfree++]=simplex.c[--simplex.rank]; + if (btFabs(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); } - void appendvertice(sSimplex& simplex,const btVector3& v) + break; + } + return (false); + } + /* Internals */ + void getsupport(const btVector3& d, sSV& sv) const + { + sv.d = d / d.length(); + sv.w = m_shape.Support(sv.d); + } + void removevertice(sSimplex& simplex) + { + m_free[m_nfree++] = simplex.c[--simplex.rank]; + } + void appendvertice(sSimplex& simplex, const btVector3& v) + { + simplex.p[simplex.rank] = 0; + simplex.c[simplex.rank] = m_free[--m_nfree]; + getsupport(v, *simplex.c[simplex.rank++]); + } + static btScalar det(const btVector3& a, const btVector3& b, const btVector3& 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 btScalar projectorigin(const btVector3& a, + const btVector3& b, + btScalar* w, U& m) + { + const btVector3 d = b - a; + const btScalar l = d.length2(); + if (l > GJK_SIMPLEX2_EPS) + { + const btScalar t(l > 0 ? -btDot(a, d) / l : 0); + if (t >= 1) { - simplex.p[simplex.rank]=0; - simplex.c[simplex.rank]=m_free[--m_nfree]; - getsupport(v,*simplex.c[simplex.rank++]); + w[0] = 0; + w[1] = 1; + m = 2; + return (b.length2()); } - static btScalar det(const btVector3& a,const btVector3& b,const btVector3& c) + else if (t <= 0) { - 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()); + w[0] = 1; + w[1] = 0; + m = 1; + return (a.length2()); } - static btScalar projectorigin( const btVector3& a, - const btVector3& b, - btScalar* w,U& m) + else { - const btVector3 d=b-a; - const btScalar l=d.length2(); - if(l>GJK_SIMPLEX2_EPS) - { - const btScalar t(l>0?-btDot(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); + w[0] = 1 - (w[1] = t); + m = 3; + return ((a + d * t).length2()); } - static btScalar projectorigin( const btVector3& a, - const btVector3& b, - const btVector3& c, - btScalar* w,U& m) + } + return (-1); + } + static btScalar projectorigin(const btVector3& a, + const btVector3& b, + const btVector3& c, + btScalar* w, U& m) + { + static const U imd3[] = {1, 2, 0}; + const btVector3* vt[] = {&a, &b, &c}; + const btVector3 dl[] = {a - b, b - c, c - a}; + const btVector3 n = btCross(dl[0], dl[1]); + const btScalar l = n.length2(); + if (l > GJK_SIMPLEX3_EPS) + { + btScalar mindist = -1; + btScalar subw[2] = {0.f, 0.f}; + U subm(0); + for (U i = 0; i < 3; ++i) { - static const U imd3[]={1,2,0}; - const btVector3* vt[]={&a,&b,&c}; - const btVector3 dl[]={a-b,b-c,c-a}; - const btVector3 n=btCross(dl[0],dl[1]); - const btScalar l=n.length2(); - if(l>GJK_SIMPLEX3_EPS) + if (btDot(*vt[i], btCross(dl[i], n)) > 0) { - btScalar mindist=-1; - btScalar subw[2]={0.f,0.f}; - U subm(0); - for(U i=0;i<3;++i) + const U j = imd3[i]; + const btScalar subd(projectorigin(*vt[i], *vt[j], subw, subm)); + if ((mindist < 0) || (subd < mindist)) { - if(btDot(*vt[i],btCross(dl[i],n))>0) - { - const U j=imd3[i]; - const btScalar subd(projectorigin(*vt[i],*vt[j],subw,subm)); - if((mindist<0)||(subd<mindist)) - { - mindist = subd; - m = static_cast<U>(((subm&1)?1<<i:0)+((subm&2)?1<<j:0)); - w[i] = subw[0]; - w[j] = subw[1]; - w[imd3[j]] = 0; - } - } + mindist = subd; + m = static_cast<U>(((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 btScalar d=btDot(a,n); - const btScalar s=btSqrt(l); - const btVector3 p=n*(d/l); - mindist = p.length2(); - m = 7; - w[0] = (btCross(dl[1],b-p)).length()/s; - w[1] = (btCross(dl[2],c-p)).length()/s; - w[2] = 1-(w[0]+w[1]); - } - return(mindist); } - return(-1); } - static btScalar projectorigin( const btVector3& a, - const btVector3& b, - const btVector3& c, - const btVector3& d, - btScalar* w,U& m) + if (mindist < 0) { - static const U imd3[]={1,2,0}; - const btVector3* vt[]={&a,&b,&c,&d}; - const btVector3 dl[]={a-d,b-d,c-d}; - const btScalar vl=det(dl[0],dl[1],dl[2]); - const bool ng=(vl*btDot(a,btCross(b-c,a-b)))<=0; - if(ng&&(btFabs(vl)>GJK_SIMPLEX4_EPS)) + const btScalar d = btDot(a, n); + const btScalar s = btSqrt(l); + const btVector3 p = n * (d / l); + mindist = p.length2(); + m = 7; + w[0] = (btCross(dl[1], b - p)).length() / s; + w[1] = (btCross(dl[2], c - p)).length() / s; + w[2] = 1 - (w[0] + w[1]); + } + return (mindist); + } + return (-1); + } + static btScalar projectorigin(const btVector3& a, + const btVector3& b, + const btVector3& c, + const btVector3& d, + btScalar* w, U& m) + { + static const U imd3[] = {1, 2, 0}; + const btVector3* vt[] = {&a, &b, &c, &d}; + const btVector3 dl[] = {a - d, b - d, c - d}; + const btScalar vl = det(dl[0], dl[1], dl[2]); + const bool ng = (vl * btDot(a, btCross(b - c, a - b))) <= 0; + if (ng && (btFabs(vl) > GJK_SIMPLEX4_EPS)) + { + btScalar mindist = -1; + btScalar subw[3] = {0.f, 0.f, 0.f}; + U subm(0); + for (U i = 0; i < 3; ++i) + { + const U j = imd3[i]; + const btScalar s = vl * btDot(d, btCross(dl[i], dl[j])); + if (s > 0) { - btScalar mindist=-1; - btScalar subw[3]={0.f,0.f,0.f}; - U subm(0); - for(U i=0;i<3;++i) - { - const U j=imd3[i]; - const btScalar s=vl*btDot(d,btCross(dl[i],dl[j])); - if(s>0) - { - const btScalar subd=projectorigin(*vt[i],*vt[j],d,subw,subm); - if((mindist<0)||(subd<mindist)) - { - mindist = subd; - m = static_cast<U>((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) + const btScalar subd = projectorigin(*vt[i], *vt[j], d, subw, subm); + if ((mindist < 0) || (subd < mindist)) { - 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]); + mindist = subd; + m = static_cast<U>((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]; } - return(mindist); } - return(-1); } - }; + 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 EPA +// EPA +struct EPA +{ + /* Types */ + typedef GJK::sSV sSV; + struct sFace { - /* Types */ - typedef GJK::sSV sSV; - struct sFace - { - btVector3 n; - btScalar d; - sSV* c[3]; - sFace* f[3]; - sFace* l[2]; - U1 e[3]; - U1 pass; - }; - struct sList - { - sFace* root; - U count; - sList() : root(0),count(0) {} - }; - struct sHorizon + btVector3 n; + btScalar d; + sSV* c[3]; + sFace* f[3]; + sFace* l[2]; + U1 e[3]; + U1 pass; + }; + struct sList + { + sFace* root; + U count; + sList() : root(0), count(0) {} + }; + struct sHorizon + { + sFace* cf; + sFace* ff; + U nf; + sHorizon() : cf(0), ff(0), nf(0) {} + }; + struct eStatus + { + enum _ { - sFace* cf; - sFace* ff; - U nf; - sHorizon() : cf(0),ff(0),nf(0) {} - }; - struct eStatus { enum _ { Valid, Touching, Degenerated, NonConvex, - InvalidHull, + InvalidHull, OutOfFaces, OutOfVertices, AccuraryReached, FallBack, - Failed };}; - /* Fields */ - eStatus::_ m_status; - GJK::sSimplex m_result; - btVector3 m_normal; - btScalar m_depth; - sSV m_sv_store[EPA_MAX_VERTICES]; - sFace m_fc_store[EPA_MAX_FACES]; - U m_nextsv; - sList m_hull; - sList m_stock; - /* Methods */ - EPA() - { - Initialize(); - } + Failed + }; + }; + /* Fields */ + eStatus::_ m_status; + GJK::sSimplex m_result; + btVector3 m_normal; + btScalar m_depth; + sSV m_sv_store[EPA_MAX_VERTICES]; + sFace m_fc_store[EPA_MAX_FACES]; + U m_nextsv; + sList m_hull; + sList m_stock; + /* Methods */ + EPA() + { + Initialize(); + } + static inline void bind(sFace* fa, U ea, sFace* fb, U eb) + { + fa->e[ea] = (U1)eb; + fa->f[ea] = fb; + fb->e[eb] = (U1)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; + } - static inline void bind(sFace* fa,U ea,sFace* fb,U eb) - { - fa->e[ea]=(U1)eb;fa->f[ea]=fb; - fb->e[eb]=(U1)ea;fb->f[eb]=fa; - } - static inline void append(sList& list,sFace* face) + void Initialize() + { + m_status = eStatus::Failed; + m_normal = btVector3(0, 0, 0); + m_depth = 0; + m_nextsv = 0; + for (U i = 0; i < EPA_MAX_FACES; ++i) + { + append(m_stock, &m_fc_store[EPA_MAX_FACES - i - 1]); + } + } + eStatus::_ Evaluate(GJK& gjk, const btVector3& guess) + { + GJK::sSimplex& simplex = *gjk.m_simplex; + if ((simplex.rank > 1) && gjk.EncloseOrigin()) + { + /* Clean up */ + while (m_hull.root) { - face->l[0] = 0; - face->l[1] = list.root; - if(list.root) list.root->l[0]=face; - list.root = face; - ++list.count; + sFace* f = m_hull.root; + remove(m_hull, f); + append(m_stock, f); } - static inline void remove(sList& list,sFace* face) + 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) { - 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; + btSwap(simplex.c[0], simplex.c[1]); + btSwap(simplex.p[0], simplex.p[1]); } - - - void Initialize() + /* 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) { - m_status = eStatus::Failed; - m_normal = btVector3(0,0,0); - m_depth = 0; - m_nextsv = 0; - for(U i=0;i<EPA_MAX_FACES;++i) + sFace* best = findbest(); + sFace outer = *best; + U pass = 0; + U 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) { - append(m_stock,&m_fc_store[EPA_MAX_FACES-i-1]); - } - } - eStatus::_ Evaluate(GJK& gjk,const btVector3& guess) - { - GJK::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) - { - btSwap(simplex.c[0],simplex.c[1]); - btSwap(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) + if (m_nextsv < EPA_MAX_VERTICES) { - sFace* best=findbest(); - sFace outer=*best; - U pass=0; - U 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) + sHorizon horizon; + sSV* w = &m_sv_store[m_nextsv++]; + bool valid = true; + best->pass = (U1)(++pass); + gjk.getsupport(best->n, *w); + const btScalar wdist = btDot(best->n, w->w) - best->d; + if (wdist > EPA_ACCURACY) { - if(m_nextsv<EPA_MAX_VERTICES) - { - sHorizon horizon; - sSV* w=&m_sv_store[m_nextsv++]; - bool valid=true; - best->pass = (U1)(++pass); - gjk.getsupport(best->n,*w); - const btScalar wdist=btDot(best->n,w->w)-best->d; - if(wdist>EPA_ACCURACY) - { - for(U 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::InvalidHull;break; } - } else { m_status=eStatus::AccuraryReached;break; } - } else { m_status=eStatus::OutOfVertices;break; } + for (U 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::InvalidHull; + break; + } + } + else + { + m_status = eStatus::AccuraryReached; + break; } - const btVector3 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] = btCross( outer.c[1]->w-projection, - outer.c[2]->w-projection).length(); - m_result.p[1] = btCross( outer.c[2]->w-projection, - outer.c[0]->w-projection).length(); - m_result.p[2] = btCross( outer.c[0]->w-projection, - outer.c[1]->w-projection).length(); - const btScalar 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 btScalar nl=m_normal.length(); - if(nl>0) - m_normal = m_normal/nl; - else - m_normal = btVector3(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, btScalar& dist) - { - const btVector3 ba = b->w - a->w; - const btVector3 n_ab = btCross(ba, face->n); // Outward facing edge normal direction, on triangle plane - const btScalar a_dot_nab = btDot(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 btScalar ba_l2 = ba.length2(); - const btScalar a_dot_ba = btDot(a->w, ba); - const btScalar b_dot_ba = btDot(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 btScalar a_dot_b = btDot(a->w, b->w); - dist = btSqrt(btMax((a->w.length2() * b->w.length2() - a_dot_b * a_dot_b) / ba_l2, (btScalar)0)); + m_status = eStatus::OutOfVertices; + break; } - - return true; } + const btVector3 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] = btCross(outer.c[1]->w - projection, + outer.c[2]->w - projection) + .length(); + m_result.p[1] = btCross(outer.c[2]->w - projection, + outer.c[0]->w - projection) + .length(); + m_result.p[2] = btCross(outer.c[0]->w - projection, + outer.c[1]->w - projection) + .length(); + const btScalar 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 btScalar nl = m_normal.length(); + if (nl > 0) + m_normal = m_normal / nl; + else + m_normal = btVector3(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, btScalar& dist) + { + const btVector3 ba = b->w - a->w; + const btVector3 n_ab = btCross(ba, face->n); // Outward facing edge normal direction, on triangle plane + const btScalar a_dot_nab = btDot(a->w, n_ab); // Only care about the sign to determine inside/outside, so not normalization required - return false; + if (a_dot_nab < 0) + { + // Outside of edge a->b + + const btScalar ba_l2 = ba.length2(); + const btScalar a_dot_ba = btDot(a->w, ba); + const btScalar b_dot_ba = btDot(b->w, ba); + + if (a_dot_ba > 0) + { + // Pick distance vertex a + dist = a->w.length(); } - sFace* newface(sSV* a,sSV* b,sSV* c,bool forced) + else if (b_dot_ba < 0) { - 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 = btCross(b->w-a->w,c->w-a->w); - const btScalar l=face->n.length(); - const bool v=l>EPA_ACCURACY; + // Pick distance vertex b + dist = b->w.length(); + } + else + { + // Pick distance to edge a->b + const btScalar a_dot_b = btDot(a->w, b->w); + dist = btSqrt(btMax((a->w.length2() * b->w.length2() - a_dot_b * a_dot_b) / ba_l2, (btScalar)0)); + } - 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 = btDot(a->w, face->n) / l; - } + return true; + } - face->n /= l; - if(forced || (face->d >= -EPA_PLANE_EPS)) - { - return face; - } - else - m_status=eStatus::NonConvex; - } - else - m_status=eStatus::Degenerated; + 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 = btCross(b->w - a->w, c->w - a->w); + const btScalar l = face->n.length(); + const bool v = l > EPA_ACCURACY; - remove(m_hull, face); - append(m_stock, face); - return 0; + 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 = btDot(a->w, face->n) / l; + } + face->n /= l; + if (forced || (face->d >= -EPA_PLANE_EPS)) + { + return face; } - m_status = m_stock.root ? eStatus::OutOfVertices : eStatus::OutOfFaces; - return 0; + 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; + btScalar mind = minf->d * minf->d; + for (sFace* f = minf->l[1]; f; f = f->l[1]) + { + const btScalar sqd = f->d * f->d; + if (sqd < mind) + { + minf = f; + mind = sqd; } - sFace* findbest() + } + return (minf); + } + bool expand(U pass, sSV* w, sFace* f, U e, sHorizon& horizon) + { + static const U i1m3[] = {1, 2, 0}; + static const U i2m3[] = {2, 0, 1}; + if (f->pass != pass) + { + const U e1 = i1m3[e]; + if ((btDot(f->n, w->w) - f->d) < -EPA_PLANE_EPS) { - sFace* minf=m_hull.root; - btScalar mind=minf->d*minf->d; - for(sFace* f=minf->l[1];f;f=f->l[1]) + sFace* nf = newface(f->c[e1], f->c[e], w, false); + if (nf) { - const btScalar sqd=f->d*f->d; - if(sqd<mind) - { - minf=f; - mind=sqd; - } + 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); } - return(minf); } - bool expand(U pass,sSV* w,sFace* f,U e,sHorizon& horizon) + else { - static const U i1m3[]={1,2,0}; - static const U i2m3[]={2,0,1}; - if(f->pass!=pass) + const U e2 = i2m3[e]; + f->pass = (U1)pass; + if (expand(pass, w, f->f[e1], f->e[e1], horizon) && + expand(pass, w, f->f[e2], f->e[e2], horizon)) { - const U e1=i1m3[e]; - if((btDot(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 U e2=i2m3[e]; - f->pass = (U1)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); - } - } + remove(m_hull, f); + append(m_stock, f); + return (true); } - return(false); } - - }; - - // - static void Initialize( const btConvexShape* shape0,const btTransform& wtrs0, - const btConvexShape* shape1,const btTransform& wtrs1, - btGjkEpaSolver2::sResults& results, - tShape& shape, - bool withmargins) - { - /* Results */ - results.witnesses[0] = - results.witnesses[1] = btVector3(0,0,0); - results.status = btGjkEpaSolver2::sResults::Separated; - /* Shape */ - shape.m_shapes[0] = shape0; - shape.m_shapes[1] = shape1; - shape.m_toshape1 = wtrs1.getBasis().transposeTimes(wtrs0.getBasis()); - shape.m_toshape0 = wtrs0.inverseTimes(wtrs1); - shape.EnableMargin(withmargins); + } + return (false); } +}; +// +static void Initialize(const btConvexShape* shape0, const btTransform& wtrs0, + const btConvexShape* shape1, const btTransform& wtrs1, + btGjkEpaSolver2::sResults& results, + tShape& shape, + bool withmargins) +{ + /* Results */ + results.witnesses[0] = + results.witnesses[1] = btVector3(0, 0, 0); + results.status = btGjkEpaSolver2::sResults::Separated; + /* Shape */ + shape.m_shapes[0] = shape0; + shape.m_shapes[1] = shape1; + shape.m_toshape1 = wtrs1.getBasis().transposeTimes(wtrs0.getBasis()); + shape.m_toshape0 = wtrs0.inverseTimes(wtrs1); + shape.EnableMargin(withmargins); } +} // namespace gjkepa2_impl + // // Api // -using namespace gjkepa2_impl; +using namespace gjkepa2_impl; // -int btGjkEpaSolver2::StackSizeRequirement() +int btGjkEpaSolver2::StackSizeRequirement() { - return(sizeof(GJK)+sizeof(EPA)); + return (sizeof(GJK) + sizeof(EPA)); } // -bool btGjkEpaSolver2::Distance( const btConvexShape* shape0, - const btTransform& wtrs0, - const btConvexShape* shape1, - const btTransform& wtrs1, - const btVector3& guess, - sResults& results) +bool btGjkEpaSolver2::Distance(const btConvexShape* shape0, + const btTransform& wtrs0, + const btConvexShape* shape1, + const btTransform& wtrs1, + const btVector3& guess, + sResults& results) { - tShape shape; - Initialize(shape0,wtrs0,shape1,wtrs1,results,shape,false); - GJK gjk; - GJK::eStatus::_ gjk_status=gjk.Evaluate(shape,guess); - if(gjk_status==GJK::eStatus::Valid) + tShape shape; + Initialize(shape0, wtrs0, shape1, wtrs1, results, shape, false); + GJK gjk; + GJK::eStatus::_ gjk_status = gjk.Evaluate(shape, guess); + if (gjk_status == GJK::eStatus::Valid) { - btVector3 w0=btVector3(0,0,0); - btVector3 w1=btVector3(0,0,0); - for(U i=0;i<gjk.m_simplex->rank;++i) + btVector3 w0 = btVector3(0, 0, 0); + btVector3 w1 = btVector3(0, 0, 0); + for (U i = 0; i < gjk.m_simplex->rank; ++i) { - const btScalar 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; + const btScalar 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; - results.normal = w0-w1; - results.distance = results.normal.length(); - results.normal /= results.distance>GJK_MIN_DISTANCE?results.distance:1; - return(true); + results.witnesses[0] = wtrs0 * w0; + results.witnesses[1] = wtrs0 * 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==GJK::eStatus::Inside? - sResults::Penetrating : - sResults::GJK_Failed ; - return(false); + results.status = gjk_status == GJK::eStatus::Inside ? sResults::Penetrating : sResults::GJK_Failed; + return (false); } } // -bool btGjkEpaSolver2::Penetration( const btConvexShape* shape0, - const btTransform& wtrs0, - const btConvexShape* shape1, - const btTransform& wtrs1, - const btVector3& guess, - sResults& results, - bool usemargins) +bool btGjkEpaSolver2::Penetration(const btConvexShape* shape0, + const btTransform& wtrs0, + const btConvexShape* shape1, + const btTransform& wtrs1, + const btVector3& guess, + sResults& results, + bool usemargins) { - tShape shape; - Initialize(shape0,wtrs0,shape1,wtrs1,results,shape,usemargins); - GJK gjk; - GJK::eStatus::_ gjk_status=gjk.Evaluate(shape,-guess); - switch(gjk_status) + tShape shape; + Initialize(shape0, wtrs0, shape1, wtrs1, results, shape, usemargins); + GJK gjk; + GJK::eStatus::_ gjk_status = gjk.Evaluate(shape, -guess); + switch (gjk_status) { - case GJK::eStatus::Inside: + case GJK::eStatus::Inside: { - EPA epa; - EPA::eStatus::_ epa_status=epa.Evaluate(gjk,-guess); - if(epa_status!=EPA::eStatus::Failed) + EPA epa; + EPA::eStatus::_ epa_status = epa.Evaluate(gjk, -guess); + if (epa_status != EPA::eStatus::Failed) { - btVector3 w0=btVector3(0,0,0); - for(U i=0;i<epa.m_result.rank;++i) + btVector3 w0 = btVector3(0, 0, 0); + for (U i = 0; i < epa.m_result.rank; ++i) { - w0+=shape.Support(epa.m_result.c[i]->d,0)*epa.m_result.p[i]; + w0 += shape.Support(epa.m_result.c[i]->d, 0) * epa.m_result.p[i]; } - results.status = sResults::Penetrating; - results.witnesses[0] = wtrs0*w0; - results.witnesses[1] = wtrs0*(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; + results.status = sResults::Penetrating; + results.witnesses[0] = wtrs0 * w0; + results.witnesses[1] = wtrs0 * (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 GJK::eStatus::Failed: - results.status=sResults::GJK_Failed; - break; + case GJK::eStatus::Failed: + results.status = sResults::GJK_Failed; + break; default: - { - } + { + } } - return(false); + return (false); } #ifndef __SPU__ // -btScalar btGjkEpaSolver2::SignedDistance(const btVector3& position, - btScalar margin, - const btConvexShape* shape0, - const btTransform& wtrs0, - sResults& results) +btScalar btGjkEpaSolver2::SignedDistance(const btVector3& position, + btScalar margin, + const btConvexShape* shape0, + const btTransform& wtrs0, + sResults& results) { - tShape shape; - btSphereShape shape1(margin); - btTransform wtrs1(btQuaternion(0,0,0,1),position); - Initialize(shape0,wtrs0,&shape1,wtrs1,results,shape,false); - GJK gjk; - GJK::eStatus::_ gjk_status=gjk.Evaluate(shape,btVector3(1,1,1)); - if(gjk_status==GJK::eStatus::Valid) + tShape shape; + btSphereShape shape1(margin); + btTransform wtrs1(btQuaternion(0, 0, 0, 1), position); + Initialize(shape0, wtrs0, &shape1, wtrs1, results, shape, false); + GJK gjk; + GJK::eStatus::_ gjk_status = gjk.Evaluate(shape, btVector3(1, 1, 1)); + if (gjk_status == GJK::eStatus::Valid) { - btVector3 w0=btVector3(0,0,0); - btVector3 w1=btVector3(0,0,0); - for(U i=0;i<gjk.m_simplex->rank;++i) + btVector3 w0 = btVector3(0, 0, 0); + btVector3 w1 = btVector3(0, 0, 0); + for (U i = 0; i < gjk.m_simplex->rank; ++i) { - const btScalar 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; + const btScalar 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 btVector3 delta= results.witnesses[1]- - results.witnesses[0]; - const btScalar margin= shape0->getMarginNonVirtual()+ - shape1.getMarginNonVirtual(); - const btScalar length= delta.length(); - results.normal = delta/length; - results.witnesses[0] += results.normal*margin; - return(length-margin); + results.witnesses[0] = wtrs0 * w0; + results.witnesses[1] = wtrs0 * w1; + const btVector3 delta = results.witnesses[1] - + results.witnesses[0]; + const btScalar margin = shape0->getMarginNonVirtual() + + shape1.getMarginNonVirtual(); + const btScalar length = delta.length(); + results.normal = delta / length; + results.witnesses[0] += results.normal * margin; + results.distance = length - margin; + return results.distance; } else { - if(gjk_status==GJK::eStatus::Inside) + if (gjk_status == GJK::eStatus::Inside) { - if(Penetration(shape0,wtrs0,&shape1,wtrs1,gjk.m_ray,results)) + if (Penetration(shape0, wtrs0, &shape1, wtrs1, gjk.m_ray, results)) { - const btVector3 delta= results.witnesses[0]- - results.witnesses[1]; - const btScalar length= delta.length(); + const btVector3 delta = results.witnesses[0] - + results.witnesses[1]; + const btScalar length = delta.length(); if (length >= SIMD_EPSILON) - results.normal = delta/length; - return(-length); + results.normal = delta / length; + return (-length); } - } + } } - return(SIMD_INFINITY); + return (SIMD_INFINITY); } // -bool btGjkEpaSolver2::SignedDistance(const btConvexShape* shape0, - const btTransform& wtrs0, - const btConvexShape* shape1, - const btTransform& wtrs1, - const btVector3& guess, - sResults& results) +bool btGjkEpaSolver2::SignedDistance(const btConvexShape* shape0, + const btTransform& wtrs0, + const btConvexShape* shape1, + const btTransform& wtrs1, + const btVector3& guess, + sResults& results) { - if(!Distance(shape0,wtrs0,shape1,wtrs1,guess,results)) - return(Penetration(shape0,wtrs0,shape1,wtrs1,guess,results,false)); + if (!Distance(shape0, wtrs0, shape1, wtrs1, guess, results)) + return (Penetration(shape0, wtrs0, shape1, wtrs1, guess, results, false)); else - return(true); + return (true); } -#endif //__SPU__ +#endif //__SPU__ -/* Symbols cleanup */ +/* Symbols cleanup */ #undef GJK_MAX_ITERATIONS -#undef GJK_ACCURARY +#undef GJK_ACCURACY #undef GJK_MIN_DISTANCE #undef GJK_DUPLICATED_EPS #undef GJK_SIMPLEX2_EPS |