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