Server job assigning logic, submarine movement syncing, submarine collision improvements, spawnpoints in levels

Farseer Physics Engine 3.5/Collision/Distance.cs

@@ -0,0 +1,794 @@
+/*
+* Farseer Physics Engine:
+* Copyright (c) 2012 Ian Qvist
+* 
+* Original source Box2D:
+* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org 
+* 
+* 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();
+
+        // GJK using Voronoi regions (Christer Ericson) and Barycentric coordinates.
+
+        /// <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.Chain:
+                    {
+                        ChainShape chain = (ChainShape)shape;
+                        Debug.Assert(0 <= index && index < chain.Vertices.Count);
+                        Vertices.Clear();
+                        Vertices.Add(chain.Vertices[index]);
+                        Vertices.Add(index + 1 < chain.Vertices.Count ? chain.Vertices[index + 1] : chain.Vertices[0]);
+
+                        Radius = chain.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 Distance.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 Distance.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.Mul(ref transformA, wALocal);
+                v.WB = MathUtils.Mul(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.Mul(ref transformA, wALocal);
+                v.WB = MathUtils.Mul(ref transformB, wBLocal);
+                v.W = v.WB - v.WA;
+                v.A = 1.0f;
+                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;
+        }
+    }
+
+    /// <summary>
+    /// The Gilbert–Johnson–Keerthi distance algorithm that provides the distance between shapes.
+    /// </summary>
+    public static class Distance
+    {
+        /// <summary>
+        /// The number of calls made to the ComputeDistance() function.
+        /// Note: This is only activated when Settings.EnableDiagnostics = true
+        /// </summary>
+        [ThreadStatic]
+        public static int GJKCalls;
+
+        /// <summary>
+        /// The number of iterations that was made on the last call to ComputeDistance().
+        /// Note: This is only activated when Settings.EnableDiagnostics = true
+        /// </summary>
+        [ThreadStatic]
+        public static int GJKIters;
+
+        /// <summary>
+        /// The maximum numer of iterations ever mae with calls to the CompteDistance() funtion.
+        /// Note: This is only activated when Settings.EnableDiagnostics = true
+        /// </summary>
+        [ThreadStatic]
+        public static int GJKMaxIters;
+
+        public static void ComputeDistance(out DistanceOutput output, out SimplexCache cache, DistanceInput input)
+        {
+            cache = new SimplexCache();
+
+            if (Settings.EnableDiagnostics) //FPE: We only gather diagnostics when enabled
+                ++GJKCalls;
+
+            // Initialize the simplex.
+            Simplex simplex = new Simplex();
+            simplex.ReadCache(ref cache, input.ProxyA, ref input.TransformA, input.ProxyB, ref input.TransformB);
+
+            // 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>();
+
+            //float distanceSqr1 = Settings.MaxFloat;
+
+            // Main iteration loop.
+            int iter = 0;
+            while (iter < Settings.MaxGJKIterations)
+            {
+                // 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;
+                }
+
+                //FPE: This code was not used anyway.
+                // Compute closest point.
+                //Vector2 p = simplex.GetClosestPoint();
+                //float 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.MulT(input.TransformA.q, -d));
+                vertex.WA = MathUtils.Mul(ref input.TransformA, input.ProxyA.Vertices[vertex.IndexA]);
+
+                vertex.IndexB = input.ProxyB.GetSupport(MathUtils.MulT(input.TransformB.q, d));
+                vertex.WB = MathUtils.Mul(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;
+
+                if (Settings.EnableDiagnostics) //FPE: We only gather diagnostics when enabled
+                    ++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;
+            }
+
+            if (Settings.EnableDiagnostics) //FPE: We only gather diagnostics when enabled
+                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;
+                }
+            }
+        }
+    }
+}

Farseer Physics Engine 3.5/Collision/DynamicTreeBroadPhase.cs

@@ -0,0 +1,347 @@
+/*
+* Farseer Physics Engine:
+* Copyright (c) 2012 Ian Qvist
+* 
+* Original source Box2D:
+* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org 
+* 
+* 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 const int NullProxy = -1;
+        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>();
+
+        /// <summary>
+        /// Constructs a new broad phase based on the dynamic tree implementation
+        /// </summary>
+        public DynamicTreeBroadPhase()
+        {
+            _queryCallback = QueryCallback;
+            _proxyCount = 0;
+
+            _pairCapacity = 16;
+            _pairCount = 0;
+            _pairBuffer = new Pair[_pairCapacity];
+
+            _moveCapacity = 16;
+            _moveCount = 0;
+            _moveBuffer = new int[_moveCapacity];
+        }
+
+        /// <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="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);
+            }
+        }
+
+        public void TouchProxy(int proxyId)
+        {
+            BufferMove(proxyId);
+        }
+
+        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] = NullProxy;
+                }
+            }
+        }
+
+        /// <summary>
+        /// This is called from DynamicTree.Query when we are gathering pairs.
+        /// </summary>
+        /// <param name="proxyId"></param>
+        /// <returns></returns>
+        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;
+        }
+
+        /// <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 == NullProxy)
+                {
+                    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 ShiftOrigin(Vector2 newOrigin)
+        {
+            _tree.ShiftOrigin(newOrigin);
+        }
+
+        /// <summary>
+        /// Get the tree quality based on the area of the tree.
+        /// </summary>
+        public float TreeQuality
+        {
+            get { return _tree.AreaRatio; }
+        }
+
+        /// <summary>
+        /// Gets the balance of the tree.
+        /// </summary>
+        public int TreeBalance
+        {
+            get { return _tree.MaxBalance; }
+        }
+
+        /// <summary>
+        /// Gets the height of the tree.
+        /// </summary>
+        public int TreeHeight
+        {
+            get { return _tree.Height; }
+        }
+    }
+}

Farseer Physics Engine 3.5/Collision/IBroadPhase.cs

@@ -0,0 +1,32 @@
+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);
+
+        void ShiftOrigin(Vector2 newOrigin);
+    }
+}

Farseer Physics Engine 3.5/Collision/Shapes/ChainShape.cs

@@ -0,0 +1,273 @@
+/*
+* Farseer Physics Engine:
+* Copyright (c) 2012 Ian Qvist
+* 
+* Original source Box2D:
+* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org 
+* 
+* 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 chain shape is a free form sequence of line segments.
+    /// The chain has two-sided collision, so you can use inside and outside collision.
+    /// Therefore, you may use any winding order.
+    /// Connectivity information is used to create smooth collisions.
+    /// WARNING: The chain will not collide properly if there are self-intersections.
+    /// </summary>
+    public class ChainShape : Shape
+    {
+        /// <summary>
+        /// The vertices. These are not owned/freed by the chain Shape.
+        /// </summary>
+        public Vertices Vertices;
+        private Vector2 _prevVertex, _nextVertex;
+        private bool _hasPrevVertex, _hasNextVertex;
+        private static EdgeShape _edgeShape = new EdgeShape();
+
+        /// <summary>
+        /// Constructor for ChainShape. By default have 0 in density.
+        /// </summary>
+        public ChainShape()
+            : base(0)
+        {
+            ShapeType = ShapeType.Chain;
+            _radius = Settings.PolygonRadius;
+        }
+
+        /// <summary>
+        /// Create a new chainshape from the vertices.
+        /// </summary>
+        /// <param name="vertices">The vertices to use. Must contain 2 or more vertices.</param>
+        /// <param name="createLoop">Set to true to create a closed loop. It connects the first vertice to the last, and automatically adjusts connectivity to create smooth collisions along the chain.</param>
+        public ChainShape(Vertices vertices, bool createLoop = false)
+            : base(0)
+        {
+            ShapeType = ShapeType.Chain;
+            _radius = Settings.PolygonRadius;
+
+            if (!(vertices != null && vertices.Count >= 2))
+            {
+                int lkmsdgkldf = 1;
+            }
+
+            Debug.Assert(vertices != null && vertices.Count >= 2);
+            Debug.Assert(vertices[0] != vertices[vertices.Count - 1]); // FPE. See http://www.box2d.org/forum/viewtopic.php?f=4&t=7973&p=35363
+
+            for (int i = 1; i < vertices.Count; ++i)
+            {
+                Vector2 v1 = vertices[i - 1];
+                Vector2 v2 = vertices[i];
+
+                // If the code crashes here, it means your vertices are too close together.
+
+                if (Vector2.DistanceSquared(v1, v2) < Settings.LinearSlop * Settings.LinearSlop)
+                {
+                    int asldmfk = 1;
+                }
+
+                Debug.Assert(Vector2.DistanceSquared(v1, v2) > Settings.LinearSlop * Settings.LinearSlop);
+            }
+
+            Vertices = new Vertices(vertices);
+
+            if (createLoop)
+            {
+                Vertices.Add(vertices[0]);
+                PrevVertex = Vertices[Vertices.Count - 2]; //FPE: We use the properties instead of the private fields here.
+                NextVertex = Vertices[1]; //FPE: We use the properties instead of the private fields here.
+            }
+        }
+
+        public override int ChildCount
+        {
+            // edge count = vertex count - 1
+            get { return Vertices.Count - 1; }
+        }
+
+        /// <summary>
+        /// Establish connectivity to a vertex that precedes the first vertex.
+        /// Don't call this for loops.
+        /// </summary>
+        public Vector2 PrevVertex
+        {
+            get { return _prevVertex; }
+            set
+            {
+                Debug.Assert(value != null);
+
+                _prevVertex = value;
+                _hasPrevVertex = true;
+            }
+        }
+
+        /// <summary>
+        /// Establish connectivity to a vertex that follows the last vertex.
+        /// Don't call this for loops.
+        /// </summary>
+        public Vector2 NextVertex
+        {
+            get { return _nextVertex; }
+            set
+            {
+                Debug.Assert(value != null);
+
+                _nextVertex = value;
+                _hasNextVertex = true;
+            }
+        }
+
+        /// <summary>
+        /// This method has been optimized to reduce garbage.
+        /// </summary>
+        /// <param name="edge">The cached edge to set properties on.</param>
+        /// <param name="index">The index.</param>
+        internal void GetChildEdge(EdgeShape edge, int index)
+        {
+            Debug.Assert(0 <= index && index < Vertices.Count - 1);
+            Debug.Assert(edge != null);
+
+            edge.ShapeType = ShapeType.Edge;
+            edge._radius = _radius;
+
+            edge.Vertex1 = Vertices[index + 0];
+            edge.Vertex2 = Vertices[index + 1];
+
+            if (index > 0)
+            {
+                edge.Vertex0 = Vertices[index - 1];
+                edge.HasVertex0 = true;
+            }
+            else
+            {
+                edge.Vertex0 = _prevVertex;
+                edge.HasVertex0 = _hasPrevVertex;
+            }
+
+            if (index < Vertices.Count - 2)
+            {
+                edge.Vertex3 = Vertices[index + 2];
+                edge.HasVertex3 = true;
+            }
+            else
+            {
+                edge.Vertex3 = _nextVertex;
+                edge.HasVertex3 = _hasNextVertex;
+            }
+        }
+
+        /// <summary>
+        /// Get a child edge.
+        /// </summary>
+        /// <param name="index">The index.</param>
+        public EdgeShape GetChildEdge(int index)
+        {
+            EdgeShape edgeShape = new EdgeShape();
+            GetChildEdge(edgeShape, index);
+            return edgeShape;
+        }
+
+        public override bool TestPoint(ref Transform transform, ref Vector2 point)
+        {
+            return false;
+        }
+
+        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);
+        }
+
+        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.Mul(ref transform, Vertices[i1]);
+            Vector2 v2 = MathUtils.Mul(ref transform, Vertices[i2]);
+
+            aabb.LowerBound = Vector2.Min(v1, v2);
+            aabb.UpperBound = Vector2.Max(v1, v2);
+        }
+
+        protected override void ComputeProperties()
+        {
+            //Does nothing. Chain shapes don't have properties.
+        }
+
+        public override float ComputeSubmergedArea(ref Vector2 normal, float offset, ref Transform xf, out Vector2 sc)
+        {
+            sc = Vector2.Zero;
+            return 0;
+        }
+
+        /// <summary>
+        /// Compare the chain to another chain
+        /// </summary>
+        /// <param name="shape">The other chain</param>
+        /// <returns>True if the two chain shapes are the same</returns>
+        public bool CompareTo(ChainShape 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 PrevVertex == shape.PrevVertex && NextVertex == shape.NextVertex;
+        }
+
+        public override Shape Clone()
+        {
+            ChainShape clone = new ChainShape();
+            clone.ShapeType = ShapeType;
+            clone._density = _density;
+            clone._radius = _radius;
+            clone.PrevVertex = _prevVertex;
+            clone.NextVertex = _nextVertex;
+            clone._hasNextVertex = _hasNextVertex;
+            clone._hasPrevVertex = _hasPrevVertex;
+            clone.Vertices = new Vertices(Vertices);
+            clone.MassData = MassData;
+            return clone;
+        }
+    }
+}

Farseer Physics Engine 3.5/Collision/Shapes/CircleShape.cs

@@ -0,0 +1,198 @@
+/*
+* Farseer Physics Engine:
+* Copyright (c) 2012 Ian Qvist
+* 
+* Original source Box2D:
+* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org
+*
+* 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.Shapes
+{
+    /// <summary>
+    /// A circle shape.
+    /// </summary>
+    public class CircleShape : Shape
+    {
+        internal Vector2 _position;
+
+        /// <summary>
+        /// Create a new circle with the desired radius and density.
+        /// </summary>
+        /// <param name="radius">The radius of the circle.</param>
+        /// <param name="density">The density of the circle.</param>
+        public CircleShape(float radius, float density)
+            : base(density)
+        {
+            Debug.Assert(radius >= 0);
+            Debug.Assert(density >= 0);
+
+            ShapeType = ShapeType.Circle;
+            _position = Vector2.Zero;
+            Radius = radius; // The Radius property cache 2radius and calls ComputeProperties(). So no need to call ComputeProperties() here.
+        }
+
+        internal CircleShape()
+            : base(0)
+        {
+            ShapeType = ShapeType.Circle;
+            _radius = 0.0f;
+            _position = Vector2.Zero;
+        }
+
+        public override int ChildCount
+        {
+            get { return 1; }
+        }
+
+        /// <summary>
+        /// Get or set the position of the circle
+        /// </summary>
+        public Vector2 Position
+        {
+            get { return _position; }
+            set
+            {
+                _position = value;
+                ComputeProperties(); //TODO: Optimize here
+            }
+        }
+
+        public override bool TestPoint(ref Transform transform, ref Vector2 point)
+        {
+            Vector2 center = transform.p + MathUtils.Mul(transform.q, Position);
+            Vector2 d = point - center;
+            return Vector2.Dot(d, d) <= _2radius;
+        }
+
+        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.p + MathUtils.Mul(transform.q, Position);
+            Vector2 s = input.Point1 - position;
+            float b = Vector2.Dot(s, s) - _2radius;
+
+            // 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;
+
+                //TODO: Check results here
+                output.Normal = s + a * r;
+                output.Normal.Normalize();
+                return true;
+            }
+
+            return false;
+        }
+
+        public override void ComputeAABB(out AABB aabb, ref Transform transform, int childIndex)
+        {
+            Vector2 p = transform.p + MathUtils.Mul(transform.q, Position);
+            aabb.LowerBound = new Vector2(p.X - Radius, p.Y - Radius);
+            aabb.UpperBound = new Vector2(p.X + Radius, p.Y + Radius);
+        }
+
+        protected override sealed void ComputeProperties()
+        {
+            float area = Settings.Pi * _2radius;
+            MassData.Area = area;
+            MassData.Mass = Density * area;
+            MassData.Centroid = Position;
+
+            // inertia about the local origin
+            MassData.Inertia = MassData.Mass * (0.5f * _2radius + Vector2.Dot(Position, Position));
+        }
+
+        public override float ComputeSubmergedArea(ref Vector2 normal, float offset, ref Transform xf, out Vector2 sc)
+        {
+            sc = Vector2.Zero;
+
+            Vector2 p = MathUtils.Mul(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 * _2radius;
+            }
+
+            //Magic
+            float l2 = l * l;
+            float area = _2radius * (float)((Math.Asin(l / Radius) + Settings.Pi / 2) + l * Math.Sqrt(_2radius - l2));
+            float com = -2.0f / 3.0f * (float)Math.Pow(_2radius - l2, 1.5f) / area;
+
+            sc.X = p.X + normal.X * com;
+            sc.Y = p.Y + normal.Y * com;
+
+            return area;
+        }
+
+        /// <summary>
+        /// Compare the circle to another circle
+        /// </summary>
+        /// <param name="shape">The other circle</param>
+        /// <returns>True if the two circles are the same size and have the same position</returns>
+        public bool CompareTo(CircleShape shape)
+        {
+            return (Radius == shape.Radius && Position == shape.Position);
+        }
+
+        public override Shape Clone()
+        {
+            CircleShape clone = new CircleShape();
+            clone.ShapeType = ShapeType;
+            clone._radius = Radius;
+            clone._2radius = _2radius; //FPE note: We also copy the cache
+            clone._density = _density;
+            clone._position = _position;
+            clone.MassData = MassData;
+            return clone;
+        }
+    }
+}

Farseer Physics Engine 3.5/Collision/Shapes/EdgeShape.cs

@@ -0,0 +1,252 @@
+/*
+* Farseer Physics Engine:
+* Copyright (c) 2012 Ian Qvist
+* 
+* Original source Box2D:
+* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org 
+* 
+* 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
+    {
+        /// <summary>
+        /// Edge start vertex
+        /// </summary>
+        internal Vector2 _vertex1;
+
+        /// <summary>
+        /// Edge end vertex
+        /// </summary>
+        internal Vector2 _vertex2;
+
+        internal EdgeShape()
+            : base(0)
+        {
+            ShapeType = ShapeType.Edge;
+            _radius = Settings.PolygonRadius;
+        }
+
+        /// <summary>
+        /// Create a new EdgeShape with the specified start and end.
+        /// </summary>
+        /// <param name="start">The start of the edge.</param>
+        /// <param name="end">The end of the edge.</param>
+        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>
+        /// Is true if the edge is connected to an adjacent vertex before vertex 1.
+        /// </summary>
+        public bool HasVertex0 { get; set; }
+
+        /// <summary>
+        /// Is true if the edge is connected to an adjacent vertex after vertex2.
+        /// </summary>
+        public bool HasVertex3 { get; set; }
+
+        /// <summary>
+        /// Optional adjacent vertices. These are used for smooth collision.
+        /// </summary>
+        public Vector2 Vertex0 { get; set; }
+
+        /// <summary>
+        /// Optional adjacent vertices. These are used for smooth collision.
+        /// </summary>
+        public Vector2 Vertex3 { get; set; }
+
+        /// <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 bool TestPoint(ref Transform transform, ref Vector2 point)
+        {
+            return false;
+        }
+
+        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.MulT(transform.q, input.Point1 - transform.p);
+            Vector2 p2 = MathUtils.MulT(transform.q, input.Point2 - transform.p);
+            Vector2 d = p2 - p1;
+
+            Vector2 v1 = _vertex1;
+            Vector2 v2 = _vertex2;
+            Vector2 e = v2 - v1;
+            Vector2 normal = new Vector2(e.Y, -e.X); //TODO: Could possibly cache the normal.
+            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 || input.MaxFraction < 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;
+        }
+
+        public override void ComputeAABB(out AABB aabb, ref Transform transform, int childIndex)
+        {
+            Vector2 v1 = MathUtils.Mul(ref transform, _vertex1);
+            Vector2 v2 = MathUtils.Mul(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;
+        }
+
+        protected override void ComputeProperties()
+        {
+            MassData.Centroid = 0.5f * (_vertex1 + _vertex2);
+        }
+
+        public override float ComputeSubmergedArea(ref Vector2 normal, float offset, ref 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);
+        }
+
+        public override Shape Clone()
+        {
+            EdgeShape clone = new EdgeShape();
+            clone.ShapeType = ShapeType;
+            clone._radius = _radius;
+            clone._density = _density;
+            clone.HasVertex0 = HasVertex0;
+            clone.HasVertex3 = HasVertex3;
+            clone.Vertex0 = Vertex0;
+            clone._vertex1 = _vertex1;
+            clone._vertex2 = _vertex2;
+            clone.Vertex3 = Vertex3;
+            clone.MassData = MassData;
+            return clone;
+        }
+    }
+}

Farseer Physics Engine 3.5/Collision/Shapes/PolygonShape.cs

@@ -0,0 +1,471 @@
+/*
+* Farseer Physics Engine:
+* Copyright (c) 2012 Ian Qvist
+* 
+* Original source Box2D:
+* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org 
+* 
+* 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.ConvexHull;
+using Microsoft.Xna.Framework;
+
+namespace FarseerPhysics.Collision.Shapes
+{
+    /// <summary>
+    /// Represents a simple non-selfintersecting convex polygon.
+    /// Create a convex hull from the given array of points.
+    /// </summary>
+    public class PolygonShape : Shape
+    {
+        private Vertices _vertices;
+        private Vertices _normals;
+
+        /// <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;
+
+            Vertices = vertices;
+        }
+
+        /// <summary>
+        /// Create a new PolygonShape with the specified density.
+        /// </summary>
+        /// <param name="density">The density.</param>
+        public PolygonShape(float density)
+            : base(density)
+        {
+            Debug.Assert(density >= 0f);
+
+            ShapeType = ShapeType.Polygon;
+            _radius = Settings.PolygonRadius;
+            _vertices = new Vertices(Settings.MaxPolygonVertices);
+            _normals = new Vertices(Settings.MaxPolygonVertices);
+        }
+
+        internal PolygonShape()
+            : base(0)
+        {
+            ShapeType = ShapeType.Polygon;
+            _radius = Settings.PolygonRadius;
+            _vertices = new Vertices(Settings.MaxPolygonVertices);
+            _normals = new Vertices(Settings.MaxPolygonVertices);
+        }
+
+        /// <summary>
+        /// Create a convex hull from the given array of local points.
+        /// The number of vertices must be in the range [3, Settings.MaxPolygonVertices].
+        /// Warning: the points may be re-ordered, even if they form a convex polygon
+        /// Warning: collinear points are handled but not removed. Collinear points may lead to poor stacking behavior.
+        /// </summary>
+        public Vertices Vertices
+        {
+            get { return _vertices; }
+            set
+            {
+                _vertices = new Vertices(value);
+
+                //Debug.Assert(_vertices.Count >= 3 && _vertices.Count <= Settings.MaxPolygonVertices);
+
+                if (Settings.UseConvexHullPolygons)
+                {
+                    //FPE note: This check is required as the GiftWrap algorithm early exits on triangles
+                    //So instead of giftwrapping a triangle, we just force it to be clock wise.
+                    if (_vertices.Count <= 3)
+                        _vertices.ForceCounterClockWise();
+                    else
+                        _vertices = GiftWrap.GetConvexHull(_vertices);
+                }
+
+                _normals = new Vertices(_vertices.Count);
+
+                // Compute normals. Ensure the edges have non-zero length.
+                for (int i = 0; i < _vertices.Count; ++i)
+                {
+                    int next = i + 1 < _vertices.Count ? i + 1 : 0;
+                    Vector2 edge = _vertices[next] - _vertices[i];
+                    Debug.Assert(edge.LengthSquared() > Settings.Epsilon * Settings.Epsilon);
+
+                    //FPE optimization: Normals.Add(MathHelper.Cross(edge, 1.0f));
+                    Vector2 temp = new Vector2(edge.Y, -edge.X);
+                    temp.Normalize();
+                    _normals.Add(temp);
+                }
+
+                // Compute the polygon mass data
+                ComputeProperties();
+            }
+        }
+
+        public Vertices Normals { get { return _normals; } }
+
+        public override int ChildCount { get { return 1; } }
+
+        protected 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);
+
+            //FPE optimization: Early exit as polygons with 0 density does not have any properties.
+            if (_density <= 0)
+                return;
+
+            //FPE optimization: Consolidated the calculate centroid and mass code to a single method.
+            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 s = Vector2.Zero;
+
+            // This code would put the reference point inside the polygon.
+            for (int i = 0; i < Vertices.Count; ++i)
+            {
+                s += Vertices[i];
+            }
+            s *= 1.0f / Vertices.Count;
+
+            const float k_inv3 = 1.0f / 3.0f;
+
+            for (int i = 0; i < Vertices.Count; ++i)
+            {
+                // Triangle vertices.
+                Vector2 e1 = Vertices[i] - s;
+                Vector2 e2 = i + 1 < Vertices.Count ? Vertices[i + 1] - s : Vertices[0] - s;
+
+                float D = MathUtils.Cross(e1, e2);
+
+                float triangleArea = 0.5f * D;
+                area += triangleArea;
+
+                // Area weighted centroid
+                center += triangleArea * k_inv3 * (e1 + e2);
+
+                float ex1 = e1.X, ey1 = e1.Y;
+                float ex2 = e2.X, ey2 = e2.Y;
+
+                float intx2 = ex1 * ex1 + ex2 * ex1 + ex2 * ex2;
+                float inty2 = ey1 * ey1 + ey2 * ey1 + ey2 * ey2;
+
+                I += (0.25f * k_inv3 * 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 + s;
+
+            // Inertia tensor relative to the local origin (point s).
+            MassData.Inertia = _density * I;
+
+            // Shift to center of mass then to original body origin.
+            MassData.Inertia += MassData.Mass * (Vector2.Dot(MassData.Centroid, MassData.Centroid) - Vector2.Dot(center, center));
+        }
+
+        public override bool TestPoint(ref Transform transform, ref Vector2 point)
+        {
+            Vector2 pLocal = MathUtils.MulT(transform.q, point - transform.p);
+
+            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;
+        }
+
+        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.MulT(transform.q, input.Point1 - transform.p);
+            Vector2 p2 = MathUtils.MulT(transform.q, input.Point2 - transform.p);
+            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.Mul(transform.q, 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.Mul(ref transform, Vertices[0]);
+            Vector2 upper = lower;
+
+            for (int i = 1; i < Vertices.Count; ++i)
+            {
+                Vector2 v = MathUtils.Mul(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 override float ComputeSubmergedArea(ref Vector2 normal, float offset, ref Transform xf, out Vector2 sc)
+        {
+            sc = Vector2.Zero;
+
+            //Transform plane into shape co-ordinates
+            Vector2 normalL = MathUtils.MulT(xf.q, normal);
+            float offsetL = offset - Vector2.Dot(normal, xf.p);
+
+            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.Mul(ref xf, MassData.Centroid);
+                        return MassData.Mass / Density;
+                    }
+
+                    //Completely dry
+                    return 0;
+                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];
+
+            const 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;
+                Vector2 p3;
+                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.Mul(ref xf, center);
+
+            return area;
+        }
+
+        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 Shape Clone()
+        {
+            PolygonShape clone = new PolygonShape();
+            clone.ShapeType = ShapeType;
+            clone._radius = _radius;
+            clone._density = _density;
+            clone._vertices = new Vertices(_vertices);
+            clone._normals = new Vertices(_normals);
+            clone.MassData = MassData;
+            return clone;
+        }
+    }
+}

Farseer Physics Engine 3.5/Collision/Shapes/Shape.cs

@@ -0,0 +1,255 @@
+/*
+* Farseer Physics Engine:
+* Copyright (c) 2012 Ian Qvist
+* 
+* Original source Box2D:
+* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org 
+* 
+* 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.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 { get; internal set; }
+
+        /// <summary>
+        /// The position of the shape's centroid relative to the shape's origin.
+        /// </summary>
+        public Vector2 Centroid { get; internal set; }
+
+        /// <summary>
+        /// The rotational inertia of the shape about the local origin.
+        /// </summary>
+        public float Inertia { get; internal set; }
+
+        /// <summary>
+        /// The mass of the shape, usually in kilograms.
+        /// </summary>
+        public float Mass { get; internal set; }
+
+        /// <summary>
+        /// The equal operator
+        /// </summary>
+        /// <param name="left"></param>
+        /// <param name="right"></param>
+        /// <returns></returns>
+        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);
+        }
+
+        /// <summary>
+        /// The not equal operator
+        /// </summary>
+        /// <param name="left"></param>
+        /// <param name="right"></param>
+        /// <returns></returns>
+        public static bool operator !=(MassData left, MassData right)
+        {
+            return !(left == right);
+        }
+
+        public bool Equals(MassData other)
+        {
+            return this == other;
+        }
+
+        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,
+        Chain = 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
+    {
+        internal float _density;
+        internal float _radius;
+        internal float _2radius;
+
+        protected Shape(float density)
+        {
+            _density = density;
+            ShapeType = ShapeType.Unknown;
+        }
+
+        /// <summary>
+        /// Contains the properties of the shape such as:
+        /// - Area of the shape
+        /// - Centroid
+        /// - Inertia
+        /// - Mass
+        /// </summary>
+        public MassData MassData;
+
+        /// <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.
+        /// Changing the density causes a recalculation of shape properties.
+        /// </summary>
+        /// <value>The density.</value>
+        public float Density
+        {
+            get { return _density; }
+            set
+            {
+                Debug.Assert(value >= 0);
+
+                _density = value;
+                ComputeProperties();
+            }
+        }
+
+        /// <summary>
+        /// Radius of the Shape
+        /// Changing the radius causes a recalculation of shape properties.
+        /// </summary>
+        public float Radius
+        {
+            get { return _radius; }
+            set
+            {
+                Debug.Assert(value >= 0);
+
+                _radius = value;
+                _2radius = _radius * _radius;
+
+                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.
+        /// Note: 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>
+        protected abstract void ComputeProperties();
+
+        /// <summary>
+        /// Compare this shape to another shape based on type and properties.
+        /// </summary>
+        /// <param name="shape">The other shape</param>
+        /// <returns>True if the two shapes are the same.</returns>
+        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);
+
+            if (shape is ChainShape && this is ChainShape)
+                return ((ChainShape)this).CompareTo((ChainShape)shape);
+
+            return false;
+        }
+
+        /// <summary>
+        /// Used for the buoyancy controller
+        /// </summary>
+        public abstract float ComputeSubmergedArea(ref Vector2 normal, float offset, ref Transform xf, out Vector2 sc);
+    }
+}

Farseer Physics Engine 3.5/Collision/TimeOfImpact.cs

@@ -0,0 +1,498 @@
+/*
+* Farseer Physics Engine:
+* Copyright (c) 2012 Ian Qvist
+* 
+* Original source Box2D:
+* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org 
+* 
+* 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
+    {
+        [ThreadStatic]
+        private static Vector2 _axis;
+        [ThreadStatic]
+        private static Vector2 _localPoint;
+        [ThreadStatic]
+        private static DistanceProxy _proxyA;
+        [ThreadStatic]
+        private static DistanceProxy _proxyB;
+        [ThreadStatic]
+        private static Sweep _sweepA, _sweepB;
+        [ThreadStatic]
+        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.Mul(ref xfA, localPointA);
+                Vector2 pointB = MathUtils.Mul(ref xfB, localPointB);
+                _axis = pointB - pointA;
+                _axis.Normalize();
+            }
+            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.Mul(ref xfB.q, _axis);
+
+                _localPoint = 0.5f * (localPointB1 + localPointB2);
+                Vector2 pointB = MathUtils.Mul(ref xfB, _localPoint);
+
+                Vector2 localPointA = proxyA.Vertices[cache.IndexA[0]];
+                Vector2 pointA = MathUtils.Mul(ref xfA, localPointA);
+
+                float s = Vector2.Dot(pointA - pointB, normal);
+                if (s < 0.0f)
+                {
+                    _axis = -_axis;
+                }
+            }
+            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.Mul(ref xfA.q, _axis);
+
+                _localPoint = 0.5f * (localPointA1 + localPointA2);
+                Vector2 pointA = MathUtils.Mul(ref xfA, _localPoint);
+
+                Vector2 localPointB = _proxyB.Vertices[cache.IndexB[0]];
+                Vector2 pointB = MathUtils.Mul(ref xfB, localPointB);
+
+                float s = Vector2.Dot(pointB - pointA, normal);
+                if (s < 0.0f)
+                {
+                    _axis = -_axis;
+                }
+            }
+
+            //FPE note: the returned value that used to be here has been removed, as it was not used.
+        }
+
+        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.MulT(ref xfA.q, _axis);
+                        Vector2 axisB = MathUtils.MulT(ref xfB.q, -_axis);
+
+                        indexA = _proxyA.GetSupport(axisA);
+                        indexB = _proxyB.GetSupport(axisB);
+
+                        Vector2 localPointA = _proxyA.Vertices[indexA];
+                        Vector2 localPointB = _proxyB.Vertices[indexB];
+
+                        Vector2 pointA = MathUtils.Mul(ref xfA, localPointA);
+                        Vector2 pointB = MathUtils.Mul(ref xfB, localPointB);
+
+                        float separation = Vector2.Dot(pointB - pointA, _axis);
+                        return separation;
+                    }
+
+                case SeparationFunctionType.FaceA:
+                    {
+                        Vector2 normal = MathUtils.Mul(ref xfA.q, _axis);
+                        Vector2 pointA = MathUtils.Mul(ref xfA, _localPoint);
+
+                        Vector2 axisB = MathUtils.MulT(ref xfB.q, -normal);
+
+                        indexA = -1;
+                        indexB = _proxyB.GetSupport(axisB);
+
+                        Vector2 localPointB = _proxyB.Vertices[indexB];
+                        Vector2 pointB = MathUtils.Mul(ref xfB, localPointB);
+
+                        float separation = Vector2.Dot(pointB - pointA, normal);
+                        return separation;
+                    }
+
+                case SeparationFunctionType.FaceB:
+                    {
+                        Vector2 normal = MathUtils.Mul(ref xfB.q, _axis);
+                        Vector2 pointB = MathUtils.Mul(ref xfB, _localPoint);
+
+                        Vector2 axisA = MathUtils.MulT(ref xfA.q, -normal);
+
+                        indexB = -1;
+                        indexA = _proxyA.GetSupport(axisA);
+
+                        Vector2 localPointA = _proxyA.Vertices[indexA];
+                        Vector2 pointA = MathUtils.Mul(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 localPointA = _proxyA.Vertices[indexA];
+                        Vector2 localPointB = _proxyB.Vertices[indexB];
+
+                        Vector2 pointA = MathUtils.Mul(ref xfA, localPointA);
+                        Vector2 pointB = MathUtils.Mul(ref xfB, localPointB);
+                        float separation = Vector2.Dot(pointB - pointA, _axis);
+
+                        return separation;
+                    }
+                case SeparationFunctionType.FaceA:
+                    {
+                        Vector2 normal = MathUtils.Mul(ref xfA.q, _axis);
+                        Vector2 pointA = MathUtils.Mul(ref xfA, _localPoint);
+
+                        Vector2 localPointB = _proxyB.Vertices[indexB];
+                        Vector2 pointB = MathUtils.Mul(ref xfB, localPointB);
+
+                        float separation = Vector2.Dot(pointB - pointA, normal);
+                        return separation;
+                    }
+                case SeparationFunctionType.FaceB:
+                    {
+                        Vector2 normal = MathUtils.Mul(ref xfB.q, _axis);
+                        Vector2 pointB = MathUtils.Mul(ref xfB, _localPoint);
+
+                        Vector2 localPointA = _proxyA.Vertices[indexA];
+                        Vector2 pointA = MathUtils.Mul(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.
+
+        [ThreadStatic]
+        public static int TOICalls, TOIIters, TOIMaxIters;
+        [ThreadStatic]
+        public static int TOIRootIters, TOIMaxRootIters;
+        [ThreadStatic]
+        private static DistanceInput _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)
+        {
+            if (Settings.EnableDiagnostics) //FPE: We only gather diagnostics when enabled
+                ++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.
+            _distanceInput = _distanceInput ?? new DistanceInput();
+            _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;
+                SimplexCache cache;
+                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);
+                        }
+
+                        ++rootIterCount;
+
+                        if (Settings.EnableDiagnostics) //FPE: We only gather diagnostics when enabled
+                            ++TOIRootIters;
+
+                        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;
+                        }
+
+                        if (rootIterCount == 50)
+                        {
+                            break;
+                        }
+                    }
+
+                    if (Settings.EnableDiagnostics) //FPE: We only gather diagnostics when enabled
+                        TOIMaxRootIters = Math.Max(TOIMaxRootIters, rootIterCount);
+
+                    ++pushBackIter;
+
+                    if (pushBackIter == Settings.MaxPolygonVertices)
+                    {
+                        break;
+                    }
+                }
+
+                ++iter;
+
+                if (Settings.EnableDiagnostics) //FPE: We only gather diagnostics when enabled
+                    ++TOIIters;
+
+                if (done)
+                {
+                    break;
+                }
+
+                if (iter == k_maxIterations)
+                {
+                    // Root finder got stuck. Semi-victory.
+                    output.State = TOIOutputState.Failed;
+                    output.T = t1;
+                    break;
+                }
+            }
+
+            if (Settings.EnableDiagnostics) //FPE: We only gather diagnostics when enabled
+                TOIMaxIters = Math.Max(TOIMaxIters, iter);
+        }
+    }
+}

Farseer Physics Engine 3.5/Common/ConvexHull/ChainHull.cs

@@ -0,0 +1,143 @@
+using System.Collections.Generic;
+using Microsoft.Xna.Framework;
+
+namespace FarseerPhysics.Common.ConvexHull
+{
+    /// <summary>
+    /// Andrew's Monotone Chain Convex Hull algorithm.
+    /// Used to get the convex hull of a point cloud.
+    /// 
+    /// Source: http://www.softsurfer.com/Archive/algorithm_0109/algorithm_0109.htm
+    /// </summary>
+    public static class ChainHull
+    {
+        //Copyright 2001, softSurfer (www.softsurfer.com)
+
+        private static PointComparer _pointComparer = new PointComparer();
+
+        /// <summary>
+        /// Returns the convex hull from the given vertices..
+        /// </summary>
+        public static Vertices GetConvexHull(Vertices vertices)
+        {
+            if (vertices.Count <= 3)
+                return vertices;
+
+            Vertices pointSet = new Vertices(vertices);
+
+            //Sort by X-axis
+            pointSet.Sort(_pointComparer);
+
+            Vector2[] h = new Vector2[pointSet.Count];
+            Vertices res;
+
+            int 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
+            const int minmin = 0;
+            float xmin = pointSet[0].X;
+            for (i = 1; i < pointSet.Count; i++)
+            {
+                if (pointSet[i].X != xmin)
+                    break;
+            }
+
+            // degenerate case: all x-coords == xmin
+            int minmax = i - 1;
+            if (minmax == pointSet.Count - 1)
+            {
+                h[++top] = pointSet[minmin];
+
+                if (pointSet[minmax].Y != pointSet[minmin].Y) // a nontrivial segment
+                    h[++top] = pointSet[minmax];
+
+                h[++top] = pointSet[minmin]; // add polygon endpoint
+
+                res = new Vertices(top + 1);
+                for (int j = 0; j < top + 1; j++)
+                {
+                    res.Add(h[j]);
+                }
+
+                return res;
+            }
+
+            top = -1;
+
+            // Get the indices of points with max x-coord and min|max y-coord
+            int maxmax = pointSet.Count - 1;
+            float xmax = pointSet[pointSet.Count - 1].X;
+            for (i = pointSet.Count - 2; i >= 0; i--)
+            {
+                if (pointSet[i].X != xmax)
+                    break;
+            }
+            int maxmin = i + 1;
+
+            // Compute the lower hull on the stack H
+            h[++top] = pointSet[minmin]; // push minmin point onto stack
+            i = minmax;
+            while (++i <= maxmin)
+            {
+                // the lower line joins P[minmin] with P[maxmin]
+                if (MathUtils.Area(pointSet[minmin], pointSet[maxmin], pointSet[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], pointSet[i]) > 0)
+                        break; // P[i] is a new hull vertex
+
+                    top--; // pop top point off stack
+                }
+                h[++top] = pointSet[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] = pointSet[maxmax]; // push maxmax point onto stack
+            int bot = top;
+            i = maxmin;
+            while (--i >= minmax)
+            {
+                // the upper line joins P[maxmax] with P[minmax]
+                if (MathUtils.Area(pointSet[maxmax], pointSet[minmax], pointSet[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], pointSet[i]) > 0)
+                        break; // P[i] is a new hull vertex
+
+                    top--; // pop top point off stack
+                }
+
+                h[++top] = pointSet[i]; // push P[i] onto stack
+            }
+
+            if (minmax != minmin)
+                h[++top] = pointSet[minmin]; // push joining endpoint onto stack
+
+            res = new Vertices(top + 1);
+
+            for (int j = 0; j < top + 1; j++)
+            {
+                res.Add(h[j]);
+            }
+
+            return res;
+        }
+
+        private 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);
+            }
+        }
+    }
+}

Farseer Physics Engine 3.5/Common/ConvexHull/GiftWrap.cs

@@ -0,0 +1,90 @@
+using Microsoft.Xna.Framework;
+
+namespace FarseerPhysics.Common.ConvexHull
+{
+    /// <summary>
+    /// Giftwrap convex hull algorithm.
+    /// O(nh) time complexity, where n is the number of points and h is the number of points on the convex hull.
+    /// 
+    /// See http://en.wikipedia.org/wiki/Gift_wrapping_algorithm for more details.
+    /// </summary>
+    public static class GiftWrap
+    {
+        //Extracted from Box2D
+
+        /// <summary>
+        /// Returns the convex hull from the given vertices.
+        /// </summary>
+        /// <param name="vertices">The vertices.</param>
+        public static Vertices GetConvexHull(Vertices vertices)
+        {
+            if (vertices.Count <= 3)
+                return vertices;
+
+            // Find the right most point on the hull
+            int i0 = 0;
+            float x0 = vertices[0].X;
+            for (int i = 1; i < vertices.Count; ++i)
+            {
+                float x = vertices[i].X;
+                if (x > x0 || (x == x0 && vertices[i].Y < vertices[i0].Y))
+                {
+                    i0 = i;
+                    x0 = x;
+                }
+            }
+
+            int[] hull = new int[vertices.Count];
+            int m = 0;
+            int ih = i0;
+
+            for (; ; )
+            {
+                hull[m] = ih;
+
+                int ie = 0;
+                for (int j = 1; j < vertices.Count; ++j)
+                {
+                    if (ie == ih)
+                    {
+                        ie = j;
+                        continue;
+                    }
+
+                    Vector2 r = vertices[ie] - vertices[hull[m]];
+                    Vector2 v = vertices[j] - vertices[hull[m]];
+
+                    float c = MathUtils.Cross(ref r, ref v);
+                    if (c < 0.0f)
+                    {
+                        ie = j;
+                    }
+
+                    // Collinearity check
+                    if (c == 0.0f && v.LengthSquared() > r.LengthSquared())
+                    {
+                        ie = j;
+                    }
+                }
+
+                ++m;
+                ih = ie;
+
+                if (ie == i0)
+                {
+                    break;
+                }
+            }
+
+            Vertices result = new Vertices(m);
+
+            // Copy vertices.
+            for (int i = 0; i < m; ++i)
+            {
+                result.Add(vertices[hull[i]]);
+            }
+
+            return result;
+        }
+    }
+}

Farseer Physics Engine 3.5/Common/ConvexHull/Melkman.cs

@@ -0,0 +1,132 @@
+using Microsoft.Xna.Framework;
+
+namespace FarseerPhysics.Common.ConvexHull
+{
+    /// <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>
+    public static class Melkman
+    {
+        //Melkman based convex hull algorithm contributed by Cowdozer
+
+        /// <summary>
+        /// Returns a convex hull from the given vertices.
+        /// </summary>
+        /// <returns>A convex hull in counter clockwise winding order.</returns>
+        public static Vertices GetConvexHull(Vertices vertices)
+        {
+            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
+
+            //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];
+                }
+            }
+
+            int qfm1 = qf == 0 ? deque.Length - 1 : qf - 1;
+            int qbm1 = qb == deque.Length - 1 ? 0 : 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
+            if (qb < qf)
+            {
+                Vertices convexHull = new Vertices(qf);
+
+                for (int i = qb; i < qf; i++)
+                    convexHull.Add(deque[i]);
+
+                return convexHull;
+            }
+            else
+            {
+                Vertices convexHull = new Vertices(qf + deque.Length);
+
+                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;
+            }
+        }
+    }
+}

Farseer Physics Engine 3.5/Common/Curve.cs

@@ -0,0 +1,273 @@
+#if !XNA && !WINDOWS_PHONE && !XBOX && !ANDROID
+
+#region License
+
+/*
+MIT License
+Copyright © 2006 The Mono.Xna Team
+
+All rights reserved.
+
+Authors:
+Olivier Dufour (Duff)
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all
+copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.
+*/
+
+#endregion License
+
+using System;
+
+namespace Microsoft.Xna.Framework
+{
+    public enum CurveLoopType
+    {
+        Constant,
+        Cycle,
+        CycleOffset,
+        Oscillate,
+        Linear
+    }
+
+    public enum CurveContinuity
+    {
+        Smooth,
+        Step
+    }
+
+    public enum CurveTangent
+    {
+        Flat,
+        Linear,
+        Smooth
+    }
+
+    public class Curve
+    {
+#region Private Fields
+
+        private CurveKeyCollection keys;
+        private CurveLoopType postLoop;
+        private CurveLoopType preLoop;
+
+#endregion Private Fields
+
+#region Public Properties
+
+        public bool IsConstant
+        {
+            get { return keys.Count <= 1; }
+        }
+
+        public CurveKeyCollection Keys
+        {
+            get { return keys; }
+        }
+
+        public CurveLoopType PostLoop
+        {
+            get { return postLoop; }
+            set { postLoop = value; }
+        }
+
+        public CurveLoopType PreLoop
+        {
+            get { return preLoop; }
+            set { preLoop = value; }
+        }
+
+#endregion Public Properties
+
+#region Public Constructors
+
+        public Curve()
+        {
+            keys = new CurveKeyCollection();
+        }
+
+#endregion Public Constructors
+
+#region Public Methods
+
+        public void ComputeTangent(int keyIndex, CurveTangent tangentInType, CurveTangent tangentOutType)
+        {
+            throw new NotImplementedException();
+        }
+
+        public void ComputeTangent(int keyIndex, CurveTangent tangentType)
+        {
+            ComputeTangent(keyIndex, tangentType, tangentType);
+        }
+
+        public void ComputeTangents(CurveTangent tangentInType, CurveTangent tangentOutType)
+        {
+            throw new NotImplementedException();
+        }
+
+        public void ComputeTangents(CurveTangent tangentType)
+        {
+            ComputeTangents(tangentType, tangentType);
+        }
+
+        public Curve Clone()
+        {
+            Curve curve = new Curve();
+
+            curve.keys = keys.Clone();
+            curve.preLoop = preLoop;
+            curve.postLoop = postLoop;
+
+            return curve;
+        }
+
+        public float Evaluate(float position)
+        {
+            CurveKey first = keys[0];
+            CurveKey last = keys[keys.Count - 1];
+
+            if (position < first.Position)
+            {
+                switch (PreLoop)
+                {
+                    case CurveLoopType.Constant:
+                        //constant
+                        return first.Value;
+
+                    case CurveLoopType.Linear:
+                        // linear y = a*x +b with a tangeant of last point
+                        return first.Value - first.TangentIn*(first.Position - position);
+
+                    case CurveLoopType.Cycle:
+                        //start -> end / start -> end
+                        int cycle = GetNumberOfCycle(position);
+                        float virtualPos = position - (cycle*(last.Position - first.Position));
+                        return GetCurvePosition(virtualPos);
+
+                    case CurveLoopType.CycleOffset:
+                        //make the curve continue (with no step) so must up the curve each cycle of delta(value)
+                        cycle = GetNumberOfCycle(position);
+                        virtualPos = position - (cycle*(last.Position - first.Position));
+                        return (GetCurvePosition(virtualPos) + cycle*(last.Value - first.Value));
+
+                    case CurveLoopType.Oscillate:
+                        //go back on curve from end and target start 
+                        // start-> end / end -> start
+                        cycle = GetNumberOfCycle(position);
+                        if (0 == cycle%2f) //if pair
+                            virtualPos = position - (cycle*(last.Position - first.Position));
+                        else
+                            virtualPos = last.Position - position + first.Position +
+                                         (cycle*(last.Position - first.Position));
+                        return GetCurvePosition(virtualPos);
+                }
+            }
+            else if (position > last.Position)
+            {
+                int cycle;
+                switch (PostLoop)
+                {
+                    case CurveLoopType.Constant:
+                        //constant
+                        return last.Value;
+
+                    case CurveLoopType.Linear:
+                        // linear y = a*x +b with a tangeant of last point
+                        return last.Value + first.TangentOut*(position - last.Position);
+
+                    case CurveLoopType.Cycle:
+                        //start -> end / start -> end
+                        cycle = GetNumberOfCycle(position);
+                        float virtualPos = position - (cycle*(last.Position - first.Position));
+                        return GetCurvePosition(virtualPos);
+
+                    case CurveLoopType.CycleOffset:
+                        //make the curve continue (with no step) so must up the curve each cycle of delta(value)
+                        cycle = GetNumberOfCycle(position);
+                        virtualPos = position - (cycle*(last.Position - first.Position));
+                        return (GetCurvePosition(virtualPos) + cycle*(last.Value - first.Value));
+
+                    case CurveLoopType.Oscillate:
+                        //go back on curve from end and target start 
+                        // start-> end / end -> start
+                        cycle = GetNumberOfCycle(position);
+                        virtualPos = position - (cycle*(last.Position - first.Position));
+                        if (0 == cycle%2f) //if pair
+                            virtualPos = position - (cycle*(last.Position - first.Position));
+                        else
+                            virtualPos = last.Position - position + first.Position +
+                                         (cycle*(last.Position - first.Position));
+                        return GetCurvePosition(virtualPos);
+                }
+            }
+
+            //in curve
+            return GetCurvePosition(position);
+        }
+
+#endregion Public Methods
+
+#region Private Methods
+
+        private int GetNumberOfCycle(float position)
+        {
+            float cycle = (position - keys[0].Position)/(keys[keys.Count - 1].Position - keys[0].Position);
+            if (cycle < 0f)
+                cycle--;
+            return (int) cycle;
+        }
+
+        private float GetCurvePosition(float position)
+        {
+            //only for position in curve
+            CurveKey prev = keys[0];
+            CurveKey next;
+            for (int i = 1; i < keys.Count; i++)
+            {
+                next = Keys[i];
+                if (next.Position >= position)
+                {
+                    if (prev.Continuity == CurveContinuity.Step)
+                    {
+                        if (position >= 1f)
+                        {
+                            return next.Value;
+                        }
+                        return prev.Value;
+                    }
+                    float t = (position - prev.Position)/(next.Position - prev.Position); //to have t in [0,1]
+                    float ts = t*t;
+                    float tss = ts*t;
+                    //After a lot of search on internet I have found all about spline function
+                    // and bezier (phi'sss ancien) but finaly use hermite curve 
+                    //http://en.wikipedia.org/wiki/Cubic_Hermite_spline
+                    //P(t) = (2*t^3 - 3t^2 + 1)*P0 + (t^3 - 2t^2 + t)m0 + (-2t^3 + 3t^2)P1 + (t^3-t^2)m1
+                    //with P0.value = prev.value , m0 = prev.tangentOut, P1= next.value, m1 = next.TangentIn
+                    return (2*tss - 3*ts + 1f)*prev.Value + (tss - 2*ts + t)*prev.TangentOut + (3*ts - 2*tss)*next.Value +
+                           (tss - ts)*next.TangentIn;
+                }
+                prev = next;
+            }
+            return 0f;
+        }
+
+#endregion
+    }
+}
+
+#endif

