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();
}
}
static void Initialize(ConvexShape shape0, btTransform wtrs0,
ConvexShape shape1, btTransform wtrs1,
ref GjkEpaSolver2.sResults results,
ref tShape shape,
bool withmargins)
{
/* Results */
results.witnesses0 =
results.witnesses1 = new btVector3(0, 0, 0);
results.status = GjkEpaSolver2.sResults.eStatus.Separated;
/* Shape */
shape.m_shapes0 = shape0;
shape.m_shapes1 = shape1;
//shape.m_toshape1 = wtrs1.Basis.transposeTimes(wtrs0.Basis);
wtrs1.Basis.transposeTimes(ref wtrs0.Basis, out shape.m_toshape1);
shape.m_toshape0 = wtrs0.inverseTimes(wtrs1);
shape.EnableMargin(withmargins);
}
