public void Initialise()
{
m_ray = Vector3.Zero;
m_nfree = 0;
m_status = GJKStatus.Failed;
m_current = 0;
m_distance = 0f;
for (int i = 0; i < m_simplices.Length; ++i)
{
m_simplices[i] = new sSimplex();
}
for (int i = 0; i < m_store.Length; ++i)
{
m_store[i] = new sSV();
}
// Free is a list of pointers/references so doesn't need objects.
//for (int i = 0; i < m_free.Length; ++i)
//{
// m_free[i] = new sSV();
//}
}
public void Initialize()
{
m_status = eStatus.Failed;
m_normal = Vector3.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]);
}
}
private void SwapSv(sSV[] array, int a, int b)
{
sSV temp = array[a];
array[a] = array[b];
array[b] = temp;
}
public bool Expand(uint pass,sSV w,sFace f,uint e,sHorizon horizon)
{
uint[] i1m3 = {1,2,0};
uint[] i2m3 = {2,0,1};
if(f.pass!=pass)
{
uint e1 = i1m3[e];
if ((Vector3.Dot(f.n, 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],horizon)&&
Expand(pass,w,f.f[e2],f.e[e2],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 = Vector3.Cross(b.w-a.w,c.w-a.w);
float l=face.n.Length();
bool v = l > GjkEpaSolver2.EPA_ACCURACY;
face.p = System.Math.Min(System.Math.Min(
Vector3.Dot(a.w,Vector3.Cross(face.n,a.w-b.w)),
Vector3.Dot(b.w,Vector3.Cross(face.n,b.w-c.w))),
Vector3.Dot(c.w,Vector3.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 = Vector3.Dot(a.w,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 void GetSupport(ref Vector3 d,ref sSV sv)
{
sv.d = d/d.Length();
sv.w = m_shape.Support(ref sv.d);
}