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 Microsoft.Xna.Framework;

namespace Box2D.XNA
{
    internal struct Pair : IComparable<Pair>
    {
        public int proxyIdA;
        public int proxyIdB;
        public int next;

        public int CompareTo(Pair other)
        {
            if (proxyIdA < other.proxyIdA)
            {
                return -1;
            }
            else if (proxyIdA == other.proxyIdA)
            {
                if (proxyIdB < other.proxyIdB)
                {
                    return -1;
                }
                else if (proxyIdB == other.proxyIdB)
                {
                    return 0;
                }
            }

            return 1;
        }
    };

    /// The broad-phase is used for computing pairs and performing volume queries and ray casts.
    /// This broad-phase does not persist pairs. Instead, this reports potentially new pairs.
    /// It is up to the client to consume the new pairs and to track subsequent overlap.
    public class BroadPhase
    {
        internal const int NullProxy = -1;

	    public BroadPhase()
        {
            _queryCallback = new Func<int, bool>(QueryCallback);

	        _proxyCount = 0;

	        _pairCapacity = 16;
	        _pairCount = 0;
            _pairBuffer = new Pair[_pairCapacity];

	        _moveCapacity = 16;
	        _moveCount = 0;
            _moveBuffer = new int[_moveCapacity];
        }

	    /// Create a proxy with an initial AABB. Pairs are not reported until
	    /// UpdatePairs is called.
	    public int CreateProxy(ref AABB aabb, object userData)
        {
	        int proxyId = _tree.CreateProxy(ref aabb, userData);
            ++_proxyCount;
	        BufferMove(proxyId);
	        return proxyId;
        }

	    /// Destroy a proxy. It is up to the client to remove any pairs.
	    public void DestroyProxy(int proxyId)
        {
	        UnBufferMove(proxyId);
            --_proxyCount;
	        _tree.DestroyProxy(proxyId);
        }

	    public void MoveProxy(int proxyId, ref AABB aabb, Vector2 displacement)
        {
            bool buffer = _tree.MoveProxy(proxyId, ref aabb, displacement);
            if (buffer)
            {
                BufferMove(proxyId);
            }
        }

	    /// Get the AABB for a proxy.
	    public void GetFatAABB(int proxyId, out AABB aabb)
        {
            _tree.GetFatAABB(proxyId, out aabb);
        }

	    /// Get user data from a proxy. Returns null if the id is invalid.
	    public object GetUserData(int proxyId)
        {
            return _tree.GetUserData(proxyId);
        }

        /// Test overlap of fat AABBs.
        public bool TestOverlap(int proxyIdA, int proxyIdB)
        {
            AABB aabbA, aabbB;
            _tree.GetFatAABB(proxyIdA, out aabbA);
	        _tree.GetFatAABB(proxyIdB, out aabbB);
	        return AABB.TestOverlap(ref aabbA, ref aabbB);
        }

	    /// Get the number of proxies.
	    public int ProxyCount
        {
            get
            {
                return _proxyCount;
            }
        }

	    /// Update the pairs. This results in pair callbacks. This can only add pairs.
	    public void UpdatePairs<T>(Action<T,T> callback)
        {
            // Reset pair buffer
	        _pairCount = 0;

	        // Perform tree queries for all moving proxies.
	        for (int j = 0; j < _moveCount; ++j)
	        {
		        _queryProxyId = _moveBuffer[j];
		        if (_queryProxyId == NullProxy)
		        {
			        continue;
		        }

                // We have to query the tree with the fat AABB so that
		        // we don't fail to create a pair that may touch later.
		        AABB fatAABB;
                _tree.GetFatAABB(_queryProxyId, out fatAABB);

                // Query tree, create pairs and add them pair buffer.
                _tree.Query(_queryCallback, ref fatAABB);
	        }

	        // Reset move buffer
	        _moveCount = 0;

	        // Sort the pair buffer to expose duplicates.
            Array.Sort(_pairBuffer, 0, _pairCount);

	        // Send the pairs back to the client.
	        int i = 0;
	        while (i < _pairCount)
	        {
		        Pair primaryPair = _pairBuffer[i];
                object userDataA = _tree.GetUserData(primaryPair.proxyIdA);
                object userDataB = _tree.GetUserData(primaryPair.proxyIdB);

		        callback((T)userDataA, (T)userDataB);
		        ++i;

		        // Skip any duplicate pairs.
		        while (i < _pairCount)
		        {
			        Pair pair = _pairBuffer[i];
			        if (pair.proxyIdA != primaryPair.proxyIdA || pair.proxyIdB != primaryPair.proxyIdB)
			        {
				        break;
			        }
			        ++i;
		        }
	        }

            // Try to keep the tree balanced.
            _tree.Rebalance(4);
        }

	    /// 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)
        {
	        _tree.Query(callback, ref aabb);
        }

        internal void RayCast(RayCastCallbackInternal callback, ref RayCastInput input)
        {
	        _tree.RayCast(callback, ref input);
        }

	    /// Compute the height of the embedded tree.
	    public int ComputeHeight()
        {
            return _tree.ComputeHeight();
        }

	    internal void BufferMove(int proxyId)
        {
	        if (_moveCount == _moveCapacity)
	        {
		        int[] oldBuffer = _moveBuffer;
		        _moveCapacity *= 2;
		        _moveBuffer = new int[_moveCapacity];
                Array.Copy(oldBuffer, _moveBuffer, _moveCount);
	        }

	        _moveBuffer[_moveCount] = proxyId;
	        ++_moveCount;
        }

	    internal void UnBufferMove(int proxyId)
        {
	        for (int i = 0; i < _moveCount; ++i)
	        {
		        if (_moveBuffer[i] == proxyId)
		        {
			        _moveBuffer[i] = NullProxy;
			        return;
		        }
	        }
        }

        internal bool QueryCallback(int proxyId)
        {
	        // A proxy cannot form a pair with itself.
	        if (proxyId == _queryProxyId)
	        {
		        return true;
	        }

	        // Grow the pair buffer as needed.
	        if (_pairCount == _pairCapacity)
	        {
		        Pair[] oldBuffer = _pairBuffer;
		        _pairCapacity *= 2;
                _pairBuffer = new Pair[_pairCapacity];
                Array.Copy(oldBuffer, _pairBuffer, _pairCount);
	        }

	        _pairBuffer[_pairCount].proxyIdA = Math.Min(proxyId, _queryProxyId);
	        _pairBuffer[_pairCount].proxyIdB = Math.Max(proxyId, _queryProxyId);
	        ++_pairCount;

            return true;
        }

	    internal DynamicTree _tree = new DynamicTree();

        internal int _proxyCount;

	    internal int[] _moveBuffer;
	    internal int _moveCapacity;
	    internal int _moveCount;

	    internal Pair[] _pairBuffer;
	    internal int _pairCapacity;
	    internal int _pairCount;

	    internal int _queryProxyId;

        Func<int, bool> _queryCallback;
    }
}


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

#include <Box2D/Collision/b2Collision.h>
#include <Box2D/Collision/Shapes/b2CircleShape.h>
#include <Box2D/Collision/Shapes/b2EdgeShape.h>
#include <Box2D/Collision/Shapes/b2PolygonShape.h>

enum b2EdgeType
{
	b2_isolated,
	b2_concave,
	b2_flat,
	b2_convex
};

// Compute contact points for edge versus circle.
// This accounts for edge connectivity.
void b2CollideEdgeAndCircle(b2Manifold* manifold,
							const b2EdgeShape* edgeA, const b2Transform& xfA,
							const b2CircleShape* circleB, const b2Transform& xfB)
{
	manifold->pointCount = 0;

	// Compute circle in frame of edge
	b2Vec2 Q = b2MulT(xfA, b2Mul(xfB, circleB->m_p));

	b2Vec2 A = edgeA->m_vertex1, B = edgeA->m_vertex2;
	b2Vec2 e = B - A;

	// Barycentric coordinates
	float32 u = b2Dot(e, B - Q);
	float32 v = b2Dot(e, Q - A);

	float32 radius = edgeA->m_radius + circleB->m_radius;

	b2ContactFeature cf;
	cf.indexB = 0;
	cf.typeB = b2ContactFeature::e_vertex;

	// Region A
	if (v <= 0.0f)
	{
		b2Vec2 P = A;
		b2Vec2 d = Q - P;
		float32 dd = b2Dot(d, d);
		if (dd > radius * radius)
		{
			return;
		}

		// Is there an edge connected to A?
		if (edgeA->m_hasVertex0)
		{
			b2Vec2 A1 = edgeA->m_vertex0;
			b2Vec2 B1 = A;
			b2Vec2 e1 = B1 - A1;
			float32 u1 = b2Dot(e1, B1 - Q);

			// Is the circle in Region AB of the previous edge?
			if (u1 > 0.0f)
			{
				return;
			}
		}

		cf.indexA = 0;
		cf.typeA = b2ContactFeature::e_vertex;
		manifold->pointCount = 1;
		manifold->type = b2Manifold::e_circles;
		manifold->localNormal.SetZero();
		manifold->localPoint = P;
		manifold->points[0].id.key = 0;
		manifold->points[0].id.cf = cf;
		manifold->points[0].localPoint = circleB->m_p;
		return;
	}

	// Region B
	if (u <= 0.0f)
	{
		b2Vec2 P = B;
		b2Vec2 d = Q - P;
		float32 dd = b2Dot(d, d);
		if (dd > radius * radius)
		{
			return;
		}

		// Is there an edge connected to B?
		if (edgeA->m_hasVertex3)
		{
			b2Vec2 B2 = edgeA->m_vertex3;
			b2Vec2 A2 = B;
			b2Vec2 e2 = B2 - A2;
			float32 v2 = b2Dot(e2, Q - A2);

			// Is the circle in Region AB of the next edge?
			if (v2 > 0.0f)
			{
				return;
			}
		}

		cf.indexA = 1;
		cf.typeA = b2ContactFeature::e_vertex;
		manifold->pointCount = 1;
		manifold->type = b2Manifold::e_circles;
		manifold->localNormal.SetZero();
		manifold->localPoint = P;
		manifold->points[0].id.key = 0;
		manifold->points[0].id.cf = cf;
		manifold->points[0].localPoint = circleB->m_p;
		return;
	}

	// Region AB
	float32 den = b2Dot(e, e);
	b2Assert(den > 0.0f);
	b2Vec2 P = (1.0f / den) * (u * A + v * B);
	b2Vec2 d = Q - P;
	float32 dd = b2Dot(d, d);
	if (dd > radius * radius)
	{
		return;
	}

	b2Vec2 n(-e.y, e.x);
	if (b2Dot(n, Q - A) < 0.0f)
	{
		n.Set(-n.x, -n.y);
	}
	n.Normalize();

	cf.indexA = 0;
	cf.typeA = b2ContactFeature::e_face;
	manifold->pointCount = 1;
	manifold->type = b2Manifold::e_faceA;
	manifold->localNormal = n;
	manifold->localPoint = A;
	manifold->points[0].id.key = 0;
	manifold->points[0].id.cf = cf;
	manifold->points[0].localPoint = circleB->m_p;
}

struct b2EPAxis
{
	enum Type
	{
		e_unknown,
		e_edgeA,
		e_edgeB,
	};

	Type type;
	int32 index;
	float32 separation;
};

static b2EPAxis b2EPEdgeSeparation(const b2Vec2& v1, const b2Vec2& v2, const b2Vec2& n, const b2PolygonShape* polygonB, float32 radius)
{
	// EdgeA separation
	b2EPAxis axis;
	axis.type = b2EPAxis::e_edgeA;
	axis.index = 0;
	axis.separation = b2Dot(n, polygonB->m_vertices[0] - v1);
	for (int32 i = 1; i < polygonB->m_vertexCount; ++i)
	{
		float32 s = b2Dot(n, polygonB->m_vertices[i] - v1);
		if (s < axis.separation)
		{
			axis.separation = s;
		}
	}

	return axis;
}

