29 #if defined(DEBUG) || defined (_DEBUG)    32 #include <spu_printf.h>    33 #define printf spu_printf    43 #define GJK_MAX_ITERATIONS      128    45 #ifdef BT_USE_DOUBLE_PRECISION    46         #define GJK_ACCURACY            ((btScalar)1e-12)    47         #define GJK_MIN_DISTANCE        ((btScalar)1e-12)    48         #define GJK_DUPLICATED_EPS      ((btScalar)1e-12)    50         #define GJK_ACCURACY            ((btScalar)0.0001)    51         #define GJK_MIN_DISTANCE        ((btScalar)0.0001)    52         #define GJK_DUPLICATED_EPS      ((btScalar)0.0001)    53 #endif //BT_USE_DOUBLE_PRECISION    56 #define GJK_SIMPLEX2_EPS        ((btScalar)0.0)    57 #define GJK_SIMPLEX3_EPS        ((btScalar)0.0)    58 #define GJK_SIMPLEX4_EPS        ((btScalar)0.0)    61 #define EPA_MAX_VERTICES        128    62 #define EPA_MAX_ITERATIONS      255    64 #ifdef BT_USE_DOUBLE_PRECISION    65         #define EPA_ACCURACY            ((btScalar)1e-12)    66         #define EPA_PLANE_EPS           ((btScalar)1e-14)    67         #define EPA_INSIDE_EPS          ((btScalar)1e-9)    69         #define EPA_ACCURACY            ((btScalar)0.0001)    70         #define EPA_PLANE_EPS           ((btScalar)0.00001)    71         #define EPA_INSIDE_EPS          ((btScalar)0.01)    74 #define EPA_FALLBACK            (10*EPA_ACCURACY)    75 #define EPA_MAX_FACES           (EPA_MAX_VERTICES*2)    79         typedef unsigned int    U;
    80         typedef unsigned char   U1;
   102                         m_enableMargin = enable;
   134                         return(((m_shapes[0])->*(
Ls))(d));
   138                         return(m_toshape0*((m_shapes[1])->*(
Ls))(m_toshape1*d));
   196                                 m_status        =       eStatus::Failed;
   208                                 m_free[0]                       =       &m_store[0];
   209                                 m_free[1]                       =       &m_store[1];
   210                                 m_free[2]                       =       &m_store[2];
   211                                 m_free[3]                       =       &m_store[3];
   214                                 m_status                        =       eStatus::Valid;
   218                                 m_simplices[0].
rank     =       0;
   221                                 appendvertice(m_simplices[0],sqrl>0?-m_ray:
btVector3(1,0,0));
   222                                 m_simplices[0].
p[0]     =       1;
   223                                 m_ray                           =       m_simplices[0].
c[0]->
w; 
   231                                         const U         next=1-m_current;
   232                                         sSimplex&       cs=m_simplices[m_current];
   238                                                 m_status=eStatus::Inside;
   242                                         appendvertice(cs,-m_ray);
   248                                                 { found=
true;
break; }
   252                                                 removevertice(m_simplices[m_current]);
   257                                                 lastw[clastw=(clastw+1)&3]=w;
   261                                         alpha=
