Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Bug fix in EPA implementation, compute closest triangle to the origin using edges/vertices, instead of the projected origin distance.

Browse Source 
See Issue 606 for detailed description. Thanks a lot to Jarno for the report and fix!
This commit is contained in:
erwin.coumans
2012-03-16 18:59:50 +00:00
parent 6fe42c183a
commit da54814d09
1 changed files with 64 additions and 22 deletions
Download Patch File Download Diff File Expand all files Collapse all files

86
src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.cpp
View File

@@ -511,7 +511,6 @@ namespace gjkepa2_impl
{
btVector3 n;
btScalar d;
btScalar p;
sSV* c[3];
sFace* f[3];
sFace* l[2];
@@ -657,7 +656,7 @@ namespace gjkepa2_impl
remove(m_hull,best);
append(m_stock,best);
best=findbest();
if(best->p>=outer.p) outer=*best;
outer=*best;
} else { m_status=eStatus::InvalidHull;break; }
} else { m_status=eStatus::AccuraryReached;break; }
} else { m_status=eStatus::OutOfVertices;break; }
@@ -696,6 +695,42 @@ namespace gjkepa2_impl
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)
@@ -710,41 +745,48 @@ namespace gjkepa2_impl
face->n = btCross(b->w-a->w,c->w-a->w);
const btScalar l=face->n.length();
const bool v=l>EPA_ACCURACY;
face->p = btMin(btMin(
btDot(a->w,btCross(face->n,a->w-b->w)),
btDot(b->w,btCross(face->n,b->w-c->w))),
btDot(c->w,btCross(face->n,c->w-a->w))) /
(v?l:1);
face->p = face->p>=-EPA_INSIDE_EPS?0:face->p;
if(v)
{
face->d = btDot(a->w,face->n)/l;
face->n /= l;
if(forced||(face->d>=-EPA_PLANE_EPS))
if(!(getedgedist(face, a, b, face->d) ||
getedgedist(face, b, c, face->d) ||
getedgedist(face, c, a, face->d)))
{
return(face);
} else m_status=eStatus::NonConvex;
} else m_status=eStatus::Degenerated;
remove(m_hull,face);
append(m_stock,face);
return(0);
// 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);
m_status = m_stock.root ? eStatus::OutOfVertices : eStatus::OutOfFaces;
return 0;
}
sFace* findbest()
{
sFace* minf=m_hull.root;
btScalar mind=minf->d*minf->d;
btScalar maxp=minf->p;
for(sFace* f=minf->l[1];f;f=f->l[1])
{
const btScalar sqd=f->d*f->d;
if((f->p>=maxp)&&(sqd<mind))
if(sqd<mind)
{
minf=f;
mind=sqd;
maxp=f->p;
}
}
return(minf);