55 #if defined(DEBUG) || defined (_DEBUG)    58 #include <spu_printf.h>    59 #define printf spu_printf    68 #define GJK_MAX_ITERATIONS      128    69 #define GJK_ACCURARY            ((btScalar)0.0001)    70 #define GJK_MIN_DISTANCE        ((btScalar)0.0001)    71 #define GJK_DUPLICATED_EPS      ((btScalar)0.0001)    72 #define GJK_SIMPLEX2_EPS        ((btScalar)0.0)    73 #define GJK_SIMPLEX3_EPS        ((btScalar)0.0)    74 #define GJK_SIMPLEX4_EPS        ((btScalar)0.0)    77 #define EPA_MAX_VERTICES        64    78 #define EPA_MAX_FACES           (EPA_MAX_VERTICES*2)    79 #define EPA_MAX_ITERATIONS      255    80 #define EPA_ACCURACY            ((btScalar)0.0001)    81 #define EPA_FALLBACK            (10*EPA_ACCURACY)    82 #define EPA_PLANE_EPS           ((btScalar)0.00001)    83 #define EPA_INSIDE_EPS          ((btScalar)0.01)    87     typedef unsigned int        U;
    88     typedef unsigned char       U1;
    91     template <
typename btConvexTemplate>
   111             m_enableMargin = enable;
   115             return m_convexAPtr->getLocalSupportWithMargin(d);
   119             return m_toshape0*m_convexBPtr->getLocalSupportWithMargin(m_toshape1*d);
   125             return(Support0(d)-Support1(-d));
   144     template <
typename btConvexTemplate>
   173         GJK(
const btConvexTemplate& a, 
const btConvexTemplate& b)
   194             m_free[0]                   =       &m_store[0];
   195             m_free[1]                   =       &m_store[1];
   196             m_free[2]                   =       &m_store[2];
   197             m_free[3]                   =       &m_store[3];
   204             m_simplices[0].
rank =       0;
   207             appendvertice(m_simplices[0],sqrl>0?-m_ray:
btVector3(1,0,0));
   208             m_simplices[0].
p[0] =       1;
   209             m_ray                               =       m_simplices[0].
c[0]->
w;
   217                 const U         next=1-m_current;
   218                 sSimplex&       cs=m_simplices[m_current];
   228                 appendvertice(cs,-m_ray);
   234                     { found=
true;
break; }
   238                     removevertice(m_simplices[m_current]);
   243                     lastw[clastw=(clastw+1)&3]=w;
   247                 alpha=
