// Find the separation between poly1 and poly2 for a give edge normal on poly1.
private static float EdgeSeparation(
int edge1,
PolygonFixture poly1,
WorldTransform xf1,
PolygonFixture poly2,
WorldTransform xf2)
{
var vertices1 = poly1.Vertices;
var normals1 = poly1.Normals;
var count2 = poly2.Count;
var vertices2 = poly2.Vertices;
System.Diagnostics.Debug.Assert(0 <= edge1 && edge1 < poly1.Count);
// Convert normal from poly1's frame into poly2's frame.
var normal1World = xf1.Rotation * normals1[edge1];
var normal1 = -xf2.Rotation * normal1World;
// Find support vertex on poly2 for -normal.
var index = 0;
var minDot = float.MaxValue;
for (var i = 0; i < count2; ++i)
{
var dot = Vector2Util.Dot(ref vertices2[i], ref normal1);
if (dot < minDot)
{
minDot = dot;
index = i;
}
}
var v1 = xf1.ToGlobal(vertices1[edge1]);
var v2 = xf2.ToGlobal(vertices2[index]);
// ReSharper disable RedundantCast Necessary for FarPhysics.
var separation = Vector2Util.Dot((Vector2)(v2 - v1), normal1World);
// ReSharper restore RedundantCast
return(separation);
}
// Find the max separation between poly1 and poly2 using edge normals from poly1.
private static float FindMaxSeparation(
out int edgeIndex,
PolygonFixture poly1,
WorldTransform xf1,
PolygonFixture poly2,
WorldTransform xf2)
{
var count1 = poly1.Count;
var normals1 = poly1.Normals;
// Vector pointing from the centroid of poly1 to the centroid of poly2.
// ReSharper disable RedundantCast Necessary for FarPhysics.
var d = (Vector2)(xf2.ToGlobal(poly2.Centroid) - xf1.ToGlobal(poly1.Centroid));
// ReSharper restore RedundantCast
var dLocal1 = -xf1.Rotation * d;
// Find edge normal on poly1 that has the largest projection onto d.
var edge = 0;
var maxDot = float.MinValue;
for (var i = 0; i < count1; ++i)
{
var dot = Vector2Util.Dot(ref normals1[i], ref dLocal1);
if (dot > maxDot)
{
maxDot = dot;
edge = i;
}
}
// Get the separation for the edge normal.
var s = EdgeSeparation(edge, poly1, xf1, poly2, xf2);
// Check the separation for the previous edge normal.
var prevEdge = edge - 1 >= 0 ? edge - 1 : count1 - 1;
var sPrev = EdgeSeparation(prevEdge, poly1, xf1, poly2, xf2);
// Check the separation for the next edge normal.
var nextEdge = edge + 1 < count1 ? edge + 1 : 0;
var sNext = EdgeSeparation(nextEdge, poly1, xf1, poly2, xf2);
// Find the best edge and the search direction.
int bestEdge;
float bestSeparation;
int increment;
if (sPrev > s && sPrev > sNext)
{
increment = -1;
bestEdge = prevEdge;
bestSeparation = sPrev;
}
else if (sNext > s)
{
increment = 1;
bestEdge = nextEdge;
bestSeparation = sNext;
}
else
{
edgeIndex = edge;
return(s);
}
// Perform a local search for the best edge normal.
for (;;)
{
if (increment == -1)
{
edge = bestEdge - 1 >= 0 ? bestEdge - 1 : count1 - 1;
}
else
{
edge = bestEdge + 1 < count1 ? bestEdge + 1 : 0;
}
s = EdgeSeparation(edge, poly1, xf1, poly2, xf2);
if (s <= bestSeparation)
{
break;
}
bestEdge = edge;
bestSeparation = s;
}
edgeIndex = bestEdge;
return(bestSeparation);
}
