code:
/*
* Box2D.XNA port of Box2D:
* Copyright (c) 2009 Brandon Furtwangler, Nathan Furtwangler
*
* Original source Box2D:
* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
*
* This software is provided 'as-is', without any express or implied
* warranty.  In no event will the authors be held liable for any damages
* arising from the use of this software.
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely, subject to the following restrictions:
* 1. The origin of this software must not be misrepresented; you must not
* claim that you wrote the original software. If you use this software
* in a product, an acknowledgment in the product documentation would be
* appreciated but is not required.
* 2. Altered source versions must be plainly marked as such, and must not be
* misrepresented as being the original software.
* 3. This notice may not be removed or altered from any source distribution.
*/

using System;
using System.Diagnostics;
using Microsoft.Xna.Framework;
using System.Collections;
using System.Collections.Generic;

namespace Box2D.XNA
{
    /// A dynamic AABB tree broad-phase, inspired by Nathanael Presson's btDbvt.

    internal delegate float RayCastCallbackInternal(ref RayCastInput input, int userData);

    /// A node in the dynamic tree. The client does not interact with this directly.
    internal struct DynamicTreeNode
    {
        internal bool IsLeaf()
	    {
		    return child1 == DynamicTree.NullNode;
	    }

        /// This is the fattened AABB.
	    internal AABB aabb;

        internal object userData;

        internal int parentOrNext;
        internal int child1;
        internal int child2;

        internal int leafCount;
    };

    /// 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.
    public class DynamicTree
    {
        internal static int NullNode = -1;

	    /// ructing the tree initializes the node pool.
	    public DynamicTree()
        {
	        _root = NullNode;

	        _nodeCapacity = 16;
	        _nodeCount = 0;
	        _nodes = new DynamicTreeNode[_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;
	        _freeList = 0;

	        _path = 0;
        }

	    /// Create a proxy. Provide a tight fitting AABB and a userData pointer.
	    public int CreateProxy(ref AABB aabb, object userData)
        {
	        int proxyId = AllocateNode();

	        // Fatten the aabb.
            Vector2 r = new Vector2(Settings.b2_aabbExtension, Settings.b2_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);

            // Rebalance if necessary.
            int iterationCount = _nodeCount >> 4;
            int tryCount = 0;
            int height = ComputeHeight();
            while (height > 64 && tryCount < 10)
            {
                Rebalance(iterationCount);
                height = ComputeHeight();
                ++tryCount;
            }

	        return proxyId;
        }

	    /// Destroy a proxy. This asserts if the id is invalid.
	    public void DestroyProxy(int proxyId)
        {
	        Debug.Assert(0 <= proxyId && proxyId < _nodeCapacity);
	        Debug.Assert(_nodes[proxyId].IsLeaf());

	        RemoveLeaf(proxyId);
	        FreeNode(proxyId);
        }

        /// 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.
        /// @return true if the proxy was re-inserted.
	    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.b2_aabbExtension, Settings.b2_aabbExtension);
            b.lowerBound = b.lowerBound - r;
            b.upperBound = b.upperBound + r;

            // Predict AABB displacement.
            Vector2 d = Settings.b2_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;
        }