btMax(omega,alpha);
   250                     removevertice(m_simplices[m_current]);
   258                     case        2:      sqdist=projectorigin(   cs.
c[0]->
w,
   261                     case        3:      sqdist=projectorigin(   cs.
c[0]->
w,
   265                     case        4:      sqdist=projectorigin(   cs.
c[0]->
w,
   276                     for(
U i=0,ni=cs.
rank;i<ni;++i)
   281                             ns.
p[ns.
rank++]             =       weights[i];
   282                             m_ray                               +=      cs.
c[i]->
w*weights[i];
   286                             m_free[m_nfree++]   =       cs.
c[i];
   293                     removevertice(m_simplices[m_current]);
   298             m_simplex=&m_simplices[m_current];
   311             switch(m_simplex->
rank)
   319                         appendvertice(*m_simplex, axis);
   320                         if(EncloseOrigin())     
return(
true);
   321                         removevertice(*m_simplex);
   322                         appendvertice(*m_simplex,-axis);
   323                         if(EncloseOrigin())     
return(
true);
   324                         removevertice(*m_simplex);
   338                             appendvertice(*m_simplex, p);
   339                             if(EncloseOrigin()) 
return(
true);
   340                             removevertice(*m_simplex);
   341                             appendvertice(*m_simplex,-p);
   342                             if(EncloseOrigin()) 
return(
true);
   343                             removevertice(*m_simplex);
   351                                               m_simplex->
c[2]->
w-m_simplex->
c[0]->
w);
   354                         appendvertice(*m_simplex,n);
   355                         if(EncloseOrigin())     
return(
true);
   356                         removevertice(*m_simplex);
   357                         appendvertice(*m_simplex,-n);
   358                         if(EncloseOrigin())     
return(
true);
   359                         removevertice(*m_simplex);
   365                     if(
btFabs(det(      m_simplex->
c[0]->
w-m_simplex->
c[3]->
w,
   366                                   m_simplex->
c[1]->
w-m_simplex->
c[3]->
w,
   367                                   m_simplex->
c[2]->
w-m_simplex->
c[3]->
w))>0)
   382             m_free[m_nfree++]=simplex.
c[--simplex.
rank];
   386             simplex.
p[simplex.
rank]=0;
   387             simplex.
c[simplex.
rank]=m_free[--m_nfree];
   388             getsupport(v,*simplex.
c[simplex.
rank++]);
   392             return(     a.
y()*b.
z()*c.
x()+a.
z()*b.
x()*c.
y()-
   393                    a.
x()*b.
z()*c.
y()-a.
y()*b.
x()*c.
z()+
   394                    a.
x()*b.
y()*c.
z()-a.
z()*b.
y()*c.
x());
   405                 if(t>=1)                { w[0]=0;w[1]=1;m=2;
return(b.
length2()); }
   406                 else if(t<=0)   { w[0]=1;w[1]=0;m=1;
return(a.length2()); }
   407                 else                    { w[0]=1-(w[1]=t);m=3;
return((a+d*t).length2()); }
   416             static const U              imd3[]={1,2,0};
   431                         const btScalar  subd(projectorigin(*vt[i],*vt[j],subw,subm));
   432                         if((mindist<0)||(subd<mindist))
   435                             m                   =       
static_cast<U>(((subm&1)?1<<i:0)+((subm&2)?1<<j:0));
   451                     w[2]        =       1-(w[0]+w[1]);
   463             static const U              imd3[]={1,2,0};
   466             const btScalar              vl=det(dl[0],dl[1],dl[2]);
   479                         const btScalar  subd=projectorigin(*vt[i],*vt[j],d,subw,subm);
   480                         if((mindist<0)||(subd<mindist))
   483                             m                   =       
static_cast<U>((subm&1?1<<i:0)+
   497                     w[0]        =       det(c,b,d)/vl;
   498                     w[1]        =       det(a,c,d)/vl;
   499                     w[2]        =       det(b,a,d)/vl;
   500                     w[3]        =       1-(w[0]+w[1]+w[2]);
   525 template <
typename btConvexTemplate>
   573             fa->
e[ea]=(
U1)eb;fa->
f[ea]=fb;
   574             fb->
e[eb]=(
U1)ea;fb->
f[eb]=fa;
   579             face->
l[1]  =       list.
root;
   586             if(face->l[1]) face->l[1]->l[0]=face->l[0];
   587             if(face->l[0]) face->l[0]->l[1]=face->l[1];
   588             if(face==list.root) list.root=face->l[1];
   601                 append(m_stock,&m_fc_store[EPA_MAX_FACES-i-1]);
   620                 if(gjk.
det(     simplex.
c[0]->
w-simplex.
c[3]->
w,
   621                            simplex.
c[1]->
w-simplex.
c[3]->
w,
   622                            simplex.
c[2]->
w-simplex.
c[3]->
w)<0)
   628                 sFace*  tetra[]={newface(simplex.
c[0],simplex.
c[1],simplex.
c[2],
true),
   629                     newface(simplex.
c[1],simplex.
c[0],simplex.
c[3],
true),
   630                     newface(simplex.
c[2],simplex.
c[1],simplex.
c[3],
true),
   631                     newface(simplex.
c[0],simplex.
c[2],simplex.
c[3],
true)};
   634                     sFace*              best=findbest();
   638                     bind(tetra[0],0,tetra[1],0);
   639                     bind(tetra[0],1,tetra[2],0);
   640                     bind(tetra[0],2,tetra[3],0);
   641                     bind(tetra[1],1,tetra[3],2);
   642                     bind(tetra[1],2,tetra[2],1);
   643                     bind(tetra[2],2,tetra[3],1);
   652                             best->
pass  =       (
U1)(++pass);
   657                                 for(
U j=0;(j<3)&&valid;++j)
   659                                     valid&=expand(      pass,w,
   660                                                   best->
f[j],best->
e[j],
   663                                 if(valid&&(horizon.
nf>=3))
   665                                     bind(horizon.
cf,1,horizon.
ff,2);
   667                                     append(m_stock,best);
   678                     m_result.
c[0]       =       outer.
c[0];
   679                     m_result.
c[1]       =       outer.
c[1];
   680                     m_result.
c[2]       =       outer.
c[2];
   681                     m_result.
p[0]       =       
btCross(        outer.
c[1]->w-projection,
   682                                                 outer.
c[2]->w-projection).
length();
   683                     m_result.
p[1]       =       
btCross(        outer.
c[2]->w-projection,
   684                                                 outer.
c[0]->w-projection).
length();
   685                     m_result.
p[2]       =       
btCross(        outer.
c[0]->w-projection,
   686                                                 outer.
c[1]->w-projection).
length();
   688                     m_result.
p[0]       /=      
sum;
   689                     m_result.
p[1]       /=      
sum;
   690                     m_result.
p[2]       /=      
sum;
   699                 m_normal        =       m_normal/nl;
   704             m_result.
c[0]=simplex.
c[0];
   727                 else if(b_dot_ba < 0)
   749                 remove(m_stock,face);
   761                     if(!(getedgedist(face, a, b, face->
d) ||
   762                          getedgedist(face, b, c, face->
d) ||
   763                          getedgedist(face, c, a, face->
d)))
   781                 remove(m_hull, face);
   782                 append(m_stock, face);
   793             for(
sFace* f=minf->
l[1];f;f=f->
l[1])
   806             static const U      i1m3[]={1,2,0};
   807             static const U      i2m3[]={2,0,1};
   813                     sFace*      nf=newface(f->
c[e1],f->
c[e],w,
false);
   817                         if(horizon.
cf) bind(horizon.
cf,1,nf,2); 
else horizon.
ff=nf;
   827                     if( expand(pass,w,f->
f[e1],f->
e[e1],horizon)&&
   828                        expand(pass,w,f->
f[e2],f->
e[e2],horizon))
   841     template <
typename btConvexTemplate>
   842     static void Initialize(     
const btConvexTemplate& a, 
const btConvexTemplate& b,
   865 template <
typename btConvexTemplate>
   884         results.
witnesses[0]    =       a.getWorldTransform()*w0;
   885         results.
witnesses[1]    =       a.getWorldTransform()*w1;
   901 template <
typename btConvexTemplate>
   903                                      const btConvexTemplate& b,
   925                 results.
witnesses[0]    =       a.getWorldTransform()*w0;
   944 int     btComputeGjkEpaPenetration2(
const btCollisionDescription& colDesc, btDistanceInfo* distInfo)
   949     bool res = btGjkEpaSolver3::Penetration(colDesc.m_objA,colDesc.m_objB,
   950                                             colDesc.m_transformA,colDesc.m_transformB,
   951                                             colDesc.m_localSupportFuncA,colDesc.m_localSupportFuncB,
   960             distInfo->m_distance = results.
distance;
   961             distInfo->m_normalBtoA = results.
normal;
   966                 tmpNormalInB = results.
normal;
   970             if (lenSqr > (SIMD_EPSILON*SIMD_EPSILON))
   972                 tmpNormalInB /= 
btSqrt(lenSqr);
   977                     distInfo->m_distance = distance2;
   978                     distInfo->m_pointOnA= results.
witnesses[0];
   979                     distInfo->m_pointOnB= results.
witnesses[1];
   980                     distInfo->m_normalBtoA= tmpNormalInB;
   992 template <
typename btConvexTemplate, 
typename btDistanceInfoTemplate>
  1004         distInfo->m_distance = results.
distance;
  1005         distInfo->m_pointOnA= results.
witnesses[0];
  1006         distInfo->m_pointOnB= results.
witnesses[1];
  1007         distInfo->m_normalBtoA= results.
normal;
  1016 #undef GJK_MAX_ITERATIONS  1018 #undef GJK_MIN_DISTANCE  1019 #undef GJK_DUPLICATED_EPS  1020 #undef GJK_SIMPLEX2_EPS  1021 #undef GJK_SIMPLEX3_EPS  1022 #undef GJK_SIMPLEX4_EPS  1024 #undef EPA_MAX_VERTICES  1025 #undef EPA_MAX_FACES  1026 #undef EPA_MAX_ITERATIONS  1029 #undef EPA_PLANE_EPS  1030 #undef EPA_INSIDE_EPS  1034 #endif //BT_GJK_EPA3_H static T sum(const btAlignedObjectArray< T > &items)
bool btGjkEpaSolver3_Penetration(const btConvexTemplate &a, const btConvexTemplate &b, const btVector3 &guess, btGjkEpaSolver3::sResults &results)
btScalar length(const btQuaternion &q)
Return the length of a quaternion. 
btVector3 Support(const btVector3 &d) const
eEpaStatus Evaluate(GJK< btConvexTemplate > &gjk, const btVector3 &guess)
int btComputeGjkDistance(const btConvexTemplate &a, const btConvexTemplate &b, const btGjkCollisionDescription &colDesc, btDistanceInfoTemplate *distInfo)
#define GJK_MAX_ITERATIONS
btScalar length2() const
Return the length of the vector squared. 
const btConvexTemplate * m_convexBPtr
btScalar btSqrt(btScalar y)
const btConvexTemplate * m_convexAPtr
static void append(sList &list, sFace *face)
static btScalar projectorigin(const btVector3 &a, const btVector3 &b, btScalar *w, U &m)
btVector3 btCross(const btVector3 &v1, const btVector3 &v2)
Return the cross product of two vectors. 
static void Initialize(const btConvexTemplate &a, const btConvexTemplate &b, btGjkEpaSolver3::sResults &results, MinkowskiDiff< btConvexTemplate > &shape)
#define GJK_DUPLICATED_EPS
btMatrix3x3 transposeTimes(const btMatrix3x3 &m) const
void EnableMargin(bool enable)
static btScalar projectorigin(const btVector3 &a, const btVector3 &b, const btVector3 &c, btScalar *w, U &m)
eGjkStatus Evaluate(const MinkowskiDiff< btConvexTemplate > &shapearg, const btVector3 &guess)
const btScalar & x() const
Return the x value. 
enum btGjkEpaSolver3::sResults::eStatus status
MinkowskiDiff(const btConvexTemplate &a, const btConvexTemplate &b)
static btScalar projectorigin(const btVector3 &a, const btVector3 &b, const btVector3 &c, const btVector3 &d, btScalar *w, U &m)
const btScalar & y() const
Return the y value. 
const btScalar & z() const
Return the z value. 
btVector3 Support1(const btVector3 &d) const
bool expand(U pass, typename GJK< btConvexTemplate >::sSV *w, sFace *f, U e, sHorizon &horizon)
GJK< btConvexTemplate >::sSV * c[3]
btVector3 can be used to represent 3D points and vectors. 
MinkowskiDiff< btConvexTemplate > m_shape
btVector3 Support(const btVector3 &d, U index) const
static btScalar det(const btVector3 &a, const btVector3 &b, const btVector3 &c)
sFace * newface(typename GJK< btConvexTemplate >::sSV *a, typename GJK< btConvexTemplate >::sSV *b, typename GJK< btConvexTemplate >::sSV *c, bool forced)
bool btGjkEpaSolver3_Distance(const btConvexTemplate &a, const btConvexTemplate &b, const btVector3 &guess, btGjkEpaSolver3::sResults &results)
static void bind(sFace *fa, U ea, sFace *fb, U eb)
btVector3 Support0(const btVector3 &d) const
void getsupport(const btVector3 &d, sSV &sv) const
void appendvertice(sSimplex &simplex, const btVector3 &v)
void removevertice(sSimplex &simplex)
const T & btMax(const T &a, const T &b)
GJK< btConvexTemplate >::sSimplex m_result
#define EPA_MAX_ITERATIONS
bool getedgedist(sFace *face, typename GJK< btConvexTemplate >::sSV *a, typename GJK< btConvexTemplate >::sSV *b, btScalar &dist)
The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with...
btScalar btDot(const btVector3 &v1, const btVector3 &v2)
Return the dot product between two vectors. 
GJK(const btConvexTemplate &a, const btConvexTemplate &b)
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
btScalar length() const
Return the length of the vector. 
btScalar btFabs(btScalar x)