btMax(omega,alpha);
   264                                                 removevertice(m_simplices[m_current]);
   272                                         case    2:      sqdist=projectorigin(   cs.
c[0]->
w,
   275                                         case    3:      sqdist=projectorigin(   cs.
c[0]->
w,
   279                                         case    4:      sqdist=projectorigin(   cs.
c[0]->
w,
   290                                                 for(U i=0,ni=cs.
rank;i<ni;++i)
   295                                                                 ns.
p[ns.
rank++]         =       weights[i];
   296                                                                 m_ray                           +=      cs.
c[i]->
w*weights[i];
   300                                                                 m_free[m_nfree++]       =       cs.
c[i];
   303                                                 if(mask==15) m_status=eStatus::Inside;
   307                                                 removevertice(m_simplices[m_current]);
   311                                 } 
while(m_status==eStatus::Valid);
   312                                 m_simplex=&m_simplices[m_current];
   315                                 case    eStatus::Valid:         m_distance=m_ray.
length();
break;
   316                                 case    eStatus::Inside:        m_distance=0;
break;
   325                                 switch(m_simplex->
rank)
   333                                                         appendvertice(*m_simplex, axis);
   334                                                         if(EncloseOrigin())     
return(
true);
   335                                                         removevertice(*m_simplex);
   336                                                         appendvertice(*m_simplex,-axis);
   337                                                         if(EncloseOrigin())     
return(
true);
   338                                                         removevertice(*m_simplex);
   352                                                                 appendvertice(*m_simplex, p);
   353                                                                 if(EncloseOrigin())     
return(
true);
   354                                                                 removevertice(*m_simplex);
   355                                                                 appendvertice(*m_simplex,-p);
   356                                                                 if(EncloseOrigin())     
return(
true);
   357                                                                 removevertice(*m_simplex);
   365                                                         m_simplex->
c[2]->
w-m_simplex->
c[0]->
w);
   368                                                         appendvertice(*m_simplex,n);
   369                                                         if(EncloseOrigin())     
return(
true);
   370                                                         removevertice(*m_simplex);
   371                                                         appendvertice(*m_simplex,-n);
   372                                                         if(EncloseOrigin())     
return(
true);
   373                                                         removevertice(*m_simplex);
   379                                                 if(
btFabs(det(  m_simplex->
c[0]->
w-m_simplex->
c[3]->
w,
   380                                                         m_simplex->
c[1]->
w-m_simplex->
c[3]->
w,
   381                                                         m_simplex->
c[2]->
w-m_simplex->
c[3]->
w))>0)
   396                                 m_free[m_nfree++]=simplex.
c[--simplex.
rank];
   400                                 simplex.
p[simplex.
rank]=0;
   401                                 simplex.
c[simplex.
rank]=m_free[--m_nfree];
   402                                 getsupport(v,*simplex.
c[simplex.
rank++]);
   406                                 return( a.
y()*b.
z()*c.
x()+a.
z()*b.
x()*c.
y()-
   407                                         a.
x()*b.
z()*c.
y()-a.
y()*b.
x()*c.
z()+
   408                                         a.
x()*b.
y()*c.
z()-a.
z()*b.
y()*c.
x());
   419                                         if(t>=1)                { w[0]=0;w[1]=1;m=2;
return(b.
length2()); }
   420                                         else if(t<=0)   { w[0]=1;w[1]=0;m=1;
return(a.length2()); }
   421                                         else                    { w[0]=1-(w[1]=t);m=3;
return((a+d*t).length2()); }
   430                                 static const U          imd3[]={1,2,0};
   445                                                         const btScalar  subd(projectorigin(*vt[i],*vt[j],subw,subm));
   446                                                         if((mindist<0)||(subd<mindist))
   449                                                                 m                       =       
static_cast<U
>(((subm&1)?1<<i:0)+((subm&2)?1<<j:0));
   465                                                 w[2]    =       1-(w[0]+w[1]);
   477                                 static const U          imd3[]={1,2,0};
   480                                 const btScalar          vl=det(dl[0],dl[1],dl[2]);
   493                                                         const btScalar  subd=projectorigin(*vt[i],*vt[j],d,subw,subm);
   494                                                         if((mindist<0)||(subd<mindist))
   497                                                                 m                       =       
static_cast<U
>((subm&1?1<<i:0)+
   511                                                 w[0]    =       det(c,b,d)/vl;
   512                                                 w[1]    =       det(a,c,d)/vl;
   513                                                 w[2]    =       det(b,a,d)/vl;
   514                                                 w[3]    =       1-(w[0]+w[1]+w[2]);
   580                                 fa->
e[ea]=(
U1)eb;fa->
f[ea]=fb;
   581                                 fb->
e[eb]=(
U1)ea;fb->
f[eb]=fa;
   586                                 face->
l[1]      =       list.
root;
   593                                 if(face->l[1]) face->l[1]->l[0]=face->l[0];
   594                                 if(face->l[0]) face->l[0]->l[1]=face->l[1];
   595                                 if(face==list.root) list.root=face->l[1];
   602                                 m_status        =       eStatus::Failed;
   608                                         append(m_stock,&m_fc_store[EPA_MAX_FACES-i-1]);
   624                                         m_status        =       eStatus::Valid;
   627                                         if(gjk.
det(     simplex.
c[0]->
w-simplex.
c[3]->
w,
   628                                                 simplex.
c[1]->
w-simplex.
c[3]->
w,
   629                                                 simplex.
c[2]->
w-simplex.
c[3]->
w)<0)
   635                                         sFace*  tetra[]={newface(simplex.
c[0],simplex.
c[1],simplex.
c[2],
true),
   636                                                 newface(simplex.
c[1],simplex.
c[0],simplex.
c[3],
true),
   637                                                 newface(simplex.
c[2],simplex.
c[1],simplex.
c[3],
true),
   638                                                 newface(simplex.
c[0],simplex.
c[2],simplex.
c[3],
true)};
   641                                                 sFace*          best=findbest();
   645                                                 bind(tetra[0],0,tetra[1],0);
   646                                                 bind(tetra[0],1,tetra[2],0);
   647                                                 bind(tetra[0],2,tetra[3],0);
   648                                                 bind(tetra[1],1,tetra[3],2);
   649                                                 bind(tetra[1],2,tetra[2],1);
   650                                                 bind(tetra[2],2,tetra[3],1);
   651                                                 m_status=eStatus::Valid;
   657                                                                 sSV*                    w=&m_sv_store[m_nextsv++];
   659                                                                 best->
pass      =       (
U1)(++pass);
   664                                                                         for(U j=0;(j<3)&&valid;++j)
   666                                                                                 valid&=expand(  pass,w,
   667                                                                                         best->
f[j],best->
e[j],
   670                                                                         if(valid&&(horizon.
nf>=3))
   672                                                                                 bind(horizon.
cf,1,horizon.
ff,2);
   674                                                                                 append(m_stock,best);
   677                                                                         } 
else { m_status=eStatus::InvalidHull;
break; }
   678                                                                 } 
else { m_status=eStatus::AccuraryReached;
break; }
   679                                                         } 