Farseer Physics Engine 3.5/Common/CurveKey.cs

@@ -0,0 +1,167 @@
+#if !XNA && !WINDOWS_PHONE && !XBOX && !ANDROID
+
+#region License
+
+/*
+MIT License
+Copyright © 2006 The Mono.Xna Team
+
+All rights reserved.
+
+Authors:
+Olivier Dufour (Duff)
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all
+copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.
+*/
+
+#endregion License
+
+using System;
+
+namespace Microsoft.Xna.Framework
+{
+    public class CurveKey : IEquatable<CurveKey>, IComparable<CurveKey>
+    {
+#region Private Fields
+
+        private CurveContinuity continuity;
+        private float position;
+        private float tangentIn;
+        private float tangentOut;
+        private float value;
+
+#endregion Private Fields
+
+#region Properties
+
+        public CurveContinuity Continuity
+        {
+            get { return continuity; }
+            set { continuity = value; }
+        }
+
+        public float Position
+        {
+            get { return position; }
+        }
+
+        public float TangentIn
+        {
+            get { return tangentIn; }
+            set { tangentIn = value; }
+        }
+
+        public float TangentOut
+        {
+            get { return tangentOut; }
+            set { tangentOut = value; }
+        }
+
+        public float Value
+        {
+            get { return value; }
+            set { this.value = value; }
+        }
+
+#endregion
+
+#region Constructors
+
+        public CurveKey(float position, float value)
+            : this(position, value, 0, 0, CurveContinuity.Smooth)
+        {
+        }
+
+        public CurveKey(float position, float value, float tangentIn, float tangentOut)
+            : this(position, value, tangentIn, tangentOut, CurveContinuity.Smooth)
+        {
+        }
+
+        public CurveKey(float position, float value, float tangentIn, float tangentOut, CurveContinuity continuity)
+        {
+            this.position = position;
+            this.value = value;
+            this.tangentIn = tangentIn;
+            this.tangentOut = tangentOut;
+            this.continuity = continuity;
+        }
+
+#endregion Constructors
+
+#region Public Methods
+
+#region IComparable<CurveKey> Members
+
+        public int CompareTo(CurveKey other)
+        {
+            return position.CompareTo(other.position);
+        }
+
+#endregion
+
+#region IEquatable<CurveKey> Members
+
+        public bool Equals(CurveKey other)
+        {
+            return (this == other);
+        }
+
+#endregion
+
+        public static bool operator !=(CurveKey a, CurveKey b)
+        {
+            return !(a == b);
+        }
+
+        public static bool operator ==(CurveKey a, CurveKey b)
+        {
+            if (Equals(a, null))
+                return Equals(b, null);
+
+            if (Equals(b, null))
+                return Equals(a, null);
+
+            return (a.position == b.position)
+                   && (a.value == b.value)
+                   && (a.tangentIn == b.tangentIn)
+                   && (a.tangentOut == b.tangentOut)
+                   && (a.continuity == b.continuity);
+        }
+
+        public CurveKey Clone()
+        {
+            return new CurveKey(position, value, tangentIn, tangentOut, continuity);
+        }
+
+        public override bool Equals(object obj)
+        {
+            return (obj is CurveKey) ? ((CurveKey) obj) == this : false;
+        }
+
+        public override int GetHashCode()
+        {
+            return position.GetHashCode() ^ value.GetHashCode() ^ tangentIn.GetHashCode() ^
+                   tangentOut.GetHashCode() ^ continuity.GetHashCode();
+        }
+
+#endregion
+    }
+}
+
+#endif

Farseer Physics Engine 3.5/Common/CurveKeyCollection.cs

@@ -0,0 +1,176 @@
+#if !XNA && !WINDOWS_PHONE && !XBOX && !ANDROID
+
+#region License
+
+/*
+MIT License
+Copyright © 2006 The Mono.Xna Team
+
+All rights reserved.
+
+Authors:
+Olivier Dufour (Duff)
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all
+copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.
+*/
+
+#endregion License
+
+using System;
+using System.Collections;
+using System.Collections.Generic;
+
+namespace Microsoft.Xna.Framework
+{
+    public class CurveKeyCollection : ICollection<CurveKey>, IEnumerable<CurveKey>, IEnumerable
+    {
+#region Private Fields
+
+        private List<CurveKey> innerlist;
+        private bool isReadOnly = false;
+
+#endregion Private Fields
+
+#region Properties
+
+        public CurveKey this[int index]
+        {
+            get { return innerlist[index]; }
+            set
+            {
+                if (value == null)
+                    throw new ArgumentNullException();
+
+                if (index >= innerlist.Count)
+                    throw new IndexOutOfRangeException();
+
+                if (innerlist[index].Position == value.Position)
+                    innerlist[index] = value;
+                else
+                {
+                    innerlist.RemoveAt(index);
+                    innerlist.Add(value);
+                }
+            }
+        }
+
+        public int Count
+        {
+            get { return innerlist.Count; }
+        }
+
+        public bool IsReadOnly
+        {
+            get { return isReadOnly; }
+        }
+
+#endregion Properties
+
+#region Constructors
+
+        public CurveKeyCollection()
+        {
+            innerlist = new List<CurveKey>();
+        }
+
+#endregion Constructors
+
+#region Public Methods
+
+        public void Add(CurveKey item)
+        {
+            if (item == null)
+                throw new ArgumentNullException("Value cannot be null.", (Exception) null);
+
+            if (innerlist.Count == 0)
+            {
+                innerlist.Add(item);
+                return;
+            }
+
+            for (int i = 0; i < innerlist.Count; i++)
+            {
+                if (item.Position < innerlist[i].Position)
+                {
+                    innerlist.Insert(i, item);
+                    return;
+                }
+            }
+
+            innerlist.Add(item);
+        }
+
+        public void Clear()
+        {
+            innerlist.Clear();
+        }
+
+        public bool Contains(CurveKey item)
+        {
+            return innerlist.Contains(item);
+        }
+
+        public void CopyTo(CurveKey[] array, int arrayIndex)
+        {
+            innerlist.CopyTo(array, arrayIndex);
+        }
+
+        public IEnumerator<CurveKey> GetEnumerator()
+        {
+            return innerlist.GetEnumerator();
+        }
+
+        public bool Remove(CurveKey item)
+        {
+            return innerlist.Remove(item);
+        }
+
+        IEnumerator IEnumerable.GetEnumerator()
+        {
+            return innerlist.GetEnumerator();
+        }
+
+        public CurveKeyCollection Clone()
+        {
+            CurveKeyCollection ckc = new CurveKeyCollection();
+            foreach (CurveKey key in innerlist)
+                ckc.Add(key);
+            return ckc;
+        }
+
+        public int IndexOf(CurveKey item)
+        {
+            return innerlist.IndexOf(item);
+        }
+
+        public void RemoveAt(int index)
+        {
+            if (index != Count && index > -1)
+                innerlist.RemoveAt(index);
+            else
+                throw new ArgumentOutOfRangeException(
+                    "Index was out of range. Must be non-negative and less than the size of the collection.\r\nParameter name: index",
+                    (Exception) null);
+        }
+
+#endregion Public Methods
+    }
+}
+
+#endif

Farseer Physics Engine 3.5/Common/Decomposition/BayazitDecomposer.cs

@@ -0,0 +1,243 @@
+using System.Collections.Generic;
+using System.Diagnostics;
+using Microsoft.Xna.Framework;
+
+namespace FarseerPhysics.Common.Decomposition
+{
+    //From phed rev 36: http://code.google.com/p/phed/source/browse/trunk/Polygon.cpp
+
+    /// <summary>
+    /// Convex decomposition algorithm created by Mark Bayazit (http://mnbayazit.com/)
+    /// 
+    /// Properties:
+    /// - Tries to decompose using polygons instead of triangles.
+    /// - Tends to produce optimal results with low processing time.
+    /// - Running time is O(nr), n = number of vertices, r = reflex vertices.
+    /// - Does not support holes.
+    /// 
+    /// For more information about this algorithm, see http://mnbayazit.com/406/bayazit
+    /// </summary>
+    internal static class BayazitDecomposer
+    {
+        /// <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.
+        /// </summary>
+        public static List<Vertices> ConvexPartition(Vertices vertices)
+        {
+            Debug.Assert(vertices.Count > 3);
+            Debug.Assert(vertices.IsCounterClockWise());
+
+            return TriangulatePolygon(vertices);
+        }
+
+        private static List<Vertices> TriangulatePolygon(Vertices vertices)
+        {
+            List<Vertices> list = new List<Vertices>();
+            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))
+                {
+                    float upperDist;
+                    float lowerDist = upperDist = float.MaxValue;
+                    for (int j = 0; j < vertices.Count; ++j)
+                    {
+                        // if line intersects with an edge
+                        float d;
+                        Vector2 p;
+                        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 p = ((lowerInt + upperInt) / 2);
+
+                        lowerPoly = Copy(i, upperIndex, vertices);
+                        lowerPoly.Add(p);
+                        upperPoly = Copy(lowerIndex, i, vertices);
+                        upperPoly.Add(p);
+                    }
+                    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(TriangulatePolygon(lowerPoly));
+                    list.AddRange(TriangulatePolygon(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(TriangulatePolygon(lowerPoly));
+                list.AddRange(TriangulatePolygon(upperPoly));
+            }
+            else
+                list.Add(vertices);
+
+            return list;
+        }
+
+        private static Vector2 At(int i, Vertices vertices)
+        {
+            int s = vertices.Count;
+            return vertices[i < 0 ? s - 1 - ((-i - 1) % s) : i % s];
+        }
+
+        private static Vertices Copy(int i, int j, Vertices vertices)
+        {
+            while (j < i)
+                j += vertices.Count;
+
+            Vertices p = new Vertices(j);
+
+            for (; i <= j; ++i)
+            {
+                p.Add(At(i, vertices));
+            }
+            return p;
+        }
+
+        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;
+        }
+
+        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;
+        }
+    }
+}

Farseer Physics Engine 3.5/Common/Decomposition/CDT/Delaunay/DelaunayTriangle.cs

@@ -0,0 +1,420 @@
+/* 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 FarseerPhysics.Common.Decomposition.CDT.Delaunay.Sweep;
+using FarseerPhysics.Common.Decomposition.CDT.Util;
+
+namespace FarseerPhysics.Common.Decomposition.CDT.Delaunay
+{
+    internal 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 Util.FixedArray3<DelaunayTriangle> Neighbors;
+
+        /** Has this triangle been marked as an interior triangle? */
+
+        public Util.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;
+        }
+    }
+}

Farseer Physics Engine 3.5/Common/Decomposition/CDT/Delaunay/Sweep/AdvancingFront.cs

@@ -0,0 +1,180 @@
+/* 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 FarseerPhysics.Common.Decomposition.CDT.Delaunay.Sweep
+{
+    /**
+     * @author Thomas Åhlen (thahlen@gmail.com)
+     */
+
+    internal 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;
+        }
+    }
+}

Farseer Physics Engine 3.5/Common/Decomposition/CDT/Delaunay/Sweep/AdvancingFrontNode.cs

@@ -0,0 +1,64 @@
+/* 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 FarseerPhysics.Common.Decomposition.CDT.Delaunay.Sweep
+{
+    internal 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; }
+        }
+    }
+}

Farseer Physics Engine 3.5/Common/Decomposition/CDT/Delaunay/Sweep/DTSweepConstraint.cs

@@ -0,0 +1,66 @@
+/* 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.Delaunay.Sweep
+{
+    internal 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);
+        }
+    }
+}

Farseer Physics Engine 3.5/Common/Decomposition/CDT/Delaunay/Sweep/DTSweepContext.cs

@@ -0,0 +1,236 @@
+/* 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.Delaunay.Sweep
+{
+    /**
+     * 
+     * @author Thomas Åhlén, thahlen@gmail.com
+     *
+     */
+
+    internal 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
+    }
+}

Farseer Physics Engine 3.5/Common/Decomposition/CDT/Delaunay/Sweep/DTSweepPointComparator.cs

@@ -0,0 +1,69 @@
+/* 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 FarseerPhysics.Common.Decomposition.CDT.Delaunay.Sweep
+{
+    internal 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
+    }
+}

Farseer Physics Engine 3.5/Common/Decomposition/CDT/Delaunay/Sweep/PointOnEdgeException.cs

@@ -0,0 +1,43 @@
+/* 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 FarseerPhysics.Common.Decomposition.CDT.Delaunay.Sweep
+{
+    internal class PointOnEdgeException : NotImplementedException
+    {
+        public PointOnEdgeException(string message)
+            : base(message)
+        {
+        }
+    }
+}

Farseer Physics Engine 3.5/Common/Decomposition/CDT/ITriangulatable.cs

@@ -0,0 +1,48 @@
+/* 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 FarseerPhysics.Common.Decomposition.CDT.Delaunay;
+
+namespace FarseerPhysics.Common.Decomposition.CDT
+{
+    internal 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();
+    }
+}

Farseer Physics Engine 3.5/Common/Decomposition/CDT/Orientation.cs

@@ -0,0 +1,40 @@
+/* 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
+{
+    internal enum Orientation
+    {
+        CW,
+        CCW,
+        Collinear
+    }
+}

Farseer Physics Engine 3.5/Common/Decomposition/CDT/Polygon/Polygon.cs

@@ -0,0 +1,272 @@
+/* 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 FarseerPhysics.Common.Decomposition.CDT.Delaunay;
+
+namespace FarseerPhysics.Common.Decomposition.CDT.Polygon
+{
+    internal 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);
+        }
+    }
+}

Farseer Physics Engine 3.5/Common/Decomposition/CDT/Polygon/PolygonPoint.cs

@@ -0,0 +1,48 @@
+/* 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 FarseerPhysics.Common.Decomposition.CDT.Polygon
+{
+    internal class PolygonPoint : TriangulationPoint
+    {
+        public PolygonPoint(double x, double y) : base(x, y)
+        {
+        }
+
+        public PolygonPoint Next { get; set; }
+        public PolygonPoint Previous { get; set; }
+    }
+}

Farseer Physics Engine 3.5/Common/Decomposition/CDT/Polygon/PolygonSet.cs

@@ -0,0 +1,65 @@
+/* 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 FarseerPhysics.Common.Decomposition.CDT.Polygon
+{
+    internal 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);
+        }
+    }
+}

Farseer Physics Engine 3.5/Common/Decomposition/CDT/Sets/ConstrainedPointSet.cs

@@ -0,0 +1,114 @@
+/* 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 FarseerPhysics.Common.Decomposition.CDT.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, thahlen@gmail.com
+     */
+
+    internal class ConstrainedPointSet : PointSet
+    {
+        private List<TriangulationPoint> _constrainedPointList;
+
+        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;
+        }
+    }
+}

Farseer Physics Engine 3.5/Common/Decomposition/CDT/Sets/PointSet.cs

@@ -0,0 +1,84 @@
+/* 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 FarseerPhysics.Common.Decomposition.CDT.Delaunay;
+
+namespace FarseerPhysics.Common.Decomposition.CDT.Sets
+{
+    internal 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
+    }
+}

Farseer Physics Engine 3.5/Common/Decomposition/CDT/TriangulationConstraint.cs

@@ -0,0 +1,46 @@
+/* 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, thahlen@gmail.com
+ */
+namespace FarseerPhysics.Common.Decomposition.CDT
+{
+    internal class TriangulationConstraint
+    {
+        public TriangulationPoint P;
+        public TriangulationPoint Q;
+    }
+}

Farseer Physics Engine 3.5/Common/Decomposition/CDT/TriangulationContext.cs

@@ -0,0 +1,84 @@
+/* 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 FarseerPhysics.Common.Decomposition.CDT.Delaunay;
+
+namespace FarseerPhysics.Common.Decomposition.CDT
+{
+    internal 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;
+        }
+    }
+}

Farseer Physics Engine 3.5/Common/Decomposition/CDT/TriangulationMode.cs

@@ -0,0 +1,40 @@
+/* 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
+{
+    internal enum TriangulationMode
+    {
+        Unconstrained,
+        Constrained,
+        Polygon
+    }
+}

Farseer Physics Engine 3.5/Common/Decomposition/CDT/TriangulationPoint.cs

@@ -0,0 +1,82 @@
+/* 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 FarseerPhysics.Common.Decomposition.CDT.Delaunay.Sweep;
+
+namespace FarseerPhysics.Common.Decomposition.CDT
+{
+    internal 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);
+        }
+    }
+}

Farseer Physics Engine 3.5/Common/Decomposition/CDT/TriangulationUtil.cs

@@ -0,0 +1,175 @@
+/* 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
+{
+    /**
+     * @author Thomas Åhlén, thahlen@gmail.com
+     */
+
+    internal 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;
+        }
+        */
+
+        public static bool InScanArea(TriangulationPoint pa, TriangulationPoint pb, TriangulationPoint pc, TriangulationPoint pd)
+        {
+            double oadb = (pa.X - pb.X) * (pd.Y - pb.Y) - (pd.X - pb.X) * (pa.Y - pb.Y);
+            if (oadb >= -EPSILON)
+            {
+                return false;
+            }
+
+            double oadc = (pa.X - pc.X) * (pd.Y - pc.Y) - (pd.X - pc.X) * (pa.Y - pc.Y);
+            if (oadc <= EPSILON)
+            {
+                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;
+        }
+    }
+}

Farseer Physics Engine 3.5/Common/Decomposition/CDT/Util/FixedArray3.cs

