29 #if defined(DEBUG) || defined (_DEBUG)    32 #include <spu_printf.h>    33 #define printf spu_printf    43 #define GJK_MAX_ITERATIONS      128    44 #define GJK_ACCURARY            ((btScalar)0.0001)    45 #define GJK_MIN_DISTANCE        ((btScalar)0.0001)    46 #define GJK_DUPLICATED_EPS      ((btScalar)0.0001)    47 #define GJK_SIMPLEX2_EPS        ((btScalar)0.0)    48 #define GJK_SIMPLEX3_EPS        ((btScalar)0.0)    49 #define GJK_SIMPLEX4_EPS        ((btScalar)0.0)    52 #define EPA_MAX_VERTICES        64    53 #define EPA_MAX_FACES           (EPA_MAX_VERTICES*2)    54 #define EPA_MAX_ITERATIONS      255    55 #define EPA_ACCURACY            ((btScalar)0.0001)    56 #define EPA_FALLBACK            (10*EPA_ACCURACY)    57 #define EPA_PLANE_EPS           ((btScalar)0.00001)    58 #define EPA_INSIDE_EPS          ((btScalar)0.01)    62         typedef unsigned int    U;
    63         typedef unsigned char   U1;
    85                         m_enableMargin = enable;
   117                         return(((m_shapes[0])->*(
Ls))(d));
   121                         return(m_toshape0*((m_shapes[1])->*(
Ls))(m_toshape1*d));
   179                                 m_status        =       eStatus::Failed;
   191                                 m_free[0]                       =       &m_store[0];
   192                                 m_free[1]                       =       &m_store[1];
   193                                 m_free[2]                       =       &m_store[2];
   194                                 m_free[3]                       =       &m_store[3];
   197                                 m_status                        =       eStatus::Valid;
   201                                 m_simplices[0].
rank     =       0;
   204                                 appendvertice(m_simplices[0],sqrl>0?-m_ray:
btVector3(1,0,0));
   205                                 m_simplices[0].
p[0]     =       1;
   206                                 m_ray                           =       m_simplices[0].
c[0]->
w; 
   214                                         const U         next=1-m_current;
   215                                         sSimplex&       cs=m_simplices[m_current];
   221                                                 m_status=eStatus::Inside;
   225                                         appendvertice(cs,-m_ray);
   231                                                 { found=
true;
break; }
   235                                                 removevertice(m_simplices[m_current]);
   240                                                 lastw[clastw=(clastw+1)&3]=w;
   244                                         alpha=
