Пример #1
0
        /// <summary>
        /// Compute the collision manifold between two circles.
        /// </summary>
        public static void CollideCircles(ref Manifold manifold, CircleShape circleA, ref Transform xfA, CircleShape circleB, ref Transform xfB)
        {
            manifold.PointCount = 0;

            Vector2 pA = MathUtils.Mul(ref xfA, circleA.Position);
            Vector2 pB = MathUtils.Mul(ref xfB, circleB.Position);

            Vector2 d = pB - pA;
            float   distSqr = Vector2.Dot(d, d);
            float   rA = circleA.Radius, rB = circleB.Radius;
            float   radius = rA + rB;

            if (distSqr > radius * radius)
            {
                return;
            }

            manifold.Type        = ManifoldType.Circles;
            manifold.LocalPoint  = circleA.Position;
            manifold.LocalNormal = Vector2.Zero;
            manifold.PointCount  = 1;

            ManifoldPoint p0 = manifold.Points[0];

            p0.LocalPoint      = circleB.Position;
            p0.Id.Key          = 0;
            manifold.Points[0] = p0;
        }
        /// <summary>
        /// Compute the collision manifold between two polygons.
        /// </summary>
        public static void CollidePolygons(ref Manifold manifold, PolygonShape polyA, ref Transform xfA, PolygonShape polyB, ref Transform xfB)
        {
            // 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

            manifold.PointCount = 0;
            GGame.Math.Fix64 totalRadius = polyA.Radius + polyB.Radius;

            int edgeA;

            GGame.Math.Fix64 separationA = FindMaxSeparation(out edgeA, polyA, ref xfA, polyB, ref xfB);
            if (separationA > totalRadius)
            {
                return;
            }

            int edgeB;

            GGame.Math.Fix64 separationB = FindMaxSeparation(out edgeB, polyB, ref xfB, polyA, ref xfA);
            if (separationB > totalRadius)
            {
                return;
            }

            PolygonShape poly1; // reference polygon
            PolygonShape poly2; // incident polygon
            Transform    xf1, xf2;
            int          edge1; // reference edge
            bool         flip;

            GGame.Math.Fix64 k_tol = 0.1f * Settings.LinearSlop;

            if (separationB > separationA + k_tol)
            {
                poly1         = polyB;
                poly2         = polyA;
                xf1           = xfB;
                xf2           = xfA;
                edge1         = edgeB;
                manifold.Type = ManifoldType.FaceB;
                flip          = true;
            }
            else
            {
                poly1         = polyA;
                poly2         = polyB;
                xf1           = xfA;
                xf2           = xfB;
                edge1         = edgeA;
                manifold.Type = ManifoldType.FaceA;
                flip          = false;
            }

            FixedArray2 <ClipVertex> incidentEdge;

            FindIncidentEdge(out incidentEdge, poly1, ref xf1, edge1, poly2, ref xf2);

            int      count1    = poly1.Vertices.Count;
            Vertices vertices1 = poly1.Vertices;

            int iv1 = edge1;
            int iv2 = edge1 + 1 < count1 ? edge1 + 1 : 0;

            Vector2 v11 = vertices1[iv1];
            Vector2 v12 = vertices1[iv2];

            Vector2 localTangent = v12 - v11;

            localTangent.Normalize();

            Vector2 localNormal = MathUtils.Cross(localTangent, 1.0f);
            Vector2 planePoint  = 0.5f * (v11 + v12);

            Vector2 tangent = MathUtils.Mul(ref xf1.q, localTangent);
            Vector2 normal  = MathUtils.Cross(tangent, 1.0f);

            v11 = MathUtils.Mul(ref xf1, v11);
            v12 = MathUtils.Mul(ref xf1, v12);

            // Face offset.
            GGame.Math.Fix64 frontOffset = Vector2.Dot(normal, v11);

            // Side offsets, extended by polytope skin thickness.
            GGame.Math.Fix64 sideOffset1 = -Vector2.Dot(tangent, v11) + totalRadius;
            GGame.Math.Fix64 sideOffset2 = Vector2.Dot(tangent, v12) + totalRadius;

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

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

            if (np < 2)
            {
                return;
            }

            // Clip to negative box side 1
            np = Collision.ClipSegmentToLine(out clipPoints2, ref clipPoints1, tangent, sideOffset2, iv2);

            if (np < 2)
            {
                return;
            }

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

            int pointCount = 0;

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

                if (separation <= totalRadius)
                {
                    ManifoldPoint cp = manifold.Points[pointCount];
                    cp.LocalPoint = MathUtils.MulT(ref xf2, clipPoints2[i].V);
                    cp.Id         = clipPoints2[i].ID;

                    if (flip)
                    {
                        // Swap features
                        ContactFeature cf = cp.Id.ContactFeature;
                        cp.Id.ContactFeature.IndexA = cf.IndexB;
                        cp.Id.ContactFeature.IndexB = cf.IndexA;
                        cp.Id.ContactFeature.TypeA  = cf.TypeB;
                        cp.Id.ContactFeature.TypeB  = cf.TypeA;
                    }

                    manifold.Points[pointCount] = cp;

                    ++pointCount;
                }
            }

            manifold.PointCount = pointCount;
        }
