FindMaxSeparation() public static method

Find the max separation between poly1 and poly2 using edge normals from poly1.
public static FindMaxSeparation ( int &edgeIndex, PolygonShape poly1, Box2DX.Common.Transform xf1, PolygonShape poly2, Box2DX.Common.Transform xf2 ) : float
edgeIndex int
poly1 PolygonShape
xf1 Box2DX.Common.Transform
poly2 PolygonShape
xf2 Box2DX.Common.Transform
return float
コード例 #1
0
        // Find edge normal of max separation on A - return if separating axis is found
        // Find edge normal of max separation on B - return if separation axis is found
        // Choose reference edge as min(minA, minB)
        // Find incident edge
        // Clip
        // The normal points from 1 to 2
        public static void CollidePolygons(ref Manifold manifold,
                                           PolygonShape polyA, Transform xfA, PolygonShape polyB, Transform xfB)
        {
            manifold.PointCount = 0;
            float totalRadius = polyA._radius + polyB._radius;

            int   edgeA       = 0;
            float separationA = Collision.FindMaxSeparation(ref edgeA, polyA, xfA, polyB, xfB);

            if (separationA > totalRadius)
            {
                return;
            }

            int   edgeB       = 0;
            float separationB = Collision.FindMaxSeparation(ref edgeB, polyB, xfB, polyA, xfA);

            if (separationB > totalRadius)
            {
                return;
            }

            PolygonShape poly1;                 // reference poly
            PolygonShape poly2;                 // incident poly
            Transform    xf1, xf2;
            int          edge1;                 // reference edge
            byte         flip;
            const float  k_relativeTol = 0.98f;
            const float  k_absoluteTol = 0.001f;

            if (separationB > k_relativeTol * separationA + k_absoluteTol)
            {
                poly1         = polyB;
                poly2         = polyA;
                xf1           = xfB;
                xf2           = xfA;
                edge1         = edgeB;
                manifold.Type = ManifoldType.FaceB;
                flip          = 1;
            }
            else
            {
                poly1         = polyA;
                poly2         = polyB;
                xf1           = xfA;
                xf2           = xfB;
                edge1         = edgeA;
                manifold.Type = ManifoldType.FaceA;
                flip          = 0;
            }

            ClipVertex[] incidentEdge;
            Collision.FindIncidentEdge(out incidentEdge, poly1, xf1, edge1, poly2, xf2);

            int count1 = poly1._vertexCount;

            Vector2[] vertices1 = poly1._vertices;

            Vector2 v11 = vertices1[edge1];
            Vector2 v12 = edge1 + 1 < count1 ? vertices1[edge1 + 1] : vertices1[0];

            Vector2 dv = v12 - v11;

            Vector2 localNormal = dv.CrossScalarPostMultiply(1.0f);

            localNormal.Normalize();
            Vector2 planePoint = 0.5f * (v11 + v12);

            Vector2 sideNormal = xf1.TransformDirection(v12 - v11);

            sideNormal.Normalize();
            Vector2 frontNormal = sideNormal.CrossScalarPostMultiply(1.0f);

            v11 = Common.Math.Mul(xf1, v11);
            v12 = Common.Math.Mul(xf1, v12);

            float frontOffset = Vector2.Dot(frontNormal, v11);
            float sideOffset1 = -Vector2.Dot(sideNormal, v11);
            float sideOffset2 = Vector2.Dot(sideNormal, v12);

            // Clip incident edge against extruded edge1 side edges.
            ClipVertex[] clipPoints1;
            ClipVertex[] clipPoints2;
            int          np;

            // Clip to box side 1
            np = Collision.ClipSegmentToLine(out clipPoints1, incidentEdge, -sideNormal, sideOffset1);

            if (np < 2)
            {
                return;
            }

            // Clip to negative box side 1
            np = ClipSegmentToLine(out clipPoints2, clipPoints1, sideNormal, sideOffset2);

            if (np < 2)
            {
                return;
            }

            // Now clipPoints2 contains the clipped points.
            manifold.LocalPlaneNormal = localNormal;
            manifold.LocalPoint       = planePoint;

            int pointCount = 0;

            for (int i = 0; i < Settings.MaxManifoldPoints; ++i)
            {
                float separation = Vector2.Dot(frontNormal, clipPoints2[i].V) - frontOffset;

                if (separation <= totalRadius)
                {
                    ManifoldPoint cp = manifold.Points[pointCount];
                    cp.LocalPoint       = xf2.InverseTransformPoint(clipPoints2[i].V);
                    cp.ID               = clipPoints2[i].ID;
                    cp.ID.Features.Flip = flip;
                    ++pointCount;
                }
            }

            manifold.PointCount = pointCount;
        }