btMax(omega,alpha);
   247                                                 removevertice(m_simplices[m_current]);
   255                                         case    2:      sqdist=projectorigin(   cs.
c[0]->
w,
   258                                         case    3:      sqdist=projectorigin(   cs.
c[0]->
w,
   262                                         case    4:      sqdist=projectorigin(   cs.
c[0]->
w,
   273                                                 for(U i=0,ni=cs.
rank;i<ni;++i)
   278                                                                 ns.
p[ns.
rank++]         =       weights[i];
   279                                                                 m_ray                           +=      cs.
c[i]->
w*weights[i];
   283                                                                 m_free[m_nfree++]       =       cs.
c[i];
   286                                                 if(mask==15) m_status=eStatus::Inside;
   290                                                 removevertice(m_simplices[m_current]);
   294                                 } 
while(m_status==eStatus::Valid);
   295                                 m_simplex=&m_simplices[m_current];
   298                                 case    eStatus::Valid:         m_distance=m_ray.
length();
break;
   299                                 case    eStatus::Inside:        m_distance=0;
break;
   308                                 switch(m_simplex->
rank)
   316                                                         appendvertice(*m_simplex, axis);
   317                                                         if(EncloseOrigin())     
return(
true);
   318                                                         removevertice(*m_simplex);
   319                                                         appendvertice(*m_simplex,-axis);
   320                                                         if(EncloseOrigin())     
return(
true);
   321                                                         removevertice(*m_simplex);
   335                                                                 appendvertice(*m_simplex, p);
   336                                                                 if(EncloseOrigin())     
return(
true);
   337                                                                 removevertice(*m_simplex);
   338                                                                 appendvertice(*m_simplex,-p);
   339                                                                 if(EncloseOrigin())     
return(
true);
   340                                                                 removevertice(*m_simplex);
   348                                                         m_simplex->
c[2]->
w-m_simplex->
c[0]->
w);
   351                                                         appendvertice(*m_simplex,n);
   352                                                         if(EncloseOrigin())     
return(
true);
   353                                                         removevertice(*m_simplex);
   354                                                         appendvertice(*m_simplex,-n);
   355                                                         if(EncloseOrigin())     
return(
true);
   356                                                         removevertice(*m_simplex);
   362                                                 if(
btFabs(det(  m_simplex->
c[0]->
w-m_simplex->
c[3]->
w,
   363                                                         m_simplex->
c[1]->
w-m_simplex->
c[3]->
w,
   364                                                         m_simplex->
c[2]->
w-m_simplex->
c[3]->
w))>0)
   379                                 m_free[m_nfree++]=simplex.
c[--simplex.
rank];
   383                                 simplex.
p[simplex.
rank]=0;
   384                                 simplex.
c[simplex.
rank]=m_free[--m_nfree];
   385                                 getsupport(v,*simplex.
c[simplex.
rank++]);
   389                                 return( a.
y()*b.
z()*c.
x()+a.
z()*b.
x()*c.
y()-
   390                                         a.
x()*b.
z()*c.
y()-a.
y()*b.
x()*c.
z()+
   391                                         a.
x()*b.
y()*c.
z()-a.
z()*b.
y()*c.
x());
   402                                         if(t>=1)                { w[0]=0;w[1]=1;m=2;
return(b.
length2()); }
   403                                         else if(t<=0)   { w[0]=1;w[1]=0;m=1;
return(a.length2()); }
   404                                         else                    { w[0]=1-(w[1]=t);m=3;
return((a+d*t).length2()); }
   413                                 static const U          imd3[]={1,2,0};
   428                                                         const btScalar  subd(projectorigin(*vt[i],*vt[j],subw,subm));
   429                                                         if((mindist<0)||(subd<mindist))
   432                                                                 m                       =       
static_cast<U
>(((subm&1)?1<<i:0)+((subm&2)?1<<j:0));
   448                                                 w[2]    =       1-(w[0]+w[1]);
   460                                 static const U          imd3[]={1,2,0};
   463                                 const btScalar          vl=det(dl[0],dl[1],dl[2]);
   476                                                         const btScalar  subd=projectorigin(*vt[i],*vt[j],d,subw,subm);
   477                                                         if((mindist<0)||(subd<mindist))
   480                                                                 m                       =       
static_cast<U
>((subm&1?1<<i:0)+
   494                                                 w[0]    =       det(c,b,d)/vl;
   495                                                 w[1]    =       det(a,c,d)/vl;
   496                                                 w[2]    =       det(b,a,d)/vl;
   497                                                 w[3]    =       1-(w[0]+w[1]+w[2]);
   563                                 fa->
e[ea]=(
U1)eb;fa->
f[ea]=fb;
   564                                 fb->
e[eb]=(
U1)ea;fb->
f[eb]=fa;
   569                                 face->
l[1]      =       list.
root;
   576                                 if(face->l[1]) face->l[1]->l[0]=face->l[0];
   577                                 if(face->l[0]) face->l[0]->l[1]=face->l[1];
   578                                 if(face==list.root) list.root=face->l[1];
   585                                 m_status        =       eStatus::Failed;
   591                                         append(m_stock,&m_fc_store[EPA_MAX_FACES-i-1]);
   607                                         m_status        =       eStatus::Valid;
   610                                         if(gjk.
det(     simplex.
c[0]->
w-simplex.
c[3]->
w,
   611                                                 simplex.
c[1]->
w-simplex.
c[3]->
w,
   612                                                 simplex.
c[2]->
w-simplex.
c[3]->
w)<0)
   618                                         sFace*  tetra[]={newface(simplex.
c[0],simplex.
c[1],simplex.
c[2],
true),
   619                                                 newface(simplex.
c[1],simplex.
c[0],simplex.
c[3],
true),
   620                                                 newface(simplex.
c[2],simplex.
c[1],simplex.
c[3],
true),
   621                                                 newface(simplex.
c[0],simplex.
c[2],simplex.
c[3],
true)};
   624                                                 sFace*          best=findbest();
   628                                                 bind(tetra[0],0,tetra[1],0);
   629                                                 bind(tetra[0],1,tetra[2],0);
   630                                                 bind(tetra[0],2,tetra[3],0);
   631                                                 bind(tetra[1],1,tetra[3],2);
   632                                                 bind(tetra[1],2,tetra[2],1);
   633                                                 bind(tetra[2],2,tetra[3],1);
   634                                                 m_status=eStatus::Valid;
   640                                                                 sSV*                    w=&m_sv_store[m_nextsv++];
   642                                                                 best->
pass      =       (
U1)(++pass);
   647                                                                         for(U j=0;(j<3)&&valid;++j)
   649                                                                                 valid&=expand(  pass,w,
   650                                                                                         best->
f[j],best->
e[j],
   653                                                                         if(valid&&(horizon.
nf>=3))
   655                                                                                 bind(horizon.
cf,1,horizon.
ff,2);
   657                                                                                 append(m_stock,best);
   660                                                                         } 
else { m_status=eStatus::InvalidHull;
break; }
   661                                                                 } 
else { m_status=eStatus::AccuraryReached;
break; }
   662                                                         } 
