private void SwapSv(sSV[] array, int a, int b)
{
sSV temp = array[a];
array[a] = array[b];
array[b] = temp;
}
public sFace NewFace(sSV a, sSV b, sSV c, bool forced)
{
if (m_stock.Count > 0)
{
sFace face = m_stock[0];
Remove(m_stock, face);
Append(m_hull, face);
face.pass = 0;
face.c[0] = a;
face.c[1] = b;
face.c[2] = c;
face.n = IndexedVector3.Cross(b.w - a.w, c.w - a.w);
float l = face.n.Length();
bool v = l > GjkEpaSolver2.EPA_ACCURACY;
face.p = Math.Min(Math.Min(
IndexedVector3.Dot(a.w, IndexedVector3.Cross(face.n, a.w - b.w)),
IndexedVector3.Dot(b.w, IndexedVector3.Cross(face.n, b.w - c.w))),
IndexedVector3.Dot(c.w, IndexedVector3.Cross(face.n, c.w - a.w))) /
(v?l:1);
face.p = face.p >= -GjkEpaSolver2.EPA_INSIDE_EPS ? 0 : face.p;
if (v)
{
face.d = IndexedVector3.Dot(ref a.w, ref face.n) / l;
face.n /= l;
if (forced || (face.d >= -GjkEpaSolver2.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.Count > 0?eStatus.OutOfVertices:eStatus.OutOfFaces;
return(null);
}
public bool Expand(uint pass, sSV w, sFace f, uint e, ref sHorizon horizon)
{
if (f.pass != pass)
{
uint e1 = i1m3[e];
if ((IndexedVector3.Dot(ref f.n, ref w.w) - f.d) < -GjkEpaSolver2.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
{
uint e2 = i2m3[e];
f.pass = (uint)pass;
if (Expand(pass, w, f.f[e1], f.e[e1], ref horizon) &&
Expand(pass, w, f.f[e2], f.e[e2], ref horizon))
{
Remove(m_hull, f);
Append(m_stock, f);
return(true);
}
}
}
return(false);
}
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 GetSupport(ref IndexedVector3 d, ref sSV sv)
{
sv.d = d / d.Length();
sv.w = m_shape.Support(ref sv.d);
}
private void SwapSv(sSV[] array, int a, int b)
{
sSV temp = array[a];
array[a] = array[b];
array[b] = temp;
}
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 GetSupport(ref IndexedVector3 d,ref sSV sv)
{
sv.d = d/d.Length();
sv.w = m_shape.Support(ref sv.d);
}
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 bool Expand(uint pass, sSV w, sFace f, uint e, ref sHorizon horizon)
{
if(f.pass!=pass)
{
uint e1 = i1m3[e];
if ((IndexedVector3.Dot(ref f.n, ref w.w) - f.d) < -GjkEpaSolver2.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
{
uint e2=i2m3[e];
f.pass = (uint)pass;
if( Expand(pass,w,f.f[e1],f.e[e1],ref horizon)&&
Expand(pass,w,f.f[e2],f.e[e2],ref horizon))
{
Remove(m_hull,f);
Append(m_stock,f);
return(true);
}
}
}
return(false);
}
public sFace NewFace(sSV a,sSV b,sSV c,bool forced)
{
if(m_stock.Count > 0)
{
sFace face=m_stock[0];
Remove(m_stock,face);
Append(m_hull,face);
face.pass = 0;
face.c[0] = a;
face.c[1] = b;
face.c[2] = c;
face.n = IndexedVector3.Cross(b.w-a.w,c.w-a.w);
float l=face.n.Length();
bool v = l > GjkEpaSolver2.EPA_ACCURACY;
face.p = Math.Min(Math.Min(
IndexedVector3.Dot(a.w,IndexedVector3.Cross(face.n,a.w-b.w)),
IndexedVector3.Dot(b.w,IndexedVector3.Cross(face.n,b.w-c.w))),
IndexedVector3.Dot(c.w,IndexedVector3.Cross(face.n,c.w-a.w))) /
(v?l:1);
face.p = face.p >= -GjkEpaSolver2.EPA_INSIDE_EPS ? 0 : face.p;
if(v)
{
face.d = IndexedVector3.Dot(ref a.w,ref face.n)/l;
face.n /= l;
if (forced || (face.d >= -GjkEpaSolver2.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.Count > 0?eStatus.OutOfVertices:eStatus.OutOfFaces;
return null;
}
