GetCentroid() public method

Get local centroid relative to the parent body.
public GetCentroid ( ) : Vec2
return Box2DX.Common.Vec2
        /// <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 static float FindMaxSeparation(ref int edgeIndex,
			PolygonShape poly1, XForm xf1, PolygonShape poly2, XForm xf2)
        {
            int count1 = poly1.VertexCount;
            Vec2[] normals1 = poly1.Normals;

            // Vector pointing from the centroid of poly1 to the centroid of poly2.
            Vec2 d = Common.Math.Mul(xf2, poly2.GetCentroid()) - Common.Math.Mul(xf1, poly1.GetCentroid());
            Vec2 dLocal1 = Common.Math.MulT(xf1.R, 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 = Vec2.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);
            if (s > 0.0f)
            {
                return s;
            }

            // 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);
            if (sPrev > 0.0f)
            {
                return sPrev;
            }

            // 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);
            if (sNext > 0.0f)
            {
                return sNext;
            }

            // 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 > 0.0f)
                {
                    return s;
                }

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

            edgeIndex = bestEdge;
            return bestSeparation;
        }
Ejemplo n.º 2
0
        /// <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 static float FindMaxSeparation(ref int edgeIndex,
                                              PolygonShape poly1, XForm xf1, PolygonShape poly2, XForm xf2)
        {
            int count1 = poly1.VertexCount;

            Vec2[] normals1 = poly1.Normals;

            // Vector pointing from the centroid of poly1 to the centroid of poly2.
            Vec2 d       = Common.Math.Mul(xf2, poly2.GetCentroid()) - Common.Math.Mul(xf1, poly1.GetCentroid());
            Vec2 dLocal1 = Common.Math.MulT(xf1.R, 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 = Vec2.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);

            if (s > 0.0f)
            {
                return(s);
            }

            // 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);

            if (sPrev > 0.0f)
            {
                return(sPrev);
            }

            // 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);

            if (sNext > 0.0f)
            {
                return(sNext);
            }

            // 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 > 0.0f)
                {
                    return(s);
                }

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

            edgeIndex = bestEdge;
            return(bestSeparation);
        }