else { m_status=eStatus::OutOfVertices;
break; }
   668                                                 m_result.
c[0]   =       outer.
c[0];
   669                                                 m_result.
c[1]   =       outer.
c[1];
   670                                                 m_result.
c[2]   =       outer.
c[2];
   671                                                 m_result.
p[0]   =       
btCross(        outer.
c[1]->
w-projection,
   673                                                 m_result.
p[1]   =       
btCross(        outer.
c[2]->
w-projection,
   675                                                 m_result.
p[2]   =       
btCross(        outer.
c[0]->
w-projection,
   678                                                 m_result.
p[0]   /=      
sum;
   679                                                 m_result.
p[1]   /=      
sum;
   680                                                 m_result.
p[2]   /=      
sum;
   685                                 m_status        =       eStatus::FallBack;
   689                                         m_normal        =       m_normal/nl;
   694                                 m_result.
c[0]=simplex.
c[0];
   717                                         else if(b_dot_ba < 0)
   739                                         remove(m_stock,face);
   751                                                 if(!(getedgedist(face, a, b, face->
d) ||
   752                                                          getedgedist(face, b, c, face->
d) ||
   753                                                          getedgedist(face, c, a, face->
d)))
   766                                                         m_status=eStatus::NonConvex;
   769                                                 m_status=eStatus::Degenerated;
   771                                         remove(m_hull, face);
   772                                         append(m_stock, face);
   776                                 m_status = m_stock.
root ? eStatus::OutOfVertices : eStatus::OutOfFaces;
   783                                 for(
sFace* f=minf->
l[1];f;f=f->
l[1])
   796                                 static const U  i1m3[]={1,2,0};
   797                                 static const U  i2m3[]={2,0,1};
   803                                                 sFace*  nf=newface(f->
c[e1],f->
c[e],w,
false);
   807                                                         if(horizon.
cf) bind(horizon.
cf,1,nf,2); 
else horizon.
ff=nf;
   817                                                 if(     expand(pass,w,f->
f[e1],f->
e[e1],horizon)&&
   818                                                         expand(pass,w,f->
f[e2],f->
e[e2],horizon))
   861         return(
sizeof(
GJK)+
sizeof(
EPA));
   873         Initialize(shape0,wtrs0,shape1,wtrs1,results,shape,
false);
   896                         sResults::Penetrating   :
   897                 sResults::GJK_Failed    ;
   912         Initialize(shape0,wtrs0,shape1,wtrs1,results,shape,usemargins);
   928                                 results.
status                  =       sResults::Penetrating;
   934                         } 
else results.
status=sResults::EPA_Failed;
   938                 results.
status=sResults::GJK_Failed;
   958         Initialize(shape0,wtrs0,&shape1,wtrs1,results,shape,
false);
   980                 return(length-margin);
   986                         if(Penetration(shape0,wtrs0,&shape1,wtrs1,gjk.
m_ray,results))
  1008         if(!Distance(shape0,wtrs0,shape1,wtrs1,guess,results))
  1009                 return(Penetration(shape0,wtrs0,shape1,wtrs1,guess,results,
false));
  1017 #undef GJK_MAX_ITERATIONS  1019 #undef GJK_MIN_DISTANCE  1020 #undef GJK_DUPLICATED_EPS  1021 #undef GJK_SIMPLEX2_EPS  1022 #undef GJK_SIMPLEX3_EPS  1023 #undef GJK_SIMPLEX4_EPS  1025 #undef EPA_MAX_VERTICES  1026 #undef EPA_MAX_FACES  1027 #undef EPA_MAX_ITERATIONS  1030 #undef EPA_PLANE_EPS  1031 #undef EPA_INSIDE_EPS static T sum(const btAlignedObjectArray< T > &items)
btScalar length(const btQuaternion &q)
Return the length of a quaternion. 
static btScalar projectorigin(const btVector3 &a, const btVector3 &b, const btVector3 &c, const btVector3 &d, btScalar *w, U &m)
void appendvertice(sSimplex &simplex, const btVector3 &v)
static void bind(sFace *fa, U ea, sFace *fb, U eb)
btVector3 Support0(const btVector3 &d) const 
btScalar btSqrt(btScalar y)
static btScalar projectorigin(const btVector3 &a, const btVector3 &b, const btVector3 &c, btScalar *w, U &m)
sFace * newface(sSV *a, sSV *b, sSV *c, bool forced)
btMatrix3x3 transposeTimes(const btMatrix3x3 &m) const 
btVector3 localGetSupportVertexWithoutMarginNonVirtual(const btVector3 &vec) const 
bool getedgedist(sFace *face, sSV *a, sSV *b, btScalar &dist)
The btSphereShape implements an implicit sphere, centered around a local origin with radius...
eStatus::_ Evaluate(GJK &gjk, const btVector3 &guess)
static void Initialize(const btConvexShape *shape0, const btTransform &wtrs0, const btConvexShape *shape1, const btTransform &wtrs1, btGjkEpaSolver2::sResults &results, tShape &shape, bool withmargins)
btScalar getMarginNonVirtual() const 
btVector3 Support(const btVector3 &d, U index) const 
btVector3(btConvexShape::* Ls)(const btVector3 &) const 
const btScalar & x() const 
Return the x value. 
The btConvexShape is an abstract shape interface, implemented by all convex shapes such as btBoxShape...
btVector3 btCross(const btVector3 &v1, const btVector3 &v2)
Return the cross product of two vectors. 
void getsupport(const btVector3 &d, sSV &sv) const 
const btConvexShape * m_shapes[2]
btVector3 Support1(const btVector3 &d) const 
bool expand(U pass, sSV *w, sFace *f, U e, sHorizon &horizon)
#define GJK_MAX_ITERATIONS
static int StackSizeRequirement()
static bool Penetration(const btConvexShape *shape0, const btTransform &wtrs0, const btConvexShape *shape1, const btTransform &wtrs1, const btVector3 &guess, sResults &results, bool usemargins=true)
btScalar length() const 
Return the length of the vector. 
void removevertice(sSimplex &simplex)
btVector3 localGetSupportVertexNonVirtual(const btVector3 &vec) const 
#define EPA_MAX_ITERATIONS
const btScalar & y() const 
Return the y value. 
btVector3 can be used to represent 3D points and vectors. 
btScalar length2() const 
Return the length of the vector squared. 
static bool Distance(const btConvexShape *shape0, const btTransform &wtrs0, const btConvexShape *shape1, const btTransform &wtrs1, const btVector3 &guess, sResults &results)
btVector3 Support(const btVector3 &d) const 
#define GJK_DUPLICATED_EPS
enum btGjkEpaSolver2::sResults::eStatus status
void EnableMargin(bool enable)
const T & btMax(const T &a, const T &b)
static btScalar SignedDistance(const btVector3 &position, btScalar margin, const btConvexShape *shape, const btTransform &wtrs, sResults &results)
static btScalar projectorigin(const btVector3 &a, const btVector3 &b, btScalar *w, U &m)
The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with...
static btScalar det(const btVector3 &a, const btVector3 &b, const btVector3 &c)
The btQuaternion implements quaternion to perform linear algebra rotations in combination with btMatr...
btScalar btDot(const btVector3 &v1, const btVector3 &v2)
Return the dot product between two vectors. 
eStatus::_ Evaluate(const tShape &shapearg, const btVector3 &guess)
static void append(sList &list, sFace *face)
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
btScalar btFabs(btScalar x)
const btScalar & z() const 
Return the z value.