@@ -0,0 +1,118 @@
+/* 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 FarseerPhysics.Common.Decomposition.CDT.Util
+{
+    internal 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];
+        }
+    }
+}

Farseer Physics Engine 3.5/Common/Decomposition/CDT/Util/FixedBitArray3.cs

@@ -0,0 +1,118 @@
+/* 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 FarseerPhysics.Common.Decomposition.CDT.Util
+{
+    internal 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];
+        }
+    }
+}

Farseer Physics Engine 3.5/Common/Decomposition/CDT/Util/PointGenerator.cs

@@ -0,0 +1,38 @@
+using System;
+using System.Collections.Generic;
+
+namespace FarseerPhysics.Common.Decomposition.CDT.Util
+{
+    internal 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;
+        }
+    }
+}

Farseer Physics Engine 3.5/Common/Decomposition/CDT/Util/PolygonGenerator.cs

@@ -0,0 +1,98 @@
+/* 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 FarseerPhysics.Common.Decomposition.CDT.Polygon;
+
+namespace FarseerPhysics.Common.Decomposition.CDT.Util
+{
+    internal 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);
+        }
+    }
+}

Farseer Physics Engine 3.5/Common/Decomposition/CDTDecomposer.cs

@@ -0,0 +1,75 @@
+/*
+* Farseer Physics Engine:
+* Copyright (c) 2012 Ian Qvist
+*/
+
+using System.Collections.Generic;
+using System.Diagnostics;
+using FarseerPhysics.Common.Decomposition.CDT;
+using FarseerPhysics.Common.Decomposition.CDT.Delaunay;
+using FarseerPhysics.Common.Decomposition.CDT.Delaunay.Sweep;
+using FarseerPhysics.Common.Decomposition.CDT.Polygon;
+using Microsoft.Xna.Framework;
+
+namespace FarseerPhysics.Common.Decomposition
+{
+    /// <summary>
+    /// 2D constrained Delaunay triangulation algorithm.
+    /// Based on the paper "Sweep-line algorithm for constrained Delaunay triangulation" by V. Domiter and and B. Zalik
+    /// 
+    /// Properties:
+    /// - Creates triangles with a large interior angle.
+    /// - Supports holes
+    /// - Generate a lot of garbage due to incapsulation of the Poly2Tri library.
+    /// - Running time is O(n^2), n = number of vertices.
+    /// - Does not care about winding order.
+    /// 
+    /// Source: http://code.google.com/p/poly2tri/
+    /// </summary>
+    internal static class CDTDecomposer
+    {
+        /// <summary>
+        /// Decompose the polygon into several smaller non-concave polygon.
+        /// </summary>
+        public static List<Vertices> ConvexPartition(Vertices vertices)
+        {
+            Debug.Assert(vertices.Count > 3);
+
+            Polygon poly = new Polygon();
+
+            foreach (Vector2 vertex in vertices)
+                poly.Points.Add(new TriangulationPoint(vertex.X, vertex.Y));
+
+            if (vertices.Holes != null)
+            {
+                foreach (Vertices holeVertices in vertices.Holes)
+                {
+                    Polygon hole = new Polygon();
+
+                    foreach (Vector2 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;
+        }
+    }
+}

Farseer Physics Engine 3.5/Common/Decomposition/EarclipDecomposer.cs

@@ -0,0 +1,403 @@
+/*
+* 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 System.Diagnostics;
+using Microsoft.Xna.Framework;
+
+namespace FarseerPhysics.Common.Decomposition
+{
+    /// <summary>
+    /// Convex decomposition algorithm using ear clipping
+    /// 
+    /// Properties:
+    /// - Only works on simple polygons.
+    /// - Does not support holes.
+    /// - Running time is O(n^2), n = number of vertices.
+    /// 
+    /// Source: http://www.ewjordan.com/earClip/
+    /// </summary>
+    internal static class EarclipDecomposer
+    {
+        //box2D rev 32 - for details, see http://www.box2d.org/forum/viewtopic.php?f=4&t=83&start=50 
+
+        /// <summary>
+        /// Decompose the polygon into several smaller non-concave polygon.
+        /// Each resulting polygon will have no more than Settings.MaxPolygonVertices vertices.
+        /// </summary>
+        /// <param name="vertices">The vertices.</param>
+        /// <param name="tolerance">The tolerance.</param>
+        public static List<Vertices> ConvexPartition(Vertices vertices, float tolerance = 0.001f)
+        {
+            Debug.Assert(vertices.Count > 3);
+            Debug.Assert(!vertices.IsCounterClockWise());
+
+            return TriangulatePolygon(vertices, tolerance);
+        }
+
+        /// <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.
+        /// </summary>
+        /// <remarks>
+        /// Only works on simple polygons.
+        /// </remarks>
+        private static List<Vertices> TriangulatePolygon(Vertices vertices, float tolerance)
+        {
+            //FPE note: Check is needed as invalid triangles can be returned in recursive calls.
+            if (vertices.Count < 3)
+                return new List<Vertices>();
+
+            List<Vertices> results = new List<Vertices>();
+
+            //Recurse and split on pinch points
+            Vertices pA, pB;
+            Vertices pin = new Vertices(vertices);
+            if (ResolvePinchPoint(pin, out pA, out pB, tolerance))
+            {
+                List<Vertices> mergeA = TriangulatePolygon(pA, tolerance);
+                List<Vertices> mergeB = TriangulatePolygon(pB, tolerance);
+
+                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 Vertices(mergeA[i]));
+                }
+                for (int i = 0; i < mergeB.Count; ++i)
+                {
+                    results.Add(new Vertices(mergeB[i]));
+                }
+
+                return results;
+            }
+
+            Vertices[] buffer = new Vertices[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(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 Vertices(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>
+        /// <param name="tolerance"></param>
+        private static bool ResolvePinchPoint(Vertices pin, out Vertices poutA, out Vertices poutB, float tolerance)
+        {
+            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) < tolerance && Math.Abs(pin[i].Y - pin[j].Y) < tolerance && 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;
+        }
+
+        /// <summary>
+        /// Checks if vertex i is the tip of an ear in polygon defined by xv[] and  yv[].
+        /// </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>
+        /// <remarks>
+        /// Assumes clockwise orientation of polygon.
+        /// </remarks>
+        /// <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;
+        }
+
+        private class Triangle : Vertices
+        {
+            //Constructor automatically fixes orientation to ccw
+            public Triangle(float x1, float y1, float x2, float y2, float x3, float y3)
+            {
+                float cross = (x2 - x1) * (y3 - y1) - (x3 - x1) * (y2 - y1);
+                if (cross > 0)
+                {
+                    Add(new Vector2(x1, y1));
+                    Add(new Vector2(x2, y2));
+                    Add(new Vector2(x3, y3));
+                }
+                else
+                {
+                    Add(new Vector2(x1, y1));
+                    Add(new Vector2(x3, y3));
+                    Add(new Vector2(x2, y2));
+                }
+            }
+
+            public bool IsInside(float x, float y)
+            {
+                Vector2 a = this[0];
+                Vector2 b = this[1];
+                Vector2 c = this[2];
+
+                if (x < a.X && x < b.X && x < c.X) return false;
+                if (x > a.X && x > b.X && x > c.X) return false;
+                if (y < a.Y && y < b.Y && y < c.Y) return false;
+                if (y > a.Y && y > b.Y && y > c.Y) return false;
+
+                float vx2 = x - a.X;
+                float vy2 = y - a.Y;
+                float vx1 = b.X - a.X;
+                float vy1 = b.Y - a.Y;
+                float vx0 = c.X - a.X;
+                float vy0 = c.Y - a.Y;
+
+                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));
+            }
+        }
+    }
+}

Farseer Physics Engine 3.5/Common/Decomposition/FlipcodeDecomposer.cs

@@ -0,0 +1,152 @@
+using System.Collections.Generic;
+using System.Diagnostics;
+using Microsoft.Xna.Framework;
+
+namespace FarseerPhysics.Common.Decomposition
+{
+    /// <summary>
+    /// Convex decomposition algorithm created by unknown
+    /// 
+    /// Properties:
+    /// - No support for holes
+    /// - Very fast
+    /// - Only works on simple polygons
+    /// - Only works on counter clockwise polygons
+    /// 
+    /// More information: http://www.flipcode.com/archives/Efficient_Polygon_Triangulation.shtml
+    /// </summary>
+    internal static class FlipcodeDecomposer
+    {
+        private static Vector2 _tmpA;
+        private static Vector2 _tmpB;
+        private static Vector2 _tmpC;
+
+        /// <summary>
+        /// Decompose the polygon into triangles.
+        /// 
+        /// Properties:
+        /// - Only works on counter clockwise polygons
+        /// 
+        /// </summary>
+        /// <param name="vertices">The list of points describing the polygon</param>
+        public static List<Vertices> ConvexPartition(Vertices vertices)
+        {
+            Debug.Assert(vertices.Count > 3);
+            Debug.Assert(vertices.IsCounterClockWise());
+
+            int[] polygon = new int[vertices.Count];
+
+            for (int v = 0; v < vertices.Count; v++)
+                polygon[v] = v;
+
+            int nv = vertices.Count;
+
+            // 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 = vertices[polygon[u]];
+                _tmpB = vertices[polygon[v]];
+                _tmpC = vertices[polygon[w]];
+
+                if (Snip(vertices, u, v, w, nv, polygon))
+                {
+                    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++)
+                    {
+                        polygon[s] = polygon[t];
+                    }
+                    nv--;
+
+                    // Reset error detection counter
+                    count = 2 * nv;
+                }
+            }
+
+            return result;
+        }
+
+        /// <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;
+        }
+    }
+}

Farseer Physics Engine 3.5/Common/Decomposition/Seidel/Edge.cs

@@ -0,0 +1,60 @@
+using System.Collections.Generic;
+
+namespace FarseerPhysics.Common.Decomposition.Seidel
+{
+    internal class Edge
+    {
+        // Pointers used for building trapezoidal map
+        public Trapezoid Above;
+        public float B;
+        public Trapezoid Below;
+
+        // Montone mountain points
+        public HashSet<Point> MPoints;
+        public Point P;
+        public Point Q;
+
+        // Slope of the line (m)
+        public float Slope;
+
+
+        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);
+        }
+    }
+}

Farseer Physics Engine 3.5/Common/Decomposition/Seidel/MonotoneMountain.cs

@@ -0,0 +1,166 @@
+using System;
+using System.Collections.Generic;
+using System.Diagnostics;
+
+namespace FarseerPhysics.Common.Decomposition.Seidel
+{
+    internal class MonotoneMountain
+    {
+        // Almost Pi!
+        private const float PiSlop = 3.1f;
+
+        // Triangles that constitute the mountain
+        public List<List<Point>> Triangles;
+        private HashSet<Point> _convexPoints;
+        private Point _head;
+
+        // Monotone mountain points
+        private List<Point> _monoPoly;
+
+        // Used to track which side of the line we are on
+        private bool _positive;
+        private int _size;
+        private Point _tail;
+
+        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.0f)
+                    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;
+        }
+    }
+}

Farseer Physics Engine 3.5/Common/Decomposition/Seidel/Node.cs

@@ -0,0 +1,41 @@
+using System.Collections.Generic;
+
+namespace FarseerPhysics.Common.Decomposition.Seidel
+{
+    // 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);
+        }
+    }
+}

Farseer Physics Engine 3.5/Common/Decomposition/Seidel/Point.cs

@@ -0,0 +1,61 @@
+namespace FarseerPhysics.Common.Decomposition.Seidel
+{
+    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;
+        }
+    }
+}

Farseer Physics Engine 3.5/Common/Decomposition/Seidel/QueryGraph.cs

@@ -0,0 +1,78 @@
+using System.Collections.Generic;
+
+namespace FarseerPhysics.Common.Decomposition.Seidel
+{
+    // 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);
+        }
+    }
+}

Farseer Physics Engine 3.5/Common/Decomposition/Seidel/Sink.cs

@@ -0,0 +1,27 @@
+namespace FarseerPhysics.Common.Decomposition.Seidel
+{
+    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;
+        }
+    }
+}

Farseer Physics Engine 3.5/Common/Decomposition/Seidel/Trapezoid.cs

@@ -0,0 +1,123 @@
+using System.Collections.Generic;
+
+namespace FarseerPhysics.Common.Decomposition.Seidel
+{
+    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> GetVertices()
+        {
+            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);
+            }
+        }
+    }
+}

Farseer Physics Engine 3.5/Common/Decomposition/Seidel/TrapezoidalMap.cs

@@ -0,0 +1,195 @@
+using System.Collections.Generic;
+
+namespace FarseerPhysics.Common.Decomposition.Seidel
+{
+    internal class TrapezoidalMap
+    {
+        // Trapezoid container
+        public HashSet<Trapezoid> Map;
+
+        // Bottom segment that spans multiple trapezoids
+        private Edge _bCross;
+
+        // Top segment that spans multiple trapezoids
+        private Edge _cross;
+
+        // AABB margin
+        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);
+        }
+    }
+}

Farseer Physics Engine 3.5/Common/Decomposition/Seidel/Triangulator.cs

@@ -0,0 +1,203 @@
+using System;
+using System.Collections.Generic;
+
+namespace FarseerPhysics.Common.Decomposition.Seidel
+{
+    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);
+        }
+    }
+}

Farseer Physics Engine 3.5/Common/Decomposition/Seidel/XNode.cs

@@ -0,0 +1,21 @@
+namespace FarseerPhysics.Common.Decomposition.Seidel
+{
+    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)
+                return RightChild.Locate(edge); // Move to the right in the graph
+
+            return LeftChild.Locate(edge); // Move to the left in the graph
+        }
+    }
+}

Farseer Physics Engine 3.5/Common/Decomposition/Seidel/YNode.cs

@@ -0,0 +1,29 @@
+namespace FarseerPhysics.Common.Decomposition.Seidel
+{
+    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))
+                return RightChild.Locate(edge); // Move down the graph
+
+            if (_edge.IsBelow(edge.P))
+                return LeftChild.Locate(edge); // Move up the graph
+
+            // s and segment share the same endpoint, p
+            if (edge.Slope < _edge.Slope)
+                return RightChild.Locate(edge); // Move down the graph
+
+            // Move up the graph
+            return LeftChild.Locate(edge);
+        }
+    }
+}

Farseer Physics Engine 3.5/Common/Decomposition/SeidelDecomposer.cs

@@ -0,0 +1,109 @@
+/*
+* Farseer Physics Engine:
+* Copyright (c) 2012 Ian Qvist
+*/
+
+using System.Collections.Generic;
+using System.Diagnostics;
+using FarseerPhysics.Common.Decomposition.Seidel;
+using Microsoft.Xna.Framework;
+using Point = FarseerPhysics.Common.Decomposition.Seidel.Point;
+
+namespace FarseerPhysics.Common.Decomposition
+{
+    /// <summary>
+    /// Convex decomposition algorithm created by Raimund Seidel
+    /// 
+    /// Properties:
+    /// - Decompose the polygon into trapezoids, then triangulate.
+    /// - To use the trapezoid data, use ConvexPartitionTrapezoid()
+    /// - Generate a lot of garbage due to incapsulation of the Poly2Tri library.
+    /// - Running time is O(n log n), n = number of vertices.
+    /// - Running time is almost linear for most simple polygons.
+    /// - Does not care about winding order. 
+    /// 
+    /// For more information, see 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
+    /// 
+    /// Original code from the Poly2Tri project by Mason Green.
+    /// http://code.google.com/p/poly2tri/source/browse?repo=archive#hg/scala/src/org/poly2tri/seidel
+    /// 
+    /// This implementation is from Dec 14, 2010
+    /// </summary>
+    internal static class SeidelDecomposer
+    {
+        /// <summary>
+        /// Decompose the polygon into several smaller non-concave polygons.
+        /// </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.</param>
+        /// <returns>A list of triangles</returns>
+        public static List<Vertices> ConvexPartition(Vertices vertices, float sheer = 0.001f)
+        {
+            Debug.Assert(vertices.Count > 3);
+
+            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 outTriangles = new Vertices(triangle.Count);
+
+                foreach (Point outTriangle in triangle)
+                {
+                    outTriangles.Add(new Vector2(outTriangle.X, outTriangle.Y));
+                }
+
+                list.Add(outTriangles);
+            }
+
+            return list;
+        }
+
+        /// <summary>
+        /// Decompose the polygon into several smaller non-concave polygons.
+        /// </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.</param>
+        /// <returns>A list of trapezoids</returns>
+        public static List<Vertices> ConvexPartitionTrapezoid(Vertices vertices, float sheer = 0.001f)
+        {
+            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.GetVertices();
+                foreach (Point point in points)
+                {
+                    verts.Add(new Vector2(point.X, point.Y));
+                }
+
+                list.Add(verts);
+            }
+
+            return list;
+        }
+    }
+}

Farseer Physics Engine 3.5/Common/Decomposition/Triangulate.cs

@@ -0,0 +1,172 @@
+using System;
+using System.Collections.Generic;
+using System.Diagnostics;
+using FarseerPhysics.Common.ConvexHull;
+
+namespace FarseerPhysics.Common.Decomposition
+{
+    public enum TriangulationAlgorithm
+    {
+        /// <summary>
+        /// Convex decomposition algorithm using ear clipping
+        /// 
+        /// Properties:
+        /// - Only works on simple polygons.
+        /// - Does not support holes.
+        /// - Running time is O(n^2), n = number of vertices.
+        /// </summary>
+        Earclip,
+
+        /// <summary>
+        /// Convex decomposition algorithm created by Mark Bayazit (http://mnbayazit.com/)
+        /// 
+        /// Properties:
+        /// - Tries to decompose using polygons instead of triangles.
+        /// - Tends to produce optimal results with low processing time.
+        /// - Running time is O(nr), n = number of vertices, r = reflex vertices.
+        /// - Does not support holes.
+        /// </summary>
+        Bayazit,
+
+        /// <summary>
+        /// Convex decomposition algorithm created by unknown
+        /// 
+        /// Properties:
+        /// - No support for holes
+        /// - Very fast
+        /// - Only works on simple polygons
+        /// - Only works on counter clockwise polygons
+        /// </summary>
+        Flipcode,
+
+        /// <summary>
+        /// Convex decomposition algorithm created by Raimund Seidel
+        /// 
+        /// Properties:
+        /// - Decompose the polygon into trapezoids, then triangulate.
+        /// - To use the trapezoid data, use ConvexPartitionTrapezoid()
+        /// - Generate a lot of garbage due to incapsulation of the Poly2Tri library.
+        /// - Running time is O(n log n), n = number of vertices.
+        /// - Running time is almost linear for most simple polygons.
+        /// - Does not care about winding order. 
+        /// </summary>
+        Seidel,
+        SeidelTrapezoids,
+
+        /// <summary>
+        /// 2D constrained Delaunay triangulation algorithm.
+        /// Based on the paper "Sweep-line algorithm for constrained Delaunay triangulation" by V. Domiter and and B. Zalik
+        /// 
+        /// Properties:
+        /// - Creates triangles with a large interior angle.
+        /// - Supports holes
+        /// - Generate a lot of garbage due to incapsulation of the Poly2Tri library.
+        /// - Running time is O(n^2), n = number of vertices.
+        /// - Does not care about winding order.
+        /// </summary>
+        Delauny
+    }
+
+    public static class Triangulate
+    {
+        public static List<Vertices> ConvexPartition(Vertices vertices, TriangulationAlgorithm algorithm, bool discardAndFixInvalid = true, float tolerance = 0.001f)
+        {
+            if (vertices.Count <= 3)
+                return new List<Vertices> { vertices };
+
+            List<Vertices> results;
+
+            switch (algorithm)
+            {
+                case TriangulationAlgorithm.Earclip:
+                    if (Settings.SkipSanityChecks)
+                        Debug.Assert(!vertices.IsCounterClockWise(), "The Earclip algorithm expects the polygon to be clockwise.");
+                    else
+                    {
+                        if (vertices.IsCounterClockWise())
+                        {
+                            Vertices temp = new Vertices(vertices);
+                            temp.Reverse();
+                            results = EarclipDecomposer.ConvexPartition(temp, tolerance);
+                        }
+                        else
+                            results = EarclipDecomposer.ConvexPartition(vertices, tolerance);
+                    }
+                    break;
+                case TriangulationAlgorithm.Bayazit:
+                    if (Settings.SkipSanityChecks)
+                        Debug.Assert(vertices.IsCounterClockWise(), "The polygon is not counter clockwise. This is needed for Bayazit to work correctly.");
+                    else
+                    {
+                        if (!vertices.IsCounterClockWise())
+                        {
+                            Vertices temp = new Vertices(vertices);
+                            temp.Reverse();
+                            results = BayazitDecomposer.ConvexPartition(temp);
+                        }
+                        else
+                            results = BayazitDecomposer.ConvexPartition(vertices);
+                    }
+                    break;
+                case TriangulationAlgorithm.Flipcode:
+                    if (Settings.SkipSanityChecks)
+                        Debug.Assert(vertices.IsCounterClockWise(), "The polygon is not counter clockwise. This is needed for Bayazit to work correctly.");
+                    else
+                    {
+                        if (!vertices.IsCounterClockWise())
+                        {
+                            Vertices temp = new Vertices(vertices);
+                            temp.Reverse();
+                            results = FlipcodeDecomposer.ConvexPartition(temp);
+                        }
+                        else
+                            results = FlipcodeDecomposer.ConvexPartition(vertices);
+                    }
+                    break;
+                case TriangulationAlgorithm.Seidel:
+                    results = SeidelDecomposer.ConvexPartition(vertices, tolerance);
+                    break;
+                case TriangulationAlgorithm.SeidelTrapezoids:
+                    results = SeidelDecomposer.ConvexPartitionTrapezoid(vertices, tolerance);
+                    break;
+                case TriangulationAlgorithm.Delauny:
+                    results = CDTDecomposer.ConvexPartition(vertices);
+                    break;
+                default:
+                    throw new ArgumentOutOfRangeException("algorithm");
+            }
+
+            if (discardAndFixInvalid)
+            {
+                for (int i = results.Count - 1; i >= 0; i--)
+                {
+                    Vertices polygon = results[i];
+
+                    if (!ValidatePolygon(polygon))
+                        results.RemoveAt(i);
+                }
+            }
+
+            return results;
+        }
+
+        private static bool ValidatePolygon(Vertices polygon)
+        {
+            PolygonError errorCode = polygon.CheckPolygon();
+
+            if (errorCode == PolygonError.InvalidAmountOfVertices || errorCode == PolygonError.AreaTooSmall || errorCode == PolygonError.SideTooSmall || errorCode == PolygonError.NotSimple)
+                return false;
+
+            if (errorCode == PolygonError.NotCounterClockWise) //NotCounterCloseWise is the last check in CheckPolygon(), thus we don't need to call ValidatePolygon again.
+                polygon.Reverse();
+
+            if (errorCode == PolygonError.NotConvex)
+            {
+                polygon = GiftWrap.GetConvexHull(polygon);
+                return ValidatePolygon(polygon);
+            }
+
+            return true;
+        }
+    }
+}

Farseer Physics Engine 3.5/Common/FixedArray.cs

@@ -0,0 +1,224 @@
+/*
+* Farseer Physics Engine:
+* Copyright (c) 2012 Ian Qvist
+* 
+* Original source Box2D:
+* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org 
+* 
+* 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();
+                }
+            }
+        }
+    }
+}

Farseer Physics Engine 3.5/Common/HashSet.cs

@@ -0,0 +1,78 @@
+#if WINDOWS_PHONE || XBOX
+
+using System.Collections;
+using System.Collections.Generic;
+
+namespace FarseerPhysics.Common
+{
+    public class HashSet<T> : ICollection<T>
+    {
+        private Dictionary<T, byte> _dict;
+
+        public HashSet(int capacity)
+        {
+            _dict = new Dictionary<T, byte>(capacity);
+        }
+
+        public HashSet()
+        {
+            _dict = new Dictionary<T, byte>();
+        }
+
+        #region ICollection<T> Members
+
+        public void Add(T item)
+        {
+            // We don't care for the value in dictionary, only keys matter.
+            if (!_dict.ContainsKey(item))
+                _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)
+        {
+            foreach (var item in _dict.Keys)
+            {
+                array[arrayIndex++] = item;
+            }
+        }
+
+        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

Farseer Physics Engine 3.5/Common/LineTools.cs

@@ -0,0 +1,287 @@
+using System;
+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 DistanceBetweenPointAndLineSegment(ref Vector2 point, ref Vector2 start, ref Vector2 end)
+        {
+            if (start == end)
+                return Vector2.Distance(point, start);
+
+            Vector2 v = Vector2.Subtract(end, start);
+            Vector2 w = Vector2.Subtract(point, start);
+
+            float c1 = Vector2.Dot(w, v);
+            if (c1 <= 0) return Vector2.Distance(point, start);
+
+            float c2 = Vector2.Dot(v, v);
+            if (c2 <= c1) return Vector2.Distance(point, end);
+
+            float b = c1 / c2;
+            Vector2 pointOnLine = Vector2.Add(start, Vector2.Multiply(v, b));
+            return Vector2.Distance(point, 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>
+        public static bool LineIntersect2(ref Vector2 a0, ref Vector2 a1, ref Vector2 b0, ref  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>
+        public static Vertices LineSegmentVerticesIntersect(ref Vector2 point1, ref Vector2 point2, Vertices vertices)
+        {
+            Vertices intersectionPoints = new Vertices();
+
+            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);
+                }
+            }
+
+            return intersectionPoints;
+        }
+
+        /// <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>
+        public static Vertices LineSegmentAABBIntersect(ref Vector2 point1, ref Vector2 point2, AABB aabb)
+        {
+            return LineSegmentVerticesIntersect(ref point1, ref point2, aabb.Vertices);
+        }
+    }
+}

Farseer Physics Engine 3.5/Common/Math.cs

@@ -0,0 +1,806 @@
+/*
+* Farseer Physics Engine:
+* Copyright (c) 2012 Ian Qvist
+* 
+* Original source Box2D:
+* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org 
+* 
+* 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(ref Vector2 a, ref Vector2 b)
+        {
+            return a.X * b.Y - a.Y * b.X;
+        }
+
+        public static float Cross(Vector2 a, Vector2 b)
+        {
+            return Cross(ref a, ref b);
+        }
+
+        /// Perform the cross product on two vectors.
+        public static Vector3 Cross(Vector3 a, Vector3 b)
+        {
+            return new Vector3(a.Y * b.Z - a.Z * b.Y, a.Z * b.X - a.X * b.Z, 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 Mul(ref Mat22 A, Vector2 v)
+        {
+            return Mul(ref A, ref v);
+        }
+
+        public static Vector2 Mul(ref Mat22 A, ref Vector2 v)
+        {
+            return new Vector2(A.ex.X * v.X + A.ey.X * v.Y, A.ex.Y * v.X + A.ey.Y * v.Y);
+        }
+
+        public static Vector2 Mul(ref Transform T, Vector2 v)
+        {
+            return Mul(ref T, ref v);
+        }
+
+        public static Vector2 Mul(ref Transform T, ref Vector2 v)
+        {
+            float x = (T.q.c * v.X - T.q.s * v.Y) + T.p.X;
+            float y = (T.q.s * v.X + T.q.c * v.Y) + T.p.Y;
+
+            return new Vector2(x, y);
+        }
+
+        public static Vector2 MulT(ref Mat22 A, Vector2 v)
+        {
+            return MulT(ref A, ref v);
+        }
+
+        public static Vector2 MulT(ref Mat22 A, ref Vector2 v)
+        {
+            return new Vector2(v.X * A.ex.X + v.Y * A.ex.Y, v.X * A.ey.X + v.Y * A.ey.Y);
+        }
+
+        public static Vector2 MulT(ref Transform T, Vector2 v)
+        {
+            return MulT(ref T, ref v);
+        }
+
+        public static Vector2 MulT(ref Transform T, ref Vector2 v)
+        {
+            float px = v.X - T.p.X;
+            float py = v.Y - T.p.Y;
+            float x = (T.q.c * px + T.q.s * py);
+            float y = (-T.q.s * px + T.q.c * py);
+
+            return new Vector2(x, y);
+        }
+
+        // A^T * B
+        public static void MulT(ref Mat22 A, ref Mat22 B, out Mat22 C)
+        {
+            C = new Mat22();
+            C.ex.X = A.ex.X * B.ex.X + A.ex.Y * B.ex.Y;
+            C.ex.Y = A.ey.X * B.ex.X + A.ey.Y * B.ex.Y;
+            C.ey.X = A.ex.X * B.ey.X + A.ex.Y * B.ey.Y;
+            C.ey.Y = A.ey.X * B.ey.X + A.ey.Y * B.ey.Y;
+        }
+
+        /// Multiply a matrix times a vector.
+        public static Vector3 Mul(Mat33 A, Vector3 v)
+        {
+            return v.X * A.ex + v.Y * A.ey + v.Z * A.ez;
+        }
+
+        // v2 = A.q.Rot(B.q.Rot(v1) + B.p) + A.p
+        //    = (A.q * B.q).Rot(v1) + A.q.Rot(B.p) + A.p
+        public static Transform Mul(Transform A, Transform B)
+        {
+            Transform C = new Transform();
+            C.q = Mul(A.q, B.q);
+            C.p = Mul(A.q, B.p) + A.p;
+            return C;
+        }
+
+        // v2 = A.q' * (B.q * v1 + B.p - A.p)
+        //    = A.q' * B.q * v1 + A.q' * (B.p - A.p)
+        public static void MulT(ref Transform A, ref Transform B, out Transform C)
+        {
+            C = new Transform();
+            C.q = MulT(A.q, B.q);
+            C.p = MulT(A.q, B.p - A.p);
+        }
+
+        public static void Swap<T>(ref T a, ref T b)
+        {
+            T tmp = a;
+            a = b;
+            b = tmp;
+        }
+
+        /// Multiply a matrix times a vector.
+        public static Vector2 Mul22(Mat33 A, Vector2 v)
+        {
+            return new Vector2(A.ex.X * v.X + A.ey.X * v.Y, A.ex.Y * v.X + A.ey.Y * v.Y);
+        }
+
+        /// Multiply two rotations: q * r
+        public static Rot Mul(Rot q, Rot r)
+        {
+            // [qc -qs] * [rc -rs] = [qc*rc-qs*rs -qc*rs-qs*rc]
+            // [qs  qc]   [rs  rc]   [qs*rc+qc*rs -qs*rs+qc*rc]
+            // s = qs * rc + qc * rs
+            // c = qc * rc - qs * rs
+            Rot qr;
+            qr.s = q.s * r.c + q.c * r.s;
+            qr.c = q.c * r.c - q.s * r.s;
+            return qr;
+        }
+
+        public static Vector2 MulT(Transform T, Vector2 v)
+        {
+            float px = v.X - T.p.X;
+            float py = v.Y - T.p.Y;
+            float x = (T.q.c * px + T.q.s * py);
+            float y = (-T.q.s * px + T.q.c * py);
+
+            return new Vector2(x, y);
+        }
+
+        /// Transpose multiply two rotations: qT * r
+        public static Rot MulT(Rot q, Rot r)
+        {
+            // [ qc qs] * [rc -rs] = [qc*rc+qs*rs -qc*rs+qs*rc]
+            // [-qs qc]   [rs  rc]   [-qs*rc+qc*rs qs*rs+qc*rc]
+            // s = qc * rs - qs * rc
+            // c = qc * rc + qs * rs
+            Rot qr;
+            qr.s = q.c * r.s - q.s * r.c;
+            qr.c = q.c * r.c + q.s * r.s;
+            return qr;
+        }
+
+        // v2 = A.q' * (B.q * v1 + B.p - A.p)
+        //    = A.q' * B.q * v1 + A.q' * (B.p - A.p)
+        public static Transform MulT(Transform A, Transform B)
+        {
+            Transform C = new Transform();
+            C.q = MulT(A.q, B.q);
+            C.p = MulT(A.q, B.p - A.p);
+            return C;
+        }
+
+        /// Rotate a vector
+        public static Vector2 Mul(Rot q, Vector2 v)
+        {
+            return new Vector2(q.c * v.X - q.s * v.Y, q.s * v.X + q.c * v.Y);
+        }
+
+        /// Inverse rotate a vector
+        public static Vector2 MulT(Rot q, Vector2 v)
+        {
+            return new Vector2(q.c * v.X + q.s * v.Y, -q.s * v.X + q.c * v.Y);
+        }
+
+        /// Get the skew vector such that dot(skew_vec, other) == cross(vec, other)
+        public static Vector2 Skew(Vector2 input)
+        {
+            return new Vector2(-input.Y, input.X);
+        }
+
+        /// <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);
+        }
+
+        /// Perform the dot product on two vectors.
+        public static float Dot(Vector3 a, Vector3 b)
+        {
+            return a.X * b.X + a.Y * b.Y + a.Z * b.Z;
+        }
+
+        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>
+        /// <param name="tolerance">The tolerance</param>
+        /// <returns></returns>
+        public static bool IsCollinear(ref Vector2 a, ref Vector2 b, ref Vector2 c, float tolerance = 0)
+        {
+            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
+
+        public static Vector2 Mul(ref Rot rot, Vector2 axis)
+        {
+            return Mul(rot, axis);
+        }
+
+        public static Vector2 MulT(ref Rot rot, Vector2 axis)
+        {
+            return MulT(rot, axis);
+        }
+    }
+
+    /// <summary>
+    /// A 2-by-2 matrix. Stored in column-major order.
+    /// </summary>
+    public struct Mat22
+    {
+        public Vector2 ex, ey;
+
+        /// <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)
+        {
+            ex = c1;
+            ey = 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)
+        {
+            ex = new Vector2(a11, a21);
+            ey = new Vector2(a12, a22);
+        }
+
+        public Mat22 Inverse
+        {
+            get
+            {
+                float a = ex.X, b = ey.X, c = ex.Y, d = ey.Y;
+                float det = a * d - b * c;
+                if (det != 0.0f)
+                {
+                    det = 1.0f / det;
+                }
+
+                Mat22 result = new Mat22();
+                result.ex.X = det * d;
+                result.ex.Y = -det * c;
+
+                result.ey.X = -det * b;
+                result.ey.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)
+        {
+            ex = c1;
+            ey = c2;
+        }
+
+        /// <summary>
+        /// Set this to the identity matrix.
+        /// </summary>
+        public void SetIdentity()
+        {
+            ex.X = 1.0f;
+            ey.X = 0.0f;
+            ex.Y = 0.0f;
+            ey.Y = 1.0f;
+        }
+
+        /// <summary>
+        /// Set this matrix to all zeros.
+        /// </summary>
+        public void SetZero()
+        {
+            ex.X = 0.0f;
+            ey.X = 0.0f;
+            ex.Y = 0.0f;
+            ey.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 = ex.X, a12 = ey.X, a21 = ex.Y, a22 = ey.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.ex = A.ex + B.ex;
+            R.ey = A.ey + B.ey;
+        }
+    }
+
+    /// <summary>
+    /// A 3-by-3 matrix. Stored in column-major order.
+    /// </summary>
+    public struct Mat33
+    {
+        public Vector3 ex, ey, ez;
+
+        /// <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)
+        {
+            ex = c1;
+            ey = c2;
+            ez = c3;
+        }
+
+        /// <summary>
+        /// Set this matrix to all zeros.
+        /// </summary>
+        public void SetZero()
+        {
+            ex = Vector3.Zero;
+            ey = Vector3.Zero;
+            ez = 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(ex, Vector3.Cross(ey, ez));
+            if (det != 0.0f)
+            {
+                det = 1.0f / det;
+            }
+
+            return new Vector3(det * Vector3.Dot(b, Vector3.Cross(ey, ez)), det * Vector3.Dot(ex, Vector3.Cross(b, ez)), det * Vector3.Dot(ex, Vector3.Cross(ey, 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 = ex.X, a12 = ey.X, a21 = ex.Y, a22 = ey.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));
+        }
+
+        /// Get the inverse of this matrix as a 2-by-2.
+        /// Returns the zero matrix if singular.
+        public void GetInverse22(ref Mat33 M)
+        {
+            float a = ex.X, b = ey.X, c = ex.Y, d = ey.Y;
+            float det = a * d - b * c;
+            if (det != 0.0f)
+            {
+                det = 1.0f / det;
+            }
+
+            M.ex.X = det * d; M.ey.X = -det * b; M.ex.Z = 0.0f;
+            M.ex.Y = -det * c; M.ey.Y = det * a; M.ey.Z = 0.0f;
+            M.ez.X = 0.0f; M.ez.Y = 0.0f; M.ez.Z = 0.0f;
+        }
+
+        /// Get the symmetric inverse of this matrix as a 3-by-3.
+        /// Returns the zero matrix if singular.
+        public void GetSymInverse33(ref Mat33 M)
+        {
+            float det = MathUtils.Dot(ex, MathUtils.Cross(ey, ez));
+            if (det != 0.0f)
+            {
+                det = 1.0f / det;
+            }
+
+            float a11 = ex.X, a12 = ey.X, a13 = ez.X;
+            float a22 = ey.Y, a23 = ez.Y;
+            float a33 = ez.Z;
+
+            M.ex.X = det * (a22 * a33 - a23 * a23);
+            M.ex.Y = det * (a13 * a23 - a12 * a33);
+            M.ex.Z = det * (a12 * a23 - a13 * a22);
+
+            M.ey.X = M.ex.Y;
+            M.ey.Y = det * (a11 * a33 - a13 * a13);
+            M.ey.Z = det * (a13 * a12 - a11 * a23);
+
+            M.ez.X = M.ex.Z;
+            M.ez.Y = M.ey.Z;
+            M.ez.Z = det * (a11 * a22 - a12 * a12);
+        }
+    }
+
+    /// <summary>
+    /// Rotation
+    /// </summary>
+    public struct Rot
+    {
+        /// Sine and cosine
+        public float s, c;
+
+        /// <summary>
+        /// Initialize from an angle in radians
+        /// </summary>
+        /// <param name="angle">Angle in radians</param>
+        public Rot(float angle)
+        {
+            // TODO_ERIN optimize
+            s = (float)Math.Sin(angle);
+            c = (float)Math.Cos(angle);
+        }
+
+        /// <summary>
+        /// Set using an angle in radians.
+        /// </summary>
+        /// <param name="angle"></param>
+        public void Set(float angle)
+        {
+            // TODO_ERIN optimize
+            s = (float)Math.Sin(angle);
+            c = (float)Math.Cos(angle);
+        }
+
+        /// <summary>
+        /// Set to the identity rotation
+        /// </summary>
+        public void SetIdentity()
+        {
+            s = 0.0f;
+            c = 1.0f;
+        }
+
+        /// <summary>
+        /// Get the angle in radians
+        /// </summary>
+        public float GetAngle()
+        {
+            return (float)Math.Atan2(s, c);
+        }
+
+        /// <summary>
+        /// Get the x-axis
+        /// </summary>
+        public Vector2 GetXAxis()
+        {
+            return new Vector2(c, s);
+        }
+
+        /// <summary>
+        /// Get the y-axis
+        /// </summary>
+        public Vector2 GetYAxis()
+        {
+            return new Vector2(-s, c);
+        }
+    }
+
+    /// <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 p;
+        public Rot q;
+
+        /// <summary>
+        /// Initialize using a position vector and a rotation matrix.
+        /// </summary>
+        /// <param name="position">The position.</param>
+        /// <param name="rotation">The r.</param>
+        public Transform(ref Vector2 position, ref Rot rotation)
+        {
+            p = position;
+            q = rotation;
+        }
+
+        /// <summary>
+        /// Set this to the identity transform.
+        /// </summary>
+        public void SetIdentity()
+        {
+            p = Vector2.Zero;
+            q.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)
+        {
+            p = position;
+            q.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="xfb">The transform.</param>
+        /// <param name="beta">beta is a factor in [0,1], where 0 indicates alpha0.</param>
+        public void GetTransform(out Transform xfb, float beta)
+        {
+            xfb = new Transform();
+            xfb.p.X = (1.0f - beta) * C0.X + beta * C.X;
+            xfb.p.Y = (1.0f - beta) * C0.Y + beta * C.Y;
+            float angle = (1.0f - beta) * A0 + beta * A;
+            xfb.q.Set(angle);
+
+            // Shift to origin
+            xfb.p -= MathUtils.Mul(xfb.q, 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 += beta * (C - C0);
+            A0 += beta * (A - A0);
+            Alpha0 = alpha;
+        }
+
+        /// <summary>
+        /// Normalize the angles.
+        /// </summary>
+        public void Normalize()
+        {
+            float d = MathHelper.TwoPi * (float)Math.Floor(A0 / MathHelper.TwoPi);
+            A0 -= d;
+            A -= d;
+        }
+    }
+}

Farseer Physics Engine 3.5/Common/MathHelper.cs

@@ -0,0 +1,158 @@
+#if !XNA && !WINDOWS_PHONE && !XBOX && !ANDROID
+
+#region License
+
+/*
+MIT License
+Copyright © 2006 The Mono.Xna Team
+
+All rights reserved.
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all
+copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.
+*/
+
+#endregion License
+
+using System;
+
+namespace Microsoft.Xna.Framework
+{
+    public static class MathHelper
+    {
+        public const float E = (float) Math.E;
+        public const float Log10E = 0.4342945f;
+        public const float Log2E = 1.442695f;
+        public const float Pi = (float) Math.PI;
+        public const float PiOver2 = (float) (Math.PI/2.0);
+        public const float PiOver4 = (float) (Math.PI/4.0);
+        public const float TwoPi = (float) (Math.PI*2.0);
+
+        public static float Barycentric(float value1, float value2, float value3, float amount1, float amount2)
+        {
+            return value1 + (value2 - value1)*amount1 + (value3 - value1)*amount2;
+        }
+
+        public static float CatmullRom(float value1, float value2, float value3, float value4, float amount)
+        {
+            // Using formula from http://www.mvps.org/directx/articles/catmull/
+            // Internally using doubles not to lose precission
+            double amountSquared = amount*amount;
+            double amountCubed = amountSquared*amount;
+            return (float) (0.5*(2.0*value2 +
+                                 (value3 - value1)*amount +
+                                 (2.0*value1 - 5.0*value2 + 4.0*value3 - value4)*amountSquared +
+                                 (3.0*value2 - value1 - 3.0*value3 + value4)*amountCubed));
+        }
+
+        public static float Clamp(float value, float min, float max)
+        {
+            // First we check to see if we're greater than the max
+            value = (value > max) ? max : value;
+
+            // Then we check to see if we're less than the min.
+            value = (value < min) ? min : value;
+
+            // There's no check to see if min > max.
+            return value;
+        }
+
+        public static float Distance(float value1, float value2)
+        {
+            return Math.Abs(value1 - value2);
+        }
+
+        public static float Hermite(float value1, float tangent1, float value2, float tangent2, float amount)
+        {
+            // All transformed to double not to lose precission
+            // Otherwise, for high numbers of param:amount the result is NaN instead of Infinity
+            double v1 = value1, v2 = value2, t1 = tangent1, t2 = tangent2, s = amount, result;
+            double sCubed = s*s*s;
+            double sSquared = s*s;
+
+            if (amount == 0f)
+                result = value1;
+            else if (amount == 1f)
+                result = value2;
+            else
+                result = (2*v1 - 2*v2 + t2 + t1)*sCubed +
+                         (3*v2 - 3*v1 - 2*t1 - t2)*sSquared +
+                         t1*s +
+                         v1;
+            return (float) result;
+        }
+
+        public static float Lerp(float value1, float value2, float amount)
+        {
+            return value1 + (value2 - value1)*amount;
+        }
+
+        public static float Max(float value1, float value2)
+        {
+            return Math.Max(value1, value2);
+        }
+
+        public static float Min(float value1, float value2)
+        {
+            return Math.Min(value1, value2);
+        }
+
+        public static float SmoothStep(float value1, float value2, float amount)
+        {
+            // It is expected that 0 < amount < 1
+            // If amount < 0, return value1
+            // If amount > 1, return value2
+            float result = Clamp(amount, 0f, 1f);
+            result = Hermite(value1, 0f, value2, 0f, result);
+            return result;
+        }
+
+        public static float ToDegrees(float radians)
+        {
+            // This method uses double precission internally,
+            // though it returns single float
+            // Factor = 180 / pi
+            return (float) (radians*57.295779513082320876798154814105);
+        }
+
+        public static float ToRadians(float degrees)
+        {
+            // This method uses double precission internally,
+            // though it returns single float
+            // Factor = pi / 180
+            return (float) (degrees*0.017453292519943295769236907684886);
+        }
+
+        public static float WrapAngle(float angle)
+        {
+            angle = (float) Math.IEEERemainder((double) angle, 6.2831854820251465); //2xPi precission is double
+            if (angle <= -3.141593f)
+            {
+                angle += 6.283185f;
+                return angle;
+            }
+            if (angle > 3.141593f)
+            {
+                angle -= 6.283185f;
+            }
+            return angle;
+        }
+    }
+}
+
+#endif

Farseer Physics Engine 3.5/Common/Path.cs

@@ -0,0 +1,337 @@
+using System;
+using System.Collections.Generic;
+using System.Text;
+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>
+    public class Path
+    {
+        /// <summary>
+        /// All the points that makes up the curve
+        /// </summary>
+        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>
+        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;
+        }
+    }
+}

