public void Initialize()
{
m_status = eStatus.Failed;
m_normal = IndexedVector3.Zero;
m_depth = 0;
m_nextsv = 0;
m_result = new sSimplex();
for (int i = 0; i < m_sv_store.Length; ++i)
{
m_sv_store[i] = new sSV();
}
for (int i = 0; i < m_fc_store.Length; ++i)
{
m_fc_store[i] = new sFace();
}
for (int i = 0; i < GjkEpaSolver2.EPA_MAX_FACES; ++i)
{
Append(m_stock, m_fc_store[GjkEpaSolver2.EPA_MAX_FACES - i - 1]);
}
}
public void Initialise()
{
m_ray = IndexedVector3.Zero;
m_nfree = 0;
m_status = GJKStatus.Failed;
m_current = 0;
m_distance = 0f;
for (int i = 0; i < m_simplices.Length; ++i)
{
if (m_simplices[i] == null)
{
m_simplices[i] = new sSimplex();
}
}
for (int i = 0; i < m_store.Length; ++i)
{
if (m_store[i] == null)
{
m_store[i] = new sSV();
}
}
}
public eStatus Evaluate(GJK gjk, ref IndexedVector3 guess)
{
sSimplex simplex = gjk.m_simplex;
if ((simplex.rank > 1) && gjk.EncloseOrigin())
{
/* Clean up */
while (m_hull.Count > 0)
{
sFace f = m_hull[0];
Remove(m_hull, f);
Append(m_stock, f);
}
m_status = eStatus.Valid;
m_nextsv = 0;
/* Orient simplex */
if (GJK.Det(simplex.c[0].w - simplex.c[3].w,
simplex.c[1].w - simplex.c[3].w,
simplex.c[2].w - simplex.c[3].w) < 0)
{
SwapSv(simplex.c, 0, 1);
SwapFloat(simplex.p, 0, 1);
}
/* Build initial hull */
tetra[0] = NewFace(simplex.c[0], simplex.c[1], simplex.c[2], true);
tetra[1] = NewFace(simplex.c[1], simplex.c[0], simplex.c[3], true);
tetra[2] = NewFace(simplex.c[2], simplex.c[1], simplex.c[3], true);
tetra[3] = NewFace(simplex.c[0], simplex.c[2], simplex.c[3], true);
if (m_hull.Count == 4)
{
sFace best = FindBest();
sFace outer = 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 < GjkEpaSolver2.EPA_MAX_ITERATIONS; ++iterations)
{
if (m_nextsv < GjkEpaSolver2.EPA_MAX_VERTICES)
{
sHorizon horizon = new sHorizon();
sSV w = m_sv_store[m_nextsv++];
bool valid = true;
best.pass = (uint)(++pass);
gjk.GetSupport(ref best.n, ref w);
float wdist = IndexedVector3.Dot(ref best.n, ref w.w) - best.d;
if (wdist > GjkEpaSolver2.EPA_ACCURACY)
{
for (int j = 0; (j < 3) && valid; ++j)
{
valid &= Expand(pass, w,
best.f[j], best.e[j],
ref horizon);
}
if (valid && (horizon.nf >= 3))
{
Bind(horizon.cf, 1, horizon.ff, 2);
Remove(m_hull, best);
Append(m_stock, best);
best = FindBest();
if (best.p >= outer.p)
{
outer = best;
}
}
else
{
m_status = eStatus.InvalidHull;
break;
}
}
else
{
m_status = eStatus.AccuraryReached;
break;
}
}
else
{
m_status = eStatus.OutOfVertices;
break;
}
}
IndexedVector3 projection = outer.n * outer.d;
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];
m_result.p[0] = IndexedVector3.Cross(outer.c[1].w - projection,
outer.c[2].w - projection).Length();
m_result.p[1] = IndexedVector3.Cross(outer.c[2].w - projection,
outer.c[0].w - projection).Length();
m_result.p[2] = IndexedVector3.Cross(outer.c[0].w - projection,
outer.c[1].w - projection).Length();
float 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;
m_normal = -guess;
float nl = m_normal.LengthSquared();
if (nl > 0)
{
m_normal.Normalize();
}
else
{
m_normal = new IndexedVector3(1, 0, 0);
}
m_depth = 0;
m_result.rank = 1;
m_result.c[0] = simplex.c[0];
m_result.p[0] = 1;
return(m_status);
}
public void AppendVertice(sSimplex simplex, ref IndexedVector3 v)
{
simplex.p[simplex.rank] = 0;
simplex.c[simplex.rank] = m_free[--m_nfree];
GetSupport(ref v, ref simplex.c[simplex.rank++]);
}
public void RemoveVertice(sSimplex simplex)
{
m_free[m_nfree++] = simplex.c[--simplex.rank];
}
public GJKStatus Evaluate(GjkEpaSolver2MinkowskiDiff shapearg, ref IndexedVector3 guess)
{
uint iterations = 0;
float sqdist = 0f;
float alpha = 0f;
uint 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 = GJKStatus.Valid;
m_shape = shapearg;
m_distance = 0f;
/* Initialize simplex */
m_simplices[0].rank = 0;
m_ray = guess;
float sqrl = m_ray.LengthSquared();
IndexedVector3 temp = sqrl > 0?-m_ray:new IndexedVector3(1, 0, 0);
AppendVertice(m_simplices[0], ref temp);
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
{
uint next = 1 - m_current;
sSimplex cs = m_simplices[m_current];
sSimplex ns = m_simplices[next];
/* Check zero */
float rl = m_ray.Length();
if (rl < GjkEpaSolver2.GJK_MIN_DISTANCE)
{/* Touching or inside */
m_status = GJKStatus.Inside;
break;
}
/* Append new vertice in -'v' direction */
IndexedVector3 temp2 = -m_ray;
AppendVertice(cs, ref temp2);
IndexedVector3 w = cs.c[cs.rank - 1].w;
bool found = false;
for (int i = 0; i < 4; ++i)
{
if ((w - lastw[i]).LengthSquared() < GjkEpaSolver2.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 = IndexedVector3.Dot(ref m_ray, ref w) / rl;
alpha = Math.Max(omega, alpha);
if (((rl - alpha) - (GjkEpaSolver2.GJK_ACCURARY * rl)) <= 0)
{/* Return old simplex */
RemoveVertice(m_simplices[m_current]);
break;
}
/* Reduce simplex */
IndexedVector4 weights = new IndexedVector4();
uint mask = 0;
switch (cs.rank)
{
case 2:
{
sqdist = GJK.ProjectOrigin(ref cs.c[0].w, ref cs.c[1].w, ref weights, ref mask);
break;
}
case 3:
{
sqdist = GJK.ProjectOrigin(ref cs.c[0].w, ref cs.c[1].w, ref cs.c[2].w, ref weights, ref mask);
break;
}
case 4:
{
sqdist = GJK.ProjectOrigin(ref cs.c[0].w, ref cs.c[1].w, ref cs.c[2].w, ref cs.c[3].w, ref weights, ref mask);
break;
}
}
if (sqdist >= 0)
{/* Valid */
ns.rank = 0;
m_ray = IndexedVector3.Zero;
m_current = next;
for (uint i = 0, ni = cs.rank; i < ni; ++i)
{
if ((mask & (1 << (int)i)) != 0)
{
ns.c[ns.rank] = cs.c[i];
float weight = weights[(int)i];
ns.p[ns.rank++] = weight;
m_ray += cs.c[i].w * weight;
}
else
{
m_free[m_nfree++] = cs.c[i];
}
}
if (mask == 15)
{
m_status = GJKStatus.Inside;
}
}
else
{/* Return old simplex */
RemoveVertice(m_simplices[m_current]);
break;
}
m_status = ((++iterations) < GjkEpaSolver2.GJK_MAX_ITERATIONS) ? m_status : GJKStatus.Failed;
} while(m_status == GJKStatus.Valid);
m_simplex = m_simplices[m_current];
switch (m_status)
{
case GJKStatus.Valid:
{
m_distance = m_ray.Length();
break;
}
case GJKStatus.Inside:
{
m_distance = 0;
break;
}
}
#if DEBUG
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugGJK)
{
BulletGlobals.g_streamWriter.WriteLine(String.Format("gjk eval dist[{0}]", m_distance));
}
#endif
return(m_status);
}