public override void Update(GameSettings settings, GameTime gameTime)
        {
            Manifold manifold = new Manifold();
            Collision.Collision.CollidePolygons(ref manifold, _polygonA, ref _transformA, _polygonB, ref _transformB);

            Vector2 normal;
            FixedArray2<Vector2> points;
            ContactSolver.WorldManifold.Initialize(ref manifold, ref _transformA, _polygonA.Radius, ref _transformB, _polygonB.Radius, out normal, out points);

            DrawString("Point count = " + manifold.PointCount);

            DebugView.BeginCustomDraw(ref GameInstance.Projection, ref GameInstance.View);
            {
                Color color = new Color(0.9f, 0.9f, 0.9f);
                Vector2[] v = new Vector2[Settings.MaxPolygonVertices];
                for (int i = 0; i < _polygonA.Vertices.Count; ++i)
                {
                    v[i] = MathUtils.Mul(ref _transformA, _polygonA.Vertices[i]);
                }
                DebugView.DrawPolygon(v, _polygonA.Vertices.Count, color);

                for (int i = 0; i < _polygonB.Vertices.Count; ++i)
                {
                    v[i] = MathUtils.Mul(ref _transformB, _polygonB.Vertices[i]);
                }
                DebugView.DrawPolygon(v, _polygonB.Vertices.Count, color);
            }

            for (int i = 0; i < manifold.PointCount; ++i)
            {
                DebugView.DrawPoint(points[i], 0.1f, new Color(0.9f, 0.3f, 0.3f));
                DrawString(points[i].ToString());
            }
            DebugView.EndCustomDraw();
        }

        public override void Update(GameSettings settings, GameTime gameTime)
        {
            Manifold manifold = new Manifold();
            Collision.Collision.CollidePolygons(ref manifold, _polygonA, ref _transformA, _polygonB, ref _transformB);

            Vector2 normal;
            FixedArray2<Vector2> points;
            Collision.Collision.GetWorldManifold(ref manifold, ref _transformA, _polygonA.Radius,
                                                 ref _transformB, _polygonB.Radius, out normal, out points);

            DebugView.DrawString(50, TextLine, "Point count = {0:n0}", manifold.PointCount);
            TextLine += 15;
            DebugView.BeginCustomDraw();
            {
                Color color = new Color(0.9f, 0.9f, 0.9f);
                Vector2[] v = new Vector2[Settings.MaxPolygonVertices];
                for (int i = 0; i < _polygonA.Vertices.Count; ++i)
                {
                    v[i] = MathUtils.Multiply(ref _transformA, _polygonA.Vertices[i]);
                }
                DebugView.DrawPolygon(v, _polygonA.Vertices.Count, color);

                for (int i = 0; i < _polygonB.Vertices.Count; ++i)
                {
                    v[i] = MathUtils.Multiply(ref _transformB, _polygonB.Vertices[i]);
                }
                DebugView.DrawPolygon(v, _polygonB.Vertices.Count, color);
            }

            for (int i = 0; i < manifold.PointCount; ++i)
            {
                DebugView.DrawPoint(points[i], 0.1f, new Color(0.9f, 0.3f, 0.3f));
            }
            DebugView.EndCustomDraw();
        }

        private void PreSolve(Contact contact, ref Manifold oldManifold)
        {
            if ((Flags & DebugViewFlags.ContactPoints) == DebugViewFlags.ContactPoints)
            {
                Manifold manifold = contact.Manifold;

                if (manifold.PointCount == 0)
                    return;

                Fixture fixtureA = contact.FixtureA;

                FixedArray2<PointState> state1, state2;
                Collision.Collision.GetPointStates(out state1, out state2, ref oldManifold, ref manifold);

                FixedArray2<Vector2> points;
                Vector2 normal;
                contact.GetWorldManifold(out normal, out points);

                for (int i = 0; i < manifold.PointCount && _pointCount < MaxContactPoints; ++i)
                {
                    if (fixtureA == null)
                        _points[i] = new ContactPoint();

                    ContactPoint cp = _points[_pointCount];
                    cp.Position = points[i];
                    cp.Normal = normal;
                    cp.State = state2[i];
                    _points[_pointCount] = cp;
                    ++_pointCount;
                }
            }
        }

        protected override void PreSolve(Contact contact, ref Manifold oldManifold)
        {
            base.PreSolve(contact, ref oldManifold);

            Fixture fixtureA = contact.FixtureA;
            Fixture fixtureB = contact.FixtureB;

            if (fixtureA == _platform)
            {
                contact.TangentSpeed = 5.0f;
            }

            if (fixtureB == _platform)
            {
                contact.TangentSpeed = -5.0f;
            }
        }

        public bool Intersect(Fixture fixture, ref Vector2 point, out Feature result)
        {
            result = new Feature();

            Manifold manifold = new Manifold();
            Vector2 normal;
            FixedArray2<Vector2> points;

            Transform transformB = new Transform();
            transformB.Set(Vector2.Zero, 0);
            circle.Position = point;

            PolygonShape polygonShape = fixture.Shape as PolygonShape;

            switch (fixture.ShapeType)
            {
                case ShapeType.Polygon:
                    Collision.Collision.CollidePolygonAndCircle(ref manifold, polygonShape, ref fixture.Body.Xf, circle, ref transformB);
                    ContactSolver.WorldManifold.Initialize(ref manifold, ref fixture.Body.Xf, polygonShape.Radius, ref transformB, circle.Radius, out normal, out points);

                    result.Distance = (point-(fixture.Body.Position + new Vector2(1.5f, 1.5f))).Length();

                    break;
                default:
                    throw new ArgumentOutOfRangeException();
            }

            //result.Position = points[0];
            //result.Normal = normal;
            //result.Distance = 1f;

            bool intersects = manifold.PointCount >= 1;//collision.Intersect(ref localPoint,  out result);

            if (intersects)
            {
                result.Position = points[0];
                result.Normal = normal;
                result.Distance *= -1;

                //Vector2 collisionPoint = points[0];
                //result.Position = Body.GetWorldPoint(ref collisionPoint);
                //result.Normal = Body.GetWorldVector(ref normal);
            }

            return intersects;
        }

        protected override void PreSolve(Contact contact, ref Manifold oldManifold)
        {
            base.PreSolve(contact, ref oldManifold);

            Fixture fixtureA = contact.FixtureA;
            Fixture fixtureB = contact.FixtureB;

            if (fixtureA != _platform && fixtureA != _character)
            {
                return;
            }

            if (fixtureB != _platform && fixtureB != _character)
            {
                return;
            }

            Vector2 position = _character.Body.Position;

            if (position.Y < _top + _radius - 3.0f * Settings.LinearSlop)
            {
                contact.Enabled = false;
            }
        }

        /// <summary>
        /// Collide and edge and polygon. This uses the SAT and clipping to produce up to 2 contact points.
        /// Edge adjacency is handle to produce locally valid contact points and normals. This is intended
        /// to allow the polygon to slide smoothly over an edge chain.
        ///
        /// Algorithm
        /// 1. Classify front-side or back-side collision with edge.
        /// 2. Compute separation
        /// 3. Process adjacent edges
        /// 4. Classify adjacent edge as convex, flat, null, or concave
        /// 5. Skip null or concave edges. Concave edges get a separate manifold.
        /// 6. If the edge is flat, compute contact points as normal. Discard boundary points.
        /// 7. If the edge is convex, compute it's separation.
        /// 8. Use the minimum separation of up to three edges. If the minimum separation
        ///    is not the primary edge, return.
        /// 9. If the minimum separation is the primary edge, compute the contact points and return.
        /// </summary>
        /// <param name="manifold">The manifold.</param>
        /// <param name="edgeA">The edge A.</param>
        /// <param name="xfA">The xf A.</param>
        /// <param name="polygonB_in">The polygon b_in.</param>
        /// <param name="xfB">The xf B.</param>
        public static void CollideEdgeAndPolygon(ref Manifold manifold,
                                                 EdgeShape edgeA, ref Transform xfA,
                                                 PolygonShape polygonB_in, ref Transform xfB)
        {
            manifold.PointCount = 0;

            Transform xf;
            MathUtils.MultiplyT(ref xfA, ref xfB, out xf);

            // Create a polygon for edge shape A
            s_polygonA.SetAsEdge(edgeA.Vertex1, edgeA.Vertex2);

            // Build polygonB in frame A
            s_polygonB.Radius = polygonB_in.Radius;
            s_polygonB.Centroid = MathUtils.Multiply(ref xf, polygonB_in.Centroid);
            s_polygonB.Vertices = new Vertices(polygonB_in.Vertices.Count);
            s_polygonB.Normals = new Vertices(polygonB_in.Vertices.Count);
            for (int i = 0; i < polygonB_in.Vertices.Count; ++i)
            {
                s_polygonB.Vertices.Add(MathUtils.Multiply(ref xf, polygonB_in.Vertices[i]));
                s_polygonB.Normals.Add(MathUtils.Multiply(ref xf.R, polygonB_in.Normals[i]));
            }

            float totalRadius = s_polygonA.Radius + s_polygonB.Radius;

            // Edge geometry
            Vector2 v1 = edgeA.Vertex1;
            Vector2 v2 = edgeA.Vertex2;
            Vector2 e = v2 - v1;
            Vector2 edgeNormal = new Vector2(e.Y, -e.X);
            edgeNormal.Normalize();

            // Determine side
            bool isFrontSide = Vector2.Dot(edgeNormal, s_polygonB.Centroid - v1) >= 0.0f;
            if (isFrontSide == false)
            {
                edgeNormal = -edgeNormal;
            }

            // Compute primary separating axis
            EPAxis edgeAxis;

            ComputeEdgeSeperation(ref v1, ref edgeNormal, s_polygonB, out edgeAxis);

            if (edgeAxis.Separation > totalRadius)
            {
                // Shapes are separated
                return;
            }

            // Classify adjacent edges
            FixedArray2<EdgeType> types = new FixedArray2<EdgeType>();
            //types[0] = EdgeType.Isolated;
            //types[1] = EdgeType.Isolated;
            if (edgeA.HasVertex0)
            {
                Vector2 v0 = edgeA.Vertex0;
                float s = Vector2.Dot(edgeNormal, v0 - v1);

                if (s > 0.1f * Settings.LinearSlop)
                {
                    types[0] = EdgeType.Concave;
                }
                else if (s >= -0.1f * Settings.LinearSlop)
                {
                    types[0] = EdgeType.Flat;
                }
                else
                {
                    types[0] = EdgeType.Convex;
                }
            }

            if (edgeA.HasVertex3)
            {
                Vector2 v3 = edgeA.Vertex3;
                float s = Vector2.Dot(edgeNormal, v3 - v2);
                if (s > 0.1f * Settings.LinearSlop)
//.........这里部分代码省略.........

 protected virtual void PreSolve(Contact contact, ref Manifold oldManifold)
 {
 }

        public void PreSolve(Contact contact, ref Manifold manifold)
        {
            EventSystem eventSystem = (EventSystem)_systemManager.getSystem(SystemType.Event);
            Fixture fixtureA = contact.FixtureA;
            Fixture fixtureB = contact.FixtureB;
            int playerId = PlayerSystem.PLAYER_ID;
            int entityA = (int)fixtureA.Body.UserData;
            int entityB = (int)fixtureB.Body.UserData;
            string levelUid = LevelSystem.currentLevelUid;

            // Check for custom collision filters
            bool fixtureAIgnoresEntityB = fixtureA.IsIgnoredEntity(entityB);
            bool fixtureBIgnoresEntityA = fixtureB.IsIgnoredEntity(entityA);
            if (fixtureAIgnoresEntityB)
                contact.Enabled = false;
            else if (fixtureBIgnoresEntityA)
                contact.Enabled = false;

            // Check for item pickup
            if (contact.IsTouching() && (entityA == playerId || entityB == playerId))
            {
                int itemEntityId = entityA == playerId ? entityB : entityA;
                Fixture fixture = entityA == playerId ? fixtureB : fixtureA;
                ItemComponent itemComponent = _entityManager.getComponent(levelUid, itemEntityId, ComponentType.Item) as ItemComponent;

                if (itemComponent != null)
                {
                    contact.Enabled = false;
                    if (itemComponent.state.inWorld)
                    {
                        InventoryComponent playerInventory = _entityManager.getComponent(levelUid, playerId, ComponentType.Inventory) as InventoryComponent;
                        EquipmentSystem equipmentSystem = _systemManager.getSystem(SystemType.Equipment) as EquipmentSystem;

                        equipmentSystem.addInventoryItem(playerInventory, itemComponent);
                        itemComponent.state.inWorld = false;
                        _bodiesToRemove.Add(fixture.Body);
                        _entityManager.killEntity(levelUid, itemEntityId);
                        eventSystem.postEvent(new GameEvent(GameEventType.OnItemPickedUp, itemEntityId));
                    }
                }
            }
        }

        /// <summary>
        /// Compute the collision manifold between two polygons.
        /// </summary>
        /// <param name="manifold">The manifold.</param>
        /// <param name="polyA">The poly A.</param>
        /// <param name="transformA">The transform A.</param>
        /// <param name="polyB">The poly B.</param>
        /// <param name="transformB">The transform B.</param>
        public static void CollidePolygons(ref Manifold manifold,
                                           PolygonShape polyA, ref Transform transformA,
                                           PolygonShape polyB, ref Transform transformB)
        {
            manifold.PointCount = 0;
            float totalRadius = polyA.Radius + polyB.Radius;

            int edgeA = 0;
            float separationA = FindMaxSeparation(out edgeA, polyA, ref transformA, polyB, ref transformB);
            if (separationA > totalRadius)
                return;

            int edgeB = 0;
            float separationB = FindMaxSeparation(out edgeB, polyB, ref transformB, polyA, ref transformA);
            if (separationB > totalRadius)
                return;

            PolygonShape poly1; // reference polygon
            PolygonShape poly2; // incident polygon
            Transform xf1, xf2;
            int edge1; // reference edge
            bool 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 = transformB;
                xf2 = transformA;
                edge1 = edgeB;
                manifold.Type = ManifoldType.FaceB;
                flip = true;
            }
            else
            {
                poly1 = polyA;
                poly2 = polyB;
                xf1 = transformA;
                xf2 = transformB;
                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;

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

            Vector2 v11 = poly1.Vertices[iv1];
            Vector2 v12 = poly1.Vertices[iv2];

            float localTangentX = v12.X - v11.X;
            float localTangentY = v12.Y - v11.Y;

            float factor = 1f / (float)Math.Sqrt(localTangentX * localTangentX + localTangentY * localTangentY);
            localTangentX = localTangentX * factor;
            localTangentY = localTangentY * factor;

            Vector2 localNormal = new Vector2(localTangentY, -localTangentX);
            Vector2 planePoint = 0.5f * (v11 + v12);

            Vector2 tangent = new Vector2(xf1.R.Col1.X * localTangentX + xf1.R.Col2.X * localTangentY,
                                          xf1.R.Col1.Y * localTangentX + xf1.R.Col2.Y * localTangentY);
            float normalx = tangent.Y;
            float normaly = -tangent.X;

            v11 = new Vector2(xf1.Position.X + xf1.R.Col1.X * v11.X + xf1.R.Col2.X * v11.Y,
                              xf1.Position.Y + xf1.R.Col1.Y * v11.X + xf1.R.Col2.Y * v11.Y);
            v12 = new Vector2(xf1.Position.X + xf1.R.Col1.X * v12.X + xf1.R.Col2.X * v12.Y,
                              xf1.Position.Y + xf1.R.Col1.Y * v12.X + xf1.R.Col2.Y * v12.Y);

            // Face offset.
            float frontOffset = normalx * v11.X + normaly * v11.Y;

            // Side offsets, extended by polytope skin thickness.
            float sideOffset1 = -(tangent.X * v11.X + tangent.Y * v11.Y) + totalRadius;
            float sideOffset2 = tangent.X * v12.X + tangent.Y * v12.Y + totalRadius;

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

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

            if (np < 2)
//.........这里部分代码省略.........

        /// Compute the collision manifold between two circles.
        public static void CollideCircles(ref Manifold manifold,
                                          CircleShape circleA, ref Transform xfA,
                                          CircleShape circleB, ref Transform xfB)
        {
            manifold.PointCount = 0;

            float pAx = xfA.Position.X + xfA.R.Col1.X * circleA.Position.X + xfA.R.Col2.X * circleA.Position.Y;
            float pAy = xfA.Position.Y + xfA.R.Col1.Y * circleA.Position.X + xfA.R.Col2.Y * circleA.Position.Y;
            float pBx = xfB.Position.X + xfB.R.Col1.X * circleB.Position.X + xfB.R.Col2.X * circleB.Position.Y;
            float pBy = xfB.Position.Y + xfB.R.Col1.Y * circleB.Position.X + xfB.R.Col2.Y * circleB.Position.Y;

            float distSqr = (pBx - pAx) * (pBx - pAx) + (pBy - pAy) * (pBy - pAy);
            float radius = circleA.Radius + circleB.Radius;
            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>
        /// Evaluate the manifold with supplied transforms. This assumes
        /// modest motion from the original state. This does not change the
        /// point count, impulses, etc. The radii must come from the Shapes
        /// that generated the manifold.
        /// </summary>
        /// <param name="manifold">The manifold.</param>
        /// <param name="transformA">The transform for A.</param>
        /// <param name="radiusA">The radius for A.</param>
        /// <param name="transformB">The transform for B.</param>
        /// <param name="radiusB">The radius for B.</param>
        /// <param name="normal">World vector pointing from A to B</param>
        /// <param name="points">Torld contact point (point of intersection).</param>
        public static void GetWorldManifold(ref Manifold manifold,
                                            ref Transform transformA, float radiusA,
                                            ref Transform transformB, float radiusB, out Vector2 normal,
                                            out FixedArray2<Vector2> points)
        {
            points = new FixedArray2<Vector2>();
            normal = Vector2.Zero;

            if (manifold.PointCount == 0)
            {
                normal = Vector2.UnitY;
                return;
            }

            switch (manifold.Type)
            {
                case ManifoldType.Circles:
                    {
                        Vector2 tmp = manifold.Points[0].LocalPoint;
                        float pointAx = transformA.Position.X + transformA.R.Col1.X * manifold.LocalPoint.X +
                                        transformA.R.Col2.X * manifold.LocalPoint.Y;

                        float pointAy = transformA.Position.Y + transformA.R.Col1.Y * manifold.LocalPoint.X +
                                        transformA.R.Col2.Y * manifold.LocalPoint.Y;

                        float pointBx = transformB.Position.X + transformB.R.Col1.X * tmp.X +
                                        transformB.R.Col2.X * tmp.Y;

                        float pointBy = transformB.Position.Y + transformB.R.Col1.Y * tmp.X +
                                        transformB.R.Col2.Y * tmp.Y;

                        normal.X = 1;
                        normal.Y = 0;

                        float result = (pointAx - pointBx) * (pointAx - pointBx) +
                                       (pointAy - pointBy) * (pointAy - pointBy);
                        if (result > Settings.Epsilon * Settings.Epsilon)
                        {
                            float tmpNormalx = pointBx - pointAx;
                            float tmpNormaly = pointBy - pointAy;
                            float factor = 1f / (float)Math.Sqrt(tmpNormalx * tmpNormalx + tmpNormaly * tmpNormaly);
                            normal.X = tmpNormalx * factor;
                            normal.Y = tmpNormaly * factor;
                        }

                        Vector2 c = Vector2.Zero;
                        c.X = (pointAx + radiusA * normal.X) + (pointBx - radiusB * normal.X);
                        c.Y = (pointAy + radiusA * normal.Y) + (pointBy - radiusB * normal.Y);

                        points[0] = 0.5f * c;
                    }
                    break;

                case ManifoldType.FaceA:
                    {
                        normal.X = transformA.R.Col1.X * manifold.LocalNormal.X +
                                   transformA.R.Col2.X * manifold.LocalNormal.Y;
                        normal.Y = transformA.R.Col1.Y * manifold.LocalNormal.X +
                                   transformA.R.Col2.Y * manifold.LocalNormal.Y;

                        float planePointx = transformA.Position.X + transformA.R.Col1.X * manifold.LocalPoint.X +
                                            transformA.R.Col2.X * manifold.LocalPoint.Y;

                        float planePointy = transformA.Position.Y + transformA.R.Col1.Y * manifold.LocalPoint.X +
                                            transformA.R.Col2.Y * manifold.LocalPoint.Y;

                        for (int i = 0; i < manifold.PointCount; ++i)
                        {
                            Vector2 tmp = manifold.Points[i].LocalPoint;

                            float clipPointx = transformB.Position.X + transformB.R.Col1.X * tmp.X +
                                               transformB.R.Col2.X * tmp.Y;

                            float clipPointy = transformB.Position.Y + transformB.R.Col1.Y * tmp.X +
                                               transformB.R.Col2.Y * tmp.Y;

                            float value = (clipPointx - planePointx) * normal.X + (clipPointy - planePointy) * normal.Y;

                            Vector2 c = Vector2.Zero;
                            c.X = (clipPointx + (radiusA - value) * normal.X) + (clipPointx - radiusB * normal.X);
                            c.Y = (clipPointy + (radiusA - value) * normal.Y) + (clipPointy - radiusB * normal.Y);

                            points[i] = 0.5f * c;
                        }
                    }
                    break;

//.........这里部分代码省略.........

        /// <summary>
        /// Compute contact points for edge versus circle.
        /// This accounts for edge connectivity.
        /// </summary>
        /// <param name="manifold">The manifold.</param>
        /// <param name="edgeA">The edge A.</param>
        /// <param name="transformA">The transform A.</param>
        /// <param name="circleB">The circle B.</param>
        /// <param name="transformB">The transform B.</param>
        public static void CollideEdgeAndCircle(ref Manifold manifold,
                                                EdgeShape edgeA, ref Transform transformA,
                                                CircleShape circleB, ref Transform transformB)
        {
            manifold.PointCount = 0;

            // Compute circle in frame of edge
            Vector2 Q = MathUtils.MultiplyT(ref transformA, MathUtils.Multiply(ref transformB, ref circleB._position));

            Vector2 A = edgeA.Vertex1, B = edgeA.Vertex2;
            Vector2 e = B - A;

            // Barycentric coordinates
            float u = Vector2.Dot(e, B - Q);
            float v = Vector2.Dot(e, Q - A);

            float radius = edgeA.Radius + circleB.Radius;

            ContactFeature cf;
            cf.IndexB = 0;
            cf.TypeB = (byte)ContactFeatureType.Vertex;

            Vector2 P, d;

            // Region A
            if (v <= 0.0f)
            {
                P = A;
                d = Q - P;
                float dd;
                Vector2.Dot(ref d, ref d, out dd);
                if (dd > radius * radius)
                {
                    return;
                }

                // Is there an edge connected to A?
                if (edgeA.HasVertex0)
                {
                    Vector2 A1 = edgeA.Vertex0;
                    Vector2 B1 = A;
                    Vector2 e1 = B1 - A1;
                    float u1 = Vector2.Dot(e1, B1 - Q);

                    // Is the circle in Region AB of the previous edge?
                    if (u1 > 0.0f)
                    {
                        return;
                    }
                }

                cf.IndexA = 0;
                cf.TypeA = (byte)ContactFeatureType.Vertex;
                manifold.PointCount = 1;
                manifold.Type = ManifoldType.Circles;
                manifold.LocalNormal = Vector2.Zero;
                manifold.LocalPoint = P;
                ManifoldPoint mp = new ManifoldPoint();
                mp.Id.Key = 0;
                mp.Id.Features = cf;
                mp.LocalPoint = circleB.Position;
                manifold.Points[0] = mp;
                return;
            }

            // Region B
            if (u <= 0.0f)
            {
                P = B;
                d = Q - P;
                float dd;
                Vector2.Dot(ref d, ref d, out dd);
                if (dd > radius * radius)
                {
                    return;
                }

                // Is there an edge connected to B?
                if (edgeA.HasVertex3)
                {
                    Vector2 B2 = edgeA.Vertex3;
                    Vector2 A2 = B;
                    Vector2 e2 = B2 - A2;
                    float v2 = Vector2.Dot(e2, Q - A2);

                    // Is the circle in Region AB of the next edge?
                    if (v2 > 0.0f)
                    {
                        return;
                    }
                }
//.........这里部分代码省略.........

        /// <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 radius = circleA.Radius + circleB.Radius;
            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>
 /// Collides and edge and a polygon, taking into account edge adjacency.
 /// </summary>
 /// <param name="manifold">The manifold.</param>
 /// <param name="edgeA">The edge A.</param>
 /// <param name="xfA">The xf A.</param>
 /// <param name="polygonB">The polygon B.</param>
 /// <param name="xfB">The xf B.</param>
 public static void CollideEdgeAndPolygon(ref Manifold manifold, EdgeShape edgeA, ref Transform xfA, PolygonShape polygonB, ref Transform xfB)
 {
     EPCollider collider = new EPCollider();
     collider.Collide(ref manifold, edgeA, ref xfA, polygonB, ref xfB);
 }

        /// <summary>
        /// Evaluate the manifold with supplied transforms. This assumes
        /// modest motion from the original state. This does not change the
        /// point count, impulses, etc. The radii must come from the Shapes
        /// that generated the manifold.
        /// </summary>
        /// <param name="manifold">The manifold.</param>
        /// <param name="transformA">The transform for A.</param>
        /// <param name="radiusA">The radius for A.</param>
        /// <param name="transformB">The transform for B.</param>
        /// <param name="radiusB">The radius for B.</param>
        public WorldManifold(ref Manifold manifold,
                             ref Transform transformA, float radiusA,
                             ref Transform transformB, float radiusB)
        {
            Points = new FixedArray2<Vector2>();

            if (manifold.PointCount == 0)
            {
                Normal = Vector2.UnitY;
                return;
            }

            switch (manifold.Type)
            {
                case ManifoldType.Circles:
                    {
                        Vector2 pointA = MathUtils.Multiply(ref transformA, manifold.LocalPoint);
                        Vector2 pointB = MathUtils.Multiply(ref transformB, manifold.Points[0].LocalPoint);
                        Normal = new Vector2(1.0f, 0.0f);
                        if (Vector2.DistanceSquared(pointA, pointB) > Settings.Epsilon * Settings.Epsilon)
                        {
                            Normal = pointB - pointA;
                            Normal.Normalize();
                        }

                        Vector2 cA = pointA + radiusA * Normal;
                        Vector2 cB = pointB - radiusB * Normal;
                        Points[0] = 0.5f * (cA + cB);
                    }
                    break;

                case ManifoldType.FaceA:
                    {
                        Normal = MathUtils.Multiply(ref transformA.R, manifold.LocalNormal);
                        Vector2 planePoint = MathUtils.Multiply(ref transformA, manifold.LocalPoint);

                        for (int i = 0; i < manifold.PointCount; ++i)
                        {
                            Vector2 clipPoint = MathUtils.Multiply(ref transformB, manifold.Points[i].LocalPoint);
                            Vector2 cA = clipPoint + (radiusA - Vector2.Dot(clipPoint - planePoint, Normal)) * Normal;
                            Vector2 cB = clipPoint - radiusB * Normal;
                            Points[i] = 0.5f * (cA + cB);
                        }
                    }
                    break;

                case ManifoldType.FaceB:
                    {
                        Normal = MathUtils.Multiply(ref transformB.R, manifold.LocalNormal);
                        Vector2 planePoint = MathUtils.Multiply(ref transformB, manifold.LocalPoint);

                        for (int i = 0; i < manifold.PointCount; ++i)
                        {
                            Vector2 clipPoint = MathUtils.Multiply(ref transformA, manifold.Points[i].LocalPoint);
                            Vector2 cA = clipPoint - radiusA * Normal;
                            Vector2 cB = clipPoint + (radiusB - Vector2.Dot(clipPoint - planePoint, Normal)) * Normal;
                            Points[i] = 0.5f * (cA + cB);
                        }
                        // Ensure normal points from A to B.
                        Normal *= -1;
                    }
                    break;
                default:
                    Normal = Vector2.UnitY;
                    break;
            }
        }

        /// <summary>
        /// Compute the collision manifold between two polygons.
        /// </summary>
        /// <param name="manifold">The manifold.</param>
        /// <param name="polyA">The poly A.</param>
        /// <param name="transformA">The transform A.</param>
        /// <param name="polyB">The poly B.</param>
        /// <param name="transformB">The transform B.</param>
        public static void CollidePolygons(ref Manifold manifold,
                                           PolygonShape polyA, ref Transform transformA,
                                           PolygonShape polyB, ref Transform transformB)
        {
            manifold.PointCount = 0;
            float totalRadius = polyA.Radius + polyB.Radius;

            int edgeA = 0;
            float separationA = FindMaxSeparation(out edgeA, polyA, ref transformA, polyB, ref transformB);
            if (separationA > totalRadius)
                return;

            int edgeB = 0;
            float separationB = FindMaxSeparation(out edgeB, polyB, ref transformB, polyA, ref transformA);
            if (separationB > totalRadius)
                return;

            PolygonShape poly1; // reference polygon
            PolygonShape poly2; // incident polygon
            Transform xf1, xf2;
            int edge1; // reference edge
            bool 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 = transformB;
                xf2 = transformA;
                edge1 = edgeB;
                manifold.Type = ManifoldType.FaceB;
                flip = true;
            }
            else
            {
                poly1 = polyA;
                poly2 = polyB;
                xf1 = transformA;
                xf2 = transformB;
                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;

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

            Vector2 v11 = poly1.Vertices[iv1];
            Vector2 v12 = poly1.Vertices[iv2];

            Vector2 localTangent = v12 - v11;
            localTangent.Normalize();

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

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

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

            // Face offset.
            float frontOffset = Vector2.Dot(normal, v11);

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

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

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

            if (np < 2)
                return;

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

            if (np < 2)
            {
//.........这里部分代码省略.........

        /// <summary>
        /// Compute the collision manifold between a polygon and a circle.
        /// </summary>
        /// <param name="manifold">The manifold.</param>
        /// <param name="polygonA">The polygon A.</param>
        /// <param name="transformA">The transform of A.</param>
        /// <param name="circleB">The circle B.</param>
        /// <param name="transformB">The transform of B.</param>
        public static void CollidePolygonAndCircle(ref Manifold manifold,
                                                   PolygonShape polygonA, ref Transform transformA,
                                                   CircleShape circleB, ref Transform transformB)
        {