Example #1
0
        // This function collides and edge and a polygon.
        // This takes into account edge adjacency.
        // Algorithm:
        // 1. Classify v1 and v2
        // 2. Classify polygon centroid as front or back
        // 3. Flip normal if necessary
        // 4. Initialize normal range to [-pi, pi] about face normal
        // 5. Adjust normal range according to adjacent edges
        // 6. Visit each separating axes, only accept axes within the range
        // 7. Return if _any_ axis indicates separation
        // 8. Clip
        public static bool CollideEdgeAndPolygon(
            Fixture fixtureA,
            WorldTransform xfA,
            Fixture fixtureB,
            WorldTransform xfB,
            out Manifold manifold)
        {
            manifold = new Manifold();

            var edgeA    = fixtureA as EdgeFixture;
            var polygonB = fixtureB as PolygonFixture;

            System.Diagnostics.Debug.Assert(edgeA != null);
            System.Diagnostics.Debug.Assert(polygonB != null);

            // This holds polygon B expressed in frame A.
            var tpv = new Vector2[Settings.MaxPolygonVertices];
            var tpn = new Vector2[Settings.MaxPolygonVertices];

            Vector2 normal0 = Vector2.Zero, normal1, normal2 = Vector2.Zero;

            var xf = xfA.MulT(xfB);

            var centroidB = xf.ToOther(polygonB.Centroid);

            var v0 = edgeA.Vertex0;
            var v1 = edgeA.Vertex1;
            var v2 = edgeA.Vertex2;
            var v3 = edgeA.Vertex3;

            var hasVertex0 = edgeA.HasVertex0;
            var hasVertex3 = edgeA.HasVertex3;

            var edge1 = v2 - v1;

            edge1.Normalize();
            normal1.X = edge1.Y;
            normal1.Y = -edge1.X;
            var offset1 = Vector2Util.Dot(normal1, centroidB - v1);
            var offset0 = 0.0f;
            var offset2 = 0.0f;
            var convex1 = false;
            var convex2 = false;

            // Is there a preceding edge?
            if (hasVertex0)
            {
                var edge0 = v1 - v0;
                edge0.Normalize();
                normal0.X = edge0.Y;
                normal0.Y = -edge0.X;
                convex1   = Vector2Util.Cross(ref edge0, ref edge1) >= 0.0f;
                offset0   = Vector2Util.Dot(normal0, centroidB - v0);
            }

            // Is there a following edge?
            if (hasVertex3)
            {
                var edge2 = v3 - v2;
                edge2.Normalize();
                normal2.X = edge2.Y;
                normal2.Y = -edge2.X;
                convex2   = Vector2Util.Cross(ref edge1, ref edge2) > 0.0f;
                offset2   = Vector2Util.Dot(normal2, centroidB - v2);
            }

            // Determine front or back collision. Determine collision normal limits.
            bool    front;
            Vector2 normal, lowerLimit, upperLimit;

            if (hasVertex0 && hasVertex3)
            {
                if (convex1 && convex2)
                {
                    front = offset0 >= 0.0f || offset1 >= 0.0f || offset2 >= 0.0f;
                    if (front)
                    {
                        normal     = normal1;
                        lowerLimit = normal0;
                        upperLimit = normal2;
                    }
                    else
                    {
                        normal     = -normal1;
                        lowerLimit = -normal1;
                        upperLimit = -normal1;
                    }
                }
                else if (convex1)
                {
                    front = offset0 >= 0.0f || (offset1 >= 0.0f && offset2 >= 0.0f);
                    if (front)
                    {
                        normal     = normal1;
                        lowerLimit = normal0;
                        upperLimit = normal1;
                    }
                    else
                    {
                        normal     = -normal1;
                        lowerLimit = -normal2;
                        upperLimit = -normal1;
                    }
                }
                else if (convex2)
                {
                    front = offset2 >= 0.0f || (offset0 >= 0.0f && offset1 >= 0.0f);
                    if (front)
                    {
                        normal     = normal1;
                        lowerLimit = normal1;
                        upperLimit = normal2;
                    }
                    else
                    {
                        normal     = -normal1;
                        lowerLimit = -normal1;
                        upperLimit = -normal0;
                    }
                }
                else
                {
                    front = offset0 >= 0.0f && offset1 >= 0.0f && offset2 >= 0.0f;
                    if (front)
                    {
                        normal     = normal1;
                        lowerLimit = normal1;
                        upperLimit = normal1;
                    }
                    else
                    {
                        normal     = -normal1;
                        lowerLimit = -normal2;
                        upperLimit = -normal0;
                    }
                }
            }
            else if (hasVertex0)
            {
                if (convex1)
                {
                    front = offset0 >= 0.0f || offset1 >= 0.0f;
                    if (front)
                    {
                        normal     = normal1;
                        lowerLimit = normal0;
                        upperLimit = -normal1;
                    }
                    else
                    {
                        normal     = -normal1;
                        lowerLimit = normal1;
                        upperLimit = -normal1;
                    }
                }
                else
                {
                    front = offset0 >= 0.0f && offset1 >= 0.0f;
                    if (front)
                    {
                        normal     = normal1;
                        lowerLimit = normal1;
                        upperLimit = -normal1;
                    }
                    else
                    {
                        normal     = -normal1;
                        lowerLimit = normal1;
                        upperLimit = -normal0;
                    }
                }
            }
            else if (hasVertex3)
            {
                if (convex2)
                {
                    front = offset1 >= 0.0f || offset2 >= 0.0f;
                    if (front)
                    {
                        normal     = normal1;
                        lowerLimit = -normal1;
                        upperLimit = normal2;
                    }
                    else
                    {
                        normal     = -normal1;
                        lowerLimit = -normal1;
                        upperLimit = normal1;
                    }
                }
                else
                {
                    front = offset1 >= 0.0f && offset2 >= 0.0f;
                    if (front)
                    {
                        normal     = normal1;
                        lowerLimit = -normal1;
                        upperLimit = normal1;
                    }
                    else
                    {
                        normal     = -normal1;
                        lowerLimit = -normal2;
                        upperLimit = normal1;
                    }
                }
            }
            else
            {
                front = offset1 >= 0.0f;
                if (front)
                {
                    normal     = normal1;
                    lowerLimit = -normal1;
                    upperLimit = -normal1;
                }
                else
                {
                    normal     = -normal1;
                    lowerLimit = normal1;
                    upperLimit = normal1;
                }
            }

            // Get polygonB in frameA.
            var tpc = polygonB.Count;

            for (var i = 0; i < tpc; ++i)
            {
                tpv[i] = xf.ToOther(polygonB.Vertices[i]);
                tpn[i] = xf.Rotation * polygonB.Normals[i];
            }

            const float radius = 2.0f * Settings.PolygonRadius;

            Axis edgeAxis;

            edgeAxis.Type       = Axis.AxisType.EdgeA;
            edgeAxis.Index      = front ? 0 : 1;
            edgeAxis.Separation = float.MaxValue;

            for (var i = 0; i < tpc; ++i)
            {
                var s = Vector2Util.Dot(normal, tpv[i] - v1);
                if (s < edgeAxis.Separation)
                {
                    edgeAxis.Separation = s;
                }
            }

            // If no valid normal can be found than this edge should not collide.
            if (edgeAxis.Type == Axis.AxisType.None)
            {
                return(false);
            }

            if (edgeAxis.Separation > radius)
            {
                return(false);
            }

            Axis polygonAxis;

            polygonAxis.Type       = Axis.AxisType.None;
            polygonAxis.Index      = -1;
            polygonAxis.Separation = float.MinValue;

            Vector2 perp;

            perp.X = -normal.Y;
            perp.Y = normal.X;

            for (var i = 0; i < tpc; ++i)
            {
                var n = -tpn[i];

                var s1 = Vector2Util.Dot(n, tpv[i] - v1);
                var s2 = Vector2Util.Dot(n, tpv[i] - v2);
                var s  = System.Math.Min(s1, s2);

                if (s > radius)
                {
                    // No collision
                    polygonAxis.Type       = Axis.AxisType.EdgeB;
                    polygonAxis.Index      = i;
                    polygonAxis.Separation = s;
                    break;
                }

                // Adjacency
                if (Vector2Util.Dot(ref n, ref perp) >= 0.0f)
                {
                    if (Vector2Util.Dot(n - upperLimit, normal) < -Settings.AngularSlop)
                    {
                        continue;
                    }
                }
                else
                {
                    if (Vector2Util.Dot(n - lowerLimit, normal) < -Settings.AngularSlop)
                    {
                        continue;
                    }
                }

                if (s > polygonAxis.Separation)
                {
                    polygonAxis.Type       = Axis.AxisType.EdgeB;
                    polygonAxis.Index      = i;
                    polygonAxis.Separation = s;
                }
            }

            if (polygonAxis.Type != Axis.AxisType.None && polygonAxis.Separation > radius)
            {
                return(false);
            }

            // Use hysteresis for jitter reduction.
            const float relativeTol = 0.98f;
            const float absoluteTol = 0.001f;

            Axis primaryAxis;

            if (polygonAxis.Type == Axis.AxisType.None)
            {
                primaryAxis = edgeAxis;
            }
            else if (polygonAxis.Separation > relativeTol * edgeAxis.Separation + absoluteTol)
            {
                primaryAxis = polygonAxis;
            }
            else
            {
                primaryAxis = edgeAxis;
            }

            FixedArray2 <ClipVertex> incidentEdge;

            // Reference face used for clipping
            int     rfi1, rfi2;
            Vector2 rfv1, rfv2;
            Vector2 rfnormal;
            Vector2 rfsideNormal1;

            if (primaryAxis.Type == Axis.AxisType.EdgeA)
            {
                manifold.Type = Manifold.ManifoldType.FaceA;

                // Search for the polygon normal that is most anti-parallel to the edge normal.
                var bestIndex = 0;
                var bestValue = Vector2Util.Dot(ref normal, ref tpn[0]);
                for (var i = 1; i < tpc; ++i)
                {
                    var value = Vector2Util.Dot(ref normal, ref tpn[i]);
                    if (value < bestValue)
                    {
                        bestValue = value;
                        bestIndex = i;
                    }
                }

                var i1 = bestIndex;
                var i2 = i1 + 1 < tpc ? i1 + 1 : 0;

                incidentEdge = new FixedArray2 <ClipVertex>
                {
                    Item1 = new ClipVertex
                    {
                        Vertex = tpv[i1],
                        Id     =
                        {
                            Feature    =
                            {
                                IndexA =                                     0,
                                IndexB = (byte)i1,
                                TypeA  = (byte)ContactFeature.FeatureType.Face,
                                TypeB  = (byte)ContactFeature.FeatureType.Vertex
                            }
                        }
                    },
                    Item2 = new ClipVertex
                    {
                        Vertex = tpv[i2],
                        Id     =
                        {
                            Feature    =
                            {
                                IndexA =                                     0,
                                IndexB = (byte)i2,
                                TypeA  = (byte)ContactFeature.FeatureType.Face,
                                TypeB  = (byte)ContactFeature.FeatureType.Vertex
                            }
                        }
                    }
                };

                if (front)
                {
                    rfi1     = 0;
                    rfi2     = 1;
                    rfv1     = v1;
                    rfv2     = v2;
                    rfnormal = normal1;
                }
                else
                {
                    rfi1     = 1;
                    rfi2     = 0;
                    rfv1     = v2;
                    rfv2     = v1;
                    rfnormal = -normal1;
                }
            }
            else
            {
                manifold.Type = Manifold.ManifoldType.FaceB;

                incidentEdge = new FixedArray2 <ClipVertex>
                {
                    Item1 = new ClipVertex
                    {
                        Vertex = v1,
                        Id     =
                        {
                            Feature    =
                            {
                                IndexA =                                       0,
                                IndexB = (byte)primaryAxis.Index,
                                TypeA  = (byte)ContactFeature.FeatureType.Vertex,
                                TypeB  = (byte)ContactFeature.FeatureType.Face
                            }
                        }
                    },
                    Item2 = new ClipVertex
                    {
                        Vertex = v2,
                        Id     =
                        {
                            Feature    =
                            {
                                IndexA =                                       0,
                                IndexB = (byte)primaryAxis.Index,
                                TypeA  = (byte)ContactFeature.FeatureType.Vertex,
                                TypeB  = (byte)ContactFeature.FeatureType.Face
                            }
                        }
                    }
                };

                rfi1     = primaryAxis.Index;
                rfi2     = rfi1 + 1 < tpc ? rfi1 + 1 : 0;
                rfv1     = tpv[rfi1];
                rfv2     = tpv[rfi2];
                rfnormal = tpn[rfi1];
            }

            rfsideNormal1.X = rfnormal.Y;
            rfsideNormal1.Y = -rfnormal.X;
            var rfsideNormal2 = -rfsideNormal1;
            var rfsideOffset1 = Vector2Util.Dot(ref rfsideNormal1, ref rfv1);
            var rfsideOffset2 = Vector2Util.Dot(ref rfsideNormal2, ref rfv2);

            // Clip incident edge against extruded edge1 side edges.
            FixedArray2 <ClipVertex> clipPoints1, clipPoints2;

            // Clip to box side 1
            var np = ClipSegmentToLine(
                out clipPoints1,
                incidentEdge,
                rfsideNormal1,
                rfsideOffset1,
                rfi1);

            if (np < 2)
            {
                return(false);
            }

            // Clip to negative box side 1
            np = ClipSegmentToLine(
                out clipPoints2,
                clipPoints1,
                rfsideNormal2,
                rfsideOffset2,
                rfi2);

            if (np < 2)
            {
                return(false);
            }

            // Now clipPoints2 contains the clipped points.
            if (primaryAxis.Type == Axis.AxisType.EdgeA)
            {
                manifold.LocalPoint  = rfv1;
                manifold.LocalNormal = rfnormal;
            }
            else
            {
                manifold.LocalPoint  = polygonB.Vertices[rfi1];
                manifold.LocalNormal = polygonB.Normals[rfi1];
            }

            var pointCount = 0;

            for (var i = 0; i < 2; ++i)
            {
                if (Vector2Util.Dot(rfnormal, clipPoints2[i].Vertex - rfv1) <= radius)
                {
                    var cp = manifold.Points[pointCount];
                    if (primaryAxis.Type == Axis.AxisType.EdgeA)
                    {
                        cp.LocalPoint = xf.FromOther(clipPoints2[i].Vertex);
                        cp.Id         = clipPoints2[i].Id;
                    }
                    else
                    {
                        cp.LocalPoint        = clipPoints2[i].Vertex;
                        cp.Id.Feature.TypeA  = clipPoints2[i].Id.Feature.TypeB;
                        cp.Id.Feature.TypeB  = clipPoints2[i].Id.Feature.TypeA;
                        cp.Id.Feature.IndexA = clipPoints2[i].Id.Feature.IndexB;
                        cp.Id.Feature.IndexB = clipPoints2[i].Id.Feature.IndexA;
                    }
                    manifold.Points[pointCount] = cp;

                    ++pointCount;
                }
            }

            manifold.PointCount = pointCount;

            return(pointCount > 0);
        }
