bool getedgedist( sFace face, sSV a, sSV b, double dist )
{
btVector3 ba; b.w.Sub( ref a.w, out ba );
btVector3 n_ab; btVector3.btCross( ref ba, ref face.n, out n_ab ); // Outward facing edge normal direction, on triangle plane
double a_dot_nab = btVector3.btDot( ref a.w, ref 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
double ba_l2 = ba.length2();
double a_dot_ba = btVector3.btDot( ref a.w, ref ba );
double b_dot_ba = btVector3.btDot( ref b.w, ref 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
double a_dot_b = btVector3.btDot( ref a.w, ref b.w );
dist = btScalar.btSqrt( btScalar.btMax( ( a.w.length2() * b.w.length2() - a_dot_b * a_dot_b ) / ba_l2, (double)0 ) );
}
return true;
}
return false;
}
bool expand( uint pass, sSV w, sFace f, uint e, sHorizon horizon )
{
int[] i1m3 = { 1, 2, 0 };
int[] i2m3 = { 2, 0, 1 };
if( f.pass != pass )
{
int e1 = i1m3[e];
if( ( btVector3.btDot( ref f.n, ref w.w ) - f.d ) < -EPA_PLANE_EPS )
{
sFace nf = newface( f.c[e1], f.c[e], w, false );
if( nf != null )
{
bind( nf, 0, f, e );
if( horizon.cf != null ) bind( horizon.cf, 1, nf, 2 ); else horizon.ff = nf;
horizon.cf = nf;
++horizon.nf;
return ( true );
}
}
else
{
int e2 = i2m3[e];
f.pass = (byte)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 remove( sList list, sFace face )
{
if( face.l[1] != null ) face.l[1].l[0] = face.l[0];
if( face.l[0] != null ) face.l[0].l[1] = face.l[1];
if( face == list.root ) list.root = face.l[1];
--list.count;
}
internal eStatus._ Evaluate( GJK gjk, ref btVector3 guess )
{
GJK.sSimplex simplex = gjk.m_simplex;
if( ( simplex.rank > 1 ) && gjk.EncloseOrigin() )
{
/* Clean up */
while( m_hull.root != null )
{
sFace f = m_hull.root;
remove( m_hull, f );
append( m_stock, f );
}
m_status = eStatus._.Valid;
m_nextsv = 0;
/* Orient simplex */
if( btVector3.det( ref simplex.c[0].w, ref simplex.c[3].w,
ref simplex.c[1].w, ref simplex.c[3].w,
ref simplex.c[2].w, ref simplex.c[3].w ) < 0 )
{
btScalar.btSwap( ref simplex.c[0], ref simplex.c[1] );
btScalar.btSwap( ref simplex.p[0], ref 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 = new sFace( best );
uint pass = 0;
uint 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 = new sHorizon();
sSV w = m_sv_store[m_nextsv++];
bool valid = true;
best.pass = (byte)( ++pass );
gjk.getsupport( ref best.n, w );
double wdist = best.n.dot( ref w.w ) - best.d;
if( wdist > EPA_ACCURACY )
{
for( uint 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 = new sFace( best );
}
else { m_status = eStatus._.InvalidHull; break; }
}
else { m_status = eStatus._.AccuraryReached; break; }
}
else { m_status = eStatus._.OutOfVertices; break; }
}
btVector3 projection; outer.n.Mult( outer.d, out projection );
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];
btVector3 tmp;
btVector3.btCross2Del( ref outer.c[1].w, ref projection,
ref outer.c[2].w, ref projection, out tmp );
m_result.p[0] = tmp.length();
btVector3.btCross2Del( ref outer.c[2].w, ref projection,
ref outer.c[0].w, ref projection, out tmp );
m_result.p[1] = tmp.length();
btVector3.btCross2Del( ref outer.c[0].w, ref projection,
ref outer.c[1].w, ref projection, out tmp );
m_result.p[2] = tmp.length();
double 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;
guess.Invert( out m_normal );
//m_normal = -guess;
double nl = m_normal.length();
if( nl > 0 )
m_normal.Div( nl, out m_normal );
else
m_normal = btVector3.xAxis;
m_depth = 0;
m_result.rank = 1;
m_result.c[0] = simplex.c[0];
m_result.p[0] = 1;
return ( m_status );
}
static void append( sList list, sFace face )
{
face.l = null;
face.l[1] = list.root;
if( list.root != null ) list.root.l[0] = face;
list.root = face;
++list.count;
}
static void bind( sFace fa, uint ea, sFace fb, uint eb )
{
fa.e[ea] = (byte)eb; fa.f[ea] = fb;
fb.e[eb] = (byte)ea; fb.f[eb] = fa;
}
internal sHorizon() { cf = ( null ); ff = ( null ); nf = ( 0 ); }
internal sList() { root = null; count = 0; }
internal sFace( sFace clone )
{
n = clone.n;
d = clone.d;
int i;
for( i = 0; i < 3; i++ )
{
c[i] = clone.c[i];
f[i] = clone.f[i];
e[i] = clone.e[i];
}
for( i = 0; i < 2; i++ )
{
l[i] = clone.l[i];
}
pass = clone.pass;
}