/// <summary>
/// Find the max separation between poly1 and poly2 using edge normals from poly1.
/// </summary>
/// <param name="edgeIndex"></param>
/// <param name="poly1"></param>
/// <param name="xf1"></param>
/// <param name="poly2"></param>
/// <param name="xf2"></param>
/// <returns></returns>
public void findMaxSeparation(EdgeResults results, PolygonShape poly1, Transform xf1, PolygonShape poly2, Transform xf2)
{
int count1 = poly1.m_count;
Vec2[] normals1 = poly1.m_normals;
//Vec2 v = poly2.m_centroid;
// Vector pointing from the centroid of poly1 to the centroid of poly2.
// before inline:
Transform.mulToOutUnsafe(xf2, poly2.m_centroid, d);
Transform.mulToOutUnsafe(xf1, poly1.m_centroid, temp);
d.subLocal(temp);
Rot.mulTransUnsafe(xf1.q, d, dLocal1);
float dLocal1x = dLocal1.x;
float dLocal1y = dLocal1.y;
// after inline:
// final float predy = xf2.p.y + xf2.q.ex.y * v.x + xf2.q.ey.y * v.y;
// final float predx = xf2.p.x + xf2.q.ex.x * v.x + xf2.q.ey.x * v.y;
// final Vec2 v1 = poly1.m_centroid;
// final float tempy = xf1.p.y + xf1.q.ex.y * v1.x + xf1.q.ey.y * v1.y;
// final float tempx = xf1.p.x + xf1.q.ex.x * v1.x + xf1.q.ey.x * v1.y;
// final float dx = predx - tempx;
// final float dy = predy - tempy;
//
// final Mat22 R = xf1.q;
// final float dLocal1x = dx * R.ex.x + dy * R.ex.y;
// final float dLocal1y = dx * R.ey.x + dy * R.ey.y;
// end inline
// Find edge normal on poly1 that has the largest projection onto d.
int edge = 0;
float dot;
//UPGRADE_TODO: The equivalent in .NET for field 'java.lang.Float.MIN_VALUE' may return a different value. "ms-help://MS.VSCC.v80/dv_commoner/local/redirect.htm?index='!DefaultContextWindowIndex'&keyword='jlca1043'"
float maxDot = Single.Epsilon;
for (int i = 0; i < count1; i++)
{
Vec2 normal = normals1[i];
dot = normal.x * dLocal1x + normal.y * dLocal1y;
if (dot > maxDot)
{
maxDot = dot;
edge = i;
}
}
// Get the separation for the edge normal.
float s = edgeSeparation(poly1, xf1, edge, poly2, xf2);
// Check the separation for the previous edge normal.
int prevEdge = edge - 1 >= 0 ? edge - 1 : count1 - 1;
float sPrev = edgeSeparation(poly1, xf1, prevEdge, poly2, xf2);
// Check the separation for the next edge normal.
int nextEdge = edge + 1 < count1 ? edge + 1 : 0;
float sNext = edgeSeparation(poly1, xf1, nextEdge, 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
{
results.edgeIndex = edge;
results.separation = s;
return;
}
// 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(poly1, xf1, edge, poly2, xf2);
if (s > bestSeparation)
{
bestEdge = edge;
bestSeparation = s;
}
else
{
break;
}
}
results.edgeIndex = bestEdge;
results.separation = bestSeparation;
}
/**
* Find the max separation between poly1 and poly2 using edge normals from poly1.
*
* @param edgeIndex
* @param poly1
* @param xf1
* @param poly2
* @param xf2
* @return
*/
public void findMaxSeparation(EdgeResults results, PolygonShape poly1,
Transform xf1, PolygonShape poly2, Transform xf2)
{
int count1 = poly1.m_count;
int count2 = poly2.m_count;
Vec2[] n1s = poly1.m_normals;
Vec2[] v1s = poly1.m_vertices;
Vec2[] v2s = poly2.m_vertices;
Transform.mulTransToOutUnsafe(xf2, xf1, ref xf);
Rot xfq = xf.q;
int bestIndex = 0;
float maxSeparation = -float.MaxValue;
for (int i = 0; i < count1; i++)
{
// Get poly1 normal in frame2.
Rot.mulToOutUnsafe(xfq, n1s[i], ref n);
Transform.mulToOutUnsafe(xf, v1s[i], ref v1);
// Find deepest point for normal i.
float si = float.MaxValue;
for (int j = 0; j < count2; ++j)
{
Vec2 v2sj = v2s[j];
float sij = n.x*(v2sj.x - v1.x) + n.y*(v2sj.y - v1.y);
if (sij < si)
{
si = sij;
}
}
if (si > maxSeparation)
{
maxSeparation = si;
bestIndex = i;
}
}
results.edgeIndex = bestIndex;
results.separation = maxSeparation;
}
