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;
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);
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);
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)
static void remove(sList &list, sFace *face)
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)
sSV m_sv_store[EPA_MAX_VERTICES]
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
sFace m_fc_store[EPA_MAX_FACES]
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