Box2DX.Collision.Collision.EdgeSeparation C# (CSharp) Method

        public static float EdgeSeparation(PolygonShape poly1, Transform xf1, int edge1, PolygonShape poly2, Transform xf2)
        {
            int count1 = poly1._vertexCount;
            Vector2[] vertices1 = poly1._vertices;
            Vector2[] normals1 = poly1._normals;

            int count2 = poly2._vertexCount;
            Vector2[] vertices2 = poly2._vertices;

            Box2DXDebug.Assert(0 <= edge1 && edge1 < count1);

            // Convert normal from poly1's frame into poly2's frame.
            Vector2 normal1World = xf1.TransformDirection(normals1[edge1]);
            Vector2 normal1 = xf2.InverseTransformDirection(normal1World);

            // Find support vertex on poly2 for -normal.
            int index = 0;
            float minDot = Common.Settings.FLT_MAX;
            for (int i = 0; i < count2; ++i)
            {
                float dot = Vector2.Dot(vertices2[i], normal1);
                if (dot < minDot)
                {
                    minDot = dot;
                    index = i;
                }
            }

            Vector2 v1 = xf1.TransformPoint(vertices1[edge1]);
            Vector2 v2 = xf2.TransformPoint(vertices2[index]);
            float separation = Vector2.Dot(v2 - v1, normal1World);
            return separation;
        }

        /// <summary>
        /// Find the max separation between poly1 and poly2 using edge normals from poly1.
        /// </summary>
        public static float FindMaxSeparation(ref int edgeIndex, PolygonShape poly1, Transform xf1, PolygonShape poly2, Transform xf2)
        {
            int count1 = poly1._vertexCount;

            Vector2[] normals1 = poly1._normals;

            // Vector pointing from the centroid of poly1 to the centroid of poly2.
            Vector2 d       = xf2.TransformPoint(poly2._centroid) - xf1.TransformPoint(poly2._centroid);
            Vector2 dLocal1 = xf1.InverseTransformDirection(d);

            // Find edge normal on poly1 that has the largest projection onto d.
            int   edge   = 0;
            float maxDot = -Common.Settings.FLT_MAX;

            for (int i = 0; i < count1; ++i)
            {
                float dot = Vector2.Dot(normals1[i], dLocal1);
                if (dot > maxDot)
                {
                    maxDot = dot;
                    edge   = i;
                }
            }

            // Get the separation for the edge normal.
            float s = Collision.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    = Collision.EdgeSeparation(poly1, xf1, prevEdge, poly2, xf2);

            // Check the separation for the next edge normal.
            int   nextEdge = edge + 1 < count1 ? edge + 1 : 0;
            float sNext    = Collision.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
            {
                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 = Collision.EdgeSeparation(poly1, xf1, edge, poly2, xf2);

                if (s > bestSeparation)
                {
                    bestEdge       = edge;
                    bestSeparation = s;
                }
                else
                {
                    break;
                }
            }

            edgeIndex = bestEdge;
            return(bestSeparation);
        }