public Microsoft.Xna.Framework.Vector2?PenetrationDepth(Func <Microsoft.Xna.Framework.Vector2, Microsoft.Xna.Framework.Vector2> supportA, Func <Microsoft.Xna.Framework.Vector2, Microsoft.Xna.Framework.Vector2> supportB, MinkowskiSimplex simplex) { iterations = 0; while (iterations < MaxIterations) { iterations++; Edge e = FindClosestEdge(simplex.Simplex); var support = MinkowskiSimplex.Support(supportA, supportB, e.Normal); Vector2 p = support.Difference; float d = Vector2.Dot(p, e.Normal); if (d - e.Distance < Tolerance) { return(d * e.Normal); } simplex.Insert(e.Index, support); } return(null); }
public MinkowskiSimplex FindMinkowskiSimplex(Microsoft.Xna.Framework.Vector2 start, Func <Microsoft.Xna.Framework.Vector2, Microsoft.Xna.Framework.Vector2> supportA, Func <Microsoft.Xna.Framework.Vector2, Microsoft.Xna.Framework.Vector2> supportB) { simplex.Initialize(); Microsoft.Xna.Framework.Vector2 d = start; Microsoft.Xna.Framework.Vector2 dperp = new Microsoft.Xna.Framework.Vector2(-d.Y, d.X); double diff = double.MaxValue; simplex.Add(MinkowskiSimplex.Support(supportA, supportB, dperp)); simplex.Add(MinkowskiSimplex.Support(supportA, supportB, d)); simplex.Add(MinkowskiSimplex.Support(supportA, supportB, -d)); d = LineSegmentPointNearestOrigin(simplex.Simplex[1], simplex.Simplex[2]); for (int i = 1; i <= 2; i++) { var newd = LineSegmentPointNearestOrigin(simplex.Simplex[0], simplex.Simplex[i]); if (newd.LengthSquared() < d.LengthSquared()) { d = newd; } } Iterations = 0; Converged = false; while (Iterations < MaxIterations && diff > Tolerance) { Iterations++; if (simplex.ContainsOrigin) { Converged = true; simplex.DistanceFromOrigin = d.Length(); return(simplex); } d = -d; var c = MinkowskiSimplex.Support(supportA, supportB, d); var dotc = Vector2.Dot(c.Difference, d); var dota = Vector2.Dot(simplex.Last(), d); diff = dotc - dota; if (diff < Tolerance) { Converged = true; simplex.DistanceFromOrigin = d.Length(); return(simplex); } var ia = simplex.Simplex.Count - 2; var ib = simplex.Simplex.Count - 1; var p1 = LineSegmentPointNearestOrigin(c.Difference, simplex.Simplex[ia]); var p2 = LineSegmentPointNearestOrigin(c.Difference, simplex.Simplex[ib]); if (p1.LengthSquared() < p2.LengthSquared()) { simplex.StaggerInsert(ib, c); d = p1; } else { simplex.StaggerInsert(ia, c); d = p2; } } simplex.DistanceFromOrigin = d.Length(); return(simplex); }