	    /// Perform some iterations to re-balance the tree.
	    public void Rebalance(int iterations)
        {
	        if (_root == NullNode)
	        {
		        return;
	        }

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

	    /// Get proxy user data.
	    /// @return the proxy user data or 0 if the id is invalid.
	    public object GetUserData(int proxyId)
        {
	        Debug.Assert(0 <= proxyId && proxyId < _nodeCapacity);
		    return _nodes[proxyId].userData;
        }

	    /// Get the fat AABB for a proxy.
        public void GetFatAABB(int proxyId, out AABB fatAABB)
        {
	        Debug.Assert(0 <= proxyId && proxyId < _nodeCapacity);
	        fatAABB = _nodes[proxyId].aabb;
        }

        /// Compute the height of the binary tree in O(N) time. Should not be
        /// called often.
	    public int ComputeHeight()
        {
	        return ComputeHeight(_root);
        }

        static Stack<int> stack = new Stack<int>(256);

	    /// Query an AABB for overlapping proxies. The callback class
	    /// is called for each proxy that overlaps the supplied AABB.
	    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 node = _nodes[nodeId];

                if (AABB.TestOverlap(ref node.aabb, ref aabb))
		        {
			        if (node.IsLeaf())
			        {
				        bool proceed = callback(nodeId);
                        if (!proceed)
                        {
                            return;
                        }
			        }
			        else
			        {
                        stack.Push(node.child1);
				        stack.Push(node.child2);
			        }
		        }
	        }
        }

	    /// 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.
	    /// @param input the ray-cast input data. The ray extends from p1 to p1 + maxFraction * (p2 - p1).
	    /// @param callback a callback class that is called for each proxy that is hit by the ray.
	    internal void RayCast(RayCastCallbackInternal callback, ref RayCastInput input)
        {
	        Vector2 p1 = input.p1;
	        Vector2 p2 = input.p2;
	        Vector2 r = p2 - p1;
	        Debug.Assert(r.LengthSquared() > 0.0f);
	        r.Normalize();

	        // v is perpendicular to the segment.
	        Vector2 v = MathUtils.Cross(1.0f, r);
	        Vector2 abs_v = MathUtils.Abs(v);

	        // 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);
		        segmentAABB.lowerBound = Vector2.Min(p1, t);
		        segmentAABB.upperBound = Vector2.Max(p1, t);
	        }

            stack.Clear();
	        stack.Push(_root);

	        while (stack.Count > 0)
	        {
		        int nodeId = stack.Pop();
		        if (nodeId == NullNode)
		        {
			        continue;
		        }

		        DynamicTreeNode 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.GetCenter();
		        Vector2 h = node.aabb.GetExtents();
		        float separation = Math.Abs(Vector2.Dot(v, p1 - c)) - Vector2.Dot(abs_v, h);
		        if (separation > 0.0f)
		        {
			        continue;
		        }

		        if (node.IsLeaf())
		        {
			        RayCastInput subInput;
			        subInput.p1 = input.p1;
			        subInput.p2 = input.p2;
			        subInput.maxFraction = maxFraction;

		        	float value = callback(ref 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 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[] oldNodes = _nodes;
		        _nodeCapacity *= 2;
                _nodes = new DynamicTreeNode[_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;
	        Vector2 leafCenter = leafAABB.GetCenter();
	        int sibling = _root;
	        while (_nodes[sibling].IsLeaf() == false)
	        {
		        // Expand the node's AABB.
		        _nodes[sibling].aabb.Combine(ref leafAABB);
		        _nodes[sibling].leafCount += 1;

		        int child1 = _nodes[sibling].child1;
		        int child2 = _nodes[sibling].child2;

#if false
		        // This seems to create imbalanced trees
		        Vector2 delta1 = Math.Abs(_nodes[child1].aabb.GetCenter() - leafCenter);
		        Vector2 delta2 = Math.Abs(_nodes[child2].aabb.GetCenter() - leafCenter);

		        float norm1 = delta1.x + delta1.y;
		        float norm2 = delta2.x + delta2.y;
#else
		        // Surface area heuristic
		        AABB aabb1 = new AABB();
                AABB  aabb2 = new AABB();
		        aabb1.Combine(ref leafAABB, ref _nodes[child1].aabb);
		        aabb2.Combine(ref leafAABB, ref _nodes[child2].aabb);
		        float norm1 = (_nodes[child1].leafCount + 1) * aabb1.GetPerimeter();
		        float norm2 = (_nodes[child2].leafCount + 1) * aabb2.GetPerimeter();
#endif

		        if (norm1 < norm2)
		        {
			        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 = null;
	        _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)
		        {
			        AABB oldAABB = _nodes[parent].aabb;
			        _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 node = _nodes[nodeId];
	        int height1 = ComputeHeight(node.child1);
	        int height2 = ComputeHeight(node.child2);
	        return 1 + Math.Max(height1, height2);
        }

	    int _root;

	    DynamicTreeNode[] _nodes;
	    int _nodeCount;
	    int _nodeCapacity;

	    int _freeList;

	    /// This is used incrementally traverse the tree for re-balancing.
	    int _path;

        int _insertionCount;
    }
}


code:
/*
* Box2D.XNA port of Box2D:
* Copyright (c) 2009 Brandon Furtwangler, Nathan Furtwangler
*
* Original source Box2D:
* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
*
* This software is provided 'as-is', without any express or implied
* warranty.  In no event will the authors be held liable for any damages
* arising from the use of this software.
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely, subject to the following restrictions:
* 1. The origin of this software must not be misrepresented; you must not
* claim that you wrote the original software. If you use this software
* in a product, an acknowledgment in the product documentation would be
* appreciated but is not required.
* 2. Altered source versions must be plainly marked as such, and must not be
* misrepresented as being the original software.
* 3. This notice may not be removed or altered from any source distribution.
*/

using System;
using System.Diagnostics;
using Microsoft.Xna.Framework;

namespace Box2D.XNA
{
    public class CircleShape : Shape
    {
        public CircleShape()
        {
	        ShapeType = ShapeType.Circle;
	        _radius = 0.0f;
	        _p = Vector2.Zero;
        }

        /// Implement Shape.
        public override Shape Clone()
        {
            CircleShape shape = new CircleShape();
            shape.ShapeType = ShapeType;
            shape._radius = _radius;
            shape._p = _p;

            return shape;
        }

        /// @see b2Shape::GetChildCount
        public override int GetChildCount()
        {
            return 1;
        }

        /// @see Shape.TestPoint
        public override bool TestPoint(ref Transform transform, Vector2 p)
        {
            Vector2 center = transform.Position + MathUtils.Multiply(ref transform.R, _p);
	        Vector2 d = p - center;
	        return Vector2.Dot(d, d) <= _radius * _radius;
        }

        // Collision Detection in Interactive 3D Environments by Gino van den Bergen
        // From Section 3.1.2
        // x = s + a * r
        // norm(x) = radius
        public override bool RayCast(out RayCastOutput output, ref RayCastInput input, ref Transform transform, int childIndex)
        {
            output = new RayCastOutput();

	        Vector2 position = transform.Position + MathUtils.Multiply(ref transform.R, _p);
	        Vector2 s = input.p1 - position;
	        float b = Vector2.Dot(s, s) - _radius * _radius;

	        // Solve quadratic equation.
            Vector2 r = input.p2 - input.p1;
	        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.b2_epsilon)
	        {
                return false;
	        }

	        // Find the point of intersection of the line with the circle.
	        float a = -(c + (float)Math.Sqrt((double)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;
        }

        /// @see Shape.ComputeAABB
        public override void ComputeAABB(out AABB aabb, ref Transform transform, int childIndex)
        {
            Vector2 p = transform.Position + MathUtils.Multiply(ref transform.R, _p);
	        aabb.lowerBound = new Vector2(p.X - _radius, p.Y - _radius);
	        aabb.upperBound = new Vector2(p.X + _radius, p.Y + _radius);
        }

        /// @see Shape.ComputeMass
        public override void ComputeMass(out MassData massData, float density)
        {
            massData.mass = density * Settings.b2_pi * _radius * _radius;
	        massData.center = _p;

	        // inertia about the local origin
	        massData.I = massData.mass * (0.5f * _radius * _radius + Vector2.Dot(_p, _p));
        }

        /// Get the vertex count.
        public int GetVertexCount() { return 1; }

        /// Get a vertex by index. Used by b2Distance.
        public Vector2 GetVertex(int index)
        {
            Debug.Assert(index == 0);
            return _p;
        }

        /// Position
        public Vector2 _p;
    }
}

repo_name: snapshot/xnamototrialracer

license: unknown


code:
/*
* 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.
*/

/// 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.
using Microsoft.Xna.Framework;
namespace Box2D.XNA
{
    public class EdgeShape : Shape
    {
	    public EdgeShape()
        {
            ShapeType = ShapeType.Edge;
	        _radius = Settings.b2_polygonRadius;
	        _hasVertex0 = false;
	        _hasVertex3 = false;
        }

	    /// Set this as an isolated edge.
	    public void Set(Vector2 v1, Vector2 v2)
        {
	        _vertex1 = v1;
	        _vertex2 = v2;
	        _hasVertex0 = false;
	        _hasVertex3 = false;
        }

	    /// Implement Shape.
        public override Shape Clone()
        {
	        var edge = new EdgeShape();
            edge._hasVertex0 = _hasVertex0;
            edge._hasVertex3 = _hasVertex3;
            edge._radius = _radius;
            edge._vertex0 = _vertex0;
            edge._vertex1 = _vertex1;
            edge._vertex2 = _vertex2;
            edge._vertex3 = _vertex3;
            return edge;
        }

	    /// @see Shape::GetChildCount
        public override int GetChildCount()
        {
	        return 1;
        }

	    /// @see Shape::TestPoint
        public override bool TestPoint(ref Transform transform, Vector2 p)
        {
	        return false;
        }

	    /// Implement Shape.
        ///

        // p = p1 + t * d
        // v = v1 + s * e
        // p1 + t * d = v1 + s * e
        // s * e - t * d = p1 - v1
        //
        public override bool RayCast(out RayCastOutput output, ref RayCastInput input,
				            ref Transform transform, int childIndex)
        {
            output = new RayCastOutput();

	        // Put the ray into the edge's frame of reference.
	        Vector2 p1 = MathUtils.MultiplyT(ref transform.R, input.p1 - transform.Position);
	        Vector2 p2 = MathUtils.MultiplyT(ref transform.R, input.p2 - 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;
        }

	    /// @see Shape::ComputeAABB
        public override void ComputeAABB(out AABB aabb, ref Transform transform, int childIndex)
        {
            aabb = new AABB();
            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;
        }

	    /// @see Shape::ComputeMass
        public override void ComputeMass(out MassData massData, float density)
        {
            massData = new MassData();
	        massData.mass = 0.0f;
	        massData.center = 0.5f * (_vertex1 + _vertex2);
	        massData.I = 0.0f;
        }

	    /// These are the edge vertices
	    public Vector2 _vertex1, _vertex2;

	    /// Optional adjacent vertices. These are used for smooth collision.
	    public Vector2 _vertex0, _vertex3;
	    public bool _hasVertex0, _hasVertex3;
    }
}


code:
/*
* 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 Microsoft.Xna.Framework;
using System.Diagnostics;

namespace Box2D.XNA
{
    /// 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.
    public class LoopShape : Shape
    {
	    public LoopShape()
        {
	        ShapeType = ShapeType.Loop;
	        _radius = Settings.b2_polygonRadius;
	        _vertices = null;
	        _count = 0;
        }

	    /// Implement Shape.
	    public override Shape Clone()
        {
            var loop = new LoopShape();
            loop._count = _count;
            loop._radius = _radius;
            loop._vertices = (Vector2[])_vertices.Clone();
            return loop;
        }

	    /// @see Shape::GetChildCount
        public override int GetChildCount()
        {
	        return _count;
        }
	    /// Get a child edge.
        public void GetChildEdge(ref EdgeShape edge, int index)
        {
	        Debug.Assert(2 <= _count);
	        Debug.Assert(0 <= index && index < _count);
	        edge.ShapeType = ShapeType.Edge;
	        edge._radius = _radius;
	        edge._hasVertex0 = true;
	        edge._hasVertex3 = true;

	        int i0 = index - 1 >= 0 ? index - 1 : _count - 1;
	        int i1 = index;
	        int i2 = index + 1 < _count ? index + 1 : 0;
	        int i3 = index + 2;
	        while (i3 >= _count)
	        {
		        i3 -= _count;
	        }

	        edge._vertex0 = _vertices[i0];
	        edge._vertex1 = _vertices[i1];
	        edge._vertex2 = _vertices[i2];
	        edge._vertex3 = _vertices[i3];
        }
	    /// This always return false.
	    /// @see Shape::TestPoint
        public override bool TestPoint(ref Transform transform, Vector2 p)
        {
	        return false;
        }

	    /// Implement Shape.
        public override bool RayCast(out RayCastOutput output, ref RayCastInput input,
					    ref Transform transform, int childIndex)
        {
	        Debug.Assert(childIndex < _count);

	        int i1 = childIndex;
	        int i2 = childIndex + 1;
	        if (i2 == _count)
	        {
		        i2 = 0;
	        }

	        s_edgeShape._vertex1 = _vertices[i1];
	        s_edgeShape._vertex2 = _vertices[i2];

	        return s_edgeShape.RayCast(out output, ref input, ref transform, 0);
        }

	    /// @see Shape::ComputeAABB
        public override void ComputeAABB(out AABB aabb, ref Transform transform, int childIndex)
        {
	        Debug.Assert(childIndex < _count);
            aabb = new AABB();

	        int i1 = childIndex;
	        int i2 = childIndex + 1;
	        if (i2 == _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);
        }

	    /// Chains have zero mass.
	    /// @see Shape::ComputeMass
        public override void ComputeMass(out MassData massData, float density)
        {
            massData = new MassData();
	        massData.mass = 0.0f;
	        massData.center = Vector2.Zero;
	        massData.I = 0.0f;
        }

	    /// The vertices. These are not owned/freed by the loop Shape.
	    public Vector2[] _vertices;

	    /// The vertex count.
	    public int _count;

        public static EdgeShape s_edgeShape = new EdgeShape();
    }
}


code:
/*
* 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 Microsoft.Xna.Framework;

namespace Box2D.XNA
{
    public class PolygonShape : Shape
    {
	    public PolygonShape()
        {
	        ShapeType = ShapeType.Polygon;
	        _radius = Settings.b2_polygonRadius;
	        _vertexCount = 0;
	        _centroid = Vector2.Zero;
        }

	    /// Implement Shape.
	    public override Shape Clone()
        {
            var clone = new PolygonShape();
            clone.ShapeType = ShapeType;
            clone._radius = _radius;
            clone._vertexCount = _vertexCount;
            clone._centroid = _centroid;
            clone._vertices = _vertices;
            clone._normals = _normals;

            return clone;
        }

        /// @see b2Shape::GetChildCount
        public override int GetChildCount()
        {
            return 1;
        }

	    /// Copy vertices. This assumes the vertices define a convex polygon.
	    /// It is assumed that the exterior is the the right of each edge.
        public void Set(Vector2[] vertices, int count)
        {
            Debug.Assert(2 <= count && count <= Settings.b2_maxPolygonVertices);
            _vertexCount = count;

            // Copy vertices.
            for (int i = 0; i < _vertexCount; ++i)
            {
	            _vertices[i] = vertices[i];
            }

            // Compute normals. Ensure the edges have non-zero length.
            for (int i = 0; i < _vertexCount; ++i)
            {
	            int i1 = i;
	            int i2 = i + 1 < _vertexCount ? i + 1 : 0;
	            Vector2 edge = _vertices[i2] - _vertices[i1];
	            Debug.Assert(edge.LengthSquared() > Settings.b2_epsilon * Settings.b2_epsilon);

                var temp = MathUtils.Cross(edge, 1.0f);
                temp.Normalize();
                _normals[i] = temp;
            }

        #if _DEBUG
            // Ensure the polygon is convex and the interior
            // is to the left of each edge.
            for (int i = 0; i < _vertexCount; ++i)
            {
	            int i1 = i;
	            int i2 = i + 1 < _vertexCount ? i + 1 : 0;
	            Vector2 edge = _vertices[i2] - _vertices[i1];

	            for (int j = 0; j < _vertexCount; ++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 = MathUtils.Cross(edge, r);
		            Debug.Assert(s > 0.0f);
	            }
            }
        #endif

            // Compute the polygon centroid.
            _centroid = ComputeCentroid(ref _vertices, _vertexCount);
        }

        static Vector2 ComputeCentroid(ref FixedArray8<Vector2> vs, int count)
        {
	        Debug.Assert(count >= 2);

	        Vector2 c = new Vector2(0.0f, 0.0f);
	        float area = 0.0f;

	        if (count == 2)
	        {
		        c = 0.5f * (vs[0] + vs[1]);
		        return c;
	        }

	        // pRef is the reference point for forming triangles.
	        // It's location doesn't change the result (except for rounding error).
	        Vector2 pRef = new Vector2(0.0f, 0.0f);
        #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 < count; ++i)
	        {
		        // Triangle vertices.
		        Vector2 p1 = pRef;
		        Vector2 p2 = vs[i];
		        Vector2 p3 = i + 1 < count ? vs[i+1] : vs[0];

		        Vector2 e1 = p2 - p1;
		        Vector2 e2 = p3 - p1;

		        float D = MathUtils.Cross(e1, e2);

		        float triangleArea = 0.5f * D;
		        area += triangleArea;

		        // Area weighted centroid
		        c += triangleArea * inv3 * (p1 + p2 + p3);
	        }

	        // Centroid
	        Debug.Assert(area > Settings.b2_epsilon);
	        c *= 1.0f / area;
	        return c;
        }

	    /// Build vertices to represent an axis-aligned box.
	    /// @param hx the half-width.
	    /// @param hy the half-height.
        public void SetAsBox(float hx, float hy)
        {
	        _vertexCount = 4;
	        _vertices[0] = new Vector2(-hx, -hy);
	        _vertices[1] = new Vector2( hx, -hy);
	        _vertices[2] = new Vector2( hx,  hy);
	        _vertices[3] = new Vector2(-hx,  hy);
	        _normals[0] = new Vector2(0.0f, -1.0f);
	        _normals[1] = new Vector2(1.0f, 0.0f);
	        _normals[2] = new Vector2(0.0f, 1.0f);
	        _normals[3] = new Vector2(-1.0f, 0.0f);
	        _centroid = Vector2.Zero;
        }

	    /// Build vertices to represent an oriented box.
	    /// @param hx the half-width.
	    /// @param hy the half-height.
	    /// @param center the center of the box in local coordinates.
	    /// @param angle the rotation of the box in local coordinates.
        public void SetAsBox(float hx, float hy, Vector2 center, float angle)
        {
	        _vertexCount = 4;
	        _vertices[0] = new Vector2(-hx, -hy);
	        _vertices[1] = new Vector2( hx, -hy);
	        _vertices[2] = new Vector2( hx,  hy);
	        _vertices[3] = new Vector2(-hx,  hy);
	        _normals[0] = new Vector2(0.0f, -1.0f);
	        _normals[1] = new Vector2(1.0f, 0.0f);
	        _normals[2] = new Vector2(0.0f, 1.0f);
	        _normals[3] = new Vector2(-1.0f, 0.0f);
	        _centroid = center;

            Transform xf = new Transform();
	        xf.Position = center;
	        xf.R.Set(angle);

	        // Transform vertices and normals.
	        for (int i = 0; i < _vertexCount; ++i)
	        {
		        _vertices[i] = MathUtils.Multiply(ref xf, _vertices[i]);
		        _normals[i] = MathUtils.Multiply(ref xf.R, _normals[i]);
	        }
        }

	    /// Set this as a single edge.
        public void SetAsEdge(Vector2 v1, Vector2 v2)
        {
	        _vertexCount = 2;
	        _vertices[0] = v1;
	        _vertices[1] = v2;
	        _centroid = 0.5f * (v1 + v2);

            var temp = MathUtils.Cross(v2 - v1, 1.0f);
            temp.Normalize();
            _normals[0] = temp;

	        _normals[1] = -_normals[0];
        }

	    /// @see Shape.TestPoint
	    public override bool TestPoint(ref Transform xf, Vector2 p)
        {
	        Vector2 pLocal = MathUtils.MultiplyT(ref xf.R, p - xf.Position);

	        for (int i = 0; i < _vertexCount; ++i)
	        {
		        float dot = Vector2.Dot(_normals[i], pLocal - _vertices[i]);
		        if (dot > 0.0f)
		        {
			        return false;
		        }
	        }

	        return true;
        }

        public override bool RayCast(out RayCastOutput output, ref RayCastInput input, ref Transform xf, int childIndex)
        {
            output = new RayCastOutput();

            // Put the ray into the polygon's frame of reference.
            Vector2 p1 = MathUtils.MultiplyT(ref xf.R, input.p1 - xf.Position);
            Vector2 p2 = MathUtils.MultiplyT(ref xf.R, input.p2 - xf.Position);
            Vector2 d = p2 - p1;

            if (_vertexCount == 2)
            {
                Vector2 v1 = _vertices[0];
                Vector2 v2 = _vertices[1];
                Vector2 normal = _normals[0];

                // 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;
            }
            else
            {
                float lower = 0.0f, upper = input.maxFraction;

                int index = -1;

                for (int i = 0; i < _vertexCount; ++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 xf.R, _normals[index]);
                    return true;
                }
            }

            return false;
        }

	    /// @see Shape.ComputeAABB
        public override void ComputeAABB(out AABB aabb, ref Transform xf, int childIndex)
        {
	        Vector2 lower = MathUtils.Multiply(ref xf, _vertices[0]);
	        Vector2 upper = lower;

	        for (int i = 1; i < _vertexCount; ++i)
	        {
		        Vector2 v = MathUtils.Multiply(ref xf, _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;
        }

	    /// @see Shape.ComputeMass
	    public override void ComputeMass(out MassData massData, float density)
        {
	        // 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(_vertexCount >= 2);

            // A line segment has zero mass.
            if (_vertexCount == 2)
            {
                massData.center = 0.5f * (_vertices[0] + _vertices[1]);
                massData.mass = 0.0f;
                massData.I = 0.0f;
                return;
            }

	        Vector2 center = new Vector2(0.0f, 0.0f);
	        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 = new Vector2(0.0f, 0.0f);

	        const float k_inv3 = 1.0f / 3.0f;

	        for (int i = 0; i < _vertexCount; ++i)
	        {
		        // Triangle vertices.
		        Vector2 p1 = pRef;
		        Vector2 p2 = _vertices[i];
		        Vector2 p3 = i + 1 < _vertexCount ? _vertices[i+1] : _vertices[0];

		        Vector2 e1 = p2 - p1;
		        Vector2 e2 = p3 - p1;

		        float D = MathUtils.Cross(e1, e2);

		        float triangleArea = 0.5f * D;
		        area += triangleArea;

		        // Area weighted centroid
		        center += triangleArea * k_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 = k_inv3 * (0.25f * (ex1*ex1 + ex2*ex1 + ex2*ex2) + (px*ex1 + px*ex2)) + 0.5f*px*px;
		        float inty2 = k_inv3 * (0.25f * (ey1*ey1 + ey2*ey1 + ey2*ey2) + (py*ey1 + py*ey2)) + 0.5f*py*py;

		        I += D * (intx2 + inty2);
	        }

	        // Total mass
	        massData.mass = density * area;

	        // Center of mass
	        Debug.Assert(area > Settings.b2_epsilon);
	        center *= 1.0f / area;
	        massData.center = center;

	        // Inertia tensor relative to the local origin.
	        massData.I = density * I;
        }

	    /// Get the vertex count.
        public int GetVertexCount() { return _vertexCount; }

	    /// Get a vertex by index.
        public Vector2 GetVertex(int index)
        {
	        Debug.Assert(0 <= index && index < _vertexCount);
	        return _vertices[index];
        }

        public Vector2 _centroid;
        public FixedArray8<Vector2> _vertices;
        public FixedArray8<Vector2> _normals;
	    public int _vertexCount;
    }
}


code:
/*
* 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 Microsoft.Xna.Framework;

namespace Box2D.XNA
{
    /// This holds the mass data computed for a shape.
    public struct MassData
    {
        /// The mass of the shape, usually in kilograms.
        public float mass;

        /// The position of the shape's centroid relative to the shape's origin.
        public Vector2 center;

        /// The rotational inertia of the shape about the local origin.
        public float I;
    };

    public enum ShapeType
    {
        Unknown = -1,
        Circle = 0,
        Edge = 1,
        Polygon = 2,
        Loop = 3,
        TypeCount = 4,
    };

    /// A shape is used for collision detection. You can create a shape however you like.
    /// Shapes used for simulation in b2World are created automatically when a b2Fixture
    /// is created. Shapes may encapsulate a one or more child shapes.
    public abstract class Shape
    {
	    public Shape()
        {
            ShapeType = ShapeType.Unknown;
        }

	    /// Clone the concrete shape using the provided allocator.
	    public abstract Shape Clone();

	    /// Get the type of this shape. You can use this to down cast to the concrete shape.
	    /// @return the shape type.
        public ShapeType ShapeType { get; internal set; }

        /// Get the number of child primitives.
        public abstract int GetChildCount();

	    /// Test a point for containment in this shape. This only works for convex shapes.
	    /// @param xf the shape world transform.
	    /// @param p a point in world coordinates.
	    public abstract bool TestPoint(ref Transform xf, Vector2 p);

        /// Cast a ray against a child shape.
	    /// @param output the ray-cast results.
	    /// @param input the ray-cast input parameters.
	    /// @param transform the transform to be applied to the shape.
        /// @param childIndex the child shape index
        public abstract bool RayCast(out RayCastOutput output, ref RayCastInput input, ref Transform transform, int childIndex);

	    /// Given a transform, compute the associated axis aligned bounding box for a child shape.
	    /// @param aabb returns the axis aligned box.
	    /// @param xf the world transform of the shape.
	    public abstract void ComputeAABB(out AABB aabb, ref Transform xf, int childIndex);

	    /// Compute the mass properties of this shape using its dimensions and density.
	    /// The inertia tensor is computed about the local origin, not the centroid.
	    /// @param massData returns the mass data for this shape.
	    /// @param density the density in kilograms per meter squared.
	    public abstract void ComputeMass(out MassData massData, float density);

	    public float _radius;
    }
}


code:
/*
* Box2D.XNA port of Box2D:
* Copyright (c) 2009 Brandon Furtwangler, Nathan Furtwangler
*
* Original source Box2D:
* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
*
* This software is provided 'as-is', without any express or implied
* warranty.  In no event will the authors be held liable for any damages
* arising from the use of this software.
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely, subject to the following restrictions:
* 1. The origin of this software must not be misrepresented; you must not
* claim that you wrote the original software. If you use this software
* in a product, an acknowledgment in the product documentation would be
* appreciated but is not required.
* 2. Altered source versions must be plainly marked as such, and must not be
* misrepresented as being the original software.
* 3. This notice may not be removed or altered from any source distribution.
*/

using System;
using System.Diagnostics;
using Microsoft.Xna.Framework;

namespace Box2D.XNA
{
    /// Input parameters for CalculateTimeOfImpact
    public struct TOIInput
    {
        public DistanceProxy proxyA;
        public DistanceProxy proxyB;
        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 struct SeparationFunction
    {
        public SeparationFunction(ref SimplexCache cache,
            ref DistanceProxy proxyA, ref Sweep sweepA,
            ref 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.GetVertex(cache.indexA[0]);
                Vector2 localPointB = _proxyB.GetVertex(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.GetVertex(cache.indexB[0]);
                Vector2 localPointB2 = proxyB.GetVertex(cache.indexB[1]);

			    _axis = MathUtils.Cross(localPointB2 - localPointB1, 1.0f);
			    _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.GetVertex(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.GetVertex(cache.indexA[0]);
			    Vector2 localPointA2 = _proxyA.GetVertex(cache.indexA[1]);

			    _axis = MathUtils.Cross(localPointA2 - localPointA1, 1.0f);
			    _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.GetVertex(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 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.GetVertex(indexA);
				    Vector2 localPointB = _proxyB.GetVertex(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.GetVertex(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.GetVertex(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 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.GetVertex(indexA);
                    Vector2 localPointB = _proxyB.GetVertex(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.GetVertex(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.GetVertex(indexA);
                    Vector2 pointA = MathUtils.Multiply(ref xfA, localPointA);

				    float separation = Vector2.Dot(pointA - pointB, normal);
				    return separation;
			    }

		    default:
			    Debug.Assert(false);
			    return 0.0f;
		    }
	    }

        DistanceProxy _proxyA;
        DistanceProxy _proxyB;
        Sweep _sweepA, _sweepB;
        SeparationFunctionType _type;
	    Vector2 _localPoint;
	    Vector2 _axis;
    };

    public static class TimeOfImpact
    {
        // CCD via the local separating axis method. This seeks progression
        // by computing the largest time at which separation is maintained.
        /// 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 b2Distance to compute the contact point and normal at the time of impact.
        public static void CalculateTimeOfImpact(out TOIOutput output, ref TOIInput input)
        {
	        ++b2_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.b2_linearSlop, totalRadius - 3.0f * Settings.b2_linearSlop);
            float tolerance = 0.25f * Settings.b2_linearSlop;
            Debug.Assert(target > tolerance);

            float t1 = 0.0f;
	        int k_maxIterations = 20;
	        int iter = 0;

	        // Prepare input for distance query.
            SimplexCache cache;
	        DistanceInput distanceInput;
            distanceInput.proxyA = input.proxyA;
            distanceInput.proxyB = input.proxyB;
	        distanceInput.useRadii = false;

            // The outer loop progressively attempts to compute new separating axes.
            // This loop terminates when an axis is repeated (no progress is made).
	        for(;;)
	        {
		        Transform xfA, xfB;
		        sweepA.GetTransform(out xfA, t1);
                sweepB.GetTransform(out xfB, t1);

                // Get the distance between shapes. We can also use the results
                // to get a separating axis.
		        distanceInput.transformA = xfA;
		        distanceInput.transformB = xfB;
		        DistanceOutput distanceOutput;
		        Distance.ComputeDistance(out distanceOutput, out cache, ref 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 fcn = new SeparationFunction(ref cache, ref input.proxyA, ref sweepA, ref 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 = fcn.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 = fcn.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 = fcn.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;
				        ++b2_toiRootIters;

				        if (rootIterCount == 50)
				        {
					        break;
				        }
			        }

			        b2_toiMaxRootIters = Math.Max(b2_toiMaxRootIters, rootIterCount);

                    ++pushBackIter;

			        if (pushBackIter == Settings.b2_maxPolygonVertices)
			        {
				        break;
			        }
		        }

		        ++iter;
		        ++b2_toiIters;

		        if (done)
		        {
			        break;
		        }

                if (iter == k_maxIterations)
		        {
			        // Root finder got stuck. Semi-victory.
			        output.State = TOIOutputState.Failed;
			        output.t = t1;
			        break;
		        }
	        }

	        b2_toiMaxIters = Math.Max(b2_toiMaxIters, iter);
        }

        public static int b2_toiCalls, b2_toiIters, b2_toiMaxIters;
        public static int b2_toiRootIters, b2_toiMaxRootIters;
        public static int b2_toiMaxOptIters;
    }
}


code:
/*
* Box2D.XNA port of Box2D:
* Copyright (c) 2009 Brandon Furtwangler, Nathan Furtwangler
*
* Original source Box2D:
* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
*
* This software is provided 'as-is', without any express or implied
* warranty.  In no event will the authors be held liable for any damages
* arising from the use of this software.
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely, subject to the following restrictions:
* 1. The origin of this software must not be misrepresented; you must not
* claim that you wrote the original software. If you use this software
* in a product, an acknowledgment in the product documentation would be
* appreciated but is not required.
* 2. Altered source versions must be plainly marked as such, and must not be
* misrepresented as being the original software.
* 3. This notice may not be removed or altered from any source distribution.
*/

using System;

namespace Box2D.XNA
{
    public struct FixedArray2<T>
    {
        public T this[int index]
        {
            get
            {
                switch (index)
                {
                    case 0:
                        return _value0;
                    case 1:
                        return _value1;
                    default:
                        throw new IndexOutOfRangeException();
                }
            }
            set
            {
                switch (index)
                {
                    case 0:
                        _value0 = value;
                        break;
                    case 1:
                        _value1 = value;
                        break;
                    default:
                        throw new IndexOutOfRangeException();
                }
            }
        }

        T _value0;
        T _value1;
    }

    public struct FixedArray3<T>
    {
        public T this[int index]
        {
            get
            {
                switch (index)
                {
                    case 0:
                        return _value0;
                    case 1:
                        return _value1;
                    case 2:
                        return _value2;
                    default:
                        throw new IndexOutOfRangeException();
                }
            }
            set
            {
                switch (index)
                {
                    case 0:
                        _value0 = value;
                        break;
                    case 1:
                        _value1 = value;
                        break;
                    case 2:
                        _value2 = value;
                        break;
                    default:
                        throw new IndexOutOfRangeException();
                }
            }
        }

        T _value0;
        T _value1;
        T _value2;
    }

    public struct FixedArray4<T>
    {
        public T this[int index]
        {
            get
            {
                switch (index)
                {
                    case 0:
                        return _value0;
                    case 1:
                        return _value1;
                    case 2:
                        return _value2;
                    case 3:
                        return _value3;
                    default:
                        throw new IndexOutOfRangeException();
                }
            }
            set
            {
                switch (index)
                {
                    case 0:
                        _value0 = value;
                        break;
                    case 1:
                        _value1 = value;
                        break;
                    case 2:
                        _value2 = value;
                        break;
                    case 3:
                        _value3 = value;
                        break;
                    default:
                        throw new IndexOutOfRangeException();
                }
            }
        }

        T _value0;
        T _value1;
        T _value2;
        T _value3;
    }

    public struct FixedArray8<T>
    {
        public T this[int index]
        {
            get
            {
                switch (index)
                {
                    case 0:
                        return _value0;
                    case 1:
                        return _value1;
                    case 2:
                        return _value2;
                    case 3:
                        return _value3;
                    case 4:
                        return _value4;
                    case 5:
                        return _value5;
                    case 6:
                        return _value6;
                    case 7:
                        return _value7;
                    default:
                        throw new IndexOutOfRangeException();
                }
            }
            set
            {
                switch (index)
                {
                    case 0:
                        _value0 = value;
                        break;
                    case 1:
                        _value1 = value;
                        break;
                    case 2:
                        _value2 = value;
                        break;
                    case 3:
                        _value3 = value;
                        break;
                    case 4:
                        _value4 = value;
                        break;
                    case 5:
                        _value5 = value;
                        break;
                    case 6:
                        _value6 = value;
                        break;
                    case 7:
                        _value7 = value;
                        break;
                    default:
                        throw new IndexOutOfRangeException();
                }
            }
        }

        T _value0;
        T _value1;
        T _value2;
        T _value3;
        T _value4;
        T _value5;
        T _value6;
        T _value7;
    }
}


code:
/*
* 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.
*/

#ifndef B2_GROWABLE_STACK_H
#define B2_GROWABLE_STACK_H
#include <Box2D/Common/b2Settings.h>

/// This is a growable LIFO stack with an initial capacity of N.
/// If the stack size exceeds the initial capacity, the heap is used
/// to increase the size of the stack.
template <typename T, int32 N>
class b2GrowableStack
{
public:
	b2GrowableStack()
	{
		m_stack = m_array;
		m_count = 0;
		m_capacity = N;
	}

	~b2GrowableStack()
	{
		if (m_stack != m_array)
		{
			b2Free(m_stack);
			m_stack = NULL;
		}
	}

	void Push(const T& element)
	{
		if (m_count == m_capacity)
		{
			T* old = m_stack;
			m_capacity *= 2;
			m_stack = (T*)b2Alloc(m_capacity * sizeof(T));
			memcpy(m_stack, old, m_count * sizeof(T));
			if (old != m_array)
			{
				b2Free(old);
			}
		}

		m_stack[m_count] = element;
		++m_count;
	}

	T Pop()
	{
		b2Assert(m_count > 0);
		--m_count;
		return m_stack[m_count];
	}

	int32 GetCount()
	{
		return m_count;
	}

private:
	T* m_stack;
	T m_array[N];
	int32 m_count;
	int32 m_capacity;
};

#endif


code:
/*
* 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.Runtime.InteropServices;
using Microsoft.Xna.Framework;

namespace Box2D.XNA
{
    public static class MathUtils
    {
        public static float Cross(Vector2 a, Vector2 b)
        {
            return a.X * b.Y - a.Y * b.X;
        }

        public static Vector2 Cross(Vector2 a, float s)
        {
            return new Vector2(s * a.Y, -s * a.X);
        }

        public static Vector2 Cross(float s, Vector2 a)
        {
            return new Vector2(-s * a.Y, s * a.X);
        }

        public static Vector2 Abs(Vector2 v)
        {
            return new Vector2(Math.Abs(v.X), Math.Abs(v.Y));
        }

        public static Vector2 Multiply(ref Mat22 A, Vector2 v)
        {
            return new Vector2(A.col1.X * v.X + A.col2.X * v.Y, A.col1.Y * v.X + A.col2.Y * v.Y);
        }

        public static Vector2 MultiplyT(ref Mat22 A, Vector2 v)
        {
            return new Vector2(Vector2.Dot(v, A.col1), Vector2.Dot(v, A.col2));
        }

        public static Vector2 Multiply(ref Transform T, Vector2 v)
        {
            float x = T.Position.X + T.R.col1.X * v.X + T.R.col2.X * v.Y;
            float y = T.Position.Y + T.R.col1.Y * v.X + T.R.col2.Y * v.Y;

            return new Vector2(x, y);
        }

        public static Vector2 MultiplyT(ref Transform T, Vector2 v)
        {
            return MultiplyT(ref T.R, v - T.Position);
        }

        // A^T * B
        public static void MultiplyT(ref Mat22 A, ref Mat22 B, out Mat22 C)
        {
            Vector2 c1 = new Vector2(Vector2.Dot(A.col1, B.col1), Vector2.Dot(A.col2, B.col1));
            Vector2 c2 = new Vector2(Vector2.Dot(A.col1, B.col2), Vector2.Dot(A.col2, B.col2));
	        C = new Mat22(c1, c2);
        }

        public static void MultiplyT(ref Transform A, ref Transform B, out Transform C)
        {
            Mat22 R;
            MultiplyT(ref A.R, ref B.R, out R);
            C = new Transform(B.Position - A.Position, ref R);
        }

        public static void Swap<T>(ref T a, ref T b)
        {
            T tmp = a;
            a = b;
            b = tmp;
        }

        /// This function is used to ensure that a floating point number is
        /// not a NaN or infinity.
        public static bool IsValid(float x)
        {
            if (float.IsNaN(x))
            {
                // NaN.
                return false;
            }

            return !float.IsInfinity(x);
        }

        public static bool IsValid(this Vector2 x)
        {
            return IsValid(x.X) && IsValid(x.Y);
        }

        [StructLayout(LayoutKind.Explicit)]
        internal struct FloatConverter
        {
            [FieldOffset(0)]
            public float x;
            [FieldOffset(0)]
            public int i;
        };

        /// This is a approximate yet fast inverse square-root.
        public static float InvSqrt(float x)
        {
            FloatConverter convert = new FloatConverter();
            convert.x = x;
            float xhalf = 0.5f * x;
            convert.i = 0x5f3759df - (convert.i >> 1);
            x = convert.x;
            x = x * (1.5f - xhalf * x * x);
            return x;
        }

        public static int Clamp(int a, int low, int high)
        {
            return Math.Max(low, Math.Min(a, high));
        }

        public static float Clamp(float a, float low, float high)
        {
            return Math.Max(low, Math.Min(a, high));
        }

        public static Vector2 Clamp(Vector2 a, Vector2 low, Vector2 high)
        {
            return Vector2.Max(low, Vector2.Min(a, high));
        }
    }

        /// A 2-by-2 matrix. Stored in column-major order.
    public struct Mat22
    {
	    /// construct this matrix using columns.
        public Mat22(Vector2 c1, Vector2 c2)
	    {
		    col1 = c1;
		    col2 = c2;
	    }

	    /// construct this matrix using scalars.
        public Mat22(float a11, float a12, float a21, float a22)
	    {
            col1 = new Vector2(a11, a21);
            col2 = new Vector2(a12, a22);
		}

	    /// construct this matrix using an angle. This matrix becomes
	    /// an orthonormal rotation matrix.
        public Mat22(float angle)
	    {
		    // TODO_ERIN compute sin+cos together.
            float c = (float)Math.Cos(angle), s = (float)Math.Sin(angle);
            col1 = new Vector2(c, s);
            col2 = new Vector2(-s, c);
	    }

	    /// Initialize this matrix using columns.
        public void Set(Vector2 c1, Vector2 c2)
	    {
		    col1 = c1;
		    col2 = c2;
	    }

	    /// Initialize this matrix using an angle. This matrix becomes
	    /// an orthonormal rotation matrix.
        public void Set(float angle)
	    {
            float c = (float)Math.Cos(angle), s = (float)Math.Sin(angle);
		    col1.X = c; col2.X = -s;
		    col1.Y = s; col2.Y = c;
	    }

	    /// Set this to the identity matrix.
        public void SetIdentity()
	    {
		    col1.X = 1.0f; col2.X = 0.0f;
		    col1.Y = 0.0f; col2.Y = 1.0f;
	    }

	    /// Set this matrix to all zeros.
        public void SetZero()
	    {
		    col1.X = 0.0f; col2.X = 0.0f;
		    col1.Y = 0.0f; col2.Y = 0.0f;
	    }

	    /// Extract the angle from this matrix (assumed to be
	    /// a rotation matrix).
        public float GetAngle()
	    {
            return (float)Math.Atan2((double)col1.Y, (double)col1.X);
	    }

        public Mat22 GetInverse()
	    {
		    float a = col1.X, b = col2.X, c = col1.Y, d = col2.Y;
		    float det = a * d - b * c;
            if (det != 0.0f)
            {
                det = 1.0f / det;
            }

            return new Mat22(new Vector2(det * d, -det * c), new Vector2(-det * b, det * a));
	    }

	    /// Solve A * x = b, where b is a column vector. This is more efficient
	    /// than computing the inverse in one-shot cases.
        public Vector2 Solve(Vector2 b)
	    {
		    float a11 = col1.X, a12 = col2.X, a21 = col1.Y, a22 = col2.Y;
		    float det = a11 * a22 - a12 * a21;
            if (det != 0.0f)
            {
                det = 1.0f / det;
            }

            return new Vector2(det * (a22 * b.X - a12 * b.Y), det * (a11 * b.Y - a21 * b.X));
	    }

        public static void Add (ref Mat22 A, ref Mat22 B, out Mat22 R)
        {
            R = new Mat22(A.col1 + B.col1, A.col2 + B.col2);
        }

        public Vector2 col1, col2;
    };

    /// A 3-by-3 matrix. Stored in column-major order.
    public struct Mat33
    {

	    /// construct this matrix using columns.
        public Mat33(Vector3 c1, Vector3 c2, Vector3 c3)
	    {
		    col1 = c1;
		    col2 = c2;
		    col3 = c3;
	    }

	    /// Set this matrix to all zeros.
        public void SetZero()
	    {
		    col1 = Vector3.Zero;
		    col2 = Vector3.Zero;
		    col3 = Vector3.Zero;
	    }

	    /// Solve A * x = b, where b is a column vector. This is more efficient
	    /// than computing the inverse in one-shot cases.
        public Vector3 Solve33(Vector3 b)
        {
            float det = Vector3.Dot(col1, Vector3.Cross(col2, col3));
            if (det != 0.0f)
            {
                det = 1.0f / det;
            }

            return new Vector3( det * Vector3.Dot(b, Vector3.Cross(col2, col3)),
                                det * Vector3.Dot(col1, Vector3.Cross(b, col3)),
                                det * Vector3.Dot(col1, Vector3.Cross(col2, b)));
        }

	    /// Solve A * x = b, where b is a column vector. This is more efficient
	    /// than computing the inverse in one-shot cases. Solve only the upper
	    /// 2-by-2 matrix equation.
        public Vector2 Solve22(Vector2 b)
        {
            float a11 = col1.X, a12 = col2.X, a21 = col1.Y, a22 = col2.Y;
            float det = a11 * a22 - a12 * a21;

            if (det != 0.0f)
            {
                det = 1.0f / det;
            }

            return new Vector2(det * (a22 * b.X - a12 * b.Y), det * (a11 * b.Y - a21 * b.X));
        }

        public Vector3 col1, col2, col3;
    }

    /// A transform contains translation and rotation. It is used to represent
    /// the position and orientation of rigid frames.
    public struct Transform
    {
	    /// Initialize using a position vector and a rotation matrix.
        public Transform(Vector2 position, ref Mat22 r)
        {
            Position = position;
            R = r;
        }

	    /// Set this to the identity transform.
        public void SetIdentity()
	    {
		    Position = Vector2.Zero;
		    R.SetIdentity();
	    }

	    /// Set this based on the position and angle.
        public void Set(Vector2 p, float angle)
	    {
		    Position = p;
		    R.Set(angle);
	    }

	    /// Calculate the angle that the rotation matrix represents.
        public float GetAngle()
	    {
		    return (float)Math.Atan2((double)R.col1.Y, (double)R.col1.X);
	    }

        public Vector2 Position;
        public Mat22 R;
    }

    /// This describes the motion of a body/shape for TOI computation.
    /// Shapes are defined with respect to the body origin, which may
    /// no coincide with the center of mass. However, to support dynamics
    /// we must interpolate the center of mass position.
    public struct Sweep
    {
	    /// Get the interpolated transform at a specific time.
	    /// @param alpha is a factor in [0,1], where 0 indicates t0.
	    public void GetTransform(out Transform xf, float alpha)
        {
            xf = new Transform();
	        xf.Position = (1.0f - alpha) * c0 + alpha * c;
	        float angle = (1.0f - alpha) * a0 + alpha * a;
	        xf.R.Set(angle);

	        // Shift to origin
	        xf.Position -= MathUtils.Multiply(ref xf.R, localCenter);
        }

	    /// Advance the sweep forward, yielding a new initial state.
	    /// @param t the new initial time.
	    public void Advance(float t)
        {
            c0 = (1.0f - t) * c0 + t * c;
            a0 = (1.0f - t) * a0 + t * a;
        }

        /// Normalize the angles.
        public void Normalize()
        {
            float twoPi = 2.0f * (float)Math.PI;
            float d = twoPi * (float)Math.Floor(a0 / twoPi);
            a0 -= d;
            a -= d;
        }

        public Vector2 localCenter;	///< local center of mass position
        public Vector2 c0, c;		///< center world positions
        public float a0, a;		///< world angles
    }
}


code:
/*
* Box2D.XNA port of Box2D:
* Copyright (c) 2009 Brandon Furtwangler, Nathan Furtwangler
*
* Original source Box2D:
* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
*
* This software is provided 'as-is', without any express or implied
* warranty.  In no event will the authors be held liable for any damages
* arising from the use of this software.
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely, subject to the following restrictions:
* 1. The origin of this software must not be misrepresented; you must not
* claim that you wrote the original software. If you use this software
* in a product, an acknowledgment in the product documentation would be
* appreciated but is not required.
* 2. Altered source versions must be plainly marked as such, and must not be
* misrepresented as being the original software.
* 3. This notice may not be removed or altered from any source distribution.
*/

using System;

namespace Box2D.XNA
{
    public static class Settings
    {
        public const float b2_maxFloat = 3.402823466e+38f;
        public const float b2_epsilon = 1.192092896e-07f;
        public const float b2_pi = 3.14159265359f;

        /// The maximum number of contact points between two convex shapes.
        public const int b2_maxManifoldPoints = 2;

        /// The maximum number of vertices on a convex polygon.
        public const int b2_maxPolygonVertices = 8;

        /// This is used to fatten AABBs in the dynamic tree. This allows proxies
        /// to move by a small amount without triggering a tree adjustment.
        /// This is in meters.
        public const float b2_aabbExtension = 0.1f;

        /// This is used to fatten AABBs in the dynamic tree. This is used to predict
        /// the future position based on the current displacement.
        /// This is a dimensionless multiplier.
        public const float b2_aabbMultiplier = 2.0f;

        /// A small length used as a collision and constraint tolerance. Usually it is
        /// chosen to be numerically significant, but visually insignificant.
        public const float b2_linearSlop = 0.005f;

        /// A small angle used as a collision and constraint tolerance. Usually it is
        /// chosen to be numerically significant, but visually insignificant.
        public const float b2_angularSlop = (2.0f / 180.0f * b2_pi);

        /// The radius of the polygon/edge shape skin. This should not be modified. Making
        /// this smaller means polygons will have an insufficient buffer for continuous collision.
        /// Making it larger may create artifacts for vertex collision.
        public const float b2_polygonRadius = (2.0f * b2_linearSlop);

        // Dynamics

        /// Maximum number of contacts to be handled to solve a TOI impact.
        public const int b2_maxTOIContacts = 32;

        /// A velocity threshold for elastic collisions. Any collision with a relative linear
        /// velocity below this threshold will be treated as inelastic.
        public const float b2_velocityThreshold = 1.0f;

        /// The maximum linear position correction used when solving constraints. This helps to
        /// prevent overshoot.
        public const float b2_maxLinearCorrection = 0.2f;

        /// The maximum angular position correction used when solving constraints. This helps to
        /// prevent overshoot.
        public const float b2_maxAngularCorrection = (8.0f / 180.0f * b2_pi);

        /// The maximum linear velocity of a body. This limit is very large and is used
        /// to prevent numerical problems. You shouldn't need to adjust this.
        public static float b2_maxTranslation = 2.0f;
        public static float b2_maxTranslationSquared = (b2_maxTranslation * b2_maxTranslation);

        /// The maximum angular velocity of a body. This limit is very large and is used
        /// to prevent numerical problems. You shouldn't need to adjust this.
        public static float b2_maxRotation = (0.5f * b2_pi);
        public static float b2_maxRotationSquared = (b2_maxRotation * b2_maxRotation);

        /// This scale factor controls how fast overlap is resolved. Ideally this would be 1 so
        /// that overlap is removed in one time step. However using values close to 1 often lead
        /// to overshoot.
        public const float b2_contactBaumgarte = 0.2f;

        // Sleep

        /// The time that a body must be still before it will go to sleep.
        public const float b2_timeToSleep = 0.5f;

        /// A body cannot sleep if its linear velocity is above this tolerance.
        public const float b2_linearSleepTolerance = 0.01f;

        /// A body cannot sleep if its angular velocity is above this tolerance.
        public const float b2_angularSleepTolerance = (2.0f / 180.0f * b2_pi);

        /// Friction mixing law. Feel free to customize this.
        public static float b2MixFriction(float friction1, float friction2)
        {
	        return (float)Math.Sqrt((double)(friction1 * friction2));
        }

        /// Restitution mixing law. Feel free to customize this.
        public static float b2MixRestitution(float restitution1, float restitution2)
        {
	        return restitution1 > restitution2 ? restitution1 : restitution2;
        }
    }
}


code:
<Project DefaultTargets="Build" xmlns="http://schemas.microsoft.com/developer/msbuild/2003" ToolsVersion="3.5">
  <PropertyGroup>
    <ProjectGuid>41f3b7dc-02d1-426f-a2f2-03fd2a8afd65</ProjectGuid>
    <ProjectTypeGuids>{96E2B04D-8817-42c6-938A-82C39BA4D311};{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}</ProjectTypeGuids>
    <Configuration Condition=" '$(Configuration)' == '' ">Debug</Configuration>
    <Platform Condition=" '$(Platform)' == '' ">x86</Platform>
    <OutputType>Library</OutputType>
    <AppDesignerFolder>Properties</AppDesignerFolder>
    <TargetFrameworkVersion>v3.5</TargetFrameworkVersion>
    <XnaFrameworkVersion>v3.1</XnaFrameworkVersion>
    <PlatformTarget>x86</PlatformTarget>
    <OutputPath>bin\$(Platform)\$(Configuration)</OutputPath>
  </PropertyGroup>
  <PropertyGroup Condition=" '$(Configuration)|$(Platform)' == 'Debug|x86' ">
    <XnaPlatform>Windows</XnaPlatform>
  </PropertyGroup>
  <PropertyGroup Condition=" '$(Configuration)|$(Platform)' == 'Release|x86' ">
    <XnaPlatform>Windows</XnaPlatform>
  </PropertyGroup>
  <ItemGroup>
    <Reference Include="Microsoft.Xna.Framework.Content.Pipeline.EffectImporter, Version=3.1.0.0, Culture=neutral, PublicKeyToken=6d5c3888ef60e27d, processorArchitecture=MSIL">
      <Private>False</Private>
    </Reference>
    <Reference Include="Microsoft.Xna.Framework.Content.Pipeline.FBXImporter, Version=3.1.0.0, Culture=neutral, PublicKeyToken=6d5c3888ef60e27d, processorArchitecture=MSIL">
      <Private>False</Private>
    </Reference>
    <Reference Include="Microsoft.Xna.Framework.Content.Pipeline.TextureImporter, Version=3.1.0.0, Culture=neutral, PublicKeyToken=6d5c3888ef60e27d, processorArchitecture=MSIL">
      <Private>False</Private>
    </Reference>
    <Reference Include="Microsoft.Xna.Framework.Content.Pipeline.XImporter, Version=3.1.0.0, Culture=neutral, PublicKeyToken=6d5c3888ef60e27d, processorArchitecture=MSIL">
      <Private>False</Private>
    </Reference>
    <Reference Include="Microsoft.Xna.Framework.Content.Pipeline.AudioImporters, Version=3.1.0.0, Culture=neutral, PublicKeyToken=6d5c3888ef60e27d, processorArchitecture=MSIL">
      <Private>False</Private>
    </Reference>
    <Reference Include="Microsoft.Xna.Framework.Content.Pipeline.VideoImporters, Version=3.1.0.0, Culture=neutral, PublicKeyToken=6d5c3888ef60e27d, processorArchitecture=MSIL">
      <Private>False</Private>
    </Reference>
  </ItemGroup>
  <Import Project="$(MSBuildExtensionsPath)\Microsoft\XNA Game Studio\$(XnaFrameworkVersion)\Microsoft.Xna.GameStudio.ContentPipeline.targets" />

  <!-- To modify your build process, add your task inside one of the targets below and uncomment it.
       Other similar extension points exist, see Microsoft.Common.targets.
  <Target Name="BeforeBuild">
  </Target>
  <Target Name="AfterBuild">
  </Target>
  -->

</Project>