public EPA(ISupportable a, ISupportable b, Transform at, Transform bt, List <Triangle> poly)
{
this.a = a;
this.b = b;
this.at = at;
this.bt = bt;
this.poly = poly;
}
public float GetDepth(out Vector3 normal, out Vector3 position)
{
int iterations = 0;
while (true)
{
iterations++;
if (iterations > GJK.iterationCap)
{
shouldFail = true;
}
if (shouldFail)
{
normal = new Vector3(float.NaN);
position = new Vector3(float.NaN);
return(-1);
}
poly.Sort((x, y) => (x.depth).CompareTo(y.depth));
Triangle triangle = poly[0];
SupportPoint newp = Support(triangle.normal);
if (triangle.Behind(newp))
{
CreateAndFillHole(newp);
}
else
{
//done
position = triangle.normal * triangle.depth;
float a = (position - triangle.a.point).Length();
float b = (position - triangle.b.point).Length();
float c = (position - triangle.c.point).Length();
float total = a + b + c;
a /= total;
b /= total;
c /= total;
position = (a * triangle.a.AverageSupport) + (b * triangle.b.AverageSupport) + (c * triangle.c.AverageSupport);
normal = -triangle.normal;
Vector3 AtoB = at.position - bt.position;
if (Vector3.Dot(normal, AtoB) < 0)
{
normal *= -1;
}
return(triangle.depth);
}
}
}
public static float DoEPA(ISupportable a, ISupportable b, Transform at, Transform bt, out Vector3 normal,
out Vector3 position, in List <Triangle> poly, List <SupportPoint> polyAsPointList)