Farseer Physics Engine 3.5/Common/PathManager.cs

@@ -0,0 +1,186 @@
+using System;
+using System.Collections.Generic;
+using FarseerPhysics.Collision.Shapes;
+using FarseerPhysics.Common.Decomposition;
+using FarseerPhysics.Dynamics;
+using FarseerPhysics.Dynamics.Joints;
+using Microsoft.Xna.Framework;
+
+namespace FarseerPhysics.Common
+{
+    /// <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)
+            {
+                ChainShape chain = new ChainShape(verts, true);
+                body.CreateFixture(chain);
+            }
+            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 = Triangulate.ConvexPartition(new Vertices(verts), TriangulationAlgorithm.Bayazit);
+
+            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 = null)
+        {
+            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;
+                b.UserData = userData;
+
+                foreach (Shape shape in shapes)
+                {
+                    b.CreateFixture(shape);
+                }
+
+                bodyList.Add(b);
+            }
+
+            return bodyList;
+        }
+
+
+        /// <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);
+        }
+
+        /// <summary>
+        /// Moves the given body along the defined 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;
+        }
+    }
+}

Farseer Physics Engine 3.5/Common/PhysicsLogic/FilterData.cs

@@ -0,0 +1,133 @@
+using FarseerPhysics.Dynamics;
+
+namespace FarseerPhysics.Common.PhysicsLogic
+{
+    /// <summary>
+    /// Contains filter data that can determine whether an object should be processed or not.
+    /// </summary>
+    public abstract class FilterData
+    {
+        /// <summary>
+        /// Disable the logic on specific categories.
+        /// Category.None by default.
+        /// </summary>
+        public Category DisabledOnCategories = Category.None;
+
+        /// <summary>
+        /// Disable the logic on specific groups
+        /// </summary>
+        public int DisabledOnGroup;
+
+        /// <summary>
+        /// Enable the logic on specific categories
+        /// Category.All by default.
+        /// </summary>
+        public Category EnabledOnCategories = Category.All;
+
+        /// <summary>
+        /// Enable the logic on specific groups.
+        /// </summary>
+        public int EnabledOnGroup;
+
+        /// <summary>
+        /// 
+        /// </summary>
+        /// <param name="body"></param>
+        /// <returns></returns>
+        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 IsInEnabledInCategory(Category category)
+        {
+            return (EnabledOnCategories & category) == category;
+        }
+    }
+}

Farseer Physics Engine 3.5/Common/PhysicsLogic/PhysicsLogic.cs

@@ -0,0 +1,66 @@
+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;
+        }
+    }
+}

Farseer Physics Engine 3.5/Common/PhysicsLogic/RealExplosion.cs

@@ -0,0 +1,418 @@
+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
+{
+    // 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
+
+    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>
+    /// Creates a realistic explosion based on raycasting. Objects in the open will be affected, but objects behind
+    /// static bodies will not. A body that is half in cover, half in the open will get half the force applied to the end in
+    /// the open.
+    /// </summary>
+    public sealed class RealExplosion : 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 RayDataComparer _rdc;
+
+        public RealExplosion(World world)
+            : base(world, PhysicsLogicType.Explosion)
+        {
+            _rdc = new RayDataComparer();
+            _data = new List<ShapeData>();
+        }
+
+        /// <summary>
+        /// Activate the explosion at the specified position.
+        /// </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 list of bodies and the amount of force that was applied to them.</returns>
+        public Dictionary<Fixture, Vector2> Activate(Vector2 pos, float radius, float maxForce)
+        {
+            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;
+                            return false;
+                        }
+
+                        containedShapes[containedShapeCount++] = fixture;
+                    }
+                    else
+                    {
+                        shapes[shapeCount++] = fixture;
+                    }
+
+                    // Continue the query.
+                    return true;
+                }, ref aabb);
+
+            if (exit)
+                return new Dictionary<Fixture, Vector2>();
+
+            Dictionary<Fixture, Vector2> exploded = new Dictionary<Fixture, Vector2>(shapeCount + containedShapeCount);
+
+            // 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)
+                            continue; // 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 fixture = 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;
+
+                                      hitClosest = true;
+                                      fixture = f;
+                                      return fr;
+                                  }, p1, p2);
+
+                //draws radius points
+                if ((hitClosest) && (fixture.Body.BodyType == BodyType.Dynamic))
+                {
+                    if ((_data.Any()) && (_data.Last().Body == fixture.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 = fixture.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
+                        if (exploded.ContainsKey(f))
+                            exploded[f] += vectImp;
+                        else
+                            exploded.Add(f, vectImp);
+
+                        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);
+
+                fix.Body.ApplyLinearImpulse(ref vectImp, ref hitPoint);
+
+                if (!exploded.ContainsKey(fix))
+                    exploded.Add(fix, vectImp);
+            }
+
+            return exploded;
+        }
+    }
+}

Farseer Physics Engine 3.5/Common/PhysicsLogic/SimpleExplosion.cs

@@ -0,0 +1,92 @@
+using System;
+using System.Collections.Generic;
+using FarseerPhysics.Collision;
+using FarseerPhysics.Dynamics;
+using Microsoft.Xna.Framework;
+
+namespace FarseerPhysics.Common.PhysicsLogic
+{
+    /// <summary>
+    /// Creates a simple explosion that ignores other bodies hiding behind static bodies.
+    /// </summary>
+    public sealed class SimpleExplosion : PhysicsLogic
+    {
+        public SimpleExplosion(World world)
+            : base(world, PhysicsLogicType.Explosion)
+        {
+            Power = 1; //linear
+        }
+
+        /// <summary>
+        /// This is the power used in the power function. A value of 1 means the force
+        /// applied to bodies in the explosion is linear. A value of 2 means it is exponential.
+        /// </summary>
+        public float Power { get; set; }
+
+        /// <summary>
+        /// Activate the explosion at the specified position.
+        /// </summary>
+        /// <param name="pos">The position (center) of the explosion.</param>
+        /// <param name="radius">The radius of the explosion.</param>
+        /// <param name="force">The force applied</param>
+        /// <param name="maxForce">A maximum amount of force. When force gets over this value, it will be equal to maxForce</param>
+        /// <returns>A list of bodies and the amount of force that was applied to them.</returns>
+        public Dictionary<Body, Vector2> Activate(Vector2 pos, float radius, float force, float maxForce = float.MaxValue)
+        {
+            HashSet<Body> affectedBodies = new HashSet<Body>();
+
+            AABB aabb;
+            aabb.LowerBound = pos - new Vector2(radius);
+            aabb.UpperBound = pos + new Vector2(radius);
+
+            // Query the world for bodies within the radius.
+            World.QueryAABB(fixture =>
+            {
+                if (Vector2.Distance(fixture.Body.Position, pos) <= radius)
+                {
+                    if (!affectedBodies.Contains(fixture.Body))
+                        affectedBodies.Add(fixture.Body);
+                }
+
+                return true;
+            }, ref aabb);
+
+            return ApplyImpulse(pos, radius, force, maxForce, affectedBodies);
+        }
+
+        private Dictionary<Body, Vector2> ApplyImpulse(Vector2 pos, float radius, float force, float maxForce, HashSet<Body> overlappingBodies)
+        {
+            Dictionary<Body, Vector2> forces = new Dictionary<Body, Vector2>(overlappingBodies.Count);
+
+            foreach (Body overlappingBody in overlappingBodies)
+            {
+                if (IsActiveOn(overlappingBody))
+                {
+                    float distance = Vector2.Distance(pos, overlappingBody.Position);
+                    float forcePercent = GetPercent(distance, radius);
+
+                    Vector2 forceVector = pos - overlappingBody.Position;
+                    forceVector *= 1f / (float)Math.Sqrt(forceVector.X * forceVector.X + forceVector.Y * forceVector.Y);
+                    forceVector *= MathHelper.Min(force * forcePercent, maxForce);
+                    forceVector *= -1;
+
+                    overlappingBody.ApplyLinearImpulse(forceVector);
+                    forces.Add(overlappingBody, forceVector);
+                }
+            }
+
+            return forces;
+        }
+
+        private float GetPercent(float distance, float radius)
+        {
+            //(1-(distance/radius))^power-1
+            float percent = (float)Math.Pow(1 - ((distance - radius) / radius), Power) - 1;
+
+            if (float.IsNaN(percent))
+                return 0f;
+
+            return MathHelper.Clamp(percent, 0f, 1f);
+        }
+    }
+}

Farseer Physics Engine 3.5/Common/PolygonManipulation/CuttingTools.cs

@@ -0,0 +1,217 @@
+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="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, 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;
+
+            //We can only cut polygons at the moment
+            if (shape == null)
+            {
+                first = new Vertices();
+                second = new Vertices();
+                return;
+            }
+
+            //Offset the entry and exit points if they are too close to the vertices
+            foreach (Vector2 vertex in shape.Vertices)
+            {
+                if (vertex.Equals(localEntryPoint))
+                    localEntryPoint -= new Vector2(0, Settings.Epsilon);
+
+                if (vertex.Equals(localExitPoint))
+                    localExitPoint += new Vector2(0, Settings.Epsilon);
+            }
+
+            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();
+
+                if (!offset.IsValid())
+                    offset = Vector2.One;
+
+                newPolygon[n][cutAdded[n]] += Settings.Epsilon * 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();
+
+                if (!offset.IsValid())
+                    offset = Vector2.One;
+
+                newPolygon[n][cutAdded[n] + 1] += Settings.Epsilon * 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>
+        /// <returns>True if the cut was performed.</returns>
+        public static bool Cut(World world, Vector2 start, Vector2 end)
+        {
+            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 false;
+
+            //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 false;
+
+            for (int i = 0; i < fixtures.Count; i++)
+            {
+                // can't cut circles or edges 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], out first, out second);
+
+                    //Delete the original shape and create two new. Retain the properties of the body.
+                    if (first.CheckPolygon() == PolygonError.NoError)
+                    {
+                        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 (second.CheckPolygon() == PolygonError.NoError)
+                    {
+                        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);
+                }
+            }
+
+            return true;
+        }
+    }
+}

Farseer Physics Engine 3.5/Common/PolygonManipulation/SimpleCombiner.cs