Пример #3
0
        public void Collide(ref Manifold manifold, EdgeShape edgeA, ref Transform xfA, PolygonShape polygonB, ref Transform xfB)
        {
            // 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

            _xf = MathUtils.MulT(xfA, xfB);

            _centroidB = MathUtils.Mul(ref _xf, polygonB.MassData.Centroid);

            _v0 = edgeA.Vertex0;
            _v1 = edgeA._vertex1;
            _v2 = edgeA._vertex2;
            _v3 = edgeA.Vertex3;

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

            Vector2 edge1 = _v2 - _v1;

            edge1.Normalize();
            _normal1 = new Vector2(edge1.Y, -edge1.X);
            float offset1 = Vector2.Dot(_normal1, _centroidB - _v1);
            float offset0 = 0.0f, offset2 = 0.0f;
            bool  convex1 = false, convex2 = false;

            // Is there a preceding edge?
            if (hasVertex0)
            {
                Vector2 edge0 = _v1 - _v0;
                edge0.Normalize();
                _normal0 = new Vector2(edge0.Y, -edge0.X);
                convex1  = MathUtils.Cross(edge0, edge1) >= 0.0f;
                offset0  = Vector2.Dot(_normal0, _centroidB - _v0);
            }

            // Is there a following edge?
            if (hasVertex3)
            {
                Vector2 edge2 = _v3 - _v2;
                edge2.Normalize();
                _normal2 = new Vector2(edge2.Y, -edge2.X);
                convex2  = MathUtils.Cross(edge1, edge2) > 0.0f;
                offset2  = Vector2.Dot(_normal2, _centroidB - _v2);
            }

            // Determine front or back collision. Determine collision normal limits.
            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
            _polygonB.Count = polygonB.Vertices.Count;
            for (int i = 0; i < polygonB.Vertices.Count; ++i)
            {
                _polygonB.Vertices[i] = MathUtils.Mul(ref _xf, polygonB.Vertices[i]);
                _polygonB.Normals[i]  = MathUtils.Mul(_xf.q, polygonB.Normals[i]);
            }

            _radius = 2.0f * Settings.PolygonRadius;

            manifold.PointCount = 0;

            EPAxis edgeAxis = ComputeEdgeSeparation();

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

            if (edgeAxis.Separation > _radius)
            {
                return;
            }

            EPAxis polygonAxis = ComputePolygonSeparation();

            if (polygonAxis.Type != EPAxisType.Unknown && polygonAxis.Separation > _radius)
            {
                return;
            }

            // Use hysteresis for jitter reduction.
            const float k_relativeTol = 0.98f;
            const float k_absoluteTol = 0.001f;

            EPAxis primaryAxis;

            if (polygonAxis.Type == EPAxisType.Unknown)
            {
                primaryAxis = edgeAxis;
            }
            else if (polygonAxis.Separation > k_relativeTol * edgeAxis.Separation + k_absoluteTol)
            {
                primaryAxis = polygonAxis;
            }
            else
            {
                primaryAxis = edgeAxis;
            }

            FixedArray2 <ClipVertex> ie = new FixedArray2 <ClipVertex>();
            ReferenceFace            rf;

            if (primaryAxis.Type == EPAxisType.EdgeA)
            {
                manifold.Type = ManifoldType.FaceA;

                // Search for the polygon normal that is most anti-parallel to the edge normal.
                int   bestIndex = 0;
                float bestValue = Vector2.Dot(_normal, _polygonB.Normals[0]);
                for (int i = 1; i < _polygonB.Count; ++i)
                {
                    float value = Vector2.Dot(_normal, _polygonB.Normals[i]);
                    if (value < bestValue)
                    {
                        bestValue = value;
                        bestIndex = i;
                    }
                }

                int i1 = bestIndex;
                int i2 = i1 + 1 < _polygonB.Count ? i1 + 1 : 0;

                ie.Value0.V = _polygonB.Vertices[i1];
                ie.Value0.ID.ContactFeature.IndexA = 0;
                ie.Value0.ID.ContactFeature.IndexB = (byte)i1;
                ie.Value0.ID.ContactFeature.TypeA  = ContactFeatureType.Face;
                ie.Value0.ID.ContactFeature.TypeB  = ContactFeatureType.Vertex;

                ie.Value1.V = _polygonB.Vertices[i2];
                ie.Value1.ID.ContactFeature.IndexA = 0;
                ie.Value1.ID.ContactFeature.IndexB = (byte)i2;
                ie.Value1.ID.ContactFeature.TypeA  = ContactFeatureType.Face;
                ie.Value1.ID.ContactFeature.TypeB  = ContactFeatureType.Vertex;

                if (_front)
                {
                    rf.i1     = 0;
                    rf.i2     = 1;
                    rf.v1     = _v1;
                    rf.v2     = _v2;
                    rf.Normal = _normal1;
                }
                else
                {
                    rf.i1     = 1;
                    rf.i2     = 0;
                    rf.v1     = _v2;
                    rf.v2     = _v1;
                    rf.Normal = -_normal1;
                }
            }
            else
            {
                manifold.Type = ManifoldType.FaceB;

                ie.Value0.V = _v1;
                ie.Value0.ID.ContactFeature.IndexA = 0;
                ie.Value0.ID.ContactFeature.IndexB = (byte)primaryAxis.Index;
                ie.Value0.ID.ContactFeature.TypeA  = ContactFeatureType.Vertex;
                ie.Value0.ID.ContactFeature.TypeB  = ContactFeatureType.Face;

                ie.Value1.V = _v2;
                ie.Value1.ID.ContactFeature.IndexA = 0;
                ie.Value1.ID.ContactFeature.IndexB = (byte)primaryAxis.Index;
                ie.Value1.ID.ContactFeature.TypeA  = ContactFeatureType.Vertex;
                ie.Value1.ID.ContactFeature.TypeB  = ContactFeatureType.Face;

                rf.i1     = primaryAxis.Index;
                rf.i2     = rf.i1 + 1 < _polygonB.Count ? rf.i1 + 1 : 0;
                rf.v1     = _polygonB.Vertices[rf.i1];
                rf.v2     = _polygonB.Vertices[rf.i2];
                rf.Normal = _polygonB.Normals[rf.i1];
            }

            rf.SideNormal1 = new Vector2(rf.Normal.Y, -rf.Normal.X);
            rf.SideNormal2 = -rf.SideNormal1;
            rf.SideOffset1 = Vector2.Dot(rf.SideNormal1, rf.v1);
            rf.SideOffset2 = Vector2.Dot(rf.SideNormal2, rf.v2);

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

            // Clip to box side 1
            np = Collision.ClipSegmentToLine(out clipPoints1, ref ie, rf.SideNormal1, rf.SideOffset1, rf.i1);

            if (np < Settings.MaxManifoldPoints)
            {
                return;
            }

            // Clip to negative box side 1
            np = Collision.ClipSegmentToLine(out clipPoints2, ref clipPoints1, rf.SideNormal2, rf.SideOffset2, rf.i2);

            if (np < Settings.MaxManifoldPoints)
            {
                return;
            }

            // Now clipPoints2 contains the clipped points.
            if (primaryAxis.Type == EPAxisType.EdgeA)
            {
                manifold.LocalNormal = rf.Normal;
                manifold.LocalPoint  = rf.v1;
            }
            else
            {
                manifold.LocalNormal = polygonB.Normals[rf.i1];
                manifold.LocalPoint  = polygonB.Vertices[rf.i1];
            }

            int pointCount = 0;

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

                if (separation <= _radius)
                {
                    ManifoldPoint cp = manifold.Points[pointCount];

                    if (primaryAxis.Type == EPAxisType.EdgeA)
                    {
                        cp.LocalPoint = MathUtils.MulT(ref _xf, clipPoints2[i].V);
                        cp.Id         = clipPoints2[i].ID;
                    }
                    else
                    {
                        cp.LocalPoint = clipPoints2[i].V;
                        cp.Id.ContactFeature.TypeA  = clipPoints2[i].ID.ContactFeature.TypeB;
                        cp.Id.ContactFeature.TypeB  = clipPoints2[i].ID.ContactFeature.TypeA;
                        cp.Id.ContactFeature.IndexA = clipPoints2[i].ID.ContactFeature.IndexB;
                        cp.Id.ContactFeature.IndexB = clipPoints2[i].ID.ContactFeature.IndexA;
                    }

                    manifold.Points[pointCount] = cp;
                    ++pointCount;
                }
            }

            manifold.PointCount = pointCount;
        }
