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)