/* Internals */
public void getsupport(btVector3 d, sSV sv)
{
sv.d = d / d.Length;
//sv.w = m_shape.Support(sv.d);
m_shape.Support(ref sv.d, out sv.w);
}
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 );
}
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;
}
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;
}
/* Internals */
internal void getsupport( ref btVector3 d, sSV sv )
{
d.normalized( out sv.d );
//sv.d = d / d.length();
m_shape.Support( ref sv.d, out sv.w );
//sv.w = ;
}
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 );
}