public float EdgeSeparation(PolygonShape poly1, Transform xf1, int edge1, PolygonShape poly2, Transform xf2)
{
int count1;
Vec2[] vertices1 = poly1.Vertices;
Vec2[] normals1 = poly1.Normals;
int count2 = poly2.VertexCount;
Vec2[] vertices2 = poly2.Vertices;
Debug.Assert(0 <= edge1 && edge1 < count1);
// Convert normal from poly1's frame into poly2's frame.
// before inline:
// Vec2 normal1World = Mul(xf1.R, normals1[edge1]);
Rot.MulToOutUnsafe(xf1.Q, normals1[edge1], normal1World);
// Vec2 normal1 = MulT(xf2.R, normal1World);
Rot.MulTransUnsafe(xf2.Q, normal1World, normal1);
float normal1X = normal1.X;
float normal1Y = normal1.Y;
// after inline:
// R.mulToOut(v,out);
// final Mat22 R = xf1.q;
// final Vec2 v = normals1[edge1];
// final float normal1Worldy = R.ex.y * v.x + R.ey.y * v.y;
// final float normal1Worldx = R.ex.x * v.x + R.ey.x * v.y;
// final Mat22 R1 = xf2.q;
// final float normal1x = normal1Worldx * R1.ex.x + normal1Worldy * R1.ex.y;
// final float normal1y = normal1Worldx * R1.ey.x + normal1Worldy * R1.ey.y;
// end inline
// Find support vertex on poly2 for -normal.
int index = 0;
float minDot = Single.MaxValue;
for (int i = 0; i < count2; ++i)
{
Vec2 a = vertices2[i];
float dot = a.X * normal1X + a.Y * normal1Y;
if (dot < minDot)
{
minDot = dot;
index = i;
}
}
// Vec2 v1 = Mul(xf1, vertices1[edge1]);
// Vec2 v2 = Mul(xf2, vertices2[index]);
// before inline:
Transform.MulToOut(xf1, vertices1[edge1], v1);
Transform.MulToOut(xf2, vertices2[index], v2);
float separation = Vec2.Dot(v2.SubLocal(v1), normal1World);
return separation;
// after inline:
// final Vec2 v3 = vertices1[edge1];
// final float v1y = xf1.p.y + R.ex.y * v3.x + R.ey.y * v3.y;
// final float v1x = xf1.p.x + R.ex.x * v3.x + R.ey.x * v3.y;
// final Vec2 v4 = vertices2[index];
// final float v2y = xf2.p.y + R1.ex.y * v4.x + R1.ey.y * v4.y - v1y;
// final float v2x = xf2.p.x + R1.ex.x * v4.x + R1.ey.x * v4.y - v1x;
//
// return v2x * normal1Worldx + v2y * normal1Worldy;
// end inline
}