static b2EPAxis b2EPPolygonSeparation(const b2Vec2& v1, const b2Vec2& v2, const b2Vec2& n, const b2PolygonShape* polygonB, float32 radius)
{
	// PolygonB separation
	b2EPAxis axis;
	axis.type = b2EPAxis::e_edgeB;
	axis.index = 0;
	axis.separation = -FLT_MAX;
	for (int32 i = 0; i < polygonB->m_vertexCount; ++i)
	{
		float32 s1 = b2Dot(polygonB->m_normals[i], v1 - polygonB->m_vertices[i]);
		float32 s2 = b2Dot(polygonB->m_normals[i], v2 - polygonB->m_vertices[i]);
		float32 s = b2Min(s1, s2);
		if (s > axis.separation)
		{
			axis.index = i;
			axis.separation = s;
			if (s > radius)
			{
				return axis;
			}
		}
	}

	return axis;
}

static void b2FindIncidentEdge(b2ClipVertex c[2], const b2PolygonShape* poly1, int32 edge1, const b2PolygonShape* poly2)
{
	int32 count1 = poly1->m_vertexCount;
	const b2Vec2* normals1 = poly1->m_normals;

	int32 count2 = poly2->m_vertexCount;
	const b2Vec2* vertices2 = poly2->m_vertices;
	const b2Vec2* normals2 = poly2->m_normals;

	b2Assert(0 <= edge1 && edge1 < count1);

	// Get the normal of the reference edge in poly2's frame.
	b2Vec2 normal1 = normals1[edge1];

	// Find the incident edge on poly2.
	int32 index = 0;
	float32 minDot = b2_maxFloat;
	for (int32 i = 0; i < count2; ++i)
	{
		float32 dot = b2Dot(normal1, normals2[i]);
		if (dot < minDot)
		{
			minDot = dot;
			index = i;
		}
	}

	// Build the clip vertices for the incident edge.
	int32 i1 = index;
	int32 i2 = i1 + 1 < count2 ? i1 + 1 : 0;

	c[0].v = vertices2[i1];
	c[0].id.cf.indexA = (uint8)edge1;
	c[0].id.cf.indexB = (uint8)i1;
	c[0].id.cf.typeA = b2ContactFeature::e_face;
	c[0].id.cf.typeB = b2ContactFeature::e_vertex;

	c[1].v = vertices2[i2];
	c[1].id.cf.indexA = (uint8)edge1;
	c[1].id.cf.indexB = (uint8)i2;
	c[1].id.cf.typeA = b2ContactFeature::e_face;
	c[1].id.cf.typeB = b2ContactFeature::e_vertex;
}

// Collide and edge and polygon. This uses the SAT and clipping to produce up to 2 contact points.
// Edge adjacency is handle to produce locally valid contact points and normals. This is intended
// to allow the polygon to slide smoothly over an edge chain.
//
// Algorithm
// 1. Classify front-side or back-side collision with edge.
// 2. Compute separation
// 3. Process adjacent edges
// 4. Classify adjacent edge as convex, flat, null, or concave
// 5. Skip null or concave edges. Concave edges get a separate manifold.
// 6. If the edge is flat, compute contact points as normal. Discard boundary points.
// 7. If the edge is convex, compute it's separation.
// 8. Use the minimum separation of up to three edges. If the minimum separation
//    is not the primary edge, return.
// 9. If the minimum separation is the primary edge, compute the contact points and return.
void b2CollideEdgeAndPolygon(	b2Manifold* manifold,
								const b2EdgeShape* edgeA, const b2Transform& xfA,
								const b2PolygonShape* polygonB_in, const b2Transform& xfB)
{
	manifold->pointCount = 0;

	b2Transform xf = b2MulT(xfA, xfB);

	// Create a polygon for edge shape A
	b2PolygonShape polygonA;
	polygonA.SetAsEdge(edgeA->m_vertex1, edgeA->m_vertex2);

	// Build polygonB in frame A
	b2PolygonShape polygonB;
	polygonB.m_radius = polygonB_in->m_radius;
	polygonB.m_vertexCount = polygonB_in->m_vertexCount;
	polygonB.m_centroid = b2Mul(xf, polygonB_in->m_centroid);
	for (int32 i = 0; i < polygonB.m_vertexCount; ++i)
	{
		polygonB.m_vertices[i] = b2Mul(xf, polygonB_in->m_vertices[i]);
		polygonB.m_normals[i] = b2Mul(xf.R, polygonB_in->m_normals[i]);
	}

	float32 totalRadius = polygonA.m_radius + polygonB.m_radius;

	// Edge geometry
	b2Vec2 v1 = edgeA->m_vertex1;
	b2Vec2 v2 = edgeA->m_vertex2;
	b2Vec2 e = v2 - v1;
	b2Vec2 edgeNormal(e.y, -e.x);
	edgeNormal.Normalize();

	// Determine side
	bool isFrontSide = b2Dot(edgeNormal, polygonB.m_centroid - v1) >= 0.0f;
	if (isFrontSide == false)
	{
		edgeNormal = -edgeNormal;
	}

	// Compute primary separating axis
	b2EPAxis edgeAxis = b2EPEdgeSeparation(v1, v2, edgeNormal, &polygonB, totalRadius);
	if (edgeAxis.separation > totalRadius)
	{
		// Shapes are separated
		return;
	}

	// Classify adjacent edges
	b2EdgeType types[2] = {b2_isolated, b2_isolated};
	if (edgeA->m_hasVertex0)
	{
		b2Vec2 v0 = edgeA->m_vertex0;
		float32 s = b2Dot(edgeNormal, v0 - v1);

		if (s > 0.1f * b2_linearSlop)
		{
			types[0] = b2_concave;
		}
		else if (s >= -0.1f * b2_linearSlop)
		{
			types[0] = b2_flat;
		}
		else
		{
			types[0] = b2_convex;
		}
	}

	if (edgeA->m_hasVertex3)
	{
		b2Vec2 v3 = edgeA->m_vertex3;
		float32 s = b2Dot(edgeNormal, v3 - v2);
		if (s > 0.1f * b2_linearSlop)
		{
			types[1] = b2_concave;
		}
		else if (s >= -0.1f * b2_linearSlop)
		{
			types[1] = b2_flat;
		}
		else
		{
			types[1] = b2_convex;
		}
	}

	if (types[0] == b2_convex)
	{
		// Check separation on previous edge.
		b2Vec2 v0 = edgeA->m_vertex0;
		b2Vec2 e0 = v1 - v0;

		b2Vec2 n0(e0.y, -e0.x);
		n0.Normalize();
		if (isFrontSide == false)
		{
			n0 = -n0;
		}

		b2EPAxis axis1 = b2EPEdgeSeparation(v0, v1, n0, &polygonB, totalRadius);
		if (axis1.separation > edgeAxis.separation)
		{
			// The polygon should collide with previous edge
			return;
		}
	}

	if (types[1] == b2_convex)
	{
		// Check separation on next edge.
		b2Vec2 v3 = edgeA->m_vertex3;
		b2Vec2 e2 = v3 - v2;

		b2Vec2 n2(e2.y, -e2.x);
		n2.Normalize();
		if (isFrontSide == false)
		{
			n2 = -n2;
		}

		b2EPAxis axis2 = b2EPEdgeSeparation(v2, v3, n2, &polygonB, totalRadius);
		if (axis2.separation > edgeAxis.separation)
		{
			// The polygon should collide with the next edge
			return;
		}
	}

	b2EPAxis polygonAxis = b2EPPolygonSeparation(v1, v2, edgeNormal, &polygonB, totalRadius);
	if (polygonAxis.separation > totalRadius)
	{
		return;
	}

	// Use hysteresis for jitter reduction.
	const float32 k_relativeTol = 0.98f;
	const float32 k_absoluteTol = 0.001f;

	b2EPAxis primaryAxis;
	if (polygonAxis.separation > k_relativeTol * edgeAxis.separation + k_absoluteTol)
	{
		primaryAxis = polygonAxis;
	}
	else
	{
		primaryAxis = edgeAxis;
	}

	b2PolygonShape* poly1;
	b2PolygonShape* poly2;
	b2ClipVertex incidentEdge[2];
	if (primaryAxis.type == b2EPAxis::e_edgeA)
	{
		poly1 = &polygonA;
		poly2 = &polygonB;
		if (isFrontSide == false)
		{
			primaryAxis.index = 1;
		}
		manifold->type = b2Manifold::e_faceA;
	}
	else
	{
		poly1 = &polygonB;
		poly2 = &polygonA;
		manifold->type = b2Manifold::e_faceB;
	}

	int32 edge1 = primaryAxis.index;

	b2FindIncidentEdge(incidentEdge, poly1, primaryAxis.index, poly2);
	int32 count1 = poly1->m_vertexCount;
	const b2Vec2* vertices1 = poly1->m_vertices;

	int32 iv1 = edge1;
	int32 iv2 = edge1 + 1 < count1 ? edge1 + 1 : 0;

	b2Vec2 v11 = vertices1[iv1];
	b2Vec2 v12 = vertices1[iv2];

	b2Vec2 tangent = v12 - v11;
	tangent.Normalize();

	b2Vec2 normal = b2Cross(tangent, 1.0f);
	b2Vec2 planePoint = 0.5f * (v11 + v12);

	// Face offset.
	float32 frontOffset = b2Dot(normal, v11);

	// Side offsets, extended by polytope skin thickness.
	float32 sideOffset1 = -b2Dot(tangent, v11) + totalRadius;
	float32 sideOffset2 = b2Dot(tangent, v12) + totalRadius;

	// Clip incident edge against extruded edge1 side edges.
	b2ClipVertex clipPoints1[2];
	b2ClipVertex clipPoints2[2];
	int np;

	// Clip to box side 1
	np = b2ClipSegmentToLine(clipPoints1, incidentEdge, -tangent, sideOffset1, iv1);

	if (np < b2_maxManifoldPoints)
	{
		return;
	}

	// Clip to negative box side 1
	np = b2ClipSegmentToLine(clipPoints2, clipPoints1,  tangent, sideOffset2, iv2);

	if (np < b2_maxManifoldPoints)
	{
		return;
	}

	// Now clipPoints2 contains the clipped points.
	if (primaryAxis.type == b2EPAxis::e_edgeA)
	{
		manifold->localNormal = normal;
		manifold->localPoint = planePoint;
	}
	else
	{
		manifold->localNormal = b2MulT(xf.R, normal);
		manifold->localPoint = b2MulT(xf, planePoint);
	}

	int32 pointCount = 0;
	for (int32 i = 0; i < b2_maxManifoldPoints; ++i)
	{
		float32 separation;

		separation = b2Dot(normal, clipPoints2[i].v) - frontOffset;

		if (separation <= totalRadius)
		{
			b2ManifoldPoint* cp = manifold->points + pointCount;

			if (primaryAxis.type == b2EPAxis::e_edgeA)
			{
				cp->localPoint = b2MulT(xf, clipPoints2[i].v);
				cp->id = clipPoints2[i].id;
			}
			else
			{
				cp->localPoint = clipPoints2[i].v;
				cp->id.cf.typeA = clipPoints2[i].id.cf.typeB;
				cp->id.cf.typeB = clipPoints2[i].id.cf.typeA;
				cp->id.cf.indexA = clipPoints2[i].id.cf.indexB;
				cp->id.cf.indexB = clipPoints2[i].id.cf.indexA;
			}

			if (cp->id.cf.typeA == b2ContactFeature::e_vertex && types[cp->id.cf.indexA] == b2_flat)
			{
				continue;
			}

			++pointCount;
		}
	}

	manifold->pointCount = pointCount;
}

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

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