@@ -0,0 +1,226 @@
+/*
+* 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.Collections.Generic;
+using System.Diagnostics;
+using Microsoft.Xna.Framework;
+
+namespace FarseerPhysics.Common.PolygonManipulation
+{
+    /// <summary>
+    /// Combines a list of triangles into a list of convex polygons.
+    /// Starts with a seed triangle, keep adding triangles to it until you can't add any more without making the polygon non-convex.
+    /// </summary>
+    public static class SimpleCombiner
+    {
+        /// <summary>
+        /// Combine a list of triangles into a list of convex polygons.
+        /// 
+        /// Note: This only works on triangles.
+        /// </summary>
+        ///<param name="triangles">The triangles.</param>
+        ///<param name="maxPolys">The maximun number of polygons to return.</param>
+        ///<param name="tolerance">The tolerance</param>
+        public static List<Vertices> PolygonizeTriangles(List<Vertices> triangles, int maxPolys = int.MaxValue, float tolerance = 0.001f)
+        {
+            if (triangles.Count <= 0)
+                return triangles;
+
+            List<Vertices> polys = new List<Vertices>();
+
+            bool[] covered = new bool[triangles.Count];
+            for (int i = 0; i < triangles.Count; ++i)
+            {
+                covered[i] = false;
+
+                //Check here for degenerate triangles
+                Vertices triangle = triangles[i];
+                Vector2 a = triangle[0];
+                Vector2 b = triangle[1];
+                Vector2 c = triangle[2];
+
+                if ((a.X == b.X && a.Y == b.Y) || (b.X == c.X && b.Y == c.Y) || (a.X == c.X && a.Y == c.Y))
+                    covered[i] = true;
+            }
+
+            int polyIndex = 0;
+
+            bool notDone = true;
+            while (notDone)
+            {
+                int currTri = -1;
+                for (int i = 0; i < triangles.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(triangles[currTri][i]);
+                    }
+
+                    covered[currTri] = true;
+                    int index = 0;
+                    for (int i = 0; i < 2 * triangles.Count; ++i, ++index)
+                    {
+                        while (index >= triangles.Count) index -= triangles.Count;
+                        if (covered[index])
+                        {
+                            continue;
+                        }
+                        Vertices newP = AddTriangle(triangles[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
+                            Debug.WriteLine("Skipping corrupt poly.");
+                    }
+
+                    if (poly.Count >= 3)
+                        polyIndex++; //Must be outside (polyIndex < polysLength) test
+                }
+            }
+
+            //TODO: Add sanity check
+            //Remove empty vertice collections
+            for (int i = polys.Count - 1; i >= 0; i--)
+            {
+                if (polys[i].Count == 0)
+                    polys.RemoveAt(i);
+            }
+
+            return polys;
+        }
+
+        private static Vertices AddTriangle(Vertices 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[0].X == vertices[i].X && t[0].Y == vertices[i].Y)
+                {
+                    if (firstP == -1)
+                    {
+                        firstP = i;
+                        firstT = 0;
+                    }
+                    else
+                    {
+                        secondP = i;
+                        secondT = 0;
+                    }
+                }
+                else if (t[1].X == vertices[i].X && t[1].Y == vertices[i].Y)
+                {
+                    if (firstP == -1)
+                    {
+                        firstP = i;
+                        firstT = 1;
+                    }
+                    else
+                    {
+                        secondP = i;
+                        secondT = 1;
+                    }
+                }
+                else if (t[2].X == vertices[i].X && t[2].Y == 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(t[tipT]);
+            }
+
+            return result;
+        }
+    }
+}

Farseer Physics Engine 3.5/Common/PolygonManipulation/SimplifyTools.cs

@@ -0,0 +1,302 @@
+using System;
+using System.Collections.Generic;
+using System.Diagnostics;
+using Microsoft.Xna.Framework;
+
+namespace FarseerPhysics.Common.PolygonManipulation
+{
+    /// <summary>
+    /// Provides a set of tools to simplify polygons in various ways.
+    /// </summary>
+    public static class SimplifyTools
+    {
+        /// <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 = 0)
+        {
+            if (vertices.Count <= 3)
+                return vertices;
+
+            Vertices simplified = new Vertices(vertices.Count);
+
+            for (int i = 0; i < vertices.Count; i++)
+            {
+                Vector2 prev = vertices.PreviousVertex(i);
+                Vector2 current = vertices[i];
+                Vector2 next = vertices.NextVertex(i);
+
+                //If they collinear, continue
+                if (MathUtils.IsCollinear(ref prev, ref current, ref next, collinearityTolerance))
+                    continue;
+
+                simplified.Add(current);
+            }
+
+            return simplified;
+        }
+
+        /// <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)
+        {
+            if (vertices.Count <= 3)
+                return vertices;
+
+            bool[] usePoint = new bool[vertices.Count];
+
+            for (int i = 0; i < vertices.Count; i++)
+                usePoint[i] = true;
+
+            SimplifySection(vertices, 0, vertices.Count - 1, usePoint, distanceTolerance);
+
+            Vertices simplified = new Vertices(vertices.Count);
+
+            for (int i = 0; i < vertices.Count; i++)
+            {
+                if (usePoint[i])
+                    simplified.Add(vertices[i]);
+            }
+
+            return simplified;
+        }
+
+        private static void SimplifySection(Vertices vertices, int i, int j, bool[] usePoint, float distanceTolerance)
+        {
+            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++)
+            {
+                Vector2 point = vertices[k];
+
+                double distance = LineTools.DistanceBetweenPointAndLineSegment(ref point, ref a, ref b);
+
+                if (distance > maxDistance)
+                {
+                    maxDistance = distance;
+                    maxIndex = k;
+                }
+            }
+
+            if (maxDistance <= distanceTolerance)
+            {
+                for (int k = i + 1; k < j; k++)
+                {
+                    usePoint[k] = false;
+                }
+            }
+            else
+            {
+                SimplifySection(vertices, i, maxIndex, usePoint, distanceTolerance);
+                SimplifySection(vertices, maxIndex, j, usePoint, distanceTolerance);
+            }
+        }
+
+        /// <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 Vertices MergeParallelEdges(Vertices vertices, float tolerance)
+        {
+            //From Eric Jordan's convex decomposition library
+
+            if (vertices.Count <= 3)
+                return vertices; //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 vertices;
+
+            int currIndex = 0;
+
+            //Copy the vertices to a new list and clear the old
+            Vertices newVertices = new Vertices(newNVertices);
+
+            for (int i = 0; i < vertices.Count; ++i)
+            {
+                if (mergeMe[i] || newNVertices == 0 || currIndex == newNVertices)
+                    continue;
+
+                Debug.Assert(currIndex < newNVertices);
+
+                newVertices.Add(vertices[i]);
+                ++currIndex;
+            }
+
+            return newVertices;
+        }
+
+        /// <summary>
+        /// Merges the identical points in the polygon.
+        /// </summary>
+        /// <param name="vertices">The vertices.</param>
+        public static Vertices MergeIdenticalPoints(Vertices vertices)
+        {
+            HashSet<Vector2> unique = new HashSet<Vector2>();
+
+            foreach (Vector2 vertex in vertices)
+            {
+                unique.Add(vertex);
+            }
+
+            return new Vertices(unique);
+        }
+
+        /// <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 removed.</param>
+        public static Vertices ReduceByDistance(Vertices vertices, float distance)
+        {
+            if (vertices.Count <= 3)
+                return vertices;
+
+            float distance2 = distance * distance;
+
+            Vertices simplified = new Vertices(vertices.Count);
+
+            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() <= distance2)
+                    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)
+        {
+            if (vertices.Count <= 3)
+                return vertices;
+
+            if (nth == 0)
+                return vertices;
+
+            Vertices simplified = new Vertices(vertices.Count);
+
+            for (int i = 0; i < vertices.Count; i++)
+            {
+                if (i % nth == 0)
+                    continue;
+
+                simplified.Add(vertices[i]);
+            }
+
+            return simplified;
+        }
+
+        /// <summary>
+        /// Simplify the polygon by removing all points that in pairs of 3 have an area less than the tolerance.
+        /// 
+        /// Pass in 0 as tolerance, and it will only remove collinear points.
+        /// </summary>
+        /// <param name="vertices"></param>
+        /// <param name="areaTolerance"></param>
+        /// <returns></returns>
+        public static Vertices ReduceByArea(Vertices vertices, float areaTolerance)
+        {
+            //From physics2d.net
+
+            if (vertices.Count <= 3)
+                return vertices;
+
+            if (areaTolerance < 0)
+                throw new ArgumentOutOfRangeException("areaTolerance", "must be equal to or greater than zero.");
+
+            Vertices simplified = new Vertices(vertices.Count);
+            Vector2 v3;
+            Vector2 v1 = vertices[vertices.Count - 2];
+            Vector2 v2 = vertices[vertices.Count - 1];
+            areaTolerance *= 2;
+
+            for (int i = 0; i < vertices.Count; ++i, v2 = v3)
+            {
+                v3 = i == vertices.Count - 1 ? simplified[0] : vertices[i];
+
+                float old1;
+                MathUtils.Cross(ref v1, ref v2, out old1);
+
+                float old2;
+                MathUtils.Cross(ref v2, ref v3, out old2);
+
+                float new1;
+                MathUtils.Cross(ref v1, ref v3, out new1);
+
+                if (Math.Abs(new1 - (old1 + old2)) > areaTolerance)
+                {
+                    simplified.Add(v2);
+                    v1 = v2;
+                }
+            }
+
+            return simplified;
+        }
+    }
+}

Farseer Physics Engine 3.5/Common/PolygonManipulation/YuPengClipper.cs

@@ -0,0 +1,514 @@
+using System;
+using System.Collections.Generic;
+using System.Diagnostics;
+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.GetAABB().LowerBound;
+      Vector2 lbClip = clip.GetAABB().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 = 0;
+        if (poly2Simplicies.Contains(poly1Simplicies[i]))
+        {
+          edgeCharacter = 1f;
+        }
+        else if (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)
+      {
+        float edgeCharacter = 0f;
+        if (!resultSimplices.Contains(poly2Simplicies[i]) &&
+            !resultSimplices.Contains(-poly2Simplicies[i]))
+        {
+          if (poly1Simplicies.Contains(-poly2Simplicies[i]) && clipType == PolyClipType.Union)
+          {
+            edgeCharacter = 1f;
+          }
+          else
+          {
+            edgeCharacter = 0f;
+            for (int j = 0; j < poly1Simplicies.Count; ++j)
+            {
+              if (!poly1Simplicies.Contains(poly2Simplicies[i]) && !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
+  }
+}

Farseer Physics Engine 3.5/Common/PolygonTools.cs

@@ -0,0 +1,360 @@
+using System;
+using System.Collections.Generic;
+using System.Diagnostics;
+using FarseerPhysics.Common.TextureTools;
+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.p = center;
+            xf.q.Set(angle);
+
+            // Transform vertices
+            for (int i = 0; i < 4; ++i)
+            {
+                vertices[i] = MathUtils.Mul(ref xf, vertices[i]);
+            }
+
+            return vertices;
+        }
+
+        //Rounded rectangle contributed by Jonathan Smars - jsmars@gmail.com
+
+        /// <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);
+        }
+    }
+}

Farseer Physics Engine 3.5/Common/Stopwatch.cs

@@ -0,0 +1,112 @@
+#if SILVERLIGHT
+using System;
+
+namespace FarseerPhysics.Common
+{
+    // Source: http://www.wiredprairie.us/blog/index.php/archives/723
+
+    /// <summary>
+    /// An emulation for the Stopwatch class for Windows Phone and Silverlight.
+    /// </summary>
+    public sealed class Stopwatch
+    {
+        private long _startTick;
+        private long _elapsed;
+        private bool _isRunning;
+
+        /// <summary>
+        /// Creates a new instance of the class and starts the watch immediately.
+        /// </summary>
+        /// <returns>An instance of Stopwatch, running.</returns>
+        public static Stopwatch StartNew()
+        {
+            Stopwatch sw = new Stopwatch();
+            sw.Start();
+            return sw;
+        }
+
+        /// <summary>
+        /// Creates an instance of the Stopwatch class.
+        /// </summary>
+        public Stopwatch() { }
+
+        /// <summary>
+        /// Completely resets and deactivates the timer.
+        /// </summary>
+        public void Reset()
+        {
+            _elapsed = 0;
+            _isRunning = false;
+            _startTick = 0;
+        }
+
+        /// <summary>
+        /// Begins the timer.
+        /// </summary>
+        public void Start()
+        {
+            if (!_isRunning)
+            {
+                _startTick = GetCurrentTicks();
+                _isRunning = true;
+            }
+        }
+
+        /// <summary>
+        /// Stops the current timer.
+        /// </summary>
+        public void Stop()
+        {
+            if (_isRunning)
+            {
+                _elapsed += GetCurrentTicks() - _startTick;
+                _isRunning = false;
+            }
+        }
+
+        /// <summary>
+        /// Gets a value indicating whether the instance is currently recording.
+        /// </summary>
+        public bool IsRunning
+        {
+            get { return _isRunning; }
+        }
+
+        /// <summary>
+        /// Gets the Elapsed time as a Timespan.
+        /// </summary>
+        public TimeSpan Elapsed
+        {
+            get { return TimeSpan.FromMilliseconds(ElapsedMilliseconds); }
+        }
+
+        /// <summary>
+        /// Gets the Elapsed time as the total number of milliseconds.
+        /// </summary>
+        public long ElapsedMilliseconds
+        {
+            get { return GetCurrentElapsedTicks() / TimeSpan.TicksPerMillisecond; }
+        }
+
+        /// <summary>
+        /// Gets the Elapsed time as the total number of ticks (which is faked
+        /// as Silverlight doesn't have a way to get at the actual "Ticks")
+        /// </summary>
+        public long ElapsedTicks
+        {
+            get { return GetCurrentElapsedTicks(); }
+        }
+
+        private long GetCurrentElapsedTicks()
+        {
+            return _elapsed + (IsRunning ? (GetCurrentTicks() - _startTick) : 0);
+        }
+
+        private long GetCurrentTicks()
+        {
+            // TickCount: Gets the number of milliseconds elapsed since the system started.
+            return Environment.TickCount * TimeSpan.TicksPerMillisecond;
+        }
+    }
+}
+#endif

Farseer Physics Engine 3.5/Common/TextureTools/MarchingSquares.cs

@@ -0,0 +1,800 @@
+using System.Collections.Generic;
+using FarseerPhysics.Collision;
+using Microsoft.Xna.Framework;
+
+namespace FarseerPhysics.Common.TextureTools
+{
+    // 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
+    }
+}

Farseer Physics Engine 3.5/Common/TextureTools/Terrain.cs

@@ -0,0 +1,264 @@
+using System.Collections.Generic;
+using FarseerPhysics.Collision;
+using FarseerPhysics.Common.Decomposition;
+using FarseerPhysics.Common.PolygonManipulation;
+using FarseerPhysics.Dynamics;
+using FarseerPhysics.Factories;
+using Microsoft.Xna.Framework;
+
+namespace FarseerPhysics.Common.TextureTools
+{
+    /// <summary>
+    /// Simple class to maintain a terrain. It can keep track
+    /// </summary>
+    public class Terrain
+    {
+        /// <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 TriangulationAlgorithm 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;
+
+        /// <summary>
+        /// Creates a new terrain.
+        /// </summary>
+        /// <param name="world">The World</param>
+        /// <param name="area">The area of the terrain.</param>
+        public Terrain(World world, AABB area)
+        {
+            World = world;
+            Width = area.Width;
+            Height = area.Height;
+            Center = area.Center;
+        }
+
+        /// <summary>
+        /// Creates a new terrain
+        /// </summary>
+        /// <param name="world">The World</param>
+        /// <param name="position">The position (center) of the terrain.</param>
+        /// <param name="width">The width of the terrain.</param>
+        /// <param name="height">The height of the terrain.</param>
+        public Terrain(World world, Vector2 position, float width, float height)
+        {
+            World = world;
+            Width = width;
+            Height = height;
+            Center = position;
+        }
+
+        /// <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 the specified texture data to the terrain.
+        /// </summary>
+        /// <param name="data"></param>
+        /// <param name="offset"></param>
+        public void ApplyData(sbyte[,] data, Vector2 offset = default(Vector2))
+        {
+            for (int x = 0; x < data.GetUpperBound(0); x++)
+            {
+                for (int y = 0; y < data.GetUpperBound(1); y++)
+                {
+                    if (x + offset.X >= 0 && x + offset.X < _localWidth && y + offset.Y >= 0 && y + offset.Y < _localHeight)
+                    {
+                        _terrainMap[(int)(x + offset.X), (int)(y + offset.Y)] = data[x, y];
+                    }
+                }
+            }
+
+            RemoveOldData(0, _xnum, 0, _ynum);
+        }
+
+        /// <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
+            int xStart = (int)(_dirtyArea.LowerBound.X / CellSize);
+            if (xStart < 0) xStart = 0;
+
+            int xEnd = (int)(_dirtyArea.UpperBound.X / CellSize) + 1;
+            if (xEnd > _xnum) xEnd = _xnum;
+
+            int yStart = (int)(_dirtyArea.LowerBound.Y / CellSize);
+            if (yStart < 0) yStart = 0;
+
+            int yEnd = (int)(_dirtyArea.UpperBound.Y / CellSize) + 1;
+            if (yEnd > _ynum) yEnd = _ynum;
+
+            RemoveOldData(xStart, xEnd, yStart, yEnd);
+
+            _dirtyArea = new AABB(new Vector2(float.MaxValue, float.MaxValue), new Vector2(float.MinValue, float.MinValue));
+        }
+
+        private void RemoveOldData(int xStart, int xEnd, int yStart, int yEnd)
+        {
+            for (int x = xStart; x < xEnd; x++)
+            {
+                for (int y = yStart; y < yEnd; y++)
+                {
+                    //remove old terrain object at grid cell
+                    if (_bodyMap[x, y] != null)
+                    {
+                        for (int i = 0; i < _bodyMap[x, y].Count; i++)
+                        {
+                            World.RemoveBody(_bodyMap[x, y][i]);
+                        }
+                    }
+
+                    _bodyMap[x, y] = null;
+
+                    //generate new one
+                    GenerateTerrain(x, y);
+                }
+            }
+        }
+
+        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 (Vertices item in polys)
+            {
+                // does this need to be negative?
+                item.Scale(ref scale);
+                item.Translate(ref _topLeft);
+                Vertices simplified = SimplifyTools.CollinearSimplify(item);
+
+                List<Vertices> decompPolys = Triangulate.ConvexPartition(simplified, Decomposer);
+
+                foreach (Vertices poly in decompPolys)
+                {
+                    if (poly.Count > 2)
+                        _bodyMap[gx, gy].Add(BodyFactory.CreatePolygon(World, poly, 1));
+                }
+            }
+        }
+    }
+}

Farseer Physics Engine 3.5/Common/Vector2.cs

@@ -0,0 +1,588 @@
+#if !XNA && !WINDOWS_PHONE && !XBOX && !ANDROID
+
+#region License
+
+/*
+MIT License
+Copyright © 2006 The Mono.Xna Team
+
+All rights reserved.
+
+Authors
+ * Alan McGovern
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all
+copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.
+*/
+
+#endregion License
+
+using System;
+using System.Runtime.InteropServices;
+using System.Text;
+
+namespace Microsoft.Xna.Framework
+{
+    [StructLayout(LayoutKind.Sequential)]
+    public struct Vector2 : IEquatable<Vector2>
+    {
+#region Private Fields
+
+        private static Vector2 zeroVector = new Vector2(0f, 0f);
+        private static Vector2 unitVector = new Vector2(1f, 1f);
+        private static Vector2 unitXVector = new Vector2(1f, 0f);
+        private static Vector2 unitYVector = new Vector2(0f, 1f);
+
+        #endregion Private Fields
+
+#region Public Fields
+
+        public float X;
+        public float Y;
+
+        #endregion Public Fields
+
+#region Properties
+
+        public static Vector2 Zero
+        {
+            get { return zeroVector; }
+        }
+
+        public static Vector2 One
+        {
+            get { return unitVector; }
+        }
+
+        public static Vector2 UnitX
+        {
+            get { return unitXVector; }
+        }
+
+        public static Vector2 UnitY
+        {
+            get { return unitYVector; }
+        }
+
+        #endregion Properties
+
+#region Constructors
+
+        /// <summary>
+        /// Constructor foe standard 2D vector.
+        /// </summary>
+        /// <param name="x">
+        /// A <see cref="System.Single"/>
+        /// </param>
+        /// <param name="y">
+        /// A <see cref="System.Single"/>
+        /// </param>
+        public Vector2(float x, float y)
+        {
+            X = x;
+            Y = y;
+        }
+
+        /// <summary>
+        /// Constructor for "square" vector.
+        /// </summary>
+        /// <param name="value">
+        /// A <see cref="System.Single"/>
+        /// </param>
+        public Vector2(float value)
+        {
+            X = value;
+            Y = value;
+        }
+
+        #endregion Constructors
+
+#region Public Methods
+
+        public static void Reflect(ref Vector2 vector, ref Vector2 normal, out Vector2 result)
+        {
+            float dot = Dot(vector, normal);
+            result.X = vector.X - ((2f*dot)*normal.X);
+            result.Y = vector.Y - ((2f*dot)*normal.Y);
+        }
+
+        public static Vector2 Reflect(Vector2 vector, Vector2 normal)
+        {
+            Vector2 result;
+            Reflect(ref vector, ref normal, out result);
+            return result;
+        }
+
+        public static Vector2 Add(Vector2 value1, Vector2 value2)
+        {
+            value1.X += value2.X;
+            value1.Y += value2.Y;
+            return value1;
+        }
+
+        public static void Add(ref Vector2 value1, ref Vector2 value2, out Vector2 result)
+        {
+            result.X = value1.X + value2.X;
+            result.Y = value1.Y + value2.Y;
+        }
+
+        public static Vector2 Barycentric(Vector2 value1, Vector2 value2, Vector2 value3, float amount1, float amount2)
+        {
+            return new Vector2(
+                MathHelper.Barycentric(value1.X, value2.X, value3.X, amount1, amount2),
+                MathHelper.Barycentric(value1.Y, value2.Y, value3.Y, amount1, amount2));
+        }
+
+        public static void Barycentric(ref Vector2 value1, ref Vector2 value2, ref Vector2 value3, float amount1,
+                                       float amount2, out Vector2 result)
+        {
+            result = new Vector2(
+                MathHelper.Barycentric(value1.X, value2.X, value3.X, amount1, amount2),
+                MathHelper.Barycentric(value1.Y, value2.Y, value3.Y, amount1, amount2));
+        }
+
+        public static Vector2 CatmullRom(Vector2 value1, Vector2 value2, Vector2 value3, Vector2 value4, float amount)
+        {
+            return new Vector2(
+                MathHelper.CatmullRom(value1.X, value2.X, value3.X, value4.X, amount),
+                MathHelper.CatmullRom(value1.Y, value2.Y, value3.Y, value4.Y, amount));
+        }
+
+        public static void CatmullRom(ref Vector2 value1, ref Vector2 value2, ref Vector2 value3, ref Vector2 value4,
+                                      float amount, out Vector2 result)
+        {
+            result = new Vector2(
+                MathHelper.CatmullRom(value1.X, value2.X, value3.X, value4.X, amount),
+                MathHelper.CatmullRom(value1.Y, value2.Y, value3.Y, value4.Y, amount));
+        }
+
+        public static Vector2 Clamp(Vector2 value1, Vector2 min, Vector2 max)
+        {
+            return new Vector2(
+                MathHelper.Clamp(value1.X, min.X, max.X),
+                MathHelper.Clamp(value1.Y, min.Y, max.Y));
+        }
+
+        public static void Clamp(ref Vector2 value1, ref Vector2 min, ref Vector2 max, out Vector2 result)
+        {
+            result = new Vector2(
+                MathHelper.Clamp(value1.X, min.X, max.X),
+                MathHelper.Clamp(value1.Y, min.Y, max.Y));
+        }
+
+        /// <summary>
+        /// Returns float precison distanve between two vectors
+        /// </summary>
+        /// <param name="value1">
+        /// A <see cref="Vector2"/>
+        /// </param>
+        /// <param name="value2">
+        /// A <see cref="Vector2"/>
+        /// </param>
+        /// <returns>
+        /// A <see cref="System.Single"/>
+        /// </returns>
+        public static float Distance(Vector2 value1, Vector2 value2)
+        {
+            float result;
+            DistanceSquared(ref value1, ref value2, out result);
+            return (float) Math.Sqrt(result);
+        }
+
+
+        public static void Distance(ref Vector2 value1, ref Vector2 value2, out float result)
+        {
+            DistanceSquared(ref value1, ref value2, out result);
+            result = (float) Math.Sqrt(result);
+        }
+
+        public static float DistanceSquared(Vector2 value1, Vector2 value2)
+        {
+            float result;
+            DistanceSquared(ref value1, ref value2, out result);
+            return result;
+        }
+
+        public static void DistanceSquared(ref Vector2 value1, ref Vector2 value2, out float result)
+        {
+            result = (value1.X - value2.X)*(value1.X - value2.X) + (value1.Y - value2.Y)*(value1.Y - value2.Y);
+        }
+
+        /// <summary>
+        /// Devide first vector with the secund vector
+        /// </summary>
+        /// <param name="value1">
+        /// A <see cref="Vector2"/>
+        /// </param>
+        /// <param name="value2">
+        /// A <see cref="Vector2"/>
+        /// </param>
+        /// <returns>
+        /// A <see cref="Vector2"/>
+        /// </returns>
+        public static Vector2 Divide(Vector2 value1, Vector2 value2)
+        {
+            value1.X /= value2.X;
+            value1.Y /= value2.Y;
+            return value1;
+        }
+
+        public static void Divide(ref Vector2 value1, ref Vector2 value2, out Vector2 result)
+        {
+            result.X = value1.X/value2.X;
+            result.Y = value1.Y/value2.Y;
+        }
+
+        public static Vector2 Divide(Vector2 value1, float divider)
+        {
+            float factor = 1/divider;
+            value1.X *= factor;
+            value1.Y *= factor;
+            return value1;
+        }
+
+        public static void Divide(ref Vector2 value1, float divider, out Vector2 result)
+        {
+            float factor = 1/divider;
+            result.X = value1.X*factor;
+            result.Y = value1.Y*factor;
+        }
+
+        public static float Dot(Vector2 value1, Vector2 value2)
+        {
+            return value1.X*value2.X + value1.Y*value2.Y;
+        }
+
+        public static void Dot(ref Vector2 value1, ref Vector2 value2, out float result)
+        {
+            result = value1.X*value2.X + value1.Y*value2.Y;
+        }
+
+        public override bool Equals(object obj)
+        {
+            return (obj is Vector2) ? this == ((Vector2) obj) : false;
+        }
+
+        public bool Equals(Vector2 other)
+        {
+            return this == other;
+        }
+
+        public override int GetHashCode()
+        {
+            return (int) (X + Y);
+        }
+
+        public static Vector2 Hermite(Vector2 value1, Vector2 tangent1, Vector2 value2, Vector2 tangent2, float amount)
+        {
+            Vector2 result = new Vector2();
+            Hermite(ref value1, ref tangent1, ref value2, ref tangent2, amount, out result);
+            return result;
+        }
+
+        public static void Hermite(ref Vector2 value1, ref Vector2 tangent1, ref Vector2 value2, ref Vector2 tangent2,
+                                   float amount, out Vector2 result)
+        {
+            result.X = MathHelper.Hermite(value1.X, tangent1.X, value2.X, tangent2.X, amount);
+            result.Y = MathHelper.Hermite(value1.Y, tangent1.Y, value2.Y, tangent2.Y, amount);
+        }
+
+        public float Length()
+        {
+            float result;
+            DistanceSquared(ref this, ref zeroVector, out result);
+            return (float) Math.Sqrt(result);
+        }
+
+        public float LengthSquared()
+        {
+            float result;
+            DistanceSquared(ref this, ref zeroVector, out result);
+            return result;
+        }
+
+        public static Vector2 Lerp(Vector2 value1, Vector2 value2, float amount)
+        {
+            return new Vector2(
+                MathHelper.Lerp(value1.X, value2.X, amount),
+                MathHelper.Lerp(value1.Y, value2.Y, amount));
+        }
+
+        public static void Lerp(ref Vector2 value1, ref Vector2 value2, float amount, out Vector2 result)
+        {
+            result = new Vector2(
+                MathHelper.Lerp(value1.X, value2.X, amount),
+                MathHelper.Lerp(value1.Y, value2.Y, amount));
+        }
+
+        public static Vector2 Max(Vector2 value1, Vector2 value2)
+        {
+            return new Vector2(
+                MathHelper.Max(value1.X, value2.X),
+                MathHelper.Max(value1.Y, value2.Y));
+        }
+
+        public static void Max(ref Vector2 value1, ref Vector2 value2, out Vector2 result)
+        {
+            result = new Vector2(
+                MathHelper.Max(value1.X, value2.X),
+                MathHelper.Max(value1.Y, value2.Y));
+        }
+
+        public static Vector2 Min(Vector2 value1, Vector2 value2)
+        {
+            return new Vector2(
+                MathHelper.Min(value1.X, value2.X),
+                MathHelper.Min(value1.Y, value2.Y));
+        }
+
+        public static void Min(ref Vector2 value1, ref Vector2 value2, out Vector2 result)
+        {
+            result = new Vector2(
+                MathHelper.Min(value1.X, value2.X),
+                MathHelper.Min(value1.Y, value2.Y));
+        }
+
+        public static Vector2 Multiply(Vector2 value1, Vector2 value2)
+        {
+            value1.X *= value2.X;
+            value1.Y *= value2.Y;
+            return value1;
+        }
+
+        public static Vector2 Multiply(Vector2 value1, float scaleFactor)
+        {
+            value1.X *= scaleFactor;
+            value1.Y *= scaleFactor;
+            return value1;
+        }
+
+        public static void Multiply(ref Vector2 value1, float scaleFactor, out Vector2 result)
+        {
+            result.X = value1.X*scaleFactor;
+            result.Y = value1.Y*scaleFactor;
+        }
+
+        public static void Multiply(ref Vector2 value1, ref Vector2 value2, out Vector2 result)
+        {
+            result.X = value1.X*value2.X;
+            result.Y = value1.Y*value2.Y;
+        }
+
+        public static Vector2 Negate(Vector2 value)
+        {
+            value.X = -value.X;
+            value.Y = -value.Y;
+            return value;
+        }
+
+        public static void Negate(ref Vector2 value, out Vector2 result)
+        {
+            result.X = -value.X;
+            result.Y = -value.Y;
+        }
+
+        public void Normalize()
+        {
+            Normalize(ref this, out this);
+        }
+
+        public static Vector2 Normalize(Vector2 value)
+        {
+            Normalize(ref value, out value);
+            return value;
+        }
+
+        public static void Normalize(ref Vector2 value, out Vector2 result)
+        {
+            float factor;
+            DistanceSquared(ref value, ref zeroVector, out factor);
+            factor = 1f/(float) Math.Sqrt(factor);
+            result.X = value.X*factor;
+            result.Y = value.Y*factor;
+        }
+
+        public static Vector2 SmoothStep(Vector2 value1, Vector2 value2, float amount)
+        {
+            return new Vector2(
+                MathHelper.SmoothStep(value1.X, value2.X, amount),
+                MathHelper.SmoothStep(value1.Y, value2.Y, amount));
+        }
+
+        public static void SmoothStep(ref Vector2 value1, ref Vector2 value2, float amount, out Vector2 result)
+        {
+            result = new Vector2(
+                MathHelper.SmoothStep(value1.X, value2.X, amount),
+                MathHelper.SmoothStep(value1.Y, value2.Y, amount));
+        }
+
+        public static Vector2 Subtract(Vector2 value1, Vector2 value2)
+        {
+            value1.X -= value2.X;
+            value1.Y -= value2.Y;
+            return value1;
+        }
+
+        public static void Subtract(ref Vector2 value1, ref Vector2 value2, out Vector2 result)
+        {
+            result.X = value1.X - value2.X;
+            result.Y = value1.Y - value2.Y;
+        }
+
+        public static Vector2 Transform(Vector2 position, Matrix matrix)
+        {
+            Transform(ref position, ref matrix, out position);
+            return position;
+        }
+
+        public static void Transform(ref Vector2 position, ref Matrix matrix, out Vector2 result)
+        {
+            result = new Vector2((position.X*matrix.M11) + (position.Y*matrix.M21) + matrix.M41,
+                                 (position.X*matrix.M12) + (position.Y*matrix.M22) + matrix.M42);
+        }
+
+        public static void Transform(Vector2[] sourceArray, ref Matrix matrix, Vector2[] destinationArray)
+        {
+            throw new NotImplementedException();
+        }
+
+        public static void Transform(Vector2[] sourceArray, int sourceIndex, ref Matrix matrix,
+                                     Vector2[] destinationArray, int destinationIndex, int length)
+        {
+            throw new NotImplementedException();
+        }
+
+        public static Vector2 TransformNormal(Vector2 normal, Matrix matrix)
+        {
+            TransformNormal(ref normal, ref matrix, out normal);
+            return normal;
+        }
+
+        public static void TransformNormal(ref Vector2 normal, ref Matrix matrix, out Vector2 result)
+        {
+            result = new Vector2((normal.X*matrix.M11) + (normal.Y*matrix.M21),
+                                 (normal.X*matrix.M12) + (normal.Y*matrix.M22));
+        }
+
+        public static void TransformNormal(Vector2[] sourceArray, ref Matrix matrix, Vector2[] destinationArray)
+        {
+            throw new NotImplementedException();
+        }
+
+        public static void TransformNormal(Vector2[] sourceArray, int sourceIndex, ref Matrix matrix,
+                                           Vector2[] destinationArray, int destinationIndex, int length)
+        {
+            throw new NotImplementedException();
+        }
+
+        public override string ToString()
+        {
+            StringBuilder sb = new StringBuilder(24);
+            sb.Append("{X:");
+            sb.Append(X);
+            sb.Append(" Y:");
+            sb.Append(Y);
+            sb.Append("}");
+            return sb.ToString();
+        }
+
+        #endregion Public Methods
+
+#region Operators
+
+        public static Vector2 operator -(Vector2 value)
+        {
+            value.X = -value.X;
+            value.Y = -value.Y;
+            return value;
+        }
+
+
+        public static bool operator ==(Vector2 value1, Vector2 value2)
+        {
+            return value1.X == value2.X && value1.Y == value2.Y;
+        }
+
+
+        public static bool operator !=(Vector2 value1, Vector2 value2)
+        {
+            return value1.X != value2.X || value1.Y != value2.Y;
+        }
+
+
+        public static Vector2 operator +(Vector2 value1, Vector2 value2)
+        {
+            value1.X += value2.X;
+            value1.Y += value2.Y;
+            return value1;
+        }
+
+
+        public static Vector2 operator -(Vector2 value1, Vector2 value2)
+        {
+            value1.X -= value2.X;
+            value1.Y -= value2.Y;
+            return value1;
+        }
+
+
+        public static Vector2 operator *(Vector2 value1, Vector2 value2)
+        {
+            value1.X *= value2.X;
+            value1.Y *= value2.Y;
+            return value1;
+        }
+
+
+        public static Vector2 operator *(Vector2 value, float scaleFactor)
+        {
+            value.X *= scaleFactor;
+            value.Y *= scaleFactor;
+            return value;
+        }
+
+
+        public static Vector2 operator *(float scaleFactor, Vector2 value)
+        {
+            value.X *= scaleFactor;
+            value.Y *= scaleFactor;
+            return value;
+        }
+
+
+        public static Vector2 operator /(Vector2 value1, Vector2 value2)
+        {
+            value1.X /= value2.X;
+            value1.Y /= value2.Y;
+            return value1;
+        }
+
+
+        public static Vector2 operator /(Vector2 value1, float divider)
+        {
+            float factor = 1/divider;
+            value1.X *= factor;
+            value1.Y *= factor;
+            return value1;
+        }
+
+        #endregion Operators
+    }
+}
+
+#endif

Farseer Physics Engine 3.5/Common/Vector3.cs

@@ -0,0 +1,648 @@
+#if !XNA && !WINDOWS_PHONE && !XBOX && !ANDROID
+
+#region License
+
+/*
+MIT License
+Copyright © 2006 The Mono.Xna Team
+
+All rights reserved.
+
+Authors:
+ * Alan McGovern
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all
+copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.
+*/
+
+#endregion License
+
+using System;
+using System.Runtime.InteropServices;
+using System.Text;
+
+namespace Microsoft.Xna.Framework
+{
+    [StructLayout(LayoutKind.Sequential)]
+    public struct Vector3 : IEquatable<Vector3>
+    {
+#region Private Fields
+
+        private static Vector3 zero = new Vector3(0f, 0f, 0f);
+        private static Vector3 one = new Vector3(1f, 1f, 1f);
+        private static Vector3 unitX = new Vector3(1f, 0f, 0f);
+        private static Vector3 unitY = new Vector3(0f, 1f, 0f);
+        private static Vector3 unitZ = new Vector3(0f, 0f, 1f);
+        private static Vector3 up = new Vector3(0f, 1f, 0f);
+        private static Vector3 down = new Vector3(0f, -1f, 0f);
+        private static Vector3 right = new Vector3(1f, 0f, 0f);
+        private static Vector3 left = new Vector3(-1f, 0f, 0f);
+        private static Vector3 forward = new Vector3(0f, 0f, -1f);
+        private static Vector3 backward = new Vector3(0f, 0f, 1f);
+
+#endregion Private Fields
+
+#region Public Fields
+
+        public float X;
+        public float Y;
+        public float Z;
+
+#endregion Public Fields
+
+#region Properties
+
+        public static Vector3 Zero
+        {
+            get { return zero; }
+        }
+
+        public static Vector3 One
+        {
+            get { return one; }
+        }
+
+        public static Vector3 UnitX
+        {
+            get { return unitX; }
+        }
+
+        public static Vector3 UnitY
+        {
+            get { return unitY; }
+        }
+
+        public static Vector3 UnitZ
+        {
+            get { return unitZ; }
+        }
+
+        public static Vector3 Up
+        {
+            get { return up; }
+        }
+
+        public static Vector3 Down
+        {
+            get { return down; }
+        }
+
+        public static Vector3 Right
+        {
+            get { return right; }
+        }
+
+        public static Vector3 Left
+        {
+            get { return left; }
+        }
+
+        public static Vector3 Forward
+        {
+            get { return forward; }
+        }
+
+        public static Vector3 Backward
+        {
+            get { return backward; }
+        }
+
+#endregion Properties
+
+#region Constructors
+
+        public Vector3(float x, float y, float z)
+        {
+            X = x;
+            Y = y;
+            Z = z;
+        }
+
+
+        public Vector3(float value)
+        {
+            X = value;
+            Y = value;
+            Z = value;
+        }
+
+
+        public Vector3(Vector2 value, float z)
+        {
+            X = value.X;
+            Y = value.Y;
+            Z = z;
+        }
+
+#endregion Constructors
+
+#region Public Methods
+
+        public static Vector3 Add(Vector3 value1, Vector3 value2)
+        {
+            value1.X += value2.X;
+            value1.Y += value2.Y;
+            value1.Z += value2.Z;
+            return value1;
+        }
+
+        public static void Add(ref Vector3 value1, ref Vector3 value2, out Vector3 result)
+        {
+            result.X = value1.X + value2.X;
+            result.Y = value1.Y + value2.Y;
+            result.Z = value1.Z + value2.Z;
+        }
+
+        public static Vector3 Barycentric(Vector3 value1, Vector3 value2, Vector3 value3, float amount1, float amount2)
+        {
+            return new Vector3(
+                MathHelper.Barycentric(value1.X, value2.X, value3.X, amount1, amount2),
+                MathHelper.Barycentric(value1.Y, value2.Y, value3.Y, amount1, amount2),
+                MathHelper.Barycentric(value1.Z, value2.Z, value3.Z, amount1, amount2));
+        }
+
+        public static void Barycentric(ref Vector3 value1, ref Vector3 value2, ref Vector3 value3, float amount1,
+                                       float amount2, out Vector3 result)
+        {
+            result = new Vector3(
+                MathHelper.Barycentric(value1.X, value2.X, value3.X, amount1, amount2),
+                MathHelper.Barycentric(value1.Y, value2.Y, value3.Y, amount1, amount2),
+                MathHelper.Barycentric(value1.Z, value2.Z, value3.Z, amount1, amount2));
+        }
+
+        public static Vector3 CatmullRom(Vector3 value1, Vector3 value2, Vector3 value3, Vector3 value4, float amount)
+        {
+            return new Vector3(
+                MathHelper.CatmullRom(value1.X, value2.X, value3.X, value4.X, amount),
+                MathHelper.CatmullRom(value1.Y, value2.Y, value3.Y, value4.Y, amount),
+                MathHelper.CatmullRom(value1.Z, value2.Z, value3.Z, value4.Z, amount));
+        }
+
+        public static void CatmullRom(ref Vector3 value1, ref Vector3 value2, ref Vector3 value3, ref Vector3 value4,
+                                      float amount, out Vector3 result)
+        {
+            result = new Vector3(
+                MathHelper.CatmullRom(value1.X, value2.X, value3.X, value4.X, amount),
+                MathHelper.CatmullRom(value1.Y, value2.Y, value3.Y, value4.Y, amount),
+                MathHelper.CatmullRom(value1.Z, value2.Z, value3.Z, value4.Z, amount));
+        }
+
+        public static Vector3 Clamp(Vector3 value1, Vector3 min, Vector3 max)
+        {
+            return new Vector3(
+                MathHelper.Clamp(value1.X, min.X, max.X),
+                MathHelper.Clamp(value1.Y, min.Y, max.Y),
+                MathHelper.Clamp(value1.Z, min.Z, max.Z));
+        }
+
+        public static void Clamp(ref Vector3 value1, ref Vector3 min, ref Vector3 max, out Vector3 result)
+        {
+            result = new Vector3(
+                MathHelper.Clamp(value1.X, min.X, max.X),
+                MathHelper.Clamp(value1.Y, min.Y, max.Y),
+                MathHelper.Clamp(value1.Z, min.Z, max.Z));
+        }
+
+        public static Vector3 Cross(Vector3 vector1, Vector3 vector2)
+        {
+            Cross(ref vector1, ref vector2, out vector1);
+            return vector1;
+        }
+
+        public static void Cross(ref Vector3 vector1, ref Vector3 vector2, out Vector3 result)
+        {
+            result = new Vector3(vector1.Y*vector2.Z - vector2.Y*vector1.Z,
+                                 -(vector1.X*vector2.Z - vector2.X*vector1.Z),
+                                 vector1.X*vector2.Y - vector2.X*vector1.Y);
+        }
+
+        public static float Distance(Vector3 vector1, Vector3 vector2)
+        {
+            float result;
+            DistanceSquared(ref vector1, ref vector2, out result);
+            return (float) Math.Sqrt(result);
+        }
+
+        public static void Distance(ref Vector3 value1, ref Vector3 value2, out float result)
+        {
+            DistanceSquared(ref value1, ref value2, out result);
+            result = (float) Math.Sqrt(result);
+        }
+
+        public static float DistanceSquared(Vector3 value1, Vector3 value2)
+        {
+            float result;
+            DistanceSquared(ref value1, ref value2, out result);
+            return result;
+        }
+
+        public static void DistanceSquared(ref Vector3 value1, ref Vector3 value2, out float result)
+        {
+            result = (value1.X - value2.X)*(value1.X - value2.X) +
+                     (value1.Y - value2.Y)*(value1.Y - value2.Y) +
+                     (value1.Z - value2.Z)*(value1.Z - value2.Z);
+        }
+
+        public static Vector3 Divide(Vector3 value1, Vector3 value2)
+        {
+            value1.X /= value2.X;
+            value1.Y /= value2.Y;
+            value1.Z /= value2.Z;
+            return value1;
+        }
+
+        public static Vector3 Divide(Vector3 value1, float value2)
+        {
+            float factor = 1/value2;
+            value1.X *= factor;
+            value1.Y *= factor;
+            value1.Z *= factor;
+            return value1;
+        }
+
+        public static void Divide(ref Vector3 value1, float divisor, out Vector3 result)
+        {
+            float factor = 1/divisor;
+            result.X = value1.X*factor;
+            result.Y = value1.Y*factor;
+            result.Z = value1.Z*factor;
+        }
+
+        public static void Divide(ref Vector3 value1, ref Vector3 value2, out Vector3 result)
+        {
+            result.X = value1.X/value2.X;
+            result.Y = value1.Y/value2.Y;
+            result.Z = value1.Z/value2.Z;
+        }
+
+        public static float Dot(Vector3 vector1, Vector3 vector2)
+        {
+            return vector1.X*vector2.X + vector1.Y*vector2.Y + vector1.Z*vector2.Z;
+        }
+
+        public static void Dot(ref Vector3 vector1, ref Vector3 vector2, out float result)
+        {
+            result = vector1.X*vector2.X + vector1.Y*vector2.Y + vector1.Z*vector2.Z;
+        }
+
+        public override bool Equals(object obj)
+        {
+            return (obj is Vector3) ? this == (Vector3) obj : false;
+        }
+
+        public bool Equals(Vector3 other)
+        {
+            return this == other;
+        }
+
+        public override int GetHashCode()
+        {
+            return (int) (X + Y + Z);
+        }
+
+        public static Vector3 Hermite(Vector3 value1, Vector3 tangent1, Vector3 value2, Vector3 tangent2, float amount)
+        {
+            Vector3 result = new Vector3();
+            Hermite(ref value1, ref tangent1, ref value2, ref tangent2, amount, out result);
+            return result;
+        }
+
+        public static void Hermite(ref Vector3 value1, ref Vector3 tangent1, ref Vector3 value2, ref Vector3 tangent2,
+                                   float amount, out Vector3 result)
+        {
+            result.X = MathHelper.Hermite(value1.X, tangent1.X, value2.X, tangent2.X, amount);
+            result.Y = MathHelper.Hermite(value1.Y, tangent1.Y, value2.Y, tangent2.Y, amount);
+            result.Z = MathHelper.Hermite(value1.Z, tangent1.Z, value2.Z, tangent2.Z, amount);
+        }
+
+        public float Length()
+        {
+            float result;
+            DistanceSquared(ref this, ref zero, out result);
+            return (float) Math.Sqrt(result);
+        }
+
+        public float LengthSquared()
+        {
+            float result;
+            DistanceSquared(ref this, ref zero, out result);
+            return result;
+        }
+
+        public static Vector3 Lerp(Vector3 value1, Vector3 value2, float amount)
+        {
+            return new Vector3(
+                MathHelper.Lerp(value1.X, value2.X, amount),
+                MathHelper.Lerp(value1.Y, value2.Y, amount),
+                MathHelper.Lerp(value1.Z, value2.Z, amount));
+        }
+
+        public static void Lerp(ref Vector3 value1, ref Vector3 value2, float amount, out Vector3 result)
+        {
+            result = new Vector3(
+                MathHelper.Lerp(value1.X, value2.X, amount),
+                MathHelper.Lerp(value1.Y, value2.Y, amount),
+                MathHelper.Lerp(value1.Z, value2.Z, amount));
+        }
+
+        public static Vector3 Max(Vector3 value1, Vector3 value2)
+        {
+            return new Vector3(
+                MathHelper.Max(value1.X, value2.X),
+                MathHelper.Max(value1.Y, value2.Y),
+                MathHelper.Max(value1.Z, value2.Z));
+        }
+
+        public static void Max(ref Vector3 value1, ref Vector3 value2, out Vector3 result)
+        {
+            result = new Vector3(
+                MathHelper.Max(value1.X, value2.X),
+                MathHelper.Max(value1.Y, value2.Y),
+                MathHelper.Max(value1.Z, value2.Z));
+        }
+
+        public static Vector3 Min(Vector3 value1, Vector3 value2)
+        {
+            return new Vector3(
+                MathHelper.Min(value1.X, value2.X),
+                MathHelper.Min(value1.Y, value2.Y),
+                MathHelper.Min(value1.Z, value2.Z));
+        }
+
+        public static void Min(ref Vector3 value1, ref Vector3 value2, out Vector3 result)
+        {
+            result = new Vector3(
+                MathHelper.Min(value1.X, value2.X),
+                MathHelper.Min(value1.Y, value2.Y),
+                MathHelper.Min(value1.Z, value2.Z));
+        }
+
+        public static Vector3 Multiply(Vector3 value1, Vector3 value2)
+        {
+            value1.X *= value2.X;
+            value1.Y *= value2.Y;
+            value1.Z *= value2.Z;
+            return value1;
+        }
+
+        public static Vector3 Multiply(Vector3 value1, float scaleFactor)
+        {
+            value1.X *= scaleFactor;
+            value1.Y *= scaleFactor;
+            value1.Z *= scaleFactor;
+            return value1;
+        }
+
+        public static void Multiply(ref Vector3 value1, float scaleFactor, out Vector3 result)
+        {
+            result.X = value1.X*scaleFactor;
+            result.Y = value1.Y*scaleFactor;
+            result.Z = value1.Z*scaleFactor;
+        }
+
+        public static void Multiply(ref Vector3 value1, ref Vector3 value2, out Vector3 result)
+        {
+            result.X = value1.X*value2.X;
+            result.Y = value1.Y*value2.Y;
+            result.Z = value1.Z*value2.Z;
+        }
+
+        public static Vector3 Negate(Vector3 value)
+        {
+            value = new Vector3(-value.X, -value.Y, -value.Z);
+            return value;
+        }
+
+        public static void Negate(ref Vector3 value, out Vector3 result)
+        {
+            result = new Vector3(-value.X, -value.Y, -value.Z);
+        }
+
+        public void Normalize()
+        {
+            Normalize(ref this, out this);
+        }
+
+        public static Vector3 Normalize(Vector3 vector)
+        {
+            Normalize(ref vector, out vector);
+            return vector;
+        }
+
+        public static void Normalize(ref Vector3 value, out Vector3 result)
+        {
+            float factor;
+            Distance(ref value, ref zero, out factor);
+            factor = 1f/factor;
+            result.X = value.X*factor;
+            result.Y = value.Y*factor;
+            result.Z = value.Z*factor;
+        }
+
+        public static Vector3 Reflect(Vector3 vector, Vector3 normal)
+        {
+            Vector3 result;
+            Reflect(ref vector, ref normal, out result);
+            return result;
+        }
+
+        public static void Reflect(ref Vector3 vector, ref Vector3 normal, out Vector3 result)
+        {
+            float dot = Dot(vector, normal);
+            result.X = vector.X - ((2f*dot)*normal.X);
+            result.Y = vector.Y - ((2f*dot)*normal.Y);
+            result.Z = vector.Z - ((2f*dot)*normal.Z);
+        }
+
+        public static Vector3 SmoothStep(Vector3 value1, Vector3 value2, float amount)
+        {
+            return new Vector3(
+                MathHelper.SmoothStep(value1.X, value2.X, amount),
+                MathHelper.SmoothStep(value1.Y, value2.Y, amount),
+                MathHelper.SmoothStep(value1.Z, value2.Z, amount));
+        }
+
+        public static void SmoothStep(ref Vector3 value1, ref Vector3 value2, float amount, out Vector3 result)
+        {
+            result = new Vector3(
+                MathHelper.SmoothStep(value1.X, value2.X, amount),
+                MathHelper.SmoothStep(value1.Y, value2.Y, amount),
+                MathHelper.SmoothStep(value1.Z, value2.Z, amount));
+        }
+
+        public static Vector3 Subtract(Vector3 value1, Vector3 value2)
+        {
+            value1.X -= value2.X;
+            value1.Y -= value2.Y;
+            value1.Z -= value2.Z;
+            return value1;
+        }
+
+        public static void Subtract(ref Vector3 value1, ref Vector3 value2, out Vector3 result)
+        {
+            result.X = value1.X - value2.X;
+            result.Y = value1.Y - value2.Y;
+            result.Z = value1.Z - value2.Z;
+        }
+
+        public override string ToString()
+        {
+            StringBuilder sb = new StringBuilder(32);
+            sb.Append("{X:");
+            sb.Append(X);
+            sb.Append(" Y:");
+            sb.Append(Y);
+            sb.Append(" Z:");
+            sb.Append(Z);
+            sb.Append("}");
+            return sb.ToString();
+        }
+
+        public static Vector3 Transform(Vector3 position, Matrix matrix)
+        {
+            Transform(ref position, ref matrix, out position);
+            return position;
+        }
+
+        public static void Transform(ref Vector3 position, ref Matrix matrix, out Vector3 result)
+        {
+            result =
+                new Vector3((position.X*matrix.M11) + (position.Y*matrix.M21) + (position.Z*matrix.M31) + matrix.M41,
+                            (position.X*matrix.M12) + (position.Y*matrix.M22) + (position.Z*matrix.M32) + matrix.M42,
+                            (position.X*matrix.M13) + (position.Y*matrix.M23) + (position.Z*matrix.M33) + matrix.M43);
+        }
+
+        public static void Transform(Vector3[] sourceArray, ref Matrix matrix, Vector3[] destinationArray)
+        {
+            throw new NotImplementedException();
+        }
+
+        public static void Transform(Vector3[] sourceArray, int sourceIndex, ref Matrix matrix,
+                                     Vector3[] destinationArray, int destinationIndex, int length)
+        {
+            throw new NotImplementedException();
+        }
+
+        public static void TransformNormal(Vector3[] sourceArray, ref Matrix matrix, Vector3[] destinationArray)
+        {
+            throw new NotImplementedException();
+        }
+
+        public static void TransformNormal(Vector3[] sourceArray, int sourceIndex, ref Matrix matrix,
+                                           Vector3[] destinationArray, int destinationIndex, int length)
+        {
+            throw new NotImplementedException();
+        }
+
+        public static Vector3 TransformNormal(Vector3 normal, Matrix matrix)
+        {
+            TransformNormal(ref normal, ref matrix, out normal);
+            return normal;
+        }
+
+        public static void TransformNormal(ref Vector3 normal, ref Matrix matrix, out Vector3 result)
+        {
+            result = new Vector3((normal.X*matrix.M11) + (normal.Y*matrix.M21) + (normal.Z*matrix.M31),
+                                 (normal.X*matrix.M12) + (normal.Y*matrix.M22) + (normal.Z*matrix.M32),
+                                 (normal.X*matrix.M13) + (normal.Y*matrix.M23) + (normal.Z*matrix.M33));
+        }
+
+#endregion Public methods
+
+#region Operators
+
+        public static bool operator ==(Vector3 value1, Vector3 value2)
+        {
+            return value1.X == value2.X
+                   && value1.Y == value2.Y
+                   && value1.Z == value2.Z;
+        }
+
+        public static bool operator !=(Vector3 value1, Vector3 value2)
+        {
+            return !(value1 == value2);
+        }
+
+        public static Vector3 operator +(Vector3 value1, Vector3 value2)
+        {
+            value1.X += value2.X;
+            value1.Y += value2.Y;
+            value1.Z += value2.Z;
+            return value1;
+        }
+
+        public static Vector3 operator -(Vector3 value)
+        {
+            value = new Vector3(-value.X, -value.Y, -value.Z);
+            return value;
+        }
+
+        public static Vector3 operator -(Vector3 value1, Vector3 value2)
+        {
+            value1.X -= value2.X;
+            value1.Y -= value2.Y;
+            value1.Z -= value2.Z;
+            return value1;
+        }
+
+        public static Vector3 operator *(Vector3 value1, Vector3 value2)
+        {
+            value1.X *= value2.X;
+            value1.Y *= value2.Y;
+            value1.Z *= value2.Z;
+            return value1;
+        }
+
+        public static Vector3 operator *(Vector3 value, float scaleFactor)
+        {
+            value.X *= scaleFactor;
+            value.Y *= scaleFactor;
+            value.Z *= scaleFactor;
+            return value;
+        }
+
+        public static Vector3 operator *(float scaleFactor, Vector3 value)
+        {
+            value.X *= scaleFactor;
+            value.Y *= scaleFactor;
+            value.Z *= scaleFactor;
+            return value;
+        }
+
+        public static Vector3 operator /(Vector3 value1, Vector3 value2)
+        {
+            value1.X /= value2.X;
+            value1.Y /= value2.Y;
+            value1.Z /= value2.Z;
+            return value1;
+        }
+
+        public static Vector3 operator /(Vector3 value, float divider)
+        {
+            float factor = 1/divider;
+            value.X *= factor;
+            value.Y *= factor;
+            value.Z *= factor;
+            return value;
+        }
+
+#endregion
+    }
+}
+
+#endif

