Box2D.Collision.Collision.FindIncidentEdge C# (CSharp) Метод

        public void FindIncidentEdge(ClipVertex[] c, PolygonShape poly1, Transform xf1, int edge1, PolygonShape poly2, Transform xf2)
        {
            int count1;
            Vec2[] normals1 = poly1.Normals;

            int count2 = poly2.VertexCount;
            Vec2[] vertices2 = poly2.Vertices;
            Vec2[] normals2 = poly2.Normals;

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

            // Get the normal of the reference edge in poly2's frame.
            Rot.MulToOutUnsafe(xf1.Q, normals1[edge1], normal1); // temporary
            // Vec2 normal1 = MulT(xf2.R, Mul(xf1.R, normals1[edge1]));
            Rot.MulTrans(xf2.Q, normal1, normal1);

            // Find the incident edge on poly2.
            int index = 0;
            float minDot = Single.MaxValue;
            for (int i = 0; i < count2; ++i)
            {
                float dot = Vec2.Dot(normal1, normals2[i]);
                if (dot < minDot)
                {
                    minDot = dot;
                    index = i;
                }
            }

            // Build the clip vertices for the incident edge.
            int i1 = index;
            int i2 = i1 + 1 < count2 ? i1 + 1 : 0;

            Transform.MulToOutUnsafe(xf2, vertices2[i1], c[0].V); // = Mul(xf2, vertices2[i1]);
            c[0].Id.IndexA = (sbyte)edge1;
            c[0].Id.IndexB = (sbyte)i1;
            c[0].Id.TypeA = (sbyte)ContactID.Type.Face;
            c[0].Id.TypeB = (sbyte)ContactID.Type.Vertex;

            Transform.MulToOutUnsafe(xf2, vertices2[i2], c[1].V); // = Mul(xf2, vertices2[i2]);
            c[1].Id.IndexA = (sbyte)edge1;
            c[1].Id.IndexB = (sbyte)i2;
            c[1].Id.TypeA = (sbyte)ContactID.Type.Face;
            c[1].Id.TypeB = (sbyte)ContactID.Type.Vertex;
        }