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 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