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 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)
static void bind(sFace *fa, U ea, sFace *fb, U eb)
btVector3 Support0(const btVector3 &d) const
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)
btMatrix3x3 transposeTimes(const btMatrix3x3 &m) const
btVector3 localGetSupportVertexWithoutMarginNonVirtual(const btVector3 &vec) const
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)
btScalar getMarginNonVirtual() const
btVector3 Support(const btVector3 &d, U index) const
btVector3(btConvexShape::* Ls)(const btVector3 &) const
const btScalar & x() const
Return the x value.
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.
void getsupport(const btVector3 &d, sSV &sv) const
const btConvexShape * m_shapes[2]
btVector3 Support1(const btVector3 &d) const
bool expand(U pass, sSV *w, sFace *f, U e, sHorizon &horizon)
#define GJK_MAX_ITERATIONS
static int StackSizeRequirement()
static bool Penetration(const btConvexShape *shape0, const btTransform &wtrs0, const btConvexShape *shape1, const btTransform &wtrs1, const btVector3 &guess, sResults &results, bool usemargins=true)
btScalar length() const
Return the length of the vector.
void removevertice(sSimplex &simplex)
btVector3 localGetSupportVertexNonVirtual(const btVector3 &vec) const
#define EPA_MAX_ITERATIONS
const btScalar & y() const
Return the y value.
btVector3 can be used to represent 3D points and vectors.
btScalar length2() const
Return the length of the vector squared.
static bool Distance(const btConvexShape *shape0, const btTransform &wtrs0, const btConvexShape *shape1, const btTransform &wtrs1, const btVector3 &guess, sResults &results)
btVector3 Support(const btVector3 &d) const
#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.
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 btFabs(btScalar x)
const btScalar & z() const
Return the z value.