namespace Box2D.XNA
{
    internal enum ContactFeatureType : byte
    {
        Vertex = 0,
        Face = 1,
    };

    /// The features that intersect to form the contact point
    /// This must be 4 bytes or less.
    public struct ContactFeature
    {
        internal byte indexA;		///< Feature index on ShapeA
        internal byte indexB;		///< Feature index on ShapeB
        internal byte typeA;		    ///< The feature type on ShapeA
        internal byte typeB;		    ///< The feature type on ShapeB
    };

    /// Contact ids to facilitate warm starting.
    [StructLayout(LayoutKind.Explicit)]
    public struct ContactID
    {
        [FieldOffset(0)]
        public ContactFeature Features;

	    /// The features that intersect to form the contact point
        [FieldOffset(0)]
        public uint Key;					///< Used to quickly compare contact ids.
    };

    /// A manifold point is a contact point belonging to a contact
    /// manifold. It holds details related to the geometry and dynamics
    /// of the contact points.
    /// The local point usage depends on the manifold type:
    /// -ShapeType.Circles: the local center of circleB
    /// -SeparationFunction.FaceA: the local center of cirlceB or the clip point of polygonB
    /// -SeparationFunction.FaceB: the clip point of polygonA
    /// This structure is stored across time steps, so we keep it small.
    /// Note: the impulses are used for internal caching and may not
    /// provide reliable contact forces, especially for high speed collisions.
    public struct ManifoldPoint
    {
        public Vector2 LocalPoint;		///< usage depends on manifold type
        public float NormalImpulse;	///< the non-penetration impulse
        public float TangentImpulse;	///< the friction impulse
        public ContactID Id;			///< uniquely identifies a contact point between two Shapes
    };

    public enum ManifoldType
    {
	    Circles,
	    FaceA,
	    FaceB
    };

    public enum EdgeType
    {
        Isolated,
        Concave,
        Flat,
        Convex
    };

    /// A manifold for two touching convex Shapes.
    /// Box2D supports multiple types of contact:
    /// - clip point versus plane with radius
    /// - point versus point with radius (circles)
    /// The local point usage depends on the manifold type:
    /// -ShapeType.Circles: the local center of circleA
    /// -SeparationFunction.FaceA: the center of faceA
    /// -SeparationFunction.FaceB: the center of faceB
    /// Similarly the local normal usage:
    /// -ShapeType.Circles: not used
    /// -SeparationFunction.FaceA: the normal on polygonA
    /// -SeparationFunction.FaceB: the normal on polygonB
    /// We store contacts in this way so that position correction can
    /// account for movement, which is critical for continuous physics.
    /// All contact scenarios must be expressed in one of these types.
    /// This structure is stored across time steps, so we keep it small.
    public struct Manifold
    {
	    public FixedArray2<ManifoldPoint> _points;	        ///< the points of contact
	    public Vector2 _localNormal;						///< not use for Type.SeparationFunction.Points
	    public Vector2 _localPoint;							///< usage depends on manifold type
	    public ManifoldType _type;
	    public int _pointCount;								///< the number of manifold points
    };

    /// This is used to compute the current state of a contact manifold.
    public struct WorldManifold
    {
	    /// Evaluate the manifold with supplied transforms. This assumes
	    /// modest motion from the original state. This does not change the
	    /// point count, impulses, etc. The radii must come from the Shapes
	    /// that generated the manifold.
        public WorldManifold(ref Manifold manifold,
					    ref Transform xfA, float radiusA,
					    ref Transform xfB, float radiusB)
        {
            _points = new FixedArray2<Vector2>();

	        if (manifold._pointCount == 0)
	        {
                _normal = Vector2.UnitY;
		        return;
	        }

	        switch (manifold._type)
	        {
	        case ManifoldType.Circles:
		        {
			        Vector2 pointA = MathUtils.Multiply(ref xfA, manifold._localPoint);
                    Vector2 pointB = MathUtils.Multiply(ref xfB, manifold._points[0].LocalPoint);
                    _normal = new Vector2(1.0f, 0.0f);
			        if (Vector2.DistanceSquared(pointA, pointB) > Settings.b2_epsilon * Settings.b2_epsilon)
			        {
                        _normal = pointB - pointA;
                        _normal.Normalize();
			        }

                    Vector2 cA = pointA + radiusA * _normal;
                    Vector2 cB = pointB - radiusB * _normal;
			        _points[0] = 0.5f * (cA + cB);
		        }
		        break;

	        case ManifoldType.FaceA:
		        {
                    _normal = MathUtils.Multiply(ref xfA.R, manifold._localNormal);
			        Vector2 planePoint = MathUtils.Multiply(ref xfA, manifold._localPoint);

			        for (int i = 0; i < manifold._pointCount; ++i)
			        {
				        Vector2 clipPoint = MathUtils.Multiply(ref xfB, manifold._points[i].LocalPoint);
                        Vector2 cA = clipPoint + (radiusA - Vector2.Dot(clipPoint - planePoint, _normal)) * _normal;
                        Vector2 cB = clipPoint - radiusB * _normal;
				        _points[i] = 0.5f * (cA + cB);
			        }
		        }
		        break;

	        case ManifoldType.FaceB:
		        {
                    _normal = MathUtils.Multiply(ref xfB.R, manifold._localNormal);
			        Vector2 planePoint = MathUtils.Multiply(ref xfB, manifold._localPoint);

			        for (int i = 0; i < manifold._pointCount; ++i)
			        {
                        Vector2 clipPoint = MathUtils.Multiply(ref xfA, manifold._points[i].LocalPoint);
                        Vector2 cA = clipPoint - radiusA * _normal;
                        Vector2 cB = clipPoint + (radiusB - Vector2.Dot(clipPoint - planePoint, _normal)) * _normal;
				        _points[i] = 0.5f * (cA + cB);
			        }
                    // Ensure normal points from A to B.
                    _normal *= -1;
		        }
		        break;
            default:
                _normal = Vector2.UnitY;
                break;
	        }
        }

	    public Vector2 _normal;						///< world vector pointing from A to B
	    public FixedArray2<Vector2> _points;   	///< world contact point (point of intersection)
    };

    /// This is used for determining the state of contact points.
    public enum PointState
    {
	    Null,		///< point does not exist
	    Add,		///< point was added in the update
	    Persist,	///< point persisted across the update
	    Remove,		///< point was removed in the update
    };

    /// Used for computing contact manifolds.
    public struct ClipVertex
    {
	    public Vector2 v;
	    public ContactID id;
    };

    /// Ray-cast input data. The ray extends from p1 to p1 + maxFraction * (p2 - p1).
    public struct RayCastInput
    {
	    public Vector2 p1, p2;
	    public float maxFraction;
    };

    /// Ray-cast output data.  The ray hits at p1 + fraction * (p2 - p1), where p1 and p2
    /// come from RayCastInput.
    public struct RayCastOutput
    {
	    public Vector2 normal;
        public float fraction;
    };

    /// An axis aligned bounding box.
    public struct AABB
    {
	    /// Verify that the bounds are sorted.
	    public bool IsValid()
        {
	        Vector2 d = upperBound - lowerBound;
	        bool valid = d.X >= 0.0f && d.Y >= 0.0f;
	        valid = valid && lowerBound.IsValid() && upperBound.IsValid();
	        return valid;
        }

	    /// Get the center of the AABB.
	    public Vector2 GetCenter()
	    {
		    return 0.5f * (lowerBound + upperBound);
	    }

	    /// Get the extents of the AABB (half-widths).
	    public Vector2 GetExtents()
	    {
		    return 0.5f * (upperBound - lowerBound);
	    }

        /// Get the perimeter length
        public float GetPerimeter()
	    {
		    float wx = upperBound.X - lowerBound.X;
		    float wy = upperBound.Y - lowerBound.Y;
		    return 2.0f * (wx + wy);
	    }

	    /// Combine an AABB into this one.
        public void Combine(ref AABB aabb)
	    {
		    lowerBound = Vector2.Min(lowerBound, aabb.lowerBound);
		    upperBound = Vector2.Max(upperBound, aabb.upperBound);
	    }

	    /// Combine two AABBs into this one.
	    public void Combine(ref AABB aabb1, ref AABB aabb2)
	    {
		    lowerBound = Vector2.Min(aabb1.lowerBound, aabb2.lowerBound);
		    upperBound = Vector2.Max(aabb1.upperBound, aabb2.upperBound);
	    }

	    /// Does this aabb contain the provided AABB.
	    public bool Contains(ref AABB aabb)
	    {
		    bool result = true;
		    result = result && lowerBound.X <= aabb.lowerBound.X;
		    result = result && lowerBound.Y <= aabb.lowerBound.Y;
		    result = result && aabb.upperBound.X <= upperBound.X;
		    result = result && aabb.upperBound.Y <= upperBound.Y;
		    return result;
	    }

        public static bool TestOverlap(ref AABB a, ref AABB b)
        {
            Vector2 d1, d2;
            d1 = b.lowerBound - a.upperBound;
            d2 = a.lowerBound - b.upperBound;

            if (d1.X > 0.0f || d1.Y > 0.0f)
	            return false;

            if (d2.X > 0.0f || d2.Y > 0.0f)
	            return false;

            return true;
        }

        public static bool TestOverlap( Shape ShapeA, int indexA,
                                        Shape ShapeB, int indexB,
                                        ref Transform xfA, ref Transform xfB)
        {
            DistanceInput input = new DistanceInput();
	        input.proxyA.Set(ShapeA, indexA);
	        input.proxyB.Set(ShapeB, indexB);
	        input.transformA = xfA;
	        input.transformB = xfB;
	        input.useRadii = true;

            SimplexCache cache;
	        DistanceOutput output;
	        Distance.ComputeDistance(out output, out cache, ref input);

	        return output.distance < 10.0f * Settings.b2_epsilon;
        }

        // From Real-time Collision Detection, p179.
	    public bool RayCast(out RayCastOutput output, ref RayCastInput input)
        {
            output = new RayCastOutput();

            float tmin = -Settings.b2_maxFloat;
            float tmax = Settings.b2_maxFloat;

	        Vector2 p = input.p1;
	        Vector2 d = input.p2 - input.p1;
	        Vector2 absD = MathUtils.Abs(d);

            Vector2 normal = Vector2.Zero;

	        for (int i = 0; i < 2; ++i)
	        {
                float absD_i = i == 0 ? absD.X : absD.Y;
                float lowerBound_i = i == 0 ? lowerBound.X : lowerBound.Y;
                float upperBound_i = i == 0 ? upperBound.X : upperBound.Y;
                float p_i = i == 0 ? p.X : p.Y;

                if (absD_i < Settings.b2_epsilon)
		        {
			        // Parallel.
                    if (p_i < lowerBound_i || upperBound_i < p_i)
			        {
				        return false;
			        }
		        }
		        else
		        {
                    float d_i = i == 0 ? d.X : d.Y;

                    float inv_d = 1.0f / d_i;
                    float t1 = (lowerBound_i - p_i) * inv_d;
                    float t2 = (upperBound_i - p_i) * inv_d;

			        // Sign of the normal vector.
			        float s = -1.0f;

			        if (t1 > t2)
			        {
				        MathUtils.Swap<float>(ref t1, ref t2);
				        s = 1.0f;
			        }

			        // Push the min up
			        if (t1 > tmin)
			        {
                        if (i == 0)
                        {
                            normal.X = s;
                        }
                        else
                        {
                            normal.Y = s;
                        }

				        tmin = t1;
			        }

			        // Pull the max down
			        tmax = Math.Min(tmax, t2);

			        if (tmin > tmax)
			        {
				        return false;
			        }
		        }
	        }

	        // Does the ray start inside the box?
	        // Does the ray intersect beyond the max fraction?
	        if (tmin < 0.0f || input.maxFraction < tmin)
	        {
		        return false;
	        }

	        // Intersection.
	        output.fraction = tmin;
	        output.normal = normal;
            return true;
        }

