Moved FarseerPhysicsEngine and Samples into FarseerPhysics

FarseerPhysicsEngine/Collision/Collision.cs

@@ -1,1945 +0,0 @@
-/*
-* Farseer Physics Engine based on Box2D.XNA port:
-* Copyright (c) 2010 Ian Qvist
-* 
-* Box2D.XNA port of Box2D:
-* Copyright (c) 2009 Brandon Furtwangler, Nathan Furtwangler
-*
-* Original source Box2D:
-* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com 
-* 
-* This software is provided 'as-is', without any express or implied 
-* warranty.  In no event will the authors be held liable for any damages 
-* arising from the use of this software. 
-* Permission is granted to anyone to use this software for any purpose, 
-* including commercial applications, and to alter it and redistribute it 
-* freely, subject to the following restrictions: 
-* 1. The origin of this software must not be misrepresented; you must not 
-* claim that you wrote the original software. If you use this software 
-* in a product, an acknowledgment in the product documentation would be 
-* appreciated but is not required. 
-* 2. Altered source versions must be plainly marked as such, and must not be 
-* misrepresented as being the original software. 
-* 3. This notice may not be removed or altered from any source distribution. 
-*/
-
-using System;
-using System.Diagnostics;
-using System.Runtime.InteropServices;
-using FarseerPhysics.Collision.Shapes;
-using FarseerPhysics.Common;
-using Microsoft.Xna.Framework;
-
-namespace FarseerPhysics.Collision
-{
-    internal enum ContactFeatureType : byte
-    {
-        Vertex = 0,
-        Face = 1,
-    }
-
-    /// <summary>
-    /// The features that intersect to form the contact point
-    /// This must be 4 bytes or less.
-    /// </summary>
-    public struct ContactFeature
-    {
-        /// <summary>
-        /// Feature index on ShapeA
-        /// </summary>
-        public byte IndexA;
-
-        /// <summary>
-        /// Feature index on ShapeB
-        /// </summary>
-        public byte IndexB;
-
-        /// <summary>
-        /// The feature type on ShapeA
-        /// </summary>
-        public byte TypeA;
-
-        /// <summary>
-        /// The feature type on ShapeB
-        /// </summary>
-        public byte TypeB;
-    }
-
-    /// <summary>
-    /// Contact ids to facilitate warm starting.
-    /// </summary>
-    [StructLayout(LayoutKind.Explicit)]
-    public struct ContactID
-    {
-        /// <summary>
-        /// The features that intersect to form the contact point
-        /// </summary>
-        [FieldOffset(0)]
-        public ContactFeature Features;
-
-        /// <summary>
-        /// Used to quickly compare contact ids.
-        /// </summary>
-        [FieldOffset(0)]
-        public uint Key;
-    }
-
-    /// <summary>
-    /// A manifold point is a contact point belonging to a contact
-    /// manifold. It holds details related to the geometry and dynamics
-    /// of the contact points.
-    /// The local point usage depends on the manifold type:
-    /// -ShapeType.Circles: the local center of circleB
-    /// -SeparationFunction.FaceA: the local center of cirlceB or the clip point of polygonB
-    /// -SeparationFunction.FaceB: the clip point of polygonA
-    /// This structure is stored across time steps, so we keep it small.
-    /// Note: the impulses are used for internal caching and may not
-    /// provide reliable contact forces, especially for high speed collisions.
-    /// </summary>
-    public struct ManifoldPoint
-    {
-        /// <summary>
-        /// Uniquely identifies a contact point between two Shapes
-        /// </summary>
-        public ContactID Id;
-
-        public Vector2 LocalPoint;
-
-        public float NormalImpulse;
-
-        public float TangentImpulse;
-    }
-
-    public enum ManifoldType
-    {
-        Circles,
-        FaceA,
-        FaceB
-    }
-
-    /// <summary>
-    /// A manifold for two touching convex Shapes.
-    /// Box2D supports multiple types of contact:
-    /// - clip point versus plane with radius
-    /// - point versus point with radius (circles)
-    /// The local point usage depends on the manifold type:
-    /// -ShapeType.Circles: the local center of circleA
-    /// -SeparationFunction.FaceA: the center of faceA
-    /// -SeparationFunction.FaceB: the center of faceB
-    /// Similarly the local normal usage:
-    /// -ShapeType.Circles: not used
-    /// -SeparationFunction.FaceA: the normal on polygonA
-    /// -SeparationFunction.FaceB: the normal on polygonB
-    /// We store contacts in this way so that position correction can
-    /// account for movement, which is critical for continuous physics.
-    /// All contact scenarios must be expressed in one of these types.
-    /// This structure is stored across time steps, so we keep it small.
-    /// </summary>
-    public struct Manifold
-    {
-        /// <summary>
-        /// Not use for Type.SeparationFunction.Points
-        /// </summary>
-        public Vector2 LocalNormal;
-
-        /// <summary>
-        /// Usage depends on manifold type
-        /// </summary>
-        public Vector2 LocalPoint;
-
-        /// <summary>
-        /// The number of manifold points
-        /// </summary>
-        public int PointCount;
-
-        /// <summary>
-        /// The points of contact
-        /// </summary>
-        public FixedArray2<ManifoldPoint> Points;
-
-        public ManifoldType Type;
-    }
-
-    /// <summary>
-    /// This is used for determining the state of contact points.
-    /// </summary>
-    public enum PointState
-    {
-        /// <summary>
-        /// Point does not exist
-        /// </summary>
-        Null,
-
-        /// <summary>
-        /// Point was added in the update
-        /// </summary>
-        Add,
-
-        /// <summary>
-        /// Point persisted across the update
-        /// </summary>
-        Persist,
-
-        /// <summary>
-        /// Point was removed in the update
-        /// </summary>
-        Remove,
-    }
-
-    /// <summary>
-    /// Used for computing contact manifolds.
-    /// </summary>
-    public struct ClipVertex
-    {
-        public ContactID ID;
-        public Vector2 V;
-    }
-
-    /// <summary>
-    /// Ray-cast input data. The ray extends from p1 to p1 + maxFraction * (p2 - p1).
-    /// </summary>
-    public struct RayCastInput
-    {
-        public float MaxFraction;
-        public Vector2 Point1, Point2;
-    }
-
-    /// <summary>
-    /// Ray-cast output data.  The ray hits at p1 + fraction * (p2 - p1), where p1 and p2
-    /// come from RayCastInput. 
-    /// </summary>
-    public struct RayCastOutput
-    {
-        public float Fraction;
-        public Vector2 Normal;
-    }
-
-    /// <summary>
-    /// An axis aligned bounding box.
-    /// </summary>
-    public struct AABB
-    {
-        private static DistanceInput _input = new DistanceInput();
-
-        /// <summary>
-        /// The lower vertex
-        /// </summary>
-        public Vector2 LowerBound;
-
-        /// <summary>
-        /// The upper vertex
-        /// </summary>
-        public Vector2 UpperBound;
-
-        public AABB(Vector2 min, Vector2 max)
-            : this(ref min, ref max)
-        {
-        }
-
-        public AABB(ref Vector2 min, ref Vector2 max)
-        {
-            LowerBound = min;
-            UpperBound = max;
-        }
-
-        public AABB(Vector2 center, float width, float height)
-        {
-            LowerBound = center - new Vector2(width / 2, height / 2);
-            UpperBound = center + new Vector2(width / 2, height / 2);
-        }
-
-        /// <summary>
-        /// Get the center of the AABB.
-        /// </summary>
-        /// <value></value>
-        public Vector2 Center
-        {
-            get { return 0.5f * (LowerBound + UpperBound); }
-        }
-
-        /// <summary>
-        /// Get the extents of the AABB (half-widths).
-        /// </summary>
-        /// <value></value>
-        public Vector2 Extents
-        {
-            get { return 0.5f * (UpperBound - LowerBound); }
-        }
-
-        /// <summary>
-        /// Get the perimeter length
-        /// </summary>
-        /// <value></value>
-        public float Perimeter
-        {
-            get
-            {
-                float wx = UpperBound.X - LowerBound.X;
-                float wy = UpperBound.Y - LowerBound.Y;
-                return 2.0f * (wx + wy);
-            }
-        }
-
-        /// <summary>
-        /// Gets the vertices of the AABB.
-        /// </summary>
-        /// <value>The corners of the AABB</value>
-        public Vertices Vertices
-        {
-            get
-            {
-                Vertices vertices = new Vertices();
-                vertices.Add(LowerBound);
-                vertices.Add(new Vector2(LowerBound.X, UpperBound.Y));
-                vertices.Add(UpperBound);
-                vertices.Add(new Vector2(UpperBound.X, LowerBound.Y));
-                return vertices;
-            }
-        }
-
-        /// <summary>
-        /// first quadrant
-        /// </summary>
-        public AABB Q1
-        {
-            get { return new AABB(Center, UpperBound); }
-        }
-
-        public AABB Q2
-        {
-            get
-            {
-                return new AABB(new Vector2(LowerBound.X, Center.Y), new Vector2(Center.X, UpperBound.Y));
-                ;
-            }
-        }
-
-        public AABB Q3
-        {
-            get { return new AABB(LowerBound, Center); }
-        }
-
-        public AABB Q4
-        {
-            get { return new AABB(new Vector2(Center.X, LowerBound.Y), new Vector2(UpperBound.X, Center.Y)); }
-        }
-
-        public Vector2[] GetVertices()
-        {
-            Vector2 p1 = UpperBound;
-            Vector2 p2 = new Vector2(UpperBound.X, LowerBound.Y);
-            Vector2 p3 = LowerBound;
-            Vector2 p4 = new Vector2(LowerBound.X, UpperBound.Y);
-            return new[] { p1, p2, p3, p4 };
-        }
-
-        /// <summary>
-        /// Verify that the bounds are sorted.
-        /// </summary>
-        /// <returns>
-        /// 	<c>true</c> if this instance is valid; otherwise, <c>false</c>.
-        /// </returns>
-        public bool IsValid()
-        {
-            Vector2 d = UpperBound - LowerBound;
-            bool valid = d.X >= 0.0f && d.Y >= 0.0f;
-            valid = valid && LowerBound.IsValid() && UpperBound.IsValid();
-            return valid;
-        }
-
-        /// <summary>
-        /// Combine an AABB into this one.
-        /// </summary>
-        /// <param name="aabb">The aabb.</param>
-        public void Combine(ref AABB aabb)
-        {
-            LowerBound = Vector2.Min(LowerBound, aabb.LowerBound);
-            UpperBound = Vector2.Max(UpperBound, aabb.UpperBound);
-        }
-
-        /// <summary>
-        /// Combine two AABBs into this one.
-        /// </summary>
-        /// <param name="aabb1">The aabb1.</param>
-        /// <param name="aabb2">The aabb2.</param>
-        public void Combine(ref AABB aabb1, ref AABB aabb2)
-        {
-            LowerBound = Vector2.Min(aabb1.LowerBound, aabb2.LowerBound);
-            UpperBound = Vector2.Max(aabb1.UpperBound, aabb2.UpperBound);
-        }
-
-        /// <summary>
-        /// Does this aabb contain the provided AABB.
-        /// </summary>
-        /// <param name="aabb">The aabb.</param>
-        /// <returns>
-        /// 	<c>true</c> if it contains the specified aabb; otherwise, <c>false</c>.
-        /// </returns>
-        public bool Contains(ref AABB aabb)
-        {
-            bool result = true;
-            result = result && LowerBound.X <= aabb.LowerBound.X;
-            result = result && LowerBound.Y <= aabb.LowerBound.Y;
-            result = result && aabb.UpperBound.X <= UpperBound.X;
-            result = result && aabb.UpperBound.Y <= UpperBound.Y;
-            return result;
-        }
-
-        /// <summary>
-        /// Determines whether the AAABB contains the specified point.
-        /// </summary>
-        /// <param name="point">The point.</param>
-        /// <returns>
-        /// 	<c>true</c> if it contains the specified point; otherwise, <c>false</c>.
-        /// </returns>
-        public bool Contains(ref Vector2 point)
-        {
-            //using epsilon to try and gaurd against float rounding errors.
-            if ((point.X > (LowerBound.X + Settings.Epsilon) && point.X < (UpperBound.X - Settings.Epsilon) &&
-                 (point.Y > (LowerBound.Y + Settings.Epsilon) && point.Y < (UpperBound.Y - Settings.Epsilon))))
-            {
-                return true;
-            }
-            return false;
-        }
-
-        public static bool TestOverlap(AABB a, AABB b)
-        {
-            return TestOverlap(ref a, ref b);
-        }
-
-        public static bool TestOverlap(ref AABB a, ref AABB b)
-        {
-            Vector2 d1 = b.LowerBound - a.UpperBound;
-            Vector2 d2 = a.LowerBound - b.UpperBound;
-
-            if (d1.X > 0.0f || d1.Y > 0.0f)
-                return false;
-
-            if (d2.X > 0.0f || d2.Y > 0.0f)
-                return false;
-
-            return true;
-        }
-
-        public static bool TestOverlap(Shape shapeA, int indexA,
-                                       Shape shapeB, int indexB,
-                                       ref Transform xfA, ref Transform xfB)
-        {
-            _input.ProxyA.Set(shapeA, indexA);
-            _input.ProxyB.Set(shapeB, indexB);
-            _input.TransformA = xfA;
-            _input.TransformB = xfB;
-            _input.UseRadii = true;
-
-            SimplexCache cache;
-            DistanceOutput output;
-            Distance.ComputeDistance(out output, out cache, _input);
-
-            return output.Distance < 10.0f * Settings.Epsilon;
-        }
-
-
-        // From Real-time Collision Detection, p179.
-        public bool RayCast(out RayCastOutput output, ref RayCastInput input)
-        {
-            output = new RayCastOutput();
-
-            float tmin = -Settings.MaxFloat;
-            float tmax = Settings.MaxFloat;
-
-            Vector2 p = input.Point1;
-            Vector2 d = input.Point2 - input.Point1;
-            Vector2 absD = MathUtils.Abs(d);
-
-            Vector2 normal = Vector2.Zero;
-
-            for (int i = 0; i < 2; ++i)
-            {
-                float absD_i = i == 0 ? absD.X : absD.Y;
-                float lowerBound_i = i == 0 ? LowerBound.X : LowerBound.Y;
-                float upperBound_i = i == 0 ? UpperBound.X : UpperBound.Y;
-                float p_i = i == 0 ? p.X : p.Y;
-
-                if (absD_i < Settings.Epsilon)
-                {
-                    // Parallel.
-                    if (p_i < lowerBound_i || upperBound_i < p_i)
-                    {
-                        return false;
-                    }
-                }
-                else
-                {
-                    float d_i = i == 0 ? d.X : d.Y;
-
-                    float inv_d = 1.0f / d_i;
-                    float t1 = (lowerBound_i - p_i) * inv_d;
-                    float t2 = (upperBound_i - p_i) * inv_d;
-
-                    // Sign of the normal vector.
-                    float s = -1.0f;
-
-                    if (t1 > t2)
-                    {
-                        MathUtils.Swap(ref t1, ref t2);
-                        s = 1.0f;
-                    }
-
-                    // Push the min up
-                    if (t1 > tmin)
-                    {
-                        if (i == 0)
-                        {
-                            normal.X = s;
-                        }
-                        else
-                        {
-                            normal.Y = s;
-                        }
-
-                        tmin = t1;
-                    }
-
-                    // Pull the max down
-                    tmax = Math.Min(tmax, t2);
-
-                    if (tmin > tmax)
-                    {
-                        return false;
-                    }
-                }
-            }
-
-            // Does the ray start inside the box?
-            // Does the ray intersect beyond the max fraction?
-            if (tmin < 0.0f || input.MaxFraction < tmin)
-            {
-                return false;
-            }
-
-            // Intersection.
-            output.Fraction = tmin;
-            output.Normal = normal;
-            return true;
-        }
-    }
-
-    /// <summary>
-    /// Edge shape plus more stuff.
-    /// </summary>
-    public struct FatEdge
-    {
-        public bool HasVertex0, HasVertex3;
-        public Vector2 Normal;
-        public Vector2 V0, V1, V2, V3;
-    }
-
-    /// <summary>
-    /// This lets us treate and edge shape and a polygon in the same
-    /// way in the SAT collider.
-    /// </summary>
-    public class EPProxy
-    {
-        public Vector2 Centroid;
-        public int Count;
-        public Vector2[] Normals = new Vector2[Settings.MaxPolygonVertices];
-        public Vector2[] Vertices = new Vector2[Settings.MaxPolygonVertices];
-    }
-
-    public struct EPAxis
-    {
-        public int Index;
-        public float Separation;
-        public EPAxisType Type;
-    }
-
-    public enum EPAxisType
-    {
-        Unknown,
-        EdgeA,
-        EdgeB,
-    }
-
-    public static class Collision
-    {
-        private static FatEdge _edgeA;
-
-        private static EPProxy _proxyA = new EPProxy();
-        private static EPProxy _proxyB = new EPProxy();
-
-        private static Transform _xf;
-        private static Vector2 _limit11, _limit12;
-        private static Vector2 _limit21, _limit22;
-        private static float _radius;
-        private static Vector2[] _tmpNormals = new Vector2[2];
-
-        /// <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;
-                    }
-                }
-            }
-        }
-
-
-        /// 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>
-        /// 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;
-            }
-        }
-
-        /// <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 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
-        }
-
-        private static EPAxis ComputeEdgeSeparation()
-        {
-            // EdgeA separation
-            EPAxis bestAxis;
-            bestAxis.Type = EPAxisType.Unknown;
-            bestAxis.Index = -1;
-            bestAxis.Separation = -Settings.MaxFloat;
-            _tmpNormals[0] = _edgeA.Normal;
-            _tmpNormals[1] = -_edgeA.Normal;
-
-            for (int i = 0; i < 2; ++i)
-            {
-                Vector2 n = _tmpNormals[i];
-
-                // Adjacency
-                bool valid1 = MathUtils.Cross(n, _limit11) >= -Settings.AngularSlop &&
-                              MathUtils.Cross(_limit12, n) >= -Settings.AngularSlop;
-                bool valid2 = MathUtils.Cross(n, _limit21) >= -Settings.AngularSlop &&
-                              MathUtils.Cross(_limit22, n) >= -Settings.AngularSlop;
-
-                if (valid1 == false || valid2 == false)
-                {
-                    continue;
-                }
-
-                EPAxis axis;
-                axis.Type = EPAxisType.EdgeA;
-                axis.Index = i;
-                axis.Separation = Settings.MaxFloat;
-
-                for (int j = 0; j < _proxyB.Count; ++j)
-                {
-                    float s = Vector2.Dot(n, _proxyB.Vertices[j] - _edgeA.V1);
-                    if (s < axis.Separation)
-                    {
-                        axis.Separation = s;
-                    }
-                }
-
-                if (axis.Separation > _radius)
-                {
-                    return axis;
-                }
-
-                if (axis.Separation > bestAxis.Separation)
-                {
-                    bestAxis = axis;
-                }
-            }
-
-            return bestAxis;
-        }
-
-        private static EPAxis ComputePolygonSeparation()
-        {
-            EPAxis axis;
-            axis.Type = EPAxisType.Unknown;
-            axis.Index = -1;
-            axis.Separation = -Settings.MaxFloat;
-            for (int i = 0; i < _proxyB.Count; ++i)
-            {
-                Vector2 n = -_proxyB.Normals[i];
-
-                // Adjacency
-                bool valid1 = MathUtils.Cross(n, _limit11) >= -Settings.AngularSlop &&
-                              MathUtils.Cross(_limit12, n) >= -Settings.AngularSlop;
-                bool valid2 = MathUtils.Cross(n, _limit21) >= -Settings.AngularSlop &&
-                              MathUtils.Cross(_limit22, n) >= -Settings.AngularSlop;
-
-                if (valid1 == false && valid2 == false)
-                {
-                    continue;
-                }
-
-                float s1 = Vector2.Dot(n, _proxyB.Vertices[i] - _edgeA.V1);
-                float s2 = Vector2.Dot(n, _proxyB.Vertices[i] - _edgeA.V2);
-                float s = Math.Min(s1, s2);
-
-                if (s > _radius)
-                {
-                    axis.Type = EPAxisType.EdgeB;
-                    axis.Index = i;
-                    axis.Separation = s;
-                }
-
-                if (s > axis.Separation)
-                {
-                    axis.Type = EPAxisType.EdgeB;
-                    axis.Index = i;
-                    axis.Separation = s;
-                }
-            }
-
-            return axis;
-        }
-
-        private static void FindIncidentEdge(ref FixedArray2<ClipVertex> c, EPProxy proxy1, int edge1, EPProxy proxy2)
-        {
-            int count2 = proxy2.Count;
-
-            Debug.Assert(0 <= edge1 && edge1 < proxy1.Count);
-
-            // Get the normal of the reference edge in proxy2's frame.
-            Vector2 normal1 = proxy1.Normals[edge1];
-
-            // Find the incident edge on proxy2.
-            int index = 0;
-            float minDot = float.MaxValue;
-            for (int i = 0; i < count2; ++i)
-            {
-                float dot = Vector2.Dot(normal1, proxy2.Normals[i]);
-                if (dot < minDot)
-                {
-                    minDot = dot;
-                    index = i;
-                }
-            }
-
-            // Build the clip vertices for the incident edge.
-            int i1 = index;
-            int i2 = i1 + 1 < count2 ? i1 + 1 : 0;
-
-            ClipVertex cTemp = new ClipVertex();
-            cTemp.V = proxy2.Vertices[i1];
-            cTemp.ID.Features.IndexA = (byte)edge1;
-            cTemp.ID.Features.IndexB = (byte)i1;
-            cTemp.ID.Features.TypeA = (byte)ContactFeatureType.Face;
-            cTemp.ID.Features.TypeB = (byte)ContactFeatureType.Vertex;
-            c[0] = cTemp;
-
-            cTemp.V = proxy2.Vertices[i2];
-            cTemp.ID.Features.IndexA = (byte)edge1;
-            cTemp.ID.Features.IndexB = (byte)i2;
-            cTemp.ID.Features.TypeA = (byte)ContactFeatureType.Face;
-            cTemp.ID.Features.TypeB = (byte)ContactFeatureType.Vertex;
-            c[1] = cTemp;
-        }
-
-        /// <summary>
-        /// Clipping for contact manifolds.
-        /// </summary>
-        /// <param name="vOut">The v out.</param>
-        /// <param name="vIn">The v in.</param>
-        /// <param name="normal">The normal.</param>
-        /// <param name="offset">The offset.</param>
-        /// <param name="vertexIndexA">The vertex index A.</param>
-        /// <returns></returns>
-        private static int ClipSegmentToLine(out FixedArray2<ClipVertex> vOut, ref FixedArray2<ClipVertex> vIn,
-                                             Vector2 normal, float offset, int vertexIndexA)
-        {
-            vOut = new FixedArray2<ClipVertex>();
-
-            ClipVertex v0 = vIn[0];
-            ClipVertex v1 = vIn[1];
-
-            // Start with no output points
-            int numOut = 0;
-
-            // Calculate the distance of end points to the line
-            float distance0 = normal.X * v0.V.X + normal.Y * v0.V.Y - offset;
-            float distance1 = normal.X * v1.V.X + normal.Y * v1.V.Y - offset;
-
-            // If the points are behind the plane
-            if (distance0 <= 0.0f) vOut[numOut++] = v0;
-            if (distance1 <= 0.0f) vOut[numOut++] = v1;
-
-            // If the points are on different sides of the plane
-            if (distance0 * distance1 < 0.0f)
-            {
-                // Find intersection point of edge and plane
-                float interp = distance0 / (distance0 - distance1);
-
-                ClipVertex cv = vOut[numOut];
-
-                cv.V.X = v0.V.X + interp * (v1.V.X - v0.V.X);
-                cv.V.Y = v0.V.Y + interp * (v1.V.Y - v0.V.Y);
-
-                // VertexA is hitting edgeB.
-                cv.ID.Features.IndexA = (byte)vertexIndexA;
-                cv.ID.Features.IndexB = v0.ID.Features.IndexB;
-                cv.ID.Features.TypeA = (byte)ContactFeatureType.Vertex;
-                cv.ID.Features.TypeB = (byte)ContactFeatureType.Face;
-
-                vOut[numOut] = cv;
-
-                ++numOut;
-            }
-
-            return numOut;
-        }
-
-        /// <summary>
-        /// Find the separation between poly1 and poly2 for a give edge normal on poly1.
-        /// </summary>
-        /// <param name="poly1">The poly1.</param>
-        /// <param name="xf1">The XF1.</param>
-        /// <param name="edge1">The edge1.</param>
-        /// <param name="poly2">The poly2.</param>
-        /// <param name="xf2">The XF2.</param>
-        /// <returns></returns>
-        private static float EdgeSeparation(PolygonShape poly1, ref Transform xf1, int edge1,
-                                            PolygonShape poly2, ref Transform xf2)
-        {
-            int count2 = poly2.Vertices.Count;
-
-            Debug.Assert(0 <= edge1 && edge1 < poly1.Vertices.Count);
-
-            // Convert normal from poly1's frame into poly2's frame.
-            Vector2 p1n = poly1.Normals[edge1];
-
-            float normalWorldx = xf1.R.Col1.X * p1n.X + xf1.R.Col2.X * p1n.Y;
-            float normalWorldy = xf1.R.Col1.Y * p1n.X + xf1.R.Col2.Y * p1n.Y;
-
-            Vector2 normal = new Vector2(normalWorldx * xf2.R.Col1.X + normalWorldy * xf2.R.Col1.Y,
-                                         normalWorldx * xf2.R.Col2.X + normalWorldy * xf2.R.Col2.Y);
-
-            // Find support vertex on poly2 for -normal.
-            int index = 0;
-            float minDot = Settings.MaxFloat;
-
-            for (int i = 0; i < count2; ++i)
-            {
-                float dot = Vector2.Dot(poly2.Vertices[i], normal);
-
-                if (dot < minDot)
-                {
-                    minDot = dot;
-                    index = i;
-                }
-            }
-
-            Vector2 p1ve = poly1.Vertices[edge1];
-            Vector2 p2vi = poly2.Vertices[index];
-
-            return ((xf2.Position.X + xf2.R.Col1.X * p2vi.X + xf2.R.Col2.X * p2vi.Y) -
-                    (xf1.Position.X + xf1.R.Col1.X * p1ve.X + xf1.R.Col2.X * p1ve.Y)) * normalWorldx +
-                   ((xf2.Position.Y + xf2.R.Col1.Y * p2vi.X + xf2.R.Col2.Y * p2vi.Y) -
-                    (xf1.Position.Y + xf1.R.Col1.Y * p1ve.X + xf1.R.Col2.Y * p1ve.Y)) * normalWorldy;
-        }
-
-        /// <summary>
-        /// Find the max separation between poly1 and poly2 using edge normals from poly1.
-        /// </summary>
-        /// <param name="edgeIndex">Index of the edge.</param>
-        /// <param name="poly1">The poly1.</param>
-        /// <param name="xf1">The XF1.</param>
-        /// <param name="poly2">The poly2.</param>
-        /// <param name="xf2">The XF2.</param>
-        /// <returns></returns>
-        private static float FindMaxSeparation(out int edgeIndex,
-                                               PolygonShape poly1, ref Transform xf1,
-                                               PolygonShape poly2, ref Transform xf2)
-        {
-            int count1 = poly1.Vertices.Count;
-
-            // Vector pointing from the centroid of poly1 to the centroid of poly2.
-            float dx = (xf2.Position.X + xf2.R.Col1.X * poly2.MassData.Centroid.X +
-                        xf2.R.Col2.X * poly2.MassData.Centroid.Y) -
-                       (xf1.Position.X + xf1.R.Col1.X * poly1.MassData.Centroid.X +
-                        xf1.R.Col2.X * poly1.MassData.Centroid.Y);
-            float dy = (xf2.Position.Y + xf2.R.Col1.Y * poly2.MassData.Centroid.X +
-                        xf2.R.Col2.Y * poly2.MassData.Centroid.Y) -
-                       (xf1.Position.Y + xf1.R.Col1.Y * poly1.MassData.Centroid.X +
-                        xf1.R.Col2.Y * poly1.MassData.Centroid.Y);
-            Vector2 dLocal1 = new Vector2(dx * xf1.R.Col1.X + dy * xf1.R.Col1.Y, dx * xf1.R.Col2.X + dy * xf1.R.Col2.Y);
-
-            // Find edge normal on poly1 that has the largest projection onto d.
-            int edge = 0;
-            float maxDot = -Settings.MaxFloat;
-            for (int i = 0; i < count1; ++i)
-            {
-                float dot = Vector2.Dot(poly1.Normals[i], dLocal1);
-                if (dot > maxDot)
-                {
-                    maxDot = dot;
-                    edge = i;
-                }
-            }
-
-            // Get the separation for the edge normal.
-            float s = EdgeSeparation(poly1, ref xf1, edge, poly2, ref xf2);
-
-            // Check the separation for the previous edge normal.
-            int prevEdge = edge - 1 >= 0 ? edge - 1 : count1 - 1;
-            float sPrev = EdgeSeparation(poly1, ref xf1, prevEdge, poly2, ref xf2);
-
-            // Check the separation for the next edge normal.
-            int nextEdge = edge + 1 < count1 ? edge + 1 : 0;
-            float sNext = EdgeSeparation(poly1, ref xf1, nextEdge, poly2, ref xf2);
-
-            // Find the best edge and the search direction.
-            int bestEdge;
-            float bestSeparation;
-            int increment;
-            if (sPrev > s && sPrev > sNext)
-            {
-                increment = -1;
-                bestEdge = prevEdge;
-                bestSeparation = sPrev;
-            }
-            else if (sNext > s)
-            {
-                increment = 1;
-                bestEdge = nextEdge;
-                bestSeparation = sNext;
-            }
-            else
-            {
-                edgeIndex = edge;
-                return s;
-            }
-
-            // Perform a local search for the best edge normal.
-            for (; ; )
-            {
-                if (increment == -1)
-                    edge = bestEdge - 1 >= 0 ? bestEdge - 1 : count1 - 1;
-                else
-                    edge = bestEdge + 1 < count1 ? bestEdge + 1 : 0;
-
-                s = EdgeSeparation(poly1, ref xf1, edge, poly2, ref xf2);
-
-                if (s > bestSeparation)
-                {
-                    bestEdge = edge;
-                    bestSeparation = s;
-                }
-                else
-                {
-                    break;
-                }
-            }
-
-            edgeIndex = bestEdge;
-            return bestSeparation;
-        }
-
-        private static void FindIncidentEdge(out FixedArray2<ClipVertex> c,
-                                             PolygonShape poly1, ref Transform xf1, int edge1,
-                                             PolygonShape poly2, ref Transform xf2)
-        {
-            c = new FixedArray2<ClipVertex>();
-
-            int count2 = poly2.Vertices.Count;
-
-            Debug.Assert(0 <= edge1 && edge1 < poly1.Vertices.Count);
-
-            // Get the normal of the reference edge in poly2's frame.
-            Vector2 v = poly1.Normals[edge1];
-            float tmpx = xf1.R.Col1.X * v.X + xf1.R.Col2.X * v.Y;
-            float tmpy = xf1.R.Col1.Y * v.X + xf1.R.Col2.Y * v.Y;
-            Vector2 normal1 = new Vector2(tmpx * xf2.R.Col1.X + tmpy * xf2.R.Col1.Y,
-                                          tmpx * xf2.R.Col2.X + tmpy * xf2.R.Col2.Y);
-
-            // Find the incident edge on poly2.
-            int index = 0;
-            float minDot = Settings.MaxFloat;
-            for (int i = 0; i < count2; ++i)
-            {
-                float dot = Vector2.Dot(normal1, poly2.Normals[i]);
-                if (dot < minDot)
-                {
-                    minDot = dot;
-                    index = i;
-                }
-            }
-
-            // Build the clip vertices for the incident edge.
-            int i1 = index;
-            int i2 = i1 + 1 < count2 ? i1 + 1 : 0;
-
-            ClipVertex cv0 = c[0];
-
-            Vector2 v1 = poly2.Vertices[i1];
-            cv0.V.X = xf2.Position.X + xf2.R.Col1.X * v1.X + xf2.R.Col2.X * v1.Y;
-            cv0.V.Y = xf2.Position.Y + xf2.R.Col1.Y * v1.X + xf2.R.Col2.Y * v1.Y;
-            cv0.ID.Features.IndexA = (byte)edge1;
-            cv0.ID.Features.IndexB = (byte)i1;
-            cv0.ID.Features.TypeA = (byte)ContactFeatureType.Face;
-            cv0.ID.Features.TypeB = (byte)ContactFeatureType.Vertex;
-
-            c[0] = cv0;
-
-            ClipVertex cv1 = c[1];
-            Vector2 v2 = poly2.Vertices[i2];
-            cv1.V.X = xf2.Position.X + xf2.R.Col1.X * v2.X + xf2.R.Col2.X * v2.Y;
-            cv1.V.Y = xf2.Position.Y + xf2.R.Col1.Y * v2.X + xf2.R.Col2.Y * v2.Y;
-            cv1.ID.Features.IndexA = (byte)edge1;
-            cv1.ID.Features.IndexB = (byte)i2;
-            cv1.ID.Features.TypeA = (byte)ContactFeatureType.Face;
-            cv1.ID.Features.TypeB = (byte)ContactFeatureType.Vertex;
-
-            c[1] = cv1;
-        }
-    }
-}

FarseerPhysicsEngine/Collision/Distance.cs

@@ -1,780 +0,0 @@
-/*
-* Farseer Physics Engine based on Box2D.XNA port:
-* Copyright (c) 2010 Ian Qvist
-* 
-* Box2D.XNA port of Box2D:
-* Copyright (c) 2009 Brandon Furtwangler, Nathan Furtwangler
-*
-* Original source Box2D:
-* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com 
-* 
-* This software is provided 'as-is', without any express or implied 
-* warranty.  In no event will the authors be held liable for any damages 
-* arising from the use of this software. 
-* Permission is granted to anyone to use this software for any purpose, 
-* including commercial applications, and to alter it and redistribute it 
-* freely, subject to the following restrictions: 
-* 1. The origin of this software must not be misrepresented; you must not 
-* claim that you wrote the original software. If you use this software 
-* in a product, an acknowledgment in the product documentation would be 
-* appreciated but is not required. 
-* 2. Altered source versions must be plainly marked as such, and must not be 
-* misrepresented as being the original software. 
-* 3. This notice may not be removed or altered from any source distribution. 
-*/
-
-using System;
-using System.Diagnostics;
-using FarseerPhysics.Collision.Shapes;
-using FarseerPhysics.Common;
-using Microsoft.Xna.Framework;
-
-namespace FarseerPhysics.Collision
-{
-    /// <summary>
-    /// A distance proxy is used by the GJK algorithm.
-    /// It encapsulates any shape.
-    /// </summary>
-    public class DistanceProxy
-    {
-        internal float Radius;
-        internal Vertices Vertices = new Vertices();
-
-        /// <summary>
-        /// Initialize the proxy using the given shape. The shape
-        /// must remain in scope while the proxy is in use.
-        /// </summary>
-        /// <param name="shape">The shape.</param>
-        /// <param name="index">The index.</param>
-        public void Set(Shape shape, int index)
-        {
-            switch (shape.ShapeType)
-            {
-                case ShapeType.Circle:
-                    {
-                        CircleShape circle = (CircleShape)shape;
-                        Vertices.Clear();
-                        Vertices.Add(circle.Position);
-                        Radius = circle.Radius;
-                    }
-                    break;
-
-                case ShapeType.Polygon:
-                    {
-                        PolygonShape polygon = (PolygonShape)shape;
-                        Vertices.Clear();
-                        for (int i = 0; i < polygon.Vertices.Count; i++)
-                        {
-                            Vertices.Add(polygon.Vertices[i]);
-                        }
-                        Radius = polygon.Radius;
-                    }
-                    break;
-
-                case ShapeType.Loop:
-                    {
-                        LoopShape loop = (LoopShape)shape;
-                        Debug.Assert(0 <= index && index < loop.Vertices.Count);
-                        Vertices.Clear();
-                        Vertices.Add(loop.Vertices[index]);
-                        Vertices.Add(index + 1 < loop.Vertices.Count ? loop.Vertices[index + 1] : loop.Vertices[0]);
-
-                        Radius = loop.Radius;
-                    }
-                    break;
-
-                case ShapeType.Edge:
-                    {
-                        EdgeShape edge = (EdgeShape)shape;
-                        Vertices.Clear();
-                        Vertices.Add(edge.Vertex1);
-                        Vertices.Add(edge.Vertex2);
-                        Radius = edge.Radius;
-                    }
-                    break;
-
-                default:
-                    Debug.Assert(false);
-                    break;
-            }
-        }
-
-        /// <summary>
-        /// Get the supporting vertex index in the given direction.
-        /// </summary>
-        /// <param name="direction">The direction.</param>
-        /// <returns></returns>
-        public int GetSupport(Vector2 direction)
-        {
-            int bestIndex = 0;
-            float bestValue = Vector2.Dot(Vertices[0], direction);
-            for (int i = 1; i < Vertices.Count; ++i)
-            {
-                float value = Vector2.Dot(Vertices[i], direction);
-                if (value > bestValue)
-                {
-                    bestIndex = i;
-                    bestValue = value;
-                }
-            }
-
-            return bestIndex;
-        }
-
-        /// <summary>
-        /// Get the supporting vertex in the given direction.
-        /// </summary>
-        /// <param name="direction">The direction.</param>
-        /// <returns></returns>
-        public Vector2 GetSupportVertex(Vector2 direction)
-        {
-            int bestIndex = 0;
-            float bestValue = Vector2.Dot(Vertices[0], direction);
-            for (int i = 1; i < Vertices.Count; ++i)
-            {
-                float value = Vector2.Dot(Vertices[i], direction);
-                if (value > bestValue)
-                {
-                    bestIndex = i;
-                    bestValue = value;
-                }
-            }
-
-            return Vertices[bestIndex];
-        }
-    }
-
-    /// <summary>
-    /// Used to warm start ComputeDistance.
-    /// Set count to zero on first call.
-    /// </summary>
-    public struct SimplexCache
-    {
-        /// <summary>
-        /// Length or area
-        /// </summary>
-        public ushort Count;
-
-        /// <summary>
-        /// Vertices on shape A
-        /// </summary>
-        public FixedArray3<byte> IndexA;
-
-        /// <summary>
-        /// Vertices on shape B
-        /// </summary>
-        public FixedArray3<byte> IndexB;
-
-        public float Metric;
-    }
-
-    /// <summary>
-    /// Input for ComputeDistance.
-    /// You have to option to use the shape radii
-    /// in the computation. 
-    /// </summary>
-    public class DistanceInput
-    {
-        public DistanceProxy ProxyA = new DistanceProxy();
-        public DistanceProxy ProxyB = new DistanceProxy();
-        public Transform TransformA;
-        public Transform TransformB;
-        public bool UseRadii;
-    }
-
-    /// <summary>
-    /// Output for ComputeDistance.
-    /// </summary>
-    public struct DistanceOutput
-    {
-        public float Distance;
-
-        /// <summary>
-        /// Number of GJK iterations used
-        /// </summary>
-        public int Iterations;
-
-        /// <summary>
-        /// Closest point on shapeA
-        /// </summary>
-        public Vector2 PointA;
-
-        /// <summary>
-        /// Closest point on shapeB
-        /// </summary>
-        public Vector2 PointB;
-    }
-
-    internal struct SimplexVertex
-    {
-        /// <summary>
-        /// Barycentric coordinate for closest point 
-        /// </summary>
-        public float A;
-
-        /// <summary>
-        /// wA index
-        /// </summary>
-        public int IndexA;
-
-        /// <summary>
-        /// wB index
-        /// </summary>
-        public int IndexB;
-
-        /// <summary>
-        /// wB - wA
-        /// </summary>
-        public Vector2 W;
-
-        /// <summary>
-        /// Support point in proxyA
-        /// </summary>
-        public Vector2 WA;
-
-        /// <summary>
-        /// Support point in proxyB
-        /// </summary>
-        public Vector2 WB;
-    }
-
-    internal struct Simplex
-    {
-        internal int Count;
-        internal FixedArray3<SimplexVertex> V;
-
-        internal void ReadCache(ref SimplexCache cache,
-                                DistanceProxy proxyA, ref Transform transformA,
-                                DistanceProxy proxyB, ref Transform transformB)
-        {
-            Debug.Assert(cache.Count <= 3);
-
-            // Copy data from cache.
-            Count = cache.Count;
-            for (int i = 0; i < Count; ++i)
-            {
-                SimplexVertex v = V[i];
-                v.IndexA = cache.IndexA[i];
-                v.IndexB = cache.IndexB[i];
-                Vector2 wALocal = proxyA.Vertices[v.IndexA];
-                Vector2 wBLocal = proxyB.Vertices[v.IndexB];
-                v.WA = MathUtils.Multiply(ref transformA, wALocal);
-                v.WB = MathUtils.Multiply(ref transformB, wBLocal);
-                v.W = v.WB - v.WA;
-                v.A = 0.0f;
-                V[i] = v;
-            }
-
-            // Compute the new simplex metric, if it is substantially different than
-            // old metric then flush the simplex.
-            if (Count > 1)
-            {
-                float metric1 = cache.Metric;
-                float metric2 = GetMetric();
-                if (metric2 < 0.5f * metric1 || 2.0f * metric1 < metric2 || metric2 < Settings.Epsilon)
-                {
-                    // Reset the simplex.
-                    Count = 0;
-                }
-            }
-
-            // If the cache is empty or invalid ...
-            if (Count == 0)
-            {
-                SimplexVertex v = V[0];
-                v.IndexA = 0;
-                v.IndexB = 0;
-                Vector2 wALocal = proxyA.Vertices[0];
-                Vector2 wBLocal = proxyB.Vertices[0];
-                v.WA = MathUtils.Multiply(ref transformA, wALocal);
-                v.WB = MathUtils.Multiply(ref transformB, wBLocal);
-                v.W = v.WB - v.WA;
-                V[0] = v;
-                Count = 1;
-            }
-        }
-
-        internal void WriteCache(ref SimplexCache cache)
-        {
-            cache.Metric = GetMetric();
-            cache.Count = (UInt16)Count;
-            for (int i = 0; i < Count; ++i)
-            {
-                cache.IndexA[i] = (byte)(V[i].IndexA);
-                cache.IndexB[i] = (byte)(V[i].IndexB);
-            }
-        }
-
-        internal Vector2 GetSearchDirection()
-        {
-            switch (Count)
-            {
-                case 1:
-                    return -V[0].W;
-
-                case 2:
-                    {
-                        Vector2 e12 = V[1].W - V[0].W;
-                        float sgn = MathUtils.Cross(e12, -V[0].W);
-                        if (sgn > 0.0f)
-                        {
-                            // Origin is left of e12.
-                            return new Vector2(-e12.Y, e12.X);
-                        }
-                        else
-                        {
-                            // Origin is right of e12.
-                            return new Vector2(e12.Y, -e12.X);
-                        }
-                    }
-
-                default:
-                    Debug.Assert(false);
-                    return Vector2.Zero;
-            }
-        }
-
-        internal Vector2 GetClosestPoint()
-        {
-            switch (Count)
-            {
-                case 0:
-                    Debug.Assert(false);
-                    return Vector2.Zero;
-
-                case 1:
-                    return V[0].W;
-
-                case 2:
-                    return V[0].A * V[0].W + V[1].A * V[1].W;
-
-                case 3:
-                    return Vector2.Zero;
-
-                default:
-                    Debug.Assert(false);
-                    return Vector2.Zero;
-            }
-        }
-
-        internal void GetWitnessPoints(out Vector2 pA, out Vector2 pB)
-        {
-            switch (Count)
-            {
-                case 0:
-                    pA = Vector2.Zero;
-                    pB = Vector2.Zero;
-                    Debug.Assert(false);
-                    break;
-
-                case 1:
-                    pA = V[0].WA;
-                    pB = V[0].WB;
-                    break;
-
-                case 2:
-                    pA = V[0].A * V[0].WA + V[1].A * V[1].WA;
-                    pB = V[0].A * V[0].WB + V[1].A * V[1].WB;
-                    break;
-
-                case 3:
-                    pA = V[0].A * V[0].WA + V[1].A * V[1].WA + V[2].A * V[2].WA;
-                    pB = pA;
-                    break;
-
-                default:
-                    throw new Exception();
-            }
-        }
-
-        internal float GetMetric()
-        {
-            switch (Count)
-            {
-                case 0:
-                    Debug.Assert(false);
-                    return 0.0f;
-
-                case 1:
-                    return 0.0f;
-
-                case 2:
-                    return (V[0].W - V[1].W).Length();
-
-                case 3:
-                    return MathUtils.Cross(V[1].W - V[0].W, V[2].W - V[0].W);
-
-                default:
-                    Debug.Assert(false);
-                    return 0.0f;
-            }
-        }
-
-        // Solve a line segment using barycentric coordinates.
-        //
-        // p = a1 * w1 + a2 * w2
-        // a1 + a2 = 1
-        //
-        // The vector from the origin to the closest point on the line is
-        // perpendicular to the line.
-        // e12 = w2 - w1
-        // dot(p, e) = 0
-        // a1 * dot(w1, e) + a2 * dot(w2, e) = 0
-        //
-        // 2-by-2 linear system
-        // [1      1     ][a1] = [1]
-        // [w1.e12 w2.e12][a2] = [0]
-        //
-        // Define
-        // d12_1 =  dot(w2, e12)
-        // d12_2 = -dot(w1, e12)
-        // d12 = d12_1 + d12_2
-        //
-        // Solution
-        // a1 = d12_1 / d12
-        // a2 = d12_2 / d12
-
-        internal void Solve2()
-        {
-            Vector2 w1 = V[0].W;
-            Vector2 w2 = V[1].W;
-            Vector2 e12 = w2 - w1;
-
-            // w1 region
-            float d12_2 = -Vector2.Dot(w1, e12);
-            if (d12_2 <= 0.0f)
-            {
-                // a2 <= 0, so we clamp it to 0
-                SimplexVertex v0 = V[0];
-                v0.A = 1.0f;
-                V[0] = v0;
-                Count = 1;
-                return;
-            }
-
-            // w2 region
-            float d12_1 = Vector2.Dot(w2, e12);
-            if (d12_1 <= 0.0f)
-            {
-                // a1 <= 0, so we clamp it to 0
-                SimplexVertex v1 = V[1];
-                v1.A = 1.0f;
-                V[1] = v1;
-                Count = 1;
-                V[0] = V[1];
-                return;
-            }
-
-            // Must be in e12 region.
-            float inv_d12 = 1.0f / (d12_1 + d12_2);
-            SimplexVertex v0_2 = V[0];
-            SimplexVertex v1_2 = V[1];
-            v0_2.A = d12_1 * inv_d12;
-            v1_2.A = d12_2 * inv_d12;
-            V[0] = v0_2;
-            V[1] = v1_2;
-            Count = 2;
-        }
-
-        // Possible regions:
-        // - points[2]
-        // - edge points[0]-points[2]
-        // - edge points[1]-points[2]
-        // - inside the triangle
-        internal void Solve3()
-        {
-            Vector2 w1 = V[0].W;
-            Vector2 w2 = V[1].W;
-            Vector2 w3 = V[2].W;
-
-            // Edge12
-            // [1      1     ][a1] = [1]
-            // [w1.e12 w2.e12][a2] = [0]
-            // a3 = 0
-            Vector2 e12 = w2 - w1;
-            float w1e12 = Vector2.Dot(w1, e12);
-            float w2e12 = Vector2.Dot(w2, e12);
-            float d12_1 = w2e12;
-            float d12_2 = -w1e12;
-
-            // Edge13
-            // [1      1     ][a1] = [1]
-            // [w1.e13 w3.e13][a3] = [0]
-            // a2 = 0
-            Vector2 e13 = w3 - w1;
-            float w1e13 = Vector2.Dot(w1, e13);
-            float w3e13 = Vector2.Dot(w3, e13);
-            float d13_1 = w3e13;
-            float d13_2 = -w1e13;
-
-            // Edge23
-            // [1      1     ][a2] = [1]
-            // [w2.e23 w3.e23][a3] = [0]
-            // a1 = 0
-            Vector2 e23 = w3 - w2;
-            float w2e23 = Vector2.Dot(w2, e23);
-            float w3e23 = Vector2.Dot(w3, e23);
-            float d23_1 = w3e23;
-            float d23_2 = -w2e23;
-
-            // Triangle123
-            float n123 = MathUtils.Cross(e12, e13);
-
-            float d123_1 = n123 * MathUtils.Cross(w2, w3);
-            float d123_2 = n123 * MathUtils.Cross(w3, w1);
-            float d123_3 = n123 * MathUtils.Cross(w1, w2);
-
-            // w1 region
-            if (d12_2 <= 0.0f && d13_2 <= 0.0f)
-            {
-                SimplexVertex v0_1 = V[0];
-                v0_1.A = 1.0f;
-                V[0] = v0_1;
-                Count = 1;
-                return;
-            }
-
-            // e12
-            if (d12_1 > 0.0f && d12_2 > 0.0f && d123_3 <= 0.0f)
-            {
-                float inv_d12 = 1.0f / (d12_1 + d12_2);
-                SimplexVertex v0_2 = V[0];
-                SimplexVertex v1_2 = V[1];
-                v0_2.A = d12_1 * inv_d12;
-                v1_2.A = d12_2 * inv_d12;
-                V[0] = v0_2;
-                V[1] = v1_2;
-                Count = 2;
-                return;
-            }
-
-            // e13
-            if (d13_1 > 0.0f && d13_2 > 0.0f && d123_2 <= 0.0f)
-            {
-                float inv_d13 = 1.0f / (d13_1 + d13_2);
-                SimplexVertex v0_3 = V[0];
-                SimplexVertex v2_3 = V[2];
-                v0_3.A = d13_1 * inv_d13;
-                v2_3.A = d13_2 * inv_d13;
-                V[0] = v0_3;
-                V[2] = v2_3;
-                Count = 2;
-                V[1] = V[2];
-                return;
-            }
-
-            // w2 region
-            if (d12_1 <= 0.0f && d23_2 <= 0.0f)
-            {
-                SimplexVertex v1_4 = V[1];
-                v1_4.A = 1.0f;
-                V[1] = v1_4;
-                Count = 1;
-                V[0] = V[1];
-                return;
-            }
-
-            // w3 region
-            if (d13_1 <= 0.0f && d23_1 <= 0.0f)
-            {
-                SimplexVertex v2_5 = V[2];
-                v2_5.A = 1.0f;
-                V[2] = v2_5;
-                Count = 1;
-                V[0] = V[2];
-                return;
-            }
-
-            // e23
-            if (d23_1 > 0.0f && d23_2 > 0.0f && d123_1 <= 0.0f)
-            {
-                float inv_d23 = 1.0f / (d23_1 + d23_2);
-                SimplexVertex v1_6 = V[1];
-                SimplexVertex v2_6 = V[2];
-                v1_6.A = d23_1 * inv_d23;
-                v2_6.A = d23_2 * inv_d23;
-                V[1] = v1_6;
-                V[2] = v2_6;
-                Count = 2;
-                V[0] = V[2];
-                return;
-            }
-
-            // Must be in triangle123
-            float inv_d123 = 1.0f / (d123_1 + d123_2 + d123_3);
-            SimplexVertex v0_7 = V[0];
-            SimplexVertex v1_7 = V[1];
-            SimplexVertex v2_7 = V[2];
-            v0_7.A = d123_1 * inv_d123;
-            v1_7.A = d123_2 * inv_d123;
-            v2_7.A = d123_3 * inv_d123;
-            V[0] = v0_7;
-            V[1] = v1_7;
-            V[2] = v2_7;
-            Count = 3;
-        }
-    }
-
-    public static class Distance
-    {
-        public static int GJKCalls, GJKIters, GJKMaxIters;
-
-        public static void ComputeDistance(out DistanceOutput output,
-                                           out SimplexCache cache,
-                                           DistanceInput input)
-        {
-            cache = new SimplexCache();
-            ++GJKCalls;
-
-            // Initialize the simplex.
-            Simplex simplex = new Simplex();
-            simplex.ReadCache(ref cache, input.ProxyA, ref input.TransformA, input.ProxyB, ref input.TransformB);
-
-            // Get simplex vertices as an array.
-            const int k_maxIters = 20;
-
-            // These store the vertices of the last simplex so that we
-            // can check for duplicates and prevent cycling.
-            FixedArray3<int> saveA = new FixedArray3<int>();
-            FixedArray3<int> saveB = new FixedArray3<int>();
-
-            Vector2 closestPoint = simplex.GetClosestPoint();
-            float distanceSqr1 = closestPoint.LengthSquared();
-            float distanceSqr2 = distanceSqr1;
-
-            // Main iteration loop.
-            int iter = 0;
-            while (iter < k_maxIters)
-            {
-                // Copy simplex so we can identify duplicates.
-                int saveCount = simplex.Count;
-                for (int i = 0; i < saveCount; ++i)
-                {
-                    saveA[i] = simplex.V[i].IndexA;
-                    saveB[i] = simplex.V[i].IndexB;
-                }
-
-                switch (simplex.Count)
-                {
-                    case 1:
-                        break;
-
-                    case 2:
-                        simplex.Solve2();
-                        break;
-
-                    case 3:
-                        simplex.Solve3();
-                        break;
-
-                    default:
-                        Debug.Assert(false);
-                        break;
-                }
-
-                // If we have 3 points, then the origin is in the corresponding triangle.
-                if (simplex.Count == 3)
-                {
-                    break;
-                }
-
-                // Compute closest point.
-                Vector2 p = simplex.GetClosestPoint();
-                distanceSqr2 = p.LengthSquared();
-
-                // Ensure progress
-                if (distanceSqr2 >= distanceSqr1)
-                {
-                    //break;
-                }
-                distanceSqr1 = distanceSqr2;
-
-                // Get search direction.
-                Vector2 d = simplex.GetSearchDirection();
-
-                // Ensure the search direction is numerically fit.
-                if (d.LengthSquared() < Settings.Epsilon * Settings.Epsilon)
-                {
-                    // The origin is probably contained by a line segment
-                    // or triangle. Thus the shapes are overlapped.
-
-                    // We can't return zero here even though there may be overlap.
-                    // In case the simplex is a point, segment, or triangle it is difficult
-                    // to determine if the origin is contained in the CSO or very close to it.
-                    break;
-                }
-
-                // Compute a tentative new simplex vertex using support points.
-                SimplexVertex vertex = simplex.V[simplex.Count];
-                vertex.IndexA = input.ProxyA.GetSupport(MathUtils.MultiplyT(ref input.TransformA.R, -d));
-                vertex.WA = MathUtils.Multiply(ref input.TransformA, input.ProxyA.Vertices[vertex.IndexA]);
-
-                vertex.IndexB = input.ProxyB.GetSupport(MathUtils.MultiplyT(ref input.TransformB.R, d));
-                vertex.WB = MathUtils.Multiply(ref input.TransformB, input.ProxyB.Vertices[vertex.IndexB]);
-                vertex.W = vertex.WB - vertex.WA;
-                simplex.V[simplex.Count] = vertex;
-
-                // Iteration count is equated to the number of support point calls.
-                ++iter;
-                ++GJKIters;
-
-                // Check for duplicate support points. This is the main termination criteria.
-                bool duplicate = false;
-                for (int i = 0; i < saveCount; ++i)
-                {
-                    if (vertex.IndexA == saveA[i] && vertex.IndexB == saveB[i])
-                    {
-                        duplicate = true;
-                        break;
-                    }
-                }
-
-                // If we found a duplicate support point we must exit to avoid cycling.
-                if (duplicate)
-                {
-                    break;
-                }
-
-                // New vertex is ok and needed.
-                ++simplex.Count;
-            }
-
-            GJKMaxIters = Math.Max(GJKMaxIters, iter);
-
-            // Prepare output.
-            simplex.GetWitnessPoints(out output.PointA, out output.PointB);
-            output.Distance = (output.PointA - output.PointB).Length();
-            output.Iterations = iter;
-
-            // Cache the simplex.
-            simplex.WriteCache(ref cache);
-
-            // Apply radii if requested.
-            if (input.UseRadii)
-            {
-                float rA = input.ProxyA.Radius;
-                float rB = input.ProxyB.Radius;
-
-                if (output.Distance > rA + rB && output.Distance > Settings.Epsilon)
-                {
-                    // Shapes are still no overlapped.
-                    // Move the witness points to the outer surface.
-                    output.Distance -= rA + rB;
-                    Vector2 normal = output.PointB - output.PointA;
-                    normal.Normalize();
-                    output.PointA += rA * normal;
-                    output.PointB -= rB * normal;
-                }
-                else
-                {
-                    // Shapes are overlapped when radii are considered.
-                    // Move the witness points to the middle.
-                    Vector2 p = 0.5f * (output.PointA + output.PointB);
-                    output.PointA = p;
-                    output.PointB = p;
-                    output.Distance = 0.0f;
-                }
-            }
-        }
-    }
-}

FarseerPhysicsEngine/Collision/DynamicTree.cs

@@ -1,654 +0,0 @@
-/*
-* Farseer Physics Engine based on Box2D.XNA port:
-* Copyright (c) 2010 Ian Qvist
-* 
-* Box2D.XNA port of Box2D:
-* Copyright (c) 2009 Brandon Furtwangler, Nathan Furtwangler
-*
-* Original source Box2D:
-* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com 
-* 
-* This software is provided 'as-is', without any express or implied 
-* warranty.  In no event will the authors be held liable for any damages 
-* arising from the use of this software. 
-* Permission is granted to anyone to use this software for any purpose, 
-* including commercial applications, and to alter it and redistribute it 
-* freely, subject to the following restrictions: 
-* 1. The origin of this software must not be misrepresented; you must not 
-* claim that you wrote the original software. If you use this software 
-* in a product, an acknowledgment in the product documentation would be 
-* appreciated but is not required. 
-* 2. Altered source versions must be plainly marked as such, and must not be 
-* misrepresented as being the original software. 
-* 3. This notice may not be removed or altered from any source distribution. 
-*/
-
-using System;
-using System.Collections.Generic;
-using System.Diagnostics;
-using FarseerPhysics.Common;
-using Microsoft.Xna.Framework;
-
-namespace FarseerPhysics.Collision
-{
-    /// <summary>
-    /// A node in the dynamic tree. The client does not interact with this directly.
-    /// </summary>
-    internal struct DynamicTreeNode<T>
-    {
-        /// <summary>
-        /// This is the fattened AABB.
-        /// </summary>
-        internal AABB AABB;
-
-        internal int Child1;
-        internal int Child2;
-
-        internal int LeafCount;
-        internal int ParentOrNext;
-        internal T UserData;
-
-        internal bool IsLeaf()
-        {
-            return Child1 == DynamicTree<T>.NullNode;
-        }
-    }
-
-    /// <summary>
-    /// A dynamic tree arranges data in a binary tree to accelerate
-    /// queries such as volume queries and ray casts. Leafs are proxies
-    /// with an AABB. In the tree we expand the proxy AABB by Settings.b2_fatAABBFactor
-    /// so that the proxy AABB is bigger than the client object. This allows the client
-    /// object to move by small amounts without triggering a tree update.
-    ///
-    /// Nodes are pooled and relocatable, so we use node indices rather than pointers.
-    /// </summary>
-    public class DynamicTree<T>
-    {
-        internal const int NullNode = -1;
-        private static Stack<int> _stack = new Stack<int>(256);
-        private int _freeList;
-        private int _insertionCount;
-        private int _nodeCapacity;
-        private int _nodeCount;
-        private DynamicTreeNode<T>[] _nodes;
-
-        /// <summary>
-        /// This is used incrementally traverse the tree for re-balancing.
-        /// </summary>
-        private int _path;
-
-        private int _root;
-
-        /// <summary>
-        /// Constructing the tree initializes the node pool.
-        /// </summary>
-        public DynamicTree()
-        {
-            _root = NullNode;
-
-            _nodeCapacity = 16;
-            _nodes = new DynamicTreeNode<T>[_nodeCapacity];
-
-            // Build a linked list for the free list.
-            for (int i = 0; i < _nodeCapacity - 1; ++i)
-            {
-                _nodes[i].ParentOrNext = i + 1;
-            }
-            _nodes[_nodeCapacity - 1].ParentOrNext = NullNode;
-        }
-
-        /// <summary>
-        /// Create a proxy in the tree as a leaf node. We return the index
-        /// of the node instead of a pointer so that we can grow
-        /// the node pool.        
-        /// /// </summary>
-        /// <param name="aabb">The aabb.</param>
-        /// <param name="userData">The user data.</param>
-        /// <returns>Index of the created proxy</returns>
-        public int AddProxy(ref AABB aabb, T userData)
-        {
-            int proxyId = AllocateNode();
-
-            // Fatten the aabb.
-            Vector2 r = new Vector2(Settings.AABBExtension, Settings.AABBExtension);
-            _nodes[proxyId].AABB.LowerBound = aabb.LowerBound - r;
-            _nodes[proxyId].AABB.UpperBound = aabb.UpperBound + r;
-            _nodes[proxyId].UserData = userData;
-            _nodes[proxyId].LeafCount = 1;
-
-            InsertLeaf(proxyId);
-
-            return proxyId;
-        }
-
-        /// <summary>
-        /// Destroy a proxy. This asserts if the id is invalid.
-        /// </summary>
-        /// <param name="proxyId">The proxy id.</param>
-        public void RemoveProxy(int proxyId)
-        {
-            Debug.Assert(0 <= proxyId && proxyId < _nodeCapacity);
-            Debug.Assert(_nodes[proxyId].IsLeaf());
-
-            RemoveLeaf(proxyId);
-            FreeNode(proxyId);
-        }
-
-        /// <summary>
-        /// Move a proxy with a swepted AABB. If the proxy has moved outside of its fattened AABB,
-        /// then the proxy is removed from the tree and re-inserted. Otherwise
-        /// the function returns immediately.
-        /// </summary>
-        /// <param name="proxyId">The proxy id.</param>
-        /// <param name="aabb">The aabb.</param>
-        /// <param name="displacement">The displacement.</param>
-        /// <returns>true if the proxy was re-inserted.</returns>
-        public bool MoveProxy(int proxyId, ref AABB aabb, Vector2 displacement)
-        {
-            Debug.Assert(0 <= proxyId && proxyId < _nodeCapacity);
-
-            Debug.Assert(_nodes[proxyId].IsLeaf());
-
-            if (_nodes[proxyId].AABB.Contains(ref aabb))
-            {
-                return false;
-            }
-
-            RemoveLeaf(proxyId);
-
-            // Extend AABB.
-            AABB b = aabb;
-            Vector2 r = new Vector2(Settings.AABBExtension, Settings.AABBExtension);
-            b.LowerBound = b.LowerBound - r;
-            b.UpperBound = b.UpperBound + r;
-
-            // Predict AABB displacement.
-            Vector2 d = Settings.AABBMultiplier * displacement;
-
-            if (d.X < 0.0f)
-            {
-                b.LowerBound.X += d.X;
-            }
-            else
-            {
-                b.UpperBound.X += d.X;
-            }
-
-            if (d.Y < 0.0f)
-            {
-                b.LowerBound.Y += d.Y;
-            }
-            else
-            {
-                b.UpperBound.Y += d.Y;
-            }
-
-            _nodes[proxyId].AABB = b;
-
-            InsertLeaf(proxyId);
-            return true;
-        }
-
-        /// <summary>
-        /// Perform some iterations to re-balance the tree.
-        /// </summary>
-        /// <param name="iterations">The iterations.</param>
-        public void Rebalance(int iterations)
-        {
-            if (_root == NullNode)
-            {
-                return;
-            }
-
-            // Rebalance the tree by removing and re-inserting leaves.
-            for (int i = 0; i < iterations; ++i)
-            {
-                int node = _root;
-
-                int bit = 0;
-                while (_nodes[node].IsLeaf() == false)
-                {
-                    // Child selector based on a bit in the path
-                    int selector = (_path >> bit) & 1;
-
-                    // Select the child nod
-                    node = (selector == 0) ? _nodes[node].Child1 : _nodes[node].Child2;
-
-                    // Keep bit between 0 and 31 because _path has 32 bits
-                    // bit = (bit + 1) % 31
-                    bit = (bit + 1) & 0x1F;
-                }
-                ++_path;
-
-                RemoveLeaf(node);
-                InsertLeaf(node);
-            }
-        }
-
-        /// <summary>
-        /// Get proxy user data.
-        /// </summary>
-        /// <typeparam name="T"></typeparam>
-        /// <param name="proxyId">The proxy id.</param>
-        /// <returns>the proxy user data or 0 if the id is invalid.</returns>
-        public T GetUserData(int proxyId)
-        {
-            Debug.Assert(0 <= proxyId && proxyId < _nodeCapacity);
-            return _nodes[proxyId].UserData;
-        }
-
-        /// <summary>
-        /// Get the fat AABB for a proxy.
-        /// </summary>
-        /// <param name="proxyId">The proxy id.</param>
-        /// <param name="fatAABB">The fat AABB.</param>
-        public void GetFatAABB(int proxyId, out AABB fatAABB)
-        {
-            Debug.Assert(0 <= proxyId && proxyId < _nodeCapacity);
-            fatAABB = _nodes[proxyId].AABB;
-        }
-
-        /// <summary>
-        /// Compute the height of the binary tree in O(N) time. Should not be
-        /// called often.
-        /// </summary>
-        /// <returns></returns>
-        public int ComputeHeight()
-        {
-            return ComputeHeight(_root);
-        }
-
-        /// <summary>
-        /// Query an AABB for overlapping proxies. The callback class
-        /// is called for each proxy that overlaps the supplied AABB.
-        /// </summary>
-        /// <param name="callback">The callback.</param>
-        /// <param name="aabb">The aabb.</param>
-        public void Query(Func<int, bool> callback, ref AABB aabb)
-        {
-            _stack.Clear();
-            _stack.Push(_root);
-
-            while (_stack.Count > 0)
-            {
-                int nodeId = _stack.Pop();
-                if (nodeId == NullNode)
-                {
-                    continue;
-                }
-
-                DynamicTreeNode<T> node = _nodes[nodeId];
-
-                if (AABB.TestOverlap(ref node.AABB, ref aabb))
-                {
-                    if (node.IsLeaf())
-                    {
-                        bool proceed = callback(nodeId);
-                        if (proceed == false)
-                        {
-                            return;
-                        }
-                    }
-                    else
-                    {
-                        _stack.Push(node.Child1);
-                        _stack.Push(node.Child2);
-                    }
-                }
-            }
-        }
-
-        /// <summary>
-        /// Ray-cast against the proxies in the tree. This relies on the callback
-        /// to perform a exact ray-cast in the case were the proxy contains a Shape.
-        /// The callback also performs the any collision filtering. This has performance
-        /// roughly equal to k * log(n), where k is the number of collisions and n is the
-        /// number of proxies in the tree.
-        /// </summary>
-        /// <param name="callback">A callback class that is called for each proxy that is hit by the ray.</param>
-        /// <param name="input">The ray-cast input data. The ray extends from p1 to p1 + maxFraction * (p2 - p1).</param>
-        public void RayCast(Func<RayCastInput, int, float> callback, ref RayCastInput input)
-        {
-            Vector2 p1 = input.Point1;
-            Vector2 p2 = input.Point2;
-            Vector2 r = p2 - p1;
-            Debug.Assert(r.LengthSquared() > 0.0f);
-            r.Normalize();
-
-            // v is perpendicular to the segment.
-            Vector2 absV = MathUtils.Abs(new Vector2(-r.Y, r.X));
-
-            // Separating axis for segment (Gino, p80).
-            // |dot(v, p1 - c)| > dot(|v|, h)
-
-            float maxFraction = input.MaxFraction;
-
-            // Build a bounding box for the segment.
-            AABB segmentAABB = new AABB();
-            {
-                Vector2 t = p1 + maxFraction * (p2 - p1);
-                Vector2.Min(ref p1, ref t, out segmentAABB.LowerBound);
-                Vector2.Max(ref p1, ref t, out segmentAABB.UpperBound);
-            }
-
-            _stack.Clear();
-            _stack.Push(_root);
-
-            while (_stack.Count > 0)
-            {
-                int nodeId = _stack.Pop();
-                if (nodeId == NullNode)
-                {
-                    continue;
-                }
-
-                DynamicTreeNode<T> node = _nodes[nodeId];
-
-                if (AABB.TestOverlap(ref node.AABB, ref segmentAABB) == false)
-                {
-                    continue;
-                }
-
-                // Separating axis for segment (Gino, p80).
-                // |dot(v, p1 - c)| > dot(|v|, h)
-                Vector2 c = node.AABB.Center;
-                Vector2 h = node.AABB.Extents;
-                float separation = Math.Abs(Vector2.Dot(new Vector2(-r.Y, r.X), p1 - c)) - Vector2.Dot(absV, h);
-                if (separation > 0.0f)
-                {
-                    continue;
-                }
-
-                if (node.IsLeaf())
-                {
-                    RayCastInput subInput;
-                    subInput.Point1 = input.Point1;
-                    subInput.Point2 = input.Point2;
-                    subInput.MaxFraction = maxFraction;
-
-                    float value = callback(subInput, nodeId);
-
-                    if (value == 0.0f)
-                    {
-                        // the client has terminated the raycast.
-                        return;
-                    }
-
-                    if (value > 0.0f)
-                    {
-                        // Update segment bounding box.
-                        maxFraction = value;
-                        Vector2 t = p1 + maxFraction * (p2 - p1);
-                        segmentAABB.LowerBound = Vector2.Min(p1, t);
-                        segmentAABB.UpperBound = Vector2.Max(p1, t);
-                    }
-                }
-                else
-                {
-                    _stack.Push(node.Child1);
-                    _stack.Push(node.Child2);
-                }
-            }
-        }
-
-        private int CountLeaves(int nodeId)
-        {
-            if (nodeId == NullNode)
-            {
-                return 0;
-            }
-
-            Debug.Assert(0 <= nodeId && nodeId < _nodeCapacity);
-            DynamicTreeNode<T> node = _nodes[nodeId];
-
-            if (node.IsLeaf())
-            {
-                Debug.Assert(node.LeafCount == 1);
-                return 1;
-            }
-
-            int count1 = CountLeaves(node.Child1);
-            int count2 = CountLeaves(node.Child2);
-            int count = count1 + count2;
-            Debug.Assert(count == node.LeafCount);
-            return count;
-        }
-
-        private void Validate()
-        {
-            CountLeaves(_root);
-        }
-
-        private int AllocateNode()
-        {
-            // Expand the node pool as needed.
-            if (_freeList == NullNode)
-            {
-                Debug.Assert(_nodeCount == _nodeCapacity);
-
-                // The free list is empty. Rebuild a bigger pool.
-                DynamicTreeNode<T>[] oldNodes = _nodes;
-                _nodeCapacity *= 2;
-                _nodes = new DynamicTreeNode<T>[_nodeCapacity];
-                Array.Copy(oldNodes, _nodes, _nodeCount);
-
-                // Build a linked list for the free list. The parent
-                // pointer becomes the "next" pointer.
-                for (int i = _nodeCount; i < _nodeCapacity - 1; ++i)
-                {
-                    _nodes[i].ParentOrNext = i + 1;
-                }
-                _nodes[_nodeCapacity - 1].ParentOrNext = NullNode;
-                _freeList = _nodeCount;
-            }
-
-            // Peel a node off the free list.
-            int nodeId = _freeList;
-            _freeList = _nodes[nodeId].ParentOrNext;
-            _nodes[nodeId].ParentOrNext = NullNode;
-            _nodes[nodeId].Child1 = NullNode;
-            _nodes[nodeId].Child2 = NullNode;
-            _nodes[nodeId].LeafCount = 0;
-            ++_nodeCount;
-            return nodeId;
-        }
-
-        private void FreeNode(int nodeId)
-        {
-            Debug.Assert(0 <= nodeId && nodeId < _nodeCapacity);
-            Debug.Assert(0 < _nodeCount);
-            _nodes[nodeId].ParentOrNext = _freeList;
-            _freeList = nodeId;
-            --_nodeCount;
-        }
-
-        private void InsertLeaf(int leaf)
-        {
-            ++_insertionCount;
-
-            if (_root == NullNode)
-            {
-                _root = leaf;
-                _nodes[_root].ParentOrNext = NullNode;
-                return;
-            }
-
-            // Find the best sibling for this node
-            AABB leafAABB = _nodes[leaf].AABB;
-            int sibling = _root;
-            while (_nodes[sibling].IsLeaf() == false)
-            {
-                int child1 = _nodes[sibling].Child1;
-                int child2 = _nodes[sibling].Child2;
-
-                // Expand the node's AABB.
-                _nodes[sibling].AABB.Combine(ref leafAABB);
-                _nodes[sibling].LeafCount += 1;
-
-                float siblingArea = _nodes[sibling].AABB.Perimeter;
-                AABB parentAABB = new AABB();
-                parentAABB.Combine(ref _nodes[sibling].AABB, ref leafAABB);
-                float parentArea = parentAABB.Perimeter;
-                float cost1 = 2.0f * parentArea;
-
-                float inheritanceCost = 2.0f * (parentArea - siblingArea);
-
-                float cost2;
-                if (_nodes[child1].IsLeaf())
-                {
-                    AABB aabb = new AABB();
-                    aabb.Combine(ref leafAABB, ref _nodes[child1].AABB);
-                    cost2 = aabb.Perimeter + inheritanceCost;
-                }
-                else
-                {
-                    AABB aabb = new AABB();
-                    aabb.Combine(ref leafAABB, ref _nodes[child1].AABB);
-                    float oldArea = _nodes[child1].AABB.Perimeter;
-                    float newArea = aabb.Perimeter;
-                    cost2 = (newArea - oldArea) + inheritanceCost;
-                }
-
-                float cost3;
-                if (_nodes[child2].IsLeaf())
-                {
-                    AABB aabb = new AABB();
-                    aabb.Combine(ref leafAABB, ref _nodes[child2].AABB);
-                    cost3 = aabb.Perimeter + inheritanceCost;
-                }
-                else
-                {
-                    AABB aabb = new AABB();
-                    aabb.Combine(ref leafAABB, ref _nodes[child2].AABB);
-                    float oldArea = _nodes[child2].AABB.Perimeter;
-                    float newArea = aabb.Perimeter;
-                    cost3 = newArea - oldArea + inheritanceCost;
-                }
-
-                // Descend according to the minimum cost.
-                if (cost1 < cost2 && cost1 < cost3)
-                {
-                    break;
-                }
-
-                // Expand the node's AABB to account for the new leaf.
-                _nodes[sibling].AABB.Combine(ref leafAABB);
-
-                // Descend
-                if (cost2 < cost3)
-                {
-                    sibling = child1;
-                }
-                else
-                {
-                    sibling = child2;
-                }
-            }
-
-            // Create a new parent for the siblings.
-            int oldParent = _nodes[sibling].ParentOrNext;
-            int newParent = AllocateNode();
-            _nodes[newParent].ParentOrNext = oldParent;
-            _nodes[newParent].UserData = default(T);
-            _nodes[newParent].AABB.Combine(ref leafAABB, ref _nodes[sibling].AABB);
-            _nodes[newParent].LeafCount = _nodes[sibling].LeafCount + 1;
-
-            if (oldParent != NullNode)
-            {
-                // The sibling was not the root.
-                if (_nodes[oldParent].Child1 == sibling)
-                {
-                    _nodes[oldParent].Child1 = newParent;
-                }
-                else
-                {
-                    _nodes[oldParent].Child2 = newParent;
-                }
-
-                _nodes[newParent].Child1 = sibling;
-                _nodes[newParent].Child2 = leaf;
-                _nodes[sibling].ParentOrNext = newParent;
-                _nodes[leaf].ParentOrNext = newParent;
-            }
-            else
-            {
-                // The sibling was the root.
-                _nodes[newParent].Child1 = sibling;
-                _nodes[newParent].Child2 = leaf;
-                _nodes[sibling].ParentOrNext = newParent;
-                _nodes[leaf].ParentOrNext = newParent;
-                _root = newParent;
-            }
-        }
-
-        private void RemoveLeaf(int leaf)
-        {
-            if (leaf == _root)
-            {
-                _root = NullNode;
-                return;
-            }
-
-            int parent = _nodes[leaf].ParentOrNext;
-            int grandParent = _nodes[parent].ParentOrNext;
-            int sibling;
-            if (_nodes[parent].Child1 == leaf)
-            {
-                sibling = _nodes[parent].Child2;
-            }
-            else
-            {
-                sibling = _nodes[parent].Child1;
-            }
-
-            if (grandParent != NullNode)
-            {
-                // Destroy parent and connect sibling to grandParent.
-                if (_nodes[grandParent].Child1 == parent)
-                {
-                    _nodes[grandParent].Child1 = sibling;
-                }
-                else
-                {
-                    _nodes[grandParent].Child2 = sibling;
-                }
-                _nodes[sibling].ParentOrNext = grandParent;
-                FreeNode(parent);
-
-                // Adjust ancestor bounds.
-                parent = grandParent;
-                while (parent != NullNode)
-                {
-                    _nodes[parent].AABB.Combine(ref _nodes[_nodes[parent].Child1].AABB,
-                                                ref _nodes[_nodes[parent].Child2].AABB);
-
-                    Debug.Assert(_nodes[parent].LeafCount > 0);
-                    _nodes[parent].LeafCount -= 1;
-
-                    parent = _nodes[parent].ParentOrNext;
-                }
-            }
-            else
-            {
-                _root = sibling;
-                _nodes[sibling].ParentOrNext = NullNode;
-                FreeNode(parent);
-            }
-        }
-
-        private int ComputeHeight(int nodeId)
-        {
-            if (nodeId == NullNode)
-            {
-                return 0;
-            }
-
-            Debug.Assert(0 <= nodeId && nodeId < _nodeCapacity);
-            DynamicTreeNode<T> node = _nodes[nodeId];
-            int height1 = ComputeHeight(node.Child1);
-            int height2 = ComputeHeight(node.Child2);
-            return 1 + Math.Max(height1, height2);
-        }
-    }
-}

FarseerPhysicsEngine/Collision/DynamicTreeBroadPhase.cs

@@ -1,324 +0,0 @@
-/*
-* Farseer Physics Engine based on Box2D.XNA port:
-* Copyright (c) 2010 Ian Qvist
-* 
-* Box2D.XNA port of Box2D:
-* Copyright (c) 2009 Brandon Furtwangler, Nathan Furtwangler
-*
-* Original source Box2D:
-* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com 
-* 
-* This software is provided 'as-is', without any express or implied 
-* warranty.  In no event will the authors be held liable for any damages 
-* arising from the use of this software. 
-* Permission is granted to anyone to use this software for any purpose, 
-* including commercial applications, and to alter it and redistribute it 
-* freely, subject to the following restrictions: 
-* 1. The origin of this software must not be misrepresented; you must not 
-* claim that you wrote the original software. If you use this software 
-* in a product, an acknowledgment in the product documentation would be 
-* appreciated but is not required. 
-* 2. Altered source versions must be plainly marked as such, and must not be 
-* misrepresented as being the original software. 
-* 3. This notice may not be removed or altered from any source distribution. 
-*/
-
-using System;
-using FarseerPhysics.Dynamics;
-using Microsoft.Xna.Framework;
-
-namespace FarseerPhysics.Collision
-{
-    internal struct Pair : IComparable<Pair>
-    {
-        public int ProxyIdA;
-        public int ProxyIdB;
-
-        #region IComparable<Pair> Members
-
-        public int CompareTo(Pair other)
-        {
-            if (ProxyIdA < other.ProxyIdA)
-            {
-                return -1;
-            }
-            if (ProxyIdA == other.ProxyIdA)
-            {
-                if (ProxyIdB < other.ProxyIdB)
-                {
-                    return -1;
-                }
-                if (ProxyIdB == other.ProxyIdB)
-                {
-                    return 0;
-                }
-            }
-
-            return 1;
-        }
-
-        #endregion
-    }
-
-    /// <summary>
-    /// The broad-phase is used for computing pairs and performing volume queries and ray casts.
-    /// This broad-phase does not persist pairs. Instead, this reports potentially new pairs.
-    /// It is up to the client to consume the new pairs and to track subsequent overlap.
-    /// </summary>
-    public class DynamicTreeBroadPhase : IBroadPhase
-    {
-        private int[] _moveBuffer;
-        private int _moveCapacity;
-        private int _moveCount;
-
-        private Pair[] _pairBuffer;
-        private int _pairCapacity;
-        private int _pairCount;
-        private int _proxyCount;
-        private Func<int, bool> _queryCallback;
-        private int _queryProxyId;
-        private DynamicTree<FixtureProxy> _tree = new DynamicTree<FixtureProxy>();
-
-        public DynamicTreeBroadPhase()
-        {
-            _queryCallback = new Func<int, bool>(QueryCallback);
-
-            _pairCapacity = 16;
-            _pairBuffer = new Pair[_pairCapacity];
-
-            _moveCapacity = 16;
-            _moveBuffer = new int[_moveCapacity];
-        }
-
-        #region IBroadPhase Members
-
-        /// <summary>
-        /// Get the number of proxies.
-        /// </summary>
-        /// <value>The proxy count.</value>
-        public int ProxyCount
-        {
-            get { return _proxyCount; }
-        }
-
-        /// <summary>
-        /// Create a proxy with an initial AABB. Pairs are not reported until
-        /// UpdatePairs is called.
-        /// </summary>
-        /// <param name="aabb">The aabb.</param>
-        /// <param name="proxy">The user data.</param>
-        /// <returns></returns>
-        public int AddProxy(ref FixtureProxy proxy)
-        {
-            int proxyId = _tree.AddProxy(ref proxy.AABB, proxy);
-            ++_proxyCount;
-            BufferMove(proxyId);
-            return proxyId;
-        }
-
-        /// <summary>
-        /// Destroy a proxy. It is up to the client to remove any pairs.
-        /// </summary>
-        /// <param name="proxyId">The proxy id.</param>
-        public void RemoveProxy(int proxyId)
-        {
-            UnBufferMove(proxyId);
-            --_proxyCount;
-            _tree.RemoveProxy(proxyId);
-        }
-
-        public void MoveProxy(int proxyId, ref AABB aabb, Vector2 displacement)
-        {
-            bool buffer = _tree.MoveProxy(proxyId, ref aabb, displacement);
-            if (buffer)
-            {
-                BufferMove(proxyId);
-            }
-        }
-
-        /// <summary>
-        /// Get the AABB for a proxy.
-        /// </summary>
-        /// <param name="proxyId">The proxy id.</param>
-        /// <param name="aabb">The aabb.</param>
-        public void GetFatAABB(int proxyId, out AABB aabb)
-        {
-            _tree.GetFatAABB(proxyId, out aabb);
-        }
-
-        /// <summary>
-        /// Get user data from a proxy. Returns null if the id is invalid.
-        /// </summary>
-        /// <param name="proxyId">The proxy id.</param>
-        /// <returns></returns>
-        public FixtureProxy GetProxy(int proxyId)
-        {
-            return _tree.GetUserData(proxyId);
-        }
-
-        /// <summary>
-        /// Test overlap of fat AABBs.
-        /// </summary>
-        /// <param name="proxyIdA">The proxy id A.</param>
-        /// <param name="proxyIdB">The proxy id B.</param>
-        /// <returns></returns>
-        public bool TestOverlap(int proxyIdA, int proxyIdB)
-        {
-            AABB aabbA, aabbB;
-            _tree.GetFatAABB(proxyIdA, out aabbA);
-            _tree.GetFatAABB(proxyIdB, out aabbB);
-            return AABB.TestOverlap(ref aabbA, ref aabbB);
-        }
-
-        /// <summary>
-        /// Update the pairs. This results in pair callbacks. This can only add pairs.
-        /// </summary>
-        /// <param name="callback">The callback.</param>
-        public void UpdatePairs(BroadphaseDelegate callback)
-        {
-            // Reset pair buffer
-            _pairCount = 0;
-
-            // Perform tree queries for all moving proxies.
-            for (int j = 0; j < _moveCount; ++j)
-            {
-                _queryProxyId = _moveBuffer[j];
-                if (_queryProxyId == -1)
-                {
-                    continue;
-                }
-
-                // We have to query the tree with the fat AABB so that
-                // we don't fail to create a pair that may touch later.
-                AABB fatAABB;
-                _tree.GetFatAABB(_queryProxyId, out fatAABB);
-
-                // Query tree, create pairs and add them pair buffer.
-                _tree.Query(_queryCallback, ref fatAABB);
-            }
-
-            // Reset move buffer
-            _moveCount = 0;
-
-            // Sort the pair buffer to expose duplicates.
-            Array.Sort(_pairBuffer, 0, _pairCount);
-
-            // Send the pairs back to the client.
-            int i = 0;
-            while (i < _pairCount)
-            {
-                Pair primaryPair = _pairBuffer[i];
-                FixtureProxy userDataA = _tree.GetUserData(primaryPair.ProxyIdA);
-                FixtureProxy userDataB = _tree.GetUserData(primaryPair.ProxyIdB);
-
-                callback(ref userDataA, ref userDataB);
-                ++i;
-
-                // Skip any duplicate pairs.
-                while (i < _pairCount)
-                {
-                    Pair pair = _pairBuffer[i];
-                    if (pair.ProxyIdA != primaryPair.ProxyIdA || pair.ProxyIdB != primaryPair.ProxyIdB)
-                    {
-                        break;
-                    }
-                    ++i;
-                }
-            }
-
-            // Try to keep the tree balanced.
-            _tree.Rebalance(4);
-        }
-
-        /// <summary>
-        /// Query an AABB for overlapping proxies. The callback class
-        /// is called for each proxy that overlaps the supplied AABB.
-        /// </summary>
-        /// <param name="callback">The callback.</param>
-        /// <param name="aabb">The aabb.</param>
-        public void Query(Func<int, bool> callback, ref AABB aabb)
-        {
-            _tree.Query(callback, ref aabb);
-        }
-
-        /// <summary>
-        /// Ray-cast against the proxies in the tree. This relies on the callback
-        /// to perform a exact ray-cast in the case were the proxy contains a shape.
-        /// The callback also performs the any collision filtering. This has performance
-        /// roughly equal to k * log(n), where k is the number of collisions and n is the
-        /// number of proxies in the tree.
-        /// </summary>
-        /// <param name="callback">A callback class that is called for each proxy that is hit by the ray.</param>
-        /// <param name="input">The ray-cast input data. The ray extends from p1 to p1 + maxFraction * (p2 - p1).</param>
-        public void RayCast(Func<RayCastInput, int, float> callback, ref RayCastInput input)
-        {
-            _tree.RayCast(callback, ref input);
-        }
-
-        public void TouchProxy(int proxyId)
-        {
-            BufferMove(proxyId);
-        }
-
-        #endregion
-
-        /// <summary>
-        /// Compute the height of the embedded tree.
-        /// </summary>
-        /// <returns></returns>
-        public int ComputeHeight()
-        {
-            return _tree.ComputeHeight();
-        }
-
-        private void BufferMove(int proxyId)
-        {
-            if (_moveCount == _moveCapacity)
-            {
-                int[] oldBuffer = _moveBuffer;
-                _moveCapacity *= 2;
-                _moveBuffer = new int[_moveCapacity];
-                Array.Copy(oldBuffer, _moveBuffer, _moveCount);
-            }
-
-            _moveBuffer[_moveCount] = proxyId;
-            ++_moveCount;
-        }
-
-        private void UnBufferMove(int proxyId)
-        {
-            for (int i = 0; i < _moveCount; ++i)
-            {
-                if (_moveBuffer[i] == proxyId)
-                {
-                    _moveBuffer[i] = -1;
-                    return;
-                }
-            }
-        }
-
-        private bool QueryCallback(int proxyId)
-        {
-            // A proxy cannot form a pair with itself.
-            if (proxyId == _queryProxyId)
-            {
-                return true;
-            }
-
-            // Grow the pair buffer as needed.
-            if (_pairCount == _pairCapacity)
-            {
-                Pair[] oldBuffer = _pairBuffer;
-                _pairCapacity *= 2;
-                _pairBuffer = new Pair[_pairCapacity];
-                Array.Copy(oldBuffer, _pairBuffer, _pairCount);
-            }
-
-            _pairBuffer[_pairCount].ProxyIdA = Math.Min(proxyId, _queryProxyId);
-            _pairBuffer[_pairCount].ProxyIdB = Math.Max(proxyId, _queryProxyId);
-            ++_pairCount;
-
-            return true;
-        }
-    }
-}

FarseerPhysicsEngine/Collision/IBroadPhase.cs

@@ -1,30 +0,0 @@
-using System;
-using FarseerPhysics.Dynamics;
-using Microsoft.Xna.Framework;
-
-namespace FarseerPhysics.Collision
-{
-    public interface IBroadPhase
-    {
-        int ProxyCount { get; }
-        void UpdatePairs(BroadphaseDelegate callback);
-
-        bool TestOverlap(int proxyIdA, int proxyIdB);
-
-        int AddProxy(ref FixtureProxy proxy);
-
-        void RemoveProxy(int proxyId);
-
-        void MoveProxy(int proxyId, ref AABB aabb, Vector2 displacement);
-
-        FixtureProxy GetProxy(int proxyId);
-
-        void TouchProxy(int proxyId);
-
-        void GetFatAABB(int proxyId, out AABB aabb);
-
-        void Query(Func<int, bool> callback, ref AABB aabb);
-
-        void RayCast(Func<RayCastInput, int, float> callback, ref RayCastInput input);
-    }
-}

FarseerPhysicsEngine/Collision/QuadTree.cs

@@ -1,267 +0,0 @@
-using System;
-using System.Collections.Generic;
-using FarseerPhysics.Collision;
-using Microsoft.Xna.Framework;
-
-public class Element<T>
-{
-    public QuadTree<T> Parent;
-    public AABB Span;
-    public T Value;
-
-    public Element(T value, AABB span)
-    {
-        Span = span;
-        Value = value;
-        Parent = null;
-    }
-}
-
-public class QuadTree<T>
-{
-    public int MaxBucket;
-    public int MaxDepth;
-    public List<Element<T>> Nodes;
-    public AABB Span;
-    public QuadTree<T>[] SubTrees;
-
-    public QuadTree(AABB span, int maxbucket, int maxdepth)
-    {
-        Span = span;
-        Nodes = new List<Element<T>>();
-
-        MaxBucket = maxbucket;
-        MaxDepth = maxdepth;
-    }
-
-    public bool IsPartitioned
-    {
-        get { return SubTrees != null; }
-    }
-
-    /// <summary>
-    /// returns the quadrant of span that entirely contains test. if none, return 0.
-    /// </summary>
-    /// <param name="span"></param>
-    /// <param name="test"></param>
-    /// <returns></returns>
-    private int Partition(AABB span, AABB test)
-    {
-        if (span.Q1.Contains(ref test)) return 1;
-        if (span.Q2.Contains(ref test)) return 2;
-        if (span.Q3.Contains(ref test)) return 3;
-        if (span.Q4.Contains(ref test)) return 4;
-
-        return 0;
-    }
-
-    public void AddNode(Element<T> node)
-    {
-        if (!IsPartitioned)
-        {
-            if (Nodes.Count >= MaxBucket && MaxDepth > 0) //bin is full and can still subdivide
-            {
-                //
-                //partition into quadrants and sort existing nodes amonst quads.
-                //
-                Nodes.Add(node); //treat new node just like other nodes for partitioning
-
-                SubTrees = new QuadTree<T>[4];
-                SubTrees[0] = new QuadTree<T>(Span.Q1, MaxBucket, MaxDepth - 1);
-                SubTrees[1] = new QuadTree<T>(Span.Q2, MaxBucket, MaxDepth - 1);
-                SubTrees[2] = new QuadTree<T>(Span.Q3, MaxBucket, MaxDepth - 1);
-                SubTrees[3] = new QuadTree<T>(Span.Q4, MaxBucket, MaxDepth - 1);
-
-                List<Element<T>> remNodes = new List<Element<T>>();
-                //nodes that are not fully contained by any quadrant
-
-                foreach (Element<T> n in Nodes)
-                {
-                    switch (Partition(Span, n.Span))
-                    {
-                        case 1: //quadrant 1
-                            SubTrees[0].AddNode(n);
-                            break;
-                        case 2:
-                            SubTrees[1].AddNode(n);
-                            break;
-                        case 3:
-                            SubTrees[2].AddNode(n);
-                            break;
-                        case 4:
-                            SubTrees[3].AddNode(n);
-                            break;
-                        default:
-                            n.Parent = this;
-                            remNodes.Add(n);
-                            break;
-                    }
-                }
-
-                Nodes = remNodes;
-            }
-            else
-            {
-                node.Parent = this;
-                Nodes.Add(node);
-                //if bin is not yet full or max depth has been reached, just add the node without subdividing
-            }
-        }
-        else //we already have children nodes
-        {
-            //
-            //add node to specific sub-tree
-            //
-            switch (Partition(Span, node.Span))
-            {
-                case 1: //quadrant 1
-                    SubTrees[0].AddNode(node);
-                    break;
-                case 2:
-                    SubTrees[1].AddNode(node);
-                    break;
-                case 3:
-                    SubTrees[2].AddNode(node);
-                    break;
-                case 4:
-                    SubTrees[3].AddNode(node);
-                    break;
-                default:
-                    node.Parent = this;
-                    Nodes.Add(node);
-                    break;
-            }
-        }
-    }
-
-    /// <summary>
-    /// tests if ray intersects AABB
-    /// </summary>
-    /// <param name="aabb"></param>
-    /// <returns></returns>
-    public static bool RayCastAABB(AABB aabb, Vector2 p1, Vector2 p2)
-    {
-        AABB segmentAABB = new AABB();
-        {
-            Vector2.Min(ref p1, ref p2, out segmentAABB.LowerBound);
-            Vector2.Max(ref p1, ref p2, out segmentAABB.UpperBound);
-        }
-        if (!AABB.TestOverlap(aabb, segmentAABB)) return false;
-
-        Vector2 rayDir = p2 - p1;
-        Vector2 rayPos = p1;
-
-        Vector2 norm = new Vector2(-rayDir.Y, rayDir.X); //normal to ray
-        if (norm.Length() == 0.0)
-            return true; //if ray is just a point, return true (iff point is within aabb, as tested earlier)
-        norm.Normalize();
-
-        float dPos = Vector2.Dot(rayPos, norm);
-
-        Vector2[] verts = aabb.GetVertices();
-        float d0 = Vector2.Dot(verts[0], norm) - dPos;
-        for (int i = 1; i < 4; i++)
-        {
-            float d = Vector2.Dot(verts[i], norm) - dPos;
-            if (Math.Sign(d) != Math.Sign(d0))
-                //return true if the ray splits the vertices (ie: sign of dot products with normal are not all same)
-                return true;
-        }
-
-        return false;
-    }
-
-    public void QueryAABB(Func<Element<T>, bool> callback, ref AABB searchR)
-    {
-        Stack<QuadTree<T>> stack = new Stack<QuadTree<T>>();
-        stack.Push(this);
-
-        while (stack.Count > 0)
-        {
-            QuadTree<T> qt = stack.Pop();
-            if (!AABB.TestOverlap(ref searchR, ref qt.Span))
-                continue;
-
-            foreach (Element<T> n in qt.Nodes)
-                if (AABB.TestOverlap(ref searchR, ref n.Span))
-                {
-                    if (!callback(n)) return;
-                }
-
-            if (qt.IsPartitioned)
-                foreach (QuadTree<T> st in qt.SubTrees)
-                    stack.Push(st);
-        }
-    }
-
-    public void RayCast(Func<RayCastInput, Element<T>, float> callback, ref RayCastInput input)
-    {
-        Stack<QuadTree<T>> stack = new Stack<QuadTree<T>>();
-        stack.Push(this);
-
-        float maxFraction = input.MaxFraction;
-        Vector2 p1 = input.Point1;
-        Vector2 p2 = p1 + (input.Point2 - input.Point1) * maxFraction;
-
-        while (stack.Count > 0)
-        {
-            QuadTree<T> qt = stack.Pop();
-
-            if (!RayCastAABB(qt.Span, p1, p2))
-                continue;
-
-            foreach (Element<T> n in qt.Nodes)
-            {
-                if (!RayCastAABB(n.Span, p1, p2))
-                    continue;
-
-                RayCastInput subInput;
-                subInput.Point1 = input.Point1;
-                subInput.Point2 = input.Point2;
-                subInput.MaxFraction = maxFraction;
-
-                float value = callback(subInput, n);
-                if (value == 0.0f)
-                    return; // the client has terminated the raycast.
-
-                if (value <= 0.0f)
-                    continue;
-
-                maxFraction = value;
-                p2 = p1 + (input.Point2 - input.Point1) * maxFraction; //update segment endpoint
-            }
-            if (IsPartitioned)
-                foreach (QuadTree<T> st in qt.SubTrees)
-                    stack.Push(st);
-        }
-    }
-
-    public void GetAllNodesR(ref List<Element<T>> nodes)
-    {
-        nodes.AddRange(Nodes);
-
-        if (IsPartitioned)
-            foreach (QuadTree<T> st in SubTrees) st.GetAllNodesR(ref nodes);
-    }
-
-    public void RemoveNode(Element<T> node)
-    {
-        node.Parent.Nodes.Remove(node);
-    }
-
-    public void Reconstruct()
-    {
-        List<Element<T>> allNodes = new List<Element<T>>();
-        GetAllNodesR(ref allNodes);
-
-        Clear();
-
-        allNodes.ForEach(AddNode);
-    }
-
-    public void Clear()
-    {
-        Nodes.Clear();
-        SubTrees = null;
-    }
-}

FarseerPhysicsEngine/Collision/QuadTreeBroadPhase.cs

@@ -1,249 +0,0 @@
-using System;
-using System.Collections.Generic;
-using FarseerPhysics;
-using FarseerPhysics.Collision;
-using FarseerPhysics.Dynamics;
-using Microsoft.Xna.Framework;
-
-public class QuadTreeBroadPhase : IBroadPhase
-{
-    private const int TreeUpdateThresh = 10000;
-    private int _currID;
-    private Dictionary<int, Element<FixtureProxy>> _idRegister;
-    private List<Element<FixtureProxy>> _moveBuffer;
-    private List<Pair> _pairBuffer;
-    private QuadTree<FixtureProxy> _quadTree;
-    private int _treeMoveNum;
-
-    /// <summary>
-    /// Creates a new quad tree broadphase with the specified span.
-    /// </summary>
-    /// <param name="span">the maximum span of the tree (world size)</param>
-    public QuadTreeBroadPhase(AABB span)
-    {
-        _quadTree = new QuadTree<FixtureProxy>(span, 5, 10);
-        _idRegister = new Dictionary<int, Element<FixtureProxy>>();
-        _moveBuffer = new List<Element<FixtureProxy>>();
-        _pairBuffer = new List<Pair>();
-    }
-
-    #region IBroadPhase Members
-
-    ///<summary>
-    /// The number of proxies
-    ///</summary>
-    public int ProxyCount
-    {
-        get { return _idRegister.Count; }
-    }
-
-    public void GetFatAABB(int proxyID, out AABB aabb)
-    {
-        if (_idRegister.ContainsKey(proxyID))
-            aabb = _idRegister[proxyID].Span;
-        else
-            throw new KeyNotFoundException("proxyID not found in register");
-    }
-
-    public void UpdatePairs(BroadphaseDelegate callback)
-    {
-        _pairBuffer.Clear();
-        foreach (Element<FixtureProxy> qtnode in _moveBuffer)
-        {
-            // Query tree, create pairs and add them pair buffer.
-            Query(proxyID => PairBufferQueryCallback(proxyID, qtnode.Value.ProxyId), ref qtnode.Span);
-        }
-        _moveBuffer.Clear();
-
-        // Sort the pair buffer to expose duplicates.
-        _pairBuffer.Sort();
-
-        // Send the pairs back to the client.
-        int i = 0;
-        while (i < _pairBuffer.Count)
-        {
-            Pair primaryPair = _pairBuffer[i];
-            FixtureProxy userDataA = GetProxy(primaryPair.ProxyIdA);
-            FixtureProxy userDataB = GetProxy(primaryPair.ProxyIdB);
-
-            callback(ref userDataA, ref userDataB);
-            ++i;
-
-            // Skip any duplicate pairs.
-            while (i < _pairBuffer.Count && _pairBuffer[i].ProxyIdA == primaryPair.ProxyIdA &&
-                   _pairBuffer[i].ProxyIdB == primaryPair.ProxyIdB)
-                ++i;
-        }
-    }
-
-    /// <summary>
-    /// Test overlap of fat AABBs.
-    /// </summary>
-    /// <param name="proxyIdA">The proxy id A.</param>
-    /// <param name="proxyIdB">The proxy id B.</param>
-    /// <returns></returns>
-    public bool TestOverlap(int proxyIdA, int proxyIdB)
-    {
-        AABB aabb1;
-        AABB aabb2;
-        GetFatAABB(proxyIdA, out aabb1);
-        GetFatAABB(proxyIdB, out aabb2);
-        return AABB.TestOverlap(ref aabb1, ref aabb2);
-    }
-
-    public int AddProxy(ref FixtureProxy proxy)
-    {
-        int proxyID = _currID++;
-        proxy.ProxyId = proxyID;
-        AABB aabb = Fatten(ref proxy.AABB);
-        Element<FixtureProxy> qtnode = new Element<FixtureProxy>(proxy, aabb);
-
-        _idRegister.Add(proxyID, qtnode);
-        _quadTree.AddNode(qtnode);
-
-        return proxyID;
-    }
-
-    public void RemoveProxy(int proxyId)
-    {
-        if (_idRegister.ContainsKey(proxyId))
-        {
-            Element<FixtureProxy> qtnode = _idRegister[proxyId];
-            UnbufferMove(qtnode);
-            _idRegister.Remove(proxyId);
-            _quadTree.RemoveNode(qtnode);
-        }
-        else
-            throw new KeyNotFoundException("proxyID not found in register");
-    }
-
-    public void MoveProxy(int proxyId, ref AABB aabb, Vector2 displacement)
-    {
-        AABB fatAABB;
-        GetFatAABB(proxyId, out fatAABB);
-
-        //exit if movement is within fat aabb
-        if (fatAABB.Contains(ref aabb))
-            return;
-
-        // Extend AABB.
-        AABB b = aabb;
-        Vector2 r = new Vector2(Settings.AABBExtension, Settings.AABBExtension);
-        b.LowerBound = b.LowerBound - r;
-        b.UpperBound = b.UpperBound + r;
-
-        // Predict AABB displacement.
-        Vector2 d = Settings.AABBMultiplier * displacement;
-
-        if (d.X < 0.0f)
-            b.LowerBound.X += d.X;
-        else
-            b.UpperBound.X += d.X;
-
-        if (d.Y < 0.0f)
-            b.LowerBound.Y += d.Y;
-        else
-            b.UpperBound.Y += d.Y;
-
-
-        Element<FixtureProxy> qtnode = _idRegister[proxyId];
-        qtnode.Value.AABB = b; //not neccesary for QTree, but might be accessed externally
-        qtnode.Span = b;
-
-        ReinsertNode(qtnode);
-
-        BufferMove(qtnode);
-    }
-
-    public FixtureProxy GetProxy(int proxyId)
-    {
-        if (_idRegister.ContainsKey(proxyId))
-            return _idRegister[proxyId].Value;
-        else
-            throw new KeyNotFoundException("proxyID not found in register");
-    }
-
-    public void TouchProxy(int proxyId)
-    {
-        if (_idRegister.ContainsKey(proxyId))
-            BufferMove(_idRegister[proxyId]);
-        else
-            throw new KeyNotFoundException("proxyID not found in register");
-    }
-
-    public void Query(Func<int, bool> callback, ref AABB query)
-    {
-        _quadTree.QueryAABB(TransformPredicate(callback), ref query);
-    }
-
-    public void RayCast(Func<RayCastInput, int, float> callback, ref RayCastInput input)
-    {
-        _quadTree.RayCast(TransformRayCallback(callback), ref input);
-    }
-
-    #endregion
-
-    private AABB Fatten(ref AABB aabb)
-    {
-        Vector2 r = new Vector2(Settings.AABBExtension, Settings.AABBExtension);
-        return new AABB(aabb.LowerBound - r, aabb.UpperBound + r);
-    }
-
-    private Func<Element<FixtureProxy>, bool> TransformPredicate(Func<int, bool> idPredicate)
-    {
-        Func<Element<FixtureProxy>, bool> qtPred = qtnode => idPredicate(qtnode.Value.ProxyId);
-        return qtPred;
-    }
-
-    private Func<RayCastInput, Element<FixtureProxy>, float> TransformRayCallback(
-        Func<RayCastInput, int, float> callback)
-    {
-        Func<RayCastInput, Element<FixtureProxy>, float> newCallback =
-            (input, qtnode) => callback(input, qtnode.Value.ProxyId);
-        return newCallback;
-    }
-
-    private bool PairBufferQueryCallback(int proxyID, int baseID)
-    {
-        // A proxy cannot form a pair with itself.
-        if (proxyID == baseID)
-            return true;
-
-        Pair p = new Pair();
-        p.ProxyIdA = Math.Min(proxyID, baseID);
-        p.ProxyIdB = Math.Max(proxyID, baseID);
-        _pairBuffer.Add(p);
-
-        return true;
-    }
-
-    private void ReconstructTree()
-    {
-        //this is faster than _quadTree.Reconstruct(), since the quadtree method runs a recusive query to find all nodes.
-        _quadTree.Clear();
-        foreach (Element<FixtureProxy> elem in _idRegister.Values)
-            _quadTree.AddNode(elem);
-    }
-
-    private void ReinsertNode(Element<FixtureProxy> qtnode)
-    {
-        _quadTree.RemoveNode(qtnode);
-        _quadTree.AddNode(qtnode);
-
-        if (++_treeMoveNum > TreeUpdateThresh)
-        {
-            ReconstructTree();
-            _treeMoveNum = 0;
-        }
-    }
-
-    private void BufferMove(Element<FixtureProxy> proxy)
-    {
-        _moveBuffer.Add(proxy);
-    }
-
-    private void UnbufferMove(Element<FixtureProxy> proxy)
-    {
-        _moveBuffer.Remove(proxy);
-    }
-}

FarseerPhysicsEngine/Collision/Shapes/CircleShape.cs

@@ -1,207 +0,0 @@
-/*
-* Farseer Physics Engine based on Box2D.XNA port:
-* Copyright (c) 2010 Ian Qvist
-* 
-* Box2D.XNA port of Box2D:
-* Copyright (c) 2009 Brandon Furtwangler, Nathan Furtwangler
-*
-* Original source Box2D:
-* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com 
-* 
-* This software is provided 'as-is', without any express or implied 
-* warranty.  In no event will the authors be held liable for any damages 
-* arising from the use of this software. 
-* Permission is granted to anyone to use this software for any purpose, 
-* including commercial applications, and to alter it and redistribute it 
-* freely, subject to the following restrictions: 
-* 1. The origin of this software must not be misrepresented; you must not 
-* claim that you wrote the original software. If you use this software 
-* in a product, an acknowledgment in the product documentation would be 
-* appreciated but is not required. 
-* 2. Altered source versions must be plainly marked as such, and must not be 
-* misrepresented as being the original software. 
-* 3. This notice may not be removed or altered from any source distribution. 
-*/
-
-using System;
-using FarseerPhysics.Common;
-using Microsoft.Xna.Framework;
-
-namespace FarseerPhysics.Collision.Shapes
-{
-    public class CircleShape : Shape
-    {
-        internal Vector2 _position;
-
-        public CircleShape(float radius, float density)
-            : base(density)
-        {
-            ShapeType = ShapeType.Circle;
-            _radius = radius;
-            _position = Vector2.Zero;
-            ComputeProperties();
-        }
-
-        internal CircleShape()
-            : base(0)
-        {
-            ShapeType = ShapeType.Circle;
-            _radius = 0.0f;
-            _position = Vector2.Zero;
-        }
-
-        public override int ChildCount
-        {
-            get { return 1; }
-        }
-
-        public Vector2 Position
-        {
-            get { return _position; }
-            set
-            {
-                _position = value;
-                ComputeProperties();
-            }
-        }
-
-        public override Shape Clone()
-        {
-            CircleShape shape = new CircleShape();
-            shape._radius = Radius;
-            shape._density = _density;
-            shape._position = _position;
-            shape.ShapeType = ShapeType;
-            shape.MassData = MassData;
-            return shape;
-        }
-
-        /// <summary>
-        /// Test a point for containment in this shape. This only works for convex shapes.
-        /// </summary>
-        /// <param name="transform">The shape world transform.</param>
-        /// <param name="point">a point in world coordinates.</param>
-        /// <returns>True if the point is inside the shape</returns>
-        public override bool TestPoint(ref Transform transform, ref Vector2 point)
-        {
-            Vector2 center = transform.Position + MathUtils.Multiply(ref transform.R, Position);
-            Vector2 d = point - center;
-            return Vector2.Dot(d, d) <= Radius * Radius;
-        }
-
-        /// <summary>
-        /// Cast a ray against a child shape.
-        /// </summary>
-        /// <param name="output">The ray-cast results.</param>
-        /// <param name="input">The ray-cast input parameters.</param>
-        /// <param name="transform">The transform to be applied to the shape.</param>
-        /// <param name="childIndex">The child shape index.</param>
-        /// <returns>True if the ray-cast hits the shape</returns>
-        public override bool RayCast(out RayCastOutput output, ref RayCastInput input, ref Transform transform,
-                                     int childIndex)
-        {
-            // Collision Detection in Interactive 3D Environments by Gino van den Bergen
-            // From Section 3.1.2
-            // x = s + a * r
-            // norm(x) = radius
-
-            output = new RayCastOutput();
-
-            Vector2 position = transform.Position + MathUtils.Multiply(ref transform.R, Position);
-            Vector2 s = input.Point1 - position;
-            float b = Vector2.Dot(s, s) - Radius * Radius;
-
-            // Solve quadratic equation.
-            Vector2 r = input.Point2 - input.Point1;
-            float c = Vector2.Dot(s, r);
-            float rr = Vector2.Dot(r, r);
-            float sigma = c * c - rr * b;
-
-            // Check for negative discriminant and short segment.
-            if (sigma < 0.0f || rr < Settings.Epsilon)
-            {
-                return false;
-            }
-
-            // Find the point of intersection of the line with the circle.
-            float a = -(c + (float)Math.Sqrt(sigma));
-
-            // Is the intersection point on the segment?
-            if (0.0f <= a && a <= input.MaxFraction * rr)
-            {
-                a /= rr;
-                output.Fraction = a;
-                Vector2 norm = (s + a * r);
-                norm.Normalize();
-                output.Normal = norm;
-                return true;
-            }
-
-            return false;
-        }
-
-        /// <summary>
-        /// Given a transform, compute the associated axis aligned bounding box for a child shape.
-        /// </summary>
-        /// <param name="aabb">The aabb results.</param>
-        /// <param name="transform">The world transform of the shape.</param>
-        /// <param name="childIndex">The child shape index.</param>
-        public override void ComputeAABB(out AABB aabb, ref Transform transform, int childIndex)
-        {
-            Vector2 p = transform.Position + MathUtils.Multiply(ref transform.R, Position);
-            aabb.LowerBound = new Vector2(p.X - Radius, p.Y - Radius);
-            aabb.UpperBound = new Vector2(p.X + Radius, p.Y + Radius);
-        }
-
-        /// <summary>
-        /// Compute the mass properties of this shape using its dimensions and density.
-        /// The inertia tensor is computed about the local origin, not the centroid.
-        /// </summary>
-        public override sealed void ComputeProperties()
-        {
-            float area = Settings.Pi * Radius * Radius;
-            MassData.Area = area;
-            MassData.Mass = Density * area;
-            MassData.Centroid = Position;
-
-            // inertia about the local origin
-            MassData.Inertia = MassData.Mass * (0.5f * Radius * Radius + Vector2.Dot(Position, Position));
-        }
-
-        public bool CompareTo(CircleShape shape)
-        {
-            return (Radius == shape.Radius &&
-                    Position == shape.Position);
-        }
-
-        public override float ComputeSubmergedArea(Vector2 normal, float offset, Transform xf, out Vector2 sc)
-        {
-            sc = Vector2.Zero;
-
-            Vector2 p = MathUtils.Multiply(ref xf, Position);
-            float l = -(Vector2.Dot(normal, p) - offset);
-            if (l < -Radius + Settings.Epsilon)
-            {
-                //Completely dry
-                return 0;
-            }
-            if (l > Radius)
-            {
-                //Completely wet
-                sc = p;
-                return Settings.Pi * Radius * Radius;
-            }
-
-            //Magic
-            float r2 = Radius * Radius;
-            float l2 = l * l;
-            float area = r2 * (float)((Math.Asin(l / Radius) + Settings.Pi / 2) + l * Math.Sqrt(r2 - l2));
-            float com = -2.0f / 3.0f * (float)Math.Pow(r2 - l2, 1.5f) / area;
-
-            sc.X = p.X + normal.X * com;
-            sc.Y = p.Y + normal.Y * com;
-
-            return area;
-        }
-    }
-}

FarseerPhysicsEngine/Collision/Shapes/EdgeShape.cs

@@ -1,266 +0,0 @@
-/*
-* Farseer Physics Engine based on Box2D.XNA port:
-* Copyright (c) 2010 Ian Qvist
-* 
-* Box2D.XNA port of Box2D:
-* Copyright (c) 2009 Brandon Furtwangler, Nathan Furtwangler
-*
-* Original source Box2D:
-* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com 
-* 
-* This software is provided 'as-is', without any express or implied 
-* warranty.  In no event will the authors be held liable for any damages 
-* arising from the use of this software. 
-* Permission is granted to anyone to use this software for any purpose, 
-* including commercial applications, and to alter it and redistribute it 
-* freely, subject to the following restrictions: 
-* 1. The origin of this software must not be misrepresented; you must not 
-* claim that you wrote the original software. If you use this software 
-* in a product, an acknowledgment in the product documentation would be 
-* appreciated but is not required. 
-* 2. Altered source versions must be plainly marked as such, and must not be 
-* misrepresented as being the original software. 
-* 3. This notice may not be removed or altered from any source distribution. 
-*/
-
-using FarseerPhysics.Common;
-using Microsoft.Xna.Framework;
-
-namespace FarseerPhysics.Collision.Shapes
-{
-    /// <summary>
-    /// A line segment (edge) Shape. These can be connected in chains or loops
-    /// to other edge Shapes. The connectivity information is used to ensure
-    /// correct contact normals.
-    /// </summary>
-    public class EdgeShape : Shape
-    {
-        public bool HasVertex0, HasVertex3;
-
-        /// <summary>
-        /// Optional adjacent vertices. These are used for smooth collision.
-        /// </summary>
-        public Vector2 Vertex0;
-
-        /// <summary>
-        /// Optional adjacent vertices. These are used for smooth collision.
-        /// </summary>
-        public Vector2 Vertex3;
-
-        /// <summary>
-        /// Edge start vertex
-        /// </summary>
-        private Vector2 _vertex1;
-
-        /// <summary>
-        /// Edge end vertex
-        /// </summary>
-        private Vector2 _vertex2;
-
-        internal EdgeShape()
-            : base(0)
-        {
-            ShapeType = ShapeType.Edge;
-            _radius = Settings.PolygonRadius;
-        }
-
-        public EdgeShape(Vector2 start, Vector2 end)
-            : base(0)
-        {
-            ShapeType = ShapeType.Edge;
-            _radius = Settings.PolygonRadius;
-            Set(start, end);
-        }
-
-        public override int ChildCount
-        {
-            get { return 1; }
-        }
-
-        /// <summary>
-        /// These are the edge vertices
-        /// </summary>
-        public Vector2 Vertex1
-        {
-            get { return _vertex1; }
-            set
-            {
-                _vertex1 = value;
-                ComputeProperties();
-            }
-        }
-
-        /// <summary>
-        /// These are the edge vertices
-        /// </summary>
-        public Vector2 Vertex2
-        {
-            get { return _vertex2; }
-            set
-            {
-                _vertex2 = value;
-                ComputeProperties();
-            }
-        }
-
-        /// <summary>
-        /// Set this as an isolated edge.
-        /// </summary>
-        /// <param name="start">The start.</param>
-        /// <param name="end">The end.</param>
-        public void Set(Vector2 start, Vector2 end)
-        {
-            _vertex1 = start;
-            _vertex2 = end;
-            HasVertex0 = false;
-            HasVertex3 = false;
-
-            ComputeProperties();
-        }
-
-        public override Shape Clone()
-        {
-            EdgeShape edge = new EdgeShape();
-            edge._radius = _radius;
-            edge._density = _density;
-            edge.HasVertex0 = HasVertex0;
-            edge.HasVertex3 = HasVertex3;
-            edge.Vertex0 = Vertex0;
-            edge._vertex1 = _vertex1;
-            edge._vertex2 = _vertex2;
-            edge.Vertex3 = Vertex3;
-            edge.MassData = MassData;
-            return edge;
-        }
-
-        /// <summary>
-        /// Test a point for containment in this shape. This only works for convex shapes.
-        /// </summary>
-        /// <param name="transform">The shape world transform.</param>
-        /// <param name="point">a point in world coordinates.</param>
-        /// <returns>True if the point is inside the shape</returns>
-        public override bool TestPoint(ref Transform transform, ref Vector2 point)
-        {
-            return false;
-        }
-
-        /// <summary>
-        /// Cast a ray against a child shape.
-        /// </summary>
-        /// <param name="output">The ray-cast results.</param>
-        /// <param name="input">The ray-cast input parameters.</param>
-        /// <param name="transform">The transform to be applied to the shape.</param>
-        /// <param name="childIndex">The child shape index.</param>
-        /// <returns>True if the ray-cast hits the shape</returns>
-        public override bool RayCast(out RayCastOutput output, ref RayCastInput input,
-                                     ref Transform transform, int childIndex)
-        {
-            // p = p1 + t * d
-            // v = v1 + s * e
-            // p1 + t * d = v1 + s * e
-            // s * e - t * d = p1 - v1
-
-            output = new RayCastOutput();
-
-            // Put the ray into the edge's frame of reference.
-            Vector2 p1 = MathUtils.MultiplyT(ref transform.R, input.Point1 - transform.Position);
-            Vector2 p2 = MathUtils.MultiplyT(ref transform.R, input.Point2 - transform.Position);
-            Vector2 d = p2 - p1;
-
-            Vector2 v1 = _vertex1;
-            Vector2 v2 = _vertex2;
-            Vector2 e = v2 - v1;
-            Vector2 normal = new Vector2(e.Y, -e.X);
-            normal.Normalize();
-
-            // q = p1 + t * d
-            // dot(normal, q - v1) = 0
-            // dot(normal, p1 - v1) + t * dot(normal, d) = 0
-            float numerator = Vector2.Dot(normal, v1 - p1);
-            float denominator = Vector2.Dot(normal, d);
-
-            if (denominator == 0.0f)
-            {
-                return false;
-            }
-
-            float t = numerator / denominator;
-            if (t < 0.0f || 1.0f < t)
-            {
-                return false;
-            }
-
-            Vector2 q = p1 + t * d;
-
-            // q = v1 + s * r
-            // s = dot(q - v1, r) / dot(r, r)
-            Vector2 r = v2 - v1;
-            float rr = Vector2.Dot(r, r);
-            if (rr == 0.0f)
-            {
-                return false;
-            }
-
-            float s = Vector2.Dot(q - v1, r) / rr;
-            if (s < 0.0f || 1.0f < s)
-            {
-                return false;
-            }
-
-            output.Fraction = t;
-            if (numerator > 0.0f)
-            {
-                output.Normal = -normal;
-            }
-            else
-            {
-                output.Normal = normal;
-            }
-            return true;
-        }
-
-        /// <summary>
-        /// Given a transform, compute the associated axis aligned bounding box for a child shape.
-        /// </summary>
-        /// <param name="aabb">The aabb results.</param>
-        /// <param name="transform">The world transform of the shape.</param>
-        /// <param name="childIndex">The child shape index.</param>
-        public override void ComputeAABB(out AABB aabb, ref Transform transform, int childIndex)
-        {
-            Vector2 v1 = MathUtils.Multiply(ref transform, _vertex1);
-            Vector2 v2 = MathUtils.Multiply(ref transform, _vertex2);
-
-            Vector2 lower = Vector2.Min(v1, v2);
-            Vector2 upper = Vector2.Max(v1, v2);
-
-            Vector2 r = new Vector2(Radius, Radius);
-            aabb.LowerBound = lower - r;
-            aabb.UpperBound = upper + r;
-        }
-
-        /// <summary>
-        /// Compute the mass properties of this shape using its dimensions and density.
-        /// The inertia tensor is computed about the local origin, not the centroid.
-        /// </summary>
-        public override void ComputeProperties()
-        {
-            MassData.Centroid = 0.5f * (_vertex1 + _vertex2);
-        }
-
-        public override float ComputeSubmergedArea(Vector2 normal, float offset, Transform xf, out Vector2 sc)
-        {
-            sc = Vector2.Zero;
-            return 0;
-        }
-
-        public bool CompareTo(EdgeShape shape)
-        {
-            return (HasVertex0 == shape.HasVertex0 &&
-                    HasVertex3 == shape.HasVertex3 &&
-                    Vertex0 == shape.Vertex0 &&
-                    Vertex1 == shape.Vertex1 &&
-                    Vertex2 == shape.Vertex2 &&
-                    Vertex3 == shape.Vertex3);
-        }
-    }
-}

FarseerPhysicsEngine/Collision/Shapes/LoopShape.cs

@@ -1,186 +0,0 @@
-/*
-* Farseer Physics Engine based on Box2D.XNA port:
-* Copyright (c) 2010 Ian Qvist
-* 
-* Box2D.XNA port of Box2D:
-* Copyright (c) 2009 Brandon Furtwangler, Nathan Furtwangler
-*
-* Original source Box2D:
-* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com 
-* 
-* This software is provided 'as-is', without any express or implied 
-* warranty.  In no event will the authors be held liable for any damages 
-* arising from the use of this software. 
-* Permission is granted to anyone to use this software for any purpose, 
-* including commercial applications, and to alter it and redistribute it 
-* freely, subject to the following restrictions: 
-* 1. The origin of this software must not be misrepresented; you must not 
-* claim that you wrote the original software. If you use this software 
-* in a product, an acknowledgment in the product documentation would be 
-* appreciated but is not required. 
-* 2. Altered source versions must be plainly marked as such, and must not be 
-* misrepresented as being the original software. 
-* 3. This notice may not be removed or altered from any source distribution. 
-*/
-
-using System.Diagnostics;
-using FarseerPhysics.Common;
-using Microsoft.Xna.Framework;
-
-namespace FarseerPhysics.Collision.Shapes
-{
-    /// <summary>
-    /// A loop Shape is a free form sequence of line segments that form a circular list.
-    /// The loop may cross upon itself, but this is not recommended for smooth collision.
-    /// The loop has double sided collision, so you can use inside and outside collision.
-    /// Therefore, you may use any winding order.
-    /// </summary>
-    public class LoopShape : Shape
-    {
-        private static EdgeShape _edgeShape = new EdgeShape();
-
-        /// <summary>
-        /// The vertices. These are not owned/freed by the loop Shape.
-        /// </summary>
-        public Vertices Vertices;
-
-        private LoopShape()
-            : base(0)
-        {
-            ShapeType = ShapeType.Loop;
-            _radius = Settings.PolygonRadius;
-        }
-
-        public LoopShape(Vertices vertices)
-            : base(0)
-        {
-            ShapeType = ShapeType.Loop;
-            _radius = Settings.PolygonRadius;
-
-            if (Settings.ConserveMemory)
-                Vertices = vertices;
-            else
-                // Copy vertices.
-                Vertices = new Vertices(vertices);
-        }
-
-        public override int ChildCount
-        {
-            get { return Vertices.Count; }
-        }
-
-        public override Shape Clone()
-        {
-            LoopShape loop = new LoopShape();
-            loop._density = _density;
-            loop._radius = _radius;
-            loop.Vertices = Vertices;
-            loop.MassData = MassData;
-            return loop;
-        }
-
-        /// <summary>
-        /// Get a child edge.
-        /// </summary>
-        /// <param name="edge">The edge.</param>
-        /// <param name="index">The index.</param>
-        public void GetChildEdge(ref EdgeShape edge, int index)
-        {
-            Debug.Assert(2 <= Vertices.Count);
-            Debug.Assert(0 <= index && index < Vertices.Count);
-            edge.ShapeType = ShapeType.Edge;
-            edge._radius = _radius;
-            edge.HasVertex0 = true;
-            edge.HasVertex3 = true;
-
-            int i0 = index - 1 >= 0 ? index - 1 : Vertices.Count - 1;
-            int i1 = index;
-            int i2 = index + 1 < Vertices.Count ? index + 1 : 0;
-            int i3 = index + 2;
-            while (i3 >= Vertices.Count)
-            {
-                i3 -= Vertices.Count;
-            }
-
-            edge.Vertex0 = Vertices[i0];
-            edge.Vertex1 = Vertices[i1];
-            edge.Vertex2 = Vertices[i2];
-            edge.Vertex3 = Vertices[i3];
-        }
-
-        /// <summary>
-        /// Test a point for containment in this shape. This only works for convex shapes.
-        /// </summary>
-        /// <param name="transform">The shape world transform.</param>
-        /// <param name="point">a point in world coordinates.</param>
-        /// <returns>True if the point is inside the shape</returns>
-        public override bool TestPoint(ref Transform transform, ref Vector2 point)
-        {
-            return false;
-        }
-
-        /// <summary>
-        /// Cast a ray against a child shape.
-        /// </summary>
-        /// <param name="output">The ray-cast results.</param>
-        /// <param name="input">The ray-cast input parameters.</param>
-        /// <param name="transform">The transform to be applied to the shape.</param>
-        /// <param name="childIndex">The child shape index.</param>
-        /// <returns>True if the ray-cast hits the shape</returns>
-        public override bool RayCast(out RayCastOutput output, ref RayCastInput input,
-                                     ref Transform transform, int childIndex)
-        {
-            Debug.Assert(childIndex < Vertices.Count);
-
-            int i1 = childIndex;
-            int i2 = childIndex + 1;
-            if (i2 == Vertices.Count)
-            {
-                i2 = 0;
-            }
-
-            _edgeShape.Vertex1 = Vertices[i1];
-            _edgeShape.Vertex2 = Vertices[i2];
-
-            return _edgeShape.RayCast(out output, ref input, ref transform, 0);
-        }
-
-        /// <summary>
-        /// Given a transform, compute the associated axis aligned bounding box for a child shape.
-        /// </summary>
-        /// <param name="aabb">The aabb results.</param>
-        /// <param name="transform">The world transform of the shape.</param>
-        /// <param name="childIndex">The child shape index.</param>
-        public override void ComputeAABB(out AABB aabb, ref Transform transform, int childIndex)
-        {
-            Debug.Assert(childIndex < Vertices.Count);
-
-            int i1 = childIndex;
-            int i2 = childIndex + 1;
-            if (i2 == Vertices.Count)
-            {
-                i2 = 0;
-            }
-
-            Vector2 v1 = MathUtils.Multiply(ref transform, Vertices[i1]);
-            Vector2 v2 = MathUtils.Multiply(ref transform, Vertices[i2]);
-
-            aabb.LowerBound = Vector2.Min(v1, v2);
-            aabb.UpperBound = Vector2.Max(v1, v2);
-        }
-
-        /// <summary>
-        /// Chains have zero mass.
-        /// </summary>
-        public override void ComputeProperties()
-        {
-            //Does nothing. Loop shapes don't have properties.
-        }
-
-        public override float ComputeSubmergedArea(Vector2 normal, float offset, Transform xf, out Vector2 sc)
-        {
-            sc = Vector2.Zero;
-            return 0;
-        }
-    }
-}

FarseerPhysicsEngine/Collision/Shapes/PolygonShape.cs

@@ -1,550 +0,0 @@
-/*
-* Farseer Physics Engine based on Box2D.XNA port:
-* Copyright (c) 2010 Ian Qvist
-* 
-* Box2D.XNA port of Box2D:
-* Copyright (c) 2009 Brandon Furtwangler, Nathan Furtwangler
-*
-* Original source Box2D:
-* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com 
-* 
-* This software is provided 'as-is', without any express or implied 
-* warranty.  In no event will the authors be held liable for any damages 
-* arising from the use of this software. 
-* Permission is granted to anyone to use this software for any purpose, 
-* including commercial applications, and to alter it and redistribute it 
-* freely, subject to the following restrictions: 
-* 1. The origin of this software must not be misrepresented; you must not 
-* claim that you wrote the original software. If you use this software 
-* in a product, an acknowledgment in the product documentation would be 
-* appreciated but is not required. 
-* 2. Altered source versions must be plainly marked as such, and must not be 
-* misrepresented as being the original software. 
-* 3. This notice may not be removed or altered from any source distribution. 
-*/
-
-using System.Diagnostics;
-using FarseerPhysics.Common;
-using FarseerPhysics.Common.Decomposition;
-using Microsoft.Xna.Framework;
-
-namespace FarseerPhysics.Collision.Shapes
-{
-    /// <summary>
-    /// Represents a simple non-selfintersecting convex polygon.
-    /// If you want to have concave polygons, you will have to use the <see cref="BayazitDecomposer"/> or the <see cref="EarclipDecomposer"/>
-    /// to decompose the concave polygon into 2 or more convex polygons.
-    /// </summary>
-    public class PolygonShape : Shape
-    {
-        public Vertices Normals;
-        public Vertices Vertices;
-
-        /// <summary>
-        /// Initializes a new instance of the <see cref="PolygonShape"/> class.
-        /// </summary>
-        /// <param name="vertices">The vertices.</param>
-        /// <param name="density">The density.</param>
-        public PolygonShape(Vertices vertices, float density)
-            : base(density)
-        {
-            ShapeType = ShapeType.Polygon;
-            _radius = Settings.PolygonRadius;
-
-            Set(vertices);
-        }
-
-        public PolygonShape(float density)
-            : base(density)
-        {
-            ShapeType = ShapeType.Polygon;
-            _radius = Settings.PolygonRadius;
-            Normals = new Vertices();
-            Vertices = new Vertices();
-        }
-
-        internal PolygonShape()
-            : base(0)
-        {
-            ShapeType = ShapeType.Polygon;
-            _radius = Settings.PolygonRadius;
-            Normals = new Vertices();
-            Vertices = new Vertices();
-        }
-
-        public override int ChildCount
-        {
-            get { return 1; }
-        }
-
-        public override Shape Clone()
-        {
-            PolygonShape clone = new PolygonShape();
-            clone.ShapeType = ShapeType;
-            clone._radius = _radius;
-            clone._density = _density;
-
-            if (Settings.ConserveMemory)
-            {
-                clone.Vertices = Vertices;
-                clone.Normals = Normals;
-            }
-            else
-            {
-                clone.Vertices = new Vertices(Vertices);
-                clone.Normals = new Vertices(Normals);
-            }
-
-            clone.MassData = MassData;
-            return clone;
-        }
-
-        /// <summary>
-        /// Copy vertices. This assumes the vertices define a convex polygon.
-        /// It is assumed that the exterior is the the right of each edge.
-        /// </summary>
-        /// <param name="vertices">The vertices.</param>
-        public void Set(Vertices vertices)
-        {
-            Debug.Assert(vertices.Count >= 3 && vertices.Count <= Settings.MaxPolygonVertices);
-
-            if (Settings.ConserveMemory)
-                Vertices = vertices;
-            else
-                // Copy vertices.
-                Vertices = new Vertices(vertices);
-
-            Normals = new Vertices(vertices.Count);
-
-            // Compute normals. Ensure the edges have non-zero length.
-            for (int i = 0; i < vertices.Count; ++i)
-            {
-                int i1 = i;
-                int i2 = i + 1 < vertices.Count ? i + 1 : 0;
-                Vector2 edge = Vertices[i2] - Vertices[i1];
-                Debug.Assert(edge.LengthSquared() > Settings.Epsilon * Settings.Epsilon);
-
-                Vector2 temp = new Vector2(edge.Y, -edge.X);
-                temp.Normalize();
-                Normals.Add(temp);
-            }
-
-#if DEBUG
-            // Ensure the polygon is convex and the interior
-            // is to the left of each edge.
-            for (int i = 0; i < Vertices.Count; ++i)
-            {
-                int i1 = i;
-                int i2 = i + 1 < Vertices.Count ? i + 1 : 0;
-                Vector2 edge = Vertices[i2] - Vertices[i1];
-
-                for (int j = 0; j < vertices.Count; ++j)
-                {
-                    // Don't check vertices on the current edge.
-                    if (j == i1 || j == i2)
-                    {
-                        continue;
-                    }
-
-                    Vector2 r = Vertices[j] - Vertices[i1];
-
-                    // Your polygon is non-convex (it has an indentation) or
-                    // has colinear edges.
-                    float s = edge.X * r.Y - edge.Y * r.X;
-
-                    Debug.Assert(s > 0.0f);
-                }
-            }
-#endif
-
-            // Compute the polygon mass data
-            ComputeProperties();
-        }
-
-        /// <summary>
-        /// Compute the mass properties of this shape using its dimensions and density.
-        /// The inertia tensor is computed about the local origin, not the centroid.
-        /// </summary>
-        public override void ComputeProperties()
-        {
-            // Polygon mass, centroid, and inertia.
-            // Let rho be the polygon density in mass per unit area.
-            // Then:
-            // mass = rho * int(dA)
-            // centroid.X = (1/mass) * rho * int(x * dA)
-            // centroid.Y = (1/mass) * rho * int(y * dA)
-            // I = rho * int((x*x + y*y) * dA)
-            //
-            // We can compute these integrals by summing all the integrals
-            // for each triangle of the polygon. To evaluate the integral
-            // for a single triangle, we make a change of variables to
-            // the (u,v) coordinates of the triangle:
-            // x = x0 + e1x * u + e2x * v
-            // y = y0 + e1y * u + e2y * v
-            // where 0 <= u && 0 <= v && u + v <= 1.
-            //
-            // We integrate u from [0,1-v] and then v from [0,1].
-            // We also need to use the Jacobian of the transformation:
-            // D = cross(e1, e2)
-            //
-            // Simplification: triangle centroid = (1/3) * (p1 + p2 + p3)
-            //
-            // The rest of the derivation is handled by computer algebra.
-
-            Debug.Assert(Vertices.Count >= 3);
-
-            if (_density <= 0)
-                return;
-
-            Vector2 center = Vector2.Zero;
-            float area = 0.0f;
-            float I = 0.0f;
-
-            // pRef is the reference point for forming triangles.
-            // It's location doesn't change the result (except for rounding error).
-            Vector2 pRef = Vector2.Zero;
-
-#if false
-    // This code would put the reference point inside the polygon.
-	        for (int i = 0; i < count; ++i)
-	        {
-		        pRef += vs[i];
-	        }
-	        pRef *= 1.0f / count;
-#endif
-
-            const float inv3 = 1.0f / 3.0f;
-
-            for (int i = 0; i < Vertices.Count; ++i)
-            {
-                // Triangle vertices.
-                Vector2 p1 = pRef;
-                Vector2 p2 = Vertices[i];
-                Vector2 p3 = i + 1 < Vertices.Count ? Vertices[i + 1] : Vertices[0];
-
-                Vector2 e1 = p2 - p1;
-                Vector2 e2 = p3 - p1;
-
-                float d;
-                MathUtils.Cross(ref e1, ref e2, out d);
-
-                float triangleArea = 0.5f * d;
-                area += triangleArea;
-
-                // Area weighted centroid
-                center += triangleArea * inv3 * (p1 + p2 + p3);
-
-                float px = p1.X, py = p1.Y;
-                float ex1 = e1.X, ey1 = e1.Y;
-                float ex2 = e2.X, ey2 = e2.Y;
-
-                float intx2 = inv3 * (0.25f * (ex1 * ex1 + ex2 * ex1 + ex2 * ex2) + (px * ex1 + px * ex2)) +
-                              0.5f * px * px;
-                float inty2 = inv3 * (0.25f * (ey1 * ey1 + ey2 * ey1 + ey2 * ey2) + (py * ey1 + py * ey2)) +
-                              0.5f * py * py;
-
-                I += d * (intx2 + inty2);
-            }
-
-            //The area is too small for the engine to handle.
-            Debug.Assert(area > Settings.Epsilon);
-
-            // We save the area
-            MassData.Area = area;
-
-            // Total mass
-            MassData.Mass = _density * area;
-
-            // Center of mass
-            center *= 1.0f / area;
-            MassData.Centroid = center;
-
-            // Inertia tensor relative to the local origin.
-            MassData.Inertia = _density * I;
-        }
-
-        /// <summary>
-        /// Build vertices to represent an axis-aligned box.
-        /// </summary>
-        /// <param name="halfWidth">The half-width.</param>
-        /// <param name="halfHeight">The half-height.</param>
-        public void SetAsBox(float halfWidth, float halfHeight)
-        {
-            Set(PolygonTools.CreateRectangle(halfWidth, halfHeight));
-        }
-
-        /// <summary>
-        /// Build vertices to represent an oriented box.
-        /// </summary>
-        /// <param name="halfWidth">The half-width..</param>
-        /// <param name="halfHeight">The half-height.</param>
-        /// <param name="center">The center of the box in local coordinates.</param>
-        /// <param name="angle">The rotation of the box in local coordinates.</param>
-        public void SetAsBox(float halfWidth, float halfHeight, Vector2 center, float angle)
-        {
-            Set(PolygonTools.CreateRectangle(halfWidth, halfHeight, center, angle));
-        }
-
-        /// <summary>
-        /// Test a point for containment in this shape. This only works for convex shapes.
-        /// </summary>
-        /// <param name="transform">The shape world transform.</param>
-        /// <param name="point">a point in world coordinates.</param>
-        /// <returns>True if the point is inside the shape</returns>
-        public override bool TestPoint(ref Transform transform, ref Vector2 point)
-        {
-            Vector2 pLocal = MathUtils.MultiplyT(ref transform.R, point - transform.Position);
-
-            for (int i = 0; i < Vertices.Count; ++i)
-            {
-                float dot = Vector2.Dot(Normals[i], pLocal - Vertices[i]);
-                if (dot > 0.0f)
-                {
-                    return false;
-                }
-            }
-
-            return true;
-        }
-
-        /// <summary>
-        /// Cast a ray against a child shape.
-        /// </summary>
-        /// <param name="output">The ray-cast results.</param>
-        /// <param name="input">The ray-cast input parameters.</param>
-        /// <param name="transform">The transform to be applied to the shape.</param>
-        /// <param name="childIndex">The child shape index.</param>
-        /// <returns>True if the ray-cast hits the shape</returns>
-        public override bool RayCast(out RayCastOutput output, ref RayCastInput input, ref Transform transform,
-                                     int childIndex)
-        {
-            output = new RayCastOutput();
-
-            // Put the ray into the polygon's frame of reference.
-            Vector2 p1 = MathUtils.MultiplyT(ref transform.R, input.Point1 - transform.Position);
-            Vector2 p2 = MathUtils.MultiplyT(ref transform.R, input.Point2 - transform.Position);
-            Vector2 d = p2 - p1;
-
-            float lower = 0.0f, upper = input.MaxFraction;
-
-            int index = -1;
-
-            for (int i = 0; i < Vertices.Count; ++i)
-            {
-                // p = p1 + a * d
-                // dot(normal, p - v) = 0
-                // dot(normal, p1 - v) + a * dot(normal, d) = 0
-                float numerator = Vector2.Dot(Normals[i], Vertices[i] - p1);
-                float denominator = Vector2.Dot(Normals[i], d);
-
-                if (denominator == 0.0f)
-                {
-                    if (numerator < 0.0f)
-                    {
-                        return false;
-                    }
-                }
-                else
-                {
-                    // Note: we want this predicate without division:
-                    // lower < numerator / denominator, where denominator < 0
-                    // Since denominator < 0, we have to flip the inequality:
-                    // lower < numerator / denominator <==> denominator * lower > numerator.
-                    if (denominator < 0.0f && numerator < lower * denominator)
-                    {
-                        // Increase lower.
-                        // The segment enters this half-space.
-                        lower = numerator / denominator;
-                        index = i;
-                    }
-                    else if (denominator > 0.0f && numerator < upper * denominator)
-                    {
-                        // Decrease upper.
-                        // The segment exits this half-space.
-                        upper = numerator / denominator;
-                    }
-                }
-
-                // The use of epsilon here causes the assert on lower to trip
-                // in some cases. Apparently the use of epsilon was to make edge
-                // shapes work, but now those are handled separately.
-                //if (upper < lower - b2_epsilon)
-                if (upper < lower)
-                {
-                    return false;
-                }
-            }
-
-            Debug.Assert(0.0f <= lower && lower <= input.MaxFraction);
-
-            if (index >= 0)
-            {
-                output.Fraction = lower;
-                output.Normal = MathUtils.Multiply(ref transform.R, Normals[index]);
-                return true;
-            }
-
-            return false;
-        }
-
-        /// <summary>
-        /// Given a transform, compute the associated axis aligned bounding box for a child shape.
-        /// </summary>
-        /// <param name="aabb">The aabb results.</param>
-        /// <param name="transform">The world transform of the shape.</param>
-        /// <param name="childIndex">The child shape index.</param>
-        public override void ComputeAABB(out AABB aabb, ref Transform transform, int childIndex)
-        {
-            Vector2 lower = MathUtils.Multiply(ref transform, Vertices[0]);
-            Vector2 upper = lower;
-
-            for (int i = 1; i < Vertices.Count; ++i)
-            {
-                Vector2 v = MathUtils.Multiply(ref transform, Vertices[i]);
-                lower = Vector2.Min(lower, v);
-                upper = Vector2.Max(upper, v);
-            }
-
-            Vector2 r = new Vector2(Radius, Radius);
-            aabb.LowerBound = lower - r;
-            aabb.UpperBound = upper + r;
-        }
-
-        public bool CompareTo(PolygonShape shape)
-        {
-            if (Vertices.Count != shape.Vertices.Count)
-                return false;
-
-            for (int i = 0; i < Vertices.Count; i++)
-            {
-                if (Vertices[i] != shape.Vertices[i])
-                    return false;
-            }
-
-            return (Radius == shape.Radius &&
-                    MassData == shape.MassData);
-        }
-
-        public override float ComputeSubmergedArea(Vector2 normal, float offset, Transform xf, out Vector2 sc)
-        {
-            sc = Vector2.Zero;
-
-            //Transform plane into shape co-ordinates
-            Vector2 normalL = MathUtils.MultiplyT(ref xf.R, normal);
-            float offsetL = offset - Vector2.Dot(normal, xf.Position);
-
-            float[] depths = new float[Settings.MaxPolygonVertices];
-            int diveCount = 0;
-            int intoIndex = -1;
-            int outoIndex = -1;
-
-            bool lastSubmerged = false;
-            int i;
-            for (i = 0; i < Vertices.Count; i++)
-            {
-                depths[i] = Vector2.Dot(normalL, Vertices[i]) - offsetL;
-                bool isSubmerged = depths[i] < -Settings.Epsilon;
-                if (i > 0)
-                {
-                    if (isSubmerged)
-                    {
-                        if (!lastSubmerged)
-                        {
-                            intoIndex = i - 1;
-                            diveCount++;
-                        }
-                    }
-                    else
-                    {
-                        if (lastSubmerged)
-                        {
-                            outoIndex = i - 1;
-                            diveCount++;
-                        }
-                    }
-                }
-                lastSubmerged = isSubmerged;
-            }
-            switch (diveCount)
-            {
-                case 0:
-                    if (lastSubmerged)
-                    {
-                        //Completely submerged
-                        sc = MathUtils.Multiply(ref xf, MassData.Centroid);
-                        return MassData.Mass / Density;
-                    }
-                    else
-                    {
-                        //Completely dry
-                        return 0;
-                    }
-                    break;
-                case 1:
-                    if (intoIndex == -1)
-                    {
-                        intoIndex = Vertices.Count - 1;
-                    }
-                    else
-                    {
-                        outoIndex = Vertices.Count - 1;
-                    }
-                    break;
-            }
-            int intoIndex2 = (intoIndex + 1) % Vertices.Count;
-            int outoIndex2 = (outoIndex + 1) % Vertices.Count;
-
-            float intoLambda = (0 - depths[intoIndex]) / (depths[intoIndex2] - depths[intoIndex]);
-            float outoLambda = (0 - depths[outoIndex]) / (depths[outoIndex2] - depths[outoIndex]);
-
-            Vector2 intoVec = new Vector2(
-                Vertices[intoIndex].X * (1 - intoLambda) + Vertices[intoIndex2].X * intoLambda,
-                Vertices[intoIndex].Y * (1 - intoLambda) + Vertices[intoIndex2].Y * intoLambda);
-            Vector2 outoVec = new Vector2(
-                Vertices[outoIndex].X * (1 - outoLambda) + Vertices[outoIndex2].X * outoLambda,
-                Vertices[outoIndex].Y * (1 - outoLambda) + Vertices[outoIndex2].Y * outoLambda);
-
-            //Initialize accumulator
-            float area = 0;
-            Vector2 center = new Vector2(0, 0);
-            Vector2 p2 = Vertices[intoIndex2];
-            Vector2 p3;
-
-            float k_inv3 = 1.0f / 3.0f;
-
-            //An awkward loop from intoIndex2+1 to outIndex2
-            i = intoIndex2;
-            while (i != outoIndex2)
-            {
-                i = (i + 1) % Vertices.Count;
-                if (i == outoIndex2)
-                    p3 = outoVec;
-                else
-                    p3 = Vertices[i];
-                //Add the triangle formed by intoVec,p2,p3
-                {
-                    Vector2 e1 = p2 - intoVec;
-                    Vector2 e2 = p3 - intoVec;
-
-                    float D = MathUtils.Cross(e1, e2);
-
-                    float triangleArea = 0.5f * D;
-
-                    area += triangleArea;
-
-                    // Area weighted centroid
-                    center += triangleArea * k_inv3 * (intoVec + p2 + p3);
-                }
-                //
-                p2 = p3;
-            }
-
-            //Normalize and transform centroid
-            center *= 1.0f / area;
-
-            sc = MathUtils.Multiply(ref xf, center);
-
-            return area;
-        }
-    }
-}

FarseerPhysicsEngine/Collision/Shapes/Shape.cs

@@ -1,222 +0,0 @@
-/*
-* Farseer Physics Engine based on Box2D.XNA port:
-* Copyright (c) 2010 Ian Qvist
-* 
-* Box2D.XNA port of Box2D:
-* Copyright (c) 2009 Brandon Furtwangler, Nathan Furtwangler
-*
-* Original source Box2D:
-* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com 
-* 
-* This software is provided 'as-is', without any express or implied 
-* warranty.  In no event will the authors be held liable for any damages 
-* arising from the use of this software. 
-* Permission is granted to anyone to use this software for any purpose, 
-* including commercial applications, and to alter it and redistribute it 
-* freely, subject to the following restrictions: 
-* 1. The origin of this software must not be misrepresented; you must not 
-* claim that you wrote the original software. If you use this software 
-* in a product, an acknowledgment in the product documentation would be 
-* appreciated but is not required. 
-* 2. Altered source versions must be plainly marked as such, and must not be 
-* misrepresented as being the original software. 
-* 3. This notice may not be removed or altered from any source distribution. 
-*/
-
-using System;
-using FarseerPhysics.Common;
-using Microsoft.Xna.Framework;
-
-namespace FarseerPhysics.Collision.Shapes
-{
-    /// <summary>
-    /// This holds the mass data computed for a shape.
-    /// </summary>
-    public struct MassData : IEquatable<MassData>
-    {
-        /// <summary>
-        /// The area of the shape
-        /// </summary>
-        public float Area;
-
-        /// <summary>
-        /// The position of the shape's centroid relative to the shape's origin.
-        /// </summary>
-        public Vector2 Centroid;
-
-        /// <summary>
-        /// The rotational inertia of the shape about the local origin.
-        /// </summary>
-        public float Inertia;
-
-        /// <summary>
-        /// The mass of the shape, usually in kilograms.
-        /// </summary>
-        public float Mass;
-
-        #region IEquatable<MassData> Members
-
-        public bool Equals(MassData other)
-        {
-            return this == other;
-        }
-
-        #endregion
-
-        public static bool operator ==(MassData left, MassData right)
-        {
-            return (left.Area == right.Area && left.Mass == right.Mass && left.Centroid == right.Centroid &&
-                    left.Inertia == right.Inertia);
-        }
-
-        public static bool operator !=(MassData left, MassData right)
-        {
-            return !(left == right);
-        }
-
-        public override bool Equals(object obj)
-        {
-            if (ReferenceEquals(null, obj)) return false;
-            if (obj.GetType() != typeof(MassData)) return false;
-            return Equals((MassData)obj);
-        }
-
-        public override int GetHashCode()
-        {
-            unchecked
-            {
-                int result = Area.GetHashCode();
-                result = (result * 397) ^ Centroid.GetHashCode();
-                result = (result * 397) ^ Inertia.GetHashCode();
-                result = (result * 397) ^ Mass.GetHashCode();
-                return result;
-            }
-        }
-    }
-
-    public enum ShapeType
-    {
-        Unknown = -1,
-        Circle = 0,
-        Edge = 1,
-        Polygon = 2,
-        Loop = 3,
-        TypeCount = 4,
-    }
-
-    /// <summary>
-    /// A shape is used for collision detection. You can create a shape however you like.
-    /// Shapes used for simulation in World are created automatically when a Fixture
-    /// is created. Shapes may encapsulate a one or more child shapes.
-    /// </summary>
-    public abstract class Shape
-    {
-        private static int _shapeIdCounter;
-        public MassData MassData;
-        public int ShapeId;
-
-        internal float _density;
-        internal float _radius;
-
-        protected Shape(float density)
-        {
-            _density = density;
-            ShapeType = ShapeType.Unknown;
-            ShapeId = _shapeIdCounter++;
-        }
-
-        /// <summary>
-        /// Get the type of this shape.
-        /// </summary>
-        /// <value>The type of the shape.</value>
-        public ShapeType ShapeType { get; internal set; }
-
-        /// <summary>
-        /// Get the number of child primitives.
-        /// </summary>
-        /// <value></value>
-        public abstract int ChildCount { get; }
-
-        /// <summary>
-        /// Gets or sets the density.
-        /// </summary>
-        /// <value>The density.</value>
-        public float Density
-        {
-            get { return _density; }
-            set
-            {
-                _density = value;
-                ComputeProperties();
-            }
-        }
-
-        /// <summary>
-        /// Radius of the Shape
-        /// </summary>
-        public float Radius
-        {
-            get { return _radius; }
-            set
-            {
-                _radius = value;
-                ComputeProperties();
-            }
-        }
-
-        /// <summary>
-        /// Clone the concrete shape
-        /// </summary>
-        /// <returns>A clone of the shape</returns>
-        public abstract Shape Clone();
-
-        /// <summary>
-        /// Test a point for containment in this shape. This only works for convex shapes.
-        /// </summary>
-        /// <param name="transform">The shape world transform.</param>
-        /// <param name="point">a point in world coordinates.</param>
-        /// <returns>True if the point is inside the shape</returns>
-        public abstract bool TestPoint(ref Transform transform, ref Vector2 point);
-
-        /// <summary>
-        /// Cast a ray against a child shape.
-        /// </summary>
-        /// <param name="output">The ray-cast results.</param>
-        /// <param name="input">The ray-cast input parameters.</param>
-        /// <param name="transform">The transform to be applied to the shape.</param>
-        /// <param name="childIndex">The child shape index.</param>
-        /// <returns>True if the ray-cast hits the shape</returns>
-        public abstract bool RayCast(out RayCastOutput output, ref RayCastInput input, ref Transform transform,
-                                     int childIndex);
-
-        /// <summary>
-        /// Given a transform, compute the associated axis aligned bounding box for a child shape.
-        /// </summary>
-        /// <param name="aabb">The aabb results.</param>
-        /// <param name="transform">The world transform of the shape.</param>
-        /// <param name="childIndex">The child shape index.</param>
-        public abstract void ComputeAABB(out AABB aabb, ref Transform transform, int childIndex);
-
-        /// <summary>
-        /// Compute the mass properties of this shape using its dimensions and density.
-        /// The inertia tensor is computed about the local origin, not the centroid.
-        /// </summary>
-        public abstract void ComputeProperties();
-
-        public bool CompareTo(Shape shape)
-        {
-            if (shape is PolygonShape && this is PolygonShape)
-                return ((PolygonShape)this).CompareTo((PolygonShape)shape);
-
-            if (shape is CircleShape && this is CircleShape)
-                return ((CircleShape)this).CompareTo((CircleShape)shape);
-
-            if (shape is EdgeShape && this is EdgeShape)
-                return ((EdgeShape)this).CompareTo((EdgeShape)shape);
-
-            return false;
-        }
-
-        public abstract float ComputeSubmergedArea(Vector2 normal, float offset, Transform xf, out Vector2 sc);
-    }
-}

FarseerPhysicsEngine/Collision/TimeOfImpact.cs

@@ -1,500 +0,0 @@
-/*
-* Farseer Physics Engine based on Box2D.XNA port:
-* Copyright (c) 2010 Ian Qvist
-* 
-* Box2D.XNA port of Box2D:
-* Copyright (c) 2009 Brandon Furtwangler, Nathan Furtwangler
-*
-* Original source Box2D:
-* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com 
-* 
-* This software is provided 'as-is', without any express or implied 
-* warranty.  In no event will the authors be held liable for any damages 
-* arising from the use of this software. 
-* Permission is granted to anyone to use this software for any purpose, 
-* including commercial applications, and to alter it and redistribute it 
-* freely, subject to the following restrictions: 
-* 1. The origin of this software must not be misrepresented; you must not 
-* claim that you wrote the original software. If you use this software 
-* in a product, an acknowledgment in the product documentation would be 
-* appreciated but is not required. 
-* 2. Altered source versions must be plainly marked as such, and must not be 
-* misrepresented as being the original software. 
-* 3. This notice may not be removed or altered from any source distribution. 
-*/
-
-using System;
-using System.Diagnostics;
-using FarseerPhysics.Common;
-using Microsoft.Xna.Framework;
-
-namespace FarseerPhysics.Collision
-{
-    /// <summary>
-    /// Input parameters for CalculateTimeOfImpact
-    /// </summary>
-    public class TOIInput
-    {
-        public DistanceProxy ProxyA = new DistanceProxy();
-        public DistanceProxy ProxyB = new DistanceProxy();
-        public Sweep SweepA;
-        public Sweep SweepB;
-        public float TMax; // defines sweep interval [0, tMax]
-    }
-
-    public enum TOIOutputState
-    {
-        Unknown,
-        Failed,
-        Overlapped,
-        Touching,
-        Seperated,
-    }
-
-    public struct TOIOutput
-    {
-        public TOIOutputState State;
-        public float T;
-    }
-
-    public enum SeparationFunctionType
-    {
-        Points,
-        FaceA,
-        FaceB
-    }
-
-    public static class SeparationFunction
-    {
-        private static Vector2 _axis;
-        private static Vector2 _localPoint;
-        private static DistanceProxy _proxyA = new DistanceProxy();
-        private static DistanceProxy _proxyB = new DistanceProxy();
-        private static Sweep _sweepA, _sweepB;
-        private static SeparationFunctionType _type;
-
-        public static void Set(ref SimplexCache cache,
-                               DistanceProxy proxyA, ref Sweep sweepA,
-                               DistanceProxy proxyB, ref Sweep sweepB,
-                               float t1)
-        {
-            _localPoint = Vector2.Zero;
-            _proxyA = proxyA;
-            _proxyB = proxyB;
-            int count = cache.Count;
-            Debug.Assert(0 < count && count < 3);
-
-            _sweepA = sweepA;
-            _sweepB = sweepB;
-
-            Transform xfA, xfB;
-            _sweepA.GetTransform(out xfA, t1);
-            _sweepB.GetTransform(out xfB, t1);
-
-            if (count == 1)
-            {
-                _type = SeparationFunctionType.Points;
-                Vector2 localPointA = _proxyA.Vertices[cache.IndexA[0]];
-                Vector2 localPointB = _proxyB.Vertices[cache.IndexB[0]];
-                Vector2 pointA = MathUtils.Multiply(ref xfA, localPointA);
-                Vector2 pointB = MathUtils.Multiply(ref xfB, localPointB);
-                _axis = pointB - pointA;
-                _axis.Normalize();
-                return;
-            }
-            else if (cache.IndexA[0] == cache.IndexA[1])
-            {
-                // Two points on B and one on A.
-                _type = SeparationFunctionType.FaceB;
-                Vector2 localPointB1 = proxyB.Vertices[cache.IndexB[0]];
-                Vector2 localPointB2 = proxyB.Vertices[cache.IndexB[1]];
-
-                Vector2 a = localPointB2 - localPointB1;
-                _axis = new Vector2(a.Y, -a.X);
-                _axis.Normalize();
-                Vector2 normal = MathUtils.Multiply(ref xfB.R, _axis);
-
-                _localPoint = 0.5f * (localPointB1 + localPointB2);
-                Vector2 pointB = MathUtils.Multiply(ref xfB, _localPoint);
-
-                Vector2 localPointA = proxyA.Vertices[cache.IndexA[0]];
-                Vector2 pointA = MathUtils.Multiply(ref xfA, localPointA);
-
-                float s = Vector2.Dot(pointA - pointB, normal);
-                if (s < 0.0f)
-                {
-                    _axis = -_axis;
-                    s = -s;
-                }
-                return;
-            }
-            else
-            {
-                // Two points on A and one or two points on B.
-                _type = SeparationFunctionType.FaceA;
-                Vector2 localPointA1 = _proxyA.Vertices[cache.IndexA[0]];
-                Vector2 localPointA2 = _proxyA.Vertices[cache.IndexA[1]];
-
-                Vector2 a = localPointA2 - localPointA1;
-                _axis = new Vector2(a.Y, -a.X);
-                _axis.Normalize();
-                Vector2 normal = MathUtils.Multiply(ref xfA.R, _axis);
-
-                _localPoint = 0.5f * (localPointA1 + localPointA2);
-                Vector2 pointA = MathUtils.Multiply(ref xfA, _localPoint);
-
-                Vector2 localPointB = _proxyB.Vertices[cache.IndexB[0]];
-                Vector2 pointB = MathUtils.Multiply(ref xfB, localPointB);
-
-                float s = Vector2.Dot(pointB - pointA, normal);
-                if (s < 0.0f)
-                {
-                    _axis = -_axis;
-                    s = -s;
-                }
-                return;
-            }
-        }
-
-        public static float FindMinSeparation(out int indexA, out int indexB, float t)
-        {
-            Transform xfA, xfB;
-            _sweepA.GetTransform(out xfA, t);
-            _sweepB.GetTransform(out xfB, t);
-
-            switch (_type)
-            {
-                case SeparationFunctionType.Points:
-                    {
-                        Vector2 axisA = MathUtils.MultiplyT(ref xfA.R, _axis);
-                        Vector2 axisB = MathUtils.MultiplyT(ref xfB.R, -_axis);
-
-                        indexA = _proxyA.GetSupport(axisA);
-                        indexB = _proxyB.GetSupport(axisB);
-
-                        Vector2 localPointA = _proxyA.Vertices[indexA];
-                        Vector2 localPointB = _proxyB.Vertices[indexB];
-
-                        Vector2 pointA = MathUtils.Multiply(ref xfA, localPointA);
-                        Vector2 pointB = MathUtils.Multiply(ref xfB, localPointB);
-
-                        float separation = Vector2.Dot(pointB - pointA, _axis);
-                        return separation;
-                    }
-
-                case SeparationFunctionType.FaceA:
-                    {
-                        Vector2 normal = MathUtils.Multiply(ref xfA.R, _axis);
-                        Vector2 pointA = MathUtils.Multiply(ref xfA, _localPoint);
-
-                        Vector2 axisB = MathUtils.MultiplyT(ref xfB.R, -normal);
-
-                        indexA = -1;
-                        indexB = _proxyB.GetSupport(axisB);
-
-                        Vector2 localPointB = _proxyB.Vertices[indexB];
-                        Vector2 pointB = MathUtils.Multiply(ref xfB, localPointB);
-
-                        float separation = Vector2.Dot(pointB - pointA, normal);
-                        return separation;
-                    }
-
-                case SeparationFunctionType.FaceB:
-                    {
-                        Vector2 normal = MathUtils.Multiply(ref xfB.R, _axis);
-                        Vector2 pointB = MathUtils.Multiply(ref xfB, _localPoint);
-
-                        Vector2 axisA = MathUtils.MultiplyT(ref xfA.R, -normal);
-
-                        indexB = -1;
-                        indexA = _proxyA.GetSupport(axisA);
-
-                        Vector2 localPointA = _proxyA.Vertices[indexA];
-                        Vector2 pointA = MathUtils.Multiply(ref xfA, localPointA);
-
-                        float separation = Vector2.Dot(pointA - pointB, normal);
-                        return separation;
-                    }
-
-                default:
-                    Debug.Assert(false);
-                    indexA = -1;
-                    indexB = -1;
-                    return 0.0f;
-            }
-        }
-
-        public static float Evaluate(int indexA, int indexB, float t)
-        {
-            Transform xfA, xfB;
-            _sweepA.GetTransform(out xfA, t);
-            _sweepB.GetTransform(out xfB, t);
-
-            switch (_type)
-            {
-                case SeparationFunctionType.Points:
-                    {
-                        Vector2 axisA = MathUtils.MultiplyT(ref xfA.R, _axis);
-                        Vector2 axisB = MathUtils.MultiplyT(ref xfB.R, -_axis);
-
-                        Vector2 localPointA = _proxyA.Vertices[indexA];
-                        Vector2 localPointB = _proxyB.Vertices[indexB];
-
-                        Vector2 pointA = MathUtils.Multiply(ref xfA, localPointA);
-                        Vector2 pointB = MathUtils.Multiply(ref xfB, localPointB);
-                        float separation = Vector2.Dot(pointB - pointA, _axis);
-
-                        return separation;
-                    }
-
-                case SeparationFunctionType.FaceA:
-                    {
-                        Vector2 normal = MathUtils.Multiply(ref xfA.R, _axis);
-                        Vector2 pointA = MathUtils.Multiply(ref xfA, _localPoint);
-
-                        Vector2 axisB = MathUtils.MultiplyT(ref xfB.R, -normal);
-
-                        Vector2 localPointB = _proxyB.Vertices[indexB];
-                        Vector2 pointB = MathUtils.Multiply(ref xfB, localPointB);
-
-                        float separation = Vector2.Dot(pointB - pointA, normal);
-                        return separation;
-                    }
-
-                case SeparationFunctionType.FaceB:
-                    {
-                        Vector2 normal = MathUtils.Multiply(ref xfB.R, _axis);
-                        Vector2 pointB = MathUtils.Multiply(ref xfB, _localPoint);
-
-                        Vector2 axisA = MathUtils.MultiplyT(ref xfA.R, -normal);
-
-                        Vector2 localPointA = _proxyA.Vertices[indexA];
-                        Vector2 pointA = MathUtils.Multiply(ref xfA, localPointA);
-
-                        float separation = Vector2.Dot(pointA - pointB, normal);
-                        return separation;
-                    }
-
-                default:
-                    Debug.Assert(false);
-                    return 0.0f;
-            }
-        }
-    }
-
-    public static class TimeOfImpact
-    {
-        // CCD via the local separating axis method. This seeks progression
-        // by computing the largest time at which separation is maintained.
-
-        public static int TOICalls, TOIIters, TOIMaxIters;
-        public static int TOIRootIters, TOIMaxRootIters;
-        private static DistanceInput _distanceInput = new DistanceInput();
-
-        /// <summary>
-        /// Compute the upper bound on time before two shapes penetrate. Time is represented as
-        /// a fraction between [0,tMax]. This uses a swept separating axis and may miss some intermediate,
-        /// non-tunneling collision. If you change the time interval, you should call this function
-        /// again.
-        /// Note: use Distance() to compute the contact point and normal at the time of impact.
-        /// </summary>
-        /// <param name="output">The output.</param>
-        /// <param name="input">The input.</param>
-        public static void CalculateTimeOfImpact(out TOIOutput output, TOIInput input)
-        {
-            ++TOICalls;
-
-            output = new TOIOutput();
-            output.State = TOIOutputState.Unknown;
-            output.T = input.TMax;
-
-            Sweep sweepA = input.SweepA;
-            Sweep sweepB = input.SweepB;
-
-            // Large rotations can make the root finder fail, so we normalize the
-            // sweep angles.
-            sweepA.Normalize();
-            sweepB.Normalize();
-
-            float tMax = input.TMax;
-
-            float totalRadius = input.ProxyA.Radius + input.ProxyB.Radius;
-            float target = Math.Max(Settings.LinearSlop, totalRadius - 3.0f * Settings.LinearSlop);
-            const float tolerance = 0.25f * Settings.LinearSlop;
-            Debug.Assert(target > tolerance);
-
-            float t1 = 0.0f;
-            const int k_maxIterations = 20;
-            int iter = 0;
-
-            // Prepare input for distance query.
-            SimplexCache cache;
-            _distanceInput.ProxyA = input.ProxyA;
-            _distanceInput.ProxyB = input.ProxyB;
-            _distanceInput.UseRadii = false;
-
-            // The outer loop progressively attempts to compute new separating axes.
-            // This loop terminates when an axis is repeated (no progress is made).
-            for (; ; )
-            {
-                Transform xfA, xfB;
-                sweepA.GetTransform(out xfA, t1);
-                sweepB.GetTransform(out xfB, t1);
-
-                // Get the distance between shapes. We can also use the results
-                // to get a separating axis.
-                _distanceInput.TransformA = xfA;
-                _distanceInput.TransformB = xfB;
-                DistanceOutput distanceOutput;
-                Distance.ComputeDistance(out distanceOutput, out cache, _distanceInput);
-
-                // If the shapes are overlapped, we give up on continuous collision.
-                if (distanceOutput.Distance <= 0.0f)
-                {
-                    // Failure!
-                    output.State = TOIOutputState.Overlapped;
-                    output.T = 0.0f;
-                    break;
-                }
-
-                if (distanceOutput.Distance < target + tolerance)
-                {
-                    // Victory!
-                    output.State = TOIOutputState.Touching;
-                    output.T = t1;
-                    break;
-                }
-
-                SeparationFunction.Set(ref cache, input.ProxyA, ref sweepA, input.ProxyB, ref sweepB, t1);
-
-                // Compute the TOI on the separating axis. We do this by successively
-                // resolving the deepest point. This loop is bounded by the number of vertices.
-                bool done = false;
-                float t2 = tMax;
-                int pushBackIter = 0;
-                for (; ; )
-                {
-                    // Find the deepest point at t2. Store the witness point indices.
-                    int indexA, indexB;
-                    float s2 = SeparationFunction.FindMinSeparation(out indexA, out indexB, t2);
-
-                    // Is the final configuration separated?
-                    if (s2 > target + tolerance)
-                    {
-                        // Victory!
-                        output.State = TOIOutputState.Seperated;
-                        output.T = tMax;
-                        done = true;
-                        break;
-                    }
-
-                    // Has the separation reached tolerance?
-                    if (s2 > target - tolerance)
-                    {
-                        // Advance the sweeps
-                        t1 = t2;
-                        break;
-                    }
-
-                    // Compute the initial separation of the witness points.
-                    float s1 = SeparationFunction.Evaluate(indexA, indexB, t1);
-
-                    // Check for initial overlap. This might happen if the root finder
-                    // runs out of iterations.
-                    if (s1 < target - tolerance)
-                    {
-                        output.State = TOIOutputState.Failed;
-                        output.T = t1;
-                        done = true;
-                        break;
-                    }
-
-                    // Check for touching
-                    if (s1 <= target + tolerance)
-                    {
-                        // Victory! t1 should hold the TOI (could be 0.0).
-                        output.State = TOIOutputState.Touching;
-                        output.T = t1;
-                        done = true;
-                        break;
-                    }
-
-                    // Compute 1D root of: f(x) - target = 0
-                    int rootIterCount = 0;
-                    float a1 = t1, a2 = t2;
-                    for (; ; )
-                    {
-                        // Use a mix of the secant rule and bisection.
-                        float t;
-                        if ((rootIterCount & 1) != 0)
-                        {
-                            // Secant rule to improve convergence.
-                            t = a1 + (target - s1) * (a2 - a1) / (s2 - s1);
-                        }
-                        else
-                        {
-                            // Bisection to guarantee progress.
-                            t = 0.5f * (a1 + a2);
-                        }
-
-                        float s = SeparationFunction.Evaluate(indexA, indexB, t);
-
-                        if (Math.Abs(s - target) < tolerance)
-                        {
-                            // t2 holds a tentative value for t1
-                            t2 = t;
-                            break;
-                        }
-
-                        // Ensure we continue to bracket the root.
-                        if (s > target)
-                        {
-                            a1 = t;
-                            s1 = s;
-                        }
-                        else
-                        {
-                            a2 = t;
-                            s2 = s;
-                        }
-
-                        ++rootIterCount;
-                        ++TOIRootIters;
-
-                        if (rootIterCount == 50)
-                        {
-                            break;
-                        }
-                    }
-
-                    TOIMaxRootIters = Math.Max(TOIMaxRootIters, rootIterCount);
-
-                    ++pushBackIter;
-
-                    if (pushBackIter == Settings.MaxPolygonVertices)
-                    {
-                        break;
-                    }
-                }
-
-                ++iter;
-                ++TOIIters;
-
-                if (done)
-                {
-                    break;
-                }
-
-                if (iter == k_maxIterations)
-                {
-                    // Root finder got stuck. Semi-victory.
-                    output.State = TOIOutputState.Failed;
-                    output.T = t1;
-                    break;
-                }
-            }
-
-            TOIMaxIters = Math.Max(TOIMaxIters, iter);
-        }
-    }
-}

FarseerPhysicsEngine/Common/ConvexHull/ChainHull.cs

@@ -1,126 +0,0 @@
-using System.Collections.Generic;
-using Microsoft.Xna.Framework;
-
-namespace FarseerPhysics.Common.ConvexHull
-{
-    public static class ChainHull
-    {
-        //Andrew's monotone chain 2D convex hull algorithm.
-        //Copyright 2001, softSurfer (www.softsurfer.com)
-
-        /// <summary>
-        /// Gets the convex hull.
-        /// </summary>
-        /// <remarks>
-        /// http://www.softsurfer.com/Archive/algorithm_0109/algorithm_0109.htm
-        /// </remarks>
-        /// <returns></returns>
-        public static Vertices GetConvexHull(Vertices P)
-        {
-            P.Sort(new PointComparer());
-
-            Vector2[] H = new Vector2[P.Count];
-            Vertices res = new Vertices();
-
-            int n = P.Count;
-
-            int bot, top = -1; // indices for bottom and top of the stack
-            int i; // array scan index
-
-            // Get the indices of points with min x-coord and min|max y-coord
-            int minmin = 0, minmax;
-            float xmin = P[0].X;
-            for (i = 1; i < n; i++)
-                if (P[i].X != xmin) break;
-            minmax = i - 1;
-            if (minmax == n - 1)
-            {
-                // degenerate case: all x-coords == xmin
-                H[++top] = P[minmin];
-                if (P[minmax].Y != P[minmin].Y) // a nontrivial segment
-                    H[++top] = P[minmax];
-                H[++top] = P[minmin]; // add polygon endpoint
-
-                for (int j = 0; j < top + 1; j++)
-                {
-                    res.Add(H[j]);
-                }
-
-                return res;
-            }
-
-            top = res.Count - 1;
-
-            // Get the indices of points with max x-coord and min|max y-coord
-            int maxmin, maxmax = n - 1;
-            float xmax = P[n - 1].X;
-            for (i = n - 2; i >= 0; i--)
-                if (P[i].X != xmax) break;
-            maxmin = i + 1;
-
-            // Compute the lower hull on the stack H
-            H[++top] = P[minmin]; // push minmin point onto stack
-            i = minmax;
-            while (++i <= maxmin)
-            {
-                // the lower line joins P[minmin] with P[maxmin]
-                if (MathUtils.Area(P[minmin], P[maxmin], P[i]) >= 0 && i < maxmin)
-                    continue; // ignore P[i] above or on the lower line
-
-                while (top > 0) // there are at least 2 points on the stack
-                {
-                    // test if P[i] is left of the line at the stack top
-                    if (MathUtils.Area(H[top - 1], H[top], P[i]) > 0)
-                        break; // P[i] is a new hull vertex
-                    else
-                        top--; // pop top point off stack
-                }
-                H[++top] = P[i]; // push P[i] onto stack
-            }
-
-            // Next, compute the upper hull on the stack H above the bottom hull
-            if (maxmax != maxmin) // if distinct xmax points
-                H[++top] = P[maxmax]; // push maxmax point onto stack
-            bot = top; // the bottom point of the upper hull stack
-            i = maxmin;
-            while (--i >= minmax)
-            {
-                // the upper line joins P[maxmax] with P[minmax]
-                if (MathUtils.Area(P[maxmax], P[minmax], P[i]) >= 0 && i > minmax)
-                    continue; // ignore P[i] below or on the upper line
-
-                while (top > bot) // at least 2 points on the upper stack
-                {
-                    // test if P[i] is left of the line at the stack top
-                    if (MathUtils.Area(H[top - 1], H[top], P[i]) > 0)
-                        break; // P[i] is a new hull vertex
-                    else
-                        top--; // pop top point off stack
-                }
-                H[++top] = P[i]; // push P[i] onto stack
-            }
-            if (minmax != minmin)
-                H[++top] = P[minmin]; // push joining endpoint onto stack
-
-            for (int j = 0; j < top + 1; j++)
-            {
-                res.Add(H[j]);
-            }
-
-            return res;
-        }
-
-        #region Nested type: PointComparer
-
-        public class PointComparer : Comparer<Vector2>
-        {
-            public override int Compare(Vector2 a, Vector2 b)
-            {
-                int f = a.X.CompareTo(b.X);
-                return f != 0 ? f : a.Y.CompareTo(b.Y);
-            }
-        }
-
-        #endregion
-    }
-}

FarseerPhysicsEngine/Common/ConvexHull/GiftWrap.cs

@@ -1,99 +0,0 @@
-using System;
-
-namespace FarseerPhysics.Common.ConvexHull
-{
-    public static class GiftWrap
-    {
-        // From Eric Jordan's convex decomposition library (box2D rev 32)
-
-        /// <summary>
-        /// Find the convex hull of a point cloud using "Gift-wrap" algorithm - start
-        /// with an extremal point, and walk around the outside edge by testing
-        /// angles.
-        /// 
-        /// Runs in O(N*S) time where S is number of sides of resulting polygon.
-        /// Worst case: point cloud is all vertices of convex polygon: O(N^2).
-        /// There may be faster algorithms to do this, should you need one -
-        /// this is just the simplest. You can get O(N log N) expected time if you
-        /// try, I think, and O(N) if you restrict inputs to simple polygons.
-        /// Returns null if number of vertices passed is less than 3.
-        /// Results should be passed through convex decomposition afterwards
-        /// to ensure that each shape has few enough points to be used in Box2d.
-        /// 
-        /// Warning: May be buggy with colinear points on hull.
-        /// </summary>
-        /// <param name="vertices">The vertices.</param>
-        /// <returns></returns>
-        public static Vertices GetConvexHull(Vertices vertices)
-        {
-            if (vertices.Count < 3)
-                return vertices;
-
-            int[] edgeList = new int[vertices.Count];
-            int numEdges = 0;
-
-            float minY = float.MaxValue;
-            int minYIndex = vertices.Count;
-            for (int i = 0; i < vertices.Count; ++i)
-            {
-                if (vertices[i].Y < minY)
-                {
-                    minY = vertices[i].Y;
-                    minYIndex = i;
-                }
-            }
-
-            int startIndex = minYIndex;
-            int winIndex = -1;
-            float dx = -1.0f;
-            float dy = 0.0f;
-            while (winIndex != minYIndex)
-            {
-                float maxDot = -2.0f;
-                float nrm;
-
-                for (int i = 0; i < vertices.Count; ++i)
-                {
-                    if (i == startIndex)
-                        continue;
-                    float newdx = vertices[i].X - vertices[startIndex].X;
-                    float newdy = vertices[i].Y - vertices[startIndex].Y;
-                    nrm = (float)Math.Sqrt(newdx * newdx + newdy * newdy);
-                    nrm = (nrm == 0.0f) ? 1.0f : nrm;
-                    newdx /= nrm;
-                    newdy /= nrm;
-
-                    //Dot products act as proxy for angle
-                    //without requiring inverse trig.
-                    float newDot = newdx * dx + newdy * dy;
-                    if (newDot > maxDot)
-                    {
-                        maxDot = newDot;
-                        winIndex = i;
-                    }
-                }
-                edgeList[numEdges++] = winIndex;
-                dx = vertices[winIndex].X - vertices[startIndex].X;
-                dy = vertices[winIndex].Y - vertices[startIndex].Y;
-                nrm = (float)Math.Sqrt(dx * dx + dy * dy);
-                nrm = (nrm == 0.0f) ? 1.0f : nrm;
-                dx /= nrm;
-                dy /= nrm;
-                startIndex = winIndex;
-            }
-
-            Vertices returnVal = new Vertices(numEdges);
-
-            for (int i = 0; i < numEdges; i++)
-            {
-                returnVal.Add(vertices[edgeList[i]]);
-                //Debug.WriteLine(string.Format("{0}, {1}", vertices[edgeList[i]].X, vertices[edgeList[i]].Y));
-            }
-
-            //Not sure if we need this
-            //returnVal.MergeParallelEdges(Settings.b2_angularSlop);
-
-            return returnVal;
-        }
-    }
-}

FarseerPhysicsEngine/Common/ConvexHull/Melkman.cs

@@ -1,122 +0,0 @@
-using Microsoft.Xna.Framework;
-
-namespace FarseerPhysics.Common.ConvexHull
-{
-    public static class Melkman
-    {
-        //Melkman based convex hull algorithm contributed by Cowdozer
-
-        /// <summary>
-        /// Creates a convex hull.
-        /// Note:
-        /// 1. Vertices must be of a simple polygon, i.e. edges do not overlap.
-        /// 2. Melkman does not work on point clouds
-        /// </summary>
-        /// <remarks>
-        /// Implemented using Melkman's Convex Hull Algorithm - O(n) time complexity.
-        /// Reference: http://www.ams.sunysb.edu/~jsbm/courses/345/melkman.pdf
-        /// </remarks>
-        /// <returns>A convex hull in counterclockwise winding order.</returns>
-        public static Vertices GetConvexHull(Vertices vertices)
-        {
-            //With less than 3 vertices, this is about the best we can do for a convex hull
-            if (vertices.Count < 3)
-                return vertices;
-
-            //We'll never need a queue larger than the current number of Vertices +1
-            //Create double-ended queue
-            Vector2[] deque = new Vector2[vertices.Count + 1];
-            int qf = 3, qb = 0; //Queue front index, queue back index
-            int qfm1, qbm1; //qfm1 = second element, qbm1 = second last element
-
-            //Start by placing first 3 vertices in convex CCW order
-            int startIndex = 3;
-            float k = MathUtils.Area(vertices[0], vertices[1], vertices[2]);
-            if (k == 0)
-            {
-                //Vertices are collinear.
-                deque[0] = vertices[0];
-                deque[1] = vertices[2]; //We can skip vertex 1 because it should be between 0 and 2
-                deque[2] = vertices[0];
-                qf = 2;
-
-                //Go until the end of the collinear sequence of vertices
-                for (startIndex = 3; startIndex < vertices.Count; startIndex++)
-                {
-                    Vector2 tmp = vertices[startIndex];
-                    if (MathUtils.Area(ref deque[0], ref deque[1], ref tmp) == 0) //This point is also collinear
-                        deque[1] = vertices[startIndex];
-                    else break;
-                }
-            }
-            else
-            {
-                deque[0] = deque[3] = vertices[2];
-                if (k > 0)
-                {
-                    //Is Left.  Set deque = {2, 0, 1, 2}
-                    deque[1] = vertices[0];
-                    deque[2] = vertices[1];
-                }
-                else
-                {
-                    //Is Right. Set deque = {2, 1, 0, 2}
-                    deque[1] = vertices[1];
-                    deque[2] = vertices[0];
-                }
-            }
-
-            qfm1 = qf == 0 ? deque.Length - 1 : qf - 1; //qfm1 = qf - 1;
-            qbm1 = qb == deque.Length - 1 ? 0 : qb + 1; //qbm1 = qb + 1;
-
-            //Add vertices one at a time and adjust convex hull as needed
-            for (int i = startIndex; i < vertices.Count; i++)
-            {
-                Vector2 nextPt = vertices[i];
-
-                //Ignore if it is already within the convex hull we have constructed
-                if (MathUtils.Area(ref deque[qfm1], ref deque[qf], ref nextPt) > 0 &&
-                    MathUtils.Area(ref deque[qb], ref deque[qbm1], ref nextPt) > 0)
-                    continue;
-
-                //Pop front until convex
-                while (!(MathUtils.Area(ref deque[qfm1], ref deque[qf], ref nextPt) > 0))
-                {
-                    //Pop the front element from the queue
-                    qf = qfm1; //qf--;
-                    qfm1 = qf == 0 ? deque.Length - 1 : qf - 1; //qfm1 = qf - 1;
-                }
-                //Add vertex to the front of the queue
-                qf = qf == deque.Length - 1 ? 0 : qf + 1; //qf++;
-                qfm1 = qf == 0 ? deque.Length - 1 : qf - 1; //qfm1 = qf - 1;
-                deque[qf] = nextPt;
-
-                //Pop back until convex
-                while (!(MathUtils.Area(ref deque[qb], ref deque[qbm1], ref nextPt) > 0))
-                {
-                    //Pop the back element from the queue
-                    qb = qbm1; //qb++;
-                    qbm1 = qb == deque.Length - 1 ? 0 : qb + 1; //qbm1 = qb + 1;
-                }
-                //Add vertex to the back of the queue
-                qb = qb == 0 ? deque.Length - 1 : qb - 1; //qb--;
-                qbm1 = qb == deque.Length - 1 ? 0 : qb + 1; //qbm1 = qb + 1;
-                deque[qb] = nextPt;
-            }
-
-            //Create the convex hull from what is left in the deque
-            Vertices convexHull = new Vertices(vertices.Count + 1);
-            if (qb < qf)
-                for (int i = qb; i < qf; i++)
-                    convexHull.Add(deque[i]);
-            else
-            {
-                for (int i = 0; i < qf; i++)
-                    convexHull.Add(deque[i]);
-                for (int i = qb; i < deque.Length; i++)
-                    convexHull.Add(deque[i]);
-            }
-            return convexHull;
-        }
-    }
-}

FarseerPhysicsEngine/Common/Decomposition/BayazitDecomposer.cs

@@ -1,253 +0,0 @@
-using System.Collections.Generic;
-using FarseerPhysics.Common.PolygonManipulation;
-using Microsoft.Xna.Framework;
-
-namespace FarseerPhysics.Common.Decomposition
-{
-    //From phed rev 36
-
-    /// <summary>
-    /// Convex decomposition algorithm created by Mark Bayazit (http://mnbayazit.com/)
-    /// For more information about this algorithm, see http://mnbayazit.com/406/bayazit
-    /// </summary>
-    public static class BayazitDecomposer
-    {
-        private static Vector2 At(int i, Vertices vertices)
-        {
-            int s = vertices.Count;
-            return vertices[i < 0 ? s - (-i % s) : i % s];
-        }
-
-        private static Vertices Copy(int i, int j, Vertices vertices)
-        {
-            Vertices p = new Vertices();
-            while (j < i) j += vertices.Count;
-            //p.reserve(j - i + 1);
-            for (; i <= j; ++i)
-            {
-                p.Add(At(i, vertices));
-            }
-            return p;
-        }
-
-        /// <summary>
-        /// Decompose the polygon into several smaller non-concave polygon.
-        /// If the polygon is already convex, it will return the original polygon, unless it is over Settings.MaxPolygonVertices.
-        /// Precondition: Counter Clockwise polygon
-        /// </summary>
-        /// <param name="vertices"></param>
-        /// <returns></returns>
-        public static List<Vertices> ConvexPartition(Vertices vertices)
-        {
-            //We force it to CCW as it is a precondition in this algorithm.
-            vertices.ForceCounterClockWise();
-
-            List<Vertices> list = new List<Vertices>();
-            float d, lowerDist, upperDist;
-            Vector2 p;
-            Vector2 lowerInt = new Vector2();
-            Vector2 upperInt = new Vector2(); // intersection points
-            int lowerIndex = 0, upperIndex = 0;
-            Vertices lowerPoly, upperPoly;
-
-            for (int i = 0; i < vertices.Count; ++i)
-            {
-                if (Reflex(i, vertices))
-                {
-                    lowerDist = upperDist = float.MaxValue; // std::numeric_limits<qreal>::max();
-                    for (int j = 0; j < vertices.Count; ++j)
-                    {
-                        // if line intersects with an edge
-                        if (Left(At(i - 1, vertices), At(i, vertices), At(j, vertices)) &&
-                            RightOn(At(i - 1, vertices), At(i, vertices), At(j - 1, vertices)))
-                        {
-                            // find the point of intersection
-                            p = LineTools.LineIntersect(At(i - 1, vertices), At(i, vertices), At(j, vertices),
-                                                        At(j - 1, vertices));
-                            if (Right(At(i + 1, vertices), At(i, vertices), p))
-                            {
-                                // make sure it's inside the poly
-                                d = SquareDist(At(i, vertices), p);
-                                if (d < lowerDist)
-                                {
-                                    // keep only the closest intersection
-                                    lowerDist = d;
-                                    lowerInt = p;
-                                    lowerIndex = j;
-                                }
-                            }
-                        }
-
-                        if (Left(At(i + 1, vertices), At(i, vertices), At(j + 1, vertices)) &&
-                            RightOn(At(i + 1, vertices), At(i, vertices), At(j, vertices)))
-                        {
-                            p = LineTools.LineIntersect(At(i + 1, vertices), At(i, vertices), At(j, vertices),
-                                                        At(j + 1, vertices));
-                            if (Left(At(i - 1, vertices), At(i, vertices), p))
-                            {
-                                d = SquareDist(At(i, vertices), p);
-                                if (d < upperDist)
-                                {
-                                    upperDist = d;
-                                    upperIndex = j;
-                                    upperInt = p;
-                                }
-                            }
-                        }
-                    }
-
-                    // if there are no vertices to connect to, choose a point in the middle
-                    if (lowerIndex == (upperIndex + 1) % vertices.Count)
-                    {
-                        Vector2 sp = ((lowerInt + upperInt) / 2);
-
-                        lowerPoly = Copy(i, upperIndex, vertices);
-                        lowerPoly.Add(sp);
-                        upperPoly = Copy(lowerIndex, i, vertices);
-                        upperPoly.Add(sp);
-                    }
-                    else
-                    {
-                        double highestScore = 0, bestIndex = lowerIndex;
-                        while (upperIndex < lowerIndex) upperIndex += vertices.Count;
-                        for (int j = lowerIndex; j <= upperIndex; ++j)
-                        {
-                            if (CanSee(i, j, vertices))
-                            {
-                                double score = 1 / (SquareDist(At(i, vertices), At(j, vertices)) + 1);
-                                if (Reflex(j, vertices))
-                                {
-                                    if (RightOn(At(j - 1, vertices), At(j, vertices), At(i, vertices)) &&
-                                        LeftOn(At(j + 1, vertices), At(j, vertices), At(i, vertices)))
-                                    {
-                                        score += 3;
-                                    }
-                                    else
-                                    {
-                                        score += 2;
-                                    }
-                                }
-                                else
-                                {
-                                    score += 1;
-                                }
-                                if (score > highestScore)
-                                {
-                                    bestIndex = j;
-                                    highestScore = score;
-                                }
-                            }
-                        }
-                        lowerPoly = Copy(i, (int)bestIndex, vertices);
-                        upperPoly = Copy((int)bestIndex, i, vertices);
-                    }
-                    list.AddRange(ConvexPartition(lowerPoly));
-                    list.AddRange(ConvexPartition(upperPoly));
-                    return list;
-                }
-            }
-
-            // polygon is already convex
-            if (vertices.Count > Settings.MaxPolygonVertices)
-            {
-                lowerPoly = Copy(0, vertices.Count / 2, vertices);
-                upperPoly = Copy(vertices.Count / 2, 0, vertices);
-                list.AddRange(ConvexPartition(lowerPoly));
-                list.AddRange(ConvexPartition(upperPoly));
-            }
-            else
-                list.Add(vertices);
-
-            //The polygons are not guaranteed to be without collinear points. We remove
-            //them to be sure.
-            for (int i = 0; i < list.Count; i++)
-            {
-                list[i] = SimplifyTools.CollinearSimplify(list[i], 0);
-            }
-
-            //Remove empty vertice collections
-            for (int i = list.Count - 1; i >= 0; i--)
-            {
-                if (list[i].Count == 0)
-                    list.RemoveAt(i);
-            }
-
-            return list;
-        }
-
-        private static bool CanSee(int i, int j, Vertices vertices)
-        {
-            if (Reflex(i, vertices))
-            {
-                if (LeftOn(At(i, vertices), At(i - 1, vertices), At(j, vertices)) &&
-                    RightOn(At(i, vertices), At(i + 1, vertices), At(j, vertices))) return false;
-            }
-            else
-            {
-                if (RightOn(At(i, vertices), At(i + 1, vertices), At(j, vertices)) ||
-                    LeftOn(At(i, vertices), At(i - 1, vertices), At(j, vertices))) return false;
-            }
-            if (Reflex(j, vertices))
-            {
-                if (LeftOn(At(j, vertices), At(j - 1, vertices), At(i, vertices)) &&
-                    RightOn(At(j, vertices), At(j + 1, vertices), At(i, vertices))) return false;
-            }
-            else
-            {
-                if (RightOn(At(j, vertices), At(j + 1, vertices), At(i, vertices)) ||
-                    LeftOn(At(j, vertices), At(j - 1, vertices), At(i, vertices))) return false;
-            }
-            for (int k = 0; k < vertices.Count; ++k)
-            {
-                if ((k + 1) % vertices.Count == i || k == i || (k + 1) % vertices.Count == j || k == j)
-                {
-                    continue; // ignore incident edges
-                }
-                Vector2 intersectionPoint;
-                if (LineTools.LineIntersect(At(i, vertices), At(j, vertices), At(k, vertices), At(k + 1, vertices), out intersectionPoint))
-                {
-                    return false;
-                }
-            }
-            return true;
-        }
-
-        // precondition: ccw
-        private static bool Reflex(int i, Vertices vertices)
-        {
-            return Right(i, vertices);
-        }
-
-        private static bool Right(int i, Vertices vertices)
-        {
-            return Right(At(i - 1, vertices), At(i, vertices), At(i + 1, vertices));
-        }
-
-        private static bool Left(Vector2 a, Vector2 b, Vector2 c)
-        {
-            return MathUtils.Area(ref a, ref b, ref c) > 0;
-        }
-
-        private static bool LeftOn(Vector2 a, Vector2 b, Vector2 c)
-        {
-            return MathUtils.Area(ref a, ref b, ref c) >= 0;
-        }
-
-        private static bool Right(Vector2 a, Vector2 b, Vector2 c)
-        {
-            return MathUtils.Area(ref a, ref b, ref c) < 0;
-        }
-
-        private static bool RightOn(Vector2 a, Vector2 b, Vector2 c)
-        {
-            return MathUtils.Area(ref a, ref b, ref c) <= 0;
-        }
-
-        private static float SquareDist(Vector2 a, Vector2 b)
-        {
-            float dx = b.X - a.X;
-            float dy = b.Y - a.Y;
-            return dx * dx + dy * dy;
-        }
-    }
-}

FarseerPhysicsEngine/Common/Decomposition/CDT/Delaunay/DelaunayTriangle.cs

@@ -1,420 +0,0 @@
-/* Poly2Tri
- * Copyright (c) 2009-2010, Poly2Tri Contributors
- * http://code.google.com/p/poly2tri/
- *
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without modification,
- * are permitted provided that the following conditions are met:
- *
- * * Redistributions of source code must retain the above copyright notice,
- *   this list of conditions and the following disclaimer.
- * * Redistributions in binary form must reproduce the above copyright notice,
- *   this list of conditions and the following disclaimer in the documentation
- *   and/or other materials provided with the distribution.
- * * Neither the name of Poly2Tri nor the names of its contributors may be
- *   used to endorse or promote products derived from this software without specific
- *   prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
- * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
- * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
- * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
- * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
- * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
- * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
- * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
- * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
- */
-
-// Changes from the Java version
-//   attributification
-// Future possibilities
-//   Flattening out the number of indirections
-//     Replacing arrays of 3 with fixed-length arrays?
-//     Replacing bool[3] with a bit array of some sort?
-//     Bundling everything into an AoS mess?
-//     Hardcode them all as ABC ?
-
-using System;
-using System.Collections.Generic;
-using System.Diagnostics;
-using Poly2Tri.Triangulation.Delaunay.Sweep;
-using Poly2Tri.Triangulation.Util;
-
-namespace Poly2Tri.Triangulation.Delaunay
-{
-    public class DelaunayTriangle
-    {
-        /** Neighbor pointers */
-
-        /** Flags to determine if an edge is a Delauney edge */
-        public FixedBitArray3 EdgeIsConstrained;
-
-        /** Flags to determine if an edge is a Constrained edge */
-        public FixedBitArray3 EdgeIsDelaunay;
-        public FixedArray3<DelaunayTriangle> Neighbors;
-
-        /** Has this triangle been marked as an interior triangle? */
-
-        public FixedArray3<TriangulationPoint> Points;
-
-        public DelaunayTriangle(TriangulationPoint p1, TriangulationPoint p2, TriangulationPoint p3)
-        {
-            Points[0] = p1;
-            Points[1] = p2;
-            Points[2] = p3;
-        }
-
-        public bool IsInterior { get; set; }
-
-        public int IndexOf(TriangulationPoint p)
-        {
-            int i = Points.IndexOf(p);
-            if (i == -1) throw new Exception("Calling index with a point that doesn't exist in triangle");
-            return i;
-        }
-
-        //TODO: Port note - different implementation
-        public int IndexCW(TriangulationPoint p)
-        {
-            int index = IndexOf(p);
-            switch (index)
-            {
-                case 0:
-                    return 2;
-                case 1:
-                    return 0;
-                default:
-                    return 1;
-            }
-        }
-
-        //TODO: Port note - different implementation
-        public int IndexCCW(TriangulationPoint p)
-        {
-            int index = IndexOf(p);
-            switch (index)
-            {
-                case 0:
-                    return 1;
-                case 1:
-                    return 2;
-                default:
-                    return 0;
-            }
-        }
-
-        public bool Contains(TriangulationPoint p)
-        {
-            return (p == Points[0] || p == Points[1] || p == Points[2]);
-        }
-
-        public bool Contains(DTSweepConstraint e)
-        {
-            return (Contains(e.P) && Contains(e.Q));
-        }
-
-        public bool Contains(TriangulationPoint p, TriangulationPoint q)
-        {
-            return (Contains(p) && Contains(q));
-        }
-
-        /// <summary>
-        /// Update neighbor pointers
-        /// </summary>
-        /// <param name="p1">Point 1 of the shared edge</param>
-        /// <param name="p2">Point 2 of the shared edge</param>
-        /// <param name="t">This triangle's new neighbor</param>
-        private void MarkNeighbor(TriangulationPoint p1, TriangulationPoint p2, DelaunayTriangle t)
-        {
-            if ((p1 == Points[2] && p2 == Points[1]) || (p1 == Points[1] && p2 == Points[2]))
-            {
-                Neighbors[0] = t;
-            }
-            else if ((p1 == Points[0] && p2 == Points[2]) || (p1 == Points[2] && p2 == Points[0]))
-            {
-                Neighbors[1] = t;
-            }
-            else if ((p1 == Points[0] && p2 == Points[1]) || (p1 == Points[1] && p2 == Points[0]))
-            {
-                Neighbors[2] = t;
-            }
-            else
-            {
-                Debug.WriteLine("Neighbor error, please report!");
-                // throw new Exception("Neighbor error, please report!");
-            }
-        }
-
-        /// <summary>
-        /// Exhaustive search to update neighbor pointers
-        /// </summary>
-        public void MarkNeighbor(DelaunayTriangle t)
-        {
-            if (t.Contains(Points[1], Points[2]))
-            {
-                Neighbors[0] = t;
-                t.MarkNeighbor(Points[1], Points[2], this);
-            }
-            else if (t.Contains(Points[0], Points[2]))
-            {
-                Neighbors[1] = t;
-                t.MarkNeighbor(Points[0], Points[2], this);
-            }
-            else if (t.Contains(Points[0], Points[1]))
-            {
-                Neighbors[2] = t;
-                t.MarkNeighbor(Points[0], Points[1], this);
-            }
-            else
-            {
-                Debug.WriteLine("markNeighbor failed");
-            }
-        }
-
-        public void ClearNeighbors()
-        {
-            Neighbors[0] = Neighbors[1] = Neighbors[2] = null;
-        }
-
-        public void ClearNeighbor(DelaunayTriangle triangle)
-        {
-            if (Neighbors[0] == triangle)
-            {
-                Neighbors[0] = null;
-            }
-            else if (Neighbors[1] == triangle)
-            {
-                Neighbors[1] = null;
-            }
-            else
-            {
-                Neighbors[2] = null;
-            }
-        }
-
-        /**
-         * Clears all references to all other triangles and points
-         */
-
-        public void Clear()
-        {
-            DelaunayTriangle t;
-            for (int i = 0; i < 3; i++)
-            {
-                t = Neighbors[i];
-                if (t != null)
-                {
-                    t.ClearNeighbor(this);
-                }
-            }
-            ClearNeighbors();
-            Points[0] = Points[1] = Points[2] = null;
-        }
-
-        /// <param name="t">Opposite triangle</param>
-        /// <param name="p">The point in t that isn't shared between the triangles</param>
-        public TriangulationPoint OppositePoint(DelaunayTriangle t, TriangulationPoint p)
-        {
-            Debug.Assert(t != this, "self-pointer error");
-            return PointCW(t.PointCW(p));
-        }
-
-        public DelaunayTriangle NeighborCW(TriangulationPoint point)
-        {
-            return Neighbors[(Points.IndexOf(point) + 1)%3];
-        }
-
-        public DelaunayTriangle NeighborCCW(TriangulationPoint point)
-        {
-            return Neighbors[(Points.IndexOf(point) + 2)%3];
-        }
-
-        public DelaunayTriangle NeighborAcross(TriangulationPoint point)
-        {
-            return Neighbors[Points.IndexOf(point)];
-        }
-
-        public TriangulationPoint PointCCW(TriangulationPoint point)
-        {
-            return Points[(IndexOf(point) + 1)%3];
-        }
-
-        public TriangulationPoint PointCW(TriangulationPoint point)
-        {
-            return Points[(IndexOf(point) + 2)%3];
-        }
-
-        private void RotateCW()
-        {
-            var t = Points[2];
-            Points[2] = Points[1];
-            Points[1] = Points[0];
-            Points[0] = t;
-        }
-
-        /// <summary>
-        /// Legalize triangle by rotating clockwise around oPoint
-        /// </summary>
-        /// <param name="oPoint">The origin point to rotate around</param>
-        /// <param name="nPoint">???</param>
-        public void Legalize(TriangulationPoint oPoint, TriangulationPoint nPoint)
-        {
-            RotateCW();
-            Points[IndexCCW(oPoint)] = nPoint;
-        }
-
-        public override string ToString()
-        {
-            return Points[0] + "," + Points[1] + "," + Points[2];
-        }
-
-        /// <summary>
-        /// Finalize edge marking
-        /// </summary>
-        public void MarkNeighborEdges()
-        {
-            for (int i = 0; i < 3; i++)
-                if (EdgeIsConstrained[i] && Neighbors[i] != null)
-                {
-                    Neighbors[i].MarkConstrainedEdge(Points[(i + 1)%3], Points[(i + 2)%3]);
-                }
-        }
-
-        public void MarkEdge(DelaunayTriangle triangle)
-        {
-            for (int i = 0; i < 3; i++)
-                if (EdgeIsConstrained[i])
-                {
-                    triangle.MarkConstrainedEdge(Points[(i + 1)%3], Points[(i + 2)%3]);
-                }
-        }
-
-        public void MarkEdge(List<DelaunayTriangle> tList)
-        {
-            foreach (DelaunayTriangle t in tList)
-                for (int i = 0; i < 3; i++)
-                    if (t.EdgeIsConstrained[i])
-                    {
-                        MarkConstrainedEdge(t.Points[(i + 1)%3], t.Points[(i + 2)%3]);
-                    }
-        }
-
-        public void MarkConstrainedEdge(int index)
-        {
-            EdgeIsConstrained[index] = true;
-        }
-
-        public void MarkConstrainedEdge(DTSweepConstraint edge)
-        {
-            MarkConstrainedEdge(edge.P, edge.Q);
-        }
-
-        /// <summary>
-        /// Mark edge as constrained
-        /// </summary>
-        public void MarkConstrainedEdge(TriangulationPoint p, TriangulationPoint q)
-        {
-            int i = EdgeIndex(p, q);
-            if (i != -1) EdgeIsConstrained[i] = true;
-        }
-
-        public double Area()
-        {
-            double b = Points[0].X - Points[1].X;
-            double h = Points[2].Y - Points[1].Y;
-
-            return Math.Abs((b*h*0.5f));
-        }
-
-        public TriangulationPoint Centroid()
-        {
-            double cx = (Points[0].X + Points[1].X + Points[2].X)/3f;
-            double cy = (Points[0].Y + Points[1].Y + Points[2].Y)/3f;
-            return new TriangulationPoint(cx, cy);
-        }
-
-        /// <summary>
-        /// Get the index of the neighbor that shares this edge (or -1 if it isn't shared)
-        /// </summary>
-        /// <returns>index of the shared edge or -1 if edge isn't shared</returns>
-        public int EdgeIndex(TriangulationPoint p1, TriangulationPoint p2)
-        {
-            int i1 = Points.IndexOf(p1);
-            int i2 = Points.IndexOf(p2);
-
-            // Points of this triangle in the edge p1-p2
-            bool a = (i1 == 0 || i2 == 0);
-            bool b = (i1 == 1 || i2 == 1);
-            bool c = (i1 == 2 || i2 == 2);
-
-            if (b && c) return 0;
-            if (a && c) return 1;
-            if (a && b) return 2;
-            return -1;
-        }
-
-        public bool GetConstrainedEdgeCCW(TriangulationPoint p)
-        {
-            return EdgeIsConstrained[(IndexOf(p) + 2)%3];
-        }
-
-        public bool GetConstrainedEdgeCW(TriangulationPoint p)
-        {
-            return EdgeIsConstrained[(IndexOf(p) + 1)%3];
-        }
-
-        public bool GetConstrainedEdgeAcross(TriangulationPoint p)
-        {
-            return EdgeIsConstrained[IndexOf(p)];
-        }
-
-        public void SetConstrainedEdgeCCW(TriangulationPoint p, bool ce)
-        {
-            EdgeIsConstrained[(IndexOf(p) + 2)%3] = ce;
-        }
-
-        public void SetConstrainedEdgeCW(TriangulationPoint p, bool ce)
-        {
-            EdgeIsConstrained[(IndexOf(p) + 1)%3] = ce;
-        }
-
-        public void SetConstrainedEdgeAcross(TriangulationPoint p, bool ce)
-        {
-            EdgeIsConstrained[IndexOf(p)] = ce;
-        }
-
-        public bool GetDelaunayEdgeCCW(TriangulationPoint p)
-        {
-            return EdgeIsDelaunay[(IndexOf(p) + 2)%3];
-        }
-
-        public bool GetDelaunayEdgeCW(TriangulationPoint p)
-        {
-            return EdgeIsDelaunay[(IndexOf(p) + 1)%3];
-        }
-
-        public bool GetDelaunayEdgeAcross(TriangulationPoint p)
-        {
-            return EdgeIsDelaunay[IndexOf(p)];
-        }
-
-        public void SetDelaunayEdgeCCW(TriangulationPoint p, bool ce)
-        {
-            EdgeIsDelaunay[(IndexOf(p) + 2)%3] = ce;
-        }
-
-        public void SetDelaunayEdgeCW(TriangulationPoint p, bool ce)
-        {
-            EdgeIsDelaunay[(IndexOf(p) + 1)%3] = ce;
-        }
-
-        public void SetDelaunayEdgeAcross(TriangulationPoint p, bool ce)
-        {
-            EdgeIsDelaunay[IndexOf(p)] = ce;
-        }
-    }
-}

FarseerPhysicsEngine/Common/Decomposition/CDT/Delaunay/Sweep/AdvancingFront.cs

@@ -1,180 +0,0 @@
-/* Poly2Tri
- * Copyright (c) 2009-2010, Poly2Tri Contributors
- * http://code.google.com/p/poly2tri/
- *
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without modification,
- * are permitted provided that the following conditions are met:
- *
- * * Redistributions of source code must retain the above copyright notice,
- *   this list of conditions and the following disclaimer.
- * * Redistributions in binary form must reproduce the above copyright notice,
- *   this list of conditions and the following disclaimer in the documentation
- *   and/or other materials provided with the distribution.
- * * Neither the name of Poly2Tri nor the names of its contributors may be
- *   used to endorse or promote products derived from this software without specific
- *   prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
- * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
- * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
- * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
- * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
- * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
- * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
- * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
- * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
- */
-
-// Changes from the Java version
-//   Removed BST code, but not all artifacts of it
-// Future possibilities
-//   Eliminate Add/RemoveNode ?
-//   Comments comments and more comments!
-
-using System;
-using System.Text;
-
-namespace Poly2Tri.Triangulation.Delaunay.Sweep
-{
-    /**
-     * @author Thomas Åhlen ([email protected])
-     */
-
-    public class AdvancingFront
-    {
-        public AdvancingFrontNode Head;
-        protected AdvancingFrontNode Search;
-        public AdvancingFrontNode Tail;
-
-        public AdvancingFront(AdvancingFrontNode head, AdvancingFrontNode tail)
-        {
-            Head = head;
-            Tail = tail;
-            Search = head;
-            AddNode(head);
-            AddNode(tail);
-        }
-
-        public void AddNode(AdvancingFrontNode node)
-        {
-            //_searchTree.put(node.key, node);
-        }
-
-        public void RemoveNode(AdvancingFrontNode node)
-        {
-            //_searchTree.delete( node.key );
-        }
-
-        public override string ToString()
-        {
-            StringBuilder sb = new StringBuilder();
-            AdvancingFrontNode node = Head;
-            while (node != Tail)
-            {
-                sb.Append(node.Point.X).Append("->");
-                node = node.Next;
-            }
-            sb.Append(Tail.Point.X);
-            return sb.ToString();
-        }
-
-        /// <summary>
-        /// MM:  This seems to be used by LocateNode to guess a position in the implicit linked list of AdvancingFrontNodes near x
-        ///      Removed an overload that depended on this being exact
-        /// </summary>
-        private AdvancingFrontNode FindSearchNode(double x)
-        {
-            // TODO: implement BST index 
-            return Search;
-        }
-
-        /// <summary>
-        /// We use a balancing tree to locate a node smaller or equal to given key value
-        /// </summary>
-        public AdvancingFrontNode LocateNode(TriangulationPoint point)
-        {
-            return LocateNode(point.X);
-        }
-
-        private AdvancingFrontNode LocateNode(double x)
-        {
-            AdvancingFrontNode node = FindSearchNode(x);
-            if (x < node.Value)
-            {
-                while ((node = node.Prev) != null)
-                    if (x >= node.Value)
-                    {
-                        Search = node;
-                        return node;
-                    }
-            }
-            else
-            {
-                while ((node = node.Next) != null)
-                    if (x < node.Value)
-                    {
-                        Search = node.Prev;
-                        return node.Prev;
-                    }
-            }
-            return null;
-        }
-
-        /// <summary>
-        /// This implementation will use simple node traversal algorithm to find a point on the front
-        /// </summary>
-        public AdvancingFrontNode LocatePoint(TriangulationPoint point)
-        {
-            double px = point.X;
-            AdvancingFrontNode node = FindSearchNode(px);
-            double nx = node.Point.X;
-
-            if (px == nx)
-            {
-                if (point != node.Point)
-                {
-                    // We might have two nodes with same x value for a short time
-                    if (point == node.Prev.Point)
-                    {
-                        node = node.Prev;
-                    }
-                    else if (point == node.Next.Point)
-                    {
-                        node = node.Next;
-                    }
-                    else
-                    {
-                        throw new Exception("Failed to find Node for given afront point");
-                        //node = null;
-                    }
-                }
-            }
-            else if (px < nx)
-            {
-                while ((node = node.Prev) != null)
-                {
-                    if (point == node.Point)
-                    {
-                        break;
-                    }
-                }
-            }
-            else
-            {
-                while ((node = node.Next) != null)
-                {
-                    if (point == node.Point)
-                    {
-                        break;
-                    }
-                }
-            }
-            Search = node;
-            return node;
-        }
-    }
-}

FarseerPhysicsEngine/Common/Decomposition/CDT/Delaunay/Sweep/AdvancingFrontNode.cs

@@ -1,64 +0,0 @@
-/* Poly2Tri
- * Copyright (c) 2009-2010, Poly2Tri Contributors
- * http://code.google.com/p/poly2tri/
- *
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without modification,
- * are permitted provided that the following conditions are met:
- *
- * * Redistributions of source code must retain the above copyright notice,
- *   this list of conditions and the following disclaimer.
- * * Redistributions in binary form must reproduce the above copyright notice,
- *   this list of conditions and the following disclaimer in the documentation
- *   and/or other materials provided with the distribution.
- * * Neither the name of Poly2Tri nor the names of its contributors may be
- *   used to endorse or promote products derived from this software without specific
- *   prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
- * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
- * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
- * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
- * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
- * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
- * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
- * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
- * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
- */
-
-// Changes from the Java version
-//   Removed getters
-//   Has* turned into attributes
-// Future possibilities
-//   Comments!
-
-namespace Poly2Tri.Triangulation.Delaunay.Sweep
-{
-    public class AdvancingFrontNode
-    {
-        public AdvancingFrontNode Next;
-        public TriangulationPoint Point;
-        public AdvancingFrontNode Prev;
-        public DelaunayTriangle Triangle;
-        public double Value;
-
-        public AdvancingFrontNode(TriangulationPoint point)
-        {
-            Point = point;
-            Value = point.X;
-        }
-
-        public bool HasNext
-        {
-            get { return Next != null; }
-        }
-
-        public bool HasPrev
-        {
-            get { return Prev != null; }
-        }
-    }
-}

FarseerPhysicsEngine/Common/Decomposition/CDT/Delaunay/Sweep/DTSweep.cs

@@ -1,1132 +0,0 @@
-/* Poly2Tri
- * Copyright (c) 2009-2010, Poly2Tri Contributors
- * http://code.google.com/p/poly2tri/
- *
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without modification,
- * are permitted provided that the following conditions are met:
- *
- * * Redistributions of source code must retain the above copyright notice,
- *   this list of conditions and the following disclaimer.
- * * Redistributions in binary form must reproduce the above copyright notice,
- *   this list of conditions and the following disclaimer in the documentation
- *   and/or other materials provided with the distribution.
- * * Neither the name of Poly2Tri nor the names of its contributors may be
- *   used to endorse or promote products derived from this software without specific
- *   prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
- * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
- * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
- * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
- * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
- * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
- * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
- * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
- * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
- */
-
-/*
- * Sweep-line, Constrained Delauney Triangulation (CDT) See: Domiter, V. and
- * Zalik, B.(2008)'Sweep-line algorithm for constrained Delaunay triangulation',
- * International Journal of Geographical Information Science
- * 
- * "FlipScan" Constrained Edge Algorithm invented by author of this code.
- * 
- * Author: Thomas Åhlén, [email protected] 
- */
-
-// Changes from the Java version
-//   Turned DTSweep into a static class
-//   Lots of deindentation via early bailout
-// Future possibilities
-//   Comments!
-
-using System;
-using System.Collections.Generic;
-using System.Diagnostics;
-using FarseerPhysics.Common.Decomposition.CDT;
-
-namespace Poly2Tri.Triangulation.Delaunay.Sweep
-{
-    public static class DTSweep
-    {
-        private const double PI_div2 = Math.PI/2;
-        private const double PI_3div4 = 3*Math.PI/4;
-
-        /// <summary>
-        /// Triangulate simple polygon with holes
-        /// </summary>
-        public static void Triangulate(DTSweepContext tcx)
-        {
-            tcx.CreateAdvancingFront();
-
-            Sweep(tcx);
-
-            // Finalize triangulation
-            if (tcx.TriangulationMode == TriangulationMode.Polygon)
-            {
-                FinalizationPolygon(tcx);
-            }
-            else
-            {
-                FinalizationConvexHull(tcx);
-            }
-
-            tcx.Done();
-        }
-
-        /// <summary>
-        /// Start sweeping the Y-sorted point set from bottom to top
-        /// </summary>
-        private static void Sweep(DTSweepContext tcx)
-        {
-            List<TriangulationPoint> points = tcx.Points;
-            TriangulationPoint point;
-            AdvancingFrontNode node;
-
-            for (int i = 1; i < points.Count; i++)
-            {
-                point = points[i];
-
-                node = PointEvent(tcx, point);
-
-                if (point.HasEdges)
-                {
-                    foreach (DTSweepConstraint e in point.Edges)
-                    {
-                        EdgeEvent(tcx, e, node);
-                    }
-                }
-                tcx.Update(null);
-            }
-        }
-
-        /// <summary>
-        /// If this is a Delaunay Triangulation of a pointset we need to fill so the triangle mesh gets a ConvexHull 
-        /// </summary>
-        private static void FinalizationConvexHull(DTSweepContext tcx)
-        {
-            AdvancingFrontNode n1, n2;
-            DelaunayTriangle t1, t2;
-            TriangulationPoint first, p1;
-
-            n1 = tcx.aFront.Head.Next;
-            n2 = n1.Next;
-            first = n1.Point;
-
-            TurnAdvancingFrontConvex(tcx, n1, n2);
-
-            // TODO: implement ConvexHull for lower right and left boundary
-
-            // Lets remove triangles connected to the two "algorithm" points
-
-            // XXX: When the first the nodes are points in a triangle we need to do a flip before 
-            //      removing triangles or we will lose a valid triangle.
-            //      Same for last three nodes!
-            // !!! If I implement ConvexHull for lower right and left boundary this fix should not be 
-            //     needed and the removed triangles will be added again by default
-            n1 = tcx.aFront.Tail.Prev;
-            if (n1.Triangle.Contains(n1.Next.Point) && n1.Triangle.Contains(n1.Prev.Point))
-            {
-                t1 = n1.Triangle.NeighborAcross(n1.Point);
-                RotateTrianglePair(n1.Triangle, n1.Point, t1, t1.OppositePoint(n1.Triangle, n1.Point));
-                tcx.MapTriangleToNodes(n1.Triangle);
-                tcx.MapTriangleToNodes(t1);
-            }
-            n1 = tcx.aFront.Head.Next;
-            if (n1.Triangle.Contains(n1.Prev.Point) && n1.Triangle.Contains(n1.Next.Point))
-            {
-                t1 = n1.Triangle.NeighborAcross(n1.Point);
-                RotateTrianglePair(n1.Triangle, n1.Point, t1, t1.OppositePoint(n1.Triangle, n1.Point));
-                tcx.MapTriangleToNodes(n1.Triangle);
-                tcx.MapTriangleToNodes(t1);
-            }
-
-            // Lower right boundary 
-            first = tcx.aFront.Head.Point;
-            n2 = tcx.aFront.Tail.Prev;
-            t1 = n2.Triangle;
-            p1 = n2.Point;
-            n2.Triangle = null;
-            do
-            {
-                tcx.RemoveFromList(t1);
-                p1 = t1.PointCCW(p1);
-                if (p1 == first) break;
-                t2 = t1.NeighborCCW(p1);
-                t1.Clear();
-                t1 = t2;
-            } while (true);
-
-            // Lower left boundary
-            first = tcx.aFront.Head.Next.Point;
-            p1 = t1.PointCW(tcx.aFront.Head.Point);
-            t2 = t1.NeighborCW(tcx.aFront.Head.Point);
-            t1.Clear();
-            t1 = t2;
-            while (p1 != first) //TODO: Port note. This was do while before.
-            {
-                tcx.RemoveFromList(t1);
-                p1 = t1.PointCCW(p1);
-                t2 = t1.NeighborCCW(p1);
-                t1.Clear();
-                t1 = t2;
-            }
-
-            // Remove current head and tail node now that we have removed all triangles attached
-            // to them. Then set new head and tail node points
-            tcx.aFront.Head = tcx.aFront.Head.Next;
-            tcx.aFront.Head.Prev = null;
-            tcx.aFront.Tail = tcx.aFront.Tail.Prev;
-            tcx.aFront.Tail.Next = null;
-
-            tcx.FinalizeTriangulation();
-        }
-
-        /// <summary>
-        /// We will traverse the entire advancing front and fill it to form a convex hull.
-        /// </summary>
-        private static void TurnAdvancingFrontConvex(DTSweepContext tcx, AdvancingFrontNode b, AdvancingFrontNode c)
-        {
-            AdvancingFrontNode first = b;
-            while (c != tcx.aFront.Tail)
-            {
-                if (TriangulationUtil.Orient2d(b.Point, c.Point, c.Next.Point) == Orientation.CCW)
-                {
-                    // [b,c,d] Concave - fill around c
-                    Fill(tcx, c);
-                    c = c.Next;
-                }
-                else
-                {
-                    // [b,c,d] Convex
-                    if (b != first && TriangulationUtil.Orient2d(b.Prev.Point, b.Point, c.Point) == Orientation.CCW)
-                    {
-                        // [a,b,c] Concave - fill around b
-                        Fill(tcx, b);
-                        b = b.Prev;
-                    }
-                    else
-                    {
-                        // [a,b,c] Convex - nothing to fill
-                        b = c;
-                        c = c.Next;
-                    }
-                }
-            }
-        }
-
-        private static void FinalizationPolygon(DTSweepContext tcx)
-        {
-            // Get an Internal triangle to start with
-            DelaunayTriangle t = tcx.aFront.Head.Next.Triangle;
-            TriangulationPoint p = tcx.aFront.Head.Next.Point;
-            while (!t.GetConstrainedEdgeCW(p))
-            {
-                t = t.NeighborCCW(p);
-            }
-
-            // Collect interior triangles constrained by edges
-            tcx.MeshClean(t);
-        }
-
-        /// <summary>
-        /// Find closes node to the left of the new point and
-        /// create a new triangle. If needed new holes and basins
-        /// will be filled to.
-        /// </summary>
-        private static AdvancingFrontNode PointEvent(DTSweepContext tcx, TriangulationPoint point)
-        {
-            AdvancingFrontNode node, newNode;
-
-            node = tcx.LocateNode(point);
-            newNode = NewFrontTriangle(tcx, point, node);
-
-            // Only need to check +epsilon since point never have smaller 
-            // x value than node due to how we fetch nodes from the front
-            if (point.X <= node.Point.X + TriangulationUtil.EPSILON)
-            {
-                Fill(tcx, node);
-            }
-
-            tcx.AddNode(newNode);
-
-            FillAdvancingFront(tcx, newNode);
-            return newNode;
-        }
-
-        /// <summary>
-        /// Creates a new front triangle and legalize it
-        /// </summary>
-        private static AdvancingFrontNode NewFrontTriangle(DTSweepContext tcx, TriangulationPoint point,
-                                                           AdvancingFrontNode node)
-        {
-            AdvancingFrontNode newNode;
-            DelaunayTriangle triangle;
-
-            triangle = new DelaunayTriangle(point, node.Point, node.Next.Point);
-            triangle.MarkNeighbor(node.Triangle);
-            tcx.Triangles.Add(triangle);
-
-            newNode = new AdvancingFrontNode(point);
-            newNode.Next = node.Next;
-            newNode.Prev = node;
-            node.Next.Prev = newNode;
-            node.Next = newNode;
-
-            tcx.AddNode(newNode); // XXX: BST
-
-            if (!Legalize(tcx, triangle))
-            {
-                tcx.MapTriangleToNodes(triangle);
-            }
-
-            return newNode;
-        }
-
-        private static void EdgeEvent(DTSweepContext tcx, DTSweepConstraint edge, AdvancingFrontNode node)
-        {
-            try
-            {
-                tcx.EdgeEvent.ConstrainedEdge = edge;
-                tcx.EdgeEvent.Right = edge.P.X > edge.Q.X;
-
-                if (IsEdgeSideOfTriangle(node.Triangle, edge.P, edge.Q))
-                {
-                    return;
-                }
-
-                // For now we will do all needed filling
-                // TODO: integrate with flip process might give some better performance 
-                //       but for now this avoid the issue with cases that needs both flips and fills
-                FillEdgeEvent(tcx, edge, node);
-
-                EdgeEvent(tcx, edge.P, edge.Q, node.Triangle, edge.Q);
-            }
-            catch (PointOnEdgeException e)
-            {
-                Debug.WriteLine(String.Format("Skipping Edge: {0}", e.Message));
-            }
-        }
-
-        private static void FillEdgeEvent(DTSweepContext tcx, DTSweepConstraint edge, AdvancingFrontNode node)
-        {
-            if (tcx.EdgeEvent.Right)
-            {
-                FillRightAboveEdgeEvent(tcx, edge, node);
-            }
-            else
-            {
-                FillLeftAboveEdgeEvent(tcx, edge, node);
-            }
-        }
-
-        private static void FillRightConcaveEdgeEvent(DTSweepContext tcx, DTSweepConstraint edge,
-                                                      AdvancingFrontNode node)
-        {
-            Fill(tcx, node.Next);
-            if (node.Next.Point != edge.P)
-            {
-                // Next above or below edge?
-                if (TriangulationUtil.Orient2d(edge.Q, node.Next.Point, edge.P) == Orientation.CCW)
-                {
-                    // Below
-                    if (TriangulationUtil.Orient2d(node.Point, node.Next.Point, node.Next.Next.Point) == Orientation.CCW)
-                    {
-                        // Next is concave
-                        FillRightConcaveEdgeEvent(tcx, edge, node);
-                    }
-                    else
-                    {
-                        // Next is convex
-                    }
-                }
-            }
-        }
-
-        private static void FillRightConvexEdgeEvent(DTSweepContext tcx, DTSweepConstraint edge, AdvancingFrontNode node)
-        {
-            // Next concave or convex?
-            if (TriangulationUtil.Orient2d(node.Next.Point, node.Next.Next.Point, node.Next.Next.Next.Point) ==
-                Orientation.CCW)
-            {
-                // Concave
-                FillRightConcaveEdgeEvent(tcx, edge, node.Next);
-            }
-            else
-            {
-                // Convex
-                // Next above or below edge?
-                if (TriangulationUtil.Orient2d(edge.Q, node.Next.Next.Point, edge.P) == Orientation.CCW)
-                {
-                    // Below
-                    FillRightConvexEdgeEvent(tcx, edge, node.Next);
-                }
-                else
-                {
-                    // Above
-                }
-            }
-        }
-
-        private static void FillRightBelowEdgeEvent(DTSweepContext tcx, DTSweepConstraint edge, AdvancingFrontNode node)
-        {
-            if (node.Point.X < edge.P.X) // needed?
-            {
-                if (TriangulationUtil.Orient2d(node.Point, node.Next.Point, node.Next.Next.Point) == Orientation.CCW)
-                {
-                    // Concave 
-                    FillRightConcaveEdgeEvent(tcx, edge, node);
-                }
-                else
-                {
-                    // Convex
-                    FillRightConvexEdgeEvent(tcx, edge, node);
-                    // Retry this one
-                    FillRightBelowEdgeEvent(tcx, edge, node);
-                }
-            }
-        }
-
-        private static void FillRightAboveEdgeEvent(DTSweepContext tcx, DTSweepConstraint edge, AdvancingFrontNode node)
-        {
-            while (node.Next.Point.X < edge.P.X)
-            {
-                // Check if next node is below the edge
-                Orientation o1 = TriangulationUtil.Orient2d(edge.Q, node.Next.Point, edge.P);
-                if (o1 == Orientation.CCW)
-                {
-                    FillRightBelowEdgeEvent(tcx, edge, node);
-                }
-                else
-                {
-                    node = node.Next;
-                }
-            }
-        }
-
-        private static void FillLeftConvexEdgeEvent(DTSweepContext tcx, DTSweepConstraint edge, AdvancingFrontNode node)
-        {
-            // Next concave or convex?
-            if (TriangulationUtil.Orient2d(node.Prev.Point, node.Prev.Prev.Point, node.Prev.Prev.Prev.Point) ==
-                Orientation.CW)
-            {
-                // Concave
-                FillLeftConcaveEdgeEvent(tcx, edge, node.Prev);
-            }
-            else
-            {
-                // Convex
-                // Next above or below edge?
-                if (TriangulationUtil.Orient2d(edge.Q, node.Prev.Prev.Point, edge.P) == Orientation.CW)
-                {
-                    // Below
-                    FillLeftConvexEdgeEvent(tcx, edge, node.Prev);
-                }
-                else
-                {
-                    // Above
-                }
-            }
-        }
-
-        private static void FillLeftConcaveEdgeEvent(DTSweepContext tcx, DTSweepConstraint edge, AdvancingFrontNode node)
-        {
-            Fill(tcx, node.Prev);
-            if (node.Prev.Point != edge.P)
-            {
-                // Next above or below edge?
-                if (TriangulationUtil.Orient2d(edge.Q, node.Prev.Point, edge.P) == Orientation.CW)
-                {
-                    // Below
-                    if (TriangulationUtil.Orient2d(node.Point, node.Prev.Point, node.Prev.Prev.Point) == Orientation.CW)
-                    {
-                        // Next is concave
-                        FillLeftConcaveEdgeEvent(tcx, edge, node);
-                    }
-                    else
-                    {
-                        // Next is convex
-                    }
-                }
-            }
-        }
-
-        private static void FillLeftBelowEdgeEvent(DTSweepContext tcx, DTSweepConstraint edge, AdvancingFrontNode node)
-        {
-            if (node.Point.X > edge.P.X)
-            {
-                if (TriangulationUtil.Orient2d(node.Point, node.Prev.Point, node.Prev.Prev.Point) == Orientation.CW)
-                {
-                    // Concave 
-                    FillLeftConcaveEdgeEvent(tcx, edge, node);
-                }
-                else
-                {
-                    // Convex
-                    FillLeftConvexEdgeEvent(tcx, edge, node);
-                    // Retry this one
-                    FillLeftBelowEdgeEvent(tcx, edge, node);
-                }
-            }
-        }
-
-        private static void FillLeftAboveEdgeEvent(DTSweepContext tcx, DTSweepConstraint edge, AdvancingFrontNode node)
-        {
-            while (node.Prev.Point.X > edge.P.X)
-            {
-                // Check if next node is below the edge
-                Orientation o1 = TriangulationUtil.Orient2d(edge.Q, node.Prev.Point, edge.P);
-                if (o1 == Orientation.CW)
-                {
-                    FillLeftBelowEdgeEvent(tcx, edge, node);
-                }
-                else
-                {
-                    node = node.Prev;
-                }
-            }
-        }
-
-        //TODO: Port note: There were some structural differences here.
-        private static bool IsEdgeSideOfTriangle(DelaunayTriangle triangle, TriangulationPoint ep, TriangulationPoint eq)
-        {
-            int index;
-            index = triangle.EdgeIndex(ep, eq);
-            if (index != -1)
-            {
-                triangle.MarkConstrainedEdge(index);
-                triangle = triangle.Neighbors[index];
-                if (triangle != null)
-                {
-                    triangle.MarkConstrainedEdge(ep, eq);
-                }
-                return true;
-            }
-            return false;
-        }
-
-        private static void EdgeEvent(DTSweepContext tcx, TriangulationPoint ep, TriangulationPoint eq,
-                                      DelaunayTriangle triangle, TriangulationPoint point)
-        {
-            TriangulationPoint p1, p2;
-
-            if (IsEdgeSideOfTriangle(triangle, ep, eq))
-            {
-                return;
-            }
-
-            p1 = triangle.PointCCW(point);
-            Orientation o1 = TriangulationUtil.Orient2d(eq, p1, ep);
-            if (o1 == Orientation.Collinear)
-            {
-                if (triangle.Contains(eq, p1))
-                {
-                    triangle.MarkConstrainedEdge(eq, p1);
-                    // We are modifying the constraint maybe it would be better to 
-                    // not change the given constraint and just keep a variable for the new constraint
-                    tcx.EdgeEvent.ConstrainedEdge.Q = p1;
-                    triangle = triangle.NeighborAcross(point);
-                    EdgeEvent(tcx, ep, p1, triangle, p1);
-                }
-                else
-                {
-                    throw new PointOnEdgeException("EdgeEvent - Point on constrained edge not supported yet");
-                }
-                if (tcx.IsDebugEnabled)
-                {
-                    Debug.WriteLine("EdgeEvent - Point on constrained edge");
-                }
-                return;
-            }
-
-            p2 = triangle.PointCW(point);
-            Orientation o2 = TriangulationUtil.Orient2d(eq, p2, ep);
-            if (o2 == Orientation.Collinear)
-            {
-                if (triangle.Contains(eq, p2))
-                {
-                    triangle.MarkConstrainedEdge(eq, p2);
-                    // We are modifying the constraint maybe it would be better to 
-                    // not change the given constraint and just keep a variable for the new constraint
-                    tcx.EdgeEvent.ConstrainedEdge.Q = p2;
-                    triangle = triangle.NeighborAcross(point);
-                    EdgeEvent(tcx, ep, p2, triangle, p2);
-                }
-                else
-                {
-                    throw new PointOnEdgeException("EdgeEvent - Point on constrained edge not supported yet");
-                }
-                if (tcx.IsDebugEnabled)
-                {
-                    Debug.WriteLine("EdgeEvent - Point on constrained edge");
-                }
-                return;
-            }
-
-            if (o1 == o2)
-            {
-                // Need to decide if we are rotating CW or CCW to get to a triangle
-                // that will cross edge
-                if (o1 == Orientation.CW)
-                {
-                    triangle = triangle.NeighborCCW(point);
-                }
-                else
-                {
-                    triangle = triangle.NeighborCW(point);
-                }
-                EdgeEvent(tcx, ep, eq, triangle, point);
-            }
-            else
-            {
-                // This triangle crosses constraint so lets flippin start!
-                FlipEdgeEvent(tcx, ep, eq, triangle, point);
-            }
-        }
-
-        private static void FlipEdgeEvent(DTSweepContext tcx, TriangulationPoint ep, TriangulationPoint eq,
-                                          DelaunayTriangle t, TriangulationPoint p)
-        {
-            TriangulationPoint op, newP;
-            DelaunayTriangle ot;
-            bool inScanArea;
-
-            ot = t.NeighborAcross(p);
-            op = ot.OppositePoint(t, p);
-
-            if (ot == null)
-            {
-                // If we want to integrate the fillEdgeEvent do it here
-                // With current implementation we should never get here
-                throw new InvalidOperationException("[BUG:FIXME] FLIP failed due to missing triangle");
-            }
-
-            inScanArea = TriangulationUtil.InScanArea(p, t.PointCCW(p), t.PointCW(p), op);
-            if (inScanArea)
-            {
-                // Lets rotate shared edge one vertex CW
-                RotateTrianglePair(t, p, ot, op);
-                tcx.MapTriangleToNodes(t);
-                tcx.MapTriangleToNodes(ot);
-
-                if (p == eq && op == ep)
-                {
-                    if (eq == tcx.EdgeEvent.ConstrainedEdge.Q
-                        && ep == tcx.EdgeEvent.ConstrainedEdge.P)
-                    {
-                        if (tcx.IsDebugEnabled) Console.WriteLine("[FLIP] - constrained edge done"); // TODO: remove
-                        t.MarkConstrainedEdge(ep, eq);
-                        ot.MarkConstrainedEdge(ep, eq);
-                        Legalize(tcx, t);
-                        Legalize(tcx, ot);
-                    }
-                    else
-                    {
-                        if (tcx.IsDebugEnabled) Console.WriteLine("[FLIP] - subedge done"); // TODO: remove
-                        // XXX: I think one of the triangles should be legalized here?
-                    }
-                }
-                else
-                {
-                    if (tcx.IsDebugEnabled)
-                        Console.WriteLine("[FLIP] - flipping and continuing with triangle still crossing edge");
-                            // TODO: remove
-                    Orientation o = TriangulationUtil.Orient2d(eq, op, ep);
-                    t = NextFlipTriangle(tcx, o, t, ot, p, op);
-                    FlipEdgeEvent(tcx, ep, eq, t, p);
-                }
-            }
-            else
-            {
-                newP = NextFlipPoint(ep, eq, ot, op);
-                FlipScanEdgeEvent(tcx, ep, eq, t, ot, newP);
-                EdgeEvent(tcx, ep, eq, t, p);
-            }
-        }
-
-        /// <summary>
-        /// When we need to traverse from one triangle to the next we need 
-        /// the point in current triangle that is the opposite point to the next
-        /// triangle. 
-        /// </summary>
-        private static TriangulationPoint NextFlipPoint(TriangulationPoint ep, TriangulationPoint eq,
-                                                        DelaunayTriangle ot, TriangulationPoint op)
-        {
-            Orientation o2d = TriangulationUtil.Orient2d(eq, op, ep);
-            if (o2d == Orientation.CW)
-            {
-                // Right
-                return ot.PointCCW(op);
-            }
-            else if (o2d == Orientation.CCW)
-            {
-                // Left
-                return ot.PointCW(op);
-            }
-            else
-            {
-                // TODO: implement support for point on constraint edge
-                throw new PointOnEdgeException("Point on constrained edge not supported yet");
-            }
-        }
-
-        /// <summary>
-        /// After a flip we have two triangles and know that only one will still be
-        /// intersecting the edge. So decide which to contiune with and legalize the other
-        /// </summary>
-        /// <param name="tcx"></param>
-        /// <param name="o">should be the result of an TriangulationUtil.orient2d( eq, op, ep )</param>
-        /// <param name="t">triangle 1</param>
-        /// <param name="ot">triangle 2</param>
-        /// <param name="p">a point shared by both triangles</param>
-        /// <param name="op">another point shared by both triangles</param>
-        /// <returns>returns the triangle still intersecting the edge</returns>
-        private static DelaunayTriangle NextFlipTriangle(DTSweepContext tcx, Orientation o, DelaunayTriangle t,
-                                                         DelaunayTriangle ot, TriangulationPoint p,
-                                                         TriangulationPoint op)
-        {
-            int edgeIndex;
-            if (o == Orientation.CCW)
-            {
-                // ot is not crossing edge after flip
-                edgeIndex = ot.EdgeIndex(p, op);
-                ot.EdgeIsDelaunay[edgeIndex] = true;
-                Legalize(tcx, ot);
-                ot.EdgeIsDelaunay.Clear();
-                return t;
-            }
-            // t is not crossing edge after flip
-            edgeIndex = t.EdgeIndex(p, op);
-            t.EdgeIsDelaunay[edgeIndex] = true;
-            Legalize(tcx, t);
-            t.EdgeIsDelaunay.Clear();
-            return ot;
-        }
-
-        /// <summary>
-        /// Scan part of the FlipScan algorithm<br>
-        /// When a triangle pair isn't flippable we will scan for the next 
-        /// point that is inside the flip triangle scan area. When found 
-        /// we generate a new flipEdgeEvent
-        /// </summary>
-        /// <param name="tcx"></param>
-        /// <param name="ep">last point on the edge we are traversing</param>
-        /// <param name="eq">first point on the edge we are traversing</param>
-        /// <param name="flipTriangle">the current triangle sharing the point eq with edge</param>
-        /// <param name="t"></param>
-        /// <param name="p"></param>
-        private static void FlipScanEdgeEvent(DTSweepContext tcx, TriangulationPoint ep, TriangulationPoint eq,
-                                              DelaunayTriangle flipTriangle, DelaunayTriangle t, TriangulationPoint p)
-        {
-            DelaunayTriangle ot;
-            TriangulationPoint op, newP;
-            bool inScanArea;
-
-            ot = t.NeighborAcross(p);
-            op = ot.OppositePoint(t, p);
-
-            if (ot == null)
-            {
-                // If we want to integrate the fillEdgeEvent do it here
-                // With current implementation we should never get here
-                throw new Exception("[BUG:FIXME] FLIP failed due to missing triangle");
-            }
-
-            inScanArea = TriangulationUtil.InScanArea(eq, flipTriangle.PointCCW(eq), flipTriangle.PointCW(eq), op);
-            if (inScanArea)
-            {
-                // flip with new edge op->eq
-                FlipEdgeEvent(tcx, eq, op, ot, op);
-                // TODO: Actually I just figured out that it should be possible to 
-                //       improve this by getting the next ot and op before the the above 
-                //       flip and continue the flipScanEdgeEvent here
-                // set new ot and op here and loop back to inScanArea test
-                // also need to set a new flipTriangle first
-                // Turns out at first glance that this is somewhat complicated
-                // so it will have to wait.
-            }
-            else
-            {
-                newP = NextFlipPoint(ep, eq, ot, op);
-                FlipScanEdgeEvent(tcx, ep, eq, flipTriangle, ot, newP);
-            }
-        }
-
-        /// <summary>
-        /// Fills holes in the Advancing Front
-        /// </summary>
-        private static void FillAdvancingFront(DTSweepContext tcx, AdvancingFrontNode n)
-        {
-            AdvancingFrontNode node;
-            double angle;
-
-            // Fill right holes
-            node = n.Next;
-            while (node.HasNext)
-            {
-                angle = HoleAngle(node);
-                if (angle > PI_div2 || angle < -PI_div2)
-                {
-                    break;
-                }
-                Fill(tcx, node);
-                node = node.Next;
-            }
-
-            // Fill left holes
-            node = n.Prev;
-            while (node.HasPrev)
-            {
-                angle = HoleAngle(node);
-                if (angle > PI_div2 || angle < -PI_div2)
-                {
-                    break;
-                }
-                Fill(tcx, node);
-                node = node.Prev;
-            }
-
-            // Fill right basins
-            if (n.HasNext && n.Next.HasNext)
-            {
-                angle = BasinAngle(n);
-                if (angle < PI_3div4)
-                {
-                    FillBasin(tcx, n);
-                }
-            }
-        }
-
-        /// <summary>
-        /// Fills a basin that has formed on the Advancing Front to the right
-        /// of given node.<br>
-        /// First we decide a left,bottom and right node that forms the 
-        /// boundaries of the basin. Then we do a reqursive fill.
-        /// </summary>
-        /// <param name="tcx"></param>
-        /// <param name="node">starting node, this or next node will be left node</param>
-        private static void FillBasin(DTSweepContext tcx, AdvancingFrontNode node)
-        {
-            if (TriangulationUtil.Orient2d(node.Point, node.Next.Point, node.Next.Next.Point) == Orientation.CCW)
-            {
-                // tcx.basin.leftNode = node.next.next;
-                tcx.Basin.leftNode = node;
-            }
-            else
-            {
-                tcx.Basin.leftNode = node.Next;
-            }
-
-            // Find the bottom and right node
-            tcx.Basin.bottomNode = tcx.Basin.leftNode;
-            while (tcx.Basin.bottomNode.HasNext && tcx.Basin.bottomNode.Point.Y >= tcx.Basin.bottomNode.Next.Point.Y)
-            {
-                tcx.Basin.bottomNode = tcx.Basin.bottomNode.Next;
-            }
-
-            if (tcx.Basin.bottomNode == tcx.Basin.leftNode)
-            {
-                // No valid basins
-                return;
-            }
-
-            tcx.Basin.rightNode = tcx.Basin.bottomNode;
-            while (tcx.Basin.rightNode.HasNext && tcx.Basin.rightNode.Point.Y < tcx.Basin.rightNode.Next.Point.Y)
-            {
-                tcx.Basin.rightNode = tcx.Basin.rightNode.Next;
-            }
-
-            if (tcx.Basin.rightNode == tcx.Basin.bottomNode)
-            {
-                // No valid basins
-                return;
-            }
-
-            tcx.Basin.width = tcx.Basin.rightNode.Point.X - tcx.Basin.leftNode.Point.X;
-            tcx.Basin.leftHighest = tcx.Basin.leftNode.Point.Y > tcx.Basin.rightNode.Point.Y;
-
-            FillBasinReq(tcx, tcx.Basin.bottomNode);
-        }
-
-        /// <summary>
-        /// Recursive algorithm to fill a Basin with triangles
-        /// </summary>
-        private static void FillBasinReq(DTSweepContext tcx, AdvancingFrontNode node)
-        {
-            // if shallow stop filling
-            if (IsShallow(tcx, node))
-            {
-                return;
-            }
-
-            Fill(tcx, node);
-            if (node.Prev == tcx.Basin.leftNode && node.Next == tcx.Basin.rightNode)
-            {
-                return;
-            }
-            else if (node.Prev == tcx.Basin.leftNode)
-            {
-                Orientation o = TriangulationUtil.Orient2d(node.Point, node.Next.Point, node.Next.Next.Point);
-                if (o == Orientation.CW)
-                {
-                    return;
-                }
-                node = node.Next;
-            }
-            else if (node.Next == tcx.Basin.rightNode)
-            {
-                Orientation o = TriangulationUtil.Orient2d(node.Point, node.Prev.Point, node.Prev.Prev.Point);
-                if (o == Orientation.CCW)
-                {
-                    return;
-                }
-                node = node.Prev;
-            }
-            else
-            {
-                // Continue with the neighbor node with lowest Y value
-                if (node.Prev.Point.Y < node.Next.Point.Y)
-                {
-                    node = node.Prev;
-                }
-                else
-                {
-                    node = node.Next;
-                }
-            }
-            FillBasinReq(tcx, node);
-        }
-
-        private static bool IsShallow(DTSweepContext tcx, AdvancingFrontNode node)
-        {
-            double height;
-
-            if (tcx.Basin.leftHighest)
-            {
-                height = tcx.Basin.leftNode.Point.Y - node.Point.Y;
-            }
-            else
-            {
-                height = tcx.Basin.rightNode.Point.Y - node.Point.Y;
-            }
-            if (tcx.Basin.width > height)
-            {
-                return true;
-            }
-            return false;
-        }
-
-        /// <summary>
-        /// ???
-        /// </summary>
-        /// <param name="node">middle node</param>
-        /// <returns>the angle between 3 front nodes</returns>
-        private static double HoleAngle(AdvancingFrontNode node)
-        {
-            // XXX: do we really need a signed angle for holeAngle?
-            //      could possible save some cycles here
-            /* Complex plane
-             * ab = cosA +i*sinA
-             * ab = (ax + ay*i)(bx + by*i) = (ax*bx + ay*by) + i(ax*by-ay*bx)
-             * atan2(y,x) computes the principal value of the argument function
-             * applied to the complex number x+iy
-             * Where x = ax*bx + ay*by
-             *       y = ax*by - ay*bx
-             */
-            double px = node.Point.X;
-            double py = node.Point.Y;
-            double ax = node.Next.Point.X - px;
-            double ay = node.Next.Point.Y - py;
-            double bx = node.Prev.Point.X - px;
-            double by = node.Prev.Point.Y - py;
-            return Math.Atan2(ax*by - ay*bx, ax*bx + ay*by);
-        }
-
-        /// <summary>
-        /// The basin angle is decided against the horizontal line [1,0]
-        /// </summary>
-        private static double BasinAngle(AdvancingFrontNode node)
-        {
-            double ax = node.Point.X - node.Next.Next.Point.X;
-            double ay = node.Point.Y - node.Next.Next.Point.Y;
-            return Math.Atan2(ay, ax);
-        }
-
-        /// <summary>
-        /// Adds a triangle to the advancing front to fill a hole.
-        /// </summary>
-        /// <param name="tcx"></param>
-        /// <param name="node">middle node, that is the bottom of the hole</param>
-        private static void Fill(DTSweepContext tcx, AdvancingFrontNode node)
-        {
-            DelaunayTriangle triangle = new DelaunayTriangle(node.Prev.Point, node.Point, node.Next.Point);
-            // TODO: should copy the cEdge value from neighbor triangles
-            //       for now cEdge values are copied during the legalize 
-            triangle.MarkNeighbor(node.Prev.Triangle);
-            triangle.MarkNeighbor(node.Triangle);
-            tcx.Triangles.Add(triangle);
-
-            // Update the advancing front
-            node.Prev.Next = node.Next;
-            node.Next.Prev = node.Prev;
-            tcx.RemoveNode(node);
-
-            // If it was legalized the triangle has already been mapped
-            if (!Legalize(tcx, triangle))
-            {
-                tcx.MapTriangleToNodes(triangle);
-            }
-        }
-
-        /// <summary>
-        /// Returns true if triangle was legalized
-        /// </summary>
-        private static bool Legalize(DTSweepContext tcx, DelaunayTriangle t)
-        {
-            int oi;
-            bool inside;
-            TriangulationPoint p, op;
-            DelaunayTriangle ot;
-
-            // To legalize a triangle we start by finding if any of the three edges
-            // violate the Delaunay condition
-            for (int i = 0; i < 3; i++)
-            {
-                // TODO: fix so that cEdge is always valid when creating new triangles then we can check it here
-                //       instead of below with ot
-                if (t.EdgeIsDelaunay[i])
-                {
-                    continue;
-                }
-
-                ot = t.Neighbors[i];
-                if (ot != null)
-                {
-                    p = t.Points[i];
-                    op = ot.OppositePoint(t, p);
-                    oi = ot.IndexOf(op);
-                    // If this is a Constrained Edge or a Delaunay Edge(only during recursive legalization)
-                    // then we should not try to legalize
-                    if (ot.EdgeIsConstrained[oi] || ot.EdgeIsDelaunay[oi])
-                    {
-                        t.EdgeIsConstrained[i] = ot.EdgeIsConstrained[oi];
-                            // XXX: have no good way of setting this property when creating new triangles so lets set it here
-                        continue;
-                    }
-
-                    inside = TriangulationUtil.SmartIncircle(p,
-                                                             t.PointCCW(p),
-                                                             t.PointCW(p),
-                                                             op);
-
-                    if (inside)
-                    {
-                        bool notLegalized;
-
-                        // Lets mark this shared edge as Delaunay 
-                        t.EdgeIsDelaunay[i] = true;
-                        ot.EdgeIsDelaunay[oi] = true;
-
-                        // Lets rotate shared edge one vertex CW to legalize it
-                        RotateTrianglePair(t, p, ot, op);
-
-                        // We now got one valid Delaunay Edge shared by two triangles
-                        // This gives us 4 new edges to check for Delaunay
-
-                        // Make sure that triangle to node mapping is done only one time for a specific triangle
-                        notLegalized = !Legalize(tcx, t);
-
-                        if (notLegalized)
-                        {
-                            tcx.MapTriangleToNodes(t);
-                        }
-                        notLegalized = !Legalize(tcx, ot);
-                        if (notLegalized)
-                        {
-                            tcx.MapTriangleToNodes(ot);
-                        }
-
-                        // Reset the Delaunay edges, since they only are valid Delaunay edges
-                        // until we add a new triangle or point.
-                        // XXX: need to think about this. Can these edges be tried after we 
-                        //      return to previous recursive level?
-                        t.EdgeIsDelaunay[i] = false;
-                        ot.EdgeIsDelaunay[oi] = false;
-
-                        // If triangle have been legalized no need to check the other edges since
-                        // the recursive legalization will handles those so we can end here.
-                        return true;
-                    }
-                }
-            }
-            return false;
-        }
-
-        /// <summary>
-        /// Rotates a triangle pair one vertex CW
-        ///       n2                    n2
-        ///  P +-----+             P +-----+
-        ///    | t  /|               |\  t |  
-        ///    |   / |               | \   |
-        ///  n1|  /  |n3           n1|  \  |n3
-        ///    | /   |    after CW   |   \ |
-        ///    |/ oT |               | oT \|
-        ///    +-----+ oP            +-----+
-        ///       n4                    n4
-        /// </summary>
-        private static void RotateTrianglePair(DelaunayTriangle t, TriangulationPoint p, DelaunayTriangle ot,
-                                               TriangulationPoint op)
-        {
-            DelaunayTriangle n1, n2, n3, n4;
-            n1 = t.NeighborCCW(p);
-            n2 = t.NeighborCW(p);
-            n3 = ot.NeighborCCW(op);
-            n4 = ot.NeighborCW(op);
-
-            bool ce1, ce2, ce3, ce4;
-            ce1 = t.GetConstrainedEdgeCCW(p);
-            ce2 = t.GetConstrainedEdgeCW(p);
-            ce3 = ot.GetConstrainedEdgeCCW(op);
-            ce4 = ot.GetConstrainedEdgeCW(op);
-
-            bool de1, de2, de3, de4;
-            de1 = t.GetDelaunayEdgeCCW(p);
-            de2 = t.GetDelaunayEdgeCW(p);
-            de3 = ot.GetDelaunayEdgeCCW(op);
-            de4 = ot.GetDelaunayEdgeCW(op);
-
-            t.Legalize(p, op);
-            ot.Legalize(op, p);
-
-            // Remap dEdge
-            ot.SetDelaunayEdgeCCW(p, de1);
-            t.SetDelaunayEdgeCW(p, de2);
-            t.SetDelaunayEdgeCCW(op, de3);
-            ot.SetDelaunayEdgeCW(op, de4);
-
-            // Remap cEdge
-            ot.SetConstrainedEdgeCCW(p, ce1);
-            t.SetConstrainedEdgeCW(p, ce2);
-            t.SetConstrainedEdgeCCW(op, ce3);
-            ot.SetConstrainedEdgeCW(op, ce4);
-
-            // Remap neighbors
-            // XXX: might optimize the markNeighbor by keeping track of
-            //      what side should be assigned to what neighbor after the 
-            //      rotation. Now mark neighbor does lots of testing to find 
-            //      the right side.
-            t.Neighbors.Clear();
-            ot.Neighbors.Clear();
-            if (n1 != null) ot.MarkNeighbor(n1);
-            if (n2 != null) t.MarkNeighbor(n2);
-            if (n3 != null) t.MarkNeighbor(n3);
-            if (n4 != null) ot.MarkNeighbor(n4);
-            t.MarkNeighbor(ot);
-        }
-    }
-}

FarseerPhysicsEngine/Common/Decomposition/CDT/Delaunay/Sweep/DTSweepConstraint.cs

@@ -1,66 +0,0 @@
-/* Poly2Tri
- * Copyright (c) 2009-2010, Poly2Tri Contributors
- * http://code.google.com/p/poly2tri/
- *
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without modification,
- * are permitted provided that the following conditions are met:
- *
- * * Redistributions of source code must retain the above copyright notice,
- *   this list of conditions and the following disclaimer.
- * * Redistributions in binary form must reproduce the above copyright notice,
- *   this list of conditions and the following disclaimer in the documentation
- *   and/or other materials provided with the distribution.
- * * Neither the name of Poly2Tri nor the names of its contributors may be
- *   used to endorse or promote products derived from this software without specific
- *   prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
- * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
- * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
- * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
- * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
- * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
- * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
- * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
- * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
- */
-
-namespace Poly2Tri.Triangulation.Delaunay.Sweep
-{
-    public class DTSweepConstraint : TriangulationConstraint
-    {
-        /// <summary>
-        /// Give two points in any order. Will always be ordered so
-        /// that q.y > p.y and q.x > p.x if same y value 
-        /// </summary>
-        public DTSweepConstraint(TriangulationPoint p1, TriangulationPoint p2)
-        {
-            P = p1;
-            Q = p2;
-            if (p1.Y > p2.Y)
-            {
-                Q = p1;
-                P = p2;
-            }
-            else if (p1.Y == p2.Y)
-            {
-                if (p1.X > p2.X)
-                {
-                    Q = p1;
-                    P = p2;
-                }
-                else if (p1.X == p2.X)
-                {
-                    //                logger.info( "Failed to create constraint {}={}", p1, p2 );
-                    //                throw new DuplicatePointException( p1 + "=" + p2 );
-                    //                return;
-                }
-            }
-            Q.AddEdge(this);
-        }
-    }
-}

FarseerPhysicsEngine/Common/Decomposition/CDT/Delaunay/Sweep/DTSweepContext.cs

@@ -1,236 +0,0 @@
-/* Poly2Tri
- * Copyright (c) 2009-2010, Poly2Tri Contributors
- * http://code.google.com/p/poly2tri/
- *
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without modification,
- * are permitted provided that the following conditions are met:
- *
- * * Redistributions of source code must retain the above copyright notice,
- *   this list of conditions and the following disclaimer.
- * * Redistributions in binary form must reproduce the above copyright notice,
- *   this list of conditions and the following disclaimer in the documentation
- *   and/or other materials provided with the distribution.
- * * Neither the name of Poly2Tri nor the names of its contributors may be
- *   used to endorse or promote products derived from this software without specific
- *   prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
- * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
- * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
- * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
- * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
- * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
- * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
- * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
- * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
- */
-
-namespace Poly2Tri.Triangulation.Delaunay.Sweep
-{
-    /**
-     * 
-     * @author Thomas Åhlén, [email protected]
-     *
-     */
-
-    public class DTSweepContext : TriangulationContext
-    {
-        // Inital triangle factor, seed triangle will extend 30% of 
-        // PointSet width to both left and right.
-        private const float ALPHA = 0.3f;
-
-        public DTSweepBasin Basin = new DTSweepBasin();
-        public DTSweepEdgeEvent EdgeEvent = new DTSweepEdgeEvent();
-
-        private DTSweepPointComparator _comparator = new DTSweepPointComparator();
-        public AdvancingFront aFront;
-
-        public DTSweepContext()
-        {
-            Clear();
-        }
-
-        public TriangulationPoint Head { get; set; }
-        public TriangulationPoint Tail { get; set; }
-
-        public void RemoveFromList(DelaunayTriangle triangle)
-        {
-            Triangles.Remove(triangle);
-            // TODO: remove all neighbor pointers to this triangle
-            //        for( int i=0; i<3; i++ )
-            //        {
-            //            if( triangle.neighbors[i] != null )
-            //            {
-            //                triangle.neighbors[i].clearNeighbor( triangle );
-            //            }
-            //        }
-            //        triangle.clearNeighbors();
-        }
-
-        public void MeshClean(DelaunayTriangle triangle)
-        {
-            MeshCleanReq(triangle);
-        }
-
-        private void MeshCleanReq(DelaunayTriangle triangle)
-        {
-            if (triangle != null && !triangle.IsInterior)
-            {
-                triangle.IsInterior = true;
-                Triangulatable.AddTriangle(triangle);
-                for (int i = 0; i < 3; i++)
-                {
-                    if (!triangle.EdgeIsConstrained[i])
-                    {
-                        MeshCleanReq(triangle.Neighbors[i]);
-                    }
-                }
-            }
-        }
-
-        public override void Clear()
-        {
-            base.Clear();
-            Triangles.Clear();
-        }
-
-        public void AddNode(AdvancingFrontNode node)
-        {
-            //        Console.WriteLine( "add:" + node.key + ":" + System.identityHashCode(node.key));
-            //        m_nodeTree.put( node.getKey(), node );
-            aFront.AddNode(node);
-        }
-
-        public void RemoveNode(AdvancingFrontNode node)
-        {
-            //        Console.WriteLine( "remove:" + node.key + ":" + System.identityHashCode(node.key));
-            //        m_nodeTree.delete( node.getKey() );
-            aFront.RemoveNode(node);
-        }
-
-        public AdvancingFrontNode LocateNode(TriangulationPoint point)
-        {
-            return aFront.LocateNode(point);
-        }
-
-        public void CreateAdvancingFront()
-        {
-            AdvancingFrontNode head, tail, middle;
-            // Initial triangle
-            DelaunayTriangle iTriangle = new DelaunayTriangle(Points[0], Tail, Head);
-            Triangles.Add(iTriangle);
-
-            head = new AdvancingFrontNode(iTriangle.Points[1]);
-            head.Triangle = iTriangle;
-            middle = new AdvancingFrontNode(iTriangle.Points[0]);
-            middle.Triangle = iTriangle;
-            tail = new AdvancingFrontNode(iTriangle.Points[2]);
-
-            aFront = new AdvancingFront(head, tail);
-            aFront.AddNode(middle);
-
-            // TODO: I think it would be more intuitive if head is middles next and not previous
-            //       so swap head and tail
-            aFront.Head.Next = middle;
-            middle.Next = aFront.Tail;
-            middle.Prev = aFront.Head;
-            aFront.Tail.Prev = middle;
-        }
-
-        /// <summary>
-        /// Try to map a node to all sides of this triangle that don't have 
-        /// a neighbor.
-        /// </summary>
-        public void MapTriangleToNodes(DelaunayTriangle t)
-        {
-            AdvancingFrontNode n;
-            for (int i = 0; i < 3; i++)
-            {
-                if (t.Neighbors[i] == null)
-                {
-                    n = aFront.LocatePoint(t.PointCW(t.Points[i]));
-                    if (n != null)
-                    {
-                        n.Triangle = t;
-                    }
-                }
-            }
-        }
-
-        public override void PrepareTriangulation(Triangulatable t)
-        {
-            base.PrepareTriangulation(t);
-
-            double xmax, xmin;
-            double ymax, ymin;
-
-            xmax = xmin = Points[0].X;
-            ymax = ymin = Points[0].Y;
-
-            // Calculate bounds. Should be combined with the sorting
-            foreach (TriangulationPoint p in Points)
-            {
-                if (p.X > xmax)
-                    xmax = p.X;
-                if (p.X < xmin)
-                    xmin = p.X;
-                if (p.Y > ymax)
-                    ymax = p.Y;
-                if (p.Y < ymin)
-                    ymin = p.Y;
-            }
-
-            double deltaX = ALPHA*(xmax - xmin);
-            double deltaY = ALPHA*(ymax - ymin);
-            TriangulationPoint p1 = new TriangulationPoint(xmax + deltaX, ymin - deltaY);
-            TriangulationPoint p2 = new TriangulationPoint(xmin - deltaX, ymin - deltaY);
-
-            Head = p1;
-            Tail = p2;
-
-            //        long time = System.nanoTime();
-            // Sort the points along y-axis
-            Points.Sort(_comparator);
-            //        logger.info( "Triangulation setup [{}ms]", ( System.nanoTime() - time ) / 1e6 );
-        }
-
-
-        public void FinalizeTriangulation()
-        {
-            Triangulatable.AddTriangles(Triangles);
-            Triangles.Clear();
-        }
-
-        public override TriangulationConstraint NewConstraint(TriangulationPoint a, TriangulationPoint b)
-        {
-            return new DTSweepConstraint(a, b);
-        }
-
-        #region Nested type: DTSweepBasin
-
-        public class DTSweepBasin
-        {
-            public AdvancingFrontNode bottomNode;
-            public bool leftHighest;
-            public AdvancingFrontNode leftNode;
-            public AdvancingFrontNode rightNode;
-            public double width;
-        }
-
-        #endregion
-
-        #region Nested type: DTSweepEdgeEvent
-
-        public class DTSweepEdgeEvent
-        {
-            public DTSweepConstraint ConstrainedEdge;
-            public bool Right;
-        }
-
-        #endregion
-    }
-}

FarseerPhysicsEngine/Common/Decomposition/CDT/Delaunay/Sweep/DTSweepPointComparator.cs

@@ -1,69 +0,0 @@
-/* Poly2Tri
- * Copyright (c) 2009-2010, Poly2Tri Contributors
- * http://code.google.com/p/poly2tri/
- *
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without modification,
- * are permitted provided that the following conditions are met:
- *
- * * Redistributions of source code must retain the above copyright notice,
- *   this list of conditions and the following disclaimer.
- * * Redistributions in binary form must reproduce the above copyright notice,
- *   this list of conditions and the following disclaimer in the documentation
- *   and/or other materials provided with the distribution.
- * * Neither the name of Poly2Tri nor the names of its contributors may be
- *   used to endorse or promote products derived from this software without specific
- *   prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
- * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
- * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
- * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
- * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
- * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
- * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
- * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
- * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
- */
-
-using System.Collections.Generic;
-
-namespace Poly2Tri.Triangulation.Delaunay.Sweep
-{
-    public class DTSweepPointComparator : IComparer<TriangulationPoint>
-    {
-        #region IComparer<TriangulationPoint> Members
-
-        public int Compare(TriangulationPoint p1, TriangulationPoint p2)
-        {
-            if (p1.Y < p2.Y)
-            {
-                return -1;
-            }
-            else if (p1.Y > p2.Y)
-            {
-                return 1;
-            }
-            else
-            {
-                if (p1.X < p2.X)
-                {
-                    return -1;
-                }
-                else if (p1.X > p2.X)
-                {
-                    return 1;
-                }
-                else
-                {
-                    return 0;
-                }
-            }
-        }
-
-        #endregion
-    }
-}

FarseerPhysicsEngine/Common/Decomposition/CDT/Delaunay/Sweep/PointOnEdgeException.cs

@@ -1,43 +0,0 @@
-/* Poly2Tri
- * Copyright (c) 2009-2010, Poly2Tri Contributors
- * http://code.google.com/p/poly2tri/
- *
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without modification,
- * are permitted provided that the following conditions are met:
- *
- * * Redistributions of source code must retain the above copyright notice,
- *   this list of conditions and the following disclaimer.
- * * Redistributions in binary form must reproduce the above copyright notice,
- *   this list of conditions and the following disclaimer in the documentation
- *   and/or other materials provided with the distribution.
- * * Neither the name of Poly2Tri nor the names of its contributors may be
- *   used to endorse or promote products derived from this software without specific
- *   prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
- * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
- * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
- * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
- * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
- * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
- * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
- * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
- * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
- */
-
-using System;
-
-namespace Poly2Tri.Triangulation.Delaunay.Sweep
-{
-    public class PointOnEdgeException : NotImplementedException
-    {
-        public PointOnEdgeException(string message)
-            : base(message)
-        {
-        }
-    }
-}

FarseerPhysicsEngine/Common/Decomposition/CDT/ITriangulatable.cs

@@ -1,48 +0,0 @@
-/* Poly2Tri
- * Copyright (c) 2009-2010, Poly2Tri Contributors
- * http://code.google.com/p/poly2tri/
- *
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without modification,
- * are permitted provided that the following conditions are met:
- *
- * * Redistributions of source code must retain the above copyright notice,
- *   this list of conditions and the following disclaimer.
- * * Redistributions in binary form must reproduce the above copyright notice,
- *   this list of conditions and the following disclaimer in the documentation
- *   and/or other materials provided with the distribution.
- * * Neither the name of Poly2Tri nor the names of its contributors may be
- *   used to endorse or promote products derived from this software without specific
- *   prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
- * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
- * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
- * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
- * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
- * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
- * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
- * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
- * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
- */
-
-using System.Collections.Generic;
-using Poly2Tri.Triangulation.Delaunay;
-
-namespace Poly2Tri.Triangulation
-{
-    public interface Triangulatable
-    {
-        IList<TriangulationPoint> Points { get; } // MM: Neither of these are used via interface (yet?)
-        IList<DelaunayTriangle> Triangles { get; }
-        TriangulationMode TriangulationMode { get; }
-        void PrepareTriangulation(TriangulationContext tcx);
-
-        void AddTriangle(DelaunayTriangle t);
-        void AddTriangles(IEnumerable<DelaunayTriangle> list);
-        void ClearTriangles();
-    }
-}

FarseerPhysicsEngine/Common/Decomposition/CDT/Orientation.cs

@@ -1,40 +0,0 @@
-/* Poly2Tri
- * Copyright (c) 2009-2010, Poly2Tri Contributors
- * http://code.google.com/p/poly2tri/
- *
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without modification,
- * are permitted provided that the following conditions are met:
- *
- * * Redistributions of source code must retain the above copyright notice,
- *   this list of conditions and the following disclaimer.
- * * Redistributions in binary form must reproduce the above copyright notice,
- *   this list of conditions and the following disclaimer in the documentation
- *   and/or other materials provided with the distribution.
- * * Neither the name of Poly2Tri nor the names of its contributors may be
- *   used to endorse or promote products derived from this software without specific
- *   prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
- * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
- * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
- * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
- * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
- * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
- * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
- * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
- * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
- */
-
-namespace FarseerPhysics.Common.Decomposition.CDT
-{
-    public enum Orientation
-    {
-        CW,
-        CCW,
-        Collinear
-    }
-}

FarseerPhysicsEngine/Common/Decomposition/CDT/Polygon/Polygon.cs

@@ -1,272 +0,0 @@
-/* Poly2Tri
- * Copyright (c) 2009-2010, Poly2Tri Contributors
- * http://code.google.com/p/poly2tri/
- *
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without modification,
- * are permitted provided that the following conditions are met:
- *
- * * Redistributions of source code must retain the above copyright notice,
- *   this list of conditions and the following disclaimer.
- * * Redistributions in binary form must reproduce the above copyright notice,
- *   this list of conditions and the following disclaimer in the documentation
- *   and/or other materials provided with the distribution.
- * * Neither the name of Poly2Tri nor the names of its contributors may be
- *   used to endorse or promote products derived from this software without specific
- *   prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
- * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
- * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
- * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
- * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
- * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
- * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
- * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
- * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
- */
-
-// Changes from the Java version
-//   Polygon constructors sprused up, checks for 3+ polys
-//   Naming of everything
-//   getTriangulationMode() -> TriangulationMode { get; }
-//   Exceptions replaced
-// Future possibilities
-//   We have a lot of Add/Clear methods -- we may prefer to just expose the container
-//   Some self-explanitory methods may deserve commenting anyways
-
-using System;
-using System.Collections.Generic;
-using System.Linq;
-using Poly2Tri.Triangulation.Delaunay;
-
-namespace Poly2Tri.Triangulation.Polygon
-{
-    public class Polygon : Triangulatable
-    {
-        protected List<Polygon> _holes;
-        protected PolygonPoint _last;
-        protected List<TriangulationPoint> _points = new List<TriangulationPoint>();
-        protected List<TriangulationPoint> _steinerPoints;
-        protected List<DelaunayTriangle> _triangles;
-
-        /// <summary>
-        /// Create a polygon from a list of at least 3 points with no duplicates.
-        /// </summary>
-        /// <param name="points">A list of unique points</param>
-        public Polygon(IList<PolygonPoint> points)
-        {
-            if (points.Count < 3) throw new ArgumentException("List has fewer than 3 points", "points");
-
-            // Lets do one sanity check that first and last point hasn't got same position
-            // Its something that often happen when importing polygon data from other formats
-            if (points[0].Equals(points[points.Count - 1])) points.RemoveAt(points.Count - 1);
-
-            _points.AddRange(points.Cast<TriangulationPoint>());
-        }
-
-        /// <summary>
-        /// Create a polygon from a list of at least 3 points with no duplicates.
-        /// </summary>
-        /// <param name="points">A list of unique points.</param>
-        public Polygon(IEnumerable<PolygonPoint> points) : this((points as IList<PolygonPoint>) ?? points.ToArray())
-        {
-        }
-
-        public Polygon()
-        {
-        }
-
-        public IList<Polygon> Holes
-        {
-            get { return _holes; }
-        }
-
-        #region Triangulatable Members
-
-        public TriangulationMode TriangulationMode
-        {
-            get { return TriangulationMode.Polygon; }
-        }
-
-        public IList<TriangulationPoint> Points
-        {
-            get { return _points; }
-        }
-
-        public IList<DelaunayTriangle> Triangles
-        {
-            get { return _triangles; }
-        }
-
-        public void AddTriangle(DelaunayTriangle t)
-        {
-            _triangles.Add(t);
-        }
-
-        public void AddTriangles(IEnumerable<DelaunayTriangle> list)
-        {
-            _triangles.AddRange(list);
-        }
-
-        public void ClearTriangles()
-        {
-            if (_triangles != null) _triangles.Clear();
-        }
-
-        /// <summary>
-        /// Creates constraints and populates the context with points
-        /// </summary>
-        /// <param name="tcx">The context</param>
-        public void PrepareTriangulation(TriangulationContext tcx)
-        {
-            if (_triangles == null)
-            {
-                _triangles = new List<DelaunayTriangle>(_points.Count);
-            }
-            else
-            {
-                _triangles.Clear();
-            }
-
-            // Outer constraints
-            for (int i = 0; i < _points.Count - 1; i++)
-            {
-                tcx.NewConstraint(_points[i], _points[i + 1]);
-            }
-            tcx.NewConstraint(_points[0], _points[_points.Count - 1]);
-            tcx.Points.AddRange(_points);
-
-            // Hole constraints
-            if (_holes != null)
-            {
-                foreach (Polygon p in _holes)
-                {
-                    for (int i = 0; i < p._points.Count - 1; i++)
-                    {
-                        tcx.NewConstraint(p._points[i], p._points[i + 1]);
-                    }
-                    tcx.NewConstraint(p._points[0], p._points[p._points.Count - 1]);
-                    tcx.Points.AddRange(p._points);
-                }
-            }
-
-            if (_steinerPoints != null)
-            {
-                tcx.Points.AddRange(_steinerPoints);
-            }
-        }
-
-        #endregion
-
-        public void AddSteinerPoint(TriangulationPoint point)
-        {
-            if (_steinerPoints == null)
-            {
-                _steinerPoints = new List<TriangulationPoint>();
-            }
-            _steinerPoints.Add(point);
-        }
-
-        public void AddSteinerPoints(List<TriangulationPoint> points)
-        {
-            if (_steinerPoints == null)
-            {
-                _steinerPoints = new List<TriangulationPoint>();
-            }
-            _steinerPoints.AddRange(points);
-        }
-
-        public void ClearSteinerPoints()
-        {
-            if (_steinerPoints != null)
-            {
-                _steinerPoints.Clear();
-            }
-        }
-
-        /// <summary>
-        /// Add a hole to the polygon.
-        /// </summary>
-        /// <param name="poly">A subtraction polygon fully contained inside this polygon.</param>
-        public void AddHole(Polygon poly)
-        {
-            if (_holes == null) _holes = new List<Polygon>();
-            _holes.Add(poly);
-            // XXX: tests could be made here to be sure it is fully inside
-            //        addSubtraction( poly.getPoints() );
-        }
-
-        /// <summary>
-        /// Inserts newPoint after point.
-        /// </summary>
-        /// <param name="point">The point to insert after in the polygon</param>
-        /// <param name="newPoint">The point to insert into the polygon</param>
-        public void InsertPointAfter(PolygonPoint point, PolygonPoint newPoint)
-        {
-            // Validate that 
-            int index = _points.IndexOf(point);
-            if (index == -1)
-                throw new ArgumentException(
-                    "Tried to insert a point into a Polygon after a point not belonging to the Polygon", "point");
-            newPoint.Next = point.Next;
-            newPoint.Previous = point;
-            point.Next.Previous = newPoint;
-            point.Next = newPoint;
-            _points.Insert(index + 1, newPoint);
-        }
-
-        /// <summary>
-        /// Inserts list (after last point in polygon?)
-        /// </summary>
-        /// <param name="list"></param>
-        public void AddPoints(IEnumerable<PolygonPoint> list)
-        {
-            PolygonPoint first;
-            foreach (PolygonPoint p in list)
-            {
-                p.Previous = _last;
-                if (_last != null)
-                {
-                    p.Next = _last.Next;
-                    _last.Next = p;
-                }
-                _last = p;
-                _points.Add(p);
-            }
-            first = (PolygonPoint) _points[0];
-            _last.Next = first;
-            first.Previous = _last;
-        }
-
-        /// <summary>
-        /// Adds a point after the last in the polygon.
-        /// </summary>
-        /// <param name="p">The point to add</param>
-        public void AddPoint(PolygonPoint p)
-        {
-            p.Previous = _last;
-            p.Next = _last.Next;
-            _last.Next = p;
-            _points.Add(p);
-        }
-
-        /// <summary>
-        /// Removes a point from the polygon.
-        /// </summary>
-        /// <param name="p"></param>
-        public void RemovePoint(PolygonPoint p)
-        {
-            PolygonPoint next, prev;
-
-            next = p.Next;
-            prev = p.Previous;
-            prev.Next = next;
-            next.Previous = prev;
-            _points.Remove(p);
-        }
-    }
-}

FarseerPhysicsEngine/Common/Decomposition/CDT/Polygon/PolygonPoint.cs

@@ -1,48 +0,0 @@
-/* Poly2Tri
- * Copyright (c) 2009-2010, Poly2Tri Contributors
- * http://code.google.com/p/poly2tri/
- *
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without modification,
- * are permitted provided that the following conditions are met:
- *
- * * Redistributions of source code must retain the above copyright notice,
- *   this list of conditions and the following disclaimer.
- * * Redistributions in binary form must reproduce the above copyright notice,
- *   this list of conditions and the following disclaimer in the documentation
- *   and/or other materials provided with the distribution.
- * * Neither the name of Poly2Tri nor the names of its contributors may be
- *   used to endorse or promote products derived from this software without specific
- *   prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
- * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
- * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
- * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
- * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
- * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
- * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
- * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
- * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
- */
-
-// Changes from the Java version
-//   Replaced get/set Next/Previous with attributes
-// Future possibilities
-//   Documentation!
-
-namespace Poly2Tri.Triangulation.Polygon
-{
-    public class PolygonPoint : TriangulationPoint
-    {
-        public PolygonPoint(double x, double y) : base(x, y)
-        {
-        }
-
-        public PolygonPoint Next { get; set; }
-        public PolygonPoint Previous { get; set; }
-    }
-}

FarseerPhysicsEngine/Common/Decomposition/CDT/Polygon/PolygonSet.cs

@@ -1,65 +0,0 @@
-/* Poly2Tri
- * Copyright (c) 2009-2010, Poly2Tri Contributors
- * http://code.google.com/p/poly2tri/
- *
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without modification,
- * are permitted provided that the following conditions are met:
- *
- * * Redistributions of source code must retain the above copyright notice,
- *   this list of conditions and the following disclaimer.
- * * Redistributions in binary form must reproduce the above copyright notice,
- *   this list of conditions and the following disclaimer in the documentation
- *   and/or other materials provided with the distribution.
- * * Neither the name of Poly2Tri nor the names of its contributors may be
- *   used to endorse or promote products derived from this software without specific
- *   prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
- * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
- * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
- * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
- * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
- * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
- * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
- * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
- * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
- */
-
-// Changes from the Java version
-//   Replaced getPolygons with attribute
-// Future possibilities
-//   Replace Add(Polygon) with exposed container?
-//   Replace entire class with HashSet<Polygon> ?
-
-using System.Collections.Generic;
-
-namespace Poly2Tri.Triangulation.Polygon
-{
-    public class PolygonSet
-    {
-        protected List<Polygon> _polygons = new List<Polygon>();
-
-        public PolygonSet()
-        {
-        }
-
-        public PolygonSet(Polygon poly)
-        {
-            _polygons.Add(poly);
-        }
-
-        public IEnumerable<Polygon> Polygons
-        {
-            get { return _polygons; }
-        }
-
-        public void Add(Polygon p)
-        {
-            _polygons.Add(p);
-        }
-    }
-}

FarseerPhysicsEngine/Common/Decomposition/CDT/Sets/ConstrainedPointSet.cs

@@ -1,114 +0,0 @@
-/* Poly2Tri
- * Copyright (c) 2009-2010, Poly2Tri Contributors
- * http://code.google.com/p/poly2tri/
- *
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without modification,
- * are permitted provided that the following conditions are met:
- *
- * * Redistributions of source code must retain the above copyright notice,
- *   this list of conditions and the following disclaimer.
- * * Redistributions in binary form must reproduce the above copyright notice,
- *   this list of conditions and the following disclaimer in the documentation
- *   and/or other materials provided with the distribution.
- * * Neither the name of Poly2Tri nor the names of its contributors may be
- *   used to endorse or promote products derived from this software without specific
- *   prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
- * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
- * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
- * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
- * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
- * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
- * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
- * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
- * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
- */
-
-using System.Collections.Generic;
-
-namespace Poly2Tri.Triangulation.Sets
-{
-    /*
-     * Extends the PointSet by adding some Constraints on how it will be triangulated<br>
-     * A constraint defines an edge between two points in the set, these edges can not
-     * be crossed. They will be enforced triangle edges after a triangulation.
-     * <p>
-     * 
-     * 
-     * @author Thomas Åhlén, [email protected]
-     */
-
-    public class ConstrainedPointSet : PointSet
-    {
-        private List<TriangulationPoint> _constrainedPointList = null;
-
-        public ConstrainedPointSet(List<TriangulationPoint> points, int[] index)
-            : base(points)
-        {
-            EdgeIndex = index;
-        }
-
-        /**
-         * 
-         * @param points - A list of all points in PointSet
-         * @param constraints - Pairs of two points defining a constraint, all points <b>must</b> be part of given PointSet!
-         */
-
-        public ConstrainedPointSet(List<TriangulationPoint> points, IEnumerable<TriangulationPoint> constraints)
-            : base(points)
-        {
-            _constrainedPointList = new List<TriangulationPoint>();
-            _constrainedPointList.AddRange(constraints);
-        }
-
-        public int[] EdgeIndex { get; private set; }
-
-        public override TriangulationMode TriangulationMode
-        {
-            get { return TriangulationMode.Constrained; }
-        }
-
-        public override void PrepareTriangulation(TriangulationContext tcx)
-        {
-            base.PrepareTriangulation(tcx);
-            if (_constrainedPointList != null)
-            {
-                TriangulationPoint p1, p2;
-                List<TriangulationPoint>.Enumerator iterator = _constrainedPointList.GetEnumerator();
-                while (iterator.MoveNext())
-                {
-                    p1 = iterator.Current;
-                    iterator.MoveNext();
-                    p2 = iterator.Current;
-                    tcx.NewConstraint(p1, p2);
-                }
-            }
-            else
-            {
-                for (int i = 0; i < EdgeIndex.Length; i += 2)
-                {
-                    // XXX: must change!!
-                    tcx.NewConstraint(Points[EdgeIndex[i]], Points[EdgeIndex[i + 1]]);
-                }
-            }
-        }
-
-        /**
-         * TODO: TO BE IMPLEMENTED!
-         * Peforms a validation on given input<br>
-         * 1. Check's if there any constraint edges are crossing or collinear<br>
-         * 2. 
-         * @return
-         */
-
-        public bool isValid()
-        {
-            return true;
-        }
-    }
-}

FarseerPhysicsEngine/Common/Decomposition/CDT/Sets/PointSet.cs

@@ -1,84 +0,0 @@
-/* Poly2Tri
- * Copyright (c) 2009-2010, Poly2Tri Contributors
- * http://code.google.com/p/poly2tri/
- *
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without modification,
- * are permitted provided that the following conditions are met:
- *
- * * Redistributions of source code must retain the above copyright notice,
- *   this list of conditions and the following disclaimer.
- * * Redistributions in binary form must reproduce the above copyright notice,
- *   this list of conditions and the following disclaimer in the documentation
- *   and/or other materials provided with the distribution.
- * * Neither the name of Poly2Tri nor the names of its contributors may be
- *   used to endorse or promote products derived from this software without specific
- *   prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
- * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
- * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
- * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
- * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
- * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
- * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
- * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
- * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
- */
-
-using System.Collections.Generic;
-using Poly2Tri.Triangulation.Delaunay;
-
-namespace Poly2Tri.Triangulation.Sets
-{
-    public class PointSet : Triangulatable
-    {
-        public PointSet(List<TriangulationPoint> points)
-        {
-            Points = new List<TriangulationPoint>(points);
-        }
-
-        #region Triangulatable Members
-
-        public IList<TriangulationPoint> Points { get; private set; }
-        public IList<DelaunayTriangle> Triangles { get; private set; }
-
-        public virtual TriangulationMode TriangulationMode
-        {
-            get { return TriangulationMode.Unconstrained; }
-        }
-
-        public void AddTriangle(DelaunayTriangle t)
-        {
-            Triangles.Add(t);
-        }
-
-        public void AddTriangles(IEnumerable<DelaunayTriangle> list)
-        {
-            foreach (DelaunayTriangle tri in list) Triangles.Add(tri);
-        }
-
-        public void ClearTriangles()
-        {
-            Triangles.Clear();
-        }
-
-        public virtual void PrepareTriangulation(TriangulationContext tcx)
-        {
-            if (Triangles == null)
-            {
-                Triangles = new List<DelaunayTriangle>(Points.Count);
-            }
-            else
-            {
-                Triangles.Clear();
-            }
-            tcx.Points.AddRange(Points);
-        }
-
-        #endregion
-    }
-}

FarseerPhysicsEngine/Common/Decomposition/CDT/TriangulationConstraint.cs

@@ -1,46 +0,0 @@
-/* Poly2Tri
- * Copyright (c) 2009-2010, Poly2Tri Contributors
- * http://code.google.com/p/poly2tri/
- *
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without modification,
- * are permitted provided that the following conditions are met:
- *
- * * Redistributions of source code must retain the above copyright notice,
- *   this list of conditions and the following disclaimer.
- * * Redistributions in binary form must reproduce the above copyright notice,
- *   this list of conditions and the following disclaimer in the documentation
- *   and/or other materials provided with the distribution.
- * * Neither the name of Poly2Tri nor the names of its contributors may be
- *   used to endorse or promote products derived from this software without specific
- *   prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
- * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
- * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
- * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
- * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
- * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
- * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
- * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
- * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
- */
-
-/**
- * Forces a triangle edge between two points p and q
- * when triangulating. For example used to enforce
- * Polygon Edges during a polygon triangulation.
- * 
- * @author Thomas Åhlén, [email protected]
- */
-namespace Poly2Tri.Triangulation
-{
-    public class TriangulationConstraint
-    {
-        public TriangulationPoint P;
-        public TriangulationPoint Q;
-    }
-}

FarseerPhysicsEngine/Common/Decomposition/CDT/TriangulationContext.cs

@@ -1,84 +0,0 @@
-/* Poly2Tri
- * Copyright (c) 2009-2010, Poly2Tri Contributors
- * http://code.google.com/p/poly2tri/
- *
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without modification,
- * are permitted provided that the following conditions are met:
- *
- * * Redistributions of source code must retain the above copyright notice,
- *   this list of conditions and the following disclaimer.
- * * Redistributions in binary form must reproduce the above copyright notice,
- *   this list of conditions and the following disclaimer in the documentation
- *   and/or other materials provided with the distribution.
- * * Neither the name of Poly2Tri nor the names of its contributors may be
- *   used to endorse or promote products derived from this software without specific
- *   prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
- * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
- * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
- * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
- * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
- * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
- * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
- * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
- * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
- */
-
-using System.Collections.Generic;
-using System.Runtime.CompilerServices;
-using Poly2Tri.Triangulation.Delaunay;
-
-namespace Poly2Tri.Triangulation
-{
-    public abstract class TriangulationContext
-    {
-        public readonly List<TriangulationPoint> Points = new List<TriangulationPoint>(200);
-        public readonly List<DelaunayTriangle> Triangles = new List<DelaunayTriangle>();
-        private int _stepTime = -1;
-
-        public TriangulationContext()
-        {
-            Terminated = false;
-        }
-
-        public TriangulationMode TriangulationMode { get; protected set; }
-        public Triangulatable Triangulatable { get; private set; }
-
-        public bool WaitUntilNotified { get; private set; }
-        public bool Terminated { get; set; }
-
-        public int StepCount { get; private set; }
-        public virtual bool IsDebugEnabled { get; protected set; }
-
-        public void Done()
-        {
-            StepCount++;
-        }
-
-        public virtual void PrepareTriangulation(Triangulatable t)
-        {
-            Triangulatable = t;
-            TriangulationMode = t.TriangulationMode;
-            t.PrepareTriangulation(this);
-        }
-
-        public abstract TriangulationConstraint NewConstraint(TriangulationPoint a, TriangulationPoint b);
-
-        [MethodImpl(MethodImplOptions.Synchronized)]
-        public void Update(string message)
-        {
-        }
-
-        public virtual void Clear()
-        {
-            Points.Clear();
-            Terminated = false;
-            StepCount = 0;
-        }
-    }
-}

FarseerPhysicsEngine/Common/Decomposition/CDT/TriangulationMode.cs

@@ -1,40 +0,0 @@
-/* Poly2Tri
- * Copyright (c) 2009-2010, Poly2Tri Contributors
- * http://code.google.com/p/poly2tri/
- *
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without modification,
- * are permitted provided that the following conditions are met:
- *
- * * Redistributions of source code must retain the above copyright notice,
- *   this list of conditions and the following disclaimer.
- * * Redistributions in binary form must reproduce the above copyright notice,
- *   this list of conditions and the following disclaimer in the documentation
- *   and/or other materials provided with the distribution.
- * * Neither the name of Poly2Tri nor the names of its contributors may be
- *   used to endorse or promote products derived from this software without specific
- *   prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
- * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
- * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
- * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
- * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
- * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
- * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
- * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
- * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
- */
-
-namespace Poly2Tri.Triangulation
-{
-    public enum TriangulationMode
-    {
-        Unconstrained,
-        Constrained,
-        Polygon
-    }
-}

FarseerPhysicsEngine/Common/Decomposition/CDT/TriangulationPoint.cs

@@ -1,82 +0,0 @@
-/* Poly2Tri
- * Copyright (c) 2009-2010, Poly2Tri Contributors
- * http://code.google.com/p/poly2tri/
- *
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without modification,
- * are permitted provided that the following conditions are met:
- *
- * * Redistributions of source code must retain the above copyright notice,
- *   this list of conditions and the following disclaimer.
- * * Redistributions in binary form must reproduce the above copyright notice,
- *   this list of conditions and the following disclaimer in the documentation
- *   and/or other materials provided with the distribution.
- * * Neither the name of Poly2Tri nor the names of its contributors may be
- *   used to endorse or promote products derived from this software without specific
- *   prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
- * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
- * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
- * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
- * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
- * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
- * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
- * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
- * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
- */
-
-using System.Collections.Generic;
-using Poly2Tri.Triangulation.Delaunay.Sweep;
-
-namespace Poly2Tri.Triangulation
-{
-    public class TriangulationPoint
-    {
-        // List of edges this point constitutes an upper ending point (CDT)
-
-        public double X, Y;
-
-        public TriangulationPoint(double x, double y)
-        {
-            X = x;
-            Y = y;
-        }
-
-        public List<DTSweepConstraint> Edges { get; private set; }
-
-        public float Xf
-        {
-            get { return (float) X; }
-            set { X = value; }
-        }
-
-        public float Yf
-        {
-            get { return (float) Y; }
-            set { Y = value; }
-        }
-
-        public bool HasEdges
-        {
-            get { return Edges != null; }
-        }
-
-        public override string ToString()
-        {
-            return "[" + X + "," + Y + "]";
-        }
-
-        public void AddEdge(DTSweepConstraint e)
-        {
-            if (Edges == null)
-            {
-                Edges = new List<DTSweepConstraint>();
-            }
-            Edges.Add(e);
-        }
-    }
-}

FarseerPhysicsEngine/Common/Decomposition/CDT/TriangulationUtil.cs

@@ -1,160 +0,0 @@
-/* Poly2Tri
- * Copyright (c) 2009-2010, Poly2Tri Contributors
- * http://code.google.com/p/poly2tri/
- *
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without modification,
- * are permitted provided that the following conditions are met:
- *
- * * Redistributions of source code must retain the above copyright notice,
- *   this list of conditions and the following disclaimer.
- * * Redistributions in binary form must reproduce the above copyright notice,
- *   this list of conditions and the following disclaimer in the documentation
- *   and/or other materials provided with the distribution.
- * * Neither the name of Poly2Tri nor the names of its contributors may be
- *   used to endorse or promote products derived from this software without specific
- *   prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
- * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
- * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
- * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
- * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
- * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
- * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
- * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
- * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
- */
-
-using FarseerPhysics.Common.Decomposition.CDT;
-
-namespace Poly2Tri.Triangulation
-{
-    /**
-     * @author Thomas Åhlén, [email protected]
-     */
-
-    public class TriangulationUtil
-    {
-        public static double EPSILON = 1e-12;
-
-        /// <summary>
-        ///   Requirements:
-        /// 1. a,b and c form a triangle.
-        /// 2. a and d is know to be on opposite side of bc
-        /// <code>
-        ///                a
-        ///                +
-        ///               / \
-        ///              /   \
-        ///            b/     \c
-        ///            +-------+ 
-        ///           /    B    \  
-        ///          /           \ 
-        /// </code>
-        ///    Facts:
-        ///  d has to be in area B to have a chance to be inside the circle formed by a,b and c
-        ///  d is outside B if orient2d(a,b,d) or orient2d(c,a,d) is CW
-        ///  This preknowledge gives us a way to optimize the incircle test
-        /// </summary>
-        /// <param name="pa">triangle point, opposite d</param>
-        /// <param name="pb">triangle point</param>
-        /// <param name="pc">triangle point</param>
-        /// <param name="pd">point opposite a</param>
-        /// <returns>true if d is inside circle, false if on circle edge</returns>
-        public static bool SmartIncircle(TriangulationPoint pa, TriangulationPoint pb, TriangulationPoint pc,
-                                         TriangulationPoint pd)
-        {
-            double pdx = pd.X;
-            double pdy = pd.Y;
-            double adx = pa.X - pdx;
-            double ady = pa.Y - pdy;
-            double bdx = pb.X - pdx;
-            double bdy = pb.Y - pdy;
-
-            double adxbdy = adx*bdy;
-            double bdxady = bdx*ady;
-            double oabd = adxbdy - bdxady;
-            //        oabd = orient2d(pa,pb,pd);
-            if (oabd <= 0) return false;
-
-            double cdx = pc.X - pdx;
-            double cdy = pc.Y - pdy;
-
-            double cdxady = cdx*ady;
-            double adxcdy = adx*cdy;
-            double ocad = cdxady - adxcdy;
-            //      ocad = orient2d(pc,pa,pd);
-            if (ocad <= 0) return false;
-
-            double bdxcdy = bdx*cdy;
-            double cdxbdy = cdx*bdy;
-
-            double alift = adx*adx + ady*ady;
-            double blift = bdx*bdx + bdy*bdy;
-            double clift = cdx*cdx + cdy*cdy;
-
-            double det = alift*(bdxcdy - cdxbdy) + blift*ocad + clift*oabd;
-
-            return det > 0;
-        }
-
-        public static bool InScanArea(TriangulationPoint pa, TriangulationPoint pb, TriangulationPoint pc,
-                                      TriangulationPoint pd)
-        {
-            double pdx = pd.X;
-            double pdy = pd.Y;
-            double adx = pa.X - pdx;
-            double ady = pa.Y - pdy;
-            double bdx = pb.X - pdx;
-            double bdy = pb.Y - pdy;
-
-            double adxbdy = adx*bdy;
-            double bdxady = bdx*ady;
-            double oabd = adxbdy - bdxady;
-            //        oabd = orient2d(pa,pb,pd);
-            if (oabd <= 0)
-            {
-                return false;
-            }
-
-            double cdx = pc.X - pdx;
-            double cdy = pc.Y - pdy;
-
-            double cdxady = cdx*ady;
-            double adxcdy = adx*cdy;
-            double ocad = cdxady - adxcdy;
-            //      ocad = orient2d(pc,pa,pd);
-            if (ocad <= 0)
-            {
-                return false;
-            }
-            return true;
-        }
-
-        /// Forumla to calculate signed area
-        /// Positive if CCW
-        /// Negative if CW
-        /// 0 if collinear
-        /// A[P1,P2,P3]  =  (x1*y2 - y1*x2) + (x2*y3 - y2*x3) + (x3*y1 - y3*x1)
-        ///              =  (x1-x3)*(y2-y3) - (y1-y3)*(x2-x3)
-        public static Orientation Orient2d(TriangulationPoint pa, TriangulationPoint pb, TriangulationPoint pc)
-        {
-            double detleft = (pa.X - pc.X)*(pb.Y - pc.Y);
-            double detright = (pa.Y - pc.Y)*(pb.X - pc.X);
-            double val = detleft - detright;
-            if (val > -EPSILON && val < EPSILON)
-            {
-                return Orientation.Collinear;
-            }
-            else if (val > 0)
-            {
-                return Orientation.CCW;
-            }
-            return Orientation.CW;
-        }
-    }
-}

FarseerPhysicsEngine/Common/Decomposition/CDT/Util/FixedArray3.cs

@@ -1,118 +0,0 @@
-/* Poly2Tri
- * Copyright (c) 2009-2010, Poly2Tri Contributors
- * http://code.google.com/p/poly2tri/
- *
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without modification,
- * are permitted provided that the following conditions are met:
- *
- * * Redistributions of source code must retain the above copyright notice,
- *   this list of conditions and the following disclaimer.
- * * Redistributions in binary form must reproduce the above copyright notice,
- *   this list of conditions and the following disclaimer in the documentation
- *   and/or other materials provided with the distribution.
- * * Neither the name of Poly2Tri nor the names of its contributors may be
- *   used to endorse or promote products derived from this software without specific
- *   prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
- * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
- * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
- * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
- * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
- * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
- * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
- * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
- * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
- */
-
-using System;
-using System.Collections;
-using System.Collections.Generic;
-
-namespace Poly2Tri.Triangulation.Util
-{
-    public struct FixedArray3<T> : IEnumerable<T> where T : class
-    {
-        public T _0, _1, _2;
-
-        public T this[int index]
-        {
-            get
-            {
-                switch (index)
-                {
-                    case 0:
-                        return _0;
-                    case 1:
-                        return _1;
-                    case 2:
-                        return _2;
-                    default:
-                        throw new IndexOutOfRangeException();
-                }
-            }
-            set
-            {
-                switch (index)
-                {
-                    case 0:
-                        _0 = value;
-                        break;
-                    case 1:
-                        _1 = value;
-                        break;
-                    case 2:
-                        _2 = value;
-                        break;
-                    default:
-                        throw new IndexOutOfRangeException();
-                }
-            }
-        }
-
-        #region IEnumerable<T> Members
-
-        public IEnumerator<T> GetEnumerator()
-        {
-            return Enumerate().GetEnumerator();
-        }
-
-        IEnumerator IEnumerable.GetEnumerator()
-        {
-            return GetEnumerator();
-        }
-
-        #endregion
-
-        public bool Contains(T value)
-        {
-            for (int i = 0; i < 3; ++i) if (this[i] == value) return true;
-            return false;
-        }
-
-        public int IndexOf(T value)
-        {
-            for (int i = 0; i < 3; ++i) if (this[i] == value) return i;
-            return -1;
-        }
-
-        public void Clear()
-        {
-            _0 = _1 = _2 = null;
-        }
-
-        public void Clear(T value)
-        {
-            for (int i = 0; i < 3; ++i) if (this[i] == value) this[i] = null;
-        }
-
-        private IEnumerable<T> Enumerate()
-        {
-            for (int i = 0; i < 3; ++i) yield return this[i];
-        }
-    }
-}

FarseerPhysicsEngine/Common/Decomposition/CDT/Util/FixedBitArray3.cs

@@ -1,118 +0,0 @@
-/* Poly2Tri
- * Copyright (c) 2009-2010, Poly2Tri Contributors
- * http://code.google.com/p/poly2tri/
- *
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without modification,
- * are permitted provided that the following conditions are met:
- *
- * * Redistributions of source code must retain the above copyright notice,
- *   this list of conditions and the following disclaimer.
- * * Redistributions in binary form must reproduce the above copyright notice,
- *   this list of conditions and the following disclaimer in the documentation
- *   and/or other materials provided with the distribution.
- * * Neither the name of Poly2Tri nor the names of its contributors may be
- *   used to endorse or promote products derived from this software without specific
- *   prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
- * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
- * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
- * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
- * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
- * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
- * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
- * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
- * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
- */
-
-using System;
-using System.Collections;
-using System.Collections.Generic;
-
-namespace Poly2Tri.Triangulation.Util
-{
-    public struct FixedBitArray3 : IEnumerable<bool>
-    {
-        public bool _0, _1, _2;
-
-        public bool this[int index]
-        {
-            get
-            {
-                switch (index)
-                {
-                    case 0:
-                        return _0;
-                    case 1:
-                        return _1;
-                    case 2:
-                        return _2;
-                    default:
-                        throw new IndexOutOfRangeException();
-                }
-            }
-            set
-            {
-                switch (index)
-                {
-                    case 0:
-                        _0 = value;
-                        break;
-                    case 1:
-                        _1 = value;
-                        break;
-                    case 2:
-                        _2 = value;
-                        break;
-                    default:
-                        throw new IndexOutOfRangeException();
-                }
-            }
-        }
-
-        #region IEnumerable<bool> Members
-
-        public IEnumerator<bool> GetEnumerator()
-        {
-            return Enumerate().GetEnumerator();
-        }
-
-        IEnumerator IEnumerable.GetEnumerator()
-        {
-            return GetEnumerator();
-        }
-
-        #endregion
-
-        public bool Contains(bool value)
-        {
-            for (int i = 0; i < 3; ++i) if (this[i] == value) return true;
-            return false;
-        }
-
-        public int IndexOf(bool value)
-        {
-            for (int i = 0; i < 3; ++i) if (this[i] == value) return i;
-            return -1;
-        }
-
-        public void Clear()
-        {
-            _0 = _1 = _2 = false;
-        }
-
-        public void Clear(bool value)
-        {
-            for (int i = 0; i < 3; ++i) if (this[i] == value) this[i] = false;
-        }
-
-        private IEnumerable<bool> Enumerate()
-        {
-            for (int i = 0; i < 3; ++i) yield return this[i];
-        }
-    }
-}

FarseerPhysicsEngine/Common/Decomposition/CDT/Util/PointGenerator.cs

@@ -1,38 +0,0 @@
-using System;
-using System.Collections.Generic;
-
-namespace Poly2Tri.Triangulation.Util
-{
-    public class PointGenerator
-    {
-        private static readonly Random RNG = new Random();
-
-        public static List<TriangulationPoint> UniformDistribution(int n, double scale)
-        {
-            List<TriangulationPoint> points = new List<TriangulationPoint>();
-            for (int i = 0; i < n; i++)
-            {
-                points.Add(new TriangulationPoint(scale*(0.5 - RNG.NextDouble()), scale*(0.5 - RNG.NextDouble())));
-            }
-            return points;
-        }
-
-        public static List<TriangulationPoint> UniformGrid(int n, double scale)
-        {
-            double x = 0;
-            double size = scale/n;
-            double halfScale = 0.5*scale;
-
-            List<TriangulationPoint> points = new List<TriangulationPoint>();
-            for (int i = 0; i < n + 1; i++)
-            {
-                x = halfScale - i*size;
-                for (int j = 0; j < n + 1; j++)
-                {
-                    points.Add(new TriangulationPoint(x, halfScale - j*size));
-                }
-            }
-            return points;
-        }
-    }
-}

FarseerPhysicsEngine/Common/Decomposition/CDT/Util/PolygonGenerator.cs

@@ -1,98 +0,0 @@
-/* Poly2Tri
- * Copyright (c) 2009-2010, Poly2Tri Contributors
- * http://code.google.com/p/poly2tri/
- *
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without modification,
- * are permitted provided that the following conditions are met:
- *
- * * Redistributions of source code must retain the above copyright notice,
- *   this list of conditions and the following disclaimer.
- * * Redistributions in binary form must reproduce the above copyright notice,
- *   this list of conditions and the following disclaimer in the documentation
- *   and/or other materials provided with the distribution.
- * * Neither the name of Poly2Tri nor the names of its contributors may be
- *   used to endorse or promote products derived from this software without specific
- *   prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
- * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
- * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
- * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
- * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
- * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
- * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
- * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
- * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
- */
-
-using System;
-using Poly2Tri.Triangulation.Polygon;
-
-namespace Poly2Tri.Triangulation.Util
-{
-    public class PolygonGenerator
-    {
-        private static readonly Random RNG = new Random();
-
-        private static double PI_2 = 2.0*Math.PI;
-
-        public static Polygon.Polygon RandomCircleSweep(double scale, int vertexCount)
-        {
-            PolygonPoint point;
-            PolygonPoint[] points;
-            double radius = scale/4;
-
-            points = new PolygonPoint[vertexCount];
-            for (int i = 0; i < vertexCount; i++)
-            {
-                do
-                {
-                    if (i%250 == 0)
-                    {
-                        radius += scale/2*(0.5 - RNG.NextDouble());
-                    }
-                    else if (i%50 == 0)
-                    {
-                        radius += scale/5*(0.5 - RNG.NextDouble());
-                    }
-                    else
-                    {
-                        radius += 25*scale/vertexCount*(0.5 - RNG.NextDouble());
-                    }
-                    radius = radius > scale/2 ? scale/2 : radius;
-                    radius = radius < scale/10 ? scale/10 : radius;
-                } while (radius < scale/10 || radius > scale/2);
-                point = new PolygonPoint(radius*Math.Cos((PI_2*i)/vertexCount),
-                                         radius*Math.Sin((PI_2*i)/vertexCount));
-                points[i] = point;
-            }
-            return new Polygon.Polygon(points);
-        }
-
-        public static Polygon.Polygon RandomCircleSweep2(double scale, int vertexCount)
-        {
-            PolygonPoint point;
-            PolygonPoint[] points;
-            double radius = scale/4;
-
-            points = new PolygonPoint[vertexCount];
-            for (int i = 0; i < vertexCount; i++)
-            {
-                do
-                {
-                    radius += scale/5*(0.5 - RNG.NextDouble());
-                    radius = radius > scale/2 ? scale/2 : radius;
-                    radius = radius < scale/10 ? scale/10 : radius;
-                } while (radius < scale/10 || radius > scale/2);
-                point = new PolygonPoint(radius*Math.Cos((PI_2*i)/vertexCount),
-                                         radius*Math.Sin((PI_2*i)/vertexCount));
-                points[i] = point;
-            }
-            return new Polygon.Polygon(points);
-        }
-    }
-}

FarseerPhysicsEngine/Common/Decomposition/CDTDecomposer.cs

@@ -1,110 +0,0 @@
-/* Poly2Tri
- * Copyright (c) 2009-2010, Poly2Tri Contributors
- * http://code.google.com/p/poly2tri/
- *
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without modification,
- * are permitted provided that the following conditions are met:
- *
- * * Redistributions of source code must retain the above copyright notice,
- *   this list of conditions and the following disclaimer.
- * * Redistributions in binary form must reproduce the above copyright notice,
- *   this list of conditions and the following disclaimer in the documentation
- *   and/or other materials provided with the distribution.
- * * Neither the name of Poly2Tri nor the names of its contributors may be
- *   used to endorse or promote products derived from this software without specific
- *   prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
- * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
- * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
- * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
- * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
- * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
- * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
- * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
- * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
- */
-
-using System.Collections.Generic;
-using Microsoft.Xna.Framework;
-using Poly2Tri.Triangulation;
-using Poly2Tri.Triangulation.Delaunay;
-using Poly2Tri.Triangulation.Delaunay.Sweep;
-using Poly2Tri.Triangulation.Polygon;
-
-using System.Linq;
-
-namespace FarseerPhysics.Common.Decomposition
-{
-    public static class CDTDecomposer
-    {
-        public static List<Vertices> ConvexPartition(Vertices vertices)
-        {
-            Polygon poly = new Polygon();
-
-            foreach (Vector2 vertex in vertices)
-            {
-                poly.Points.Add(new TriangulationPoint(vertex.X, vertex.Y));
-            }
-
-            DTSweepContext tcx = new DTSweepContext();
-            tcx.PrepareTriangulation(poly);
-            DTSweep.Triangulate(tcx);
-
-            List<Vertices> results = new List<Vertices>();
-
-            foreach (DelaunayTriangle triangle in poly.Triangles)
-            {
-                Vertices v = new Vertices();
-                foreach (TriangulationPoint p in triangle.Points)
-                {
-                    v.Add(new Vector2((float)p.X, (float)p.Y));
-                }
-                results.Add(v);
-            }
-
-            return results;
-        }
-
-        public static List<Vertices> ConvexPartition(DetectedVertices vertices)
-        {
-            Polygon poly = new Polygon();
-            foreach (var vertex in vertices)
-                poly.Points.Add(new TriangulationPoint(vertex.X, vertex.Y));
-
-            if (vertices.Holes != null)
-            {
-                foreach (var holeVertices in vertices.Holes)
-                {
-                    Polygon hole = new Polygon();
-                    foreach (var vertex in holeVertices)
-                        hole.Points.Add(new TriangulationPoint(vertex.X, vertex.Y));
-
-                    poly.AddHole(hole);
-                }
-            }
-
-            DTSweepContext tcx = new DTSweepContext();
-            tcx.PrepareTriangulation(poly);
-            DTSweep.Triangulate(tcx);
-
-            List<Vertices> results = new List<Vertices>();
-
-            foreach (DelaunayTriangle triangle in poly.Triangles)
-            {
-                Vertices v = new Vertices();
-                foreach (TriangulationPoint p in triangle.Points)
-                {
-                    v.Add(new Vector2((float)p.X, (float)p.Y));
-                }
-                results.Add(v);
-            }
-
-            return results;
-        }
-    }
-}

FarseerPhysicsEngine/Common/Decomposition/EarclipDecomposer.cs

@@ -1,691 +0,0 @@
-/*
- * C# Version Ported by Matt Bettcher and Ian Qvist 2009-2010
- * 
- * Original C++ Version Copyright (c) 2007 Eric Jordan
- *
- * This software is provided 'as-is', without any express or implied
- * warranty.  In no event will the authors be held liable for any damages
- * arising from the use of this software.
- * Permission is granted to anyone to use this software for any purpose,
- * including commercial applications, and to alter it and redistribute it
- * freely, subject to the following restrictions:
- * 1. The origin of this software must not be misrepresented; you must not
- * claim that you wrote the original software. If you use this software
- * in a product, an acknowledgment in the product documentation would be
- * appreciated but is not required.
- * 2. Altered source versions must be plainly marked as such, and must not be
- * misrepresented as being the original software.
- * 3. This notice may not be removed or altered from any source distribution.
- */
-
-using System;
-using System.Collections.Generic;
-using FarseerPhysics.Common.PolygonManipulation;
-using Microsoft.Xna.Framework;
-
-namespace FarseerPhysics.Common.Decomposition
-{
-    /// <summary>
-    /// Ported from jBox2D. Original author: ewjordan 
-    /// Triangulates a polygon using simple ear-clipping algorithm.
-    /// 
-    /// Only works on simple polygons.
-    /// 
-    /// Triangles may be degenerate, especially if you have identical points
-    /// in the input to the algorithm.  Check this before you use them.
-    /// </summary>
-    public static class EarclipDecomposer
-    {
-        //box2D rev 32 - for details, see http://www.box2d.org/forum/viewtopic.php?f=4&t=83&start=50
-
-        private const float Tol = .001f;
-
-        /// <summary>
-        /// Decomposes a non-convex polygon into a number of convex polygons, up
-        /// to maxPolys (remaining pieces are thrown out).
-        ///
-        /// Each resulting polygon will have no more than Settings.MaxPolygonVertices
-        /// vertices.
-        /// 
-        /// Warning: Only works on simple polygons
-        /// </summary>
-        /// <param name="vertices">The vertices.</param>
-        /// <returns></returns>
-        public static List<Vertices> ConvexPartition(Vertices vertices)
-        {
-            return ConvexPartition(vertices, int.MaxValue, 0);
-        }
-
-        /// <summary>
-        /// Decomposes a non-convex polygon into a number of convex polygons, up
-        /// to maxPolys (remaining pieces are thrown out).
-        /// Each resulting polygon will have no more than Settings.MaxPolygonVertices
-        /// vertices.
-        /// Warning: Only works on simple polygons
-        /// </summary>
-        /// <param name="vertices">The vertices.</param>
-        /// <param name="maxPolys">The maximum number of polygons.</param>
-        /// <param name="tolerance">The tolerance.</param>
-        /// <returns></returns>
-        public static List<Vertices> ConvexPartition(Vertices vertices, int maxPolys, float tolerance)
-        {
-            if (vertices.Count < 3)
-                return new List<Vertices> { vertices };
-            /*
-            if (vertices.IsConvex() && vertices.Count <= Settings.MaxPolygonVertices)
-            {
-                if (vertices.IsCounterClockWise())
-                {
-                    Vertices tempP = new Vertices(vertices);
-                    tempP.Reverse();
-                    tempP = SimplifyTools.CollinearSimplify(tempP);
-                    tempP.ForceCounterClockWise();
-                    return new List<Vertices> { tempP };
-                }
-                vertices = SimplifyTools.CollinearSimplify(vertices);
-                vertices.ForceCounterClockWise();
-                return new List<Vertices> { vertices };
-            }
-            */
-            List<Triangle> triangulated;
-
-            if (vertices.IsCounterClockWise())
-            {
-                Vertices tempP = new Vertices(vertices);
-                tempP.Reverse();
-                triangulated = TriangulatePolygon(tempP);
-            }
-            else
-            {
-                triangulated = TriangulatePolygon(vertices);
-            }
-            if (triangulated.Count < 1)
-            {
-                //Still no luck?  Oh well...
-                throw new Exception("Can't triangulate your polygon.");
-            }
-
-            List<Vertices> polygonizedTriangles = PolygonizeTriangles(triangulated, maxPolys, tolerance);
-
-            //The polygonized triangles are not guaranteed to be without collinear points. We remove
-            //them to be sure.
-            for (int i = 0; i < polygonizedTriangles.Count; i++)
-            {
-                polygonizedTriangles[i] = SimplifyTools.CollinearSimplify(polygonizedTriangles[i], 0);
-            }
-
-            //Remove empty vertice collections
-            for (int i = polygonizedTriangles.Count - 1; i >= 0; i--)
-            {
-                if (polygonizedTriangles[i].Count == 0)
-                    polygonizedTriangles.RemoveAt(i);
-            }
-
-            return polygonizedTriangles;
-        }
-
-        /// <summary>
-        /// Turns a list of triangles into a list of convex polygons. Very simple
-        /// method - start with a seed triangle, keep adding triangles to it until
-        /// you can't add any more without making the polygon non-convex.
-        ///
-        /// Returns an integer telling how many polygons were created.  Will fill
-        /// polys array up to polysLength entries, which may be smaller or larger
-        /// than the return value.
-        /// 
-        /// Takes O(N///P) where P is the number of resultant polygons, N is triangle
-        /// count.
-        /// 
-        /// The final polygon list will not necessarily be minimal, though in
-        /// practice it works fairly well.
-        /// </summary>
-        /// <param name="triangulated">The triangulated.</param>
-        ///<param name="maxPolys">The maximun number of polygons</param>
-        ///<param name="tolerance">The tolerance</param>
-        ///<returns></returns>
-        public static List<Vertices> PolygonizeTriangles(List<Triangle> triangulated, int maxPolys, float tolerance)
-        {
-            List<Vertices> polys = new List<Vertices>(50);
-
-            int polyIndex = 0;
-
-            if (triangulated.Count <= 0)
-            {
-                //return empty polygon list
-                return polys;
-            }
-
-            bool[] covered = new bool[triangulated.Count];
-            for (int i = 0; i < triangulated.Count; ++i)
-            {
-                covered[i] = false;
-
-                //Check here for degenerate triangles
-                if (((triangulated[i].X[0] == triangulated[i].X[1]) && (triangulated[i].Y[0] == triangulated[i].Y[1]))
-                    ||
-                    ((triangulated[i].X[1] == triangulated[i].X[2]) && (triangulated[i].Y[1] == triangulated[i].Y[2]))
-                    ||
-                    ((triangulated[i].X[0] == triangulated[i].X[2]) && (triangulated[i].Y[0] == triangulated[i].Y[2])))
-                {
-                    covered[i] = true;
-                }
-            }
-
-            bool notDone = true;
-            while (notDone)
-            {
-                int currTri = -1;
-                for (int i = 0; i < triangulated.Count; ++i)
-                {
-                    if (covered[i])
-                        continue;
-                    currTri = i;
-                    break;
-                }
-                if (currTri == -1)
-                {
-                    notDone = false;
-                }
-                else
-                {
-                    Vertices poly = new Vertices(3);
-
-                    for (int i = 0; i < 3; i++)
-                    {
-                        poly.Add(new Vector2(triangulated[currTri].X[i], triangulated[currTri].Y[i]));
-                    }
-
-                    covered[currTri] = true;
-                    int index = 0;
-                    for (int i = 0; i < 2 * triangulated.Count; ++i, ++index)
-                    {
-                        while (index >= triangulated.Count) index -= triangulated.Count;
-                        if (covered[index])
-                        {
-                            continue;
-                        }
-                        Vertices newP = AddTriangle(triangulated[index], poly);
-                        if (newP == null)
-                            continue; // is this right
-
-                        if (newP.Count > Settings.MaxPolygonVertices)
-                            continue;
-
-                        if (newP.IsConvex())
-                        {
-                            //Or should it be IsUsable?  Maybe re-write IsConvex to apply the angle threshold from Box2d
-                            poly = new Vertices(newP);
-                            covered[index] = true;
-                        }
-                    }
-
-                    //We have a maximum of polygons that we need to keep under.
-                    if (polyIndex < maxPolys)
-                    {
-                        //SimplifyTools.MergeParallelEdges(poly, tolerance);
-
-                        //If identical points are present, a triangle gets
-                        //borked by the MergeParallelEdges function, hence
-                        //the vertex number check
-                        if (poly.Count >= 3)
-                            polys.Add(new Vertices(poly));
-                        //else
-                        //    printf("Skipping corrupt poly\n");
-                    }
-                    if (poly.Count >= 3)
-                        polyIndex++; //Must be outside (polyIndex < polysLength) test
-                }
-            }
-
-            return polys;
-        }
-
-        /// <summary>
-        /// Triangulates a polygon using simple ear-clipping algorithm. Returns
-        /// size of Triangle array unless the polygon can't be triangulated.
-        /// This should only happen if the polygon self-intersects,
-        /// though it will not _always_ return null for a bad polygon - it is the
-        /// caller's responsibility to check for self-intersection, and if it
-        /// doesn't, it should at least check that the return value is non-null
-        /// before using. You're warned!
-        ///
-        /// Triangles may be degenerate, especially if you have identical points
-        /// in the input to the algorithm.  Check this before you use them.
-        ///
-        /// This is totally unoptimized, so for large polygons it should not be part
-        /// of the simulation loop.
-        ///
-        /// Warning: Only works on simple polygons.
-        /// </summary>
-        /// <returns></returns>
-        public static List<Triangle> TriangulatePolygon(Vertices vertices)
-        {
-            List<Triangle> results = new List<Triangle>();
-            if (vertices.Count < 3)
-                return new List<Triangle>();
-
-            //Recurse and split on pinch points
-            Vertices pA, pB;
-            Vertices pin = new Vertices(vertices);
-            if (ResolvePinchPoint(pin, out pA, out pB))
-            {
-                List<Triangle> mergeA = TriangulatePolygon(pA);
-                List<Triangle> mergeB = TriangulatePolygon(pB);
-
-                if (mergeA.Count == -1 || mergeB.Count == -1)
-                    throw new Exception("Can't triangulate your polygon.");
-
-                for (int i = 0; i < mergeA.Count; ++i)
-                {
-                    results.Add(new Triangle(mergeA[i]));
-                }
-                for (int i = 0; i < mergeB.Count; ++i)
-                {
-                    results.Add(new Triangle(mergeB[i]));
-                }
-
-                return results;
-            }
-
-            Triangle[] buffer = new Triangle[vertices.Count - 2];
-            int bufferSize = 0;
-            float[] xrem = new float[vertices.Count];
-            float[] yrem = new float[vertices.Count];
-            for (int i = 0; i < vertices.Count; ++i)
-            {
-                xrem[i] = vertices[i].X;
-                yrem[i] = vertices[i].Y;
-            }
-
-            int vNum = vertices.Count;
-
-            while (vNum > 3)
-            {
-                // Find an ear
-                int earIndex = -1;
-                float earMaxMinCross = -10.0f;
-                for (int i = 0; i < vNum; ++i)
-                {
-                    if (IsEar(i, xrem, yrem, vNum))
-                    {
-                        int lower = Remainder(i - 1, vNum);
-                        int upper = Remainder(i + 1, vNum);
-                        Vector2 d1 = new Vector2(xrem[upper] - xrem[i], yrem[upper] - yrem[i]);
-                        Vector2 d2 = new Vector2(xrem[i] - xrem[lower], yrem[i] - yrem[lower]);
-                        Vector2 d3 = new Vector2(xrem[lower] - xrem[upper], yrem[lower] - yrem[upper]);
-
-                        d1.Normalize();
-                        d2.Normalize();
-                        d3.Normalize();
-                        float cross12;
-                        MathUtils.Cross(ref d1, ref d2, out cross12);
-                        cross12 = Math.Abs(cross12);
-
-                        float cross23;
-                        MathUtils.Cross(ref d2, ref d3, out cross23);
-                        cross23 = Math.Abs(cross23);
-
-                        float cross31;
-                        MathUtils.Cross(ref d3, ref d1, out cross31);
-                        cross31 = Math.Abs(cross31);
-
-                        //Find the maximum minimum angle
-                        float minCross = Math.Min(cross12, Math.Min(cross23, cross31));
-                        if (minCross > earMaxMinCross)
-                        {
-                            earIndex = i;
-                            earMaxMinCross = minCross;
-                        }
-                    }
-                }
-
-                // If we still haven't found an ear, we're screwed.
-                // Note: sometimes this is happening because the
-                // remaining points are collinear.  Really these
-                // should just be thrown out without halting triangulation.
-                if (earIndex == -1)
-                {
-                    for (int i = 0; i < bufferSize; i++)
-                    {
-                        results.Add(new Triangle(buffer[i]));
-                    }
-
-                    return results;
-                }
-
-                // Clip off the ear:
-                // - remove the ear tip from the list
-
-                --vNum;
-                float[] newx = new float[vNum];
-                float[] newy = new float[vNum];
-                int currDest = 0;
-                for (int i = 0; i < vNum; ++i)
-                {
-                    if (currDest == earIndex) ++currDest;
-                    newx[i] = xrem[currDest];
-                    newy[i] = yrem[currDest];
-                    ++currDest;
-                }
-
-                // - add the clipped triangle to the triangle list
-                int under = (earIndex == 0) ? (vNum) : (earIndex - 1);
-                int over = (earIndex == vNum) ? 0 : (earIndex + 1);
-                Triangle toAdd = new Triangle(xrem[earIndex], yrem[earIndex], xrem[over], yrem[over], xrem[under],
-                                              yrem[under]);
-                buffer[bufferSize] = toAdd;
-                ++bufferSize;
-
-                // - replace the old list with the new one
-                xrem = newx;
-                yrem = newy;
-            }
-
-            Triangle tooAdd = new Triangle(xrem[1], yrem[1], xrem[2], yrem[2], xrem[0], yrem[0]);
-            buffer[bufferSize] = tooAdd;
-            ++bufferSize;
-
-            for (int i = 0; i < bufferSize; i++)
-            {
-                results.Add(new Triangle(buffer[i]));
-            }
-
-            return results;
-        }
-
-        /// <summary>
-        /// Finds and fixes "pinch points," points where two polygon
-        /// vertices are at the same point.
-        /// 
-        /// If a pinch point is found, pin is broken up into poutA and poutB
-        /// and true is returned; otherwise, returns false.
-        /// 
-        /// Mostly for internal use.
-        /// 
-        /// O(N^2) time, which sucks...
-        /// </summary>
-        /// <param name="pin">The pin.</param>
-        /// <param name="poutA">The pout A.</param>
-        /// <param name="poutB">The pout B.</param>
-        /// <returns></returns>
-        private static bool ResolvePinchPoint(Vertices pin, out Vertices poutA, out Vertices poutB)
-        {
-            poutA = new Vertices();
-            poutB = new Vertices();
-
-            if (pin.Count < 3)
-                return false;
-
-            bool hasPinchPoint = false;
-            int pinchIndexA = -1;
-            int pinchIndexB = -1;
-            for (int i = 0; i < pin.Count; ++i)
-            {
-                for (int j = i + 1; j < pin.Count; ++j)
-                {
-                    //Don't worry about pinch points where the points
-                    //are actually just dupe neighbors
-                    if (Math.Abs(pin[i].X - pin[j].X) < Tol && Math.Abs(pin[i].Y - pin[j].Y) < Tol && j != i + 1)
-                    {
-                        pinchIndexA = i;
-                        pinchIndexB = j;
-                        hasPinchPoint = true;
-                        break;
-                    }
-                }
-                if (hasPinchPoint) break;
-            }
-            if (hasPinchPoint)
-            {
-                int sizeA = pinchIndexB - pinchIndexA;
-                if (sizeA == pin.Count) return false; //has dupe points at wraparound, not a problem here
-                for (int i = 0; i < sizeA; ++i)
-                {
-                    int ind = Remainder(pinchIndexA + i, pin.Count); // is this right
-                    poutA.Add(pin[ind]);
-                }
-
-                int sizeB = pin.Count - sizeA;
-                for (int i = 0; i < sizeB; ++i)
-                {
-                    int ind = Remainder(pinchIndexB + i, pin.Count); // is this right    
-                    poutB.Add(pin[ind]);
-                }
-            }
-            return hasPinchPoint;
-        }
-
-        /// <summary>
-        /// Fix for obnoxious behavior for the % operator for negative numbers...
-        /// </summary>
-        /// <param name="x">The x.</param>
-        /// <param name="modulus">The modulus.</param>
-        /// <returns></returns>
-        private static int Remainder(int x, int modulus)
-        {
-            int rem = x % modulus;
-            while (rem < 0)
-            {
-                rem += modulus;
-            }
-            return rem;
-        }
-
-        private static Vertices AddTriangle(Triangle t, Vertices vertices)
-        {
-            // First, find vertices that connect
-            int firstP = -1;
-            int firstT = -1;
-            int secondP = -1;
-            int secondT = -1;
-            for (int i = 0; i < vertices.Count; i++)
-            {
-                if (t.X[0] == vertices[i].X && t.Y[0] == vertices[i].Y)
-                {
-                    if (firstP == -1)
-                    {
-                        firstP = i;
-                        firstT = 0;
-                    }
-                    else
-                    {
-                        secondP = i;
-                        secondT = 0;
-                    }
-                }
-                else if (t.X[1] == vertices[i].X && t.Y[1] == vertices[i].Y)
-                {
-                    if (firstP == -1)
-                    {
-                        firstP = i;
-                        firstT = 1;
-                    }
-                    else
-                    {
-                        secondP = i;
-                        secondT = 1;
-                    }
-                }
-                else if (t.X[2] == vertices[i].X && t.Y[2] == vertices[i].Y)
-                {
-                    if (firstP == -1)
-                    {
-                        firstP = i;
-                        firstT = 2;
-                    }
-                    else
-                    {
-                        secondP = i;
-                        secondT = 2;
-                    }
-                }
-            }
-            // Fix ordering if first should be last vertex of poly
-            if (firstP == 0 && secondP == vertices.Count - 1)
-            {
-                firstP = vertices.Count - 1;
-                secondP = 0;
-            }
-
-            // Didn't find it
-            if (secondP == -1)
-            {
-                return null;
-            }
-
-            // Find tip index on triangle
-            int tipT = 0;
-            if (tipT == firstT || tipT == secondT)
-                tipT = 1;
-            if (tipT == firstT || tipT == secondT)
-                tipT = 2;
-
-            Vertices result = new Vertices(vertices.Count + 1);
-            for (int i = 0; i < vertices.Count; i++)
-            {
-                result.Add(vertices[i]);
-
-                if (i == firstP)
-                    result.Add(new Vector2(t.X[tipT], t.Y[tipT]));
-            }
-
-            return result;
-        }
-
-        /// <summary>
-        /// Checks if vertex i is the tip of an ear in polygon defined by xv[] and
-        /// yv[].
-        ///
-        /// Assumes clockwise orientation of polygon...ick
-        /// </summary>
-        /// <param name="i">The i.</param>
-        /// <param name="xv">The xv.</param>
-        /// <param name="yv">The yv.</param>
-        /// <param name="xvLength">Length of the xv.</param>
-        /// <returns>
-        /// 	<c>true</c> if the specified i is ear; otherwise, <c>false</c>.
-        /// </returns>
-        private static bool IsEar(int i, float[] xv, float[] yv, int xvLength)
-        {
-            float dx0, dy0, dx1, dy1;
-            if (i >= xvLength || i < 0 || xvLength < 3)
-            {
-                return false;
-            }
-            int upper = i + 1;
-            int lower = i - 1;
-            if (i == 0)
-            {
-                dx0 = xv[0] - xv[xvLength - 1];
-                dy0 = yv[0] - yv[xvLength - 1];
-                dx1 = xv[1] - xv[0];
-                dy1 = yv[1] - yv[0];
-                lower = xvLength - 1;
-            }
-            else if (i == xvLength - 1)
-            {
-                dx0 = xv[i] - xv[i - 1];
-                dy0 = yv[i] - yv[i - 1];
-                dx1 = xv[0] - xv[i];
-                dy1 = yv[0] - yv[i];
-                upper = 0;
-            }
-            else
-            {
-                dx0 = xv[i] - xv[i - 1];
-                dy0 = yv[i] - yv[i - 1];
-                dx1 = xv[i + 1] - xv[i];
-                dy1 = yv[i + 1] - yv[i];
-            }
-            float cross = dx0 * dy1 - dx1 * dy0;
-            if (cross > 0)
-                return false;
-            Triangle myTri = new Triangle(xv[i], yv[i], xv[upper], yv[upper],
-                                          xv[lower], yv[lower]);
-            for (int j = 0; j < xvLength; ++j)
-            {
-                if (j == i || j == lower || j == upper)
-                    continue;
-                if (myTri.IsInside(xv[j], yv[j]))
-                    return false;
-            }
-            return true;
-        }
-    }
-
-    public class Triangle
-    {
-        public float[] X;
-        public float[] Y;
-
-        //Constructor automatically fixes orientation to ccw
-        public Triangle(float x1, float y1, float x2, float y2, float x3, float y3)
-        {
-            X = new float[3];
-            Y = new float[3];
-            float dx1 = x2 - x1;
-            float dx2 = x3 - x1;
-            float dy1 = y2 - y1;
-            float dy2 = y3 - y1;
-            float cross = dx1 * dy2 - dx2 * dy1;
-            bool ccw = (cross > 0);
-            if (ccw)
-            {
-                X[0] = x1;
-                X[1] = x2;
-                X[2] = x3;
-                Y[0] = y1;
-                Y[1] = y2;
-                Y[2] = y3;
-            }
-            else
-            {
-                X[0] = x1;
-                X[1] = x3;
-                X[2] = x2;
-                Y[0] = y1;
-                Y[1] = y3;
-                Y[2] = y2;
-            }
-        }
-
-        public Triangle(Triangle t)
-        {
-            X = new float[3];
-            Y = new float[3];
-
-            X[0] = t.X[0];
-            X[1] = t.X[1];
-            X[2] = t.X[2];
-            Y[0] = t.Y[0];
-            Y[1] = t.Y[1];
-            Y[2] = t.Y[2];
-        }
-
-        public bool IsInside(float x, float y)
-        {
-            if (x < X[0] && x < X[1] && x < X[2]) return false;
-            if (x > X[0] && x > X[1] && x > X[2]) return false;
-            if (y < Y[0] && y < Y[1] && y < Y[2]) return false;
-            if (y > Y[0] && y > Y[1] && y > Y[2]) return false;
-
-            float vx2 = x - X[0];
-            float vy2 = y - Y[0];
-            float vx1 = X[1] - X[0];
-            float vy1 = Y[1] - Y[0];
-            float vx0 = X[2] - X[0];
-            float vy0 = Y[2] - Y[0];
-
-            float dot00 = vx0 * vx0 + vy0 * vy0;
-            float dot01 = vx0 * vx1 + vy0 * vy1;
-            float dot02 = vx0 * vx2 + vy0 * vy2;
-            float dot11 = vx1 * vx1 + vy1 * vy1;
-            float dot12 = vx1 * vx2 + vy1 * vy2;
-            float invDenom = 1.0f / (dot00 * dot11 - dot01 * dot01);
-            float u = (dot11 * dot02 - dot01 * dot12) * invDenom;
-            float v = (dot00 * dot12 - dot01 * dot02) * invDenom;
-
-            return ((u > 0) && (v > 0) && (u + v < 1));
-        }
-    }
-}

FarseerPhysicsEngine/Common/Decomposition/FlipcodeDecomposer.cs

@@ -1,160 +0,0 @@
-using System.Collections.Generic;
-using Microsoft.Xna.Framework;
-
-namespace FarseerPhysics.Common.Decomposition
-{
-    // Original code can be found here: http://www.flipcode.com/archives/Efficient_Polygon_Triangulation.shtml
-
-    /// <summary>
-    /// Triangulates a polygon into triangles.
-    /// Doesn't handle holes.
-    /// </summary>
-    public static class FlipcodeDecomposer
-    {
-        private static Vector2 _tmpA;
-        private static Vector2 _tmpB;
-        private static Vector2 _tmpC;
-
-        /// <summary>
-        /// Check if the point P is inside the triangle defined by
-        /// the points A, B, C
-        /// </summary>
-        /// <param name="a">The A point.</param>
-        /// <param name="b">The B point.</param>
-        /// <param name="c">The C point.</param>
-        /// <param name="p">The point to be tested.</param>
-        /// <returns>True if the point is inside the triangle</returns>
-        private static bool InsideTriangle(ref Vector2 a, ref Vector2 b, ref Vector2 c, ref Vector2 p)
-        {
-            //A cross bp
-            float abp = (c.X - b.X) * (p.Y - b.Y) - (c.Y - b.Y) * (p.X - b.X);
-
-            //A cross ap
-            float aap = (b.X - a.X) * (p.Y - a.Y) - (b.Y - a.Y) * (p.X - a.X);
-
-            //b cross cp
-            float bcp = (a.X - c.X) * (p.Y - c.Y) - (a.Y - c.Y) * (p.X - c.X);
-
-            return ((abp >= 0.0f) && (bcp >= 0.0f) && (aap >= 0.0f));
-        }
-
-        /// <summary>
-        /// Cut a the contour and add a triangle into V to describe the 
-        /// location of the cut
-        /// </summary>
-        /// <param name="contour">The list of points defining the polygon</param>
-        /// <param name="u">The index of the first point</param>
-        /// <param name="v">The index of the second point</param>
-        /// <param name="w">The index of the third point</param>
-        /// <param name="n">The number of elements in the array.</param>
-        /// <param name="V">The array to populate with indicies of triangles.</param>
-        /// <returns>True if a triangle was found</returns>
-        private static bool Snip(Vertices contour, int u, int v, int w, int n,
-                                 int[] V)
-        {
-            if (Settings.Epsilon > MathUtils.Area(ref _tmpA, ref _tmpB, ref _tmpC))
-            {
-                return false;
-            }
-
-            for (int p = 0; p < n; p++)
-            {
-                if ((p == u) || (p == v) || (p == w))
-                {
-                    continue;
-                }
-
-                Vector2 point = contour[V[p]];
-
-                if (InsideTriangle(ref _tmpA, ref _tmpB, ref _tmpC, ref point))
-                {
-                    return false;
-                }
-            }
-
-            return true;
-        }
-
-        /// <summary>
-        /// Decompose the polygon into triangles
-        /// </summary>
-        /// <param name="contour">The list of points describing the polygon</param>
-        /// <returns></returns>
-        public static List<Vertices> ConvexPartition(Vertices contour)
-        {
-            int n = contour.Count;
-            if (n < 3)
-                return new List<Vertices>();
-
-            int[] V = new int[n];
-
-            // We want a counter-clockwise polygon in V
-            if (contour.IsCounterClockWise())
-            {
-                for (int v = 0; v < n; v++)
-                    V[v] = v;
-            }
-            else
-            {
-                for (int v = 0; v < n; v++)
-                    V[v] = (n - 1) - v;
-            }
-
-            int nv = n;
-
-            // Remove nv-2 Vertices, creating 1 triangle every time
-            int count = 2 * nv; /* error detection */
-
-            List<Vertices> result = new List<Vertices>();
-
-            for (int v = nv - 1; nv > 2; )
-            {
-                // If we loop, it is probably a non-simple polygon 
-                if (0 >= (count--))
-                {
-                    // Triangulate: ERROR - probable bad polygon!
-                    return new List<Vertices>();
-                }
-
-                // Three consecutive vertices in current polygon, <u,v,w>
-                int u = v;
-                if (nv <= u)
-                    u = 0; // Previous 
-                v = u + 1;
-                if (nv <= v)
-                    v = 0; // New v   
-                int w = v + 1;
-                if (nv <= w)
-                    w = 0; // Next 
-
-                _tmpA = contour[V[u]];
-                _tmpB = contour[V[v]];
-                _tmpC = contour[V[w]];
-
-                if (Snip(contour, u, v, w, nv, V))
-                {
-                    int s, t;
-
-                    // Output Triangle
-                    Vertices triangle = new Vertices(3);
-                    triangle.Add(_tmpA);
-                    triangle.Add(_tmpB);
-                    triangle.Add(_tmpC);
-                    result.Add(triangle);
-
-                    // Remove v from remaining polygon 
-                    for (s = v, t = v + 1; t < nv; s++, t++)
-                    {
-                        V[s] = V[t];
-                    }
-                    nv--;
-
-                    // Reset error detection counter
-                    count = 2 * nv;
-                }
-            }
-
-            return result;
-        }
-    }
-}

FarseerPhysicsEngine/Common/Decomposition/SeidelDecomposer.cs

@@ -1,1057 +0,0 @@
-using System;
-using System.Collections.Generic;
-using System.Diagnostics;
-using Microsoft.Xna.Framework;
-
-namespace FarseerPhysics.Common.Decomposition
-{
-    //From the Poly2Tri project by Mason Green: http://code.google.com/p/poly2tri/source/browse?repo=archive#hg/scala/src/org/poly2tri/seidel
-
-    /// <summary>
-    /// Convex decomposition algorithm based on Raimund Seidel's paper "A simple and fast incremental randomized
-    /// algorithm for computing trapezoidal decompositions and for triangulating polygons"
-    /// See also: "Computational Geometry", 3rd edition, by Mark de Berg et al, Chapter 6.2
-    ///           "Computational Geometry in C", 2nd edition, by Joseph O'Rourke
-    /// </summary>
-    public static class SeidelDecomposer
-    {
-        /// <summary>
-        /// Decompose the polygon into several smaller non-concave polygon.
-        /// </summary>
-        /// <param name="vertices">The polygon to decompose.</param>
-        /// <param name="sheer">The sheer to use. If you get bad results, try using a higher value. The default value is 0.001</param>
-        /// <returns>A list of triangles</returns>
-        public static List<Vertices> ConvexPartition(Vertices vertices, float sheer)
-        {
-            List<Point> compatList = new List<Point>(vertices.Count);
-
-            foreach (Vector2 vertex in vertices)
-            {
-                compatList.Add(new Point(vertex.X, vertex.Y));
-            }
-
-            Triangulator t = new Triangulator(compatList, sheer);
-
-            List<Vertices> list = new List<Vertices>();
-
-            foreach (List<Point> triangle in t.Triangles)
-            {
-                Vertices verts = new Vertices(triangle.Count);
-
-                foreach (Point point in triangle)
-                {
-                    verts.Add(new Vector2(point.X, point.Y));
-                }
-
-                list.Add(verts);
-            }
-
-            return list;
-        }
-
-        /// <summary>
-        /// Decompose the polygon into several smaller non-concave polygon.
-        /// </summary>
-        /// <param name="vertices">The polygon to decompose.</param>
-        /// <param name="sheer">The sheer to use. If you get bad results, try using a higher value. The default value is 0.001</param>
-        /// <returns>A list of trapezoids</returns>
-        public static List<Vertices> ConvexPartitionTrapezoid(Vertices vertices, float sheer)
-        {
-            List<Point> compatList = new List<Point>(vertices.Count);
-
-            foreach (Vector2 vertex in vertices)
-            {
-                compatList.Add(new Point(vertex.X, vertex.Y));
-            }
-
-            Triangulator t = new Triangulator(compatList, sheer);
-
-            List<Vertices> list = new List<Vertices>();
-
-            foreach (Trapezoid trapezoid in t.Trapezoids)
-            {
-                Vertices verts = new Vertices();
-
-                List<Point> points = trapezoid.Vertices();
-                foreach (Point point in points)
-                {
-                    verts.Add(new Vector2(point.X, point.Y));
-                }
-
-                list.Add(verts);
-            }
-
-            return list;
-        }
-    }
-
-    internal class MonotoneMountain
-    {
-        private const float PiSlop = 3.1f;
-        public List<List<Point>> Triangles;
-        private HashSet<Point> _convexPoints;
-        private Point _head;
-
-        // Monotone mountain points
-        private List<Point> _monoPoly;
-
-        // Triangles that constitute the mountain
-
-        // Used to track which side of the line we are on
-        private bool _positive;
-        private int _size;
-        private Point _tail;
-
-        // Almost Pi!
-
-        public MonotoneMountain()
-        {
-            _size = 0;
-            _tail = null;
-            _head = null;
-            _positive = false;
-            _convexPoints = new HashSet<Point>();
-            _monoPoly = new List<Point>();
-            Triangles = new List<List<Point>>();
-        }
-
-        // Append a point to the list
-        public void Add(Point point)
-        {
-            if (_size == 0)
-            {
-                _head = point;
-                _size = 1;
-            }
-            else if (_size == 1)
-            {
-                // Keep repeat points out of the list
-                _tail = point;
-                _tail.Prev = _head;
-                _head.Next = _tail;
-                _size = 2;
-            }
-            else
-            {
-                // Keep repeat points out of the list
-                _tail.Next = point;
-                point.Prev = _tail;
-                _tail = point;
-                _size += 1;
-            }
-        }
-
-        // Remove a point from the list
-        public void Remove(Point point)
-        {
-            Point next = point.Next;
-            Point prev = point.Prev;
-            point.Prev.Next = next;
-            point.Next.Prev = prev;
-            _size -= 1;
-        }
-
-        // Partition a x-monotone mountain into triangles O(n)
-        // See "Computational Geometry in C", 2nd edition, by Joseph O'Rourke, page 52
-        public void Process()
-        {
-            // Establish the proper sign
-            _positive = AngleSign();
-            // create monotone polygon - for dubug purposes
-            GenMonoPoly();
-
-            // Initialize internal angles at each nonbase vertex
-            // Link strictly convex vertices into a list, ignore reflex vertices
-            Point p = _head.Next;
-            while (p.Neq(_tail))
-            {
-                float a = Angle(p);
-                // If the point is almost colinear with it's neighbor, remove it!
-                if (a >= PiSlop || a <= -PiSlop || a == 0.0)
-                    Remove(p);
-                else if (IsConvex(p))
-                    _convexPoints.Add(p);
-                p = p.Next;
-            }
-
-            Triangulate();
-        }
-
-        private void Triangulate()
-        {
-            while (_convexPoints.Count != 0)
-            {
-                IEnumerator<Point> e = _convexPoints.GetEnumerator();
-                e.MoveNext();
-                Point ear = e.Current;
-
-                _convexPoints.Remove(ear);
-                Point a = ear.Prev;
-                Point b = ear;
-                Point c = ear.Next;
-                List<Point> triangle = new List<Point>(3);
-                triangle.Add(a);
-                triangle.Add(b);
-                triangle.Add(c);
-
-                Triangles.Add(triangle);
-
-                // Remove ear, update angles and convex list
-                Remove(ear);
-                if (Valid(a))
-                    _convexPoints.Add(a);
-                if (Valid(c))
-                    _convexPoints.Add(c);
-            }
-
-            Debug.Assert(_size <= 3, "Triangulation bug, please report");
-        }
-
-        private bool Valid(Point p)
-        {
-            return p.Neq(_head) && p.Neq(_tail) && IsConvex(p);
-        }
-
-        // Create the monotone polygon
-        private void GenMonoPoly()
-        {
-            Point p = _head;
-            while (p != null)
-            {
-                _monoPoly.Add(p);
-                p = p.Next;
-            }
-        }
-
-        private float Angle(Point p)
-        {
-            Point a = (p.Next - p);
-            Point b = (p.Prev - p);
-            return (float)Math.Atan2(a.Cross(b), a.Dot(b));
-        }
-
-        private bool AngleSign()
-        {
-            Point a = (_head.Next - _head);
-            Point b = (_tail - _head);
-            return Math.Atan2(a.Cross(b), a.Dot(b)) >= 0;
-        }
-
-        // Determines if the inslide angle is convex or reflex
-        private bool IsConvex(Point p)
-        {
-            if (_positive != (Angle(p) >= 0))
-                return false;
-            return true;
-        }
-    }
-
-    // Node for a Directed Acyclic graph (DAG)
-    internal abstract class Node
-    {
-        protected Node LeftChild;
-        public List<Node> ParentList;
-        protected Node RightChild;
-
-        protected Node(Node left, Node right)
-        {
-            ParentList = new List<Node>();
-            LeftChild = left;
-            RightChild = right;
-
-            if (left != null)
-                left.ParentList.Add(this);
-            if (right != null)
-                right.ParentList.Add(this);
-        }
-
-        public abstract Sink Locate(Edge s);
-
-        // Replace a node in the graph with this node
-        // Make sure parent pointers are updated
-        public void Replace(Node node)
-        {
-            foreach (Node parent in node.ParentList)
-            {
-                // Select the correct node to replace (left or right child)
-                if (parent.LeftChild == node)
-                    parent.LeftChild = this;
-                else
-                    parent.RightChild = this;
-            }
-            ParentList.AddRange(node.ParentList);
-        }
-    }
-
-    // Directed Acyclic graph (DAG)
-    // See "Computational Geometry", 3rd edition, by Mark de Berg et al, Chapter 6.2
-    internal class QueryGraph
-    {
-        private Node _head;
-
-        public QueryGraph(Node head)
-        {
-            _head = head;
-        }
-
-        private Trapezoid Locate(Edge edge)
-        {
-            return _head.Locate(edge).Trapezoid;
-        }
-
-        public List<Trapezoid> FollowEdge(Edge edge)
-        {
-            List<Trapezoid> trapezoids = new List<Trapezoid>();
-            trapezoids.Add(Locate(edge));
-            int j = 0;
-
-            while (edge.Q.X > trapezoids[j].RightPoint.X)
-            {
-                if (edge.IsAbove(trapezoids[j].RightPoint))
-                {
-                    trapezoids.Add(trapezoids[j].UpperRight);
-                }
-                else
-                {
-                    trapezoids.Add(trapezoids[j].LowerRight);
-                }
-                j += 1;
-            }
-            return trapezoids;
-        }
-
-        private void Replace(Sink sink, Node node)
-        {
-            if (sink.ParentList.Count == 0)
-                _head = node;
-            else
-                node.Replace(sink);
-        }
-
-        public void Case1(Sink sink, Edge edge, Trapezoid[] tList)
-        {
-            YNode yNode = new YNode(edge, Sink.Isink(tList[1]), Sink.Isink(tList[2]));
-            XNode qNode = new XNode(edge.Q, yNode, Sink.Isink(tList[3]));
-            XNode pNode = new XNode(edge.P, Sink.Isink(tList[0]), qNode);
-            Replace(sink, pNode);
-        }
-
-        public void Case2(Sink sink, Edge edge, Trapezoid[] tList)
-        {
-            YNode yNode = new YNode(edge, Sink.Isink(tList[1]), Sink.Isink(tList[2]));
-            XNode pNode = new XNode(edge.P, Sink.Isink(tList[0]), yNode);
-            Replace(sink, pNode);
-        }
-
-        public void Case3(Sink sink, Edge edge, Trapezoid[] tList)
-        {
-            YNode yNode = new YNode(edge, Sink.Isink(tList[0]), Sink.Isink(tList[1]));
-            Replace(sink, yNode);
-        }
-
-        public void Case4(Sink sink, Edge edge, Trapezoid[] tList)
-        {
-            YNode yNode = new YNode(edge, Sink.Isink(tList[0]), Sink.Isink(tList[1]));
-            XNode qNode = new XNode(edge.Q, yNode, Sink.Isink(tList[2]));
-            Replace(sink, qNode);
-        }
-    }
-
-    internal class Sink : Node
-    {
-        public Trapezoid Trapezoid;
-
-        private Sink(Trapezoid trapezoid)
-            : base(null, null)
-        {
-            Trapezoid = trapezoid;
-            trapezoid.Sink = this;
-        }
-
-        public static Sink Isink(Trapezoid trapezoid)
-        {
-            if (trapezoid.Sink == null)
-                return new Sink(trapezoid);
-            return trapezoid.Sink;
-        }
-
-        public override Sink Locate(Edge edge)
-        {
-            return this;
-        }
-    }
-
-    // See "Computational Geometry", 3rd edition, by Mark de Berg et al, Chapter 6.2
-    internal class TrapezoidalMap
-    {
-        // Trapezoid container
-        public HashSet<Trapezoid> Map;
-
-        // AABB margin
-
-        // Bottom segment that spans multiple trapezoids
-        private Edge _bCross;
-
-        // Top segment that spans multiple trapezoids
-        private Edge _cross;
-        private float _margin;
-
-        public TrapezoidalMap()
-        {
-            Map = new HashSet<Trapezoid>();
-            _margin = 50.0f;
-            _bCross = null;
-            _cross = null;
-        }
-
-        public void Clear()
-        {
-            _bCross = null;
-            _cross = null;
-        }
-
-        // Case 1: segment completely enclosed by trapezoid
-        //         break trapezoid into 4 smaller trapezoids
-        public Trapezoid[] Case1(Trapezoid t, Edge e)
-        {
-            Trapezoid[] trapezoids = new Trapezoid[4];
-            trapezoids[0] = new Trapezoid(t.LeftPoint, e.P, t.Top, t.Bottom);
-            trapezoids[1] = new Trapezoid(e.P, e.Q, t.Top, e);
-            trapezoids[2] = new Trapezoid(e.P, e.Q, e, t.Bottom);
-            trapezoids[3] = new Trapezoid(e.Q, t.RightPoint, t.Top, t.Bottom);
-
-            trapezoids[0].UpdateLeft(t.UpperLeft, t.LowerLeft);
-            trapezoids[1].UpdateLeftRight(trapezoids[0], null, trapezoids[3], null);
-            trapezoids[2].UpdateLeftRight(null, trapezoids[0], null, trapezoids[3]);
-            trapezoids[3].UpdateRight(t.UpperRight, t.LowerRight);
-
-            return trapezoids;
-        }
-
-        // Case 2: Trapezoid contains point p, q lies outside
-        //         break trapezoid into 3 smaller trapezoids
-        public Trapezoid[] Case2(Trapezoid t, Edge e)
-        {
-            Point rp;
-            if (e.Q.X == t.RightPoint.X)
-                rp = e.Q;
-            else
-                rp = t.RightPoint;
-
-            Trapezoid[] trapezoids = new Trapezoid[3];
-            trapezoids[0] = new Trapezoid(t.LeftPoint, e.P, t.Top, t.Bottom);
-            trapezoids[1] = new Trapezoid(e.P, rp, t.Top, e);
-            trapezoids[2] = new Trapezoid(e.P, rp, e, t.Bottom);
-
-            trapezoids[0].UpdateLeft(t.UpperLeft, t.LowerLeft);
-            trapezoids[1].UpdateLeftRight(trapezoids[0], null, t.UpperRight, null);
-            trapezoids[2].UpdateLeftRight(null, trapezoids[0], null, t.LowerRight);
-
-            _bCross = t.Bottom;
-            _cross = t.Top;
-
-            e.Above = trapezoids[1];
-            e.Below = trapezoids[2];
-
-            return trapezoids;
-        }
-
-        // Case 3: Trapezoid is bisected
-        public Trapezoid[] Case3(Trapezoid t, Edge e)
-        {
-            Point lp;
-            if (e.P.X == t.LeftPoint.X)
-                lp = e.P;
-            else
-                lp = t.LeftPoint;
-
-            Point rp;
-            if (e.Q.X == t.RightPoint.X)
-                rp = e.Q;
-            else
-                rp = t.RightPoint;
-
-            Trapezoid[] trapezoids = new Trapezoid[2];
-
-            if (_cross == t.Top)
-            {
-                trapezoids[0] = t.UpperLeft;
-                trapezoids[0].UpdateRight(t.UpperRight, null);
-                trapezoids[0].RightPoint = rp;
-            }
-            else
-            {
-                trapezoids[0] = new Trapezoid(lp, rp, t.Top, e);
-                trapezoids[0].UpdateLeftRight(t.UpperLeft, e.Above, t.UpperRight, null);
-            }
-
-            if (_bCross == t.Bottom)
-            {
-                trapezoids[1] = t.LowerLeft;
-                trapezoids[1].UpdateRight(null, t.LowerRight);
-                trapezoids[1].RightPoint = rp;
-            }
-            else
-            {
-                trapezoids[1] = new Trapezoid(lp, rp, e, t.Bottom);
-                trapezoids[1].UpdateLeftRight(e.Below, t.LowerLeft, null, t.LowerRight);
-            }
-
-            _bCross = t.Bottom;
-            _cross = t.Top;
-
-            e.Above = trapezoids[0];
-            e.Below = trapezoids[1];
-
-            return trapezoids;
-        }
-
-        // Case 4: Trapezoid contains point q, p lies outside
-        //         break trapezoid into 3 smaller trapezoids
-        public Trapezoid[] Case4(Trapezoid t, Edge e)
-        {
-            Point lp;
-            if (e.P.X == t.LeftPoint.X)
-                lp = e.P;
-            else
-                lp = t.LeftPoint;
-
-            Trapezoid[] trapezoids = new Trapezoid[3];
-
-            if (_cross == t.Top)
-            {
-                trapezoids[0] = t.UpperLeft;
-                trapezoids[0].RightPoint = e.Q;
-            }
-            else
-            {
-                trapezoids[0] = new Trapezoid(lp, e.Q, t.Top, e);
-                trapezoids[0].UpdateLeft(t.UpperLeft, e.Above);
-            }
-
-            if (_bCross == t.Bottom)
-            {
-                trapezoids[1] = t.LowerLeft;
-                trapezoids[1].RightPoint = e.Q;
-            }
-            else
-            {
-                trapezoids[1] = new Trapezoid(lp, e.Q, e, t.Bottom);
-                trapezoids[1].UpdateLeft(e.Below, t.LowerLeft);
-            }
-
-            trapezoids[2] = new Trapezoid(e.Q, t.RightPoint, t.Top, t.Bottom);
-            trapezoids[2].UpdateLeftRight(trapezoids[0], trapezoids[1], t.UpperRight, t.LowerRight);
-
-            return trapezoids;
-        }
-
-        // Create an AABB around segments
-        public Trapezoid BoundingBox(List<Edge> edges)
-        {
-            Point max = edges[0].P + _margin;
-            Point min = edges[0].Q - _margin;
-
-            foreach (Edge e in edges)
-            {
-                if (e.P.X > max.X) max = new Point(e.P.X + _margin, max.Y);
-                if (e.P.Y > max.Y) max = new Point(max.X, e.P.Y + _margin);
-                if (e.Q.X > max.X) max = new Point(e.Q.X + _margin, max.Y);
-                if (e.Q.Y > max.Y) max = new Point(max.X, e.Q.Y + _margin);
-                if (e.P.X < min.X) min = new Point(e.P.X - _margin, min.Y);
-                if (e.P.Y < min.Y) min = new Point(min.X, e.P.Y - _margin);
-                if (e.Q.X < min.X) min = new Point(e.Q.X - _margin, min.Y);
-                if (e.Q.Y < min.Y) min = new Point(min.X, e.Q.Y - _margin);
-            }
-
-            Edge top = new Edge(new Point(min.X, max.Y), new Point(max.X, max.Y));
-            Edge bottom = new Edge(new Point(min.X, min.Y), new Point(max.X, min.Y));
-            Point left = bottom.P;
-            Point right = top.Q;
-
-            return new Trapezoid(left, right, top, bottom);
-        }
-    }
-
-    internal class Point
-    {
-        // Pointers to next and previous points in Monontone Mountain
-        public Point Next, Prev;
-        public float X, Y;
-
-        public Point(float x, float y)
-        {
-            X = x;
-            Y = y;
-            Next = null;
-            Prev = null;
-        }
-
-        public static Point operator -(Point p1, Point p2)
-        {
-            return new Point(p1.X - p2.X, p1.Y - p2.Y);
-        }
-
-        public static Point operator +(Point p1, Point p2)
-        {
-            return new Point(p1.X + p2.X, p1.Y + p2.Y);
-        }
-
-        public static Point operator -(Point p1, float f)
-        {
-            return new Point(p1.X - f, p1.Y - f);
-        }
-
-        public static Point operator +(Point p1, float f)
-        {
-            return new Point(p1.X + f, p1.Y + f);
-        }
-
-        public float Cross(Point p)
-        {
-            return X * p.Y - Y * p.X;
-        }
-
-        public float Dot(Point p)
-        {
-            return X * p.X + Y * p.Y;
-        }
-
-        public bool Neq(Point p)
-        {
-            return p.X != X || p.Y != Y;
-        }
-
-        public float Orient2D(Point pb, Point pc)
-        {
-            float acx = X - pc.X;
-            float bcx = pb.X - pc.X;
-            float acy = Y - pc.Y;
-            float bcy = pb.Y - pc.Y;
-            return acx * bcy - acy * bcx;
-        }
-    }
-
-    internal class Edge
-    {
-        // Pointers used for building trapezoidal map
-        public Trapezoid Above;
-        public float B;
-        public Trapezoid Below;
-
-        // Equation of a line: y = m*x + b
-        // Slope of the line (m)
-
-        // Montone mountain points
-        public HashSet<Point> MPoints;
-        public Point P;
-        public Point Q;
-        public float Slope;
-
-        // Y intercept
-
-        public Edge(Point p, Point q)
-        {
-            P = p;
-            Q = q;
-
-            if (q.X - p.X != 0)
-                Slope = (q.Y - p.Y) / (q.X - p.X);
-            else
-                Slope = 0;
-
-            B = p.Y - (p.X * Slope);
-            Above = null;
-            Below = null;
-            MPoints = new HashSet<Point>();
-            MPoints.Add(p);
-            MPoints.Add(q);
-        }
-
-        public bool IsAbove(Point point)
-        {
-            return P.Orient2D(Q, point) < 0;
-        }
-
-        public bool IsBelow(Point point)
-        {
-            return P.Orient2D(Q, point) > 0;
-        }
-
-        public void AddMpoint(Point point)
-        {
-            foreach (Point mp in MPoints)
-                if (!mp.Neq(point))
-                    return;
-
-            MPoints.Add(point);
-        }
-    }
-
-    internal class Trapezoid
-    {
-        public Edge Bottom;
-        public bool Inside;
-        public Point LeftPoint;
-
-        // Neighbor pointers
-        public Trapezoid LowerLeft;
-        public Trapezoid LowerRight;
-
-        public Point RightPoint;
-        public Sink Sink;
-
-        public Edge Top;
-        public Trapezoid UpperLeft;
-        public Trapezoid UpperRight;
-
-        public Trapezoid(Point leftPoint, Point rightPoint, Edge top, Edge bottom)
-        {
-            LeftPoint = leftPoint;
-            RightPoint = rightPoint;
-            Top = top;
-            Bottom = bottom;
-            UpperLeft = null;
-            UpperRight = null;
-            LowerLeft = null;
-            LowerRight = null;
-            Inside = true;
-            Sink = null;
-        }
-
-        // Update neighbors to the left
-        public void UpdateLeft(Trapezoid ul, Trapezoid ll)
-        {
-            UpperLeft = ul;
-            if (ul != null) ul.UpperRight = this;
-            LowerLeft = ll;
-            if (ll != null) ll.LowerRight = this;
-        }
-
-        // Update neighbors to the right
-        public void UpdateRight(Trapezoid ur, Trapezoid lr)
-        {
-            UpperRight = ur;
-            if (ur != null) ur.UpperLeft = this;
-            LowerRight = lr;
-            if (lr != null) lr.LowerLeft = this;
-        }
-
-        // Update neighbors on both sides
-        public void UpdateLeftRight(Trapezoid ul, Trapezoid ll, Trapezoid ur, Trapezoid lr)
-        {
-            UpperLeft = ul;
-            if (ul != null) ul.UpperRight = this;
-            LowerLeft = ll;
-            if (ll != null) ll.LowerRight = this;
-            UpperRight = ur;
-            if (ur != null) ur.UpperLeft = this;
-            LowerRight = lr;
-            if (lr != null) lr.LowerLeft = this;
-        }
-
-        // Recursively trim outside neighbors
-        public void TrimNeighbors()
-        {
-            if (Inside)
-            {
-                Inside = false;
-                if (UpperLeft != null) UpperLeft.TrimNeighbors();
-                if (LowerLeft != null) LowerLeft.TrimNeighbors();
-                if (UpperRight != null) UpperRight.TrimNeighbors();
-                if (LowerRight != null) LowerRight.TrimNeighbors();
-            }
-        }
-
-        // Determines if this point lies inside the trapezoid
-        public bool Contains(Point point)
-        {
-            return (point.X > LeftPoint.X && point.X < RightPoint.X && Top.IsAbove(point) && Bottom.IsBelow(point));
-        }
-
-        public List<Point> Vertices()
-        {
-            List<Point> verts = new List<Point>(4);
-            verts.Add(LineIntersect(Top, LeftPoint.X));
-            verts.Add(LineIntersect(Bottom, LeftPoint.X));
-            verts.Add(LineIntersect(Bottom, RightPoint.X));
-            verts.Add(LineIntersect(Top, RightPoint.X));
-            return verts;
-        }
-
-        private Point LineIntersect(Edge edge, float x)
-        {
-            float y = edge.Slope * x + edge.B;
-            return new Point(x, y);
-        }
-
-        // Add points to monotone mountain
-        public void AddPoints()
-        {
-            if (LeftPoint != Bottom.P)
-            {
-                Bottom.AddMpoint(LeftPoint);
-            }
-            if (RightPoint != Bottom.Q)
-            {
-                Bottom.AddMpoint(RightPoint);
-            }
-            if (LeftPoint != Top.P)
-            {
-                Top.AddMpoint(LeftPoint);
-            }
-            if (RightPoint != Top.Q)
-            {
-                Top.AddMpoint(RightPoint);
-            }
-        }
-    }
-
-    internal class XNode : Node
-    {
-        private Point _point;
-
-        public XNode(Point point, Node lChild, Node rChild)
-            : base(lChild, rChild)
-        {
-            _point = point;
-        }
-
-        public override Sink Locate(Edge edge)
-        {
-            if (edge.P.X >= _point.X)
-                // Move to the right in the graph
-                return RightChild.Locate(edge);
-            // Move to the left in the graph
-            return LeftChild.Locate(edge);
-        }
-    }
-
-    internal class YNode : Node
-    {
-        private Edge _edge;
-
-        public YNode(Edge edge, Node lChild, Node rChild)
-            : base(lChild, rChild)
-        {
-            _edge = edge;
-        }
-
-        public override Sink Locate(Edge edge)
-        {
-            if (_edge.IsAbove(edge.P))
-                // Move down the graph
-                return RightChild.Locate(edge);
-
-            if (_edge.IsBelow(edge.P))
-                // Move up the graph
-                return LeftChild.Locate(edge);
-
-            // s and segment share the same endpoint, p
-            if (edge.Slope < _edge.Slope)
-                // Move down the graph
-                return RightChild.Locate(edge);
-
-            // Move up the graph
-            return LeftChild.Locate(edge);
-        }
-    }
-
-    internal class Triangulator
-    {
-        // Trapezoid decomposition list
-        public List<Trapezoid> Trapezoids;
-        public List<List<Point>> Triangles;
-
-        // Initialize trapezoidal map and query structure
-        private Trapezoid _boundingBox;
-        private List<Edge> _edgeList;
-        private QueryGraph _queryGraph;
-        private float _sheer = 0.001f;
-        private TrapezoidalMap _trapezoidalMap;
-        private List<MonotoneMountain> _xMonoPoly;
-
-        public Triangulator(List<Point> polyLine, float sheer)
-        {
-            _sheer = sheer;
-            Triangles = new List<List<Point>>();
-            Trapezoids = new List<Trapezoid>();
-            _xMonoPoly = new List<MonotoneMountain>();
-            _edgeList = InitEdges(polyLine);
-            _trapezoidalMap = new TrapezoidalMap();
-            _boundingBox = _trapezoidalMap.BoundingBox(_edgeList);
-            _queryGraph = new QueryGraph(Sink.Isink(_boundingBox));
-
-            Process();
-        }
-
-        // Build the trapezoidal map and query graph
-        private void Process()
-        {
-            foreach (Edge edge in _edgeList)
-            {
-                List<Trapezoid> traps = _queryGraph.FollowEdge(edge);
-
-                // Remove trapezoids from trapezoidal Map
-                foreach (Trapezoid t in traps)
-                {
-                    _trapezoidalMap.Map.Remove(t);
-
-                    bool cp = t.Contains(edge.P);
-                    bool cq = t.Contains(edge.Q);
-                    Trapezoid[] tList;
-
-                    if (cp && cq)
-                    {
-                        tList = _trapezoidalMap.Case1(t, edge);
-                        _queryGraph.Case1(t.Sink, edge, tList);
-                    }
-                    else if (cp && !cq)
-                    {
-                        tList = _trapezoidalMap.Case2(t, edge);
-                        _queryGraph.Case2(t.Sink, edge, tList);
-                    }
-                    else if (!cp && !cq)
-                    {
-                        tList = _trapezoidalMap.Case3(t, edge);
-                        _queryGraph.Case3(t.Sink, edge, tList);
-                    }
-                    else
-                    {
-                        tList = _trapezoidalMap.Case4(t, edge);
-                        _queryGraph.Case4(t.Sink, edge, tList);
-                    }
-                    // Add new trapezoids to map
-                    foreach (Trapezoid y in tList)
-                    {
-                        _trapezoidalMap.Map.Add(y);
-                    }
-                }
-                _trapezoidalMap.Clear();
-            }
-
-            // Mark outside trapezoids
-            foreach (Trapezoid t in _trapezoidalMap.Map)
-            {
-                MarkOutside(t);
-            }
-
-            // Collect interior trapezoids
-            foreach (Trapezoid t in _trapezoidalMap.Map)
-            {
-                if (t.Inside)
-                {
-                    Trapezoids.Add(t);
-                    t.AddPoints();
-                }
-            }
-
-            // Generate the triangles
-            CreateMountains();
-        }
-
-        // Build a list of x-monotone mountains
-        private void CreateMountains()
-        {
-            foreach (Edge edge in _edgeList)
-            {
-                if (edge.MPoints.Count > 2)
-                {
-                    MonotoneMountain mountain = new MonotoneMountain();
-
-                    // Sorting is a perfromance hit. Literature says this can be accomplised in
-                    // linear time, although I don't see a way around using traditional methods
-                    // when using a randomized incremental algorithm
-
-                    // Insertion sort is one of the fastest algorithms for sorting arrays containing 
-                    // fewer than ten elements, or for lists that are already mostly sorted.
-
-                    List<Point> points = new List<Point>(edge.MPoints);
-                    points.Sort((p1, p2) => p1.X.CompareTo(p2.X));
-
-                    foreach (Point p in points)
-                        mountain.Add(p);
-
-                    // Triangulate monotone mountain
-                    mountain.Process();
-
-                    // Extract the triangles into a single list
-                    foreach (List<Point> t in mountain.Triangles)
-                    {
-                        Triangles.Add(t);
-                    }
-
-                    _xMonoPoly.Add(mountain);
-                }
-            }
-        }
-
-        // Mark the outside trapezoids surrounding the polygon
-        private void MarkOutside(Trapezoid t)
-        {
-            if (t.Top == _boundingBox.Top || t.Bottom == _boundingBox.Bottom)
-                t.TrimNeighbors();
-        }
-
-        // Create segments and connect end points; update edge event pointer
-        private List<Edge> InitEdges(List<Point> points)
-        {
-            List<Edge> edges = new List<Edge>();
-
-            for (int i = 0; i < points.Count - 1; i++)
-            {
-                edges.Add(new Edge(points[i], points[i + 1]));
-            }
-            edges.Add(new Edge(points[0], points[points.Count - 1]));
-            return OrderSegments(edges);
-        }
-
-        private List<Edge> OrderSegments(List<Edge> edgeInput)
-        {
-            // Ignore vertical segments!
-            List<Edge> edges = new List<Edge>();
-
-            foreach (Edge e in edgeInput)
-            {
-                Point p = ShearTransform(e.P);
-                Point q = ShearTransform(e.Q);
-
-                // Point p must be to the left of point q
-                if (p.X > q.X)
-                {
-                    edges.Add(new Edge(q, p));
-                }
-                else if (p.X < q.X)
-                {
-                    edges.Add(new Edge(p, q));
-                }
-            }
-
-            // Randomized triangulation improves performance
-            // See Seidel's paper, or O'Rourke's book, p. 57 
-            Shuffle(edges);
-            return edges;
-        }
-
-        private static void Shuffle<T>(IList<T> list)
-        {
-            Random rng = new Random();
-            int n = list.Count;
-            while (n > 1)
-            {
-                n--;
-                int k = rng.Next(n + 1);
-                T value = list[k];
-                list[k] = list[n];
-                list[n] = value;
-            }
-        }
-
-        // Prevents any two distinct endpoints from lying on a common vertical line, and avoiding
-        // the degenerate case. See Mark de Berg et al, Chapter 6.3
-        private Point ShearTransform(Point point)
-        {
-            return new Point(point.X + _sheer * point.Y, point.Y);
-        }
-    }
-}

FarseerPhysicsEngine/Common/FixedArray.cs

@@ -1,227 +0,0 @@
-/*
-* Farseer Physics Engine based on Box2D.XNA port:
-* Copyright (c) 2010 Ian Qvist
-* 
-* Box2D.XNA port of Box2D:
-* Copyright (c) 2009 Brandon Furtwangler, Nathan Furtwangler
-*
-* Original source Box2D:
-* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com 
-* 
-* This software is provided 'as-is', without any express or implied 
-* warranty.  In no event will the authors be held liable for any damages 
-* arising from the use of this software. 
-* Permission is granted to anyone to use this software for any purpose, 
-* including commercial applications, and to alter it and redistribute it 
-* freely, subject to the following restrictions: 
-* 1. The origin of this software must not be misrepresented; you must not 
-* claim that you wrote the original software. If you use this software 
-* in a product, an acknowledgment in the product documentation would be 
-* appreciated but is not required. 
-* 2. Altered source versions must be plainly marked as such, and must not be 
-* misrepresented as being the original software. 
-* 3. This notice may not be removed or altered from any source distribution. 
-*/
-
-using System;
-
-namespace FarseerPhysics.Common
-{
-    public struct FixedArray2<T>
-    {
-        private T _value0;
-        private T _value1;
-
-        public T this[int index]
-        {
-            get
-            {
-                switch (index)
-                {
-                    case 0:
-                        return _value0;
-                    case 1:
-                        return _value1;
-                    default:
-                        throw new IndexOutOfRangeException();
-                }
-            }
-            set
-            {
-                switch (index)
-                {
-                    case 0:
-                        _value0 = value;
-                        break;
-                    case 1:
-                        _value1 = value;
-                        break;
-                    default:
-                        throw new IndexOutOfRangeException();
-                }
-            }
-        }
-    }
-
-    public struct FixedArray3<T>
-    {
-        private T _value0;
-        private T _value1;
-        private T _value2;
-
-        public T this[int index]
-        {
-            get
-            {
-                switch (index)
-                {
-                    case 0:
-                        return _value0;
-                    case 1:
-                        return _value1;
-                    case 2:
-                        return _value2;
-                    default:
-                        throw new IndexOutOfRangeException();
-                }
-            }
-            set
-            {
-                switch (index)
-                {
-                    case 0:
-                        _value0 = value;
-                        break;
-                    case 1:
-                        _value1 = value;
-                        break;
-                    case 2:
-                        _value2 = value;
-                        break;
-                    default:
-                        throw new IndexOutOfRangeException();
-                }
-            }
-        }
-    }
-
-    public struct FixedArray4<T>
-    {
-        private T _value0;
-        private T _value1;
-        private T _value2;
-        private T _value3;
-
-        public T this[int index]
-        {
-            get
-            {
-                switch (index)
-                {
-                    case 0:
-                        return _value0;
-                    case 1:
-                        return _value1;
-                    case 2:
-                        return _value2;
-                    case 3:
-                        return _value3;
-                    default:
-                        throw new IndexOutOfRangeException();
-                }
-            }
-            set
-            {
-                switch (index)
-                {
-                    case 0:
-                        _value0 = value;
-                        break;
-                    case 1:
-                        _value1 = value;
-                        break;
-                    case 2:
-                        _value2 = value;
-                        break;
-                    case 3:
-                        _value3 = value;
-                        break;
-                    default:
-                        throw new IndexOutOfRangeException();
-                }
-            }
-        }
-    }
-
-    public struct FixedArray8<T>
-    {
-        private T _value0;
-        private T _value1;
-        private T _value2;
-        private T _value3;
-        private T _value4;
-        private T _value5;
-        private T _value6;
-        private T _value7;
-
-        public T this[int index]
-        {
-            get
-            {
-                switch (index)
-                {
-                    case 0:
-                        return _value0;
-                    case 1:
-                        return _value1;
-                    case 2:
-                        return _value2;
-                    case 3:
-                        return _value3;
-                    case 4:
-                        return _value4;
-                    case 5:
-                        return _value5;
-                    case 6:
-                        return _value6;
-                    case 7:
-                        return _value7;
-                    default:
-                        throw new IndexOutOfRangeException();
-                }
-            }
-            set
-            {
-                switch (index)
-                {
-                    case 0:
-                        _value0 = value;
-                        break;
-                    case 1:
-                        _value1 = value;
-                        break;
-                    case 2:
-                        _value2 = value;
-                        break;
-                    case 3:
-                        _value3 = value;
-                        break;
-                    case 4:
-                        _value4 = value;
-                        break;
-                    case 5:
-                        _value5 = value;
-                        break;
-                    case 6:
-                        _value6 = value;
-                        break;
-                    case 7:
-                        _value7 = value;
-                        break;
-                    default:
-                        throw new IndexOutOfRangeException();
-                }
-            }
-        }
-    }
-}

FarseerPhysicsEngine/Common/HashSet.cs

@@ -1,81 +0,0 @@
-
-#if WINDOWS_PHONE || XBOX
-
-//TODO: FIX
-
-using System;
-using System.Collections;
-using System.Collections.Generic;
-
-namespace FarseerPhysics.Common
-{
-    
-    public class HashSet<T> : ICollection<T>
-    {
-        private Dictionary<T, short> _dict;
-
-        public HashSet(int capacity)
-        {
-            _dict = new Dictionary<T, short>(capacity);
-        }
-
-        public HashSet()
-        {
-            _dict = new Dictionary<T, short>();
-        }
-
-        // Methods
-
-#region ICollection<T> Members
-
-        public void Add(T item)
-        {
-            // We don't care for the value in dictionary, Keys matter.
-            _dict.Add(item, 0);
-        }
-
-        public void Clear()
-        {
-            _dict.Clear();
-        }
-
-        public bool Contains(T item)
-        {
-            return _dict.ContainsKey(item);
-        }
-
-        public void CopyTo(T[] array, int arrayIndex)
-        {
-            throw new NotImplementedException();
-        }
-
-        public bool Remove(T item)
-        {
-            return _dict.Remove(item);
-        }
-
-        public IEnumerator<T> GetEnumerator()
-        {
-            return _dict.Keys.GetEnumerator();
-        }
-
-        IEnumerator IEnumerable.GetEnumerator()
-        {
-            return _dict.Keys.GetEnumerator();
-        }
-
-        // Properties
-        public int Count
-        {
-            get { return _dict.Keys.Count; }
-        }
-
-        public bool IsReadOnly
-        {
-            get { return false; }
-        }
-
-        #endregion
-    }
-}
-#endif

FarseerPhysicsEngine/Common/LineTools.cs

@@ -1,308 +0,0 @@
-using System;
-using System.Collections.Generic;
-using FarseerPhysics.Collision;
-using Microsoft.Xna.Framework;
-
-namespace FarseerPhysics.Common
-{
-    /// <summary>
-    /// Collection of helper methods for misc collisions.
-    /// Does float tolerance and line collisions with lines and AABBs.
-    /// </summary>
-    public static class LineTools
-    {
-        public static float DistanceBetweenPointAndPoint(ref Vector2 point1, ref Vector2 point2)
-        {
-            Vector2 v;
-            Vector2.Subtract(ref point1, ref point2, out v);
-            return v.Length();
-        }
-
-        public static float DistanceBetweenPointAndLineSegment(ref Vector2 point, ref Vector2 lineEndPoint1,
-                                                               ref Vector2 lineEndPoint2)
-        {
-            Vector2 v = Vector2.Subtract(lineEndPoint2, lineEndPoint1);
-            Vector2 w = Vector2.Subtract(point, lineEndPoint1);
-
-            float c1 = Vector2.Dot(w, v);
-            if (c1 <= 0) return DistanceBetweenPointAndPoint(ref point, ref lineEndPoint1);
-
-            float c2 = Vector2.Dot(v, v);
-            if (c2 <= c1) return DistanceBetweenPointAndPoint(ref point, ref lineEndPoint2);
-
-            float b = c1 / c2;
-            Vector2 pointOnLine = Vector2.Add(lineEndPoint1, Vector2.Multiply(v, b));
-            return DistanceBetweenPointAndPoint(ref point, ref pointOnLine);
-        }
-
-        // From Eric Jordan's convex decomposition library
-        /// <summary>
-        ///Check if the lines a0->a1 and b0->b1 cross.
-        ///If they do, intersectionPoint will be filled
-        ///with the point of crossing.
-        ///
-        ///Grazing lines should not return true.
-        /// 
-        /// </summary>
-        /// <param name="a0"></param>
-        /// <param name="a1"></param>
-        /// <param name="b0"></param>
-        /// <param name="b1"></param>
-        /// <param name="intersectionPoint"></param>
-        /// <returns></returns>
-        public static bool LineIntersect2(Vector2 a0, Vector2 a1, Vector2 b0, Vector2 b1, out Vector2 intersectionPoint)
-        {
-            intersectionPoint = Vector2.Zero;
-
-            if (a0 == b0 || a0 == b1 || a1 == b0 || a1 == b1)
-                return false;
-
-            float x1 = a0.X;
-            float y1 = a0.Y;
-            float x2 = a1.X;
-            float y2 = a1.Y;
-            float x3 = b0.X;
-            float y3 = b0.Y;
-            float x4 = b1.X;
-            float y4 = b1.Y;
-
-            //AABB early exit
-            if (Math.Max(x1, x2) < Math.Min(x3, x4) || Math.Max(x3, x4) < Math.Min(x1, x2))
-                return false;
-
-            if (Math.Max(y1, y2) < Math.Min(y3, y4) || Math.Max(y3, y4) < Math.Min(y1, y2))
-                return false;
-
-            float ua = ((x4 - x3) * (y1 - y3) - (y4 - y3) * (x1 - x3));
-            float ub = ((x2 - x1) * (y1 - y3) - (y2 - y1) * (x1 - x3));
-            float denom = (y4 - y3) * (x2 - x1) - (x4 - x3) * (y2 - y1);
-            if (Math.Abs(denom) < Settings.Epsilon)
-            {
-                //Lines are too close to parallel to call
-                return false;
-            }
-            ua /= denom;
-            ub /= denom;
-
-            if ((0 < ua) && (ua < 1) && (0 < ub) && (ub < 1))
-            {
-                intersectionPoint.X = (x1 + ua * (x2 - x1));
-                intersectionPoint.Y = (y1 + ua * (y2 - y1));
-                return true;
-            }
-
-            return false;
-        }
-
-        //From Mark Bayazit's convex decomposition algorithm
-        public static Vector2 LineIntersect(Vector2 p1, Vector2 p2, Vector2 q1, Vector2 q2)
-        {
-            Vector2 i = Vector2.Zero;
-            float a1 = p2.Y - p1.Y;
-            float b1 = p1.X - p2.X;
-            float c1 = a1 * p1.X + b1 * p1.Y;
-            float a2 = q2.Y - q1.Y;
-            float b2 = q1.X - q2.X;
-            float c2 = a2 * q1.X + b2 * q1.Y;
-            float det = a1 * b2 - a2 * b1;
-
-            if (!MathUtils.FloatEquals(det, 0))
-            {
-                // lines are not parallel
-                i.X = (b2 * c1 - b1 * c2) / det;
-                i.Y = (a1 * c2 - a2 * c1) / det;
-            }
-            return i;
-        }
-
-        /// <summary>
-        /// This method detects if two line segments (or lines) intersect,
-        /// and, if so, the point of intersection. Use the <paramref name="firstIsSegment"/> and
-        /// <paramref name="secondIsSegment"/> parameters to set whether the intersection point
-        /// must be on the first and second line segments. Setting these
-        /// both to true means you are doing a line-segment to line-segment
-        /// intersection. Setting one of them to true means you are doing a
-        /// line to line-segment intersection test, and so on.
-        /// Note: If two line segments are coincident, then 
-        /// no intersection is detected (there are actually
-        /// infinite intersection points).
-        /// Author: Jeremy Bell
-        /// </summary>
-        /// <param name="point1">The first point of the first line segment.</param>
-        /// <param name="point2">The second point of the first line segment.</param>
-        /// <param name="point3">The first point of the second line segment.</param>
-        /// <param name="point4">The second point of the second line segment.</param>
-        /// <param name="point">This is set to the intersection
-        /// point if an intersection is detected.</param>
-        /// <param name="firstIsSegment">Set this to true to require that the 
-        /// intersection point be on the first line segment.</param>
-        /// <param name="secondIsSegment">Set this to true to require that the
-        /// intersection point be on the second line segment.</param>
-        /// <returns>True if an intersection is detected, false otherwise.</returns>
-        public static bool LineIntersect(ref Vector2 point1, ref Vector2 point2, ref Vector2 point3, ref Vector2 point4,
-                                         bool firstIsSegment, bool secondIsSegment,
-                                         out Vector2 point)
-        {
-            point = new Vector2();
-
-            // these are reused later.
-            // each lettered sub-calculation is used twice, except
-            // for b and d, which are used 3 times
-            float a = point4.Y - point3.Y;
-            float b = point2.X - point1.X;
-            float c = point4.X - point3.X;
-            float d = point2.Y - point1.Y;
-
-            // denominator to solution of linear system
-            float denom = (a * b) - (c * d);
-
-            // if denominator is 0, then lines are parallel
-            if (!(denom >= -Settings.Epsilon && denom <= Settings.Epsilon))
-            {
-                float e = point1.Y - point3.Y;
-                float f = point1.X - point3.X;
-                float oneOverDenom = 1.0f / denom;
-
-                // numerator of first equation
-                float ua = (c * e) - (a * f);
-                ua *= oneOverDenom;
-
-                // check if intersection point of the two lines is on line segment 1
-                if (!firstIsSegment || ua >= 0.0f && ua <= 1.0f)
-                {
-                    // numerator of second equation
-                    float ub = (b * e) - (d * f);
-                    ub *= oneOverDenom;
-
-                    // check if intersection point of the two lines is on line segment 2
-                    // means the line segments intersect, since we know it is on
-                    // segment 1 as well.
-                    if (!secondIsSegment || ub >= 0.0f && ub <= 1.0f)
-                    {
-                        // check if they are coincident (no collision in this case)
-                        if (ua != 0f || ub != 0f)
-                        {
-                            //There is an intersection
-                            point.X = point1.X + ua * b;
-                            point.Y = point1.Y + ua * d;
-                            return true;
-                        }
-                    }
-                }
-            }
-
-            return false;
-        }
-
-        /// <summary>
-        /// This method detects if two line segments (or lines) intersect,
-        /// and, if so, the point of intersection. Use the <paramref name="firstIsSegment"/> and
-        /// <paramref name="secondIsSegment"/> parameters to set whether the intersection point
-        /// must be on the first and second line segments. Setting these
-        /// both to true means you are doing a line-segment to line-segment
-        /// intersection. Setting one of them to true means you are doing a
-        /// line to line-segment intersection test, and so on.
-        /// Note: If two line segments are coincident, then 
-        /// no intersection is detected (there are actually
-        /// infinite intersection points).
-        /// Author: Jeremy Bell
-        /// </summary>
-        /// <param name="point1">The first point of the first line segment.</param>
-        /// <param name="point2">The second point of the first line segment.</param>
-        /// <param name="point3">The first point of the second line segment.</param>
-        /// <param name="point4">The second point of the second line segment.</param>
-        /// <param name="intersectionPoint">This is set to the intersection
-        /// point if an intersection is detected.</param>
-        /// <param name="firstIsSegment">Set this to true to require that the 
-        /// intersection point be on the first line segment.</param>
-        /// <param name="secondIsSegment">Set this to true to require that the
-        /// intersection point be on the second line segment.</param>
-        /// <returns>True if an intersection is detected, false otherwise.</returns>
-        public static bool LineIntersect(Vector2 point1, Vector2 point2, Vector2 point3, Vector2 point4,
-                                         bool firstIsSegment,
-                                         bool secondIsSegment, out Vector2 intersectionPoint)
-        {
-            return LineIntersect(ref point1, ref point2, ref point3, ref point4, firstIsSegment, secondIsSegment,
-                                 out intersectionPoint);
-        }
-
-        /// <summary>
-        /// This method detects if two line segments intersect,
-        /// and, if so, the point of intersection. 
-        /// Note: If two line segments are coincident, then 
-        /// no intersection is detected (there are actually
-        /// infinite intersection points).
-        /// </summary>
-        /// <param name="point1">The first point of the first line segment.</param>
-        /// <param name="point2">The second point of the first line segment.</param>
-        /// <param name="point3">The first point of the second line segment.</param>
-        /// <param name="point4">The second point of the second line segment.</param>
-        /// <param name="intersectionPoint">This is set to the intersection
-        /// point if an intersection is detected.</param>
-        /// <returns>True if an intersection is detected, false otherwise.</returns>
-        public static bool LineIntersect(ref Vector2 point1, ref Vector2 point2, ref Vector2 point3, ref Vector2 point4,
-                                         out Vector2 intersectionPoint)
-        {
-            return LineIntersect(ref point1, ref point2, ref point3, ref point4, true, true, out intersectionPoint);
-        }
-
-        /// <summary>
-        /// This method detects if two line segments intersect,
-        /// and, if so, the point of intersection. 
-        /// Note: If two line segments are coincident, then 
-        /// no intersection is detected (there are actually
-        /// infinite intersection points).
-        /// </summary>
-        /// <param name="point1">The first point of the first line segment.</param>
-        /// <param name="point2">The second point of the first line segment.</param>
-        /// <param name="point3">The first point of the second line segment.</param>
-        /// <param name="point4">The second point of the second line segment.</param>
-        /// <param name="intersectionPoint">This is set to the intersection
-        /// point if an intersection is detected.</param>
-        /// <returns>True if an intersection is detected, false otherwise.</returns>
-        public static bool LineIntersect(Vector2 point1, Vector2 point2, Vector2 point3, Vector2 point4,
-                                         out Vector2 intersectionPoint)
-        {
-            return LineIntersect(ref point1, ref point2, ref point3, ref point4, true, true, out intersectionPoint);
-        }
-
-        /// <summary>
-        /// Get all intersections between a line segment and a list of vertices
-        /// representing a polygon. The vertices reuse adjacent points, so for example
-        /// edges one and two are between the first and second vertices and between the
-        /// second and third vertices. The last edge is between vertex vertices.Count - 1
-        /// and verts0. (ie, vertices from a Geometry or AABB)
-        /// </summary>
-        /// <param name="point1">The first point of the line segment to test</param>
-        /// <param name="point2">The second point of the line segment to test.</param>
-        /// <param name="vertices">The vertices, as described above</param>
-        /// <param name="intersectionPoints">An list of intersection points. Any intersection points
-        /// found will be added to this list.</param>
-        public static void LineSegmentVerticesIntersect(ref Vector2 point1, ref Vector2 point2, Vertices vertices,
-                                                        ref List<Vector2> intersectionPoints)
-        {
-            for (int i = 0; i < vertices.Count; i++)
-            {
-                Vector2 point;
-                if (LineIntersect(vertices[i], vertices[vertices.NextIndex(i)],
-                                  point1, point2, true, true, out point))
-                {
-                    intersectionPoints.Add(point);
-                }
-            }
-        }
-
-        /// <summary>
-        /// Get all intersections between a line segment and an AABB. 
-        /// </summary>
-        /// <param name="point1">The first point of the line segment to test</param>
-        /// <param name="point2">The second point of the line segment to test.</param>
-        /// <param name="aabb">The AABB that is used for testing intersection.</param>
-        /// <param name="intersectionPoints">An list of intersection points. Any intersection points found will be added to this list.</param>
-        public static void LineSegmentAABBIntersect(ref Vector2 point1, ref Vector2 point2, AABB aabb,
-                                                    ref List<Vector2> intersectionPoints)
-        {
-            LineSegmentVerticesIntersect(ref point1, ref point2, aabb.Vertices, ref intersectionPoints);
-        }
-    }
-}

FarseerPhysicsEngine/Common/Math.cs

@@ -1,638 +0,0 @@
-/*
-* Farseer Physics Engine based on Box2D.XNA port:
-* Copyright (c) 2010 Ian Qvist
-* 
-* Box2D.XNA port of Box2D:
-* Copyright (c) 2009 Brandon Furtwangler, Nathan Furtwangler
-*
-* Original source Box2D:
-* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com 
-* 
-* This software is provided 'as-is', without any express or implied 
-* warranty.  In no event will the authors be held liable for any damages 
-* arising from the use of this software. 
-* Permission is granted to anyone to use this software for any purpose, 
-* including commercial applications, and to alter it and redistribute it 
-* freely, subject to the following restrictions: 
-* 1. The origin of this software must not be misrepresented; you must not 
-* claim that you wrote the original software. If you use this software 
-* in a product, an acknowledgment in the product documentation would be 
-* appreciated but is not required. 
-* 2. Altered source versions must be plainly marked as such, and must not be 
-* misrepresented as being the original software. 
-* 3. This notice may not be removed or altered from any source distribution. 
-*/
-
-using System;
-using System.Diagnostics;
-using System.Runtime.InteropServices;
-using Microsoft.Xna.Framework;
-
-namespace FarseerPhysics.Common
-{
-    public static class MathUtils
-    {
-        public static float Cross(Vector2 a, Vector2 b)
-        {
-            return a.X * b.Y - a.Y * b.X;
-        }
-
-        public static Vector2 Cross(Vector2 a, float s)
-        {
-            return new Vector2(s * a.Y, -s * a.X);
-        }
-
-        public static Vector2 Cross(float s, Vector2 a)
-        {
-            return new Vector2(-s * a.Y, s * a.X);
-        }
-
-        public static Vector2 Abs(Vector2 v)
-        {
-            return new Vector2(Math.Abs(v.X), Math.Abs(v.Y));
-        }
-
-        public static Vector2 Multiply(ref Mat22 A, Vector2 v)
-        {
-            return Multiply(ref A, ref v);
-        }
-
-        public static Vector2 Multiply(ref Mat22 A, ref Vector2 v)
-        {
-            return new Vector2(A.Col1.X * v.X + A.Col2.X * v.Y, A.Col1.Y * v.X + A.Col2.Y * v.Y);
-        }
-
-        public static Vector2 MultiplyT(ref Mat22 A, Vector2 v)
-        {
-            return MultiplyT(ref A, ref v);
-        }
-
-        public static Vector2 MultiplyT(ref Mat22 A, ref Vector2 v)
-        {
-            return new Vector2(v.X * A.Col1.X + v.Y * A.Col1.Y, v.X * A.Col2.X + v.Y * A.Col2.Y);
-        }
-
-        public static Vector2 Multiply(ref Transform T, Vector2 v)
-        {
-            return Multiply(ref T, ref v);
-        }
-
-        public static Vector2 Multiply(ref Transform T, ref Vector2 v)
-        {
-            return new Vector2(T.Position.X + T.R.Col1.X * v.X + T.R.Col2.X * v.Y,
-                               T.Position.Y + T.R.Col1.Y * v.X + T.R.Col2.Y * v.Y);
-        }
-
-        public static Vector2 MultiplyT(ref Transform T, Vector2 v)
-        {
-            return MultiplyT(ref T, ref v);
-        }
-
-        public static Vector2 MultiplyT(ref Transform T, ref Vector2 v)
-        {
-            Vector2 tmp = Vector2.Zero;
-            tmp.X = v.X - T.Position.X;
-            tmp.Y = v.Y - T.Position.Y;
-            return MultiplyT(ref T.R, ref tmp);
-        }
-
-        // A^T * B
-        public static void MultiplyT(ref Mat22 A, ref Mat22 B, out Mat22 C)
-        {
-            C = new Mat22();
-            C.Col1.X = A.Col1.X * B.Col1.X + A.Col1.Y * B.Col1.Y;
-            C.Col1.Y = A.Col2.X * B.Col1.X + A.Col2.Y * B.Col1.Y;
-            C.Col2.X = A.Col1.X * B.Col2.X + A.Col1.Y * B.Col2.Y;
-            C.Col2.Y = A.Col2.X * B.Col2.X + A.Col2.Y * B.Col2.Y;
-        }
-
-        // v2 = A.R' * (B.R * v1 + B.p - A.p) = (A.R' * B.R) * v1 + (B.p - A.p)
-        public static void MultiplyT(ref Transform A, ref Transform B, out Transform C)
-        {
-            C = new Transform();
-            MultiplyT(ref A.R, ref B.R, out C.R);
-            C.Position.X = B.Position.X - A.Position.X;
-            C.Position.Y = B.Position.Y - A.Position.Y;
-        }
-
-        public static void Swap<T>(ref T a, ref T b)
-        {
-            T tmp = a;
-            a = b;
-            b = tmp;
-        }
-
-        /// <summary>
-        /// This function is used to ensure that a floating point number is
-        /// not a NaN or infinity.
-        /// </summary>
-        /// <param name="x">The x.</param>
-        /// <returns>
-        /// 	<c>true</c> if the specified x is valid; otherwise, <c>false</c>.
-        /// </returns>
-        public static bool IsValid(float x)
-        {
-            if (float.IsNaN(x))
-            {
-                // NaN.
-                return false;
-            }
-
-            return !float.IsInfinity(x);
-        }
-
-        public static bool IsValid(this Vector2 x)
-        {
-            return IsValid(x.X) && IsValid(x.Y);
-        }
-
-        /// <summary>
-        /// This is a approximate yet fast inverse square-root.
-        /// </summary>
-        /// <param name="x">The x.</param>
-        /// <returns></returns>
-        public static float InvSqrt(float x)
-        {
-            FloatConverter convert = new FloatConverter();
-            convert.x = x;
-            float xhalf = 0.5f * x;
-            convert.i = 0x5f3759df - (convert.i >> 1);
-            x = convert.x;
-            x = x * (1.5f - xhalf * x * x);
-            return x;
-        }
-
-        public static int Clamp(int a, int low, int high)
-        {
-            return Math.Max(low, Math.Min(a, high));
-        }
-
-        public static float Clamp(float a, float low, float high)
-        {
-            return Math.Max(low, Math.Min(a, high));
-        }
-
-        public static Vector2 Clamp(Vector2 a, Vector2 low, Vector2 high)
-        {
-            return Vector2.Max(low, Vector2.Min(a, high));
-        }
-
-        public static void Cross(ref Vector2 a, ref Vector2 b, out float c)
-        {
-            c = a.X * b.Y - a.Y * b.X;
-        }
-
-        /// <summary>
-        /// Return the angle between two vectors on a plane
-        /// The angle is from vector 1 to vector 2, positive anticlockwise
-        /// The result is between -pi -> pi
-        /// </summary>
-        public static double VectorAngle(ref Vector2 p1, ref Vector2 p2)
-        {
-            double theta1 = Math.Atan2(p1.Y, p1.X);
-            double theta2 = Math.Atan2(p2.Y, p2.X);
-            double dtheta = theta2 - theta1;
-            while (dtheta > Math.PI)
-                dtheta -= (2 * Math.PI);
-            while (dtheta < -Math.PI)
-                dtheta += (2 * Math.PI);
-
-            return (dtheta);
-        }
-
-        public static double VectorAngle(Vector2 p1, Vector2 p2)
-        {
-            return VectorAngle(ref p1, ref p2);
-        }
-
-        /// <summary>
-        /// Returns a positive number if c is to the left of the line going from a to b.
-        /// </summary>
-        /// <returns>Positive number if point is left, negative if point is right, 
-        /// and 0 if points are collinear.</returns>
-        public static float Area(Vector2 a, Vector2 b, Vector2 c)
-        {
-            return Area(ref a, ref b, ref c);
-        }
-
-        /// <summary>
-        /// Returns a positive number if c is to the left of the line going from a to b.
-        /// </summary>
-        /// <returns>Positive number if point is left, negative if point is right, 
-        /// and 0 if points are collinear.</returns>
-        public static float Area(ref Vector2 a, ref Vector2 b, ref Vector2 c)
-        {
-            return a.X * (b.Y - c.Y) + b.X * (c.Y - a.Y) + c.X * (a.Y - b.Y);
-        }
-
-        /// <summary>
-        /// Determines if three vertices are collinear (ie. on a straight line)
-        /// </summary>
-        /// <param name="a">First vertex</param>
-        /// <param name="b">Second vertex</param>
-        /// <param name="c">Third vertex</param>
-        /// <returns></returns>
-        public static bool Collinear(ref Vector2 a, ref Vector2 b, ref Vector2 c)
-        {
-            return Collinear(ref a, ref b, ref c, 0);
-        }
-
-        public static bool Collinear(ref Vector2 a, ref Vector2 b, ref Vector2 c, float tolerance)
-        {
-            return FloatInRange(Area(ref a, ref b, ref c), -tolerance, tolerance);
-        }
-
-        public static void Cross(float s, ref Vector2 a, out Vector2 b)
-        {
-            b = new Vector2(-s * a.Y, s * a.X);
-        }
-
-        public static bool FloatEquals(float value1, float value2)
-        {
-            return Math.Abs(value1 - value2) <= Settings.Epsilon;
-        }
-
-        /// <summary>
-        /// Checks if a floating point Value is equal to another,
-        /// within a certain tolerance.
-        /// </summary>
-        /// <param name="value1">The first floating point Value.</param>
-        /// <param name="value2">The second floating point Value.</param>
-        /// <param name="delta">The floating point tolerance.</param>
-        /// <returns>True if the values are "equal", false otherwise.</returns>
-        public static bool FloatEquals(float value1, float value2, float delta)
-        {
-            return FloatInRange(value1, value2 - delta, value2 + delta);
-        }
-
-        /// <summary>
-        /// Checks if a floating point Value is within a specified
-        /// range of values (inclusive).
-        /// </summary>
-        /// <param name="value">The Value to check.</param>
-        /// <param name="min">The minimum Value.</param>
-        /// <param name="max">The maximum Value.</param>
-        /// <returns>True if the Value is within the range specified,
-        /// false otherwise.</returns>
-        public static bool FloatInRange(float value, float min, float max)
-        {
-            return (value >= min && value <= max);
-        }
-
-        #region Nested type: FloatConverter
-
-        [StructLayout(LayoutKind.Explicit)]
-        private struct FloatConverter
-        {
-            [FieldOffset(0)]
-            public float x;
-            [FieldOffset(0)]
-            public int i;
-        }
-
-        #endregion
-    }
-
-    /// <summary>
-    /// A 2-by-2 matrix. Stored in column-major order.
-    /// </summary>
-    public struct Mat22
-    {
-        public Vector2 Col1, Col2;
-
-        /// <summary>
-        /// Construct this matrix using columns.
-        /// </summary>
-        /// <param name="c1">The c1.</param>
-        /// <param name="c2">The c2.</param>
-        public Mat22(Vector2 c1, Vector2 c2)
-        {
-            Col1 = c1;
-            Col2 = c2;
-        }
-
-        /// <summary>
-        /// Construct this matrix using scalars.
-        /// </summary>
-        /// <param name="a11">The a11.</param>
-        /// <param name="a12">The a12.</param>
-        /// <param name="a21">The a21.</param>
-        /// <param name="a22">The a22.</param>
-        public Mat22(float a11, float a12, float a21, float a22)
-        {
-            Col1 = new Vector2(a11, a21);
-            Col2 = new Vector2(a12, a22);
-        }
-
-        /// <summary>
-        /// Construct this matrix using an angle. This matrix becomes
-        /// an orthonormal rotation matrix.
-        /// </summary>
-        /// <param name="angle">The angle.</param>
-        public Mat22(float angle)
-        {
-            // TODO_ERIN compute sin+cos together.
-            float c = (float)Math.Cos(angle), s = (float)Math.Sin(angle);
-            Col1 = new Vector2(c, s);
-            Col2 = new Vector2(-s, c);
-        }
-
-        /// <summary>
-        /// Extract the angle from this matrix (assumed to be
-        /// a rotation matrix).
-        /// </summary>
-        /// <value></value>
-        public float Angle
-        {
-            get { return (float)Math.Atan2(Col1.Y, Col1.X); }
-        }
-
-        public Mat22 Inverse
-        {
-            get
-            {
-                float a = Col1.X, b = Col2.X, c = Col1.Y, d = Col2.Y;
-                float det = a * d - b * c;
-                if (det != 0.0f)
-                {
-                    det = 1.0f / det;
-                }
-
-                Mat22 result = new Mat22();
-                result.Col1.X = det * d;
-                result.Col1.Y = -det * c;
-
-                result.Col2.X = -det * b;
-                result.Col2.Y = det * a;
-
-                return result;
-            }
-        }
-
-        /// <summary>
-        /// Initialize this matrix using columns.
-        /// </summary>
-        /// <param name="c1">The c1.</param>
-        /// <param name="c2">The c2.</param>
-        public void Set(Vector2 c1, Vector2 c2)
-        {
-            Col1 = c1;
-            Col2 = c2;
-        }
-
-        /// <summary>
-        /// Initialize this matrix using an angle. This matrix becomes
-        /// an orthonormal rotation matrix.
-        /// </summary>
-        /// <param name="angle">The angle.</param>
-        public void Set(float angle)
-        {
-            float c = (float)Math.Cos(angle), s = (float)Math.Sin(angle);
-            Col1.X = c;
-            Col2.X = -s;
-            Col1.Y = s;
-            Col2.Y = c;
-        }
-
-        /// <summary>
-        /// Set this to the identity matrix.
-        /// </summary>
-        public void SetIdentity()
-        {
-            Col1.X = 1.0f;
-            Col2.X = 0.0f;
-            Col1.Y = 0.0f;
-            Col2.Y = 1.0f;
-        }
-
-        /// <summary>
-        /// Set this matrix to all zeros.
-        /// </summary>
-        public void SetZero()
-        {
-            Col1.X = 0.0f;
-            Col2.X = 0.0f;
-            Col1.Y = 0.0f;
-            Col2.Y = 0.0f;
-        }
-
-        /// <summary>
-        /// Solve A * x = b, where b is a column vector. This is more efficient
-        /// than computing the inverse in one-shot cases.
-        /// </summary>
-        /// <param name="b">The b.</param>
-        /// <returns></returns>
-        public Vector2 Solve(Vector2 b)
-        {
-            float a11 = Col1.X, a12 = Col2.X, a21 = Col1.Y, a22 = Col2.Y;
-            float det = a11 * a22 - a12 * a21;
-            if (det != 0.0f)
-            {
-                det = 1.0f / det;
-            }
-
-            return new Vector2(det * (a22 * b.X - a12 * b.Y), det * (a11 * b.Y - a21 * b.X));
-        }
-
-        public static void Add(ref Mat22 A, ref Mat22 B, out Mat22 R)
-        {
-            R.Col1 = A.Col1 + B.Col1;
-            R.Col2 = A.Col2 + B.Col2;
-        }
-    }
-
-    /// <summary>
-    /// A 3-by-3 matrix. Stored in column-major order.
-    /// </summary>
-    public struct Mat33
-    {
-        public Vector3 Col1, Col2, Col3;
-
-        /// <summary>
-        /// Construct this matrix using columns.
-        /// </summary>
-        /// <param name="c1">The c1.</param>
-        /// <param name="c2">The c2.</param>
-        /// <param name="c3">The c3.</param>
-        public Mat33(Vector3 c1, Vector3 c2, Vector3 c3)
-        {
-            Col1 = c1;
-            Col2 = c2;
-            Col3 = c3;
-        }
-
-        /// <summary>
-        /// Set this matrix to all zeros.
-        /// </summary>
-        public void SetZero()
-        {
-            Col1 = Vector3.Zero;
-            Col2 = Vector3.Zero;
-            Col3 = Vector3.Zero;
-        }
-
-        /// <summary>
-        /// Solve A * x = b, where b is a column vector. This is more efficient
-        /// than computing the inverse in one-shot cases.
-        /// </summary>
-        /// <param name="b">The b.</param>
-        /// <returns></returns>
-        public Vector3 Solve33(Vector3 b)
-        {
-            float det = Vector3.Dot(Col1, Vector3.Cross(Col2, Col3));
-            if (det != 0.0f)
-            {
-                det = 1.0f / det;
-            }
-
-            return new Vector3(det * Vector3.Dot(b, Vector3.Cross(Col2, Col3)),
-                               det * Vector3.Dot(Col1, Vector3.Cross(b, Col3)),
-                               det * Vector3.Dot(Col1, Vector3.Cross(Col2, b)));
-        }
-
-        /// <summary>
-        /// Solve A * x = b, where b is a column vector. This is more efficient
-        /// than computing the inverse in one-shot cases. Solve only the upper
-        /// 2-by-2 matrix equation.
-        /// </summary>
-        /// <param name="b">The b.</param>
-        /// <returns></returns>
-        public Vector2 Solve22(Vector2 b)
-        {
-            float a11 = Col1.X, a12 = Col2.X, a21 = Col1.Y, a22 = Col2.Y;
-            float det = a11 * a22 - a12 * a21;
-
-            if (det != 0.0f)
-            {
-                det = 1.0f / det;
-            }
-
-            return new Vector2(det * (a22 * b.X - a12 * b.Y), det * (a11 * b.Y - a21 * b.X));
-        }
-    }
-
-    /// <summary>
-    /// A transform contains translation and rotation. It is used to represent
-    /// the position and orientation of rigid frames.
-    /// </summary>
-    public struct Transform
-    {
-        public Vector2 Position;
-        public Mat22 R;
-
-        /// <summary>
-        /// Initialize using a position vector and a rotation matrix.
-        /// </summary>
-        /// <param name="position">The position.</param>
-        /// <param name="r">The r.</param>
-        public Transform(ref Vector2 position, ref Mat22 r)
-        {
-            Position = position;
-            R = r;
-        }
-
-        /// <summary>
-        /// Calculate the angle that the rotation matrix represents.
-        /// </summary>
-        /// <value></value>
-        public float Angle
-        {
-            get { return (float)Math.Atan2(R.Col1.Y, R.Col1.X); }
-        }
-
-        /// <summary>
-        /// Set this to the identity transform.
-        /// </summary>
-        public void SetIdentity()
-        {
-            Position = Vector2.Zero;
-            R.SetIdentity();
-        }
-
-        /// <summary>
-        /// Set this based on the position and angle.
-        /// </summary>
-        /// <param name="position">The position.</param>
-        /// <param name="angle">The angle.</param>
-        public void Set(Vector2 position, float angle)
-        {
-            Position = position;
-            R.Set(angle);
-        }
-    }
-
-    /// <summary>
-    /// This describes the motion of a body/shape for TOI computation.
-    /// Shapes are defined with respect to the body origin, which may
-    /// no coincide with the center of mass. However, to support dynamics
-    /// we must interpolate the center of mass position.
-    /// </summary>
-    public struct Sweep
-    {
-        /// <summary>
-        /// World angles
-        /// </summary>
-        public float A;
-
-        public float A0;
-
-        /// <summary>
-        /// Fraction of the current time step in the range [0,1]
-        /// c0 and a0 are the positions at alpha0.
-        /// </summary>
-        public float Alpha0;
-
-        /// <summary>
-        /// Center world positions
-        /// </summary>
-        public Vector2 C;
-
-        public Vector2 C0;
-
-        /// <summary>
-        /// Local center of mass position
-        /// </summary>
-        public Vector2 LocalCenter;
-
-        /// <summary>
-        /// Get the interpolated transform at a specific time.
-        /// </summary>
-        /// <param name="xf">The transform.</param>
-        /// <param name="beta">beta is a factor in [0,1], where 0 indicates alpha0.</param>
-        public void GetTransform(out Transform xf, float beta)
-        {
-            xf = new Transform();
-            xf.Position.X = (1.0f - beta) * C0.X + beta * C.X;
-            xf.Position.Y = (1.0f - beta) * C0.Y + beta * C.Y;
-            float angle = (1.0f - beta) * A0 + beta * A;
-            xf.R.Set(angle);
-
-            // Shift to origin
-            xf.Position -= MathUtils.Multiply(ref xf.R, ref LocalCenter);
-        }
-
-        /// <summary>
-        /// Advance the sweep forward, yielding a new initial state.
-        /// </summary>
-        /// <param name="alpha">new initial time..</param>
-        public void Advance(float alpha)
-        {
-            Debug.Assert(Alpha0 < 1.0f);
-            float beta = (alpha - Alpha0) / (1.0f - Alpha0);
-            C0.X = (1.0f - beta) * C0.X + beta * C.X;
-            C0.Y = (1.0f - beta) * C0.Y + beta * C.Y;
-            A0 = (1.0f - beta) * A0 + beta * A;
-            Alpha0 = alpha;
-        }
-
-        /// <summary>
-        /// Normalize the angles.
-        /// </summary>
-        public void Normalize()
-        {
-            float d = MathHelper.TwoPi * (float)Math.Floor(A0 / MathHelper.TwoPi);
-            A0 -= d;
-            A -= d;
-        }
-    }
-}

FarseerPhysicsEngine/Common/Path.cs

@@ -1,341 +0,0 @@
-using System;
-using System.Collections.Generic;
-using System.Text;
-using System.Xml.Serialization;
-using Microsoft.Xna.Framework;
-
-namespace FarseerPhysics.Common
-{
-    //Contributed by Matthew Bettcher
-
-    /// <summary>
-    /// Path:
-    /// Very similar to Vertices, but this
-    /// class contains vectors describing
-    /// control points on a Catmull-Rom
-    /// curve.
-    /// </summary>
-    [XmlRoot("Path")]
-    public class Path
-    {
-        /// <summary>
-        /// All the points that makes up the curve
-        /// </summary>
-        [XmlElement("ControlPoints")]
-        public List<Vector2> ControlPoints;
-
-        private float _deltaT;
-
-        /// <summary>
-        /// Initializes a new instance of the <see cref="Path"/> class.
-        /// </summary>
-        public Path()
-        {
-            ControlPoints = new List<Vector2>();
-        }
-
-        /// <summary>
-        /// Initializes a new instance of the <see cref="Path"/> class.
-        /// </summary>
-        /// <param name="vertices">The vertices to created the path from.</param>
-        public Path(Vector2[] vertices)
-        {
-            ControlPoints = new List<Vector2>(vertices.Length);
-
-            for (int i = 0; i < vertices.Length; i++)
-            {
-                Add(vertices[i]);
-            }
-        }
-
-        /// <summary>
-        /// Initializes a new instance of the <see cref="Path"/> class.
-        /// </summary>
-        /// <param name="vertices">The vertices to created the path from.</param>
-        public Path(IList<Vector2> vertices)
-        {
-            ControlPoints = new List<Vector2>(vertices.Count);
-            for (int i = 0; i < vertices.Count; i++)
-            {
-                Add(vertices[i]);
-            }
-        }
-
-        /// <summary>
-        /// True if the curve is closed.
-        /// </summary>
-        /// <value><c>true</c> if closed; otherwise, <c>false</c>.</value>
-        [XmlElement("Closed")]
-        public bool Closed { get; set; }
-
-        /// <summary>
-        /// Gets the next index of a controlpoint
-        /// </summary>
-        /// <param name="index">The index.</param>
-        /// <returns></returns>
-        public int NextIndex(int index)
-        {
-            if (index == ControlPoints.Count - 1)
-            {
-                return 0;
-            }
-            return index + 1;
-        }
-
-        /// <summary>
-        /// Gets the previous index of a controlpoint
-        /// </summary>
-        /// <param name="index">The index.</param>
-        /// <returns></returns>
-        public int PreviousIndex(int index)
-        {
-            if (index == 0)
-            {
-                return ControlPoints.Count - 1;
-            }
-            return index - 1;
-        }
-
-        /// <summary>
-        /// Translates the control points by the specified vector.
-        /// </summary>
-        /// <param name="vector">The vector.</param>
-        public void Translate(ref Vector2 vector)
-        {
-            for (int i = 0; i < ControlPoints.Count; i++)
-                ControlPoints[i] = Vector2.Add(ControlPoints[i], vector);
-        }
-
-        /// <summary>
-        /// Scales the control points by the specified vector.
-        /// </summary>
-        /// <param name="value">The Value.</param>
-        public void Scale(ref Vector2 value)
-        {
-            for (int i = 0; i < ControlPoints.Count; i++)
-                ControlPoints[i] = Vector2.Multiply(ControlPoints[i], value);
-        }
-
-        /// <summary>
-        /// Rotate the control points by the defined value in radians.
-        /// </summary>
-        /// <param name="value">The amount to rotate by in radians.</param>
-        public void Rotate(float value)
-        {
-            Matrix rotationMatrix;
-            Matrix.CreateRotationZ(value, out rotationMatrix);
-
-            for (int i = 0; i < ControlPoints.Count; i++)
-                ControlPoints[i] = Vector2.Transform(ControlPoints[i], rotationMatrix);
-        }
-
-        public override string ToString()
-        {
-            StringBuilder builder = new StringBuilder();
-            for (int i = 0; i < ControlPoints.Count; i++)
-            {
-                builder.Append(ControlPoints[i].ToString());
-                if (i < ControlPoints.Count - 1)
-                {
-                    builder.Append(" ");
-                }
-            }
-            return builder.ToString();
-        }
-
-        /// <summary>
-        /// Returns a set of points defining the
-        /// curve with the specifed number of divisions
-        /// between each control point.
-        /// </summary>
-        /// <param name="divisions">Number of divisions between each control point.</param>
-        /// <returns></returns>
-        public Vertices GetVertices(int divisions)
-        {
-            Vertices verts = new Vertices();
-
-            float timeStep = 1f / divisions;
-
-            for (float i = 0; i < 1f; i += timeStep)
-            {
-                verts.Add(GetPosition(i));
-            }
-
-            return verts;
-        }
-
-        public Vector2 GetPosition(float time)
-        {
-            Vector2 temp;
-
-            if (ControlPoints.Count < 2)
-                throw new Exception("You need at least 2 control points to calculate a position.");
-
-            if (Closed)
-            {
-                Add(ControlPoints[0]);
-
-                _deltaT = 1f / (ControlPoints.Count - 1);
-
-                int p = (int)(time / _deltaT);
-
-                // use a circular indexing system
-                int p0 = p - 1;
-                if (p0 < 0) p0 = p0 + (ControlPoints.Count - 1);
-                else if (p0 >= ControlPoints.Count - 1) p0 = p0 - (ControlPoints.Count - 1);
-                int p1 = p;
-                if (p1 < 0) p1 = p1 + (ControlPoints.Count - 1);
-                else if (p1 >= ControlPoints.Count - 1) p1 = p1 - (ControlPoints.Count - 1);
-                int p2 = p + 1;
-                if (p2 < 0) p2 = p2 + (ControlPoints.Count - 1);
-                else if (p2 >= ControlPoints.Count - 1) p2 = p2 - (ControlPoints.Count - 1);
-                int p3 = p + 2;
-                if (p3 < 0) p3 = p3 + (ControlPoints.Count - 1);
-                else if (p3 >= ControlPoints.Count - 1) p3 = p3 - (ControlPoints.Count - 1);
-
-                // relative time
-                float lt = (time - _deltaT * p) / _deltaT;
-
-                temp = Vector2.CatmullRom(ControlPoints[p0], ControlPoints[p1], ControlPoints[p2], ControlPoints[p3], lt);
-
-                RemoveAt(ControlPoints.Count - 1);
-            }
-            else
-            {
-                int p = (int)(time / _deltaT);
-
-                // 
-                int p0 = p - 1;
-                if (p0 < 0) p0 = 0;
-                else if (p0 >= ControlPoints.Count - 1) p0 = ControlPoints.Count - 1;
-                int p1 = p;
-                if (p1 < 0) p1 = 0;
-                else if (p1 >= ControlPoints.Count - 1) p1 = ControlPoints.Count - 1;
-                int p2 = p + 1;
-                if (p2 < 0) p2 = 0;
-                else if (p2 >= ControlPoints.Count - 1) p2 = ControlPoints.Count - 1;
-                int p3 = p + 2;
-                if (p3 < 0) p3 = 0;
-                else if (p3 >= ControlPoints.Count - 1) p3 = ControlPoints.Count - 1;
-
-                // relative time
-                float lt = (time - _deltaT * p) / _deltaT;
-
-                temp = Vector2.CatmullRom(ControlPoints[p0], ControlPoints[p1], ControlPoints[p2], ControlPoints[p3], lt);
-            }
-
-            return temp;
-        }
-
-        /// <summary>
-        /// Gets the normal for the given time.
-        /// </summary>
-        /// <param name="time">The time</param>
-        /// <returns>The normal.</returns>
-        public Vector2 GetPositionNormal(float time)
-        {
-            float offsetTime = time + 0.0001f;
-
-            Vector2 a = GetPosition(time);
-            Vector2 b = GetPosition(offsetTime);
-
-            Vector2 output, temp;
-
-            Vector2.Subtract(ref a, ref b, out temp);
-
-#if (XBOX360 || WINDOWS_PHONE)
-output = new Vector2();
-#endif
-            output.X = -temp.Y;
-            output.Y = temp.X;
-
-            Vector2.Normalize(ref output, out output);
-
-            return output;
-        }
-
-        public void Add(Vector2 point)
-        {
-            ControlPoints.Add(point);
-            _deltaT = 1f / (ControlPoints.Count - 1);
-        }
-
-        public void Remove(Vector2 point)
-        {
-            ControlPoints.Remove(point);
-            _deltaT = 1f / (ControlPoints.Count - 1);
-        }
-
-        public void RemoveAt(int index)
-        {
-            ControlPoints.RemoveAt(index);
-            _deltaT = 1f / (ControlPoints.Count - 1);
-        }
-
-        public float GetLength()
-        {
-            List<Vector2> verts = GetVertices(ControlPoints.Count * 25);
-            float length = 0;
-
-            for (int i = 1; i < verts.Count; i++)
-            {
-                length += Vector2.Distance(verts[i - 1], verts[i]);
-            }
-
-            if (Closed)
-                length += Vector2.Distance(verts[ControlPoints.Count - 1], verts[0]);
-
-            return length;
-        }
-
-        public List<Vector3> SubdivideEvenly(int divisions)
-        {
-            List<Vector3> verts = new List<Vector3>();
-
-            float length = GetLength();
-
-            float deltaLength = length / divisions + 0.001f;
-            float t = 0.000f;
-
-            // we always start at the first control point
-            Vector2 start = ControlPoints[0];
-            Vector2 end = GetPosition(t);
-
-            // increment t until we are at half the distance
-            while (deltaLength * 0.5f >= Vector2.Distance(start, end))
-            {
-                end = GetPosition(t);
-                t += 0.0001f;
-
-                if (t >= 1f)
-                    break;
-            }
-
-            start = end;
-
-            // for each box
-            for (int i = 1; i < divisions; i++)
-            {
-                Vector2 normal = GetPositionNormal(t);
-                float angle = (float)Math.Atan2(normal.Y, normal.X);
-
-                verts.Add(new Vector3(end, angle));
-
-                // until we reach the correct distance down the curve
-                while (deltaLength >= Vector2.Distance(start, end))
-                {
-                    end = GetPosition(t);
-                    t += 0.00001f;
-
-                    if (t >= 1f)
-                        break;
-                }
-                if (t >= 1f)
-                    break;
-
-                start = end;
-            }
-            return verts;
-        }
-    }
-}

FarseerPhysicsEngine/Common/PathManager.cs

@@ -1,240 +0,0 @@
-using System;
-using System.Collections.Generic;
-using FarseerPhysics.Collision.Shapes;
-using FarseerPhysics.Common;
-using FarseerPhysics.Common.Decomposition;
-using FarseerPhysics.Dynamics;
-using FarseerPhysics.Dynamics.Joints;
-using Microsoft.Xna.Framework;
-
-namespace FarseerPhysics.Factories
-{
-    /// <summary>
-    /// An easy to use manager for creating paths.
-    /// </summary>
-    public static class PathManager
-    {
-        #region LinkType enum
-
-        public enum LinkType
-        {
-            Revolute,
-            Slider
-        }
-
-        #endregion
-
-        //Contributed by Matthew Bettcher
-
-        /// <summary>
-        /// Convert a path into a set of edges and attaches them to the specified body.
-        /// Note: use only for static edges.
-        /// </summary>
-        /// <param name="path">The path.</param>
-        /// <param name="body">The body.</param>
-        /// <param name="subdivisions">The subdivisions.</param>
-        public static void ConvertPathToEdges(Path path, Body body, int subdivisions)
-        {
-            Vertices verts = path.GetVertices(subdivisions);
-
-            if (path.Closed)
-            {
-                LoopShape loop = new LoopShape(verts);
-                body.CreateFixture(loop);
-            }
-            else
-            {
-                for (int i = 1; i < verts.Count; i++)
-                {
-                    body.CreateFixture(new EdgeShape(verts[i], verts[i - 1]));
-                }
-            }
-        }
-
-        /// <summary>
-        /// Convert a closed path into a polygon.
-        /// Convex decomposition is automatically performed.
-        /// </summary>
-        /// <param name="path">The path.</param>
-        /// <param name="body">The body.</param>
-        /// <param name="density">The density.</param>
-        /// <param name="subdivisions">The subdivisions.</param>
-        public static void ConvertPathToPolygon(Path path, Body body, float density, int subdivisions)
-        {
-            if (!path.Closed)
-                throw new Exception("The path must be closed to convert to a polygon.");
-
-            List<Vector2> verts = path.GetVertices(subdivisions);
-
-            List<Vertices> decomposedVerts = EarclipDecomposer.ConvexPartition(new Vertices(verts));
-            //List<Vertices> decomposedVerts = BayazitDecomposer.ConvexPartition(new Vertices(verts));
-
-            foreach (Vertices item in decomposedVerts)
-            {
-                body.CreateFixture(new PolygonShape(item, density));
-            }
-        }
-
-        /// <summary>
-        /// Duplicates the given Body along the given path for approximatly the given copies.
-        /// </summary>
-        /// <param name="world">The world.</param>
-        /// <param name="path">The path.</param>
-        /// <param name="shapes">The shapes.</param>
-        /// <param name="type">The type.</param>
-        /// <param name="copies">The copies.</param>
-        /// <param name="userData"></param>
-        /// <returns></returns>
-        public static List<Body> EvenlyDistributeShapesAlongPath(World world, Path path, IEnumerable<Shape> shapes,
-                                                                 BodyType type, int copies, object userData)
-        {
-            List<Vector3> centers = path.SubdivideEvenly(copies);
-            List<Body> bodyList = new List<Body>();
-
-            for (int i = 0; i < centers.Count; i++)
-            {
-                Body b = new Body(world);
-
-                // copy the type from original body
-                b.BodyType = type;
-                b.Position = new Vector2(centers[i].X, centers[i].Y);
-                b.Rotation = centers[i].Z;
-
-                foreach (Shape shape in shapes)
-                {
-                    b.CreateFixture(shape, userData);
-                }
-
-                bodyList.Add(b);
-            }
-
-            return bodyList;
-        }
-
-        public static List<Body> EvenlyDistributeShapesAlongPath(World world, Path path, IEnumerable<Shape> shapes,
-                                                                 BodyType type, int copies)
-        {
-            return EvenlyDistributeShapesAlongPath(world, path, shapes, type, copies, null);
-        }
-
-
-        /// <summary>
-        /// Duplicates the given Body along the given path for approximatly the given copies.
-        /// </summary>
-        /// <param name="world">The world.</param>
-        /// <param name="path">The path.</param>
-        /// <param name="shape">The shape.</param>
-        /// <param name="type">The type.</param>
-        /// <param name="copies">The copies.</param>
-        /// <param name="userData">The user data.</param>
-        /// <returns></returns>
-        public static List<Body> EvenlyDistributeShapesAlongPath(World world, Path path, Shape shape, BodyType type,
-                                                                 int copies, object userData)
-        {
-            List<Shape> shapes = new List<Shape>(1);
-            shapes.Add(shape);
-
-            return EvenlyDistributeShapesAlongPath(world, path, shapes, type, copies, userData);
-        }
-
-        public static List<Body> EvenlyDistributeShapesAlongPath(World world, Path path, Shape shape, BodyType type,
-                                                                 int copies)
-        {
-            return EvenlyDistributeShapesAlongPath(world, path, shape, type, copies, null);
-        }
-
-        //TODO: Comment better
-        /// <summary>
-        /// Moves the body on the path.
-        /// </summary>
-        /// <param name="path">The path.</param>
-        /// <param name="body">The body.</param>
-        /// <param name="time">The time.</param>
-        /// <param name="strength">The strength.</param>
-        /// <param name="timeStep">The time step.</param>
-        public static void MoveBodyOnPath(Path path, Body body, float time, float strength, float timeStep)
-        {
-            Vector2 destination = path.GetPosition(time);
-            Vector2 positionDelta = body.Position - destination;
-            Vector2 velocity = (positionDelta / timeStep) * strength;
-
-            body.LinearVelocity = -velocity;
-        }
-
-        /// <summary>
-        /// Attaches the bodies with revolute joints.
-        /// </summary>
-        /// <param name="world">The world.</param>
-        /// <param name="bodies">The bodies.</param>
-        /// <param name="localAnchorA">The local anchor A.</param>
-        /// <param name="localAnchorB">The local anchor B.</param>
-        /// <param name="connectFirstAndLast">if set to <c>true</c> [connect first and last].</param>
-        /// <param name="collideConnected">if set to <c>true</c> [collide connected].</param>
-        public static List<RevoluteJoint> AttachBodiesWithRevoluteJoint(World world, List<Body> bodies,
-                                                                        Vector2 localAnchorA,
-                                                                        Vector2 localAnchorB, bool connectFirstAndLast,
-                                                                        bool collideConnected)
-        {
-            List<RevoluteJoint> joints = new List<RevoluteJoint>(bodies.Count + 1);
-
-            for (int i = 1; i < bodies.Count; i++)
-            {
-                RevoluteJoint joint = new RevoluteJoint(bodies[i], bodies[i - 1], localAnchorA, localAnchorB);
-                joint.CollideConnected = collideConnected;
-                world.AddJoint(joint);
-                joints.Add(joint);
-            }
-
-            if (connectFirstAndLast)
-            {
-                RevoluteJoint lastjoint = new RevoluteJoint(bodies[0], bodies[bodies.Count - 1], localAnchorA,
-                                                            localAnchorB);
-                lastjoint.CollideConnected = collideConnected;
-                world.AddJoint(lastjoint);
-                joints.Add(lastjoint);
-            }
-
-            return joints;
-        }
-
-        /// <summary>
-        /// Attaches the bodies with revolute joints.
-        /// </summary>
-        /// <param name="world">The world.</param>
-        /// <param name="bodies">The bodies.</param>
-        /// <param name="localAnchorA">The local anchor A.</param>
-        /// <param name="localAnchorB">The local anchor B.</param>
-        /// <param name="connectFirstAndLast">if set to <c>true</c> [connect first and last].</param>
-        /// <param name="collideConnected">if set to <c>true</c> [collide connected].</param>
-        /// <param name="minLength">Minimum length of the slider joint.</param>
-        /// <param name="maxLength">Maximum length of the slider joint.</param>
-        /// <returns></returns>
-        public static List<SliderJoint> AttachBodiesWithSliderJoint(World world, List<Body> bodies, Vector2 localAnchorA,
-                                                                    Vector2 localAnchorB, bool connectFirstAndLast,
-                                                                    bool collideConnected, float minLength,
-                                                                    float maxLength)
-        {
-            List<SliderJoint> joints = new List<SliderJoint>(bodies.Count + 1);
-
-            for (int i = 1; i < bodies.Count; i++)
-            {
-                SliderJoint joint = new SliderJoint(bodies[i], bodies[i - 1], localAnchorA, localAnchorB, minLength,
-                                                    maxLength);
-                joint.CollideConnected = collideConnected;
-                world.AddJoint(joint);
-                joints.Add(joint);
-            }
-
-            if (connectFirstAndLast)
-            {
-                SliderJoint lastjoint = new SliderJoint(bodies[0], bodies[bodies.Count - 1], localAnchorA, localAnchorB,
-                                                        minLength, maxLength);
-                lastjoint.CollideConnected = collideConnected;
-                world.AddJoint(lastjoint);
-                joints.Add(lastjoint);
-            }
-
-            return joints;
-        }
-    }
-}

FarseerPhysicsEngine/Common/PhysicsLogic/Explosion.cs

@@ -1,464 +0,0 @@
-using System;
-using System.Collections.Generic;
-using System.Linq;
-using FarseerPhysics.Collision;
-using FarseerPhysics.Collision.Shapes;
-using FarseerPhysics.Dynamics;
-using Microsoft.Xna.Framework;
-
-namespace FarseerPhysics.Common.PhysicsLogic
-{
-    internal struct ShapeData
-    {
-        public Body Body;
-        public float Max;
-        public float Min; // absolute angles
-    }
-
-    /// <summary>
-    /// This is a comprarer used for 
-    /// detecting angle difference between rays
-    /// </summary>
-    internal class RayDataComparer : IComparer<float>
-    {
-        #region IComparer<float> Members
-
-        int IComparer<float>.Compare(float a, float b)
-        {
-            float diff = (a - b);
-            if (diff > 0)
-                return 1;
-            if (diff < 0)
-                return -1;
-            return 0;
-        }
-
-        #endregion
-    }
-
-    /* Methodology:
-     * Force applied at a ray is inversely proportional to the square of distance from source
-     * AABB is used to query for shapes that may be affected
-     * For each RIGID BODY (not shape -- this is an optimization) that is matched, loop through its vertices to determine
-     *		the extreme points -- if there is structure that contains outlining polygon, use that as an additional optimization
-     * Evenly cast a number of rays against the shape - number roughly proportional to the arc coverage
-     *		-Something like every 3 degrees should do the trick although this can be altered depending on the distance (if really close don't need such a high density of rays)
-     *		-There should be a minimum number of rays (3-5?) applied to each body so that small bodies far away are still accurately modeled
-     *		-Be sure to have the forces of each ray be proportional to the average arc length covered by each.
-     * For each ray that actually intersects with the shape (non intersections indicate something blocking the path of explosion):
-     *		> apply the appropriate force dotted with the negative of the collision normal at the collision point
-     *		> optionally apply linear interpolation between aforementioned Normal force and the original explosion force in the direction of ray to simulate "surface friction" of sorts
-     */
-
-    /// <summary>
-    /// This is an explosive... it explodes.
-    /// </summary>
-    /// <remarks>
-    /// Original Code by Steven Lu - see http://www.box2d.org/forum/viewtopic.php?f=3&t=1688
-    /// Ported to Farseer 3.0 by Nicolás Hormazábal
-    /// </remarks>
-    public sealed class Explosion : PhysicsLogic
-    {
-        /// <summary>
-        /// Two degrees: maximum angle from edges to first ray tested
-        /// </summary>
-        private const float MaxEdgeOffset = MathHelper.Pi / 90;
-
-        /// <summary>
-        /// Ratio of arc length to angle from edges to first ray tested.
-        /// Defaults to 1/40.
-        /// </summary>
-        public float EdgeRatio = 1.0f / 40.0f;
-
-        /// <summary>
-        /// Ignore Explosion if it happens inside a shape.
-        /// Default value is false.
-        /// </summary>
-        public bool IgnoreWhenInsideShape = false;
-
-        /// <summary>
-        /// Max angle between rays (used when segment is large).
-        /// Defaults to 15 degrees
-        /// </summary>
-        public float MaxAngle = MathHelper.Pi / 15;
-
-        /// <summary>
-        /// Maximum number of shapes involved in the explosion.
-        /// Defaults to 100
-        /// </summary>
-        public int MaxShapes = 100;
-
-        /// <summary>
-        /// How many rays per shape/body/segment.
-        /// Defaults to 5
-        /// </summary>
-        public int MinRays = 5;
-
-        private List<ShapeData> _data = new List<ShapeData>();
-        private Dictionary<Fixture, List<Vector2>> _exploded;
-        private RayDataComparer _rdc;
-
-        public Explosion(World world)
-            : base(world, PhysicsLogicType.Explosion)
-        {
-            _exploded = new Dictionary<Fixture, List<Vector2>>();
-            _rdc = new RayDataComparer();
-            _data = new List<ShapeData>();
-        }
-
-        /// <summary>
-        /// This makes the explosive explode
-        /// </summary>
-        /// <param name="pos">
-        /// The position where the explosion happens
-        /// </param>
-        /// <param name="radius">
-        /// The explosion radius
-        /// </param>
-        /// <param name="maxForce">
-        /// The explosion force at the explosion point
-        /// (then is inversely proportional to the square of the distance)
-        /// </param>
-        /// <returns>
-        /// A dictionnary containing all the "exploded" fixtures
-        /// with a list of the applied impulses
-        /// </returns>
-        public Dictionary<Fixture, List<Vector2>> Activate(Vector2 pos, float radius, float maxForce)
-        {
-            _exploded.Clear();
-
-            AABB aabb;
-            aabb.LowerBound = pos + new Vector2(-radius, -radius);
-            aabb.UpperBound = pos + new Vector2(radius, radius);
-            Fixture[] shapes = new Fixture[MaxShapes];
-
-            // More than 5 shapes in an explosion could be possible, but still strange.
-            Fixture[] containedShapes = new Fixture[5];
-            bool exit = false;
-
-            int shapeCount = 0;
-            int containedShapeCount = 0;
-
-            // Query the world for overlapping shapes.
-            World.QueryAABB(
-                fixture =>
-                {
-                    if (fixture.TestPoint(ref pos))
-                    {
-                        if (IgnoreWhenInsideShape)
-                            exit = true;
-                        else
-                            containedShapes[containedShapeCount++] = fixture;
-                    }
-                    else
-                    {
-                        shapes[shapeCount++] = fixture;
-                    }
-
-                    // Continue the query.
-                    return true;
-                }, ref aabb);
-
-            if (exit)
-            {
-                return _exploded;
-            }
-
-            // Per shape max/min angles for now.
-            float[] vals = new float[shapeCount * 2];
-            int valIndex = 0;
-            for (int i = 0; i < shapeCount; ++i)
-            {
-                PolygonShape ps;
-                CircleShape cs = shapes[i].Shape as CircleShape;
-                if (cs != null)
-                {
-                    // We create a "diamond" approximation of the circle
-                    Vertices v = new Vertices();
-                    Vector2 vec = Vector2.Zero + new Vector2(cs.Radius, 0);
-                    v.Add(vec);
-                    vec = Vector2.Zero + new Vector2(0, cs.Radius);
-                    v.Add(vec);
-                    vec = Vector2.Zero + new Vector2(-cs.Radius, cs.Radius);
-                    v.Add(vec);
-                    vec = Vector2.Zero + new Vector2(0, -cs.Radius);
-                    v.Add(vec);
-                    ps = new PolygonShape(v, 0);
-                }
-                else
-                    ps = shapes[i].Shape as PolygonShape;
-
-                if ((shapes[i].Body.BodyType == BodyType.Dynamic) && ps != null)
-                {
-                    Vector2 toCentroid = shapes[i].Body.GetWorldPoint(ps.MassData.Centroid) - pos;
-                    float angleToCentroid = (float)Math.Atan2(toCentroid.Y, toCentroid.X);
-                    float min = float.MaxValue;
-                    float max = float.MinValue;
-                    float minAbsolute = 0.0f;
-                    float maxAbsolute = 0.0f;
-
-                    for (int j = 0; j < (ps.Vertices.Count()); ++j)
-                    {
-                        Vector2 toVertex = (shapes[i].Body.GetWorldPoint(ps.Vertices[j]) - pos);
-                        float newAngle = (float)Math.Atan2(toVertex.Y, toVertex.X);
-                        float diff = (newAngle - angleToCentroid);
-
-                        diff = (diff - MathHelper.Pi) % (2 * MathHelper.Pi);
-                        // the minus pi is important. It means cutoff for going other direction is at 180 deg where it needs to be
-
-                        if (diff < 0.0f)
-                            diff += 2 * MathHelper.Pi; // correction for not handling negs
-
-                        diff -= MathHelper.Pi;
-
-                        if (Math.Abs(diff) > MathHelper.Pi)
-                            throw new ArgumentException("OMG!");
-                        // Something's wrong, point not in shape but exists angle diff > 180
-
-                        if (diff > max)
-                        {
-                            max = diff;
-                            maxAbsolute = newAngle;
-                        }
-                        if (diff < min)
-                        {
-                            min = diff;
-                            minAbsolute = newAngle;
-                        }
-                    }
-
-                    vals[valIndex] = minAbsolute;
-                    ++valIndex;
-                    vals[valIndex] = maxAbsolute;
-                    ++valIndex;
-                }
-            }
-
-            Array.Sort(vals, 0, valIndex, _rdc);
-            _data.Clear();
-            bool rayMissed = true;
-
-            for (int i = 0; i < valIndex; ++i)
-            {
-                Fixture shape = null;
-                float midpt;
-
-                int iplus = (i == valIndex - 1 ? 0 : i + 1);
-                if (vals[i] == vals[iplus])
-                    continue;
-
-                if (i == valIndex - 1)
-                {
-                    // the single edgecase
-                    midpt = (vals[0] + MathHelper.Pi * 2 + vals[i]);
-                }
-                else
-                {
-                    midpt = (vals[i + 1] + vals[i]);
-                }
-
-                midpt = midpt / 2;
-
-                Vector2 p1 = pos;
-                Vector2 p2 = radius * new Vector2((float)Math.Cos(midpt),
-                                                (float)Math.Sin(midpt)) + pos;
-
-                // RaycastOne
-                bool hitClosest = false;
-                World.RayCast((f, p, n, fr) =>
-                                  {
-                                      Body body = f.Body;
-
-                                      if (!IsActiveOn(body))
-                                          return 0;
-
-                                      if (body.UserData != null)
-                                      {
-                                          int index = (int)body.UserData;
-                                          if (index == 0)
-                                          {
-                                              // filter
-                                              return -1.0f;
-                                          }
-                                      }
-
-                                      hitClosest = true;
-                                      shape = f;
-                                      return fr;
-                                  }, p1, p2);
-
-                //draws radius points
-                if ((hitClosest) && (shape.Body.BodyType == BodyType.Dynamic))
-                {
-                    if ((_data.Count() > 0) && (_data.Last().Body == shape.Body) && (!rayMissed))
-                    {
-                        int laPos = _data.Count - 1;
-                        ShapeData la = _data[laPos];
-                        la.Max = vals[iplus];
-                        _data[laPos] = la;
-                    }
-                    else
-                    {
-                        // make new
-                        ShapeData d;
-                        d.Body = shape.Body;
-                        d.Min = vals[i];
-                        d.Max = vals[iplus];
-                        _data.Add(d);
-                    }
-
-                    if ((_data.Count() > 1)
-                        && (i == valIndex - 1)
-                        && (_data.Last().Body == _data.First().Body)
-                        && (_data.Last().Max == _data.First().Min))
-                    {
-                        ShapeData fi = _data[0];
-                        fi.Min = _data.Last().Min;
-                        _data.RemoveAt(_data.Count() - 1);
-                        _data[0] = fi;
-                        while (_data.First().Min >= _data.First().Max)
-                        {
-                            fi.Min -= MathHelper.Pi * 2;
-                            _data[0] = fi;
-                        }
-                    }
-
-                    int lastPos = _data.Count - 1;
-                    ShapeData last = _data[lastPos];
-                    while ((_data.Count() > 0)
-                           && (_data.Last().Min >= _data.Last().Max)) // just making sure min<max
-                    {
-                        last.Min = _data.Last().Min - 2 * MathHelper.Pi;
-                        _data[lastPos] = last;
-                    }
-                    rayMissed = false;
-                }
-                else
-                {
-                    rayMissed = true; // raycast did not find a shape
-                }
-            }
-
-            for (int i = 0; i < _data.Count(); ++i)
-            {
-                if (!IsActiveOn(_data[i].Body))
-                    continue;
-
-                float arclen = _data[i].Max - _data[i].Min;
-
-                float first = MathHelper.Min(MaxEdgeOffset, EdgeRatio * arclen);
-                int insertedRays = (int)Math.Ceiling(((arclen - 2.0f * first) - (MinRays - 1) * MaxAngle) / MaxAngle);
-
-                if (insertedRays < 0)
-                    insertedRays = 0;
-
-                float offset = (arclen - first * 2.0f) / ((float)MinRays + insertedRays - 1);
-
-                //Note: This loop can go into infinite as it operates on floats.
-                //Added FloatEquals with a large epsilon.
-                for (float j = _data[i].Min + first;
-                     j < _data[i].Max || MathUtils.FloatEquals(j, _data[i].Max, 0.0001f);
-                     j += offset)
-                {
-                    Vector2 p1 = pos;
-                    Vector2 p2 = pos + radius * new Vector2((float)Math.Cos(j), (float)Math.Sin(j));
-                    Vector2 hitpoint = Vector2.Zero;
-                    float minlambda = float.MaxValue;
-
-                    List<Fixture> fl = _data[i].Body.FixtureList;
-                    for (int x = 0; x < fl.Count; x++)
-                    {
-                        Fixture f = fl[x];
-                        RayCastInput ri;
-                        ri.Point1 = p1;
-                        ri.Point2 = p2;
-                        ri.MaxFraction = 50f;
-
-                        RayCastOutput ro;
-                        if (f.RayCast(out ro, ref ri, 0))
-                        {
-                            if (minlambda > ro.Fraction)
-                            {
-                                minlambda = ro.Fraction;
-                                hitpoint = ro.Fraction * p2 + (1 - ro.Fraction) * p1;
-                            }
-                        }
-
-                        // the force that is to be applied for this particular ray.
-                        // offset is angular coverage. lambda*length of segment is distance.
-                        float impulse = (arclen / (MinRays + insertedRays)) * maxForce * 180.0f / MathHelper.Pi *
-                                        (1.0f - Math.Min(1.0f, minlambda));
-
-                        // We Apply the impulse!!!
-                        Vector2 vectImp = Vector2.Dot(impulse * new Vector2((float)Math.Cos(j),
-                                                                          (float)Math.Sin(j)), -ro.Normal) *
-                                          new Vector2((float)Math.Cos(j),
-                                                      (float)Math.Sin(j));
-
-                        _data[i].Body.ApplyLinearImpulse(ref vectImp, ref hitpoint);
-
-                        // We gather the fixtures for returning them
-                        Vector2 val = Vector2.Zero;
-                        List<Vector2> vectorList;
-                        if (_exploded.TryGetValue(f, out vectorList))
-                        {
-                            val.X += Math.Abs(vectImp.X);
-                            val.Y += Math.Abs(vectImp.Y);
-
-                            vectorList.Add(val);
-                        }
-                        else
-                        {
-                            vectorList = new List<Vector2>();
-                            val.X = Math.Abs(vectImp.X);
-                            val.Y = Math.Abs(vectImp.Y);
-
-                            vectorList.Add(val);
-                            _exploded.Add(f, vectorList);
-                        }
-
-                        if (minlambda > 1.0f)
-                        {
-                            hitpoint = p2;
-                        }
-                    }
-                }
-            }
-
-            // We check contained shapes
-            for (int i = 0; i < containedShapeCount; ++i)
-            {
-                Fixture fix = containedShapes[i];
-
-                if (!IsActiveOn(fix.Body))
-                    continue;
-
-                float impulse = MinRays * maxForce * 180.0f / MathHelper.Pi;
-                Vector2 hitPoint;
-
-                CircleShape circShape = fix.Shape as CircleShape;
-                if (circShape != null)
-                {
-                    hitPoint = fix.Body.GetWorldPoint(circShape.Position);
-                }
-                else
-                {
-                    PolygonShape shape = fix.Shape as PolygonShape;
-                    hitPoint = fix.Body.GetWorldPoint(shape.MassData.Centroid);
-                }
-
-                Vector2 vectImp = impulse * (hitPoint - pos);
-
-                List<Vector2> vectorList = new List<Vector2>();
-                vectorList.Add(vectImp);
-
-                fix.Body.ApplyLinearImpulse(ref vectImp, ref hitPoint);
-
-                if (!_exploded.ContainsKey(fix))
-                    _exploded.Add(fix, vectorList);
-            }
-
-            return _exploded;
-        }
-    }
-}

FarseerPhysicsEngine/Common/PhysicsLogic/PhysicsLogic.cs

@@ -1,66 +0,0 @@
-using System;
-using FarseerPhysics.Dynamics;
-
-namespace FarseerPhysics.Common.PhysicsLogic
-{
-    [Flags]
-    public enum PhysicsLogicType
-    {
-        Explosion = (1 << 0)
-    }
-
-    public struct PhysicsLogicFilter
-    {
-        public PhysicsLogicType ControllerIgnores;
-
-        /// <summary>
-        /// Ignores the controller. The controller has no effect on this body.
-        /// </summary>
-        /// <param name="type">The logic type.</param>
-        public void IgnorePhysicsLogic(PhysicsLogicType type)
-        {
-            ControllerIgnores |= type;
-        }
-
-        /// <summary>
-        /// Restore the controller. The controller affects this body.
-        /// </summary>
-        /// <param name="type">The logic type.</param>
-        public void RestorePhysicsLogic(PhysicsLogicType type)
-        {
-            ControllerIgnores &= ~type;
-        }
-
-        /// <summary>
-        /// Determines whether this body ignores the the specified controller.
-        /// </summary>
-        /// <param name="type">The logic type.</param>
-        /// <returns>
-        /// 	<c>true</c> if the body has the specified flag; otherwise, <c>false</c>.
-        /// </returns>
-        public bool IsPhysicsLogicIgnored(PhysicsLogicType type)
-        {
-            return (ControllerIgnores & type) == type;
-        }
-    }
-
-    public abstract class PhysicsLogic : FilterData
-    {
-        private PhysicsLogicType _type;
-        public World World;
-
-        public override bool IsActiveOn(Body body)
-        {
-            if (body.PhysicsLogicFilter.IsPhysicsLogicIgnored(_type))
-                return false;
-
-            return base.IsActiveOn(body);
-        }
-
-        public PhysicsLogic(World world, PhysicsLogicType type)
-        {
-            _type = type;
-            World = world;
-        }
-    }
-}

FarseerPhysicsEngine/Common/PolygonManipulation/CuttingTools.cs

@@ -1,246 +0,0 @@
-using System.Collections.Generic;
-using System.Diagnostics;
-using FarseerPhysics.Collision.Shapes;
-using FarseerPhysics.Dynamics;
-using FarseerPhysics.Factories;
-using Microsoft.Xna.Framework;
-
-namespace FarseerPhysics.Common.PolygonManipulation
-{
-    public static class CuttingTools
-    {
-        //Cutting a shape into two is based on the work of Daid and his prototype BoxCutter: http://www.box2d.org/forum/viewtopic.php?f=3&t=1473
-
-        /// <summary>
-        /// Split a fixture into 2 vertice collections using the given entry and exit-point.
-        /// </summary>
-        /// <param name="fixture">The Fixture to split</param>
-        /// <param name="entryPoint">The entry point - The start point</param>
-        /// <param name="exitPoint">The exit point - The end point</param>
-        /// <param name="splitSize">The size of the split. Think of this as the laser-width</param>
-        /// <param name="first">The first collection of vertexes</param>
-        /// <param name="second">The second collection of vertexes</param>
-        public static void SplitShape(Fixture fixture, Vector2 entryPoint, Vector2 exitPoint, float splitSize,
-                                      out Vertices first, out Vertices second)
-        {
-            Vector2 localEntryPoint = fixture.Body.GetLocalPoint(ref entryPoint);
-            Vector2 localExitPoint = fixture.Body.GetLocalPoint(ref exitPoint);
-
-            PolygonShape shape = fixture.Shape as PolygonShape;
-
-            if (shape == null)
-            {
-                first = new Vertices();
-                second = new Vertices();
-                return;
-            }
-
-            Vertices vertices = new Vertices(shape.Vertices);
-            Vertices[] newPolygon = new Vertices[2];
-
-            for (int i = 0; i < newPolygon.Length; i++)
-            {
-                newPolygon[i] = new Vertices(vertices.Count);
-            }
-
-            int[] cutAdded = { -1, -1 };
-            int last = -1;
-            for (int i = 0; i < vertices.Count; i++)
-            {
-                int n;
-                //Find out if this vertex is on the old or new shape.
-                if (Vector2.Dot(MathUtils.Cross(localExitPoint - localEntryPoint, 1), vertices[i] - localEntryPoint) > Settings.Epsilon)
-                    n = 0;
-                else
-                    n = 1;
-
-                if (last != n)
-                {
-                    //If we switch from one shape to the other add the cut vertices.
-                    if (last == 0)
-                    {
-                        Debug.Assert(cutAdded[0] == -1);
-                        cutAdded[0] = newPolygon[last].Count;
-                        newPolygon[last].Add(localExitPoint);
-                        newPolygon[last].Add(localEntryPoint);
-                    }
-                    if (last == 1)
-                    {
-                        Debug.Assert(cutAdded[last] == -1);
-                        cutAdded[last] = newPolygon[last].Count;
-                        newPolygon[last].Add(localEntryPoint);
-                        newPolygon[last].Add(localExitPoint);
-                    }
-                }
-
-                newPolygon[n].Add(vertices[i]);
-                last = n;
-            }
-
-            //Add the cut in case it has not been added yet.
-            if (cutAdded[0] == -1)
-            {
-                cutAdded[0] = newPolygon[0].Count;
-                newPolygon[0].Add(localExitPoint);
-                newPolygon[0].Add(localEntryPoint);
-            }
-            if (cutAdded[1] == -1)
-            {
-                cutAdded[1] = newPolygon[1].Count;
-                newPolygon[1].Add(localEntryPoint);
-                newPolygon[1].Add(localExitPoint);
-            }
-
-            for (int n = 0; n < 2; n++)
-            {
-                Vector2 offset;
-                if (cutAdded[n] > 0)
-                {
-                    offset = (newPolygon[n][cutAdded[n] - 1] - newPolygon[n][cutAdded[n]]);
-                }
-                else
-                {
-                    offset = (newPolygon[n][newPolygon[n].Count - 1] - newPolygon[n][0]);
-                }
-                offset.Normalize();
-
-                newPolygon[n][cutAdded[n]] += splitSize * offset;
-
-                if (cutAdded[n] < newPolygon[n].Count - 2)
-                {
-                    offset = (newPolygon[n][cutAdded[n] + 2] - newPolygon[n][cutAdded[n] + 1]);
-                }
-                else
-                {
-                    offset = (newPolygon[n][0] - newPolygon[n][newPolygon[n].Count - 1]);
-                }
-                offset.Normalize();
-
-                newPolygon[n][cutAdded[n] + 1] += splitSize * offset;
-            }
-
-            first = newPolygon[0];
-            second = newPolygon[1];
-        }
-
-        /// <summary>
-        /// This is a high-level function to cuts fixtures inside the given world, using the start and end points.
-        /// Note: We don't support cutting when the start or end is inside a shape.
-        /// </summary>
-        /// <param name="world">The world.</param>
-        /// <param name="start">The startpoint.</param>
-        /// <param name="end">The endpoint.</param>
-        /// <param name="thickness">The thickness of the cut</param>
-        public static void Cut(World world, Vector2 start, Vector2 end, float thickness)
-        {
-            List<Fixture> fixtures = new List<Fixture>();
-            List<Vector2> entryPoints = new List<Vector2>();
-            List<Vector2> exitPoints = new List<Vector2>();
-
-            //We don't support cutting when the start or end is inside a shape.
-            if (world.TestPoint(start) != null || world.TestPoint(end) != null)
-                return;
-
-            //Get the entry points
-            world.RayCast((f, p, n, fr) =>
-                              {
-                                  fixtures.Add(f);
-                                  entryPoints.Add(p);
-                                  return 1;
-                              }, start, end);
-
-            //Reverse the ray to get the exitpoints
-            world.RayCast((f, p, n, fr) =>
-                              {
-                                  exitPoints.Add(p);
-                                  return 1;
-                              }, end, start);
-
-            //We only have a single point. We need at least 2
-            if (entryPoints.Count + exitPoints.Count < 2)
-                return;
-
-            for (int i = 0; i < fixtures.Count; i++)
-            {
-                // can't cut circles yet !
-                if (fixtures[i].Shape.ShapeType != ShapeType.Polygon)
-                    continue;
-
-                if (fixtures[i].Body.BodyType != BodyType.Static)
-                {
-                    //Split the shape up into two shapes
-                    Vertices first;
-                    Vertices second;
-                    SplitShape(fixtures[i], entryPoints[i], exitPoints[i], thickness, out first, out second);
-
-                    //Delete the original shape and create two new. Retain the properties of the body.
-                    if (SanityCheck(first))
-                    {
-                        Body firstFixture = BodyFactory.CreatePolygon(world, first, fixtures[i].Shape.Density,
-                                                                            fixtures[i].Body.Position);
-                        firstFixture.Rotation = fixtures[i].Body.Rotation;
-                        firstFixture.LinearVelocity = fixtures[i].Body.LinearVelocity;
-                        firstFixture.AngularVelocity = fixtures[i].Body.AngularVelocity;
-                        firstFixture.BodyType = BodyType.Dynamic;
-                    }
-
-                    if (SanityCheck(second))
-                    {
-                        Body secondFixture = BodyFactory.CreatePolygon(world, second, fixtures[i].Shape.Density,
-                                                                             fixtures[i].Body.Position);
-                        secondFixture.Rotation = fixtures[i].Body.Rotation;
-                        secondFixture.LinearVelocity = fixtures[i].Body.LinearVelocity;
-                        secondFixture.AngularVelocity = fixtures[i].Body.AngularVelocity;
-                        secondFixture.BodyType = BodyType.Dynamic;
-                    }
-                    world.RemoveBody(fixtures[i].Body);
-                }
-            }
-        }
-
-        private static bool SanityCheck(Vertices vertices)
-        {
-            if (vertices.Count < 3)
-                return false;
-
-            if (vertices.GetArea() < 0.00001f)
-                return false;
-
-            for (int i = 0; i < vertices.Count; ++i)
-            {
-                int i1 = i;
-                int i2 = i + 1 < vertices.Count ? i + 1 : 0;
-                Vector2 edge = vertices[i2] - vertices[i1];
-                if (edge.LengthSquared() < Settings.Epsilon * Settings.Epsilon)
-                    return false;
-            }
-
-            for (int i = 0; i < vertices.Count; ++i)
-            {
-                int i1 = i;
-                int i2 = i + 1 < vertices.Count ? i + 1 : 0;
-                Vector2 edge = vertices[i2] - vertices[i1];
-
-                for (int j = 0; j < vertices.Count; ++j)
-                {
-                    // Don't check vertices on the current edge.
-                    if (j == i1 || j == i2)
-                    {
-                        continue;
-                    }
-
-                    Vector2 r = vertices[j] - vertices[i1];
-
-                    // Your polygon is non-convex (it has an indentation) or
-                    // has colinear edges.
-                    float s = edge.X * r.Y - edge.Y * r.X;
-
-                    if (s < 0.0f)
-                        return false;
-                }
-            }
-
-            return true;
-        }
-    }
-}

FarseerPhysicsEngine/Common/PolygonManipulation/SimplifyTools.cs

@@ -1,359 +0,0 @@
-using System;
-using System.Collections.Generic;
-using System.Diagnostics;
-using Microsoft.Xna.Framework;
-
-namespace FarseerPhysics.Common.PolygonManipulation
-{
-    public static class SimplifyTools
-    {
-        private static bool[] _usePt;
-        private static double _distanceTolerance;
-
-        /// <summary>
-        /// Removes all collinear points on the polygon.
-        /// </summary>
-        /// <param name="vertices">The polygon that needs simplification.</param>
-        /// <param name="collinearityTolerance">The collinearity tolerance.</param>
-        /// <returns>A simplified polygon.</returns>
-        public static Vertices CollinearSimplify(Vertices vertices, float collinearityTolerance)
-        {
-            //We can't simplify polygons under 3 vertices
-            if (vertices.Count < 3)
-                return vertices;
-
-            Vertices simplified = new Vertices();
-
-            for (int i = 0; i < vertices.Count; i++)
-            {
-                int prevId = vertices.PreviousIndex(i);
-                int nextId = vertices.NextIndex(i);
-
-                Vector2 prev = vertices[prevId];
-                Vector2 current = vertices[i];
-                Vector2 next = vertices[nextId];
-
-                //If they collinear, continue
-                if (MathUtils.Collinear(ref prev, ref current, ref next, collinearityTolerance))
-                    continue;
-
-                simplified.Add(current);
-            }
-
-            return simplified;
-        }
-
-        /// <summary>
-        /// Removes all collinear points on the polygon.
-        /// Has a default bias of 0
-        /// </summary>
-        /// <param name="vertices">The polygon that needs simplification.</param>
-        /// <returns>A simplified polygon.</returns>
-        public static Vertices CollinearSimplify(Vertices vertices)
-        {
-            return CollinearSimplify(vertices, 0);
-        }
-
-        /// <summary>
-        /// Ramer-Douglas-Peucker polygon simplification algorithm. This is the general recursive version that does not use the
-        /// speed-up technique by using the Melkman convex hull.
-        /// 
-        /// If you pass in 0, it will remove all collinear points
-        /// </summary>
-        /// <returns>The simplified polygon</returns>
-        public static Vertices DouglasPeuckerSimplify(Vertices vertices, float distanceTolerance)
-        {
-            _distanceTolerance = distanceTolerance;
-
-            _usePt = new bool[vertices.Count];
-            for (int i = 0; i < vertices.Count; i++)
-                _usePt[i] = true;
-
-            SimplifySection(vertices, 0, vertices.Count - 1);
-            Vertices result = new Vertices();
-
-            for (int i = 0; i < vertices.Count; i++)
-                if (_usePt[i])
-                    result.Add(vertices[i]);
-
-            return result;
-        }
-
-        private static void SimplifySection(Vertices vertices, int i, int j)
-        {
-            if ((i + 1) == j)
-                return;
-
-            Vector2 A = vertices[i];
-            Vector2 B = vertices[j];
-            double maxDistance = -1.0;
-            int maxIndex = i;
-            for (int k = i + 1; k < j; k++)
-            {
-                double distance = DistancePointLine(vertices[k], A, B);
-
-                if (distance > maxDistance)
-                {
-                    maxDistance = distance;
-                    maxIndex = k;
-                }
-            }
-            if (maxDistance <= _distanceTolerance)
-                for (int k = i + 1; k < j; k++)
-                    _usePt[k] = false;
-            else
-            {
-                SimplifySection(vertices, i, maxIndex);
-                SimplifySection(vertices, maxIndex, j);
-            }
-        }
-
-        private static double DistancePointPoint(Vector2 p, Vector2 p2)
-        {
-            double dx = p.X - p2.X;
-            double dy = p.Y - p2.X;
-            return Math.Sqrt(dx * dx + dy * dy);
-        }
-
-        private static double DistancePointLine(Vector2 p, Vector2 A, Vector2 B)
-        {
-            // if start == end, then use point-to-point distance
-            if (A.X == B.X && A.Y == B.Y)
-                return DistancePointPoint(p, A);
-
-            // otherwise use comp.graphics.algorithms Frequently Asked Questions method
-            /*(1)     	      AC dot AB
-                        r =   ---------
-                              ||AB||^2
-             
-		                r has the following meaning:
-		                r=0 Point = A
-		                r=1 Point = B
-		                r<0 Point is on the backward extension of AB
-		                r>1 Point is on the forward extension of AB
-		                0<r<1 Point is interior to AB
-	        */
-
-            double r = ((p.X - A.X) * (B.X - A.X) + (p.Y - A.Y) * (B.Y - A.Y))
-                       /
-                       ((B.X - A.X) * (B.X - A.X) + (B.Y - A.Y) * (B.Y - A.Y));
-
-            if (r <= 0.0) return DistancePointPoint(p, A);
-            if (r >= 1.0) return DistancePointPoint(p, B);
-
-
-            /*(2)
-		                    (Ay-Cy)(Bx-Ax)-(Ax-Cx)(By-Ay)
-		                s = -----------------------------
-		             	                Curve^2
-
-		                Then the distance from C to Point = |s|*Curve.
-	        */
-
-            double s = ((A.Y - p.Y) * (B.X - A.X) - (A.X - p.X) * (B.Y - A.Y))
-                       /
-                       ((B.X - A.X) * (B.X - A.X) + (B.Y - A.Y) * (B.Y - A.Y));
-
-            return Math.Abs(s) * Math.Sqrt(((B.X - A.X) * (B.X - A.X) + (B.Y - A.Y) * (B.Y - A.Y)));
-        }
-
-        //From physics2d.net
-        public static Vertices ReduceByArea(Vertices vertices, float areaTolerance)
-        {
-            if (vertices.Count <= 3)
-                return vertices;
-
-            if (areaTolerance < 0)
-            {
-                throw new ArgumentOutOfRangeException("areaTolerance", "must be equal to or greater then zero.");
-            }
-
-            Vertices result = new Vertices();
-            Vector2 v1, v2, v3;
-            float old1, old2, new1;
-            v1 = vertices[vertices.Count - 2];
-            v2 = vertices[vertices.Count - 1];
-            areaTolerance *= 2;
-            for (int index = 0; index < vertices.Count; ++index, v2 = v3)
-            {
-                if (index == vertices.Count - 1)
-                {
-                    if (result.Count == 0)
-                    {
-                        throw new ArgumentOutOfRangeException("areaTolerance", "The tolerance is too high!");
-                    }
-                    v3 = result[0];
-                }
-                else
-                {
-                    v3 = vertices[index];
-                }
-                MathUtils.Cross(ref v1, ref v2, out old1);
-                MathUtils.Cross(ref v2, ref v3, out old2);
-                MathUtils.Cross(ref v1, ref v3, out new1);
-                if (Math.Abs(new1 - (old1 + old2)) > areaTolerance)
-                {
-                    result.Add(v2);
-                    v1 = v2;
-                }
-            }
-            return result;
-        }
-
-        //From Eric Jordan's convex decomposition library
-
-        /// <summary>
-        /// Merges all parallel edges in the list of vertices
-        /// </summary>
-        /// <param name="vertices">The vertices.</param>
-        /// <param name="tolerance">The tolerance.</param>
-        public static void MergeParallelEdges(Vertices vertices, float tolerance)
-        {
-            if (vertices.Count <= 3)
-                return; //Can't do anything useful here to a triangle
-
-            bool[] mergeMe = new bool[vertices.Count];
-            int newNVertices = vertices.Count;
-
-            //Gather points to process
-            for (int i = 0; i < vertices.Count; ++i)
-            {
-                int lower = (i == 0) ? (vertices.Count - 1) : (i - 1);
-                int middle = i;
-                int upper = (i == vertices.Count - 1) ? (0) : (i + 1);
-
-                float dx0 = vertices[middle].X - vertices[lower].X;
-                float dy0 = vertices[middle].Y - vertices[lower].Y;
-                float dx1 = vertices[upper].Y - vertices[middle].X;
-                float dy1 = vertices[upper].Y - vertices[middle].Y;
-                float norm0 = (float)Math.Sqrt(dx0 * dx0 + dy0 * dy0);
-                float norm1 = (float)Math.Sqrt(dx1 * dx1 + dy1 * dy1);
-
-                if (!(norm0 > 0.0f && norm1 > 0.0f) && newNVertices > 3)
-                {
-                    //Merge identical points
-                    mergeMe[i] = true;
-                    --newNVertices;
-                }
-
-                dx0 /= norm0;
-                dy0 /= norm0;
-                dx1 /= norm1;
-                dy1 /= norm1;
-                float cross = dx0 * dy1 - dx1 * dy0;
-                float dot = dx0 * dx1 + dy0 * dy1;
-
-                if (Math.Abs(cross) < tolerance && dot > 0 && newNVertices > 3)
-                {
-                    mergeMe[i] = true;
-                    --newNVertices;
-                }
-                else
-                    mergeMe[i] = false;
-            }
-
-            if (newNVertices == vertices.Count || newNVertices == 0)
-                return;
-
-            int currIndex = 0;
-
-            //Copy the vertices to a new list and clear the old
-            Vertices oldVertices = new Vertices(vertices);
-            vertices.Clear();
-
-            for (int i = 0; i < oldVertices.Count; ++i)
-            {
-                if (mergeMe[i] || newNVertices == 0 || currIndex == newNVertices)
-                    continue;
-
-                Debug.Assert(currIndex < newNVertices);
-
-                vertices.Add(oldVertices[i]);
-                ++currIndex;
-            }
-        }
-
-        //Misc
-
-        /// <summary>
-        /// Merges the identical points in the polygon.
-        /// </summary>
-        /// <param name="vertices">The vertices.</param>
-        /// <returns></returns>
-        public static Vertices MergeIdenticalPoints(Vertices vertices)
-        {
-            //We use a dictonary here because HashSet is not avaliable on all platforms.
-            HashSet<Vector2> results = new HashSet<Vector2>();
-
-            for (int i = 0; i < vertices.Count; i++)
-            {
-                results.Add(vertices[i]);
-            }
-
-            Vertices returnResults = new Vertices();
-            foreach (Vector2 v in results)
-            {
-                returnResults.Add(v);
-            }
-
-            return returnResults;
-        }
-
-        /// <summary>
-        /// Reduces the polygon by distance.
-        /// </summary>
-        /// <param name="vertices">The vertices.</param>
-        /// <param name="distance">The distance between points. Points closer than this will be 'joined'.</param>
-        /// <returns></returns>
-        public static Vertices ReduceByDistance(Vertices vertices, float distance)
-        {
-            //We can't simplify polygons under 3 vertices
-            if (vertices.Count < 3)
-                return vertices;
-
-            Vertices simplified = new Vertices();
-
-            for (int i = 0; i < vertices.Count; i++)
-            {
-                Vector2 current = vertices[i];
-                Vector2 next = vertices.NextVertex(i);
-
-                //If they are closer than the distance, continue
-                if ((next - current).LengthSquared() <= distance)
-                    continue;
-
-                simplified.Add(current);
-            }
-
-            return simplified;
-        }
-
-        /// <summary>
-        /// Reduces the polygon by removing the Nth vertex in the vertices list.
-        /// </summary>
-        /// <param name="vertices">The vertices.</param>
-        /// <param name="nth">The Nth point to remove. Example: 5.</param>
-        /// <returns></returns>
-        public static Vertices ReduceByNth(Vertices vertices, int nth)
-        {
-            //We can't simplify polygons under 3 vertices
-            if (vertices.Count < 3)
-                return vertices;
-
-            if (nth == 0)
-                return vertices;
-
-            Vertices result = new Vertices(vertices.Count);
-
-            for (int i = 0; i < vertices.Count; i++)
-            {
-                if (i % nth == 0)
-                    continue;
-
-                result.Add(vertices[i]);
-            }
-
-            return result;
-        }
-    }
-}

FarseerPhysicsEngine/Common/PolygonManipulation/YuPengClipper.cs

@@ -1,513 +0,0 @@
-using System;
-using System.Collections.Generic;
-using System.Diagnostics;
-using FarseerPhysics.Collision.Shapes;
-using Microsoft.Xna.Framework;
-
-namespace FarseerPhysics.Common.PolygonManipulation
-{
-    internal enum PolyClipType
-    {
-        Intersect,
-        Union,
-        Difference
-    }
-
-    public enum PolyClipError
-    {
-        None,
-        DegeneratedOutput,
-        NonSimpleInput,
-        BrokenResult
-    }
-
-    //Clipper contributed by Helge Backhaus
-
-    public static class YuPengClipper
-    {
-        private const float ClipperEpsilonSquared = 1.192092896e-07f;
-
-        public static List<Vertices> Union(Vertices polygon1, Vertices polygon2, out PolyClipError error)
-        {
-            return Execute(polygon1, polygon2, PolyClipType.Union, out error);
-        }
-
-        public static List<Vertices> Difference(Vertices polygon1, Vertices polygon2, out PolyClipError error)
-        {
-            return Execute(polygon1, polygon2, PolyClipType.Difference, out error);
-        }
-
-        public static List<Vertices> Intersect(Vertices polygon1, Vertices polygon2, out PolyClipError error)
-        {
-            return Execute(polygon1, polygon2, PolyClipType.Intersect, out error);
-        }
-
-        /// <summary>
-        /// Implements "A new algorithm for Boolean operations on general polygons" 
-        /// available here: http://liama.ia.ac.cn/wiki/_media/user:dong:dong_cg_05.pdf
-        /// Merges two polygons, a subject and a clip with the specified operation. Polygons may not be 
-        /// self-intersecting.
-        /// 
-        /// Warning: May yield incorrect results or even crash if polygons contain collinear points.
-        /// </summary>
-        /// <param name="subject">The subject polygon.</param>
-        /// <param name="clip">The clip polygon, which is added, 
-        /// substracted or intersected with the subject</param>
-        /// <param name="clipType">The operation to be performed. Either
-        /// Union, Difference or Intersection.</param>
-        /// <param name="error">The error generated (if any)</param>
-        /// <returns>A list of closed polygons, which make up the result of the clipping operation.
-        /// Outer contours are ordered counter clockwise, holes are ordered clockwise.</returns>
-        private static List<Vertices> Execute(Vertices subject, Vertices clip,
-                                              PolyClipType clipType, out PolyClipError error)
-        {
-            Debug.Assert(subject.IsSimple() && clip.IsSimple(), "Non simple input!", "Input polygons must be simple (cannot intersect themselves).");
-
-            // Copy polygons
-            Vertices slicedSubject;
-            Vertices slicedClip;
-            // Calculate the intersection and touch points between
-            // subject and clip and add them to both
-            CalculateIntersections(subject, clip, out slicedSubject, out slicedClip);
-
-            // Translate polygons into upper right quadrant
-            // as the algorithm depends on it
-            Vector2 lbSubject = subject.GetCollisionBox().LowerBound;
-            Vector2 lbClip = clip.GetCollisionBox().LowerBound;
-            Vector2 translate;
-            Vector2.Min(ref lbSubject, ref lbClip, out translate);
-            translate = Vector2.One - translate;
-            if (translate != Vector2.Zero)
-            {
-                slicedSubject.Translate(ref translate);
-                slicedClip.Translate(ref translate);
-            }
-
-            // Enforce counterclockwise contours
-            slicedSubject.ForceCounterClockWise();
-            slicedClip.ForceCounterClockWise();
-
-            List<Edge> subjectSimplices;
-            List<float> subjectCoeff;
-            List<Edge> clipSimplices;
-            List<float> clipCoeff;
-            // Build simplical chains from the polygons and calculate the
-            // the corresponding coefficients
-            CalculateSimplicalChain(slicedSubject, out subjectCoeff, out subjectSimplices);
-            CalculateSimplicalChain(slicedClip, out clipCoeff, out clipSimplices);
-
-            List<Edge> resultSimplices;
-
-            // Determine the characteristics function for all non-original edges
-            // in subject and clip simplical chain and combine the edges contributing
-            // to the result, depending on the clipType
-            CalculateResultChain(subjectCoeff, subjectSimplices, clipCoeff, clipSimplices, clipType,
-                                 out resultSimplices);
-
-            List<Vertices> result;
-            // Convert result chain back to polygon(s)
-            error = BuildPolygonsFromChain(resultSimplices, out result);
-
-            // Reverse the polygon translation from the beginning
-            // and remove collinear points from output
-            translate *= -1f;
-            for (int i = 0; i < result.Count; ++i)
-            {
-                result[i].Translate(ref translate);
-                SimplifyTools.CollinearSimplify(result[i]);
-            }
-            return result;
-        }
-
-        /// <summary>
-        /// Calculates all intersections between two polygons.
-        /// </summary>
-        /// <param name="polygon1">The first polygon.</param>
-        /// <param name="polygon2">The second polygon.</param>
-        /// <param name="slicedPoly1">Returns the first polygon with added intersection points.</param>
-        /// <param name="slicedPoly2">Returns the second polygon with added intersection points.</param>
-        private static void CalculateIntersections(Vertices polygon1, Vertices polygon2,
-                                                   out Vertices slicedPoly1, out Vertices slicedPoly2)
-        {
-            slicedPoly1 = new Vertices(polygon1);
-            slicedPoly2 = new Vertices(polygon2);
-
-            // Iterate through polygon1's edges
-            for (int i = 0; i < polygon1.Count; i++)
-            {
-                // Get edge vertices
-                Vector2 a = polygon1[i];
-                Vector2 b = polygon1[polygon1.NextIndex(i)];
-
-                // Get intersections between this edge and polygon2
-                for (int j = 0; j < polygon2.Count; j++)
-                {
-                    Vector2 c = polygon2[j];
-                    Vector2 d = polygon2[polygon2.NextIndex(j)];
-
-                    Vector2 intersectionPoint;
-                    // Check if the edges intersect
-                    if (LineTools.LineIntersect(a, b, c, d, out intersectionPoint))
-                    {
-                        // calculate alpha values for sorting multiple intersections points on a edge
-                        float alpha;
-                        // Insert intersection point into first polygon
-                        alpha = GetAlpha(a, b, intersectionPoint);
-                        if (alpha > 0f && alpha < 1f)
-                        {
-                            int index = slicedPoly1.IndexOf(a) + 1;
-                            while (index < slicedPoly1.Count &&
-                                   GetAlpha(a, b, slicedPoly1[index]) <= alpha)
-                            {
-                                ++index;
-                            }
-                            slicedPoly1.Insert(index, intersectionPoint);
-                        }
-                        // Insert intersection point into second polygon
-                        alpha = GetAlpha(c, d, intersectionPoint);
-                        if (alpha > 0f && alpha < 1f)
-                        {
-                            int index = slicedPoly2.IndexOf(c) + 1;
-                            while (index < slicedPoly2.Count &&
-                                   GetAlpha(c, d, slicedPoly2[index]) <= alpha)
-                            {
-                                ++index;
-                            }
-                            slicedPoly2.Insert(index, intersectionPoint);
-                        }
-                    }
-                }
-            }
-            // Check for very small edges
-            for (int i = 0; i < slicedPoly1.Count; ++i)
-            {
-                int iNext = slicedPoly1.NextIndex(i);
-                //If they are closer than the distance remove vertex
-                if ((slicedPoly1[iNext] - slicedPoly1[i]).LengthSquared() <= ClipperEpsilonSquared)
-                {
-                    slicedPoly1.RemoveAt(i);
-                    --i;
-                }
-            }
-            for (int i = 0; i < slicedPoly2.Count; ++i)
-            {
-                int iNext = slicedPoly2.NextIndex(i);
-                //If they are closer than the distance remove vertex
-                if ((slicedPoly2[iNext] - slicedPoly2[i]).LengthSquared() <= ClipperEpsilonSquared)
-                {
-                    slicedPoly2.RemoveAt(i);
-                    --i;
-                }
-            }
-        }
-
-        /// <summary>
-        /// Calculates the simplical chain corresponding to the input polygon.
-        /// </summary>
-        /// <remarks>Used by method <c>Execute()</c>.</remarks>
-        private static void CalculateSimplicalChain(Vertices poly, out List<float> coeff,
-                                                    out List<Edge> simplicies)
-        {
-            simplicies = new List<Edge>();
-            coeff = new List<float>();
-            for (int i = 0; i < poly.Count; ++i)
-            {
-                simplicies.Add(new Edge(poly[i], poly[poly.NextIndex(i)]));
-                coeff.Add(CalculateSimplexCoefficient(Vector2.Zero, poly[i], poly[poly.NextIndex(i)]));
-            }
-        }
-
-        /// <summary>
-        /// Calculates the characteristics function for all edges of
-        /// the given simplical chains and builds the result chain.
-        /// </summary>
-        /// <remarks>Used by method <c>Execute()</c>.</remarks>
-        private static void CalculateResultChain(List<float> poly1Coeff, List<Edge> poly1Simplicies,
-                                                   List<float> poly2Coeff, List<Edge> poly2Simplicies,
-                                                   PolyClipType clipType, out List<Edge> resultSimplices)
-        {
-            resultSimplices = new List<Edge>();
-
-            for (int i = 0; i < poly1Simplicies.Count; ++i)
-            {
-                float edgeCharacter = 0f;
-                if (poly2Simplicies.Contains(poly1Simplicies[i]) ||
-                    (poly2Simplicies.Contains(-poly1Simplicies[i]) && clipType == PolyClipType.Union))
-                {
-                    edgeCharacter = 1f;
-                }
-                else
-                {
-                    for (int j = 0; j < poly2Simplicies.Count; ++j)
-                    {
-                        if (!poly2Simplicies.Contains(-poly1Simplicies[i]))
-                        {
-                            edgeCharacter += CalculateBeta(poly1Simplicies[i].GetCenter(),
-                                                           poly2Simplicies[j], poly2Coeff[j]);
-                        }
-                    }
-                }
-                if (clipType == PolyClipType.Intersect)
-                {
-                    if (edgeCharacter == 1f)
-                    {
-                        resultSimplices.Add(poly1Simplicies[i]);
-                    }
-                }
-                else
-                {
-                    if (edgeCharacter == 0f)
-                    {
-                        resultSimplices.Add(poly1Simplicies[i]);
-                    }
-                }
-            }
-            for (int i = 0; i < poly2Simplicies.Count; ++i)
-            {
-                if (!resultSimplices.Contains(poly2Simplicies[i]) &&
-                    !resultSimplices.Contains(-poly2Simplicies[i]))
-                {
-                    float edgeCharacter = 0f;
-                    if (poly1Simplicies.Contains(poly2Simplicies[i]) ||
-                        (poly1Simplicies.Contains(-poly2Simplicies[i]) && clipType == PolyClipType.Union))
-                    {
-                        edgeCharacter = 1f;
-                    }
-                    else
-                    {
-                        for (int j = 0; j < poly1Simplicies.Count; ++j)
-                        {
-                            if (!poly1Simplicies.Contains(-poly2Simplicies[i]))
-                            {
-                                edgeCharacter += CalculateBeta(poly2Simplicies[i].GetCenter(),
-                                                               poly1Simplicies[j], poly1Coeff[j]);
-                            }
-                        }
-                    }
-                    if (clipType == PolyClipType.Intersect || clipType == PolyClipType.Difference)
-                    {
-                        if (edgeCharacter == 1f)
-                        {
-                            resultSimplices.Add(-poly2Simplicies[i]);
-                        }
-                    }
-                    else
-                    {
-                        if (edgeCharacter == 0f)
-                        {
-                            resultSimplices.Add(poly2Simplicies[i]);
-                        }
-                    }
-                }
-            }
-        }
-
-        /// <summary>
-        /// Calculates the polygon(s) from the result simplical chain.
-        /// </summary>
-        /// <remarks>Used by method <c>Execute()</c>.</remarks>
-        private static PolyClipError BuildPolygonsFromChain(List<Edge> simplicies, out List<Vertices> result)
-        {
-            result = new List<Vertices>();
-            PolyClipError errVal = PolyClipError.None;
-
-            while (simplicies.Count > 0)
-            {
-                Vertices output = new Vertices();
-                output.Add(simplicies[0].EdgeStart);
-                output.Add(simplicies[0].EdgeEnd);
-                simplicies.RemoveAt(0);
-                bool closed = false;
-                int index = 0;
-                int count = simplicies.Count; // Needed to catch infinite loops
-                while (!closed && simplicies.Count > 0)
-                {
-                    if (VectorEqual(output[output.Count - 1], simplicies[index].EdgeStart))
-                    {
-                        if (VectorEqual(simplicies[index].EdgeEnd, output[0]))
-                        {
-                            closed = true;
-                        }
-                        else
-                        {
-                            output.Add(simplicies[index].EdgeEnd);
-                        }
-                        simplicies.RemoveAt(index);
-                        --index;
-                    }
-                    else if (VectorEqual(output[output.Count - 1], simplicies[index].EdgeEnd))
-                    {
-                        if (VectorEqual(simplicies[index].EdgeStart, output[0]))
-                        {
-                            closed = true;
-                        }
-                        else
-                        {
-                            output.Add(simplicies[index].EdgeStart);
-                        }
-                        simplicies.RemoveAt(index);
-                        --index;
-                    }
-                    if (!closed)
-                    {
-                        if (++index == simplicies.Count)
-                        {
-                            if (count == simplicies.Count)
-                            {
-                                result = new List<Vertices>();
-                                Debug.WriteLine("Undefined error while building result polygon(s).");
-                                return PolyClipError.BrokenResult;
-                            }
-                            index = 0;
-                            count = simplicies.Count;
-                        }
-                    }
-                }
-                if (output.Count < 3)
-                {
-                    errVal = PolyClipError.DegeneratedOutput;
-                    Debug.WriteLine("Degenerated output polygon produced (vertices < 3).");
-                }
-                result.Add(output);
-            }
-            return errVal;
-        }
-
-        /// <summary>
-        /// Needed to calculate the characteristics function of a simplex.
-        /// </summary>
-        /// <remarks>Used by method <c>CalculateEdgeCharacter()</c>.</remarks>
-        private static float CalculateBeta(Vector2 point, Edge e, float coefficient)
-        {
-            float result = 0f;
-            if (PointInSimplex(point, e))
-            {
-                result = coefficient;
-            }
-            if (PointOnLineSegment(Vector2.Zero, e.EdgeStart, point) ||
-                PointOnLineSegment(Vector2.Zero, e.EdgeEnd, point))
-            {
-                result = .5f * coefficient;
-            }
-            return result;
-        }
-
-        /// <summary>
-        /// Needed for sorting multiple intersections points on the same edge.
-        /// </summary>
-        /// <remarks>Used by method <c>CalculateIntersections()</c>.</remarks>
-        private static float GetAlpha(Vector2 start, Vector2 end, Vector2 point)
-        {
-            return (point - start).LengthSquared() / (end - start).LengthSquared();
-        }
-
-        /// <summary>
-        /// Returns the coefficient of a simplex.
-        /// </summary>
-        /// <remarks>Used by method <c>CalculateSimplicalChain()</c>.</remarks>
-        private static float CalculateSimplexCoefficient(Vector2 a, Vector2 b, Vector2 c)
-        {
-            float isLeft = MathUtils.Area(ref a, ref b, ref c);
-            if (isLeft < 0f)
-            {
-                return -1f;
-            }
-
-            if (isLeft > 0f)
-            {
-                return 1f;
-            }
-
-            return 0f;
-        }
-
-        /// <summary>
-        /// Winding number test for a point in a simplex.
-        /// </summary>
-        /// <param name="point">The point to be tested.</param>
-        /// <param name="edge">The edge that the point is tested against.</param>
-        /// <returns>False if the winding number is even and the point is outside
-        /// the simplex and True otherwise.</returns>
-        private static bool PointInSimplex(Vector2 point, Edge edge)
-        {
-            Vertices polygon = new Vertices();
-            polygon.Add(Vector2.Zero);
-            polygon.Add(edge.EdgeStart);
-            polygon.Add(edge.EdgeEnd);
-            return (polygon.PointInPolygon(ref point) == 1);
-        }
-
-        /// <summary>
-        /// Tests if a point lies on a line segment.
-        /// </summary>
-        /// <remarks>Used by method <c>CalculateBeta()</c>.</remarks>
-        private static bool PointOnLineSegment(Vector2 start, Vector2 end, Vector2 point)
-        {
-            Vector2 segment = end - start;
-            return MathUtils.Area(ref start, ref end, ref point) == 0f &&
-                   Vector2.Dot(point - start, segment) >= 0f &&
-                   Vector2.Dot(point - end, segment) <= 0f;
-        }
-
-        private static bool VectorEqual(Vector2 vec1, Vector2 vec2)
-        {
-            return (vec2 - vec1).LengthSquared() <= ClipperEpsilonSquared;
-        }
-
-        #region Nested type: Edge
-
-        /// <summary>Specifies an Edge. Edges are used to represent simplicies in simplical chains</summary>
-        private sealed class Edge
-        {
-            public Edge(Vector2 edgeStart, Vector2 edgeEnd)
-            {
-                EdgeStart = edgeStart;
-                EdgeEnd = edgeEnd;
-            }
-
-            public Vector2 EdgeStart { get; private set; }
-            public Vector2 EdgeEnd { get; private set; }
-
-            public Vector2 GetCenter()
-            {
-                return (EdgeStart + EdgeEnd) / 2f;
-            }
-
-            public static Edge operator -(Edge e)
-            {
-                return new Edge(e.EdgeEnd, e.EdgeStart);
-            }
-
-            public override bool Equals(Object obj)
-            {
-                // If parameter is null return false.
-                if (obj == null)
-                {
-                    return false;
-                }
-
-                // If parameter cannot be cast to Point return false.
-                return Equals(obj as Edge);
-            }
-
-            public bool Equals(Edge e)
-            {
-                // If parameter is null return false:
-                if (e == null)
-                {
-                    return false;
-                }
-
-                // Return true if the fields match
-                return VectorEqual(EdgeStart, e.EdgeStart) && VectorEqual(EdgeEnd, e.EdgeEnd);
-            }
-
-            public override int GetHashCode()
-            {
-                return EdgeStart.GetHashCode() ^ EdgeEnd.GetHashCode();
-            }
-        }
-
-        #endregion
-    }
-}

FarseerPhysicsEngine/Common/PolygonTools.cs

@@ -1,359 +0,0 @@
-using System;
-using System.Collections.Generic;
-using System.Diagnostics;
-using Microsoft.Xna.Framework;
-
-namespace FarseerPhysics.Common
-{
-    public static class PolygonTools
-    {
-        /// <summary>
-        /// Build vertices to represent an axis-aligned box.
-        /// </summary>
-        /// <param name="hx">the half-width.</param>
-        /// <param name="hy">the half-height.</param>
-        public static Vertices CreateRectangle(float hx, float hy)
-        {
-            Vertices vertices = new Vertices(4);
-            vertices.Add(new Vector2(-hx, -hy));
-            vertices.Add(new Vector2(hx, -hy));
-            vertices.Add(new Vector2(hx, hy));
-            vertices.Add(new Vector2(-hx, hy));
-
-            return vertices;
-        }
-
-        /// <summary>
-        /// Build vertices to represent an oriented box.
-        /// </summary>
-        /// <param name="hx">the half-width.</param>
-        /// <param name="hy">the half-height.</param>
-        /// <param name="center">the center of the box in local coordinates.</param>
-        /// <param name="angle">the rotation of the box in local coordinates.</param>
-        public static Vertices CreateRectangle(float hx, float hy, Vector2 center, float angle)
-        {
-            Vertices vertices = CreateRectangle(hx, hy);
-
-            Transform xf = new Transform();
-            xf.Position = center;
-            xf.R.Set(angle);
-
-            // Transform vertices
-            for (int i = 0; i < 4; ++i)
-            {
-                vertices[i] = MathUtils.Multiply(ref xf, vertices[i]);
-            }
-
-            return vertices;
-        }
-
-        //Rounded rectangle contributed by Jonathan Smars - [email protected]
-
-        /// <summary>
-        /// Creates a rounded rectangle with the specified width and height.
-        /// </summary>
-        /// <param name="width">The width.</param>
-        /// <param name="height">The height.</param>
-        /// <param name="xRadius">The rounding X radius.</param>
-        /// <param name="yRadius">The rounding Y radius.</param>
-        /// <param name="segments">The number of segments to subdivide the edges.</param>
-        /// <returns></returns>
-        public static Vertices CreateRoundedRectangle(float width, float height, float xRadius, float yRadius,
-                                                      int segments)
-        {
-            if (yRadius > height / 2 || xRadius > width / 2)
-                throw new Exception("Rounding amount can't be more than half the height and width respectively.");
-            if (segments < 0)
-                throw new Exception("Segments must be zero or more.");
-
-            //We need at least 8 vertices to create a rounded rectangle
-            Debug.Assert(Settings.MaxPolygonVertices >= 8);
-
-            Vertices vertices = new Vertices();
-            if (segments == 0)
-            {
-                vertices.Add(new Vector2(width * .5f - xRadius, -height * .5f));
-                vertices.Add(new Vector2(width * .5f, -height * .5f + yRadius));
-
-                vertices.Add(new Vector2(width * .5f, height * .5f - yRadius));
-                vertices.Add(new Vector2(width * .5f - xRadius, height * .5f));
-
-                vertices.Add(new Vector2(-width * .5f + xRadius, height * .5f));
-                vertices.Add(new Vector2(-width * .5f, height * .5f - yRadius));
-
-                vertices.Add(new Vector2(-width * .5f, -height * .5f + yRadius));
-                vertices.Add(new Vector2(-width * .5f + xRadius, -height * .5f));
-            }
-            else
-            {
-                int numberOfEdges = (segments * 4 + 8);
-
-                float stepSize = MathHelper.TwoPi / (numberOfEdges - 4);
-                int perPhase = numberOfEdges / 4;
-
-                Vector2 posOffset = new Vector2(width / 2 - xRadius, height / 2 - yRadius);
-                vertices.Add(posOffset + new Vector2(xRadius, -yRadius + yRadius));
-                short phase = 0;
-                for (int i = 1; i < numberOfEdges; i++)
-                {
-                    if (i - perPhase == 0 || i - perPhase * 3 == 0)
-                    {
-                        posOffset.X *= -1;
-                        phase--;
-                    }
-                    else if (i - perPhase * 2 == 0)
-                    {
-                        posOffset.Y *= -1;
-                        phase--;
-                    }
-
-                    vertices.Add(posOffset + new Vector2(xRadius * (float)Math.Cos(stepSize * -(i + phase)),
-                                                         -yRadius * (float)Math.Sin(stepSize * -(i + phase))));
-                }
-            }
-
-            return vertices;
-        }
-
-        /// <summary>
-        /// Set this as a single edge.
-        /// </summary>
-        /// <param name="start">The first point.</param>
-        /// <param name="end">The second point.</param>
-        public static Vertices CreateLine(Vector2 start, Vector2 end)
-        {
-            Vertices vertices = new Vertices(2);
-            vertices.Add(start);
-            vertices.Add(end);
-
-            return vertices;
-        }
-
-        /// <summary>
-        /// Creates a circle with the specified radius and number of edges.
-        /// </summary>
-        /// <param name="radius">The radius.</param>
-        /// <param name="numberOfEdges">The number of edges. The more edges, the more it resembles a circle</param>
-        /// <returns></returns>
-        public static Vertices CreateCircle(float radius, int numberOfEdges)
-        {
-            return CreateEllipse(radius, radius, numberOfEdges);
-        }
-
-        /// <summary>
-        /// Creates a ellipse with the specified width, height and number of edges.
-        /// </summary>
-        /// <param name="xRadius">Width of the ellipse.</param>
-        /// <param name="yRadius">Height of the ellipse.</param>
-        /// <param name="numberOfEdges">The number of edges. The more edges, the more it resembles an ellipse</param>
-        /// <returns></returns>
-        public static Vertices CreateEllipse(float xRadius, float yRadius, int numberOfEdges)
-        {
-            Vertices vertices = new Vertices();
-
-            float stepSize = MathHelper.TwoPi / numberOfEdges;
-
-            vertices.Add(new Vector2(xRadius, 0));
-            for (int i = numberOfEdges - 1; i > 0; --i)
-                vertices.Add(new Vector2(xRadius * (float)Math.Cos(stepSize * i),
-                                         -yRadius * (float)Math.Sin(stepSize * i)));
-
-            return vertices;
-        }
-
-        public static Vertices CreateArc(float radians, int sides, float radius)
-        {
-            Debug.Assert(radians > 0, "The arc needs to be larger than 0");
-            Debug.Assert(sides > 1, "The arc needs to have more than 1 sides");
-            Debug.Assert(radius > 0, "The arc needs to have a radius larger than 0");
-
-            Vertices vertices = new Vertices();
-
-            float stepSize = radians / sides;
-            for (int i = sides - 1; i > 0; i--)
-            {
-                vertices.Add(new Vector2(radius * (float)Math.Cos(stepSize * i),
-                                         radius * (float)Math.Sin(stepSize * i)));
-            }
-
-            return vertices;
-        }
-
-        //Capsule contributed by Yobiv
-
-        /// <summary>
-        /// Creates an capsule with the specified height, radius and number of edges.
-        /// A capsule has the same form as a pill capsule.
-        /// </summary>
-        /// <param name="height">Height (inner height + 2 * radius) of the capsule.</param>
-        /// <param name="endRadius">Radius of the capsule ends.</param>
-        /// <param name="edges">The number of edges of the capsule ends. The more edges, the more it resembles an capsule</param>
-        /// <returns></returns>
-        public static Vertices CreateCapsule(float height, float endRadius, int edges)
-        {
-            if (endRadius >= height / 2)
-                throw new ArgumentException(
-                    "The radius must be lower than height / 2. Higher values of radius would create a circle, and not a half circle.",
-                    "endRadius");
-
-            return CreateCapsule(height, endRadius, edges, endRadius, edges);
-        }
-
-        /// <summary>
-        /// Creates an capsule with the specified  height, radius and number of edges.
-        /// A capsule has the same form as a pill capsule.
-        /// </summary>
-        /// <param name="height">Height (inner height + radii) of the capsule.</param>
-        /// <param name="topRadius">Radius of the top.</param>
-        /// <param name="topEdges">The number of edges of the top. The more edges, the more it resembles an capsule</param>
-        /// <param name="bottomRadius">Radius of bottom.</param>
-        /// <param name="bottomEdges">The number of edges of the bottom. The more edges, the more it resembles an capsule</param>
-        /// <returns></returns>
-        public static Vertices CreateCapsule(float height, float topRadius, int topEdges, float bottomRadius,
-                                             int bottomEdges)
-        {
-            if (height <= 0)
-                throw new ArgumentException("Height must be longer than 0", "height");
-
-            if (topRadius <= 0)
-                throw new ArgumentException("The top radius must be more than 0", "topRadius");
-
-            if (topEdges <= 0)
-                throw new ArgumentException("Top edges must be more than 0", "topEdges");
-
-            if (bottomRadius <= 0)
-                throw new ArgumentException("The bottom radius must be more than 0", "bottomRadius");
-
-            if (bottomEdges <= 0)
-                throw new ArgumentException("Bottom edges must be more than 0", "bottomEdges");
-
-            if (topRadius >= height / 2)
-                throw new ArgumentException(
-                    "The top radius must be lower than height / 2. Higher values of top radius would create a circle, and not a half circle.",
-                    "topRadius");
-
-            if (bottomRadius >= height / 2)
-                throw new ArgumentException(
-                    "The bottom radius must be lower than height / 2. Higher values of bottom radius would create a circle, and not a half circle.",
-                    "bottomRadius");
-
-            Vertices vertices = new Vertices();
-
-            float newHeight = (height - topRadius - bottomRadius) * 0.5f;
-
-            // top
-            vertices.Add(new Vector2(topRadius, newHeight));
-
-            float stepSize = MathHelper.Pi / topEdges;
-            for (int i = 1; i < topEdges; i++)
-            {
-                vertices.Add(new Vector2(topRadius * (float)Math.Cos(stepSize * i),
-                                         topRadius * (float)Math.Sin(stepSize * i) + newHeight));
-            }
-
-            vertices.Add(new Vector2(-topRadius, newHeight));
-
-            // bottom
-            vertices.Add(new Vector2(-bottomRadius, -newHeight));
-
-            stepSize = MathHelper.Pi / bottomEdges;
-            for (int i = 1; i < bottomEdges; i++)
-            {
-                vertices.Add(new Vector2(-bottomRadius * (float)Math.Cos(stepSize * i),
-                                         -bottomRadius * (float)Math.Sin(stepSize * i) - newHeight));
-            }
-
-            vertices.Add(new Vector2(bottomRadius, -newHeight));
-
-            return vertices;
-        }
-
-        /// <summary>
-        /// Creates a gear shape with the specified radius and number of teeth.
-        /// </summary>
-        /// <param name="radius">The radius.</param>
-        /// <param name="numberOfTeeth">The number of teeth.</param>
-        /// <param name="tipPercentage">The tip percentage.</param>
-        /// <param name="toothHeight">Height of the tooth.</param>
-        /// <returns></returns>
-        public static Vertices CreateGear(float radius, int numberOfTeeth, float tipPercentage, float toothHeight)
-        {
-            Vertices vertices = new Vertices();
-
-            float stepSize = MathHelper.TwoPi / numberOfTeeth;
-            tipPercentage /= 100f;
-            MathHelper.Clamp(tipPercentage, 0f, 1f);
-            float toothTipStepSize = (stepSize / 2f) * tipPercentage;
-
-            float toothAngleStepSize = (stepSize - (toothTipStepSize * 2f)) / 2f;
-
-            for (int i = numberOfTeeth - 1; i >= 0; --i)
-            {
-                if (toothTipStepSize > 0f)
-                {
-                    vertices.Add(
-                        new Vector2(radius *
-                                    (float)Math.Cos(stepSize * i + toothAngleStepSize * 2f + toothTipStepSize),
-                                    -radius *
-                                    (float)Math.Sin(stepSize * i + toothAngleStepSize * 2f + toothTipStepSize)));
-
-                    vertices.Add(
-                        new Vector2((radius + toothHeight) *
-                                    (float)Math.Cos(stepSize * i + toothAngleStepSize + toothTipStepSize),
-                                    -(radius + toothHeight) *
-                                    (float)Math.Sin(stepSize * i + toothAngleStepSize + toothTipStepSize)));
-                }
-
-                vertices.Add(new Vector2((radius + toothHeight) *
-                                         (float)Math.Cos(stepSize * i + toothAngleStepSize),
-                                         -(radius + toothHeight) *
-                                         (float)Math.Sin(stepSize * i + toothAngleStepSize)));
-
-                vertices.Add(new Vector2(radius * (float)Math.Cos(stepSize * i),
-                                         -radius * (float)Math.Sin(stepSize * i)));
-            }
-
-            return vertices;
-        }
-
-        /// <summary>
-        /// Detects the vertices by analyzing the texture data.
-        /// </summary>
-        /// <param name="data">The texture data.</param>
-        /// <param name="width">The texture width.</param>
-        /// <returns></returns>
-        public static Vertices CreatePolygon(uint[] data, int width)
-        {
-            return TextureConverter.DetectVertices(data, width);
-        }
-
-        /// <summary>
-        /// Detects the vertices by analyzing the texture data.
-        /// </summary>
-        /// <param name="data">The texture data.</param>
-        /// <param name="width">The texture width.</param>
-        /// <param name="holeDetection">if set to <c>true</c> it will perform hole detection.</param>
-        /// <returns></returns>
-        public static Vertices CreatePolygon(uint[] data, int width, bool holeDetection)
-        {
-            return TextureConverter.DetectVertices(data, width, holeDetection);
-        }
-
-        /// <summary>
-        /// Detects the vertices by analyzing the texture data.
-        /// </summary>
-        /// <param name="data">The texture data.</param>
-        /// <param name="width">The texture width.</param>
-        /// <param name="hullTolerance">The hull tolerance.</param>
-        /// <param name="alphaTolerance">The alpha tolerance.</param>
-        /// <param name="multiPartDetection">if set to <c>true</c> it will perform multi part detection.</param>
-        /// <param name="holeDetection">if set to <c>true</c> it will perform hole detection.</param>
-        /// <returns></returns>
-        public static List<Vertices> CreatePolygon(uint[] data, int width, float hullTolerance,
-                                                   byte alphaTolerance, bool multiPartDetection, bool holeDetection)
-        {
-            return TextureConverter.DetectVertices(data, width, hullTolerance, alphaTolerance,
-                                                   multiPartDetection, holeDetection);
-        }
-    }
-}

FarseerPhysicsEngine/Common/Serialization.cs

@@ -1,1453 +0,0 @@
-using System;
-using System.Collections.Generic;
-using System.IO;
-using System.Xml;
-using System.Xml.Serialization;
-using FarseerPhysics.Collision.Shapes;
-using FarseerPhysics.Dynamics;
-using FarseerPhysics.Dynamics.Joints;
-using Microsoft.Xna.Framework;
-
-namespace FarseerPhysics.Common
-{
-    public static class WorldSerializer
-    {
-        public static void Serialize(World world, string filename)
-        {
-            using (FileStream fs = new FileStream(filename, FileMode.Create))
-            {
-                new WorldXmlSerializer().Serialize(world, fs);
-            }
-        }
-
-        public static void Deserialize(World world, string filename)
-        {
-            using (FileStream fs = new FileStream(filename, FileMode.Open))
-            {
-                new WorldXmlDeserializer().Deserialize(world, fs);
-            }
-        }
-
-        public static World Deserialize(string filename)
-        {
-            using (FileStream fs = new FileStream(filename, FileMode.Open))
-            {
-                return new WorldXmlDeserializer().Deserialize(fs);
-            }
-        }
-    }
-
-    ///<summary>
-    ///</summary>
-    public class WorldXmlSerializer
-    {
-        private List<Body> _bodies = new List<Body>();
-        private List<Fixture> _serializedFixtures = new List<Fixture>();
-        private List<Shape> _serializedShapes = new List<Shape>();
-        private XmlWriter _writer;
-
-        private void SerializeShape(Shape shape)
-        {
-            _writer.WriteStartElement("Shape");
-            _writer.WriteAttributeString("Type", shape.ShapeType.ToString());
-
-            switch (shape.ShapeType)
-            {
-                case ShapeType.Circle:
-                    {
-                        CircleShape circle = (CircleShape)shape;
-
-                        _writer.WriteElementString("Radius", circle.Radius.ToString());
-
-                        WriteElement("Position", circle.Position);
-                    }
-                    break;
-                case ShapeType.Polygon:
-                    {
-                        PolygonShape poly = (PolygonShape)shape;
-
-                        _writer.WriteStartElement("Vertices");
-                        foreach (Vector2 v in poly.Vertices)
-                            WriteElement("Vertex", v);
-                        _writer.WriteEndElement();
-
-                        WriteElement("Centroid", poly.MassData.Centroid);
-                    }
-                    break;
-                case ShapeType.Edge:
-                    {
-                        EdgeShape poly = (EdgeShape)shape;
-                        WriteElement("Vertex1", poly.Vertex1);
-                        WriteElement("Vertex2", poly.Vertex2);
-                    }
-                    break;
-                default:
-                    throw new Exception();
-            }
-
-            _writer.WriteEndElement();
-        }
-
-        private void SerializeFixture(Fixture fixture)
-        {
-            _writer.WriteStartElement("Fixture");
-            _writer.WriteElementString("Shape", FindShapeIndex(fixture.Shape).ToString());
-            _writer.WriteElementString("Density", fixture.Shape.Density.ToString());
-
-            _writer.WriteStartElement("FilterData");
-            _writer.WriteElementString("CategoryBits", ((int)fixture.CollisionCategories).ToString());
-            _writer.WriteElementString("MaskBits", ((int)fixture.CollidesWith).ToString());
-            _writer.WriteElementString("GroupIndex", fixture.CollisionGroup.ToString());
-            _writer.WriteEndElement();
-
-            _writer.WriteElementString("Friction", fixture.Friction.ToString());
-            _writer.WriteElementString("IsSensor", fixture.IsSensor.ToString());
-            _writer.WriteElementString("Restitution", fixture.Restitution.ToString());
-
-            if (fixture.UserData != null)
-            {
-                _writer.WriteStartElement("UserData");
-                WriteDynamicType(fixture.UserData.GetType(), fixture.UserData);
-                _writer.WriteEndElement();
-            }
-
-            _writer.WriteEndElement();
-        }
-
-        private void SerializeBody(Body body)
-        {
-            _writer.WriteStartElement("Body");
-            _writer.WriteAttributeString("Type", body.BodyType.ToString());
-
-            _writer.WriteElementString("Active", body.Enabled.ToString());
-            _writer.WriteElementString("AllowSleep", body.SleepingAllowed.ToString());
-            _writer.WriteElementString("Angle", body.Rotation.ToString());
-            _writer.WriteElementString("AngularDamping", body.AngularDamping.ToString());
-            _writer.WriteElementString("AngularVelocity", body.AngularVelocity.ToString());
-            _writer.WriteElementString("Awake", body.Awake.ToString());
-            _writer.WriteElementString("Bullet", body.IsBullet.ToString());
-            _writer.WriteElementString("FixedRotation", body.FixedRotation.ToString());
-            _writer.WriteElementString("LinearDamping", body.LinearDamping.ToString());
-            WriteElement("LinearVelocity", body.LinearVelocity);
-            WriteElement("Position", body.Position);
-
-            if (body.UserData != null)
-            {
-                _writer.WriteStartElement("UserData");
-                WriteDynamicType(body.UserData.GetType(), body.UserData);
-                _writer.WriteEndElement();
-            }
-
-            _writer.WriteStartElement("Fixtures");
-            for (int i = 0; i < body.FixtureList.Count; i++)
-            {
-                _writer.WriteElementString("ID", FindFixtureIndex(body.FixtureList[i]).ToString());
-            }
-
-            _writer.WriteEndElement();
-            _writer.WriteEndElement();
-        }
-
-        private void SerializeJoint(Joint joint)
-        {
-            if (joint.IsFixedType())
-                return;
-
-            _writer.WriteStartElement("Joint");
-
-            _writer.WriteAttributeString("Type", joint.JointType.ToString());
-
-            WriteElement("BodyA", FindBodyIndex(joint.BodyA));
-            WriteElement("BodyB", FindBodyIndex(joint.BodyB));
-
-            WriteElement("CollideConnected", joint.CollideConnected);
-
-            WriteElement("Breakpoint", joint.Breakpoint);
-
-            if (joint.UserData != null)
-            {
-                _writer.WriteStartElement("UserData");
-                WriteDynamicType(joint.UserData.GetType(), joint.UserData);
-                _writer.WriteEndElement();
-            }
-
-            switch (joint.JointType)
-            {
-                case JointType.Distance:
-                    {
-                        DistanceJoint djd = (DistanceJoint)joint;
-
-                        WriteElement("DampingRatio", djd.DampingRatio);
-                        WriteElement("FrequencyHz", djd.Frequency);
-                        WriteElement("Length", djd.Length);
-                        WriteElement("LocalAnchorA", djd.LocalAnchorA);
-                        WriteElement("LocalAnchorB", djd.LocalAnchorB);
-                    }
-                    break;
-                case JointType.Friction:
-                    {
-                        FrictionJoint fjd = (FrictionJoint)joint;
-                        WriteElement("LocalAnchorA", fjd.LocalAnchorA);
-                        WriteElement("LocalAnchorB", fjd.LocalAnchorB);
-                        WriteElement("MaxForce", fjd.MaxForce);
-                        WriteElement("MaxTorque", fjd.MaxTorque);
-                    }
-                    break;
-                case JointType.Gear:
-                    throw new Exception("Gear joint not supported by serialization");
-                case JointType.Line:
-                    {
-                        LineJoint ljd = (LineJoint)joint;
-
-                        WriteElement("EnableMotor", ljd.MotorEnabled);
-                        WriteElement("LocalAnchorA", ljd.LocalAnchorA);
-                        WriteElement("LocalAnchorB", ljd.LocalAnchorB);
-                        WriteElement("MotorSpeed", ljd.MotorSpeed);
-                        WriteElement("DampingRatio", ljd.DampingRatio);
-                        WriteElement("MaxMotorTorque", ljd.MaxMotorTorque);
-                        WriteElement("FrequencyHz", ljd.Frequency);
-                        WriteElement("LocalXAxis", ljd.LocalXAxis);
-                    }
-                    break;
-                case JointType.Prismatic:
-                    {
-                        PrismaticJoint pjd = (PrismaticJoint)joint;
-
-                        //NOTE: Does not conform with Box2DScene
-
-                        WriteElement("EnableLimit", pjd.LimitEnabled);
-                        WriteElement("EnableMotor", pjd.MotorEnabled);
-                        WriteElement("LocalAnchorA", pjd.LocalAnchorA);
-                        WriteElement("LocalAnchorB", pjd.LocalAnchorB);
-                        WriteElement("LocalXAxis1", pjd.LocalXAxis1);
-                        WriteElement("LowerTranslation", pjd.LowerLimit);
-                        WriteElement("UpperTranslation", pjd.UpperLimit);
-                        WriteElement("MaxMotorForce", pjd.MaxMotorForce);
-                        WriteElement("MotorSpeed", pjd.MotorSpeed);
-                    }
-                    break;
-                case JointType.Pulley:
-                    {
-                        PulleyJoint pjd = (PulleyJoint)joint;
-
-                        WriteElement("GroundAnchorA", pjd.GroundAnchorA);
-                        WriteElement("GroundAnchorB", pjd.GroundAnchorB);
-                        WriteElement("LengthA", pjd.LengthA);
-                        WriteElement("LengthB", pjd.LengthB);
-                        WriteElement("LocalAnchorA", pjd.LocalAnchorA);
-                        WriteElement("LocalAnchorB", pjd.LocalAnchorB);
-                        WriteElement("MaxLengthA", pjd.MaxLengthA);
-                        WriteElement("MaxLengthB", pjd.MaxLengthB);
-                        WriteElement("Ratio", pjd.Ratio);
-                    }
-                    break;
-                case JointType.Revolute:
-                    {
-                        RevoluteJoint rjd = (RevoluteJoint)joint;
-
-                        WriteElement("EnableLimit", rjd.LimitEnabled);
-                        WriteElement("EnableMotor", rjd.MotorEnabled);
-                        WriteElement("LocalAnchorA", rjd.LocalAnchorA);
-                        WriteElement("LocalAnchorB", rjd.LocalAnchorB);
-                        WriteElement("LowerAngle", rjd.LowerLimit);
-                        WriteElement("MaxMotorTorque", rjd.MaxMotorTorque);
-                        WriteElement("MotorSpeed", rjd.MotorSpeed);
-                        WriteElement("ReferenceAngle", rjd.ReferenceAngle);
-                        WriteElement("UpperAngle", rjd.UpperLimit);
-                    }
-                    break;
-                case JointType.Weld:
-                    {
-                        WeldJoint wjd = (WeldJoint)joint;
-
-                        WriteElement("LocalAnchorA", wjd.LocalAnchorA);
-                        WriteElement("LocalAnchorB", wjd.LocalAnchorB);
-                    }
-                    break;
-                //
-                // Not part of Box2DScene
-                //
-                case JointType.Rope:
-                    {
-                        RopeJoint rjd = (RopeJoint)joint;
-
-                        WriteElement("LocalAnchorA", rjd.LocalAnchorA);
-                        WriteElement("LocalAnchorB", rjd.LocalAnchorB);
-                        WriteElement("MaxLength", rjd.MaxLength);
-                    }
-                    break;
-                case JointType.Angle:
-                    {
-                        AngleJoint aj = (AngleJoint)joint;
-                        WriteElement("BiasFactor", aj.BiasFactor);
-                        WriteElement("MaxImpulse", aj.MaxImpulse);
-                        WriteElement("Softness", aj.Softness);
-                        WriteElement("TargetAngle", aj.TargetAngle);
-                    }
-                    break;
-                case JointType.Slider:
-                    {
-                        SliderJoint sliderJoint = (SliderJoint)joint;
-                        WriteElement("DampingRatio", sliderJoint.DampingRatio);
-                        WriteElement("FrequencyHz", sliderJoint.Frequency);
-                        WriteElement("MaxLength", sliderJoint.MaxLength);
-                        WriteElement("MinLength", sliderJoint.MinLength);
-                        WriteElement("LocalAnchorA", sliderJoint.LocalAnchorA);
-                        WriteElement("LocalAnchorB", sliderJoint.LocalAnchorB);
-                    }
-                    break;
-                default:
-                    throw new Exception("Joint not supported");
-            }
-
-            _writer.WriteEndElement();
-        }
-
-        private void WriteDynamicType(Type type, object val)
-        {
-            _writer.WriteElementString("Type", type.FullName);
-
-            _writer.WriteStartElement("Value");
-            XmlSerializer serializer = new XmlSerializer(type);
-            XmlSerializerNamespaces xmlnsEmpty = new XmlSerializerNamespaces();
-            xmlnsEmpty.Add("", "");
-            serializer.Serialize(_writer, val, xmlnsEmpty);
-            _writer.WriteEndElement();
-        }
-
-        private void WriteElement(string name, Vector2 vec)
-        {
-            _writer.WriteElementString(name, vec.X + " " + vec.Y);
-        }
-
-        private void WriteElement(string name, int val)
-        {
-            _writer.WriteElementString(name, val.ToString());
-        }
-
-        private void WriteElement(string name, bool val)
-        {
-            _writer.WriteElementString(name, val.ToString());
-        }
-
-        private void WriteElement(string name, float val)
-        {
-            _writer.WriteElementString(name, val.ToString());
-        }
-
-        public void Serialize(World world, Stream stream)
-        {
-            XmlWriterSettings settings = new XmlWriterSettings();
-            settings.Indent = true;
-            settings.NewLineOnAttributes = false;
-            settings.OmitXmlDeclaration = true;
-
-            _writer = XmlWriter.Create(stream, settings);
-
-            _writer.WriteStartElement("World");
-            _writer.WriteAttributeString("Version", "2");
-            WriteElement("Gravity", world.Gravity);
-
-            _writer.WriteStartElement("Shapes");
-
-            for (int i = 0; i < world.BodyList.Count; i++)
-            {
-                Body body = world.BodyList[i];
-                for (int j = 0; j < body.FixtureList.Count; j++)
-                {
-                    Fixture fixture = body.FixtureList[j];
-
-                    bool alreadyThere = false;
-                    for (int k = 0; k < _serializedShapes.Count; k++)
-                    {
-                        Shape s2 = _serializedShapes[k];
-                        if (fixture.Shape.CompareTo(s2))
-                        {
-                            alreadyThere = true;
-                            break;
-                        }
-                    }
-
-                    if (!alreadyThere)
-                    {
-                        SerializeShape(fixture.Shape);
-                        _serializedShapes.Add(fixture.Shape);
-                    }
-                }
-            }
-
-            _writer.WriteEndElement();
-            _writer.WriteStartElement("Fixtures");
-
-
-            for (int i = 0; i < world.BodyList.Count; i++)
-            {
-                Body body = world.BodyList[i];
-                for (int j = 0; j < body.FixtureList.Count; j++)
-                {
-                    Fixture fixture = body.FixtureList[j];
-                    bool alreadyThere = false;
-                    for (int k = 0; k < _serializedFixtures.Count; k++)
-                    {
-                        Fixture f2 = _serializedFixtures[k];
-                        if (fixture.CompareTo(f2))
-                        {
-                            alreadyThere = true;
-                            break;
-                        }
-                    }
-
-                    if (!alreadyThere)
-                    {
-                        SerializeFixture(fixture);
-                        _serializedFixtures.Add(fixture);
-                    }
-                }
-            }
-
-            _writer.WriteEndElement();
-            _writer.WriteStartElement("Bodies");
-
-            for (int i = 0; i < world.BodyList.Count; i++)
-            {
-                Body body = world.BodyList[i];
-                _bodies.Add(body);
-                SerializeBody(body);
-            }
-
-            _writer.WriteEndElement();
-            _writer.WriteStartElement("Joints");
-
-            for (int i = 0; i < world.JointList.Count; i++)
-            {
-                Joint joint = world.JointList[i];
-                SerializeJoint(joint);
-            }
-
-            _writer.WriteEndElement();
-            _writer.WriteEndElement();
-
-            _writer.Flush();
-            _writer.Close();
-        }
-
-        private int FindBodyIndex(Body body)
-        {
-            for (int i = 0; i < _bodies.Count; ++i)
-                if (_bodies[i] == body)
-                    return i;
-
-            return -1;
-        }
-
-        private int FindFixtureIndex(Fixture fixture)
-        {
-            for (int i = 0; i < _serializedFixtures.Count; ++i)
-            {
-                if (_serializedFixtures[i].CompareTo(fixture))
-                    return i;
-            }
-
-            return -1;
-        }
-
-        private int FindShapeIndex(Shape shape)
-        {
-            for (int i = 0; i < _serializedShapes.Count; ++i)
-            {
-                if (_serializedShapes[i].CompareTo(shape))
-                    return i;
-            }
-
-            return -1;
-        }
-    }
-
-    public class WorldXmlDeserializer
-    {
-        private List<Body> _bodies = new List<Body>();
-        private List<Fixture> _fixtures = new List<Fixture>();
-        private List<Joint> _joints = new List<Joint>();
-        private List<Shape> _shapes = new List<Shape>();
-
-        public World Deserialize(Stream stream)
-        {
-            World world = new World(Vector2.Zero);
-            Deserialize(world, stream);
-            return world;
-        }
-
-        public void Deserialize(World world, Stream stream)
-        {
-            world.Clear();
-
-            XMLFragmentElement root = XMLFragmentParser.LoadFromStream(stream);
-
-            if (root.Name.ToLower() != "world")
-                throw new Exception();
-
-            foreach (XMLFragmentElement main in root.Elements)
-            {
-                if (main.Name.ToLower() == "gravity")
-                {
-                    world.Gravity = ReadVector(main);
-                }
-            }
-
-            foreach (XMLFragmentElement shapeElement in root.Elements)
-            {
-                if (shapeElement.Name.ToLower() == "shapes")
-                {
-                    foreach (XMLFragmentElement n in shapeElement.Elements)
-                    {
-                        if (n.Name.ToLower() != "shape")
-                            throw new Exception();
-
-                        ShapeType type = (ShapeType)Enum.Parse(typeof(ShapeType), n.Attributes[0].Value, true);
-
-                        switch (type)
-                        {
-                            case ShapeType.Circle:
-                                {
-                                    CircleShape shape = new CircleShape();
-
-                                    foreach (XMLFragmentElement sn in n.Elements)
-                                    {
-                                        switch (sn.Name.ToLower())
-                                        {
-                                            case "radius":
-                                                shape.Radius = float.Parse(sn.Value);
-                                                break;
-                                            case "position":
-                                                shape.Position = ReadVector(sn);
-                                                break;
-                                            default:
-                                                throw new Exception();
-                                        }
-                                    }
-
-                                    _shapes.Add(shape);
-                                }
-                                break;
-                            case ShapeType.Polygon:
-                                {
-                                    PolygonShape shape = new PolygonShape();
-
-                                    foreach (XMLFragmentElement sn in n.Elements)
-                                    {
-                                        switch (sn.Name.ToLower())
-                                        {
-                                            case "vertices":
-                                                {
-                                                    List<Vector2> verts = new List<Vector2>();
-
-                                                    foreach (XMLFragmentElement vert in sn.Elements)
-                                                        verts.Add(ReadVector(vert));
-
-                                                    shape.Set(new Vertices(verts.ToArray()));
-                                                }
-                                                break;
-                                            case "centroid":
-                                                shape.MassData.Centroid = ReadVector(sn);
-                                                break;
-                                        }
-                                    }
-
-                                    _shapes.Add(shape);
-                                }
-                                break;
-                            case ShapeType.Edge:
-                                {
-                                    EdgeShape shape = new EdgeShape();
-                                    foreach (XMLFragmentElement sn in n.Elements)
-                                    {
-                                        switch (sn.Name.ToLower())
-                                        {
-                                            case "hasvertex0":
-                                                shape.HasVertex0 = bool.Parse(sn.Value);
-                                                break;
-                                            case "hasvertex3":
-                                                shape.HasVertex0 = bool.Parse(sn.Value);
-                                                break;
-                                            case "vertex0":
-                                                shape.Vertex0 = ReadVector(sn);
-                                                break;
-                                            case "vertex1":
-                                                shape.Vertex1 = ReadVector(sn);
-                                                break;
-                                            case "vertex2":
-                                                shape.Vertex2 = ReadVector(sn);
-                                                break;
-                                            case "vertex3":
-                                                shape.Vertex3 = ReadVector(sn);
-                                                break;
-                                            default:
-                                                throw new Exception();
-                                        }
-                                    }
-                                    _shapes.Add(shape);
-                                }
-                                break;
-                        }
-                    }
-                }
-            }
-
-            foreach (XMLFragmentElement fixtureElement in root.Elements)
-            {
-                if (fixtureElement.Name.ToLower() == "fixtures")
-                {
-                    foreach (XMLFragmentElement n in fixtureElement.Elements)
-                    {
-                        Fixture fixture = new Fixture();
-
-                        if (n.Name.ToLower() != "fixture")
-                            throw new Exception();
-
-                        foreach (XMLFragmentElement sn in n.Elements)
-                        {
-                            switch (sn.Name.ToLower())
-                            {
-                                case "shape":
-                                    fixture.Shape = _shapes[int.Parse(sn.Value)];
-                                    break;
-                                case "density":
-                                    fixture.Shape.Density = float.Parse(sn.Value);
-                                    break;
-                                case "filterdata":
-                                    foreach (XMLFragmentElement ssn in sn.Elements)
-                                    {
-                                        switch (ssn.Name.ToLower())
-                                        {
-                                            case "categorybits":
-                                                fixture._collisionCategories = (Category)int.Parse(ssn.Value);
-                                                break;
-                                            case "maskbits":
-                                                fixture._collidesWith = (Category)int.Parse(ssn.Value);
-                                                break;
-                                            case "groupindex":
-                                                fixture._collisionGroup = short.Parse(ssn.Value);
-                                                break;
-                                        }
-                                    }
-
-                                    break;
-                                case "friction":
-                                    fixture.Friction = float.Parse(sn.Value);
-                                    break;
-                                case "issensor":
-                                    fixture.IsSensor = bool.Parse(sn.Value);
-                                    break;
-                                case "restitution":
-                                    fixture.Restitution = float.Parse(sn.Value);
-                                    break;
-                                case "userdata":
-                                    fixture.UserData = ReadSimpleType(sn, null, false);
-                                    break;
-                            }
-                        }
-
-                        _fixtures.Add(fixture);
-                    }
-                }
-            }
-
-            foreach (XMLFragmentElement bodyElement in root.Elements)
-            {
-                if (bodyElement.Name.ToLower() == "bodies")
-                {
-                    foreach (XMLFragmentElement n in bodyElement.Elements)
-                    {
-                        Body body = new Body(world);
-
-                        if (n.Name.ToLower() != "body")
-                            throw new Exception();
-
-                        body.BodyType = (BodyType)Enum.Parse(typeof(BodyType), n.Attributes[0].Value, true);
-
-                        foreach (XMLFragmentElement sn in n.Elements)
-                        {
-                            switch (sn.Name.ToLower())
-                            {
-                                case "active":
-                                    if (bool.Parse(sn.Value))
-                                        body.Flags |= BodyFlags.Enabled;
-                                    else
-                                        body.Flags &= ~BodyFlags.Enabled;
-                                    break;
-                                case "allowsleep":
-                                    body.SleepingAllowed = bool.Parse(sn.Value);
-                                    break;
-                                case "angle":
-                                    {
-                                        Vector2 position = body.Position;
-                                        body.SetTransformIgnoreContacts(ref position, float.Parse(sn.Value));
-                                    }
-                                    break;
-                                case "angulardamping":
-                                    body.AngularDamping = float.Parse(sn.Value);
-                                    break;
-                                case "angularvelocity":
-                                    body.AngularVelocity = float.Parse(sn.Value);
-                                    break;
-                                case "awake":
-                                    body.Awake = bool.Parse(sn.Value);
-                                    break;
-                                case "bullet":
-                                    body.IsBullet = bool.Parse(sn.Value);
-                                    break;
-                                case "fixedrotation":
-                                    body.FixedRotation = bool.Parse(sn.Value);
-                                    break;
-                                case "lineardamping":
-                                    body.LinearDamping = float.Parse(sn.Value);
-                                    break;
-                                case "linearvelocity":
-                                    body.LinearVelocity = ReadVector(sn);
-                                    break;
-                                case "position":
-                                    {
-                                        float rotation = body.Rotation;
-                                        Vector2 position = ReadVector(sn);
-                                        body.SetTransformIgnoreContacts(ref position, rotation);
-                                    }
-                                    break;
-                                case "userdata":
-                                    body.UserData = ReadSimpleType(sn, null, false);
-                                    break;
-                                case "fixtures":
-                                    {
-                                        foreach (XMLFragmentElement v in sn.Elements)
-                                        {
-                                            Fixture blueprint = _fixtures[int.Parse(v.Value)];
-                                            Fixture f = new Fixture(body, blueprint.Shape);
-                                            f.Restitution = blueprint.Restitution;
-                                            f.UserData = blueprint.UserData;
-                                            f.Friction = blueprint.Friction;
-                                            f.CollidesWith = blueprint.CollidesWith;
-                                            f.CollisionCategories = blueprint.CollisionCategories;
-                                            f.CollisionGroup = blueprint.CollisionGroup;
-                                        }
-                                        break;
-                                    }
-                            }
-                        }
-
-                        _bodies.Add(body);
-                    }
-                }
-            }
-
-            foreach (XMLFragmentElement jointElement in root.Elements)
-            {
-                if (jointElement.Name.ToLower() == "joints")
-                {
-                    foreach (XMLFragmentElement n in jointElement.Elements)
-                    {
-                        Joint joint;
-
-                        if (n.Name.ToLower() != "joint")
-                            throw new Exception();
-
-                        JointType type = (JointType)Enum.Parse(typeof(JointType), n.Attributes[0].Value, true);
-
-                        int bodyAIndex = -1, bodyBIndex = -1;
-                        bool collideConnected = false;
-                        object userData = null;
-
-                        foreach (XMLFragmentElement sn in n.Elements)
-                        {
-                            switch (sn.Name.ToLower())
-                            {
-                                case "bodya":
-                                    bodyAIndex = int.Parse(sn.Value);
-                                    break;
-                                case "bodyb":
-                                    bodyBIndex = int.Parse(sn.Value);
-                                    break;
-                                case "collideconnected":
-                                    collideConnected = bool.Parse(sn.Value);
-                                    break;
-                                case "userdata":
-                                    userData = ReadSimpleType(sn, null, false);
-                                    break;
-                            }
-                        }
-
-                        Body bodyA = _bodies[bodyAIndex];
-                        Body bodyB = _bodies[bodyBIndex];
-
-                        switch (type)
-                        {
-                            case JointType.Distance:
-                                joint = new DistanceJoint();
-                                break;
-                            case JointType.Friction:
-                                joint = new FrictionJoint();
-                                break;
-                            case JointType.Line:
-                                joint = new LineJoint();
-                                break;
-                            case JointType.Prismatic:
-                                joint = new PrismaticJoint();
-                                break;
-                            case JointType.Pulley:
-                                joint = new PulleyJoint();
-                                break;
-                            case JointType.Revolute:
-                                joint = new RevoluteJoint();
-                                break;
-                            case JointType.Weld:
-                                joint = new WeldJoint();
-                                break;
-                            case JointType.Rope:
-                                joint = new RopeJoint();
-                                break;
-                            case JointType.Angle:
-                                joint = new AngleJoint();
-                                break;
-                            case JointType.Slider:
-                                joint = new SliderJoint();
-                                break;
-                            case JointType.Gear:
-                                throw new Exception("GearJoint is not supported.");
-                            default:
-                                throw new Exception("Invalid or unsupported joint.");
-                        }
-
-                        joint.CollideConnected = collideConnected;
-                        joint.UserData = userData;
-                        joint.BodyA = bodyA;
-                        joint.BodyB = bodyB;
-                        _joints.Add(joint);
-                        world.AddJoint(joint);
-
-                        foreach (XMLFragmentElement sn in n.Elements)
-                        {
-                            // check for specific nodes
-                            switch (type)
-                            {
-                                case JointType.Distance:
-                                    {
-                                        switch (sn.Name.ToLower())
-                                        {
-                                            case "dampingratio":
-                                                ((DistanceJoint)joint).DampingRatio = float.Parse(sn.Value);
-                                                break;
-                                            case "frequencyhz":
-                                                ((DistanceJoint)joint).Frequency = float.Parse(sn.Value);
-                                                break;
-                                            case "length":
-                                                ((DistanceJoint)joint).Length = float.Parse(sn.Value);
-                                                break;
-                                            case "localanchora":
-                                                ((DistanceJoint)joint).LocalAnchorA = ReadVector(sn);
-                                                break;
-                                            case "localanchorb":
-                                                ((DistanceJoint)joint).LocalAnchorB = ReadVector(sn);
-                                                break;
-                                        }
-                                    }
-                                    break;
-                                case JointType.Friction:
-                                    {
-                                        switch (sn.Name.ToLower())
-                                        {
-                                            case "localanchora":
-                                                ((FrictionJoint)joint).LocalAnchorA = ReadVector(sn);
-                                                break;
-                                            case "localanchorb":
-                                                ((FrictionJoint)joint).LocalAnchorB = ReadVector(sn);
-                                                break;
-                                            case "maxforce":
-                                                ((FrictionJoint)joint).MaxForce = float.Parse(sn.Value);
-                                                break;
-                                            case "maxtorque":
-                                                ((FrictionJoint)joint).MaxTorque = float.Parse(sn.Value);
-                                                break;
-                                        }
-                                    }
-                                    break;
-                                case JointType.Line:
-                                    {
-                                        switch (sn.Name.ToLower())
-                                        {
-                                            case "enablemotor":
-                                                ((LineJoint)joint).MotorEnabled = bool.Parse(sn.Value);
-                                                break;
-                                            case "localanchora":
-                                                ((LineJoint)joint).LocalAnchorA = ReadVector(sn);
-                                                break;
-                                            case "localanchorb":
-                                                ((LineJoint)joint).LocalAnchorB = ReadVector(sn);
-                                                break;
-                                            case "motorspeed":
-                                                ((LineJoint)joint).MotorSpeed = float.Parse(sn.Value);
-                                                break;
-                                            case "dampingratio":
-                                                ((LineJoint)joint).DampingRatio = float.Parse(sn.Value);
-                                                break;
-                                            case "maxmotortorque":
-                                                ((LineJoint)joint).MaxMotorTorque = float.Parse(sn.Value);
-                                                break;
-                                            case "frequencyhz":
-                                                ((LineJoint)joint).Frequency = float.Parse(sn.Value);
-                                                break;
-                                            case "localxaxis":
-                                                ((LineJoint)joint).LocalXAxis = ReadVector(sn);
-                                                break;
-                                        }
-                                    }
-                                    break;
-                                case JointType.Prismatic:
-                                    {
-                                        switch (sn.Name.ToLower())
-                                        {
-                                            case "enablelimit":
-                                                ((PrismaticJoint)joint).LimitEnabled = bool.Parse(sn.Value);
-                                                break;
-                                            case "enablemotor":
-                                                ((PrismaticJoint)joint).MotorEnabled = bool.Parse(sn.Value);
-                                                break;
-                                            case "localanchora":
-                                                ((PrismaticJoint)joint).LocalAnchorA = ReadVector(sn);
-                                                break;
-                                            case "localanchorb":
-                                                ((PrismaticJoint)joint).LocalAnchorB = ReadVector(sn);
-                                                break;
-                                            case "local1axis1":
-                                                ((PrismaticJoint)joint).LocalXAxis1 = ReadVector(sn);
-                                                break;
-                                            case "maxmotorforce":
-                                                ((PrismaticJoint)joint).MaxMotorForce = float.Parse(sn.Value);
-                                                break;
-                                            case "motorspeed":
-                                                ((PrismaticJoint)joint).MotorSpeed = float.Parse(sn.Value);
-                                                break;
-                                            case "lowertranslation":
-                                                ((PrismaticJoint)joint).LowerLimit = float.Parse(sn.Value);
-                                                break;
-                                            case "uppertranslation":
-                                                ((PrismaticJoint)joint).UpperLimit = float.Parse(sn.Value);
-                                                break;
-                                            case "referenceangle":
-                                                ((PrismaticJoint)joint).ReferenceAngle = float.Parse(sn.Value);
-                                                break;
-                                        }
-                                    }
-                                    break;
-                                case JointType.Pulley:
-                                    {
-                                        switch (sn.Name.ToLower())
-                                        {
-                                            case "groundanchora":
-                                                ((PulleyJoint)joint).GroundAnchorA = ReadVector(sn);
-                                                break;
-                                            case "groundanchorb":
-                                                ((PulleyJoint)joint).GroundAnchorB = ReadVector(sn);
-                                                break;
-                                            case "lengtha":
-                                                ((PulleyJoint)joint).LengthA = float.Parse(sn.Value);
-                                                break;
-                                            case "lengthb":
-                                                ((PulleyJoint)joint).LengthB = float.Parse(sn.Value);
-                                                break;
-                                            case "localanchora":
-                                                ((PulleyJoint)joint).LocalAnchorA = ReadVector(sn);
-                                                break;
-                                            case "localanchorb":
-                                                ((PulleyJoint)joint).LocalAnchorB = ReadVector(sn);
-                                                break;
-                                            case "maxlengtha":
-                                                ((PulleyJoint)joint).MaxLengthA = float.Parse(sn.Value);
-                                                break;
-                                            case "maxlengthb":
-                                                ((PulleyJoint)joint).MaxLengthB = float.Parse(sn.Value);
-                                                break;
-                                            case "ratio":
-                                                ((PulleyJoint)joint).Ratio = float.Parse(sn.Value);
-                                                break;
-                                        }
-                                    }
-                                    break;
-                                case JointType.Revolute:
-                                    {
-                                        switch (sn.Name.ToLower())
-                                        {
-                                            case "enablelimit":
-                                                ((RevoluteJoint)joint).LimitEnabled = bool.Parse(sn.Value);
-                                                break;
-                                            case "enablemotor":
-                                                ((RevoluteJoint)joint).MotorEnabled = bool.Parse(sn.Value);
-                                                break;
-                                            case "localanchora":
-                                                ((RevoluteJoint)joint).LocalAnchorA = ReadVector(sn);
-                                                break;
-                                            case "localanchorb":
-                                                ((RevoluteJoint)joint).LocalAnchorB = ReadVector(sn);
-                                                break;
-                                            case "maxmotortorque":
-                                                ((RevoluteJoint)joint).MaxMotorTorque = float.Parse(sn.Value);
-                                                break;
-                                            case "motorspeed":
-                                                ((RevoluteJoint)joint).MotorSpeed = float.Parse(sn.Value);
-                                                break;
-                                            case "lowerangle":
-                                                ((RevoluteJoint)joint).LowerLimit = float.Parse(sn.Value);
-                                                break;
-                                            case "upperangle":
-                                                ((RevoluteJoint)joint).UpperLimit = float.Parse(sn.Value);
-                                                break;
-                                            case "referenceangle":
-                                                ((RevoluteJoint)joint).ReferenceAngle = float.Parse(sn.Value);
-                                                break;
-                                        }
-                                    }
-                                    break;
-                                case JointType.Weld:
-                                    {
-                                        switch (sn.Name.ToLower())
-                                        {
-                                            case "localanchora":
-                                                ((WeldJoint)joint).LocalAnchorA = ReadVector(sn);
-                                                break;
-                                            case "localanchorb":
-                                                ((WeldJoint)joint).LocalAnchorB = ReadVector(sn);
-                                                break;
-                                        }
-                                    }
-                                    break;
-                                case JointType.Rope:
-                                    {
-                                        switch (sn.Name.ToLower())
-                                        {
-                                            case "localanchora":
-                                                ((RopeJoint)joint).LocalAnchorA = ReadVector(sn);
-                                                break;
-                                            case "localanchorb":
-                                                ((RopeJoint)joint).LocalAnchorB = ReadVector(sn);
-                                                break;
-                                            case "maxlength":
-                                                ((RopeJoint)joint).MaxLength = float.Parse(sn.Value);
-                                                break;
-                                        }
-                                    }
-                                    break;
-                                case JointType.Gear:
-                                    throw new Exception("Gear joint is unsupported");
-                                case JointType.Angle:
-                                    {
-                                        switch (sn.Name.ToLower())
-                                        {
-                                            case "biasfactor":
-                                                ((AngleJoint)joint).BiasFactor = float.Parse(sn.Value);
-                                                break;
-                                            case "maximpulse":
-                                                ((AngleJoint)joint).MaxImpulse = float.Parse(sn.Value);
-                                                break;
-                                            case "softness":
-                                                ((AngleJoint)joint).Softness = float.Parse(sn.Value);
-                                                break;
-                                            case "targetangle":
-                                                ((AngleJoint)joint).TargetAngle = float.Parse(sn.Value);
-                                                break;
-                                        }
-                                    }
-                                    break;
-                                case JointType.Slider:
-                                    {
-                                        switch (sn.Name.ToLower())
-                                        {
-                                            case "dampingratio":
-                                                ((SliderJoint)joint).DampingRatio = float.Parse(sn.Value);
-                                                break;
-                                            case "frequencyhz":
-                                                ((SliderJoint)joint).Frequency = float.Parse(sn.Value);
-                                                break;
-                                            case "maxlength":
-                                                ((SliderJoint)joint).MaxLength = float.Parse(sn.Value);
-                                                break;
-                                            case "minlength":
-                                                ((SliderJoint)joint).MinLength = float.Parse(sn.Value);
-                                                break;
-                                            case "localanchora":
-                                                ((SliderJoint)joint).LocalAnchorA = ReadVector(sn);
-                                                break;
-                                            case "localanchorb":
-                                                ((SliderJoint)joint).LocalAnchorB = ReadVector(sn);
-                                                break;
-                                        }
-                                    }
-                                    break;
-                            }
-                        }
-                    }
-                }
-            }
-        }
-
-        private Vector2 ReadVector(XMLFragmentElement node)
-        {
-            string[] values = node.Value.Split(' ');
-            return new Vector2(float.Parse(values[0]), float.Parse(values[1]));
-        }
-
-        private object ReadSimpleType(XMLFragmentElement node, Type type, bool outer)
-        {
-            if (type == null)
-                return ReadSimpleType(node.Elements[1], Type.GetType(node.Elements[0].Value), outer);
-
-            XmlSerializer serializer = new XmlSerializer(type);
-            XmlSerializerNamespaces xmlnsEmpty = new XmlSerializerNamespaces();
-            xmlnsEmpty.Add("", "");
-
-            using (MemoryStream stream = new MemoryStream())
-            {
-                StreamWriter writer = new StreamWriter(stream);
-                {
-                    writer.Write((outer) ? node.OuterXml : node.InnerXml);
-                    writer.Flush();
-                    stream.Position = 0;
-                }
-                XmlReaderSettings settings = new XmlReaderSettings();
-                settings.ConformanceLevel = ConformanceLevel.Fragment;
-
-                return serializer.Deserialize(XmlReader.Create(stream, settings));
-            }
-        }
-    }
-
-    #region XMLFragment
-
-    public class XMLFragmentAttribute
-    {
-        public string Name { get; set; }
-
-        public string Value { get; set; }
-    }
-
-    public class XMLFragmentElement
-    {
-        private List<XMLFragmentAttribute> _attributes = new List<XMLFragmentAttribute>();
-        private List<XMLFragmentElement> _elements = new List<XMLFragmentElement>();
-
-        public IList<XMLFragmentElement> Elements
-        {
-            get { return _elements; }
-        }
-
-        public IList<XMLFragmentAttribute> Attributes
-        {
-            get { return _attributes; }
-        }
-
-        public string Name { get; set; }
-
-        public string Value { get; set; }
-
-        public string OuterXml { get; set; }
-
-        public string InnerXml { get; set; }
-    }
-
-    public class XMLFragmentException : Exception
-    {
-        public XMLFragmentException()
-        {
-        }
-
-        public XMLFragmentException(string message)
-            : base(message)
-        {
-        }
-
-        public XMLFragmentException(string message, Exception inner)
-            : base(message, inner)
-        {
-        }
-    }
-
-    public class FileBuffer
-    {
-        public FileBuffer(Stream stream)
-        {
-            using (StreamReader sr = new StreamReader(stream))
-                Buffer = sr.ReadToEnd();
-
-            Position = 0;
-        }
-
-        public string Buffer { get; set; }
-
-        public int Position { get; set; }
-
-        public int Length
-        {
-            get { return Buffer.Length; }
-        }
-
-        public char Next
-        {
-            get
-            {
-                char c = Buffer[Position];
-                Position++;
-                return c;
-            }
-        }
-
-        public char Peek
-        {
-            get { return Buffer[Position]; }
-        }
-
-        public bool EndOfBuffer
-        {
-            get { return Position == Length; }
-        }
-    }
-
-    public class XMLFragmentParser
-    {
-        private static List<char> _punctuation = new List<char> { '/', '<', '>', '=' };
-        private FileBuffer _buffer;
-        private XMLFragmentElement _rootNode;
-
-        public XMLFragmentParser(Stream stream)
-        {
-            Load(stream);
-        }
-
-        public XMLFragmentParser(string fileName)
-        {
-            using (FileStream fs = new FileStream(fileName, FileMode.Open, FileAccess.Read))
-                Load(fs);
-        }
-
-        public XMLFragmentElement RootNode
-        {
-            get { return _rootNode; }
-        }
-
-        public void Load(Stream stream)
-        {
-            _buffer = new FileBuffer(stream);
-        }
-
-        public static XMLFragmentElement LoadFromFile(string fileName)
-        {
-            XMLFragmentParser x = new XMLFragmentParser(fileName);
-            x.Parse();
-            return x.RootNode;
-        }
-
-        public static XMLFragmentElement LoadFromStream(Stream stream)
-        {
-            XMLFragmentParser x = new XMLFragmentParser(stream);
-            x.Parse();
-            return x.RootNode;
-        }
-
-        private string NextToken()
-        {
-            string str = "";
-            bool _done = false;
-
-            while (true)
-            {
-                char c = _buffer.Next;
-
-                if (_punctuation.Contains(c))
-                {
-                    if (str != "")
-                    {
-                        _buffer.Position--;
-                        break;
-                    }
-
-                    _done = true;
-                }
-                else if (char.IsWhiteSpace(c))
-                {
-                    if (str != "")
-                        break;
-                    else
-                        continue;
-                }
-
-                str += c;
-
-                if (_done)
-                    break;
-            }
-
-            str = TrimControl(str);
-
-            // Trim quotes from start and end
-            if (str[0] == '\"')
-                str = str.Remove(0, 1);
-
-            if (str[str.Length - 1] == '\"')
-                str = str.Remove(str.Length - 1, 1);
-
-            return str;
-        }
-
-        private string PeekToken()
-        {
-            int oldPos = _buffer.Position;
-            string str = NextToken();
-            _buffer.Position = oldPos;
-            return str;
-        }
-
-        private string ReadUntil(char c)
-        {
-            string str = "";
-
-            while (true)
-            {
-                char ch = _buffer.Next;
-
-                if (ch == c)
-                {
-                    _buffer.Position--;
-                    break;
-                }
-
-                str += ch;
-            }
-
-            // Trim quotes from start and end
-            if (str[0] == '\"')
-                str = str.Remove(0, 1);
-
-            if (str[str.Length - 1] == '\"')
-                str = str.Remove(str.Length - 1, 1);
-
-            return str;
-        }
-
-        private string TrimControl(string str)
-        {
-            string newStr = str;
-
-            // Trim control characters
-            int i = 0;
-            while (true)
-            {
-                if (i == newStr.Length)
-                    break;
-
-                if (char.IsControl(newStr[i]))
-                    newStr = newStr.Remove(i, 1);
-                else
-                    i++;
-            }
-
-            return newStr;
-        }
-
-        private string TrimTags(string outer)
-        {
-            int start = outer.IndexOf('>') + 1;
-            int end = outer.LastIndexOf('<');
-
-            return TrimControl(outer.Substring(start, end - start));
-        }
-
-        public XMLFragmentElement TryParseNode()
-        {
-            if (_buffer.EndOfBuffer)
-                return null;
-
-            int startOuterXml = _buffer.Position;
-            string token = NextToken();
-
-            if (token != "<")
-                throw new XMLFragmentException("Expected \"<\", got " + token);
-
-            XMLFragmentElement element = new XMLFragmentElement();
-            element.Name = NextToken();
-
-            while (true)
-            {
-                token = NextToken();
-
-                if (token == ">")
-                    break;
-                else if (token == "/") // quick-exit case
-                {
-                    NextToken();
-
-                    element.OuterXml =
-                        TrimControl(_buffer.Buffer.Substring(startOuterXml, _buffer.Position - startOuterXml)).Trim();
-                    element.InnerXml = "";
-
-                    return element;
-                }
-                else
-                {
-                    XMLFragmentAttribute attribute = new XMLFragmentAttribute();
-                    attribute.Name = token;
-                    if ((token = NextToken()) != "=")
-                        throw new XMLFragmentException("Expected \"=\", got " + token);
-                    attribute.Value = NextToken();
-
-                    element.Attributes.Add(attribute);
-                }
-            }
-
-            while (true)
-            {
-                int oldPos = _buffer.Position; // for restoration below
-                token = NextToken();
-
-                if (token == "<")
-                {
-                    token = PeekToken();
-
-                    if (token == "/") // finish element
-                    {
-                        NextToken(); // skip the / again
-                        token = NextToken();
-                        NextToken(); // skip >
-
-                        element.OuterXml =
-                            TrimControl(_buffer.Buffer.Substring(startOuterXml, _buffer.Position - startOuterXml)).Trim();
-                        element.InnerXml = TrimTags(element.OuterXml);
-
-                        if (token != element.Name)
-                            throw new XMLFragmentException("Mismatched element pairs: \"" + element.Name + "\" vs \"" +
-                                                           token + "\"");
-
-                        break;
-                    }
-                    else
-                    {
-                        _buffer.Position = oldPos;
-                        element.Elements.Add(TryParseNode());
-                    }
-                }
-                else
-                {
-                    // value, probably
-                    _buffer.Position = oldPos;
-                    element.Value = ReadUntil('<');
-                }
-            }
-
-            return element;
-        }
-
-        public void Parse()
-        {
-            _rootNode = TryParseNode();
-
-            if (_rootNode == null)
-                throw new XMLFragmentException("Unable to load root node");
-        }
-    }
-
-    #endregion
-}

FarseerPhysicsEngine/Common/TextureTools/MSTerrain.cs

@@ -1,367 +0,0 @@
-using System.Collections.Generic;
-using Microsoft.Xna.Framework;
-using Microsoft.Xna.Framework.Graphics;
-using FarseerPhysics.Dynamics;
-using FarseerPhysics.Collision;
-using FarseerPhysics.Factories;
-
-namespace FarseerPhysics.Common
-{
-    public enum Decomposer
-    {
-        Bayazit,
-        CDT,
-        Earclip,
-        Flipcode,
-        Seidel,
-    }
-
-    /// <summary>
-    /// Return true if the specified color is inside the terrain.
-    /// </summary>
-    public delegate bool TerrainTester(Color color);
-
-    /// <summary>
-    /// Simple class to maintain a terrain.
-    /// </summary>
-    public class MSTerrain
-    {
-        /// <summary>
-        /// World to manage terrain in.
-        /// </summary>
-        public World World;
-
-        /// <summary>
-        /// Center of terrain in world units.
-        /// </summary>
-        public Vector2 Center;
-
-        /// <summary>
-        /// Width of terrain in world units.
-        /// </summary>
-        public float Width;
-
-        /// <summary>
-        /// Height of terrain in world units.
-        /// </summary>
-        public float Height;
-
-        /// <summary>
-        /// Points per each world unit used to define the terrain in the point cloud.
-        /// </summary>
-        public int PointsPerUnit;
-
-        /// <summary>
-        /// Points per cell.
-        /// </summary>
-        public int CellSize;
-
-        /// <summary>
-        /// Points per sub cell.
-        /// </summary>
-        public int SubCellSize;
-
-        /// <summary>
-        /// Number of iterations to perform in the Marching Squares algorithm.
-        /// Note: More then 3 has almost no effect on quality.
-        /// </summary>
-        public int Iterations = 2;
-
-        /// <summary>
-        /// Decomposer to use when regenerating terrain. Can be changed on the fly without consequence.
-        /// Note: Some decomposerers are unstable.
-        /// </summary>
-        public Decomposer Decomposer;
-
-        /// <summary>
-        /// Point cloud defining the terrain.
-        /// </summary>
-        private sbyte[,] _terrainMap;
-
-        /// <summary>
-        /// Generated bodies.
-        /// </summary>
-        private List<Body>[,] _bodyMap;
-
-        private float _localWidth;
-        private float _localHeight;
-        private int _xnum;
-        private int _ynum;
-        private AABB _dirtyArea;
-        private Vector2 _topLeft;
-
-        public MSTerrain(World world, AABB area)
-        {
-            World = world;
-            Width = area.Extents.X * 2;
-            Height = area.Extents.Y * 2;
-            Center = area.Center;
-        }
-
-        /// <summary>
-        /// Initialize the terrain for use.
-        /// </summary>
-        public void Initialize()
-        {
-            // find top left of terrain in world space
-            _topLeft = new Vector2(Center.X - (Width * 0.5f), Center.Y - (-Height * 0.5f));
-
-            // convert the terrains size to a point cloud size
-            _localWidth = Width * PointsPerUnit;
-            _localHeight = Height * PointsPerUnit;
-
-            _terrainMap = new sbyte[(int)_localWidth + 1, (int)_localHeight + 1];
-
-            for (int x = 0; x < _localWidth; x++)
-            {
-                for (int y = 0; y < _localHeight; y++)
-                {
-                    _terrainMap[x, y] = 1;
-                }
-            }
-
-            _xnum = (int)(_localWidth / CellSize);
-            _ynum = (int)(_localHeight / CellSize);
-            _bodyMap = new List<Body>[_xnum, _ynum];
-
-            // make sure to mark the dirty area to an infinitely small box
-            _dirtyArea = new AABB(new Vector2(float.MaxValue, float.MaxValue), new Vector2(float.MinValue, float.MinValue));
-        }
-
-        /// <summary>
-        /// Apply a texture to the terrain using the specified TerrainTester.
-        /// </summary>
-        /// <param name="texture">Texture to apply.</param>
-        /// <param name="position">Top left position of the texture relative to the terrain.</param>
-        /// <param name="tester">Delegate method used to determine what colors should be included in the terrain.</param>
-        public void ApplyTexture(Texture2D texture, Vector2 position, TerrainTester tester)
-        {
-            Color[] colorData = new Color[texture.Width * texture.Height];
-
-            texture.GetData(colorData);
-
-            for (int y = (int)position.Y; y < texture.Height + (int)position.Y; y++)
-            {
-                for (int x = (int)position.X; x < texture.Width + (int)position.X; x++)
-                {
-                    if (x >= 0 && x < _localWidth && y >= 0 && y < _localHeight)
-                    {
-                        bool inside = tester(colorData[((y - (int)position.Y) * texture.Width) + (x - (int)position.X)]);
-
-                        if (!inside)
-                            _terrainMap[x, y] = 1;
-                        else
-                            _terrainMap[x, y] = -1;
-                    }
-                }
-            }
-
-            // generate terrain
-            for (int gy = 0; gy < _ynum; gy++)
-            {
-                for (int gx = 0; gx < _xnum; gx++)
-                {
-                    //remove old terrain object at grid cell
-                    if (_bodyMap[gx, gy] != null)
-                    {
-                        for (int i = 0; i < _bodyMap[gx, gy].Count; i++)
-                        {
-                            World.RemoveBody(_bodyMap[gx, gy][i]);
-                        }
-                    }
-
-                    _bodyMap[gx, gy] = null;
-
-                    //generate new one
-                    GenerateTerrain(gx, gy);
-                }
-            }
-        }
-
-        /// <summary>
-        /// Apply a texture to the terrain using the specified TerrainTester.
-        /// </summary>
-        /// <param name="position">Top left position of the texture relative to the terrain.</param>
-        public void ApplyData(sbyte[,] data, Vector2 position)
-        {
-            for (int y = (int)position.Y; y < data.GetUpperBound(1) + (int)position.Y; y++)
-            {
-                for (int x = (int)position.X; x < data.GetUpperBound(0) + (int)position.X; x++)
-                {
-                    if (x >= 0 && x < _localWidth && y >= 0 && y < _localHeight)
-                    {
-                        _terrainMap[x, y] = data[x, y];
-                    }
-                }
-            }
-
-            // generate terrain
-            for (int gy = 0; gy < _ynum; gy++)
-            {
-                for (int gx = 0; gx < _xnum; gx++)
-                {
-                    //remove old terrain object at grid cell
-                    if (_bodyMap[gx, gy] != null)
-                    {
-                        for (int i = 0; i < _bodyMap[gx, gy].Count; i++)
-                        {
-                            World.RemoveBody(_bodyMap[gx, gy][i]);
-                        }
-                    }
-
-                    _bodyMap[gx, gy] = null;
-
-                    //generate new one
-                    GenerateTerrain(gx, gy);
-                }
-            }
-        }
-
-        /// <summary>
-        /// Convert a texture to an sbtye array compatible with ApplyData().
-        /// </summary>
-        /// <param name="texture">Texture to convert.</param>
-        /// <param name="tester"></param>
-        /// <returns></returns>
-        public static sbyte[,] ConvertTextureToData(Texture2D texture, TerrainTester tester)
-        {
-            sbyte[,] data = new sbyte[texture.Width, texture.Height];
-            Color[] colorData = new Color[texture.Width * texture.Height];
-
-            texture.GetData(colorData);
-
-            for (int y = 0; y < texture.Height; y++)
-            {
-                for (int x = 0; x < texture.Width; x++)
-                {
-                    bool inside = tester(colorData[(y * texture.Width) + x]);
-
-                    if (!inside)
-                        data[x, y] = 1;
-                    else
-                        data[x, y] = -1;
-                }
-            }
-
-            return data;
-        }
-
-        /// <summary>
-        /// Modify a single point in the terrain.
-        /// </summary>
-        /// <param name="location">World location to modify. Automatically clipped.</param>
-        /// <param name="value">-1 = inside terrain, 1 = outside terrain</param>
-        public void ModifyTerrain(Vector2 location, sbyte value)
-        {
-            // find local position
-            // make position local to map space
-            Vector2 p = location - _topLeft;
-
-            // find map position for each axis
-            p.X = p.X * _localWidth / Width;
-            p.Y = p.Y * -_localHeight / Height;
-
-            if (p.X >= 0 && p.X < _localWidth && p.Y >= 0 && p.Y < _localHeight)
-            {
-                _terrainMap[(int)p.X, (int)p.Y] = value;
-
-                // expand dirty area
-                if (p.X < _dirtyArea.LowerBound.X) _dirtyArea.LowerBound.X = p.X;
-                if (p.X > _dirtyArea.UpperBound.X) _dirtyArea.UpperBound.X = p.X;
-
-                if (p.Y < _dirtyArea.LowerBound.Y) _dirtyArea.LowerBound.Y = p.Y;
-                if (p.Y > _dirtyArea.UpperBound.Y) _dirtyArea.UpperBound.Y = p.Y;
-            }
-        }
-
-        /// <summary>
-        /// Regenerate the terrain.
-        /// </summary>
-        public void RegenerateTerrain()
-        {
-            //iterate effected cells
-            var gx0 = (int)(_dirtyArea.LowerBound.X / CellSize);
-            var gx1 = (int)(_dirtyArea.UpperBound.X / CellSize) + 1;
-            if (gx0 < 0) gx0 = 0;
-            if (gx1 > _xnum) gx1 = _xnum;
-            var gy0 = (int)(_dirtyArea.LowerBound.Y / CellSize);
-            var gy1 = (int)(_dirtyArea.UpperBound.Y / CellSize) + 1;
-            if (gy0 < 0) gy0 = 0;
-            if (gy1 > _ynum) gy1 = _ynum;
-
-            for (int gx = gx0; gx < gx1; gx++)
-            {
-                for (int gy = gy0; gy < gy1; gy++)
-                {
-                    //remove old terrain object at grid cell
-                    if (_bodyMap[gx, gy] != null)
-                    {
-                        for (int i = 0; i < _bodyMap[gx, gy].Count; i++)
-                        {
-                            World.RemoveBody(_bodyMap[gx, gy][i]);
-                        }
-                    }
-
-                    _bodyMap[gx, gy] = null;
-
-                    //generate new one
-                    GenerateTerrain(gx, gy);
-                }
-            }
-
-            _dirtyArea = new AABB(new Vector2(float.MaxValue, float.MaxValue), new Vector2(float.MinValue, float.MinValue));
-        }
-
-        private void GenerateTerrain(int gx, int gy)
-        {
-            float ax = gx * CellSize;
-            float ay = gy * CellSize;
-
-            List<Vertices> polys = MarchingSquares.DetectSquares(new AABB(new Vector2(ax, ay), new Vector2(ax + CellSize, ay + CellSize)), SubCellSize, SubCellSize, _terrainMap, Iterations, true);
-            if (polys.Count == 0) return;
-
-            _bodyMap[gx, gy] = new List<Body>();
-
-            // create the scale vector
-            Vector2 scale = new Vector2(1f / PointsPerUnit, 1f / -PointsPerUnit);
-
-            // create physics object for this grid cell
-            foreach (var item in polys)
-            {
-                // does this need to be negative?
-                item.Scale(ref scale);
-                item.Translate(ref _topLeft);
-                item.ForceCounterClockWise();
-                Vertices p = FarseerPhysics.Common.PolygonManipulation.SimplifyTools.CollinearSimplify(item);
-                List<Vertices> decompPolys = new List<Vertices>();
-
-                switch (Decomposer)
-                {
-                    case Decomposer.Bayazit:
-                        decompPolys = Decomposition.BayazitDecomposer.ConvexPartition(p);
-                        break;
-                    case Decomposer.CDT:
-                        decompPolys = Decomposition.CDTDecomposer.ConvexPartition(p);
-                        break;
-                    case Decomposer.Earclip:
-                        decompPolys = Decomposition.EarclipDecomposer.ConvexPartition(p);
-                        break;
-                    case Decomposer.Flipcode:
-                        decompPolys = Decomposition.FlipcodeDecomposer.ConvexPartition(p);
-                        break;
-                    case Decomposer.Seidel:
-                        decompPolys = Decomposition.SeidelDecomposer.ConvexPartition(p, 0.001f);
-                        break;
-                    default:
-                        break;
-                }
-
-                foreach (Vertices poly in decompPolys)
-                {
-                    if (poly.Count > 2)
-                        _bodyMap[gx, gy].Add(BodyFactory.CreatePolygon(World, poly, 1));
-                }
-            }
-        }
-    }
-}

FarseerPhysicsEngine/Common/TextureTools/MarchingSquares.cs

@@ -1,800 +0,0 @@
-using System.Collections.Generic;
-using FarseerPhysics.Collision;
-using Microsoft.Xna.Framework;
-
-namespace FarseerPhysics.Common
-{
-    // Ported by Matthew Bettcher - Feb 2011
-
-    /*
-    Copyright (c) 2010, Luca Deltodesco
-    All rights reserved.
-
-    Redistribution and use in source and binary forms, with or without modification, are permitted
-    provided that the following conditions are met:
-
-        * Redistributions of source code must retain the above copyright notice, this list of conditions
-	      and the following disclaimer.
-        * Redistributions in binary form must reproduce the above copyright notice, this list of
-	      conditions and the following disclaimer in the documentation and/or other materials provided
-	      with the distribution.
-        * Neither the name of the nape project nor the names of its contributors may be used to endorse
-	     or promote products derived from this software without specific prior written permission.
-
-    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR
-    IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND
-    FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
-    CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
-    DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
-    DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER
-    IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT
-    OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
-    */
-
-    public static class MarchingSquares
-    {
-        /// <summary>
-        /// Marching squares over the given domain using the mesh defined via the dimensions
-        ///    (wid,hei) to build a set of polygons such that f(x,y) less than 0, using the given number
-        ///    'bin' for recursive linear inteprolation along cell boundaries.
-        ///
-        ///    if 'comb' is true, then the polygons will also be composited into larger possible concave
-        ///    polygons.
-        /// </summary>
-        /// <param name="domain"></param>
-        /// <param name="cellWidth"></param>
-        /// <param name="cellHeight"></param>
-        /// <param name="f"></param>
-        /// <param name="lerpCount"></param>
-        /// <param name="combine"></param>
-        /// <returns></returns>
-        public static List<Vertices> DetectSquares(AABB domain, float cellWidth, float cellHeight, sbyte[,] f,
-                                                   int lerpCount, bool combine)
-        {
-            CxFastList<GeomPoly> ret = new CxFastList<GeomPoly>();
-
-            List<Vertices> verticesList = new List<Vertices>();
-
-            //NOTE: removed assignments as they were not used.
-            List<GeomPoly> polyList;
-            GeomPoly gp;
-
-            int xn = (int)(domain.Extents.X * 2 / cellWidth);
-            bool xp = xn == (domain.Extents.X * 2 / cellWidth);
-            int yn = (int)(domain.Extents.Y * 2 / cellHeight);
-            bool yp = yn == (domain.Extents.Y * 2 / cellHeight);
-            if (!xp) xn++;
-            if (!yp) yn++;
-
-            sbyte[,] fs = new sbyte[xn + 1, yn + 1];
-            GeomPolyVal[,] ps = new GeomPolyVal[xn + 1, yn + 1];
-
-            //populate shared function lookups.
-            for (int x = 0; x < xn + 1; x++)
-            {
-                int x0;
-                if (x == xn) x0 = (int)domain.UpperBound.X;
-                else x0 = (int)(x * cellWidth + domain.LowerBound.X);
-                for (int y = 0; y < yn + 1; y++)
-                {
-                    int y0;
-                    if (y == yn) y0 = (int)domain.UpperBound.Y;
-                    else y0 = (int)(y * cellHeight + domain.LowerBound.Y);
-                    fs[x, y] = f[x0, y0];
-                }
-            }
-
-            //generate sub-polys and combine to scan lines
-            for (int y = 0; y < yn; y++)
-            {
-                float y0 = y * cellHeight + domain.LowerBound.Y;
-                float y1;
-                if (y == yn - 1) y1 = domain.UpperBound.Y;
-                else y1 = y0 + cellHeight;
-                GeomPoly pre = null;
-                for (int x = 0; x < xn; x++)
-                {
-                    float x0 = x * cellWidth + domain.LowerBound.X;
-                    float x1;
-                    if (x == xn - 1) x1 = domain.UpperBound.X;
-                    else x1 = x0 + cellWidth;
-
-                    gp = new GeomPoly();
-
-                    int key = MarchSquare(f, fs, ref gp, x, y, x0, y0, x1, y1, lerpCount);
-                    if (gp.Length != 0)
-                    {
-                        if (combine && pre != null && (key & 9) != 0)
-                        {
-                            combLeft(ref pre, ref gp);
-                            gp = pre;
-                        }
-                        else
-                            ret.Add(gp);
-                        ps[x, y] = new GeomPolyVal(gp, key);
-                    }
-                    else
-                        gp = null;
-                    pre = gp;
-                }
-            }
-            if (!combine)
-            {
-                polyList = ret.GetListOfElements();
-
-                foreach (GeomPoly poly in polyList)
-                {
-                    verticesList.Add(new Vertices(poly.Points.GetListOfElements()));
-                }
-
-                return verticesList;
-            }
-
-            //combine scan lines together
-            for (int y = 1; y < yn; y++)
-            {
-                int x = 0;
-                while (x < xn)
-                {
-                    GeomPolyVal p = ps[x, y];
-
-                    //skip along scan line if no polygon exists at this point
-                    if (p == null)
-                    {
-                        x++;
-                        continue;
-                    }
-
-                    //skip along if current polygon cannot be combined above.
-                    if ((p.Key & 12) == 0)
-                    {
-                        x++;
-                        continue;
-                    }
-
-                    //skip along if no polygon exists above.
-                    GeomPolyVal u = ps[x, y - 1];
-                    if (u == null)
-                    {
-                        x++;
-                        continue;
-                    }
-
-                    //skip along if polygon above cannot be combined with.
-                    if ((u.Key & 3) == 0)
-                    {
-                        x++;
-                        continue;
-                    }
-
-                    float ax = x * cellWidth + domain.LowerBound.X;
-                    float ay = y * cellHeight + domain.LowerBound.Y;
-
-                    CxFastList<Vector2> bp = p.GeomP.Points;
-                    CxFastList<Vector2> ap = u.GeomP.Points;
-
-                    //skip if it's already been combined with above polygon
-                    if (u.GeomP == p.GeomP)
-                    {
-                        x++;
-                        continue;
-                    }
-
-                    //combine above (but disallow the hole thingies
-                    CxFastListNode<Vector2> bi = bp.Begin();
-                    while (Square(bi.Elem().Y - ay) > Settings.Epsilon || bi.Elem().X < ax) bi = bi.Next();
-
-                    //NOTE: Unused
-                    //Vector2 b0 = bi.elem();
-                    Vector2 b1 = bi.Next().Elem();
-                    if (Square(b1.Y - ay) > Settings.Epsilon)
-                    {
-                        x++;
-                        continue;
-                    }
-
-                    bool brk = true;
-                    CxFastListNode<Vector2> ai = ap.Begin();
-                    while (ai != ap.End())
-                    {
-                        if (VecDsq(ai.Elem(), b1) < Settings.Epsilon)
-                        {
-                            brk = false;
-                            break;
-                        }
-                        ai = ai.Next();
-                    }
-                    if (brk)
-                    {
-                        x++;
-                        continue;
-                    }
-
-                    CxFastListNode<Vector2> bj = bi.Next().Next();
-                    if (bj == bp.End()) bj = bp.Begin();
-                    while (bj != bi)
-                    {
-                        ai = ap.Insert(ai, bj.Elem()); // .clone()
-                        bj = bj.Next();
-                        if (bj == bp.End()) bj = bp.Begin();
-                        u.GeomP.Length++;
-                    }
-                    //u.p.simplify(float.Epsilon,float.Epsilon);
-                    //
-                    ax = x + 1;
-                    while (ax < xn)
-                    {
-                        GeomPolyVal p2 = ps[(int)ax, y];
-                        if (p2 == null || p2.GeomP != p.GeomP)
-                        {
-                            ax++;
-                            continue;
-                        }
-                        p2.GeomP = u.GeomP;
-                        ax++;
-                    }
-                    ax = x - 1;
-                    while (ax >= 0)
-                    {
-                        GeomPolyVal p2 = ps[(int)ax, y];
-                        if (p2 == null || p2.GeomP != p.GeomP)
-                        {
-                            ax--;
-                            continue;
-                        }
-                        p2.GeomP = u.GeomP;
-                        ax--;
-                    }
-                    ret.Remove(p.GeomP);
-                    p.GeomP = u.GeomP;
-
-                    x = (int)((bi.Next().Elem().X - domain.LowerBound.X) / cellWidth) + 1;
-                    //x++; this was already commented out!
-                }
-            }
-
-            polyList = ret.GetListOfElements();
-
-            foreach (GeomPoly poly in polyList)
-            {
-                verticesList.Add(new Vertices(poly.Points.GetListOfElements()));
-            }
-
-            return verticesList;
-        }
-
-        #region Private Methods
-
-        //- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 
-
-        /** Linearly interpolate between (x0 to x1) given a value at these coordinates (v0 and v1)
-            such as to approximate value(return) = 0
-        **/
-
-        private static int[] _lookMarch = {
-                                              0x00, 0xE0, 0x38, 0xD8, 0x0E, 0xEE, 0x36, 0xD6, 0x83, 0x63, 0xBB, 0x5B, 0x8D,
-                                              0x6D, 0xB5, 0x55
-                                          };
-
-        private static float Lerp(float x0, float x1, float v0, float v1)
-        {
-            float dv = v0 - v1;
-            float t;
-            if (dv * dv < Settings.Epsilon)
-                t = 0.5f;
-            else t = v0 / dv;
-            return x0 + t * (x1 - x0);
-        }
-
-        //- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 
-
-        /** Recursive linear interpolation for use in marching squares **/
-
-        private static float Xlerp(float x0, float x1, float y, float v0, float v1, sbyte[,] f, int c)
-        {
-            float xm = Lerp(x0, x1, v0, v1);
-            if (c == 0)
-                return xm;
-
-            sbyte vm = f[(int)xm, (int)y];
-
-            if (v0 * vm < 0)
-                return Xlerp(x0, xm, y, v0, vm, f, c - 1);
-
-            return Xlerp(xm, x1, y, vm, v1, f, c - 1);
-        }
-
-        /** Recursive linear interpolation for use in marching squares **/
-
-        private static float Ylerp(float y0, float y1, float x, float v0, float v1, sbyte[,] f, int c)
-        {
-            float ym = Lerp(y0, y1, v0, v1);
-            if (c == 0)
-                return ym;
-
-            sbyte vm = f[(int)x, (int)ym];
-
-            if (v0 * vm < 0)
-                return Ylerp(y0, ym, x, v0, vm, f, c - 1);
-
-            return Ylerp(ym, y1, x, vm, v1, f, c - 1);
-        }
-
-        /** Square value for use in marching squares **/
-
-        private static float Square(float x)
-        {
-            return x * x;
-        }
-
-        private static float VecDsq(Vector2 a, Vector2 b)
-        {
-            Vector2 d = a - b;
-            return d.X * d.X + d.Y * d.Y;
-        }
-
-        private static float VecCross(Vector2 a, Vector2 b)
-        {
-            return a.X * b.Y - a.Y * b.X;
-        }
-
-        /** Look-up table to relate polygon key with the vertices that should be used for
-            the sub polygon in marching squares
-        **/
-
-        /** Perform a single celled marching square for for the given cell defined by (x0,y0) (x1,y1)
-            using the function f for recursive interpolation, given the look-up table 'fs' of
-            the values of 'f' at cell vertices with the result to be stored in 'poly' given the actual
-            coordinates of 'ax' 'ay' in the marching squares mesh.
-        **/
-
-        private static int MarchSquare(sbyte[,] f, sbyte[,] fs, ref GeomPoly poly, int ax, int ay, float x0, float y0,
-                                       float x1, float y1, int bin)
-        {
-            //key lookup
-            int key = 0;
-            sbyte v0 = fs[ax, ay];
-            if (v0 < 0) key |= 8;
-            sbyte v1 = fs[ax + 1, ay];
-            if (v1 < 0) key |= 4;
-            sbyte v2 = fs[ax + 1, ay + 1];
-            if (v2 < 0) key |= 2;
-            sbyte v3 = fs[ax, ay + 1];
-            if (v3 < 0) key |= 1;
-
-            int val = _lookMarch[key];
-            if (val != 0)
-            {
-                CxFastListNode<Vector2> pi = null;
-                for (int i = 0; i < 8; i++)
-                {
-                    Vector2 p;
-                    if ((val & (1 << i)) != 0)
-                    {
-                        if (i == 7 && (val & 1) == 0)
-                            poly.Points.Add(p = new Vector2(x0, Ylerp(y0, y1, x0, v0, v3, f, bin)));
-                        else
-                        {
-                            if (i == 0) p = new Vector2(x0, y0);
-                            else if (i == 2) p = new Vector2(x1, y0);
-                            else if (i == 4) p = new Vector2(x1, y1);
-                            else if (i == 6) p = new Vector2(x0, y1);
-
-                            else if (i == 1) p = new Vector2(Xlerp(x0, x1, y0, v0, v1, f, bin), y0);
-                            else if (i == 5) p = new Vector2(Xlerp(x0, x1, y1, v3, v2, f, bin), y1);
-
-                            else if (i == 3) p = new Vector2(x1, Ylerp(y0, y1, x1, v1, v2, f, bin));
-                            else p = new Vector2(x0, Ylerp(y0, y1, x0, v0, v3, f, bin));
-
-                            pi = poly.Points.Insert(pi, p);
-                        }
-                        poly.Length++;
-                    }
-                }
-                //poly.simplify(float.Epsilon,float.Epsilon);
-            }
-            return key;
-        }
-
-        /** Used in polygon composition to composit polygons into scan lines
-            Combining polya and polyb into one super-polygon stored in polya.
-        **/
-
-        private static void combLeft(ref GeomPoly polya, ref GeomPoly polyb)
-        {
-            CxFastList<Vector2> ap = polya.Points;
-            CxFastList<Vector2> bp = polyb.Points;
-            CxFastListNode<Vector2> ai = ap.Begin();
-            CxFastListNode<Vector2> bi = bp.Begin();
-
-            Vector2 b = bi.Elem();
-            CxFastListNode<Vector2> prea = null;
-            while (ai != ap.End())
-            {
-                Vector2 a = ai.Elem();
-                if (VecDsq(a, b) < Settings.Epsilon)
-                {
-                    //ignore shared vertex if parallel
-                    if (prea != null)
-                    {
-                        Vector2 a0 = prea.Elem();
-                        b = bi.Next().Elem();
-
-                        Vector2 u = a - a0;
-                        //vec_new(u); vec_sub(a.p.p, a0.p.p, u);
-                        Vector2 v = b - a;
-                        //vec_new(v); vec_sub(b.p.p, a.p.p, v);
-                        float dot = VecCross(u, v);
-                        if (dot * dot < Settings.Epsilon)
-                        {
-                            ap.Erase(prea, ai);
-                            polya.Length--;
-                            ai = prea;
-                        }
-                    }
-
-                    //insert polyb into polya
-                    bool fst = true;
-                    CxFastListNode<Vector2> preb = null;
-                    while (!bp.Empty())
-                    {
-                        Vector2 bb = bp.Front();
-                        bp.Pop();
-                        if (!fst && !bp.Empty())
-                        {
-                            ai = ap.Insert(ai, bb);
-                            polya.Length++;
-                            preb = ai;
-                        }
-                        fst = false;
-                    }
-
-                    //ignore shared vertex if parallel
-                    ai = ai.Next();
-                    Vector2 a1 = ai.Elem();
-                    ai = ai.Next();
-                    if (ai == ap.End()) ai = ap.Begin();
-                    Vector2 a2 = ai.Elem();
-                    Vector2 a00 = preb.Elem();
-                    Vector2 uu = a1 - a00;
-                    //vec_new(u); vec_sub(a1.p, a0.p, u);
-                    Vector2 vv = a2 - a1;
-                    //vec_new(v); vec_sub(a2.p, a1.p, v);
-                    float dot1 = VecCross(uu, vv);
-                    if (dot1 * dot1 < Settings.Epsilon)
-                    {
-                        ap.Erase(preb, preb.Next());
-                        polya.Length--;
-                    }
-
-                    return;
-                }
-                prea = ai;
-                ai = ai.Next();
-            }
-        }
-
-        #endregion
-
-        #region CxFastList from nape physics
-
-        #region Nested type: CxFastList
-
-        /// <summary>
-        /// Designed as a complete port of CxFastList from CxStd.
-        /// </summary>
-        internal class CxFastList<T>
-        {
-            // first node in the list
-            private CxFastListNode<T> _head;
-            private int _count;
-
-            /// <summary>
-            /// Iterator to start of list (O(1))
-            /// </summary>
-            public CxFastListNode<T> Begin()
-            {
-                return _head;
-            }
-
-            /// <summary>
-            /// Iterator to end of list (O(1))
-            /// </summary>
-            public CxFastListNode<T> End()
-            {
-                return null;
-            }
-
-            /// <summary>
-            /// Returns first element of list (O(1))
-            /// </summary>
-            public T Front()
-            {
-                return _head.Elem();
-            }
-
-            /// <summary>
-            /// add object to list (O(1))
-            /// </summary>
-            public CxFastListNode<T> Add(T value)
-            {
-                CxFastListNode<T> newNode = new CxFastListNode<T>(value);
-                if (_head == null)
-                {
-                    newNode._next = null;
-                    _head = newNode;
-                    _count++;
-                    return newNode;
-                }
-                newNode._next = _head;
-                _head = newNode;
-
-                _count++;
-
-                return newNode;
-            }
-
-            /// <summary>
-            /// remove object from list, returns true if an element was removed (O(n))
-            /// </summary>
-            public bool Remove(T value)
-            {
-                CxFastListNode<T> head = _head;
-                CxFastListNode<T> prev = _head;
-
-                EqualityComparer<T> comparer = EqualityComparer<T>.Default;
-
-                if (head != null)
-                {
-                    if (value != null)
-                    {
-                        do
-                        {
-                            // if we are on the value to be removed
-                            if (comparer.Equals(head._elt, value))
-                            {
-                                // then we need to patch the list
-                                // check to see if we are removing the _head
-                                if (head == _head)
-                                {
-                                    _head = head._next;
-                                    _count--;
-                                    return true;
-                                }
-                                else
-                                {
-                                    // were not at the head
-                                    prev._next = head._next;
-                                    _count--;
-                                    return true;
-                                }
-                            }
-                            // cache the current as the previous for the next go around
-                            prev = head;
-                            head = head._next;
-                        } while (head != null);
-                    }
-                }
-                return false;
-            }
-
-            /// <summary>
-            /// pop element from head of list (O(1)) Note: this does not return the object popped! 
-            /// There is good reason to this, and it regards the Alloc list variants which guarantee 
-            /// objects are released to the object pool. You do not want to retrieve an element 
-            /// through pop or else that object may suddenly be used by another piece of code which 
-            /// retrieves it from the object pool.
-            /// </summary>
-            public CxFastListNode<T> Pop()
-            {
-                return Erase(null, _head);
-            }
-
-            /// <summary>
-            /// insert object after 'node' returning an iterator to the inserted object.
-            /// </summary>
-            public CxFastListNode<T> Insert(CxFastListNode<T> node, T value)
-            {
-                if (node == null)
-                {
-                    return Add(value);
-                }
-                CxFastListNode<T> newNode = new CxFastListNode<T>(value);
-                CxFastListNode<T> nextNode = node._next;
-                newNode._next = nextNode;
-                node._next = newNode;
-
-                _count++;
-
-                return newNode;
-            }
-
-            /// <summary>
-            /// removes the element pointed to by 'node' with 'prev' being the previous iterator, 
-            /// returning an iterator to the element following that of 'node' (O(1))
-            /// </summary>
-            public CxFastListNode<T> Erase(CxFastListNode<T> prev, CxFastListNode<T> node)
-            {
-                // cache the node after the node to be removed
-                CxFastListNode<T> nextNode = node._next;
-                if (prev != null)
-                    prev._next = nextNode;
-                else if (_head != null)
-                    _head = _head._next;
-                else
-                    return null;
-
-                _count--;
-                return nextNode;
-            }
-
-            /// <summary>
-            /// whether the list is empty (O(1))
-            /// </summary>
-            public bool Empty()
-            {
-                if (_head == null)
-                    return true;
-                return false;
-            }
-
-            /// <summary>
-            /// computes size of list (O(n))
-            /// </summary>
-            public int Size()
-            {
-                CxFastListNode<T> i = Begin();
-                int count = 0;
-
-                do
-                {
-                    count++;
-                } while (i.Next() != null);
-
-                return count;
-            }
-
-            /// <summary>
-            /// empty the list (O(1) if CxMixList, O(n) otherwise)
-            /// </summary>
-            public void Clear()
-            {
-                CxFastListNode<T> head = _head;
-                while (head != null)
-                {
-                    CxFastListNode<T> node2 = head;
-                    head = head._next;
-                    node2._next = null;
-                }
-                _head = null;
-                _count = 0;
-            }
-
-            /// <summary>
-            /// returns true if 'value' is an element of the list (O(n))
-            /// </summary>
-            public bool Has(T value)
-            {
-                return (Find(value) != null);
-            }
-
-            // Non CxFastList Methods 
-            public CxFastListNode<T> Find(T value)
-            {
-                // start at head
-                CxFastListNode<T> head = _head;
-                EqualityComparer<T> comparer = EqualityComparer<T>.Default;
-                if (head != null)
-                {
-                    if (value != null)
-                    {
-                        do
-                        {
-                            if (comparer.Equals(head._elt, value))
-                            {
-                                return head;
-                            }
-                            head = head._next;
-                        } while (head != _head);
-                    }
-                    else
-                    {
-                        do
-                        {
-                            if (head._elt == null)
-                            {
-                                return head;
-                            }
-                            head = head._next;
-                        } while (head != _head);
-                    }
-                }
-                return null;
-            }
-
-            public List<T> GetListOfElements()
-            {
-                List<T> list = new List<T>();
-
-                CxFastListNode<T> iter = Begin();
-
-                if (iter != null)
-                {
-                    do
-                    {
-                        list.Add(iter._elt);
-                        iter = iter._next;
-                    } while (iter != null);
-                }
-                return list;
-            }
-        }
-
-        #endregion
-
-        #region Nested type: CxFastListNode
-
-        internal class CxFastListNode<T>
-        {
-            internal T _elt;
-            internal CxFastListNode<T> _next;
-
-            public CxFastListNode(T obj)
-            {
-                _elt = obj;
-            }
-
-            public T Elem()
-            {
-                return _elt;
-            }
-
-            public CxFastListNode<T> Next()
-            {
-                return _next;
-            }
-        }
-
-        #endregion
-
-        #endregion
-
-        #region Internal Stuff
-
-        #region Nested type: GeomPoly
-
-        internal class GeomPoly
-        {
-            public int Length;
-            public CxFastList<Vector2> Points;
-
-            public GeomPoly()
-            {
-                Points = new CxFastList<Vector2>();
-                Length = 0;
-            }
-        }
-
-        #endregion
-
-        #region Nested type: GeomPolyVal
-
-        private class GeomPolyVal
-        {
-            /** Associated polygon at coordinate **/
-            /** Key of original sub-polygon **/
-            public int Key;
-            public GeomPoly GeomP;
-
-            public GeomPolyVal(GeomPoly geomP, int K)
-            {
-                GeomP = geomP;
-                Key = K;
-            }
-        }
-
-        #endregion
-
-        #endregion
-    }
-}

FarseerPhysicsEngine/Common/TextureTools/TextureConverter.cs

@@ -1,1338 +0,0 @@
-using System;
-using System.Collections.Generic;
-using System.Diagnostics;
-using Microsoft.Xna.Framework;
-
-namespace FarseerPhysics.Common
-{
-    // User contribution from Sickbattery aka David Reschke :).
-
-    #region ToDo: Create a new file for each ...
-    /// <summary>
-    /// The detection type affects the resulting polygon data.
-    /// </summary>
-    public enum VerticesDetectionType
-    {
-        /// <summary>
-        /// Holes are integrated into the main polygon.
-        /// </summary>
-        Integrated = 0,
-
-        /// <summary>
-        /// The data of the main polygon and hole polygons is returned separately.
-        /// </summary>
-        Separated = 1
-    }
-
-    /// <summary>
-    /// Detected vertices of a single polygon.
-    /// </summary>
-    public class DetectedVertices : Vertices
-    {
-        private List<Vertices> _holes;
-
-        public List<Vertices> Holes
-        {
-            get { return _holes; }
-            set { _holes = value; }
-        }
-
-        public DetectedVertices()
-            : base()
-        {
-        }
-
-        public DetectedVertices(Vertices vertices)
-            : base(vertices)
-        {
-        }
-
-        public void Transform(Matrix transform)
-        {
-            // Transform main polygon
-            for (int i = 0; i < this.Count; i++)
-                this[i] = Vector2.Transform(this[i], transform);
-
-            // Transform holes
-            Vector2[] temp = null;
-            if (_holes != null && _holes.Count > 0)
-            {
-                for (int i = 0; i < _holes.Count; i++)
-                {
-                    temp = _holes[i].ToArray();
-                    Vector2.Transform(temp, ref transform, temp);
-
-                    _holes[i] = new Vertices(temp);
-                }
-            }
-        }
-    }
-    #endregion
-
-    /// <summary>
-    /// 
-    /// </summary>
-    public sealed class TextureConverter
-    {
-        private const int _CLOSEPIXELS_LENGTH = 8;
-
-        /// <summary>
-        /// This array is ment to be readonly.
-        /// It's not because it is accessed very frequently.
-        /// </summary>
-        private static /*readonly*/ int[,] ClosePixels =
-            new int[_CLOSEPIXELS_LENGTH, 2] { { -1, -1 }, { 0, -1 }, { 1, -1 }, { 1, 0 }, { 1, 1 }, { 0, 1 }, { -1, 1 }, { -1, 0 } };
-
-        private uint[] _data;
-        private int _dataLength;
-        private int _width;
-        private int _height;
-
-        private VerticesDetectionType _polygonDetectionType;
-
-        private uint _alphaTolerance;
-        private float _hullTolerance;
-
-        private bool _holeDetection;
-        private bool _multipartDetection;
-        private bool _pixelOffsetOptimization;
-
-        private Matrix _transform = Matrix.Identity;
-
-        #region Properties
-        /// <summary>
-        /// Get or set the polygon detection type.
-        /// </summary>
-        public VerticesDetectionType PolygonDetectionType
-        {
-            get { return _polygonDetectionType; }
-            set { _polygonDetectionType = value; }
-        }
-
-        /// <summary>
-        /// Will detect texture 'holes' if set to true. Slows down the detection. Default is false.
-        /// </summary>
-        public bool HoleDetection
-        {
-            get { return _holeDetection; }
-            set { _holeDetection = value; }
-        }
-
-        /// <summary>
-        /// Will detect texture multiple 'solid' isles if set to true. Slows down the detection. Default is false.
-        /// </summary>
-        public bool MultipartDetection
-        {
-            get { return _multipartDetection; }
-            set { _multipartDetection = value; }
-        }
-
-        /// <summary>
-        /// Will optimize the vertex positions along the interpolated normal between two edges about a half pixel (post processing). Default is false.
-        /// </summary>
-        public bool PixelOffsetOptimization
-        {
-            get { return _pixelOffsetOptimization; }
-            set { _pixelOffsetOptimization = value; }
-        }
-
-        /// <summary>
-        /// Can be used for scaling.
-        /// </summary>
-        public Matrix Transform
-        {
-            get { return _transform; }
-            set { _transform = value; }
-        }
-
-        /// <summary>
-        /// Alpha (coverage) tolerance. Default is 20: Every pixel with a coverage value equal or greater to 20 will be counts as solid.
-        /// </summary>
-        public byte AlphaTolerance
-        {
-            get { return (byte)(_alphaTolerance >> 24); }
-            set { _alphaTolerance = (uint)value << 24; }
-        }
-
-        /// <summary>
-        /// Default is 1.5f.
-        /// </summary>
-        public float HullTolerance
-        {
-            get { return _hullTolerance; }
-            set
-            {
-                if (value > 4f)
-                {
-                    _hullTolerance = 4f;
-                }
-                else if (value < 0.9f)
-                {
-                    _hullTolerance = 0.9f;
-                }
-                else
-                {
-                    _hullTolerance = value;
-                }
-            }
-        }
-        #endregion
-
-        #region Constructors
-        public TextureConverter()
-        {
-            Initialize(null, null, null, null, null, null, null, null);
-        }
-
-        public TextureConverter(byte? alphaTolerance, float? hullTolerance,
-            bool? holeDetection, bool? multipartDetection, bool? pixelOffsetOptimization, Matrix? transform)
-        {
-            Initialize(null, null, alphaTolerance, hullTolerance, holeDetection,
-                multipartDetection, pixelOffsetOptimization, transform);
-        }
-
-        public TextureConverter(uint[] data, int width)
-        {
-            Initialize(data, width, null, null, null, null, null, null);
-        }
-
-        public TextureConverter(uint[] data, int width, byte? alphaTolerance,
-            float? hullTolerance, bool? holeDetection, bool? multipartDetection,
-            bool? pixelOffsetOptimization, Matrix? transform)
-        {
-            Initialize(data, width, alphaTolerance, hullTolerance, holeDetection,
-                multipartDetection, pixelOffsetOptimization, transform);
-        }
-        #endregion
-
-        #region Initialization
-        private void Initialize(uint[] data, int? width, byte? alphaTolerance,
-            float? hullTolerance, bool? holeDetection, bool? multipartDetection,
-            bool? pixelOffsetOptimization, Matrix? transform)
-        {
-            if (data != null && !width.HasValue)
-                throw new ArgumentNullException("width", "'width' can't be null if 'data' is set.");
-
-            if (data == null && width.HasValue)
-                throw new ArgumentNullException("data", "'data' can't be null if 'width' is set.");
-
-            if (data != null && width.HasValue)
-                SetTextureData(data, width.Value);
-
-            if (alphaTolerance.HasValue)
-                AlphaTolerance = alphaTolerance.Value;
-            else
-                AlphaTolerance = 20;
-
-            if (hullTolerance.HasValue)
-                HullTolerance = hullTolerance.Value;
-            else
-                HullTolerance = 1.5f;
-
-            if (holeDetection.HasValue)
-                HoleDetection = holeDetection.Value;
-            else
-                HoleDetection = false;
-
-            if (multipartDetection.HasValue)
-                MultipartDetection = multipartDetection.Value;
-            else
-                MultipartDetection = false;
-
-            if (pixelOffsetOptimization.HasValue)
-                PixelOffsetOptimization = pixelOffsetOptimization.Value;
-            else
-                PixelOffsetOptimization = false;
-
-            if (transform.HasValue)
-                Transform = transform.Value;
-            else
-                Transform = Matrix.Identity;
-        }
-        #endregion
-
-        /// <summary>
-        /// 
-        /// </summary>
-        /// <param name="data"></param>
-        /// <param name="width"></param>
-        private void SetTextureData(uint[] data, int width)
-        {
-            if (data == null)
-                throw new ArgumentNullException("data", "'data' can't be null.");
-
-            if (data.Length < 4)
-                throw new ArgumentOutOfRangeException("data", "'data' length can't be less then 4. Your texture must be at least 2 x 2 pixels in size.");
-
-            if (width < 2)
-                throw new ArgumentOutOfRangeException("width", "'width' can't be less then 2. Your texture must be at least 2 x 2 pixels in size.");
-
-            if (data.Length % width != 0)
-                throw new ArgumentException("'width' has an invalid value.");
-
-            _data = data;
-            _dataLength = _data.Length;
-            _width = width;
-            _height = _dataLength / width;
-        }
-
-        /// <summary>
-        /// Detects the vertices of the supplied texture data. (PolygonDetectionType.Integrated)
-        /// </summary>
-        /// <param name="data">The texture data.</param>
-        /// <param name="width">The texture width.</param>
-        /// <returns></returns>
-        public static Vertices DetectVertices(uint[] data, int width)
-        {
-            TextureConverter tc = new TextureConverter(data, width);
-
-            List<DetectedVertices> detectedVerticesList = tc.DetectVertices();
-
-            return detectedVerticesList[0];
-        }
-
-        /// <summary>
-        /// Detects the vertices of the supplied texture data.
-        /// </summary>
-        /// <param name="data">The texture data.</param>
-        /// <param name="width">The texture width.</param>
-        /// <param name="holeDetection">if set to <c>true</c> it will perform hole detection.</param>
-        /// <returns></returns>
-        public static Vertices DetectVertices(uint[] data, int width, bool holeDetection)
-        {
-            TextureConverter tc =
-                new TextureConverter(data, width)
-                {
-                    HoleDetection = holeDetection
-                };
-
-            List<DetectedVertices> detectedVerticesList = tc.DetectVertices();
-
-            return detectedVerticesList[0];
-        }
-
-        /// <summary>
-        /// Detects the vertices of the supplied texture data.
-        /// </summary>
-        /// <param name="data">The texture data.</param>
-        /// <param name="width">The texture width.</param>
-        /// <param name="holeDetection">if set to <c>true</c> it will perform hole detection.</param>
-        /// <param name="hullTolerance">The hull tolerance.</param>
-        /// <param name="alphaTolerance">The alpha tolerance.</param>
-        /// <param name="multiPartDetection">if set to <c>true</c> it will perform multi part detection.</param>
-        /// <returns></returns>
-        public static List<Vertices> DetectVertices(uint[] data, int width, float hullTolerance,
-                                                    byte alphaTolerance, bool multiPartDetection, bool holeDetection)
-        {
-            TextureConverter tc =
-                new TextureConverter(data, width)
-                {
-                    HullTolerance = hullTolerance,
-                    AlphaTolerance = alphaTolerance,
-                    MultipartDetection = multiPartDetection,
-                    HoleDetection = holeDetection
-                };
-
-            List<DetectedVertices> detectedVerticesList = tc.DetectVertices();
-            List<Vertices> result = new List<Vertices>();
-
-            for (int i = 0; i < detectedVerticesList.Count; i++)
-            {
-                result.Add(detectedVerticesList[i]);
-            }
-
-            return result;
-        }
-
-        public List<DetectedVertices> DetectVertices()
-        {
-            #region Check TextureConverter setup.
-
-            if (_data == null)
-                throw new Exception(
-                    "'_data' can't be null. You have to use SetTextureData(uint[] data, int width) before calling this method.");
-
-            if (_data.Length < 4)
-                throw new Exception(
-                    "'_data' length can't be less then 4. Your texture must be at least 2 x 2 pixels in size. " +
-                    "You have to use SetTextureData(uint[] data, int width) before calling this method.");
-
-            if (_width < 2)
-                throw new Exception(
-                    "'_width' can't be less then 2. Your texture must be at least 2 x 2 pixels in size. " +
-                    "You have to use SetTextureData(uint[] data, int width) before calling this method.");
-
-            if (_data.Length % _width != 0)
-                throw new Exception(
-                    "'_width' has an invalid value. You have to use SetTextureData(uint[] data, int width) before calling this method.");
-
-            #endregion
-
-
-            List<DetectedVertices> detectedPolygons = new List<DetectedVertices>();
-
-            DetectedVertices polygon;
-            Vertices holePolygon;
-
-            Vector2? holeEntrance = null;
-            Vector2? polygonEntrance = null;
-
-            List<Vector2> blackList = new List<Vector2>();
-
-            bool searchOn;
-            do
-            {
-                if (detectedPolygons.Count == 0)
-                {
-                    // First pass / single polygon
-                    polygon = new DetectedVertices(CreateSimplePolygon(Vector2.Zero, Vector2.Zero));
-
-                    if (polygon.Count > 2)
-                        polygonEntrance = GetTopMostVertex(polygon);
-                }
-                else if (polygonEntrance.HasValue)
-                {
-                    // Multi pass / multiple polygons
-                    polygon = new DetectedVertices(CreateSimplePolygon(
-                        polygonEntrance.Value, new Vector2(polygonEntrance.Value.X - 1f, polygonEntrance.Value.Y)));
-                }
-                else
-                    break;
-
-                searchOn = false;
-
-
-                if (polygon.Count > 2)
-                {
-                    if (_holeDetection)
-                    {
-                        do
-                        {
-                            holeEntrance = SearchHoleEntrance(polygon, holeEntrance);
-
-                            if (holeEntrance.HasValue)
-                            {
-                                if (!blackList.Contains(holeEntrance.Value))
-                                {
-                                    blackList.Add(holeEntrance.Value);
-                                    holePolygon = CreateSimplePolygon(holeEntrance.Value,
-                                        new Vector2(holeEntrance.Value.X + 1, holeEntrance.Value.Y));
-
-                                    if (holePolygon != null && holePolygon.Count > 2)
-                                    {
-                                        switch (_polygonDetectionType)
-                                        {
-                                            case VerticesDetectionType.Integrated:
-
-                                                // Add first hole polygon vertex to close the hole polygon.
-                                                holePolygon.Add(holePolygon[0]);
-
-                                                int vertex1Index, vertex2Index;
-                                                if (SplitPolygonEdge(polygon, holeEntrance.Value, out vertex1Index, out vertex2Index))
-                                                    polygon.InsertRange(vertex2Index, holePolygon);
-
-                                                break;
-
-                                            case VerticesDetectionType.Separated:
-                                                if (polygon.Holes == null)
-                                                    polygon.Holes = new List<Vertices>();
-
-                                                polygon.Holes.Add(holePolygon);
-                                                break;
-                                        }
-                                    }
-                                }
-                                else
-                                    break;
-                            }
-                            else
-                                break;
-                        }
-                        while (true);
-                    }
-
-                    detectedPolygons.Add(polygon);
-                }
-
-                if (_multipartDetection || polygon.Count <= 2)
-                {
-                    if (SearchNextHullEntrance(detectedPolygons, polygonEntrance.Value, out polygonEntrance))
-                        searchOn = true;
-                }
-            }
-            while (searchOn);
-
-            if (detectedPolygons == null || (detectedPolygons != null && detectedPolygons.Count == 0))
-                throw new Exception("Couldn't detect any vertices.");
-
-
-            // Post processing.
-            if (PolygonDetectionType == VerticesDetectionType.Separated) // Only when VerticesDetectionType.Separated? -> Recheck.
-                ApplyTriangulationCompatibleWinding(ref detectedPolygons);
-
-            if (_pixelOffsetOptimization)
-                ApplyPixelOffsetOptimization(ref detectedPolygons);
-
-            if (_transform != Matrix.Identity)
-                ApplyTransform(ref detectedPolygons);
-
-
-            return detectedPolygons;
-        }
-
-        private void ApplyTriangulationCompatibleWinding(ref List<DetectedVertices> detectedPolygons)
-        {
-            for (int i = 0; i < detectedPolygons.Count; i++)
-            {
-                detectedPolygons[i].Reverse();
-
-                if (detectedPolygons[i].Holes != null && detectedPolygons[i].Holes.Count > 0)
-                {
-                    for (int j = 0; j < detectedPolygons[i].Holes.Count; j++)
-                        detectedPolygons[i].Holes[j].Reverse();
-                }
-            }
-        }
-
-        private void ApplyPixelOffsetOptimization(ref List<DetectedVertices> detectedPolygons)
-        {
-
-        }
-
-        private void ApplyTransform(ref List<DetectedVertices> detectedPolygons)
-        {
-            for (int i = 0; i < detectedPolygons.Count; i++)
-                detectedPolygons[i].Transform(_transform);
-        }
-
-        #region Data[] functions
-        private int _tempIsSolidX;
-        private int _tempIsSolidY;
-        public bool IsSolid(ref Vector2 v)
-        {
-            _tempIsSolidX = (int)v.X;
-            _tempIsSolidY = (int)v.Y;
-
-            if (_tempIsSolidX >= 0 && _tempIsSolidX < _width && _tempIsSolidY >= 0 && _tempIsSolidY < _height)
-                return (_data[_tempIsSolidX + _tempIsSolidY * _width] >= _alphaTolerance);
-            //return ((_data[_tempIsSolidX + _tempIsSolidY * _width] & 0xFF000000) >= _alphaTolerance);
-
-            return false;
-        }
-
-        public bool IsSolid(ref int x, ref int y)
-        {
-            if (x >= 0 && x < _width && y >= 0 && y < _height)
-                return (_data[x + y * _width] >= _alphaTolerance);
-            //return ((_data[x + y * _width] & 0xFF000000) >= _alphaTolerance);
-
-            return false;
-        }
-
-        public bool IsSolid(ref int index)
-        {
-            if (index >= 0 && index < _dataLength)
-                return (_data[index] >= _alphaTolerance);
-            //return ((_data[index] & 0xFF000000) >= _alphaTolerance);
-
-            return false;
-        }
-
-        public bool InBounds(ref Vector2 coord)
-        {
-            return (coord.X >= 0f && coord.X < _width && coord.Y >= 0f && coord.Y < _height);
-        }
-        #endregion
-
-        /// <summary>
-        /// Function to search for an entrance point of a hole in a polygon. It searches the polygon from top to bottom between the polygon edges.
-        /// </summary>
-        /// <param name="polygon">The polygon to search in.</param>
-        /// <param name="lastHoleEntrance">The last entrance point.</param>
-        /// <returns>The next holes entrance point. Null if ther are no holes.</returns>
-        private Vector2? SearchHoleEntrance(Vertices polygon, Vector2? lastHoleEntrance)
-        {
-            if (polygon == null)
-                throw new ArgumentNullException("'polygon' can't be null.");
-
-            if (polygon.Count < 3)
-                throw new ArgumentException("'polygon.MainPolygon.Count' can't be less then 3.");
-
-
-            List<float> xCoords;
-            Vector2? entrance;
-
-            int startY;
-            int endY;
-
-            int lastSolid = 0;
-            bool foundSolid;
-            bool foundTransparent;
-
-            // Set start y coordinate.
-            if (lastHoleEntrance.HasValue)
-            {
-                // We need the y coordinate only.
-                startY = (int)lastHoleEntrance.Value.Y;
-            }
-            else
-            {
-                // Start from the top of the polygon if last entrance == null.
-                startY = (int)GetTopMostCoord(polygon);
-            }
-
-            // Set the end y coordinate.
-            endY = (int)GetBottomMostCoord(polygon);
-
-            if (startY > 0 && startY < _height && endY > 0 && endY < _height)
-            {
-                // go from top to bottom of the polygon
-                for (int y = startY; y <= endY; y++)
-                {
-                    // get x-coord of every polygon edge which crosses y
-                    xCoords = SearchCrossingEdges(polygon, y);
-
-                    // We need an even number of crossing edges. 
-                    // It's always a pair of start and end edge: nothing | polygon | hole | polygon | nothing ...
-                    // If it's not then don't bother, it's probably a peak ...
-                    // ...which should be filtered out by SearchCrossingEdges() anyway.
-                    if (xCoords.Count > 1 && xCoords.Count % 2 == 0)
-                    {
-                        // Ok, this is short, but probably a little bit confusing.
-                        // This part searches from left to right between the edges inside the polygon.
-                        // The problem: We are using the polygon data to search in the texture data.
-                        // That's simply not accurate, but necessary because of performance.
-                        for (int i = 0; i < xCoords.Count; i += 2)
-                        {
-                            foundSolid = false;
-                            foundTransparent = false;
-
-                            // We search between the edges inside the polygon.
-                            for (int x = (int)xCoords[i]; x <= (int)xCoords[i + 1]; x++)
-                            {
-                                // First pass: IsSolid might return false.
-                                // In that case the polygon edge doesn't lie on the texture's solid pixel, because of the hull tolearance.
-                                // If the edge lies before the first solid pixel then we need to skip our transparent pixel finds.
-
-                                // The algorithm starts to search for a relevant transparent pixel (which indicates a possible hole) 
-                                // after it has found a solid pixel.
-
-                                // After we've found a solid and a transparent pixel (a hole's left edge) 
-                                // we search for a solid pixel again (a hole's right edge).
-                                // When found the distance of that coodrinate has to be greater then the hull tolerance.
-
-                                if (IsSolid(ref x, ref y))
-                                {
-                                    if (!foundTransparent)
-                                    {
-                                        foundSolid = true;
-                                        lastSolid = x;
-                                    }
-
-                                    if (foundSolid && foundTransparent)
-                                    {
-                                        entrance = new Vector2(lastSolid, y);
-
-                                        if (DistanceToHullAcceptable(polygon, entrance.Value, true))
-                                            return entrance;
-
-                                        entrance = null;
-                                        break;
-                                    }
-                                }
-                                else
-                                {
-                                    if (foundSolid)
-                                        foundTransparent = true;
-                                }
-                            }
-                        }
-                    }
-                    else
-                    {
-                        if (xCoords.Count % 2 == 0)
-                            Debug.WriteLine("SearchCrossingEdges() % 2 != 0");
-                    }
-                }
-            }
-
-            return null;
-        }
-
-        private bool DistanceToHullAcceptable(DetectedVertices polygon, Vector2 point, bool higherDetail)
-        {
-            if (polygon == null)
-                throw new ArgumentNullException("polygon", "'polygon' can't be null.");
-
-            if (polygon.Count < 3)
-                throw new ArgumentException("'polygon.MainPolygon.Count' can't be less then 3.");
-
-            // Check the distance to main polygon.
-            if (DistanceToHullAcceptable((Vertices)polygon, point, higherDetail))
-            {
-                if (polygon.Holes != null)
-                {
-                    for (int i = 0; i < polygon.Holes.Count; i++)
-                    {
-                        // If there is one distance not acceptable then return false.
-                        if (!DistanceToHullAcceptable(polygon.Holes[i], point, higherDetail))
-                            return false;
-                    }
-                }
-
-                // All distances are larger then _hullTolerance.
-                return true;
-            }
-
-            // Default to false.
-            return false;
-        }
-
-        private bool DistanceToHullAcceptable(Vertices polygon, Vector2 point, bool higherDetail)
-        {
-            if (polygon == null)
-                throw new ArgumentNullException("polygon", "'polygon' can't be null.");
-
-            if (polygon.Count < 3)
-                throw new ArgumentException("'polygon.Count' can't be less then 3.");
-
-
-            Vector2 edgeVertex2 = polygon[polygon.Count - 1];
-            Vector2 edgeVertex1;
-
-            if (higherDetail)
-            {
-                for (int i = 0; i < polygon.Count; i++)
-                {
-                    edgeVertex1 = polygon[i];
-
-                    if (LineTools.DistanceBetweenPointAndLineSegment(ref point, ref edgeVertex1, ref edgeVertex2) <= _hullTolerance ||
-                        LineTools.DistanceBetweenPointAndPoint(ref point, ref edgeVertex1) <= _hullTolerance)
-                    {
-                        return false;
-                    }
-
-                    edgeVertex2 = polygon[i];
-                }
-
-                return true;
-            }
-            else
-            {
-                for (int i = 0; i < polygon.Count; i++)
-                {
-                    edgeVertex1 = polygon[i];
-
-                    if (LineTools.DistanceBetweenPointAndLineSegment(ref point, ref edgeVertex1, ref edgeVertex2) <= _hullTolerance)
-                    {
-                        return false;
-                    }
-
-                    edgeVertex2 = polygon[i];
-                }
-
-                return true;
-            }
-        }
-
-        private bool InPolygon(DetectedVertices polygon, Vector2 point)
-        {
-            bool inPolygon = !DistanceToHullAcceptable(polygon, point, true);
-
-            if (!inPolygon)
-            {
-                List<float> xCoords = SearchCrossingEdges(polygon, (int)point.Y);
-
-                if (xCoords.Count > 0 && xCoords.Count % 2 == 0)
-                {
-                    for (int i = 0; i < xCoords.Count; i += 2)
-                    {
-                        if (xCoords[i] <= point.X && xCoords[i + 1] >= point.X)
-                            return true;
-                    }
-                }
-
-                return false;
-            }
-
-            return true;
-        }
-
-        private Vector2? GetTopMostVertex(Vertices vertices)
-        {
-            float topMostValue = float.MaxValue;
-            Vector2? topMost = null;
-
-            for (int i = 0; i < vertices.Count; i++)
-            {
-                if (topMostValue > vertices[i].Y)
-                {
-                    topMostValue = vertices[i].Y;
-                    topMost = vertices[i];
-                }
-            }
-
-            return topMost;
-        }
-
-        private float GetTopMostCoord(Vertices vertices)
-        {
-            float returnValue = float.MaxValue;
-
-            for (int i = 0; i < vertices.Count; i++)
-            {
-                if (returnValue > vertices[i].Y)
-                {
-                    returnValue = vertices[i].Y;
-                }
-            }
-
-            return returnValue;
-        }
-
-        private float GetBottomMostCoord(Vertices vertices)
-        {
-            float returnValue = float.MinValue;
-
-            for (int i = 0; i < vertices.Count; i++)
-            {
-                if (returnValue < vertices[i].Y)
-                {
-                    returnValue = vertices[i].Y;
-                }
-            }
-
-            return returnValue;
-        }
-
-        private List<float> SearchCrossingEdges(DetectedVertices polygon, int y)
-        {
-            if (polygon == null)
-                throw new ArgumentNullException("polygon", "'polygon' can't be null.");
-
-            if (polygon.Count < 3)
-                throw new ArgumentException("'polygon.MainPolygon.Count' can't be less then 3.");
-
-            List<float> result = SearchCrossingEdges((Vertices)polygon, y);
-
-            if (polygon.Holes != null)
-            {
-                for (int i = 0; i < polygon.Holes.Count; i++)
-                {
-                    result.AddRange(SearchCrossingEdges(polygon.Holes[i], y));
-                }
-            }
-
-            result.Sort();
-            return result;
-        }
-
-        /// <summary>
-        /// Searches the polygon for the x coordinates of the edges that cross the specified y coordinate.
-        /// </summary>
-        /// <param name="polygon">Polygon to search in.</param>
-        /// <param name="y">Y coordinate to check for edges.</param>
-        /// <returns>Descending sorted list of x coordinates of edges that cross the specified y coordinate.</returns>
-        private List<float> SearchCrossingEdges(Vertices polygon, int y)
-        {
-            // sick-o-note:
-            // Used to search the x coordinates of edges in the polygon for a specific y coordinate.
-            // (Usualy comming from the texture data, that's why it's an int and not a float.)
-
-            List<float> edges = new List<float>();
-
-            // current edge
-            Vector2 slope;
-            Vector2 vertex1;    // i
-            Vector2 vertex2;    // i - 1
-
-            // next edge
-            Vector2 nextSlope;
-            Vector2 nextVertex; // i + 1
-
-            bool addFind;
-
-            if (polygon.Count > 2)
-            {
-                // There is a gap between the last and the first vertex in the vertex list.
-                // We will bridge that by setting the last vertex (vertex2) to the last 
-                // vertex in the list.
-                vertex2 = polygon[polygon.Count - 1];
-
-                // We are moving along the polygon edges.
-                for (int i = 0; i < polygon.Count; i++)
-                {
-                    vertex1 = polygon[i];
-
-                    // Approx. check if the edge crosses our y coord.
-                    if ((vertex1.Y >= y && vertex2.Y <= y) ||
-                        (vertex1.Y <= y && vertex2.Y >= y))
-                    {
-                        // Ignore edges that are parallel to y.
-                        if (vertex1.Y != vertex2.Y)
-                        {
-                            addFind = true;
-                            slope = vertex2 - vertex1;
-
-                            // Special threatment for edges that end at the y coord.
-                            if (vertex1.Y == y)
-                            {
-                                // Create preview of the next edge.
-                                nextVertex = polygon[(i + 1) % polygon.Count];
-                                nextSlope = vertex1 - nextVertex;
-
-                                // Ignore peaks. 
-                                // If thwo edges are aligned like this: /\ and the y coordinate lies on the top,
-                                // then we get the same x coord twice and we don't need that.
-                                if (slope.Y > 0)
-                                    addFind = (nextSlope.Y <= 0);
-                                else
-                                    addFind = (nextSlope.Y >= 0);
-                            }
-
-                            if (addFind)
-                                edges.Add((y - vertex1.Y) / slope.Y * slope.X + vertex1.X); // Calculate and add the x coord.
-                        }
-                    }
-
-                    // vertex1 becomes vertex2 :).
-                    vertex2 = vertex1;
-                }
-            }
-
-            edges.Sort();
-            return edges;
-        }
-
-        private bool SplitPolygonEdge(Vertices polygon, Vector2 coordInsideThePolygon,
-                                             out int vertex1Index, out int vertex2Index)
-        {
-            Vector2 slope;
-            int nearestEdgeVertex1Index = 0;
-            int nearestEdgeVertex2Index = 0;
-            bool edgeFound = false;
-
-            float shortestDistance = float.MaxValue;
-
-            bool edgeCoordFound = false;
-            Vector2 foundEdgeCoord = Vector2.Zero;
-
-            List<float> xCoords = SearchCrossingEdges(polygon, (int)coordInsideThePolygon.Y);
-
-            vertex1Index = 0;
-            vertex2Index = 0;
-
-            foundEdgeCoord.Y = coordInsideThePolygon.Y;
-
-            if (xCoords != null && xCoords.Count > 1 && xCoords.Count % 2 == 0)
-            {
-                float distance;
-                for (int i = 0; i < xCoords.Count; i++)
-                {
-                    if (xCoords[i] < coordInsideThePolygon.X)
-                    {
-                        distance = coordInsideThePolygon.X - xCoords[i];
-
-                        if (distance < shortestDistance)
-                        {
-                            shortestDistance = distance;
-                            foundEdgeCoord.X = xCoords[i];
-
-                            edgeCoordFound = true;
-                        }
-                    }
-                }
-
-                if (edgeCoordFound)
-                {
-                    shortestDistance = float.MaxValue;
-
-                    int edgeVertex2Index = polygon.Count - 1;
-
-                    int edgeVertex1Index;
-                    for (edgeVertex1Index = 0; edgeVertex1Index < polygon.Count; edgeVertex1Index++)
-                    {
-                        Vector2 tempVector1 = polygon[edgeVertex1Index];
-                        Vector2 tempVector2 = polygon[edgeVertex2Index];
-                        distance = LineTools.DistanceBetweenPointAndLineSegment(ref foundEdgeCoord,
-                                                                                ref tempVector1, ref tempVector2);
-                        if (distance < shortestDistance)
-                        {
-                            shortestDistance = distance;
-
-                            nearestEdgeVertex1Index = edgeVertex1Index;
-                            nearestEdgeVertex2Index = edgeVertex2Index;
-
-                            edgeFound = true;
-                        }
-
-                        edgeVertex2Index = edgeVertex1Index;
-                    }
-
-                    if (edgeFound)
-                    {
-                        slope = polygon[nearestEdgeVertex2Index] - polygon[nearestEdgeVertex1Index];
-                        slope.Normalize();
-
-                        Vector2 tempVector = polygon[nearestEdgeVertex1Index];
-                        distance = LineTools.DistanceBetweenPointAndPoint(ref tempVector, ref foundEdgeCoord);
-
-                        vertex1Index = nearestEdgeVertex1Index;
-                        vertex2Index = nearestEdgeVertex1Index + 1;
-
-                        polygon.Insert(nearestEdgeVertex1Index, distance * slope + polygon[vertex1Index]);
-                        polygon.Insert(nearestEdgeVertex1Index, distance * slope + polygon[vertex2Index]);
-
-                        return true;
-                    }
-                }
-            }
-
-            return false;
-        }
-
-        /// <summary>
-        /// 
-        /// </summary>
-        /// <param name="entrance"></param>
-        /// <param name="last"></param>
-        /// <returns></returns>
-        private Vertices CreateSimplePolygon(Vector2 entrance, Vector2 last)
-        {
-            bool entranceFound = false;
-            bool endOfHull = false;
-
-            Vertices polygon = new Vertices(32);
-            Vertices hullArea = new Vertices(32);
-            Vertices endOfHullArea = new Vertices(32);
-
-            Vector2 current = Vector2.Zero;
-
-            #region Entrance check
-
-            // Get the entrance point. //todo: alle möglichkeiten testen
-            if (entrance == Vector2.Zero || !InBounds(ref entrance))
-            {
-                entranceFound = SearchHullEntrance(out entrance);
-
-                if (entranceFound)
-                {
-                    current = new Vector2(entrance.X - 1f, entrance.Y);
-                }
-            }
-            else
-            {
-                if (IsSolid(ref entrance))
-                {
-                    if (IsNearPixel(ref entrance, ref last))
-                    {
-                        current = last;
-                        entranceFound = true;
-                    }
-                    else
-                    {
-                        Vector2 temp;
-                        if (SearchNearPixels(false, ref entrance, out temp))
-                        {
-                            current = temp;
-                            entranceFound = true;
-                        }
-                        else
-                        {
-                            entranceFound = false;
-                        }
-                    }
-                }
-            }
-
-            #endregion
-
-            if (entranceFound)
-            {
-                polygon.Add(entrance);
-                hullArea.Add(entrance);
-
-                Vector2 next = entrance;
-
-                do
-                {
-                    // Search in the pre vision list for an outstanding point.
-                    Vector2 outstanding;
-                    if (SearchForOutstandingVertex(hullArea, out outstanding))
-                    {
-                        if (endOfHull)
-                        {
-                            // We have found the next pixel, but is it on the last bit of the hull?
-                            if (endOfHullArea.Contains(outstanding))
-                            {
-                                // Indeed.
-                                polygon.Add(outstanding);
-                            }
-
-                            // That's enough, quit.
-                            break;
-                        }
-
-                        // Add it and remove all vertices that don't matter anymore
-                        // (all the vertices before the outstanding).
-                        polygon.Add(outstanding);
-                        hullArea.RemoveRange(0, hullArea.IndexOf(outstanding));
-                    }
-
-                    // Last point gets current and current gets next. Our little spider is moving forward on the hull ;).
-                    last = current;
-                    current = next;
-
-                    // Get the next point on hull.
-                    if (GetNextHullPoint(ref last, ref current, out next))
-                    {
-                        // Add the vertex to a hull pre vision list.
-                        hullArea.Add(next);
-                    }
-                    else
-                    {
-                        // Quit
-                        break;
-                    }
-
-                    if (next == entrance && !endOfHull)
-                    {
-                        // It's the last bit of the hull, search on and exit at next found vertex.
-                        endOfHull = true;
-                        endOfHullArea.AddRange(hullArea);
-
-                        // We don't want the last vertex to be the same as the first one, because it causes the triangulation code to crash.
-                        if (endOfHullArea.Contains(entrance))
-                            endOfHullArea.Remove(entrance);
-                    }
-
-                } while (true);
-            }
-
-            return polygon;
-        }
-
-        private bool SearchNearPixels(bool searchingForSolidPixel, ref Vector2 current, out Vector2 foundPixel)
-        {
-            for (int i = 0; i < _CLOSEPIXELS_LENGTH; i++)
-            {
-                int x = (int)current.X + ClosePixels[i, 0];
-                int y = (int)current.Y + ClosePixels[i, 1];
-
-                if (!searchingForSolidPixel ^ IsSolid(ref x, ref y))
-                {
-                    foundPixel = new Vector2(x, y);
-                    return true;
-                }
-            }
-
-            // Nothing found.
-            foundPixel = Vector2.Zero;
-            return false;
-        }
-
-        private bool IsNearPixel(ref Vector2 current, ref Vector2 near)
-        {
-            for (int i = 0; i < _CLOSEPIXELS_LENGTH; i++)
-            {
-                int x = (int)current.X + ClosePixels[i, 0];
-                int y = (int)current.Y + ClosePixels[i, 1];
-
-                if (x >= 0 && x <= _width && y >= 0 && y <= _height)
-                {
-                    if (x == (int)near.X && y == (int)near.Y)
-                    {
-                        return true;
-                    }
-                }
-            }
-
-            return false;
-        }
-
-        private bool SearchHullEntrance(out Vector2 entrance)
-        {
-            // Search for first solid pixel.
-            for (int y = 0; y <= _height; y++)
-            {
-                for (int x = 0; x <= _width; x++)
-                {
-                    if (IsSolid(ref x, ref y))
-                    {
-                        entrance = new Vector2(x, y);
-                        return true;
-                    }
-                }
-            }
-
-            // If there are no solid pixels.
-            entrance = Vector2.Zero;
-            return false;
-        }
-
-        /// <summary>
-        /// Searches for the next shape.
-        /// </summary>
-        /// <param name="detectedPolygons">Already detected polygons.</param>
-        /// <param name="start">Search start coordinate.</param>
-        /// <param name="entrance">Returns the found entrance coordinate. Null if no other shapes found.</param>
-        /// <returns>True if a new shape was found.</returns>
-        private bool SearchNextHullEntrance(List<DetectedVertices> detectedPolygons, Vector2 start, out Vector2? entrance)
-        {
-            int x;
-
-            bool foundTransparent = false;
-            bool inPolygon = false;
-
-            for (int i = (int)start.X + (int)start.Y * _width; i <= _dataLength; i++)
-            {
-                if (IsSolid(ref i))
-                {
-                    if (foundTransparent)
-                    {
-                        x = i % _width;
-                        entrance = new Vector2(x, (i - x) / (float)_width);
-
-                        inPolygon = false;
-                        for (int polygonIdx = 0; polygonIdx < detectedPolygons.Count; polygonIdx++)
-                        {
-                            if (InPolygon(detectedPolygons[polygonIdx], entrance.Value))
-                            {
-                                inPolygon = true;
-                                break;
-                            }
-                        }
-
-                        if (inPolygon)
-                            foundTransparent = false;
-                        else
-                            return true;
-                    }
-                }
-                else
-                    foundTransparent = true;
-            }
-
-            entrance = null;
-            return false;
-        }
-
-        private bool GetNextHullPoint(ref Vector2 last, ref Vector2 current, out Vector2 next)
-        {
-            int x;
-            int y;
-
-            int indexOfFirstPixelToCheck = GetIndexOfFirstPixelToCheck(ref last, ref current);
-            int indexOfPixelToCheck;
-
-            for (int i = 0; i < _CLOSEPIXELS_LENGTH; i++)
-            {
-                indexOfPixelToCheck = (indexOfFirstPixelToCheck + i) % _CLOSEPIXELS_LENGTH;
-
-                x = (int)current.X + ClosePixels[indexOfPixelToCheck, 0];
-                y = (int)current.Y + ClosePixels[indexOfPixelToCheck, 1];
-
-                if (x >= 0 && x < _width && y >= 0 && y <= _height)
-                {
-                    if (IsSolid(ref x, ref y))
-                    {
-                        next = new Vector2(x, y);
-                        return true;
-                    }
-                }
-            }
-
-            next = Vector2.Zero;
-            return false;
-        }
-
-        private bool SearchForOutstandingVertex(Vertices hullArea, out Vector2 outstanding)
-        {
-            Vector2 outstandingResult = Vector2.Zero;
-            bool found = false;
-
-            if (hullArea.Count > 2)
-            {
-                int hullAreaLastPoint = hullArea.Count - 1;
-
-                Vector2 tempVector1;
-                Vector2 tempVector2 = hullArea[0];
-                Vector2 tempVector3 = hullArea[hullAreaLastPoint];
-
-                // Search between the first and last hull point.
-                for (int i = 1; i < hullAreaLastPoint; i++)
-                {
-                    tempVector1 = hullArea[i];
-
-                    // Check if the distance is over the one that's tolerable.
-                    if (LineTools.DistanceBetweenPointAndLineSegment(ref tempVector1, ref tempVector2, ref tempVector3) >= _hullTolerance)
-                    {
-                        outstandingResult = hullArea[i];
-                        found = true;
-                        break;
-                    }
-                }
-            }
-
-            outstanding = outstandingResult;
-            return found;
-        }
-
-        private int GetIndexOfFirstPixelToCheck(ref Vector2 last, ref Vector2 current)
-        {
-            // .: pixel
-            // l: last position
-            // c: current position
-            // f: first pixel for next search
-
-            // f . .
-            // l c .
-            // . . .
-
-            //Calculate in which direction the last move went and decide over the next pixel to check.
-            switch ((int)(current.X - last.X))
-            {
-                case 1:
-                    switch ((int)(current.Y - last.Y))
-                    {
-                        case 1:
-                            return 1;
-
-                        case 0:
-                            return 0;
-
-                        case -1:
-                            return 7;
-                    }
-                    break;
-
-                case 0:
-                    switch ((int)(current.Y - last.Y))
-                    {
-                        case 1:
-                            return 2;
-
-                        case -1:
-                            return 6;
-                    }
-                    break;
-
-                case -1:
-                    switch ((int)(current.Y - last.Y))
-                    {
-                        case 1:
-                            return 3;
-
-                        case 0:
-                            return 4;
-
-                        case -1:
-                            return 5;
-                    }
-                    break;
-            }
-
-            return 0;
-        }
-    }
-}

FarseerPhysicsEngine/Common/Vertices.cs

@@ -1,955 +0,0 @@
-using System;
-using System.Collections.Generic;
-using System.Diagnostics;
-using System.Text;
-using FarseerPhysics.Collision;
-using Microsoft.Xna.Framework;
-
-namespace FarseerPhysics.Common
-{
-#if !(XBOX360)
-    [DebuggerDisplay("Count = {Count} Vertices = {ToString()}")]
-#endif
-    public class Vertices : List<Vector2>
-    {
-        public Vertices()
-        {
-        }
-
-        public Vertices(int capacity)
-        {
-            Capacity = capacity;
-        }
-
-        public Vertices(Vector2[] vector2)
-        {
-            for (int i = 0; i < vector2.Length; i++)
-            {
-                Add(vector2[i]);
-            }
-        }
-
-        public Vertices(IList<Vector2> vertices)
-        {
-            for (int i = 0; i < vertices.Count; i++)
-            {
-                Add(vertices[i]);
-            }
-        }
-
-        /// <summary>
-        /// Nexts the index.
-        /// </summary>
-        /// <param name="index">The index.</param>
-        /// <returns></returns>
-        public int NextIndex(int index)
-        {
-            if (index == Count - 1)
-            {
-                return 0;
-            }
-            return index + 1;
-        }
-
-        public Vector2 NextVertex(int index)
-        {
-            return this[NextIndex(index)];
-        }
-
-        /// <summary>
-        /// Gets the previous index.
-        /// </summary>
-        /// <param name="index">The index.</param>
-        /// <returns></returns>
-        public int PreviousIndex(int index)
-        {
-            if (index == 0)
-            {
-                return Count - 1;
-            }
-            return index - 1;
-        }
-
-        public Vector2 PreviousVertex(int index)
-        {
-            return this[PreviousIndex(index)];
-        }
-
-        /// <summary>
-        /// Gets the signed area.
-        /// </summary>
-        /// <returns></returns>
-        public float GetSignedArea()
-        {
-            int i;
-            float area = 0;
-
-            for (i = 0; i < Count; i++)
-            {
-                int j = (i + 1) % Count;
-                area += this[i].X * this[j].Y;
-                area -= this[i].Y * this[j].X;
-            }
-            area /= 2.0f;
-            return area;
-        }
-
-        /// <summary>
-        /// Gets the area.
-        /// </summary>
-        /// <returns></returns>
-        public float GetArea()
-        {
-            int i;
-            float area = 0;
-
-            for (i = 0; i < Count; i++)
-            {
-                int j = (i + 1) % Count;
-                area += this[i].X * this[j].Y;
-                area -= this[i].Y * this[j].X;
-            }
-            area /= 2.0f;
-            return (area < 0 ? -area : area);
-        }
-
-        /// <summary>
-        /// Gets the centroid.
-        /// </summary>
-        /// <returns></returns>
-        public Vector2 GetCentroid()
-        {
-            // Same algorithm is used by Box2D
-
-            Vector2 c = Vector2.Zero;
-            float area = 0.0f;
-
-            const float inv3 = 1.0f / 3.0f;
-            Vector2 pRef = Vector2.Zero;
-            for (int i = 0; i < Count; ++i)
-            {
-                // Triangle vertices.
-                Vector2 p1 = pRef;
-                Vector2 p2 = this[i];
-                Vector2 p3 = i + 1 < Count ? this[i + 1] : this[0];
-
-                Vector2 e1 = p2 - p1;
-                Vector2 e2 = p3 - p1;
-
-                float D = MathUtils.Cross(e1, e2);
-
-                float triangleArea = 0.5f * D;
-                area += triangleArea;
-
-                // Area weighted centroid
-                c += triangleArea * inv3 * (p1 + p2 + p3);
-            }
-
-            // Centroid
-            c *= 1.0f / area;
-            return c;
-        }
-
-        /// <summary>
-        /// Gets the radius based on area.
-        /// </summary>
-        /// <returns></returns>
-        public float GetRadius()
-        {
-            float area = GetSignedArea();
-
-            double radiusSqrd = (double)area / MathHelper.Pi;
-            if (radiusSqrd < 0)
-            {
-                radiusSqrd *= -1;
-            }
-
-            return (float)Math.Sqrt(radiusSqrd);
-        }
-
-        /// <summary>
-        /// Returns an AABB for vertex.
-        /// </summary>
-        /// <returns></returns>
-        public AABB GetCollisionBox()
-        {
-            AABB aabb;
-            Vector2 lowerBound = new Vector2(float.MaxValue, float.MaxValue);
-            Vector2 upperBound = new Vector2(float.MinValue, float.MinValue);
-
-            for (int i = 0; i < Count; ++i)
-            {
-                if (this[i].X < lowerBound.X)
-                {
-                    lowerBound.X = this[i].X;
-                }
-                if (this[i].X > upperBound.X)
-                {
-                    upperBound.X = this[i].X;
-                }
-
-                if (this[i].Y < lowerBound.Y)
-                {
-                    lowerBound.Y = this[i].Y;
-                }
-                if (this[i].Y > upperBound.Y)
-                {
-                    upperBound.Y = this[i].Y;
-                }
-            }
-
-            aabb.LowerBound = lowerBound;
-            aabb.UpperBound = upperBound;
-
-            return aabb;
-        }
-
-        public void Translate(Vector2 vector)
-        {
-            Translate(ref vector);
-        }
-
-        /// <summary>
-        /// Translates the vertices with the specified vector.
-        /// </summary>
-        /// <param name="vector">The vector.</param>
-        public void Translate(ref Vector2 vector)
-        {
-            for (int i = 0; i < Count; i++)
-                this[i] = Vector2.Add(this[i], vector);
-        }
-
-        /// <summary>
-        /// Scales the vertices with the specified vector.
-        /// </summary>
-        /// <param name="value">The Value.</param>
-        public void Scale(ref Vector2 value)
-        {
-            for (int i = 0; i < Count; i++)
-                this[i] = Vector2.Multiply(this[i], value);
-        }
-
-        /// <summary>
-        /// Rotate the vertices with the defined value in radians.
-        /// </summary>
-        /// <param name="value">The amount to rotate by in radians.</param>
-        public void Rotate(float value)
-        {
-            Matrix rotationMatrix;
-            Matrix.CreateRotationZ(value, out rotationMatrix);
-
-            for (int i = 0; i < Count; i++)
-                this[i] = Vector2.Transform(this[i], rotationMatrix);
-        }
-
-        /// <summary>
-        /// Assuming the polygon is simple; determines whether the polygon is convex.
-        /// NOTE: It will also return false if the input contains colinear edges.
-        /// </summary>
-        /// <returns>
-        /// 	<c>true</c> if it is convex; otherwise, <c>false</c>.
-        /// </returns>
-        public bool IsConvex()
-        {
-            // Ensure the polygon is convex and the interior
-            // is to the left of each edge.
-            for (int i = 0; i < Count; ++i)
-            {
-                int i1 = i;
-                int i2 = i + 1 < Count ? i + 1 : 0;
-                Vector2 edge = this[i2] - this[i1];
-
-                for (int j = 0; j < Count; ++j)
-                {
-                    // Don't check vertices on the current edge.
-                    if (j == i1 || j == i2)
-                    {
-                        continue;
-                    }
-
-                    Vector2 r = this[j] - this[i1];
-
-                    float s = edge.X * r.Y - edge.Y * r.X;
-
-                    if (s <= 0.0f)
-                        return false;
-                }
-            }
-            return true;
-        }
-
-        public bool IsCounterClockWise()
-        {
-            //We just return true for lines
-            if (Count < 3)
-                return true;
-
-            return (GetSignedArea() > 0.0f);
-        }
-
-        /// <summary>
-        /// Forces counter clock wise order.
-        /// </summary>
-        public void ForceCounterClockWise()
-        {
-            if (!IsCounterClockWise())
-            {
-                Reverse();
-            }
-        }
-
-        /// <summary>
-        /// Check for edge crossings
-        /// </summary>
-        /// <returns></returns>
-        public bool IsSimple()
-        {
-            for (int i = 0; i < Count; ++i)
-            {
-                int iplus = (i + 1 > Count - 1) ? 0 : i + 1;
-                Vector2 a1 = new Vector2(this[i].X, this[i].Y);
-                Vector2 a2 = new Vector2(this[iplus].X, this[iplus].Y);
-                for (int j = i + 1; j < Count; ++j)
-                {
-                    int jplus = (j + 1 > Count - 1) ? 0 : j + 1;
-                    Vector2 b1 = new Vector2(this[j].X, this[j].Y);
-                    Vector2 b2 = new Vector2(this[jplus].X, this[jplus].Y);
-
-                    Vector2 temp;
-
-                    if (LineTools.LineIntersect2(a1, a2, b1, b2, out temp))
-                    {
-                        return false;
-                    }
-                }
-            }
-            return true;
-        }
-
-        //TODO: Test
-        //Implementation found here: http://www.gamedev.net/community/forums/topic.asp?topic_id=548477
-        public bool IsSimple2()
-        {
-            for (int i = 0; i < Count; ++i)
-            {
-                if (i < Count - 1)
-                {
-                    for (int h = i + 1; h < Count; ++h)
-                    {
-                        // Do two vertices lie on top of one another?
-                        if (this[i] == this[h])
-                        {
-                            return true;
-                        }
-                    }
-                }
-
-                int j = (i + 1) % Count;
-                Vector2 iToj = this[j] - this[i];
-                Vector2 iTojNormal = new Vector2(iToj.Y, -iToj.X);
-
-                // i is the first vertex and j is the second
-                int startK = (j + 1) % Count;
-                int endK = (i - 1 + Count) % Count;
-                endK += startK < endK ? 0 : startK + 1;
-                int k = startK;
-                Vector2 iTok = this[k] - this[i];
-                bool onLeftSide = Vector2.Dot(iTok, iTojNormal) >= 0;
-                Vector2 prevK = this[k];
-                ++k;
-                for (; k <= endK; ++k)
-                {
-                    int modK = k % Count;
-                    iTok = this[modK] - this[i];
-                    if (onLeftSide != Vector2.Dot(iTok, iTojNormal) >= 0)
-                    {
-                        Vector2 prevKtoK = this[modK] - prevK;
-                        Vector2 prevKtoKNormal = new Vector2(prevKtoK.Y, -prevKtoK.X);
-                        if ((Vector2.Dot(this[i] - prevK, prevKtoKNormal) >= 0) !=
-                            (Vector2.Dot(this[j] - prevK, prevKtoKNormal) >= 0))
-                        {
-                            return true;
-                        }
-                    }
-                    onLeftSide = Vector2.Dot(iTok, iTojNormal) > 0;
-                    prevK = this[modK];
-                }
-            }
-            return false;
-        }
-
-        // From Eric Jordan's convex decomposition library
-
-        /// <summary>
-        /// Checks if polygon is valid for use in Box2d engine.
-        /// Last ditch effort to ensure no invalid polygons are
-        /// added to world geometry.
-        ///
-        /// Performs a full check, for simplicity, convexity,
-        /// orientation, minimum angle, and volume.  This won't
-        /// be very efficient, and a lot of it is redundant when
-        /// other tools in this section are used.
-        /// </summary>
-        /// <returns></returns>
-        public bool CheckPolygon()
-        {
-            int error = -1;
-            if (Count < 3 || Count > Settings.MaxPolygonVertices)
-            {
-                error = 0;
-            }
-            if (!IsConvex())
-            {
-                error = 1;
-            }
-            if (!IsSimple())
-            {
-                error = 2;
-            }
-            if (GetArea() < Settings.Epsilon)
-            {
-                error = 3;
-            }
-
-            //Compute normals
-            Vector2[] normals = new Vector2[Count];
-            Vertices vertices = new Vertices(Count);
-            for (int i = 0; i < Count; ++i)
-            {
-                vertices.Add(new Vector2(this[i].X, this[i].Y));
-                int i1 = i;
-                int i2 = i + 1 < Count ? i + 1 : 0;
-                Vector2 edge = new Vector2(this[i2].X - this[i1].X, this[i2].Y - this[i1].Y);
-                normals[i] = MathUtils.Cross(edge, 1.0f);
-                normals[i].Normalize();
-            }
-
-            //Required side checks
-            for (int i = 0; i < Count; ++i)
-            {
-                int iminus = (i == 0) ? Count - 1 : i - 1;
-
-                //Parallel sides check
-                float cross = MathUtils.Cross(normals[iminus], normals[i]);
-                cross = MathUtils.Clamp(cross, -1.0f, 1.0f);
-                float angle = (float)Math.Asin(cross);
-                if (angle <= Settings.AngularSlop)
-                {
-                    error = 4;
-                    break;
-                }
-
-                //Too skinny check
-                for (int j = 0; j < Count; ++j)
-                {
-                    if (j == i || j == (i + 1) % Count)
-                    {
-                        continue;
-                    }
-                    float s = Vector2.Dot(normals[i], vertices[j] - vertices[i]);
-                    if (s >= -Settings.LinearSlop)
-                    {
-                        error = 5;
-                    }
-                }
-
-
-                Vector2 centroid = vertices.GetCentroid();
-                Vector2 n1 = normals[iminus];
-                Vector2 n2 = normals[i];
-                Vector2 v = vertices[i] - centroid;
-
-                Vector2 d = new Vector2();
-                d.X = Vector2.Dot(n1, v); // - toiSlop;
-                d.Y = Vector2.Dot(n2, v); // - toiSlop;
-
-                // Shifting the edge inward by toiSlop should
-                // not cause the plane to pass the centroid.
-                if ((d.X < 0.0f) || (d.Y < 0.0f))
-                {
-                    error = 6;
-                }
-            }
-
-            if (error != -1)
-            {
-                Debug.WriteLine("Found invalid polygon, ");
-                switch (error)
-                {
-                    case 0:
-                        Debug.WriteLine(string.Format("must have between 3 and {0} vertices.\n",
-                                                      Settings.MaxPolygonVertices));
-                        break;
-                    case 1:
-                        Debug.WriteLine("must be convex.\n");
-                        break;
-                    case 2:
-                        Debug.WriteLine("must be simple (cannot intersect itself).\n");
-                        break;
-                    case 3:
-                        Debug.WriteLine("area is too small.\n");
-                        break;
-                    case 4:
-                        Debug.WriteLine("sides are too close to parallel.\n");
-                        break;
-                    case 5:
-                        Debug.WriteLine("polygon is too thin.\n");
-                        break;
-                    case 6:
-                        Debug.WriteLine("core shape generation would move edge past centroid (too thin).\n");
-                        break;
-                    default:
-                        Debug.WriteLine("don't know why.\n");
-                        break;
-                }
-            }
-            return error != -1;
-        }
-
-        // From Eric Jordan's convex decomposition library
-
-        /// <summary>
-        /// Trace the edge of a non-simple polygon and return a simple polygon.
-        /// 
-        /// Method:
-        /// Start at vertex with minimum y (pick maximum x one if there are two).
-        /// We aim our "lastDir" vector at (1.0, 0)
-        /// We look at the two rays going off from our start vertex, and follow whichever
-        /// has the smallest angle (in -Pi . Pi) wrt lastDir ("rightest" turn)
-        /// Loop until we hit starting vertex:
-        /// We add our current vertex to the list.
-        /// We check the seg from current vertex to next vertex for intersections
-        /// - if no intersections, follow to next vertex and continue
-        /// - if intersections, pick one with minimum distance
-        /// - if more than one, pick one with "rightest" next point (two possibilities for each)
-        /// </summary>
-        /// <param name="verts">The vertices.</param>
-        /// <returns></returns>
-        public Vertices TraceEdge(Vertices verts)
-        {
-            PolyNode[] nodes = new PolyNode[verts.Count * verts.Count];
-            //overkill, but sufficient (order of mag. is right)
-            int nNodes = 0;
-
-            //Add base nodes (raw outline)
-            for (int i = 0; i < verts.Count; ++i)
-            {
-                Vector2 pos = new Vector2(verts[i].X, verts[i].Y);
-                nodes[i].Position = pos;
-                ++nNodes;
-                int iplus = (i == verts.Count - 1) ? 0 : i + 1;
-                int iminus = (i == 0) ? verts.Count - 1 : i - 1;
-                nodes[i].AddConnection(nodes[iplus]);
-                nodes[i].AddConnection(nodes[iminus]);
-            }
-
-            //Process intersection nodes - horribly inefficient
-            bool dirty = true;
-            int counter = 0;
-            while (dirty)
-            {
-                dirty = false;
-                for (int i = 0; i < nNodes; ++i)
-                {
-                    for (int j = 0; j < nodes[i].NConnected; ++j)
-                    {
-                        for (int k = 0; k < nNodes; ++k)
-                        {
-                            if (k == i || nodes[k] == nodes[i].Connected[j]) continue;
-                            for (int l = 0; l < nodes[k].NConnected; ++l)
-                            {
-                                if (nodes[k].Connected[l] == nodes[i].Connected[j] ||
-                                    nodes[k].Connected[l] == nodes[i]) continue;
-
-                                //Check intersection
-                                Vector2 intersectPt;
-
-                                bool crosses = LineTools.LineIntersect(nodes[i].Position, nodes[i].Connected[j].Position,
-                                                                       nodes[k].Position, nodes[k].Connected[l].Position,
-                                                                       out intersectPt);
-                                if (crosses)
-                                {
-                                    dirty = true;
-                                    //Destroy and re-hook connections at crossing point
-                                    PolyNode connj = nodes[i].Connected[j];
-                                    PolyNode connl = nodes[k].Connected[l];
-                                    nodes[i].Connected[j].RemoveConnection(nodes[i]);
-                                    nodes[i].RemoveConnection(connj);
-                                    nodes[k].Connected[l].RemoveConnection(nodes[k]);
-                                    nodes[k].RemoveConnection(connl);
-                                    nodes[nNodes] = new PolyNode(intersectPt);
-                                    nodes[nNodes].AddConnection(nodes[i]);
-                                    nodes[i].AddConnection(nodes[nNodes]);
-                                    nodes[nNodes].AddConnection(nodes[k]);
-                                    nodes[k].AddConnection(nodes[nNodes]);
-                                    nodes[nNodes].AddConnection(connj);
-                                    connj.AddConnection(nodes[nNodes]);
-                                    nodes[nNodes].AddConnection(connl);
-                                    connl.AddConnection(nodes[nNodes]);
-                                    ++nNodes;
-                                    goto SkipOut;
-                                }
-                            }
-                        }
-                    }
-                }
-            SkipOut:
-                ++counter;
-            }
-
-            //Collapse duplicate points
-            bool foundDupe = true;
-            int nActive = nNodes;
-            while (foundDupe)
-            {
-                foundDupe = false;
-                for (int i = 0; i < nNodes; ++i)
-                {
-                    if (nodes[i].NConnected == 0) continue;
-                    for (int j = i + 1; j < nNodes; ++j)
-                    {
-                        if (nodes[j].NConnected == 0) continue;
-                        Vector2 diff = nodes[i].Position - nodes[j].Position;
-                        if (diff.LengthSquared() <= Settings.Epsilon * Settings.Epsilon)
-                        {
-                            if (nActive <= 3)
-                                return new Vertices();
-
-                            //printf("Found dupe, %d left\n",nActive);
-                            --nActive;
-                            foundDupe = true;
-                            PolyNode inode = nodes[i];
-                            PolyNode jnode = nodes[j];
-                            //Move all of j's connections to i, and orphan j
-                            int njConn = jnode.NConnected;
-                            for (int k = 0; k < njConn; ++k)
-                            {
-                                PolyNode knode = jnode.Connected[k];
-                                Debug.Assert(knode != jnode);
-                                if (knode != inode)
-                                {
-                                    inode.AddConnection(knode);
-                                    knode.AddConnection(inode);
-                                }
-                                knode.RemoveConnection(jnode);
-                            }
-                            jnode.NConnected = 0;
-                        }
-                    }
-                }
-            }
-
-            //Now walk the edge of the list
-
-            //Find node with minimum y value (max x if equal)
-            float minY = float.MaxValue;
-            float maxX = -float.MaxValue;
-            int minYIndex = -1;
-            for (int i = 0; i < nNodes; ++i)
-            {
-                if (nodes[i].Position.Y < minY && nodes[i].NConnected > 1)
-                {
-                    minY = nodes[i].Position.Y;
-                    minYIndex = i;
-                    maxX = nodes[i].Position.X;
-                }
-                else if (nodes[i].Position.Y == minY && nodes[i].Position.X > maxX && nodes[i].NConnected > 1)
-                {
-                    minYIndex = i;
-                    maxX = nodes[i].Position.X;
-                }
-            }
-
-            Vector2 origDir = new Vector2(1.0f, 0.0f);
-            Vector2[] resultVecs = new Vector2[4 * nNodes];
-            // nodes may be visited more than once, unfortunately - change to growable array!
-            int nResultVecs = 0;
-            PolyNode currentNode = nodes[minYIndex];
-            PolyNode startNode = currentNode;
-            Debug.Assert(currentNode.NConnected > 0);
-            PolyNode nextNode = currentNode.GetRightestConnection(origDir);
-            if (nextNode == null)
-            {
-                Vertices vertices = new Vertices(nResultVecs);
-
-                for (int i = 0; i < nResultVecs; ++i)
-                {
-                    vertices.Add(resultVecs[i]);
-                }
-
-                return vertices;
-            }
-
-            // Borked, clean up our mess and return
-            resultVecs[0] = startNode.Position;
-            ++nResultVecs;
-            while (nextNode != startNode)
-            {
-                if (nResultVecs > 4 * nNodes)
-                {
-                    Debug.Assert(false);
-                }
-                resultVecs[nResultVecs++] = nextNode.Position;
-                PolyNode oldNode = currentNode;
-                currentNode = nextNode;
-                nextNode = currentNode.GetRightestConnection(oldNode);
-                if (nextNode == null)
-                {
-                    Vertices vertices = new Vertices(nResultVecs);
-                    for (int i = 0; i < nResultVecs; ++i)
-                    {
-                        vertices.Add(resultVecs[i]);
-                    }
-                    return vertices;
-                }
-                // There was a problem, so jump out of the loop and use whatever garbage we've generated so far
-            }
-
-            return new Vertices();
-        }
-
-        private class PolyNode
-        {
-            private const int MaxConnected = 32;
-
-            /*
-             * Given sines and cosines, tells if A's angle is less than B's on -Pi, Pi
-             * (in other words, is A "righter" than B)
-             */
-            public PolyNode[] Connected = new PolyNode[MaxConnected];
-            public int NConnected;
-            public Vector2 Position;
-
-            public PolyNode(Vector2 pos)
-            {
-                Position = pos;
-                NConnected = 0;
-            }
-
-            private bool IsRighter(float sinA, float cosA, float sinB, float cosB)
-            {
-                if (sinA < 0)
-                {
-                    if (sinB > 0 || cosA <= cosB) return true;
-                    else return false;
-                }
-                else
-                {
-                    if (sinB < 0 || cosA <= cosB) return false;
-                    else return true;
-                }
-            }
-
-            public void AddConnection(PolyNode toMe)
-            {
-                Debug.Assert(NConnected < MaxConnected);
-
-                // Ignore duplicate additions
-                for (int i = 0; i < NConnected; ++i)
-                {
-                    if (Connected[i] == toMe) return;
-                }
-                Connected[NConnected] = toMe;
-                ++NConnected;
-            }
-
-            public void RemoveConnection(PolyNode fromMe)
-            {
-                bool isFound = false;
-                int foundIndex = -1;
-                for (int i = 0; i < NConnected; ++i)
-                {
-                    if (fromMe == Connected[i])
-                    {
-                        //.position == connected[i].position){
-                        isFound = true;
-                        foundIndex = i;
-                        break;
-                    }
-                }
-                Debug.Assert(isFound);
-                --NConnected;
-                for (int i = foundIndex; i < NConnected; ++i)
-                {
-                    Connected[i] = Connected[i + 1];
-                }
-            }
-
-            public PolyNode GetRightestConnection(PolyNode incoming)
-            {
-                if (NConnected == 0) Debug.Assert(false); // This means the connection graph is inconsistent
-                if (NConnected == 1)
-                {
-                    //b2Assert(false);
-                    // Because of the possibility of collapsing nearby points,
-                    // we may end up with "spider legs" dangling off of a region.
-                    // The correct behavior here is to turn around.
-                    return incoming;
-                }
-                Vector2 inDir = Position - incoming.Position;
-
-                float inLength = inDir.Length();
-                inDir.Normalize();
-
-                Debug.Assert(inLength > Settings.Epsilon);
-
-                PolyNode result = null;
-                for (int i = 0; i < NConnected; ++i)
-                {
-                    if (Connected[i] == incoming) continue;
-                    Vector2 testDir = Connected[i].Position - Position;
-                    float testLengthSqr = testDir.LengthSquared();
-                    testDir.Normalize();
-                    Debug.Assert(testLengthSqr >= Settings.Epsilon * Settings.Epsilon);
-                    float myCos = Vector2.Dot(inDir, testDir);
-                    float mySin = MathUtils.Cross(inDir, testDir);
-                    if (result != null)
-                    {
-                        Vector2 resultDir = result.Position - Position;
-                        resultDir.Normalize();
-                        float resCos = Vector2.Dot(inDir, resultDir);
-                        float resSin = MathUtils.Cross(inDir, resultDir);
-                        if (IsRighter(mySin, myCos, resSin, resCos))
-                        {
-                            result = Connected[i];
-                        }
-                    }
-                    else
-                    {
-                        result = Connected[i];
-                    }
-                }
-
-                Debug.Assert(result != null);
-
-                return result;
-            }
-
-            public PolyNode GetRightestConnection(Vector2 incomingDir)
-            {
-                Vector2 diff = Position - incomingDir;
-                PolyNode temp = new PolyNode(diff);
-                PolyNode res = GetRightestConnection(temp);
-                Debug.Assert(res != null);
-                return res;
-            }
-        }
-
-        public override string ToString()
-        {
-            StringBuilder builder = new StringBuilder();
-            for (int i = 0; i < Count; i++)
-            {
-                builder.Append(this[i].ToString());
-                if (i < Count - 1)
-                {
-                    builder.Append(" ");
-                }
-            }
-            return builder.ToString();
-        }
-
-        /// <summary>
-        /// Projects to axis.
-        /// </summary>
-        /// <param name="axis">The axis.</param>
-        /// <param name="min">The min.</param>
-        /// <param name="max">The max.</param>
-        public void ProjectToAxis(ref Vector2 axis, out float min, out float max)
-        {
-            // To project a point on an axis use the dot product
-            float dotProduct = Vector2.Dot(axis, this[0]);
-            min = dotProduct;
-            max = dotProduct;
-
-            for (int i = 0; i < Count; i++)
-            {
-                dotProduct = Vector2.Dot(this[i], axis);
-                if (dotProduct < min)
-                {
-                    min = dotProduct;
-                }
-                else
-                {
-                    if (dotProduct > max)
-                    {
-                        max = dotProduct;
-                    }
-                }
-            }
-        }
-
-        /// <summary>
-        /// Winding number test for a point in a polygon.
-        /// </summary>
-        /// See more info about the algorithm here: http://softsurfer.com/Archive/algorithm_0103/algorithm_0103.htm
-        /// <param name="point">The point to be tested.</param>
-        /// <returns>-1 if the winding number is zero and the point is outside
-        /// the polygon, 1 if the point is inside the polygon, and 0 if the point
-        /// is on the polygons edge.</returns>
-        public int PointInPolygon(ref Vector2 point)
-        {
-            // Winding number
-            int wn = 0;
-
-            // Iterate through polygon's edges
-            for (int i = 0; i < Count; i++)
-            {
-                // Get points
-                Vector2 p1 = this[i];
-                Vector2 p2 = this[NextIndex(i)];
-
-                // Test if a point is directly on the edge
-                Vector2 edge = p2 - p1;
-                float area = MathUtils.Area(ref p1, ref p2, ref point);
-                if (area == 0f && Vector2.Dot(point - p1, edge) >= 0f && Vector2.Dot(point - p2, edge) <= 0f)
-                {
-                    return 0;
-                }
-                // Test edge for intersection with ray from point
-                if (p1.Y <= point.Y)
-                {
-                    if (p2.Y > point.Y && area > 0f)
-                    {
-                        ++wn;
-                    }
-                }
-                else
-                {
-                    if (p2.Y <= point.Y && area < 0f)
-                    {
-                        --wn;
-                    }
-                }
-            }
-            return (wn == 0 ? -1 : 1);
-        }
-
-        /// <summary>
-        /// Compute the sum of the angles made between the test point and each pair of points making up the polygon. 
-        /// If this sum is 2pi then the point is an interior point, if 0 then the point is an exterior point. 
-        /// ref: http://ozviz.wasp.uwa.edu.au/~pbourke/geometry/insidepoly/  - Solution 2 
-        /// </summary>
-        public bool PointInPolygonAngle(ref Vector2 point)
-        {
-            double angle = 0;
-
-            // Iterate through polygon's edges
-            for (int i = 0; i < Count; i++)
-            {
-                // Get points
-                Vector2 p1 = this[i] - point;
-                Vector2 p2 = this[NextIndex(i)] - point;
-
-                angle += MathUtils.VectorAngle(ref p1, ref p2);
-            }
-
-            if (Math.Abs(angle) < Math.PI)
-            {
-                return false;
-            }
-
-            return true;
-        }
-    }
-}

FarseerPhysicsEngine/Controllers/AbstractForceController.cs

@@ -1,323 +0,0 @@
-using System;
-using FarseerPhysics.Dynamics;
-using Microsoft.Xna.Framework;
-
-namespace FarseerPhysics.Controllers
-{
-    public abstract class AbstractForceController : Controller
-    {
-        #region DecayModes enum
-
-        /// <summary>
-        /// Modes for Decay. Actual Decay must be implemented in inheriting 
-        /// classes
-        /// </summary>
-        public enum DecayModes
-        {
-            None,
-            Step,
-            Linear,
-            InverseSquare,
-            Curve
-        }
-
-        #endregion
-
-        #region ForceTypes enum
-
-        /// <summary>
-        /// Forcetypes are used in the decay math to properly get the distance.
-        /// They are also used to draw a representation in DebugView
-        /// </summary>
-        public enum ForceTypes
-        {
-            Point,
-            Line,
-            Area
-        }
-
-        #endregion
-
-        #region TimingModes enum
-
-        /// <summary>
-        /// Timing Modes
-        /// Switched: Standard on/off mode using the baseclass enabled property
-        /// Triggered: When the Trigger() method is called the force is active 
-        /// for a specified Impulse Length
-        /// Curve: Still to be defined. The basic idea is having a Trigger 
-        /// combined with a curve for the strength
-        /// </summary>
-        public enum TimingModes
-        {
-            Switched,
-            Triggered,
-            Curve
-        }
-
-        #endregion
-
-        /// <summary>
-        /// Curve to be used for Decay in Curve mode
-        /// </summary>
-        public Curve DecayCurve;
-
-        /// <summary>
-        /// The Forcetype of the instance
-        /// </summary>
-        public ForceTypes ForceType;
-
-        /// <summary>
-        /// Provided for reuse to provide Variation functionality in 
-        /// inheriting classes
-        /// </summary>
-        protected Random Randomize;
-
-        /// <summary>
-        /// Curve used by Curve Mode as an animated multiplier for the force 
-        /// strength.
-        /// Only positions between 0 and 1 are considered as that range is 
-        /// stretched to have ImpulseLength.
-        /// </summary>
-        public Curve StrengthCurve;
-
-        /// <summary>
-        /// Constructor
-        /// </summary>
-        public AbstractForceController()
-            : base(ControllerType.AbstractForceController)
-        {
-            Enabled = true;
-
-            Strength = 1.0f;
-            Position = new Vector2(0, 0);
-            MaximumSpeed = 100.0f;
-            TimingMode = TimingModes.Switched;
-            ImpulseTime = 0.0f;
-            ImpulseLength = 1.0f;
-            Triggered = false;
-            StrengthCurve = new Curve();
-            Variation = 0.0f;
-            Randomize = new Random(1234);
-            DecayMode = DecayModes.None;
-            DecayCurve = new Curve();
-            DecayStart = 0.0f;
-            DecayEnd = 0.0f;
-
-            StrengthCurve.Keys.Add(new CurveKey(0, 5));
-            StrengthCurve.Keys.Add(new CurveKey(0.1f, 5));
-            StrengthCurve.Keys.Add(new CurveKey(0.2f, -4));
-            StrengthCurve.Keys.Add(new CurveKey(1f, 0));
-        }
-
-        /// <summary>
-        /// Overloaded Contstructor with supplying Timing Mode
-        /// </summary>
-        /// <param name="mode"></param>
-        public AbstractForceController(TimingModes mode)
-            : base(ControllerType.AbstractForceController)
-        {
-            TimingMode = mode;
-            switch (mode)
-            {
-                case TimingModes.Switched:
-                    Enabled = true;
-                    break;
-                case TimingModes.Triggered:
-                    Enabled = false;
-                    break;
-                case TimingModes.Curve:
-                    Enabled = false;
-                    break;
-            }
-        }
-
-        /// <summary>
-        /// Global Strength of the force to be applied
-        /// </summary>
-        public float Strength { get; set; }
-
-        /// <summary>
-        /// Position of the Force. Can be ignored (left at (0,0) for forces
-        /// that are not position-dependent
-        /// </summary>
-        public Vector2 Position { get; set; }
-
-        /// <summary>
-        /// Maximum speed of the bodies. Bodies that are travelling faster are
-        /// supposed to be ignored
-        /// </summary>
-        public float MaximumSpeed { get; set; }
-
-        /// <summary>
-        /// Maximum Force to be applied. As opposed to Maximum Speed this is 
-        /// independent of the velocity of
-        /// the affected body
-        /// </summary>
-        public float MaximumForce { get; set; }
-
-        /// <summary>
-        /// Timing Mode of the force instance
-        /// </summary>
-        public TimingModes TimingMode { get; set; }
-
-        /// <summary>
-        /// Time of the current impulse. Incremented in update till 
-        /// ImpulseLength is reached
-        /// </summary>
-        public float ImpulseTime { get; private set; }
-
-        /// <summary>
-        /// Length of a triggered impulse. Used in both Triggered and Curve Mode
-        /// </summary>
-        public float ImpulseLength { get; set; }
-
-        /// <summary>
-        /// Indicating if we are currently during an Impulse 
-        /// (Triggered and Curve Mode)
-        /// </summary>
-        public bool Triggered { get; private set; }
-
-        /// <summary>
-        /// Variation of the force applied to each body affected
-        /// !! Must be used in inheriting classes properly !!
-        /// </summary>
-        public float Variation { get; set; }
-
-        /// <summary>
-        /// See DecayModes
-        /// </summary>
-        public DecayModes DecayMode { get; set; }
-
-        /// <summary>
-        /// Start of the distance based Decay. To set a non decaying area
-        /// </summary>
-        public float DecayStart { get; set; }
-
-        /// <summary>
-        /// Maximum distance a force should be applied
-        /// </summary>
-        public float DecayEnd { get; set; }
-
-        /// <summary>
-        /// Calculate the Decay for a given body. Meant to ease force 
-        /// development and stick to the DRY principle and provide unified and 
-        /// predictable decay math.
-        /// </summary>
-        /// <param name="body">The body to calculate decay for</param>
-        /// <returns>A multiplier to multiply the force with to add decay 
-        /// support in inheriting classes</returns>
-        protected float GetDecayMultiplier(Body body)
-        {
-            //TODO: Consider ForceType in distance calculation!
-            float distance = (body.Position - Position).Length();
-            switch (DecayMode)
-            {
-                case DecayModes.None:
-                    {
-                        return 1.0f;
-                    }
-                case DecayModes.Step:
-                    {
-                        if (distance < DecayEnd)
-                            return 1.0f;
-                        else
-                            return 0.0f;
-                    }
-                case DecayModes.Linear:
-                    {
-                        if (distance < DecayStart)
-                            return 1.0f;
-                        if (distance > DecayEnd)
-                            return 0.0f;
-                        return (DecayEnd - DecayStart / distance - DecayStart);
-                    }
-                case DecayModes.InverseSquare:
-                    {
-                        if (distance < DecayStart)
-                            return 1.0f;
-                        else
-                            return 1.0f / ((distance - DecayStart) * (distance - DecayStart));
-                    }
-                case DecayModes.Curve:
-                    {
-                        if (distance < DecayStart)
-                            return 1.0f;
-                        else
-                            return DecayCurve.Evaluate(distance - DecayStart);
-                    }
-                default:
-                    return 1.0f;
-            }
-        }
-
-        /// <summary>
-        /// Triggers the trigger modes (Trigger and Curve)
-        /// </summary>
-        public void Trigger()
-        {
-            Triggered = true;
-            ImpulseTime = 0;
-        }
-
-        /// <summary>
-        /// Inherited from Controller
-        /// Depending on the TimingMode perform timing logic and call ApplyForce()
-        /// </summary>
-        /// <param name="dt"></param>
-        public override void Update(float dt)
-        {
-            switch (TimingMode)
-            {
-                case TimingModes.Switched:
-                    {
-                        if (Enabled)
-                        {
-                            ApplyForce(dt, Strength);
-                        }
-                        break;
-                    }
-                case TimingModes.Triggered:
-                    {
-                        if (Enabled && Triggered)
-                        {
-                            if (ImpulseTime < ImpulseLength)
-                            {
-                                ApplyForce(dt, Strength);
-                                ImpulseTime += dt;
-                            }
-                            else
-                            {
-                                Triggered = false;
-                            }
-                        }
-                        break;
-                    }
-                case TimingModes.Curve:
-                    {
-                        if (Enabled && Triggered)
-                        {
-                            if (ImpulseTime < ImpulseLength)
-                            {
-                                ApplyForce(dt, Strength * StrengthCurve.Evaluate(ImpulseTime));
-                                ImpulseTime += dt;
-                            }
-                            else
-                            {
-                                Triggered = false;
-                            }
-                        }
-                        break;
-                    }
-            }
-        }
-
-        /// <summary>
-        /// Apply the force supplying strength (wich is modified in Update() 
-        /// according to the TimingMode
-        /// </summary>
-        /// <param name="dt"></param>
-        /// <param name="strength">The strength</param>
-        public abstract void ApplyForce(float dt, float strength);
-    }
-}

FarseerPhysicsEngine/Controllers/BuoyancyController.cs

@@ -1,135 +0,0 @@
-using System.Collections.Generic;
-using FarseerPhysics.Collision;
-using FarseerPhysics.Collision.Shapes;
-using FarseerPhysics.Dynamics;
-using Microsoft.Xna.Framework;
-
-namespace FarseerPhysics.Controllers
-{
-    public sealed class BuoyancyController : Controller
-    {
-        /// <summary>
-        /// Controls the rotational drag that the fluid exerts on the bodies within it. Use higher values will simulate thick fluid, like honey, lower values to
-        /// simulate water-like fluids. 
-        /// </summary>
-        public float AngularDragCoefficient;
-
-        /// <summary>
-        /// Density of the fluid. Higher values will make things more buoyant, lower values will cause things to sink.
-        /// </summary>
-        public float Density;
-
-        /// <summary>
-        /// Controls the linear drag that the fluid exerts on the bodies within it.  Use higher values will simulate thick fluid, like honey, lower values to
-        /// simulate water-like fluids.
-        /// </summary>
-        public float LinearDragCoefficient;
-
-        /// <summary>
-        /// Acts like waterflow. Defaults to 0,0.
-        /// </summary>
-        public Vector2 Velocity;
-
-        private AABB _container;
-
-        private Vector2 _gravity;
-        private Vector2 _normal;
-        private float _offset;
-        private Dictionary<int, Body> _uniqueBodies = new Dictionary<int, Body>();
-
-        /// <summary>
-        /// Initializes a new instance of the <see cref="BuoyancyController"/> class.
-        /// </summary>
-        /// <param name="container">Only bodies inside this AABB will be influenced by the controller</param>
-        /// <param name="density">Density of the fluid</param>
-        /// <param name="linearDragCoefficient">Linear drag coefficient of the fluid</param>
-        /// <param name="rotationalDragCoefficient">Rotational drag coefficient of the fluid</param>
-        /// <param name="gravity">The direction gravity acts. Buoyancy force will act in opposite direction of gravity.</param>
-        public BuoyancyController(AABB container, float density, float linearDragCoefficient,
-                                  float rotationalDragCoefficient, Vector2 gravity)
-            : base(ControllerType.BuoyancyController)
-        {
-            Container = container;
-            _normal = new Vector2(0, 1);
-            Density = density;
-            LinearDragCoefficient = linearDragCoefficient;
-            AngularDragCoefficient = rotationalDragCoefficient;
-            _gravity = gravity;
-        }
-
-        public AABB Container
-        {
-            get { return _container; }
-            set
-            {
-                _container = value;
-                _offset = _container.UpperBound.Y;
-            }
-        }
-
-        public override void Update(float dt)
-        {
-            _uniqueBodies.Clear();
-            World.QueryAABB(fixture =>
-                                {
-                                    if (fixture.Body.IsStatic || !fixture.Body.Awake)
-                                        return true;
-
-                                    if (!_uniqueBodies.ContainsKey(fixture.Body.BodyId))
-                                        _uniqueBodies.Add(fixture.Body.BodyId, fixture.Body);
-
-                                    return true;
-                                }, ref _container);
-
-            foreach (KeyValuePair<int, Body> kv in _uniqueBodies)
-            {
-                Body body = kv.Value;
-
-                Vector2 areac = Vector2.Zero;
-                Vector2 massc = Vector2.Zero;
-                float area = 0;
-                float mass = 0;
-
-                for (int j = 0; j < body.FixtureList.Count; j++)
-                {
-                    Fixture fixture = body.FixtureList[j];
-
-                    if (fixture.Shape.ShapeType != ShapeType.Polygon && fixture.Shape.ShapeType != ShapeType.Circle)
-                        continue;
-
-                    Shape shape = fixture.Shape;
-
-                    Vector2 sc;
-                    float sarea = shape.ComputeSubmergedArea(_normal, _offset, body.Xf, out sc);
-                    area += sarea;
-                    areac.X += sarea * sc.X;
-                    areac.Y += sarea * sc.Y;
-
-                    mass += sarea * shape.Density;
-                    massc.X += sarea * sc.X * shape.Density;
-                    massc.Y += sarea * sc.Y * shape.Density;
-                }
-
-                areac.X /= area;
-                areac.Y /= area;
-                massc.X /= mass;
-                massc.Y /= mass;
-
-                if (area < Settings.Epsilon)
-                    continue;
-
-                //Buoyancy
-                Vector2 buoyancyForce = -Density * area * _gravity;
-                body.ApplyForce(buoyancyForce, massc);
-
-                //Linear drag
-                Vector2 dragForce = body.GetLinearVelocityFromWorldPoint(areac) - Velocity;
-                dragForce *= -LinearDragCoefficient * area;
-                body.ApplyForce(dragForce, areac);
-
-                //Angular drag
-                body.ApplyTorque(-body.Inertia / body.Mass * area * body.AngularVelocity * AngularDragCoefficient);
-            }
-        }
-    }
-}

FarseerPhysicsEngine/Controllers/Controller.cs

@@ -1,71 +0,0 @@
-using System;
-using FarseerPhysics.Dynamics;
-
-namespace FarseerPhysics.Controllers
-{
-    [Flags]
-    public enum ControllerType
-    {
-        GravityController = (1 << 0),
-        VelocityLimitController = (1 << 1),
-        AbstractForceController = (1 << 2),
-        BuoyancyController = (1 << 3),
-    }
-
-    public struct ControllerFilter
-    {
-        public ControllerType ControllerFlags;
-
-        /// <summary>
-        /// Ignores the controller. The controller has no effect on this body.
-        /// </summary>
-        /// <param name="controller">The controller type.</param>
-        public void IgnoreController(ControllerType controller)
-        {
-            ControllerFlags |= controller;
-        }
-
-        /// <summary>
-        /// Restore the controller. The controller affects this body.
-        /// </summary>
-        /// <param name="controller">The controller type.</param>
-        public void RestoreController(ControllerType controller)
-        {
-            ControllerFlags &= ~controller;
-        }
-
-        /// <summary>
-        /// Determines whether this body ignores the the specified controller.
-        /// </summary>
-        /// <param name="controller">The controller type.</param>
-        /// <returns>
-        /// 	<c>true</c> if the body has the specified flag; otherwise, <c>false</c>.
-        /// </returns>
-        public bool IsControllerIgnored(ControllerType controller)
-        {
-            return (ControllerFlags & controller) == controller;
-        }
-    }
-
-    public abstract class Controller : FilterData
-    {
-        public bool Enabled;
-        public World World;
-        private ControllerType _type;
-
-        public Controller(ControllerType controllerType)
-        {
-            _type = controllerType;
-        }
-
-        public override bool IsActiveOn(Body body)
-        {
-            if (body.ControllerFilter.IsControllerIgnored(_type))
-                return false;
-
-            return base.IsActiveOn(body);
-        }
-
-        public abstract void Update(float dt);
-    }
-}

FarseerPhysicsEngine/Controllers/GravityController.cs

@@ -1,117 +0,0 @@
-using System;
-using System.Collections.Generic;
-using FarseerPhysics.Dynamics;
-using Microsoft.Xna.Framework;
-
-namespace FarseerPhysics.Controllers
-{
-    public enum GravityType
-    {
-        Linear,
-        DistanceSquared
-    }
-
-    public class GravityController : Controller
-    {
-        public List<Body> Bodies = new List<Body>();
-        public List<Vector2> Points = new List<Vector2>();
-
-        public GravityController(float strength)
-            : base(ControllerType.GravityController)
-        {
-            Strength = strength;
-            MaxRadius = float.MaxValue;
-        }
-
-        public GravityController(float strength, float maxRadius, float minRadius)
-            : base(ControllerType.GravityController)
-        {
-            MinRadius = minRadius;
-            MaxRadius = maxRadius;
-            Strength = strength;
-        }
-
-        public float MinRadius { get; set; }
-        public float MaxRadius { get; set; }
-        public float Strength { get; set; }
-        public GravityType GravityType { get; set; }
-
-        public override void Update(float dt)
-        {
-            Vector2 f = Vector2.Zero;
-
-            foreach (Body body1 in World.BodyList)
-            {
-                if (!IsActiveOn(body1))
-                    continue;
-
-                foreach (Body body2 in Bodies)
-                {
-                    if (body1 == body2 || (body1.IsStatic && body2.IsStatic) || !body2.Enabled)
-                        continue;
-
-                    Vector2 d = body2.WorldCenter - body1.WorldCenter;
-                    float r2 = d.LengthSquared();
-
-                    if (r2 < Settings.Epsilon)
-                        continue;
-
-                    float r = d.Length();
-
-                    if (r >= MaxRadius || r <= MinRadius)
-                        continue;
-
-                    switch (GravityType)
-                    {
-                        case GravityType.DistanceSquared:
-                            f = Strength / r2 / (float)Math.Sqrt(r2) * body1.Mass * body2.Mass * d;
-                            break;
-                        case GravityType.Linear:
-                            f = Strength / r2 * body1.Mass * body2.Mass * d;
-                            break;
-                    }
-
-                    body1.ApplyForce(ref f);
-                    Vector2.Negate(ref f, out f);
-                    body2.ApplyForce(ref f);
-                }
-
-                foreach (Vector2 point in Points)
-                {
-                    Vector2 d = point - body1.Position;
-                    float r2 = d.LengthSquared();
-
-                    if (r2 < Settings.Epsilon)
-                        continue;
-
-                    float r = d.Length();
-
-                    if (r >= MaxRadius || r <= MinRadius)
-                        continue;
-
-                    switch (GravityType)
-                    {
-                        case GravityType.DistanceSquared:
-                            f = Strength / r2 / (float)Math.Sqrt(r2) * body1.Mass * d;
-                            break;
-                        case GravityType.Linear:
-                            f = Strength / r2 * body1.Mass * d;
-                            break;
-                    }
-
-                    body1.ApplyForce(ref f);
-                }
-            }
-        }
-
-        public void AddBody(Body body)
-        {
-            Bodies.Add(body);
-        }
-
-        public void AddPoint(Vector2 point)
-        {
-            Points.Add(point);
-        }
-    }
-}

FarseerPhysicsEngine/Controllers/SimpleWindForce.cs

@@ -1,75 +0,0 @@
-using FarseerPhysics.Dynamics;
-using Microsoft.Xna.Framework;
-
-namespace FarseerPhysics.Controllers
-{
-    /// <summary>
-    /// Reference implementation for forces based on AbstractForceController
-    /// It supports all features provided by the base class and illustrates proper
-    /// usage as an easy to understand example.
-    /// As a side-effect it is a nice and easy to use wind force for your projects
-    /// </summary>
-    public class SimpleWindForce : AbstractForceController
-    {
-        /// <summary>
-        /// Direction of the windforce
-        /// </summary>
-        public Vector2 Direction { get; set; }
-
-        /// <summary>
-        /// The amount of Direction randomization. Allowed range is 0-1.
-        /// </summary>
-        public float Divergence { get; set; }
-
-        /// <summary>
-        /// Ignore the position and apply the force. If off only in the "front" (relative to position and direction)
-        /// will be affected
-        /// </summary>
-        public bool IgnorePosition { get; set; }
-
-
-        public override void ApplyForce(float dt, float strength)
-        {
-            foreach (Body body in World.BodyList)
-            {
-                //TODO: Consider Force Type
-                float decayMultiplier = GetDecayMultiplier(body);
-
-                if (decayMultiplier != 0)
-                {
-                    Vector2 forceVector;
-
-                    if (ForceType == ForceTypes.Point)
-                    {
-                        forceVector = body.Position - Position;
-                    }
-                    else
-                    {
-                        Direction.Normalize();
-
-                        forceVector = Direction;
-
-                        if (forceVector.Length() == 0)
-                            forceVector = new Vector2(0, 1);
-                    }
-
-                    //TODO: Consider Divergence:
-                    //forceVector = Vector2.Transform(forceVector, Matrix.CreateRotationZ((MathHelper.Pi - MathHelper.Pi/2) * (float)Randomize.NextDouble()));
-
-                    // Calculate random Variation
-                    if (Variation != 0)
-                    {
-                        float strengthVariation = (float)Randomize.NextDouble() * MathHelper.Clamp(Variation, 0, 1);
-                        forceVector.Normalize();
-                        body.ApplyForce(forceVector * strength * decayMultiplier * strengthVariation);
-                    }
-                    else
-                    {
-                        forceVector.Normalize();
-                        body.ApplyForce(forceVector * strength * decayMultiplier);
-                    }
-                }
-            }
-        }
-    }
-}

FarseerPhysicsEngine/Controllers/VelocityLimitController.cs

@@ -1,129 +0,0 @@
-using System;
-using System.Collections.Generic;
-using FarseerPhysics.Dynamics;
-
-namespace FarseerPhysics.Controllers
-{
-    /// <summary>
-    /// Put a limit on the linear (translation - the movespeed) and angular (rotation) velocity
-    /// of bodies added to this controller.
-    /// </summary>
-    public class VelocityLimitController : Controller
-    {
-        public bool LimitAngularVelocity = true;
-        public bool LimitLinearVelocity = true;
-        private List<Body> _bodies = new List<Body>();
-        private float _maxAngularSqared;
-        private float _maxAngularVelocity;
-        private float _maxLinearSqared;
-        private float _maxLinearVelocity;
-
-        /// <summary>
-        /// Initializes a new instance of the <see cref="VelocityLimitController"/> class.
-        /// Sets the max linear velocity to Settings.MaxTranslation
-        /// Sets the max angular velocity to Settings.MaxRotation
-        /// </summary>
-        public VelocityLimitController()
-            : base(ControllerType.VelocityLimitController)
-        {
-            MaxLinearVelocity = Settings.MaxTranslation;
-            MaxAngularVelocity = Settings.MaxRotation;
-        }
-
-        /// <summary>
-        /// Initializes a new instance of the <see cref="VelocityLimitController"/> class.
-        /// Pass in 0 or float.MaxValue to disable the limit.
-        /// maxAngularVelocity = 0 will disable the angular velocity limit.
-        /// </summary>
-        /// <param name="maxLinearVelocity">The max linear velocity.</param>
-        /// <param name="maxAngularVelocity">The max angular velocity.</param>
-        public VelocityLimitController(float maxLinearVelocity, float maxAngularVelocity)
-            : base(ControllerType.VelocityLimitController)
-        {
-            if (maxLinearVelocity == 0 || maxLinearVelocity == float.MaxValue)
-                LimitLinearVelocity = false;
-
-            if (maxAngularVelocity == 0 || maxAngularVelocity == float.MaxValue)
-                LimitAngularVelocity = false;
-
-            MaxLinearVelocity = maxLinearVelocity;
-            MaxAngularVelocity = maxAngularVelocity;
-        }
-
-        /// <summary>
-        /// Gets or sets the max angular velocity.
-        /// </summary>
-        /// <value>The max angular velocity.</value>
-        public float MaxAngularVelocity
-        {
-            get { return _maxAngularVelocity; }
-            set
-            {
-                _maxAngularVelocity = value;
-                _maxAngularSqared = _maxAngularVelocity * _maxAngularVelocity;
-            }
-        }
-
-        /// <summary>
-        /// Gets or sets the max linear velocity.
-        /// </summary>
-        /// <value>The max linear velocity.</value>
-        public float MaxLinearVelocity
-        {
-            get { return _maxLinearVelocity; }
-            set
-            {
-                _maxLinearVelocity = value;
-                _maxLinearSqared = _maxLinearVelocity * _maxLinearVelocity;
-            }
-        }
-
-        public override void Update(float dt)
-        {
-            foreach (Body body in _bodies)
-            {
-                if (!IsActiveOn(body))
-                    continue;
-
-                if (LimitLinearVelocity)
-                {
-                    //Translation
-                    // Check for large velocities.
-                    float translationX = dt * body.LinearVelocityInternal.X;
-                    float translationY = dt * body.LinearVelocityInternal.Y;
-                    float result = translationX * translationX + translationY * translationY;
-
-                    if (result > dt * _maxLinearSqared)
-                    {
-                        float sq = (float)Math.Sqrt(result);
-
-                        float ratio = _maxLinearVelocity / sq;
-                        body.LinearVelocityInternal.X *= ratio;
-                        body.LinearVelocityInternal.Y *= ratio;
-                    }
-                }
-
-                if (LimitAngularVelocity)
-                {
-                    //Rotation
-                    float rotation = dt * body.AngularVelocityInternal;
-                    if (rotation * rotation > _maxAngularSqared)
-                    {
-                        float ratio = _maxAngularVelocity / Math.Abs(rotation);
-                        body.AngularVelocityInternal *= ratio;
-                    }
-                }
-            }
-        }
-
-        public void AddBody(Body body)
-        {
-            _bodies.Add(body);
-        }
-
-        public void RemoveBody(Body body)
-        {
-            _bodies.Remove(body);
-        }
-    }
-}

FarseerPhysicsEngine/DebugView.cs

@@ -1,185 +0,0 @@
-/*
-* Farseer Physics Engine based on Box2D.XNA port:
-* Copyright (c) 2010 Ian Qvist
-* 
-* Box2D.XNA port of Box2D:
-* Copyright (c) 2009 Brandon Furtwangler, Nathan Furtwangler
-*
-* Original source Box2D:
-* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com 
-* 
-* This software is provided 'as-is', without any express or implied 
-* warranty.  In no event will the authors be held liable for any damages 
-* arising from the use of this software. 
-* Permission is granted to anyone to use this software for any purpose, 
-* including commercial applications, and to alter it and redistribute it 
-* freely, subject to the following restrictions: 
-* 1. The origin of this software must not be misrepresented; you must not 
-* claim that you wrote the original software. If you use this software 
-* in a product, an acknowledgment in the product documentation would be 
-* appreciated but is not required. 
-* 2. Altered source versions must be plainly marked as such, and must not be 
-* misrepresented as being the original software. 
-* 3. This notice may not be removed or altered from any source distribution. 
-*/
-
-using System;
-using FarseerPhysics.Common;
-using FarseerPhysics.Dynamics;
-using Microsoft.Xna.Framework;
-
-namespace FarseerPhysics
-{
-    [Flags]
-    public enum DebugViewFlags
-    {
-        /// <summary>
-        /// Draw shapes.
-        /// </summary>
-        Shape = (1 << 0),
-
-        /// <summary>
-        /// Draw joint connections.
-        /// </summary>
-        Joint = (1 << 1),
-
-        /// <summary>
-        /// Draw axis aligned bounding boxes.
-        /// </summary>
-        AABB = (1 << 2),
-
-        /// <summary>
-        /// Draw broad-phase pairs.
-        /// </summary>
-        Pair = (1 << 3),
-
-        /// <summary>
-        /// Draw center of mass frame.
-        /// </summary>
-        CenterOfMass = (1 << 4),
-
-        /// <summary>
-        /// Draw useful debug data such as timings and number of bodies, joints, contacts and more.
-        /// </summary>
-        DebugPanel = (1 << 5),
-
-        /// <summary>
-        /// Draw contact points between colliding bodies.
-        /// </summary>
-        ContactPoints = (1 << 6),
-
-        /// <summary>
-        /// Draw contact normals. Need ContactPoints to be enabled first.
-        /// </summary>
-        ContactNormals = (1 << 7),
-
-        /// <summary>
-        /// Draws the vertices of polygons.
-        /// </summary>
-        PolygonPoints = (1 << 8),
-
-        /// <summary>
-        /// Draws the performance graph.
-        /// </summary>
-        PerformanceGraph = (1 << 9),
-
-        /// <summary>
-        /// Draws controllers.
-        /// </summary>
-        Controllers = (1 << 10)
-    }
-
-    /// Implement and register this class with a World to provide debug drawing of physics
-    /// entities in your game.
-    public abstract class DebugView
-    {
-        protected DebugView(World world)
-        {
-            World = world;
-        }
-
-        protected World World { get; private set; }
-
-        /// <summary>
-        /// Gets or sets the debug view flags.
-        /// </summary>
-        /// <value>The flags.</value>
-        public DebugViewFlags Flags { get; set; }
-
-        /// <summary>
-        /// Append flags to the current flags.
-        /// </summary>
-        /// <param name="flags">The flags.</param>
-        public void AppendFlags(DebugViewFlags flags)
-        {
-            Flags |= flags;
-        }
-
-        /// <summary>
-        /// Remove flags from the current flags.
-        /// </summary>
-        /// <param name="flags">The flags.</param>
-        public void RemoveFlags(DebugViewFlags flags)
-        {
-            Flags &= ~flags;
-        }
-
-        /// <summary>
-        /// Draw a closed polygon provided in CCW order.
-        /// </summary>
-        /// <param name="vertices">The vertices.</param>
-        /// <param name="count">The vertex count.</param>
-        /// <param name="red">The red value.</param>
-        /// <param name="blue">The blue value.</param>
-        /// <param name="green">The green value.</param>
-        public abstract void DrawPolygon(Vector2[] vertices, int count, float red, float blue, float green);
-
-        /// <summary>
-        /// Draw a solid closed polygon provided in CCW order.
-        /// </summary>
-        /// <param name="vertices">The vertices.</param>
-        /// <param name="count">The vertex count.</param>
-        /// <param name="red">The red value.</param>
-        /// <param name="blue">The blue value.</param>
-        /// <param name="green">The green value.</param>
-        public abstract void DrawSolidPolygon(Vector2[] vertices, int count, float red, float blue, float green);
-
-        /// <summary>
-        /// Draw a circle.
-        /// </summary>
-        /// <param name="center">The center.</param>
-        /// <param name="radius">The radius.</param>
-        /// <param name="red">The red value.</param>
-        /// <param name="blue">The blue value.</param>
-        /// <param name="green">The green value.</param>
-        public abstract void DrawCircle(Vector2 center, float radius, float red, float blue, float green);
-
-        /// <summary>
-        /// Draw a solid circle.
-        /// </summary>
-        /// <param name="center">The center.</param>
-        /// <param name="radius">The radius.</param>
-        /// <param name="axis">The axis.</param>
-        /// <param name="red">The red value.</param>
-        /// <param name="blue">The blue value.</param>
-        /// <param name="green">The green value.</param>
-        public abstract void DrawSolidCircle(Vector2 center, float radius, Vector2 axis, float red, float blue,
-                                             float green);
-
-        /// <summary>
-        /// Draw a line segment.
-        /// </summary>
-        /// <param name="start">The start.</param>
-        /// <param name="end">The end.</param>
-        /// <param name="red">The red value.</param>
-        /// <param name="blue">The blue value.</param>
-        /// <param name="green">The green value.</param>
-        public abstract void DrawSegment(Vector2 start, Vector2 end, float red, float blue, float green);
-
-        /// <summary>
-        /// Draw a transform. Choose your own length scale.
-        /// </summary>
-        /// <param name="transform">The transform.</param>
-        public abstract void DrawTransform(ref Transform transform);
-    }
-}

FarseerPhysicsEngine/Dynamics/Body.cs

@@ -1,1375 +0,0 @@
-/*
-* Farseer Physics Engine based on Box2D.XNA port:
-* Copyright (c) 2010 Ian Qvist
-* 
-* Box2D.XNA port of Box2D:
-* Copyright (c) 2009 Brandon Furtwangler, Nathan Furtwangler
-*
-* Original source Box2D:
-* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com 
-* 
-* This software is provided 'as-is', without any express or implied 
-* warranty.  In no event will the authors be held liable for any damages 
-* arising from the use of this software. 
-* Permission is granted to anyone to use this software for any purpose, 
-* including commercial applications, and to alter it and redistribute it 
-* freely, subject to the following restrictions: 
-* 1. The origin of this software must not be misrepresented; you must not 
-* claim that you wrote the original software. If you use this software 
-* in a product, an acknowledgment in the product documentation would be 
-* appreciated but is not required. 
-* 2. Altered source versions must be plainly marked as such, and must not be 
-* misrepresented as being the original software. 
-* 3. This notice may not be removed or altered from any source distribution. 
-*/
-
-using System;
-using System.Collections.Generic;
-using System.Diagnostics;
-using FarseerPhysics.Collision;
-using FarseerPhysics.Collision.Shapes;
-using FarseerPhysics.Common;
-using FarseerPhysics.Common.PhysicsLogic;
-using FarseerPhysics.Controllers;
-using FarseerPhysics.Dynamics.Contacts;
-using FarseerPhysics.Dynamics.Joints;
-using Microsoft.Xna.Framework;
-
-namespace FarseerPhysics.Dynamics
-{
-    /// <summary>
-    /// The body type.
-    /// </summary>
-    public enum BodyType
-    {
-        /// <summary>
-        /// Zero velocity, may be manually moved. Note: even static bodies have mass.
-        /// </summary>
-        Static,
-        /// <summary>
-        /// Zero mass, non-zero velocity set by user, moved by solver
-        /// </summary>
-        Kinematic,
-        /// <summary>
-        /// Positive mass, non-zero velocity determined by forces, moved by solver
-        /// </summary>
-        Dynamic,
-    }
-
-    [Flags]
-    public enum BodyFlags
-    {
-        None = 0,
-        Island = (1 << 0),
-        Awake = (1 << 1),
-        AutoSleep = (1 << 2),
-        Bullet = (1 << 3),
-        FixedRotation = (1 << 4),
-        Enabled = (1 << 5),
-        IgnoreGravity = (1 << 6),
-        IgnoreCCD = (1 << 7),
-    }
-
-    public class Body : IDisposable
-    {
-        private static int _bodyIdCounter;
-        internal float AngularVelocityInternal;
-        public int BodyId;
-        public ControllerFilter ControllerFilter;
-        internal BodyFlags Flags;
-        internal Vector2 Force;
-        internal float InvI;
-        internal float InvMass;
-        internal Vector2 LinearVelocityInternal;
-        public PhysicsLogicFilter PhysicsLogicFilter;
-        internal float SleepTime;
-        internal Sweep Sweep; // the swept motion for CCD
-        internal float Torque;
-        internal World World;
-        internal Transform Xf; // the body origin transform
-        private float _angularDamping;
-        private BodyType _bodyType;
-        private float _inertia;
-        private float _linearDamping;
-        private float _mass;
-
-        internal Body()
-        {
-            FixtureList = new List<Fixture>(32);
-        }
-
-        public Body(World world)
-            : this(world, null)
-        {
-        }
-
-        public Body(World world, object userData)
-        {
-            FixtureList = new List<Fixture>(32);
-            BodyId = _bodyIdCounter++;
-
-            World = world;
-            UserData = userData;
-
-            FixedRotation = false;
-            IsBullet = false;
-            SleepingAllowed = true;
-            Awake = true;
-            BodyType = BodyType.Static;
-            Enabled = true;
-
-            Xf.R.Set(0);
-
-            world.AddBody(this);
-        }
-
-        /// <summary>
-        /// Gets the total number revolutions the body has made.
-        /// </summary>
-        /// <value>The revolutions.</value>
-        public float Revolutions
-        {
-            get { return Rotation / (float)Math.PI; }
-        }
-
-        /// <summary>
-        /// Gets or sets the body type.
-        /// </summary>
-        /// <value>The type of body.</value>
-        public BodyType BodyType
-        {
-            get { return _bodyType; }
-            set
-            {
-                if (_bodyType == value)
-                {
-                    return;
-                }
-
-                _bodyType = value;
-
-                ResetMassData();
-
-                if (_bodyType == BodyType.Static)
-                {
-                    LinearVelocityInternal = Vector2.Zero;
-                    AngularVelocityInternal = 0.0f;
-                }
-
-                Awake = true;
-
-                Force = Vector2.Zero;
-                Torque = 0.0f;
-
-                // Since the body type changed, we need to flag contacts for filtering.
-                for (int i = 0; i < FixtureList.Count; i++)
-                {
-                    Fixture f = FixtureList[i];
-                    f.Refilter();
-                }
-            }
-        }
-
-        /// <summary>
-        /// Get or sets the linear velocity of the center of mass.
-        /// </summary>
-        /// <value>The linear velocity.</value>
-        public Vector2 LinearVelocity
-        {
-            set
-            {
-                Debug.Assert(!float.IsNaN(value.X) && !float.IsNaN(value.Y));
-
-                if (_bodyType == BodyType.Static)
-                    return;
-
-                if (Vector2.Dot(value, value) > 0.0f)
-                    Awake = true;
-
-                LinearVelocityInternal = value;
-            }
-            get { return LinearVelocityInternal; }
-        }
-
-        /// <summary>
-        /// Gets or sets the angular velocity. Radians/second.
-        /// </summary>
-        /// <value>The angular velocity.</value>
-        public float AngularVelocity
-        {
-            set
-            {
-                Debug.Assert(!float.IsNaN(value));
-
-                if (_bodyType == BodyType.Static)
-                    return;
-
-                if (value * value > 0.0f)
-                    Awake = true;
-
-                AngularVelocityInternal = value;
-            }
-            get { return AngularVelocityInternal; }
-        }
-
-        /// <summary>
-        /// Gets or sets the linear damping.
-        /// </summary>
-        /// <value>The linear damping.</value>
-        public float LinearDamping
-        {
-            get { return _linearDamping; }
-            set
-            {
-                Debug.Assert(!float.IsNaN(value));
-
-                _linearDamping = value;
-            }
-        }
-
-        /// <summary>
-        /// Gets or sets the angular damping.
-        /// </summary>
-        /// <value>The angular damping.</value>
-        public float AngularDamping
-        {
-            get { return _angularDamping; }
-            set
-            {
-                Debug.Assert(!float.IsNaN(value));
-
-                _angularDamping = value;
-            }
-        }
-
-        /// <summary>
-        /// Gets or sets a value indicating whether this body should be included in the CCD solver.
-        /// </summary>
-        /// <value><c>true</c> if this instance is included in CCD; otherwise, <c>false</c>.</value>
-        public bool IsBullet
-        {
-            set
-            {
-                if (value)
-                {
-                    Flags |= BodyFlags.Bullet;
-                }
-                else
-                {
-                    Flags &= ~BodyFlags.Bullet;
-                }
-            }
-            get { return (Flags & BodyFlags.Bullet) == BodyFlags.Bullet; }
-        }
-
-        /// <summary>
-        /// You can disable sleeping on this body. If you disable sleeping, the
-        /// body will be woken.
-        /// </summary>
-        /// <value><c>true</c> if sleeping is allowed; otherwise, <c>false</c>.</value>
-        public bool SleepingAllowed
-        {
-            set
-            {
-                if (value)
-                {
-                    Flags |= BodyFlags.AutoSleep;
-                }
-                else
-                {
-                    Flags &= ~BodyFlags.AutoSleep;
-                    Awake = true;
-                }
-            }
-            get { return (Flags & BodyFlags.AutoSleep) == BodyFlags.AutoSleep; }
-        }
-
-        /// <summary>
-        /// Set the sleep state of the body. A sleeping body has very
-        /// low CPU cost.
-        /// </summary>
-        /// <value><c>true</c> if awake; otherwise, <c>false</c>.</value>
-        public bool Awake
-        {
-            set
-            {
-                if (value)
-                {
-                    if ((Flags & BodyFlags.Awake) == 0)
-                    {
-                        Flags |= BodyFlags.Awake;
-                        SleepTime = 0.0f;
-                    }
-                }
-                else
-                {
-                    Flags &= ~BodyFlags.Awake;
-                    SleepTime = 0.0f;
-                    LinearVelocityInternal = Vector2.Zero;
-                    AngularVelocityInternal = 0.0f;
-                    Force = Vector2.Zero;
-                    Torque = 0.0f;
-                }
-            }
-            get { return (Flags & BodyFlags.Awake) == BodyFlags.Awake; }
-        }
-
-        /// <summary>
-        /// Set the active state of the body. An inactive body is not
-        /// simulated and cannot be collided with or woken up.
-        /// If you pass a flag of true, all fixtures will be added to the
-        /// broad-phase.
-        /// If you pass a flag of false, all fixtures will be removed from
-        /// the broad-phase and all contacts will be destroyed.
-        /// Fixtures and joints are otherwise unaffected. You may continue
-        /// to create/destroy fixtures and joints on inactive bodies.
-        /// Fixtures on an inactive body are implicitly inactive and will
-        /// not participate in collisions, ray-casts, or queries.
-        /// Joints connected to an inactive body are implicitly inactive.
-        /// An inactive body is still owned by a b2World object and remains
-        /// in the body list.
-        /// </summary>
-        /// <value><c>true</c> if active; otherwise, <c>false</c>.</value>
-        public bool Enabled
-        {
-            set
-            {
-                if (value == Enabled)
-                {
-                    return;
-                }
-
-                if (value)
-                {
-                    Flags |= BodyFlags.Enabled;
-
-                    // Create all proxies.
-                    IBroadPhase broadPhase = World.ContactManager.BroadPhase;
-                    for (int i = 0; i < FixtureList.Count; i++)
-                    {
-                        FixtureList[i].CreateProxies(broadPhase, ref Xf);
-                    }
-
-                    // Contacts are created the next time step.
-                }
-                else
-                {
-                    Flags &= ~BodyFlags.Enabled;
-
-                    // Destroy all proxies.
-                    IBroadPhase broadPhase = World.ContactManager.BroadPhase;
-
-                    for (int i = 0; i < FixtureList.Count; i++)
-                    {
-                        FixtureList[i].DestroyProxies(broadPhase);
-                    }
-
-                    // Destroy the attached contacts.
-                    ContactEdge ce = ContactList;
-                    while (ce != null)
-                    {
-                        ContactEdge ce0 = ce;
-                        ce = ce.Next;
-                        World.ContactManager.Destroy(ce0.Contact);
-                    }
-                    ContactList = null;
-                }
-            }
-            get { return (Flags & BodyFlags.Enabled) == BodyFlags.Enabled; }
-        }
-
-        /// <summary>
-        /// Set this body to have fixed rotation. This causes the mass
-        /// to be reset.
-        /// </summary>
-        /// <value><c>true</c> if it has fixed rotation; otherwise, <c>false</c>.</value>
-        public bool FixedRotation
-        {
-            set
-            {
-                if (value)
-                {
-                    Flags |= BodyFlags.FixedRotation;
-                }
-                else
-                {
-                    Flags &= ~BodyFlags.FixedRotation;
-                }
-
-                ResetMassData();
-            }
-            get { return (Flags & BodyFlags.FixedRotation) == BodyFlags.FixedRotation; }
-        }
-
-        /// <summary>
-        /// Gets all the fixtures attached to this body.
-        /// </summary>
-        /// <value>The fixture list.</value>
-        public List<Fixture> FixtureList { get; internal set; }
-
-        /// <summary>
-        /// Get the list of all joints attached to this body.
-        /// </summary>
-        /// <value>The joint list.</value>
-        public JointEdge JointList { get; internal set; }
-
-        /// <summary>
-        /// Get the list of all contacts attached to this body.
-        /// Warning: this list changes during the time step and you may
-        /// miss some collisions if you don't use ContactListener.
-        /// </summary>
-        /// <value>The contact list.</value>
-        public ContactEdge ContactList { get; internal set; }
-
-        /// <summary>
-        /// Set the user data. Use this to store your application specific data.
-        /// </summary>
-        /// <value>The user data.</value>
-        public object UserData { get; set; }
-
-        /// <summary>
-        /// Get the world body origin position.
-        /// </summary>
-        /// <returns>Return the world position of the body's origin.</returns>
-        public Vector2 Position
-        {
-            get { return Xf.Position; }
-            set
-            {
-                Debug.Assert(!float.IsNaN(value.X) && !float.IsNaN(value.Y));
-
-                SetTransform(ref value, Rotation);
-            }
-        }
-
-        /// <summary>
-        /// Get the angle in radians.
-        /// </summary>
-        /// <returns>Return the current world rotation angle in radians.</returns>
-        public float Rotation
-        {
-            get { return Sweep.A; }
-            set
-            {
-                Debug.Assert(!float.IsNaN(value));
-
-                SetTransform(ref Xf.Position, value);
-            }
-        }
-
-        /// <summary>
-        /// Gets or sets a value indicating whether this body is static.
-        /// </summary>
-        /// <value><c>true</c> if this instance is static; otherwise, <c>false</c>.</value>
-        public bool IsStatic
-        {
-            get { return _bodyType == BodyType.Static; }
-            set
-            {
-                if (value)
-                    BodyType = BodyType.Static;
-                else
-                    BodyType = BodyType.Dynamic;
-            }
-        }
-
-        /// <summary>
-        /// Gets or sets a value indicating whether this body ignores gravity.
-        /// </summary>
-        /// <value><c>true</c> if  it ignores gravity; otherwise, <c>false</c>.</value>
-        public bool IgnoreGravity
-        {
-            get { return (Flags & BodyFlags.IgnoreGravity) == BodyFlags.IgnoreGravity; }
-            set
-            {
-                if (value)
-                    Flags |= BodyFlags.IgnoreGravity;
-                else
-                    Flags &= ~BodyFlags.IgnoreGravity;
-            }
-        }
-
-        /// <summary>
-        /// Get the world position of the center of mass.
-        /// </summary>
-        /// <value>The world position.</value>
-        public Vector2 WorldCenter
-        {
-            get { return Sweep.C; }
-        }
-
-        /// <summary>
-        /// Get the local position of the center of mass.
-        /// </summary>
-        /// <value>The local position.</value>
-        public Vector2 LocalCenter
-        {
-            get { return Sweep.LocalCenter; }
-            set
-            {
-                if (_bodyType != BodyType.Dynamic)
-                    return;
-
-                // Move center of mass.
-                Vector2 oldCenter = Sweep.C;
-                Sweep.LocalCenter = value;
-                Sweep.C0 = Sweep.C = MathUtils.Multiply(ref Xf, ref Sweep.LocalCenter);
-
-                // Update center of mass velocity.
-                Vector2 a = Sweep.C - oldCenter;
-                LinearVelocityInternal += new Vector2(-AngularVelocityInternal * a.Y, AngularVelocityInternal * a.X);
-            }
-        }
-
-        /// <summary>
-        /// Gets or sets the mass. Usually in kilograms (kg).
-        /// </summary>
-        /// <value>The mass.</value>
-        public float Mass
-        {
-            get { return _mass; }
-            set
-            {
-                Debug.Assert(!float.IsNaN(value));
-
-                if (_bodyType != BodyType.Dynamic)
-                    return;
-
-                _mass = value;
-
-                if (_mass <= 0.0f)
-                    _mass = 1.0f;
-
-                InvMass = 1.0f / _mass;
-            }
-        }
-
-        /// <summary>
-        /// Get or set the rotational inertia of the body about the local origin. usually in kg-m^2.
-        /// </summary>
-        /// <value>The inertia.</value>
-        public float Inertia
-        {
-            get { return _inertia + Mass * Vector2.Dot(Sweep.LocalCenter, Sweep.LocalCenter); }
-            set
-            {
-                Debug.Assert(!float.IsNaN(value));
-
-                if (_bodyType != BodyType.Dynamic)
-                    return;
-
-                if (value > 0.0f && (Flags & BodyFlags.FixedRotation) == 0)
-                {
-                    _inertia = value - Mass * Vector2.Dot(LocalCenter, LocalCenter);
-                    Debug.Assert(_inertia > 0.0f);
-                    InvI = 1.0f / _inertia;
-                }
-            }
-        }
-
-        public float Restitution
-        {
-            get
-            {
-                float res = 0;
-
-                for (int i = 0; i < FixtureList.Count; i++)
-                {
-                    Fixture f = FixtureList[i];
-                    res += f.Restitution;
-                }
-
-                return res / FixtureList.Count;
-            }
-            set
-            {
-                for (int i = 0; i < FixtureList.Count; i++)
-                {
-                    Fixture f = FixtureList[i];
-                    f.Restitution = value;
-                }
-            }
-        }
-
-        public float Friction
-        {
-            get
-            {
-                float res = 0;
-
-                for (int i = 0; i < FixtureList.Count; i++)
-                {
-                    Fixture f = FixtureList[i];
-                    res += f.Friction;
-                }
-
-                return res / FixtureList.Count;
-            }
-            set
-            {
-                for (int i = 0; i < FixtureList.Count; i++)
-                {
-                    Fixture f = FixtureList[i];
-                    f.Friction = value;
-                }
-            }
-        }
-
-        public Category CollisionCategories
-        {
-            set
-            {
-                for (int i = 0; i < FixtureList.Count; i++)
-                {
-                    Fixture f = FixtureList[i];
-                    f.CollisionCategories = value;
-                }
-            }
-        }
-
-        public Category CollidesWith
-        {
-            set
-            {
-                for (int i = 0; i < FixtureList.Count; i++)
-                {
-                    Fixture f = FixtureList[i];
-                    f.CollidesWith = value;
-                }
-            }
-        }
-
-        public short CollisionGroup
-        {
-            set
-            {
-                for (int i = 0; i < FixtureList.Count; i++)
-                {
-                    Fixture f = FixtureList[i];
-                    f.CollisionGroup = value;
-                }
-            }
-        }
-
-        public bool IsSensor
-        {
-            set
-            {
-                for (int i = 0; i < FixtureList.Count; i++)
-                {
-                    Fixture f = FixtureList[i];
-                    f.IsSensor = value;
-                }
-            }
-        }
-
-        public bool IgnoreCCD
-        {
-            get { return (Flags & BodyFlags.IgnoreCCD) == BodyFlags.IgnoreCCD; }
-            set
-            {
-                if (value)
-                    Flags |= BodyFlags.IgnoreCCD;
-                else
-                    Flags &= ~BodyFlags.IgnoreCCD;
-            }
-        }
-
-        #region IDisposable Members
-
-        public bool IsDisposed { get; set; }
-
-        public void Dispose()
-        {
-            if (!IsDisposed)
-            {
-
-                World.RemoveBody(this);
-                IsDisposed = true;
-                GC.SuppressFinalize(this);
-            }
-        }
-
-        #endregion
-
-        /// <summary>
-        /// Resets the dynamics of this body.
-        /// Sets torque, force and linear/angular velocity to 0
-        /// </summary>
-        public void ResetDynamics()
-        {
-            Torque = 0;
-            AngularVelocityInternal = 0;
-            Force = Vector2.Zero;
-            LinearVelocityInternal = Vector2.Zero;
-        }
-
-        /// <summary>
-        /// Creates a fixture and attach it to this body.
-        /// If the density is non-zero, this function automatically updates the mass of the body.
-        /// Contacts are not created until the next time step.
-        /// Warning: This function is locked during callbacks.
-        /// </summary>
-        /// <param name="shape">The shape.</param>
-        /// <returns></returns>
-        public Fixture CreateFixture(Shape shape)
-        {
-            return new Fixture(this, shape);
-        }
-
-        /// <summary>
-        /// Creates a fixture and attach it to this body.
-        /// If the density is non-zero, this function automatically updates the mass of the body.
-        /// Contacts are not created until the next time step.
-        /// Warning: This function is locked during callbacks.
-        /// </summary>
-        /// <param name="shape">The shape.</param>
-        /// <param name="userData">Application specific data</param>
-        /// <returns></returns>
-        public Fixture CreateFixture(Shape shape, object userData)
-        {
-            return new Fixture(this, shape, userData);
-        }
-
-        /// <summary>
-        /// Destroy a fixture. This removes the fixture from the broad-phase and
-        /// destroys all contacts associated with this fixture. This will
-        /// automatically adjust the mass of the body if the body is dynamic and the
-        /// fixture has positive density.
-        /// All fixtures attached to a body are implicitly destroyed when the body is destroyed.
-        /// Warning: This function is locked during callbacks.
-        /// </summary>
-        /// <param name="fixture">The fixture to be removed.</param>
-        public void DestroyFixture(Fixture fixture)
-        {
-            Debug.Assert(fixture.Body == this);
-
-            // Remove the fixture from this body's singly linked list.
-            Debug.Assert(FixtureList.Count > 0);
-
-            // You tried to remove a fixture that not present in the fixturelist.
-            Debug.Assert(FixtureList.Contains(fixture));
-
-            // Destroy any contacts associated with the fixture.
-            ContactEdge edge = ContactList;
-            while (edge != null)
-            {
-                Contact c = edge.Contact;
-                edge = edge.Next;
-
-                Fixture fixtureA = c.FixtureA;
-                Fixture fixtureB = c.FixtureB;
-
-                if (fixture == fixtureA || fixture == fixtureB)
-                {
-                    // This destroys the contact and removes it from
-                    // this body's contact list.
-                    World.ContactManager.Destroy(c);
-                }
-            }
-
-            if ((Flags & BodyFlags.Enabled) == BodyFlags.Enabled)
-            {
-                IBroadPhase broadPhase = World.ContactManager.BroadPhase;
-                fixture.DestroyProxies(broadPhase);
-            }
-
-            FixtureList.Remove(fixture);
-            fixture.Destroy();
-            fixture.Body = null;
-
-            ResetMassData();
-        }
-
-        /// <summary>
-        /// Set the position of the body's origin and rotation.
-        /// This breaks any contacts and wakes the other bodies.
-        /// Manipulating a body's transform may cause non-physical behavior.
-        /// </summary>
-        /// <param name="position">The world position of the body's local origin.</param>
-        /// <param name="rotation">The world rotation in radians.</param>
-        public void SetTransform(ref Vector2 position, float rotation)
-        {
-            SetTransformIgnoreContacts(ref position, rotation);
-
-            World.ContactManager.FindNewContacts();
-        }
-
-        /// <summary>
-        /// Set the position of the body's origin and rotation.
-        /// This breaks any contacts and wakes the other bodies.
-        /// Manipulating a body's transform may cause non-physical behavior.
-        /// </summary>
-        /// <param name="position">The world position of the body's local origin.</param>
-        /// <param name="rotation">The world rotation in radians.</param>
-        public void SetTransform(Vector2 position, float rotation)
-        {
-            SetTransform(ref position, rotation);
-        }
-
-        /// <summary>
-        /// For teleporting a body without considering new contacts immediately.
-        /// </summary>
-        /// <param name="position">The position.</param>
-        /// <param name="angle">The angle.</param>
-        public void SetTransformIgnoreContacts(ref Vector2 position, float angle)
-        {
-            Xf.R.Set(angle);
-            Xf.Position = position;
-
-            Sweep.C0 =
-                Sweep.C =
-                new Vector2(Xf.Position.X + Xf.R.Col1.X * Sweep.LocalCenter.X + Xf.R.Col2.X * Sweep.LocalCenter.Y,
-                            Xf.Position.Y + Xf.R.Col1.Y * Sweep.LocalCenter.X + Xf.R.Col2.Y * Sweep.LocalCenter.Y);
-            Sweep.A0 = Sweep.A = angle;
-
-            IBroadPhase broadPhase = World.ContactManager.BroadPhase;
-            for (int i = 0; i < FixtureList.Count; i++)
-            {
-                FixtureList[i].Synchronize(broadPhase, ref Xf, ref Xf);
-            }
-        }
-
-        /// <summary>
-        /// Get the body transform for the body's origin.
-        /// </summary>
-        /// <param name="transform">The transform of the body's origin.</param>
-        public void GetTransform(out Transform transform)
-        {
-            transform = Xf;
-        }
-
-        /// <summary>
-        /// Apply a force at a world point. If the force is not
-        /// applied at the center of mass, it will generate a torque and
-        /// affect the angular velocity. This wakes up the body.
-        /// </summary>
-        /// <param name="force">The world force vector, usually in Newtons (N).</param>
-        /// <param name="point">The world position of the point of application.</param>
-        public void ApplyForce(Vector2 force, Vector2 point)
-        {
-            ApplyForce(ref force, ref point);
-        }
-
-        /// <summary>
-        /// Applies a force at the center of mass.
-        /// </summary>
-        /// <param name="force">The force.</param>
-        public void ApplyForce(ref Vector2 force)
-        {
-            ApplyForce(ref force, ref Xf.Position);
-        }
-
-        /// <summary>
-        /// Applies a force at the center of mass.
-        /// </summary>
-        /// <param name="force">The force.</param>
-        public void ApplyForce(Vector2 force)
-        {
-            ApplyForce(ref force, ref Xf.Position);
-        }
-
-        /// <summary>
-        /// Apply a force at a world point. If the force is not
-        /// applied at the center of mass, it will generate a torque and
-        /// affect the angular velocity. This wakes up the body.
-        /// </summary>
-        /// <param name="force">The world force vector, usually in Newtons (N).</param>
-        /// <param name="point">The world position of the point of application.</param>
-        public void ApplyForce(ref Vector2 force, ref Vector2 point)
-        {
-            Debug.Assert(!float.IsNaN(force.X));
-            Debug.Assert(!float.IsNaN(force.Y));
-            Debug.Assert(!float.IsNaN(point.X));
-            Debug.Assert(!float.IsNaN(point.Y));
-
-            if (_bodyType == BodyType.Dynamic)
-            {
-                if (Awake == false)
-                {
-                    Awake = true;
-                }
-
-                Force += force;
-                Torque += (point.X - Sweep.C.X) * force.Y - (point.Y - Sweep.C.Y) * force.X;
-            }
-        }
-
-        /// <summary>
-        /// Apply a torque. This affects the angular velocity
-        /// without affecting the linear velocity of the center of mass.
-        /// This wakes up the body.
-        /// </summary>
-        /// <param name="torque">The torque about the z-axis (out of the screen), usually in N-m.</param>
-        public void ApplyTorque(float torque)
-        {
-            Debug.Assert(!float.IsNaN(torque));
-
-            if (_bodyType == BodyType.Dynamic)
-            {
-                if (Awake == false)
-                {
-                    Awake = true;
-                }
-
-                Torque += torque;
-            }
-        }
-
-        /// <summary>
-        /// Apply an impulse at a point. This immediately modifies the velocity.
-        /// This wakes up the body.
-        /// </summary>
-        /// <param name="impulse">The world impulse vector, usually in N-seconds or kg-m/s.</param>
-        public void ApplyLinearImpulse(Vector2 impulse)
-        {
-            ApplyLinearImpulse(ref impulse);
-        }
-
-        /// <summary>
-        /// Apply an impulse at a point. This immediately modifies the velocity.
-        /// It also modifies the angular velocity if the point of application
-        /// is not at the center of mass.
-        /// This wakes up the body.
-        /// </summary>
-        /// <param name="impulse">The world impulse vector, usually in N-seconds or kg-m/s.</param>
-        /// <param name="point">The world position of the point of application.</param>
-        public void ApplyLinearImpulse(Vector2 impulse, Vector2 point)
-        {
-            ApplyLinearImpulse(ref impulse, ref point);
-        }
-
-        /// <summary>
-        /// Apply an impulse at a point. This immediately modifies the velocity.
-        /// This wakes up the body.
-        /// </summary>
-        /// <param name="impulse">The world impulse vector, usually in N-seconds or kg-m/s.</param>
-        public void ApplyLinearImpulse(ref Vector2 impulse)
-        {
-            if (_bodyType != BodyType.Dynamic)
-            {
-                return;
-            }
-            if (Awake == false)
-            {
-                Awake = true;
-            }
-            LinearVelocityInternal += InvMass * impulse;
-        }
-
-        /// <summary>
-        /// Apply an impulse at a point. This immediately modifies the velocity.
-        /// It also modifies the angular velocity if the point of application
-        /// is not at the center of mass.
-        /// This wakes up the body.
-        /// </summary>
-        /// <param name="impulse">The world impulse vector, usually in N-seconds or kg-m/s.</param>
-        /// <param name="point">The world position of the point of application.</param>
-        public void ApplyLinearImpulse(ref Vector2 impulse, ref Vector2 point)
-        {
-            if (_bodyType != BodyType.Dynamic)
-                return;
-
-            if (Awake == false)
-                Awake = true;
-
-            LinearVelocityInternal += InvMass * impulse;
-            AngularVelocityInternal += InvI * ((point.X - Sweep.C.X) * impulse.Y - (point.Y - Sweep.C.Y) * impulse.X);
-        }
-
-        /// <summary>
-        /// Apply an angular impulse.
-        /// </summary>
-        /// <param name="impulse">The angular impulse in units of kg*m*m/s.</param>
-        public void ApplyAngularImpulse(float impulse)
-        {
-            if (_bodyType != BodyType.Dynamic)
-            {
-                return;
-            }
-
-            if (Awake == false)
-            {
-                Awake = true;
-            }
-
-            AngularVelocityInternal += InvI * impulse;
-        }
-
-        /// <summary>
-        /// This resets the mass properties to the sum of the mass properties of the fixtures.
-        /// This normally does not need to be called unless you called SetMassData to override
-        /// the mass and you later want to reset the mass.
-        /// </summary>
-        public void ResetMassData()
-        {
-            // Compute mass data from shapes. Each shape has its own density.
-            _mass = 0.0f;
-            InvMass = 0.0f;
-            _inertia = 0.0f;
-            InvI = 0.0f;
-            Sweep.LocalCenter = Vector2.Zero;
-
-            // Kinematic bodies have zero mass.
-            if (BodyType == BodyType.Kinematic)
-            {
-                Sweep.C0 = Sweep.C = Xf.Position;
-                return;
-            }
-
-            Debug.Assert(BodyType == BodyType.Dynamic || BodyType == BodyType.Static);
-
-            // Accumulate mass over all fixtures.
-            Vector2 center = Vector2.Zero;
-            foreach (Fixture f in FixtureList)
-            {
-                if (f.Shape._density == 0)
-                {
-                    continue;
-                }
-
-                MassData massData = f.Shape.MassData;
-                _mass += massData.Mass;
-                center += massData.Mass * massData.Centroid;
-                _inertia += massData.Inertia;
-            }
-
-            //Static bodies only have mass, they don't have other properties. A little hacky tho...
-            if (BodyType == BodyType.Static)
-            {
-                Sweep.C0 = Sweep.C = Xf.Position;
-                return;
-            }
-
-            // Compute center of mass.
-            if (_mass > 0.0f)
-            {
-                InvMass = 1.0f / _mass;
-                center *= InvMass;
-            }
-            else
-            {
-                // Force all dynamic bodies to have a positive mass.
-                _mass = 1.0f;
-                InvMass = 1.0f;
-            }
-
-            if (_inertia > 0.0f && (Flags & BodyFlags.FixedRotation) == 0)
-            {
-                // Center the inertia about the center of mass.
-                _inertia -= _mass * Vector2.Dot(center, center);
-
-                Debug.Assert(_inertia > 0.0f);
-                InvI = 1.0f / _inertia;
-            }
-            else
-            {
-                _inertia = 0.0f;
-                InvI = 0.0f;
-            }
-
-            // Move center of mass.
-            Vector2 oldCenter = Sweep.C;
-            Sweep.LocalCenter = center;
-            Sweep.C0 = Sweep.C = MathUtils.Multiply(ref Xf, ref Sweep.LocalCenter);
-
-            // Update center of mass velocity.
-            Vector2 a = Sweep.C - oldCenter;
-            LinearVelocityInternal += new Vector2(-AngularVelocityInternal * a.Y, AngularVelocityInternal * a.X);
-        }
-
-        /// <summary>
-        /// Get the world coordinates of a point given the local coordinates.
-        /// </summary>
-        /// <param name="localPoint">A point on the body measured relative the the body's origin.</param>
-        /// <returns>The same point expressed in world coordinates.</returns>
-        public Vector2 GetWorldPoint(ref Vector2 localPoint)
-        {
-            return new Vector2(Xf.Position.X + Xf.R.Col1.X * localPoint.X + Xf.R.Col2.X * localPoint.Y,
-                               Xf.Position.Y + Xf.R.Col1.Y * localPoint.X + Xf.R.Col2.Y * localPoint.Y);
-        }
-
-        /// <summary>
-        /// Get the world coordinates of a point given the local coordinates.
-        /// </summary>
-        /// <param name="localPoint">A point on the body measured relative the the body's origin.</param>
-        /// <returns>The same point expressed in world coordinates.</returns>
-        public Vector2 GetWorldPoint(Vector2 localPoint)
-        {
-            return GetWorldPoint(ref localPoint);
-        }
-
-        /// <summary>
-        /// Get the world coordinates of a vector given the local coordinates.
-        /// Note that the vector only takes the rotation into account, not the position.
-        /// </summary>
-        /// <param name="localVector">A vector fixed in the body.</param>
-        /// <returns>The same vector expressed in world coordinates.</returns>
-        public Vector2 GetWorldVector(ref Vector2 localVector)
-        {
-            return new Vector2(Xf.R.Col1.X * localVector.X + Xf.R.Col2.X * localVector.Y,
-                               Xf.R.Col1.Y * localVector.X + Xf.R.Col2.Y * localVector.Y);
-        }
-
-        /// <summary>
-        /// Get the world coordinates of a vector given the local coordinates.
-        /// </summary>
-        /// <param name="localVector">A vector fixed in the body.</param>
-        /// <returns>The same vector expressed in world coordinates.</returns>
-        public Vector2 GetWorldVector(Vector2 localVector)
-        {
-            return GetWorldVector(ref localVector);
-        }
-
-        /// <summary>
-        /// Gets a local point relative to the body's origin given a world point.
-        /// Note that the vector only takes the rotation into account, not the position.
-        /// </summary>
-        /// <param name="worldPoint">A point in world coordinates.</param>
-        /// <returns>The corresponding local point relative to the body's origin.</returns>
-        public Vector2 GetLocalPoint(ref Vector2 worldPoint)
-        {
-            return
-                new Vector2((worldPoint.X - Xf.Position.X) * Xf.R.Col1.X + (worldPoint.Y - Xf.Position.Y) * Xf.R.Col1.Y,
-                            (worldPoint.X - Xf.Position.X) * Xf.R.Col2.X + (worldPoint.Y - Xf.Position.Y) * Xf.R.Col2.Y);
-        }
-
-        /// <summary>
-        /// Gets a local point relative to the body's origin given a world point.
-        /// </summary>
-        /// <param name="worldPoint">A point in world coordinates.</param>
-        /// <returns>The corresponding local point relative to the body's origin.</returns>
-        public Vector2 GetLocalPoint(Vector2 worldPoint)
-        {
-            return GetLocalPoint(ref worldPoint);
-        }
-
-        /// <summary>
-        /// Gets a local vector given a world vector.
-        /// Note that the vector only takes the rotation into account, not the position.
-        /// </summary>
-        /// <param name="worldVector">A vector in world coordinates.</param>
-        /// <returns>The corresponding local vector.</returns>
-        public Vector2 GetLocalVector(ref Vector2 worldVector)
-        {
-            return new Vector2(worldVector.X * Xf.R.Col1.X + worldVector.Y * Xf.R.Col1.Y,
-                               worldVector.X * Xf.R.Col2.X + worldVector.Y * Xf.R.Col2.Y);
-        }
-
-        /// <summary>
-        /// Gets a local vector given a world vector.
-        /// Note that the vector only takes the rotation into account, not the position.
-        /// </summary>
-        /// <param name="worldVector">A vector in world coordinates.</param>
-        /// <returns>The corresponding local vector.</returns>
-        public Vector2 GetLocalVector(Vector2 worldVector)
-        {
-            return GetLocalVector(ref worldVector);
-        }
-
-        /// <summary>
-        /// Get the world linear velocity of a world point attached to this body.
-        /// </summary>
-        /// <param name="worldPoint">A point in world coordinates.</param>
-        /// <returns>The world velocity of a point.</returns>
-        public Vector2 GetLinearVelocityFromWorldPoint(Vector2 worldPoint)
-        {
-            return GetLinearVelocityFromWorldPoint(ref worldPoint);
-        }
-
-        /// <summary>
-        /// Get the world linear velocity of a world point attached to this body.
-        /// </summary>
-        /// <param name="worldPoint">A point in world coordinates.</param>
-        /// <returns>The world velocity of a point.</returns>
-        public Vector2 GetLinearVelocityFromWorldPoint(ref Vector2 worldPoint)
-        {
-            return LinearVelocityInternal +
-                   new Vector2(-AngularVelocityInternal * (worldPoint.Y - Sweep.C.Y),
-                               AngularVelocityInternal * (worldPoint.X - Sweep.C.X));
-        }
-
-        /// <summary>
-        /// Get the world velocity of a local point.
-        /// </summary>
-        /// <param name="localPoint">A point in local coordinates.</param>
-        /// <returns>The world velocity of a point.</returns>
-        public Vector2 GetLinearVelocityFromLocalPoint(Vector2 localPoint)
-        {
-            return GetLinearVelocityFromLocalPoint(ref localPoint);
-        }
-
-        /// <summary>
-        /// Get the world velocity of a local point.
-        /// </summary>
-        /// <param name="localPoint">A point in local coordinates.</param>
-        /// <returns>The world velocity of a point.</returns>
-        public Vector2 GetLinearVelocityFromLocalPoint(ref Vector2 localPoint)
-        {
-            return GetLinearVelocityFromWorldPoint(GetWorldPoint(ref localPoint));
-        }
-
-        public Body DeepClone()
-        {
-            Body body = Clone();
-
-            for (int i = 0; i < FixtureList.Count; i++)
-            {
-                FixtureList[i].Clone(body);
-            }
-
-            return body;
-        }
-
-        public Body Clone()
-        {
-            Body body = new Body();
-            body.World = World;
-            body.UserData = UserData;
-            body.LinearDamping = LinearDamping;
-            body.LinearVelocityInternal = LinearVelocityInternal;
-            body.AngularDamping = AngularDamping;
-            body.AngularVelocityInternal = AngularVelocityInternal;
-            body.Position = Position;
-            body.Rotation = Rotation;
-            body._bodyType = _bodyType;
-            body.Flags = Flags;
-
-            World.AddBody(body);
-
-            return body;
-        }
-
-        internal void SynchronizeFixtures()
-        {
-            Transform xf1 = new Transform();
-            float c = (float)Math.Cos(Sweep.A0), s = (float)Math.Sin(Sweep.A0);
-            xf1.R.Col1.X = c;
-            xf1.R.Col2.X = -s;
-            xf1.R.Col1.Y = s;
-            xf1.R.Col2.Y = c;
-
-            xf1.Position.X = Sweep.C0.X - (xf1.R.Col1.X * Sweep.LocalCenter.X + xf1.R.Col2.X * Sweep.LocalCenter.Y);
-            xf1.Position.Y = Sweep.C0.Y - (xf1.R.Col1.Y * Sweep.LocalCenter.X + xf1.R.Col2.Y * Sweep.LocalCenter.Y);
-
-            IBroadPhase broadPhase = World.ContactManager.BroadPhase;
-            for (int i = 0; i < FixtureList.Count; i++)
-            {
-                FixtureList[i].Synchronize(broadPhase, ref xf1, ref Xf);
-            }
-        }
-
-        internal void SynchronizeTransform()
-        {
-            Xf.R.Set(Sweep.A);
-
-            float vx = Xf.R.Col1.X * Sweep.LocalCenter.X + Xf.R.Col2.X * Sweep.LocalCenter.Y;
-            float vy = Xf.R.Col1.Y * Sweep.LocalCenter.X + Xf.R.Col2.Y * Sweep.LocalCenter.Y;
-
-            Xf.Position.X = Sweep.C.X - vx;
-            Xf.Position.Y = Sweep.C.Y - vy;
-        }
-
-        /// <summary>
-        /// This is used to prevent connected bodies from colliding.
-        /// It may lie, depending on the collideConnected flag.
-        /// </summary>
-        /// <param name="other">The other body.</param>
-        /// <returns></returns>
-        internal bool ShouldCollide(Body other)
-        {
-            // At least one body should be dynamic.
-            if (_bodyType != BodyType.Dynamic && other._bodyType != BodyType.Dynamic)
-            {
-                return false;
-            }
-
-            // Does a joint prevent collision?
-            for (JointEdge jn = JointList; jn != null; jn = jn.Next)
-            {
-                if (jn.Other == other)
-                {
-                    if (jn.Joint.CollideConnected == false)
-                    {
-                        return false;
-                    }
-                }
-            }
-
-            return true;
-        }
-
-        internal void Advance(float alpha)
-        {
-            // Advance to the new safe time.
-            Sweep.Advance(alpha);
-            Sweep.C = Sweep.C0;
-            Sweep.A = Sweep.A0;
-            SynchronizeTransform();
-        }
-
-        public event OnCollisionEventHandler OnCollision
-        {
-            add
-            {
-                for (int i = 0; i < FixtureList.Count; i++)
-                {
-                    FixtureList[i].OnCollision += value;
-                }
-            }
-            remove
-            {
-                for (int i = 0; i < FixtureList.Count; i++)
-                {
-                    FixtureList[i].OnCollision -= value;
-                }
-            }
-        }
-
-        public event OnSeparationEventHandler OnSeparation
-        {
-            add
-            {
-                for (int i = 0; i < FixtureList.Count; i++)
-                {
-                    FixtureList[i].OnSeparation += value;
-                }
-            }
-            remove
-            {
-                for (int i = 0; i < FixtureList.Count; i++)
-                {
-                    FixtureList[i].OnSeparation -= value;
-                }
-            }
-        }
-
-        public void IgnoreCollisionWith(Body other)
-        {
-            for (int i = 0; i < FixtureList.Count; i++)
-            {
-                Fixture f = FixtureList[i];
-                for (int j = 0; j < other.FixtureList.Count; j++)
-                {
-                    Fixture f2 = other.FixtureList[j];
-
-                    f.IgnoreCollisionWith(f2);
-                }
-            }
-        }
-
-        public void RestoreCollisionWith(Body other)
-        {
-            for (int i = 0; i < FixtureList.Count; i++)
-            {
-                Fixture f = FixtureList[i];
-                for (int j = 0; j < other.FixtureList.Count; j++)
-                {
-                    Fixture f2 = other.FixtureList[j];
-
-                    f.RestoreCollisionWith(f2);
-                }
-            }
-        }
-    }
-}

FarseerPhysicsEngine/Dynamics/BreakableBody.cs

@@ -1,137 +0,0 @@
-using System;
-using System.Collections.Generic;
-using FarseerPhysics.Collision.Shapes;
-using FarseerPhysics.Common;
-using FarseerPhysics.Dynamics.Contacts;
-using FarseerPhysics.Factories;
-using Microsoft.Xna.Framework;
-
-namespace FarseerPhysics.Dynamics
-{
-    /// <summary>
-    /// A type of body that supports multiple fixtures that can break apart.
-    /// </summary>
-    public class BreakableBody
-    {
-        public bool Broken;
-        public Body MainBody;
-        public List<Fixture> Parts = new List<Fixture>(8);
-
-        /// <summary>
-        /// The force needed to break the body apart.
-        /// Default: 500
-        /// </summary>
-        public float Strength = 500.0f;
-
-        private float[] _angularVelocitiesCache = new float[8];
-        private bool _break;
-        private Vector2[] _velocitiesCache = new Vector2[8];
-        private World _world;
-
-        public BreakableBody(IEnumerable<Vertices> vertices, World world, float density)
-            : this(vertices, world, density, null)
-        {
-        }
-
-        public BreakableBody(IEnumerable<Vertices> vertices, World world, float density, object userData)
-        {
-            _world = world;
-            _world.ContactManager.PostSolve += PostSolve;
-            MainBody = new Body(_world);
-            MainBody.BodyType = BodyType.Dynamic;
-
-            foreach (Vertices part in vertices)
-            {
-                PolygonShape polygonShape = new PolygonShape(part, density);
-                Fixture fixture = MainBody.CreateFixture(polygonShape, userData);
-                Parts.Add(fixture);
-            }
-        }
-
-        private void PostSolve(Contact contact, ContactConstraint impulse)
-        {
-            if (!Broken)
-            {
-                if (Parts.Contains(contact.FixtureA) || Parts.Contains(contact.FixtureB))
-                {
-                    float maxImpulse = 0.0f;
-                    int count = contact.Manifold.PointCount;
-
-                    for (int i = 0; i < count; ++i)
-                    {
-                        maxImpulse = Math.Max(maxImpulse, impulse.Points[i].NormalImpulse);
-                    }
-
-                    if (maxImpulse > Strength)
-                    {
-                        // Flag the body for breaking.
-                        _break = true;
-                    }
-                }
-            }
-        }
-
-        public void Update()
-        {
-            if (_break)
-            {
-                Decompose();
-                Broken = true;
-                _break = false;
-            }
-
-            // Cache velocities to improve movement on breakage.
-            if (Broken == false)
-            {
-                //Enlarge the cache if needed
-                if (Parts.Count > _angularVelocitiesCache.Length)
-                {
-                    _velocitiesCache = new Vector2[Parts.Count];
-                    _angularVelocitiesCache = new float[Parts.Count];
-                }
-
-                //Cache the linear and angular velocities.
-                for (int i = 0; i < Parts.Count; i++)
-                {
-                    _velocitiesCache[i] = Parts[i].Body.LinearVelocity;
-                    _angularVelocitiesCache[i] = Parts[i].Body.AngularVelocity;
-                }
-            }
-        }
-
-        private void Decompose()
-        {
-            //Unsubsribe from the PostSolve delegate
-            _world.ContactManager.PostSolve -= PostSolve;
-
-            for (int i = 0; i < Parts.Count; i++)
-            {
-                Fixture fixture = Parts[i];
-
-                Shape shape = fixture.Shape.Clone();
-
-                object userdata = fixture.UserData;
-                MainBody.DestroyFixture(fixture);
-
-                Body body = BodyFactory.CreateBody(_world);
-                body.BodyType = BodyType.Dynamic;
-                body.Position = MainBody.Position;
-                body.Rotation = MainBody.Rotation;
-                body.UserData = MainBody.UserData;
-
-                body.CreateFixture(shape, userdata);
-
-                body.AngularVelocity = _angularVelocitiesCache[i];
-                body.LinearVelocity = _velocitiesCache[i];
-            }
-
-            _world.RemoveBody(MainBody);
-            _world.RemoveBreakableBody(this);
-        }
-
-        public void Break()
-        {
-            _break = true;
-        }
-    }
-}

FarseerPhysicsEngine/Dynamics/ContactManager.cs

@@ -1,340 +0,0 @@
-/*
-* Farseer Physics Engine based on Box2D.XNA port:
-* Copyright (c) 2010 Ian Qvist
-* 
-* Box2D.XNA port of Box2D:
-* Copyright (c) 2009 Brandon Furtwangler, Nathan Furtwangler
-*
-* Original source Box2D:
-* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com 
-* 
-* This software is provided 'as-is', without any express or implied 
-* warranty.  In no event will the authors be held liable for any damages 
-* arising from the use of this software. 
-* Permission is granted to anyone to use this software for any purpose, 
-* including commercial applications, and to alter it and redistribute it 
-* freely, subject to the following restrictions: 
-* 1. The origin of this software must not be misrepresented; you must not 
-* claim that you wrote the original software. If you use this software 
-* in a product, an acknowledgment in the product documentation would be 
-* appreciated but is not required. 
-* 2. Altered source versions must be plainly marked as such, and must not be 
-* misrepresented as being the original software. 
-* 3. This notice may not be removed or altered from any source distribution. 
-*/
-
-using System.Collections.Generic;
-using FarseerPhysics.Collision;
-using FarseerPhysics.Dynamics.Contacts;
-
-namespace FarseerPhysics.Dynamics
-{
-    public class ContactManager
-    {
-        /// <summary>
-        /// Fires when a contact is created
-        /// </summary>
-        public BeginContactDelegate BeginContact;
-
-        public IBroadPhase BroadPhase;
-
-        /// <summary>
-        /// The filter used by the contact manager.
-        /// </summary>
-        public CollisionFilterDelegate ContactFilter;
-
-        public List<Contact> ContactList = new List<Contact>(128);
-
-        /// <summary>
-        /// Fires when a contact is deleted
-        /// </summary>
-        public EndContactDelegate EndContact;
-
-        /// <summary>
-        /// Fires when the broadphase detects that two Fixtures are close to each other.
-        /// </summary>
-        public BroadphaseDelegate OnBroadphaseCollision;
-
-        /// <summary>
-        /// Fires after the solver has run
-        /// </summary>
-        public PostSolveDelegate PostSolve;
-
-        /// <summary>
-        /// Fires before the solver runs
-        /// </summary>
-        public PreSolveDelegate PreSolve;
-
-        internal ContactManager(IBroadPhase broadPhase)
-        {
-            BroadPhase = broadPhase;
-            OnBroadphaseCollision = AddPair;
-        }
-
-        // Broad-phase callback.
-        private void AddPair(ref FixtureProxy proxyA, ref FixtureProxy proxyB)
-        {
-            Fixture fixtureA = proxyA.Fixture;
-            Fixture fixtureB = proxyB.Fixture;
-
-            int indexA = proxyA.ChildIndex;
-            int indexB = proxyB.ChildIndex;
-
-            Body bodyA = fixtureA.Body;
-            Body bodyB = fixtureB.Body;
-
-            // Are the fixtures on the same body?
-            if (bodyA == bodyB)
-            {
-                return;
-            }
-
-            // Does a contact already exist?
-            ContactEdge edge = bodyB.ContactList;
-            while (edge != null)
-            {
-                if (edge.Other == bodyA)
-                {
-                    Fixture fA = edge.Contact.FixtureA;
-                    Fixture fB = edge.Contact.FixtureB;
-                    int iA = edge.Contact.ChildIndexA;
-                    int iB = edge.Contact.ChildIndexB;
-
-                    if (fA == fixtureA && fB == fixtureB && iA == indexA && iB == indexB)
-                    {
-                        // A contact already exists.
-                        return;
-                    }
-
-                    if (fA == fixtureB && fB == fixtureA && iA == indexB && iB == indexA)
-                    {
-                        // A contact already exists.
-                        return;
-                    }
-                }
-
-                edge = edge.Next;
-            }
-
-            // Does a joint override collision? Is at least one body dynamic?
-            if (bodyB.ShouldCollide(bodyA) == false)
-                return;
-
-            //Check default filter
-            if (ShouldCollide(fixtureA, fixtureB) == false)
-                return;
-
-            // Check user filtering.
-            if (ContactFilter != null && ContactFilter(fixtureA, fixtureB) == false)
-                return;
-
-            if (fixtureA.BeforeCollision != null && fixtureA.BeforeCollision(fixtureA, fixtureB) == false)
-                return;
-
-            if (fixtureB.BeforeCollision != null && fixtureB.BeforeCollision(fixtureB, fixtureA) == false)
-                return;
-
-            // Call the factory.
-            Contact c = Contact.Create(fixtureA, indexA, fixtureB, indexB);
-
-            // Contact creation may swap fixtures.
-            fixtureA = c.FixtureA;
-            fixtureB = c.FixtureB;
-            bodyA = fixtureA.Body;
-            bodyB = fixtureB.Body;
-
-            // Insert into the world.
-            ContactList.Add(c);
-
-            // Connect to island graph.
-
-            // Connect to body A
-            c.NodeA.Contact = c;
-            c.NodeA.Other = bodyB;
-
-            c.NodeA.Prev = null;
-            c.NodeA.Next = bodyA.ContactList;
-            if (bodyA.ContactList != null)
-            {
-                bodyA.ContactList.Prev = c.NodeA;
-            }
-            bodyA.ContactList = c.NodeA;
-
-            // Connect to body B
-            c.NodeB.Contact = c;
-            c.NodeB.Other = bodyA;
-
-            c.NodeB.Prev = null;
-            c.NodeB.Next = bodyB.ContactList;
-            if (bodyB.ContactList != null)
-            {
-                bodyB.ContactList.Prev = c.NodeB;
-            }
-            bodyB.ContactList = c.NodeB;
-        }
-
-        internal void FindNewContacts()
-        {
-            BroadPhase.UpdatePairs(OnBroadphaseCollision);
-        }
-
-        internal void Destroy(Contact contact)
-        {
-            Fixture fixtureA = contact.FixtureA;
-            Fixture fixtureB = contact.FixtureB;
-            Body bodyA = fixtureA.Body;
-            Body bodyB = fixtureB.Body;
-
-            if (EndContact != null && contact.IsTouching())
-            {
-                EndContact(contact);
-            }
-
-            // Remove from the world.
-            ContactList.Remove(contact);
-
-            // Remove from body 1
-            if (contact.NodeA.Prev != null)
-            {
-                contact.NodeA.Prev.Next = contact.NodeA.Next;
-            }
-
-            if (contact.NodeA.Next != null)
-            {
-                contact.NodeA.Next.Prev = contact.NodeA.Prev;
-            }
-
-            if (contact.NodeA == bodyA.ContactList)
-            {
-                bodyA.ContactList = contact.NodeA.Next;
-            }
-
-            // Remove from body 2
-            if (contact.NodeB.Prev != null)
-            {
-                contact.NodeB.Prev.Next = contact.NodeB.Next;
-            }
-
-            if (contact.NodeB.Next != null)
-            {
-                contact.NodeB.Next.Prev = contact.NodeB.Prev;
-            }
-
-            if (contact.NodeB == bodyB.ContactList)
-            {
-                bodyB.ContactList = contact.NodeB.Next;
-            }
-
-            contact.Destroy();
-        }
-
-        internal void Collide()
-        {
-            // Update awake contacts.
-            for (int i = 0; i < ContactList.Count; i++)
-            {
-                Contact c = ContactList[i];
-                Fixture fixtureA = c.FixtureA;
-                Fixture fixtureB = c.FixtureB;
-                int indexA = c.ChildIndexA;
-                int indexB = c.ChildIndexB;
-                Body bodyA = fixtureA.Body;
-                Body bodyB = fixtureB.Body;
-
-                if (bodyA.Awake == false && bodyB.Awake == false)
-                {
-                    continue;
-                }
-
-                // Is this contact flagged for filtering?
-                if ((c.Flags & ContactFlags.Filter) == ContactFlags.Filter)
-                {
-                    // Should these bodies collide?
-                    if (bodyB.ShouldCollide(bodyA) == false)
-                    {
-                        Contact cNuke = c;
-                        Destroy(cNuke);
-                        continue;
-                    }
-
-                    // Check default filtering
-                    if (ShouldCollide(fixtureA, fixtureB) == false)
-                    {
-                        Contact cNuke = c;
-                        Destroy(cNuke);
-                        continue;
-                    }
-
-                    // Check user filtering.
-                    if (ContactFilter != null && ContactFilter(fixtureA, fixtureB) == false)
-                    {
-                        Contact cNuke = c;
-                        Destroy(cNuke);
-                        continue;
-                    }
-
-                    // Clear the filtering flag.
-                    c.Flags &= ~ContactFlags.Filter;
-                }
-
-                int proxyIdA = fixtureA.Proxies[indexA].ProxyId;
-                int proxyIdB = fixtureB.Proxies[indexB].ProxyId;
-
-                bool overlap = BroadPhase.TestOverlap(proxyIdA, proxyIdB);
-
-                // Here we destroy contacts that cease to overlap in the broad-phase.
-                if (overlap == false)
-                {
-                    Contact cNuke = c;
-                    Destroy(cNuke);
-                    continue;
-                }
-
-                // The contact persists.
-                c.Update(this);
-            }
-        }
-
-        private static bool ShouldCollide(Fixture fixtureA, Fixture fixtureB)
-        {
-            if (Settings.UseFPECollisionCategories)
-            {
-                if ((fixtureA.CollisionGroup == fixtureB.CollisionGroup) &&
-                    fixtureA.CollisionGroup != 0 && fixtureB.CollisionGroup != 0)
-                    return false;
-
-                if (((fixtureA.CollisionCategories & fixtureB.CollidesWith) ==
-                     Category.None) &
-                    ((fixtureB.CollisionCategories & fixtureA.CollidesWith) ==
-                     Category.None))
-                    return false;
-
-                if (fixtureA.IsFixtureIgnored(fixtureB) ||
-                    fixtureB.IsFixtureIgnored(fixtureA))
-                    return false;
-
-                return true;
-            }
-
-            if (fixtureA.CollisionGroup == fixtureB.CollisionGroup &&
-                fixtureA.CollisionGroup != 0)
-            {
-                return fixtureA.CollisionGroup > 0;
-            }
-
-            bool collide = (fixtureA.CollidesWith & fixtureB.CollisionCategories) != 0 &&
-                           (fixtureA.CollisionCategories & fixtureB.CollidesWith) != 0;
-
-            if (collide)
-            {
-                if (fixtureA.IsFixtureIgnored(fixtureB) ||
-                    fixtureB.IsFixtureIgnored(fixtureA))
-                {
-                    return false;
-                }
-            }
-
-            return collide;
-        }
-    }
-}

FarseerPhysicsEngine/Dynamics/Contacts/Contact.cs

@@ -1,502 +0,0 @@
-/*
-* Farseer Physics Engine based on Box2D.XNA port:
-* Copyright (c) 2010 Ian Qvist
-* 
-* Box2D.XNA port of Box2D:
-* Copyright (c) 2009 Brandon Furtwangler, Nathan Furtwangler
-*
-* Original source Box2D:
-* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com 
-* 
-* This software is provided 'as-is', without any express or implied 
-* warranty.  In no event will the authors be held liable for any damages 
-* arising from the use of this software. 
-* Permission is granted to anyone to use this software for any purpose, 
-* including commercial applications, and to alter it and redistribute it 
-* freely, subject to the following restrictions: 
-* 1. The origin of this software must not be misrepresented; you must not 
-* claim that you wrote the original software. If you use this software 
-* in a product, an acknowledgment in the product documentation would be 
-* appreciated but is not required. 
-* 2. Altered source versions must be plainly marked as such, and must not be 
-* misrepresented as being the original software. 
-* 3. This notice may not be removed or altered from any source distribution. 
-*/
-
-using System;
-using System.Collections.Generic;
-using System.Diagnostics;
-using FarseerPhysics.Collision;
-using FarseerPhysics.Collision.Shapes;
-using FarseerPhysics.Common;
-using Microsoft.Xna.Framework;
-
-namespace FarseerPhysics.Dynamics.Contacts
-{
-    /// <summary>
-    /// A contact edge is used to connect bodies and contacts together
-    /// in a contact graph where each body is a node and each contact
-    /// is an edge. A contact edge belongs to a doubly linked list
-    /// maintained in each attached body. Each contact has two contact
-    /// nodes, one for each attached body.
-    /// </summary>
-    public sealed class ContactEdge
-    {
-        /// <summary>
-        /// The contact
-        /// </summary>
-        public Contact Contact;
-
-        /// <summary>
-        /// The next contact edge in the body's contact list
-        /// </summary>
-        public ContactEdge Next;
-
-        /// <summary>
-        /// Provides quick access to the other body attached.
-        /// </summary>
-        public Body Other;
-
-        /// <summary>
-        /// The previous contact edge in the body's contact list
-        /// </summary>
-        public ContactEdge Prev;
-    }
-
-    [Flags]
-    public enum ContactFlags
-    {
-        None = 0,
-
-        /// <summary>
-        /// Used when crawling contact graph when forming islands.
-        /// </summary>
-        Island = 0x0001,
-
-        /// <summary>
-        /// Set when the shapes are touching.
-        /// </summary>
-        Touching = 0x0002,
-
-        /// <summary>
-        /// This contact can be disabled (by user)
-        /// </summary>
-        Enabled = 0x0004,
-
-        /// <summary>
-        /// This contact needs filtering because a fixture filter was changed.
-        /// </summary>
-        Filter = 0x0008,
-
-        /// <summary>
-        /// This bullet contact had a TOI event
-        /// </summary>
-        BulletHit = 0x0010,
-
-        /// <summary>
-        /// This contact has a valid TOI i the field TOI
-        /// </summary>
-        TOI = 0x0020
-    }
-
-    /// <summary>
-    /// The class manages contact between two shapes. A contact exists for each overlapping
-    /// AABB in the broad-phase (except if filtered). Therefore a contact object may exist
-    /// that has no contact points.
-    /// </summary>
-    public class Contact
-    {
-        private static EdgeShape _edge = new EdgeShape();
-
-        private static ContactType[,] _registers = new[,]
-                                                       {
-                                                           {
-                                                               ContactType.Circle,
-                                                               ContactType.EdgeAndCircle,
-                                                               ContactType.PolygonAndCircle,
-                                                               ContactType.LoopAndCircle,
-                                                           },
-                                                           {
-                                                               ContactType.EdgeAndCircle,
-                                                               ContactType.NotSupported,
-                                                               // 1,1 is invalid (no ContactType.Edge)
-                                                               ContactType.EdgeAndPolygon,
-                                                               ContactType.NotSupported,
-                                                               // 1,3 is invalid (no ContactType.EdgeAndLoop)
-                                                           },
-                                                           {
-                                                               ContactType.PolygonAndCircle,
-                                                               ContactType.EdgeAndPolygon,
-                                                               ContactType.Polygon,
-                                                               ContactType.LoopAndPolygon,
-                                                           },
-                                                           {
-                                                               ContactType.LoopAndCircle,
-                                                               ContactType.NotSupported,
-                                                               // 3,1 is invalid (no ContactType.EdgeAndLoop)
-                                                               ContactType.LoopAndPolygon,
-                                                               ContactType.NotSupported,
-                                                               // 3,3 is invalid (no ContactType.Loop)
-                                                           },
-                                                       };
-
-        public Fixture FixtureA;
-        public Fixture FixtureB;
-        internal ContactFlags Flags;
-
-        public Manifold Manifold;
-
-        // Nodes for connecting bodies.
-        internal ContactEdge NodeA = new ContactEdge();
-        internal ContactEdge NodeB = new ContactEdge();
-        public float TOI;
-        internal int TOICount;
-        private ContactType _type;
-
-        private Contact(Fixture fA, int indexA, Fixture fB, int indexB)
-        {
-            Reset(fA, indexA, fB, indexB);
-        }
-
-        /// Enable/disable this contact. This can be used inside the pre-solve
-        /// contact listener. The contact is only disabled for the current
-        /// time step (or sub-step in continuous collisions).
-        public bool Enabled
-        {
-            set
-            {
-                if (value)
-                {
-                    Flags |= ContactFlags.Enabled;
-                }
-                else
-                {
-                    Flags &= ~ContactFlags.Enabled;
-                }
-            }
-
-            get { return (Flags & ContactFlags.Enabled) == ContactFlags.Enabled; }
-        }
-
-        /// <summary>
-        /// Get the child primitive index for fixture A.
-        /// </summary>
-        /// <value>The child index A.</value>
-        public int ChildIndexA { get; internal set; }
-
-        /// <summary>
-        /// Get the child primitive index for fixture B.
-        /// </summary>
-        /// <value>The child index B.</value>
-        public int ChildIndexB { get; internal set; }
-
-        /// <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;
-        }
-
-        /// <summary>
-        /// Gets the world manifold.
-        /// </summary>
-        public void GetWorldManifold(out Vector2 normal, out FixedArray2<Vector2> points)
-        {
-            Body bodyA = FixtureA.Body;
-            Body bodyB = FixtureB.Body;
-            Shape shapeA = FixtureA.Shape;
-            Shape shapeB = FixtureB.Shape;
-
-            Collision.Collision.GetWorldManifold(ref Manifold, ref bodyA.Xf, shapeA.Radius, ref bodyB.Xf, shapeB.Radius,
-                                                 out normal, out points);
-        }
-
-        /// <summary>
-        /// Determines whether this contact is touching.
-        /// </summary>
-        /// <returns>
-        /// 	<c>true</c> if this instance is touching; otherwise, <c>false</c>.
-        /// </returns>
-        public bool IsTouching()
-        {
-            return (Flags & ContactFlags.Touching) == ContactFlags.Touching;
-        }
-
-        /// <summary>
-        /// Flag this contact for filtering. Filtering will occur the next time step.
-        /// </summary>
-        public void FlagForFiltering()
-        {
-            Flags |= ContactFlags.Filter;
-        }
-
-        private void Reset(Fixture fA, int indexA, Fixture fB, int indexB)
-        {
-            Flags = ContactFlags.Enabled;
-
-            FixtureA = fA;
-            FixtureB = fB;
-
-            ChildIndexA = indexA;
-            ChildIndexB = indexB;
-
-            Manifold.PointCount = 0;
-
-            NodeA.Contact = null;
-            NodeA.Prev = null;
-            NodeA.Next = null;
-            NodeA.Other = null;
-
-            NodeB.Contact = null;
-            NodeB.Prev = null;
-            NodeB.Next = null;
-            NodeB.Other = null;
-
-            TOICount = 0;
-        }
-
-        /// <summary>
-        /// Update the contact manifold and touching status.
-        /// Note: do not assume the fixture AABBs are overlapping or are valid.
-        /// </summary>
-        /// <param name="contactManager">The contact manager.</param>
-        internal void Update(ContactManager contactManager)
-        {
-            Manifold oldManifold = Manifold;
-
-            // Re-enable this contact.
-            Flags |= ContactFlags.Enabled;
-
-            bool touching;
-            bool wasTouching = (Flags & ContactFlags.Touching) == ContactFlags.Touching;
-
-            bool sensor = FixtureA.IsSensor || FixtureB.IsSensor;
-
-            Body bodyA = FixtureA.Body;
-            Body bodyB = FixtureB.Body;
-
-            // Is this contact a sensor?
-            if (sensor)
-            {
-                Shape shapeA = FixtureA.Shape;
-                Shape shapeB = FixtureB.Shape;
-                touching = AABB.TestOverlap(shapeA, ChildIndexA, shapeB, ChildIndexB, ref bodyA.Xf, ref bodyB.Xf);
-
-                // Sensors don't generate manifolds.
-                Manifold.PointCount = 0;
-            }
-            else
-            {
-                Evaluate(ref Manifold, ref bodyA.Xf, ref bodyB.Xf);
-                touching = Manifold.PointCount > 0;
-
-                // Match old contact ids to new contact ids and copy the
-                // stored impulses to warm start the solver.
-                for (int i = 0; i < Manifold.PointCount; ++i)
-                {
-                    ManifoldPoint mp2 = Manifold.Points[i];
-                    mp2.NormalImpulse = 0.0f;
-                    mp2.TangentImpulse = 0.0f;
-                    ContactID id2 = mp2.Id;
-                    bool found = false;
-
-                    for (int j = 0; j < oldManifold.PointCount; ++j)
-                    {
-                        ManifoldPoint mp1 = oldManifold.Points[j];
-
-                        if (mp1.Id.Key == id2.Key)
-                        {
-                            mp2.NormalImpulse = mp1.NormalImpulse;
-                            mp2.TangentImpulse = mp1.TangentImpulse;
-                            found = true;
-                            break;
-                        }
-                    }
-                    if (found == false)
-                    {
-                        mp2.NormalImpulse = 0.0f;
-                        mp2.TangentImpulse = 0.0f;
-                    }
-
-                    Manifold.Points[i] = mp2;
-                }
-
-                if (touching != wasTouching)
-                {
-                    bodyA.Awake = true;
-                    bodyB.Awake = true;
-                }
-            }
-
-            if (touching)
-            {
-                Flags |= ContactFlags.Touching;
-            }
-            else
-            {
-                Flags &= ~ContactFlags.Touching;
-            }
-
-            if (wasTouching == false && touching)
-            {
-                //Report the collision to both participants:
-                if (FixtureA.OnCollision != null)
-                    Enabled = FixtureA.OnCollision(FixtureA, FixtureB, this);
-
-                //Reverse the order of the reported fixtures. The first fixture is always the one that the
-                //user subscribed to.
-                if (FixtureB.OnCollision != null)
-                    Enabled = FixtureB.OnCollision(FixtureB, FixtureA, this);
-
-                //BeginContact can also return false and disable the contact
-                if (contactManager.BeginContact != null)
-                    Enabled = contactManager.BeginContact(this);
-
-                //if the user disabled the contact (needed to exclude it in TOI solver), we also need to mark
-                //it as not touching.
-                if (Enabled == false)
-                    Flags &= ~ContactFlags.Touching;
-            }
-
-            if (wasTouching && touching == false)
-            {
-                //Report the separation to both participants:
-                if (FixtureA != null && FixtureA.OnSeparation != null)
-                    FixtureA.OnSeparation(FixtureA, FixtureB);
-
-                //Reverse the order of the reported fixtures. The first fixture is always the one that the
-                //user subscribed to.
-                if (FixtureB != null && FixtureB.OnSeparation != null)
-                    FixtureB.OnSeparation(FixtureB, FixtureA);
-
-                if (contactManager.EndContact != null)
-                    contactManager.EndContact(this);
-            }
-
-            if (sensor)
-                return;
-
-            if (contactManager.PreSolve != null)
-                contactManager.PreSolve(this, ref oldManifold);
-        }
-
-        /// <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;
-            }
-        }
-
-        internal static Contact Create(Fixture fixtureA, int indexA, Fixture fixtureB, int indexB)
-        {
-            ShapeType type1 = fixtureA.ShapeType;
-            ShapeType type2 = fixtureB.ShapeType;
-
-            Debug.Assert(ShapeType.Unknown < type1 && type1 < ShapeType.TypeCount);
-            Debug.Assert(ShapeType.Unknown < type2 && type2 < ShapeType.TypeCount);
-
-            Contact c;
-            Queue<Contact> pool = fixtureA.Body.World.ContactPool;
-            if (pool.Count > 0)
-            {
-                c = pool.Dequeue();
-                if ((type1 >= type2 || (type1 == ShapeType.Edge && type2 == ShapeType.Polygon))
-                    &&
-                    !(type2 == ShapeType.Edge && type1 == ShapeType.Polygon))
-                {
-                    c.Reset(fixtureA, indexA, fixtureB, indexB);
-                }
-                else
-                {
-                    c.Reset(fixtureB, indexB, fixtureA, indexA);
-                }
-            }
-            else
-            {
-                // Edge+Polygon is non-symetrical due to the way Erin handles collision type registration.
-                if ((type1 >= type2 || (type1 == ShapeType.Edge && type2 == ShapeType.Polygon))
-                    &&
-                    !(type2 == ShapeType.Edge && type1 == ShapeType.Polygon))
-                {
-                    c = new Contact(fixtureA, indexA, fixtureB, indexB);
-                }
-                else
-                {
-                    c = new Contact(fixtureB, indexB, fixtureA, indexA);
-                }
-            }
-
-            c._type = _registers[(int)type1, (int)type2];
-
-            return c;
-        }
-
-        internal void Destroy()
-        {
-            FixtureA.Body.World.ContactPool.Enqueue(this);
-            Reset(null, 0, null, 0);
-        }
-
-        #region Nested type: ContactType
-
-        private enum ContactType
-        {
-            NotSupported,
-            Polygon,
-            PolygonAndCircle,
-            Circle,
-            EdgeAndPolygon,
-            EdgeAndCircle,
-            LoopAndPolygon,
-            LoopAndCircle,
-        }
-
-        #endregion
-    }
-}

FarseerPhysicsEngine/Dynamics/Contacts/ContactSolver.cs

@@ -1,794 +0,0 @@
-/*
-* Farseer Physics Engine based on Box2D.XNA port:
-* Copyright (c) 2010 Ian Qvist
-* 
-* Box2D.XNA port of Box2D:
-* Copyright (c) 2009 Brandon Furtwangler, Nathan Furtwangler
-*
-* Original source Box2D:
-* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com 
-* 
-* This software is provided 'as-is', without any express or implied 
-* warranty.  In no event will the authors be held liable for any damages 
-* arising from the use of this software. 
-* Permission is granted to anyone to use this software for any purpose, 
-* including commercial applications, and to alter it and redistribute it 
-* freely, subject to the following restrictions: 
-* 1. The origin of this software must not be misrepresented; you must not 
-* claim that you wrote the original software. If you use this software 
-* in a product, an acknowledgment in the product documentation would be 
-* appreciated but is not required. 
-* 2. Altered source versions must be plainly marked as such, and must not be 
-* misrepresented as being the original software. 
-* 3. This notice may not be removed or altered from any source distribution. 
-*/
-
-using System;
-using System.Diagnostics;
-using FarseerPhysics.Collision;
-using FarseerPhysics.Collision.Shapes;
-using FarseerPhysics.Common;
-using Microsoft.Xna.Framework;
-
-namespace FarseerPhysics.Dynamics.Contacts
-{
-    public sealed class ContactConstraintPoint
-    {
-        public Vector2 LocalPoint;
-        public float NormalImpulse;
-        public float NormalMass;
-        public float TangentImpulse;
-        public float TangentMass;
-        public float VelocityBias;
-        public Vector2 rA;
-        public Vector2 rB;
-    }
-
-    public sealed class ContactConstraint
-    {
-        public Body BodyA;
-        public Body BodyB;
-        public float Friction;
-        public Mat22 K;
-        public Vector2 LocalNormal;
-        public Vector2 LocalPoint;
-        public Manifold Manifold;
-        public Vector2 Normal;
-        public Mat22 NormalMass;
-        public int PointCount;
-        public ContactConstraintPoint[] Points = new ContactConstraintPoint[Settings.MaxPolygonVertices];
-        public float RadiusA;
-        public float RadiusB;
-        public float Restitution;
-        public ManifoldType Type;
-
-        public ContactConstraint()
-        {
-            for (int i = 0; i < Settings.MaxManifoldPoints; i++)
-            {
-                Points[i] = new ContactConstraintPoint();
-            }
-        }
-    }
-
-    public class ContactSolver
-    {
-        public ContactConstraint[] Constraints;
-        private int _constraintCount; // collection can be bigger.
-        private Contact[] _contacts;
-
-        public void Reset(Contact[] contacts, int contactCount, float impulseRatio, bool warmstarting)
-        {
-            _contacts = contacts;
-
-            _constraintCount = contactCount;
-
-            // grow the array
-            if (Constraints == null || Constraints.Length < _constraintCount)
-            {
-                Constraints = new ContactConstraint[_constraintCount * 2];
-
-                for (int i = 0; i < Constraints.Length; i++)
-                {
-                    Constraints[i] = new ContactConstraint();
-                }
-            }
-
-            // Initialize position independent portions of the constraints.
-            for (int i = 0; i < _constraintCount; ++i)
-            {
-                Contact contact = contacts[i];
-
-                Fixture fixtureA = contact.FixtureA;
-                Fixture fixtureB = contact.FixtureB;
-                Shape shapeA = fixtureA.Shape;
-                Shape shapeB = fixtureB.Shape;
-                float radiusA = shapeA.Radius;
-                float radiusB = shapeB.Radius;
-                Body bodyA = fixtureA.Body;
-                Body bodyB = fixtureB.Body;
-                Manifold manifold = contact.Manifold;
-
-                Debug.Assert(manifold.PointCount > 0);
-
-                ContactConstraint cc = Constraints[i];
-                cc.Friction = Settings.MixFriction(fixtureA.Friction, fixtureB.Friction);
-                cc.Restitution = Settings.MixRestitution(fixtureA.Restitution, fixtureB.Restitution);
-                cc.BodyA = bodyA;
-                cc.BodyB = bodyB;
-                cc.Manifold = manifold;
-                cc.Normal = Vector2.Zero;
-                cc.PointCount = manifold.PointCount;
-
-                cc.LocalNormal = manifold.LocalNormal;
-                cc.LocalPoint = manifold.LocalPoint;
-                cc.RadiusA = radiusA;
-                cc.RadiusB = radiusB;
-                cc.Type = manifold.Type;
-
-                for (int j = 0; j < cc.PointCount; ++j)
-                {
-                    ManifoldPoint cp = manifold.Points[j];
-                    ContactConstraintPoint ccp = cc.Points[j];
-
-                    if (warmstarting)
-                    {
-                        ccp.NormalImpulse = impulseRatio * cp.NormalImpulse;
-                        ccp.TangentImpulse = impulseRatio * cp.TangentImpulse;
-                    }
-                    else
-                    {
-                        ccp.NormalImpulse = 0.0f;
-                        ccp.TangentImpulse = 0.0f;
-                    }
-
-                    ccp.LocalPoint = cp.LocalPoint;
-                    ccp.rA = Vector2.Zero;
-                    ccp.rB = Vector2.Zero;
-                    ccp.NormalMass = 0.0f;
-                    ccp.TangentMass = 0.0f;
-                    ccp.VelocityBias = 0.0f;
-                }
-
-                cc.K.SetZero();
-                cc.NormalMass.SetZero();
-            }
-        }
-
-        public void InitializeVelocityConstraints()
-        {
-            for (int i = 0; i < _constraintCount; ++i)
-            {
-                ContactConstraint cc = Constraints[i];
-
-                float radiusA = cc.RadiusA;
-                float radiusB = cc.RadiusB;
-                Body bodyA = cc.BodyA;
-                Body bodyB = cc.BodyB;
-                Manifold manifold = cc.Manifold;
-
-                Vector2 vA = bodyA.LinearVelocity;
-                Vector2 vB = bodyB.LinearVelocity;
-                float wA = bodyA.AngularVelocity;
-                float wB = bodyB.AngularVelocity;
-
-                Debug.Assert(manifold.PointCount > 0);
-                FixedArray2<Vector2> points;
-
-                Collision.Collision.GetWorldManifold(ref manifold, ref bodyA.Xf, radiusA, ref bodyB.Xf, radiusB,
-                                                     out cc.Normal, out points);
-                Vector2 tangent = new Vector2(cc.Normal.Y, -cc.Normal.X);
-
-                for (int j = 0; j < cc.PointCount; ++j)
-                {
-                    ContactConstraintPoint ccp = cc.Points[j];
-
-                    ccp.rA = points[j] - bodyA.Sweep.C;
-                    ccp.rB = points[j] - bodyB.Sweep.C;
-
-                    float rnA = ccp.rA.X * cc.Normal.Y - ccp.rA.Y * cc.Normal.X;
-                    float rnB = ccp.rB.X * cc.Normal.Y - ccp.rB.Y * cc.Normal.X;
-                    rnA *= rnA;
-                    rnB *= rnB;
-
-                    float kNormal = bodyA.InvMass + bodyB.InvMass + bodyA.InvI * rnA + bodyB.InvI * rnB;
-
-                    Debug.Assert(kNormal > Settings.Epsilon);
-                    ccp.NormalMass = 1.0f / kNormal;
-
-                    float rtA = ccp.rA.X * tangent.Y - ccp.rA.Y * tangent.X;
-                    float rtB = ccp.rB.X * tangent.Y - ccp.rB.Y * tangent.X;
-
-                    rtA *= rtA;
-                    rtB *= rtB;
-                    float kTangent = bodyA.InvMass + bodyB.InvMass + bodyA.InvI * rtA + bodyB.InvI * rtB;
-
-                    Debug.Assert(kTangent > Settings.Epsilon);
-                    ccp.TangentMass = 1.0f / kTangent;
-
-                    // Setup a velocity bias for restitution.
-                    ccp.VelocityBias = 0.0f;
-                    float vRel = cc.Normal.X * (vB.X + -wB * ccp.rB.Y - vA.X - -wA * ccp.rA.Y) +
-                                 cc.Normal.Y * (vB.Y + wB * ccp.rB.X - vA.Y - wA * ccp.rA.X);
-                    if (vRel < -Settings.VelocityThreshold)
-                    {
-                        ccp.VelocityBias = -cc.Restitution * vRel;
-                    }
-                }
-
-                // If we have two points, then prepare the block solver.
-                if (cc.PointCount == 2)
-                {
-                    ContactConstraintPoint ccp1 = cc.Points[0];
-                    ContactConstraintPoint ccp2 = cc.Points[1];
-
-                    float invMassA = bodyA.InvMass;
-                    float invIA = bodyA.InvI;
-                    float invMassB = bodyB.InvMass;
-                    float invIB = bodyB.InvI;
-
-                    float rn1A = ccp1.rA.X * cc.Normal.Y - ccp1.rA.Y * cc.Normal.X;
-                    float rn1B = ccp1.rB.X * cc.Normal.Y - ccp1.rB.Y * cc.Normal.X;
-                    float rn2A = ccp2.rA.X * cc.Normal.Y - ccp2.rA.Y * cc.Normal.X;
-                    float rn2B = ccp2.rB.X * cc.Normal.Y - ccp2.rB.Y * cc.Normal.X;
-
-                    float k11 = invMassA + invMassB + invIA * rn1A * rn1A + invIB * rn1B * rn1B;
-                    float k22 = invMassA + invMassB + invIA * rn2A * rn2A + invIB * rn2B * rn2B;
-                    float k12 = invMassA + invMassB + invIA * rn1A * rn2A + invIB * rn1B * rn2B;
-
-                    // Ensure a reasonable condition number.
-                    const float k_maxConditionNumber = 100.0f;
-                    if (k11 * k11 < k_maxConditionNumber * (k11 * k22 - k12 * k12))
-                    {
-                        // K is safe to invert.
-                        cc.K.Col1.X = k11;
-                        cc.K.Col1.Y = k12;
-                        cc.K.Col2.X = k12;
-                        cc.K.Col2.Y = k22;
-
-                        float a = cc.K.Col1.X, b = cc.K.Col2.X, c = cc.K.Col1.Y, d = cc.K.Col2.Y;
-                        float det = a * d - b * c;
-                        if (det != 0.0f)
-                        {
-                            det = 1.0f / det;
-                        }
-
-                        cc.NormalMass.Col1.X = det * d;
-                        cc.NormalMass.Col1.Y = -det * c;
-                        cc.NormalMass.Col2.X = -det * b;
-                        cc.NormalMass.Col2.Y = det * a;
-                    }
-                    else
-                    {
-                        // The constraints are redundant, just use one.
-                        // TODO_ERIN use deepest?
-                        cc.PointCount = 1;
-                    }
-                }
-            }
-        }
-
-        public void WarmStart()
-        {
-            // Warm start.
-            for (int i = 0; i < _constraintCount; ++i)
-            {
-                ContactConstraint c = Constraints[i];
-
-                float tangentx = c.Normal.Y;
-                float tangenty = -c.Normal.X;
-
-                for (int j = 0; j < c.PointCount; ++j)
-                {
-                    ContactConstraintPoint ccp = c.Points[j];
-                    float px = ccp.NormalImpulse * c.Normal.X + ccp.TangentImpulse * tangentx;
-                    float py = ccp.NormalImpulse * c.Normal.Y + ccp.TangentImpulse * tangenty;
-                    c.BodyA.AngularVelocityInternal -= c.BodyA.InvI * (ccp.rA.X * py - ccp.rA.Y * px);
-                    c.BodyA.LinearVelocityInternal.X -= c.BodyA.InvMass * px;
-                    c.BodyA.LinearVelocityInternal.Y -= c.BodyA.InvMass * py;
-                    c.BodyB.AngularVelocityInternal += c.BodyB.InvI * (ccp.rB.X * py - ccp.rB.Y * px);
-                    c.BodyB.LinearVelocityInternal.X += c.BodyB.InvMass * px;
-                    c.BodyB.LinearVelocityInternal.Y += c.BodyB.InvMass * py;
-                }
-            }
-        }
-
-        public void SolveVelocityConstraints()
-        {
-            for (int i = 0; i < _constraintCount; ++i)
-            {
-                ContactConstraint c = Constraints[i];
-                float wA = c.BodyA.AngularVelocityInternal;
-                float wB = c.BodyB.AngularVelocityInternal;
-
-                float tangentx = c.Normal.Y;
-                float tangenty = -c.Normal.X;
-
-                float friction = c.Friction;
-
-                Debug.Assert(c.PointCount == 1 || c.PointCount == 2);
-
-                // Solve tangent constraints
-                for (int j = 0; j < c.PointCount; ++j)
-                {
-                    ContactConstraintPoint ccp = c.Points[j];
-                    float lambda = ccp.TangentMass *
-                                   -((c.BodyB.LinearVelocityInternal.X + (-wB * ccp.rB.Y) -
-                                      c.BodyA.LinearVelocityInternal.X - (-wA * ccp.rA.Y)) * tangentx +
-                                     (c.BodyB.LinearVelocityInternal.Y + (wB * ccp.rB.X) -
-                                      c.BodyA.LinearVelocityInternal.Y - (wA * ccp.rA.X)) * tangenty);
-
-                    // MathUtils.Clamp the accumulated force
-                    float maxFriction = friction * ccp.NormalImpulse;
-                    float newImpulse = Math.Max(-maxFriction, Math.Min(ccp.TangentImpulse + lambda, maxFriction));
-                    lambda = newImpulse - ccp.TangentImpulse;
-
-                    // Apply contact impulse
-                    float px = lambda * tangentx;
-                    float py = lambda * tangenty;
-
-                    c.BodyA.LinearVelocityInternal.X -= c.BodyA.InvMass * px;
-                    c.BodyA.LinearVelocityInternal.Y -= c.BodyA.InvMass * py;
-                    wA -= c.BodyA.InvI * (ccp.rA.X * py - ccp.rA.Y * px);
-
-                    c.BodyB.LinearVelocityInternal.X += c.BodyB.InvMass * px;
-                    c.BodyB.LinearVelocityInternal.Y += c.BodyB.InvMass * py;
-                    wB += c.BodyB.InvI * (ccp.rB.X * py - ccp.rB.Y * px);
-
-                    ccp.TangentImpulse = newImpulse;
-                }
-
-                // Solve normal constraints
-                if (c.PointCount == 1)
-                {
-                    ContactConstraintPoint ccp = c.Points[0];
-
-                    // Relative velocity at contact
-                    // Compute normal impulse
-                    float lambda = -ccp.NormalMass *
-                                   ((c.BodyB.LinearVelocityInternal.X + (-wB * ccp.rB.Y) -
-                                     c.BodyA.LinearVelocityInternal.X - (-wA * ccp.rA.Y)) * c.Normal.X +
-                                    (c.BodyB.LinearVelocityInternal.Y + (wB * ccp.rB.X) -
-                                     c.BodyA.LinearVelocityInternal.Y -
-                                     (wA * ccp.rA.X)) * c.Normal.Y - ccp.VelocityBias);
-
-                    // Clamp the accumulated impulse
-                    float newImpulse = Math.Max(ccp.NormalImpulse + lambda, 0.0f);
-                    lambda = newImpulse - ccp.NormalImpulse;
-
-                    // Apply contact impulse
-                    float px = lambda * c.Normal.X;
-                    float py = lambda * c.Normal.Y;
-
-                    c.BodyA.LinearVelocityInternal.X -= c.BodyA.InvMass * px;
-                    c.BodyA.LinearVelocityInternal.Y -= c.BodyA.InvMass * py;
-                    wA -= c.BodyA.InvI * (ccp.rA.X * py - ccp.rA.Y * px);
-
-                    c.BodyB.LinearVelocityInternal.X += c.BodyB.InvMass * px;
-                    c.BodyB.LinearVelocityInternal.Y += c.BodyB.InvMass * py;
-                    wB += c.BodyB.InvI * (ccp.rB.X * py - ccp.rB.Y * px);
-
-                    ccp.NormalImpulse = newImpulse;
-                }
-                else
-                {
-                    // Block solver developed in collaboration with Dirk Gregorius (back in 01/07 on Box2D_Lite).
-                    // Build the mini LCP for this contact patch
-                    //
-                    // vn = A * x + b, vn >= 0, , vn >= 0, x >= 0 and vn_i * x_i = 0 with i = 1..2
-                    //
-                    // A = J * W * JT and J = ( -n, -r1 x n, n, r2 x n )
-                    // b = vn_0 - velocityBias
-                    //
-                    // The system is solved using the "Total enumeration method" (s. Murty). The complementary constraint vn_i * x_i
-                    // implies that we must have in any solution either vn_i = 0 or x_i = 0. So for the 2D contact problem the cases
-                    // vn1 = 0 and vn2 = 0, x1 = 0 and x2 = 0, x1 = 0 and vn2 = 0, x2 = 0 and vn1 = 0 need to be tested. The first valid
-                    // solution that satisfies the problem is chosen.
-                    // 
-                    // In order to account of the accumulated impulse 'a' (because of the iterative nature of the solver which only requires
-                    // that the accumulated impulse is clamped and not the incremental impulse) we change the impulse variable (x_i).
-                    //
-                    // Substitute:
-                    // 
-                    // x = x' - a
-                    // 
-                    // Plug into above equation:
-                    //
-                    // vn = A * x + b
-                    //    = A * (x' - a) + b
-                    //    = A * x' + b - A * a
-                    //    = A * x' + b'
-                    // b' = b - A * a;
-
-                    ContactConstraintPoint cp1 = c.Points[0];
-                    ContactConstraintPoint cp2 = c.Points[1];
-
-                    float ax = cp1.NormalImpulse;
-                    float ay = cp2.NormalImpulse;
-                    Debug.Assert(ax >= 0.0f && ay >= 0.0f);
-
-                    // Relative velocity at contact
-                    // Compute normal velocity
-                    float vn1 = (c.BodyB.LinearVelocityInternal.X + (-wB * cp1.rB.Y) - c.BodyA.LinearVelocityInternal.X -
-                                 (-wA * cp1.rA.Y)) * c.Normal.X +
-                                (c.BodyB.LinearVelocityInternal.Y + (wB * cp1.rB.X) - c.BodyA.LinearVelocityInternal.Y -
-                                 (wA * cp1.rA.X)) * c.Normal.Y;
-                    float vn2 = (c.BodyB.LinearVelocityInternal.X + (-wB * cp2.rB.Y) - c.BodyA.LinearVelocityInternal.X -
-                                 (-wA * cp2.rA.Y)) * c.Normal.X +
-                                (c.BodyB.LinearVelocityInternal.Y + (wB * cp2.rB.X) - c.BodyA.LinearVelocityInternal.Y -
-                                 (wA * cp2.rA.X)) * c.Normal.Y;
-
-                    float bx = vn1 - cp1.VelocityBias - (c.K.Col1.X * ax + c.K.Col2.X * ay);
-                    float by = vn2 - cp2.VelocityBias - (c.K.Col1.Y * ax + c.K.Col2.Y * ay);
-
-                    float xx = -(c.NormalMass.Col1.X * bx + c.NormalMass.Col2.X * by);
-                    float xy = -(c.NormalMass.Col1.Y * bx + c.NormalMass.Col2.Y * by);
-
-                    while (true)
-                    {
-                        //
-                        // Case 1: vn = 0
-                        //
-                        // 0 = A * x' + b'
-                        //
-                        // Solve for x':
-                        //
-                        // x' = - inv(A) * b'
-                        //
-                        if (xx >= 0.0f && xy >= 0.0f)
-                        {
-                            // Resubstitute for the incremental impulse
-                            float dx = xx - ax;
-                            float dy = xy - ay;
-
-                            // Apply incremental impulse
-                            float p1x = dx * c.Normal.X;
-                            float p1y = dx * c.Normal.Y;
-
-                            float p2x = dy * c.Normal.X;
-                            float p2y = dy * c.Normal.Y;
-
-                            float p12x = p1x + p2x;
-                            float p12y = p1y + p2y;
-
-                            c.BodyA.LinearVelocityInternal.X -= c.BodyA.InvMass * p12x;
-                            c.BodyA.LinearVelocityInternal.Y -= c.BodyA.InvMass * p12y;
-                            wA -= c.BodyA.InvI * ((cp1.rA.X * p1y - cp1.rA.Y * p1x) + (cp2.rA.X * p2y - cp2.rA.Y * p2x));
-
-                            c.BodyB.LinearVelocityInternal.X += c.BodyB.InvMass * p12x;
-                            c.BodyB.LinearVelocityInternal.Y += c.BodyB.InvMass * p12y;
-                            wB += c.BodyB.InvI * ((cp1.rB.X * p1y - cp1.rB.Y * p1x) + (cp2.rB.X * p2y - cp2.rB.Y * p2x));
-
-                            // Accumulate
-                            cp1.NormalImpulse = xx;
-                            cp2.NormalImpulse = xy;
-
-#if B2_DEBUG_SOLVER 
-                            
-			                float k_errorTol = 1e-3f;
-
-					        // Postconditions
-					        dv1 = vB + MathUtils.Cross(wB, cp1.rB) - vA - MathUtils.Cross(wA, cp1.rA);
-					        dv2 = vB + MathUtils.Cross(wB, cp2.rB) - vA - MathUtils.Cross(wA, cp2.rA);
-
-					        // Compute normal velocity
-					        vn1 = Vector2.Dot(dv1, normal);
-					        vn2 = Vector2.Dot(dv2, normal);
-
-					        Debug.Assert(MathUtils.Abs(vn1 - cp1.velocityBias) < k_errorTol);
-					        Debug.Assert(MathUtils.Abs(vn2 - cp2.velocityBias) < k_errorTol);
-#endif
-                            break;
-                        }
-
-                        //
-                        // Case 2: vn1 = 0 and x2 = 0
-                        //
-                        //   0 = a11 * x1' + a12 * 0 + b1' 
-                        // vn2 = a21 * x1' + a22 * 0 + b2'
-                        //
-                        xx = -cp1.NormalMass * bx;
-                        xy = 0.0f;
-                        vn1 = 0.0f;
-                        vn2 = c.K.Col1.Y * xx + by;
-
-                        if (xx >= 0.0f && vn2 >= 0.0f)
-                        {
-                            // Resubstitute for the incremental impulse
-                            float dx = xx - ax;
-                            float dy = xy - ay;
-
-                            // Apply incremental impulse
-                            float p1x = dx * c.Normal.X;
-                            float p1y = dx * c.Normal.Y;
-
-                            float p2x = dy * c.Normal.X;
-                            float p2y = dy * c.Normal.Y;
-
-                            float p12x = p1x + p2x;
-                            float p12y = p1y + p2y;
-
-                            c.BodyA.LinearVelocityInternal.X -= c.BodyA.InvMass * p12x;
-                            c.BodyA.LinearVelocityInternal.Y -= c.BodyA.InvMass * p12y;
-                            wA -= c.BodyA.InvI * ((cp1.rA.X * p1y - cp1.rA.Y * p1x) + (cp2.rA.X * p2y - cp2.rA.Y * p2x));
-
-                            c.BodyB.LinearVelocityInternal.X += c.BodyB.InvMass * p12x;
-                            c.BodyB.LinearVelocityInternal.Y += c.BodyB.InvMass * p12y;
-                            wB += c.BodyB.InvI * ((cp1.rB.X * p1y - cp1.rB.Y * p1x) + (cp2.rB.X * p2y - cp2.rB.Y * p2x));
-
-                            // Accumulate
-                            cp1.NormalImpulse = xx;
-                            cp2.NormalImpulse = xy;
-
-#if B2_DEBUG_SOLVER 
-    // Postconditions
-					        dv1 = vB + MathUtils.Cross(wB, cp1.rB) - vA - MathUtils.Cross(wA, cp1.rA);
-
-					        // Compute normal velocity
-					        vn1 = Vector2.Dot(dv1, normal);
-
-					        Debug.Assert(MathUtils.Abs(vn1 - cp1.velocityBias) < k_errorTol);
-#endif
-                            break;
-                        }
-
-
-                        //
-                        // Case 3: vn2 = 0 and x1 = 0
-                        //
-                        // vn1 = a11 * 0 + a12 * x2' + b1' 
-                        //   0 = a21 * 0 + a22 * x2' + b2'
-                        //
-                        xx = 0.0f;
-                        xy = -cp2.NormalMass * by;
-                        vn1 = c.K.Col2.X * xy + bx;
-                        vn2 = 0.0f;
-
-                        if (xy >= 0.0f && vn1 >= 0.0f)
-                        {
-                            // Resubstitute for the incremental impulse
-                            float dx = xx - ax;
-                            float dy = xy - ay;
-
-                            // Apply incremental impulse
-                            float p1x = dx * c.Normal.X;
-                            float p1y = dx * c.Normal.Y;
-
-                            float p2x = dy * c.Normal.X;
-                            float p2y = dy * c.Normal.Y;
-
-                            float p12x = p1x + p2x;
-                            float p12y = p1y + p2y;
-
-                            c.BodyA.LinearVelocityInternal.X -= c.BodyA.InvMass * p12x;
-                            c.BodyA.LinearVelocityInternal.Y -= c.BodyA.InvMass * p12y;
-                            wA -= c.BodyA.InvI * ((cp1.rA.X * p1y - cp1.rA.Y * p1x) + (cp2.rA.X * p2y - cp2.rA.Y * p2x));
-
-                            c.BodyB.LinearVelocityInternal.X += c.BodyB.InvMass * p12x;
-                            c.BodyB.LinearVelocityInternal.Y += c.BodyB.InvMass * p12y;
-                            wB += c.BodyB.InvI * ((cp1.rB.X * p1y - cp1.rB.Y * p1x) + (cp2.rB.X * p2y - cp2.rB.Y * p2x));
-
-                            // Accumulate
-                            cp1.NormalImpulse = xx;
-                            cp2.NormalImpulse = xy;
-
-#if B2_DEBUG_SOLVER 
-    // Postconditions
-					        dv2 = vB + MathUtils.Cross(wB, cp2.rB) - vA - MathUtils.Cross(wA, cp2.rA);
-
-					        // Compute normal velocity
-					        vn2 = Vector2.Dot(dv2, normal);
-
-					        Debug.Assert(MathUtils.Abs(vn2 - cp2.velocityBias) < k_errorTol);
-#endif
-                            break;
-                        }
-
-                        //
-                        // Case 4: x1 = 0 and x2 = 0
-                        // 
-                        // vn1 = b1
-                        // vn2 = b2;
-                        xx = 0.0f;
-                        xy = 0.0f;
-                        vn1 = bx;
-                        vn2 = by;
-
-                        if (vn1 >= 0.0f && vn2 >= 0.0f)
-                        {
-                            // Resubstitute for the incremental impulse
-                            float dx = xx - ax;
-                            float dy = xy - ay;
-
-                            // Apply incremental impulse
-                            float p1x = dx * c.Normal.X;
-                            float p1y = dx * c.Normal.Y;
-
-                            float p2x = dy * c.Normal.X;
-                            float p2y = dy * c.Normal.Y;
-
-                            float p12x = p1x + p2x;
-                            float p12y = p1y + p2y;
-
-                            c.BodyA.LinearVelocityInternal.X -= c.BodyA.InvMass * p12x;
-                            c.BodyA.LinearVelocityInternal.Y -= c.BodyA.InvMass * p12y;
-                            wA -= c.BodyA.InvI * ((cp1.rA.X * p1y - cp1.rA.Y * p1x) + (cp2.rA.X * p2y - cp2.rA.Y * p2x));
-
-                            c.BodyB.LinearVelocityInternal.X += c.BodyB.InvMass * p12x;
-                            c.BodyB.LinearVelocityInternal.Y += c.BodyB.InvMass * p12y;
-                            wB += c.BodyB.InvI * ((cp1.rB.X * p1y - cp1.rB.Y * p1x) + (cp2.rB.X * p2y - cp2.rB.Y * p2x));
-
-                            // Accumulate
-                            cp1.NormalImpulse = xx;
-                            cp2.NormalImpulse = xy;
-
-                            break;
-                        }
-
-                        // No solution, give up. This is hit sometimes, but it doesn't seem to matter.
-                        break;
-                    }
-                }
-
-                c.BodyA.AngularVelocityInternal = wA;
-                c.BodyB.AngularVelocityInternal = wB;
-            }
-        }
-
-        public void StoreImpulses()
-        {
-            for (int i = 0; i < _constraintCount; ++i)
-            {
-                ContactConstraint c = Constraints[i];
-                Manifold m = c.Manifold;
-
-                for (int j = 0; j < c.PointCount; ++j)
-                {
-                    ManifoldPoint pj = m.Points[j];
-                    ContactConstraintPoint cp = c.Points[j];
-
-                    pj.NormalImpulse = cp.NormalImpulse;
-                    pj.TangentImpulse = cp.TangentImpulse;
-
-                    m.Points[j] = pj;
-                }
-
-                c.Manifold = m;
-                _contacts[i].Manifold = m;
-            }
-        }
-
-        public bool SolvePositionConstraints(float baumgarte)
-        {
-            float minSeparation = 0.0f;
-
-            for (int i = 0; i < _constraintCount; ++i)
-            {
-                ContactConstraint c = Constraints[i];
-
-                Body bodyA = c.BodyA;
-                Body bodyB = c.BodyB;
-
-                float invMassA = bodyA.Mass * bodyA.InvMass;
-                float invIA = bodyA.Mass * bodyA.InvI;
-                float invMassB = bodyB.Mass * bodyB.InvMass;
-                float invIB = bodyB.Mass * bodyB.InvI;
-
-                // Solve normal constraints
-                for (int j = 0; j < c.PointCount; ++j)
-                {
-                    Vector2 normal;
-                    Vector2 point;
-                    float separation;
-
-                    Solve(c, j, out normal, out point, out separation);
-
-                    float rax = point.X - bodyA.Sweep.C.X;
-                    float ray = point.Y - bodyA.Sweep.C.Y;
-
-                    float rbx = point.X - bodyB.Sweep.C.X;
-                    float rby = point.Y - bodyB.Sweep.C.Y;
-
-                    // Track max constraint error.
-                    minSeparation = Math.Min(minSeparation, separation);
-
-                    // Prevent large corrections and allow slop.
-                    float C = Math.Max(-Settings.MaxLinearCorrection,
-                                       Math.Min(baumgarte * (separation + Settings.LinearSlop), 0.0f));
-
-                    // Compute the effective mass.
-                    float rnA = rax * normal.Y - ray * normal.X;
-                    float rnB = rbx * normal.Y - rby * normal.X;
-                    float K = invMassA + invMassB + invIA * rnA * rnA + invIB * rnB * rnB;
-
-                    // Compute normal impulse
-                    float impulse = K > 0.0f ? -C / K : 0.0f;
-
-                    float px = impulse * normal.X;
-                    float py = impulse * normal.Y;
-
-                    bodyA.Sweep.C.X -= invMassA * px;
-                    bodyA.Sweep.C.Y -= invMassA * py;
-                    bodyA.Sweep.A -= invIA * (rax * py - ray * px);
-
-                    bodyB.Sweep.C.X += invMassB * px;
-                    bodyB.Sweep.C.Y += invMassB * py;
-                    bodyB.Sweep.A += invIB * (rbx * py - rby * px);
-
-                    bodyA.SynchronizeTransform();
-                    bodyB.SynchronizeTransform();
-                }
-            }
-
-            // We can't expect minSpeparation >= -Settings.b2_linearSlop because we don't
-            // push the separation above -Settings.b2_linearSlop.
-            return minSeparation >= -1.5f * Settings.LinearSlop;
-        }
-
-        private static void Solve(ContactConstraint cc, int index, out Vector2 normal, out Vector2 point,
-                                  out float separation)
-        {
-            Debug.Assert(cc.PointCount > 0);
-
-            normal = Vector2.Zero;
-
-            switch (cc.Type)
-            {
-                case ManifoldType.Circles:
-                    {
-                        Vector2 pointA = cc.BodyA.GetWorldPoint(ref cc.LocalPoint);
-                        Vector2 pointB = cc.BodyB.GetWorldPoint(ref cc.Points[0].LocalPoint);
-                        float a = (pointA.X - pointB.X) * (pointA.X - pointB.X) +
-                                  (pointA.Y - pointB.Y) * (pointA.Y - pointB.Y);
-                        if (a > Settings.Epsilon * Settings.Epsilon)
-                        {
-                            Vector2 normalTmp = pointB - pointA;
-                            float factor = 1f / (float)Math.Sqrt(normalTmp.X * normalTmp.X + normalTmp.Y * normalTmp.Y);
-                            normal.X = normalTmp.X * factor;
-                            normal.Y = normalTmp.Y * factor;
-                        }
-                        else
-                        {
-                            normal.X = 1;
-                            normal.Y = 0;
-                        }
-
-                        point = 0.5f * (pointA + pointB);
-                        separation = (pointB.X - pointA.X) * normal.X + (pointB.Y - pointA.Y) * normal.Y - cc.RadiusA -
-                                     cc.RadiusB;
-                    }
-                    break;
-
-                case ManifoldType.FaceA:
-                    {
-                        normal = cc.BodyA.GetWorldVector(ref cc.LocalNormal);
-                        Vector2 planePoint = cc.BodyA.GetWorldPoint(ref cc.LocalPoint);
-                        Vector2 clipPoint = cc.BodyB.GetWorldPoint(ref cc.Points[index].LocalPoint);
-                        separation = (clipPoint.X - planePoint.X) * normal.X + (clipPoint.Y - planePoint.Y) * normal.Y -
-                                     cc.RadiusA - cc.RadiusB;
-                        point = clipPoint;
-                    }
-                    break;
-
-                case ManifoldType.FaceB:
-                    {
-                        normal = cc.BodyB.GetWorldVector(ref cc.LocalNormal);
-                        Vector2 planePoint = cc.BodyB.GetWorldPoint(ref cc.LocalPoint);
-
-                        Vector2 clipPoint = cc.BodyA.GetWorldPoint(ref cc.Points[index].LocalPoint);
-                        separation = (clipPoint.X - planePoint.X) * normal.X + (clipPoint.Y - planePoint.Y) * normal.Y -
-                                     cc.RadiusA - cc.RadiusB;
-                        point = clipPoint;
-
-                        // Ensure normal points from A to B
-                        normal = -normal;
-                    }
-                    break;
-                default:
-                    point = Vector2.Zero;
-                    separation = 0.0f;
-                    break;
-            }
-        }
-    }
-}

FarseerPhysicsEngine/Dynamics/Fixture.cs

@@ -1,607 +0,0 @@
-/*
-* Farseer Physics Engine based on Box2D.XNA port:
-* Copyright (c) 2010 Ian Qvist
-* 
-* Box2D.XNA port of Box2D:
-* Copyright (c) 2009 Brandon Furtwangler, Nathan Furtwangler
-*
-* Original source Box2D:
-* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com 
-* 
-* This software is provided 'as-is', without any express or implied 
-* warranty.  In no event will the authors be held liable for any damages 
-* arising from the use of this software. 
-* Permission is granted to anyone to use this software for any purpose, 
-* including commercial applications, and to alter it and redistribute it 
-* freely, subject to the following restrictions: 
-* 1. The origin of this software must not be misrepresented; you must not 
-* claim that you wrote the original software. If you use this software 
-* in a product, an acknowledgment in the product documentation would be 
-* appreciated but is not required. 
-* 2. Altered source versions must be plainly marked as such, and must not be 
-* misrepresented as being the original software. 
-* 3. This notice may not be removed or altered from any source distribution. 
-*/
-
-using System;
-using System.Collections.Generic;
-using System.Diagnostics;
-using FarseerPhysics.Collision;
-using FarseerPhysics.Collision.Shapes;
-using FarseerPhysics.Common;
-using FarseerPhysics.Dynamics.Contacts;
-using Microsoft.Xna.Framework;
-
-namespace FarseerPhysics.Dynamics
-{
-    [Flags]
-    public enum Category
-    {
-        None = 0,
-        All = int.MaxValue,
-        Cat1 = 1,
-        Cat2 = 2,
-        Cat3 = 4,
-        Cat4 = 8,
-        Cat5 = 16,
-        Cat6 = 32,
-        Cat7 = 64,
-        Cat8 = 128,
-        Cat9 = 256,
-        Cat10 = 512,
-        Cat11 = 1024,
-        Cat12 = 2048,
-        Cat13 = 4096,
-        Cat14 = 8192,
-        Cat15 = 16384,
-        Cat16 = 32768,
-        Cat17 = 65536,
-        Cat18 = 131072,
-        Cat19 = 262144,
-        Cat20 = 524288,
-        Cat21 = 1048576,
-        Cat22 = 2097152,
-        Cat23 = 4194304,
-        Cat24 = 8388608,
-        Cat25 = 16777216,
-        Cat26 = 33554432,
-        Cat27 = 67108864,
-        Cat28 = 134217728,
-        Cat29 = 268435456,
-        Cat30 = 536870912,
-        Cat31 = 1073741824
-    }
-
-    /// <summary>
-    /// This proxy is used internally to connect fixtures to the broad-phase.
-    /// </summary>
-    public struct FixtureProxy
-    {
-        public AABB AABB;
-        public int ChildIndex;
-        public Fixture Fixture;
-        public int ProxyId;
-    }
-
-    /// <summary>
-    /// A fixture is used to attach a Shape to a body for collision detection. A fixture
-    /// inherits its transform from its parent. Fixtures hold additional non-geometric data
-    /// such as friction, collision filters, etc.
-    /// Fixtures are created via Body.CreateFixture.
-    /// Warning: You cannot reuse fixtures.
-    /// </summary>
-    public class Fixture : IDisposable
-    {
-        private static int _fixtureIdCounter;
-
-        /// <summary>
-        /// Fires after two shapes has collided and are solved. This gives you a chance to get the impact force.
-        /// </summary>
-        public AfterCollisionEventHandler AfterCollision;
-
-        /// <summary>
-        /// Fires when two fixtures are close to each other.
-        /// Due to how the broadphase works, this can be quite inaccurate as shapes are approximated using AABBs.
-        /// </summary>
-        public BeforeCollisionEventHandler BeforeCollision;
-
-        /// <summary>
-        /// Fires when two shapes collide and a contact is created between them.
-        /// Note that the first fixture argument is always the fixture that the delegate is subscribed to.
-        /// </summary>
-        public OnCollisionEventHandler OnCollision;
-
-        /// <summary>
-        /// Fires when two shapes separate and a contact is removed between them.
-        /// Note that the first fixture argument is always the fixture that the delegate is subscribed to.
-        /// </summary>
-        public OnSeparationEventHandler OnSeparation;
-
-        public FixtureProxy[] Proxies;
-        public int ProxyCount;
-        internal Category _collidesWith;
-        internal Category _collisionCategories;
-        internal short _collisionGroup;
-        internal Dictionary<int, bool> _collisionIgnores;
-        private float _friction;
-        private float _restitution;
-
-        internal Fixture()
-        {
-        }
-
-        public Fixture(Body body, Shape shape)
-            : this(body, shape, null)
-        {
-        }
-
-        public Fixture(Body body, Shape shape, object userData)
-        {
-            if (Settings.UseFPECollisionCategories)
-                _collisionCategories = Category.All;
-            else
-                _collisionCategories = Category.Cat1;
-
-            _collidesWith = Category.All;
-            _collisionGroup = 0;
-
-            //Fixture defaults
-            Friction = 0.2f;
-            Restitution = 0;
-
-            IsSensor = false;
-
-            Body = body;
-            UserData = userData;
-
-            if (Settings.ConserveMemory)
-                Shape = shape;
-            else
-                Shape = shape.Clone();
-
-            RegisterFixture();
-        }
-
-        /// <summary>
-        /// Defaults to 0
-        /// 
-        /// If Settings.UseFPECollisionCategories is set to false:
-        /// Collision groups allow a certain group of objects to never collide (negative)
-        /// or always collide (positive). Zero means no collision group. Non-zero group
-        /// filtering always wins against the mask bits.
-        /// 
-        /// If Settings.UseFPECollisionCategories is set to true:
-        /// If 2 fixtures are in the same collision group, they will not collide.
-        /// </summary>
-        public short CollisionGroup
-        {
-            set
-            {
-                if (_collisionGroup == value)
-                    return;
-
-                _collisionGroup = value;
-                Refilter();
-            }
-            get { return _collisionGroup; }
-        }
-
-        /// <summary>
-        /// Defaults to Category.All
-        /// 
-        /// The collision mask bits. This states the categories that this
-        /// fixture would accept for collision.
-        /// Use Settings.UseFPECollisionCategories to change the behavior.
-        /// </summary>
-        public Category CollidesWith
-        {
-            get { return _collidesWith; }
-
-            set
-            {
-                if (_collidesWith == value)
-                    return;
-
-                _collidesWith = value;
-                Refilter();
-            }
-        }
-
-        /// <summary>
-        /// The collision categories this fixture is a part of.
-        /// 
-        /// If Settings.UseFPECollisionCategories is set to false:
-        /// Defaults to Category.Cat1
-        /// 
-        /// If Settings.UseFPECollisionCategories is set to true:
-        /// Defaults to Category.All
-        /// </summary>
-        public Category CollisionCategories
-        {
-            get { return _collisionCategories; }
-
-            set
-            {
-                if (_collisionCategories == value)
-                    return;
-
-                _collisionCategories = value;
-                Refilter();
-            }
-        }
-
-        /// <summary>
-        /// Get the type of the child Shape. You can use this to down cast to the concrete Shape.
-        /// </summary>
-        /// <value>The type of the shape.</value>
-        public ShapeType ShapeType
-        {
-            get { return Shape.ShapeType; }
-        }
-
-        /// <summary>
-        /// Get the child Shape. You can modify the child Shape, however you should not change the
-        /// number of vertices because this will crash some collision caching mechanisms.
-        /// </summary>
-        /// <value>The shape.</value>
-        public Shape Shape { get; internal set; }
-
-        /// <summary>
-        /// Gets or sets a value indicating whether this fixture is a sensor.
-        /// </summary>
-        /// <value><c>true</c> if this instance is a sensor; otherwise, <c>false</c>.</value>
-        public bool IsSensor { get; set; }
-
-        /// <summary>
-        /// Get the parent body of this fixture. This is null if the fixture is not attached.
-        /// </summary>
-        /// <value>The body.</value>
-        public Body Body { get; internal set; }
-
-        /// <summary>
-        /// Set the user data. Use this to store your application specific data.
-        /// </summary>
-        /// <value>The user data.</value>
-        public object UserData { get; set; }
-
-        /// <summary>
-        /// Get or set the coefficient of friction.
-        /// </summary>
-        /// <value>The friction.</value>
-        public float Friction
-        {
-            get { return _friction; }
-            set
-            {
-                Debug.Assert(!float.IsNaN(value));
-
-                _friction = value;
-            }
-        }
-
-        /// <summary>
-        /// Get or set the coefficient of restitution.
-        /// </summary>
-        /// <value>The restitution.</value>
-        public float Restitution
-        {
-            get { return _restitution; }
-            set
-            {
-                Debug.Assert(!float.IsNaN(value));
-
-                _restitution = value;
-            }
-        }
-
-        /// <summary>
-        /// Gets a unique ID for this fixture.
-        /// </summary>
-        /// <value>The fixture id.</value>
-        public int FixtureId { get; private set; }
-
-        #region IDisposable Members
-
-        public bool IsDisposed { get; set; }
-
-        public void Dispose()
-        {
-            if (!IsDisposed)
-            {
-                Body.DestroyFixture(this);
-                IsDisposed = true;
-                GC.SuppressFinalize(this);
-            }
-        }
-
-        #endregion
-
-        /// <summary>
-        /// Restores collisions between this fixture and the provided fixture.
-        /// </summary>
-        /// <param name="fixture">The fixture.</param>
-        public void RestoreCollisionWith(Fixture fixture)
-        {
-            if (_collisionIgnores == null)
-                return;
-
-            if (_collisionIgnores.ContainsKey(fixture.FixtureId))
-            {
-                _collisionIgnores[fixture.FixtureId] = false;
-                Refilter();
-            }
-        }
-
-        /// <summary>
-        /// Ignores collisions between this fixture and the provided fixture.
-        /// </summary>
-        /// <param name="fixture">The fixture.</param>
-        public void IgnoreCollisionWith(Fixture fixture)
-        {
-            if (_collisionIgnores == null)
-                _collisionIgnores = new Dictionary<int, bool>();
-
-            if (_collisionIgnores.ContainsKey(fixture.FixtureId))
-                _collisionIgnores[fixture.FixtureId] = true;
-            else
-                _collisionIgnores.Add(fixture.FixtureId, true);
-
-            Refilter();
-        }
-
-        /// <summary>
-        /// Determines whether collisions are ignored between this fixture and the provided fixture.
-        /// </summary>
-        /// <param name="fixture">The fixture.</param>
-        /// <returns>
-        /// 	<c>true</c> if the fixture is ignored; otherwise, <c>false</c>.
-        /// </returns>
-        public bool IsFixtureIgnored(Fixture fixture)
-        {
-            if (_collisionIgnores == null)
-                return false;
-
-            if (_collisionIgnores.ContainsKey(fixture.FixtureId))
-                return _collisionIgnores[fixture.FixtureId];
-
-            return false;
-        }
-
-        /// <summary>
-        /// Contacts are persistant and will keep being persistant unless they are
-        /// flagged for filtering.
-        /// This methods flags all contacts associated with the body for filtering.
-        /// </summary>
-        internal void Refilter()
-        {
-            // Flag associated contacts for filtering.
-            ContactEdge edge = Body.ContactList;
-            while (edge != null)
-            {
-                Contact contact = edge.Contact;
-                Fixture fixtureA = contact.FixtureA;
-                Fixture fixtureB = contact.FixtureB;
-                if (fixtureA == this || fixtureB == this)
-                {
-                    contact.FlagForFiltering();
-                }
-
-                edge = edge.Next;
-            }
-
-            World world = Body.World;
-
-            if (world == null)
-            {
-                return;
-            }
-
-            // Touch each proxy so that new pairs may be created
-            IBroadPhase broadPhase = world.ContactManager.BroadPhase;
-            for (int i = 0; i < ProxyCount; ++i)
-            {
-                broadPhase.TouchProxy(Proxies[i].ProxyId);
-            }
-        }
-
-        private void RegisterFixture()
-        {
-            // Reserve proxy space
-            Proxies = new FixtureProxy[Shape.ChildCount];
-            ProxyCount = 0;
-
-            FixtureId = _fixtureIdCounter++;
-
-            if ((Body.Flags & BodyFlags.Enabled) == BodyFlags.Enabled)
-            {
-                IBroadPhase broadPhase = Body.World.ContactManager.BroadPhase;
-                CreateProxies(broadPhase, ref Body.Xf);
-            }
-
-            Body.FixtureList.Add(this);
-
-            // Adjust mass properties if needed.
-            if (Shape._density > 0.0f)
-            {
-                Body.ResetMassData();
-            }
-
-            // Let the world know we have a new fixture. This will cause new contacts
-            // to be created at the beginning of the next time step.
-            Body.World.Flags |= WorldFlags.NewFixture;
-
-            if (Body.World.FixtureAdded != null)
-            {
-                Body.World.FixtureAdded(this);
-            }
-        }
-
-        /// <summary>
-        /// Test a point for containment in this fixture.
-        /// </summary>
-        /// <param name="point">A point in world coordinates.</param>
-        /// <returns></returns>
-        public bool TestPoint(ref Vector2 point)
-        {
-            return Shape.TestPoint(ref Body.Xf, ref point);
-        }
-
-        /// <summary>
-        /// Cast a ray against this Shape.
-        /// </summary>
-        /// <param name="output">The ray-cast results.</param>
-        /// <param name="input">The ray-cast input parameters.</param>
-        /// <param name="childIndex">Index of the child.</param>
-        /// <returns></returns>
-        public bool RayCast(out RayCastOutput output, ref RayCastInput input, int childIndex)
-        {
-            return Shape.RayCast(out output, ref input, ref Body.Xf, childIndex);
-        }
-
-        /// <summary>
-        /// Get the fixture's AABB. This AABB may be enlarge and/or stale.
-        /// If you need a more accurate AABB, compute it using the Shape and
-        /// the body transform.
-        /// </summary>
-        /// <param name="aabb">The aabb.</param>
-        /// <param name="childIndex">Index of the child.</param>
-        public void GetAABB(out AABB aabb, int childIndex)
-        {
-            Debug.Assert(0 <= childIndex && childIndex < ProxyCount);
-            aabb = Proxies[childIndex].AABB;
-        }
-
-        public Fixture Clone(Body body)
-        {
-            Fixture fixture = new Fixture();
-            fixture.Body = body;
-
-            if (Settings.ConserveMemory)
-                fixture.Shape = Shape;
-            else
-                fixture.Shape = Shape.Clone();
-
-            fixture.UserData = UserData;
-            fixture.Restitution = Restitution;
-            fixture.Friction = Friction;
-            fixture.IsSensor = IsSensor;
-            fixture._collisionGroup = CollisionGroup;
-            fixture._collisionCategories = CollisionCategories;
-            fixture._collidesWith = CollidesWith;
-
-            if (_collisionIgnores != null)
-            {
-                fixture._collisionIgnores = new Dictionary<int, bool>();
-
-                foreach (KeyValuePair<int, bool> pair in _collisionIgnores)
-                {
-                    fixture._collisionIgnores.Add(pair.Key, pair.Value);
-                }
-            }
-
-            fixture.RegisterFixture();
-            return fixture;
-        }
-
-        public Fixture DeepClone()
-        {
-            Fixture fix = Clone(Body.Clone());
-            return fix;
-        }
-
-        internal void Destroy()
-        {
-            // The proxies must be destroyed before calling this.
-            Debug.Assert(ProxyCount == 0);
-
-            // Free the proxy array.
-            Proxies = null;
-            Shape = null;
-
-            BeforeCollision = null;
-            OnCollision = null;
-            OnSeparation = null;
-            AfterCollision = null;
-
-            if (Body.World.FixtureRemoved != null)
-            {
-                Body.World.FixtureRemoved(this);
-            }
-
-            Body.World.FixtureAdded = null;
-            Body.World.FixtureRemoved = null;
-            OnSeparation = null;
-            OnCollision = null;
-        }
-
-        // These support body activation/deactivation.
-        internal void CreateProxies(IBroadPhase broadPhase, ref Transform xf)
-        {
-            Debug.Assert(ProxyCount == 0);
-
-            // Create proxies in the broad-phase.
-            ProxyCount = Shape.ChildCount;
-
-            for (int i = 0; i < ProxyCount; ++i)
-            {
-                FixtureProxy proxy = new FixtureProxy();
-                Shape.ComputeAABB(out proxy.AABB, ref xf, i);
-
-                proxy.Fixture = this;
-                proxy.ChildIndex = i;
-                proxy.ProxyId = broadPhase.AddProxy(ref proxy);
-
-                Proxies[i] = proxy;
-            }
-        }
-
-        internal void DestroyProxies(IBroadPhase broadPhase)
-        {
-            // Destroy proxies in the broad-phase.
-            for (int i = 0; i < ProxyCount; ++i)
-            {
-                broadPhase.RemoveProxy(Proxies[i].ProxyId);
-                Proxies[i].ProxyId = -1;
-            }
-
-            ProxyCount = 0;
-        }
-
-        internal void Synchronize(IBroadPhase broadPhase, ref Transform transform1, ref Transform transform2)
-        {
-            if (ProxyCount == 0)
-            {
-                return;
-            }
-
-            for (int i = 0; i < ProxyCount; ++i)
-            {
-                FixtureProxy proxy = Proxies[i];
-
-                // Compute an AABB that covers the swept Shape (may miss some rotation effect).
-                AABB aabb1, aabb2;
-                Shape.ComputeAABB(out aabb1, ref transform1, proxy.ChildIndex);
-                Shape.ComputeAABB(out aabb2, ref transform2, proxy.ChildIndex);
-
-                proxy.AABB.Combine(ref aabb1, ref aabb2);
-
-                Vector2 displacement = transform2.Position - transform1.Position;
-
-                broadPhase.MoveProxy(proxy.ProxyId, ref proxy.AABB, displacement);
-            }
-        }
-
-        internal bool CompareTo(Fixture fixture)
-        {
-            return (
-                       CollidesWith == fixture.CollidesWith &&
-                       CollisionCategories == fixture.CollisionCategories &&
-                       CollisionGroup == fixture.CollisionGroup &&
-                       Friction == fixture.Friction &&
-                       IsSensor == fixture.IsSensor &&
-                       Restitution == fixture.Restitution &&
-                       Shape.CompareTo(fixture.Shape) &&
-                       UserData == fixture.UserData);
-        }
-    }
-}

FarseerPhysicsEngine/Dynamics/Island.cs

@@ -1,484 +0,0 @@
-/*
-* Farseer Physics Engine based on Box2D.XNA port:
-* Copyright (c) 2010 Ian Qvist
-* 
-* Box2D.XNA port of Box2D:
-* Copyright (c) 2009 Brandon Furtwangler, Nathan Furtwangler
-*
-* Original source Box2D:
-* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com 
-* 
-* This software is provided 'as-is', without any express or implied 
-* warranty.  In no event will the authors be held liable for any damages 
-* arising from the use of this software. 
-* Permission is granted to anyone to use this software for any purpose, 
-* including commercial applications, and to alter it and redistribute it 
-* freely, subject to the following restrictions: 
-* 1. The origin of this software must not be misrepresented; you must not 
-* claim that you wrote the original software. If you use this software 
-* in a product, an acknowledgment in the product documentation would be 
-* appreciated but is not required. 
-* 2. Altered source versions must be plainly marked as such, and must not be 
-* misrepresented as being the original software. 
-* 3. This notice may not be removed or altered from any source distribution. 
-*/
-
-using System;
-using System.Diagnostics;
-using FarseerPhysics.Common;
-using FarseerPhysics.Dynamics.Contacts;
-using FarseerPhysics.Dynamics.Joints;
-using Microsoft.Xna.Framework;
-
-namespace FarseerPhysics.Dynamics
-{
-    /// <summary>
-    /// This is an internal class.
-    /// </summary>
-    public class Island
-    {
-        public Body[] Bodies;
-        public int BodyCount;
-        public int ContactCount;
-        public int JointCount;
-        private int _bodyCapacity;
-        private int _contactCapacity;
-        private ContactManager _contactManager;
-        private ContactSolver _contactSolver = new ContactSolver();
-        private Contact[] _contacts;
-        private int _jointCapacity;
-        private Joint[] _joints;
-        public float JointUpdateTime;
-
-        private const float LinTolSqr = Settings.LinearSleepTolerance * Settings.LinearSleepTolerance;
-        private const float AngTolSqr = Settings.AngularSleepTolerance * Settings.AngularSleepTolerance;
-
-#if (!SILVERLIGHT)
-        private Stopwatch _watch = new Stopwatch();
-#endif
-
-        public void Reset(int bodyCapacity, int contactCapacity, int jointCapacity, ContactManager contactManager)
-        {
-            _bodyCapacity = bodyCapacity;
-            _contactCapacity = contactCapacity;
-            _jointCapacity = jointCapacity;
-
-            BodyCount = 0;
-            ContactCount = 0;
-            JointCount = 0;
-
-            _contactManager = contactManager;
-
-            if (Bodies == null || Bodies.Length < bodyCapacity)
-            {
-                Bodies = new Body[bodyCapacity];
-            }
-
-            if (_contacts == null || _contacts.Length < contactCapacity)
-            {
-                _contacts = new Contact[contactCapacity * 2];
-            }
-
-            if (_joints == null || _joints.Length < jointCapacity)
-            {
-                _joints = new Joint[jointCapacity * 2];
-            }
-        }
-
-        public void Clear()
-        {
-            BodyCount = 0;
-            ContactCount = 0;
-            JointCount = 0;
-        }
-
-        private float _tmpTime;
-
-        public void Solve(ref TimeStep step, ref Vector2 gravity)
-        {
-            // Integrate velocities and apply damping.
-            for (int i = 0; i < BodyCount; ++i)
-            {
-                Body b = Bodies[i];
-
-                if (b.BodyType != BodyType.Dynamic)
-                {
-                    continue;
-                }
-
-                // Integrate velocities.
-                // FPE 3 only - Only apply gravity if the body wants it.
-                if (b.IgnoreGravity)
-                {
-                    b.LinearVelocityInternal.X += step.dt * (b.InvMass * b.Force.X);
-                    b.LinearVelocityInternal.Y += step.dt * (b.InvMass * b.Force.Y);
-                    b.AngularVelocityInternal += step.dt * b.InvI * b.Torque;
-                }
-                else
-                {
-                    b.LinearVelocityInternal.X += step.dt * (gravity.X + b.InvMass * b.Force.X);
-                    b.LinearVelocityInternal.Y += step.dt * (gravity.Y + b.InvMass * b.Force.Y);
-                    b.AngularVelocityInternal += step.dt * b.InvI * b.Torque;
-                }
-
-                // Apply damping.
-                // ODE: dv/dt + c * v = 0
-                // Solution: v(t) = v0 * exp(-c * t)
-                // Time step: v(t + dt) = v0 * exp(-c * (t + dt)) = v0 * exp(-c * t) * exp(-c * dt) = v * exp(-c * dt)
-                // v2 = exp(-c * dt) * v1
-                // Taylor expansion:
-                // v2 = (1.0f - c * dt) * v1
-                b.LinearVelocityInternal *= MathUtils.Clamp(1.0f - step.dt * b.LinearDamping, 0.0f, 1.0f);
-                b.AngularVelocityInternal *= MathUtils.Clamp(1.0f - step.dt * b.AngularDamping, 0.0f, 1.0f);
-            }
-
-            // Partition contacts so that contacts with static bodies are solved last.
-            int i1 = -1;
-            for (int i2 = 0; i2 < ContactCount; ++i2)
-            {
-                Fixture fixtureA = _contacts[i2].FixtureA;
-                Fixture fixtureB = _contacts[i2].FixtureB;
-                Body bodyA = fixtureA.Body;
-                Body bodyB = fixtureB.Body;
-                bool nonStatic = bodyA.BodyType != BodyType.Static && bodyB.BodyType != BodyType.Static;
-                if (nonStatic)
-                {
-                    ++i1;
-
-                    //TODO: Only swap if they are not the same? see http://code.google.com/p/box2d/issues/detail?id=162
-                    Contact tmp = _contacts[i1];
-                    _contacts[i1] = _contacts[i2];
-                    _contacts[i2] = tmp;
-                }
-            }
-
-            // Initialize velocity constraints.
-            _contactSolver.Reset(_contacts, ContactCount, step.dtRatio, Settings.EnableWarmstarting);
-            _contactSolver.InitializeVelocityConstraints();
-
-            if (Settings.EnableWarmstarting)
-            {
-                _contactSolver.WarmStart();
-            }
-
-#if (!SILVERLIGHT)
-            if (Settings.EnableDiagnostics)
-            {
-                _watch.Start();
-                _tmpTime = 0;
-            }
-#endif
-
-            for (int i = 0; i < JointCount; ++i)
-            {
-                if (_joints[i].Enabled)
-                    _joints[i].InitVelocityConstraints(ref step);
-            }
-
-#if (!SILVERLIGHT)
-            if (Settings.EnableDiagnostics)
-            {
-                _tmpTime += _watch.ElapsedTicks;
-            }
-#endif
-
-            // Solve velocity constraints.
-            for (int i = 0; i < Settings.VelocityIterations; ++i)
-            {
-#if (!SILVERLIGHT)
-                if (Settings.EnableDiagnostics)
-                    _watch.Start();
-#endif
-                for (int j = 0; j < JointCount; ++j)
-                {
-                    Joint joint = _joints[j];
-
-                    if (!joint.Enabled)
-                        continue;
-
-                    joint.SolveVelocityConstraints(ref step);
-                    joint.Validate(step.inv_dt);
-                }
-
-#if (!SILVERLIGHT)
-                if (Settings.EnableDiagnostics)
-                {
-                    _watch.Stop();
-                    _tmpTime += _watch.ElapsedTicks;
-                    _watch.Reset();
-                }
-#endif
-
-                _contactSolver.SolveVelocityConstraints();
-            }
-
-            // Post-solve (store impulses for warm starting).
-            _contactSolver.StoreImpulses();
-
-            // Integrate positions.
-            for (int i = 0; i < BodyCount; ++i)
-            {
-                Body b = Bodies[i];
-
-                if (b.BodyType == BodyType.Static)
-                {
-                    continue;
-                }
-
-                // Check for large velocities.
-                float translationX = step.dt * b.LinearVelocityInternal.X;
-                float translationY = step.dt * b.LinearVelocityInternal.Y;
-                float result = translationX * translationX + translationY * translationY;
-
-                if (result > Settings.MaxTranslationSquared)
-                {
-                    float sq = (float)Math.Sqrt(result);
-
-                    float ratio = Settings.MaxTranslation / sq;
-                    b.LinearVelocityInternal.X *= ratio;
-                    b.LinearVelocityInternal.Y *= ratio;
-                }
-
-                float rotation = step.dt * b.AngularVelocityInternal;
-                if (rotation * rotation > Settings.MaxRotationSquared)
-                {
-                    float ratio = Settings.MaxRotation / Math.Abs(rotation);
-                    b.AngularVelocityInternal *= ratio;
-                }
-
-                // Store positions for continuous collision.
-                b.Sweep.C0.X = b.Sweep.C.X;
-                b.Sweep.C0.Y = b.Sweep.C.Y;
-                b.Sweep.A0 = b.Sweep.A;
-
-                // Integrate
-                b.Sweep.C.X += step.dt * b.LinearVelocityInternal.X;
-                b.Sweep.C.Y += step.dt * b.LinearVelocityInternal.Y;
-                b.Sweep.A += step.dt * b.AngularVelocityInternal;
-
-                // Compute new transform
-                b.SynchronizeTransform();
-
-                // Note: shapes are synchronized later.
-            }
-
-            // Iterate over constraints.
-            for (int i = 0; i < Settings.PositionIterations; ++i)
-            {
-                bool contactsOkay = _contactSolver.SolvePositionConstraints(Settings.ContactBaumgarte);
-                bool jointsOkay = true;
-
-#if (!SILVERLIGHT)
-                if (Settings.EnableDiagnostics)
-                    _watch.Start();
-#endif
-                for (int j = 0; j < JointCount; ++j)
-                {
-                    Joint joint = _joints[j];
-                    if (!joint.Enabled)
-                        continue;
-
-                    bool jointOkay = joint.SolvePositionConstraints();
-                    jointsOkay = jointsOkay && jointOkay;
-                }
-
-#if (!SILVERLIGHT)
-                if (Settings.EnableDiagnostics)
-                {
-                    _watch.Stop();
-                    _tmpTime += _watch.ElapsedTicks;
-                    _watch.Reset();
-                }
-#endif
-                if (contactsOkay && jointsOkay)
-                {
-                    // Exit early if the position errors are small.
-                    break;
-                }
-            }
-
-#if (!SILVERLIGHT)
-            if (Settings.EnableDiagnostics)
-            {
-                JointUpdateTime = _tmpTime;
-            }
-#endif
-
-            Report(_contactSolver.Constraints);
-
-            if (Settings.AllowSleep)
-            {
-                float minSleepTime = Settings.MaxFloat;
-
-                for (int i = 0; i < BodyCount; ++i)
-                {
-                    Body b = Bodies[i];
-                    if (b.BodyType == BodyType.Static)
-                    {
-                        continue;
-                    }
-
-                    if ((b.Flags & BodyFlags.AutoSleep) == 0)
-                    {
-                        b.SleepTime = 0.0f;
-                        minSleepTime = 0.0f;
-                    }
-
-                    if ((b.Flags & BodyFlags.AutoSleep) == 0 ||
-                        b.AngularVelocityInternal * b.AngularVelocityInternal > AngTolSqr ||
-                        Vector2.Dot(b.LinearVelocityInternal, b.LinearVelocityInternal) > LinTolSqr)
-                    {
-                        b.SleepTime = 0.0f;
-                        minSleepTime = 0.0f;
-                    }
-                    else
-                    {
-                        b.SleepTime += step.dt;
-                        minSleepTime = Math.Min(minSleepTime, b.SleepTime);
-                    }
-                }
-
-                if (minSleepTime >= Settings.TimeToSleep)
-                {
-                    for (int i = 0; i < BodyCount; ++i)
-                    {
-                        Body b = Bodies[i];
-                        b.Awake = false;
-                    }
-                }
-            }
-        }
-
-        internal void SolveTOI(ref TimeStep subStep)
-        {
-            _contactSolver.Reset(_contacts, ContactCount, subStep.dtRatio, false);
-
-            // Solve position constraints.
-            const float kTOIBaumgarte = 0.75f;
-            for (int i = 0; i < Settings.TOIPositionIterations; ++i)
-            {
-                bool contactsOkay = _contactSolver.SolvePositionConstraints(kTOIBaumgarte);
-                if (contactsOkay)
-                {
-                    break;
-                }
-
-                if (i == Settings.TOIPositionIterations - 1)
-                {
-                    i += 0;
-                }
-            }
-
-            // Leap of faith to new safe state.
-            for (int i = 0; i < BodyCount; ++i)
-            {
-                Body body = Bodies[i];
-                body.Sweep.A0 = body.Sweep.A;
-                body.Sweep.C0 = body.Sweep.C;
-            }
-
-            // No warm starting is needed for TOI events because warm
-            // starting impulses were applied in the discrete solver.
-            _contactSolver.InitializeVelocityConstraints();
-
-            // Solve velocity constraints.
-            for (int i = 0; i < Settings.TOIVelocityIterations; ++i)
-            {
-                _contactSolver.SolveVelocityConstraints();
-            }
-
-            // Don't store the TOI contact forces for warm starting
-            // because they can be quite large.
-
-            // Integrate positions.
-            for (int i = 0; i < BodyCount; ++i)
-            {
-                Body b = Bodies[i];
-
-                if (b.BodyType == BodyType.Static)
-                {
-                    continue;
-                }
-
-                // Check for large velocities.
-                float translationx = subStep.dt * b.LinearVelocityInternal.X;
-                float translationy = subStep.dt * b.LinearVelocityInternal.Y;
-                float dot = translationx * translationx + translationy * translationy;
-                if (dot > Settings.MaxTranslationSquared)
-                {
-                    float norm = 1f / (float)Math.Sqrt(dot);
-                    float value = Settings.MaxTranslation * subStep.inv_dt;
-                    b.LinearVelocityInternal.X = value * (translationx * norm);
-                    b.LinearVelocityInternal.Y = value * (translationy * norm);
-                }
-
-                float rotation = subStep.dt * b.AngularVelocity;
-                if (rotation * rotation > Settings.MaxRotationSquared)
-                {
-                    if (rotation < 0.0)
-                    {
-                        b.AngularVelocityInternal = -subStep.inv_dt * Settings.MaxRotation;
-                    }
-                    else
-                    {
-                        b.AngularVelocityInternal = subStep.inv_dt * Settings.MaxRotation;
-                    }
-                }
-
-                // Integrate
-                b.Sweep.C.X += subStep.dt * b.LinearVelocityInternal.X;
-                b.Sweep.C.Y += subStep.dt * b.LinearVelocityInternal.Y;
-                b.Sweep.A += subStep.dt * b.AngularVelocityInternal;
-
-                // Compute new transform
-                b.SynchronizeTransform();
-
-                // Note: shapes are synchronized later.
-            }
-
-            Report(_contactSolver.Constraints);
-        }
-
-        public void Add(Body body)
-        {
-            Debug.Assert(BodyCount < _bodyCapacity);
-            Bodies[BodyCount++] = body;
-        }
-
-        public void Add(Contact contact)
-        {
-            Debug.Assert(ContactCount < _contactCapacity);
-            _contacts[ContactCount++] = contact;
-        }
-
-        public void Add(Joint joint)
-        {
-            Debug.Assert(JointCount < _jointCapacity);
-            _joints[JointCount++] = joint;
-        }
-
-        private void Report(ContactConstraint[] constraints)
-        {
-            if (_contactManager == null)
-                return;
-
-            for (int i = 0; i < ContactCount; ++i)
-            {
-                Contact c = _contacts[i];
-
-                if (c.FixtureA.AfterCollision != null)
-                    c.FixtureA.AfterCollision(c.FixtureA, c.FixtureB, c);
-
-                if (c.FixtureB.AfterCollision != null)
-                    c.FixtureB.AfterCollision(c.FixtureB, c.FixtureA, c);
-
-                if (_contactManager.PostSolve != null)
-                {
-                    ContactConstraint cc = constraints[i];
-
-                    _contactManager.PostSolve(c, cc);
-                }
-            }
-        }
-    }
-}

FarseerPhysicsEngine/Dynamics/Joints/AngleJoint.cs

@@ -1,93 +0,0 @@
-using System;
-using System.Diagnostics;
-using Microsoft.Xna.Framework;
-
-namespace FarseerPhysics.Dynamics.Joints
-{
-    /// <summary>
-    /// Maintains a fixed angle between two bodies
-    /// </summary>
-    public class AngleJoint : Joint
-    {
-        public float BiasFactor;
-        public float MaxImpulse;
-        public float Softness;
-        private float _bias;
-        private float _jointError;
-        private float _massFactor;
-        private float _targetAngle;
-
-        internal AngleJoint()
-        {
-            JointType = JointType.Angle;
-        }
-
-        public AngleJoint(Body bodyA, Body bodyB)
-            : base(bodyA, bodyB)
-        {
-            JointType = JointType.Angle;
-            TargetAngle = 0;
-            BiasFactor = .2f;
-            Softness = 0f;
-            MaxImpulse = float.MaxValue;
-        }
-
-        public float TargetAngle
-        {
-            get { return _targetAngle; }
-            set
-            {
-                if (value != _targetAngle)
-                {
-                    _targetAngle = value;
-                    WakeBodies();
-                }
-            }
-        }
-
-        public override Vector2 WorldAnchorA
-        {
-            get { return BodyA.Position; }
-        }
-
-        public override Vector2 WorldAnchorB
-        {
-            get { return BodyB.Position; }
-            set { Debug.Assert(false, "You can't set the world anchor on this joint type."); }
-        }
-
-        public override Vector2 GetReactionForce(float inv_dt)
-        {
-            //TODO
-            //return _inv_dt * _impulse;
-            return Vector2.Zero;
-        }
-
-        public override float GetReactionTorque(float inv_dt)
-        {
-            return 0;
-        }
-
-        internal override void InitVelocityConstraints(ref TimeStep step)
-        {
-            _jointError = (BodyB.Sweep.A - BodyA.Sweep.A - TargetAngle);
-
-            _bias = -BiasFactor * step.inv_dt * _jointError;
-
-            _massFactor = (1 - Softness) / (BodyA.InvI + BodyB.InvI);
-        }
-
-        internal override void SolveVelocityConstraints(ref TimeStep step)
-        {
-            float p = (_bias - BodyB.AngularVelocity + BodyA.AngularVelocity) * _massFactor;
-            BodyA.AngularVelocity -= BodyA.InvI * Math.Sign(p) * Math.Min(Math.Abs(p), MaxImpulse);
-            BodyB.AngularVelocity += BodyB.InvI * Math.Sign(p) * Math.Min(Math.Abs(p), MaxImpulse);
-        }
-
-        internal override bool SolvePositionConstraints()
-        {
-            //no position solving for this joint
-            return true;
-        }
-    }
-}

FarseerPhysicsEngine/Dynamics/Joints/DistanceJoint.cs

@@ -1,286 +0,0 @@
-/*
-* Farseer Physics Engine based on Box2D.XNA port:
-* Copyright (c) 2010 Ian Qvist
-* 
-* Box2D.XNA port of Box2D:
-* Copyright (c) 2009 Brandon Furtwangler, Nathan Furtwangler
-*
-* Original source Box2D:
-* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com 
-* 
-* This software is provided 'as-is', without any express or implied 
-* warranty.  In no event will the authors be held liable for any damages 
-* arising from the use of this software. 
-* Permission is granted to anyone to use this software for any purpose, 
-* including commercial applications, and to alter it and redistribute it 
-* freely, subject to the following restrictions: 
-* 1. The origin of this software must not be misrepresented; you must not 
-* claim that you wrote the original software. If you use this software 
-* in a product, an acknowledgment in the product documentation would be 
-* appreciated but is not required. 
-* 2. Altered source versions must be plainly marked as such, and must not be 
-* misrepresented as being the original software. 
-* 3. This notice may not be removed or altered from any source distribution. 
-*/
-
-using System;
-using System.Diagnostics;
-using FarseerPhysics.Common;
-using Microsoft.Xna.Framework;
-
-namespace FarseerPhysics.Dynamics.Joints
-{
-    // 1-D rained system
-    // m (v2 - v1) = lambda
-    // v2 + (beta/h) * x1 + gamma * lambda = 0, gamma has units of inverse mass.
-    // x2 = x1 + h * v2
-
-    // 1-D mass-damper-spring system
-    // m (v2 - v1) + h * d * v2 + h * k * 
-
-    // C = norm(p2 - p1) - L
-    // u = (p2 - p1) / norm(p2 - p1)
-    // Cdot = dot(u, v2 + cross(w2, r2) - v1 - cross(w1, r1))
-    // J = [-u -cross(r1, u) u cross(r2, u)]
-    // K = J * invM * JT
-    //   = invMass1 + invI1 * cross(r1, u)^2 + invMass2 + invI2 * cross(r2, u)^2
-
-    /// <summary>
-    /// A distance joint rains two points on two bodies
-    /// to remain at a fixed distance from each other. You can view
-    /// this as a massless, rigid rod.
-    /// </summary>
-    public class DistanceJoint : Joint
-    {
-        /// <summary>
-        /// The local anchor point relative to bodyA's origin.
-        /// </summary>
-        public Vector2 LocalAnchorA;
-
-        /// <summary>
-        /// The local anchor point relative to bodyB's origin.
-        /// </summary>
-        public Vector2 LocalAnchorB;
-
-        private float _bias;
-        private float _gamma;
-        private float _impulse;
-        private float _mass;
-        private float _tmpFloat1;
-        private Vector2 _tmpVector1;
-        private Vector2 _u;
-
-        internal DistanceJoint()
-        {
-            JointType = JointType.Distance;
-        }
-
-        /// <summary>
-        /// This requires defining an
-        /// anchor point on both bodies and the non-zero length of the
-        /// distance joint. If you don't supply a length, the local anchor points
-        /// is used so that the initial configuration can violate the constraint
-        /// slightly. This helps when saving and loading a game.
-        /// @warning Do not use a zero or short length.
-        /// </summary>
-        /// <param name="bodyA">The first body</param>
-        /// <param name="bodyB">The second body</param>
-        /// <param name="localAnchorA">The first body anchor</param>
-        /// <param name="localAnchorB">The second body anchor</param>
-        public DistanceJoint(Body bodyA, Body bodyB, Vector2 localAnchorA, Vector2 localAnchorB)
-            : base(bodyA, bodyB)
-        {
-            JointType = JointType.Distance;
-
-            LocalAnchorA = localAnchorA;
-            LocalAnchorB = localAnchorB;
-
-            Vector2 d = WorldAnchorB - WorldAnchorA;
-            Length = d.Length();
-        }
-
-        /// <summary>
-        /// The natural length between the anchor points.
-        /// Manipulating the length can lead to non-physical behavior when the frequency is zero.
-        /// </summary>
-        public float Length { get; set; }
-
-        /// <summary>
-        /// The mass-spring-damper frequency in Hertz.
-        /// </summary>
-        public float Frequency { get; set; }
-
-        /// <summary>
-        /// The damping ratio. 0 = no damping, 1 = critical damping.
-        /// </summary>
-        public float DampingRatio { get; set; }
-
-        public override sealed Vector2 WorldAnchorA
-        {
-            get { return BodyA.GetWorldPoint(LocalAnchorA); }
-        }
-
-        public override sealed Vector2 WorldAnchorB
-        {
-            get { return BodyB.GetWorldPoint(LocalAnchorB); }
-            set { Debug.Assert(false, "You can't set the world anchor on this joint type."); }
-        }
-
-        public override Vector2 GetReactionForce(float inv_dt)
-        {
-            Vector2 F = (inv_dt * _impulse) * _u;
-            return F;
-        }
-
-        public override float GetReactionTorque(float inv_dt)
-        {
-            return 0.0f;
-        }
-
-        internal override void InitVelocityConstraints(ref TimeStep step)
-        {
-            Body b1 = BodyA;
-            Body b2 = BodyB;
-
-            // Compute the effective mass matrix.
-            Vector2 r1 = MathUtils.Multiply(ref b1.Xf.R, LocalAnchorA - b1.LocalCenter);
-            Vector2 r2 = MathUtils.Multiply(ref b2.Xf.R, LocalAnchorB - b2.LocalCenter);
-            _u = b2.Sweep.C + r2 - b1.Sweep.C - r1;
-
-            // Handle singularity.
-            float length = _u.Length();
-            if (length > Settings.LinearSlop)
-            {
-                _u *= 1.0f / length;
-            }
-            else
-            {
-                _u = Vector2.Zero;
-            }
-
-            float cr1u, cr2u;
-            MathUtils.Cross(ref r1, ref _u, out cr1u);
-            MathUtils.Cross(ref r2, ref _u, out cr2u);
-            float invMass = b1.InvMass + b1.InvI * cr1u * cr1u + b2.InvMass + b2.InvI * cr2u * cr2u;
-            Debug.Assert(invMass > Settings.Epsilon);
-            _mass = invMass != 0.0f ? 1.0f / invMass : 0.0f;
-
-            if (Frequency > 0.0f)
-            {
-                float C = length - Length;
-
-                // Frequency
-                float omega = 2.0f * Settings.Pi * Frequency;
-
-                // Damping coefficient
-                float d = 2.0f * _mass * DampingRatio * omega;
-
-                // Spring stiffness
-                float k = _mass * omega * omega;
-
-                // magic formulas
-                _gamma = step.dt * (d + step.dt * k);
-                _gamma = _gamma != 0.0f ? 1.0f / _gamma : 0.0f;
-                _bias = C * step.dt * k * _gamma;
-
-                _mass = invMass + _gamma;
-                _mass = _mass != 0.0f ? 1.0f / _mass : 0.0f;
-            }
-
-            if (Settings.EnableWarmstarting)
-            {
-                // Scale the impulse to support a variable time step.
-                _impulse *= step.dtRatio;
-
-                Vector2 P = _impulse * _u;
-                b1.LinearVelocityInternal -= b1.InvMass * P;
-                MathUtils.Cross(ref r1, ref P, out _tmpFloat1);
-                b1.AngularVelocityInternal -= b1.InvI * /* r1 x P */ _tmpFloat1;
-                b2.LinearVelocityInternal += b2.InvMass * P;
-                MathUtils.Cross(ref r2, ref P, out _tmpFloat1);
-                b2.AngularVelocityInternal += b2.InvI * /* r2 x P */ _tmpFloat1;
-            }
-            else
-            {
-                _impulse = 0.0f;
-            }
-        }
-
-        internal override void SolveVelocityConstraints(ref TimeStep step)
-        {
-            Body b1 = BodyA;
-            Body b2 = BodyB;
-
-            Transform xf1, xf2;
-            b1.GetTransform(out xf1);
-            b2.GetTransform(out xf2);
-
-            Vector2 r1 = MathUtils.Multiply(ref xf1.R, LocalAnchorA - b1.LocalCenter);
-            Vector2 r2 = MathUtils.Multiply(ref xf2.R, LocalAnchorB - b2.LocalCenter);
-
-            // Cdot = dot(u, v + cross(w, r))
-            MathUtils.Cross(b1.AngularVelocityInternal, ref r1, out _tmpVector1);
-            Vector2 v1 = b1.LinearVelocityInternal + _tmpVector1;
-            MathUtils.Cross(b2.AngularVelocityInternal, ref r2, out _tmpVector1);
-            Vector2 v2 = b2.LinearVelocityInternal + _tmpVector1;
-            float Cdot = Vector2.Dot(_u, v2 - v1);
-
-            float impulse = -_mass * (Cdot + _bias + _gamma * _impulse);
-            _impulse += impulse;
-
-            Vector2 P = impulse * _u;
-            b1.LinearVelocityInternal -= b1.InvMass * P;
-            MathUtils.Cross(ref r1, ref P, out _tmpFloat1);
-            b1.AngularVelocityInternal -= b1.InvI * _tmpFloat1;
-            b2.LinearVelocityInternal += b2.InvMass * P;
-            MathUtils.Cross(ref r2, ref P, out _tmpFloat1);
-            b2.AngularVelocityInternal += b2.InvI * _tmpFloat1;
-        }
-
-        internal override bool SolvePositionConstraints()
-        {
-            if (Frequency > 0.0f)
-            {
-                // There is no position correction for soft distance constraints.
-                return true;
-            }
-
-            Body b1 = BodyA;
-            Body b2 = BodyB;
-
-            Transform xf1, xf2;
-            b1.GetTransform(out xf1);
-            b2.GetTransform(out xf2);
-
-            Vector2 r1 = MathUtils.Multiply(ref xf1.R, LocalAnchorA - b1.LocalCenter);
-            Vector2 r2 = MathUtils.Multiply(ref xf2.R, LocalAnchorB - b2.LocalCenter);
-
-            Vector2 d = b2.Sweep.C + r2 - b1.Sweep.C - r1;
-
-            float length = d.Length();
-
-            if (length == 0.0f)
-                return true;
-
-            d /= length;
-            float C = length - Length;
-            C = MathUtils.Clamp(C, -Settings.MaxLinearCorrection, Settings.MaxLinearCorrection);
-
-            float impulse = -_mass * C;
-            _u = d;
-            Vector2 P = impulse * _u;
-
-            b1.Sweep.C -= b1.InvMass * P;
-            MathUtils.Cross(ref r1, ref P, out _tmpFloat1);
-            b1.Sweep.A -= b1.InvI * _tmpFloat1;
-            b2.Sweep.C += b2.InvMass * P;
-            MathUtils.Cross(ref r2, ref P, out _tmpFloat1);
-            b2.Sweep.A += b2.InvI * _tmpFloat1;
-
-            b1.SynchronizeTransform();
-            b2.SynchronizeTransform();
-
-            return Math.Abs(C) < Settings.LinearSlop;
-        }
-    }
-}

FarseerPhysicsEngine/Dynamics/Joints/FixedAngleJoint.cs

@@ -1,84 +0,0 @@
-using System;
-using System.Diagnostics;
-using Microsoft.Xna.Framework;
-
-namespace FarseerPhysics.Dynamics.Joints
-{
-    public class FixedAngleJoint : Joint
-    {
-        public float BiasFactor;
-        public float MaxImpulse;
-        public float Softness;
-        private float _bias;
-        private float _jointError;
-        private float _massFactor;
-        private float _targetAngle;
-
-        public FixedAngleJoint(Body bodyA)
-            : base(bodyA)
-        {
-            JointType = JointType.FixedAngle;
-            TargetAngle = 0;
-            BiasFactor = .2f;
-            Softness = 0f;
-            MaxImpulse = float.MaxValue;
-        }
-
-        public float TargetAngle
-        {
-            get { return _targetAngle; }
-            set
-            {
-                if (value != _targetAngle)
-                {
-                    _targetAngle = value;
-                    WakeBodies();
-                }
-            }
-        }
-
-        public override Vector2 WorldAnchorA
-        {
-            get { return BodyA.Position; }
-        }
-
-        public override Vector2 WorldAnchorB
-        {
-            get { return BodyA.Position; }
-            set { Debug.Assert(false, "You can't set the world anchor on this joint type."); }
-        }
-
-        public override Vector2 GetReactionForce(float inv_dt)
-        {
-            //TODO
-            //return _inv_dt * _impulse;
-            return Vector2.Zero;
-        }
-
-        public override float GetReactionTorque(float inv_dt)
-        {
-            return 0;
-        }
-
-        internal override void InitVelocityConstraints(ref TimeStep step)
-        {
-            _jointError = BodyA.Sweep.A - TargetAngle;
-
-            _bias = -BiasFactor * step.inv_dt * _jointError;
-
-            _massFactor = (1 - Softness) / (BodyA.InvI);
-        }
-
-        internal override void SolveVelocityConstraints(ref TimeStep step)
-        {
-            float p = (_bias - BodyA.AngularVelocity) * _massFactor;
-            BodyA.AngularVelocity += BodyA.InvI * Math.Sign(p) * Math.Min(Math.Abs(p), MaxImpulse);
-        }
-
-        internal override bool SolvePositionConstraints()
-        {
-            //no position solving for this joint
-            return true;
-        }
-    }
-}

FarseerPhysicsEngine/Dynamics/Joints/FixedDistanceJoint.cs

@@ -1,255 +0,0 @@
-/*
-* Farseer Physics Engine based on Box2D.XNA port:
-* Copyright (c) 2010 Ian Qvist
-* 
-* Box2D.XNA port of Box2D:
-* Copyright (c) 2009 Brandon Furtwangler, Nathan Furtwangler
-*
-* Original source Box2D:
-* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com 
-* 
-* This software is provided 'as-is', without any express or implied 
-* warranty.  In no event will the authors be held liable for any damages 
-* arising from the use of this software. 
-* Permission is granted to anyone to use this software for any purpose, 
-* including commercial applications, and to alter it and redistribute it 
-* freely, subject to the following restrictions: 
-* 1. The origin of this software must not be misrepresented; you must not 
-* claim that you wrote the original software. If you use this software 
-* in a product, an acknowledgment in the product documentation would be 
-* appreciated but is not required. 
-* 2. Altered source versions must be plainly marked as such, and must not be 
-* misrepresented as being the original software. 
-* 3. This notice may not be removed or altered from any source distribution. 
-*/
-
-using System;
-using System.Diagnostics;
-using FarseerPhysics.Common;
-using Microsoft.Xna.Framework;
-
-namespace FarseerPhysics.Dynamics.Joints
-{
-    // 1-D rained system
-    // m (v2 - v1) = lambda
-    // v2 + (beta/h) * x1 + gamma * lambda = 0, gamma has units of inverse mass.
-    // x2 = x1 + h * v2
-
-    // 1-D mass-damper-spring system
-    // m (v2 - v1) + h * d * v2 + h * k * 
-
-    // C = norm(p2 - p1) - L
-    // u = (p2 - p1) / norm(p2 - p1)
-    // Cdot = dot(u, v2 + cross(w2, r2) - v1 - cross(w1, r1))
-    // J = [-u -cross(r1, u) u cross(r2, u)]
-    // K = J * invM * JT
-    //   = invMass1 + invI1 * cross(r1, u)^2 + invMass2 + invI2 * cross(r2, u)^2
-
-    /// <summary>
-    /// A distance joint rains two points on two bodies
-    /// to remain at a fixed distance from each other. You can view
-    /// this as a massless, rigid rod.
-    /// </summary>
-    public class FixedDistanceJoint : Joint
-    {
-        /// <summary>
-        /// The local anchor point relative to bodyA's origin.
-        /// </summary>
-        public Vector2 LocalAnchorA;
-
-        private float _bias;
-        private float _gamma;
-        private float _impulse;
-        private float _mass;
-        private Vector2 _u;
-        private Vector2 _worldAnchorB;
-
-        /// <summary>
-        /// This requires defining an
-        /// anchor point on both bodies and the non-zero length of the
-        /// distance joint. If you don't supply a length, the local anchor points
-        /// is used so that the initial configuration can violate the constraint
-        /// slightly. This helps when saving and loading a game.
-        /// @warning Do not use a zero or short length.
-        /// </summary>
-        /// <param name="body">The body.</param>
-        /// <param name="bodyAnchor">The body anchor.</param>
-        /// <param name="worldAnchor">The world anchor.</param>
-        public FixedDistanceJoint(Body body, Vector2 bodyAnchor, Vector2 worldAnchor)
-            : base(body)
-        {
-            JointType = JointType.FixedDistance;
-
-            LocalAnchorA = bodyAnchor;
-            _worldAnchorB = worldAnchor;
-
-            //Calculate the length
-            Vector2 d = WorldAnchorB - WorldAnchorA;
-            Length = d.Length();
-        }
-
-        /// <summary>
-        /// The natural length between the anchor points.
-        /// Manipulating the length can lead to non-physical behavior when the frequency is zero.
-        /// </summary>
-        public float Length { get; set; }
-
-        /// <summary>
-        /// The mass-spring-damper frequency in Hertz.
-        /// </summary>
-        public float Frequency { get; set; }
-
-        /// <summary>
-        /// The damping ratio. 0 = no damping, 1 = critical damping.
-        /// </summary>
-        public float DampingRatio { get; set; }
-
-        public override sealed Vector2 WorldAnchorA
-        {
-            get { return BodyA.GetWorldPoint(LocalAnchorA); }
-        }
-
-        public override sealed Vector2 WorldAnchorB
-        {
-            get { return _worldAnchorB; }
-            set { _worldAnchorB = value; }
-        }
-
-        public override Vector2 GetReactionForce(float invDt)
-        {
-            return (invDt * _impulse) * _u;
-        }
-
-        public override float GetReactionTorque(float invDt)
-        {
-            return 0.0f;
-        }
-
-        internal override void InitVelocityConstraints(ref TimeStep step)
-        {
-            Body b1 = BodyA;
-
-            Transform xf1;
-            b1.GetTransform(out xf1);
-
-            // Compute the effective mass matrix.
-            Vector2 r1 = MathUtils.Multiply(ref xf1.R, LocalAnchorA - b1.LocalCenter);
-            Vector2 r2 = _worldAnchorB;
-            _u = r2 - b1.Sweep.C - r1;
-
-            // Handle singularity.
-            float length = _u.Length();
-            if (length > Settings.LinearSlop)
-            {
-                _u *= 1.0f / length;
-            }
-            else
-            {
-                _u = Vector2.Zero;
-            }
-
-            float cr1u = MathUtils.Cross(r1, _u);
-            float cr2u = MathUtils.Cross(r2, _u);
-            float invMass = b1.InvMass + b1.InvI * cr1u * cr1u + 0 * cr2u * cr2u;
-            Debug.Assert(invMass > Settings.Epsilon);
-            _mass = invMass != 0.0f ? 1.0f / invMass : 0.0f;
-
-            if (Frequency > 0.0f)
-            {
-                float C = length - Length;
-
-                // Frequency
-                float omega = 2.0f * Settings.Pi * Frequency;
-
-                // Damping coefficient
-                float d = 2.0f * _mass * DampingRatio * omega;
-
-                // Spring stiffness
-                float k = _mass * omega * omega;
-
-                // magic formulas
-                _gamma = step.dt * (d + step.dt * k);
-                _gamma = _gamma != 0.0f ? 1.0f / _gamma : 0.0f;
-                _bias = C * step.dt * k * _gamma;
-
-                _mass = invMass + _gamma;
-                _mass = _mass != 0.0f ? 1.0f / _mass : 0.0f;
-            }
-
-            if (Settings.EnableWarmstarting)
-            {
-                // Scale the impulse to support a variable time step.
-                _impulse *= step.dtRatio;
-
-                Vector2 P = _impulse * _u;
-                b1.LinearVelocityInternal -= b1.InvMass * P;
-                b1.AngularVelocityInternal -= b1.InvI * MathUtils.Cross(r1, P);
-            }
-            else
-            {
-                _impulse = 0.0f;
-            }
-        }
-
-        internal override void SolveVelocityConstraints(ref TimeStep step)
-        {
-            Body b1 = BodyA;
-
-            Transform xf1;
-            b1.GetTransform(out xf1);
-
-            Vector2 r1 = MathUtils.Multiply(ref xf1.R, LocalAnchorA - b1.LocalCenter);
-
-            // Cdot = dot(u, v + cross(w, r))
-            Vector2 v1 = b1.LinearVelocityInternal + MathUtils.Cross(b1.AngularVelocityInternal, r1);
-            Vector2 v2 = Vector2.Zero;
-            float Cdot = Vector2.Dot(_u, v2 - v1);
-
-            float impulse = -_mass * (Cdot + _bias + _gamma * _impulse);
-            _impulse += impulse;
-
-            Vector2 P = impulse * _u;
-            b1.LinearVelocityInternal -= b1.InvMass * P;
-            b1.AngularVelocityInternal -= b1.InvI * MathUtils.Cross(r1, P);
-        }
-
-        internal override bool SolvePositionConstraints()
-        {
-            if (Frequency > 0.0f)
-            {
-                // There is no position correction for soft distance constraints.
-                return true;
-            }
-
-            Body b1 = BodyA;
-
-            Transform xf1;
-            b1.GetTransform(out xf1);
-
-            Vector2 r1 = MathUtils.Multiply(ref xf1.R, LocalAnchorA - b1.LocalCenter);
-            Vector2 r2 = _worldAnchorB;
-
-            Vector2 d = r2 - b1.Sweep.C - r1;
-
-            float length = d.Length();
-
-            if (length == 0.0f)
-                return true;
-
-            d /= length;
-            float C = length - Length;
-            C = MathUtils.Clamp(C, -Settings.MaxLinearCorrection, Settings.MaxLinearCorrection);
-
-            float impulse = -_mass * C;
-            _u = d;
-            Vector2 P = impulse * _u;
-
-            b1.Sweep.C -= b1.InvMass * P;
-            b1.Sweep.A -= b1.InvI * MathUtils.Cross(r1, P);
-
-            b1.SynchronizeTransform();
-
-            return Math.Abs(C) < Settings.LinearSlop;
-        }
-    }
-}

FarseerPhysicsEngine/Dynamics/Joints/FixedFrictionJoint.cs

@@ -1,227 +0,0 @@
-/*
-* Farseer Physics Engine based on Box2D.XNA port:
-* Copyright (c) 2010 Ian Qvist
-* 
-* Box2D.XNA port of Box2D:
-* Copyright (c) 2009 Brandon Furtwangler, Nathan Furtwangler
-*
-* Original source Box2D:
-* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com 
-* 
-* This software is provided 'as-is', without any express or implied 
-* warranty.  In no event will the authors be held liable for any damages 
-* arising from the use of this software. 
-* Permission is granted to anyone to use this software for any purpose, 
-* including commercial applications, and to alter it and redistribute it 
-* freely, subject to the following restrictions: 
-* 1. The origin of this software must not be misrepresented; you must not 
-* claim that you wrote the original software. If you use this software 
-* in a product, an acknowledgment in the product documentation would be 
-* appreciated but is not required. 
-* 2. Altered source versions must be plainly marked as such, and must not be 
-* misrepresented as being the original software. 
-* 3. This notice may not be removed or altered from any source distribution. 
-*/
-
-using System;
-using System.Diagnostics;
-using FarseerPhysics.Common;
-using Microsoft.Xna.Framework;
-
-namespace FarseerPhysics.Dynamics.Joints
-{
-    // Point-to-point constraint
-    // Cdot = v2 - v1
-    //      = v2 + cross(w2, r2) - v1 - cross(w1, r1)
-    // J = [-I -r1_skew I r2_skew ]
-    // Identity used:
-    // w k % (rx i + ry j) = w * (-ry i + rx j)
-
-    // Angle constraint
-    // Cdot = w2 - w1
-    // J = [0 0 -1 0 0 1]
-    // K = invI1 + invI2
-
-    /// <summary>
-    /// Friction joint. This is used for top-down friction.
-    /// It provides 2D translational friction and angular friction.
-    /// </summary>
-    public class FixedFrictionJoint : Joint
-    {
-        public Vector2 LocalAnchorA;
-
-        /// <summary>
-        /// The maximum friction force in N.
-        /// </summary>
-        public float MaxForce;
-
-        /// <summary>
-        /// The maximum friction torque in N-m.
-        /// </summary>
-        public float MaxTorque;
-
-        private float _angularImpulse;
-        private float _angularMass;
-        private Vector2 _linearImpulse;
-        private Mat22 _linearMass;
-
-        public FixedFrictionJoint(Body body, Vector2 localAnchorA)
-            : base(body)
-        {
-            JointType = JointType.FixedFriction;
-            LocalAnchorA = localAnchorA;
-
-            //Setting default max force and max torque
-            const float gravity = 10.0f;
-
-            // For a circle: I = 0.5 * m * r * r ==> r = sqrt(2 * I / m)
-            float radius = (float)Math.Sqrt(2.0 * (body.Inertia / body.Mass));
-
-            MaxForce = body.Mass * gravity;
-            MaxTorque = body.Mass * radius * gravity;
-        }
-
-        public override Vector2 WorldAnchorA
-        {
-            get { return BodyA.GetWorldPoint(LocalAnchorA); }
-        }
-
-        public override Vector2 WorldAnchorB
-        {
-            get { return Vector2.Zero; }
-            set { Debug.Assert(false, "You can't set the world anchor on this joint type."); }
-        }
-
-        public override Vector2 GetReactionForce(float invDT)
-        {
-            return invDT * _linearImpulse;
-        }
-
-        public override float GetReactionTorque(float invDT)
-        {
-            return invDT * _angularImpulse;
-        }
-
-        internal override void InitVelocityConstraints(ref TimeStep step)
-        {
-            Body bA = BodyA;
-
-            Transform xfA;
-            bA.GetTransform(out xfA);
-
-            // Compute the effective mass matrix.
-            Vector2 rA = MathUtils.Multiply(ref xfA.R, LocalAnchorA - bA.LocalCenter);
-
-            // J = [-I -r1_skew I r2_skew]
-            //     [ 0       -1 0       1]
-            // r_skew = [-ry; rx]
-
-            // Matlab
-            // K = [ mA+r1y^2*iA+mB+r2y^2*iB,  -r1y*iA*r1x-r2y*iB*r2x,          -r1y*iA-r2y*iB]
-            //     [  -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB,           r1x*iA+r2x*iB]
-            //     [          -r1y*iA-r2y*iB,           r1x*iA+r2x*iB,                   iA+iB]
-
-            float mA = bA.InvMass;
-            float iA = bA.InvI;
-
-            Mat22 K1 = new Mat22();
-            K1.Col1.X = mA;
-            K1.Col2.X = 0.0f;
-            K1.Col1.Y = 0.0f;
-            K1.Col2.Y = mA;
-
-            Mat22 K2 = new Mat22();
-            K2.Col1.X = iA * rA.Y * rA.Y;
-            K2.Col2.X = -iA * rA.X * rA.Y;
-            K2.Col1.Y = -iA * rA.X * rA.Y;
-            K2.Col2.Y = iA * rA.X * rA.X;
-
-            Mat22 K12;
-            Mat22.Add(ref K1, ref K2, out K12);
-
-            _linearMass = K12.Inverse;
-
-            _angularMass = iA;
-            if (_angularMass > 0.0f)
-            {
-                _angularMass = 1.0f / _angularMass;
-            }
-
-            if (Settings.EnableWarmstarting)
-            {
-                // Scale impulses to support a variable time step.
-                _linearImpulse *= step.dtRatio;
-                _angularImpulse *= step.dtRatio;
-
-                Vector2 P = new Vector2(_linearImpulse.X, _linearImpulse.Y);
-
-                bA.LinearVelocityInternal -= mA * P;
-                bA.AngularVelocityInternal -= iA * (MathUtils.Cross(rA, P) + _angularImpulse);
-            }
-            else
-            {
-                _linearImpulse = Vector2.Zero;
-                _angularImpulse = 0.0f;
-            }
-        }
-
-        internal override void SolveVelocityConstraints(ref TimeStep step)
-        {
-            Body bA = BodyA;
-
-            Vector2 vA = bA.LinearVelocityInternal;
-            float wA = bA.AngularVelocityInternal;
-
-            float mA = bA.InvMass;
-            float iA = bA.InvI;
-
-            Transform xfA;
-            bA.GetTransform(out xfA);
-
-            Vector2 rA = MathUtils.Multiply(ref xfA.R, LocalAnchorA - bA.LocalCenter);
-
-            // Solve angular friction
-            {
-                float Cdot = -wA;
-                float impulse = -_angularMass * Cdot;
-
-                float oldImpulse = _angularImpulse;
-                float maxImpulse = step.dt * MaxTorque;
-                _angularImpulse = MathUtils.Clamp(_angularImpulse + impulse, -maxImpulse, maxImpulse);
-                impulse = _angularImpulse - oldImpulse;
-
-                wA -= iA * impulse;
-            }
-
-            // Solve linear friction
-            {
-                Vector2 Cdot = -vA - MathUtils.Cross(wA, rA);
-
-                Vector2 impulse = -MathUtils.Multiply(ref _linearMass, Cdot);
-                Vector2 oldImpulse = _linearImpulse;
-                _linearImpulse += impulse;
-
-                float maxImpulse = step.dt * MaxForce;
-
-                if (_linearImpulse.LengthSquared() > maxImpulse * maxImpulse)
-                {
-                    _linearImpulse.Normalize();
-                    _linearImpulse *= maxImpulse;
-                }
-
-                impulse = _linearImpulse - oldImpulse;
-
-                vA -= mA * impulse;
-                wA -= iA * MathUtils.Cross(rA, impulse);
-            }
-
-            bA.LinearVelocityInternal = vA;
-            bA.AngularVelocityInternal = wA;
-        }
-
-        internal override bool SolvePositionConstraints()
-        {
-            return true;
-        }
-    }
-}

FarseerPhysicsEngine/Dynamics/Joints/FixedLineJoint.cs

@@ -1,413 +0,0 @@
-/*
-* Farseer Physics Engine based on Box2D.XNA port:
-* Copyright (c) 2010 Ian Qvist
-* 
-* Box2D.XNA port of Box2D:
-* Copyright (c) 2009 Brandon Furtwangler, Nathan Furtwangler
-*
-* Original source Box2D:
-* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com 
-* 
-* This software is provided 'as-is', without any express or implied 
-* warranty.  In no event will the authors be held liable for any damages 
-* arising from the use of this software. 
-* Permission is granted to anyone to use this software for any purpose, 
-* including commercial applications, and to alter it and redistribute it 
-* freely, subject to the following restrictions: 
-* 1. The origin of this software must not be misrepresented; you must not 
-* claim that you wrote the original software. If you use this software 
-* in a product, an acknowledgment in the product documentation would be 
-* appreciated but is not required. 
-* 2. Altered source versions must be plainly marked as such, and must not be 
-* misrepresented as being the original software. 
-* 3. This notice may not be removed or altered from any source distribution. 
-*/
-
-using System;
-using System.Diagnostics;
-using FarseerPhysics.Common;
-using Microsoft.Xna.Framework;
-
-namespace FarseerPhysics.Dynamics.Joints
-{
-    public class FixedLineJoint : Joint
-    {
-        private Vector2 _ax, _ay;
-        private float _bias;
-        private bool _enableMotor;
-        private float _gamma;
-        private float _impulse;
-        private Vector2 _localXAxis;
-        private Vector2 _localYAxisA;
-        private float _mass;
-        private float _maxMotorTorque;
-        private float _motorImpulse;
-        private float _motorMass;
-        private float _motorSpeed;
-
-        private float _sAx;
-        private float _sAy;
-        private float _sBx;
-        private float _sBy;
-
-        private float _springImpulse;
-        private float _springMass;
-
-        // Linear constraint (point-to-line)
-        // d = pB - pA = xB + rB - xA - rA
-        // C = dot(ay, d)
-        // Cdot = dot(d, cross(wA, ay)) + dot(ay, vB + cross(wB, rB) - vA - cross(wA, rA))
-        //      = -dot(ay, vA) - dot(cross(d + rA, ay), wA) + dot(ay, vB) + dot(cross(rB, ay), vB)
-        // J = [-ay, -cross(d + rA, ay), ay, cross(rB, ay)]
-
-        // Spring linear constraint
-        // C = dot(ax, d)
-        // Cdot = = -dot(ax, vA) - dot(cross(d + rA, ax), wA) + dot(ax, vB) + dot(cross(rB, ax), vB)
-        // J = [-ax -cross(d+rA, ax) ax cross(rB, ax)]
-
-        // Motor rotational constraint
-        // Cdot = wB - wA
-        // J = [0 0 -1 0 0 1]
-
-        internal FixedLineJoint() { JointType = JointType.FixedLine; }
-
-        public FixedLineJoint(Body body, Vector2 worldAnchor, Vector2 axis)
-            : base(body)
-        {
-            JointType = JointType.FixedLine;
-
-            BodyB = BodyA;
-
-            LocalAnchorA = worldAnchor;
-            LocalAnchorB = BodyB.GetLocalPoint(worldAnchor);
-            LocalXAxis = axis;
-        }
-
-        public Vector2 LocalAnchorA { get; set; }
-
-        public Vector2 LocalAnchorB { get; set; }
-
-        public override Vector2 WorldAnchorA
-        {
-            get { return LocalAnchorA; }
-        }
-
-        public override Vector2 WorldAnchorB
-        {
-            get { return BodyA.GetWorldPoint(LocalAnchorB); }
-            set { Debug.Assert(false, "You can't set the world anchor on this joint type."); }
-        }
-
-        public float JointTranslation
-        {
-            get
-            {
-                Body bA = BodyA;
-                Body bB = BodyB;
-
-                Vector2 pA = bA.GetWorldPoint(LocalAnchorA);
-                Vector2 pB = bB.GetWorldPoint(LocalAnchorB);
-                Vector2 d = pB - pA;
-                Vector2 axis = bA.GetWorldVector(LocalXAxis);
-
-                float translation = Vector2.Dot(d, axis);
-                return translation;
-            }
-        }
-
-        public float JointSpeed
-        {
-            get
-            {
-                float wA = BodyA.AngularVelocityInternal;
-                float wB = BodyB.AngularVelocityInternal;
-                return wB - wA;
-            }
-        }
-
-        public bool MotorEnabled
-        {
-            get { return _enableMotor; }
-            set
-            {
-                BodyA.Awake = true;
-                BodyB.Awake = true;
-                _enableMotor = value;
-            }
-        }
-
-        public float MotorSpeed
-        {
-            set
-            {
-                BodyA.Awake = true;
-                BodyB.Awake = true;
-                _motorSpeed = value;
-            }
-            get { return _motorSpeed; }
-        }
-
-        public float MaxMotorTorque
-        {
-            set
-            {
-                BodyA.Awake = true;
-                BodyB.Awake = true;
-                _maxMotorTorque = value;
-            }
-            get { return _maxMotorTorque; }
-        }
-
-        public float Frequency { get; set; }
-
-        public float DampingRatio { get; set; }
-
-        public Vector2 LocalXAxis
-        {
-            get { return _localXAxis; }
-            set
-            {
-                _localXAxis = value;
-                _localYAxisA = MathUtils.Cross(1.0f, _localXAxis);
-            }
-        }
-
-        public override Vector2 GetReactionForce(float invDt)
-        {
-            return invDt * (_impulse * _ay + _springImpulse * _ax);
-        }
-
-        public override float GetReactionTorque(float invDt)
-        {
-            return invDt * _motorImpulse;
-        }
-
-        internal override void InitVelocityConstraints(ref TimeStep step)
-        {
-            Body bB = BodyB;
-
-            LocalCenterA = Vector2.Zero;
-            LocalCenterB = bB.LocalCenter;
-
-            Transform xfB;
-            bB.GetTransform(out xfB);
-
-            // Compute the effective masses.
-            Vector2 rA = LocalAnchorA;
-            Vector2 rB = MathUtils.Multiply(ref xfB.R, LocalAnchorB - LocalCenterB);
-            Vector2 d = bB.Sweep.C + rB - rA;
-
-            InvMassA = 0.0f;
-            InvIA = 0.0f;
-            InvMassB = bB.InvMass;
-            InvIB = bB.InvI;
-
-            // Point to line constraint
-            {
-                _ay = _localYAxisA;
-                _sAy = MathUtils.Cross(d + rA, _ay);
-                _sBy = MathUtils.Cross(rB, _ay);
-
-                _mass = InvMassA + InvMassB + InvIA * _sAy * _sAy + InvIB * _sBy * _sBy;
-
-                if (_mass > 0.0f)
-                {
-                    _mass = 1.0f / _mass;
-                }
-            }
-
-            // Spring constraint
-            _springMass = 0.0f;
-            if (Frequency > 0.0f)
-            {
-                _ax = LocalXAxis;
-                _sAx = MathUtils.Cross(d + rA, _ax);
-                _sBx = MathUtils.Cross(rB, _ax);
-
-                float invMass = InvMassA + InvMassB + InvIA * _sAx * _sAx + InvIB * _sBx * _sBx;
-
-                if (invMass > 0.0f)
-                {
-                    _springMass = 1.0f / invMass;
-
-                    float C = Vector2.Dot(d, _ax);
-
-                    // Frequency
-                    float omega = 2.0f * Settings.Pi * Frequency;
-
-                    // Damping coefficient
-                    float da = 2.0f * _springMass * DampingRatio * omega;
-
-                    // Spring stiffness
-                    float k = _springMass * omega * omega;
-
-                    // magic formulas
-                    _gamma = step.dt * (da + step.dt * k);
-                    if (_gamma > 0.0f)
-                    {
-                        _gamma = 1.0f / _gamma;
-                    }
-
-                    _bias = C * step.dt * k * _gamma;
-
-                    _springMass = invMass + _gamma;
-                    if (_springMass > 0.0f)
-                    {
-                        _springMass = 1.0f / _springMass;
-                    }
-                }
-            }
-            else
-            {
-                _springImpulse = 0.0f;
-                _springMass = 0.0f;
-            }
-
-            // Rotational motor
-            if (_enableMotor)
-            {
-                _motorMass = InvIA + InvIB;
-                if (_motorMass > 0.0f)
-                {
-                    _motorMass = 1.0f / _motorMass;
-                }
-            }
-            else
-            {
-                _motorMass = 0.0f;
-                _motorImpulse = 0.0f;
-            }
-
-            if (Settings.EnableWarmstarting)
-            {
-                // Account for variable time step.
-                _impulse *= step.dtRatio;
-                _springImpulse *= step.dtRatio;
-                _motorImpulse *= step.dtRatio;
-
-                Vector2 P = _impulse * _ay + _springImpulse * _ax;
-                float LB = _impulse * _sBy + _springImpulse * _sBx + _motorImpulse;
-
-                bB.LinearVelocityInternal += InvMassB * P;
-                bB.AngularVelocityInternal += InvIB * LB;
-            }
-            else
-            {
-                _impulse = 0.0f;
-                _springImpulse = 0.0f;
-                _motorImpulse = 0.0f;
-            }
-        }
-
-        internal override void SolveVelocityConstraints(ref TimeStep step)
-        {
-            Body bB = BodyB;
-
-            Vector2 vA = Vector2.Zero;
-            float wA = 0.0f;
-            Vector2 vB = bB.LinearVelocityInternal;
-            float wB = bB.AngularVelocityInternal;
-
-            // Solve spring constraint
-            {
-                float Cdot = Vector2.Dot(_ax, vB - vA) + _sBx * wB - _sAx * wA;
-                float impulse = -_springMass * (Cdot + _bias + _gamma * _springImpulse);
-                _springImpulse += impulse;
-
-                Vector2 P = impulse * _ax;
-                float LA = impulse * _sAx;
-                float LB = impulse * _sBx;
-
-                vA -= InvMassA * P;
-                wA -= InvIA * LA;
-
-                vB += InvMassB * P;
-                wB += InvIB * LB;
-            }
-
-            // Solve rotational motor constraint
-            {
-                float Cdot = wB - wA - _motorSpeed;
-                float impulse = -_motorMass * Cdot;
-
-                float oldImpulse = _motorImpulse;
-                float maxImpulse = step.dt * _maxMotorTorque;
-                _motorImpulse = MathUtils.Clamp(_motorImpulse + impulse, -maxImpulse, maxImpulse);
-                impulse = _motorImpulse - oldImpulse;
-
-                wA -= InvIA * impulse;
-                wB += InvIB * impulse;
-            }
-
-            // Solve point to line constraint
-            {
-                float Cdot = Vector2.Dot(_ay, vB - vA) + _sBy * wB - _sAy * wA;
-                float impulse = _mass * (-Cdot);
-                _impulse += impulse;
-
-                Vector2 P = impulse * _ay;
-                float LB = impulse * _sBy;
-
-                vB += InvMassB * P;
-                wB += InvIB * LB;
-            }
-
-            bB.LinearVelocityInternal = vB;
-            bB.AngularVelocityInternal = wB;
-        }
-
-        internal override bool SolvePositionConstraints()
-        {
-            Body bB = BodyB;
-
-            Vector2 xA = Vector2.Zero;
-            const float angleA = 0.0f;
-
-            Vector2 xB = bB.Sweep.C;
-            float angleB = bB.Sweep.A;
-
-            Mat22 RA = new Mat22(angleA);
-            Mat22 RB = new Mat22(angleB);
-
-            Vector2 rA = MathUtils.Multiply(ref RA, LocalAnchorA - LocalCenterA);
-            Vector2 rB = MathUtils.Multiply(ref RB, LocalAnchorB - LocalCenterB);
-            Vector2 d = xB + rB - xA - rA;
-
-            Vector2 ay = MathUtils.Multiply(ref RA, _localYAxisA);
-
-            float sBy = MathUtils.Cross(rB, ay);
-
-            float C = Vector2.Dot(d, ay);
-
-            float k = InvMassA + InvMassB + InvIA * _sAy * _sAy + InvIB * _sBy * _sBy;
-
-            float impulse;
-            if (k != 0.0f)
-            {
-                impulse = -C / k;
-            }
-            else
-            {
-                impulse = 0.0f;
-            }
-
-            Vector2 P = impulse * ay;
-            float LB = impulse * sBy;
-
-            xB += InvMassB * P;
-            angleB += InvIB * LB;
-
-            // TODO_ERIN remove need for this.
-            bB.Sweep.C = xB;
-            bB.Sweep.A = angleB;
-            bB.SynchronizeTransform();
-
-            return Math.Abs(C) <= Settings.LinearSlop;
-        }
-
-        public float GetMotorTorque(float invDt)
-        {
-            return invDt * _motorImpulse;
-        }
-    }
-}

FarseerPhysicsEngine/Dynamics/Joints/FixedMouseJoint.cs

@@ -1,209 +0,0 @@
-/*
-* Farseer Physics Engine based on Box2D.XNA port:
-* Copyright (c) 2010 Ian Qvist
-* 
-* Box2D.XNA port of Box2D:
-* Copyright (c) 2009 Brandon Furtwangler, Nathan Furtwangler
-*
-* Original source Box2D:
-* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com 
-* 
-* This software is provided 'as-is', without any express or implied 
-* warranty.  In no event will the authors be held liable for any damages 
-* arising from the use of this software. 
-* Permission is granted to anyone to use this software for any purpose, 
-* including commercial applications, and to alter it and redistribute it 
-* freely, subject to the following restrictions: 
-* 1. The origin of this software must not be misrepresented; you must not 
-* claim that you wrote the original software. If you use this software 
-* in a product, an acknowledgment in the product documentation would be 
-* appreciated but is not required. 
-* 2. Altered source versions must be plainly marked as such, and must not be 
-* misrepresented as being the original software. 
-* 3. This notice may not be removed or altered from any source distribution. 
-*/
-
-using System.Diagnostics;
-using FarseerPhysics.Common;
-using Microsoft.Xna.Framework;
-
-namespace FarseerPhysics.Dynamics.Joints
-{
-    /// <summary>
-    /// A mouse joint is used to make a point on a body track a
-    /// specified world point. This a soft constraint with a maximum
-    /// force. This allows the constraint to stretch and without
-    /// applying huge forces.
-    /// NOTE: this joint is not documented in the manual because it was
-    /// developed to be used in the testbed. If you want to learn how to
-    /// use the mouse joint, look at the testbed.
-    /// </summary>
-    public class FixedMouseJoint : Joint
-    {
-        public Vector2 LocalAnchorA;
-        private Vector2 _C; // position error
-        private float _beta;
-        private float _gamma;
-        private Vector2 _impulse;
-        private Mat22 _mass; // effective mass for point-to-point constraint.
-
-        private Vector2 _worldAnchor;
-
-        /// <summary>
-        /// This requires a world target point,
-        /// tuning parameters, and the time step.
-        /// </summary>
-        /// <param name="body">The body.</param>
-        /// <param name="worldAnchor">The target.</param>
-        public FixedMouseJoint(Body body, Vector2 worldAnchor)
-            : base(body)
-        {
-            JointType = JointType.FixedMouse;
-            Frequency = 5.0f;
-            DampingRatio = 0.7f;
-
-            Debug.Assert(worldAnchor.IsValid());
-
-            Transform xf1;
-            BodyA.GetTransform(out xf1);
-
-            _worldAnchor = worldAnchor;
-            LocalAnchorA = BodyA.GetLocalPoint(worldAnchor);
-        }
-
-        public override Vector2 WorldAnchorA
-        {
-            get { return BodyA.GetWorldPoint(LocalAnchorA); }
-        }
-
-        public override Vector2 WorldAnchorB
-        {
-            get { return _worldAnchor; }
-            set
-            {
-                BodyA.Awake = true;
-                _worldAnchor = value;
-            }
-        }
-
-        /// <summary>
-        /// The maximum constraint force that can be exerted
-        /// to move the candidate body. Usually you will express
-        /// as some multiple of the weight (multiplier * mass * gravity).
-        /// </summary>
-        public float MaxForce { get; set; }
-
-        /// <summary>
-        /// The response speed.
-        /// </summary>
-        public float Frequency { get; set; }
-
-        /// <summary>
-        /// The damping ratio. 0 = no damping, 1 = critical damping.
-        /// </summary>
-        public float DampingRatio { get; set; }
-
-        public override Vector2 GetReactionForce(float inv_dt)
-        {
-            return inv_dt * _impulse;
-        }
-
-        public override float GetReactionTorque(float inv_dt)
-        {
-            return inv_dt * 0.0f;
-        }
-
-        internal override void InitVelocityConstraints(ref TimeStep step)
-        {
-            Body b = BodyA;
-
-            float mass = b.Mass;
-
-            // Frequency
-            float omega = 2.0f * Settings.Pi * Frequency;
-
-            // Damping coefficient
-            float d = 2.0f * mass * DampingRatio * omega;
-
-            // Spring stiffness
-            float k = mass * (omega * omega);
-
-            // magic formulas
-            // gamma has units of inverse mass.
-            // beta has units of inverse time.
-            Debug.Assert(d + step.dt * k > Settings.Epsilon);
-
-            _gamma = step.dt * (d + step.dt * k);
-            if (_gamma != 0.0f)
-            {
-                _gamma = 1.0f / _gamma;
-            }
-
-            _beta = step.dt * k * _gamma;
-
-            // Compute the effective mass matrix.
-            Transform xf1;
-            b.GetTransform(out xf1);
-            Vector2 r = MathUtils.Multiply(ref xf1.R, LocalAnchorA - b.LocalCenter);
-
-            // K    = [(1/m1 + 1/m2) * eye(2) - skew(r1) * invI1 * skew(r1) - skew(r2) * invI2 * skew(r2)]
-            //      = [1/m1+1/m2     0    ] + invI1 * [r1.Y*r1.Y -r1.X*r1.Y] + invI2 * [r1.Y*r1.Y -r1.X*r1.Y]
-            //        [    0     1/m1+1/m2]           [-r1.X*r1.Y r1.X*r1.X]           [-r1.X*r1.Y r1.X*r1.X]
-            float invMass = b.InvMass;
-            float invI = b.InvI;
-
-            Mat22 K1 = new Mat22(new Vector2(invMass, 0.0f), new Vector2(0.0f, invMass));
-            Mat22 K2 = new Mat22(new Vector2(invI * r.Y * r.Y, -invI * r.X * r.Y),
-                                 new Vector2(-invI * r.X * r.Y, invI * r.X * r.X));
-
-            Mat22 K;
-            Mat22.Add(ref K1, ref K2, out K);
-
-            K.Col1.X += _gamma;
-            K.Col2.Y += _gamma;
-
-            _mass = K.Inverse;
-
-            _C = b.Sweep.C + r - _worldAnchor;
-
-            // Cheat with some damping
-            b.AngularVelocityInternal *= 0.98f;
-
-            // Warm starting.
-            _impulse *= step.dtRatio;
-            b.LinearVelocityInternal += invMass * _impulse;
-            b.AngularVelocityInternal += invI * MathUtils.Cross(r, _impulse);
-        }
-
-        internal override void SolveVelocityConstraints(ref TimeStep step)
-        {
-            Body b = BodyA;
-
-            Transform xf1;
-            b.GetTransform(out xf1);
-
-            Vector2 r = MathUtils.Multiply(ref xf1.R, LocalAnchorA - b.LocalCenter);
-
-            // Cdot = v + cross(w, r)
-            Vector2 Cdot = b.LinearVelocityInternal + MathUtils.Cross(b.AngularVelocityInternal, r);
-            Vector2 impulse = MathUtils.Multiply(ref _mass, -(Cdot + _beta * _C + _gamma * _impulse));
-
-            Vector2 oldImpulse = _impulse;
-            _impulse += impulse;
-            float maxImpulse = step.dt * MaxForce;
-            if (_impulse.LengthSquared() > maxImpulse * maxImpulse)
-            {
-                _impulse *= maxImpulse / _impulse.Length();
-            }
-            impulse = _impulse - oldImpulse;
-
-            b.LinearVelocityInternal += b.InvMass * impulse;
-            b.AngularVelocityInternal += b.InvI * MathUtils.Cross(r, impulse);
-        }
-
-        internal override bool SolvePositionConstraints()
-        {
-            return true;
-        }
-    }
-}

FarseerPhysicsEngine/Dynamics/Joints/FixedPrismaticJoint.cs

@@ -1,636 +0,0 @@
-/*
-* Farseer Physics Engine based on Box2D.XNA port:
-* Copyright (c) 2010 Ian Qvist
-* 
-* Box2D.XNA port of Box2D:
-* Copyright (c) 2009 Brandon Furtwangler, Nathan Furtwangler
-*
-* Original source Box2D:
-* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com 
-* 
-* This software is provided 'as-is', without any express or implied 
-* warranty.  In no event will the authors be held liable for any damages 
-* arising from the use of this software. 
-* Permission is granted to anyone to use this software for any purpose, 
-* including commercial applications, and to alter it and redistribute it 
-* freely, subject to the following restrictions: 
-* 1. The origin of this software must not be misrepresented; you must not 
-* claim that you wrote the original software. If you use this software 
-* in a product, an acknowledgment in the product documentation would be 
-* appreciated but is not required. 
-* 2. Altered source versions must be plainly marked as such, and must not be 
-* misrepresented as being the original software. 
-* 3. This notice may not be removed or altered from any source distribution. 
-*/
-
-using System;
-using System.Diagnostics;
-using FarseerPhysics.Common;
-using Microsoft.Xna.Framework;
-
-namespace FarseerPhysics.Dynamics.Joints
-{
-    // Linear constraint (point-to-line)
-    // d = p2 - p1 = x2 + r2 - x1 - r1
-    // C = dot(perp, d)
-    // Cdot = dot(d, cross(w1, perp)) + dot(perp, v2 + cross(w2, r2) - v1 - cross(w1, r1))
-    //      = -dot(perp, v1) - dot(cross(d + r1, perp), w1) + dot(perp, v2) + dot(cross(r2, perp), v2)
-    // J = [-perp, -cross(d + r1, perp), perp, cross(r2,perp)]
-    //
-    // Angular constraint
-    // C = a2 - a1 + a_initial
-    // Cdot = w2 - w1
-    // J = [0 0 -1 0 0 1]
-    //
-    // K = J * invM * JT
-    //
-    // J = [-a -s1 a s2]
-    //     [0  -1  0  1]
-    // a = perp
-    // s1 = cross(d + r1, a) = cross(p2 - x1, a)
-    // s2 = cross(r2, a) = cross(p2 - x2, a)
-    // Motor/Limit linear constraint
-    // C = dot(ax1, d)
-    // Cdot = = -dot(ax1, v1) - dot(cross(d + r1, ax1), w1) + dot(ax1, v2) + dot(cross(r2, ax1), v2)
-    // J = [-ax1 -cross(d+r1,ax1) ax1 cross(r2,ax1)]
-    // Block Solver
-    // We develop a block solver that includes the joint limit. This makes the limit stiff (inelastic) even
-    // when the mass has poor distribution (leading to large torques about the joint anchor points).
-    //
-    // The Jacobian has 3 rows:
-    // J = [-uT -s1 uT s2] // linear
-    //     [0   -1   0  1] // angular
-    //     [-vT -a1 vT a2] // limit
-    //
-    // u = perp
-    // v = axis
-    // s1 = cross(d + r1, u), s2 = cross(r2, u)
-    // a1 = cross(d + r1, v), a2 = cross(r2, v)
-    // M * (v2 - v1) = JT * df
-    // J * v2 = bias
-    //
-    // v2 = v1 + invM * JT * df
-    // J * (v1 + invM * JT * df) = bias
-    // K * df = bias - J * v1 = -Cdot
-    // K = J * invM * JT
-    // Cdot = J * v1 - bias
-    //
-    // Now solve for f2.
-    // df = f2 - f1
-    // K * (f2 - f1) = -Cdot
-    // f2 = invK * (-Cdot) + f1
-    //
-    // Clamp accumulated limit impulse.
-    // lower: f2(3) = max(f2(3), 0)
-    // upper: f2(3) = min(f2(3), 0)
-    //
-    // Solve for correct f2(1:2)
-    // K(1:2, 1:2) * f2(1:2) = -Cdot(1:2) - K(1:2,3) * f2(3) + K(1:2,1:3) * f1
-    //                       = -Cdot(1:2) - K(1:2,3) * f2(3) + K(1:2,1:2) * f1(1:2) + K(1:2,3) * f1(3)
-    // K(1:2, 1:2) * f2(1:2) = -Cdot(1:2) - K(1:2,3) * (f2(3) - f1(3)) + K(1:2,1:2) * f1(1:2)
-    // f2(1:2) = invK(1:2,1:2) * (-Cdot(1:2) - K(1:2,3) * (f2(3) - f1(3))) + f1(1:2)
-    //
-    // Now compute impulse to be applied:
-    // df = f2 - f1
-
-    /// <summary>
-    /// A prismatic joint. This joint provides one degree of freedom: translation
-    /// along an axis fixed in body1. Relative rotation is prevented. You can
-    /// use a joint limit to restrict the range of motion and a joint motor to
-    /// drive the motion or to model joint friction.
-    /// </summary>
-    public class FixedPrismaticJoint : Joint
-    {
-        private Mat33 _K;
-        private float _a1, _a2;
-        private Vector2 _axis;
-        private bool _enableLimit;
-        private bool _enableMotor;
-        private Vector3 _impulse;
-        private LimitState _limitState;
-        private Vector2 _localXAxis1;
-        private Vector2 _localYAxis1;
-        private float _lowerTranslation;
-        private float _maxMotorForce;
-        private float _motorMass; // effective mass for motor/limit translational constraint.
-        private float _motorSpeed;
-        private Vector2 _perp;
-        private float _refAngle;
-        private float _s1, _s2;
-        private float _upperTranslation;
-
-        /// <summary>
-        /// This requires defining a line of
-        /// motion using an axis and an anchor point. The definition uses local
-        /// anchor points and a local axis so that the initial configuration
-        /// can violate the constraint slightly. The joint translation is zero
-        /// when the local anchor points coincide in world space. Using local
-        /// anchors and a local axis helps when saving and loading a game.
-        /// </summary>
-        /// <param name="body">The body.</param>
-        /// <param name="worldAnchor">The anchor.</param>
-        /// <param name="axis">The axis.</param>
-        public FixedPrismaticJoint(Body body, Vector2 worldAnchor, Vector2 axis)
-            : base(body)
-        {
-            JointType = JointType.FixedPrismatic;
-
-            BodyB = BodyA;
-
-            LocalAnchorA = worldAnchor;
-            LocalAnchorB = BodyB.GetLocalPoint(worldAnchor);
-
-            _localXAxis1 = axis;
-            _localYAxis1 = MathUtils.Cross(1.0f, _localXAxis1);
-            _refAngle = BodyB.Rotation;
-
-            _limitState = LimitState.Inactive;
-        }
-
-        public Vector2 LocalAnchorA { get; set; }
-
-        public Vector2 LocalAnchorB { get; set; }
-
-        public override Vector2 WorldAnchorA
-        {
-            get { return LocalAnchorA; }
-        }
-
-        public override Vector2 WorldAnchorB
-        {
-            get { return BodyA.GetWorldPoint(LocalAnchorB); }
-            set { Debug.Assert(false, "You can't set the world anchor on this joint type."); }
-        }
-
-        /// <summary>
-        /// Get the current joint translation, usually in meters.
-        /// </summary>
-        /// <value></value>
-        public float JointTranslation
-        {
-            get
-            {
-                Vector2 d = BodyB.GetWorldPoint(LocalAnchorB) - LocalAnchorA;
-                Vector2 axis = _localXAxis1;
-
-                return Vector2.Dot(d, axis);
-            }
-        }
-
-        /// <summary>
-        /// Get the current joint translation speed, usually in meters per second.
-        /// </summary>
-        /// <value></value>
-        public float JointSpeed
-        {
-            get
-            {
-                Transform xf2;
-                BodyB.GetTransform(out xf2);
-
-                Vector2 r1 = LocalAnchorA;
-                Vector2 r2 = MathUtils.Multiply(ref xf2.R, LocalAnchorB - BodyB.LocalCenter);
-                Vector2 p1 = r1;
-                Vector2 p2 = BodyB.Sweep.C + r2;
-                Vector2 d = p2 - p1;
-                Vector2 axis = _localXAxis1;
-
-                Vector2 v1 = Vector2.Zero;
-                Vector2 v2 = BodyB.LinearVelocityInternal;
-                const float w1 = 0.0f;
-                float w2 = BodyB.AngularVelocityInternal;
-
-                float speed = Vector2.Dot(d, MathUtils.Cross(w1, axis)) +
-                              Vector2.Dot(axis, v2 + MathUtils.Cross(w2, r2) - v1 - MathUtils.Cross(w1, r1));
-                return speed;
-            }
-        }
-
-        /// <summary>
-        /// Is the joint limit enabled?
-        /// </summary>
-        /// <value><c>true</c> if [limit enabled]; otherwise, <c>false</c>.</value>
-        public bool LimitEnabled
-        {
-            get { return _enableLimit; }
-            set
-            {
-                Debug.Assert(BodyA.FixedRotation == false, "Warning: limits does currently not work with fixed rotation");
-
-                WakeBodies();
-                _enableLimit = value;
-            }
-        }
-
-        /// <summary>
-        /// Get the lower joint limit, usually in meters.
-        /// </summary>
-        /// <value></value>
-        public float LowerLimit
-        {
-            get { return _lowerTranslation; }
-            set
-            {
-                WakeBodies();
-                _lowerTranslation = value;
-            }
-        }
-
-        /// <summary>
-        /// Get the upper joint limit, usually in meters.
-        /// </summary>
-        /// <value></value>
-        public float UpperLimit
-        {
-            get { return _upperTranslation; }
-            set
-            {
-                WakeBodies();
-                _upperTranslation = value;
-            }
-        }
-
-        /// <summary>
-        /// Is the joint motor enabled?
-        /// </summary>
-        /// <value><c>true</c> if [motor enabled]; otherwise, <c>false</c>.</value>
-        public bool MotorEnabled
-        {
-            get { return _enableMotor; }
-            set
-            {
-                WakeBodies();
-                _enableMotor = value;
-            }
-        }
-
-        /// <summary>
-        /// Set the motor speed, usually in meters per second.
-        /// </summary>
-        /// <value>The speed.</value>
-        public float MotorSpeed
-        {
-            set
-            {
-                WakeBodies();
-                _motorSpeed = value;
-            }
-            get { return _motorSpeed; }
-        }
-
-        /// <summary>
-        /// Set the maximum motor force, usually in N.
-        /// </summary>
-        /// <value>The force.</value>
-        public float MaxMotorForce
-        {
-            set
-            {
-                WakeBodies();
-                _maxMotorForce = value;
-            }
-        }
-
-        /// <summary>
-        /// Get the current motor force, usually in N.
-        /// </summary>
-        /// <value></value>
-        public float MotorForce { get; set; }
-
-        public Vector2 LocalXAxis1
-        {
-            get { return _localXAxis1; }
-            set
-            {
-                _localXAxis1 = value;
-                _localYAxis1 = MathUtils.Cross(1.0f, _localXAxis1);
-            }
-        }
-
-        public override Vector2 GetReactionForce(float inv_dt)
-        {
-            return inv_dt * (_impulse.X * _perp + (MotorForce + _impulse.Z) * _axis);
-        }
-
-        public override float GetReactionTorque(float inv_dt)
-        {
-            return inv_dt * _impulse.Y;
-        }
-
-        internal override void InitVelocityConstraints(ref TimeStep step)
-        {
-            Body bB = BodyB;
-
-            LocalCenterA = Vector2.Zero;
-            LocalCenterB = bB.LocalCenter;
-
-            Transform xf2;
-            bB.GetTransform(out xf2);
-
-            // Compute the effective masses.
-            Vector2 r1 = LocalAnchorA;
-            Vector2 r2 = MathUtils.Multiply(ref xf2.R, LocalAnchorB - LocalCenterB);
-            Vector2 d = bB.Sweep.C + r2 - /* b1._sweep.Center - */ r1;
-
-            InvMassA = 0.0f;
-            InvIA = 0.0f;
-            InvMassB = bB.InvMass;
-            InvIB = bB.InvI;
-
-            // Compute motor Jacobian and effective mass.
-            {
-                _axis = _localXAxis1;
-                _a1 = MathUtils.Cross(d + r1, _axis);
-                _a2 = MathUtils.Cross(r2, _axis);
-
-                _motorMass = InvMassA + InvMassB + InvIA * _a1 * _a1 + InvIB * _a2 * _a2;
-
-                if (_motorMass > Settings.Epsilon)
-                {
-                    _motorMass = 1.0f / _motorMass;
-                }
-            }
-
-            // Prismatic constraint.
-            {
-                _perp = _localYAxis1;
-
-                _s1 = MathUtils.Cross(d + r1, _perp);
-                _s2 = MathUtils.Cross(r2, _perp);
-
-                float m1 = InvMassA, m2 = InvMassB;
-                float i1 = InvIA, i2 = InvIB;
-
-                float k11 = m1 + m2 + i1 * _s1 * _s1 + i2 * _s2 * _s2;
-                float k12 = i1 * _s1 + i2 * _s2;
-                float k13 = i1 * _s1 * _a1 + i2 * _s2 * _a2;
-                float k22 = i1 + i2;
-                float k23 = i1 * _a1 + i2 * _a2;
-                float k33 = m1 + m2 + i1 * _a1 * _a1 + i2 * _a2 * _a2;
-
-                _K.Col1 = new Vector3(k11, k12, k13);
-                _K.Col2 = new Vector3(k12, k22, k23);
-                _K.Col3 = new Vector3(k13, k23, k33);
-            }
-
-            // Compute motor and limit terms.
-            if (_enableLimit)
-            {
-                float jointTranslation = Vector2.Dot(_axis, d);
-                if (Math.Abs(_upperTranslation - _lowerTranslation) < 2.0f * Settings.LinearSlop)
-                {
-                    _limitState = LimitState.Equal;
-                }
-                else if (jointTranslation <= _lowerTranslation)
-                {
-                    if (_limitState != LimitState.AtLower)
-                    {
-                        _limitState = LimitState.AtLower;
-                        _impulse.Z = 0.0f;
-                    }
-                }
-                else if (jointTranslation >= _upperTranslation)
-                {
-                    if (_limitState != LimitState.AtUpper)
-                    {
-                        _limitState = LimitState.AtUpper;
-                        _impulse.Z = 0.0f;
-                    }
-                }
-                else
-                {
-                    _limitState = LimitState.Inactive;
-                    _impulse.Z = 0.0f;
-                }
-            }
-            else
-            {
-                _limitState = LimitState.Inactive;
-            }
-
-            if (_enableMotor == false)
-            {
-                MotorForce = 0.0f;
-            }
-
-            if (Settings.EnableWarmstarting)
-            {
-                // Account for variable time step.
-                _impulse *= step.dtRatio;
-                MotorForce *= step.dtRatio;
-
-                Vector2 P = _impulse.X * _perp + (MotorForce + _impulse.Z) * _axis;
-                float L2 = _impulse.X * _s2 + _impulse.Y + (MotorForce + _impulse.Z) * _a2;
-
-                bB.LinearVelocityInternal += InvMassB * P;
-                bB.AngularVelocityInternal += InvIB * L2;
-            }
-            else
-            {
-                _impulse = Vector3.Zero;
-                MotorForce = 0.0f;
-            }
-        }
-
-        internal override void SolveVelocityConstraints(ref TimeStep step)
-        {
-            Body bB = BodyB;
-
-            Vector2 v1 = Vector2.Zero;
-            float w1 = 0.0f;
-            Vector2 v2 = bB.LinearVelocityInternal;
-            float w2 = bB.AngularVelocityInternal;
-
-            // Solve linear motor constraint.
-            if (_enableMotor && _limitState != LimitState.Equal)
-            {
-                float Cdot = Vector2.Dot(_axis, v2 - v1) + _a2 * w2 - _a1 * w1;
-                float impulse = _motorMass * (_motorSpeed - Cdot);
-                float oldImpulse = MotorForce;
-                float maxImpulse = step.dt * _maxMotorForce;
-                MotorForce = MathUtils.Clamp(MotorForce + impulse, -maxImpulse, maxImpulse);
-                impulse = MotorForce - oldImpulse;
-
-                Vector2 P = impulse * _axis;
-                float L1 = impulse * _a1;
-                float L2 = impulse * _a2;
-
-                v1 -= InvMassA * P;
-                w1 -= InvIA * L1;
-
-                v2 += InvMassB * P;
-                w2 += InvIB * L2;
-            }
-
-            Vector2 Cdot1 = new Vector2(Vector2.Dot(_perp, v2 - v1) + _s2 * w2 - _s1 * w1, w2 - w1);
-
-            if (_enableLimit && _limitState != LimitState.Inactive)
-            {
-                // Solve prismatic and limit constraint in block form.
-                float Cdot2 = Vector2.Dot(_axis, v2 - v1) + _a2 * w2 - _a1 * w1;
-                Vector3 Cdot = new Vector3(Cdot1.X, Cdot1.Y, Cdot2);
-
-                Vector3 f1 = _impulse;
-                Vector3 df = _K.Solve33(-Cdot);
-                _impulse += df;
-
-                if (_limitState == LimitState.AtLower)
-                {
-                    _impulse.Z = Math.Max(_impulse.Z, 0.0f);
-                }
-                else if (_limitState == LimitState.AtUpper)
-                {
-                    _impulse.Z = Math.Min(_impulse.Z, 0.0f);
-                }
-
-                // f2(1:2) = invK(1:2,1:2) * (-Cdot(1:2) - K(1:2,3) * (f2(3) - f1(3))) + f1(1:2)
-                Vector2 b = -Cdot1 - (_impulse.Z - f1.Z) * new Vector2(_K.Col3.X, _K.Col3.Y);
-                Vector2 f2r = _K.Solve22(b) + new Vector2(f1.X, f1.Y);
-                _impulse.X = f2r.X;
-                _impulse.Y = f2r.Y;
-
-                df = _impulse - f1;
-
-                Vector2 P = df.X * _perp + df.Z * _axis;
-                float L2 = df.X * _s2 + df.Y + df.Z * _a2;
-
-                v2 += InvMassB * P;
-                w2 += InvIB * L2;
-            }
-            else
-            {
-                // Limit is inactive, just solve the prismatic constraint in block form.
-                Vector2 df = _K.Solve22(-Cdot1);
-                _impulse.X += df.X;
-                _impulse.Y += df.Y;
-
-                Vector2 P = df.X * _perp;
-                float L2 = df.X * _s2 + df.Y;
-
-                v2 += InvMassB * P;
-                w2 += InvIB * L2;
-            }
-
-            bB.LinearVelocityInternal = v2;
-            bB.AngularVelocityInternal = w2;
-        }
-
-        internal override bool SolvePositionConstraints()
-        {
-            //Body b1 = BodyA;
-            Body b2 = BodyB;
-
-            Vector2 c1 = Vector2.Zero; // b1._sweep.Center;
-            float a1 = 0.0f; // b1._sweep.Angle;
-
-            Vector2 c2 = b2.Sweep.C;
-            float a2 = b2.Sweep.A;
-
-            // Solve linear limit constraint.
-            float linearError = 0.0f;
-            bool active = false;
-            float C2 = 0.0f;
-
-            Mat22 R1 = new Mat22(a1);
-            Mat22 R2 = new Mat22(a2);
-
-            Vector2 r1 = MathUtils.Multiply(ref R1, LocalAnchorA - LocalCenterA);
-            Vector2 r2 = MathUtils.Multiply(ref R2, LocalAnchorB - LocalCenterB);
-            Vector2 d = c2 + r2 - c1 - r1;
-
-            if (_enableLimit)
-            {
-                _axis = MathUtils.Multiply(ref R1, _localXAxis1);
-
-                _a1 = MathUtils.Cross(d + r1, _axis);
-                _a2 = MathUtils.Cross(r2, _axis);
-
-                float translation = Vector2.Dot(_axis, d);
-                if (Math.Abs(_upperTranslation - _lowerTranslation) < 2.0f * Settings.LinearSlop)
-                {
-                    // Prevent large angular corrections
-                    C2 = MathUtils.Clamp(translation, -Settings.MaxLinearCorrection, Settings.MaxLinearCorrection);
-                    linearError = Math.Abs(translation);
-                    active = true;
-                }
-                else if (translation <= _lowerTranslation)
-                {
-                    // Prevent large linear corrections and allow some slop.
-                    C2 = MathUtils.Clamp(translation - _lowerTranslation + Settings.LinearSlop,
-                                         -Settings.MaxLinearCorrection, 0.0f);
-                    linearError = _lowerTranslation - translation;
-                    active = true;
-                }
-                else if (translation >= _upperTranslation)
-                {
-                    // Prevent large linear corrections and allow some slop.
-                    C2 = MathUtils.Clamp(translation - _upperTranslation - Settings.LinearSlop, 0.0f,
-                                         Settings.MaxLinearCorrection);
-                    linearError = translation - _upperTranslation;
-                    active = true;
-                }
-            }
-
-            _perp = MathUtils.Multiply(ref R1, _localYAxis1);
-
-            _s1 = MathUtils.Cross(d + r1, _perp);
-            _s2 = MathUtils.Cross(r2, _perp);
-
-            Vector3 impulse;
-            Vector2 C1 = new Vector2(Vector2.Dot(_perp, d), a2 - a1 - _refAngle);
-
-            linearError = Math.Max(linearError, Math.Abs(C1.X));
-            float angularError = Math.Abs(C1.Y);
-
-            if (active)
-            {
-                float m1 = InvMassA, m2 = InvMassB;
-                float i1 = InvIA, i2 = InvIB;
-
-                float k11 = m1 + m2 + i1 * _s1 * _s1 + i2 * _s2 * _s2;
-                float k12 = i1 * _s1 + i2 * _s2;
-                float k13 = i1 * _s1 * _a1 + i2 * _s2 * _a2;
-                float k22 = i1 + i2;
-                float k23 = i1 * _a1 + i2 * _a2;
-                float k33 = m1 + m2 + i1 * _a1 * _a1 + i2 * _a2 * _a2;
-
-                _K.Col1 = new Vector3(k11, k12, k13);
-                _K.Col2 = new Vector3(k12, k22, k23);
-                _K.Col3 = new Vector3(k13, k23, k33);
-
-                Vector3 C = new Vector3(-C1.X, -C1.Y, -C2);
-                impulse = _K.Solve33(C); // negated above
-            }
-            else
-            {
-                float m1 = InvMassA, m2 = InvMassB;
-                float i1 = InvIA, i2 = InvIB;
-
-                float k11 = m1 + m2 + i1 * _s1 * _s1 + i2 * _s2 * _s2;
-                float k12 = i1 * _s1 + i2 * _s2;
-                float k22 = i1 + i2;
-
-                _K.Col1 = new Vector3(k11, k12, 0.0f);
-                _K.Col2 = new Vector3(k12, k22, 0.0f);
-
-                Vector2 impulse1 = _K.Solve22(-C1);
-                impulse.X = impulse1.X;
-                impulse.Y = impulse1.Y;
-                impulse.Z = 0.0f;
-            }
-
-            Vector2 P = impulse.X * _perp + impulse.Z * _axis;
-            float L2 = impulse.X * _s2 + impulse.Y + impulse.Z * _a2;
-
-            c2 += InvMassB * P;
-            a2 += InvIB * L2;
-
-            // TODO_ERIN remove need for this.
-            b2.Sweep.C = c2;
-            b2.Sweep.A = a2;
-            b2.SynchronizeTransform();
-
-            return linearError <= Settings.LinearSlop && angularError <= Settings.AngularSlop;
-        }
-    }
-}

FarseerPhysicsEngine/Dynamics/Joints/FixedRevoluteJoint.cs

@@ -1,541 +0,0 @@
-/*
-* Farseer Physics Engine based on Box2D.XNA port:
-* Copyright (c) 2010 Ian Qvist
-* 
-* Box2D.XNA port of Box2D:
-* Copyright (c) 2009 Brandon Furtwangler, Nathan Furtwangler
-*
-* Original source Box2D:
-* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com 
-* 
-* This software is provided 'as-is', without any express or implied 
-* warranty.  In no event will the authors be held liable for any damages 
-* arising from the use of this software. 
-* Permission is granted to anyone to use this software for any purpose, 
-* including commercial applications, and to alter it and redistribute it 
-* freely, subject to the following restrictions: 
-* 1. The origin of this software must not be misrepresented; you must not 
-* claim that you wrote the original software. If you use this software 
-* in a product, an acknowledgment in the product documentation would be 
-* appreciated but is not required. 
-* 2. Altered source versions must be plainly marked as such, and must not be 
-* misrepresented as being the original software. 
-* 3. This notice may not be removed or altered from any source distribution. 
-*/
-
-using System;
-using System.Diagnostics;
-using FarseerPhysics.Common;
-using Microsoft.Xna.Framework;
-
-namespace FarseerPhysics.Dynamics.Joints
-{
-    /// <summary>
-    /// A revolute joint rains to bodies to share a common point while they
-    /// are free to rotate about the point. The relative rotation about the shared
-    /// point is the joint angle. You can limit the relative rotation with
-    /// a joint limit that specifies a lower and upper angle. You can use a motor
-    /// to drive the relative rotation about the shared point. A maximum motor torque
-    /// is provided so that infinite forces are not generated.
-    /// </summary>
-    public class FixedRevoluteJoint : Joint
-    {
-        private bool _enableLimit;
-        private bool _enableMotor;
-        private Vector3 _impulse;
-        private LimitState _limitState;
-        private float _lowerAngle;
-        private Mat33 _mass; // effective mass for point-to-point constraint.
-        private float _maxMotorTorque;
-        private float _motorImpulse;
-        private float _motorMass; // effective mass for motor/limit angular constraint.
-        private float _motorSpeed;
-        private float _upperAngle;
-        private Vector2 _worldAnchor;
-
-        /// <summary>
-        /// Initialize the bodies, anchors, and reference angle using the world
-        /// anchor.
-        /// This requires defining an
-        /// anchor point where the bodies are joined. The definition
-        /// uses local anchor points so that the initial configuration
-        /// can violate the constraint slightly. You also need to
-        /// specify the initial relative angle for joint limits. This
-        /// helps when saving and loading a game.
-        /// The local anchor points are measured from the body's origin
-        /// rather than the center of mass because:
-        /// 1. you might not know where the center of mass will be.
-        /// 2. if you add/remove shapes from a body and recompute the mass,
-        /// the joints will be broken.
-        /// </summary>
-        /// <param name="body">The body.</param>
-        /// <param name="bodyAnchor">The body anchor.</param>
-        /// <param name="worldAnchor">The world anchor.</param>
-        public FixedRevoluteJoint(Body body, Vector2 bodyAnchor, Vector2 worldAnchor)
-            : base(body)
-        {
-            JointType = JointType.FixedRevolute;
-
-            // Changed to local coordinates.
-            LocalAnchorA = bodyAnchor;
-            _worldAnchor = worldAnchor;
-
-            ReferenceAngle = -BodyA.Rotation;
-
-            _impulse = Vector3.Zero;
-
-            _limitState = LimitState.Inactive;
-        }
-
-        public override Vector2 WorldAnchorA
-        {
-            get { return BodyA.GetWorldPoint(LocalAnchorA); }
-        }
-
-        public override Vector2 WorldAnchorB
-        {
-            get { return _worldAnchor; }
-            set { _worldAnchor = value; }
-        }
-
-        public Vector2 LocalAnchorA { get; set; }
-
-        public float ReferenceAngle { get; set; }
-
-        /// <summary>
-        /// Get the current joint angle in radians.
-        /// </summary>
-        /// <value></value>
-        public float JointAngle
-        {
-            get { return BodyA.Sweep.A - ReferenceAngle; }
-        }
-
-        /// <summary>
-        /// Get the current joint angle speed in radians per second.
-        /// </summary>
-        /// <value></value>
-        public float JointSpeed
-        {
-            get { return BodyA.AngularVelocityInternal; }
-        }
-
-        /// <summary>
-        /// Is the joint limit enabled?
-        /// </summary>
-        /// <value><c>true</c> if [limit enabled]; otherwise, <c>false</c>.</value>
-        public bool LimitEnabled
-        {
-            get { return _enableLimit; }
-            set
-            {
-                WakeBodies();
-                _enableLimit = value;
-            }
-        }
-
-        /// <summary>
-        /// Get the lower joint limit in radians.
-        /// </summary>
-        /// <value></value>
-        public float LowerLimit
-        {
-            get { return _lowerAngle; }
-            set
-            {
-                WakeBodies();
-                _lowerAngle = value;
-            }
-        }
-
-        /// <summary>
-        /// Get the upper joint limit in radians.
-        /// </summary>
-        /// <value></value>
-        public float UpperLimit
-        {
-            get { return _upperAngle; }
-            set
-            {
-                WakeBodies();
-                _upperAngle = value;
-            }
-        }
-
-        /// <summary>
-        /// Is the joint motor enabled?
-        /// </summary>
-        /// <value><c>true</c> if [motor enabled]; otherwise, <c>false</c>.</value>
-        public bool MotorEnabled
-        {
-            get { return _enableMotor; }
-            set
-            {
-                WakeBodies();
-                _enableMotor = value;
-            }
-        }
-
-        /// <summary>
-        /// Set the motor speed in radians per second.
-        /// </summary>
-        /// <value>The speed.</value>
-        public float MotorSpeed
-        {
-            set
-            {
-                WakeBodies();
-                _motorSpeed = value;
-            }
-            get { return _motorSpeed; }
-        }
-
-        /// <summary>
-        /// Set the maximum motor torque, usually in N-m.
-        /// </summary>
-        /// <value>The torque.</value>
-        public float MaxMotorTorque
-        {
-            set
-            {
-                WakeBodies();
-                _maxMotorTorque = value;
-            }
-            get { return _maxMotorTorque; }
-        }
-
-        /// <summary>
-        /// Get the current motor torque, usually in N-m.
-        /// </summary>
-        /// <value></value>
-        public float MotorTorque
-        {
-            get { return _motorImpulse; }
-            set
-            {
-                WakeBodies();
-                _motorImpulse = value;
-            }
-        }
-
-        public override Vector2 GetReactionForce(float inv_dt)
-        {
-            return inv_dt * new Vector2(_impulse.X, _impulse.Y);
-        }
-
-        public override float GetReactionTorque(float inv_dt)
-        {
-            return inv_dt * _impulse.Z;
-        }
-
-        internal override void InitVelocityConstraints(ref TimeStep step)
-        {
-            Body b1 = BodyA;
-
-            if (_enableMotor || _enableLimit)
-            {
-                // You cannot create a rotation limit between bodies that
-                // both have fixed rotation.
-                Debug.Assert(b1.InvI > 0.0f /* || b2._invI > 0.0f*/);
-            }
-
-            // Compute the effective mass matrix.
-            Transform xf1;
-            b1.GetTransform(out xf1);
-
-            Vector2 r1 = MathUtils.Multiply(ref xf1.R, LocalAnchorA - b1.LocalCenter);
-            Vector2 r2 = _worldAnchor; // MathUtils.Multiply(ref xf2.R, LocalAnchorB - b2.LocalCenter);
-
-            // J = [-I -r1_skew I r2_skew]
-            //     [ 0       -1 0       1]
-            // r_skew = [-ry; rx]
-
-            // Matlab
-            // K = [ m1+r1y^2*i1+m2+r2y^2*i2,  -r1y*i1*r1x-r2y*i2*r2x,          -r1y*i1-r2y*i2]
-            //     [  -r1y*i1*r1x-r2y*i2*r2x, m1+r1x^2*i1+m2+r2x^2*i2,           r1x*i1+r2x*i2]
-            //     [          -r1y*i1-r2y*i2,           r1x*i1+r2x*i2,                   i1+i2]
-
-            float m1 = b1.InvMass;
-            const float m2 = 0;
-            float i1 = b1.InvI;
-            const float i2 = 0;
-
-            _mass.Col1.X = m1 + m2 + r1.Y * r1.Y * i1 + r2.Y * r2.Y * i2;
-            _mass.Col2.X = -r1.Y * r1.X * i1 - r2.Y * r2.X * i2;
-            _mass.Col3.X = -r1.Y * i1 - r2.Y * i2;
-            _mass.Col1.Y = _mass.Col2.X;
-            _mass.Col2.Y = m1 + m2 + r1.X * r1.X * i1 + r2.X * r2.X * i2;
-            _mass.Col3.Y = r1.X * i1 + r2.X * i2;
-            _mass.Col1.Z = _mass.Col3.X;
-            _mass.Col2.Z = _mass.Col3.Y;
-            _mass.Col3.Z = i1 + i2;
-
-            _motorMass = i1 + i2;
-            if (_motorMass > 0.0f)
-            {
-                _motorMass = 1.0f / _motorMass;
-            }
-
-            if (_enableMotor == false)
-            {
-                _motorImpulse = 0.0f;
-            }
-
-            if (_enableLimit)
-            {
-                float jointAngle = 0 - b1.Sweep.A - ReferenceAngle;
-                if (Math.Abs(_upperAngle - _lowerAngle) < 2.0f * Settings.AngularSlop)
-                {
-                    _limitState = LimitState.Equal;
-                }
-                else if (jointAngle <= _lowerAngle)
-                {
-                    if (_limitState != LimitState.AtLower)
-                    {
-                        _impulse.Z = 0.0f;
-                    }
-                    _limitState = LimitState.AtLower;
-                }
-                else if (jointAngle >= _upperAngle)
-                {
-                    if (_limitState != LimitState.AtUpper)
-                    {
-                        _impulse.Z = 0.0f;
-                    }
-                    _limitState = LimitState.AtUpper;
-                }
-                else
-                {
-                    _limitState = LimitState.Inactive;
-                    _impulse.Z = 0.0f;
-                }
-            }
-            else
-            {
-                _limitState = LimitState.Inactive;
-            }
-
-            if (Settings.EnableWarmstarting)
-            {
-                // Scale impulses to support a variable time step.
-                _impulse *= step.dtRatio;
-                _motorImpulse *= step.dtRatio;
-
-                Vector2 P = new Vector2(_impulse.X, _impulse.Y);
-
-                b1.LinearVelocityInternal -= m1 * P;
-                b1.AngularVelocityInternal -= i1 * (MathUtils.Cross(r1, P) + _motorImpulse + _impulse.Z);
-            }
-            else
-            {
-                _impulse = Vector3.Zero;
-                _motorImpulse = 0.0f;
-            }
-        }
-
-        internal override void SolveVelocityConstraints(ref TimeStep step)
-        {
-            Body b1 = BodyA;
-
-            Vector2 v1 = b1.LinearVelocityInternal;
-            float w1 = b1.AngularVelocityInternal;
-            Vector2 v2 = Vector2.Zero;
-            const float w2 = 0;
-
-            float m1 = b1.InvMass;
-            float i1 = b1.InvI;
-
-            // Solve motor constraint.
-            if (_enableMotor && _limitState != LimitState.Equal)
-            {
-                float Cdot = w2 - w1 - _motorSpeed;
-                float impulse = _motorMass * (-Cdot);
-                float oldImpulse = _motorImpulse;
-                float maxImpulse = step.dt * _maxMotorTorque;
-                _motorImpulse = MathUtils.Clamp(_motorImpulse + impulse, -maxImpulse, maxImpulse);
-                impulse = _motorImpulse - oldImpulse;
-
-                w1 -= i1 * impulse;
-            }
-
-            // Solve limit constraint.
-            if (_enableLimit && _limitState != LimitState.Inactive)
-            {
-                Transform xf1;
-                b1.GetTransform(out xf1);
-
-                Vector2 r1 = MathUtils.Multiply(ref xf1.R, LocalAnchorA - b1.LocalCenter);
-                Vector2 r2 = _worldAnchor;
-
-                // Solve point-to-point constraint
-                Vector2 Cdot1 = v2 + MathUtils.Cross(w2, r2) - v1 - MathUtils.Cross(w1, r1);
-                float Cdot2 = w2 - w1;
-                Vector3 Cdot = new Vector3(Cdot1.X, Cdot1.Y, Cdot2);
-
-                Vector3 impulse = _mass.Solve33(-Cdot);
-
-                if (_limitState == LimitState.Equal)
-                {
-                    _impulse += impulse;
-                }
-                else if (_limitState == LimitState.AtLower)
-                {
-                    float newImpulse = _impulse.Z + impulse.Z;
-                    if (newImpulse < 0.0f)
-                    {
-                        Vector2 reduced = _mass.Solve22(-Cdot1);
-                        impulse.X = reduced.X;
-                        impulse.Y = reduced.Y;
-                        impulse.Z = -_impulse.Z;
-                        _impulse.X += reduced.X;
-                        _impulse.Y += reduced.Y;
-                        _impulse.Z = 0.0f;
-                    }
-                }
-                else if (_limitState == LimitState.AtUpper)
-                {
-                    float newImpulse = _impulse.Z + impulse.Z;
-                    if (newImpulse > 0.0f)
-                    {
-                        Vector2 reduced = _mass.Solve22(-Cdot1);
-                        impulse.X = reduced.X;
-                        impulse.Y = reduced.Y;
-                        impulse.Z = -_impulse.Z;
-                        _impulse.X += reduced.X;
-                        _impulse.Y += reduced.Y;
-                        _impulse.Z = 0.0f;
-                    }
-                }
-
-                Vector2 P = new Vector2(impulse.X, impulse.Y);
-
-                v1 -= m1 * P;
-                w1 -= i1 * (MathUtils.Cross(r1, P) + impulse.Z);
-            }
-            else
-            {
-                Transform xf1;
-                b1.GetTransform(out xf1);
-
-                Vector2 r1 = MathUtils.Multiply(ref xf1.R, LocalAnchorA - b1.LocalCenter);
-                Vector2 r2 = _worldAnchor;
-
-                // Solve point-to-point constraint
-                Vector2 Cdot = v2 + MathUtils.Cross(w2, r2) - v1 - MathUtils.Cross(w1, r1);
-                Vector2 impulse = _mass.Solve22(-Cdot);
-
-                _impulse.X += impulse.X;
-                _impulse.Y += impulse.Y;
-
-                v1 -= m1 * impulse;
-                w1 -= i1 * MathUtils.Cross(r1, impulse);
-            }
-
-            b1.LinearVelocityInternal = v1;
-            b1.AngularVelocityInternal = w1;
-        }
-
-        internal override bool SolvePositionConstraints()
-        {
-            // TODO_ERIN block solve with limit. COME ON ERIN
-
-            Body b1 = BodyA;
-
-            float angularError = 0.0f;
-            float positionError;
-
-            // Solve angular limit constraint.
-            if (_enableLimit && _limitState != LimitState.Inactive)
-            {
-                float angle = 0 - b1.Sweep.A - ReferenceAngle;
-                float limitImpulse = 0.0f;
-
-                if (_limitState == LimitState.Equal)
-                {
-                    // Prevent large angular corrections
-                    float C = MathUtils.Clamp(angle - _lowerAngle, -Settings.MaxAngularCorrection,
-                                              Settings.MaxAngularCorrection);
-                    limitImpulse = -_motorMass * C;
-                    angularError = Math.Abs(C);
-                }
-                else if (_limitState == LimitState.AtLower)
-                {
-                    float C = angle - _lowerAngle;
-                    angularError = -C;
-
-                    // Prevent large angular corrections and allow some slop.
-                    C = MathUtils.Clamp(C + Settings.AngularSlop, -Settings.MaxAngularCorrection, 0.0f);
-                    limitImpulse = -_motorMass * C;
-                }
-                else if (_limitState == LimitState.AtUpper)
-                {
-                    float C = angle - _upperAngle;
-                    angularError = C;
-
-                    // Prevent large angular corrections and allow some slop.
-                    C = MathUtils.Clamp(C - Settings.AngularSlop, 0.0f, Settings.MaxAngularCorrection);
-                    limitImpulse = -_motorMass * C;
-                }
-
-                b1.Sweep.A -= b1.InvI * limitImpulse;
-
-                b1.SynchronizeTransform();
-            }
-
-            // Solve point-to-point constraint.
-            {
-                Transform xf1;
-                b1.GetTransform(out xf1);
-
-                Vector2 r1 = MathUtils.Multiply(ref xf1.R, LocalAnchorA - b1.LocalCenter);
-                Vector2 r2 = _worldAnchor;
-
-                Vector2 C = Vector2.Zero + r2 - b1.Sweep.C - r1;
-                positionError = C.Length();
-
-                float invMass1 = b1.InvMass;
-                const float invMass2 = 0;
-                float invI1 = b1.InvI;
-                const float invI2 = 0;
-
-                // Handle large detachment.
-                const float k_allowedStretch = 10.0f * Settings.LinearSlop;
-                if (C.LengthSquared() > k_allowedStretch * k_allowedStretch)
-                {
-                    // Use a particle solution (no rotation).
-                    Vector2 u = C;
-                    u.Normalize();
-                    float k = invMass1 + invMass2;
-                    Debug.Assert(k > Settings.Epsilon);
-                    float m = 1.0f / k;
-                    Vector2 impulse2 = m * (-C);
-                    const float k_beta = 0.5f;
-                    b1.Sweep.C -= k_beta * invMass1 * impulse2;
-
-                    C = Vector2.Zero + r2 - b1.Sweep.C - r1;
-                }
-
-                Mat22 K1 = new Mat22(new Vector2(invMass1 + invMass2, 0.0f), new Vector2(0.0f, invMass1 + invMass2));
-                Mat22 K2 = new Mat22(new Vector2(invI1 * r1.Y * r1.Y, -invI1 * r1.X * r1.Y),
-                                     new Vector2(-invI1 * r1.X * r1.Y, invI1 * r1.X * r1.X));
-                Mat22 K3 = new Mat22(new Vector2(invI2 * r2.Y * r2.Y, -invI2 * r2.X * r2.Y),
-                                     new Vector2(-invI2 * r2.X * r2.Y, invI2 * r2.X * r2.X));
-
-                Mat22 Ka;
-                Mat22.Add(ref K1, ref K2, out Ka);
-
-                Mat22 K;
-                Mat22.Add(ref Ka, ref K3, out K);
-
-                Vector2 impulse = K.Solve(-C);
-
-                b1.Sweep.C -= b1.InvMass * impulse;
-                b1.Sweep.A -= b1.InvI * MathUtils.Cross(r1, impulse);
-
-                b1.SynchronizeTransform();
-            }
-
-            return positionError <= Settings.LinearSlop && angularError <= Settings.AngularSlop;
-        }
-    }
-}

FarseerPhysicsEngine/Dynamics/Joints/FrictionJoint.cs

@@ -1,249 +0,0 @@
-/*
-* Farseer Physics Engine based on Box2D.XNA port:
-* Copyright (c) 2010 Ian Qvist
-* 
-* Box2D.XNA port of Box2D:
-* Copyright (c) 2009 Brandon Furtwangler, Nathan Furtwangler
-*
-* Original source Box2D:
-* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com 
-* 
-* This software is provided 'as-is', without any express or implied 
-* warranty.  In no event will the authors be held liable for any damages 
-* arising from the use of this software. 
-* Permission is granted to anyone to use this software for any purpose, 
-* including commercial applications, and to alter it and redistribute it 
-* freely, subject to the following restrictions: 
-* 1. The origin of this software must not be misrepresented; you must not 
-* claim that you wrote the original software. If you use this software 
-* in a product, an acknowledgment in the product documentation would be 
-* appreciated but is not required. 
-* 2. Altered source versions must be plainly marked as such, and must not be 
-* misrepresented as being the original software. 
-* 3. This notice may not be removed or altered from any source distribution. 
-*/
-
-using System.Diagnostics;
-using FarseerPhysics.Common;
-using Microsoft.Xna.Framework;
-
-namespace FarseerPhysics.Dynamics.Joints
-{
-    // Point-to-point constraint
-    // Cdot = v2 - v1
-    //      = v2 + cross(w2, r2) - v1 - cross(w1, r1)
-    // J = [-I -r1_skew I r2_skew ]
-    // Identity used:
-    // w k % (rx i + ry j) = w * (-ry i + rx j)
-
-    // Angle constraint
-    // Cdot = w2 - w1
-    // J = [0 0 -1 0 0 1]
-    // K = invI1 + invI2
-
-    /// <summary>
-    /// Friction joint. This is used for top-down friction.
-    /// It provides 2D translational friction and angular friction.
-    /// </summary>
-    public class FrictionJoint : Joint
-    {
-        public Vector2 LocalAnchorA;
-        public Vector2 LocalAnchorB;
-        private float _angularImpulse;
-        private float _angularMass;
-        private Vector2 _linearImpulse;
-        private Mat22 _linearMass;
-
-        internal FrictionJoint()
-        {
-            JointType = JointType.Friction;
-        }
-
-        public FrictionJoint(Body bodyA, Body bodyB, Vector2 localAnchorA, Vector2 localAnchorB)
-            : base(bodyA, bodyB)
-        {
-            JointType = JointType.Friction;
-            LocalAnchorA = localAnchorA;
-            LocalAnchorB = localAnchorB;
-        }
-
-        public override Vector2 WorldAnchorA
-        {
-            get { return BodyA.GetWorldPoint(LocalAnchorA); }
-        }
-
-        public override Vector2 WorldAnchorB
-        {
-            get { return BodyB.GetWorldPoint(LocalAnchorB); }
-            set { Debug.Assert(false, "You can't set the world anchor on this joint type."); }
-        }
-
-        /// <summary>
-        /// The maximum friction force in N.
-        /// </summary>
-        public float MaxForce { get; set; }
-
-        /// <summary>
-        /// The maximum friction torque in N-m.
-        /// </summary>
-        public float MaxTorque { get; set; }
-
-        public override Vector2 GetReactionForce(float inv_dt)
-        {
-            return inv_dt * _linearImpulse;
-        }
-
-        public override float GetReactionTorque(float inv_dt)
-        {
-            return inv_dt * _angularImpulse;
-        }
-
-        internal override void InitVelocityConstraints(ref TimeStep step)
-        {
-            Body bA = BodyA;
-            Body bB = BodyB;
-
-            Transform xfA, xfB;
-            bA.GetTransform(out xfA);
-            bB.GetTransform(out xfB);
-
-            // Compute the effective mass matrix.
-            Vector2 rA = MathUtils.Multiply(ref xfA.R, LocalAnchorA - bA.LocalCenter);
-            Vector2 rB = MathUtils.Multiply(ref xfB.R, LocalAnchorB - bB.LocalCenter);
-
-            // J = [-I -r1_skew I r2_skew]
-            //     [ 0       -1 0       1]
-            // r_skew = [-ry; rx]
-
-            // Matlab
-            // K = [ mA+r1y^2*iA+mB+r2y^2*iB,  -r1y*iA*r1x-r2y*iB*r2x,          -r1y*iA-r2y*iB]
-            //     [  -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB,           r1x*iA+r2x*iB]
-            //     [          -r1y*iA-r2y*iB,           r1x*iA+r2x*iB,                   iA+iB]
-
-            float mA = bA.InvMass, mB = bB.InvMass;
-            float iA = bA.InvI, iB = bB.InvI;
-
-            Mat22 K1 = new Mat22();
-            K1.Col1.X = mA + mB;
-            K1.Col2.X = 0.0f;
-            K1.Col1.Y = 0.0f;
-            K1.Col2.Y = mA + mB;
-
-            Mat22 K2 = new Mat22();
-            K2.Col1.X = iA * rA.Y * rA.Y;
-            K2.Col2.X = -iA * rA.X * rA.Y;
-            K2.Col1.Y = -iA * rA.X * rA.Y;
-            K2.Col2.Y = iA * rA.X * rA.X;
-
-            Mat22 K3 = new Mat22();
-            K3.Col1.X = iB * rB.Y * rB.Y;
-            K3.Col2.X = -iB * rB.X * rB.Y;
-            K3.Col1.Y = -iB * rB.X * rB.Y;
-            K3.Col2.Y = iB * rB.X * rB.X;
-
-            Mat22 K12;
-            Mat22.Add(ref K1, ref K2, out K12);
-
-            Mat22 K;
-            Mat22.Add(ref K12, ref K3, out K);
-
-            _linearMass = K.Inverse;
-
-            _angularMass = iA + iB;
-            if (_angularMass > 0.0f)
-            {
-                _angularMass = 1.0f / _angularMass;
-            }
-
-            if (Settings.EnableWarmstarting)
-            {
-                // Scale impulses to support a variable time step.
-                _linearImpulse *= step.dtRatio;
-                _angularImpulse *= step.dtRatio;
-
-                Vector2 P = new Vector2(_linearImpulse.X, _linearImpulse.Y);
-
-                bA.LinearVelocityInternal -= mA * P;
-                bA.AngularVelocityInternal -= iA * (MathUtils.Cross(rA, P) + _angularImpulse);
-
-                bB.LinearVelocityInternal += mB * P;
-                bB.AngularVelocityInternal += iB * (MathUtils.Cross(rB, P) + _angularImpulse);
-            }
-            else
-            {
-                _linearImpulse = Vector2.Zero;
-                _angularImpulse = 0.0f;
-            }
-        }
-
-        internal override void SolveVelocityConstraints(ref TimeStep step)
-        {
-            Body bA = BodyA;
-            Body bB = BodyB;
-
-            Vector2 vA = bA.LinearVelocityInternal;
-            float wA = bA.AngularVelocityInternal;
-            Vector2 vB = bB.LinearVelocityInternal;
-            float wB = bB.AngularVelocityInternal;
-
-            float mA = bA.InvMass, mB = bB.InvMass;
-            float iA = bA.InvI, iB = bB.InvI;
-
-            Transform xfA, xfB;
-            bA.GetTransform(out xfA);
-            bB.GetTransform(out xfB);
-
-            Vector2 rA = MathUtils.Multiply(ref xfA.R, LocalAnchorA - bA.LocalCenter);
-            Vector2 rB = MathUtils.Multiply(ref xfB.R, LocalAnchorB - bB.LocalCenter);
-
-            // Solve angular friction
-            {
-                float Cdot = wB - wA;
-                float impulse = -_angularMass * Cdot;
-
-                float oldImpulse = _angularImpulse;
-                float maxImpulse = step.dt * MaxTorque;
-                _angularImpulse = MathUtils.Clamp(_angularImpulse + impulse, -maxImpulse, maxImpulse);
-                impulse = _angularImpulse - oldImpulse;
-
-                wA -= iA * impulse;
-                wB += iB * impulse;
-            }
-
-            // Solve linear friction
-            {
-                Vector2 Cdot = vB + MathUtils.Cross(wB, rB) - vA - MathUtils.Cross(wA, rA);
-
-                Vector2 impulse = -MathUtils.Multiply(ref _linearMass, Cdot);
-                Vector2 oldImpulse = _linearImpulse;
-                _linearImpulse += impulse;
-
-                float maxImpulse = step.dt * MaxForce;
-
-                if (_linearImpulse.LengthSquared() > maxImpulse * maxImpulse)
-                {
-                    _linearImpulse.Normalize();
-                    _linearImpulse *= maxImpulse;
-                }
-
-                impulse = _linearImpulse - oldImpulse;
-
-                vA -= mA * impulse;
-                wA -= iA * MathUtils.Cross(rA, impulse);
-
-                vB += mB * impulse;
-                wB += iB * MathUtils.Cross(rB, impulse);
-            }
-
-            bA.LinearVelocityInternal = vA;
-            bA.AngularVelocityInternal = wA;
-            bB.LinearVelocityInternal = vB;
-            bB.AngularVelocityInternal = wB;
-        }
-
-        internal override bool SolvePositionConstraints()
-        {
-            return true;
-        }
-    }
-}

FarseerPhysicsEngine/Dynamics/Joints/GearJoint.cs

@@ -1,350 +0,0 @@
-/*
-* Farseer Physics Engine based on Box2D.XNA port:
-* Copyright (c) 2010 Ian Qvist
-* 
-* Box2D.XNA port of Box2D:
-* Copyright (c) 2009 Brandon Furtwangler, Nathan Furtwangler
-*
-* Original source Box2D:
-* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com 
-* 
-* This software is provided 'as-is', without any express or implied 
-* warranty.  In no event will the authors be held liable for any damages 
-* arising from the use of this software. 
-* Permission is granted to anyone to use this software for any purpose, 
-* including commercial applications, and to alter it and redistribute it 
-* freely, subject to the following restrictions: 
-* 1. The origin of this software must not be misrepresented; you must not 
-* claim that you wrote the original software. If you use this software 
-* in a product, an acknowledgment in the product documentation would be 
-* appreciated but is not required. 
-* 2. Altered source versions must be plainly marked as such, and must not be 
-* misrepresented as being the original software. 
-* 3. This notice may not be removed or altered from any source distribution. 
-*/
-
-using System.Diagnostics;
-using FarseerPhysics.Common;
-using Microsoft.Xna.Framework;
-
-namespace FarseerPhysics.Dynamics.Joints
-{
-    /// <summary>
-    /// A gear joint is used to connect two joints together. Either joint
-    /// can be a revolute or prismatic joint. You specify a gear ratio
-    /// to bind the motions together:
-    /// coordinate1 + ratio * coordinate2 = ant
-    /// The ratio can be negative or positive. If one joint is a revolute joint
-    /// and the other joint is a prismatic joint, then the ratio will have units
-    /// of length or units of 1/length.
-    /// @warning The revolute and prismatic joints must be attached to
-    /// fixed bodies (which must be body1 on those joints).
-    /// </summary>
-    public class GearJoint : Joint
-    {
-        private Jacobian _J;
-
-        private float _ant;
-        private FixedPrismaticJoint _fixedPrismatic1;
-        private FixedPrismaticJoint _fixedPrismatic2;
-        private FixedRevoluteJoint _fixedRevolute1;
-        private FixedRevoluteJoint _fixedRevolute2;
-        private float _impulse;
-        private float _mass;
-        private PrismaticJoint _prismatic1;
-        private PrismaticJoint _prismatic2;
-        private RevoluteJoint _revolute1;
-        private RevoluteJoint _revolute2;
-
-        /// <summary>
-        /// Requires two existing revolute or prismatic joints (any combination will work).
-        /// The provided joints must attach a dynamic body to a static body.
-        /// </summary>
-        /// <param name="jointA">The first joint.</param>
-        /// <param name="jointB">The second joint.</param>
-        /// <param name="ratio">The ratio.</param>
-        public GearJoint(Joint jointA, Joint jointB, float ratio)
-            : base(jointA.BodyA, jointA.BodyB)
-        {
-            JointType = JointType.Gear;
-            JointA = jointA;
-            JointB = jointB;
-            Ratio = ratio;
-
-            JointType type1 = jointA.JointType;
-            JointType type2 = jointB.JointType;
-
-            // Make sure its the right kind of joint
-            Debug.Assert(type1 == JointType.Revolute ||
-                         type1 == JointType.Prismatic ||
-                         type1 == JointType.FixedRevolute ||
-                         type1 == JointType.FixedPrismatic);
-            Debug.Assert(type2 == JointType.Revolute ||
-                         type2 == JointType.Prismatic ||
-                         type2 == JointType.FixedRevolute ||
-                         type2 == JointType.FixedPrismatic);
-
-            // In the case of a prismatic and revolute joint, the first body must be static.
-            if (type1 == JointType.Revolute || type1 == JointType.Prismatic)
-                Debug.Assert(jointA.BodyA.BodyType == BodyType.Static);
-            if (type2 == JointType.Revolute || type2 == JointType.Prismatic)
-                Debug.Assert(jointB.BodyA.BodyType == BodyType.Static);
-
-            float coordinate1 = 0.0f, coordinate2 = 0.0f;
-
-            switch (type1)
-            {
-                case JointType.Revolute:
-                    BodyA = jointA.BodyB;
-                    _revolute1 = (RevoluteJoint)jointA;
-                    LocalAnchor1 = _revolute1.LocalAnchorB;
-                    coordinate1 = _revolute1.JointAngle;
-                    break;
-                case JointType.Prismatic:
-                    BodyA = jointA.BodyB;
-                    _prismatic1 = (PrismaticJoint)jointA;
-                    LocalAnchor1 = _prismatic1.LocalAnchorB;
-                    coordinate1 = _prismatic1.JointTranslation;
-                    break;
-                case JointType.FixedRevolute:
-                    BodyA = jointA.BodyA;
-                    _fixedRevolute1 = (FixedRevoluteJoint)jointA;
-                    LocalAnchor1 = _fixedRevolute1.LocalAnchorA;
-                    coordinate1 = _fixedRevolute1.JointAngle;
-                    break;
-                case JointType.FixedPrismatic:
-                    BodyA = jointA.BodyA;
-                    _fixedPrismatic1 = (FixedPrismaticJoint)jointA;
-                    LocalAnchor1 = _fixedPrismatic1.LocalAnchorA;
-                    coordinate1 = _fixedPrismatic1.JointTranslation;
-                    break;
-            }
-
-            switch (type2)
-            {
-                case JointType.Revolute:
-                    BodyB = jointB.BodyB;
-                    _revolute2 = (RevoluteJoint)jointB;
-                    LocalAnchor2 = _revolute2.LocalAnchorB;
-                    coordinate2 = _revolute2.JointAngle;
-                    break;
-                case JointType.Prismatic:
-                    BodyB = jointB.BodyB;
-                    _prismatic2 = (PrismaticJoint)jointB;
-                    LocalAnchor2 = _prismatic2.LocalAnchorB;
-                    coordinate2 = _prismatic2.JointTranslation;
-                    break;
-                case JointType.FixedRevolute:
-                    BodyB = jointB.BodyA;
-                    _fixedRevolute2 = (FixedRevoluteJoint)jointB;
-                    LocalAnchor2 = _fixedRevolute2.LocalAnchorA;
-                    coordinate2 = _fixedRevolute2.JointAngle;
-                    break;
-                case JointType.FixedPrismatic:
-                    BodyB = jointB.BodyA;
-                    _fixedPrismatic2 = (FixedPrismaticJoint)jointB;
-                    LocalAnchor2 = _fixedPrismatic2.LocalAnchorA;
-                    coordinate2 = _fixedPrismatic2.JointTranslation;
-                    break;
-            }
-
-            _ant = coordinate1 + Ratio * coordinate2;
-        }
-
-        public override Vector2 WorldAnchorA
-        {
-            get { return BodyA.GetWorldPoint(LocalAnchor1); }
-        }
-
-        public override Vector2 WorldAnchorB
-        {
-            get { return BodyB.GetWorldPoint(LocalAnchor2); }
-            set { Debug.Assert(false, "You can't set the world anchor on this joint type."); }
-        }
-
-        /// <summary>
-        /// The gear ratio.
-        /// </summary>
-        public float Ratio { get; set; }
-
-        /// <summary>
-        /// The first revolute/prismatic joint attached to the gear joint.
-        /// </summary>
-        public Joint JointA { get; set; }
-
-        /// <summary>
-        /// The second revolute/prismatic joint attached to the gear joint.
-        /// </summary>
-        public Joint JointB { get; set; }
-
-        public Vector2 LocalAnchor1 { get; private set; }
-        public Vector2 LocalAnchor2 { get; private set; }
-
-        public override Vector2 GetReactionForce(float inv_dt)
-        {
-            Vector2 P = _impulse * _J.LinearB;
-            return inv_dt * P;
-        }
-
-        public override float GetReactionTorque(float inv_dt)
-        {
-            Transform xf1;
-            BodyB.GetTransform(out xf1);
-
-            Vector2 r = MathUtils.Multiply(ref xf1.R, LocalAnchor2 - BodyB.LocalCenter);
-            Vector2 P = _impulse * _J.LinearB;
-            float L = _impulse * _J.AngularB - MathUtils.Cross(r, P);
-            return inv_dt * L;
-        }
-
-        internal override void InitVelocityConstraints(ref TimeStep step)
-        {
-            Body b1 = BodyA;
-            Body b2 = BodyB;
-
-            float K = 0.0f;
-            _J.SetZero();
-
-            if (_revolute1 != null || _fixedRevolute1 != null)
-            {
-                _J.AngularA = -1.0f;
-                K += b1.InvI;
-            }
-            else
-            {
-                Vector2 ug;
-                if (_prismatic1 != null)
-                    ug = _prismatic1.LocalXAxis1; // MathUtils.Multiply(ref xfg1.R, _prismatic1.LocalXAxis1);
-                else
-                    ug = _fixedPrismatic1.LocalXAxis1; // MathUtils.Multiply(ref xfg1.R, _prismatic1.LocalXAxis1);
-
-                Transform xf1 /*, xfg1*/;
-                b1.GetTransform(out xf1);
-                //g1.GetTransform(out xfg1);
-
-
-                Vector2 r = MathUtils.Multiply(ref xf1.R, LocalAnchor1 - b1.LocalCenter);
-                float crug = MathUtils.Cross(r, ug);
-                _J.LinearA = -ug;
-                _J.AngularA = -crug;
-                K += b1.InvMass + b1.InvI * crug * crug;
-            }
-
-            if (_revolute2 != null || _fixedRevolute2 != null)
-            {
-                _J.AngularB = -Ratio;
-                K += Ratio * Ratio * b2.InvI;
-            }
-            else
-            {
-                Vector2 ug;
-                if (_prismatic2 != null)
-                    ug = _prismatic2.LocalXAxis1; // MathUtils.Multiply(ref xfg1.R, _prismatic1.LocalXAxis1);
-                else
-                    ug = _fixedPrismatic2.LocalXAxis1; // MathUtils.Multiply(ref xfg1.R, _prismatic1.LocalXAxis1);
-
-                Transform /*xfg1,*/ xf2;
-                //g1.GetTransform(out xfg1);
-                b2.GetTransform(out xf2);
-
-                Vector2 r = MathUtils.Multiply(ref xf2.R, LocalAnchor2 - b2.LocalCenter);
-                float crug = MathUtils.Cross(r, ug);
-                _J.LinearB = -Ratio * ug;
-                _J.AngularB = -Ratio * crug;
-                K += Ratio * Ratio * (b2.InvMass + b2.InvI * crug * crug);
-            }
-
-            // Compute effective mass.
-            Debug.Assert(K > 0.0f);
-            _mass = K > 0.0f ? 1.0f / K : 0.0f;
-
-            if (Settings.EnableWarmstarting)
-            {
-                // Warm starting.
-                b1.LinearVelocityInternal += b1.InvMass * _impulse * _J.LinearA;
-                b1.AngularVelocityInternal += b1.InvI * _impulse * _J.AngularA;
-                b2.LinearVelocityInternal += b2.InvMass * _impulse * _J.LinearB;
-                b2.AngularVelocityInternal += b2.InvI * _impulse * _J.AngularB;
-            }
-            else
-            {
-                _impulse = 0.0f;
-            }
-        }
-
-        internal override void SolveVelocityConstraints(ref TimeStep step)
-        {
-            Body b1 = BodyA;
-            Body b2 = BodyB;
-
-            float Cdot = _J.Compute(b1.LinearVelocityInternal, b1.AngularVelocityInternal,
-                                    b2.LinearVelocityInternal, b2.AngularVelocityInternal);
-
-            float impulse = _mass * (-Cdot);
-            _impulse += impulse;
-
-            b1.LinearVelocityInternal += b1.InvMass * impulse * _J.LinearA;
-            b1.AngularVelocityInternal += b1.InvI * impulse * _J.AngularA;
-            b2.LinearVelocityInternal += b2.InvMass * impulse * _J.LinearB;
-            b2.AngularVelocityInternal += b2.InvI * impulse * _J.AngularB;
-        }
-
-        internal override bool SolvePositionConstraints()
-        {
-            const float linearError = 0.0f;
-
-            Body b1 = BodyA;
-            Body b2 = BodyB;
-
-            float coordinate1 = 0.0f, coordinate2 = 0.0f;
-            if (_revolute1 != null)
-            {
-                coordinate1 = _revolute1.JointAngle;
-            }
-            else if (_fixedRevolute1 != null)
-            {
-                coordinate1 = _fixedRevolute1.JointAngle;
-            }
-            else if (_prismatic1 != null)
-            {
-                coordinate1 = _prismatic1.JointTranslation;
-            }
-            else if (_fixedPrismatic1 != null)
-            {
-                coordinate1 = _fixedPrismatic1.JointTranslation;
-            }
-
-            if (_revolute2 != null)
-            {
-                coordinate2 = _revolute2.JointAngle;
-            }
-            else if (_fixedRevolute2 != null)
-            {
-                coordinate2 = _fixedRevolute2.JointAngle;
-            }
-            else if (_prismatic2 != null)
-            {
-                coordinate2 = _prismatic2.JointTranslation;
-            }
-            else if (_fixedPrismatic2 != null)
-            {
-                coordinate2 = _fixedPrismatic2.JointTranslation;
-            }
-
-            float C = _ant - (coordinate1 + Ratio * coordinate2);
-
-            float impulse = _mass * (-C);
-
-            b1.Sweep.C += b1.InvMass * impulse * _J.LinearA;
-            b1.Sweep.A += b1.InvI * impulse * _J.AngularA;
-            b2.Sweep.C += b2.InvMass * impulse * _J.LinearB;
-            b2.Sweep.A += b2.InvI * impulse * _J.AngularB;
-
-            b1.SynchronizeTransform();
-            b2.SynchronizeTransform();
-
-            // TODO_ERIN not implemented
-            return linearError < Settings.LinearSlop;
-        }
-    }
-}

FarseerPhysicsEngine/Dynamics/Joints/Joint.cs

@@ -1,282 +0,0 @@
-/*
-* Farseer Physics Engine based on Box2D.XNA port:
-* Copyright (c) 2010 Ian Qvist
-* 
-* Box2D.XNA port of Box2D:
-* Copyright (c) 2009 Brandon Furtwangler, Nathan Furtwangler
-*
-* Original source Box2D:
-* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com 
-* 
-* This software is provided 'as-is', without any express or implied 
-* warranty.  In no event will the authors be held liable for any damages 
-* arising from the use of this software. 
-* Permission is granted to anyone to use this software for any purpose, 
-* including commercial applications, and to alter it and redistribute it 
-* freely, subject to the following restrictions: 
-* 1. The origin of this software must not be misrepresented; you must not 
-* claim that you wrote the original software. If you use this software 
-* in a product, an acknowledgment in the product documentation would be 
-* appreciated but is not required. 
-* 2. Altered source versions must be plainly marked as such, and must not be 
-* misrepresented as being the original software. 
-* 3. This notice may not be removed or altered from any source distribution. 
-*/
-
-using System;
-using System.Diagnostics;
-using Microsoft.Xna.Framework;
-
-namespace FarseerPhysics.Dynamics.Joints
-{
-    public enum JointType
-    {
-        Revolute,
-        Prismatic,
-        Distance,
-        Pulley,
-        Gear,
-        Line,
-        Weld,
-        Friction,
-        Slider,
-        Angle,
-        Rope,
-        FixedMouse,
-        FixedRevolute,
-        FixedDistance,
-        FixedLine,
-        FixedPrismatic,
-        FixedAngle,
-        FixedFriction,
-    }
-
-    public enum LimitState
-    {
-        Inactive,
-        AtLower,
-        AtUpper,
-        Equal,
-    }
-
-    internal struct Jacobian
-    {
-        public float AngularA;
-        public float AngularB;
-        public Vector2 LinearA;
-        public Vector2 LinearB;
-
-        public void SetZero()
-        {
-            LinearA = Vector2.Zero;
-            AngularA = 0.0f;
-            LinearB = Vector2.Zero;
-            AngularB = 0.0f;
-        }
-
-        public void Set(Vector2 x1, float a1, Vector2 x2, float a2)
-        {
-            LinearA = x1;
-            AngularA = a1;
-            LinearB = x2;
-            AngularB = a2;
-        }
-
-        public float Compute(Vector2 x1, float a1, Vector2 x2, float a2)
-        {
-            return Vector2.Dot(LinearA, x1) + AngularA * a1 + Vector2.Dot(LinearB, x2) + AngularB * a2;
-        }
-    }
-
-    /// <summary>
-    /// A joint edge is used to connect bodies and joints together
-    /// in a joint graph where each body is a node and each joint
-    /// is an edge. A joint edge belongs to a doubly linked list
-    /// maintained in each attached body. Each joint has two joint
-    /// nodes, one for each attached body.
-    /// </summary>
-    public sealed class JointEdge
-    {
-        /// <summary>
-        /// The joint.
-        /// </summary>
-        public Joint Joint;
-
-        /// <summary>
-        /// The next joint edge in the body's joint list.
-        /// </summary>
-        public JointEdge Next;
-
-        /// <summary>
-        /// Provides quick access to the other body attached.
-        /// </summary>
-        public Body Other;
-
-        /// <summary>
-        /// The previous joint edge in the body's joint list.
-        /// </summary>
-        public JointEdge Prev;
-    }
-
-    public abstract class Joint
-    {
-        /// <summary>
-        /// The Breakpoint simply indicates the maximum Value the JointError can be before it breaks.
-        /// The default value is float.MaxValue
-        /// </summary>
-        public float Breakpoint = float.MaxValue;
-
-        internal JointEdge EdgeA = new JointEdge();
-        internal JointEdge EdgeB = new JointEdge();
-        public bool Enabled = true;
-        protected float InvIA;
-        protected float InvIB;
-        protected float InvMassA;
-        protected float InvMassB;
-        internal bool IslandFlag;
-        protected Vector2 LocalCenterA, LocalCenterB;
-
-        protected Joint()
-        {
-        }
-
-        protected Joint(Body body, Body bodyB)
-        {
-            Debug.Assert(body != bodyB);
-
-            BodyA = body;
-            BodyB = bodyB;
-
-            //Connected bodies should not collide by default
-            CollideConnected = false;
-        }
-
-        /// <summary>
-        /// Constructor for fixed joint
-        /// </summary>
-        protected Joint(Body body)
-        {
-            BodyA = body;
-
-            //Connected bodies should not collide by default
-            CollideConnected = false;
-        }
-
-        /// <summary>
-        /// Gets or sets the type of the joint.
-        /// </summary>
-        /// <value>The type of the joint.</value>
-        public JointType JointType { get; protected set; }
-
-        /// <summary>
-        /// Get the first body attached to this joint.
-        /// </summary>
-        /// <value></value>
-        public Body BodyA { get; set; }
-
-        /// <summary>
-        /// Get the second body attached to this joint.
-        /// </summary>
-        /// <value></value>
-        public Body BodyB { get; set; }
-
-        /// <summary>
-        /// Get the anchor point on body1 in world coordinates.
-        /// </summary>
-        /// <value></value>
-        public abstract Vector2 WorldAnchorA { get; }
-
-        /// <summary>
-        /// Get the anchor point on body2 in world coordinates.
-        /// </summary>
-        /// <value></value>
-        public abstract Vector2 WorldAnchorB { get; set; }
-
-        /// <summary>
-        /// Set the user data pointer.
-        /// </summary>
-        /// <value>The data.</value>
-        public object UserData { get; set; }
-
-        /// <summary>
-        /// Short-cut function to determine if either body is inactive.
-        /// </summary>
-        /// <value><c>true</c> if active; otherwise, <c>false</c>.</value>
-        public bool Active
-        {
-            get { return BodyA.Enabled && BodyB.Enabled; }
-        }
-
-        /// <summary>
-        /// Set this flag to true if the attached bodies should collide.
-        /// </summary>
-        public bool CollideConnected { get; set; }
-
-        /// <summary>
-        /// Fires when the joint is broken.
-        /// </summary>
-        public event Action<Joint, float> Broke;
-
-        /// <summary>
-        /// Get the reaction force on body2 at the joint anchor in Newtons.
-        /// </summary>
-        /// <param name="inv_dt">The inv_dt.</param>
-        /// <returns></returns>
-        public abstract Vector2 GetReactionForce(float inv_dt);
-
-        /// <summary>
-        /// Get the reaction torque on body2 in N*m.
-        /// </summary>
-        /// <param name="inv_dt">The inv_dt.</param>
-        /// <returns></returns>
-        public abstract float GetReactionTorque(float inv_dt);
-
-        protected void WakeBodies()
-        {
-            BodyA.Awake = true;
-            if (BodyB != null)
-            {
-                BodyB.Awake = true;
-            }
-        }
-
-        /// <summary>
-        /// Return true if the joint is a fixed type.
-        /// </summary>
-        public bool IsFixedType()
-        {
-            return JointType == JointType.FixedRevolute ||
-                   JointType == JointType.FixedDistance ||
-                   JointType == JointType.FixedPrismatic ||
-                   JointType == JointType.FixedLine ||
-                   JointType == JointType.FixedMouse ||
-                   JointType == JointType.FixedAngle ||
-                   JointType == JointType.FixedFriction;
-        }
-
-        internal abstract void InitVelocityConstraints(ref TimeStep step);
-
-        internal void Validate(float invDT)
-        {
-            if (!Enabled)
-                return;
-
-            float jointError = GetReactionForce(invDT).Length();
-            if (Math.Abs(jointError) <= Breakpoint)
-                return;
-
-            Enabled = false;
-
-            if (Broke != null)
-                Broke(this, jointError);
-        }
-
-        internal abstract void SolveVelocityConstraints(ref TimeStep step);
-
-        /// <summary>
-        /// Solves the position constraints.
-        /// </summary>
-        /// <returns>returns true if the position errors are within tolerance.</returns>
-        internal abstract bool SolvePositionConstraints();
-    }
-}

FarseerPhysicsEngine/Dynamics/Joints/LineJoint.cs

@@ -1,436 +0,0 @@
-/*
-* Farseer Physics Engine based on Box2D.XNA port:
-* Copyright (c) 2010 Ian Qvist
-* 
-* Box2D.XNA port of Box2D:
-* Copyright (c) 2009 Brandon Furtwangler, Nathan Furtwangler
-*
-* Original source Box2D:
-* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com 
-* 
-* This software is provided 'as-is', without any express or implied 
-* warranty.  In no event will the authors be held liable for any damages 
-* arising from the use of this software. 
-* Permission is granted to anyone to use this software for any purpose, 
-* including commercial applications, and to alter it and redistribute it 
-* freely, subject to the following restrictions: 
-* 1. The origin of this software must not be misrepresented; you must not 
-* claim that you wrote the original software. If you use this software 
-* in a product, an acknowledgment in the product documentation would be 
-* appreciated but is not required. 
-* 2. Altered source versions must be plainly marked as such, and must not be 
-* misrepresented as being the original software. 
-* 3. This notice may not be removed or altered from any source distribution. 
-*/
-
-using System;
-using System.Diagnostics;
-using FarseerPhysics.Common;
-using Microsoft.Xna.Framework;
-
-namespace FarseerPhysics.Dynamics.Joints
-{
-    public class LineJoint : Joint
-    {
-        private Vector2 _ax, _ay;
-        private float _bias;
-        private bool _enableMotor;
-        private float _gamma;
-        private float _impulse;
-        private Vector2 _localXAxis;
-        private Vector2 _localYAxisA;
-        private float _mass;
-        private float _maxMotorTorque;
-        private float _motorImpulse;
-        private float _motorMass;
-        private float _motorSpeed;
-
-        private float _sAx;
-        private float _sAy;
-        private float _sBx;
-        private float _sBy;
-
-        private float _springImpulse;
-        private float _springMass;
-
-        // Linear constraint (point-to-line)
-        // d = pB - pA = xB + rB - xA - rA
-        // C = dot(ay, d)
-        // Cdot = dot(d, cross(wA, ay)) + dot(ay, vB + cross(wB, rB) - vA - cross(wA, rA))
-        //      = -dot(ay, vA) - dot(cross(d + rA, ay), wA) + dot(ay, vB) + dot(cross(rB, ay), vB)
-        // J = [-ay, -cross(d + rA, ay), ay, cross(rB, ay)]
-
-        // Spring linear constraint
-        // C = dot(ax, d)
-        // Cdot = = -dot(ax, vA) - dot(cross(d + rA, ax), wA) + dot(ax, vB) + dot(cross(rB, ax), vB)
-        // J = [-ax -cross(d+rA, ax) ax cross(rB, ax)]
-
-        // Motor rotational constraint
-        // Cdot = wB - wA
-        // J = [0 0 -1 0 0 1]
-
-        internal LineJoint()
-        {
-            JointType = JointType.Line;
-        }
-
-        public LineJoint(Body bA, Body bB, Vector2 anchor, Vector2 axis)
-            : base(bA, bB)
-        {
-            JointType = JointType.Line;
-
-            LocalAnchorA = bA.GetLocalPoint(anchor);
-            LocalAnchorB = bB.GetLocalPoint(anchor);
-            LocalXAxis = bA.GetLocalVector(axis);
-        }
-
-        public Vector2 LocalAnchorA { get; set; }
-
-        public Vector2 LocalAnchorB { get; set; }
-
-        public override Vector2 WorldAnchorA
-        {
-            get { return BodyA.GetWorldPoint(LocalAnchorA); }
-        }
-
-        public override Vector2 WorldAnchorB
-        {
-            get { return BodyB.GetWorldPoint(LocalAnchorB); }
-            set { Debug.Assert(false, "You can't set the world anchor on this joint type."); }
-        }
-
-        public float JointTranslation
-        {
-            get
-            {
-                Body bA = BodyA;
-                Body bB = BodyB;
-
-                Vector2 pA = bA.GetWorldPoint(LocalAnchorA);
-                Vector2 pB = bB.GetWorldPoint(LocalAnchorB);
-                Vector2 d = pB - pA;
-                Vector2 axis = bA.GetWorldVector(LocalXAxis);
-
-                float translation = Vector2.Dot(d, axis);
-                return translation;
-            }
-        }
-
-        public float JointSpeed
-        {
-            get
-            {
-                float wA = BodyA.AngularVelocityInternal;
-                float wB = BodyB.AngularVelocityInternal;
-                return wB - wA;
-            }
-        }
-
-        public bool MotorEnabled
-        {
-            get { return _enableMotor; }
-            set
-            {
-                BodyA.Awake = true;
-                BodyB.Awake = true;
-                _enableMotor = value;
-            }
-        }
-
-        public float MotorSpeed
-        {
-            set
-            {
-                BodyA.Awake = true;
-                BodyB.Awake = true;
-                _motorSpeed = value;
-            }
-            get { return _motorSpeed; }
-        }
-
-        public float MaxMotorTorque
-        {
-            set
-            {
-                BodyA.Awake = true;
-                BodyB.Awake = true;
-                _maxMotorTorque = value;
-            }
-            get { return _maxMotorTorque; }
-        }
-
-        public float Frequency { get; set; }
-
-        public float DampingRatio { get; set; }
-
-        public Vector2 LocalXAxis
-        {
-            get { return _localXAxis; }
-            set
-            {
-                _localXAxis = value;
-                _localYAxisA = MathUtils.Cross(1.0f, _localXAxis);
-            }
-        }
-
-        public override Vector2 GetReactionForce(float invDt)
-        {
-            return invDt * (_impulse * _ay + _springImpulse * _ax);
-        }
-
-        public override float GetReactionTorque(float invDt)
-        {
-            return invDt * _motorImpulse;
-        }
-
-        internal override void InitVelocityConstraints(ref TimeStep step)
-        {
-            Body bA = BodyA;
-            Body bB = BodyB;
-
-            LocalCenterA = bA.LocalCenter;
-            LocalCenterB = bB.LocalCenter;
-
-            Transform xfA;
-            bA.GetTransform(out xfA);
-            Transform xfB;
-            bB.GetTransform(out xfB);
-
-            // Compute the effective masses.
-            Vector2 rA = MathUtils.Multiply(ref xfA.R, LocalAnchorA - LocalCenterA);
-            Vector2 rB = MathUtils.Multiply(ref xfB.R, LocalAnchorB - LocalCenterB);
-            Vector2 d = bB.Sweep.C + rB - bA.Sweep.C - rA;
-
-            InvMassA = bA.InvMass;
-            InvIA = bA.InvI;
-            InvMassB = bB.InvMass;
-            InvIB = bB.InvI;
-
-            // Point to line constraint
-            {
-                _ay = MathUtils.Multiply(ref xfA.R, _localYAxisA);
-                _sAy = MathUtils.Cross(d + rA, _ay);
-                _sBy = MathUtils.Cross(rB, _ay);
-
-                _mass = InvMassA + InvMassB + InvIA * _sAy * _sAy + InvIB * _sBy * _sBy;
-
-                if (_mass > 0.0f)
-                {
-                    _mass = 1.0f / _mass;
-                }
-            }
-
-            // Spring constraint
-            _springMass = 0.0f;
-            if (Frequency > 0.0f)
-            {
-                _ax = MathUtils.Multiply(ref xfA.R, LocalXAxis);
-                _sAx = MathUtils.Cross(d + rA, _ax);
-                _sBx = MathUtils.Cross(rB, _ax);
-
-                float invMass = InvMassA + InvMassB + InvIA * _sAx * _sAx + InvIB * _sBx * _sBx;
-
-                if (invMass > 0.0f)
-                {
-                    _springMass = 1.0f / invMass;
-
-                    float C = Vector2.Dot(d, _ax);
-
-                    // Frequency
-                    float omega = 2.0f * Settings.Pi * Frequency;
-
-                    // Damping coefficient
-                    float da = 2.0f * _springMass * DampingRatio * omega;
-
-                    // Spring stiffness
-                    float k = _springMass * omega * omega;
-
-                    // magic formulas
-                    _gamma = step.dt * (da + step.dt * k);
-                    if (_gamma > 0.0f)
-                    {
-                        _gamma = 1.0f / _gamma;
-                    }
-
-                    _bias = C * step.dt * k * _gamma;
-
-                    _springMass = invMass + _gamma;
-                    if (_springMass > 0.0f)
-                    {
-                        _springMass = 1.0f / _springMass;
-                    }
-                }
-            }
-            else
-            {
-                _springImpulse = 0.0f;
-                _springMass = 0.0f;
-            }
-
-            // Rotational motor
-            if (_enableMotor)
-            {
-                _motorMass = InvIA + InvIB;
-                if (_motorMass > 0.0f)
-                {
-                    _motorMass = 1.0f / _motorMass;
-                }
-            }
-            else
-            {
-                _motorMass = 0.0f;
-                _motorImpulse = 0.0f;
-            }
-
-            if (Settings.EnableWarmstarting)
-            {
-                // Account for variable time step.
-                _impulse *= step.dtRatio;
-                _springImpulse *= step.dtRatio;
-                _motorImpulse *= step.dtRatio;
-
-                Vector2 P = _impulse * _ay + _springImpulse * _ax;
-                float LA = _impulse * _sAy + _springImpulse * _sAx + _motorImpulse;
-                float LB = _impulse * _sBy + _springImpulse * _sBx + _motorImpulse;
-
-                bA.LinearVelocityInternal -= InvMassA * P;
-                bA.AngularVelocityInternal -= InvIA * LA;
-
-                bB.LinearVelocityInternal += InvMassB * P;
-                bB.AngularVelocityInternal += InvIB * LB;
-            }
-            else
-            {
-                _impulse = 0.0f;
-                _springImpulse = 0.0f;
-                _motorImpulse = 0.0f;
-            }
-        }
-
-        internal override void SolveVelocityConstraints(ref TimeStep step)
-        {
-            Body bA = BodyA;
-            Body bB = BodyB;
-
-            Vector2 vA = bA.LinearVelocity;
-            float wA = bA.AngularVelocityInternal;
-            Vector2 vB = bB.LinearVelocityInternal;
-            float wB = bB.AngularVelocityInternal;
-
-            // Solve spring constraint
-            {
-                float Cdot = Vector2.Dot(_ax, vB - vA) + _sBx * wB - _sAx * wA;
-                float impulse = -_springMass * (Cdot + _bias + _gamma * _springImpulse);
-                _springImpulse += impulse;
-
-                Vector2 P = impulse * _ax;
-                float LA = impulse * _sAx;
-                float LB = impulse * _sBx;
-
-                vA -= InvMassA * P;
-                wA -= InvIA * LA;
-
-                vB += InvMassB * P;
-                wB += InvIB * LB;
-            }
-
-            // Solve rotational motor constraint
-            {
-                float Cdot = wB - wA - _motorSpeed;
-                float impulse = -_motorMass * Cdot;
-
-                float oldImpulse = _motorImpulse;
-                float maxImpulse = step.dt * _maxMotorTorque;
-                _motorImpulse = MathUtils.Clamp(_motorImpulse + impulse, -maxImpulse, maxImpulse);
-                impulse = _motorImpulse - oldImpulse;
-
-                wA -= InvIA * impulse;
-                wB += InvIB * impulse;
-            }
-
-            // Solve point to line constraint
-            {
-                float Cdot = Vector2.Dot(_ay, vB - vA) + _sBy * wB - _sAy * wA;
-                float impulse = _mass * (-Cdot);
-                _impulse += impulse;
-
-                Vector2 P = impulse * _ay;
-                float LA = impulse * _sAy;
-                float LB = impulse * _sBy;
-
-                vA -= InvMassA * P;
-                wA -= InvIA * LA;
-
-                vB += InvMassB * P;
-                wB += InvIB * LB;
-            }
-
-            bA.LinearVelocityInternal = vA;
-            bA.AngularVelocityInternal = wA;
-            bB.LinearVelocityInternal = vB;
-            bB.AngularVelocityInternal = wB;
-        }
-
-        internal override bool SolvePositionConstraints()
-        {
-            Body bA = BodyA;
-            Body bB = BodyB;
-
-            Vector2 xA = bA.Sweep.C;
-            float angleA = bA.Sweep.A;
-
-            Vector2 xB = bB.Sweep.C;
-            float angleB = bB.Sweep.A;
-
-            Mat22 RA = new Mat22(angleA);
-            Mat22 RB = new Mat22(angleB);
-
-            Vector2 rA = MathUtils.Multiply(ref RA, LocalAnchorA - LocalCenterA);
-            Vector2 rB = MathUtils.Multiply(ref RB, LocalAnchorB - LocalCenterB);
-            Vector2 d = xB + rB - xA - rA;
-
-            Vector2 ay = MathUtils.Multiply(ref RA, _localYAxisA);
-
-            float sAy = MathUtils.Cross(d + rA, ay);
-            float sBy = MathUtils.Cross(rB, ay);
-
-            float C = Vector2.Dot(d, ay);
-
-            float k = InvMassA + InvMassB + InvIA * _sAy * _sAy + InvIB * _sBy * _sBy;
-
-            float impulse;
-            if (k != 0.0f)
-            {
-                impulse = -C / k;
-            }
-            else
-            {
-                impulse = 0.0f;
-            }
-
-            Vector2 P = impulse * ay;
-            float LA = impulse * sAy;
-            float LB = impulse * sBy;
-
-            xA -= InvMassA * P;
-            angleA -= InvIA * LA;
-            xB += InvMassB * P;
-            angleB += InvIB * LB;
-
-            // TODO_ERIN remove need for this.
-            bA.Sweep.C = xA;
-            bA.Sweep.A = angleA;
-            bB.Sweep.C = xB;
-            bB.Sweep.A = angleB;
-            bA.SynchronizeTransform();
-            bB.SynchronizeTransform();
-
-            return Math.Abs(C) <= Settings.LinearSlop;
-        }
-
-        public float GetMotorTorque(float invDt)
-        {
-            return invDt * _motorImpulse;
-        }
-    }
-}

FarseerPhysicsEngine/Dynamics/Joints/PrismaticJoint.cs

@@ -1,677 +0,0 @@
-/*
-* Farseer Physics Engine based on Box2D.XNA port:
-* Copyright (c) 2010 Ian Qvist
-* 
-* Box2D.XNA port of Box2D:
-* Copyright (c) 2009 Brandon Furtwangler, Nathan Furtwangler
-*
-* Original source Box2D:
-* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com 
-* 
-* This software is provided 'as-is', without any express or implied 
-* warranty.  In no event will the authors be held liable for any damages 
-* arising from the use of this software. 
-* Permission is granted to anyone to use this software for any purpose, 
-* including commercial applications, and to alter it and redistribute it 
-* freely, subject to the following restrictions: 
-* 1. The origin of this software must not be misrepresented; you must not 
-* claim that you wrote the original software. If you use this software 
-* in a product, an acknowledgment in the product documentation would be 
-* appreciated but is not required. 
-* 2. Altered source versions must be plainly marked as such, and must not be 
-* misrepresented as being the original software. 
-* 3. This notice may not be removed or altered from any source distribution. 
-*/
-
-using System;
-using System.Diagnostics;
-using FarseerPhysics.Common;
-using Microsoft.Xna.Framework;
-
-namespace FarseerPhysics.Dynamics.Joints
-{
-    // Linear constraint (point-to-line)
-    // d = p2 - p1 = x2 + r2 - x1 - r1
-    // C = dot(perp, d)
-    // Cdot = dot(d, cross(w1, perp)) + dot(perp, v2 + cross(w2, r2) - v1 - cross(w1, r1))
-    //      = -dot(perp, v1) - dot(cross(d + r1, perp), w1) + dot(perp, v2) + dot(cross(r2, perp), v2)
-    // J = [-perp, -cross(d + r1, perp), perp, cross(r2,perp)]
-    //
-    // Angular constraint
-    // C = a2 - a1 + a_initial
-    // Cdot = w2 - w1
-    // J = [0 0 -1 0 0 1]
-    //
-    // K = J * invM * JT
-    //
-    // J = [-a -s1 a s2]
-    //     [0  -1  0  1]
-    // a = perp
-    // s1 = cross(d + r1, a) = cross(p2 - x1, a)
-    // s2 = cross(r2, a) = cross(p2 - x2, a)
-    // Motor/Limit linear constraint
-    // C = dot(ax1, d)
-    // Cdot = = -dot(ax1, v1) - dot(cross(d + r1, ax1), w1) + dot(ax1, v2) + dot(cross(r2, ax1), v2)
-    // J = [-ax1 -cross(d+r1,ax1) ax1 cross(r2,ax1)]
-    // Block Solver
-    // We develop a block solver that includes the joint limit. This makes the limit stiff (inelastic) even
-    // when the mass has poor distribution (leading to large torques about the joint anchor points).
-    //
-    // The Jacobian has 3 rows:
-    // J = [-uT -s1 uT s2] // linear
-    //     [0   -1   0  1] // angular
-    //     [-vT -a1 vT a2] // limit
-    //
-    // u = perp
-    // v = axis
-    // s1 = cross(d + r1, u), s2 = cross(r2, u)
-    // a1 = cross(d + r1, v), a2 = cross(r2, v)
-    // M * (v2 - v1) = JT * df
-    // J * v2 = bias
-    //
-    // v2 = v1 + invM * JT * df
-    // J * (v1 + invM * JT * df) = bias
-    // K * df = bias - J * v1 = -Cdot
-    // K = J * invM * JT
-    // Cdot = J * v1 - bias
-    //
-    // Now solve for f2.
-    // df = f2 - f1
-    // K * (f2 - f1) = -Cdot
-    // f2 = invK * (-Cdot) + f1
-    //
-    // Clamp accumulated limit impulse.
-    // lower: f2(3) = max(f2(3), 0)
-    // upper: f2(3) = min(f2(3), 0)
-    //
-    // Solve for correct f2(1:2)
-    // K(1:2, 1:2) * f2(1:2) = -Cdot(1:2) - K(1:2,3) * f2(3) + K(1:2,1:3) * f1
-    //                       = -Cdot(1:2) - K(1:2,3) * f2(3) + K(1:2,1:2) * f1(1:2) + K(1:2,3) * f1(3)
-    // K(1:2, 1:2) * f2(1:2) = -Cdot(1:2) - K(1:2,3) * (f2(3) - f1(3)) + K(1:2,1:2) * f1(1:2)
-    // f2(1:2) = invK(1:2,1:2) * (-Cdot(1:2) - K(1:2,3) * (f2(3) - f1(3))) + f1(1:2)
-    //
-    // Now compute impulse to be applied:
-    // df = f2 - f1
-
-    /// <summary>
-    /// A prismatic joint. This joint provides one degree of freedom: translation
-    /// along an axis fixed in body1. Relative rotation is prevented. You can
-    /// use a joint limit to restrict the range of motion and a joint motor to
-    /// drive the motion or to model joint friction.
-    /// </summary>
-    public class PrismaticJoint : Joint
-    {
-        public Vector2 LocalAnchorA;
-
-        public Vector2 LocalAnchorB;
-        private Mat33 _K;
-        private float _a1, _a2;
-        private Vector2 _axis;
-        private bool _enableLimit;
-        private bool _enableMotor;
-        private Vector3 _impulse;
-        private LimitState _limitState;
-        private Vector2 _localXAxis1;
-        private Vector2 _localYAxis1;
-        private float _lowerTranslation;
-        private float _maxMotorForce;
-        private float _motorImpulse;
-        private float _motorMass; // effective mass for motor/limit translational constraint.
-        private float _motorSpeed;
-        private Vector2 _perp;
-        private float _refAngle;
-        private float _s1, _s2;
-        private float _upperTranslation;
-
-        internal PrismaticJoint()
-        {
-            JointType = JointType.Prismatic;
-        }
-
-        /// <summary>
-        /// This requires defining a line of
-        /// motion using an axis and an anchor point. The definition uses local
-        /// anchor points and a local axis so that the initial configuration
-        /// can violate the constraint slightly. The joint translation is zero
-        /// when the local anchor points coincide in world space. Using local
-        /// anchors and a local axis helps when saving and loading a game.
-        /// </summary>
-        /// <param name="bodyA">The first body.</param>
-        /// <param name="bodyB">The second body.</param>
-        /// <param name="localAnchorA">The first body anchor.</param>
-        /// <param name="localAnchorB">The second body anchor.</param>
-        /// <param name="axis">The axis.</param>
-        public PrismaticJoint(Body bodyA, Body bodyB, Vector2 localAnchorA, Vector2 localAnchorB, Vector2 axis)
-            : base(bodyA, bodyB)
-        {
-            JointType = JointType.Prismatic;
-
-            LocalAnchorA = localAnchorA;
-            LocalAnchorB = localAnchorB;
-
-            _localXAxis1 = BodyA.GetLocalVector(axis);
-            _localYAxis1 = MathUtils.Cross(1.0f, _localXAxis1);
-            _refAngle = BodyB.Rotation - BodyA.Rotation;
-
-            _limitState = LimitState.Inactive;
-        }
-
-        public override Vector2 WorldAnchorA
-        {
-            get { return BodyA.GetWorldPoint(LocalAnchorA); }
-        }
-
-        public override Vector2 WorldAnchorB
-        {
-            get { return BodyB.GetWorldPoint(LocalAnchorB); }
-            set { Debug.Assert(false, "You can't set the world anchor on this joint type."); }
-        }
-
-        /// <summary>
-        /// Get the current joint translation, usually in meters.
-        /// </summary>
-        /// <value></value>
-        public float JointTranslation
-        {
-            get
-            {
-                Vector2 d = BodyB.GetWorldPoint(LocalAnchorB) - BodyA.GetWorldPoint(LocalAnchorA);
-                Vector2 axis = BodyA.GetWorldVector(ref _localXAxis1);
-
-                return Vector2.Dot(d, axis);
-            }
-        }
-
-        /// <summary>
-        /// Get the current joint translation speed, usually in meters per second.
-        /// </summary>
-        /// <value></value>
-        public float JointSpeed
-        {
-            get
-            {
-                Transform xf1, xf2;
-                BodyA.GetTransform(out xf1);
-                BodyB.GetTransform(out xf2);
-
-                Vector2 r1 = MathUtils.Multiply(ref xf1.R, LocalAnchorA - BodyA.LocalCenter);
-                Vector2 r2 = MathUtils.Multiply(ref xf2.R, LocalAnchorB - BodyB.LocalCenter);
-                Vector2 p1 = BodyA.Sweep.C + r1;
-                Vector2 p2 = BodyB.Sweep.C + r2;
-                Vector2 d = p2 - p1;
-                Vector2 axis = BodyA.GetWorldVector(ref _localXAxis1);
-
-                Vector2 v1 = BodyA.LinearVelocityInternal;
-                Vector2 v2 = BodyB.LinearVelocityInternal;
-                float w1 = BodyA.AngularVelocityInternal;
-                float w2 = BodyB.AngularVelocityInternal;
-
-                float speed = Vector2.Dot(d, MathUtils.Cross(w1, axis)) +
-                              Vector2.Dot(axis, v2 + MathUtils.Cross(w2, r2) - v1 - MathUtils.Cross(w1, r1));
-                return speed;
-            }
-        }
-
-        /// <summary>
-        /// Is the joint limit enabled?
-        /// </summary>
-        /// <value><c>true</c> if [limit enabled]; otherwise, <c>false</c>.</value>
-        public bool LimitEnabled
-        {
-            get { return _enableLimit; }
-            set
-            {
-                Debug.Assert(BodyA.FixedRotation == false || BodyB.FixedRotation == false,
-                             "Warning: limits does currently not work with fixed rotation");
-
-                WakeBodies();
-                _enableLimit = value;
-            }
-        }
-
-        /// <summary>
-        /// Get the lower joint limit, usually in meters.
-        /// </summary>
-        /// <value></value>
-        public float LowerLimit
-        {
-            get { return _lowerTranslation; }
-            set
-            {
-                WakeBodies();
-                _lowerTranslation = value;
-            }
-        }
-
-        /// <summary>
-        /// Get the upper joint limit, usually in meters.
-        /// </summary>
-        /// <value></value>
-        public float UpperLimit
-        {
-            get { return _upperTranslation; }
-            set
-            {
-                WakeBodies();
-                _upperTranslation = value;
-            }
-        }
-
-        /// <summary>
-        /// Is the joint motor enabled?
-        /// </summary>
-        /// <value><c>true</c> if [motor enabled]; otherwise, <c>false</c>.</value>
-        public bool MotorEnabled
-        {
-            get { return _enableMotor; }
-            set
-            {
-                WakeBodies();
-                _enableMotor = value;
-            }
-        }
-
-        /// <summary>
-        /// Set the motor speed, usually in meters per second.
-        /// </summary>
-        /// <value>The speed.</value>
-        public float MotorSpeed
-        {
-            set
-            {
-                WakeBodies();
-                _motorSpeed = value;
-            }
-            get { return _motorSpeed; }
-        }
-
-        /// <summary>
-        /// Set the maximum motor force, usually in N.
-        /// </summary>
-        /// <value>The force.</value>
-        public float MaxMotorForce
-        {
-            get { return _maxMotorForce; }
-            set
-            {
-                WakeBodies();
-                _maxMotorForce = value;
-            }
-        }
-
-        /// <summary>
-        /// Get the current motor force, usually in N.
-        /// </summary>
-        /// <value></value>
-        public float MotorForce
-        {
-            get { return _motorImpulse; }
-            set { _motorImpulse = value; }
-        }
-
-        public Vector2 LocalXAxis1
-        {
-            get { return _localXAxis1; }
-            set
-            {
-                _localXAxis1 = BodyA.GetLocalVector(value);
-                _localYAxis1 = MathUtils.Cross(1.0f, _localXAxis1);
-            }
-        }
-
-        public float ReferenceAngle
-        {
-            get { return _refAngle; }
-            set { _refAngle = value; }
-        }
-
-        public override Vector2 GetReactionForce(float inv_dt)
-        {
-            return inv_dt * (_impulse.X * _perp + (_motorImpulse + _impulse.Z) * _axis);
-        }
-
-        public override float GetReactionTorque(float inv_dt)
-        {
-            return inv_dt * _impulse.Y;
-        }
-
-        internal override void InitVelocityConstraints(ref TimeStep step)
-        {
-            Body b1 = BodyA;
-            Body b2 = BodyB;
-
-            LocalCenterA = b1.LocalCenter;
-            LocalCenterB = b2.LocalCenter;
-
-            Transform xf1, xf2;
-            b1.GetTransform(out xf1);
-            b2.GetTransform(out xf2);
-
-            // Compute the effective masses.
-            Vector2 r1 = MathUtils.Multiply(ref xf1.R, LocalAnchorA - LocalCenterA);
-            Vector2 r2 = MathUtils.Multiply(ref xf2.R, LocalAnchorB - LocalCenterB);
-            Vector2 d = b2.Sweep.C + r2 - b1.Sweep.C - r1;
-
-            InvMassA = b1.InvMass;
-            InvIA = b1.InvI;
-            InvMassB = b2.InvMass;
-            InvIB = b2.InvI;
-
-            // Compute motor Jacobian and effective mass.
-            {
-                _axis = MathUtils.Multiply(ref xf1.R, _localXAxis1);
-                _a1 = MathUtils.Cross(d + r1, _axis);
-                _a2 = MathUtils.Cross(r2, _axis);
-
-                _motorMass = InvMassA + InvMassB + InvIA * _a1 * _a1 + InvIB * _a2 * _a2;
-
-                if (_motorMass > Settings.Epsilon)
-                {
-                    _motorMass = 1.0f / _motorMass;
-                }
-            }
-
-            // Prismatic constraint.
-            {
-                _perp = MathUtils.Multiply(ref xf1.R, _localYAxis1);
-
-                _s1 = MathUtils.Cross(d + r1, _perp);
-                _s2 = MathUtils.Cross(r2, _perp);
-
-                float m1 = InvMassA, m2 = InvMassB;
-                float i1 = InvIA, i2 = InvIB;
-
-                float k11 = m1 + m2 + i1 * _s1 * _s1 + i2 * _s2 * _s2;
-                float k12 = i1 * _s1 + i2 * _s2;
-                float k13 = i1 * _s1 * _a1 + i2 * _s2 * _a2;
-                float k22 = i1 + i2;
-                float k23 = i1 * _a1 + i2 * _a2;
-                float k33 = m1 + m2 + i1 * _a1 * _a1 + i2 * _a2 * _a2;
-
-                _K.Col1 = new Vector3(k11, k12, k13);
-                _K.Col2 = new Vector3(k12, k22, k23);
-                _K.Col3 = new Vector3(k13, k23, k33);
-            }
-
-            // Compute motor and limit terms.
-            if (_enableLimit)
-            {
-                float jointTranslation = Vector2.Dot(_axis, d);
-                if (Math.Abs(_upperTranslation - _lowerTranslation) < 2.0f * Settings.LinearSlop)
-                {
-                    _limitState = LimitState.Equal;
-                }
-                else if (jointTranslation <= _lowerTranslation)
-                {
-                    if (_limitState != LimitState.AtLower)
-                    {
-                        _limitState = LimitState.AtLower;
-                        _impulse.Z = 0.0f;
-                    }
-                }
-                else if (jointTranslation >= _upperTranslation)
-                {
-                    if (_limitState != LimitState.AtUpper)
-                    {
-                        _limitState = LimitState.AtUpper;
-                        _impulse.Z = 0.0f;
-                    }
-                }
-                else
-                {
-                    _limitState = LimitState.Inactive;
-                    _impulse.Z = 0.0f;
-                }
-            }
-            else
-            {
-                _limitState = LimitState.Inactive;
-            }
-
-            if (_enableMotor == false)
-            {
-                _motorImpulse = 0.0f;
-            }
-
-            if (Settings.EnableWarmstarting)
-            {
-                // Account for variable time step.
-                _impulse *= step.dtRatio;
-                _motorImpulse *= step.dtRatio;
-
-                Vector2 P = _impulse.X * _perp + (_motorImpulse + _impulse.Z) * _axis;
-                float L1 = _impulse.X * _s1 + _impulse.Y + (_motorImpulse + _impulse.Z) * _a1;
-                float L2 = _impulse.X * _s2 + _impulse.Y + (_motorImpulse + _impulse.Z) * _a2;
-
-                b1.LinearVelocityInternal -= InvMassA * P;
-                b1.AngularVelocityInternal -= InvIA * L1;
-
-                b2.LinearVelocityInternal += InvMassB * P;
-                b2.AngularVelocityInternal += InvIB * L2;
-            }
-            else
-            {
-                _impulse = Vector3.Zero;
-                _motorImpulse = 0.0f;
-            }
-        }
-
-        internal override void SolveVelocityConstraints(ref TimeStep step)
-        {
-            Body b1 = BodyA;
-            Body b2 = BodyB;
-
-            Vector2 v1 = b1.LinearVelocityInternal;
-            float w1 = b1.AngularVelocityInternal;
-            Vector2 v2 = b2.LinearVelocityInternal;
-            float w2 = b2.AngularVelocityInternal;
-
-            // Solve linear motor constraint.
-            if (_enableMotor && _limitState != LimitState.Equal)
-            {
-                float Cdot = Vector2.Dot(_axis, v2 - v1) + _a2 * w2 - _a1 * w1;
-                float impulse = _motorMass * (_motorSpeed - Cdot);
-                float oldImpulse = _motorImpulse;
-                float maxImpulse = step.dt * _maxMotorForce;
-                _motorImpulse = MathUtils.Clamp(_motorImpulse + impulse, -maxImpulse, maxImpulse);
-                impulse = _motorImpulse - oldImpulse;
-
-                Vector2 P = impulse * _axis;
-                float L1 = impulse * _a1;
-                float L2 = impulse * _a2;
-
-                v1 -= InvMassA * P;
-                w1 -= InvIA * L1;
-
-                v2 += InvMassB * P;
-                w2 += InvIB * L2;
-            }
-
-            Vector2 Cdot1 = new Vector2(Vector2.Dot(_perp, v2 - v1) + _s2 * w2 - _s1 * w1, w2 - w1);
-
-            if (_enableLimit && _limitState != LimitState.Inactive)
-            {
-                // Solve prismatic and limit constraint in block form.
-                float Cdot2 = Vector2.Dot(_axis, v2 - v1) + _a2 * w2 - _a1 * w1;
-                Vector3 Cdot = new Vector3(Cdot1.X, Cdot1.Y, Cdot2);
-
-                Vector3 f1 = _impulse;
-                Vector3 df = _K.Solve33(-Cdot);
-                _impulse += df;
-
-                if (_limitState == LimitState.AtLower)
-                {
-                    _impulse.Z = Math.Max(_impulse.Z, 0.0f);
-                }
-                else if (_limitState == LimitState.AtUpper)
-                {
-                    _impulse.Z = Math.Min(_impulse.Z, 0.0f);
-                }
-
-                // f2(1:2) = invK(1:2,1:2) * (-Cdot(1:2) - K(1:2,3) * (f2(3) - f1(3))) + f1(1:2)
-                Vector2 b = -Cdot1 - (_impulse.Z - f1.Z) * new Vector2(_K.Col3.X, _K.Col3.Y);
-                Vector2 f2r = _K.Solve22(b) + new Vector2(f1.X, f1.Y);
-                _impulse.X = f2r.X;
-                _impulse.Y = f2r.Y;
-
-                df = _impulse - f1;
-
-                Vector2 P = df.X * _perp + df.Z * _axis;
-                float L1 = df.X * _s1 + df.Y + df.Z * _a1;
-                float L2 = df.X * _s2 + df.Y + df.Z * _a2;
-
-                v1 -= InvMassA * P;
-                w1 -= InvIA * L1;
-
-                v2 += InvMassB * P;
-                w2 += InvIB * L2;
-            }
-            else
-            {
-                // Limit is inactive, just solve the prismatic constraint in block form.
-                Vector2 df = _K.Solve22(-Cdot1);
-                _impulse.X += df.X;
-                _impulse.Y += df.Y;
-
-                Vector2 P = df.X * _perp;
-                float L1 = df.X * _s1 + df.Y;
-                float L2 = df.X * _s2 + df.Y;
-
-                v1 -= InvMassA * P;
-                w1 -= InvIA * L1;
-
-                v2 += InvMassB * P;
-                w2 += InvIB * L2;
-            }
-
-            b1.LinearVelocityInternal = v1;
-            b1.AngularVelocityInternal = w1;
-            b2.LinearVelocityInternal = v2;
-            b2.AngularVelocityInternal = w2;
-        }
-
-        internal override bool SolvePositionConstraints()
-        {
-            Body b1 = BodyA;
-            Body b2 = BodyB;
-
-            Vector2 c1 = b1.Sweep.C;
-            float a1 = b1.Sweep.A;
-
-            Vector2 c2 = b2.Sweep.C;
-            float a2 = b2.Sweep.A;
-
-            // Solve linear limit constraint.
-            float linearError = 0.0f;
-            bool active = false;
-            float C2 = 0.0f;
-
-            Mat22 R1 = new Mat22(a1);
-            Mat22 R2 = new Mat22(a2);
-
-            Vector2 r1 = MathUtils.Multiply(ref R1, LocalAnchorA - LocalCenterA);
-            Vector2 r2 = MathUtils.Multiply(ref R2, LocalAnchorB - LocalCenterB);
-            Vector2 d = c2 + r2 - c1 - r1;
-
-            if (_enableLimit)
-            {
-                _axis = MathUtils.Multiply(ref R1, _localXAxis1);
-
-                _a1 = MathUtils.Cross(d + r1, _axis);
-                _a2 = MathUtils.Cross(r2, _axis);
-
-                float translation = Vector2.Dot(_axis, d);
-                if (Math.Abs(_upperTranslation - _lowerTranslation) < 2.0f * Settings.LinearSlop)
-                {
-                    // Prevent large angular corrections
-                    C2 = MathUtils.Clamp(translation, -Settings.MaxLinearCorrection, Settings.MaxLinearCorrection);
-                    linearError = Math.Abs(translation);
-                    active = true;
-                }
-                else if (translation <= _lowerTranslation)
-                {
-                    // Prevent large linear corrections and allow some slop.
-                    C2 = MathUtils.Clamp(translation - _lowerTranslation + Settings.LinearSlop,
-                                         -Settings.MaxLinearCorrection, 0.0f);
-                    linearError = _lowerTranslation - translation;
-                    active = true;
-                }
-                else if (translation >= _upperTranslation)
-                {
-                    // Prevent large linear corrections and allow some slop.
-                    C2 = MathUtils.Clamp(translation - _upperTranslation - Settings.LinearSlop, 0.0f,
-                                         Settings.MaxLinearCorrection);
-                    linearError = translation - _upperTranslation;
-                    active = true;
-                }
-            }
-
-            _perp = MathUtils.Multiply(ref R1, _localYAxis1);
-
-            _s1 = MathUtils.Cross(d + r1, _perp);
-            _s2 = MathUtils.Cross(r2, _perp);
-
-            Vector3 impulse;
-            Vector2 C1 = new Vector2(Vector2.Dot(_perp, d), a2 - a1 - ReferenceAngle);
-
-            linearError = Math.Max(linearError, Math.Abs(C1.X));
-            float angularError = Math.Abs(C1.Y);
-
-            if (active)
-            {
-                float m1 = InvMassA, m2 = InvMassB;
-                float i1 = InvIA, i2 = InvIB;
-
-                float k11 = m1 + m2 + i1 * _s1 * _s1 + i2 * _s2 * _s2;
-                float k12 = i1 * _s1 + i2 * _s2;
-                float k13 = i1 * _s1 * _a1 + i2 * _s2 * _a2;
-                float k22 = i1 + i2;
-                float k23 = i1 * _a1 + i2 * _a2;
-                float k33 = m1 + m2 + i1 * _a1 * _a1 + i2 * _a2 * _a2;
-
-                _K.Col1 = new Vector3(k11, k12, k13);
-                _K.Col2 = new Vector3(k12, k22, k23);
-                _K.Col3 = new Vector3(k13, k23, k33);
-
-                Vector3 C = new Vector3(-C1.X, -C1.Y, -C2);
-                impulse = _K.Solve33(C); // negated above
-            }
-            else
-            {
-                float m1 = InvMassA, m2 = InvMassB;
-                float i1 = InvIA, i2 = InvIB;
-
-                float k11 = m1 + m2 + i1 * _s1 * _s1 + i2 * _s2 * _s2;
-                float k12 = i1 * _s1 + i2 * _s2;
-                float k22 = i1 + i2;
-
-                _K.Col1 = new Vector3(k11, k12, 0.0f);
-                _K.Col2 = new Vector3(k12, k22, 0.0f);
-
-                Vector2 impulse1 = _K.Solve22(-C1);
-                impulse.X = impulse1.X;
-                impulse.Y = impulse1.Y;
-                impulse.Z = 0.0f;
-            }
-
-            Vector2 P = impulse.X * _perp + impulse.Z * _axis;
-            float L1 = impulse.X * _s1 + impulse.Y + impulse.Z * _a1;
-            float L2 = impulse.X * _s2 + impulse.Y + impulse.Z * _a2;
-
-            c1 -= InvMassA * P;
-            a1 -= InvIA * L1;
-            c2 += InvMassB * P;
-            a2 += InvIB * L2;
-
-            // TODO_ERIN remove need for this.
-            b1.Sweep.C = c1;
-            b1.Sweep.A = a1;
-            b2.Sweep.C = c2;
-            b2.Sweep.A = a2;
-            b1.SynchronizeTransform();
-            b2.SynchronizeTransform();
-
-            return linearError <= Settings.LinearSlop && angularError <= Settings.AngularSlop;
-        }
-    }
-}

FarseerPhysicsEngine/Dynamics/Joints/PulleyJoint.cs

@@ -1,507 +0,0 @@
-/*
-* Farseer Physics Engine based on Box2D.XNA port:
-* Copyright (c) 2010 Ian Qvist
-* 
-* Box2D.XNA port of Box2D:
-* Copyright (c) 2009 Brandon Furtwangler, Nathan Furtwangler
-*
-* Original source Box2D:
-* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com 
-* 
-* This software is provided 'as-is', without any express or implied 
-* warranty.  In no event will the authors be held liable for any damages 
-* arising from the use of this software. 
-* Permission is granted to anyone to use this software for any purpose, 
-* including commercial applications, and to alter it and redistribute it 
-* freely, subject to the following restrictions: 
-* 1. The origin of this software must not be misrepresented; you must not 
-* claim that you wrote the original software. If you use this software 
-* in a product, an acknowledgment in the product documentation would be 
-* appreciated but is not required. 
-* 2. Altered source versions must be plainly marked as such, and must not be 
-* misrepresented as being the original software. 
-* 3. This notice may not be removed or altered from any source distribution. 
-*/
-
-using System;
-using System.Diagnostics;
-using FarseerPhysics.Common;
-using Microsoft.Xna.Framework;
-
-namespace FarseerPhysics.Dynamics.Joints
-{
-    /// <summary>
-    /// The pulley joint is connected to two bodies and two fixed ground points.
-    /// The pulley supports a ratio such that:
-    /// length1 + ratio * length2 <!--<-->= ant
-    /// Yes, the force transmitted is scaled by the ratio.
-    /// The pulley also enforces a maximum length limit on both sides. This is
-    /// useful to prevent one side of the pulley hitting the top.
-    /// </summary>
-    public class PulleyJoint : Joint
-    {
-        /// <summary>
-        /// Get the first ground anchor.
-        /// </summary>
-        /// <value></value>
-        public Vector2 GroundAnchorA;
-
-        /// <summary>
-        /// Get the second ground anchor.
-        /// </summary>
-        /// <value></value>
-        public Vector2 GroundAnchorB;
-
-        public Vector2 LocalAnchorA;
-        public Vector2 LocalAnchorB;
-
-        public float MinPulleyLength = 2.0f;
-        private float _ant;
-        private float _impulse;
-        private float _lengthA;
-        private float _lengthB;
-        private float _limitImpulse1;
-        private float _limitImpulse2;
-        private float _limitMass1;
-        private float _limitMass2;
-        private LimitState _limitState1;
-        private LimitState _limitState2;
-        private float _maxLengthA;
-        private float _maxLengthB;
-
-        // Effective masses
-        private float _pulleyMass;
-        private LimitState _state;
-        private Vector2 _u1;
-        private Vector2 _u2;
-
-        internal PulleyJoint()
-        {
-            JointType = JointType.Pulley;
-        }
-
-        /// <summary>
-        /// Initialize the bodies, anchors, lengths, max lengths, and ratio using the world anchors.
-        /// This requires two ground anchors,
-        /// two dynamic body anchor points, max lengths for each side,
-        /// and a pulley ratio.
-        /// </summary>
-        /// <param name="bodyA">The first body.</param>
-        /// <param name="bodyB">The second body.</param>
-        /// <param name="groundAnchorA">The ground anchor for the first body.</param>
-        /// <param name="groundAnchorB">The ground anchor for the second body.</param>
-        /// <param name="localAnchorA">The first body anchor.</param>
-        /// <param name="localAnchorB">The second body anchor.</param>
-        /// <param name="ratio">The ratio.</param>
-        public PulleyJoint(Body bodyA, Body bodyB,
-                           Vector2 groundAnchorA, Vector2 groundAnchorB,
-                           Vector2 localAnchorA, Vector2 localAnchorB,
-                           float ratio)
-            : base(bodyA, bodyB)
-        {
-            JointType = JointType.Pulley;
-
-            GroundAnchorA = groundAnchorA;
-            GroundAnchorB = groundAnchorB;
-            LocalAnchorA = localAnchorA;
-            LocalAnchorB = localAnchorB;
-
-            Vector2 d1 = BodyA.GetWorldPoint(localAnchorA) - groundAnchorA;
-            _lengthA = d1.Length();
-
-            Vector2 d2 = BodyB.GetWorldPoint(localAnchorB) - groundAnchorB;
-            _lengthB = d2.Length();
-
-            Debug.Assert(ratio != 0.0f);
-            Debug.Assert(ratio > Settings.Epsilon);
-            Ratio = ratio;
-
-            float C = _lengthA + Ratio * _lengthB;
-
-            MaxLengthA = C - Ratio * MinPulleyLength;
-            MaxLengthB = (C - MinPulleyLength) / Ratio;
-
-            _ant = _lengthA + Ratio * _lengthB;
-
-            MaxLengthA = Math.Min(MaxLengthA, _ant - Ratio * MinPulleyLength);
-            MaxLengthB = Math.Min(MaxLengthB, (_ant - MinPulleyLength) / Ratio);
-
-            _impulse = 0.0f;
-            _limitImpulse1 = 0.0f;
-            _limitImpulse2 = 0.0f;
-        }
-
-        public override Vector2 WorldAnchorA
-        {
-            get { return BodyA.GetWorldPoint(LocalAnchorA); }
-        }
-
-        public override Vector2 WorldAnchorB
-        {
-            get { return BodyB.GetWorldPoint(LocalAnchorB); }
-            set { Debug.Assert(false, "You can't set the world anchor on this joint type."); }
-        }
-
-        /// <summary>
-        /// Get the current length of the segment attached to body1.
-        /// </summary>
-        /// <value></value>
-        public float LengthA
-        {
-            get
-            {
-                Vector2 d = BodyA.GetWorldPoint(LocalAnchorA) - GroundAnchorA;
-                return d.Length();
-            }
-            set { _lengthA = value; }
-        }
-
-        /// <summary>
-        /// Get the current length of the segment attached to body2.
-        /// </summary>
-        /// <value></value>
-        public float LengthB
-        {
-            get
-            {
-                Vector2 d = BodyB.GetWorldPoint(LocalAnchorB) - GroundAnchorB;
-                return d.Length();
-            }
-            set { _lengthB = value; }
-        }
-
-        /// <summary>
-        /// Get the pulley ratio.
-        /// </summary>
-        /// <value></value>
-        public float Ratio { get; set; }
-
-        public float MaxLengthA
-        {
-            get { return _maxLengthA; }
-            set { _maxLengthA = value; }
-        }
-
-        public float MaxLengthB
-        {
-            get { return _maxLengthB; }
-            set { _maxLengthB = value; }
-        }
-
-        public override Vector2 GetReactionForce(float inv_dt)
-        {
-            Vector2 P = _impulse * _u2;
-            return inv_dt * P;
-        }
-
-        public override float GetReactionTorque(float inv_dt)
-        {
-            return 0.0f;
-        }
-
-        internal override void InitVelocityConstraints(ref TimeStep step)
-        {
-            Body b1 = BodyA;
-            Body b2 = BodyB;
-
-            Transform xf1, xf2;
-            b1.GetTransform(out xf1);
-            b2.GetTransform(out xf2);
-
-            Vector2 r1 = MathUtils.Multiply(ref xf1.R, LocalAnchorA - b1.LocalCenter);
-            Vector2 r2 = MathUtils.Multiply(ref xf2.R, LocalAnchorB - b2.LocalCenter);
-
-            Vector2 p1 = b1.Sweep.C + r1;
-            Vector2 p2 = b2.Sweep.C + r2;
-
-            Vector2 s1 = GroundAnchorA;
-            Vector2 s2 = GroundAnchorB;
-
-            // Get the pulley axes.
-            _u1 = p1 - s1;
-            _u2 = p2 - s2;
-
-            float length1 = _u1.Length();
-            float length2 = _u2.Length();
-
-            if (length1 > Settings.LinearSlop)
-            {
-                _u1 *= 1.0f / length1;
-            }
-            else
-            {
-                _u1 = Vector2.Zero;
-            }
-
-            if (length2 > Settings.LinearSlop)
-            {
-                _u2 *= 1.0f / length2;
-            }
-            else
-            {
-                _u2 = Vector2.Zero;
-            }
-
-            float C = _ant - length1 - Ratio * length2;
-            if (C > 0.0f)
-            {
-                _state = LimitState.Inactive;
-                _impulse = 0.0f;
-            }
-            else
-            {
-                _state = LimitState.AtUpper;
-            }
-
-            if (length1 < MaxLengthA)
-            {
-                _limitState1 = LimitState.Inactive;
-                _limitImpulse1 = 0.0f;
-            }
-            else
-            {
-                _limitState1 = LimitState.AtUpper;
-            }
-
-            if (length2 < MaxLengthB)
-            {
-                _limitState2 = LimitState.Inactive;
-                _limitImpulse2 = 0.0f;
-            }
-            else
-            {
-                _limitState2 = LimitState.AtUpper;
-            }
-
-            // Compute effective mass.
-            float cr1u1 = MathUtils.Cross(r1, _u1);
-            float cr2u2 = MathUtils.Cross(r2, _u2);
-
-            _limitMass1 = b1.InvMass + b1.InvI * cr1u1 * cr1u1;
-            _limitMass2 = b2.InvMass + b2.InvI * cr2u2 * cr2u2;
-            _pulleyMass = _limitMass1 + Ratio * Ratio * _limitMass2;
-            Debug.Assert(_limitMass1 > Settings.Epsilon);
-            Debug.Assert(_limitMass2 > Settings.Epsilon);
-            Debug.Assert(_pulleyMass > Settings.Epsilon);
-            _limitMass1 = 1.0f / _limitMass1;
-            _limitMass2 = 1.0f / _limitMass2;
-            _pulleyMass = 1.0f / _pulleyMass;
-
-            if (Settings.EnableWarmstarting)
-            {
-                // Scale impulses to support variable time steps.
-                _impulse *= step.dtRatio;
-                _limitImpulse1 *= step.dtRatio;
-                _limitImpulse2 *= step.dtRatio;
-
-                // Warm starting.
-                Vector2 P1 = -(_impulse + _limitImpulse1) * _u1;
-                Vector2 P2 = (-Ratio * _impulse - _limitImpulse2) * _u2;
-                b1.LinearVelocityInternal += b1.InvMass * P1;
-                b1.AngularVelocityInternal += b1.InvI * MathUtils.Cross(r1, P1);
-                b2.LinearVelocityInternal += b2.InvMass * P2;
-                b2.AngularVelocityInternal += b2.InvI * MathUtils.Cross(r2, P2);
-            }
-            else
-            {
-                _impulse = 0.0f;
-                _limitImpulse1 = 0.0f;
-                _limitImpulse2 = 0.0f;
-            }
-        }
-
-        internal override void SolveVelocityConstraints(ref TimeStep step)
-        {
-            Body b1 = BodyA;
-            Body b2 = BodyB;
-
-            Transform xf1, xf2;
-            b1.GetTransform(out xf1);
-            b2.GetTransform(out xf2);
-
-            Vector2 r1 = MathUtils.Multiply(ref xf1.R, LocalAnchorA - b1.LocalCenter);
-            Vector2 r2 = MathUtils.Multiply(ref xf2.R, LocalAnchorB - b2.LocalCenter);
-
-            if (_state == LimitState.AtUpper)
-            {
-                Vector2 v1 = b1.LinearVelocityInternal + MathUtils.Cross(b1.AngularVelocityInternal, r1);
-                Vector2 v2 = b2.LinearVelocityInternal + MathUtils.Cross(b2.AngularVelocityInternal, r2);
-
-                float Cdot = -Vector2.Dot(_u1, v1) - Ratio * Vector2.Dot(_u2, v2);
-                float impulse = _pulleyMass * (-Cdot);
-                float oldImpulse = _impulse;
-                _impulse = Math.Max(0.0f, _impulse + impulse);
-                impulse = _impulse - oldImpulse;
-
-                Vector2 P1 = -impulse * _u1;
-                Vector2 P2 = -Ratio * impulse * _u2;
-                b1.LinearVelocityInternal += b1.InvMass * P1;
-                b1.AngularVelocityInternal += b1.InvI * MathUtils.Cross(r1, P1);
-                b2.LinearVelocityInternal += b2.InvMass * P2;
-                b2.AngularVelocityInternal += b2.InvI * MathUtils.Cross(r2, P2);
-            }
-
-            if (_limitState1 == LimitState.AtUpper)
-            {
-                Vector2 v1 = b1.LinearVelocityInternal + MathUtils.Cross(b1.AngularVelocityInternal, r1);
-
-                float Cdot = -Vector2.Dot(_u1, v1);
-                float impulse = -_limitMass1 * Cdot;
-                float oldImpulse = _limitImpulse1;
-                _limitImpulse1 = Math.Max(0.0f, _limitImpulse1 + impulse);
-                impulse = _limitImpulse1 - oldImpulse;
-
-                Vector2 P1 = -impulse * _u1;
-                b1.LinearVelocityInternal += b1.InvMass * P1;
-                b1.AngularVelocityInternal += b1.InvI * MathUtils.Cross(r1, P1);
-            }
-
-            if (_limitState2 == LimitState.AtUpper)
-            {
-                Vector2 v2 = b2.LinearVelocityInternal + MathUtils.Cross(b2.AngularVelocityInternal, r2);
-
-                float Cdot = -Vector2.Dot(_u2, v2);
-                float impulse = -_limitMass2 * Cdot;
-                float oldImpulse = _limitImpulse2;
-                _limitImpulse2 = Math.Max(0.0f, _limitImpulse2 + impulse);
-                impulse = _limitImpulse2 - oldImpulse;
-
-                Vector2 P2 = -impulse * _u2;
-                b2.LinearVelocityInternal += b2.InvMass * P2;
-                b2.AngularVelocityInternal += b2.InvI * MathUtils.Cross(r2, P2);
-            }
-        }
-
-        internal override bool SolvePositionConstraints()
-        {
-            Body b1 = BodyA;
-            Body b2 = BodyB;
-
-            Vector2 s1 = GroundAnchorA;
-            Vector2 s2 = GroundAnchorB;
-
-            float linearError = 0.0f;
-
-            if (_state == LimitState.AtUpper)
-            {
-                Transform xf1, xf2;
-                b1.GetTransform(out xf1);
-                b2.GetTransform(out xf2);
-
-                Vector2 r1 = MathUtils.Multiply(ref xf1.R, LocalAnchorA - b1.LocalCenter);
-                Vector2 r2 = MathUtils.Multiply(ref xf2.R, LocalAnchorB - b2.LocalCenter);
-
-                Vector2 p1 = b1.Sweep.C + r1;
-                Vector2 p2 = b2.Sweep.C + r2;
-
-                // Get the pulley axes.
-                _u1 = p1 - s1;
-                _u2 = p2 - s2;
-
-                float length1 = _u1.Length();
-                float length2 = _u2.Length();
-
-                if (length1 > Settings.LinearSlop)
-                {
-                    _u1 *= 1.0f / length1;
-                }
-                else
-                {
-                    _u1 = Vector2.Zero;
-                }
-
-                if (length2 > Settings.LinearSlop)
-                {
-                    _u2 *= 1.0f / length2;
-                }
-                else
-                {
-                    _u2 = Vector2.Zero;
-                }
-
-                float C = _ant - length1 - Ratio * length2;
-                linearError = Math.Max(linearError, -C);
-
-                C = MathUtils.Clamp(C + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0.0f);
-                float impulse = -_pulleyMass * C;
-
-                Vector2 P1 = -impulse * _u1;
-                Vector2 P2 = -Ratio * impulse * _u2;
-
-                b1.Sweep.C += b1.InvMass * P1;
-                b1.Sweep.A += b1.InvI * MathUtils.Cross(r1, P1);
-                b2.Sweep.C += b2.InvMass * P2;
-                b2.Sweep.A += b2.InvI * MathUtils.Cross(r2, P2);
-
-                b1.SynchronizeTransform();
-                b2.SynchronizeTransform();
-            }
-
-            if (_limitState1 == LimitState.AtUpper)
-            {
-                Transform xf1;
-                b1.GetTransform(out xf1);
-
-                Vector2 r1 = MathUtils.Multiply(ref xf1.R, LocalAnchorA - b1.LocalCenter);
-                Vector2 p1 = b1.Sweep.C + r1;
-
-                _u1 = p1 - s1;
-                float length1 = _u1.Length();
-
-                if (length1 > Settings.LinearSlop)
-                {
-                    _u1 *= 1.0f / length1;
-                }
-                else
-                {
-                    _u1 = Vector2.Zero;
-                }
-
-                float C = MaxLengthA - length1;
-                linearError = Math.Max(linearError, -C);
-                C = MathUtils.Clamp(C + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0.0f);
-                float impulse = -_limitMass1 * C;
-
-                Vector2 P1 = -impulse * _u1;
-                b1.Sweep.C += b1.InvMass * P1;
-                b1.Sweep.A += b1.InvI * MathUtils.Cross(r1, P1);
-
-                b1.SynchronizeTransform();
-            }
-
-            if (_limitState2 == LimitState.AtUpper)
-            {
-                Transform xf2;
-                b2.GetTransform(out xf2);
-
-                Vector2 r2 = MathUtils.Multiply(ref xf2.R, LocalAnchorB - b2.LocalCenter);
-                Vector2 p2 = b2.Sweep.C + r2;
-
-                _u2 = p2 - s2;
-                float length2 = _u2.Length();
-
-                if (length2 > Settings.LinearSlop)
-                {
-                    _u2 *= 1.0f / length2;
-                }
-                else
-                {
-                    _u2 = Vector2.Zero;
-                }
-
-                float C = MaxLengthB - length2;
-                linearError = Math.Max(linearError, -C);
-                C = MathUtils.Clamp(C + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0.0f);
-                float impulse = -_limitMass2 * C;
-
-                Vector2 P2 = -impulse * _u2;
-                b2.Sweep.C += b2.InvMass * P2;
-                b2.Sweep.A += b2.InvI * MathUtils.Cross(r2, P2);
-
-                b2.SynchronizeTransform();
-            }
-
-            return linearError < Settings.LinearSlop;
-        }
-    }
-}

FarseerPhysicsEngine/Dynamics/Joints/RevoluteJoint.cs

@@ -1,595 +0,0 @@
-/*
-* Farseer Physics Engine based on Box2D.XNA port:
-* Copyright (c) 2010 Ian Qvist
-* 
-* Box2D.XNA port of Box2D:
-* Copyright (c) 2009 Brandon Furtwangler, Nathan Furtwangler
-*
-* Original source Box2D:
-* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com 
-* 
-* This software is provided 'as-is', without any express or implied 
-* warranty.  In no event will the authors be held liable for any damages 
-* arising from the use of this software. 
-* Permission is granted to anyone to use this software for any purpose, 
-* including commercial applications, and to alter it and redistribute it 
-* freely, subject to the following restrictions: 
-* 1. The origin of this software must not be misrepresented; you must not 
-* claim that you wrote the original software. If you use this software 
-* in a product, an acknowledgment in the product documentation would be 
-* appreciated but is not required. 
-* 2. Altered source versions must be plainly marked as such, and must not be 
-* misrepresented as being the original software. 
-* 3. This notice may not be removed or altered from any source distribution. 
-*/
-
-using System;
-using System.Diagnostics;
-using FarseerPhysics.Common;
-using Microsoft.Xna.Framework;
-
-namespace FarseerPhysics.Dynamics.Joints
-{
-    /// <summary>
-    /// A revolute joint rains to bodies to share a common point while they
-    /// are free to rotate about the point. The relative rotation about the shared
-    /// point is the joint angle. You can limit the relative rotation with
-    /// a joint limit that specifies a lower and upper angle. You can use a motor
-    /// to drive the relative rotation about the shared point. A maximum motor torque
-    /// is provided so that infinite forces are not generated.
-    /// </summary>
-    public class RevoluteJoint : Joint
-    {
-        public Vector2 LocalAnchorA;
-
-        public Vector2 LocalAnchorB;
-        private bool _enableLimit;
-        private bool _enableMotor;
-        private Vector3 _impulse;
-        private LimitState _limitState;
-        private float _lowerAngle;
-        private Mat33 _mass; // effective mass for point-to-point constraint.
-        private float _maxMotorTorque;
-        private float _motorImpulse;
-        private float _motorMass; // effective mass for motor/limit angular constraint.
-        private float _motorSpeed;
-        private float _referenceAngle;
-        private float _tmpFloat1;
-        private Vector2 _tmpVector1, _tmpVector2;
-        private float _upperAngle;
-
-        internal RevoluteJoint()
-        {
-            JointType = JointType.Revolute;
-        }
-
-        /// <summary>
-        /// Initialize the bodies and local anchor.
-        /// This requires defining an
-        /// anchor point where the bodies are joined. The definition
-        /// uses local anchor points so that the initial configuration
-        /// can violate the constraint slightly. You also need to
-        /// specify the initial relative angle for joint limits. This
-        /// helps when saving and loading a game.
-        /// The local anchor points are measured from the body's origin
-        /// rather than the center of mass because:
-        /// 1. you might not know where the center of mass will be.
-        /// 2. if you add/remove shapes from a body and recompute the mass,
-        /// the joints will be broken.
-        /// </summary>
-        /// <param name="bodyA">The first body.</param>
-        /// <param name="bodyB">The second body.</param>
-        /// <param name="localAnchorA">The first body anchor.</param>
-        /// <param name="localAnchorB">The second anchor.</param>
-        public RevoluteJoint(Body bodyA, Body bodyB, Vector2 localAnchorA, Vector2 localAnchorB)
-            : base(bodyA, bodyB)
-        {
-            JointType = JointType.Revolute;
-
-            // Changed to local coordinates.
-            LocalAnchorA = localAnchorA;
-            LocalAnchorB = localAnchorB;
-
-            ReferenceAngle = BodyB.Rotation - BodyA.Rotation;
-
-            _impulse = Vector3.Zero;
-
-            _limitState = LimitState.Inactive;
-        }
-
-        public override Vector2 WorldAnchorA
-        {
-            get { return BodyA.GetWorldPoint(LocalAnchorA); }
-        }
-
-        public override Vector2 WorldAnchorB
-        {
-            get { return BodyB.GetWorldPoint(LocalAnchorB); }
-            set { Debug.Assert(false, "You can't set the world anchor on this joint type."); }
-        }
-
-        public float ReferenceAngle
-        {
-            get { return _referenceAngle; }
-            set
-            {
-                WakeBodies();
-                _referenceAngle = value;
-            }
-        }
-
-        /// <summary>
-        /// Get the current joint angle in radians.
-        /// </summary>
-        /// <value></value>
-        public float JointAngle
-        {
-            get { return BodyB.Sweep.A - BodyA.Sweep.A - ReferenceAngle; }
-        }
-
-        /// <summary>
-        /// Get the current joint angle speed in radians per second.
-        /// </summary>
-        /// <value></value>
-        public float JointSpeed
-        {
-            get { return BodyB.AngularVelocityInternal - BodyA.AngularVelocityInternal; }
-        }
-
-        /// <summary>
-        /// Is the joint limit enabled?
-        /// </summary>
-        /// <value><c>true</c> if [limit enabled]; otherwise, <c>false</c>.</value>
-        public bool LimitEnabled
-        {
-            get { return _enableLimit; }
-            set
-            {
-                WakeBodies();
-                _enableLimit = value;
-            }
-        }
-
-        /// <summary>
-        /// Get the lower joint limit in radians.
-        /// </summary>
-        /// <value></value>
-        public float LowerLimit
-        {
-            get { return _lowerAngle; }
-            set
-            {
-                WakeBodies();
-                _lowerAngle = value;
-            }
-        }
-
-        /// <summary>
-        /// Get the upper joint limit in radians.
-        /// </summary>
-        /// <value></value>
-        public float UpperLimit
-        {
-            get { return _upperAngle; }
-            set
-            {
-                WakeBodies();
-                _upperAngle = value;
-            }
-        }
-
-        /// <summary>
-        /// Is the joint motor enabled?
-        /// </summary>
-        /// <value><c>true</c> if [motor enabled]; otherwise, <c>false</c>.</value>
-        public bool MotorEnabled
-        {
-            get { return _enableMotor; }
-            set
-            {
-                WakeBodies();
-                _enableMotor = value;
-            }
-        }
-
-        /// <summary>
-        /// Set the motor speed in radians per second.
-        /// </summary>
-        /// <value>The speed.</value>
-        public float MotorSpeed
-        {
-            set
-            {
-                WakeBodies();
-                _motorSpeed = value;
-            }
-            get { return _motorSpeed; }
-        }
-
-        /// <summary>
-        /// Set the maximum motor torque, usually in N-m.
-        /// </summary>
-        /// <value>The torque.</value>
-        public float MaxMotorTorque
-        {
-            set
-            {
-                WakeBodies();
-                _maxMotorTorque = value;
-            }
-            get { return _maxMotorTorque; }
-        }
-
-        /// <summary>
-        /// Get the current motor torque, usually in N-m.
-        /// </summary>
-        /// <value></value>
-        public float MotorTorque
-        {
-            get { return _motorImpulse; }
-            set
-            {
-                WakeBodies();
-                _motorImpulse = value;
-            }
-        }
-
-        public override Vector2 GetReactionForce(float inv_dt)
-        {
-            Vector2 P = new Vector2(_impulse.X, _impulse.Y);
-            return inv_dt * P;
-        }
-
-        public override float GetReactionTorque(float inv_dt)
-        {
-            return inv_dt * _impulse.Z;
-        }
-
-        internal override void InitVelocityConstraints(ref TimeStep step)
-        {
-            Body b1 = BodyA;
-            Body b2 = BodyB;
-
-            if (_enableMotor || _enableLimit)
-            {
-                // You cannot create a rotation limit between bodies that
-                // both have fixed rotation.
-                Debug.Assert(b1.InvI > 0.0f || b2.InvI > 0.0f);
-            }
-
-            // Compute the effective mass matrix.
-            /*Transform xf1, xf2;
-            b1.GetTransform(out xf1);
-            b2.GetTransform(out xf2);*/
-
-            Vector2 r1 = MathUtils.Multiply(ref b1.Xf.R, LocalAnchorA - b1.LocalCenter);
-            Vector2 r2 = MathUtils.Multiply(ref b2.Xf.R, LocalAnchorB - b2.LocalCenter);
-
-            // J = [-I -r1_skew I r2_skew]
-            //     [ 0       -1 0       1]
-            // r_skew = [-ry; rx]
-
-            // Matlab
-            // K = [ m1+r1y^2*i1+m2+r2y^2*i2,  -r1y*i1*r1x-r2y*i2*r2x,          -r1y*i1-r2y*i2]
-            //     [  -r1y*i1*r1x-r2y*i2*r2x, m1+r1x^2*i1+m2+r2x^2*i2,           r1x*i1+r2x*i2]
-            //     [          -r1y*i1-r2y*i2,           r1x*i1+r2x*i2,                   i1+i2]
-
-            float m1 = b1.InvMass, m2 = b2.InvMass;
-            float i1 = b1.InvI, i2 = b2.InvI;
-
-            _mass.Col1.X = m1 + m2 + r1.Y * r1.Y * i1 + r2.Y * r2.Y * i2;
-            _mass.Col2.X = -r1.Y * r1.X * i1 - r2.Y * r2.X * i2;
-            _mass.Col3.X = -r1.Y * i1 - r2.Y * i2;
-            _mass.Col1.Y = _mass.Col2.X;
-            _mass.Col2.Y = m1 + m2 + r1.X * r1.X * i1 + r2.X * r2.X * i2;
-            _mass.Col3.Y = r1.X * i1 + r2.X * i2;
-            _mass.Col1.Z = _mass.Col3.X;
-            _mass.Col2.Z = _mass.Col3.Y;
-            _mass.Col3.Z = i1 + i2;
-
-            _motorMass = i1 + i2;
-            if (_motorMass > 0.0f)
-            {
-                _motorMass = 1.0f / _motorMass;
-            }
-
-            if (_enableMotor == false)
-            {
-                _motorImpulse = 0.0f;
-            }
-
-            if (_enableLimit)
-            {
-                float jointAngle = b2.Sweep.A - b1.Sweep.A - ReferenceAngle;
-                if (Math.Abs(_upperAngle - _lowerAngle) < 2.0f * Settings.AngularSlop)
-                {
-                    _limitState = LimitState.Equal;
-                }
-                else if (jointAngle <= _lowerAngle)
-                {
-                    if (_limitState != LimitState.AtLower)
-                    {
-                        _impulse.Z = 0.0f;
-                    }
-                    _limitState = LimitState.AtLower;
-                }
-                else if (jointAngle >= _upperAngle)
-                {
-                    if (_limitState != LimitState.AtUpper)
-                    {
-                        _impulse.Z = 0.0f;
-                    }
-                    _limitState = LimitState.AtUpper;
-                }
-                else
-                {
-                    _limitState = LimitState.Inactive;
-                    _impulse.Z = 0.0f;
-                }
-            }
-            else
-            {
-                _limitState = LimitState.Inactive;
-            }
-
-            if (Settings.EnableWarmstarting)
-            {
-                // Scale impulses to support a variable time step.
-                _impulse *= step.dtRatio;
-                _motorImpulse *= step.dtRatio;
-
-                Vector2 P = new Vector2(_impulse.X, _impulse.Y);
-
-                b1.LinearVelocityInternal -= m1 * P;
-                MathUtils.Cross(ref r1, ref P, out _tmpFloat1);
-                b1.AngularVelocityInternal -= i1 * ( /* r1 x P */_tmpFloat1 + _motorImpulse + _impulse.Z);
-
-                b2.LinearVelocityInternal += m2 * P;
-                MathUtils.Cross(ref r2, ref P, out _tmpFloat1);
-                b2.AngularVelocityInternal += i2 * ( /* r2 x P */_tmpFloat1 + _motorImpulse + _impulse.Z);
-            }
-            else
-            {
-                _impulse = Vector3.Zero;
-                _motorImpulse = 0.0f;
-            }
-        }
-
-        internal override void SolveVelocityConstraints(ref TimeStep step)
-        {
-            Body b1 = BodyA;
-            Body b2 = BodyB;
-
-            Vector2 v1 = b1.LinearVelocityInternal;
-            float w1 = b1.AngularVelocityInternal;
-            Vector2 v2 = b2.LinearVelocityInternal;
-            float w2 = b2.AngularVelocityInternal;
-
-            float m1 = b1.InvMass, m2 = b2.InvMass;
-            float i1 = b1.InvI, i2 = b2.InvI;
-
-            // Solve motor constraint.
-            if (_enableMotor && _limitState != LimitState.Equal)
-            {
-                float Cdot = w2 - w1 - _motorSpeed;
-                float impulse = _motorMass * (-Cdot);
-                float oldImpulse = _motorImpulse;
-                float maxImpulse = step.dt * _maxMotorTorque;
-                _motorImpulse = MathUtils.Clamp(_motorImpulse + impulse, -maxImpulse, maxImpulse);
-                impulse = _motorImpulse - oldImpulse;
-
-                w1 -= i1 * impulse;
-                w2 += i2 * impulse;
-            }
-
-            // Solve limit constraint.
-            if (_enableLimit && _limitState != LimitState.Inactive)
-            {
-                /*Transform xf1, xf2;
-                b1.GetTransform(out xf1);
-                b2.GetTransform(out xf2);*/
-
-                Vector2 r1 = MathUtils.Multiply(ref b1.Xf.R, LocalAnchorA - b1.LocalCenter);
-                Vector2 r2 = MathUtils.Multiply(ref b2.Xf.R, LocalAnchorB - b2.LocalCenter);
-
-                // Solve point-to-point constraint
-                MathUtils.Cross(w2, ref r2, out _tmpVector2);
-                MathUtils.Cross(w1, ref r1, out _tmpVector1);
-                Vector2 Cdot1 = v2 + /* w2 x r2 */ _tmpVector2 - v1 - /* w1 x r1 */ _tmpVector1;
-                float Cdot2 = w2 - w1;
-                Vector3 Cdot = new Vector3(Cdot1.X, Cdot1.Y, Cdot2);
-
-                Vector3 impulse = _mass.Solve33(-Cdot);
-
-                if (_limitState == LimitState.Equal)
-                {
-                    _impulse += impulse;
-                }
-                else if (_limitState == LimitState.AtLower)
-                {
-                    float newImpulse = _impulse.Z + impulse.Z;
-                    if (newImpulse < 0.0f)
-                    {
-                        Vector2 reduced = _mass.Solve22(-Cdot1);
-                        impulse.X = reduced.X;
-                        impulse.Y = reduced.Y;
-                        impulse.Z = -_impulse.Z;
-                        _impulse.X += reduced.X;
-                        _impulse.Y += reduced.Y;
-                        _impulse.Z = 0.0f;
-                    }
-                }
-                else if (_limitState == LimitState.AtUpper)
-                {
-                    float newImpulse = _impulse.Z + impulse.Z;
-                    if (newImpulse > 0.0f)
-                    {
-                        Vector2 reduced = _mass.Solve22(-Cdot1);
-                        impulse.X = reduced.X;
-                        impulse.Y = reduced.Y;
-                        impulse.Z = -_impulse.Z;
-                        _impulse.X += reduced.X;
-                        _impulse.Y += reduced.Y;
-                        _impulse.Z = 0.0f;
-                    }
-                }
-
-                Vector2 P = new Vector2(impulse.X, impulse.Y);
-
-                v1 -= m1 * P;
-                MathUtils.Cross(ref r1, ref P, out _tmpFloat1);
-                w1 -= i1 * ( /* r1 x P */_tmpFloat1 + impulse.Z);
-
-                v2 += m2 * P;
-                MathUtils.Cross(ref r2, ref P, out _tmpFloat1);
-                w2 += i2 * ( /* r2 x P */_tmpFloat1 + impulse.Z);
-            }
-            else
-            {
-                /*Transform xf1, xf2;
-                b1.GetTransform(out xf1);
-                b2.GetTransform(out xf2);*/
-
-                _tmpVector1 = LocalAnchorA - b1.LocalCenter;
-                _tmpVector2 = LocalAnchorB - b2.LocalCenter;
-                Vector2 r1 = MathUtils.Multiply(ref b1.Xf.R, ref _tmpVector1);
-                Vector2 r2 = MathUtils.Multiply(ref b2.Xf.R, ref _tmpVector2);
-
-                // Solve point-to-point constraint
-                MathUtils.Cross(w2, ref r2, out _tmpVector2);
-                MathUtils.Cross(w1, ref r1, out _tmpVector1);
-                Vector2 Cdot = v2 + /* w2 x r2 */ _tmpVector2 - v1 - /* w1 x r1 */ _tmpVector1;
-                Vector2 impulse = _mass.Solve22(-Cdot);
-
-                _impulse.X += impulse.X;
-                _impulse.Y += impulse.Y;
-
-                v1 -= m1 * impulse;
-                MathUtils.Cross(ref r1, ref impulse, out _tmpFloat1);
-                w1 -= i1 * /* r1 x impulse */ _tmpFloat1;
-
-                v2 += m2 * impulse;
-                MathUtils.Cross(ref r2, ref impulse, out _tmpFloat1);
-                w2 += i2 * /* r2 x impulse */ _tmpFloat1;
-            }
-
-            b1.LinearVelocityInternal = v1;
-            b1.AngularVelocityInternal = w1;
-            b2.LinearVelocityInternal = v2;
-            b2.AngularVelocityInternal = w2;
-        }
-
-        internal override bool SolvePositionConstraints()
-        {
-            // TODO_ERIN block solve with limit. COME ON ERIN
-
-            Body b1 = BodyA;
-            Body b2 = BodyB;
-
-            float angularError = 0.0f;
-            float positionError;
-
-            // Solve angular limit constraint.
-            if (_enableLimit && _limitState != LimitState.Inactive)
-            {
-                float angle = b2.Sweep.A - b1.Sweep.A - ReferenceAngle;
-                float limitImpulse = 0.0f;
-
-                if (_limitState == LimitState.Equal)
-                {
-                    // Prevent large angular corrections
-                    float C = MathUtils.Clamp(angle - _lowerAngle, -Settings.MaxAngularCorrection,
-                                              Settings.MaxAngularCorrection);
-                    limitImpulse = -_motorMass * C;
-                    angularError = Math.Abs(C);
-                }
-                else if (_limitState == LimitState.AtLower)
-                {
-                    float C = angle - _lowerAngle;
-                    angularError = -C;
-
-                    // Prevent large angular corrections and allow some slop.
-                    C = MathUtils.Clamp(C + Settings.AngularSlop, -Settings.MaxAngularCorrection, 0.0f);
-                    limitImpulse = -_motorMass * C;
-                }
-                else if (_limitState == LimitState.AtUpper)
-                {
-                    float C = angle - _upperAngle;
-                    angularError = C;
-
-                    // Prevent large angular corrections and allow some slop.
-                    C = MathUtils.Clamp(C - Settings.AngularSlop, 0.0f, Settings.MaxAngularCorrection);
-                    limitImpulse = -_motorMass * C;
-                }
-
-                b1.Sweep.A -= b1.InvI * limitImpulse;
-                b2.Sweep.A += b2.InvI * limitImpulse;
-
-                b1.SynchronizeTransform();
-                b2.SynchronizeTransform();
-            }
-
-            // Solve point-to-point constraint.
-            {
-                /*Transform xf1, xf2;
-                b1.GetTransform(out xf1);
-                b2.GetTransform(out xf2);*/
-
-                Vector2 r1 = MathUtils.Multiply(ref b1.Xf.R, LocalAnchorA - b1.LocalCenter);
-                Vector2 r2 = MathUtils.Multiply(ref b2.Xf.R, LocalAnchorB - b2.LocalCenter);
-
-                Vector2 C = b2.Sweep.C + r2 - b1.Sweep.C - r1;
-                positionError = C.Length();
-
-                float invMass1 = b1.InvMass, invMass2 = b2.InvMass;
-                float invI1 = b1.InvI, invI2 = b2.InvI;
-
-                // Handle large detachment.
-                const float k_allowedStretch = 10.0f * Settings.LinearSlop;
-                if (C.LengthSquared() > k_allowedStretch * k_allowedStretch)
-                {
-                    // Use a particle solution (no rotation).
-                    Vector2 u = C;
-                    u.Normalize();
-                    float k = invMass1 + invMass2;
-                    Debug.Assert(k > Settings.Epsilon);
-                    float m = 1.0f / k;
-                    Vector2 impulse2 = m * (-C);
-                    const float k_beta = 0.5f;
-                    b1.Sweep.C -= k_beta * invMass1 * impulse2;
-                    b2.Sweep.C += k_beta * invMass2 * impulse2;
-
-                    C = b2.Sweep.C + r2 - b1.Sweep.C - r1;
-                }
-
-                Mat22 K1 = new Mat22(new Vector2(invMass1 + invMass2, 0.0f), new Vector2(0.0f, invMass1 + invMass2));
-                Mat22 K2 = new Mat22(new Vector2(invI1 * r1.Y * r1.Y, -invI1 * r1.X * r1.Y),
-                                     new Vector2(-invI1 * r1.X * r1.Y, invI1 * r1.X * r1.X));
-                Mat22 K3 = new Mat22(new Vector2(invI2 * r2.Y * r2.Y, -invI2 * r2.X * r2.Y),
-                                     new Vector2(-invI2 * r2.X * r2.Y, invI2 * r2.X * r2.X));
-
-                Mat22 Ka;
-                Mat22.Add(ref K1, ref K2, out Ka);
-
-                Mat22 K;
-                Mat22.Add(ref Ka, ref K3, out K);
-
-
-                Vector2 impulse = K.Solve(-C);
-
-                b1.Sweep.C -= b1.InvMass * impulse;
-                MathUtils.Cross(ref r1, ref impulse, out _tmpFloat1);
-                b1.Sweep.A -= b1.InvI * /* r1 x impulse */ _tmpFloat1;
-
-                b2.Sweep.C += b2.InvMass * impulse;
-                MathUtils.Cross(ref r2, ref impulse, out _tmpFloat1);
-                b2.Sweep.A += b2.InvI * /* r2 x impulse */ _tmpFloat1;
-
-                b1.SynchronizeTransform();
-                b2.SynchronizeTransform();
-            }
-
-            return positionError <= Settings.LinearSlop && angularError <= Settings.AngularSlop;
-        }
-    }
-}

FarseerPhysicsEngine/Dynamics/Joints/RopeJoint.cs

@@ -1,239 +0,0 @@
-/*
-* Copyright (c) 2006-2010 Erin Catto http://www.gphysics.com
-*
-* This software is provided 'as-is', without any express or implied
-* warranty.  In no event will the authors be held liable for any damages
-* arising from the use of this software.
-* Permission is granted to anyone to use this software for any purpose,
-* including commercial applications, and to alter it and redistribute it
-* freely, subject to the following restrictions:
-* 1. The origin of this software must not be misrepresented; you must not
-* claim that you wrote the original software. If you use this software
-* in a product, an acknowledgment in the product documentation would be
-* appreciated but is not required.
-* 2. Altered source versions must be plainly marked as such, and must not be
-* misrepresented as being the original software.
-* 3. This notice may not be removed or altered from any source distribution.
-*/
-
-using System;
-using System.Diagnostics;
-using FarseerPhysics.Common;
-using Microsoft.Xna.Framework;
-
-namespace FarseerPhysics.Dynamics.Joints
-{
-    // Limit:
-    // C = norm(pB - pA) - L
-    // u = (pB - pA) / norm(pB - pA)
-    // Cdot = dot(u, vB + cross(wB, rB) - vA - cross(wA, rA))
-    // J = [-u -cross(rA, u) u cross(rB, u)]
-    // K = J * invM * JT
-    //   = invMassA + invIA * cross(rA, u)^2 + invMassB + invIB * cross(rB, u)^2
-
-    /// <summary>
-    /// A rope joint enforces a maximum distance between two points
-    /// on two bodies. It has no other effect.
-    /// Warning: if you attempt to change the maximum length during
-    /// the simulation you will get some non-physical behavior.
-    /// A model that would allow you to dynamically modify the length
-    /// would have some sponginess, so I chose not to implement it
-    /// that way. See b2DistanceJoint if you want to dynamically
-    /// control length.
-    /// </summary>
-    public class RopeJoint : Joint
-    {
-        public Vector2 LocalAnchorA;
-        public Vector2 LocalAnchorB;
-
-        private float _impulse;
-        private float _length;
-
-        private float _mass;
-        private Vector2 _rA, _rB;
-        private LimitState _state;
-        private Vector2 _u;
-
-        internal RopeJoint()
-        {
-            JointType = JointType.Rope;
-        }
-
-        public RopeJoint(Body bodyA, Body bodyB, Vector2 localAnchorA, Vector2 localAnchorB)
-            : base(bodyA, bodyB)
-        {
-            JointType = JointType.Rope;
-            LocalAnchorA = localAnchorA;
-            LocalAnchorB = localAnchorB;
-
-            Vector2 d = WorldAnchorB - WorldAnchorA;
-            MaxLength = d.Length();
-
-            _mass = 0.0f;
-            _impulse = 0.0f;
-            _state = LimitState.Inactive;
-            _length = 0.0f;
-        }
-
-        /// Get the maximum length of the rope.
-        public float MaxLength { get; set; }
-
-        public LimitState State
-        {
-            get { return _state; }
-        }
-
-        public override sealed Vector2 WorldAnchorA
-        {
-            get { return BodyA.GetWorldPoint(LocalAnchorA); }
-        }
-
-        public override sealed Vector2 WorldAnchorB
-        {
-            get { return BodyB.GetWorldPoint(LocalAnchorB); }
-            set { Debug.Assert(false, "You can't set the world anchor on this joint type."); }
-        }
-
-        public override Vector2 GetReactionForce(float invDt)
-        {
-            return (invDt * _impulse) * _u;
-        }
-
-        public override float GetReactionTorque(float invDt)
-        {
-            return 0;
-        }
-
-        internal override void InitVelocityConstraints(ref TimeStep step)
-        {
-            Body bA = BodyA;
-            Body bB = BodyB;
-
-            Transform xf1;
-            bA.GetTransform(out xf1);
-
-            Transform xf2;
-            bB.GetTransform(out xf2);
-
-            _rA = MathUtils.Multiply(ref xf1.R, LocalAnchorA - bA.LocalCenter);
-            _rB = MathUtils.Multiply(ref xf2.R, LocalAnchorB - bB.LocalCenter);
-
-            // Rope axis
-            _u = bB.Sweep.C + _rB - bA.Sweep.C - _rA;
-
-            _length = _u.Length();
-
-            float C = _length - MaxLength;
-            if (C > 0.0f)
-            {
-                _state = LimitState.AtUpper;
-            }
-            else
-            {
-                _state = LimitState.Inactive;
-            }
-
-            if (_length > Settings.LinearSlop)
-            {
-                _u *= 1.0f / _length;
-            }
-            else
-            {
-                _u = Vector2.Zero;
-                _mass = 0.0f;
-                _impulse = 0.0f;
-                return;
-            }
-
-            // Compute effective mass.
-            float crA = MathUtils.Cross(_rA, _u);
-            float crB = MathUtils.Cross(_rB, _u);
-            float invMass = bA.InvMass + bA.InvI * crA * crA + bB.InvMass + bB.InvI * crB * crB;
-
-            _mass = invMass != 0.0f ? 1.0f / invMass : 0.0f;
-
-            if (Settings.EnableWarmstarting)
-            {
-                // Scale the impulse to support a variable time step.
-                _impulse *= step.dtRatio;
-
-                Vector2 P = _impulse * _u;
-                bA.LinearVelocity -= bA.InvMass * P;
-                bA.AngularVelocity -= bA.InvI * MathUtils.Cross(_rA, P);
-                bB.LinearVelocity += bB.InvMass * P;
-                bB.AngularVelocity += bB.InvI * MathUtils.Cross(_rB, P);
-            }
-            else
-            {
-                _impulse = 0.0f;
-            }
-        }
-
-        internal override void SolveVelocityConstraints(ref TimeStep step)
-        {
-            Body bA = BodyA;
-            Body bB = BodyB;
-
-            // Cdot = dot(u, v + cross(w, r))
-            Vector2 vA = bA.LinearVelocity + MathUtils.Cross(bA.AngularVelocity, _rA);
-            Vector2 vB = bB.LinearVelocity + MathUtils.Cross(bB.AngularVelocity, _rB);
-            float C = _length - MaxLength;
-            float Cdot = Vector2.Dot(_u, vB - vA);
-
-            // Predictive constraint.
-            if (C < 0.0f)
-            {
-                Cdot += step.inv_dt * C;
-            }
-
-            float impulse = -_mass * Cdot;
-            float oldImpulse = _impulse;
-            _impulse = Math.Min(0.0f, _impulse + impulse);
-            impulse = _impulse - oldImpulse;
-
-            Vector2 P = impulse * _u;
-            bA.LinearVelocity -= bA.InvMass * P;
-            bA.AngularVelocity -= bA.InvI * MathUtils.Cross(_rA, P);
-            bB.LinearVelocity += bB.InvMass * P;
-            bB.AngularVelocity += bB.InvI * MathUtils.Cross(_rB, P);
-        }
-
-        internal override bool SolvePositionConstraints()
-        {
-            Body bA = BodyA;
-            Body bB = BodyB;
-
-            Transform xf1;
-            bA.GetTransform(out xf1);
-
-            Transform xf2;
-            bB.GetTransform(out xf2);
-
-            Vector2 rA = MathUtils.Multiply(ref xf1.R, LocalAnchorA - bA.LocalCenter);
-            Vector2 rB = MathUtils.Multiply(ref xf2.R, LocalAnchorB - bB.LocalCenter);
-
-            Vector2 u = bB.Sweep.C + rB - bA.Sweep.C - rA;
-
-
-            float length = u.Length();
-            u.Normalize();
-
-            float C = length - MaxLength;
-
-            C = MathUtils.Clamp(C, 0.0f, Settings.MaxLinearCorrection);
-
-            float impulse = -_mass * C;
-            Vector2 P = impulse * u;
-
-            bA.Sweep.C -= bA.InvMass * P;
-            bA.Sweep.A -= bA.InvI * MathUtils.Cross(rA, P);
-            bB.Sweep.C += bB.InvMass * P;
-            bB.Sweep.A += bB.InvI * MathUtils.Cross(rB, P);
-
-            bA.SynchronizeTransform();
-            bB.SynchronizeTransform();
-
-            return length - MaxLength < Settings.LinearSlop;
-        }
-    }
-}

FarseerPhysicsEngine/Dynamics/Joints/SliderJoint.cs

@@ -1,298 +0,0 @@
-/*
-* Farseer Physics Engine based on Box2D.XNA port:
-* Copyright (c) 2010 Ian Qvist
-* 
-* Box2D.XNA port of Box2D:
-* Copyright (c) 2009 Brandon Furtwangler, Nathan Furtwangler
-*
-* Original source Box2D:
-* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com 
-* 
-* This software is provided 'as-is', without any express or implied 
-* warranty.  In no event will the authors be held liable for any damages 
-* arising from the use of this software. 
-* Permission is granted to anyone to use this software for any purpose, 
-* including commercial applications, and to alter it and redistribute it 
-* freely, subject to the following restrictions: 
-* 1. The origin of this software must not be misrepresented; you must not 
-* claim that you wrote the original software. If you use this software 
-* in a product, an acknowledgment in the product documentation would be 
-* appreciated but is not required. 
-* 2. Altered source versions must be plainly marked as such, and must not be 
-* misrepresented as being the original software. 
-* 3. This notice may not be removed or altered from any source distribution. 
-*/
-
-using System;
-using System.Diagnostics;
-using FarseerPhysics.Common;
-using Microsoft.Xna.Framework;
-
-namespace FarseerPhysics.Dynamics.Joints
-{
-    /// <summary>
-    /// A distance joint contrains two points on two bodies
-    /// to remain at a fixed distance from each other. You can view
-    /// this as a massless, rigid rod.
-    /// </summary>
-    public class SliderJoint : Joint
-    {
-        // 1-D constrained system
-        // m (v2 - v1) = lambda
-        // v2 + (beta/h) * x1 + gamma * lambda = 0, gamma has units of inverse mass.
-        // x2 = x1 + h * v2
-
-        // 1-D mass-damper-spring system
-        // m (v2 - v1) + h * d * v2 + h * k * 
-
-        // C = norm(p2 - p1) - L
-        // u = (p2 - p1) / norm(p2 - p1)
-        // Cdot = dot(u, v2 + cross(w2, r2) - v1 - cross(w1, r1))
-        // J = [-u -cross(r1, u) u cross(r2, u)]
-        // K = J * invM * JT
-        //   = invMass1 + invI1 * cross(r1, u)^2 + invMass2 + invI2 * cross(r2, u)^2
-
-        public Vector2 LocalAnchorA;
-
-        public Vector2 LocalAnchorB;
-        private float _bias;
-        private float _gamma;
-        private float _impulse;
-        private float _mass;
-        private Vector2 _u;
-
-        internal SliderJoint()
-        {
-            JointType = JointType.Slider;
-        }
-
-        /// <summary>
-        /// Initializes a new instance of the <see cref="SliderJoint"/> class.
-        /// Warning: Do not use a zero or short length.
-        /// </summary>
-        /// <param name="bodyA">The first body.</param>
-        /// <param name="bodyB">The second body.</param>
-        /// <param name="localAnchorA">The first body anchor.</param>
-        /// <param name="localAnchorB">The second body anchor.</param>
-        /// <param name="minLength">The minimum length between anchorpoints</param>
-        /// <param name="maxlength">The maximum length between anchorpoints.</param>
-        public SliderJoint(Body bodyA, Body bodyB, Vector2 localAnchorA, Vector2 localAnchorB, float minLength,
-                           float maxlength)
-            : base(bodyA, bodyB)
-        {
-            JointType = JointType.Slider;
-
-            LocalAnchorA = localAnchorA;
-            LocalAnchorB = localAnchorB;
-            MaxLength = maxlength;
-            MinLength = minLength;
-        }
-
-        /// <summary>
-        /// The maximum length between the anchor points.
-        /// </summary>
-        /// <value>The length.</value>
-        public float MaxLength { get; set; }
-
-        /// <summary>
-        /// The minimal length between the anchor points.
-        /// </summary>
-        /// <value>The length.</value>
-        public float MinLength { get; set; }
-
-        /// <summary>
-        /// The mass-spring-damper frequency in Hertz.
-        /// </summary>
-        /// <value>The frequency.</value>
-        public float Frequency { get; set; }
-
-        /// <summary>
-        /// The damping ratio. 0 = no damping, 1 = critical damping.
-        /// </summary>
-        /// <value>The damping ratio.</value>
-        public float DampingRatio { get; set; }
-
-        public override Vector2 WorldAnchorA
-        {
-            get { return BodyA.GetWorldPoint(LocalAnchorA); }
-        }
-
-        public override Vector2 WorldAnchorB
-        {
-            get { return BodyB.GetWorldPoint(LocalAnchorB); }
-            set { Debug.Assert(false, "You can't set the world anchor on this joint type."); }
-        }
-
-        public override Vector2 GetReactionForce(float inv_dt)
-        {
-            Vector2 F = (inv_dt * _impulse) * _u;
-            return F;
-        }
-
-        public override float GetReactionTorque(float inv_dt)
-        {
-            return 0.0f;
-        }
-
-        internal override void InitVelocityConstraints(ref TimeStep step)
-        {
-            Body b1 = BodyA;
-            Body b2 = BodyB;
-
-            Transform xf1, xf2;
-            b1.GetTransform(out xf1);
-            b2.GetTransform(out xf2);
-
-            // Compute the effective mass matrix.
-            Vector2 r1 = MathUtils.Multiply(ref xf1.R, LocalAnchorA - b1.LocalCenter);
-            Vector2 r2 = MathUtils.Multiply(ref xf2.R, LocalAnchorB - b2.LocalCenter);
-            _u = b2.Sweep.C + r2 - b1.Sweep.C - r1;
-
-            // Handle singularity.
-            float length = _u.Length();
-
-            if (length < MaxLength && length > MinLength)
-            {
-                return;
-            }
-
-            if (length > Settings.LinearSlop)
-            {
-                _u *= 1.0f / length;
-            }
-            else
-            {
-                _u = Vector2.Zero;
-            }
-
-            float cr1u = MathUtils.Cross(r1, _u);
-            float cr2u = MathUtils.Cross(r2, _u);
-            float invMass = b1.InvMass + b1.InvI * cr1u * cr1u + b2.InvMass + b2.InvI * cr2u * cr2u;
-            Debug.Assert(invMass > Settings.Epsilon);
-            _mass = invMass != 0.0f ? 1.0f / invMass : 0.0f;
-
-            if (Frequency > 0.0f)
-            {
-                float C = length - MaxLength;
-
-                // Frequency
-                float omega = 2.0f * Settings.Pi * Frequency;
-
-                // Damping coefficient
-                float d = 2.0f * _mass * DampingRatio * omega;
-
-                // Spring stiffness
-                float k = _mass * omega * omega;
-
-                // magic formulas
-                _gamma = step.dt * (d + step.dt * k);
-                _gamma = _gamma != 0.0f ? 1.0f / _gamma : 0.0f;
-                _bias = C * step.dt * k * _gamma;
-
-                _mass = invMass + _gamma;
-                _mass = _mass != 0.0f ? 1.0f / _mass : 0.0f;
-            }
-
-            if (Settings.EnableWarmstarting)
-            {
-                // Scale the impulse to support a variable time step.
-                _impulse *= step.dtRatio;
-
-                Vector2 P = _impulse * _u;
-                b1.LinearVelocityInternal -= b1.InvMass * P;
-                b1.AngularVelocityInternal -= b1.InvI * MathUtils.Cross(r1, P);
-                b2.LinearVelocityInternal += b2.InvMass * P;
-                b2.AngularVelocityInternal += b2.InvI * MathUtils.Cross(r2, P);
-            }
-            else
-            {
-                _impulse = 0.0f;
-            }
-        }
-
-        internal override void SolveVelocityConstraints(ref TimeStep step)
-        {
-            Body b1 = BodyA;
-            Body b2 = BodyB;
-
-            Transform xf1, xf2;
-            b1.GetTransform(out xf1);
-            b2.GetTransform(out xf2);
-
-            Vector2 r1 = MathUtils.Multiply(ref xf1.R, LocalAnchorA - b1.LocalCenter);
-            Vector2 r2 = MathUtils.Multiply(ref xf2.R, LocalAnchorB - b2.LocalCenter);
-
-            Vector2 d = b2.Sweep.C + r2 - b1.Sweep.C - r1;
-
-            float length = d.Length();
-
-            if (length < MaxLength && length > MinLength)
-            {
-                return;
-            }
-
-            // Cdot = dot(u, v + cross(w, r))
-            Vector2 v1 = b1.LinearVelocityInternal + MathUtils.Cross(b1.AngularVelocityInternal, r1);
-            Vector2 v2 = b2.LinearVelocityInternal + MathUtils.Cross(b2.AngularVelocityInternal, r2);
-            float Cdot = Vector2.Dot(_u, v2 - v1);
-
-            float impulse = -_mass * (Cdot + _bias + _gamma * _impulse);
-            _impulse += impulse;
-
-            Vector2 P = impulse * _u;
-            b1.LinearVelocityInternal -= b1.InvMass * P;
-            b1.AngularVelocityInternal -= b1.InvI * MathUtils.Cross(r1, P);
-            b2.LinearVelocityInternal += b2.InvMass * P;
-            b2.AngularVelocityInternal += b2.InvI * MathUtils.Cross(r2, P);
-        }
-
-        internal override bool SolvePositionConstraints()
-        {
-            if (Frequency > 0.0f)
-            {
-                // There is no position correction for soft distance constraints.
-                return true;
-            }
-
-            Body b1 = BodyA;
-            Body b2 = BodyB;
-
-            Transform xf1, xf2;
-            b1.GetTransform(out xf1);
-            b2.GetTransform(out xf2);
-
-            Vector2 r1 = MathUtils.Multiply(ref xf1.R, LocalAnchorA - b1.LocalCenter);
-            Vector2 r2 = MathUtils.Multiply(ref xf2.R, LocalAnchorB - b2.LocalCenter);
-
-            Vector2 d = b2.Sweep.C + r2 - b1.Sweep.C - r1;
-
-            float length = d.Length();
-
-            if (length < MaxLength && length > MinLength)
-            {
-                return true;
-            }
-
-            if (length == 0.0f)
-                return true;
-
-            d /= length;
-            float C = length - MaxLength;
-            C = MathUtils.Clamp(C, -Settings.MaxLinearCorrection, Settings.MaxLinearCorrection);
-
-            float impulse = -_mass * C;
-            _u = d;
-            Vector2 P = impulse * _u;
-
-            b1.Sweep.C -= b1.InvMass * P;
-            b1.Sweep.A -= b1.InvI * MathUtils.Cross(r1, P);
-            b2.Sweep.C += b2.InvMass * P;
-            b2.Sweep.A += b2.InvI * MathUtils.Cross(r2, P);
-
-            b1.SynchronizeTransform();
-            b2.SynchronizeTransform();
-
-            return Math.Abs(C) < Settings.LinearSlop;
-        }
-    }
-}

FarseerPhysicsEngine/Dynamics/Joints/WeldJoint.cs

@@ -1,263 +0,0 @@
-/*
-* Farseer Physics Engine based on Box2D.XNA port:
-* Copyright (c) 2010 Ian Qvist
-* 
-* Box2D.XNA port of Box2D:
-* Copyright (c) 2009 Brandon Furtwangler, Nathan Furtwangler
-*
-* Original source Box2D:
-* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com 
-* 
-* This software is provided 'as-is', without any express or implied 
-* warranty.  In no event will the authors be held liable for any damages 
-* arising from the use of this software. 
-* Permission is granted to anyone to use this software for any purpose, 
-* including commercial applications, and to alter it and redistribute it 
-* freely, subject to the following restrictions: 
-* 1. The origin of this software must not be misrepresented; you must not 
-* claim that you wrote the original software. If you use this software 
-* in a product, an acknowledgment in the product documentation would be 
-* appreciated but is not required. 
-* 2. Altered source versions must be plainly marked as such, and must not be 
-* misrepresented as being the original software. 
-* 3. This notice may not be removed or altered from any source distribution. 
-*/
-
-using System;
-using System.Diagnostics;
-using FarseerPhysics.Common;
-using Microsoft.Xna.Framework;
-
-namespace FarseerPhysics.Dynamics.Joints
-{
-    // Point-to-point constraint
-    // C = p2 - p1
-    // Cdot = v2 - v1
-    //      = v2 + cross(w2, r2) - v1 - cross(w1, r1)
-    // J = [-I -r1_skew I r2_skew ]
-    // Identity used:
-    // w k % (rx i + ry j) = w * (-ry i + rx j)
-
-    // Angle constraint
-    // C = angle2 - angle1 - referenceAngle
-    // Cdot = w2 - w1
-    // J = [0 0 -1 0 0 1]
-    // K = invI1 + invI2
-
-    /// <summary>
-    /// A weld joint essentially glues two bodies together. A weld joint may
-    /// distort somewhat because the island constraint solver is approximate.
-    /// </summary>
-    public class WeldJoint : Joint
-    {
-        public Vector2 LocalAnchorA;
-        public Vector2 LocalAnchorB;
-        private Vector3 _impulse;
-        private Mat33 _mass;
-
-        internal WeldJoint()
-        {
-            JointType = JointType.Weld;
-        }
-
-        /// <summary>
-        /// You need to specify a local anchor point
-        /// where they are attached and the relative body angle. The position
-        /// of the anchor point is important for computing the reaction torque.
-        /// You can change the anchor points relative to bodyA or bodyB by changing LocalAnchorA
-        /// and/or LocalAnchorB.
-        /// </summary>
-        /// <param name="bodyA">The first body</param>
-        /// <param name="bodyB">The second body</param>
-        /// <param name="localAnchorA">The first body anchor.</param>
-        /// <param name="localAnchorB">The second body anchor.</param>
-        public WeldJoint(Body bodyA, Body bodyB, Vector2 localAnchorA, Vector2 localAnchorB)
-            : base(bodyA, bodyB)
-        {
-            JointType = JointType.Weld;
-
-            LocalAnchorA = localAnchorA;
-            LocalAnchorB = localAnchorB;
-            ReferenceAngle = BodyB.Rotation - BodyA.Rotation;
-        }
-
-        public override Vector2 WorldAnchorA
-        {
-            get { return BodyA.GetWorldPoint(LocalAnchorA); }
-        }
-
-        public override Vector2 WorldAnchorB
-        {
-            get { return BodyB.GetWorldPoint(LocalAnchorB); }
-            set { Debug.Assert(false, "You can't set the world anchor on this joint type."); }
-        }
-
-        /// <summary>
-        /// The body2 angle minus body1 angle in the reference state (radians).
-        /// </summary>
-        public float ReferenceAngle { get; private set; }
-
-        public override Vector2 GetReactionForce(float inv_dt)
-        {
-            return inv_dt * new Vector2(_impulse.X, _impulse.Y);
-        }
-
-        public override float GetReactionTorque(float inv_dt)
-        {
-            return inv_dt * _impulse.Z;
-        }
-
-        internal override void InitVelocityConstraints(ref TimeStep step)
-        {
-            Body bA = BodyA;
-            Body bB = BodyB;
-
-            Transform xfA, xfB;
-            bA.GetTransform(out xfA);
-            bB.GetTransform(out xfB);
-
-            // Compute the effective mass matrix.
-            Vector2 rA = MathUtils.Multiply(ref xfA.R, LocalAnchorA - bA.LocalCenter);
-            Vector2 rB = MathUtils.Multiply(ref xfB.R, LocalAnchorB - bB.LocalCenter);
-
-            // J = [-I -r1_skew I r2_skew]
-            //     [ 0       -1 0       1]
-            // r_skew = [-ry; rx]
-
-            // Matlab
-            // K = [ mA+r1y^2*iA+mB+r2y^2*iB,  -r1y*iA*r1x-r2y*iB*r2x,          -r1y*iA-r2y*iB]
-            //     [  -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB,           r1x*iA+r2x*iB]
-            //     [          -r1y*iA-r2y*iB,           r1x*iA+r2x*iB,                   iA+iB]
-
-            float mA = bA.InvMass, mB = bB.InvMass;
-            float iA = bA.InvI, iB = bB.InvI;
-
-            _mass.Col1.X = mA + mB + rA.Y * rA.Y * iA + rB.Y * rB.Y * iB;
-            _mass.Col2.X = -rA.Y * rA.X * iA - rB.Y * rB.X * iB;
-            _mass.Col3.X = -rA.Y * iA - rB.Y * iB;
-            _mass.Col1.Y = _mass.Col2.X;
-            _mass.Col2.Y = mA + mB + rA.X * rA.X * iA + rB.X * rB.X * iB;
-            _mass.Col3.Y = rA.X * iA + rB.X * iB;
-            _mass.Col1.Z = _mass.Col3.X;
-            _mass.Col2.Z = _mass.Col3.Y;
-            _mass.Col3.Z = iA + iB;
-
-            if (Settings.EnableWarmstarting)
-            {
-                // Scale impulses to support a variable time step.
-                _impulse *= step.dtRatio;
-
-                Vector2 P = new Vector2(_impulse.X, _impulse.Y);
-
-                bA.LinearVelocityInternal -= mA * P;
-                bA.AngularVelocityInternal -= iA * (MathUtils.Cross(rA, P) + _impulse.Z);
-
-                bB.LinearVelocityInternal += mB * P;
-                bB.AngularVelocityInternal += iB * (MathUtils.Cross(rB, P) + _impulse.Z);
-            }
-            else
-            {
-                _impulse = Vector3.Zero;
-            }
-        }
-
-        internal override void SolveVelocityConstraints(ref TimeStep step)
-        {
-            Body bA = BodyA;
-            Body bB = BodyB;
-
-            Vector2 vA = bA.LinearVelocityInternal;
-            float wA = bA.AngularVelocityInternal;
-            Vector2 vB = bB.LinearVelocityInternal;
-            float wB = bB.AngularVelocityInternal;
-
-            float mA = bA.InvMass, mB = bB.InvMass;
-            float iA = bA.InvI, iB = bB.InvI;
-
-            Transform xfA, xfB;
-            bA.GetTransform(out xfA);
-            bB.GetTransform(out xfB);
-
-            Vector2 rA = MathUtils.Multiply(ref xfA.R, LocalAnchorA - bA.LocalCenter);
-            Vector2 rB = MathUtils.Multiply(ref xfB.R, LocalAnchorB - bB.LocalCenter);
-
-            //  Solve point-to-point constraint
-            Vector2 Cdot1 = vB + MathUtils.Cross(wB, rB) - vA - MathUtils.Cross(wA, rA);
-            float Cdot2 = wB - wA;
-            Vector3 Cdot = new Vector3(Cdot1.X, Cdot1.Y, Cdot2);
-
-            Vector3 impulse = _mass.Solve33(-Cdot);
-            _impulse += impulse;
-
-            Vector2 P = new Vector2(impulse.X, impulse.Y);
-
-            vA -= mA * P;
-            wA -= iA * (MathUtils.Cross(rA, P) + impulse.Z);
-
-            vB += mB * P;
-            wB += iB * (MathUtils.Cross(rB, P) + impulse.Z);
-
-            bA.LinearVelocityInternal = vA;
-            bA.AngularVelocityInternal = wA;
-            bB.LinearVelocityInternal = vB;
-            bB.AngularVelocityInternal = wB;
-        }
-
-        internal override bool SolvePositionConstraints()
-        {
-            Body bA = BodyA;
-            Body bB = BodyB;
-
-            float mA = bA.InvMass, mB = bB.InvMass;
-            float iA = bA.InvI, iB = bB.InvI;
-
-            Transform xfA;
-            Transform xfB;
-            bA.GetTransform(out xfA);
-            bB.GetTransform(out xfB);
-
-            Vector2 rA = MathUtils.Multiply(ref xfA.R, LocalAnchorA - bA.LocalCenter);
-            Vector2 rB = MathUtils.Multiply(ref xfB.R, LocalAnchorB - bB.LocalCenter);
-
-            Vector2 C1 = bB.Sweep.C + rB - bA.Sweep.C - rA;
-            float C2 = bB.Sweep.A - bA.Sweep.A - ReferenceAngle;
-
-            // Handle large detachment.
-            const float k_allowedStretch = 10.0f * Settings.LinearSlop;
-            float positionError = C1.Length();
-            float angularError = Math.Abs(C2);
-            if (positionError > k_allowedStretch)
-            {
-                iA *= 1.0f;
-                iB *= 1.0f;
-            }
-
-            _mass.Col1.X = mA + mB + rA.Y * rA.Y * iA + rB.Y * rB.Y * iB;
-            _mass.Col2.X = -rA.Y * rA.X * iA - rB.Y * rB.X * iB;
-            _mass.Col3.X = -rA.Y * iA - rB.Y * iB;
-            _mass.Col1.Y = _mass.Col2.X;
-            _mass.Col2.Y = mA + mB + rA.X * rA.X * iA + rB.X * rB.X * iB;
-            _mass.Col3.Y = rA.X * iA + rB.X * iB;
-            _mass.Col1.Z = _mass.Col3.X;
-            _mass.Col2.Z = _mass.Col3.Y;
-            _mass.Col3.Z = iA + iB;
-
-            Vector3 C = new Vector3(C1.X, C1.Y, C2);
-
-            Vector3 impulse = _mass.Solve33(-C);
-
-            Vector2 P = new Vector2(impulse.X, impulse.Y);
-
-            bA.Sweep.C -= mA * P;
-            bA.Sweep.A -= iA * (MathUtils.Cross(rA, P) + impulse.Z);
-
-            bB.Sweep.C += mB * P;
-            bB.Sweep.A += iB * (MathUtils.Cross(rB, P) + impulse.Z);
-
-            bA.SynchronizeTransform();
-            bB.SynchronizeTransform();
-
-            return positionError <= Settings.LinearSlop && angularError <= Settings.AngularSlop;
-        }
-    }
-}

FarseerPhysicsEngine/Dynamics/TimeStep.cs

@@ -1,45 +0,0 @@
-/*
-* Box2D.XNA port of Box2D:
-* Copyright (c) 2009 Brandon Furtwangler, Nathan Furtwangler
-*
-* Original source Box2D:
-* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com 
-* 
-* This software is provided 'as-is', without any express or implied 
-* warranty.  In no event will the authors be held liable for any damages 
-* arising from the use of this software. 
-* Permission is granted to anyone to use this software for any purpose, 
-* including commercial applications, and to alter it and redistribute it 
-* freely, subject to the following restrictions: 
-* 1. The origin of this software must not be misrepresented; you must not 
-* claim that you wrote the original software. If you use this software 
-* in a product, an acknowledgment in the product documentation would be 
-* appreciated but is not required. 
-* 2. Altered source versions must be plainly marked as such, and must not be 
-* misrepresented as being the original software. 
-* 3. This notice may not be removed or altered from any source distribution. 
-*/
-
-namespace FarseerPhysics.Dynamics
-{
-    /// <summary>
-    /// This is an internal structure.
-    /// </summary>
-    public struct TimeStep
-    {
-        /// <summary>
-        /// Time step (Delta time)
-        /// </summary>
-        public float dt;
-
-        /// <summary>
-        /// dt * inv_dt0
-        /// </summary>
-        public float dtRatio;
-
-        /// <summary>
-        /// Inverse time step (0 if dt == 0).
-        /// </summary>
-        public float inv_dt;
-    }
-}

FarseerPhysicsEngine/Dynamics/World.cs

@@ -1,1456 +0,0 @@
-/*
-* Farseer Physics Engine based on Box2D.XNA port:
-* Copyright (c) 2010 Ian Qvist
-* 
-* Box2D.XNA port of Box2D:
-* Copyright (c) 2009 Brandon Furtwangler, Nathan Furtwangler
-*
-* Original source Box2D:
-* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com 
-* 
-* This software is provided 'as-is', without any express or implied 
-* warranty.  In no event will the authors be held liable for any damages 
-* arising from the use of this software. 
-* Permission is granted to anyone to use this software for any purpose, 
-* including commercial applications, and to alter it and redistribute it 
-* freely, subject to the following restrictions: 
-* 1. The origin of this software must not be misrepresented; you must not 
-* claim that you wrote the original software. If you use this software 
-* in a product, an acknowledgment in the product documentation would be 
-* appreciated but is not required. 
-* 2. Altered source versions must be plainly marked as such, and must not be 
-* misrepresented as being the original software. 
-* 3. This notice may not be removed or altered from any source distribution. 
-*/
-
-using System;
-using System.Collections.Generic;
-using System.Diagnostics;
-using FarseerPhysics.Collision;
-using FarseerPhysics.Common;
-using FarseerPhysics.Controllers;
-using FarseerPhysics.Dynamics.Contacts;
-using FarseerPhysics.Dynamics.Joints;
-using Microsoft.Xna.Framework;
-
-namespace FarseerPhysics.Dynamics
-{
-    /// <summary>
-    /// Contains filter data that can determine whether an object should be processed or not.
-    /// </summary>
-    public abstract class FilterData
-    {
-        public Category DisabledOnCategories = Category.None;
-
-        public int DisabledOnGroup;
-        public Category EnabledOnCategories = Category.All;
-        public int EnabledOnGroup;
-
-        public virtual bool IsActiveOn(Body body)
-        {
-            if (body == null || !body.Enabled || body.IsStatic)
-                return false;
-
-            if (body.FixtureList == null)
-                return false;
-
-            foreach (Fixture fixture in body.FixtureList)
-            {
-                //Disable
-                if ((fixture.CollisionGroup == DisabledOnGroup) &&
-                    fixture.CollisionGroup != 0 && DisabledOnGroup != 0)
-                    return false;
-
-                if ((fixture.CollisionCategories & DisabledOnCategories) != Category.None)
-                    return false;
-
-                if (EnabledOnGroup != 0 || EnabledOnCategories != Category.All)
-                {
-                    //Enable
-                    if ((fixture.CollisionGroup == EnabledOnGroup) &&
-                        fixture.CollisionGroup != 0 && EnabledOnGroup != 0)
-                        return true;
-
-                    if ((fixture.CollisionCategories & EnabledOnCategories) != Category.None &&
-                        EnabledOnCategories != Category.All)
-                        return true;
-                }
-                else
-                {
-                    return true;
-                }
-            }
-
-            return false;
-        }
-
-        /// <summary>
-        /// Adds the category.
-        /// </summary>
-        /// <param name="category">The category.</param>
-        public void AddDisabledCategory(Category category)
-        {
-            DisabledOnCategories |= category;
-        }
-
-        /// <summary>
-        /// Removes the category.
-        /// </summary>
-        /// <param name="category">The category.</param>
-        public void RemoveDisabledCategory(Category category)
-        {
-            DisabledOnCategories &= ~category;
-        }
-
-        /// <summary>
-        /// Determines whether this body ignores the the specified controller.
-        /// </summary>
-        /// <param name="category">The category.</param>
-        /// <returns>
-        /// 	<c>true</c> if the object has the specified category; otherwise, <c>false</c>.
-        /// </returns>
-        public bool IsInDisabledCategory(Category category)
-        {
-            return (DisabledOnCategories & category) == category;
-        }
-
-        /// <summary>
-        /// Adds the category.
-        /// </summary>
-        /// <param name="category">The category.</param>
-        public void AddEnabledCategory(Category category)
-        {
-            EnabledOnCategories |= category;
-        }
-
-        /// <summary>
-        /// Removes the category.
-        /// </summary>
-        /// <param name="category">The category.</param>
-        public void RemoveEnabledCategory(Category category)
-        {
-            EnabledOnCategories &= ~category;
-        }
-
-        /// <summary>
-        /// Determines whether this body ignores the the specified controller.
-        /// </summary>
-        /// <param name="category">The category.</param>
-        /// <returns>
-        /// 	<c>true</c> if the object has the specified category; otherwise, <c>false</c>.
-        /// </returns>
-        public bool IsInEnabledCategory(Category category)
-        {
-            return (EnabledOnCategories & category) == category;
-        }
-    }
-
-    [Flags]
-    public enum WorldFlags
-    {
-        /// <summary>
-        /// Flag that indicates a new fixture has been added to the world.
-        /// </summary>
-        NewFixture = (1 << 0),
-
-        /// <summary>
-        /// Flag that clear the forces after each time step.
-        /// </summary>
-        ClearForces = (1 << 2),
-
-        SubStepping = (1 << 4),
-    }
-
-    /// <summary>
-    /// The world class manages all physics entities, dynamic simulation,
-    /// and asynchronous queries.
-    /// </summary>
-    public class World
-    {
-        /// <summary>
-        /// Fires whenever a body has been added
-        /// </summary>
-        public BodyDelegate BodyAdded;
-
-        /// <summary>
-        /// Fires whenever a body has been removed
-        /// </summary>
-        public BodyDelegate BodyRemoved;
-
-        internal Queue<Contact> ContactPool = new Queue<Contact>(256);
-
-        /// <summary>
-        /// Fires whenever a fixture has been added
-        /// </summary>
-        public FixtureDelegate FixtureAdded;
-
-        /// <summary>
-        /// Fires whenever a fixture has been removed
-        /// </summary>
-        public FixtureDelegate FixtureRemoved;
-
-        internal WorldFlags Flags;
-
-        /// <summary>
-        /// Fires whenever a joint has been added
-        /// </summary>
-        public JointDelegate JointAdded;
-
-        /// <summary>
-        /// Fires whenever a joint has been removed
-        /// </summary>
-        public JointDelegate JointRemoved;
-
-        public ControllerDelegate ControllerAdded;
-
-        public ControllerDelegate ControllerRemoved;
-
-        private float _invDt0;
-        public Island Island = new Island();
-        private Body[] _stack = new Body[64];
-        private bool _stepComplete;
-        private HashSet<Body> _bodyAddList = new HashSet<Body>();
-        private HashSet<Body> _bodyRemoveList = new HashSet<Body>();
-        private HashSet<Joint> _jointAddList = new HashSet<Joint>();
-        private HashSet<Joint> _jointRemoveList = new HashSet<Joint>();
-        private TOIInput _input = new TOIInput();
-
-        /// <summary>
-        /// If false, the whole simulation stops. It still processes added and removed geometries.
-        /// </summary>
-        public bool Enabled = true;
-
-#if (!SILVERLIGHT)
-        private Stopwatch _watch = new Stopwatch();
-#endif
-
-        /// <summary>
-        /// Initializes a new instance of the <see cref="World"/> class.
-        /// </summary>
-        private World()
-        {
-            Flags = WorldFlags.ClearForces;
-
-            ControllerList = new List<Controller>();
-            BreakableBodyList = new List<BreakableBody>();
-            BodyList = new List<Body>(32);
-            JointList = new List<Joint>(32);
-        }
-
-        public World(Vector2 gravity, AABB span)
-            : this()
-        {
-            Gravity = gravity;
-            ContactManager = new ContactManager(new QuadTreeBroadPhase(span));
-        }
-
-        /// <summary>
-        /// Initializes a new instance of the <see cref="World"/> class.
-        /// </summary>
-        /// <param name="gravity">The gravity.</param>
-        public World(Vector2 gravity)
-            : this()
-        {
-            ContactManager = new ContactManager(new DynamicTreeBroadPhase());
-            Gravity = gravity;
-        }
-
-        public List<Controller> ControllerList { get; private set; }
-
-        public List<BreakableBody> BreakableBodyList { get; private set; }
-
-        public float UpdateTime { get; private set; }
-
-        public float ContinuousPhysicsTime { get; private set; }
-
-        public float ControllersUpdateTime { get; private set; }
-
-        public float AddRemoveTime { get; private set; }
-
-        public float ContactsUpdateTime { get; private set; }
-
-        public float SolveUpdateTime { get; private set; }
-
-        /// <summary>
-        /// Get the number of broad-phase proxies.
-        /// </summary>
-        /// <value>The proxy count.</value>
-        public int ProxyCount
-        {
-            get { return ContactManager.BroadPhase.ProxyCount; }
-        }
-
-        /// <summary>
-        /// Change the global gravity vector.
-        /// </summary>
-        /// <value>The gravity.</value>
-        public Vector2 Gravity;
-
-        /// <summary>
-        /// Set flag to control automatic clearing of forces after each time step.
-        /// </summary>
-        /// <value><c>true</c> if it should auto clear forces; otherwise, <c>false</c>.</value>
-        public bool AutoClearForces
-        {
-            set
-            {
-                if (value)
-                {
-                    Flags |= WorldFlags.ClearForces;
-                }
-                else
-                {
-                    Flags &= ~WorldFlags.ClearForces;
-                }
-            }
-            get { return (Flags & WorldFlags.ClearForces) == WorldFlags.ClearForces; }
-        }
-
-        /// <summary>
-        /// Get the contact manager for testing.
-        /// </summary>
-        /// <value>The contact manager.</value>
-        public ContactManager ContactManager { get; private set; }
-
-        /// <summary>
-        /// Get the world body list.
-        /// </summary>
-        /// <value>Thehead of the world body list.</value>
-        public List<Body> BodyList { get; private set; }
-
-        /// <summary>
-        /// Get the world joint list. 
-        /// </summary>
-        /// <value>The joint list.</value>
-        public List<Joint> JointList { get; private set; }
-
-        /// <summary>
-        /// Get the world contact list. With the returned contact, use Contact.GetNext to get
-        /// the next contact in the world list. A null contact indicates the end of the list.
-        /// </summary>
-        /// <value>The head of the world contact list.</value>
-        public List<Contact> ContactList
-        {
-            get { return ContactManager.ContactList; }
-        }
-
-        /// <summary>
-        /// Enable/disable single stepped continuous physics. For testing.
-        /// </summary>
-        public bool EnableSubStepping
-        {
-            set
-            {
-                if (value)
-                {
-                    Flags |= WorldFlags.SubStepping;
-                }
-                else
-                {
-                    Flags &= ~WorldFlags.SubStepping;
-                }
-            }
-            get { return (Flags & WorldFlags.SubStepping) == WorldFlags.SubStepping; }
-        }
-
-        /// <summary>
-        /// Add a rigid body.
-        /// </summary>
-        /// <returns></returns>
-        internal void AddBody(Body body)
-        {
-            Debug.Assert(!_bodyAddList.Contains(body), "You are adding the same body more than once.");
-
-            if (!_bodyAddList.Contains(body))
-                _bodyAddList.Add(body);
-        }
-
-        /// <summary>
-        /// Destroy a rigid body.
-        /// Warning: This automatically deletes all associated shapes and joints.
-        /// </summary>
-        /// <param name="body">The body.</param>
-        public void RemoveBody(Body body)
-        {
-            Debug.Assert(!_bodyRemoveList.Contains(body),
-                         "The body is already marked for removal. You are removing the body more than once.");
-
-            if (!_bodyRemoveList.Contains(body))
-                _bodyRemoveList.Add(body);
-        }
-
-        /// <summary>
-        /// Create a joint to constrain bodies together. This may cause the connected bodies to cease colliding.
-        /// </summary>
-        /// <param name="joint">The joint.</param>
-        public void AddJoint(Joint joint)
-        {
-            Debug.Assert(!_jointAddList.Contains(joint), "You are adding the same joint more than once.");
-
-            if (!_jointAddList.Contains(joint))
-                _jointAddList.Add(joint);
-        }
-
-        private void RemoveJoint(Joint joint, bool doCheck)
-        {
-            if (doCheck)
-            {
-                Debug.Assert(!_jointRemoveList.Contains(joint),
-                             "The joint is already marked for removal. You are removing the joint more than once.");
-            }
-
-            if (!_jointRemoveList.Contains(joint))
-                _jointRemoveList.Add(joint);
-        }
-
-        /// <summary>
-        /// Destroy a joint. This may cause the connected bodies to begin colliding.
-        /// </summary>
-        /// <param name="joint">The joint.</param>
-        public void RemoveJoint(Joint joint)
-        {
-            RemoveJoint(joint, true);
-        }
-
-        /// <summary>
-        /// All adds and removes are cached by the World duing a World step.
-        /// To process the changes before the world updates again, call this method.
-        /// </summary>
-        public void ProcessChanges()
-        {
-            ProcessAddedBodies();
-            ProcessAddedJoints();
-
-            ProcessRemovedBodies();
-            ProcessRemovedJoints();
-        }
-
-        private void ProcessRemovedJoints()
-        {
-            if (_jointRemoveList.Count > 0)
-            {
-                foreach (Joint joint in _jointRemoveList)
-                {
-                    bool collideConnected = joint.CollideConnected;
-
-                    // Remove from the world list.
-                    JointList.Remove(joint);
-
-                    // Disconnect from island graph.
-                    Body bodyA = joint.BodyA;
-                    Body bodyB = joint.BodyB;
-
-                    // Wake up connected bodies.
-                    bodyA.Awake = true;
-
-                    // WIP David
-                    if (!joint.IsFixedType())
-                    {
-                        bodyB.Awake = true;
-                    }
-
-                    // Remove from body 1.
-                    if (joint.EdgeA.Prev != null)
-                    {
-                        joint.EdgeA.Prev.Next = joint.EdgeA.Next;
-                    }
-
-                    if (joint.EdgeA.Next != null)
-                    {
-                        joint.EdgeA.Next.Prev = joint.EdgeA.Prev;
-                    }
-
-                    if (joint.EdgeA == bodyA.JointList)
-                    {
-                        bodyA.JointList = joint.EdgeA.Next;
-                    }
-
-                    joint.EdgeA.Prev = null;
-                    joint.EdgeA.Next = null;
-
-                    // WIP David
-                    if (!joint.IsFixedType())
-                    {
-                        // Remove from body 2
-                        if (joint.EdgeB.Prev != null)
-                        {
-                            joint.EdgeB.Prev.Next = joint.EdgeB.Next;
-                        }
-
-                        if (joint.EdgeB.Next != null)
-                        {
-                            joint.EdgeB.Next.Prev = joint.EdgeB.Prev;
-                        }
-
-                        if (joint.EdgeB == bodyB.JointList)
-                        {
-                            bodyB.JointList = joint.EdgeB.Next;
-                        }
-
-                        joint.EdgeB.Prev = null;
-                        joint.EdgeB.Next = null;
-                    }
-
-                    // WIP David
-                    if (!joint.IsFixedType())
-                    {
-                        // If the joint prevents collisions, then flag any contacts for filtering.
-                        if (collideConnected == false)
-                        {
-                            ContactEdge edge = bodyB.ContactList;
-                            while (edge != null)
-                            {
-                                if (edge.Other == bodyA)
-                                {
-                                    // Flag the contact for filtering at the next time step (where either
-                                    // body is awake).
-                                    edge.Contact.FlagForFiltering();
-                                }
-
-                                edge = edge.Next;
-                            }
-                        }
-                    }
-
-                    if (JointRemoved != null)
-                    {
-                        JointRemoved(joint);
-                    }
-                }
-
-                _jointRemoveList.Clear();
-            }
-        }
-
-        private void ProcessAddedJoints()
-        {
-            if (_jointAddList.Count > 0)
-            {
-                foreach (Joint joint in _jointAddList)
-                {
-                    // Connect to the world list.
-                    JointList.Add(joint);
-
-                    // Connect to the bodies' doubly linked lists.
-                    joint.EdgeA.Joint = joint;
-                    joint.EdgeA.Other = joint.BodyB;
-                    joint.EdgeA.Prev = null;
-                    joint.EdgeA.Next = joint.BodyA.JointList;
-
-                    if (joint.BodyA.JointList != null)
-                        joint.BodyA.JointList.Prev = joint.EdgeA;
-
-                    joint.BodyA.JointList = joint.EdgeA;
-
-                    // WIP David
-                    if (!joint.IsFixedType())
-                    {
-                        joint.EdgeB.Joint = joint;
-                        joint.EdgeB.Other = joint.BodyA;
-                        joint.EdgeB.Prev = null;
-                        joint.EdgeB.Next = joint.BodyB.JointList;
-
-                        if (joint.BodyB.JointList != null)
-                            joint.BodyB.JointList.Prev = joint.EdgeB;
-
-                        joint.BodyB.JointList = joint.EdgeB;
-
-                        Body bodyA = joint.BodyA;
-                        Body bodyB = joint.BodyB;
-
-                        // If the joint prevents collisions, then flag any contacts for filtering.
-                        if (joint.CollideConnected == false)
-                        {
-                            ContactEdge edge = bodyB.ContactList;
-                            while (edge != null)
-                            {
-                                if (edge.Other == bodyA)
-                                {
-                                    // Flag the contact for filtering at the next time step (where either
-                                    // body is awake).
-                                    edge.Contact.FlagForFiltering();
-                                }
-
-                                edge = edge.Next;
-                            }
-                        }
-                    }
-
-                    if (JointAdded != null)
-                        JointAdded(joint);
-
-                    // Note: creating a joint doesn't wake the bodies.
-                }
-
-                _jointAddList.Clear();
-            }
-        }
-
-        private void ProcessAddedBodies()
-        {
-            if (_bodyAddList.Count > 0)
-            {
-                foreach (Body body in _bodyAddList)
-                {
-                    // Add to world list.
-                    BodyList.Add(body);
-
-                    if (BodyAdded != null)
-                        BodyAdded(body);
-                }
-
-                _bodyAddList.Clear();
-            }
-        }
-
-        private void ProcessRemovedBodies()
-        {
-            if (_bodyRemoveList.Count > 0)
-            {
-                foreach (Body body in _bodyRemoveList)
-                {
-                    Debug.Assert(BodyList.Count > 0);
-
-                    // You tried to remove a body that is not contained in the BodyList.
-                    // Are you removing the body more than once?
-                    Debug.Assert(BodyList.Contains(body));
-
-                    // Delete the attached joints.
-                    JointEdge je = body.JointList;
-                    while (je != null)
-                    {
-                        JointEdge je0 = je;
-                        je = je.Next;
-
-                        RemoveJoint(je0.Joint, false);
-                    }
-                    body.JointList = null;
-
-                    // Delete the attached contacts.
-                    ContactEdge ce = body.ContactList;
-                    while (ce != null)
-                    {
-                        ContactEdge ce0 = ce;
-                        ce = ce.Next;
-                        ContactManager.Destroy(ce0.Contact);
-                    }
-                    body.ContactList = null;
-
-                    // Delete the attached fixtures. This destroys broad-phase proxies.
-                    for (int i = 0; i < body.FixtureList.Count; i++)
-                    {
-                        body.FixtureList[i].DestroyProxies(ContactManager.BroadPhase);
-                        body.FixtureList[i].Destroy();
-                    }
-
-                    body.FixtureList = null;
-
-                    // Remove world body list.
-                    BodyList.Remove(body);
-
-                    if (BodyRemoved != null)
-                        BodyRemoved(body);
-                }
-
-                _bodyRemoveList.Clear();
-            }
-        }
-
-        /// <summary>
-        /// Take a time step. This performs collision detection, integration,
-        /// and consraint solution.
-        /// </summary>
-        /// <param name="dt">The amount of time to simulate, this should not vary.</param>
-        public void Step(float dt)
-        {
-#if (!SILVERLIGHT)
-            if (Settings.EnableDiagnostics)
-                _watch.Start();
-#endif
-
-            ProcessChanges();
-
-#if (!SILVERLIGHT)
-            if (Settings.EnableDiagnostics)
-                AddRemoveTime = _watch.ElapsedTicks;
-#endif
-            //If there is no change in time, no need to calculate anything.
-            if (dt == 0 || !Enabled)
-            {
-#if (!SILVERLIGHT)
-                if (Settings.EnableDiagnostics)
-                {
-                    _watch.Stop();
-                    _watch.Reset();
-                }
-#endif
-                return;
-            }
-
-            // If new fixtures were added, we need to find the new contacts.
-            if ((Flags & WorldFlags.NewFixture) == WorldFlags.NewFixture)
-            {
-                ContactManager.FindNewContacts();
-                Flags &= ~WorldFlags.NewFixture;
-            }
-
-            TimeStep step;
-            step.inv_dt = 1.0f / dt;
-            step.dt = dt;
-            step.dtRatio = _invDt0 * dt;
-
-            //Update controllers
-            for (int i = 0; i < ControllerList.Count; i++)
-            {
-                ControllerList[i].Update(dt);
-            }
-
-#if (!SILVERLIGHT)
-            if (Settings.EnableDiagnostics)
-                ControllersUpdateTime = _watch.ElapsedTicks - AddRemoveTime;
-#endif
-
-            // Update contacts. This is where some contacts are destroyed.
-            ContactManager.Collide();
-
-#if (!SILVERLIGHT)
-            if (Settings.EnableDiagnostics)
-                ContactsUpdateTime = _watch.ElapsedTicks - (AddRemoveTime + ControllersUpdateTime);
-#endif
-            // Integrate velocities, solve velocity raints, and integrate positions.
-            Solve(ref step);
-
-#if (!SILVERLIGHT)
-            if (Settings.EnableDiagnostics)
-                SolveUpdateTime = _watch.ElapsedTicks - (AddRemoveTime + ControllersUpdateTime + ContactsUpdateTime);
-#endif
-
-            // Handle TOI events.
-            if (Settings.ContinuousPhysics)
-            {
-                SolveTOI(ref step);
-            }
-
-#if (!SILVERLIGHT)
-            if (Settings.EnableDiagnostics)
-                ContinuousPhysicsTime = _watch.ElapsedTicks -
-                                        (AddRemoveTime + ControllersUpdateTime + ContactsUpdateTime + SolveUpdateTime);
-#endif
-            _invDt0 = step.inv_dt;
-
-            if ((Flags & WorldFlags.ClearForces) != 0)
-            {
-                ClearForces();
-            }
-
-            for (int i = 0; i < BreakableBodyList.Count; i++)
-            {
-                BreakableBodyList[i].Update();
-            }
-
-#if (!SILVERLIGHT)
-            if (Settings.EnableDiagnostics)
-            {
-                _watch.Stop();
-                //AddRemoveTime = 1000 * AddRemoveTime / Stopwatch.Frequency;
-
-                UpdateTime = _watch.ElapsedTicks;
-                _watch.Reset();
-            }
-#endif
-        }
-
-        /// <summary>
-        /// Call this after you are done with time steps to clear the forces. You normally
-        /// call this after each call to Step, unless you are performing sub-steps. By default,
-        /// forces will be automatically cleared, so you don't need to call this function.
-        /// </summary>
-        public void ClearForces()
-        {
-            for (int i = 0; i < BodyList.Count; i++)
-            {
-                Body body = BodyList[i];
-                body.Force = Vector2.Zero;
-                body.Torque = 0.0f;
-            }
-        }
-
-        /// <summary>
-        /// Query the world for all fixtures that potentially overlap the
-        /// provided AABB.
-        /// 
-        /// Inside the callback:
-        /// Return true: Continues the query
-        /// Return false: Terminate the query
-        /// </summary>
-        /// <param name="callback">A user implemented callback class.</param>
-        /// <param name="aabb">The aabb query box.</param>
-        public void QueryAABB(Func<Fixture, bool> callback, ref AABB aabb)
-        {
-            ContactManager.BroadPhase.Query(proxyId =>
-                                                {
-                                                    FixtureProxy proxy = ContactManager.BroadPhase.GetProxy(proxyId);
-                                                    return callback(proxy.Fixture);
-                                                }, ref aabb);
-        }
-
-        /// <summary>
-        /// Ray-cast the world for all fixtures in the path of the ray. Your callback
-        /// controls whether you get the closest point, any point, or n-points.
-        /// The ray-cast ignores shapes that contain the starting point.
-        /// 
-        /// Inside the callback:
-        /// return -1: ignore this fixture and continue
-        /// return 0: terminate the ray cast
-        /// return fraction: clip the ray to this point
-        /// return 1: don't clip the ray and continue
-        /// </summary>
-        /// <param name="callback">A user implemented callback class.</param>
-        /// <param name="point1">The ray starting point.</param>
-        /// <param name="point2">The ray ending point.</param>
-        public void RayCast(RayCastCallback callback, Vector2 point1, Vector2 point2)
-        {
-            RayCastInput input = new RayCastInput();
-            input.MaxFraction = 1.0f;
-            input.Point1 = point1;
-            input.Point2 = point2;
-
-            ContactManager.BroadPhase.RayCast((rayCastInput, proxyId) =>
-                                                  {
-                                                      FixtureProxy proxy = ContactManager.BroadPhase.GetProxy(proxyId);
-                                                      Fixture fixture = proxy.Fixture;
-                                                      int index = proxy.ChildIndex;
-                                                      RayCastOutput output;
-                                                      bool hit = fixture.RayCast(out output, ref rayCastInput, index);
-
-                                                      if (hit)
-                                                      {
-                                                          float fraction = output.Fraction;
-                                                          Vector2 point = (1.0f - fraction) * input.Point1 +
-                                                                          fraction * input.Point2;
-                                                          return callback(fixture, point, output.Normal, fraction);
-                                                      }
-
-                                                      return input.MaxFraction;
-                                                  }, ref input);
-        }
-
-        private void Solve(ref TimeStep step)
-        {
-            // Size the island for the worst case.
-            Island.Reset(BodyList.Count,
-                         ContactManager.ContactList.Count,
-                         JointList.Count,
-                         ContactManager);
-
-            // Clear all the island flags.
-            foreach (Body b in BodyList)
-            {
-                b.Flags &= ~BodyFlags.Island;
-            }
-
-            for (int i = 0; i < ContactManager.ContactList.Count; i++)
-            {
-                Contact c = ContactManager.ContactList[i];
-                c.Flags &= ~ContactFlags.Island;
-            }
-            foreach (Joint j in JointList)
-            {
-                j.IslandFlag = false;
-            }
-
-            // Build and simulate all awake islands.
-            int stackSize = BodyList.Count;
-            if (stackSize > _stack.Length)
-                _stack = new Body[Math.Max(_stack.Length * 2, stackSize)];
-
-            for (int index = BodyList.Count - 1; index >= 0; index--)
-            {
-                Body seed = BodyList[index];
-                if ((seed.Flags & (BodyFlags.Island)) != BodyFlags.None)
-                {
-                    continue;
-                }
-
-                if (seed.Awake == false || seed.Enabled == false)
-                {
-                    continue;
-                }
-
-                // The seed can be dynamic or kinematic.
-                if (seed.BodyType == BodyType.Static)
-                {
-                    continue;
-                }
-
-                // Reset island and stack.
-                Island.Clear();
-                int stackCount = 0;
-                _stack[stackCount++] = seed;
-                seed.Flags |= BodyFlags.Island;
-
-                // Perform a depth first search (DFS) on the constraint graph.
-                while (stackCount > 0)
-                {
-                    // Grab the next body off the stack and add it to the island.
-                    Body b = _stack[--stackCount];
-                    Debug.Assert(b.Enabled);
-                    Island.Add(b);
-
-                    // Make sure the body is awake.
-                    b.Awake = true;
-
-                    // To keep islands as small as possible, we don't
-                    // propagate islands across static bodies.
-                    if (b.BodyType == BodyType.Static)
-                    {
-                        continue;
-                    }
-
-                    // Search all contacts connected to this body.
-                    for (ContactEdge ce = b.ContactList; ce != null; ce = ce.Next)
-                    {
-                        Contact contact = ce.Contact;
-
-                        // Has this contact already been added to an island?
-                        if ((contact.Flags & ContactFlags.Island) != ContactFlags.None)
-                        {
-                            continue;
-                        }
-
-                        // Is this contact solid and touching?
-                        if (!ce.Contact.Enabled || !ce.Contact.IsTouching())
-                        {
-                            continue;
-                        }
-
-                        // Skip sensors.
-                        bool sensorA = contact.FixtureA.IsSensor;
-                        bool sensorB = contact.FixtureB.IsSensor;
-                        if (sensorA || sensorB)
-                        {
-                            continue;
-                        }
-
-                        Island.Add(contact);
-                        contact.Flags |= ContactFlags.Island;
-
-                        Body other = ce.Other;
-
-                        // Was the other body already added to this island?
-                        if ((other.Flags & BodyFlags.Island) != BodyFlags.None)
-                        {
-                            continue;
-                        }
-
-                        Debug.Assert(stackCount < stackSize);
-                        _stack[stackCount++] = other;
-                        other.Flags |= BodyFlags.Island;
-                    }
-
-                    // Search all joints connect to this body.
-                    for (JointEdge je = b.JointList; je != null; je = je.Next)
-                    {
-                        if (je.Joint.IslandFlag)
-                        {
-                            continue;
-                        }
-
-                        Body other = je.Other;
-
-                        // WIP David
-                        //Enter here when it's a non-fixed joint. Non-fixed joints have a other body.
-                        if (other != null)
-                        {
-                            // Don't simulate joints connected to inactive bodies.
-                            if (other.Enabled == false)
-                            {
-                                continue;
-                            }
-
-                            Island.Add(je.Joint);
-                            je.Joint.IslandFlag = true;
-
-                            if ((other.Flags & BodyFlags.Island) != BodyFlags.None)
-                            {
-                                continue;
-                            }
-
-                            Debug.Assert(stackCount < stackSize);
-                            _stack[stackCount++] = other;
-                            other.Flags |= BodyFlags.Island;
-                        }
-                        else
-                        {
-                            Island.Add(je.Joint);
-                            je.Joint.IslandFlag = true;
-                        }
-                    }
-                }
-
-                Island.Solve(ref step, ref Gravity);
-
-                // Post solve cleanup.
-                for (int i = 0; i < Island.BodyCount; ++i)
-                {
-                    // Allow static bodies to participate in other islands.
-                    Body b = Island.Bodies[i];
-                    if (b.BodyType == BodyType.Static)
-                    {
-                        b.Flags &= ~BodyFlags.Island;
-                    }
-                }
-            }
-
-            // Synchronize fixtures, check for out of range bodies.
-            foreach (Body b in BodyList)
-            {
-                // If a body was not in an island then it did not move.
-                if ((b.Flags & BodyFlags.Island) != BodyFlags.Island)
-                {
-                    continue;
-                }
-
-                if (b.BodyType == BodyType.Static)
-                {
-                    continue;
-                }
-
-                // Update fixtures (for broad-phase).
-                b.SynchronizeFixtures();
-            }
-
-            // Look for new contacts.
-            ContactManager.FindNewContacts();
-        }
-
-        /// <summary>
-        /// Find TOI contacts and solve them.
-        /// </summary>
-        /// <param name="step">The step.</param>
-        private void SolveTOI(ref TimeStep step)
-        {
-            Island.Reset(2 * Settings.MaxTOIContacts, Settings.MaxTOIContacts, 0, ContactManager);
-
-            if (_stepComplete)
-            {
-                for (int i = 0; i < BodyList.Count; i++)
-                {
-                    BodyList[i].Flags &= ~BodyFlags.Island;
-                    BodyList[i].Sweep.Alpha0 = 0.0f;
-                }
-
-                for (int i = 0; i < ContactManager.ContactList.Count; i++)
-                {
-                    Contact c = ContactManager.ContactList[i];
-
-                    // Invalidate TOI
-                    c.Flags &= ~(ContactFlags.TOI | ContactFlags.Island);
-                    c.TOICount = 0;
-                    c.TOI = 1.0f;
-                }
-            }
-
-            // Find TOI events and solve them.
-            for (; ; )
-            {
-                // Find the first TOI.
-                Contact minContact = null;
-                float minAlpha = 1.0f;
-
-                for (int i = 0; i < ContactManager.ContactList.Count; i++)
-                {
-                    Contact c = ContactManager.ContactList[i];
-
-                    // Is this contact disabled?
-                    if (c.Enabled == false)
-                    {
-                        continue;
-                    }
-
-                    // Prevent excessive sub-stepping.
-                    if (c.TOICount > Settings.MaxSubSteps)
-                    {
-                        continue;
-                    }
-
-                    float alpha;
-                    if ((c.Flags & ContactFlags.TOI) == ContactFlags.TOI)
-                    {
-                        // This contact has a valid cached TOI.
-                        alpha = c.TOI;
-                    }
-                    else
-                    {
-                        Fixture fA = c.FixtureA;
-                        Fixture fB = c.FixtureB;
-
-                        // Is there a sensor?
-                        if (fA.IsSensor || fB.IsSensor)
-                        {
-                            continue;
-                        }
-
-                        Body bA = fA.Body;
-                        Body bB = fB.Body;
-
-                        BodyType typeA = bA.BodyType;
-                        BodyType typeB = bB.BodyType;
-                        Debug.Assert(typeA == BodyType.Dynamic || typeB == BodyType.Dynamic);
-
-                        bool awakeA = bA.Awake && typeA != BodyType.Static;
-                        bool awakeB = bB.Awake && typeB != BodyType.Static;
-
-                        // Is at least one body awake?
-                        if (awakeA == false && awakeB == false)
-                        {
-                            continue;
-                        }
-
-                        bool collideA = (bA.IsBullet || typeA != BodyType.Dynamic) && !bA.IgnoreCCD;
-                        bool collideB = (bB.IsBullet || typeB != BodyType.Dynamic) && !bB.IgnoreCCD;
-
-                        // Are these two non-bullet dynamic bodies?
-                        if (collideA == false && collideB == false)
-                        {
-                            continue;
-                        }
-
-                        // Compute the TOI for this contact.
-                        // Put the sweeps onto the same time interval.
-                        float alpha0 = bA.Sweep.Alpha0;
-
-                        if (bA.Sweep.Alpha0 < bB.Sweep.Alpha0)
-                        {
-                            alpha0 = bB.Sweep.Alpha0;
-                            bA.Sweep.Advance(alpha0);
-                        }
-                        else if (bB.Sweep.Alpha0 < bA.Sweep.Alpha0)
-                        {
-                            alpha0 = bA.Sweep.Alpha0;
-                            bB.Sweep.Advance(alpha0);
-                        }
-
-                        Debug.Assert(alpha0 < 1.0f);
-
-                        // Compute the time of impact in interval [0, minTOI]
-                        _input.ProxyA.Set(fA.Shape, c.ChildIndexA);
-                        _input.ProxyB.Set(fB.Shape, c.ChildIndexB);
-                        _input.SweepA = bA.Sweep;
-                        _input.SweepB = bB.Sweep;
-                        _input.TMax = 1.0f;
-
-                        TOIOutput output;
-                        TimeOfImpact.CalculateTimeOfImpact(out output, _input);
-
-                        // Beta is the fraction of the remaining portion of the .
-                        float beta = output.T;
-                        if (output.State == TOIOutputState.Touching)
-                        {
-                            alpha = Math.Min(alpha0 + (1.0f - alpha0) * beta, 1.0f);
-                        }
-                        else
-                        {
-                            alpha = 1.0f;
-                        }
-
-                        c.TOI = alpha;
-                        c.Flags |= ContactFlags.TOI;
-                    }
-
-                    if (alpha < minAlpha)
-                    {
-                        // This is the minimum TOI found so far.
-                        minContact = c;
-                        minAlpha = alpha;
-                    }
-                }
-
-                if (minContact == null || 1.0f - 10.0f * Settings.Epsilon < minAlpha)
-                {
-                    // No more TOI events. Done!
-                    _stepComplete = true;
-                    break;
-                }
-
-                // Advance the bodies to the TOI.
-                Fixture fA1 = minContact.FixtureA;
-                Fixture fB1 = minContact.FixtureB;
-                Body bA1 = fA1.Body;
-                Body bB1 = fB1.Body;
-
-                Sweep backup1 = bA1.Sweep;
-                Sweep backup2 = bB1.Sweep;
-
-                bA1.Advance(minAlpha);
-                bB1.Advance(minAlpha);
-
-                // The TOI contact likely has some new contact points.
-                minContact.Update(ContactManager);
-                minContact.Flags &= ~ContactFlags.TOI;
-                ++minContact.TOICount;
-
-                // Is the contact solid?
-                if (minContact.Enabled == false || minContact.IsTouching() == false)
-                {
-                    // Restore the sweeps.
-                    minContact.Enabled = false;
-                    bA1.Sweep = backup1;
-                    bB1.Sweep = backup2;
-                    bA1.SynchronizeTransform();
-                    bB1.SynchronizeTransform();
-                    continue;
-                }
-
-                bA1.Awake = true;
-                bB1.Awake = true;
-
-                // Build the island
-                Island.Clear();
-                Island.Add(bA1);
-                Island.Add(bB1);
-                Island.Add(minContact);
-
-                bA1.Flags |= BodyFlags.Island;
-                bB1.Flags |= BodyFlags.Island;
-                minContact.Flags |= ContactFlags.Island;
-
-                // Get contacts on bodyA and bodyB.
-                Body[] bodies = { bA1, bB1 };
-                for (int i = 0; i < 2; ++i)
-                {
-                    Body body = bodies[i];
-                    if (body.BodyType == BodyType.Dynamic)
-                    {
-                        // for (ContactEdge ce = body.ContactList; ce && Island.BodyCount < Settings.MaxTOIContacts; ce = ce.Next)
-                        for (ContactEdge ce = body.ContactList; ce != null; ce = ce.Next)
-                        {
-                            Contact contact = ce.Contact;
-
-                            // Has this contact already been added to the island?
-                            if ((contact.Flags & ContactFlags.Island) == ContactFlags.Island)
-                            {
-                                continue;
-                            }
-
-                            // Only add static, kinematic, or bullet bodies.
-                            Body other = ce.Other;
-                            if (other.BodyType == BodyType.Dynamic &&
-                                body.IsBullet == false && other.IsBullet == false)
-                            {
-                                continue;
-                            }
-
-                            // Skip sensors.
-                            if (contact.FixtureA.IsSensor || contact.FixtureB.IsSensor)
-                            {
-                                continue;
-                            }
-
-                            // Tentatively advance the body to the TOI.
-                            Sweep backup = other.Sweep;
-                            if ((other.Flags & BodyFlags.Island) == 0)
-                            {
-                                other.Advance(minAlpha);
-                            }
-
-                            // Update the contact points
-                            contact.Update(ContactManager);
-
-                            // Was the contact disabled by the user?
-                            if (contact.Enabled == false)
-                            {
-                                other.Sweep = backup;
-                                other.SynchronizeTransform();
-                                continue;
-                            }
-
-                            // Are there contact points?
-                            if (contact.IsTouching() == false)
-                            {
-                                other.Sweep = backup;
-                                other.SynchronizeTransform();
-                                continue;
-                            }
-
-                            // Add the contact to the island
-                            contact.Flags |= ContactFlags.Island;
-                            Island.Add(contact);
-
-                            // Has the other body already been added to the island?
-                            if ((other.Flags & BodyFlags.Island) == BodyFlags.Island)
-                            {
-                                continue;
-                            }
-
-                            // Add the other body to the island.
-                            other.Flags |= BodyFlags.Island;
-
-                            if (other.BodyType != BodyType.Static)
-                            {
-                                other.Awake = true;
-                            }
-
-                            Island.Add(other);
-                        }
-                    }
-                }
-
-                TimeStep subStep;
-                subStep.dt = (1.0f - minAlpha) * step.dt;
-                subStep.inv_dt = 1.0f / subStep.dt;
-                subStep.dtRatio = 1.0f;
-                //subStep.positionIterations = 20;
-                //subStep.velocityIterations = step.velocityIterations;
-                //subStep.warmStarting = false;
-                Island.SolveTOI(ref subStep);
-
-                // Reset island flags and synchronize broad-phase proxies.
-                for (int i = 0; i < Island.BodyCount; ++i)
-                {
-                    Body body = Island.Bodies[i];
-                    body.Flags &= ~BodyFlags.Island;
-
-                    if (body.BodyType != BodyType.Dynamic)
-                    {
-                        continue;
-                    }
-
-                    body.SynchronizeFixtures();
-
-                    // Invalidate all contact TOIs on this displaced body.
-                    for (ContactEdge ce = body.ContactList; ce != null; ce = ce.Next)
-                    {
-                        ce.Contact.Flags &= ~(ContactFlags.TOI | ContactFlags.Island);
-                    }
-                }
-
-                // Commit fixture proxy movements to the broad-phase so that new contacts are created.
-                // Also, some contacts can be destroyed.
-                ContactManager.FindNewContacts();
-
-                if (EnableSubStepping)
-                {
-                    _stepComplete = false;
-                    break;
-                }
-            }
-        }
-
-        public void AddController(Controller controller)
-        {
-            Debug.Assert(!ControllerList.Contains(controller), "You are adding the same controller more than once.");
-
-            controller.World = this;
-            ControllerList.Add(controller);
-
-            if (ControllerAdded != null)
-                ControllerAdded(controller);
-        }
-
-        public void RemoveController(Controller controller)
-        {
-            Debug.Assert(ControllerList.Contains(controller),
-                         "You are removing a controller that is not in the simulation.");
-
-            if (ControllerList.Contains(controller))
-            {
-                ControllerList.Remove(controller);
-
-                if (ControllerRemoved != null)
-                    ControllerRemoved(controller);
-            }
-        }
-
-        public void AddBreakableBody(BreakableBody breakableBody)
-        {
-            BreakableBodyList.Add(breakableBody);
-        }
-
-        public void RemoveBreakableBody(BreakableBody breakableBody)
-        {
-            //The breakable body list does not contain the body you tried to remove.
-            Debug.Assert(BreakableBodyList.Contains(breakableBody));
-
-            BreakableBodyList.Remove(breakableBody);
-        }
-
-        public Fixture TestPoint(Vector2 point)
-        {
-            AABB aabb;
-            Vector2 d = new Vector2(Settings.Epsilon, Settings.Epsilon);
-            aabb.LowerBound = point - d;
-            aabb.UpperBound = point + d;
-
-            Fixture myFixture = null;
-
-            // Query the world for overlapping shapes.
-            QueryAABB(
-                fixture =>
-                {
-                    bool inside = fixture.TestPoint(ref point);
-                    if (inside)
-                    {
-                        myFixture = fixture;
-                        return false;
-                    }
-
-                    // Continue the query.
-                    return true;
-                }, ref aabb);
-
-            return myFixture;
-        }
-
-        /// <summary>
-        /// Returns a list of fixtures that are at the specified point.
-        /// </summary>
-        /// <param name="point">The point.</param>
-        /// <returns></returns>
-        public List<Fixture> TestPointAll(Vector2 point)
-        {
-            AABB aabb;
-            Vector2 d = new Vector2(Settings.Epsilon, Settings.Epsilon);
-            aabb.LowerBound = point - d;
-            aabb.UpperBound = point + d;
-
-            List<Fixture> fixtures = new List<Fixture>();
-
-            // Query the world for overlapping shapes.
-            QueryAABB(
-                fixture =>
-                {
-                    bool inside = fixture.TestPoint(ref point);
-                    if (inside)
-                        fixtures.Add(fixture);
-
-                    // Continue the query.
-                    return true;
-                }, ref aabb);
-
-            return fixtures;
-        }
-
-        public void Clear()
-        {
-            ProcessChanges();
-
-            for (int i = BodyList.Count - 1; i >= 0; i--)
-            {
-                RemoveBody(BodyList[i]);
-            }
-
-            for (int i = ControllerList.Count - 1; i >= 0; i--)
-            {
-                RemoveController(ControllerList[i]);
-            }
-
-            for (int i = BreakableBodyList.Count - 1; i >= 0; i--)
-            {
-                RemoveBreakableBody(BreakableBodyList[i]);
-            }
-
-            ProcessChanges();
-        }
-    }
-}

FarseerPhysicsEngine/Dynamics/WorldCallbacks.cs

@@ -1,74 +0,0 @@
-/*
-* Farseer Physics Engine based on Box2D.XNA port:
-* Copyright (c) 2010 Ian Qvist
-* 
-* Box2D.XNA port of Box2D:
-* Copyright (c) 2009 Brandon Furtwangler, Nathan Furtwangler
-*
-* Original source Box2D:
-* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com 
-* 
-* This software is provided 'as-is', without any express or implied 
-* warranty.  In no event will the authors be held liable for any damages 
-* arising from the use of this software. 
-* Permission is granted to anyone to use this software for any purpose, 
-* including commercial applications, and to alter it and redistribute it 
-* freely, subject to the following restrictions: 
-* 1. The origin of this software must not be misrepresented; you must not 
-* claim that you wrote the original software. If you use this software 
-* in a product, an acknowledgment in the product documentation would be 
-* appreciated but is not required. 
-* 2. Altered source versions must be plainly marked as such, and must not be 
-* misrepresented as being the original software. 
-* 3. This notice may not be removed or altered from any source distribution. 
-*/
-
-using FarseerPhysics.Collision;
-using FarseerPhysics.Controllers;
-using FarseerPhysics.Dynamics.Contacts;
-using FarseerPhysics.Dynamics.Joints;
-using Microsoft.Xna.Framework;
-
-namespace FarseerPhysics.Dynamics
-{
-    /// <summary>
-    /// Called for each fixture found in the query. You control how the ray cast
-    /// proceeds by returning a float:
-    /// <returns>-1 to filter, 0 to terminate, fraction to clip the ray for closest hit, 1 to continue</returns>
-    /// </summary>
-    public delegate float RayCastCallback(Fixture fixture, Vector2 point, Vector2 normal, float fraction);
-
-    /// <summary>
-    /// This delegate is called when a contact is deleted
-    /// </summary>
-    public delegate void EndContactDelegate(Contact contact);
-
-    /// <summary>
-    /// This delegate is called when a contact is created
-    /// </summary>
-    public delegate bool BeginContactDelegate(Contact contact);
-
-    public delegate void PreSolveDelegate(Contact contact, ref Manifold oldManifold);
-
-    public delegate void PostSolveDelegate(Contact contact, ContactConstraint impulse);
-
-    public delegate void FixtureDelegate(Fixture fixture);
-
-    public delegate void JointDelegate(Joint joint);
-
-    public delegate void BodyDelegate(Body body);
-
-    public delegate void ControllerDelegate(Controller controller);
-
-    public delegate bool CollisionFilterDelegate(Fixture fixtureA, Fixture fixtureB);
-
-    public delegate void BroadphaseDelegate(ref FixtureProxy proxyA, ref FixtureProxy proxyB);
-
-    public delegate bool BeforeCollisionEventHandler(Fixture fixtureA, Fixture fixtureB);
-
-    public delegate bool OnCollisionEventHandler(Fixture fixtureA, Fixture fixtureB, Contact contact);
-
-    public delegate void AfterCollisionEventHandler(Fixture fixtureA, Fixture fixtureB, Contact contact);
-
-    public delegate void OnSeparationEventHandler(Fixture fixtureA, Fixture fixtureB);
-}

FarseerPhysicsEngine/Factories/BodyFactory.cs

@@ -1,399 +0,0 @@
-using System;
-using System.Collections.Generic;
-using FarseerPhysics.Collision.Shapes;
-using FarseerPhysics.Common;
-using FarseerPhysics.Common.Decomposition;
-using FarseerPhysics.Dynamics;
-using Microsoft.Xna.Framework;
-
-namespace FarseerPhysics.Factories
-{
-    public static class BodyFactory
-    {
-        public static Body CreateBody(World world)
-        {
-            return CreateBody(world, null);
-        }
-
-        public static Body CreateBody(World world, object userData)
-        {
-            Body body = new Body(world, userData);
-            return body;
-        }
-
-        public static Body CreateBody(World world, Vector2 position)
-        {
-            return CreateBody(world, position, null);
-        }
-
-        public static Body CreateBody(World world, Vector2 position, object userData)
-        {
-            Body body = CreateBody(world, userData);
-            body.Position = position;
-            return body;
-        }
-
-        public static Body CreateEdge(World world, Vector2 start, Vector2 end)
-        {
-            return CreateEdge(world, start, end, null);
-        }
-
-        public static Body CreateEdge(World world, Vector2 start, Vector2 end, object userData)
-        {
-            Body body = CreateBody(world);
-            FixtureFactory.AttachEdge(start, end, body, userData);
-            return body;
-        }
-
-        public static Body CreateLoopShape(World world, Vertices vertices)
-        {
-            return CreateLoopShape(world, vertices, null);
-        }
-
-        public static Body CreateLoopShape(World world, Vertices vertices, object userData)
-        {
-            return CreateLoopShape(world, vertices, Vector2.Zero, userData);
-        }
-
-        public static Body CreateLoopShape(World world, Vertices vertices, Vector2 position)
-        {
-            return CreateLoopShape(world, vertices, position, null);
-        }
-
-        public static Body CreateLoopShape(World world, Vertices vertices, Vector2 position,
-                                           object userData)
-        {
-            Body body = CreateBody(world, position);
-            FixtureFactory.AttachLoopShape(vertices, body, userData);
-            return body;
-        }
-
-        public static Body CreateRectangle(World world, float width, float height, float density)
-        {
-            return CreateRectangle(world, width, height, density, null);
-        }
-
-        public static Body CreateRectangle(World world, float width, float height, float density, object userData)
-        {
-            return CreateRectangle(world, width, height, density, Vector2.Zero, userData);
-        }
-
-        public static Body CreateRectangle(World world, float width, float height, float density, Vector2 position)
-        {
-            return CreateRectangle(world, width, height, density, position, null);
-        }
-
-        public static Body CreateRectangle(World world, float width, float height, float density, Vector2 position,
-                                           object userData)
-        {
-            if (width <= 0)
-                throw new ArgumentOutOfRangeException("width", "Width must be more than 0 meters");
-
-            if (height <= 0)
-                throw new ArgumentOutOfRangeException("height", "Height must be more than 0 meters");
-
-            Body newBody = CreateBody(world, position);
-            Vertices rectangleVertices = PolygonTools.CreateRectangle(width / 2, height / 2);
-            PolygonShape rectangleShape = new PolygonShape(rectangleVertices, density);
-            newBody.CreateFixture(rectangleShape, userData);
-
-            return newBody;
-        }
-
-        public static Body CreateCircle(World world, float radius, float density)
-        {
-            return CreateCircle(world, radius, density, null);
-        }
-
-        public static Body CreateCircle(World world, float radius, float density, object userData)
-        {
-            return CreateCircle(world, radius, density, Vector2.Zero, userData);
-        }
-
-        public static Body CreateCircle(World world, float radius, float density, Vector2 position)
-        {
-            return CreateCircle(world, radius, density, position, null);
-        }
-
-        public static Body CreateCircle(World world, float radius, float density, Vector2 position, object userData)
-        {
-            Body body = CreateBody(world, position);
-            FixtureFactory.AttachCircle(radius, density, body, userData);
-            return body;
-        }
-
-        public static Body CreateEllipse(World world, float xRadius, float yRadius, int edges, float density)
-        {
-            return CreateEllipse(world, xRadius, yRadius, edges, density, null);
-        }
-
-        public static Body CreateEllipse(World world, float xRadius, float yRadius, int edges, float density,
-                                         object userData)
-        {
-            return CreateEllipse(world, xRadius, yRadius, edges, density, Vector2.Zero, userData);
-        }
-
-        public static Body CreateEllipse(World world, float xRadius, float yRadius, int edges, float density,
-                                         Vector2 position)
-        {
-            return CreateEllipse(world, xRadius, yRadius, edges, density, position, null);
-        }
-
-        public static Body CreateEllipse(World world, float xRadius, float yRadius, int edges, float density,
-                                         Vector2 position, object userData)
-        {
-            Body body = CreateBody(world, position);
-            FixtureFactory.AttachEllipse(xRadius, yRadius, edges, density, body, userData);
-            return body;
-        }
-
-        public static Body CreatePolygon(World world, Vertices vertices, float density)
-        {
-            return CreatePolygon(world, vertices, density, null);
-        }
-
-        public static Body CreatePolygon(World world, Vertices vertices, float density, object userData)
-        {
-            return CreatePolygon(world, vertices, density, Vector2.Zero, userData);
-        }
-
-        public static Body CreatePolygon(World world, Vertices vertices, float density, Vector2 position)
-        {
-            return CreatePolygon(world, vertices, density, position, null);
-        }
-
-        public static Body CreatePolygon(World world, Vertices vertices, float density, Vector2 position,
-                                         object userData)
-        {
-            Body body = CreateBody(world, position);
-            FixtureFactory.AttachPolygon(vertices, density, body, userData);
-            return body;
-        }
-
-        public static Body CreateCompoundPolygon(World world, List<Vertices> list, float density)
-        {
-            return CreateCompoundPolygon(world, list, density, BodyType.Static);
-        }
-
-        public static Body CreateCompoundPolygon(World world, List<Vertices> list, float density,
-                                                 object userData)
-        {
-            return CreateCompoundPolygon(world, list, density, Vector2.Zero, userData);
-        }
-
-        public static Body CreateCompoundPolygon(World world, List<Vertices> list, float density,
-                                                 Vector2 position)
-        {
-            return CreateCompoundPolygon(world, list, density, position, null);
-        }
-
-        public static Body CreateCompoundPolygon(World world, List<Vertices> list, float density,
-                                                 Vector2 position, object userData)
-        {
-            //We create a single body
-            Body polygonBody = CreateBody(world, position);
-            FixtureFactory.AttachCompoundPolygon(list, density, polygonBody, userData);
-            return polygonBody;
-        }
-
-
-        public static Body CreateGear(World world, float radius, int numberOfTeeth, float tipPercentage,
-                                      float toothHeight, float density)
-        {
-            return CreateGear(world, radius, numberOfTeeth, tipPercentage, toothHeight, density, null);
-        }
-
-        public static Body CreateGear(World world, float radius, int numberOfTeeth, float tipPercentage,
-                                      float toothHeight, float density, object userData)
-        {
-            Vertices gearPolygon = PolygonTools.CreateGear(radius, numberOfTeeth, tipPercentage, toothHeight);
-
-            //Gears can in some cases be convex
-            if (!gearPolygon.IsConvex())
-            {
-                //Decompose the gear:
-                List<Vertices> list = EarclipDecomposer.ConvexPartition(gearPolygon);
-
-                return CreateCompoundPolygon(world, list, density, userData);
-            }
-
-            return CreatePolygon(world, gearPolygon, density, userData);
-        }
-
-        /// <summary>
-        /// Creates a capsule.
-        /// Note: Automatically decomposes the capsule if it contains too many vertices (controlled by Settings.MaxPolygonVertices)
-        /// </summary>
-        /// <param name="world">The world.</param>
-        /// <param name="height">The height.</param>
-        /// <param name="topRadius">The top radius.</param>
-        /// <param name="topEdges">The top edges.</param>
-        /// <param name="bottomRadius">The bottom radius.</param>
-        /// <param name="bottomEdges">The bottom edges.</param>
-        /// <param name="density">The density.</param>
-        /// <param name="position">The position.</param>
-        /// <returns></returns>
-        public static Body CreateCapsule(World world, float height, float topRadius, int topEdges,
-                                         float bottomRadius,
-                                         int bottomEdges, float density, Vector2 position, object userData)
-        {
-            Vertices verts = PolygonTools.CreateCapsule(height, topRadius, topEdges, bottomRadius, bottomEdges);
-
-            Body body;
-
-            //There are too many vertices in the capsule. We decompose it.
-            if (verts.Count >= Settings.MaxPolygonVertices)
-            {
-                List<Vertices> vertList = EarclipDecomposer.ConvexPartition(verts);
-                body = CreateCompoundPolygon(world, vertList, density, userData);
-                body.Position = position;
-
-                return body;
-            }
-
-            body = CreatePolygon(world, verts, density, userData);
-            body.Position = position;
-
-            return body;
-        }
-
-        public static Body CreateCapsule(World world, float height, float topRadius, int topEdges,
-                                         float bottomRadius,
-                                         int bottomEdges, float density, Vector2 position)
-        {
-            return CreateCapsule(world, height, topRadius, topEdges, bottomRadius, bottomEdges, density, position, null);
-        }
-
-        public static Body CreateCapsule(World world, float height, float endRadius, float density)
-        {
-            return CreateCapsule(world, height, endRadius, density, null);
-        }
-
-        public static Body CreateCapsule(World world, float height, float endRadius, float density,
-                                         object userData)
-        {
-            //Create the middle rectangle
-            Vertices rectangle = PolygonTools.CreateRectangle(endRadius, height / 2);
-
-            List<Vertices> list = new List<Vertices>();
-            list.Add(rectangle);
-
-            Body body = CreateCompoundPolygon(world, list, density, userData);
-
-            //Create the two circles
-            CircleShape topCircle = new CircleShape(endRadius, density);
-            topCircle.Position = new Vector2(0, height / 2);
-            body.CreateFixture(topCircle, userData);
-
-            CircleShape bottomCircle = new CircleShape(endRadius, density);
-            bottomCircle.Position = new Vector2(0, -(height / 2));
-            body.CreateFixture(bottomCircle, userData);
-            return body;
-        }
-
-        /// <summary>
-        /// Creates a rounded rectangle.
-        /// Note: Automatically decomposes the capsule if it contains too many vertices (controlled by Settings.MaxPolygonVertices)
-        /// </summary>
-        /// <param name="world">The world.</param>
-        /// <param name="width">The width.</param>
-        /// <param name="height">The height.</param>
-        /// <param name="xRadius">The x radius.</param>
-        /// <param name="yRadius">The y radius.</param>
-        /// <param name="segments">The segments.</param>
-        /// <param name="density">The density.</param>
-        /// <param name="position">The position.</param>
-        /// <returns></returns>
-        public static Body CreateRoundedRectangle(World world, float width, float height, float xRadius,
-                                                  float yRadius,
-                                                  int segments, float density, Vector2 position,
-                                                  object userData)
-        {
-            Vertices verts = PolygonTools.CreateRoundedRectangle(width, height, xRadius, yRadius, segments);
-
-            //There are too many vertices in the capsule. We decompose it.
-            if (verts.Count >= Settings.MaxPolygonVertices)
-            {
-                List<Vertices> vertList = EarclipDecomposer.ConvexPartition(verts);
-                Body body = CreateCompoundPolygon(world, vertList, density, userData);
-                body.Position = position;
-                return body;
-            }
-
-            return CreatePolygon(world, verts, density);
-        }
-
-        public static Body CreateRoundedRectangle(World world, float width, float height, float xRadius,
-                                                  float yRadius,
-                                                  int segments, float density, Vector2 position)
-        {
-            return CreateRoundedRectangle(world, width, height, xRadius, yRadius, segments, density, position, null);
-        }
-
-        public static Body CreateRoundedRectangle(World world, float width, float height, float xRadius,
-                                                  float yRadius,
-                                                  int segments, float density)
-        {
-            return CreateRoundedRectangle(world, width, height, xRadius, yRadius, segments, density, null);
-        }
-
-        public static Body CreateRoundedRectangle(World world, float width, float height, float xRadius,
-                                                  float yRadius,
-                                                  int segments, float density, object userData)
-        {
-            return CreateRoundedRectangle(world, width, height, xRadius, yRadius, segments, density, Vector2.Zero,
-                                          userData);
-        }
-
-        public static BreakableBody CreateBreakableBody(World world, Vertices vertices, float density)
-        {
-            return CreateBreakableBody(world, vertices, density, null);
-        }
-
-        public static BreakableBody CreateBreakableBody(World world, Vertices vertices, float density, object userData)
-        {
-            return CreateBreakableBody(world, vertices, density, Vector2.Zero, userData);
-        }
-
-        /// <summary>
-        /// Creates a breakable body. You would want to remove collinear points before using this.
-        /// </summary>
-        /// <param name="world">The world.</param>
-        /// <param name="vertices">The vertices.</param>
-        /// <param name="density">The density.</param>
-        /// <param name="position">The position.</param>
-        /// <returns></returns>
-        public static BreakableBody CreateBreakableBody(World world, Vertices vertices, float density, Vector2 position,
-                                                        object userData)
-        {
-            List<Vertices> triangles = EarclipDecomposer.ConvexPartition(vertices);
-
-            BreakableBody breakableBody = new BreakableBody(triangles, world, density, userData);
-            breakableBody.MainBody.Position = position;
-            world.AddBreakableBody(breakableBody);
-
-            return breakableBody;
-        }
-
-        public static BreakableBody CreateBreakableBody(World world, Vertices vertices, float density, Vector2 position)
-        {
-            return CreateBreakableBody(world, vertices, density, position, null);
-        }
-
-        public static Body CreateLineArc(World world, float radians, int sides, float radius, Vector2 position,
-                                         float angle, bool closed)
-        {
-            Body body = CreateBody(world);
-            FixtureFactory.AttachLineArc(radians, sides, radius, position, angle, closed, body);
-            return body;
-        }
-
-        public static Body CreateSolidArc(World world, float density, float radians, int sides, float radius,
-                                          Vector2 position, float angle)
-        {
-            Body body = CreateBody(world);
-            FixtureFactory.AttachSolidArc(density, radians, sides, radius, position, angle, body);
-            return body;
-        }
-    }
-}

FarseerPhysicsEngine/Factories/FixtureFactory.cs

@@ -1,179 +0,0 @@
-using System;
-using System.Collections.Generic;
-using FarseerPhysics.Collision.Shapes;
-using FarseerPhysics.Common;
-using FarseerPhysics.Common.Decomposition;
-using FarseerPhysics.Dynamics;
-using Microsoft.Xna.Framework;
-
-namespace FarseerPhysics.Factories
-{
-    /// <summary>
-    /// An easy to use factory for creating bodies
-    /// </summary>
-    public static class FixtureFactory
-    {
-        public static Fixture AttachEdge(Vector2 start, Vector2 end, Body body)
-        {
-            return AttachEdge(start, end, body, null);
-        }
-
-        public static Fixture AttachEdge(Vector2 start, Vector2 end, Body body, object userData)
-        {
-            EdgeShape edgeShape = new EdgeShape(start, end);
-            return body.CreateFixture(edgeShape, userData);
-        }
-
-        public static Fixture AttachLoopShape(Vertices vertices, Body body)
-        {
-            return AttachLoopShape(vertices, body, null);
-        }
-
-        public static Fixture AttachLoopShape(Vertices vertices, Body body, object userData)
-        {
-            LoopShape shape = new LoopShape(vertices);
-            return body.CreateFixture(shape, userData);
-        }
-
-        public static Fixture AttachRectangle(float width, float height, float density, Vector2 offset, Body body,
-                                              object userData)
-        {
-            Vertices rectangleVertices = PolygonTools.CreateRectangle(width / 2, height / 2);
-            rectangleVertices.Translate(ref offset);
-            PolygonShape rectangleShape = new PolygonShape(rectangleVertices, density);
-            return body.CreateFixture(rectangleShape, userData);
-        }
-
-        public static Fixture AttachRectangle(float width, float height, float density, Vector2 offset, Body body)
-        {
-            return AttachRectangle(width, height, density, offset, body, null);
-        }
-
-        public static Fixture AttachCircle(float radius, float density, Body body)
-        {
-            return AttachCircle(radius, density, body, null);
-        }
-
-        public static Fixture AttachCircle(float radius, float density, Body body, object userData)
-        {
-            if (radius <= 0)
-                throw new ArgumentOutOfRangeException("radius", "Radius must be more than 0 meters");
-
-            CircleShape circleShape = new CircleShape(radius, density);
-            return body.CreateFixture(circleShape, userData);
-        }
-
-        public static Fixture AttachCircle(float radius, float density, Body body, Vector2 offset)
-        {
-            return AttachCircle(radius, density, body, offset, null);
-        }
-
-        public static Fixture AttachCircle(float radius, float density, Body body, Vector2 offset, object userData)
-        {
-            if (radius <= 0)
-                throw new ArgumentOutOfRangeException("radius", "Radius must be more than 0 meters");
-
-            CircleShape circleShape = new CircleShape(radius, density);
-            circleShape.Position = offset;
-            return body.CreateFixture(circleShape, userData);
-        }
-
-        public static Fixture AttachPolygon(Vertices vertices, float density, Body body)
-        {
-            return AttachPolygon(vertices, density, body, null);
-        }
-
-        public static Fixture AttachPolygon(Vertices vertices, float density, Body body, object userData)
-        {
-            if (vertices.Count <= 1)
-                throw new ArgumentOutOfRangeException("vertices", "Too few points to be a polygon");
-
-            PolygonShape polygon = new PolygonShape(vertices, density);
-            return body.CreateFixture(polygon, userData);
-        }
-
-        public static Fixture AttachEllipse(float xRadius, float yRadius, int edges, float density, Body body)
-        {
-            return AttachEllipse(xRadius, yRadius, edges, density, body, null);
-        }
-
-        public static Fixture AttachEllipse(float xRadius, float yRadius, int edges, float density, Body body,
-                                            object userData)
-        {
-            if (xRadius <= 0)
-                throw new ArgumentOutOfRangeException("xRadius", "X-radius must be more than 0");
-
-            if (yRadius <= 0)
-                throw new ArgumentOutOfRangeException("yRadius", "Y-radius must be more than 0");
-
-            Vertices ellipseVertices = PolygonTools.CreateEllipse(xRadius, yRadius, edges);
-            PolygonShape polygonShape = new PolygonShape(ellipseVertices, density);
-            return body.CreateFixture(polygonShape, userData);
-        }
-
-        public static List<Fixture> AttachCompoundPolygon(List<Vertices> list, float density, Body body)
-        {
-            return AttachCompoundPolygon(list, density, body, null);
-        }
-
-        public static List<Fixture> AttachCompoundPolygon(List<Vertices> list, float density, Body body, object userData)
-        {
-            List<Fixture> res = new List<Fixture>(list.Count);
-
-            //Then we create several fixtures using the body
-            foreach (Vertices vertices in list)
-            {
-                if (vertices.Count == 2)
-                {
-                    EdgeShape shape = new EdgeShape(vertices[0], vertices[1]);
-                    res.Add(body.CreateFixture(shape, userData));
-                }
-                else
-                {
-                    PolygonShape shape = new PolygonShape(vertices, density);
-                    res.Add(body.CreateFixture(shape, userData));
-                }
-            }
-
-            return res;
-        }
-
-        public static List<Fixture> AttachLineArc(float radians, int sides, float radius, Vector2 position, float angle,
-                                                  bool closed, Body body)
-        {
-            Vertices arc = PolygonTools.CreateArc(radians, sides, radius);
-            arc.Rotate((MathHelper.Pi - radians) / 2 + angle);
-            arc.Translate(ref position);
-
-            List<Fixture> fixtures = new List<Fixture>(arc.Count);
-
-            if (closed)
-            {
-                fixtures.Add(AttachLoopShape(arc, body));
-            }
-
-            for (int i = 1; i < arc.Count; i++)
-            {
-                fixtures.Add(AttachEdge(arc[i], arc[i - 1], body));
-            }
-
-            return fixtures;
-        }
-
-        public static List<Fixture> AttachSolidArc(float density, float radians, int sides, float radius,
-                                                   Vector2 position, float angle, Body body)
-        {
-            Vertices arc = PolygonTools.CreateArc(radians, sides, radius);
-            arc.Rotate((MathHelper.Pi - radians) / 2 + angle);
-
-            arc.Translate(ref position);
-
-            //Close the arc
-            arc.Add(arc[0]);
-
-            List<Vertices> triangles = EarclipDecomposer.ConvexPartition(arc);
-
-            return AttachCompoundPolygon(triangles, density, body);
-        }
-    }
-}