Farseer Physics Engine 3.5/Common/Vertices.cs

@@ -0,0 +1,582 @@
+/*
+* Farseer Physics Engine:
+* Copyright (c) 2012 Ian Qvist
+*/
+
+using System;
+using System.Collections.Generic;
+using System.Diagnostics;
+using System.Text;
+using FarseerPhysics.Collision;
+using Microsoft.Xna.Framework;
+
+namespace FarseerPhysics.Common
+{
+    public enum PolygonError
+    {
+        /// <summary>
+        /// There were no errors in the polygon
+        /// </summary>
+        NoError,
+
+        /// <summary>
+        /// Polygon must have between 3 and Settings.MaxPolygonVertices vertices.
+        /// </summary>
+        InvalidAmountOfVertices,
+
+        /// <summary>
+        /// Polygon must be simple. This means no overlapping edges.
+        /// </summary>
+        NotSimple,
+
+        /// <summary>
+        /// Polygon must have a counter clockwise winding.
+        /// </summary>
+        NotCounterClockWise,
+
+        /// <summary>
+        /// The polygon is concave, it needs to be convex.
+        /// </summary>
+        NotConvex,
+
+        /// <summary>
+        /// Polygon area is too small.
+        /// </summary>
+        AreaTooSmall,
+
+        /// <summary>
+        /// The polygon has a side that is too short.
+        /// </summary>
+        SideTooSmall
+    }
+
+#if !(XBOX360)
+    [DebuggerDisplay("Count = {Count} Vertices = {ToString()}")]
+#endif
+    public class Vertices : List<Vector2>
+    {
+        public Vertices() { }
+
+        public Vertices(int capacity) : base(capacity) { }
+
+        public Vertices(IEnumerable<Vector2> vertices)
+        {
+            AddRange(vertices);
+        }
+
+        internal bool AttachedToBody { get; set; }
+
+        /// <summary>
+        /// You can add holes to this collection.
+        /// It will get respected by some of the triangulation algoithms, but otherwise not used.
+        /// </summary>
+        public List<Vertices> Holes { get; set; }
+
+        /// <summary>
+        /// Gets the next index. Used for iterating all the edges with wrap-around.
+        /// </summary>
+        /// <param name="index">The current index</param>
+        public int NextIndex(int index)
+        {
+            return (index + 1 > Count - 1) ? 0 : index + 1;
+        }
+
+        /// <summary>
+        /// Gets the next vertex. Used for iterating all the edges with wrap-around.
+        /// </summary>
+        /// <param name="index">The current index</param>
+        public Vector2 NextVertex(int index)
+        {
+            return this[NextIndex(index)];
+        }
+
+        /// <summary>
+        /// Gets the previous index. Used for iterating all the edges with wrap-around.
+        /// </summary>
+        /// <param name="index">The current index</param>
+        public int PreviousIndex(int index)
+        {
+            return index - 1 < 0 ? Count - 1 : index - 1;
+        }
+
+        /// <summary>
+        /// Gets the previous vertex. Used for iterating all the edges with wrap-around.
+        /// </summary>
+        /// <param name="index">The current index</param>
+        public Vector2 PreviousVertex(int index)
+        {
+            return this[PreviousIndex(index)];
+        }
+
+        /// <summary>
+        /// Gets the signed area.
+        /// If the area is less than 0, it indicates that the polygon is clockwise winded.
+        /// </summary>
+        /// <returns>The signed area</returns>
+        public float GetSignedArea()
+        {
+            //The simplest polygon which can exist in the Euclidean plane has 3 sides.
+            if (Count < 3)
+                return 0;
+
+            int i;
+            float area = 0;
+
+            for (i = 0; i < Count; i++)
+            {
+                int j = (i + 1) % Count;
+
+                Vector2 vi = this[i];
+                Vector2 vj = this[j];
+
+                area += vi.X * vj.Y;
+                area -= vi.Y * vj.X;
+            }
+            area /= 2.0f;
+            return area;
+        }
+
+        /// <summary>
+        /// Gets the area.
+        /// </summary>
+        /// <returns></returns>
+        public float GetArea()
+        {
+            float area = GetSignedArea();
+            return (area < 0 ? -area : area);
+        }
+
+        /// <summary>
+        /// Gets the centroid.
+        /// </summary>
+        /// <returns></returns>
+        public Vector2 GetCentroid()
+        {
+            //The simplest polygon which can exist in the Euclidean plane has 3 sides.
+            if (Count < 3)
+                return new Vector2(float.NaN, float.NaN);
+
+            // Same algorithm is used by Box2D
+            Vector2 c = Vector2.Zero;
+            float area = 0.0f;
+            const float inv3 = 1.0f / 3.0f;
+
+            for (int i = 0; i < Count; ++i)
+            {
+                // Triangle vertices.
+                Vector2 current = this[i];
+                Vector2 next = (i + 1 < Count ? this[i + 1] : this[0]);
+
+                float triangleArea = 0.5f * (current.X * next.Y - current.Y * next.X);
+                area += triangleArea;
+
+                // Area weighted centroid
+                c += triangleArea * inv3 * (current + next);
+            }
+
+            // Centroid
+            c *= 1.0f / area;
+            return c;
+        }
+
+        /// <summary>
+        /// Returns an AABB that fully contains this polygon.
+        /// </summary>
+        public AABB GetAABB()
+        {
+            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;
+        }
+
+        /// <summary>
+        /// Translates the vertices with the specified vector.
+        /// </summary>
+        /// <param name="value">The value.</param>
+        public void Translate(Vector2 value)
+        {
+            Translate(ref value);
+        }
+
+        /// <summary>
+        /// Translates the vertices with the specified vector.
+        /// </summary>
+        /// <param name="value">The vector.</param>
+        public void Translate(ref Vector2 value)
+        {
+            Debug.Assert(!AttachedToBody, "Translating vertices that are used by a Body can result in unstable behavior. Use Body.Position instead.");
+
+            for (int i = 0; i < Count; i++)
+                this[i] = Vector2.Add(this[i], value);
+
+            if (Holes != null && Holes.Count > 0)
+            {
+                foreach (Vertices hole in Holes)
+                {
+                    hole.Translate(ref value);
+                }
+            }
+        }
+
+        /// <summary>
+        /// Scales the vertices with the specified vector.
+        /// </summary>
+        /// <param name="value">The Value.</param>
+        public void Scale(Vector2 value)
+        {
+            Scale(ref value);
+        }
+
+        /// <summary>
+        /// Scales the vertices with the specified vector.
+        /// </summary>
+        /// <param name="value">The Value.</param>
+        public void Scale(ref Vector2 value)
+        {
+            Debug.Assert(!AttachedToBody, "Scaling vertices that are used by a Body can result in unstable behavior.");
+
+            for (int i = 0; i < Count; i++)
+                this[i] = Vector2.Multiply(this[i], value);
+
+            if (Holes != null && Holes.Count > 0)
+            {
+                foreach (Vertices hole in Holes)
+                {
+                    hole.Scale(ref value);
+                }
+            }
+        }
+
+        /// <summary>
+        /// Rotate the vertices with the defined value in radians.
+        /// 
+        /// Warning: Using this method on an active set of vertices of a Body,
+        /// will cause problems with collisions. Use Body.Rotation instead.
+        /// </summary>
+        /// <param name="value">The amount to rotate by in radians.</param>
+        public void Rotate(float value)
+        {
+            Debug.Assert(!AttachedToBody, "Rotating vertices that are used by a Body can result in unstable behavior.");
+
+            float num1 = (float)Math.Cos(value);
+            float num2 = (float)Math.Sin(value);
+
+            for (int i = 0; i < Count; i++)
+            {
+                Vector2 position = this[i];
+                this[i] = new Vector2((position.X * num1 + position.Y * -num2), (position.X * num2 + position.Y * num1));
+            }
+
+            if (Holes != null && Holes.Count > 0)
+            {
+                foreach (Vertices hole in Holes)
+                {
+                    hole.Rotate(value);
+                }
+            }
+        }
+
+        /// <summary>
+        /// Determines whether the polygon is convex.
+        /// O(n^2) running time.
+        /// 
+        /// Assumptions:
+        /// - The polygon is in counter clockwise order
+        /// - The polygon has no overlapping edges
+        /// </summary>
+        /// <returns>
+        /// 	<c>true</c> if it is convex; otherwise, <c>false</c>.
+        /// </returns>
+        public bool IsConvex()
+        {
+            //The simplest polygon which can exist in the Euclidean plane has 3 sides.
+            if (Count < 3)
+                return false;
+
+            //Triangles are always convex
+            if (Count == 3)
+                return true;
+
+            // Checks the polygon is convex and the interior is to the left of each edge.
+            for (int i = 0; i < Count; ++i)
+            {
+                int next = i + 1 < Count ? i + 1 : 0;
+                Vector2 edge = this[next] - this[i];
+
+                for (int j = 0; j < Count; ++j)
+                {
+                    // Don't check vertices on the current edge.
+                    if (j == i || j == next)
+                        continue;
+
+                    Vector2 r = this[j] - this[i];
+
+                    float s = edge.X * r.Y - edge.Y * r.X;
+
+                    if (s <= 0.0f)
+                        return false;
+                }
+            }
+            return true;
+        }
+
+        /// <summary>
+        /// Indicates if the vertices are in counter clockwise order.
+        /// Warning: If the area of the polygon is 0, it is unable to determine the winding.
+        /// </summary>
+        public bool IsCounterClockWise()
+        {
+            //The simplest polygon which can exist in the Euclidean plane has 3 sides.
+            if (Count < 3)
+                return false;
+
+            return (GetSignedArea() > 0.0f);
+        }
+
+        /// <summary>
+        /// Forces the vertices to be counter clock wise order.
+        /// </summary>
+        public void ForceCounterClockWise()
+        {
+            //The simplest polygon which can exist in the Euclidean plane has 3 sides.
+            if (Count < 3)
+                return;
+
+            if (!IsCounterClockWise())
+                Reverse();
+        }
+
+        /// <summary>
+        /// Checks if the vertices forms an simple polygon by checking for edge crossings.
+        /// </summary>
+        public bool IsSimple()
+        {
+            //The simplest polygon which can exist in the Euclidean plane has 3 sides.
+            if (Count < 3)
+                return false;
+
+            for (int i = 0; i < Count; ++i)
+            {
+                Vector2 a1 = this[i];
+                Vector2 a2 = NextVertex(i);
+                for (int j = i + 1; j < Count; ++j)
+                {
+                    Vector2 b1 = this[j];
+                    Vector2 b2 = NextVertex(j);
+
+                    Vector2 temp;
+
+                    if (LineTools.LineIntersect2(ref a1, ref a2, ref b1, ref b2, out temp))
+                        return false;
+                }
+            }
+            return true;
+        }
+
+        /// <summary>
+        /// Checks if the polygon is valid for use in the engine.
+        ///
+        /// Performs a full check, for simplicity, convexity,
+        /// orientation, minimum angle, and volume.
+        /// 
+        /// From Eric Jordan's convex decomposition library
+        /// </summary>
+        /// <returns>PolygonError.NoError if there were no error.</returns>
+        public PolygonError CheckPolygon()
+        {
+            if (Count < 3 || Count > Settings.MaxPolygonVertices)
+                return PolygonError.InvalidAmountOfVertices;
+
+            if (!IsSimple())
+                return PolygonError.NotSimple;
+
+            if (GetArea() <= Settings.Epsilon)
+                return PolygonError.AreaTooSmall;
+
+            if (!IsConvex())
+                return PolygonError.NotConvex;
+
+            //Check if the sides are of adequate length.
+            for (int i = 0; i < Count; ++i)
+            {
+                int next = i + 1 < Count ? i + 1 : 0;
+                Vector2 edge = this[next] - this[i];
+                if (edge.LengthSquared() <= Settings.Epsilon*Settings.Epsilon)
+                {
+                    return PolygonError.SideTooSmall;
+                }
+            }
+
+            if (!IsCounterClockWise())
+                return PolygonError.NotCounterClockWise;
+
+            return PolygonError.NoError;
+        }
+
+        /// <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;
+        }
+
+        /// <summary>
+        /// Transforms the polygon using the defined matrix.
+        /// </summary>
+        /// <param name="transform">The matrix to use as transformation.</param>
+        public void Transform(ref Matrix transform)
+        {
+            // Transform main polygon
+            for (int i = 0; i < Count; i++)
+                this[i] = Vector2.Transform(this[i], transform);
+
+            // Transform holes
+            if (Holes != null && Holes.Count > 0)
+            {
+                for (int i = 0; i < Holes.Count; i++)
+                {
+                    Vector2[] temp = Holes[i].ToArray();
+                    Vector2.Transform(temp, ref transform, temp);
+
+                    Holes[i] = new Vertices(temp);
+                }
+            }
+        }
+
+        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();
+        }
+    }
+}

Farseer Physics Engine 3.5/Controllers/AbstractForceController.cs

@@ -0,0 +1,323 @@
+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);
+    }
+}

Farseer Physics Engine 3.5/Controllers/BuoyancyController.cs

@@ -0,0 +1,134 @@
+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(ref _normal, _offset, ref 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);
+            }
+        }
+    }
+}

Farseer Physics Engine 3.5/Controllers/Controller.cs

@@ -0,0 +1,72 @@
+using System;
+using FarseerPhysics.Common.PhysicsLogic;
+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);
+    }
+}

Farseer Physics Engine 3.5/Controllers/GravityController.cs

@@ -0,0 +1,110 @@
+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 GravityController(float strength)
+            : base(ControllerType.GravityController)
+        {
+            Strength = strength;
+            MaxRadius = float.MaxValue;
+            GravityType = GravityType.DistanceSquared;
+            Points = new List<Vector2>();
+            Bodies = new List<Body>();
+        }
+
+        public GravityController(float strength, float maxRadius, float minRadius)
+            : base(ControllerType.GravityController)
+        {
+            MinRadius = minRadius;
+            MaxRadius = maxRadius;
+            Strength = strength;
+            GravityType = GravityType.DistanceSquared;
+            Points = new List<Vector2>();
+            Bodies = new List<Body>();
+        }
+
+        public float MinRadius { get; set; }
+        public float MaxRadius { get; set; }
+        public float Strength { get; set; }
+        public GravityType GravityType { get; set; }
+        public List<Body> Bodies { get; set; }
+        public List<Vector2> Points { get; set; }
+
+        public override void Update(float dt)
+        {
+            Vector2 f = Vector2.Zero;
+
+            foreach (Body worldBody in World.BodyList)
+            {
+                if (!IsActiveOn(worldBody))
+                    continue;
+
+                foreach (Body controllerBody in Bodies)
+                {
+                    if (worldBody == controllerBody || (worldBody.IsStatic && controllerBody.IsStatic) || !controllerBody.Enabled)
+                        continue;
+
+                    Vector2 d = controllerBody.Position - worldBody.Position;
+                    float r2 = d.LengthSquared();
+
+                    if (r2 <= Settings.Epsilon || r2 > MaxRadius * MaxRadius || r2 < MinRadius * MinRadius)
+                        continue;
+
+                    switch (GravityType)
+                    {
+                        case GravityType.DistanceSquared:
+                            f = Strength / r2 * worldBody.Mass * controllerBody.Mass * d;
+                            break;
+                        case GravityType.Linear:
+                            f = Strength / (float)Math.Sqrt(r2) * worldBody.Mass * controllerBody.Mass * d;
+                            break;
+                    }
+
+                    worldBody.ApplyForce(ref f);
+                }
+
+                foreach (Vector2 point in Points)
+                {
+                    Vector2 d = point - worldBody.Position;
+                    float r2 = d.LengthSquared();
+
+                    if (r2 <= Settings.Epsilon || r2 > MaxRadius * MaxRadius || r2 < MinRadius * MinRadius)
+                        continue;
+
+                    switch (GravityType)
+                    {
+                        case GravityType.DistanceSquared:
+                            f = Strength / r2 * worldBody.Mass * d;
+                            break;
+                        case GravityType.Linear:
+                            f = Strength / (float)Math.Sqrt(r2) * worldBody.Mass * d;
+                            break;
+                    }
+
+                    worldBody.ApplyForce(ref f);
+                }
+            }
+        }
+
+        public void AddBody(Body body)
+        {
+            Bodies.Add(body);
+        }
+
+        public void AddPoint(Vector2 point)
+        {
+            Points.Add(point);
+        }
+    }
+}

Farseer Physics Engine 3.5/Controllers/SimpleWindForce.cs

@@ -0,0 +1,75 @@
+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);
+                    }
+                }
+            }
+        }
+    }
+}

Farseer Physics Engine 3.5/Controllers/VelocityLimitController.cs

@@ -0,0 +1,129 @@
+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._linearVelocity.X;
+                    float translationY = dt * body._linearVelocity.Y;
+                    float result = translationX * translationX + translationY * translationY;
+
+                    if (result > dt * _maxLinearSqared)
+                    {
+                        float sq = (float)Math.Sqrt(result);
+
+                        float ratio = _maxLinearVelocity / sq;
+                        body._linearVelocity.X *= ratio;
+                        body._linearVelocity.Y *= ratio;
+                    }
+                }
+
+                if (LimitAngularVelocity)
+                {
+                    //Rotation
+                    float rotation = dt * body._angularVelocity;
+                    if (rotation * rotation > _maxAngularSqared)
+                    {
+                        float ratio = _maxAngularVelocity / Math.Abs(rotation);
+                        body._angularVelocity *= ratio;
+                    }
+                }
+            }
+        }
+
+        public void AddBody(Body body)
+        {
+            _bodies.Add(body);
+        }
+
+        public void RemoveBody(Body body)
+        {
+            _bodies.Remove(body);
+        }
+    }
+}

Farseer Physics Engine 3.5/ConvertUnits.cs

@@ -0,0 +1,108 @@
+/*
+* Farseer Physics Engine:
+* Copyright (c) 2012 Ian Qvist
+*/
+
+using Microsoft.Xna.Framework;
+
+namespace FarseerPhysics
+{
+    /// <summary>
+    /// Convert units between display and simulation units.
+    /// </summary>
+    public static class ConvertUnits
+    {
+        private static float _displayUnitsToSimUnitsRatio = 100f;
+        private static float _simUnitsToDisplayUnitsRatio = 1 / _displayUnitsToSimUnitsRatio;
+
+        public static void SetDisplayUnitToSimUnitRatio(float displayUnitsPerSimUnit)
+        {
+            _displayUnitsToSimUnitsRatio = displayUnitsPerSimUnit;
+            _simUnitsToDisplayUnitsRatio = 1 / displayUnitsPerSimUnit;
+        }
+
+        public static float ToDisplayUnits(float simUnits)
+        {
+            return simUnits * _displayUnitsToSimUnitsRatio;
+        }
+
+        public static float ToDisplayUnits(int simUnits)
+        {
+            return simUnits * _displayUnitsToSimUnitsRatio;
+        }
+
+        public static Vector2 ToDisplayUnits(Vector2 simUnits)
+        {
+            return simUnits * _displayUnitsToSimUnitsRatio;
+        }
+
+        public static void ToDisplayUnits(ref Vector2 simUnits, out Vector2 displayUnits)
+        {
+            Vector2.Multiply(ref simUnits, _displayUnitsToSimUnitsRatio, out displayUnits);
+        }
+
+        public static Vector3 ToDisplayUnits(Vector3 simUnits)
+        {
+            return simUnits * _displayUnitsToSimUnitsRatio;
+        }
+
+        public static Vector2 ToDisplayUnits(float x, float y)
+        {
+            return new Vector2(x, y) * _displayUnitsToSimUnitsRatio;
+        }
+
+        public static void ToDisplayUnits(float x, float y, out Vector2 displayUnits)
+        {
+            displayUnits = Vector2.Zero;
+            displayUnits.X = x * _displayUnitsToSimUnitsRatio;
+            displayUnits.Y = y * _displayUnitsToSimUnitsRatio;
+        }
+
+        public static float ToSimUnits(float displayUnits)
+        {
+            return displayUnits * _simUnitsToDisplayUnitsRatio;
+        }
+
+        public static float ToSimUnits(double displayUnits)
+        {
+            return (float)displayUnits * _simUnitsToDisplayUnitsRatio;
+        }
+
+        public static float ToSimUnits(int displayUnits)
+        {
+            return displayUnits * _simUnitsToDisplayUnitsRatio;
+        }
+
+        public static Vector2 ToSimUnits(Vector2 displayUnits)
+        {
+            return displayUnits * _simUnitsToDisplayUnitsRatio;
+        }
+
+        public static Vector3 ToSimUnits(Vector3 displayUnits)
+        {
+            return displayUnits * _simUnitsToDisplayUnitsRatio;
+        }
+
+        public static void ToSimUnits(ref Vector2 displayUnits, out Vector2 simUnits)
+        {
+            Vector2.Multiply(ref displayUnits, _simUnitsToDisplayUnitsRatio, out simUnits);
+        }
+
+        public static Vector2 ToSimUnits(float x, float y)
+        {
+            return new Vector2(x, y) * _simUnitsToDisplayUnitsRatio;
+        }
+
+        public static Vector2 ToSimUnits(double x, double y)
+        {
+            return new Vector2((float)x, (float)y) * _simUnitsToDisplayUnitsRatio;
+        }
+
+        public static void ToSimUnits(float x, float y, out Vector2 simUnits)
+        {
+            simUnits = Vector2.Zero;
+            simUnits.X = x * _simUnitsToDisplayUnitsRatio;
+            simUnits.Y = y * _simUnitsToDisplayUnitsRatio;
+        }
+    }
+}

Farseer Physics Engine 3.5/DebugViewBase.cs

@@ -0,0 +1,165 @@
+/*
+* Farseer Physics Engine:
+* Copyright (c) 2012 Ian Qvist
+*/
+
+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 DebugViewBase
+    {
+        protected DebugViewBase(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, bool closed = true);
+
+        /// <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);
+    }
+}

Farseer Physics Engine 3.5/Dynamics/BreakableBody.cs

@@ -0,0 +1,148 @@
+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
+    {
+        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)
+        {
+            _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);
+                Parts.Add(fixture);
+            }
+        }
+
+        public BreakableBody(IEnumerable<Shape> shapes, World world)
+        {
+            _world = world;
+            _world.ContactManager.PostSolve += PostSolve;
+            MainBody = new Body(_world);
+            MainBody.BodyType = BodyType.Dynamic;
+
+            foreach (Shape part in shapes)
+            {
+                Fixture fixture = MainBody.CreateFixture(part);
+                Parts.Add(fixture);
+            }
+        }
+
+        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 void PostSolve(Contact contact, ContactVelocityConstraint 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 oldFixture = Parts[i];
+
+                Shape shape = oldFixture.Shape.Clone();
+                object userData = oldFixture.UserData;
+
+                MainBody.DestroyFixture(oldFixture);
+
+                Body body = BodyFactory.CreateBody(_world);
+                body.BodyType = BodyType.Dynamic;
+                body.Position = MainBody.Position;
+                body.Rotation = MainBody.Rotation;
+                body.UserData = MainBody.UserData;
+
+                Fixture newFixture = body.CreateFixture(shape);
+                newFixture.UserData = userData;
+                Parts[i] = newFixture;
+
+                body.AngularVelocity = _angularVelocitiesCache[i];
+                body.LinearVelocity = _velocitiesCache[i];
+            }
+
+            _world.RemoveBody(MainBody);
+            _world.RemoveBreakableBody(this);
+        }
+
+        public void Break()
+        {
+            _break = true;
+        }
+    }
+}

Farseer Physics Engine 3.5/Dynamics/ContactManager.cs

@@ -0,0 +1,445 @@
+/*
+* Farseer Physics Engine:
+* Copyright (c) 2012 Ian Qvist
+* 
+* Original source Box2D:
+* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org 
+* 
+* 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. 
+*/
+//#define USE_ACTIVE_CONTACT_SET
+
+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);
+
+#if USE_ACTIVE_CONTACT_SET
+        /// <summary>
+        /// The set of active contacts.
+        /// </summary>
+		public HashSet<Contact> ActiveContacts = new HashSet<Contact>();
+
+        /// <summary>
+        /// A temporary copy of active contacts that is used during updates so
+		/// the hash set can have members added/removed during the update.
+		/// This list is cleared after every update.
+        /// </summary>
+		List<Contact> ActiveList = new List<Contact>();
+#endif
+
+        /// <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;
+
+            //FPE feature: BeforeCollision delegate
+            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);
+
+            if (c == null)
+                return;
+
+            // Contact creation may swap fixtures.
+            fixtureA = c.FixtureA;
+            fixtureB = c.FixtureB;
+            bodyA = fixtureA.Body;
+            bodyB = fixtureB.Body;
+
+            // Insert into the world.
+            ContactList.Add(c);
+
+#if USE_ACTIVE_CONTACT_SET
+			ActiveContacts.Add(c);
+#endif
+            // 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;
+
+            // Wake up the bodies
+            if (fixtureA.IsSensor == false && fixtureB.IsSensor == false)
+            {
+                bodyA.Awake = true;
+                bodyB.Awake = true;
+            }
+        }
+
+        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 (contact.IsTouching)
+            {
+                //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 (EndContact != null)
+                    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;
+            }
+
+#if USE_ACTIVE_CONTACT_SET
+			if (ActiveContacts.Contains(contact))
+			{
+				ActiveContacts.Remove(contact);
+			}
+#endif
+            contact.Destroy();
+        }
+
+        internal void Collide()
+        {
+            // Update awake contacts.
+#if USE_ACTIVE_CONTACT_SET
+			ActiveList.AddRange(ActiveContacts);
+
+			foreach (var c in ActiveList)
+			{
+#else
+            for (int i = 0; i < ContactList.Count; i++)
+            {
+                Contact c = ContactList[i];
+#endif
+                Fixture fixtureA = c.FixtureA;
+                Fixture fixtureB = c.FixtureB;
+                int indexA = c.ChildIndexA;
+                int indexB = c.ChildIndexB;
+                Body bodyA = fixtureA.Body;
+                Body bodyB = fixtureB.Body;
+
+                //Do no try to collide disabled bodies
+                if (!bodyA.Enabled || !bodyB.Enabled)
+                    continue;
+
+                // Is this contact flagged for filtering?
+                if (c.FilterFlag)
+                {
+                    // 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.FilterFlag = false;
+                }
+
+                bool activeA = bodyA.Awake && bodyA.BodyType != BodyType.Static;
+                bool activeB = bodyB.Awake && bodyB.BodyType != BodyType.Static;
+
+                // At least one body must be awake and it must be dynamic or kinematic.
+                if (activeA == false && activeB == false)
+                {
+#if USE_ACTIVE_CONTACT_SET
+					ActiveContacts.Remove(c);
+#endif
+                    continue;
+                }
+
+                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);
+            }
+
+#if USE_ACTIVE_CONTACT_SET
+			ActiveList.Clear();
+#endif
+        }
+
+        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;
+        }
+
+        internal void UpdateContacts(ContactEdge contactEdge, bool value)
+        {
+#if USE_ACTIVE_CONTACT_SET
+			if(value)
+			{
+				while(contactEdge != null)
+				{
+					var c = contactEdge.Contact;
+					if (!ActiveContacts.Contains(c))
+					{
+						ActiveContacts.Add(c);
+					}
+					contactEdge = contactEdge.Next;
+				}
+			}
+			else
+			{
+				while (contactEdge != null)
+				{
+					var c = contactEdge.Contact;
+					if (!contactEdge.Other.Awake)
+					{
+						if (ActiveContacts.Contains(c))
+						{
+							ActiveContacts.Remove(c);
+						}
+					}
+					contactEdge = contactEdge.Next;
+				}
+			}
+#endif
+        }
+
+#if USE_ACTIVE_CONTACT_SET
+		internal void RemoveActiveContact(Contact contact)
+		{
+			if (ActiveContacts.Contains(contact))
+				ActiveContacts.Remove(contact);
+		}
+#endif
+    }
+}