	    public Vector2 lowerBound;	///< the lower vertex
	    public Vector2 upperBound;	///< the upper vertex
    };

    public static class Collision
    {
        public static void GetPointStates(out FixedArray2<PointState> state1, out FixedArray2<PointState> state2,
					          ref Manifold manifold1, ref Manifold manifold2)
        {
            state1 = new FixedArray2<PointState>();
            state2 = new FixedArray2<PointState>();

	        // Detect persists and removes.
	        for (int i = 0; i < manifold1._pointCount; ++i)
	        {
		        ContactID id = manifold1._points[i].Id;

                state1[i] = PointState.Remove;

		        for (int j = 0; j < manifold2._pointCount; ++j)
		        {
			        if (manifold2._points[j].Id.Key == id.Key)
			        {
                        state1[i] = PointState.Persist;
				        break;
			        }
		        }
	        }

	        // Detect persists and adds.
	        for (int i = 0; i < manifold2._pointCount; ++i)
	        {
		        ContactID id = manifold2._points[i].Id;

                state2[i] = PointState.Add;

		        for (int j = 0; j < manifold1._pointCount; ++j)
		        {
			        if (manifold1._points[j].Id.Key == id.Key)
			        {
                        state2[i] = PointState.Persist;
				        break;
			        }
		        }
	        }
        }

        /// Compute the collision manifold between two circles.
        public static void CollideCircles(ref Manifold manifold,
					          CircleShape circleA, ref Transform xfA,
					          CircleShape circleB, ref Transform xfB)
        {
	        manifold._pointCount = 0;

	        Vector2 pA = MathUtils.Multiply(ref xfA, circleA._p);
	        Vector2 pB = MathUtils.Multiply(ref xfB, circleB._p);

	        Vector2 d = pB - pA;
	        float distSqr = Vector2.Dot(d, d);
            float rA = circleA._radius;
            float rB = circleB._radius;
            float radius = rA + rB;
	        if (distSqr > radius * radius)
	        {
		        return;
	        }

	        manifold._type = ManifoldType.Circles;
	        manifold._localPoint = circleA._p;
	        manifold._localNormal = Vector2.Zero;
	        manifold._pointCount = 1;

            var p0 = manifold._points[0];

            p0.LocalPoint = circleB._p;
            p0.Id.Key = 0;

            manifold._points[0] = p0;
        }

        /// Compute the collision manifold between a polygon and a circle.
        public static void CollidePolygonAndCircle(ref Manifold manifold,
							           PolygonShape polygonA, ref Transform xfA,
							           CircleShape circleB, ref Transform xfB)
        {
	        manifold._pointCount = 0;

	        // Compute circle position in the frame of the polygon.
	        Vector2 c = MathUtils.Multiply(ref xfB, circleB._p);
	        Vector2 cLocal = MathUtils.MultiplyT(ref xfA, c);

	        // Find the min separating edge.
	        int normalIndex = 0;
	        float separation = -Settings.b2_maxFloat;
	        float radius = polygonA._radius + circleB._radius;
	        int vertexCount = polygonA._vertexCount;

	        for (int i = 0; i < vertexCount; ++i)
	        {
                float s = Vector2.Dot(polygonA._normals[i], cLocal - polygonA._vertices[i]);

		        if (s > radius)
		        {
			        // Early out.
			        return;
		        }

		        if (s > separation)
		        {
			        separation = s;
			        normalIndex = i;
		        }
	        }

	        // Vertices that subtend the incident face.
	        int vertIndex1 = normalIndex;
	        int vertIndex2 = vertIndex1 + 1 < vertexCount ? vertIndex1 + 1 : 0;
            Vector2 v1 = polygonA._vertices[vertIndex1];
            Vector2 v2 = polygonA._vertices[vertIndex2];

	        // If the center is inside the polygon ...
	        if (separation < Settings.b2_epsilon)
	        {
		        manifold._pointCount = 1;
		        manifold._type = ManifoldType.FaceA;
                manifold._localNormal = polygonA._normals[normalIndex];
		        manifold._localPoint = 0.5f * (v1 + v2);

                var p0 = manifold._points[0];

                p0.LocalPoint = circleB._p;
                p0.Id.Key = 0;

                manifold._points[0] = p0;

		        return;
	        }

	        // Compute barycentric coordinates
	        float u1 = Vector2.Dot(cLocal - v1, v2 - v1);
	        float u2 = Vector2.Dot(cLocal - v2, v1 - v2);
	        if (u1 <= 0.0f)
	        {
		        if (Vector2.DistanceSquared(cLocal, v1) > radius * radius)
		        {
			        return;
		        }

		        manifold._pointCount = 1;
		        manifold._type = ManifoldType.FaceA;
		        manifold._localNormal = cLocal - v1;
		        manifold._localNormal.Normalize();
		        manifold._localPoint = v1;

                var p0b = manifold._points[0];

                p0b.LocalPoint = circleB._p;
                p0b.Id.Key = 0;

                manifold._points[0] = p0b;

	        }
	        else if (u2 <= 0.0f)
	        {
		        if (Vector2.DistanceSquared(cLocal, v2) > radius * radius)
		        {
			        return;
		        }

		        manifold._pointCount = 1;
		        manifold._type = ManifoldType.FaceA;
		        manifold._localNormal = cLocal - v2;
		        manifold._localNormal.Normalize();
		        manifold._localPoint = v2;

                var p0c = manifold._points[0];

                p0c.LocalPoint = circleB._p;
                p0c.Id.Key = 0;

                manifold._points[0] = p0c;
	        }
	        else
	        {
		        Vector2 faceCenter = 0.5f * (v1 + v2);
                float separation2 = Vector2.Dot(cLocal - faceCenter, polygonA._normals[vertIndex1]);
		        if (separation2 > radius)
		        {
			        return;
		        }

		        manifold._pointCount = 1;
		        manifold._type = ManifoldType.FaceA;
                manifold._localNormal = polygonA._normals[vertIndex1];
		        manifold._localPoint = faceCenter;

                var p0d = manifold._points[0];

                p0d.LocalPoint = circleB._p;
                p0d.Id.Key = 0;

                manifold._points[0] = p0d;
	        }
        }

        /// Compute the collision manifold between two polygons.
        public static void CollidePolygons(ref Manifold manifold,
                               PolygonShape polyA, ref Transform xfA,
                               PolygonShape polyB, ref Transform xfB)
        {
	        manifold._pointCount = 0;
	        float totalRadius = polyA._radius + polyB._radius;

	        int edgeA = 0;
	        float separationA = FindMaxSeparation(out edgeA, polyA, ref xfA, polyB, ref xfB);
	        if (separationA > totalRadius)
		        return;

	        int edgeB = 0;
	        float separationB = FindMaxSeparation(out edgeB, polyB, ref xfB, polyA, ref xfA);
	        if (separationB > totalRadius)
		        return;

	        PolygonShape poly1;	// reference polygon
	        PolygonShape poly2;	// incident polygon
	        Transform xf1, xf2;
	        int edge1;		// reference edge
	        bool flip;
	        float k_relativeTol = 0.98f;
	        float k_absoluteTol = 0.001f;

	        if (separationB > k_relativeTol * separationA + k_absoluteTol)
	        {
		        poly1 = polyB;
		        poly2 = polyA;
		        xf1 = xfB;
		        xf2 = xfA;
		        edge1 = edgeB;
		        manifold._type = ManifoldType.FaceB;
		        flip = true;
	        }
	        else
	        {
		        poly1 = polyA;
		        poly2 = polyB;
		        xf1 = xfA;
		        xf2 = xfB;
		        edge1 = edgeA;
		        manifold._type = ManifoldType.FaceA;
		        flip = false;
	        }

	        FixedArray2<ClipVertex> incidentEdge;
	        FindIncidentEdge(out incidentEdge, poly1, ref xf1, edge1, poly2, ref xf2);

	        int count1 = poly1._vertexCount;

            int iv1 = edge1;
            int iv2 = edge1 + 1 < count1 ? edge1 + 1 : 0;

            Vector2 v11 = poly1._vertices[iv1];
            Vector2 v12 = poly1._vertices[iv2];

	        Vector2 localTangent = v12 - v11;
            localTangent.Normalize();

            Vector2 localNormal = MathUtils.Cross(localTangent, 1.0f);
	        Vector2 planePoint = 0.5f * (v11 + v12);

            Vector2 tangent = MathUtils.Multiply(ref xf1.R, localTangent);
	        Vector2 normal = MathUtils.Cross(tangent, 1.0f);

	        v11 = MathUtils.Multiply(ref xf1, v11);
	        v12 = MathUtils.Multiply(ref xf1, v12);

            // Face offset.
	        float frontOffset = Vector2.Dot(normal, v11);

            // Side offsets, extended by polytope skin thickness.
            float sideOffset1 = -Vector2.Dot(tangent, v11) + totalRadius;
            float sideOffset2 = Vector2.Dot(tangent, v12) + totalRadius;

	        // Clip incident edge against extruded edge1 side edges.
	        FixedArray2<ClipVertex> clipPoints1;
	        FixedArray2<ClipVertex> clipPoints2;
	        int np;

	        // Clip to box side 1
            np = ClipSegmentToLine(out clipPoints1, ref incidentEdge, -tangent, sideOffset1, iv1);

	        if (np < 2)
		        return;

	        // Clip to negative box side 1
            np = ClipSegmentToLine(out clipPoints2, ref clipPoints1, tangent, sideOffset2, iv2);

	        if (np < 2)
	        {
		        return;
	        }

	        // Now clipPoints2 contains the clipped points.
	        manifold._localNormal = localNormal;
	        manifold._localPoint = planePoint;

	        int pointCount = 0;
	        for (int i = 0; i < Settings.b2_maxManifoldPoints; ++i)
	        {
		        float separation = Vector2.Dot(normal, clipPoints2[i].v) - frontOffset;

		        if (separation <= totalRadius)
		        {
			        ManifoldPoint cp = manifold._points[pointCount];
			        cp.LocalPoint = MathUtils.MultiplyT(ref xf2, clipPoints2[i].v);
			        cp.Id = clipPoints2[i].id;

                    if (flip)
                    {
                        // Swap features
                        ContactFeature cf = cp.Id.Features;
                        cp.Id.Features.indexA = cf.indexB;
                        cp.Id.Features.indexB = cf.indexA;
                        cp.Id.Features.typeA = cf.typeB;
                        cp.Id.Features.typeB = cf.typeA;
                    }

                    manifold._points[pointCount] = cp;

			        ++pointCount;
		        }
	        }

	        manifold._pointCount = pointCount;
        }