else { m_status=eStatus::OutOfVertices;
break; }
   685                                                 m_result.
c[0]   =       outer.
c[0];
   686                                                 m_result.
c[1]   =       outer.
c[1];
   687                                                 m_result.
c[2]   =       outer.
c[2];
   688                                                 m_result.
p[0]   =       
btCross(        outer.
c[1]->
w-projection,
   690                                                 m_result.
p[1]   =       
btCross(        outer.
c[2]->
w-projection,
   692                                                 m_result.
p[2]   =       
btCross(        outer.
c[0]->
w-projection,
   695                                                 m_result.
p[0]   /=      
sum;
   696                                                 m_result.
p[1]   /=      
sum;
   697                                                 m_result.
p[2]   /=      
sum;
   702                                 m_status        =       eStatus::FallBack;
   706                                         m_normal        =       m_normal/nl;
   711                                 m_result.
c[0]=simplex.
c[0];
   734                                         else if(b_dot_ba < 0)
   756                                         remove(m_stock,face);
   768                                                 if(!(getedgedist(face, a, b, face->
d) ||
   769                                                          getedgedist(face, b, c, face->
d) ||
   770                                                          getedgedist(face, c, a, face->
d)))
   783                                                         m_status=eStatus::NonConvex;
   786                                                 m_status=eStatus::Degenerated;
   788                                         remove(m_hull, face);
   789                                         append(m_stock, face);
   793                                 m_status = m_stock.
root ? eStatus::OutOfVertices : eStatus::OutOfFaces;
   800                                 for(
sFace* f=minf->
l[1];f;f=f->
l[1])
   813                                 static const U  i1m3[]={1,2,0};
   814                                 static const U  i2m3[]={2,0,1};
   820                                                 sFace*  nf=newface(f->
c[e1],f->
c[e],w,
false);
   824                                                         if(horizon.
cf) bind(horizon.
cf,1,nf,2); 
else horizon.
ff=nf;
   834                                                 if(     expand(pass,w,f->
f[e1],f->
e[e1],horizon)&&
   835                                                         expand(pass,w,f->
f[e2],f->
e[e2],horizon))
   878         return(
sizeof(
GJK)+
sizeof(
EPA));
   890         Initialize(shape0,wtrs0,shape1,wtrs1,results,shape,
false);
   913                         sResults::Penetrating   :
   914                 sResults::GJK_Failed    ;
   929         Initialize(shape0,wtrs0,shape1,wtrs1,results,shape,usemargins);
   945                                 results.
status                  =       sResults::Penetrating;
   951                         } 
else results.
status=sResults::EPA_Failed;
   955                 results.
status=sResults::GJK_Failed;
   975         Initialize(shape0,wtrs0,&shape1,wtrs1,results,shape,
false);
   997                 return(length-margin);
  1003                         if(Penetration(shape0,wtrs0,&shape1,wtrs1,gjk.
m_ray,results))
  1025         if(!Distance(shape0,wtrs0,shape1,wtrs1,guess,results))
  1026                 return(Penetration(shape0,wtrs0,shape1,wtrs1,guess,results,
false));
  1034 #undef GJK_MAX_ITERATIONS  1036 #undef GJK_MIN_DISTANCE  1037 #undef GJK_DUPLICATED_EPS  1038 #undef GJK_SIMPLEX2_EPS  1039 #undef GJK_SIMPLEX3_EPS  1040 #undef GJK_SIMPLEX4_EPS  1042 #undef EPA_MAX_VERTICES  1043 #undef EPA_MAX_FACES  1044 #undef EPA_MAX_ITERATIONS  1047 #undef EPA_PLANE_EPS  1048 #undef EPA_INSIDE_EPS btVector3 localGetSupportVertexNonVirtual(const btVector3 &vec) const
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)
btVector3 Support(const btVector3 &d) const
btScalar length2() const
Return the length of the vector squared. 
static void bind(sFace *fa, U ea, sFace *fb, U eb)
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)
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)
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. 
const btConvexShape * m_shapes[2]
btMatrix3x3 transposeTimes(const btMatrix3x3 &m) const
btVector3 Support0(const btVector3 &d) const
const btScalar & x() const
Return the x value. 
bool expand(U pass, sSV *w, sFace *f, U e, sHorizon &horizon)
#define GJK_MAX_ITERATIONS
static int StackSizeRequirement()
const btScalar & y() const
Return the y value. 
const btScalar & z() const
Return the z value. 
static bool Penetration(const btConvexShape *shape0, const btTransform &wtrs0, const btConvexShape *shape1, const btTransform &wtrs1, const btVector3 &guess, sResults &results, bool usemargins=true)
void removevertice(sSimplex &simplex)
btVector3 Support1(const btVector3 &d) const
void getsupport(const btVector3 &d, sSV &sv) const
#define EPA_MAX_ITERATIONS
btVector3 can be used to represent 3D points and vectors. 
btScalar getMarginNonVirtual() const
static bool Distance(const btConvexShape *shape0, const btTransform &wtrs0, const btConvexShape *shape1, const btTransform &wtrs1, const btVector3 &guess, sResults &results)
#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. 
btVector3 localGetSupportVertexWithoutMarginNonVirtual(const btVector3 &vec) const
btVector3(btConvexShape::* Ls)(const btVector3 &) const
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 length() const
Return the length of the vector. 
btScalar btFabs(btScalar x)
btVector3 Support(const btVector3 &d, U index) const