public eStatus Evaluate(tShape shapearg, btVector3 guess)
{
U iterations = 0;
float sqdist = 0;
float alpha = 0;
//btVector3 *lastw=stackalloc btVector3[4];
//btVector3[] lastw = new btVector3[4];
StackPtr<btVector3> lastw = StackPtr<btVector3>.Allocate(4);
try
{
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;
float sqrl = m_ray.Length2;
appendvertice(m_simplices[0], sqrl > 0 ? -m_ray : new 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
{
U next = 1 - m_current;
sSimplex cs = m_simplices[m_current];
sSimplex ns = m_simplices[next];
/* Check zero */
float 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);
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 */
float omega = btVector3.dot(m_ray, w) / rl;
alpha = (float)Math.Max(omega, alpha);
if (((rl - alpha) - (GJK_ACCURARY * rl)) <= 0)
{/* Return old simplex */
removevertice(m_simplices[m_current]);
break;
}
/* Reduce simplex */
//float* weights = stackalloc float[4];
StackPtr<float> weights = StackPtr<float>.Allocate(4);
try
{
U mask = 0;
switch (cs.rank)
{
case 2: sqdist = projectorigin(cs.c[0].w,
cs.c[1].w,
weights, ref mask); break;
case 3: sqdist = projectorigin(cs.c[0].w,
cs.c[1].w,
cs.c[2].w,
weights, ref mask); break;
case 4: sqdist = projectorigin(cs.c[0].w,
cs.c[1].w,
cs.c[2].w,
cs.c[3].w,
weights, ref mask); break;
}
if (sqdist >= 0)
{/* Valid */
ns.rank = 0;
m_ray = new btVector3(0, 0, 0);
m_current = next;
for (U i = 0, ni = cs.rank; i < ni; ++i)
{
if ((mask & (1 << (int)i)) != 0)
{
ns.c[ns.rank] = cs.c[i];
ns.p[ns.rank++] = weights[i];
#region m_ray += cs.c[i].w * weights[i];
{
btVector3 temp;
btVector3.Multiply(ref cs.c[i].w, weights[i], out temp);
m_ray.Add(ref temp);
}
#endregion
}
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;
}
finally
{
weights.Dispose();
}
} 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:
{
break;
}
}
return (m_status);
}
finally
{
lastw.Dispose();
}
}
void removevertice(sSimplex simplex)
{
m_free[m_nfree++] = simplex.c[--simplex.rank];
}
void appendvertice(sSimplex simplex, btVector3 v)
{
simplex.p[simplex.rank] = 0;
simplex.c[simplex.rank] = m_free[--m_nfree];
getsupport(v, simplex.c[simplex.rank++]);
}
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 );
}