コード例 #2
0
        // Find edge normal of max separation on A - return if separating axis is found
        // Find edge normal of max separation on B - return if separation axis is found
        // Choose reference edge as min(minA, minB)
        // Find incident edge
        // Clip
        // The normal points from 1 to 2
        public static void CollidePolygons(ref Manifold manifold,
                                           PolygonShape polyA, XForm xfA, PolygonShape polyB, XForm xfB)
        {
            manifold.PointCount = 0;

            int   edgeA       = 0;
            float separationA = Collision.FindMaxSeparation(ref edgeA, polyA, xfA, polyB, xfB);

            if (separationA > 0.0f)
            {
                return;
            }

            int   edgeB       = 0;
            float separationB = Collision.FindMaxSeparation(ref edgeB, polyB, xfB, polyA, xfA);

            if (separationB > 0.0f)
            {
                return;
            }

            PolygonShape poly1;                 // reference poly
            PolygonShape poly2;                 // incident poly
            XForm        xf1, xf2;
            int          edge1;                 // reference edge
            byte         flip;
            float        k_relativeTol = 0.98f;
            float        k_absoluteTol = 0.001f;

            // TODO_ERIN use "radius" of poly for absolute tolerance.
            if (separationB > k_relativeTol * separationA + k_absoluteTol)
            {
                poly1 = polyB;
                poly2 = polyA;
                xf1   = xfB;
                xf2   = xfA;
                edge1 = edgeB;
                flip  = 1;
            }
            else
            {
                poly1 = polyA;
                poly2 = polyB;
                xf1   = xfA;
                xf2   = xfB;
                edge1 = edgeA;
                flip  = 0;
            }

            ClipVertex[] incidentEdge;
            Collision.FindIncidentEdge(out incidentEdge, poly1, xf1, edge1, poly2, xf2);

            int count1 = poly1.VertexCount;

            Vec2[] vertices1 = poly1.GetVertices();

            Vec2 v11 = vertices1[edge1];
            Vec2 v12 = edge1 + 1 < count1 ? vertices1[edge1 + 1] : vertices1[0];

            Vec2 dv         = v12 - v11;
            Vec2 sideNormal = Common.Math.Mul(xf1.R, v12 - v11);

            sideNormal.Normalize();
            Vec2 frontNormal = Vec2.Cross(sideNormal, 1.0f);

            v11 = Common.Math.Mul(xf1, v11);
            v12 = Common.Math.Mul(xf1, v12);

            float frontOffset = Vec2.Dot(frontNormal, v11);
            float sideOffset1 = -Vec2.Dot(sideNormal, v11);
            float sideOffset2 = Vec2.Dot(sideNormal, v12);

            // Clip incident edge against extruded edge1 side edges.
            ClipVertex[] clipPoints1;
            ClipVertex[] clipPoints2;
            int          np;

            // Clip to box side 1
            np = Collision.ClipSegmentToLine(out clipPoints1, incidentEdge, -sideNormal, sideOffset1);

            if (np < 2)
            {
                return;
            }

            // Clip to negative box side 1
            np = ClipSegmentToLine(out clipPoints2, clipPoints1, sideNormal, sideOffset2);

            if (np < 2)
            {
                return;
            }

            // Now clipPoints2 contains the clipped points.
            manifold.Normal = flip != 0 ? -frontNormal : frontNormal;

            int pointCount = 0;

            for (int i = 0; i < Settings.MaxManifoldPoints; ++i)
            {
                float separation = Vec2.Dot(frontNormal, clipPoints2[i].V) - frontOffset;

                if (separation <= 0.0f)
                {
                    ManifoldPoint cp = manifold.Points[pointCount];
                    cp.Separation       = separation;
                    cp.LocalPoint1      = Box2DX.Common.Math.MulT(xfA, clipPoints2[i].V);
                    cp.LocalPoint2      = Box2DX.Common.Math.MulT(xfB, clipPoints2[i].V);
                    cp.ID               = clipPoints2[i].ID;
                    cp.ID.Features.Flip = flip;
                    ++pointCount;
                }
            }

            manifold.PointCount = pointCount;
        }