        // Compute contact points for edge versus circle.
        // This accounts for edge connectivity.
        public static void CollideEdgeAndCircle(ref Manifold manifold,
					           EdgeShape edgeA, ref Transform xfA,
					           CircleShape circleB, ref Transform xfB)
        {
	        manifold._pointCount = 0;

	        // Compute circle in frame of edge
	        Vector2 Q = MathUtils.MultiplyT(ref xfA, MathUtils.Multiply(ref xfB, circleB._p));

	        Vector2 A = edgeA._vertex1, B = edgeA._vertex2;
	        Vector2 e = B - A;

	        // Barycentric coordinates
	        float u = Vector2.Dot(e, B - Q);
	        float v = Vector2.Dot(e, Q - A);

	        float radius = edgeA._radius + circleB._radius;

	        ContactFeature cf;
	        cf.indexB = 0;
	        cf.typeB = (byte)ContactFeatureType.Vertex;

            Vector2 P, d;

	        // Region A
	        if (v <= 0.0f)
	        {
		        P = A;
		        d = Q - P;
		        float dd = Vector2.Dot(d, d);
		        if (dd > radius * radius)
		        {
			        return;
		        }

		        // Is there an edge connected to A?
		        if (edgeA._hasVertex0)
		        {
			        Vector2 A1 = edgeA._vertex0;
			        Vector2 B1 = A;
			        Vector2 e1 = B1 - A1;
			        float u1 = Vector2.Dot(e1, B1 - Q);

			        // Is the circle in Region AB of the previous edge?
			        if (u1 > 0.0f)
			        {
				        return;
			        }
		        }

		        cf.indexA = 0;
		        cf.typeA = (byte)ContactFeatureType.Vertex;
		        manifold._pointCount = 1;
		        manifold._type = ManifoldType.Circles;
		        manifold._localNormal = Vector2.Zero;
		        manifold._localPoint = P;
                var mp = new ManifoldPoint();
		        mp.Id.Key = 0;
		        mp.Id.Features = cf;
		        mp.LocalPoint = circleB._p;
                manifold._points[0] = mp;
		        return;
	        }

	        // Region B
	        if (u <= 0.0f)
	        {
		        P = B;
		        d = Q - P;
		        float dd = Vector2.Dot(d, d);
		        if (dd > radius * radius)
		        {
			        return;
		        }

		        // Is there an edge connected to B?
		        if (edgeA._hasVertex3)
		        {
			        Vector2 B2 = edgeA._vertex3;
			        Vector2 A2 = B;
			        Vector2 e2 = B2 - A2;
			        float v2 = Vector2.Dot(e2, Q - A2);

			        // Is the circle in Region AB of the next edge?
			        if (v2 > 0.0f)
			        {
				        return;
			        }
		        }

		        cf.indexA = 1;
		        cf.typeA = (byte)ContactFeatureType.Vertex;
		        manifold._pointCount = 1;
		        manifold._type = ManifoldType.Circles;
		        manifold._localNormal = Vector2.Zero;
		        manifold._localPoint = P;
	            var mp = new ManifoldPoint();
		        mp.Id.Key = 0;
		        mp.Id.Features = cf;
		        mp.LocalPoint = circleB._p;
                manifold._points[0] = mp;
		        return;
	        }

	        // Region AB
	        float den = Vector2.Dot(e, e);
	        Debug.Assert(den > 0.0f);
	        P = (1.0f / den) * (u * A + v * B);
	        d = Q - P;
	        float dd2 = Vector2.Dot(d, d);
	        if (dd2 > radius * radius)
	        {
		        return;
	        }

	        Vector2 n = new Vector2(-e.Y, e.X);
	        if (Vector2.Dot(n, Q - A) < 0.0f)
	        {
		        n = new Vector2(-n.X, -n.Y);
	        }
	        n.Normalize();

	        cf.indexA = 0;
	        cf.typeA = (byte)ContactFeatureType.Face;
	        manifold._pointCount = 1;
	        manifold._type = ManifoldType.FaceA;
	        manifold._localNormal = n;
	        manifold._localPoint = A;
            var mp2 = new ManifoldPoint();
	        mp2.Id.Key = 0;
	        mp2.Id.Features = cf;
            mp2.LocalPoint = circleB._p;
            manifold._points[0] = mp2;
        }

    	public enum EPAxisType
        {
	        Unknown,
	        EdgeA,
	        EdgeB,
        };

        public struct EPAxis
        {
	        public EPAxisType type;
	        public int index;
	        public float separation;
        };

        static EPAxis ComputeEdgeSeperation(Vector2 v1, Vector2 v2, Vector2 n, PolygonShape polygonB, float radius)
        {
	        // EdgeA separation
	        EPAxis axis;
	        axis.type = EPAxisType.EdgeA;
	        axis.index = 0;
	        axis.separation = Vector2.Dot(n, polygonB._vertices[0] - v1);
	        for (int i = 1; i < polygonB._vertexCount; ++i)
	        {
		        float s = Vector2.Dot(n, polygonB._vertices[i] - v1);
		        if (s < axis.separation)
		        {
			        axis.separation = s;
		        }
	        }

	        return axis;
        }

        static EPAxis ComputePolygonSeperation(Vector2 v1, Vector2 v2, Vector2 n, PolygonShape polygonB, float radius)
        {
	        // PolygonB separation
	        EPAxis axis;
	        axis.type = EPAxisType.EdgeB;
	        axis.index = 0;
	        axis.separation = float.MinValue;
	        for (int i = 0; i < polygonB._vertexCount; ++i)
	        {
		        float s1 = Vector2.Dot(polygonB._normals[i], v1 - polygonB._vertices[i]);
		        float s2 = Vector2.Dot(polygonB._normals[i], v2 - polygonB._vertices[i]);
		        float s = Math.Min(s1, s2);
		        if (s > axis.separation)
		        {
			        axis.index = i;
			        axis.separation = s;
			        if (s > radius)
			        {
				        return axis;
			        }
		        }
	        }

	        return axis;
        }

        static void FindIncidentEdge(ref FixedArray2<ClipVertex> c, PolygonShape poly1, int edge1, PolygonShape poly2)
        {
	        int count1 = poly1._vertexCount;
	        int count2 = poly2._vertexCount;

	        Debug.Assert(0 <= edge1 && edge1 < count1);

	        // Get the normal of the reference edge in poly2's frame.
            Vector2 normal1 = poly1._normals[edge1];

	        // Find the incident edge on poly2.
	        int index = 0;
	        float minDot = float.MaxValue;
	        for (int i = 0; i < count2; ++i)
	        {
                float dot = Vector2.Dot(normal1, poly2._normals[i]);
		        if (dot < minDot)
		        {
			        minDot = dot;
			        index = i;
		        }
	        }

	        // Build the clip vertices for the incident edge.
	        int i1 = index;
	        int i2 = i1 + 1 < count2 ? i1 + 1 : 0;

            ClipVertex ctemp = new ClipVertex();
            ctemp.v = poly2._vertices[i1];
            ctemp.id.Features.indexA = (byte)edge1;
            ctemp.id.Features.indexB = (byte)i1;
            ctemp.id.Features.typeA = (byte)ContactFeatureType.Face;
            ctemp.id.Features.typeB = (byte)ContactFeatureType.Vertex;
            c[0] = ctemp;

            ctemp.v = poly2._vertices[i2];
            ctemp.id.Features.indexA = (byte)edge1;
            ctemp.id.Features.indexB = (byte)i2;
            ctemp.id.Features.typeA = (byte)ContactFeatureType.Face;
            ctemp.id.Features.typeB = (byte)ContactFeatureType.Vertex;
            c[1] = ctemp;
        }

        // Collide and edge and polygon. This uses the SAT and clipping to produce up to 2 contact points.
        // Edge adjacency is handle to produce locally valid contact points and normals. This is intended
        // to allow the polygon to slide smoothly over an edge chain.
        //
        // Algorithm
        // 1. Classify front-side or back-side collision with edge.
        // 2. Compute separation
        // 3. Process adjacent edges
        // 4. Classify adjacent edge as convex, flat, null, or concave
        // 5. Skip null or concave edges. Concave edges get a separate manifold.
        // 6. If the edge is flat, compute contact points as normal. Discard boundary points.
        // 7. If the edge is convex, compute it's separation.
        // 8. Use the minimum separation of up to three edges. If the minimum separation
        //    is not the primary edge, return.
        // 9. If the minimum separation is the primary edge, compute the contact points and return.

