Adding initial files

axios/Collision/Distance.cs

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

axios/Collision/DynamicTree.cs

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

axios/Collision/DynamicTreeBroadPhase.cs

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

axios/Collision/IBroadPhase.cs

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

axios/Collision/QuadTree.cs

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

axios/Collision/QuadTreeBroadPhase.cs

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

axios/Collision/Shapes/CircleShape.cs

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

axios/Collision/Shapes/EdgeShape.cs

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

axios/Collision/Shapes/LoopShape.cs

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

axios/Collision/Shapes/PolygonShape.cs

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

axios/Collision/Shapes/Shape.cs

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

axios/Collision/TimeOfImpact.cs

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