public bool Set(ref Face f, Gjk.MinkowskiVertice a, Gjk.MinkowskiVertice b, Gjk.MinkowskiVertice c)
{
Vector3 nrm = Vector3.Cross(b.Vertice - a.Vertice, c.Vertice - a.Vertice);
float len = nrm.Length();
bool valid = (Vector3.Dot(Vector3.Cross(a.Vertice, b.Vertice), nrm) >= -_epaInFaceEpsilon &&
Vector3.Dot(Vector3.Cross(b.Vertice, c.Vertice), nrm) >= -_epaInFaceEpsilon &&
Vector3.Dot(Vector3.Cross(c.Vertice, a.Vertice), nrm) >= -_epaInFaceEpsilon);
f._vertices[0] = a;
f._vertices[1] = b;
f._vertices[2] = c;
f._mark = 0;
f._n = nrm / (len > 0 ? len : _infinity);
f._d = Max(0, -Vector3.Dot(f._n, a.Vertice));
return(valid);
}
public Face NewFace(Gjk.MinkowskiVertice a, Gjk.MinkowskiVertice b, Gjk.MinkowskiVertice c)
{
Face pf = new Face();
if (Set(ref pf, a, b, c))
{
if (_root != null)
{
_root._prev = pf;
}
pf._prev = null;
pf._next = _root;
_root = pf;
++_nfaces;
}
else
{
pf._prev = pf._next = null;
}
return(pf);
}