        static PolygonShape s_polygonA = new PolygonShape();
        static PolygonShape s_polygonB = new PolygonShape();
        public static void CollideEdgeAndPolygon(ref Manifold manifold,
					           EdgeShape edgeA, ref Transform xfA,
					           PolygonShape polygonB_in, ref Transform xfB)
        {
	        manifold._pointCount = 0;

            Transform xf;
            MathUtils.MultiplyT(ref xfA, ref xfB, out xf);

	        // Create a polygon for edge shape A
            s_polygonA.SetAsEdge(edgeA._vertex1, edgeA._vertex2);

	        // Build polygonB in frame A
            s_polygonB._radius = polygonB_in._radius;
            s_polygonB._vertexCount = polygonB_in._vertexCount;
            s_polygonB._centroid = MathUtils.Multiply(ref xf, polygonB_in._centroid);
            for (int i = 0; i < s_polygonB._vertexCount; ++i)
	        {
                s_polygonB._vertices[i] = MathUtils.Multiply(ref xf, polygonB_in._vertices[i]);
                s_polygonB._normals[i] = MathUtils.Multiply(ref xf.R, polygonB_in._normals[i]);
	        }

            float totalRadius = s_polygonA._radius + s_polygonB._radius;

	        // Edge geometry
	        Vector2 v1 = edgeA._vertex1;
	        Vector2 v2 = edgeA._vertex2;
	        Vector2 e = v2 - v1;
	        Vector2 edgeNormal = new Vector2(e.Y, -e.X);
	        edgeNormal.Normalize();

	        // Determine side
            bool isFrontSide = Vector2.Dot(edgeNormal, s_polygonB._centroid - v1) >= 0.0f;
	        if (isFrontSide == false)
	        {
		        edgeNormal = -edgeNormal;
	        }

	        // Compute primary separating axis
            EPAxis edgeAxis = ComputeEdgeSeperation(v1, v2, edgeNormal, s_polygonB, totalRadius);
	        if (edgeAxis.separation > totalRadius)
	        {
		        // Shapes are separated
		        return;
	        }

	        // Classify adjacent edges
            FixedArray2<EdgeType> types = new FixedArray2<EdgeType>();
            //types[0] = EdgeType.Isolated;
            //types[1] = EdgeType.Isolated;
	        if (edgeA._hasVertex0)
	        {
		        Vector2 v0 = edgeA._vertex0;
		        float s = Vector2.Dot(edgeNormal, v0 - v1);

		        if (s > 0.1f * Settings.b2_linearSlop)
		        {
			        types[0] = EdgeType.Concave;
		        }
		        else if (s >= -0.1f * Settings.b2_linearSlop)
		        {
			        types[0] = EdgeType.Flat;
		        }
		        else
		        {
			        types[0] = EdgeType.Convex;
		        }
	        }

	        if (edgeA._hasVertex3)
	        {
		        Vector2 v3 = edgeA._vertex3;
		        float s = Vector2.Dot(edgeNormal, v3 - v2);
		        if (s > 0.1f * Settings.b2_linearSlop)
		        {
			        types[1] = EdgeType.Concave;
		        }
		        else if (s >= -0.1f * Settings.b2_linearSlop)
		        {
			        types[1] = EdgeType.Flat;
		        }
		        else
		        {
			        types[1] = EdgeType.Convex;
		        }
	        }

	        if (types[0] == EdgeType.Convex)
	        {
		        // Check separation on previous edge.
		        Vector2 v0 = edgeA._vertex0;
		        Vector2 e0 = v1 - v0;

		        Vector2 n0 = new Vector2(e0.Y, -e0.X);
		        n0.Normalize();
		        if (isFrontSide == false)
		        {
			        n0 = -n0;
		        }

                EPAxis axis1 = ComputeEdgeSeperation(v0, v1, n0, s_polygonB, totalRadius);
		        if (axis1.separation > edgeAxis.separation)
		        {
			        // The polygon should collide with previous edge
			        return;
		        }
	        }

	        if (types[1] == EdgeType.Convex)
	        {
		        // Check separation on next edge.
		        Vector2 v3 = edgeA._vertex3;
		        Vector2 e2 = v3 - v2;

		        Vector2 n2 = new Vector2(e2.Y, -e2.X);
		        n2.Normalize();
		        if (isFrontSide == false)
		        {
			        n2 = -n2;
		        }

                EPAxis axis2 = ComputeEdgeSeperation(v2, v3, n2, s_polygonB, totalRadius);
		        if (axis2.separation > edgeAxis.separation)
		        {
			        // The polygon should collide with the next edge
			        return;
		        }
	        }

            EPAxis polygonAxis = ComputePolygonSeperation(v1, v2, edgeNormal, s_polygonB, totalRadius);
	        if (polygonAxis.separation > totalRadius)
	        {
		        return;
	        }

	        // Use hysteresis for jitter reduction.
	        float k_relativeTol = 0.98f;
	        float k_absoluteTol = 0.001f;

	        EPAxis primaryAxis;
	        if (polygonAxis.separation > k_relativeTol * edgeAxis.separation + k_absoluteTol)
	        {
		        primaryAxis = polygonAxis;
	        }
	        else
	        {
		        primaryAxis = edgeAxis;
	        }

	        PolygonShape poly1;
	        PolygonShape poly2;
	        if (primaryAxis.type == EPAxisType.EdgeA)
	        {
                poly1 = s_polygonA;
                poly2 = s_polygonB;
		        if (isFrontSide == false)
		        {
			        primaryAxis.index = 1;
		        }
		        manifold._type = ManifoldType.FaceA;
	        }
	        else
	        {
                poly1 = s_polygonB;
                poly2 = s_polygonA;
		        manifold._type = ManifoldType.FaceB;
	        }

	        int edge1 = primaryAxis.index;

            FixedArray2<ClipVertex> incidentEdge = new FixedArray2<ClipVertex>();
	        FindIncidentEdge(ref incidentEdge, poly1, primaryAxis.index, poly2);
	        int count1 = poly1._vertexCount;
	        int iv1 = edge1;
	        int iv2 = edge1 + 1 < count1 ? edge1 + 1 : 0;

	        Vector2 v11 = poly1._vertices[iv1];
	        Vector2 v12 = poly1._vertices[iv2];

	        Vector2 tangent = v12 - v11;
	        tangent.Normalize();

	        Vector2 normal = MathUtils.Cross(tangent, 1.0f);
	        Vector2 planePoint = 0.5f * (v11 + v12);

	        // Face offset.
	        float frontOffset = Vector2.Dot(normal, v11);

	        // Side offsets, extended by polytope skin thickness.
	        float sideOffset1 = -Vector2.Dot(tangent, v11) + totalRadius;
	        float sideOffset2 = Vector2.Dot(tangent, v12) + totalRadius;

	        // Clip incident edge against extruded edge1 side edges.
	        FixedArray2<ClipVertex> clipPoints1;
	        FixedArray2<ClipVertex> clipPoints2;
	        int np;

	        // Clip to box side 1
	        np = ClipSegmentToLine(out clipPoints1, ref incidentEdge, -tangent, sideOffset1, iv1);

	        if (np < Settings.b2_maxManifoldPoints)
	        {
		        return;
	        }

	        // Clip to negative box side 1
	        np = ClipSegmentToLine(out clipPoints2, ref clipPoints1,  tangent, sideOffset2, iv2);

            if (np < Settings.b2_maxManifoldPoints)
	        {
		        return;
	        }

	        // Now clipPoints2 contains the clipped points.
	        if (primaryAxis.type == EPAxisType.EdgeA)
	        {
		        manifold._localNormal = normal;
		        manifold._localPoint = planePoint;
	        }
	        else
	        {
		        manifold._localNormal = MathUtils.MultiplyT(ref xf.R, normal);
		        manifold._localPoint = MathUtils.MultiplyT(ref xf, planePoint);
	        }

	        int pointCount = 0;
	        for (int i = 0; i < Settings.b2_maxManifoldPoints; ++i)
	        {
		        float separation;

		        separation = Vector2.Dot(normal, clipPoints2[i].v) - frontOffset;

		        if (separation <= totalRadius)
		        {
			        ManifoldPoint cp = manifold._points[pointCount];

			        if (primaryAxis.type == EPAxisType.EdgeA)
			        {
				        cp.LocalPoint = MathUtils.MultiplyT(ref xf, clipPoints2[i].v);
                        cp.Id = clipPoints2[i].id;
			        }
			        else
			        {
				        cp.LocalPoint = clipPoints2[i].v;
                        cp.Id.Features.typeA = clipPoints2[i].id.Features.typeB;
                        cp.Id.Features.typeB = clipPoints2[i].id.Features.typeA;
                        cp.Id.Features.indexA = clipPoints2[i].id.Features.indexB;
                        cp.Id.Features.indexB = clipPoints2[i].id.Features.indexA;
			        }

                    manifold._points[pointCount] = cp;
                    if (cp.Id.Features.typeA == (byte)ContactFeatureType.Vertex && types[cp.Id.Features.indexA] == EdgeType.Flat)
			        {
				        continue;
			        }

			        ++pointCount;
		        }
	        }

	        manifold._pointCount = pointCount;
        }

        /// Clipping for contact manifolds.
        public static int ClipSegmentToLine(out FixedArray2<ClipVertex> vOut, ref FixedArray2<ClipVertex> vIn,
							        Vector2 normal, float offset, int vertexIndexA)
        {
            vOut = new FixedArray2<ClipVertex>();

	        // Start with no output points
	        int numOut = 0;

	        // Calculate the distance of end points to the line
	        float distance0 = Vector2.Dot(normal, vIn[0].v) - offset;
	        float distance1 = Vector2.Dot(normal, vIn[1].v) - offset;

	        // If the points are behind the plane
	        if (distance0 <= 0.0f) vOut[numOut++] = vIn[0];
	        if (distance1 <= 0.0f) vOut[numOut++] = vIn[1];

	        // If the points are on different sides of the plane
	        if (distance0 * distance1 < 0.0f)
	        {
		        // Find intersection point of edge and plane
		        float interp = distance0 / (distance0 - distance1);

                var cv = vOut[numOut];

                cv.v = vIn[0].v + interp * (vIn[1].v - vIn[0].v);

                // VertexA is hitting edgeB.
		        cv.id.Features.indexA = (byte)vertexIndexA;
                cv.id.Features.indexB = vIn[0].id.Features.indexB;
                cv.id.Features.typeA = (byte)ContactFeatureType.Vertex;
                cv.id.Features.typeB = (byte)ContactFeatureType.Face;

                vOut[numOut] = cv;

		        ++numOut;
	        }

	        return numOut;
        }

        // Find the separation between poly1 and poly2 for a give edge normal on poly1.
        static float EdgeSeparation(PolygonShape poly1, ref Transform xf1, int edge1,
							        PolygonShape poly2, ref Transform xf2)
        {
	        int count1 = poly1._vertexCount;
	        int count2 = poly2._vertexCount;

	        Debug.Assert(0 <= edge1 && edge1 < count1);

	        // Convert normal from poly1's frame into poly2's frame.
#if MATH_OVERLOADS
            Vector2 normal1World = MathUtils.Multiply(ref xf1.R, poly1._normals[edge1]);
            Vector2 normal1 = MathUtils.MultiplyT(ref xf2.R, normal1World);
#else
            Vector2 p1n = poly1._normals[edge1];
            Vector2 normal1World = new Vector2(xf1.R.col1.X * p1n.X + xf1.R.col2.X * p1n.Y, xf1.R.col1.Y * p1n.X + xf1.R.col2.Y * p1n.Y);
            Vector2 normal1 = new Vector2(normal1World.X * xf2.R.col1.X + normal1World.Y * xf2.R.col1.Y, normal1World.X * xf2.R.col2.X + normal1World.Y * xf2.R.col2.Y);
#endif
	        // Find support vertex on poly2 for -normal.
	        int index = 0;
	        float minDot = Settings.b2_maxFloat;

	        for (int i = 0; i < count2; ++i)
	        {
#if !MATH_OVERLOADS // inlining this made it 1ms slower
		        float dot = Vector2.Dot(poly2._vertices[i], normal1);
#else
                Vector2 p2vi = poly2._vertices[i];
                float dot = p2vi.X * normal1.X + p2vi.Y * normal1.Y;
#endif
		        if (dot < minDot)
		        {
			        minDot = dot;
			        index = i;
		        }
	        }

#if MATH_OVERLOADS
	        Vector2 v1 = MathUtils.Multiply(ref xf1, poly1._vertices[edge1]);
	        Vector2 v2 = MathUtils.Multiply(ref xf2, poly2._vertices[index]);
#else
            Vector2 p1ve = poly1._vertices[edge1];
            Vector2 p2vi = poly2._vertices[index];
            Vector2 v1 = new Vector2(xf1.Position.X + xf1.R.col1.X * p1ve.X + xf1.R.col2.X * p1ve.Y,
                                     xf1.Position.Y + xf1.R.col1.Y * p1ve.X + xf1.R.col2.Y * p1ve.Y);
            Vector2 v2 = new Vector2(xf2.Position.X + xf2.R.col1.X * p2vi.X + xf2.R.col2.X * p2vi.Y,
                                     xf2.Position.Y + xf2.R.col1.Y * p2vi.X + xf2.R.col2.Y * p2vi.Y);
#endif

#if !MATH_OVERLOADS // inlining is 1ms slower
            float separation = Vector2.Dot(v2 - v1, normal1World);
#else
            Vector2 v2subv1 = new Vector2(v2.X - v1.X, v2.Y - v1.Y);
            float separation = v2subv1.X * normal1World.X + v2subv1.Y * normal1World.Y;
#endif
            return separation;
        }

         // Find the max separation between poly1 and poly2 using edge normals from poly1.
        static float FindMaxSeparation( out int edgeIndex,
								        PolygonShape poly1, ref Transform xf1,
								        PolygonShape poly2, ref Transform xf2)
        {
            edgeIndex = -1;
	        int count1 = poly1._vertexCount;

	        // Vector pointing from the centroid of poly1 to the centroid of poly2.
	        Vector2 d = MathUtils.Multiply(ref xf2, poly2._centroid) - MathUtils.Multiply(ref xf1, poly1._centroid);
	        Vector2 dLocal1 = MathUtils.MultiplyT(ref xf1.R, d);

	        // Find edge normal on poly1 that has the largest projection onto d.
	        int edge = 0;
	        float maxDot = -Settings.b2_maxFloat;
	        for (int i = 0; i < count1; ++i)
	        {
		        float dot = Vector2.Dot(poly1._normals[i], dLocal1);
		        if (dot > maxDot)
		        {
			        maxDot = dot;
			        edge = i;
		        }
	        }

	        // Get the separation for the edge normal.
	        float s = EdgeSeparation(poly1, ref xf1, edge, poly2, ref xf2);

	        // Check the separation for the previous edge normal.
	        int prevEdge = edge - 1 >= 0 ? edge - 1 : count1 - 1;
	        float sPrev = EdgeSeparation(poly1, ref xf1, prevEdge, poly2, ref xf2);

	        // Check the separation for the next edge normal.
	        int nextEdge = edge + 1 < count1 ? edge + 1 : 0;
	        float sNext = EdgeSeparation(poly1, ref xf1, nextEdge, poly2, ref xf2);

	        // Find the best edge and the search direction.
	        int bestEdge;
	        float bestSeparation;
	        int increment;
	        if (sPrev > s && sPrev > sNext)
	        {
		        increment = -1;
		        bestEdge = prevEdge;
		        bestSeparation = sPrev;
	        }
	        else if (sNext > s)
	        {
		        increment = 1;
		        bestEdge = nextEdge;
		        bestSeparation = sNext;
	        }
	        else
	        {
		        edgeIndex = edge;
		        return s;
	        }

	        // Perform a local search for the best edge normal.
	        for ( ; ; )
	        {
		        if (increment == -1)
			        edge = bestEdge - 1 >= 0 ? bestEdge - 1 : count1 - 1;
		        else
			        edge = bestEdge + 1 < count1 ? bestEdge + 1 : 0;

		        s = EdgeSeparation(poly1, ref xf1, edge, poly2, ref xf2);

		        if (s > bestSeparation)
		        {
			        bestEdge = edge;
			        bestSeparation = s;
		        }
		        else
		        {
			        break;
		        }
	        }

	        edgeIndex = bestEdge;
	        return bestSeparation;
        }