Example #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 bool CollidePolygons(
            Fixture fixtureA,
            WorldTransform transformA,
            Fixture fixtureB,
            WorldTransform transformB,
            out Manifold manifold)
        {
            manifold = new Manifold();

            var polygonA = fixtureA as PolygonFixture;
            var polygonB = fixtureB as PolygonFixture;

            System.Diagnostics.Debug.Assert(polygonA != null);
            System.Diagnostics.Debug.Assert(polygonB != null);

            var totalRadius = polygonA.Radius + polygonB.Radius;

            int edgeA;
            var separationA = FindMaxSeparation(out edgeA, polygonA, transformA, polygonB, transformB);

            if (separationA > totalRadius)
            {
                return(false);
            }

            int edgeB;
            var separationB = FindMaxSeparation(out edgeB, polygonB, transformB, polygonA, transformA);

            if (separationB > totalRadius)
            {
                return(false);
            }

            PolygonFixture polygon1; // reference polygon
            PolygonFixture polygon2; // incident polygon
            WorldTransform transform1, transform2;
            int            edge1;    // reference edge
            bool           flip;
            const float    relativeTol = 0.98f;
            const float    absoluteTol = 0.001f;

            if (separationB > relativeTol * separationA + absoluteTol)
            {
                polygon1      = polygonB;
                polygon2      = polygonA;
                transform1    = transformB;
                transform2    = transformA;
                edge1         = edgeB;
                manifold.Type = Manifold.ManifoldType.FaceB;
                flip          = true;
            }
            else
            {
                polygon1      = polygonA;
                polygon2      = polygonB;
                transform1    = transformA;
                transform2    = transformB;
                edge1         = edgeA;
                manifold.Type = Manifold.ManifoldType.FaceA;
                flip          = false;
            }

            // Transformation mapping from the second polygon's frame of reference to
            // first one's. We use this to directly map points around, without getting
            // into the global coordinate system.
            var transform21 = transform1.MulT(transform2);

            // Begin inlined FindIncidentEdge()
            FixedArray2 <ClipVertex> incidentEdge;
            {
                System.Diagnostics.Debug.Assert(0 <= edge1 && edge1 < polygon1.Count);

                var normals1 = polygon1.Normals;

                var vertices2 = polygon2.Vertices;
                var normals2  = polygon2.Normals;
                var count2    = polygon2.Count;

                // Get the normal of the reference edge in poly2's frame.
                var normal12 = -transform2.Rotation * (transform1.Rotation * normals1[edge1]);

                // Find the incident edge on poly2 by finding the clip vertices
                // for the incident edge.
                // Get the face whose own normal has the smallest angular
                // difference to the incident normal.
                var edge2  = 0;
                var minDot = float.MaxValue;
                for (var i = 0; i < count2; ++i)
                {
                    var dot = Vector2Util.Dot(ref normal12, ref normals2[i]);
                    if (dot < minDot)
                    {
                        minDot = dot;
                        edge2  = i;
                    }
                }

                // The edge's index coincides with the first vertex used to define
                // that edge, so we can use that and wrap around as necessary for the
                // second one.
                var index21 = edge2;
                var index22 = index21 + 1 < count2 ? index21 + 1 : 0;

                // Get the incident edge as defined by its two vertices, in the first
                // polygon's frame of reference.
                incidentEdge = new FixedArray2 <ClipVertex>
                {
                    Item1 = new ClipVertex
                    {
                        //Vertex = xf2.ToGlobal(vertices2[i1]),
                        Vertex = transform21.ToOther(vertices2[index21]),
                        Id     =
                        {
                            Feature    =
                            {
                                IndexA = (byte)edge1,
                                IndexB = (byte)index21,
                                TypeA  = (byte)ContactFeature.FeatureType.Face,
                                TypeB  = (byte)ContactFeature.FeatureType.Vertex
                            }
                        }
                    },
                    Item2 = new ClipVertex
                    {
                        //Vertex = xf2.ToGlobal(vertices2[i2]),
                        Vertex = transform21.ToOther(vertices2[index22]),
                        Id     =
                        {
                            Feature    =
                            {
                                IndexA = (byte)edge1,
                                IndexB = (byte)index22,
                                TypeA  = (byte)ContactFeature.FeatureType.Face,
                                TypeB  = (byte)ContactFeature.FeatureType.Vertex
                            }
                        }
                    }
                };
            }
            // End inlined FindIncidentEdge()

            var vertices1 = polygon1.Vertices;
            var count1    = polygon1.Count;

            var index11 = edge1;
            var index12 = edge1 + 1 < count1 ? edge1 + 1 : 0;

            var vertex11 = vertices1[index11];
            var vertex12 = vertices1[index12];

            var tangent1 = vertex12 - vertex11;

            tangent1.Normalize();

            var normal1     = Vector2Util.Cross(ref tangent1, 1);
            var planePoint1 = 0.5f * (vertex11 + vertex12);

            // Face offset.
            var frontOffset = Vector2Util.Dot(ref normal1, ref vertex11);

            // Side offsets, extended by polytope skin thickness.
            var sideOffset1 = -Vector2Util.Dot(ref tangent1, ref vertex11) + totalRadius;
            var sideOffset2 = Vector2Util.Dot(ref tangent1, ref vertex12) + totalRadius;

            // Clip incident edge against extruded edge1 side edges.
            FixedArray2 <ClipVertex> clipPoints1, clipPoints2;

            // Clip to box side 1
            var np = ClipSegmentToLine(out clipPoints1, incidentEdge, -tangent1, sideOffset1, index11);

            if (np < 2)
            {
                return(false);
            }

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

            if (np < 2)
            {
                return(false);
            }

            // Now clipPoints2 contains the clipped points.
            manifold.LocalNormal = normal1;
            manifold.LocalPoint  = planePoint1;

            var pointCount = 0;

            for (var i = 0; i < 2; ++i)
            {
                //if (Vector2Util.Dot(normal1g, clipPoints2[i].Vertex) - frontOffset <= totalRadius)
                if (Vector2Util.Dot(normal1, clipPoints2[i].Vertex) - frontOffset <= totalRadius)
                {
                    var cp = manifold.Points[pointCount];
                    //cp.localPoint = transform2.ToLocal(clipPoints2[i].Vertex);
                    cp.LocalPoint = transform21.FromOther(clipPoints2[i].Vertex);
                    cp.Id         = clipPoints2[i].Id;
                    if (flip)
                    {
                        // Swap features
                        var cf = cp.Id.Feature;
                        cp.Id.Feature.IndexA = cf.IndexB;
                        cp.Id.Feature.IndexB = cf.IndexA;
                        cp.Id.Feature.TypeA  = cf.TypeB;
                        cp.Id.Feature.TypeB  = cf.TypeA;
                    }
                    manifold.Points[pointCount] = cp;
                    ++pointCount;
                }
            }

            manifold.PointCount = pointCount;

            return(pointCount > 0);
        }