sFace newface( sSV a, sSV b, sSV c, bool forced )
{
if( m_stock.root != null )
{
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;
btVector3.btCross2Del( ref b.w, ref a.w, ref c.w, ref a.w, out face.n );
double l = face.n.length();
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 = btVector3.btDot( ref a.w, ref face.n ) / l;
}
face.n.Div( l, out face.n );
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 null;
}
m_status = m_stock.root != null ? eStatus._.OutOfVertices : eStatus._.OutOfFaces;
return null;
}
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 );
}
internal eStatus._ Evaluate( tShape shapearg, ref btVector3 guess )
{
uint iterations = 0;
double sqdist = 0;
double alpha = 0;
btVector3[] lastw = new btVector3[4];
uint clastw = 0;
/* Initialize solver */
m_free[0] = new sSV();
m_free[1] = new sSV();
m_free[2] = new sSV();
m_free[3] = new sSV();
m_nfree = 4;
m_current = 0;
m_status = eStatus._.Valid;
m_shape = shapearg;
m_distance = 0;
/* Initialize simplex */
m_simplices0.rank = 0;
m_ray = guess;
double sqrl = m_ray.length2();
btVector3 tmp;
if( sqrl > 0 )
m_ray.Invert( out tmp );
else
tmp = btVector3.xAxis;
appendvertice( m_simplices0, ref tmp );
m_simplices0.p[0] = 1;
m_ray = m_simplices0.c[0].w;
sqdist = sqrl;
lastw[0] =
lastw[1] =
lastw[2] =
lastw[3] = m_ray;
/* Loop */
do
{
uint next = 1 - m_current;
sSimplex cs = m_current==0?m_simplices0:m_simplices1;
sSimplex ns = next==0?m_simplices0:m_simplices1;
/* Check zero */
double rl = m_ray.length();
if( rl < GJK_MIN_DISTANCE )
{/* Touching or inside */
m_status = eStatus._.Inside;
break;
}
/* Append new vertice in -'v' direction */
m_ray.Invert( out tmp );
appendvertice( cs, ref tmp );
btVector3 w = cs.c[cs.rank - 1].w;
bool found = false;
for( uint i = 0; i < 4; ++i )
{
w.Sub( ref lastw[i], out tmp );
if( tmp.length2() < GJK_DUPLICATED_EPS )
{ found = true; break; }
}
if( found )
{/* Return old simplex */
removevertice( cs );
break;
}
else
{/* Update lastw */
lastw[clastw = ( clastw + 1 ) & 3] = w;
}
/* Check for termination */
double omega = btVector3.btDot( ref m_ray, ref w ) / rl;
alpha = btScalar.btMax( omega, alpha );
if( ( ( rl - alpha ) - ( GJK_ACCURARY * rl ) ) <= 0 )
{/* Return old simplex */
removevertice( cs );
break;
}
/* Reduce simplex */
double[] weights = new double[4];
uint mask = 0;
switch( cs.rank )
{
case 2:
sqdist = projectorigin( ref cs.c[0].w,
ref cs.c[1].w,
weights, out mask ); break;
case 3:
sqdist = projectorigin( ref cs.c[0].w,
ref cs.c[1].w,
ref cs.c[2].w,
weights, out mask ); break;
case 4:
sqdist = projectorigin( ref cs.c[0].w,
ref cs.c[1].w,
ref cs.c[2].w,
ref cs.c[3].w,
weights, out mask ); break;
}
if( sqdist >= 0 )
{/* Valid */
ns.rank = 0;
m_ray = btVector3.Zero;
m_current = next;
for( int i = 0, ni = (int)cs.rank; i < ni; ++i )
{
if( ( mask & ( (uint)1 << i ) ) != 0 )
{
ns.c[ns.rank] = cs.c[i];
ns.p[ns.rank++] = weights[i];
btVector3 tmp2;
cs.c[i].w.Mult( weights[i], out tmp2 );
m_ray.Add( ref tmp2, out m_ray );
}
else
{
m_free[m_nfree++] = cs.c[i];
}
}
if( mask == 15 ) m_status = eStatus._.Inside;
}
else
{/* Return old simplex */
removevertice( cs );
break;
}
m_status = ( ( ++iterations ) < GJK_MAX_ITERATIONS ) ? m_status : eStatus._.Failed;
} while( m_status == eStatus._.Valid );
m_simplex = m_current==0?m_simplices0:m_simplices1;
switch( m_status )
{
case eStatus._.Valid: m_distance = m_ray.length(); break;
case eStatus._.Inside: m_distance = 0; break;
default:
break;
}
return ( m_status );
}
void Initialize()
{
m_status = eStatus._.Failed;
m_normal = btVector3.Zero;
m_depth = 0;
m_nextsv = 0;
for( int i = 0; i < EPA_MAX_FACES; ++i )
{
append( m_stock, m_fc_store[EPA_MAX_FACES - i - 1] );
}
}
internal void Initialize()
{
m_ray = btVector3.Zero;
m_nfree = 0;
m_status = eStatus._.Failed;
m_current = 0;
m_distance = 0;
}