Farseer Physics Engine 3.5/Dynamics/Contacts/Contact.cs

@@ -0,0 +1,480 @@
+/*
+* Farseer Physics Engine:
+* Copyright (c) 2012 Ian Qvist
+* 
+* Original source Box2D:
+* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org 
+* 
+* 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. 
+*/
+//#define USE_ACTIVE_CONTACT_SET
+
+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;
+    }
+
+    /// <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 ContactType _type;
+
+        private static EdgeShape _edge = new EdgeShape();
+
+        private static ContactType[,] _registers = new[,]
+                                                       {
+                                                           {
+                                                               ContactType.Circle,
+                                                               ContactType.EdgeAndCircle,
+                                                               ContactType.PolygonAndCircle,
+                                                               ContactType.ChainAndCircle,
+                                                           },
+                                                           {
+                                                               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.ChainAndPolygon,
+                                                           },
+                                                           {
+                                                               ContactType.ChainAndCircle,
+                                                               ContactType.NotSupported,
+                                                               // 3,1 is invalid (no ContactType.EdgeAndLoop)
+                                                               ContactType.ChainAndPolygon,
+                                                               ContactType.NotSupported,
+                                                               // 3,3 is invalid (no ContactType.Loop)
+                                                           },
+                                                       };
+        // Nodes for connecting bodies.
+        internal ContactEdge _nodeA = new ContactEdge();
+        internal ContactEdge _nodeB = new ContactEdge();
+        internal int _toiCount;
+        internal float _toi;
+
+        public Fixture FixtureA;
+        public Fixture FixtureB;
+        public float Friction { get; set; }
+        public float Restitution { get; set; }
+
+        /// <summary>
+        /// Get the contact manifold. Do not modify the manifold unless you understand the
+        /// internals of Box2D.
+        /// </summary>
+        public Manifold Manifold;
+
+        /// Get or set the desired tangent speed for a conveyor belt behavior. In meters per second.
+        public float TangentSpeed { get; set; }
+
+        /// 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).
+        /// NOTE: If you are setting Enabled to a constant true or false,
+        /// use the explicit Enable() or Disable() functions instead to 
+        /// save the CPU from doing a branch operation.
+        public bool Enabled { get; set; }
+
+        /// <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>
+        /// Determines whether this contact is touching.
+        /// </summary>
+        /// <returns>
+        /// 	<c>true</c> if this instance is touching; otherwise, <c>false</c>.
+        /// </returns>
+        public bool IsTouching { get; set; }
+
+        internal bool IslandFlag { get; set; }
+        internal bool TOIFlag { get; set; }
+        internal bool FilterFlag { get; set; }
+
+        public void ResetRestitution()
+        {
+            Restitution = Settings.MixRestitution(FixtureA.Restitution, FixtureB.Restitution);
+        }
+
+        public void ResetFriction()
+        {
+            Friction = Settings.MixFriction(FixtureA.Friction, FixtureB.Friction);
+        }
+
+        private Contact(Fixture fA, int indexA, Fixture fB, int indexB)
+        {
+            Reset(fA, indexA, fB, indexB);
+        }
+
+        /// <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;
+
+            ContactSolver.WorldManifold.Initialize(ref Manifold, ref bodyA._xf, shapeA.Radius, ref bodyB._xf, shapeB.Radius, out normal, out points);
+        }
+
+        private void Reset(Fixture fA, int indexA, Fixture fB, int indexB)
+        {
+            Enabled = true;
+            IsTouching = false;
+            IslandFlag = false;
+            FilterFlag = false;
+            TOIFlag = false;
+
+            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;
+
+            //FPE: We only set the friction and restitution if we are not destroying the contact
+            if (FixtureA != null && FixtureB != null)
+            {
+                Friction = Settings.MixFriction(FixtureA.Friction, FixtureB.Friction);
+                Restitution = Settings.MixRestitution(FixtureA.Restitution, FixtureB.Restitution);
+            }
+
+            TangentSpeed = 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)
+        {
+            Body bodyA = FixtureA.Body;
+            Body bodyB = FixtureB.Body;
+
+            if (FixtureA == null || FixtureB == null)
+                return;
+
+            Manifold oldManifold = Manifold;
+
+            // Re-enable this contact.
+            Enabled = true;
+
+            bool touching;
+            bool wasTouching = IsTouching;
+
+            bool sensor = FixtureA.IsSensor || FixtureB.IsSensor;
+
+            // Is this contact a sensor?
+            if (sensor)
+            {
+                Shape shapeA = FixtureA.Shape;
+                Shape shapeB = FixtureB.Shape;
+                touching = Collision.Collision.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;
+
+                    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;
+                            break;
+                        }
+                    }
+
+                    Manifold.Points[i] = mp2;
+                }
+
+                if (touching != wasTouching)
+                {
+                    bodyA.Awake = true;
+                    bodyB.Awake = true;
+                }
+            }
+
+            IsTouching = touching;
+
+            if (wasTouching == false)
+            {
+                if (touching)
+                {
+                    if (Settings.AllCollisionCallbacksAgree)
+                    {
+                        bool enabledA = true, enabledB = true;
+
+                        // Report the collision to both participants. Track which ones returned true so we can
+                        // later call OnSeparation if the contact is disabled for a different reason.
+                        if (FixtureA.OnCollision != null)
+                            foreach (OnCollisionEventHandler handler in FixtureA.OnCollision.GetInvocationList())
+                                enabledA = handler(FixtureA, FixtureB, this) && enabledA;
+
+                        // Reverse the order of the reported fixtures. The first fixture is always the one that the
+                        // user subscribed to.
+                        if (FixtureB.OnCollision != null)
+                            foreach (OnCollisionEventHandler handler in FixtureB.OnCollision.GetInvocationList())
+                                enabledB = handler(FixtureB, FixtureA, this) && enabledB;
+
+                        Enabled = enabledA && enabledB;
+
+                        // BeginContact can also return false and disable the contact
+                        if (enabledA && enabledB && contactManager.BeginContact != null)
+                            Enabled = contactManager.BeginContact(this);
+                    }
+                    else
+                    {
+                        //Report the collision to both participants:
+                        if (FixtureA.OnCollision != null)
+                            foreach (OnCollisionEventHandler handler in FixtureA.OnCollision.GetInvocationList())
+                                Enabled = handler(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)
+                            foreach (OnCollisionEventHandler handler in FixtureB.OnCollision.GetInvocationList())
+                                Enabled = handler(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) at any point by
+                    // any of the callbacks, we need to mark it as not touching and call any separation
+                    // callbacks for fixtures that didn't explicitly disable the collision.
+                    if (!Enabled)
+                        IsTouching = false;
+                }
+            }
+            else
+            {
+                if (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.ChainAndCircle:
+                    ChainShape chain = (ChainShape)FixtureA.Shape;
+                    chain.GetChildEdge(_edge, ChildIndexA);
+                    Collision.Collision.CollideEdgeAndCircle(ref manifold, _edge, ref transformA, (CircleShape)FixtureB.Shape, ref transformB);
+                    break;
+                case ContactType.ChainAndPolygon:
+                    ChainShape loop2 = (ChainShape)FixtureA.Shape;
+                    loop2.GetChildEdge(_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.Shape.ShapeType;
+            ShapeType type2 = fixtureB.Shape.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()
+        {
+#if USE_ACTIVE_CONTACT_SET
+            FixtureA.Body.World.ContactManager.RemoveActiveContact(this);
+#endif
+            FixtureA.Body._world._contactPool.Enqueue(this);
+
+            if (Manifold.PointCount > 0 && FixtureA.IsSensor == false && FixtureB.IsSensor == false)
+            {
+                FixtureA.Body.Awake = true;
+                FixtureB.Body.Awake = true;
+            }
+
+            Reset(null, 0, null, 0);
+        }
+
+        #region Nested type: ContactType
+
+        private enum ContactType
+        {
+            NotSupported,
+            Polygon,
+            PolygonAndCircle,
+            Circle,
+            EdgeAndPolygon,
+            EdgeAndCircle,
+            ChainAndPolygon,
+            ChainAndCircle,
+        }
+
+        #endregion
+    }
+}

Farseer Physics Engine 3.5/Dynamics/Contacts/ContactSolver.cs

@@ -0,0 +1,979 @@
+/*
+* Farseer Physics Engine:
+* Copyright (c) 2012 Ian Qvist
+* 
+* Original source Box2D:
+* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org 
+* 
+* 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 ContactPositionConstraint
+    {
+        public Vector2[] localPoints = new Vector2[Settings.MaxManifoldPoints];
+        public Vector2 localNormal;
+        public Vector2 localPoint;
+        public int indexA;
+        public int indexB;
+        public float invMassA, invMassB;
+        public Vector2 localCenterA, localCenterB;
+        public float invIA, invIB;
+        public ManifoldType type;
+        public float radiusA, radiusB;
+        public int pointCount;
+    }
+
+    public sealed class VelocityConstraintPoint
+    {
+        public Vector2 rA;
+        public Vector2 rB;
+        public float normalImpulse;
+        public float tangentImpulse;
+        public float normalMass;
+        public float tangentMass;
+        public float velocityBias;
+    }
+
+    public sealed class ContactVelocityConstraint
+    {
+        public VelocityConstraintPoint[] points = new VelocityConstraintPoint[Settings.MaxManifoldPoints];
+        public Vector2 normal;
+        public Mat22 normalMass;
+        public Mat22 K;
+        public int indexA;
+        public int indexB;
+        public float invMassA, invMassB;
+        public float invIA, invIB;
+        public float friction;
+        public float restitution;
+        public float tangentSpeed;
+        public int pointCount;
+        public int contactIndex;
+
+        public ContactVelocityConstraint()
+        {
+            for (int i = 0; i < Settings.MaxManifoldPoints; i++)
+            {
+                points[i] = new VelocityConstraintPoint();
+            }
+        }
+    }
+
+    public class ContactSolver
+    {
+        public TimeStep _step;
+        public Position[] _positions;
+        public Velocity[] _velocities;
+        public ContactPositionConstraint[] _positionConstraints;
+        public ContactVelocityConstraint[] _velocityConstraints;
+        public Contact[] _contacts;
+        public int _count;
+
+        public void Reset(TimeStep step, int count, Contact[] contacts, Position[] positions, Velocity[] velocities, bool warmstarting = Settings.EnableWarmstarting)
+        {
+            _step = step;
+            _count = count;
+            _positions = positions;
+            _velocities = velocities;
+            _contacts = contacts;
+
+            // grow the array
+            if (_velocityConstraints == null || _velocityConstraints.Length < count)
+            {
+                _velocityConstraints = new ContactVelocityConstraint[count * 2];
+                _positionConstraints = new ContactPositionConstraint[count * 2];
+
+                for (int i = 0; i < _velocityConstraints.Length; i++)
+                {
+                    _velocityConstraints[i] = new ContactVelocityConstraint();
+                }
+
+                for (int i = 0; i < _positionConstraints.Length; i++)
+                {
+                    _positionConstraints[i] = new ContactPositionConstraint();
+                }
+            }
+
+            // Initialize position independent portions of the constraints.
+            for (int i = 0; i < _count; ++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;
+
+                int pointCount = manifold.PointCount;
+                Debug.Assert(pointCount > 0);
+
+                ContactVelocityConstraint vc = _velocityConstraints[i];
+                vc.friction = contact.Friction;
+                vc.restitution = contact.Restitution;
+                vc.tangentSpeed = contact.TangentSpeed;
+                vc.indexA = bodyA.IslandIndex;
+                vc.indexB = bodyB.IslandIndex;
+                vc.invMassA = bodyA._invMass;
+                vc.invMassB = bodyB._invMass;
+                vc.invIA = bodyA._invI;
+                vc.invIB = bodyB._invI;
+                vc.contactIndex = i;
+                vc.pointCount = pointCount;
+                vc.K.SetZero();
+                vc.normalMass.SetZero();
+
+                ContactPositionConstraint pc = _positionConstraints[i];
+                pc.indexA = bodyA.IslandIndex;
+                pc.indexB = bodyB.IslandIndex;
+                pc.invMassA = bodyA._invMass;
+                pc.invMassB = bodyB._invMass;
+                pc.localCenterA = bodyA._sweep.LocalCenter;
+                pc.localCenterB = bodyB._sweep.LocalCenter;
+                pc.invIA = bodyA._invI;
+                pc.invIB = bodyB._invI;
+                pc.localNormal = manifold.LocalNormal;
+                pc.localPoint = manifold.LocalPoint;
+                pc.pointCount = pointCount;
+                pc.radiusA = radiusA;
+                pc.radiusB = radiusB;
+                pc.type = manifold.Type;
+
+                for (int j = 0; j < pointCount; ++j)
+                {
+                    ManifoldPoint cp = manifold.Points[j];
+                    VelocityConstraintPoint vcp = vc.points[j];
+
+                    if (Settings.EnableWarmstarting)
+                    {
+                        vcp.normalImpulse = _step.dtRatio * cp.NormalImpulse;
+                        vcp.tangentImpulse = _step.dtRatio * cp.TangentImpulse;
+                    }
+                    else
+                    {
+                        vcp.normalImpulse = 0.0f;
+                        vcp.tangentImpulse = 0.0f;
+                    }
+
+                    vcp.rA = Vector2.Zero;
+                    vcp.rB = Vector2.Zero;
+                    vcp.normalMass = 0.0f;
+                    vcp.tangentMass = 0.0f;
+                    vcp.velocityBias = 0.0f;
+
+                    pc.localPoints[j] = cp.LocalPoint;
+                }
+            }
+        }
+
+        public void InitializeVelocityConstraints()
+        {
+            for (int i = 0; i < _count; ++i)
+            {
+                ContactVelocityConstraint vc = _velocityConstraints[i];
+                ContactPositionConstraint pc = _positionConstraints[i];
+
+                float radiusA = pc.radiusA;
+                float radiusB = pc.radiusB;
+                Manifold manifold = _contacts[vc.contactIndex].Manifold;
+
+                int indexA = vc.indexA;
+                int indexB = vc.indexB;
+
+                float mA = vc.invMassA;
+                float mB = vc.invMassB;
+                float iA = vc.invIA;
+                float iB = vc.invIB;
+                Vector2 localCenterA = pc.localCenterA;
+                Vector2 localCenterB = pc.localCenterB;
+
+                Vector2 cA = _positions[indexA].c;
+                float aA = _positions[indexA].a;
+                Vector2 vA = _velocities[indexA].v;
+                float wA = _velocities[indexA].w;
+
+                Vector2 cB = _positions[indexB].c;
+                float aB = _positions[indexB].a;
+                Vector2 vB = _velocities[indexB].v;
+                float wB = _velocities[indexB].w;
+
+                Debug.Assert(manifold.PointCount > 0);
+
+                Transform xfA = new Transform();
+                Transform xfB = new Transform();
+                xfA.q.Set(aA);
+                xfB.q.Set(aB);
+                xfA.p = cA - MathUtils.Mul(xfA.q, localCenterA);
+                xfB.p = cB - MathUtils.Mul(xfB.q, localCenterB);
+
+                Vector2 normal;
+                FixedArray2<Vector2> points;
+                WorldManifold.Initialize(ref manifold, ref xfA, radiusA, ref xfB, radiusB, out normal, out points);
+
+                vc.normal = normal;
+
+                int pointCount = vc.pointCount;
+                for (int j = 0; j < pointCount; ++j)
+                {
+                    VelocityConstraintPoint vcp = vc.points[j];
+
+                    vcp.rA = points[j] - cA;
+                    vcp.rB = points[j] - cB;
+
+                    float rnA = MathUtils.Cross(vcp.rA, vc.normal);
+                    float rnB = MathUtils.Cross(vcp.rB, vc.normal);
+
+                    float kNormal = mA + mB + iA * rnA * rnA + iB * rnB * rnB;
+
+                    vcp.normalMass = kNormal > 0.0f ? 1.0f / kNormal : 0.0f;
+
+                    Vector2 tangent = MathUtils.Cross(vc.normal, 1.0f);
+
+                    float rtA = MathUtils.Cross(vcp.rA, tangent);
+                    float rtB = MathUtils.Cross(vcp.rB, tangent);
+
+                    float kTangent = mA + mB + iA * rtA * rtA + iB * rtB * rtB;
+
+                    vcp.tangentMass = kTangent > 0.0f ? 1.0f / kTangent : 0.0f;
+
+                    // Setup a velocity bias for restitution.
+                    vcp.velocityBias = 0.0f;
+                    float vRel = Vector2.Dot(vc.normal, vB + MathUtils.Cross(wB, vcp.rB) - vA - MathUtils.Cross(wA, vcp.rA));
+                    if (vRel < -Settings.VelocityThreshold)
+                    {
+                        vcp.velocityBias = -vc.restitution * vRel;
+                    }
+                }
+
+                // If we have two points, then prepare the block solver.
+                if (vc.pointCount == 2)
+                {
+                    VelocityConstraintPoint vcp1 = vc.points[0];
+                    VelocityConstraintPoint vcp2 = vc.points[1];
+
+                    float rn1A = MathUtils.Cross(vcp1.rA, vc.normal);
+                    float rn1B = MathUtils.Cross(vcp1.rB, vc.normal);
+                    float rn2A = MathUtils.Cross(vcp2.rA, vc.normal);
+                    float rn2B = MathUtils.Cross(vcp2.rB, vc.normal);
+
+                    float k11 = mA + mB + iA * rn1A * rn1A + iB * rn1B * rn1B;
+                    float k22 = mA + mB + iA * rn2A * rn2A + iB * rn2B * rn2B;
+                    float k12 = mA + mB + iA * rn1A * rn2A + iB * rn1B * rn2B;
+
+                    // Ensure a reasonable condition number.
+                    const float k_maxConditionNumber = 1000.0f;
+                    if (k11 * k11 < k_maxConditionNumber * (k11 * k22 - k12 * k12))
+                    {
+                        // K is safe to invert.
+                        vc.K.ex = new Vector2(k11, k12);
+                        vc.K.ey = new Vector2(k12, k22);
+                        vc.normalMass = vc.K.Inverse;
+                    }
+                    else
+                    {
+                        // The constraints are redundant, just use one.
+                        // TODO_ERIN use deepest?
+                        vc.pointCount = 1;
+                    }
+                }
+            }
+        }
+
+        public void WarmStart()
+        {
+            // Warm start.
+            for (int i = 0; i < _count; ++i)
+            {
+                ContactVelocityConstraint vc = _velocityConstraints[i];
+
+                int indexA = vc.indexA;
+                int indexB = vc.indexB;
+                float mA = vc.invMassA;
+                float iA = vc.invIA;
+                float mB = vc.invMassB;
+                float iB = vc.invIB;
+                int pointCount = vc.pointCount;
+
+                Vector2 vA = _velocities[indexA].v;
+                float wA = _velocities[indexA].w;
+                Vector2 vB = _velocities[indexB].v;
+                float wB = _velocities[indexB].w;
+
+                Vector2 normal = vc.normal;
+                Vector2 tangent = MathUtils.Cross(normal, 1.0f);
+
+                for (int j = 0; j < pointCount; ++j)
+                {
+                    VelocityConstraintPoint vcp = vc.points[j];
+                    Vector2 P = vcp.normalImpulse * normal + vcp.tangentImpulse * tangent;
+                    wA -= iA * MathUtils.Cross(vcp.rA, P);
+                    vA -= mA * P;
+                    wB += iB * MathUtils.Cross(vcp.rB, P);
+                    vB += mB * P;
+                }
+
+                _velocities[indexA].v = vA;
+                _velocities[indexA].w = wA;
+                _velocities[indexB].v = vB;
+                _velocities[indexB].w = wB;
+            }
+        }
+
+        public void SolveVelocityConstraints()
+        {
+            for (int i = 0; i < _count; ++i)
+            {
+                ContactVelocityConstraint vc = _velocityConstraints[i];
+
+                int indexA = vc.indexA;
+                int indexB = vc.indexB;
+                float mA = vc.invMassA;
+                float iA = vc.invIA;
+                float mB = vc.invMassB;
+                float iB = vc.invIB;
+                int pointCount = vc.pointCount;
+
+                Vector2 vA = _velocities[indexA].v;
+                float wA = _velocities[indexA].w;
+                Vector2 vB = _velocities[indexB].v;
+                float wB = _velocities[indexB].w;
+
+                Vector2 normal = vc.normal;
+                Vector2 tangent = MathUtils.Cross(normal, 1.0f);
+                float friction = vc.friction;
+
+                Debug.Assert(pointCount == 1 || pointCount == 2);
+
+                // Solve tangent constraints first because non-penetration is more important
+                // than friction.
+                for (int j = 0; j < pointCount; ++j)
+                {
+                    VelocityConstraintPoint vcp = vc.points[j];
+
+                    // Relative velocity at contact
+                    Vector2 dv = vB + MathUtils.Cross(wB, vcp.rB) - vA - MathUtils.Cross(wA, vcp.rA);
+
+                    // Compute tangent force
+                    float vt = Vector2.Dot(dv, tangent) - vc.tangentSpeed;
+                    float lambda = vcp.tangentMass * (-vt);
+
+                    // b2Clamp the accumulated force
+                    float maxFriction = friction * vcp.normalImpulse;
+                    float newImpulse = MathUtils.Clamp(vcp.tangentImpulse + lambda, -maxFriction, maxFriction);
+                    lambda = newImpulse - vcp.tangentImpulse;
+                    vcp.tangentImpulse = newImpulse;
+
+                    // Apply contact impulse
+                    Vector2 P = lambda * tangent;
+
+                    vA -= mA * P;
+                    wA -= iA * MathUtils.Cross(vcp.rA, P);
+
+                    vB += mB * P;
+                    wB += iB * MathUtils.Cross(vcp.rB, P);
+                }
+
+                // Solve normal constraints
+                if (vc.pointCount == 1)
+                {
+                    VelocityConstraintPoint vcp = vc.points[0];
+
+                    // Relative velocity at contact
+                    Vector2 dv = vB + MathUtils.Cross(wB, vcp.rB) - vA - MathUtils.Cross(wA, vcp.rA);
+
+                    // Compute normal impulse
+                    float vn = Vector2.Dot(dv, normal);
+                    float lambda = -vcp.normalMass * (vn - vcp.velocityBias);
+
+                    // b2Clamp the accumulated impulse
+                    float newImpulse = Math.Max(vcp.normalImpulse + lambda, 0.0f);
+                    lambda = newImpulse - vcp.normalImpulse;
+                    vcp.normalImpulse = newImpulse;
+
+                    // Apply contact impulse
+                    Vector2 P = lambda * normal;
+                    vA -= mA * P;
+                    wA -= iA * MathUtils.Cross(vcp.rA, P);
+
+                    vB += mB * P;
+                    wB += iB * MathUtils.Cross(vcp.rB, P);
+                }
+                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 = vn0 - 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 = a + d
+                    // 
+                    // a := old total impulse
+                    // x := new total impulse
+                    // d := incremental impulse 
+                    //
+                    // For the current iteration we extend the formula for the incremental impulse
+                    // to compute the new total impulse:
+                    //
+                    // vn = A * d + b
+                    //    = A * (x - a) + b
+                    //    = A * x + b - A * a
+                    //    = A * x + b'
+                    // b' = b - A * a;
+
+                    VelocityConstraintPoint cp1 = vc.points[0];
+                    VelocityConstraintPoint cp2 = vc.points[1];
+
+                    Vector2 a = new Vector2(cp1.normalImpulse, cp2.normalImpulse);
+                    Debug.Assert(a.X >= 0.0f && a.Y >= 0.0f);
+
+                    // Relative velocity at contact
+                    Vector2 dv1 = vB + MathUtils.Cross(wB, cp1.rB) - vA - MathUtils.Cross(wA, cp1.rA);
+                    Vector2 dv2 = vB + MathUtils.Cross(wB, cp2.rB) - vA - MathUtils.Cross(wA, cp2.rA);
+
+                    // Compute normal velocity
+                    float vn1 = Vector2.Dot(dv1, normal);
+                    float vn2 = Vector2.Dot(dv2, normal);
+
+                    Vector2 b = new Vector2();
+                    b.X = vn1 - cp1.velocityBias;
+                    b.Y = vn2 - cp2.velocityBias;
+
+                    // Compute b'
+                    b -= MathUtils.Mul(ref vc.K, a);
+
+                    const float k_errorTol = 1e-3f;
+                    //B2_NOT_USED(k_errorTol);
+
+                    for (; ; )
+                    {
+                        //
+                        // Case 1: vn = 0
+                        //
+                        // 0 = A * x + b'
+                        //
+                        // Solve for x:
+                        //
+                        // x = - inv(A) * b'
+                        //
+                        Vector2 x = -MathUtils.Mul(ref vc.normalMass, b);
+
+                        if (x.X >= 0.0f && x.Y >= 0.0f)
+                        {
+                            // Get the incremental impulse
+                            Vector2 d = x - a;
+
+                            // Apply incremental impulse
+                            Vector2 P1 = d.X * normal;
+                            Vector2 P2 = d.Y * normal;
+                            vA -= mA * (P1 + P2);
+                            wA -= iA * (MathUtils.Cross(cp1.rA, P1) + MathUtils.Cross(cp2.rA, P2));
+
+                            vB += mB * (P1 + P2);
+                            wB += iB * (MathUtils.Cross(cp1.rB, P1) + MathUtils.Cross(cp2.rB, P2));
+
+                            // Accumulate
+                            cp1.normalImpulse = x.X;
+                            cp2.normalImpulse = x.Y;
+
+#if B2_DEBUG_SOLVER 
+					// 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);
+
+					b2Assert(b2Abs(vn1 - cp1.velocityBias) < k_errorTol);
+					b2Assert(b2Abs(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'
+                        //
+                        x.X = -cp1.normalMass * b.X;
+                        x.Y = 0.0f;
+                        vn1 = 0.0f;
+                        vn2 = vc.K.ex.Y * x.X + b.Y;
+
+                        if (x.X >= 0.0f && vn2 >= 0.0f)
+                        {
+                            // Get the incremental impulse
+                            Vector2 d = x - a;
+
+                            // Apply incremental impulse
+                            Vector2 P1 = d.X * normal;
+                            Vector2 P2 = d.Y * normal;
+                            vA -= mA * (P1 + P2);
+                            wA -= iA * (MathUtils.Cross(cp1.rA, P1) + MathUtils.Cross(cp2.rA, P2));
+
+                            vB += mB * (P1 + P2);
+                            wB += iB * (MathUtils.Cross(cp1.rB, P1) + MathUtils.Cross(cp2.rB, P2));
+
+                            // Accumulate
+                            cp1.normalImpulse = x.X;
+                            cp2.normalImpulse = x.Y;
+
+#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);
+
+					b2Assert(b2Abs(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'
+                        //
+                        x.X = 0.0f;
+                        x.Y = -cp2.normalMass * b.Y;
+                        vn1 = vc.K.ey.X * x.Y + b.X;
+                        vn2 = 0.0f;
+
+                        if (x.Y >= 0.0f && vn1 >= 0.0f)
+                        {
+                            // Resubstitute for the incremental impulse
+                            Vector2 d = x - a;
+
+                            // Apply incremental impulse
+                            Vector2 P1 = d.X * normal;
+                            Vector2 P2 = d.Y * normal;
+                            vA -= mA * (P1 + P2);
+                            wA -= iA * (MathUtils.Cross(cp1.rA, P1) + MathUtils.Cross(cp2.rA, P2));
+
+                            vB += mB * (P1 + P2);
+                            wB += iB * (MathUtils.Cross(cp1.rB, P1) + MathUtils.Cross(cp2.rB, P2));
+
+                            // Accumulate
+                            cp1.normalImpulse = x.X;
+                            cp2.normalImpulse = x.Y;
+
+#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);
+
+					b2Assert(b2Abs(vn2 - cp2.velocityBias) < k_errorTol);
+#endif
+                            break;
+                        }
+
+                        //
+                        // Case 4: x1 = 0 and x2 = 0
+                        // 
+                        // vn1 = b1
+                        // vn2 = b2;
+                        x.X = 0.0f;
+                        x.Y = 0.0f;
+                        vn1 = b.X;
+                        vn2 = b.Y;
+
+                        if (vn1 >= 0.0f && vn2 >= 0.0f)
+                        {
+                            // Resubstitute for the incremental impulse
+                            Vector2 d = x - a;
+
+                            // Apply incremental impulse
+                            Vector2 P1 = d.X * normal;
+                            Vector2 P2 = d.Y * normal;
+                            vA -= mA * (P1 + P2);
+                            wA -= iA * (MathUtils.Cross(cp1.rA, P1) + MathUtils.Cross(cp2.rA, P2));
+
+                            vB += mB * (P1 + P2);
+                            wB += iB * (MathUtils.Cross(cp1.rB, P1) + MathUtils.Cross(cp2.rB, P2));
+
+                            // Accumulate
+                            cp1.normalImpulse = x.X;
+                            cp2.normalImpulse = x.Y;
+
+                            break;
+                        }
+
+                        // No solution, give up. This is hit sometimes, but it doesn't seem to matter.
+                        break;
+                    }
+                }
+
+                _velocities[indexA].v = vA;
+                _velocities[indexA].w = wA;
+                _velocities[indexB].v = vB;
+                _velocities[indexB].w = wB;
+            }
+        }
+
+        public void StoreImpulses()
+        {
+            for (int i = 0; i < _count; ++i)
+            {
+                ContactVelocityConstraint vc = _velocityConstraints[i];
+                Manifold manifold = _contacts[vc.contactIndex].Manifold;
+
+                for (int j = 0; j < vc.pointCount; ++j)
+                {
+                    ManifoldPoint point = manifold.Points[j];
+                    point.NormalImpulse = vc.points[j].normalImpulse;
+                    point.TangentImpulse = vc.points[j].tangentImpulse;
+                    manifold.Points[j] = point;
+                }
+
+                _contacts[vc.contactIndex].Manifold = manifold;
+            }
+        }
+
+        public bool SolvePositionConstraints()
+        {
+            float minSeparation = 0.0f;
+
+            for (int i = 0; i < _count; ++i)
+            {
+                ContactPositionConstraint pc = _positionConstraints[i];
+
+                int indexA = pc.indexA;
+                int indexB = pc.indexB;
+                Vector2 localCenterA = pc.localCenterA;
+                float mA = pc.invMassA;
+                float iA = pc.invIA;
+                Vector2 localCenterB = pc.localCenterB;
+                float mB = pc.invMassB;
+                float iB = pc.invIB;
+                int pointCount = pc.pointCount;
+
+                Vector2 cA = _positions[indexA].c;
+                float aA = _positions[indexA].a;
+
+                Vector2 cB = _positions[indexB].c;
+                float aB = _positions[indexB].a;
+
+                // Solve normal constraints
+                for (int j = 0; j < pointCount; ++j)
+                {
+                    Transform xfA = new Transform();
+                    Transform xfB = new Transform();
+                    xfA.q.Set(aA);
+                    xfB.q.Set(aB);
+                    xfA.p = cA - MathUtils.Mul(xfA.q, localCenterA);
+                    xfB.p = cB - MathUtils.Mul(xfB.q, localCenterB);
+
+                    Vector2 normal;
+                    Vector2 point;
+                    float separation;
+
+                    PositionSolverManifold.Initialize(pc, xfA, xfB, j, out normal, out point, out separation);
+
+                    Vector2 rA = point - cA;
+                    Vector2 rB = point - cB;
+
+                    // Track max constraint error.
+                    minSeparation = Math.Min(minSeparation, separation);
+
+                    // Prevent large corrections and allow slop.
+                    float C = MathUtils.Clamp(Settings.Baumgarte * (separation + Settings.LinearSlop), -Settings.MaxLinearCorrection, 0.0f);
+
+                    // Compute the effective mass.
+                    float rnA = MathUtils.Cross(rA, normal);
+                    float rnB = MathUtils.Cross(rB, normal);
+                    float K = mA + mB + iA * rnA * rnA + iB * rnB * rnB;
+
+                    // Compute normal impulse
+                    float impulse = K > 0.0f ? -C / K : 0.0f;
+
+                    Vector2 P = impulse * normal;
+
+                    cA -= mA * P;
+                    aA -= iA * MathUtils.Cross(rA, P);
+
+                    cB += mB * P;
+                    aB += iB * MathUtils.Cross(rB, P);
+                }
+
+                _positions[indexA].c = cA;
+                _positions[indexA].a = aA;
+
+                _positions[indexB].c = cB;
+                _positions[indexB].a = aB;
+            }
+
+            // We can't expect minSpeparation >= -b2_linearSlop because we don't
+            // push the separation above -b2_linearSlop.
+            return minSeparation >= -3.0f * Settings.LinearSlop;
+        }
+
+        // Sequential position solver for position constraints.
+        public bool SolveTOIPositionConstraints(int toiIndexA, int toiIndexB)
+        {
+            float minSeparation = 0.0f;
+
+            for (int i = 0; i < _count; ++i)
+            {
+                ContactPositionConstraint pc = _positionConstraints[i];
+
+                int indexA = pc.indexA;
+                int indexB = pc.indexB;
+                Vector2 localCenterA = pc.localCenterA;
+                Vector2 localCenterB = pc.localCenterB;
+                int pointCount = pc.pointCount;
+
+                float mA = 0.0f;
+                float iA = 0.0f;
+                if (indexA == toiIndexA || indexA == toiIndexB)
+                {
+                    mA = pc.invMassA;
+                    iA = pc.invIA;
+                }
+
+                float mB = 0.0f;
+                float iB = 0.0f;
+                if (indexB == toiIndexA || indexB == toiIndexB)
+                {
+                    mB = pc.invMassB;
+                    iB = pc.invIB;
+                }
+
+                Vector2 cA = _positions[indexA].c;
+                float aA = _positions[indexA].a;
+
+                Vector2 cB = _positions[indexB].c;
+                float aB = _positions[indexB].a;
+
+                // Solve normal constraints
+                for (int j = 0; j < pointCount; ++j)
+                {
+                    Transform xfA = new Transform();
+                    Transform xfB = new Transform();
+                    xfA.q.Set(aA);
+                    xfB.q.Set(aB);
+                    xfA.p = cA - MathUtils.Mul(xfA.q, localCenterA);
+                    xfB.p = cB - MathUtils.Mul(xfB.q, localCenterB);
+
+                    Vector2 normal;
+                    Vector2 point;
+                    float separation;
+
+                    PositionSolverManifold.Initialize(pc, xfA, xfB, j, out normal, out point, out separation);
+
+                    Vector2 rA = point - cA;
+                    Vector2 rB = point - cB;
+
+                    // Track max constraint error.
+                    minSeparation = Math.Min(minSeparation, separation);
+
+                    // Prevent large corrections and allow slop.
+                    float C = MathUtils.Clamp(Settings.Baumgarte * (separation + Settings.LinearSlop), -Settings.MaxLinearCorrection, 0.0f);
+
+                    // Compute the effective mass.
+                    float rnA = MathUtils.Cross(rA, normal);
+                    float rnB = MathUtils.Cross(rB, normal);
+                    float K = mA + mB + iA * rnA * rnA + iB * rnB * rnB;
+
+                    // Compute normal impulse
+                    float impulse = K > 0.0f ? -C / K : 0.0f;
+
+                    Vector2 P = impulse * normal;
+
+                    cA -= mA * P;
+                    aA -= iA * MathUtils.Cross(rA, P);
+
+                    cB += mB * P;
+                    aB += iB * MathUtils.Cross(rB, P);
+                }
+
+                _positions[indexA].c = cA;
+                _positions[indexA].a = aA;
+
+                _positions[indexB].c = cB;
+                _positions[indexB].a = aB;
+            }
+
+            // We can't expect minSpeparation >= -b2_linearSlop because we don't
+            // push the separation above -b2_linearSlop.
+            return minSeparation >= -1.5f * Settings.LinearSlop;
+        }
+
+        public static class WorldManifold
+        {
+            /// <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="xfA">The transform for A.</param>
+            /// <param name="radiusA">The radius for A.</param>
+            /// <param name="xfB">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 Initialize(ref Manifold manifold, ref Transform xfA, float radiusA, ref Transform xfB, float radiusB, out Vector2 normal, out FixedArray2<Vector2> points)
+            {
+                normal = Vector2.Zero;
+                points = new FixedArray2<Vector2>();
+
+                if (manifold.PointCount == 0)
+                {
+                    return;
+                }
+
+                switch (manifold.Type)
+                {
+                    case ManifoldType.Circles:
+                        {
+                            normal = new Vector2(1.0f, 0.0f);
+                            Vector2 pointA = MathUtils.Mul(ref xfA, manifold.LocalPoint);
+                            Vector2 pointB = MathUtils.Mul(ref xfB, manifold.Points[0].LocalPoint);
+                            if (Vector2.DistanceSquared(pointA, pointB) > Settings.Epsilon * Settings.Epsilon)
+                            {
+                                normal = pointB - pointA;
+                                normal.Normalize();
+                            }
+
+                            Vector2 cA = pointA + radiusA * normal;
+                            Vector2 cB = pointB - radiusB * normal;
+                            points[0] = 0.5f * (cA + cB);
+                        }
+                        break;
+
+                    case ManifoldType.FaceA:
+                        {
+                            normal = MathUtils.Mul(xfA.q, manifold.LocalNormal);
+                            Vector2 planePoint = MathUtils.Mul(ref xfA, manifold.LocalPoint);
+
+                            for (int i = 0; i < manifold.PointCount; ++i)
+                            {
+                                Vector2 clipPoint = MathUtils.Mul(ref xfB, manifold.Points[i].LocalPoint);
+                                Vector2 cA = clipPoint + (radiusA - Vector2.Dot(clipPoint - planePoint, normal)) * normal;
+                                Vector2 cB = clipPoint - radiusB * normal;
+                                points[i] = 0.5f * (cA + cB);
+                            }
+                        }
+                        break;
+
+                    case ManifoldType.FaceB:
+                        {
+                            normal = MathUtils.Mul(xfB.q, manifold.LocalNormal);
+                            Vector2 planePoint = MathUtils.Mul(ref xfB, manifold.LocalPoint);
+
+                            for (int i = 0; i < manifold.PointCount; ++i)
+                            {
+                                Vector2 clipPoint = MathUtils.Mul(ref xfA, manifold.Points[i].LocalPoint);
+                                Vector2 cB = clipPoint + (radiusB - Vector2.Dot(clipPoint - planePoint, normal)) * normal;
+                                Vector2 cA = clipPoint - radiusA * normal;
+                                points[i] = 0.5f * (cA + cB);
+                            }
+
+                            // Ensure normal points from A to B.
+                            normal = -normal;
+                        }
+                        break;
+                }
+            }
+        }
+
+        private static class PositionSolverManifold
+        {
+            public static void Initialize(ContactPositionConstraint pc, Transform xfA, Transform xfB, int index, out Vector2 normal, out Vector2 point, out float separation)
+            {
+                Debug.Assert(pc.pointCount > 0);
+
+
+                switch (pc.type)
+                {
+                    case ManifoldType.Circles:
+                        {
+                            Vector2 pointA = MathUtils.Mul(ref xfA, pc.localPoint);
+                            Vector2 pointB = MathUtils.Mul(ref xfB, pc.localPoints[0]);
+                            normal = pointB - pointA;
+                            normal.Normalize();
+                            point = 0.5f * (pointA + pointB);
+                            separation = Vector2.Dot(pointB - pointA, normal) - pc.radiusA - pc.radiusB;
+                        }
+                        break;
+
+                    case ManifoldType.FaceA:
+                        {
+                            normal = MathUtils.Mul(xfA.q, pc.localNormal);
+                            Vector2 planePoint = MathUtils.Mul(ref xfA, pc.localPoint);
+
+                            Vector2 clipPoint = MathUtils.Mul(ref xfB, pc.localPoints[index]);
+                            separation = Vector2.Dot(clipPoint - planePoint, normal) - pc.radiusA - pc.radiusB;
+                            point = clipPoint;
+                        }
+                        break;
+
+                    case ManifoldType.FaceB:
+                        {
+                            normal = MathUtils.Mul(xfB.q, pc.localNormal);
+                            Vector2 planePoint = MathUtils.Mul(ref xfB, pc.localPoint);
+
+                            Vector2 clipPoint = MathUtils.Mul(ref xfA, pc.localPoints[index]);
+                            separation = Vector2.Dot(clipPoint - planePoint, normal) - pc.radiusA - pc.radiusB;
+                            point = clipPoint;
+
+                            // Ensure normal points from A to B
+                            normal = -normal;
+                        }
+                        break;
+                    default:
+                        normal = Vector2.Zero;
+                        point = Vector2.Zero;
+                        separation = 0;
+                        break;
+
+                }
+            }
+        }
+    }
+}

Farseer Physics Engine 3.5/Dynamics/Fixture.cs

@@ -0,0 +1,619 @@
+/*
+* Farseer Physics Engine:
+* Copyright (c) 2012 Ian Qvist
+* 
+* Original source Box2D:
+* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org 
+* 
+* 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. 
+*/
+//#define USE_IGNORE_CCD_CATEGORIES
+
+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
+    {
+        [ThreadStatic]
+        private static int _fixtureIdCounter;
+        private bool _isSensor;
+        private float _friction;
+        private float _restitution;
+
+        internal Category _collidesWith;
+        internal Category _collisionCategories;
+        internal short _collisionGroup;
+        internal HashSet<int> _collisionIgnores;
+
+        public FixtureProxy[] Proxies;
+        public int ProxyCount;
+        public Category IgnoreCCDWith;
+
+        /// <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: This can in some cases be called multiple times, as a fixture can have multiple contacts.
+        /// Note The first fixture argument is always the fixture that the delegate is subscribed to.
+        /// </summary>
+        public OnSeparationEventHandler OnSeparation;
+
+        internal Fixture()
+        {
+            FixtureId = _fixtureIdCounter++;
+
+            _collisionCategories = Settings.DefaultFixtureCollisionCategories;
+            _collidesWith = Settings.DefaultFixtureCollidesWith;
+            _collisionGroup = 0;
+            _collisionIgnores = new HashSet<int>();
+
+            IgnoreCCDWith = Settings.DefaultFixtureIgnoreCCDWith;
+
+            //Fixture defaults
+            Friction = 0.2f;
+            Restitution = 0;
+        }
+
+        internal Fixture(Body body, Shape shape, object userData = null)
+            : this()
+        {
+#if DEBUG
+            if (shape.ShapeType == ShapeType.Polygon)
+                ((PolygonShape)shape).Vertices.AttachedToBody = true;
+#endif
+
+            Body = body;
+            UserData = userData;
+            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 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 { return _isSensor; }
+            set
+            {
+                if (Body != null)
+                    Body.Awake = true;
+
+                _isSensor = value;
+            }
+        }
+
+        /// <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>
+        /// Set the coefficient of friction. This will _not_ change the friction of
+        /// existing contacts.
+        /// </summary>
+        /// <value>The friction.</value>
+        public float Friction
+        {
+            get { return _friction; }
+            set
+            {
+                Debug.Assert(!float.IsNaN(value));
+
+                _friction = value;
+            }
+        }
+
+        /// <summary>
+        /// Set the coefficient of restitution. This will not change the restitution of
+        /// existing contacts.
+        /// </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; internal 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.Contains(fixture.FixtureId))
+            {
+                _collisionIgnores.Remove(fixture.FixtureId);
+                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.Contains(fixture.FixtureId))
+            {
+                _collisionIgnores.Add(fixture.FixtureId);
+                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)
+        {
+            return _collisionIgnores.Contains(fixture.FixtureId);
+        }
+
+        /// <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>
+        private 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.FilterFlag = true;
+                }
+
+                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;
+
+            if (Body.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._worldHasNewFixture = true;
+
+            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;
+        }
+
+        internal void Destroy()
+        {
+#if DEBUG
+            if (Shape.ShapeType == ShapeType.Polygon)
+                ((PolygonShape)Shape).Vertices.AttachedToBody = false;
+#endif
+
+            // The proxies must be destroyed before calling this.
+            Debug.Assert(ProxyCount == 0);
+
+            // Free the proxy array.
+            Proxies = null;
+            Shape = null;
+
+            //FPE: We set the userdata to null here to help prevent bugs related to stale references in GC
+            UserData = 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;
+
+                //FPE note: This line needs to be after the previous two because FixtureProxy is a struct
+                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.p - transform1.p;
+
+                broadPhase.MoveProxy(proxy.ProxyId, ref proxy.AABB, displacement);
+            }
+        }
+
+        /// <summary>
+        /// Only compares the values of this fixture, and not the attached shape or body.
+        /// This is used for deduplication in serialization only.
+        /// </summary>
+        internal bool CompareTo(Fixture fixture)
+        {
+            return (_collidesWith == fixture._collidesWith &&
+                    _collisionCategories == fixture._collisionCategories &&
+                    _collisionGroup == fixture._collisionGroup &&
+                    Friction == fixture.Friction &&
+                    IsSensor == fixture.IsSensor &&
+                    Restitution == fixture.Restitution &&
+                    UserData == fixture.UserData &&
+                    IgnoreCCDWith == fixture.IgnoreCCDWith &&
+                    SequenceEqual(_collisionIgnores, fixture._collisionIgnores));
+        }
+
+        private bool SequenceEqual<T>(HashSet<T> first, HashSet<T> second)
+        {
+            if (first.Count != second.Count)
+                return false;
+
+            using (IEnumerator<T> enumerator1 = first.GetEnumerator())
+            {
+                using (IEnumerator<T> enumerator2 = second.GetEnumerator())
+                {
+                    while (enumerator1.MoveNext())
+                    {
+                        if (!enumerator2.MoveNext() || !Equals(enumerator1.Current, enumerator2.Current))
+                            return false;
+                    }
+
+                    if (enumerator2.MoveNext())
+                        return false;
+                }
+            }
+
+            return true;
+        }
+
+        /// <summary>
+        /// Clones the fixture and attached shape onto the specified body.
+        /// </summary>
+        /// <param name="body">The body you wish to clone the fixture onto.</param>
+        /// <returns>The cloned fixture.</returns>
+        public Fixture CloneOnto(Body body)
+        {
+            Fixture fixture = new Fixture();
+            fixture.Body = body;
+            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;
+            fixture.IgnoreCCDWith = IgnoreCCDWith;
+
+            foreach (int ignore in _collisionIgnores)
+            {
+                fixture._collisionIgnores.Add(ignore);
+            }
+
+            fixture.RegisterFixture();
+            return fixture;
+        }
+    }
+}

