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);
}