        static void FindIncidentEdge(out FixedArray2<ClipVertex> c,
							         PolygonShape poly1, ref Transform xf1, int edge1,
							         PolygonShape poly2, ref Transform xf2)
        {
            c = new FixedArray2<ClipVertex>();

	        int count1 = poly1._vertexCount;
	        int count2 = poly2._vertexCount;

	        Debug.Assert(0 <= edge1 && edge1 < count1);

	        // Get the normal of the reference edge in poly2's frame.
	        Vector2 normal1 = MathUtils.MultiplyT(ref xf2.R, MathUtils.Multiply(ref xf1.R, poly1._normals[edge1]));

	        // Find the incident edge on poly2.
	        int index = 0;
	        float minDot = Settings.b2_maxFloat;
	        for (int i = 0; i < count2; ++i)
	        {
		        float dot = Vector2.Dot(normal1, poly2._normals[i]);
		        if (dot < minDot)
		        {
			        minDot = dot;
			        index = i;
		        }
	        }

	        // Build the clip vertices for the incident edge.
	        int i1 = index;
	        int i2 = i1 + 1 < count2 ? i1 + 1 : 0;

            var cv0 = c[0];

            cv0.v = MathUtils.Multiply(ref xf2, poly2._vertices[i1]);
            cv0.id.Features.indexA = (byte)edge1;
            cv0.id.Features.indexB = (byte)i1;
            cv0.id.Features.typeA = (byte)ContactFeatureType.Face;
            cv0.id.Features.typeB = (byte)ContactFeatureType.Vertex;

            c[0] = cv0;

            var cv1 = c[1];
            cv1.v = MathUtils.Multiply(ref xf2, poly2._vertices[i2]);
            cv1.id.Features.indexA = (byte)edge1;
            cv1.id.Features.indexB = (byte)i2;
            cv1.id.Features.typeA = (byte)ContactFeatureType.Face;
            cv1.id.Features.typeB = (byte)ContactFeatureType.Vertex;

            c[1] = cv1;
        }
    }
}


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
{
    /// A distance proxy is used by the GJK algorithm.
    /// It encapsulates any shape.
    public struct DistanceProxy
    {
	    /// Initialize the proxy using the given shape. The shape
	    /// must remain in scope while the proxy is in use.
	    public void Set(Shape shape, int index)
        {
            switch (shape.ShapeType)
	        {
	        case ShapeType.Circle:
		        {
			        CircleShape circle = (CircleShape)shape;
                    _vertices[0] = circle._p;
			        _count = 1;
			        _radius = circle._radius;
		        }
		        break;

            case ShapeType.Polygon:
		        {
			        PolygonShape polygon = (PolygonShape)shape;
			        _vertices = polygon._vertices;
			        _count = polygon._vertexCount;
			        _radius = polygon._radius;
		        }
		        break;

	        case ShapeType.Loop:
	            {
		            LoopShape loop = (LoopShape)shape;
		            Debug.Assert(0 <= index && index < loop._count);

		            _buffer[0] = loop._vertices[index];
		            if (index + 1 < loop._count)
		            {
			            _buffer[1] = loop._vertices[index + 1];
		            }
		            else
		            {
			            _buffer[1] = loop._vertices[0];
		            }

                    _vertices[0] = _buffer[0];
                    _vertices[1] = _buffer[1];
		            _count = 2;
		            _radius = loop._radius;
	            }
	            break;

	        case ShapeType.Edge:
	            {
		            EdgeShape edge = (EdgeShape)shape;
                    _vertices[0] = edge._vertex1;
                    _vertices[1] = edge._vertex2;
		            _count = 2;
		            _radius = edge._radius;
                }
                break;

	        default:
		        Debug.Assert(false);
                break;
	        }

        }

	    /// Get the supporting vertex index in the given direction.
	    public int GetSupport(Vector2 d)
        {
            int bestIndex = 0;
            float bestValue = Vector2.Dot(_vertices[0], d);
            for (int i = 1; i < _count; ++i)
            {
                float value = Vector2.Dot(_vertices[i], d);
                if (value > bestValue)
                {
                    bestIndex = i;
                    bestValue = value;
                }
            }

            return bestIndex;
        }

	    /// Get the supporting vertex in the given direction.
	    public Vector2 GetSupportVertex(Vector2 d)
        {
            int bestIndex = 0;
            float bestValue = Vector2.Dot(_vertices[0], d);
            for (int i = 1; i < _count; ++i)
            {
                float value = Vector2.Dot(_vertices[i], d);
                if (value > bestValue)
                {
                    bestIndex = i;
                    bestValue = value;
                }
            }

            return _vertices[bestIndex];
        }

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

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

	    internal FixedArray8<Vector2> _vertices;
        internal FixedArray2<Vector2> _buffer;
	    internal int _count;
	    internal float _radius;
    };

    /// Used to warm start ComputeDistance.
    /// Set count to zero on first call.
    public struct SimplexCache
    {
	    public float metric;		///< length or area
        public UInt16 count;
        public FixedArray3<byte> indexA;	///< vertices on shape A
        public FixedArray3<byte> indexB;	///< vertices on shape B
    };

    /// Input for ComputeDistance.
    /// You have to option to use the shape radii
    /// in the computation. Even
    public struct DistanceInput
    {
        public DistanceProxy proxyA;
        public DistanceProxy proxyB;
        public Transform transformA;
        public Transform transformB;
        public bool useRadii;
    };

    /// Output for ComputeDistance.
    public struct DistanceOutput
    {
        public Vector2 pointA;		///< closest point on shapeA
        public Vector2 pointB;		///< closest point on shapeB
        public float distance;
        public int iterations;	///< number of GJK iterations used
    };

    internal struct SimplexVertex
    {
        public Vector2 wA;		// support point in proxyA
        public Vector2 wB;		// support point in proxyB
        public Vector2 w;		// wB - wA
        public float a;		// barycentric coordinate for closest point
        public int indexA;	// wA index
        public int indexB;	// wB index
    };

    internal struct Simplex
    {
	    internal void ReadCache(ref SimplexCache cache,
					    ref DistanceProxy proxyA, ref Transform transformA,
                        ref DistanceProxy proxyB, ref Transform transformB)
	    {
		    Debug.Assert(cache.count <= 3);

		    // Copy data from cache.
		    _count = cache.count;
		    for (int i = 0; i < _count; ++i)
		    {
			    SimplexVertex v = _v[i];
			    v.indexA = cache.indexA[i];
			    v.indexB = cache.indexB[i];
			    Vector2 wALocal = proxyA.GetVertex(v.indexA);
                Vector2 wBLocal = proxyB.GetVertex(v.indexB);
			    v.wA = MathUtils.Multiply(ref transformA, wALocal);
			    v.wB = MathUtils.Multiply(ref transformB, wBLocal);
			    v.w = v.wB - v.wA;
			    v.a = 0.0f;
                _v[i] = v;
		    }

		    // Compute the new simplex metric, if it is substantially different than
		    // old metric then flush the simplex.
		    if (_count > 1)
		    {
			    float metric1 = cache.metric;
			    float metric2 = GetMetric();
			    if (metric2 < 0.5f * metric1 || 2.0f * metric1 < metric2 || metric2 < Settings.b2_epsilon)
			    {
				    // Reset the simplex.
				    _count = 0;
			    }
		    }

		    // If the cache is empty or invalid ...
		    if (_count == 0)
		    {
			    SimplexVertex v = _v[0];
			    v.indexA = 0;
			    v.indexB = 0;
			    Vector2 wALocal = proxyA.GetVertex(0);
                Vector2 wBLocal = proxyB.GetVertex(0);
			    v.wA = MathUtils.Multiply(ref transformA, wALocal);
			    v.wB = MathUtils.Multiply(ref transformB, wBLocal);
			    v.w = v.wB - v.wA;
                _v[0] = v;
			    _count = 1;
		    }
	    }

        internal void WriteCache(ref SimplexCache cache)
	    {
		    cache.metric = GetMetric();
		    cache.count = (UInt16)_count;
		    for (int i = 0; i < _count; ++i)
		    {
                cache.indexA[i] = (byte)(_v[i].indexA);
                cache.indexB[i] = (byte)(_v[i].indexB);
		    }
	    }

        internal Vector2 GetSearchDirection()
	    {
		    switch (_count)
		    {
		    case 1:
			    return -_v[0].w;

		    case 2:
			    {
				    Vector2 e12 = _v[1].w - _v[0].w;
				    float sgn = MathUtils.Cross(e12, -_v[0].w);
				    if (sgn > 0.0f)
				    {
					    // Origin is left of e12.
					    return MathUtils.Cross(1.0f, e12);
				    }
				    else
				    {
					    // Origin is right of e12.
					    return MathUtils.Cross(e12, 1.0f);
				    }
			    }

		    default:
			    Debug.Assert(false);
			    return Vector2.Zero;
		    }
	    }

        internal Vector2 GetClosestPoint()
	    {
		    switch (_count)
		    {
		    case 0:
			    Debug.Assert(false);
			    return Vector2.Zero;

		    case 1:
			    return _v[0].w;

		    case 2:
			    return _v[0].a * _v[0].w + _v[1].a * _v[1].w;

		    case 3:
			    return Vector2.Zero;

		    default:
			    Debug.Assert(false);
			    return Vector2.Zero;
		    }
	    }

        internal void GetWitnessPoints(out Vector2 pA, out Vector2 pB)
	    {
		    switch (_count)
		    {
		    case 0:
                pA = Vector2.Zero;
                pB = Vector2.Zero;
			    Debug.Assert(false);
			    break;

		    case 1:
			    pA = _v[0].wA;
			    pB = _v[0].wB;
			    break;

		    case 2:
			    pA = _v[0].a * _v[0].wA + _v[1].a * _v[1].wA;
			    pB = _v[0].a * _v[0].wB + _v[1].a * _v[1].wB;
			    break;

		    case 3:
			    pA = _v[0].a * _v[0].wA + _v[1].a * _v[1].wA + _v[2].a * _v[2].wA;
			    pB = pA;
			    break;

		    default:
                throw new Exception();
		    }
	    }

        internal float GetMetric()
	    {
		    switch (_count)
		    {
		    case 0:
			    Debug.Assert(false);
			    return 0.0f;

		    case 1:
			    return 0.0f;

		    case 2:
			    return (_v[0].w - _v[1].w).Length();

		    case 3:
			    return MathUtils.Cross(_v[1].w - _v[0].w, _v[2].w - _v[0].w);

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

        // Solve a line segment using barycentric coordinates.
        //
        // p = a1 * w1 + a2 * w2
        // a1 + a2 = 1
        //
        // The vector from the origin to the closest point on the line is
        // perpendicular to the line.
        // e12 = w2 - w1
        // dot(p, e) = 0
        // a1 * dot(w1, e) + a2 * dot(w2, e) = 0
        //
        // 2-by-2 linear system
        // [1      1     ][a1] = [1]
        // [w1.e12 w2.e12][a2] = [0]
        //
        // Define
        // d12_1 =  dot(w2, e12)
        // d12_2 = -dot(w1, e12)
        // d12 = d12_1 + d12_2
        //
        // Solution
        // a1 = d12_1 / d12
        // a2 = d12_2 / d12

        internal void Solve2()
        {
            Vector2 w1 = _v[0].w;
            Vector2 w2 = _v[1].w;
            Vector2 e12 = w2 - w1;

            // w1 region
            float d12_2 = -Vector2.Dot(w1, e12);
            if (d12_2 <= 0.0f)
            {
                // a2 <= 0, so we clamp it to 0
                var v0 = _v[0];
                v0.a = 1.0f;
                _v[0] = v0;
                _count = 1;
                return;
            }

            // w2 region
            float d12_1 = Vector2.Dot(w2, e12);
            if (d12_1 <= 0.0f)
            {
                // a1 <= 0, so we clamp it to 0
                var v1 = _v[1];
                v1.a = 1.0f;
                _v[1] = v1;
                _count = 1;
                _v[0] = _v[1];
                return;
            }

            // Must be in e12 region.
            float inv_d12 = 1.0f / (d12_1 + d12_2);
            var v0_2 = _v[0];
            var v1_2 = _v[1];
            v0_2.a = d12_1 * inv_d12;
            v1_2.a = d12_2 * inv_d12;
            _v[0] = v0_2;
            _v[1] = v1_2;
            _count = 2;
        }

        // Possible regions:
        // - points[2]
        // - edge points[0]-points[2]
        // - edge points[1]-points[2]
        // - inside the triangle
        internal void Solve3()
        {
            Vector2 w1 = _v[0].w;
            Vector2 w2 = _v[1].w;
            Vector2 w3 = _v[2].w;

            // Edge12
            // [1      1     ][a1] = [1]
            // [w1.e12 w2.e12][a2] = [0]
            // a3 = 0
            Vector2 e12 = w2 - w1;
            float w1e12 = Vector2.Dot(w1, e12);
            float w2e12 = Vector2.Dot(w2, e12);
            float d12_1 = w2e12;
            float d12_2 = -w1e12;

            // Edge13
            // [1      1     ][a1] = [1]
            // [w1.e13 w3.e13][a3] = [0]
            // a2 = 0
            Vector2 e13 = w3 - w1;
            float w1e13 = Vector2.Dot(w1, e13);
            float w3e13 = Vector2.Dot(w3, e13);
            float d13_1 = w3e13;
            float d13_2 = -w1e13;

            // Edge23
            // [1      1     ][a2] = [1]
            // [w2.e23 w3.e23][a3] = [0]
            // a1 = 0
            Vector2 e23 = w3 - w2;
            float w2e23 = Vector2.Dot(w2, e23);
            float w3e23 = Vector2.Dot(w3, e23);
            float d23_1 = w3e23;
            float d23_2 = -w2e23;

            // Triangle123
            float n123 = MathUtils.Cross(e12, e13);

            float d123_1 = n123 * MathUtils.Cross(w2, w3);
            float d123_2 = n123 * MathUtils.Cross(w3, w1);
            float d123_3 = n123 * MathUtils.Cross(w1, w2);

            // w1 region
            if (d12_2 <= 0.0f && d13_2 <= 0.0f)
            {
                var v0_1 = _v[0];
                v0_1.a = 1.0f;
                _v[0] = v0_1;
                _count = 1;
                return;
            }

            // e12
            if (d12_1 > 0.0f && d12_2 > 0.0f && d123_3 <= 0.0f)
            {
                float inv_d12 = 1.0f / (d12_1 + d12_2);
                var v0_2 = _v[0];
                var v1_2 = _v[1];
                v0_2.a = d12_1 * inv_d12;
                v1_2.a = d12_2 * inv_d12;
                _v[0] = v0_2;
                _v[1] = v1_2;
                _count = 2;
                return;
            }

            // e13
            if (d13_1 > 0.0f && d13_2 > 0.0f && d123_2 <= 0.0f)
            {
                float inv_d13 = 1.0f / (d13_1 + d13_2);
                var v0_3 = _v[0];
                var v2_3 = _v[2];
                v0_3.a = d13_1 * inv_d13;
                v2_3.a = d13_2 * inv_d13;
                _v[0] = v0_3;
                _v[2] = v2_3;
                _count = 2;
                _v[1] = _v[2];
                return;
            }

            // w2 region
            if (d12_1 <= 0.0f && d23_2 <= 0.0f)
            {
                var v1_4 = _v[1];
                v1_4.a = 1.0f;
                _v[1] = v1_4;
                _count = 1;
                _v[0] = _v[1];
                return;
            }

            // w3 region
            if (d13_1 <= 0.0f && d23_1 <= 0.0f)
            {
                var v2_5 = _v[2];
                v2_5.a = 1.0f;
                _v[2] = v2_5;
                _count = 1;
                _v[0] = _v[2];
                return;
            }

            // e23
            if (d23_1 > 0.0f && d23_2 > 0.0f && d123_1 <= 0.0f)
            {
                float inv_d23 = 1.0f / (d23_1 + d23_2);
                var v1_6 = _v[1];
                var v2_6 = _v[2];
                v1_6.a = d23_1 * inv_d23;
                v2_6.a = d23_2 * inv_d23;
                _v[1] = v1_6;
                _v[2] = v2_6;
                _count = 2;
                _v[0] = _v[2];
                return;
            }

            // Must be in triangle123
            float inv_d123 = 1.0f / (d123_1 + d123_2 + d123_3);
            var v0_7 = _v[0];
            var v1_7 = _v[1];
            var v2_7 = _v[2];
            v0_7.a = d123_1 * inv_d123;
            v1_7.a = d123_2 * inv_d123;
            v2_7.a = d123_3 * inv_d123;
            _v[0] = v0_7;
            _v[1] = v1_7;
            _v[2] = v2_7;
            _count = 3;
        }

	    internal FixedArray3<SimplexVertex> _v;
        internal int _count;
    };

    public static class Distance
    {
        public static void ComputeDistance(out DistanceOutput output,
				                           out SimplexCache cache,
				                           ref DistanceInput input)
        {
            cache = new SimplexCache();
	        ++b2_gjkCalls;

	        // Initialize the simplex.
	        Simplex simplex = new Simplex();
            simplex.ReadCache(ref cache, ref input.proxyA, ref input.transformA, ref input.proxyB, ref input.transformB);

	        // Get simplex vertices as an array.
	        int k_maxIters = 20;

	        // These store the vertices of the last simplex so that we
	        // can check for duplicates and prevent cycling.
            FixedArray3<int> saveA = new FixedArray3<int>();
            FixedArray3<int> saveB = new FixedArray3<int>();
	        int saveCount = 0;

	        Vector2 closestPoint = simplex.GetClosestPoint();
	        float distanceSqr1 = closestPoint.LengthSquared();
	        float distanceSqr2 = distanceSqr1;

	        // Main iteration loop.
	        int iter = 0;
	        while (iter < k_maxIters)
	        {
		        // Copy simplex so we can identify duplicates.
		        saveCount = simplex._count;
		        for (int i = 0; i < saveCount; ++i)
		        {
			        saveA[i] = simplex._v[i].indexA;
                    saveB[i] = simplex._v[i].indexB;
		        }

		        switch (simplex._count)
		        {
		        case 1:
			        break;

		        case 2:
			        simplex.Solve2();
			        break;

		        case 3:
			        simplex.Solve3();
			        break;

		        default:
			        Debug.Assert(false);
                    break;
		        }

		        // If we have 3 points, then the origin is in the corresponding triangle.
		        if (simplex._count == 3)
		        {
			        break;
		        }

		        // Compute closest point.
		        Vector2 p = simplex.GetClosestPoint();
		        distanceSqr2 = p.LengthSquared();

		        // Ensure progress
		        if (distanceSqr2 >= distanceSqr1)
		        {
			        //break;
		        }
		        distanceSqr1 = distanceSqr2;

		        // Get search direction.
		        Vector2 d = simplex.GetSearchDirection();

		        // Ensure the search direction is numerically fit.
		        if (d.LengthSquared() < Settings.b2_epsilon * Settings.b2_epsilon)
		        {
			        // The origin is probably contained by a line segment
			        // or triangle. Thus the shapes are overlapped.

			        // We can't return zero here even though there may be overlap.
			        // In case the simplex is a point, segment, or triangle it is difficult
			        // to determine if the origin is contained in the CSO or very close to it.
			        break;
		        }

		        // Compute a tentative new simplex vertex using support points.
                SimplexVertex vertex = simplex._v[simplex._count];
                vertex.indexA = input.proxyA.GetSupport(MathUtils.MultiplyT(ref input.transformA.R, -d));
                vertex.wA = MathUtils.Multiply(ref input.transformA, input.proxyA.GetVertex(vertex.indexA));

                vertex.indexB = input.proxyB.GetSupport(MathUtils.MultiplyT(ref input.transformB.R, d));
                vertex.wB = MathUtils.Multiply(ref input.transformB, input.proxyB.GetVertex(vertex.indexB));
		        vertex.w = vertex.wB - vertex.wA;
                simplex._v[simplex._count] = vertex;

		        // Iteration count is equated to the number of support point calls.
		        ++iter;
		        ++b2_gjkIters;

		        // Check for duplicate support points. This is the main termination criteria.
		        bool duplicate = false;
		        for (int i = 0; i < saveCount; ++i)
		        {
			        if (vertex.indexA == saveA[i] && vertex.indexB == saveB[i])
			        {
				        duplicate = true;
				        break;
			        }
		        }

		        // If we found a duplicate support point we must exit to avoid cycling.
		        if (duplicate)
		        {
			        break;
		        }

		        // New vertex is ok and needed.
		        ++simplex._count;
	        }

	        b2_gjkMaxIters = Math.Max(b2_gjkMaxIters, iter);

	        // Prepare output.
	        simplex.GetWitnessPoints(out output.pointA, out output.pointB);
	        output.distance = (output.pointA - output.pointB).Length();
	        output.iterations = iter;

	        // Cache the simplex.
	        simplex.WriteCache(ref cache);

	        // Apply radii if requested.
	        if (input.useRadii)
	        {
                float rA = input.proxyA._radius;
                float rB = input.proxyB._radius;

		        if (output.distance > rA + rB && output.distance > Settings.b2_epsilon)
		        {
			        // Shapes are still no overlapped.
			        // Move the witness points to the outer surface.
			        output.distance -= rA + rB;
			        Vector2 normal = output.pointB - output.pointA;
			        normal.Normalize();
			        output.pointA += rA * normal;
			        output.pointB -= rB * normal;
		        }
		        else
		        {
			        // Shapes are overlapped when radii are considered.
			        // Move the witness points to the middle.
			        Vector2 p = 0.5f * (output.pointA + output.pointB);
			        output.pointA = p;
			        output.pointB = p;
			        output.distance = 0.0f;
		        }
	        }
        }

        public static int b2_gjkCalls, b2_gjkIters, b2_gjkMaxIters;
    }
}