Beispiel #1
0
 /// <summary>
 /// Get the contact manifold. Do not modify the manifold unless you understand the
 /// internals of Box2D.
 /// </summary>
 /// <param name="manifold">The manifold.</param>
 public void GetManifold(out Manifold manifold)
 {
     manifold = Manifold;
 }
Beispiel #2
0
        /// <summary>
        /// Evaluate this contact with your own manifold and transforms.
        /// </summary>
        /// <param name="manifold">The manifold.</param>
        /// <param name="transformA">The first transform.</param>
        /// <param name="transformB">The second transform.</param>
        private void Evaluate(ref Manifold manifold, ref Transform transformA, ref Transform transformB)
        {
            switch (_type)
            {
                case ContactType.Polygon:
                    Collision.Collision.CollidePolygons(ref manifold,
                                                        (PolygonShape)FixtureA.Shape, ref transformA,
                                                        (PolygonShape)FixtureB.Shape, ref transformB);
                    break;

                case ContactType.PolygonAndCircle:
                    Collision.Collision.CollidePolygonAndCircle(ref manifold,
                                                                (PolygonShape)FixtureA.Shape, ref transformA,
                                                                (CircleShape)FixtureB.Shape, ref transformB);
                    break;

                case ContactType.EdgeAndCircle:
                    Collision.Collision.CollideEdgeAndCircle(ref manifold,
                                                             (EdgeShape)FixtureA.Shape, ref transformA,
                                                             (CircleShape)FixtureB.Shape, ref transformB);
                    break;

                case ContactType.EdgeAndPolygon:
                    Collision.Collision.CollideEdgeAndPolygon(ref manifold,
                                                              (EdgeShape)FixtureA.Shape, ref transformA,
                                                              (PolygonShape)FixtureB.Shape, ref transformB);
                    break;

                case ContactType.LoopAndCircle:
                    LoopShape loop = (LoopShape)FixtureA.Shape;
                    loop.GetChildEdge(ref _edge, ChildIndexA);
                    Collision.Collision.CollideEdgeAndCircle(ref manifold, _edge, ref transformA,
                                                             (CircleShape)FixtureB.Shape, ref transformB);
                    break;

                case ContactType.LoopAndPolygon:
                    LoopShape loop2 = (LoopShape)FixtureA.Shape;
                    loop2.GetChildEdge(ref _edge, ChildIndexA);
                    Collision.Collision.CollideEdgeAndPolygon(ref manifold, _edge, ref transformA,
                                                              (PolygonShape)FixtureB.Shape, ref transformB);
                    break;

                case ContactType.Circle:
                    Collision.Collision.CollideCircles(ref manifold,
                                                       (CircleShape)FixtureA.Shape, ref transformA,
                                                       (CircleShape)FixtureB.Shape, ref transformB);
                    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;
                    }
                }

                cf.IndexA = 1;
                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 AB
            float den;
            Vector2.Dot(ref e, ref e, out den);
            Debug.Assert(den > 0.0f);
            P = (1.0f / den) * (u * A + v * B);
            d = Q - P;
            float dd2;
            Vector2.Dot(ref d, ref d, out dd2);
            if (dd2 > radius * radius)
            {
                return;
            }

            Vector2 n = new Vector2(-e.Y, e.X);
            if (Vector2.Dot(n, Q - A) < 0.0f)
            {
                n = new Vector2(-n.X, -n.Y);
            }
            n.Normalize();

            cf.IndexA = 0;
            cf.TypeA = (byte)ContactFeatureType.Face;
            manifold.PointCount = 1;
            manifold.Type = ManifoldType.FaceA;
            manifold.LocalNormal = n;
            manifold.LocalPoint = A;
            ManifoldPoint mp2 = new ManifoldPoint();
            mp2.Id.Key = 0;
            mp2.Id.Features = cf;
            mp2.LocalPoint = circleB.Position;
            manifold.Points[0] = mp2;
        }
        /// <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)
        {
            MathUtils.MultiplyT(ref xfA, ref xfB, out _xf);

            // Edge geometry
            _edgeA.V0 = edgeA.Vertex0;
            _edgeA.V1 = edgeA.Vertex1;
            _edgeA.V2 = edgeA.Vertex2;
            _edgeA.V3 = edgeA.Vertex3;
            Vector2 e = _edgeA.V2 - _edgeA.V1;

            // Normal points outwards in CCW order.
            _edgeA.Normal = new Vector2(e.Y, -e.X);
            _edgeA.Normal.Normalize();
            _edgeA.HasVertex0 = edgeA.HasVertex0;
            _edgeA.HasVertex3 = edgeA.HasVertex3;

            // Proxy for edge
            _proxyA.Vertices[0] = _edgeA.V1;
            _proxyA.Vertices[1] = _edgeA.V2;
            _proxyA.Normals[0] = _edgeA.Normal;
            _proxyA.Normals[1] = -_edgeA.Normal;
            _proxyA.Centroid = 0.5f * (_edgeA.V1 + _edgeA.V2);
            _proxyA.Count = 2;

            // Proxy for polygon
            _proxyB.Count = polygonB.Vertices.Count;
            _proxyB.Centroid = MathUtils.Multiply(ref _xf, ref polygonB.MassData.Centroid);
            for (int i = 0; i < polygonB.Vertices.Count; ++i)
            {
                _proxyB.Vertices[i] = MathUtils.Multiply(ref _xf, polygonB.Vertices[i]);
                _proxyB.Normals[i] = MathUtils.Multiply(ref _xf.R, polygonB.Normals[i]);
            }

            _radius = 2.0f * Settings.PolygonRadius;

            _limit11 = Vector2.Zero;
            _limit12 = Vector2.Zero;
            _limit21 = Vector2.Zero;
            _limit22 = Vector2.Zero;

            //Collide(ref manifold); inline start
            manifold.PointCount = 0;

            //ComputeAdjacency(); inline start
            Vector2 v0 = _edgeA.V0;
            Vector2 v1 = _edgeA.V1;
            Vector2 v2 = _edgeA.V2;
            Vector2 v3 = _edgeA.V3;

            // Determine allowable the normal regions based on adjacency.
            // Note: it may be possible that no normal is admissable.
            Vector2 centerB = _proxyB.Centroid;
            if (_edgeA.HasVertex0)
            {
                Vector2 e0 = v1 - v0;
                Vector2 e1 = v2 - v1;
                Vector2 n0 = new Vector2(e0.Y, -e0.X);
                Vector2 n1 = new Vector2(e1.Y, -e1.X);
                n0.Normalize();
                n1.Normalize();

                bool convex = MathUtils.Cross(n0, n1) >= 0.0f;
                bool front0 = Vector2.Dot(n0, centerB - v0) >= 0.0f;
                bool front1 = Vector2.Dot(n1, centerB - v1) >= 0.0f;

                if (convex)
                {
                    if (front0 || front1)
                    {
                        _limit11 = n1;
                        _limit12 = n0;
                    }
                    else
                    {
                        _limit11 = -n1;
                        _limit12 = -n0;
                    }
                }
                else
                {
                    if (front0 && front1)
                    {
                        _limit11 = n0;
                        _limit12 = n1;
                    }
                    else
                    {
                        _limit11 = -n0;
                        _limit12 = -n1;
                    }
                }
            }
            else
            {
                _limit11 = Vector2.Zero;
                _limit12 = Vector2.Zero;
            }

            if (_edgeA.HasVertex3)
            {
                Vector2 e1 = v2 - v1;
                Vector2 e2 = v3 - v2;
                Vector2 n1 = new Vector2(e1.Y, -e1.X);
                Vector2 n2 = new Vector2(e2.Y, -e2.X);
                n1.Normalize();
                n2.Normalize();

                bool convex = MathUtils.Cross(n1, n2) >= 0.0f;
                bool front1 = Vector2.Dot(n1, centerB - v1) >= 0.0f;
                bool front2 = Vector2.Dot(n2, centerB - v2) >= 0.0f;

                if (convex)
                {
                    if (front1 || front2)
                    {
                        _limit21 = n2;
                        _limit22 = n1;
                    }
                    else
                    {
                        _limit21 = -n2;
                        _limit22 = -n1;
                    }
                }
                else
                {
                    if (front1 && front2)
                    {
                        _limit21 = n1;
                        _limit22 = n2;
                    }
                    else
                    {
                        _limit21 = -n1;
                        _limit22 = -n2;
                    }
                }
            }
            else
            {
                _limit21 = Vector2.Zero;
                _limit22 = Vector2.Zero;
            }

            //ComputeAdjacency(); inline end

            //EPAxis edgeAxis = ComputeEdgeSeparation(); inline start
            EPAxis edgeAxis = ComputeEdgeSeparation();

            // If no valid normal can be found than this edge should not collide.
            // This can happen on the middle edge of a 3-edge zig-zag chain.
            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;
            }

            EPProxy proxy1;
            EPProxy proxy2;
            FixedArray2<ClipVertex> incidentEdge = new FixedArray2<ClipVertex>();
            if (primaryAxis.Type == EPAxisType.EdgeA)
            {
                proxy1 = _proxyA;
                proxy2 = _proxyB;
                manifold.Type = ManifoldType.FaceA;
            }
            else
            {
                proxy1 = _proxyB;
                proxy2 = _proxyA;
                manifold.Type = ManifoldType.FaceB;
            }

            int edge1 = primaryAxis.Index;

            FindIncidentEdge(ref incidentEdge, proxy1, primaryAxis.Index, proxy2);
            int count1 = proxy1.Count;

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

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

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

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

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

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

            // 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 < Settings.MaxManifoldPoints)
            {
                return;
            }

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

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

            // Now clipPoints2 contains the clipped points.
            if (primaryAxis.Type == EPAxisType.EdgeA)
            {
                manifold.LocalNormal = normal;
                manifold.LocalPoint = planePoint;
            }
            else
            {
                manifold.LocalNormal = MathUtils.MultiplyT(ref _xf.R, ref normal);
                manifold.LocalPoint = MathUtils.MultiplyT(ref _xf, ref planePoint);
            }

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

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

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

                    manifold.Points[pointCount] = cp;

                    ++pointCount;
                }
            }

            manifold.PointCount = pointCount;

            //Collide(ref manifold); inline end
        }
        /// 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;

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

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

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

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

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

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

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

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

                            points[i] = 0.5f * c;
                        }

                        // Ensure normal points from A to B.
                        normal *= -1;
                    }
                    break;

                default:
                    normal = Vector2.UnitY;
                    break;
            }
        }
        public static void GetPointStates(out FixedArray2<PointState> state1, out FixedArray2<PointState> state2,
                                          ref Manifold manifold1, ref Manifold manifold2)
        {
            state1 = new FixedArray2<PointState>();
            state2 = new FixedArray2<PointState>();

            // Detect persists and removes.
            for (int i = 0; i < manifold1.PointCount; ++i)
            {
                ContactID id = manifold1.Points[i].Id;

                state1[i] = PointState.Remove;

                for (int j = 0; j < manifold2.PointCount; ++j)
                {
                    if (manifold2.Points[j].Id.Key == id.Key)
                    {
                        state1[i] = PointState.Persist;
                        break;
                    }
                }
            }

            // Detect persists and adds.
            for (int i = 0; i < manifold2.PointCount; ++i)
            {
                ContactID id = manifold2.Points[i].Id;

                state2[i] = PointState.Add;

                for (int j = 0; j < manifold1.PointCount; ++j)
                {
                    if (manifold1.Points[j].Id.Key == id.Key)
                    {
                        state2[i] = PointState.Persist;
                        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];

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

            // Clip to negative box side 1
            np = 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)
            {
                Vector2 value = clipPoints2[i].V;
                float separation = normalx * value.X + normaly * value.Y - frontOffset;

                if (separation <= totalRadius)
                {
                    ManifoldPoint cp = manifold.Points[pointCount];
                    Vector2 tmp = clipPoints2[i].V;
                    float tmp1X = tmp.X - xf2.Position.X;
                    float tmp1Y = tmp.Y - xf2.Position.Y;
                    cp.LocalPoint.X = tmp1X * xf2.R.Col1.X + tmp1Y * xf2.R.Col1.Y;
                    cp.LocalPoint.Y = tmp1X * xf2.R.Col2.X + tmp1Y * xf2.R.Col2.Y;
                    cp.Id = clipPoints2[i].ID;

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

                    manifold.Points[pointCount] = cp;

                    ++pointCount;
                }
            }

            manifold.PointCount = pointCount;
        }
        /// <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)
        {
            manifold.PointCount = 0;

            // Compute circle position in the frame of the polygon.
            Vector2 c =
                new Vector2(
                    transformB.Position.X + transformB.R.Col1.X * circleB.Position.X +
                    transformB.R.Col2.X * circleB.Position.Y,
                    transformB.Position.Y + transformB.R.Col1.Y * circleB.Position.X +
                    transformB.R.Col2.Y * circleB.Position.Y);
            Vector2 cLocal =
                new Vector2(
                    (c.X - transformA.Position.X) * transformA.R.Col1.X +
                    (c.Y - transformA.Position.Y) * transformA.R.Col1.Y,
                    (c.X - transformA.Position.X) * transformA.R.Col2.X +
                    (c.Y - transformA.Position.Y) * transformA.R.Col2.Y);

            // Find the min separating edge.
            int normalIndex = 0;
            float separation = -Settings.MaxFloat;
            float radius = polygonA.Radius + circleB.Radius;
            int vertexCount = polygonA.Vertices.Count;

            for (int i = 0; i < vertexCount; ++i)
            {
                Vector2 value1 = polygonA.Normals[i];
                Vector2 value2 = cLocal - polygonA.Vertices[i];
                float s = value1.X * value2.X + value1.Y * value2.Y;

                if (s > radius)
                {
                    // Early out.
                    return;
                }

                if (s > separation)
                {
                    separation = s;
                    normalIndex = i;
                }
            }

            // Vertices that subtend the incident face.
            int vertIndex1 = normalIndex;
            int vertIndex2 = vertIndex1 + 1 < vertexCount ? vertIndex1 + 1 : 0;
            Vector2 v1 = polygonA.Vertices[vertIndex1];
            Vector2 v2 = polygonA.Vertices[vertIndex2];

            // If the center is inside the polygon ...
            if (separation < Settings.Epsilon)
            {
                manifold.PointCount = 1;
                manifold.Type = ManifoldType.FaceA;
                manifold.LocalNormal = polygonA.Normals[normalIndex];
                manifold.LocalPoint = 0.5f * (v1 + v2);

                ManifoldPoint p0 = manifold.Points[0];

                p0.LocalPoint = circleB.Position;
                p0.Id.Key = 0;

                manifold.Points[0] = p0;

                return;
            }

            // Compute barycentric coordinates
            float u1 = (cLocal.X - v1.X) * (v2.X - v1.X) + (cLocal.Y - v1.Y) * (v2.Y - v1.Y);
            float u2 = (cLocal.X - v2.X) * (v1.X - v2.X) + (cLocal.Y - v2.Y) * (v1.Y - v2.Y);

            if (u1 <= 0.0f)
            {
                float r = (cLocal.X - v1.X) * (cLocal.X - v1.X) + (cLocal.Y - v1.Y) * (cLocal.Y - v1.Y);
                if (r > radius * radius)
                {
                    return;
                }

                manifold.PointCount = 1;
                manifold.Type = ManifoldType.FaceA;
                manifold.LocalNormal = cLocal - v1;
                float factor = 1f /
                               (float)
                               Math.Sqrt(manifold.LocalNormal.X * manifold.LocalNormal.X +
                                         manifold.LocalNormal.Y * manifold.LocalNormal.Y);
                manifold.LocalNormal.X = manifold.LocalNormal.X * factor;
                manifold.LocalNormal.Y = manifold.LocalNormal.Y * factor;
                manifold.LocalPoint = v1;

                ManifoldPoint p0b = manifold.Points[0];

                p0b.LocalPoint = circleB.Position;
                p0b.Id.Key = 0;

                manifold.Points[0] = p0b;
            }
            else if (u2 <= 0.0f)
            {
                float r = (cLocal.X - v2.X) * (cLocal.X - v2.X) + (cLocal.Y - v2.Y) * (cLocal.Y - v2.Y);
                if (r > radius * radius)
                {
                    return;
                }

                manifold.PointCount = 1;
                manifold.Type = ManifoldType.FaceA;
                manifold.LocalNormal = cLocal - v2;
                float factor = 1f /
                               (float)
                               Math.Sqrt(manifold.LocalNormal.X * manifold.LocalNormal.X +
                                         manifold.LocalNormal.Y * manifold.LocalNormal.Y);
                manifold.LocalNormal.X = manifold.LocalNormal.X * factor;
                manifold.LocalNormal.Y = manifold.LocalNormal.Y * factor;
                manifold.LocalPoint = v2;

                ManifoldPoint p0c = manifold.Points[0];

                p0c.LocalPoint = circleB.Position;
                p0c.Id.Key = 0;

                manifold.Points[0] = p0c;
            }
            else
            {
                Vector2 faceCenter = 0.5f * (v1 + v2);
                Vector2 value1 = cLocal - faceCenter;
                Vector2 value2 = polygonA.Normals[vertIndex1];
                float separation2 = value1.X * value2.X + value1.Y * value2.Y;
                if (separation2 > radius)
                {
                    return;
                }

                manifold.PointCount = 1;
                manifold.Type = ManifoldType.FaceA;
                manifold.LocalNormal = polygonA.Normals[vertIndex1];
                manifold.LocalPoint = faceCenter;

                ManifoldPoint p0d = manifold.Points[0];

                p0d.LocalPoint = circleB.Position;
                p0d.Id.Key = 0;

                manifold.Points[0] = p0d;
            }
        }