NoEdge/Danbias
NoEdge
/
Danbias
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity
Danbias/Code/Physics/Bullet Source/BulletCollision/NarrowPhaseCollision/btGjkEpa2.cpp

1032 lines
25 KiB
C++
Raw Blame History

/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2008 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the
use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it
freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not
claim that you wrote the original software. If you use this software in a
product, an acknowledgment in the product documentation would be appreciated
but is not required.
2. Altered source versions must be plainly marked as such, and must not be
misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
/*
GJK-EPA collision solver by Nathanael Presson, 2008
*/
#include "BulletCollision/CollisionShapes/btConvexInternalShape.h"
#include "BulletCollision/CollisionShapes/btSphereShape.h"
#include "btGjkEpa2.h"
#if defined(DEBUG) || defined (_DEBUG)
#include <stdio.h> //for debug printf
#ifdef __SPU__
#include <spu_printf.h>
#define printf spu_printf
#endif //__SPU__
#endif
namespace gjkepa2_impl
{
// Config
/* GJK */
#define GJK_MAX_ITERATIONS 128
#define GJK_ACCURARY ((btScalar)0.0001)
#define GJK_MIN_DISTANCE ((btScalar)0.0001)
#define GJK_DUPLICATED_EPS ((btScalar)0.0001)
#define GJK_SIMPLEX2_EPS ((btScalar)0.0)
#define GJK_SIMPLEX3_EPS ((btScalar)0.0)
#define GJK_SIMPLEX4_EPS ((btScalar)0.0)
/* EPA */
#define EPA_MAX_VERTICES 64
#define EPA_MAX_FACES (EPA_MAX_VERTICES*2)
#define EPA_MAX_ITERATIONS 255
#define EPA_ACCURACY ((btScalar)0.0001)
#define EPA_FALLBACK (10*EPA_ACCURACY)
#define EPA_PLANE_EPS ((btScalar)0.00001)
#define EPA_INSIDE_EPS ((btScalar)0.01)
// Shorthands
typedef unsigned int U;
typedef unsigned char U1;
// MinkowskiDiff
struct MinkowskiDiff
{
const btConvexShape* m_shapes[2];
btMatrix3x3 m_toshape1;
btTransform m_toshape0;
#ifdef __SPU__
bool m_enableMargin;
#else
btVector3 (btConvexShape::*Ls)(const btVector3&) const;
#endif//__SPU__
MinkowskiDiff()
{
}
#ifdef __SPU__
void EnableMargin(bool enable)
{
m_enableMargin = enable;
}
inline btVector3 Support0(const btVector3& d) const
{
if (m_enableMargin)
{
return m_shapes[0]->localGetSupportVertexNonVirtual(d);
} else
{
return m_shapes[0]->localGetSupportVertexWithoutMarginNonVirtual(d);
}
}
inline btVector3 Support1(const btVector3& d) const
{
if (m_enableMargin)
{
return m_toshape0*(m_shapes[1]->localGetSupportVertexNonVirtual(m_toshape1*d));
} else
{
return m_toshape0*(m_shapes[1]->localGetSupportVertexWithoutMarginNonVirtual(m_toshape1*d));
}
}
#else
void EnableMargin(bool enable)
{
if(enable)
Ls=&btConvexShape::localGetSupportVertexNonVirtual;
else
Ls=&btConvexShape::localGetSupportVertexWithoutMarginNonVirtual;
}
inline btVector3 Support0(const btVector3& d) const
{
return(((m_shapes[0])->*(Ls))(d));
}
inline btVector3 Support1(const btVector3& d) const
{
return(m_toshape0*((m_shapes[1])->*(Ls))(m_toshape1*d));
}
#endif //__SPU__
inline btVector3 Support(const btVector3& d) const
{
return(Support0(d)-Support1(-d));
}
btVector3 Support(const btVector3& d,U index) const
{
if(index)
return(Support1(d));
else
return(Support0(d));
}
};
typedef MinkowskiDiff tShape;
// GJK
struct GJK
{
/* Types */
struct sSV
{
btVector3 d,w;
};
struct sSimplex
{
sSV* c[4];
btScalar p[4];
U rank;
};
struct eStatus { enum _ {
Valid,
Inside,
Failed };};
/* Fields */
tShape m_shape;
btVector3 m_ray;
btScalar m_distance;
sSimplex m_simplices[2];
sSV m_store[4];
sSV* m_free[4];
U m_nfree;
U m_current;
sSimplex* m_simplex;
eStatus::_ m_status;
/* Methods */
GJK()
{
Initialize();
}
void Initialize()
{
m_ray = btVector3(0,0,0);
m_nfree = 0;
m_status = eStatus::Failed;
m_current = 0;
m_distance = 0;
}
eStatus::_ Evaluate(const tShape& shapearg,const btVector3& guess)
{
U iterations=0;
btScalar sqdist=0;
btScalar alpha=0;
btVector3 lastw[4];
U clastw=0;
/* Initialize solver */
m_free[0] = &m_store[0];
m_free[1] = &m_store[1];
m_free[2] = &m_store[2];
m_free[3] = &m_store[3];
m_nfree = 4;
m_current = 0;
m_status = eStatus::Valid;
m_shape = shapearg;
m_distance = 0;
/* Initialize simplex */
m_simplices[0].rank = 0;
m_ray = guess;
const btScalar sqrl= m_ray.length2();
appendvertice(m_simplices[0],sqrl>0?-m_ray:btVector3(1,0,0));
m_simplices[0].p[0] = 1;
m_ray = m_simplices[0].c[0]->w;
sqdist = sqrl;
lastw[0] =
lastw[1] =
lastw[2] =
lastw[3] = m_ray;
/* Loop */
do {
const U next=1-m_current;
sSimplex& cs=m_simplices[m_current];
sSimplex& ns=m_simplices[next];
/* Check zero */
const btScalar rl=m_ray.length();
if(rl<GJK_MIN_DISTANCE)
{/* Touching or inside */
m_status=eStatus::Inside;
break;
}
/* Append new vertice in -'v' direction */
appendvertice(cs,-m_ray);
const btVector3& w=cs.c[cs.rank-1]->w;
bool found=false;
for(U i=0;i<4;++i)
{
if((w-lastw[i]).length2()<GJK_DUPLICATED_EPS)
{ found=true;break; }
}
if(found)
{/* Return old simplex */
removevertice(m_simplices[m_current]);
break;
}
else
{/* Update lastw */
lastw[clastw=(clastw+1)&3]=w;
}
/* Check for termination */
const btScalar omega=btDot(m_ray,w)/rl;
alpha=btMax(omega,alpha);
if(((rl-alpha)-(GJK_ACCURARY*rl))<=0)
{/* Return old simplex */
removevertice(m_simplices[m_current]);
break;
}
/* Reduce simplex */
btScalar weights[4];
U mask=0;
switch(cs.rank)
{
case 2: sqdist=projectorigin( cs.c[0]->w,
cs.c[1]->w,
weights,mask);break;
case 3: sqdist=projectorigin( cs.c[0]->w,
cs.c[1]->w,
cs.c[2]->w,
weights,mask);break;
case 4: sqdist=projectorigin( cs.c[0]->w,
cs.c[1]->w,
cs.c[2]->w,
cs.c[3]->w,
weights,mask);break;
}
if(sqdist>=0)
{/* Valid */
ns.rank = 0;
m_ray = btVector3(0,0,0);
m_current = next;
for(U i=0,ni=cs.rank;i<ni;++i)
{
if(mask&(1<<i))
{
ns.c[ns.rank] = cs.c[i];
ns.p[ns.rank++] = weights[i];
m_ray += cs.c[i]->w*weights[i];
}
else
{
m_free[m_nfree++] = cs.c[i];
}
}
if(mask==15) m_status=eStatus::Inside;
}
else
{/* Return old simplex */
removevertice(m_simplices[m_current]);
break;
}
m_status=((++iterations)<GJK_MAX_ITERATIONS)?m_status:eStatus::Failed;
} while(m_status==eStatus::Valid);
m_simplex=&m_simplices[m_current];
switch(m_status)
{
case eStatus::Valid: m_distance=m_ray.length();break;
case eStatus::Inside: m_distance=0;break;
default:
{
}
}
return(m_status);
}
bool EncloseOrigin()
{
switch(m_simplex->rank)
{
case 1:
{
for(U i=0;i<3;++i)
{
btVector3 axis=btVector3(0,0,0);
axis[i]=1;
appendvertice(*m_simplex, axis);
if(EncloseOrigin()) return(true);
removevertice(*m_simplex);
appendvertice(*m_simplex,-axis);
if(EncloseOrigin()) return(true);
removevertice(*m_simplex);
}
}
break;
case 2:
{
const btVector3 d=m_simplex->c[1]->w-m_simplex->c[0]->w;
for(U i=0;i<3;++i)
{
btVector3 axis=btVector3(0,0,0);
axis[i]=1;
const btVector3 p=btCross(d,axis);
if(p.length2()>0)
{
appendvertice(*m_simplex, p);
if(EncloseOrigin()) return(true);
removevertice(*m_simplex);
appendvertice(*m_simplex,-p);
if(EncloseOrigin()) return(true);
removevertice(*m_simplex);
}
}
}
break;
case 3:
{
const btVector3 n=btCross(m_simplex->c[1]->w-m_simplex->c[0]->w,
m_simplex->c[2]->w-m_simplex->c[0]->w);
if(n.length2()>0)
{
appendvertice(*m_simplex,n);
if(EncloseOrigin()) return(true);
removevertice(*m_simplex);
appendvertice(*m_simplex,-n);
if(EncloseOrigin()) return(true);
removevertice(*m_simplex);
}
}
break;
case 4:
{
if(btFabs(det( m_simplex->c[0]->w-m_simplex->c[3]->w,
m_simplex->c[1]->w-m_simplex->c[3]->w,
m_simplex->c[2]->w-m_simplex->c[3]->w))>0)
return(true);
}
break;
}
return(false);
}
/* Internals */
void getsupport(const btVector3& d,sSV& sv) const
{
sv.d = d/d.length();
sv.w = m_shape.Support(sv.d);
}
void removevertice(sSimplex& simplex)
{
m_free[m_nfree++]=simplex.c[--simplex.rank];
}
void appendvertice(sSimplex& simplex,const btVector3& v)
{
simplex.p[simplex.rank]=0;
simplex.c[simplex.rank]=m_free[--m_nfree];
getsupport(v,*simplex.c[simplex.rank++]);
}
static btScalar det(const btVector3& a,const btVector3& b,const btVector3& c)
{
return( a.y()*b.z()*c.x()+a.z()*b.x()*c.y()-
a.x()*b.z()*c.y()-a.y()*b.x()*c.z()+
a.x()*b.y()*c.z()-a.z()*b.y()*c.x());
}
static btScalar projectorigin( const btVector3& a,
const btVector3& b,
btScalar* w,U& m)
{
const btVector3 d=b-a;
const btScalar l=d.length2();
if(l>GJK_SIMPLEX2_EPS)
{
const btScalar t(l>0?-btDot(a,d)/l:0);
if(t>=1) { w[0]=0;w[1]=1;m=2;return(b.length2()); }
else if(t<=0) { w[0]=1;w[1]=0;m=1;return(a.length2()); }
else { w[0]=1-(w[1]=t);m=3;return((a+d*t).length2()); }
}
return(-1);
}
static btScalar projectorigin( const btVector3& a,
const btVector3& b,
const btVector3& c,
btScalar* w,U& m)
{
static const U imd3[]={1,2,0};
const btVector3* vt[]={&a,&b,&c};
const btVector3 dl[]={a-b,b-c,c-a};
const btVector3 n=btCross(dl[0],dl[1]);
const btScalar l=n.length2();
if(l>GJK_SIMPLEX3_EPS)
{
btScalar mindist=-1;
btScalar subw[2]={0.f,0.f};
U subm(0);
for(U i=0;i<3;++i)
{
if(btDot(*vt[i],btCross(dl[i],n))>0)
{
const U j=imd3[i];
const btScalar subd(projectorigin(*vt[i],*vt[j],subw,subm));
if((mindist<0)||(subd<mindist))
{
mindist = subd;
m = static_cast<U>(((subm&1)?1<<i:0)+((subm&2)?1<<j:0));
w[i] = subw[0];
w[j] = subw[1];
w[imd3[j]] = 0;
}
}
}
if(mindist<0)
{
const btScalar d=btDot(a,n);
const btScalar s=btSqrt(l);
const btVector3 p=n*(d/l);
mindist = p.length2();
m = 7;
w[0] = (btCross(dl[1],b-p)).length()/s;
w[1] = (btCross(dl[2],c-p)).length()/s;
w[2] = 1-(w[0]+w[1]);
}
return(mindist);
}
return(-1);
}
static btScalar projectorigin( const btVector3& a,
const btVector3& b,
const btVector3& c,
const btVector3& d,
btScalar* w,U& m)
{
static const U imd3[]={1,2,0};
const btVector3* vt[]={&a,&b,&c,&d};
const btVector3 dl[]={a-d,b-d,c-d};
const btScalar vl=det(dl[0],dl[1],dl[2]);
const bool ng=(vl*btDot(a,btCross(b-c,a-b)))<=0;
if(ng&&(btFabs(vl)>GJK_SIMPLEX4_EPS))
{
btScalar mindist=-1;
btScalar subw[3]={0.f,0.f,0.f};
U subm(0);
for(U i=0;i<3;++i)
{
const U j=imd3[i];
const btScalar s=vl*btDot(d,btCross(dl[i],dl[j]));
if(s>0)
{
const btScalar subd=projectorigin(*vt[i],*vt[j],d,subw,subm);
if((mindist<0)||(subd<mindist))
{
mindist = subd;
m = static_cast<U>((subm&1?1<<i:0)+
(subm&2?1<<j:0)+
(subm&4?8:0));
w[i] = subw[0];
w[j] = subw[1];
w[imd3[j]] = 0;
w[3] = subw[2];
}
}
}
if(mindist<0)
{
mindist = 0;
m = 15;
w[0] = det(c,b,d)/vl;
w[1] = det(a,c,d)/vl;
w[2] = det(b,a,d)/vl;
w[3] = 1-(w[0]+w[1]+w[2]);
}
return(mindist);
}
return(-1);
}
};
// EPA
struct EPA
{
/* Types */
typedef GJK::sSV sSV;
struct sFace
{
btVector3 n;
btScalar d;
sSV* c[3];
sFace* f[3];
sFace* l[2];
U1 e[3];
U1 pass;
};
struct sList
{
sFace* root;
U count;
sList() : root(0),count(0) {}
};
struct sHorizon
{
sFace* cf;
sFace* ff;
U nf;
sHorizon() : cf(0),ff(0),nf(0) {}
};
struct eStatus { enum _ {
Valid,
Touching,
Degenerated,
NonConvex,
InvalidHull,
OutOfFaces,
OutOfVertices,
AccuraryReached,
FallBack,
Failed };};
/* Fields */
eStatus::_ m_status;
GJK::sSimplex m_result;
btVector3 m_normal;
btScalar m_depth;
sSV m_sv_store[EPA_MAX_VERTICES];
sFace m_fc_store[EPA_MAX_FACES];
U m_nextsv;
sList m_hull;
sList m_stock;
/* Methods */
EPA()
{
Initialize();
}
static inline void bind(sFace* fa,U ea,sFace* fb,U eb)
{
fa->e[ea]=(U1)eb;fa->f[ea]=fb;
fb->e[eb]=(U1)ea;fb->f[eb]=fa;
}
static inline void append(sList& list,sFace* face)
{
face->l[0] = 0;
face->l[1] = list.root;
if(list.root) list.root->l[0]=face;
list.root = face;
++list.count;
}
static inline void remove(sList& list,sFace* face)
{
if(face->l[1]) face->l[1]->l[0]=face->l[0];
if(face->l[0]) face->l[0]->l[1]=face->l[1];
if(face==list.root) list.root=face->l[1];
--list.count;
}
void Initialize()
{
m_status = eStatus::Failed;
m_normal = btVector3(0,0,0);
m_depth = 0;
m_nextsv = 0;
for(U i=0;i<EPA_MAX_FACES;++i)
{
append(m_stock,&m_fc_store[EPA_MAX_FACES-i-1]);
}
}
eStatus::_ Evaluate(GJK& gjk,const btVector3& guess)
{
GJK::sSimplex& simplex=*gjk.m_simplex;
if((simplex.rank>1)&&gjk.EncloseOrigin())
{
/* Clean up */
while(m_hull.root)
{
sFace* f = m_hull.root;
remove(m_hull,f);
append(m_stock,f);
}
m_status = eStatus::Valid;
m_nextsv = 0;
/* Orient simplex */
if(gjk.det( simplex.c[0]->w-simplex.c[3]->w,
simplex.c[1]->w-simplex.c[3]->w,
simplex.c[2]->w-simplex.c[3]->w)<0)
{
btSwap(simplex.c[0],simplex.c[1]);
btSwap(simplex.p[0],simplex.p[1]);
}
/* Build initial hull */
sFace* tetra[]={newface(simplex.c[0],simplex.c[1],simplex.c[2],true),
newface(simplex.c[1],simplex.c[0],simplex.c[3],true),
newface(simplex.c[2],simplex.c[1],simplex.c[3],true),
newface(simplex.c[0],simplex.c[2],simplex.c[3],true)};
if(m_hull.count==4)
{
sFace* best=findbest();
sFace outer=*best;
U pass=0;
U iterations=0;
bind(tetra[0],0,tetra[1],0);
bind(tetra[0],1,tetra[2],0);
bind(tetra[0],2,tetra[3],0);
bind(tetra[1],1,tetra[3],2);
bind(tetra[1],2,tetra[2],1);
bind(tetra[2],2,tetra[3],1);
m_status=eStatus::Valid;
for(;iterations<EPA_MAX_ITERATIONS;++iterations)
{
if(m_nextsv<EPA_MAX_VERTICES)
{
sHorizon horizon;
sSV* w=&m_sv_store[m_nextsv++];
bool valid=true;
best->pass = (U1)(++pass);
gjk.getsupport(best->n,*w);
const btScalar wdist=btDot(best->n,w->w)-best->d;
if(wdist>EPA_ACCURACY)
{
for(U j=0;(j<3)&&valid;++j)
{
valid&=expand( pass,w,
best->f[j],best->e[j],
horizon);
}
if(valid&&(horizon.nf>=3))
{
bind(horizon.cf,1,horizon.ff,2);
remove(m_hull,best);
append(m_stock,best);
best=findbest();
outer=*best;
} else { m_status=eStatus::InvalidHull;break; }
} else { m_status=eStatus::AccuraryReached;break; }
} else { m_status=eStatus::OutOfVertices;break; }
}
const btVector3 projection=outer.n*outer.d;
m_normal = outer.n;
m_depth = outer.d;
m_result.rank = 3;
m_result.c[0] = outer.c[0];
m_result.c[1] = outer.c[1];
m_result.c[2] = outer.c[2];
m_result.p[0] = btCross( outer.c[1]->w-projection,
outer.c[2]->w-projection).length();
m_result.p[1] = btCross( outer.c[2]->w-projection,
outer.c[0]->w-projection).length();
m_result.p[2] = btCross( outer.c[0]->w-projection,
outer.c[1]->w-projection).length();
const btScalar sum=m_result.p[0]+m_result.p[1]+m_result.p[2];
m_result.p[0] /= sum;
m_result.p[1] /= sum;
m_result.p[2] /= sum;
return(m_status);
}
}
/* Fallback */
m_status = eStatus::FallBack;
m_normal = -guess;
const btScalar nl=m_normal.length();
if(nl>0)
m_normal = m_normal/nl;
else
m_normal = btVector3(1,0,0);
m_depth = 0;
m_result.rank=1;
m_result.c[0]=simplex.c[0];
m_result.p[0]=1;
return(m_status);
}
bool getedgedist(sFace* face, sSV* a, sSV* b, btScalar& dist)
{
const btVector3 ba = b->w - a->w;
const btVector3 n_ab = btCross(ba, face->n); // Outward facing edge normal direction, on triangle plane
const btScalar a_dot_nab = btDot(a->w, n_ab); // Only care about the sign to determine inside/outside, so not normalization required
if(a_dot_nab < 0)
{
// Outside of edge a->b
const btScalar ba_l2 = ba.length2();
const btScalar a_dot_ba = btDot(a->w, ba);
const btScalar b_dot_ba = btDot(b->w, ba);
if(a_dot_ba > 0)
{
// Pick distance vertex a
dist = a->w.length();
}
else if(b_dot_ba < 0)
{
// Pick distance vertex b
dist = b->w.length();
}
else
{
// Pick distance to edge a->b
const btScalar a_dot_b = btDot(a->w, b->w);
dist = btSqrt(btMax((a->w.length2() * b->w.length2() - a_dot_b * a_dot_b) / ba_l2, (btScalar)0));
}
return true;
}
return false;
}
sFace* newface(sSV* a,sSV* b,sSV* c,bool forced)
{
if(m_stock.root)
{
sFace* face=m_stock.root;
remove(m_stock,face);
append(m_hull,face);
face->pass = 0;
face->c[0] = a;
face->c[1] = b;
face->c[2] = c;
face->n = btCross(b->w-a->w,c->w-a->w);
const btScalar l=face->n.length();
const bool v=l>EPA_ACCURACY;
if(v)
{
if(!(getedgedist(face, a, b, face->d) ||
getedgedist(face, b, c, face->d) ||
getedgedist(face, c, a, face->d)))
{
// Origin projects to the interior of the triangle
// Use distance to triangle plane
face->d = btDot(a->w, face->n) / l;
}
face->n /= l;
if(forced || (face->d >= -EPA_PLANE_EPS))
{
return face;
}
else
m_status=eStatus::NonConvex;
}
else
m_status=eStatus::Degenerated;
remove(m_hull, face);
append(m_stock, face);
return 0;
}
m_status = m_stock.root ? eStatus::OutOfVertices : eStatus::OutOfFaces;
return 0;
}
sFace* findbest()
{
sFace* minf=m_hull.root;
btScalar mind=minf->d*minf->d;
for(sFace* f=minf->l[1];f;f=f->l[1])
{
const btScalar sqd=f->d*f->d;
if(sqd<mind)
{
minf=f;
mind=sqd;
}
}
return(minf);
}
bool expand(U pass,sSV* w,sFace* f,U e,sHorizon& horizon)
{
static const U i1m3[]={1,2,0};
static const U i2m3[]={2,0,1};
if(f->pass!=pass)
{
const U e1=i1m3[e];
if((btDot(f->n,w->w)-f->d)<-EPA_PLANE_EPS)
{
sFace* nf=newface(f->c[e1],f->c[e],w,false);
if(nf)
{
bind(nf,0,f,e);
if(horizon.cf) bind(horizon.cf,1,nf,2); else horizon.ff=nf;
horizon.cf=nf;
++horizon.nf;
return(true);
}
}
else
{
const U e2=i2m3[e];
f->pass = (U1)pass;
if( expand(pass,w,f->f[e1],f->e[e1],horizon)&&
expand(pass,w,f->f[e2],f->e[e2],horizon))
{
remove(m_hull,f);
append(m_stock,f);
return(true);
}
}
}
return(false);
}
};
//
static void Initialize( const btConvexShape* shape0,const btTransform& wtrs0,
const btConvexShape* shape1,const btTransform& wtrs1,
btGjkEpaSolver2::sResults& results,
tShape& shape,
bool withmargins)
{
/* Results */
results.witnesses[0] =
results.witnesses[1] = btVector3(0,0,0);
results.status = btGjkEpaSolver2::sResults::Separated;
/* Shape */
shape.m_shapes[0] = shape0;
shape.m_shapes[1] = shape1;
shape.m_toshape1 = wtrs1.getBasis().transposeTimes(wtrs0.getBasis());
shape.m_toshape0 = wtrs0.inverseTimes(wtrs1);
shape.EnableMargin(withmargins);
}
}
//
// Api
//
using namespace gjkepa2_impl;
//
int btGjkEpaSolver2::StackSizeRequirement()
{
return(sizeof(GJK)+sizeof(EPA));
}
//
bool btGjkEpaSolver2::Distance( const btConvexShape* shape0,
const btTransform& wtrs0,
const btConvexShape* shape1,
const btTransform& wtrs1,
const btVector3& guess,
sResults& results)
{
tShape shape;
Initialize(shape0,wtrs0,shape1,wtrs1,results,shape,false);
GJK gjk;
GJK::eStatus::_ gjk_status=gjk.Evaluate(shape,guess);
if(gjk_status==GJK::eStatus::Valid)
{
btVector3 w0=btVector3(0,0,0);
btVector3 w1=btVector3(0,0,0);
for(U i=0;i<gjk.m_simplex->rank;++i)
{
const btScalar p=gjk.m_simplex->p[i];
w0+=shape.Support( gjk.m_simplex->c[i]->d,0)*p;
w1+=shape.Support(-gjk.m_simplex->c[i]->d,1)*p;
}
results.witnesses[0] = wtrs0*w0;
results.witnesses[1] = wtrs0*w1;
results.normal = w0-w1;
results.distance = results.normal.length();
results.normal /= results.distance>GJK_MIN_DISTANCE?results.distance:1;
return(true);
}
else
{
results.status = gjk_status==GJK::eStatus::Inside?
sResults::Penetrating :
sResults::GJK_Failed ;
return(false);
}
}
//
bool btGjkEpaSolver2::Penetration( const btConvexShape* shape0,
const btTransform& wtrs0,
const btConvexShape* shape1,
const btTransform& wtrs1,
const btVector3& guess,
sResults& results,
bool usemargins)
{
tShape shape;
Initialize(shape0,wtrs0,shape1,wtrs1,results,shape,usemargins);
GJK gjk;
GJK::eStatus::_ gjk_status=gjk.Evaluate(shape,-guess);
switch(gjk_status)
{
case GJK::eStatus::Inside:
{
EPA epa;
EPA::eStatus::_ epa_status=epa.Evaluate(gjk,-guess);
if(epa_status!=EPA::eStatus::Failed)
{
btVector3 w0=btVector3(0,0,0);
for(U i=0;i<epa.m_result.rank;++i)
{
w0+=shape.Support(epa.m_result.c[i]->d,0)*epa.m_result.p[i];
}
results.status = sResults::Penetrating;
results.witnesses[0] = wtrs0*w0;
results.witnesses[1] = wtrs0*(w0-epa.m_normal*epa.m_depth);
results.normal = -epa.m_normal;
results.distance = -epa.m_depth;
return(true);
} else results.status=sResults::EPA_Failed;
}
break;
case GJK::eStatus::Failed:
results.status=sResults::GJK_Failed;
break;
default:
{
}
}
return(false);
}
#ifndef __SPU__
//
btScalar btGjkEpaSolver2::SignedDistance(const btVector3& position,
btScalar margin,
const btConvexShape* shape0,
const btTransform& wtrs0,
sResults& results)
{
tShape shape;
btSphereShape shape1(margin);
btTransform wtrs1(btQuaternion(0,0,0,1),position);
Initialize(shape0,wtrs0,&shape1,wtrs1,results,shape,false);
GJK gjk;
GJK::eStatus::_ gjk_status=gjk.Evaluate(shape,btVector3(1,1,1));
if(gjk_status==GJK::eStatus::Valid)
{
btVector3 w0=btVector3(0,0,0);
btVector3 w1=btVector3(0,0,0);
for(U i=0;i<gjk.m_simplex->rank;++i)
{
const btScalar p=gjk.m_simplex->p[i];
w0+=shape.Support( gjk.m_simplex->c[i]->d,0)*p;
w1+=shape.Support(-gjk.m_simplex->c[i]->d,1)*p;
}
results.witnesses[0] = wtrs0*w0;
results.witnesses[1] = wtrs0*w1;
const btVector3 delta= results.witnesses[1]-
results.witnesses[0];
const btScalar margin= shape0->getMarginNonVirtual()+
shape1.getMarginNonVirtual();
const btScalar length= delta.length();
results.normal = delta/length;
results.witnesses[0] += results.normal*margin;
return(length-margin);
}
else
{
if(gjk_status==GJK::eStatus::Inside)
{
if(Penetration(shape0,wtrs0,&shape1,wtrs1,gjk.m_ray,results))
{
const btVector3 delta= results.witnesses[0]-
results.witnesses[1];
const btScalar length= delta.length();
if (length >= SIMD_EPSILON)
results.normal = delta/length;
return(-length);
}
}
}
return(SIMD_INFINITY);
}
//
bool btGjkEpaSolver2::SignedDistance(const btConvexShape* shape0,
const btTransform& wtrs0,
const btConvexShape* shape1,
const btTransform& wtrs1,
const btVector3& guess,
sResults& results)
{
if(!Distance(shape0,wtrs0,shape1,wtrs1,guess,results))
return(Penetration(shape0,wtrs0,shape1,wtrs1,guess,results,false));
else
return(true);
}
#endif //__SPU__
/* Symbols cleanup */
#undef GJK_MAX_ITERATIONS
#undef GJK_ACCURARY
#undef GJK_MIN_DISTANCE
#undef GJK_DUPLICATED_EPS
#undef GJK_SIMPLEX2_EPS
#undef GJK_SIMPLEX3_EPS
#undef GJK_SIMPLEX4_EPS
#undef EPA_MAX_VERTICES
#undef EPA_MAX_FACES
#undef EPA_MAX_ITERATIONS
#undef EPA_ACCURACY
#undef EPA_FALLBACK
#undef EPA_PLANE_EPS
#undef EPA_INSIDE_EPS
Reference in New Issue View Git Blame Copy Permalink