Пример #4
0
        public static void Collide(ref Manifold manifold, EdgeShape edgeA, ref Transform xfA, PolygonShape polygonB, ref Transform xfB)
        {
            // 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
            bool    front;
            Vector2 lowerLimit, upperLimit;
            Vector2 normal;
            Vector2 normal0 = Vector2.Zero;
            Vector2 normal2 = Vector2.Zero;

            Transform xf = MathUtils.MulT(xfA, xfB);

            Vector2 centroidB = MathUtils.Mul(ref xf, polygonB.MassData.Centroid);

            Vector2 v0 = edgeA.Vertex0;
            Vector2 v1 = edgeA._vertex1;
            Vector2 v2 = edgeA._vertex2;
            Vector2 v3 = edgeA.Vertex3;

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

            Vector2 edge1 = v2 - v1;

            edge1.Normalize();
            Vector2 normal1 = new Vector2(edge1.Y, -edge1.X);

            GGame.Math.Fix64 offset1 = Vector2.Dot(normal1, centroidB - v1);
            GGame.Math.Fix64 offset0 = 0.0f, offset2 = 0.0f;
            bool             convex1 = false, convex2 = false;

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

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

            // Determine front or back collision. Determine collision normal limits.
            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
            Vector2[] normals  = new Vector2[Settings.MaxPolygonVertices];
            Vector2[] vertices = new Vector2[Settings.MaxPolygonVertices];
            int       count    = polygonB.Vertices.Count;

            for (int i = 0; i < polygonB.Vertices.Count; ++i)
            {
                vertices[i] = MathUtils.Mul(ref xf, polygonB.Vertices[i]);
                normals[i]  = MathUtils.Mul(xf.q, polygonB.Normals[i]);
            }

            GGame.Math.Fix64 radius = polygonB.Radius + edgeA.Radius;

            manifold.PointCount = 0;

            //Velcro: ComputeEdgeSeparation() was manually inlined here
            EPAxis edgeAxis;

            edgeAxis.Type       = EPAxisType.EdgeA;
            edgeAxis.Index      = front ? 0 : 1;
            edgeAxis.Separation = Settings.MaxFloat;

            for (int i = 0; i < count; ++i)
            {
                GGame.Math.Fix64 s = Vector2.Dot(normal, vertices[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 == EPAxisType.Unknown)
            {
                return;
            }

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

            //Velcro: ComputePolygonSeparation() was manually inlined here
            EPAxis polygonAxis;

            polygonAxis.Type       = EPAxisType.Unknown;
            polygonAxis.Index      = -1;
            polygonAxis.Separation = -Settings.MaxFloat;

            Vector2 perp = new Vector2(-normal.Y, normal.X);

            for (int i = 0; i < count; ++i)
            {
                Vector2 n = -normals[i];

                GGame.Math.Fix64 s1 = Vector2.Dot(n, vertices[i] - v1);
                GGame.Math.Fix64 s2 = Vector2.Dot(n, vertices[i] - v2);
                GGame.Math.Fix64 s  = Math.Min((float)s1, (float)s2);

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

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

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

            if (polygonAxis.Type != EPAxisType.Unknown && polygonAxis.Separation > radius)
            {
                return;
            }

            // Use hysteresis for jitter reduction.
            GGame.Math.Fix64 k_relativeTol = 0.98f;
            GGame.Math.Fix64 k_absoluteTol = 0.001f;

            EPAxis primaryAxis;

            if (polygonAxis.Type == EPAxisType.Unknown)
            {
                primaryAxis = edgeAxis;
            }
            else if (polygonAxis.Separation > k_relativeTol * edgeAxis.Separation + k_absoluteTol)
            {
                primaryAxis = polygonAxis;
            }
            else
            {
                primaryAxis = edgeAxis;
            }

            FixedArray2 <ClipVertex> ie = new FixedArray2 <ClipVertex>();
            ReferenceFace            rf;

            if (primaryAxis.Type == EPAxisType.EdgeA)
            {
                manifold.Type = ManifoldType.FaceA;

                // Search for the polygon normal that is most anti-parallel to the edge normal.
                int bestIndex = 0;
                GGame.Math.Fix64 bestValue = Vector2.Dot(normal, normals[0]);
                for (int i = 1; i < count; ++i)
                {
                    GGame.Math.Fix64 value = Vector2.Dot(normal, normals[i]);
                    if (value < bestValue)
                    {
                        bestValue = value;
                        bestIndex = i;
                    }
                }

                int i1 = bestIndex;
                int i2 = i1 + 1 < count ? i1 + 1 : 0;

                ie.Value0.V = vertices[i1];
                ie.Value0.ID.ContactFeature.IndexA = 0;
                ie.Value0.ID.ContactFeature.IndexB = (byte)i1;
                ie.Value0.ID.ContactFeature.TypeA  = ContactFeatureType.Face;
                ie.Value0.ID.ContactFeature.TypeB  = ContactFeatureType.Vertex;

                ie.Value1.V = vertices[i2];
                ie.Value1.ID.ContactFeature.IndexA = 0;
                ie.Value1.ID.ContactFeature.IndexB = (byte)i2;
                ie.Value1.ID.ContactFeature.TypeA  = ContactFeatureType.Face;
                ie.Value1.ID.ContactFeature.TypeB  = ContactFeatureType.Vertex;

                if (front)
                {
                    rf.i1     = 0;
                    rf.i2     = 1;
                    rf.v1     = v1;
                    rf.v2     = v2;
                    rf.Normal = normal1;
                }
                else
                {
                    rf.i1     = 1;
                    rf.i2     = 0;
                    rf.v1     = v2;
                    rf.v2     = v1;
                    rf.Normal = -normal1;
                }
            }
            else
            {
                manifold.Type = ManifoldType.FaceB;

                ie.Value0.V = v1;
                ie.Value0.ID.ContactFeature.IndexA = 0;
                ie.Value0.ID.ContactFeature.IndexB = (byte)primaryAxis.Index;
                ie.Value0.ID.ContactFeature.TypeA  = ContactFeatureType.Vertex;
                ie.Value0.ID.ContactFeature.TypeB  = ContactFeatureType.Face;

                ie.Value1.V = v2;
                ie.Value1.ID.ContactFeature.IndexA = 0;
                ie.Value1.ID.ContactFeature.IndexB = (byte)primaryAxis.Index;
                ie.Value1.ID.ContactFeature.TypeA  = ContactFeatureType.Vertex;
                ie.Value1.ID.ContactFeature.TypeB  = ContactFeatureType.Face;

                rf.i1     = primaryAxis.Index;
                rf.i2     = rf.i1 + 1 < count ? rf.i1 + 1 : 0;
                rf.v1     = vertices[rf.i1];
                rf.v2     = vertices[rf.i2];
                rf.Normal = normals[rf.i1];
            }

            rf.SideNormal1 = new Vector2(rf.Normal.Y, -rf.Normal.X);
            rf.SideNormal2 = -rf.SideNormal1;
            rf.SideOffset1 = Vector2.Dot(rf.SideNormal1, rf.v1);
            rf.SideOffset2 = Vector2.Dot(rf.SideNormal2, rf.v2);

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

            // Clip to box side 1
            np = Collision.ClipSegmentToLine(out clipPoints1, ref ie, rf.SideNormal1, rf.SideOffset1, rf.i1);

            if (np < Settings.MaxManifoldPoints)
            {
                return;
            }

            // Clip to negative box side 1
            np = Collision.ClipSegmentToLine(out clipPoints2, ref clipPoints1, rf.SideNormal2, rf.SideOffset2, rf.i2);

            if (np < Settings.MaxManifoldPoints)
            {
                return;
            }

            // Now clipPoints2 contains the clipped points.
            if (primaryAxis.Type == EPAxisType.EdgeA)
            {
                manifold.LocalNormal = rf.Normal;
                manifold.LocalPoint  = rf.v1;
            }
            else
            {
                manifold.LocalNormal = polygonB.Normals[rf.i1];
                manifold.LocalPoint  = polygonB.Vertices[rf.i1];
            }

            int pointCount = 0;

            for (int i = 0; i < Settings.MaxManifoldPoints; ++i)
            {
                GGame.Math.Fix64 separation = Vector2.Dot(rf.Normal, clipPoints2[i].V - rf.v1);

                if (separation <= radius)
                {
                    ManifoldPoint cp = manifold.Points[pointCount];

                    if (primaryAxis.Type == EPAxisType.EdgeA)
                    {
                        cp.LocalPoint = MathUtils.MulT(ref xf, clipPoints2[i].V);
                        cp.Id         = clipPoints2[i].ID;
                    }
                    else
                    {
                        cp.LocalPoint = clipPoints2[i].V;
                        cp.Id.ContactFeature.TypeA  = clipPoints2[i].ID.ContactFeature.TypeB;
                        cp.Id.ContactFeature.TypeB  = clipPoints2[i].ID.ContactFeature.TypeA;
                        cp.Id.ContactFeature.IndexA = clipPoints2[i].ID.ContactFeature.IndexB;
                        cp.Id.ContactFeature.IndexB = clipPoints2[i].ID.ContactFeature.IndexA;
                    }

                    manifold.Points[pointCount] = cp;
                    ++pointCount;
                }
            }

            manifold.PointCount = pointCount;
        }