Farseer Physics Engine 3.5/Dynamics/Island.cs

@@ -0,0 +1,449 @@
+/*
+* Farseer Physics Engine:
+* Copyright (c) 2012 Ian Qvist
+* 
+* Original source Box2D:
+* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org 
+* 
+* 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
+    {
+        private ContactManager _contactManager;
+        private ContactSolver _contactSolver = new ContactSolver();
+        private Contact[] _contacts;
+        private Joint[] _joints;
+
+        private const float LinTolSqr = Settings.LinearSleepTolerance * Settings.LinearSleepTolerance;
+        private const float AngTolSqr = Settings.AngularSleepTolerance * Settings.AngularSleepTolerance;
+        private Stopwatch _watch = new Stopwatch();
+
+        public Body[] Bodies;
+        public int BodyCount;
+        public int ContactCount;
+        public int JointCount;
+
+        public Velocity[] _velocities;
+        public Position[] _positions;
+
+        public int BodyCapacity;
+        public int ContactCapacity;
+        public int JointCapacity;
+        public float JointUpdateTime;
+
+        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];
+                _velocities = new Velocity[bodyCapacity];
+                _positions = new Position[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;
+        }
+
+        public void Solve(ref TimeStep step, ref Vector2 gravity)
+        {
+            float h = step.dt;
+
+            // Integrate velocities and apply damping. Initialize the body state.
+            for (int i = 0; i < BodyCount; ++i)
+            {
+                Body b = Bodies[i];
+
+                Vector2 c = b._sweep.C;
+                float a = b._sweep.A;
+                Vector2 v = b._linearVelocity;
+                float w = b._angularVelocity;
+
+                // Store positions for continuous collision.
+                b._sweep.C0 = b._sweep.C;
+                b._sweep.A0 = b._sweep.A;
+
+                if (b.BodyType == BodyType.Dynamic)
+                {
+                    // Integrate velocities.
+
+                    // FPE: Only apply gravity if the body wants it.
+                    if (b.IgnoreGravity)
+                        v += h * (b._invMass * b._force);
+                    else
+                        v += h * (b.GravityScale * gravity + b._invMass * b._force);
+
+                    w += h * 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
+                    v *= MathUtils.Clamp(1.0f - h * b.LinearDamping, 0.0f, 1.0f);
+                    w *= MathUtils.Clamp(1.0f - h * b.AngularDamping, 0.0f, 1.0f);
+                }
+
+                _positions[i].c = c;
+                _positions[i].a = a;
+                _velocities[i].v = v;
+                _velocities[i].w = w;
+            }
+
+            // Solver data
+            SolverData solverData = new SolverData();
+            solverData.step = step;
+            solverData.positions = _positions;
+            solverData.velocities = _velocities;
+
+            _contactSolver.Reset(step, ContactCount, _contacts, _positions, _velocities);
+            _contactSolver.InitializeVelocityConstraints();
+
+            if (Settings.EnableWarmstarting)
+            {
+                _contactSolver.WarmStart();
+            }
+
+            if (Settings.EnableDiagnostics)
+                _watch.Start();
+
+            for (int i = 0; i < JointCount; ++i)
+            {
+                if (_joints[i].Enabled)
+                    _joints[i].InitVelocityConstraints(ref solverData);
+            }
+
+            if (Settings.EnableDiagnostics)
+                _watch.Stop();
+
+            // Solve velocity constraints.
+            for (int i = 0; i < Settings.VelocityIterations; ++i)
+            {
+                for (int j = 0; j < JointCount; ++j)
+                {
+                    Joint joint = _joints[j];
+
+                    if (!joint.Enabled)
+                        continue;
+
+                    if (Settings.EnableDiagnostics)
+                        _watch.Start();
+
+                    joint.SolveVelocityConstraints(ref solverData);
+                    joint.Validate(step.inv_dt);
+
+                    if (Settings.EnableDiagnostics)
+                        _watch.Stop();
+                }
+
+                _contactSolver.SolveVelocityConstraints();
+            }
+
+            // Store impulses for warm starting.
+            _contactSolver.StoreImpulses();
+
+            // Integrate positions
+            for (int i = 0; i < BodyCount; ++i)
+            {
+                Vector2 c = _positions[i].c;
+                float a = _positions[i].a;
+                Vector2 v = _velocities[i].v;
+                float w = _velocities[i].w;
+
+                // Check for large velocities
+                Vector2 translation = h * v;
+                if (Vector2.Dot(translation, translation) > Settings.MaxTranslationSquared)
+                {
+                    float ratio = Settings.MaxTranslation / translation.Length();
+                    v *= ratio;
+                }
+
+                float rotation = h * w;
+                if (rotation * rotation > Settings.MaxRotationSquared)
+                {
+                    float ratio = Settings.MaxRotation / Math.Abs(rotation);
+                    w *= ratio;
+                }
+
+                // Integrate
+                c += h * v;
+                a += h * w;
+
+                _positions[i].c = c;
+                _positions[i].a = a;
+                _velocities[i].v = v;
+                _velocities[i].w = w;
+            }
+
+
+            // Solve position constraints
+            bool positionSolved = false;
+            for (int i = 0; i < Settings.PositionIterations; ++i)
+            {
+                bool contactsOkay = _contactSolver.SolvePositionConstraints();
+
+                bool jointsOkay = true;
+                for (int j = 0; j < JointCount; ++j)
+                {
+                    Joint joint = _joints[j];
+
+                    if (!joint.Enabled)
+                        continue;
+
+                    if (Settings.EnableDiagnostics)
+                        _watch.Start();
+
+                    bool jointOkay = joint.SolvePositionConstraints(ref solverData);
+
+                    if (Settings.EnableDiagnostics)
+                        _watch.Stop();
+
+                    jointsOkay = jointsOkay && jointOkay;
+                }
+
+                if (contactsOkay && jointsOkay)
+                {
+                    // Exit early if the position errors are small.
+                    positionSolved = true;
+                    break;
+                }
+            }
+
+            if (Settings.EnableDiagnostics)
+            {
+                JointUpdateTime = _watch.ElapsedTicks;
+                _watch.Reset();
+            }
+
+            // Copy state buffers back to the bodies
+            for (int i = 0; i < BodyCount; ++i)
+            {
+                Body body = Bodies[i];
+                body._sweep.C = _positions[i].c;
+                body._sweep.A = _positions[i].a;
+                body._linearVelocity = _velocities[i].v;
+                body._angularVelocity = _velocities[i].w;
+                body.SynchronizeTransform();
+            }
+
+            Report(_contactSolver._velocityConstraints);
+
+            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.SleepingAllowed || b._angularVelocity * b._angularVelocity > AngTolSqr || Vector2.Dot(b._linearVelocity, b._linearVelocity) > LinTolSqr)
+                    {
+                        b._sleepTime = 0.0f;
+                        minSleepTime = 0.0f;
+                    }
+                    else
+                    {
+                        b._sleepTime += h;
+                        minSleepTime = Math.Min(minSleepTime, b._sleepTime);
+                    }
+                }
+
+                if (minSleepTime >= Settings.TimeToSleep && positionSolved)
+                {
+                    for (int i = 0; i < BodyCount; ++i)
+                    {
+                        Body b = Bodies[i];
+                        b.Awake = false;
+                    }
+                }
+            }
+        }
+
+        internal void SolveTOI(ref TimeStep subStep, int toiIndexA, int toiIndexB, bool warmstarting)
+        {
+            Debug.Assert(toiIndexA < BodyCount);
+            Debug.Assert(toiIndexB < BodyCount);
+
+            // Initialize the body state.
+            for (int i = 0; i < BodyCount; ++i)
+            {
+                Body b = Bodies[i];
+                _positions[i].c = b._sweep.C;
+                _positions[i].a = b._sweep.A;
+                _velocities[i].v = b._linearVelocity;
+                _velocities[i].w = b._angularVelocity;
+            }
+
+            _contactSolver.Reset(subStep, ContactCount, _contacts, _positions, _velocities, warmstarting);
+
+            // Solve position constraints.
+            for (int i = 0; i < Settings.TOIPositionIterations; ++i)
+            {
+                bool contactsOkay = _contactSolver.SolveTOIPositionConstraints(toiIndexA, toiIndexB);
+                if (contactsOkay)
+                {
+                    break;
+                }
+            }
+
+            // Leap of faith to new safe state.
+            Bodies[toiIndexA]._sweep.C0 = _positions[toiIndexA].c;
+            Bodies[toiIndexA]._sweep.A0 = _positions[toiIndexA].a;
+            Bodies[toiIndexB]._sweep.C0 = _positions[toiIndexB].c;
+            Bodies[toiIndexB]._sweep.A0 = _positions[toiIndexB].a;
+
+            // 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.
+
+            float h = subStep.dt;
+
+            // Integrate positions.
+            for (int i = 0; i < BodyCount; ++i)
+            {
+                Vector2 c = _positions[i].c;
+                float a = _positions[i].a;
+                Vector2 v = _velocities[i].v;
+                float w = _velocities[i].w;
+
+                // Check for large velocities
+                Vector2 translation = h * v;
+                if (Vector2.Dot(translation, translation) > Settings.MaxTranslationSquared)
+                {
+                    float ratio = Settings.MaxTranslation / translation.Length();
+                    v *= ratio;
+                }
+
+                float rotation = h * w;
+                if (rotation * rotation > Settings.MaxRotationSquared)
+                {
+                    float ratio = Settings.MaxRotation / Math.Abs(rotation);
+                    w *= ratio;
+                }
+
+                // Integrate
+                c += h * v;
+                a += h * w;
+
+                _positions[i].c = c;
+                _positions[i].a = a;
+                _velocities[i].v = v;
+                _velocities[i].w = w;
+
+                // Sync bodies
+                Body body = Bodies[i];
+                body._sweep.C = c;
+                body._sweep.A = a;
+                body._linearVelocity = v;
+                body._angularVelocity = w;
+                body.SynchronizeTransform();
+            }
+
+            Report(_contactSolver._velocityConstraints);
+        }
+
+        public void Add(Body body)
+        {
+            Debug.Assert(BodyCount < BodyCapacity);
+            body.IslandIndex = BodyCount;
+            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(ContactVelocityConstraint[] constraints)
+        {
+            if (_contactManager == null)
+                return;
+
+            for (int i = 0; i < ContactCount; ++i)
+            {
+                Contact c = _contacts[i];
+
+                //FPE optimization: We don't store the impulses and send it to the delegate. We just send the whole contact.
+                //FPE feature: added after collision
+                if (c.FixtureA.AfterCollision != null)
+                    c.FixtureA.AfterCollision(c.FixtureA, c.FixtureB, c, constraints[i]);
+
+                if (c.FixtureB.AfterCollision != null)
+                    c.FixtureB.AfterCollision(c.FixtureB, c.FixtureA, c, constraints[i]);
+
+                if (_contactManager.PostSolve != null)
+                {
+                    _contactManager.PostSolve(c, constraints[i]);
+                }
+            }
+        }
+    }
+}

Farseer Physics Engine 3.5/Dynamics/Joints/AngleJoint.cs

@@ -0,0 +1,128 @@
+/*
+* Farseer Physics Engine:
+* Copyright (c) 2012 Ian Qvist
+*/
+
+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
+    {
+        private float _bias;
+        private float _jointError;
+        private float _massFactor;
+        private float _targetAngle;
+
+        internal AngleJoint()
+        {
+            JointType = JointType.Angle;
+        }
+
+        /// <summary>
+        /// Constructor for AngleJoint
+        /// </summary>
+        /// <param name="bodyA">The first body</param>
+        /// <param name="bodyB">The second body</param>
+        public AngleJoint(Body bodyA, Body bodyB)
+            : base(bodyA, bodyB)
+        {
+            JointType = JointType.Angle;
+            BiasFactor = .2f;
+            MaxImpulse = float.MaxValue;
+        }
+
+        public override Vector2 WorldAnchorA
+        {
+            get { return BodyA.Position; }
+            set { Debug.Assert(false, "You can't set the world anchor on this joint type."); }
+        }
+
+        public override Vector2 WorldAnchorB
+        {
+            get { return BodyB.Position; }
+            set { Debug.Assert(false, "You can't set the world anchor on this joint type."); }
+        }
+
+        /// <summary>
+        /// The desired angle between BodyA and BodyB
+        /// </summary>
+        public float TargetAngle
+        {
+            get { return _targetAngle; }
+            set
+            {
+                if (value != _targetAngle)
+                {
+                    _targetAngle = value;
+                    WakeBodies();
+                }
+            }
+        }
+
+        /// <summary>
+        /// Gets or sets the bias factor.
+        /// Defaults to 0.2
+        /// </summary>
+        public float BiasFactor { get; set; }
+        
+        /// <summary>
+        /// Gets or sets the maximum impulse
+        /// Defaults to float.MaxValue
+        /// </summary>
+        public float MaxImpulse { get; set; }
+        
+        /// <summary>
+        /// Gets or sets the softness of the joint
+        /// Defaults to 0
+        /// </summary>
+        public float Softness { get; set; }
+
+        public override Vector2 GetReactionForce(float invDt)
+        {
+            //TODO
+            //return _inv_dt * _impulse;
+            return Vector2.Zero;
+        }
+
+        public override float GetReactionTorque(float invDt)
+        {
+            return 0;
+        }
+
+        internal override void InitVelocityConstraints(ref SolverData data)
+        {
+            int indexA = BodyA.IslandIndex;
+            int indexB = BodyB.IslandIndex;
+
+            float aW = data.positions[indexA].a;
+            float bW = data.positions[indexB].a;
+
+            _jointError = (bW - aW - TargetAngle);
+            _bias = -BiasFactor * data.step.inv_dt * _jointError;
+            _massFactor = (1 - Softness) / (BodyA._invI + BodyB._invI);
+        }
+
+        internal override void SolveVelocityConstraints(ref SolverData data)
+        {
+            int indexA = BodyA.IslandIndex;
+            int indexB = BodyB.IslandIndex;
+
+            float p = (_bias - data.velocities[indexB].w + data.velocities[indexA].w) * _massFactor;
+
+            data.velocities[indexA].w -= BodyA._invI * Math.Sign(p) * Math.Min(Math.Abs(p), MaxImpulse);
+            data.velocities[indexB].w += BodyB._invI * Math.Sign(p) * Math.Min(Math.Abs(p), MaxImpulse);
+        }
+
+        internal override bool SolvePositionConstraints(ref SolverData data)
+        {
+            //no position solving for this joint
+            return true;
+        }
+    }
+}

Farseer Physics Engine 3.5/Dynamics/Joints/DistanceJoint.cs

@@ -0,0 +1,331 @@
+/*
+* Farseer Physics Engine:
+* Copyright (c) 2012 Ian Qvist
+* 
+* Original source Box2D:
+* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org 
+* 
+* 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
+    {
+        // Solver shared
+        private float _bias;
+        private float _gamma;
+        private float _impulse;
+
+        // Solver temp
+        private int _indexA;
+        private int _indexB;
+        private Vector2 _u;
+        private Vector2 _rA;
+        private Vector2 _rB;
+        private Vector2 _localCenterA;
+        private Vector2 _localCenterB;
+        private float _invMassA;
+        private float _invMassB;
+        private float _invIA;
+        private float _invIB;
+        private float _mass;
+
+        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="anchorA">The first body anchor</param>
+        /// <param name="anchorB">The second body anchor</param>
+        /// <param name="useWorldCoordinates">Set to true if you are using world coordinates as anchors.</param>
+        public DistanceJoint(Body bodyA, Body bodyB, Vector2 anchorA, Vector2 anchorB, bool useWorldCoordinates = false)
+            : base(bodyA, bodyB)
+        {
+            JointType = JointType.Distance;
+
+            if (useWorldCoordinates)
+            {
+                LocalAnchorA = bodyA.GetLocalPoint(ref anchorA);
+                LocalAnchorB = bodyB.GetLocalPoint(ref anchorB);
+                Length = (anchorB - anchorA).Length();
+            }
+            else
+            {
+                LocalAnchorA = anchorA;
+                LocalAnchorB = anchorB;
+                Length = (BodyB.GetWorldPoint(ref anchorB) - BodyA.GetWorldPoint(ref anchorA)).Length();
+            }
+        }
+
+        /// <summary>
+        /// The local anchor point relative to bodyA's origin.
+        /// </summary>
+        public Vector2 LocalAnchorA { get; set; }
+
+        /// <summary>
+        /// The local anchor point relative to bodyB's origin.
+        /// </summary>
+        public Vector2 LocalAnchorB { get; set; }
+
+        public override sealed Vector2 WorldAnchorA
+        {
+            get { return BodyA.GetWorldPoint(LocalAnchorA); }
+            set { Debug.Assert(false, "You can't set the world anchor on this joint type."); }
+        }
+
+        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."); }
+        }
+
+        /// <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. A value of 0
+        /// disables softness.
+        /// </summary>
+        public float Frequency { get; set; }
+
+        /// <summary>
+        /// The damping ratio. 0 = no damping, 1 = critical damping.
+        /// </summary>
+        public float DampingRatio { get; set; }
+
+        /// <summary>
+        /// Get the reaction force given the inverse time step. Unit is N.
+        /// </summary>
+        /// <param name="invDt"></param>
+        /// <returns></returns>
+        public override Vector2 GetReactionForce(float invDt)
+        {
+            Vector2 F = (invDt * _impulse) * _u;
+            return F;
+        }
+
+        /// <summary>
+        /// Get the reaction torque given the inverse time step.
+        /// Unit is N*m. This is always zero for a distance joint.
+        /// </summary>
+        /// <param name="invDt"></param>
+        /// <returns></returns>
+        public override float GetReactionTorque(float invDt)
+        {
+            return 0.0f;
+        }
+
+        internal override void InitVelocityConstraints(ref SolverData data)
+        {
+            _indexA = BodyA.IslandIndex;
+            _indexB = BodyB.IslandIndex;
+            _localCenterA = BodyA._sweep.LocalCenter;
+            _localCenterB = BodyB._sweep.LocalCenter;
+            _invMassA = BodyA._invMass;
+            _invMassB = BodyB._invMass;
+            _invIA = BodyA._invI;
+            _invIB = BodyB._invI;
+
+            Vector2 cA = data.positions[_indexA].c;
+            float aA = data.positions[_indexA].a;
+            Vector2 vA = data.velocities[_indexA].v;
+            float wA = data.velocities[_indexA].w;
+
+            Vector2 cB = data.positions[_indexB].c;
+            float aB = data.positions[_indexB].a;
+            Vector2 vB = data.velocities[_indexB].v;
+            float wB = data.velocities[_indexB].w;
+
+            Rot qA = new Rot(aA), qB = new Rot(aB);
+
+            _rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA);
+            _rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB);
+            _u = cB + _rB - cA - _rA;
+
+            // Handle singularity.
+            float length = _u.Length();
+            if (length > Settings.LinearSlop)
+            {
+                _u *= 1.0f / length;
+            }
+            else
+            {
+                _u = Vector2.Zero;
+            }
+
+            float crAu = MathUtils.Cross(_rA, _u);
+            float crBu = MathUtils.Cross(_rB, _u);
+            float invMass = _invMassA + _invIA * crAu * crAu + _invMassB + _invIB * crBu * crBu;
+
+            // Compute the effective mass matrix.
+            _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
+                float h = data.step.dt;
+                _gamma = h * (d + h * k);
+                _gamma = _gamma != 0.0f ? 1.0f / _gamma : 0.0f;
+                _bias = C * h * k * _gamma;
+
+                invMass += _gamma;
+                _mass = invMass != 0.0f ? 1.0f / invMass : 0.0f;
+            }
+            else
+            {
+                _gamma = 0.0f;
+                _bias = 0.0f;
+            }
+
+            if (Settings.EnableWarmstarting)
+            {
+                // Scale the impulse to support a variable time step.
+                _impulse *= data.step.dtRatio;
+
+                Vector2 P = _impulse * _u;
+                vA -= _invMassA * P;
+                wA -= _invIA * MathUtils.Cross(_rA, P);
+                vB += _invMassB * P;
+                wB += _invIB * MathUtils.Cross(_rB, P);
+            }
+            else
+            {
+                _impulse = 0.0f;
+            }
+
+            data.velocities[_indexA].v = vA;
+            data.velocities[_indexA].w = wA;
+            data.velocities[_indexB].v = vB;
+            data.velocities[_indexB].w = wB;
+        }
+
+        internal override void SolveVelocityConstraints(ref SolverData data)
+        {
+            Vector2 vA = data.velocities[_indexA].v;
+            float wA = data.velocities[_indexA].w;
+            Vector2 vB = data.velocities[_indexB].v;
+            float wB = data.velocities[_indexB].w;
+
+            // Cdot = dot(u, v + cross(w, r))
+            Vector2 vpA = vA + MathUtils.Cross(wA, _rA);
+            Vector2 vpB = vB + MathUtils.Cross(wB, _rB);
+            float Cdot = Vector2.Dot(_u, vpB - vpA);
+
+            float impulse = -_mass * (Cdot + _bias + _gamma * _impulse);
+            _impulse += impulse;
+
+            Vector2 P = impulse * _u;
+            vA -= _invMassA * P;
+            wA -= _invIA * MathUtils.Cross(_rA, P);
+            vB += _invMassB * P;
+            wB += _invIB * MathUtils.Cross(_rB, P);
+
+            data.velocities[_indexA].v = vA;
+            data.velocities[_indexA].w = wA;
+            data.velocities[_indexB].v = vB;
+            data.velocities[_indexB].w = wB;
+
+        }
+
+        internal override bool SolvePositionConstraints(ref SolverData data)
+        {
+            if (Frequency > 0.0f)
+            {
+                // There is no position correction for soft distance constraints.
+                return true;
+            }
+
+            Vector2 cA = data.positions[_indexA].c;
+            float aA = data.positions[_indexA].a;
+            Vector2 cB = data.positions[_indexB].c;
+            float aB = data.positions[_indexB].a;
+
+            Rot qA = new Rot(aA), qB = new Rot(aB);
+
+            Vector2 rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA);
+            Vector2 rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB);
+            Vector2 u = cB + rB - cA - rA;
+
+            float length = u.Length(); u.Normalize();
+            float C = length - Length;
+            C = MathUtils.Clamp(C, -Settings.MaxLinearCorrection, Settings.MaxLinearCorrection);
+
+            float impulse = -_mass * C;
+            Vector2 P = impulse * u;
+
+            cA -= _invMassA * P;
+            aA -= _invIA * MathUtils.Cross(rA, P);
+            cB += _invMassB * P;
+            aB += _invIB * MathUtils.Cross(rB, P);
+
+            data.positions[_indexA].c = cA;
+            data.positions[_indexA].a = aA;
+            data.positions[_indexB].c = cB;
+            data.positions[_indexB].a = aB;
+
+            return Math.Abs(C) < Settings.LinearSlop;
+        }
+    }
+}