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)
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;
248 { found=
true;
break; }
257 lastw[clastw=(clastw+1)&3]=w;
261 alpha=
btMax(omega,alpha);
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];
400 simplex.
p[simplex.
rank]=0;
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};
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};
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];
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)
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)};
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);
659 best->
pass = (
U1)(++pass);
664 for(
U j=0;(j<3)&&valid;++j)
667 best->
f[j],best->
e[j],
670 if(valid&&(horizon.
nf>=3))
734 else if(b_dot_ba < 0)
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};
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);
893 if(gjk_status==GJK::eStatus::Valid)
912 results.
status = gjk_status==GJK::eStatus::Inside?
929 Initialize(shape0,wtrs0,shape1,wtrs1,results,shape,usemargins);
934 case GJK::eStatus::Inside:
938 if(epa_status!=EPA::eStatus::Failed)
954 case GJK::eStatus::Failed:
975 Initialize(shape0,wtrs0,&shape1,wtrs1,results,shape,
false);
978 if(gjk_status==GJK::eStatus::Valid)
1001 if(gjk_status==GJK::eStatus::Inside)
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
1044#undef EPA_MAX_ITERATIONS
1048#undef EPA_INSIDE_EPS
#define GJK_DUPLICATED_EPS
#define GJK_MAX_ITERATIONS
#define EPA_MAX_ITERATIONS
static void Initialize(const btConvexTemplate &a, const btConvexTemplate &b, btGjkEpaSolver3::sResults &results, MinkowskiDiff< btConvexTemplate > &shape)
const T & btMax(const T &a, const T &b)
btScalar length(const btQuaternion &q)
Return the length of a quaternion.
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
btScalar btSqrt(btScalar y)
btScalar btFabs(btScalar x)
static T sum(const btAlignedObjectArray< T > &items)
btScalar btDot(const btVector3 &v1, const btVector3 &v2)
Return the dot product between two vectors.
btVector3 btCross(const btVector3 &v1, const btVector3 &v2)
Return the cross product of two vectors.
The btConvexShape is an abstract shape interface, implemented by all convex shapes such as btBoxShape...
btScalar getMarginNonVirtual() const
btVector3 localGetSupportVertexNonVirtual(const btVector3 &vec) const
btVector3 localGetSupportVertexWithoutMarginNonVirtual(const btVector3 &vec) const
The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with...
btMatrix3x3 transposeTimes(const btMatrix3x3 &m) const
The btQuaternion implements quaternion to perform linear algebra rotations in combination with btMatr...
The btSphereShape implements an implicit sphere, centered around a local origin with radius.
btVector3 can be used to represent 3D points and vectors.
const btScalar & z() const
Return the z value.
btScalar length() const
Return the length of the vector.
btScalar length2() const
Return the length of the vector squared.
const btScalar & x() const
Return the x value.
const btScalar & y() const
Return the y value.
static void Initialize(const btConvexShape *shape0, const btTransform &wtrs0, const btConvexShape *shape1, const btTransform &wtrs1, btGjkEpaSolver2::sResults &results, tShape &shape, bool withmargins)
enum btGjkEpaSolver2::sResults::eStatus status
static btScalar SignedDistance(const btVector3 &position, btScalar margin, const btConvexShape *shape, const btTransform &wtrs, sResults &results)
static bool Penetration(const btConvexShape *shape0, const btTransform &wtrs0, const btConvexShape *shape1, const btTransform &wtrs1, const btVector3 &guess, sResults &results, bool usemargins=true)
static bool Distance(const btConvexShape *shape0, const btTransform &wtrs0, const btConvexShape *shape1, const btTransform &wtrs1, const btVector3 &guess, sResults &results)
static int StackSizeRequirement()
bool expand(U pass, sSV *w, sFace *f, U e, sHorizon &horizon)
static void remove(sList &list, sFace *face)
sSV m_sv_store[EPA_MAX_VERTICES]
eStatus::_ Evaluate(GJK &gjk, const btVector3 &guess)
sFace m_fc_store[EPA_MAX_FACES]
static void append(sList &list, sFace *face)
sFace * newface(sSV *a, sSV *b, sSV *c, bool forced)
static void bind(sFace *fa, U ea, sFace *fb, U eb)
bool getedgedist(sFace *face, sSV *a, sSV *b, btScalar &dist)
void removevertice(sSimplex &simplex)
void getsupport(const btVector3 &d, sSV &sv) const
static btScalar det(const btVector3 &a, const btVector3 &b, const btVector3 &c)
void appendvertice(sSimplex &simplex, const btVector3 &v)
static btScalar projectorigin(const btVector3 &a, const btVector3 &b, const btVector3 &c, btScalar *w, U &m)
static btScalar projectorigin(const btVector3 &a, const btVector3 &b, const btVector3 &c, const btVector3 &d, btScalar *w, U &m)
static btScalar projectorigin(const btVector3 &a, const btVector3 &b, btScalar *w, U &m)
eStatus::_ Evaluate(const tShape &shapearg, const btVector3 &guess)
btVector3 Support0(const btVector3 &d) const
void EnableMargin(bool enable)
const btConvexShape * m_shapes[2]
btVector3 Support1(const btVector3 &d) const
btVector3 Support(const btVector3 &d, U index) const
btVector3(btConvexShape::* Ls)(const btVector3 &) const
btVector3 Support(const btVector3 &d) const