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);
+        }
+    }
+}