/*
* Copyright (c) 2006-2007 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.
*/

package Box2D.Collision
{
    import Box2D.Collision.Shapes.b2CircleShape;
    import Box2D.Collision.Shapes.b2PolygonShape;
    import Box2D.Common.b2Settings;
    import Box2D.Common.Math.b2Mat22;
    import Box2D.Common.Math.b2Transform;
    import Box2D.Common.Math.b2Vec2;

    /**
    * @private
    */
    public class b2Collision
    {

        // Null feature
        static public const b2_nullFeature:uint = 0x000000ff;//UCHAR_MAX;



        private static var csl_cv:ClipVertex;
        private static var csl_numOut:int;

        private static var csl_vIn0:b2Vec2;
        private static var csl_vIn1:b2Vec2;

        private static var csl_distance0:Number;
        private static var csl_distance1:Number;

        private static var csl_interp:Number;
        private static var csl_tVec:b2Vec2;
        private static var csl_cv2:ClipVertex;

        private static var vInCurrent:ClipVertex;
        private static var vOutCurrent:ClipVertex;

        // Sutherland-Hodgman clipping.

        [Inline]
        static public function ClipSegmentToLine(vOut:Vector.<ClipVertex>, vIn:Vector.<ClipVertex>, normal:b2Vec2, offset:Number):int
        {
            csl_cv = null;

            // Start with no output points
            csl_numOut = 0;

            csl_cv = vIn[0];

            csl_vIn0 = csl_cv.v;

            csl_cv = vIn[1];

            csl_vIn1 = csl_cv.v;

            // Calculate the distance of end points to the line
            csl_distance0 = normal.x * csl_vIn0.x + normal.y * csl_vIn0.y - offset;
            csl_distance1 = normal.x * csl_vIn1.x + normal.y * csl_vIn1.y - offset;

            // If the points are behind the plane
            if(csl_distance0 <= b2Settings.DECIMAL_ZERO)
            {
                vInCurrent = vIn[0];

                vOutCurrent = vOut[csl_numOut++];
                vOutCurrent.v.x = vInCurrent.v.x;
                vOutCurrent.v.y = vInCurrent.v.y;

                vOutCurrent.id.Set(vInCurrent.id);
            }

            if(csl_distance1 <= b2Settings.DECIMAL_ZERO)
            {
                vInCurrent = vIn[1];
                vOutCurrent = vOut[csl_numOut++];

                vOutCurrent.v.x = vInCurrent.v.x;
                vOutCurrent.v.y = vInCurrent.v.y;

                vOutCurrent.id.Set(vInCurrent.id);
            }

            // If the points are on different sides of the plane
            if (csl_distance0 * csl_distance1 < b2Settings.DECIMAL_ZERO)
            {
                // Find intersection point of edge and plane
                csl_interp = csl_distance0 / (csl_distance0 - csl_distance1);

                // expanded for performance
                // vOut[numOut].v = vIn[0].v + interp * (vIn[1].v - vIn[0].v);
                csl_cv = vOut[csl_numOut];

                csl_tVec = csl_cv.v;

                csl_tVec.x = csl_vIn0.x + csl_interp * (csl_vIn1.x - csl_vIn0.x);
                csl_tVec.y = csl_vIn0.y + csl_interp * (csl_vIn1.y - csl_vIn0.y);

                csl_cv = vOut[csl_numOut];

                csl_cv2 = null;

                if(csl_distance0 > b2Settings.DECIMAL_ZERO)
                {
                    csl_cv2 = vIn[0];
                    csl_cv.id = csl_cv2.id;
                }
                else
                {
                    csl_cv2 = vIn[1];
                    csl_cv.id = csl_cv2.id;
                }

                ++csl_numOut;
            }

            return csl_numOut;
        }



        private static var es_vertices1:Array;
        private static var es_normals1:Array;

        private static var es_count2:int;
        private static var es_vertices2:Array;

        private static var es_tMat:b2Mat22;
        private static var es_tVec:b2Vec2;

        private static var es_normal1WorldX:Number;
        private static var es_normal1WorldY:Number;

        private static var es_normal1X:Number;
        private static var es_normal1Y:Number;

        private static var es_index:int;
        private static var es_minDot:Number;
        private static var es_i:int
        private static var es_dot:Number;

        private static var es_v1X:Number;
        private static var es_v1Y:Number;

        private static var es_v2X:Number;
        private static var es_v2Y:Number;

        // Find the separation between poly1 and poly2 for a give edge normal on poly1.

        [Inline]
        static public function EdgeSeparation(  poly1:b2PolygonShape, xf1:b2Transform, edge1:int,
                                                poly2:b2PolygonShape, xf2:b2Transform):Number
        {
            es_vertices1 = poly1.m_vertices;
            es_normals1 = poly1.m_normals;

            es_count2 = poly2.m_vertexCount;
            es_vertices2 = poly2.m_vertices;

            es_tMat = null;
            es_tVec = null;

            // Convert normal from poly1's frame into poly2's frame.
            //b2Vec2 normal1World = b2Mul(xf1.R, normals1[edge1]);
            es_tMat = xf1.R;
            es_tVec = es_normals1[edge1];

            es_normal1WorldX = (es_tMat.col1.x * es_tVec.x + es_tMat.col2.x * es_tVec.y);
            es_normal1WorldY = (es_tMat.col1.y * es_tVec.x + es_tMat.col2.y * es_tVec.y);

            es_tMat = xf2.R;

            es_normal1X = (es_tMat.col1.x * es_normal1WorldX + es_tMat.col1.y * es_normal1WorldY);
            es_normal1Y = (es_tMat.col2.x * es_normal1WorldX + es_tMat.col2.y * es_normal1WorldY);

            // Find support vertex on poly2 for -normal.
            es_index = 0;
            es_minDot = Number.MAX_VALUE;

            for(es_i = 0; es_i < es_count2; ++es_i)
            {
                es_tVec = es_vertices2[es_i];

                es_dot = es_tVec.x * es_normal1X + es_tVec.y * es_normal1Y;

                if(es_dot < es_minDot)
                {
                    es_minDot = es_dot;
                    es_index = es_i;
                }
            }

            //b2Vec2 v1 = b2Mul(xf1, vertices1[edge1]);
            es_tVec = es_vertices1[edge1];
            es_tMat = xf1.R;

            es_dot = xf1.position.x + (es_tMat.col1.x * es_tVec.x + es_tMat.col2.x * es_tVec.y);
            es_v1Y = xf1.position.y + (es_tMat.col1.y * es_tVec.x + es_tMat.col2.y * es_tVec.y);

            es_tVec = es_vertices2[es_index];
            es_tMat = xf2.R;

            es_v2X = xf2.position.x + (es_tMat.col1.x * es_tVec.x + es_tMat.col2.x * es_tVec.y);
            es_v2Y = xf2.position.y + (es_tMat.col1.y * es_tVec.x + es_tMat.col2.y * es_tVec.y);

            es_v2X -= es_dot;
            es_v2Y -= es_v1Y;

            es_vertices1 = null;
            es_normals1 = null;

            return es_v2X * es_normal1WorldX + es_v2Y * es_normal1WorldY;
        }




        // Find the max separation between poly1 and poly2 using edge normals
        // from poly1.


        private static var fms_count1:int;
        private static var fms_normals1:Array;

        private static var fms_tVec:b2Vec2;
        private static var fms_tMat:b2Mat22;

        private static var fms_dX:Number;
        private static var fms_dY:Number;

        private static var fms_dLocal1X:Number;
        private static var fms_dLocal1Y:Number;

        private static var fms_edge:int;
        private static var fms_maxDot:Number;
        private static var fms_i:int
        private static var fms_dot:Number;

        private static var fms_s:Number;

        private static var fms_prevEdge:int;
        private static var fms_sPrev:Number;

        private static var fms_nextEdge:int;
        private static var fms_sNext:Number;


        private static var fms_bestEdge:int;
        private static var fms_bestSeparation:Number;
        private static var fms_increment:int;

        [Inline]
        static public function FindMaxSeparation(edgeIndex:Vector.<int>,
                                                poly1:b2PolygonShape, xf1:b2Transform,
                                                poly2:b2PolygonShape, xf2:b2Transform):Number
        {
            fms_count1 = poly1.m_vertexCount;
            fms_normals1 = poly1.m_normals;

            fms_tVec = null;
            fms_tMat = null;

            // Vector pointing from the centroid of poly1 to the centroid of poly2.
            //b2Vec2 d = b2Mul(xf2, poly2->m_centroid) - b2Mul(xf1, poly1->m_centroid);
            fms_tMat = xf2.R;
            fms_tVec = poly2.m_centroid;

            fms_dX = xf2.position.x + (fms_tMat.col1.x * fms_tVec.x + fms_tMat.col2.x * fms_tVec.y);
            fms_dY = xf2.position.y + (fms_tMat.col1.y * fms_tVec.x + fms_tMat.col2.y * fms_tVec.y);

            fms_tMat = xf1.R;
            fms_tVec = poly1.m_centroid;

            fms_dX -= xf1.position.x + (fms_tMat.col1.x * fms_tVec.x + fms_tMat.col2.x * fms_tVec.y);
            fms_dY -= xf1.position.y + (fms_tMat.col1.y * fms_tVec.x + fms_tMat.col2.y * fms_tVec.y);

            //b2Vec2 dLocal1 = b2MulT(xf1.R, d);
            fms_dLocal1X = (fms_dX * xf1.R.col1.x + fms_dY * xf1.R.col1.y);
            fms_dLocal1Y = (fms_dX * xf1.R.col2.x + fms_dY * xf1.R.col2.y);

            // Get support vertex as a hint for our search
            fms_edge = 0;
            fms_maxDot = -Number.MAX_VALUE;

            for (fms_i = 0; fms_i < fms_count1; ++fms_i)
            {
                //var dot:Number = b2Math.b2Dot(normals1[i], dLocal1);
                fms_tVec = fms_normals1[fms_i];

                fms_dot = (fms_tVec.x * fms_dLocal1X + fms_tVec.y * fms_dLocal1Y);

                if(fms_dot > fms_maxDot)
                {
                    fms_maxDot = fms_dot;
                    fms_edge = fms_i;
                }
            }

            // Get the separation for the edge normal.
            fms_s = EdgeSeparation(poly1, xf1, fms_edge, poly2, xf2);

            // Check the separation for the previous edge normal.
            fms_prevEdge = fms_edge - 1 >= 0 ? fms_edge - 1 : fms_count1 - 1;
            fms_sPrev = EdgeSeparation(poly1, xf1, fms_prevEdge, poly2, xf2);

            // Check the separation for the next edge normal.
            fms_nextEdge = fms_edge + 1 < fms_count1 ? fms_edge + 1 : 0;
            fms_sNext = EdgeSeparation(poly1, xf1, fms_nextEdge, poly2, xf2);

            fms_normals1 = null;

            // Find the best edge and the search direction.
            fms_bestEdge = 0;
            fms_bestSeparation = 0;
            fms_increment = 0;

            if (fms_sPrev > fms_s && fms_sPrev > fms_sNext)
            {
                fms_increment = -1;
                fms_bestEdge = fms_prevEdge;
                fms_bestSeparation = fms_sPrev;
            }
            else if (fms_sNext > fms_s)
            {
                fms_increment = 1;
                fms_bestEdge = fms_nextEdge;
                fms_bestSeparation = fms_sNext;
            }
            else
            {
                // pointer out
                edgeIndex[0] = fms_edge;
                return fms_s;
            }

            // Perform a local search for the best edge normal.
            while (true)
            {

                if (fms_increment == -1)
                    fms_edge = fms_bestEdge - 1 >= 0 ? fms_bestEdge - 1 : fms_count1 - 1;
                else
                    fms_edge = fms_bestEdge + 1 < fms_count1 ? fms_bestEdge + 1 : 0;

                fms_s = EdgeSeparation(poly1, xf1, fms_edge, poly2, xf2);

                if (fms_s > fms_bestSeparation)
                {
                    fms_bestEdge = fms_edge;
                    fms_bestSeparation = fms_s;
                }
                else
                {
                    break;
                }
            }

            // pointer out
            edgeIndex[0] = fms_bestEdge;
            return fms_bestSeparation;
        }

        private static var fie_normals1:Array;

        private static var fie_count2:int;
        private static var fie_vertices2:Array;
        private static var fie_normals2:Array;

        private static var fie_tMat:b2Mat22;
        private static var fie_tVec:b2Vec2;

        private static var fie_normal1X:Number;
        private static var fie_normal1Y:Number;

        private static var fie_tX:Number;

        private static var fie_index:int;
        private static var fie_minDot:Number;
        private static var fie_i:int;
        private static var fie_dot:Number;

        private static var fie_tClip:ClipVertex;

        private static var fie_i1:int;
        private static var fie_i2:int;


        [Inline]
        static public function FindIncidentEdge(c:Vector.<ClipVertex>,
                                                poly1:b2PolygonShape, xf1:b2Transform, edge1:int,
                                                poly2:b2PolygonShape, xf2:b2Transform) : void
        {
            fie_normals1 = poly1.m_normals;

            fie_count2 = poly2.m_vertexCount;

            fie_vertices2 = poly2.m_vertices;
            fie_normals2 = poly2.m_normals;

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

            fie_tMat = null;
            fie_tVec = null;

            // Get the normal of the reference edge in poly2's frame.
            //b2Vec2 normal1 = b2MulT(xf2.R, b2Mul(xf1.R, normals1[edge1]));
            fie_tMat = xf1.R;
            fie_tVec = fie_normals1[edge1];

            fie_normal1X = (fie_tMat.col1.x * fie_tVec.x + fie_tMat.col2.x * fie_tVec.y);
            fie_normal1Y = (fie_tMat.col1.y * fie_tVec.x + fie_tMat.col2.y * fie_tVec.y);

            fie_tMat = xf2.R;

            fie_tX = (fie_tMat.col1.x * fie_normal1X + fie_tMat.col1.y * fie_normal1Y);

            fie_normal1Y =  (fie_tMat.col2.x * fie_normal1X + fie_tMat.col2.y * fie_normal1Y);
            fie_normal1X = fie_tX;

            // Find the incident edge on poly2.
            fie_index = 0;
            fie_minDot = Number.MAX_VALUE;

            for (fie_i = 0; fie_i < fie_count2; ++fie_i)
            {
                //var dot:Number = b2Dot(normal1, normals2[i]);
                fie_tVec = fie_normals2[fie_i];
                fie_dot = (fie_normal1X * fie_tVec.x + fie_normal1Y * fie_tVec.y);

                if (fie_dot < fie_minDot)
                {
                    fie_minDot = fie_dot;
                    fie_index = fie_i;
                }
            }

            fie_tClip = null;
            // Build the clip vertices for the incident edge.
            fie_i1 = fie_index;
            fie_i2 = fie_i1 + 1 < fie_count2 ? fie_i1 + 1 : 0;

            fie_tClip = c[0];
            //c[0].v = b2Mul(xf2, vertices2[i1]);
            fie_tVec = fie_vertices2[fie_i1];
            fie_tMat = xf2.R;
            fie_tClip.v.x = xf2.position.x + (fie_tMat.col1.x * fie_tVec.x + fie_tMat.col2.x * fie_tVec.y);
            fie_tClip.v.y = xf2.position.y + (fie_tMat.col1.y * fie_tVec.x + fie_tMat.col2.y * fie_tVec.y);

            fie_tClip.id.features.referenceEdge = edge1;
            fie_tClip.id.features.incidentEdge = fie_i1;
            fie_tClip.id.features.incidentVertex = 0;

            fie_tClip = c[1];
            //c[1].v = b2Mul(xf2, vertices2[i2]);
            fie_tVec = fie_vertices2[fie_i2];
            fie_tMat = xf2.R;
            fie_tClip.v.x = xf2.position.x + (fie_tMat.col1.x * fie_tVec.x + fie_tMat.col2.x * fie_tVec.y);
            fie_tClip.v.y = xf2.position.y + (fie_tMat.col1.y * fie_tVec.x + fie_tMat.col2.y * fie_tVec.y);

            fie_tClip.id.features.referenceEdge = edge1;
            fie_tClip.id.features.incidentEdge = fie_i2;
            fie_tClip.id.features.incidentVertex = 1;


            fie_normals1 = null;
            fie_vertices2 = null;
            fie_normals2 = null;
        }


        private static function MakeClipPointVector():Vector.<ClipVertex>
        {
            var r:Vector.<ClipVertex> = new Vector.<ClipVertex>(2);
            r[0] = new ClipVertex();
            r[1] = new ClipVertex();
            return r;
        }

        private static const k_relativeTol:Number = 0.98;
        private static const k_absoluteTol:Number = 0.001;


        private static var s_incidentEdge:Vector.<ClipVertex> = MakeClipPointVector();
        private static var s_clipPoints1:Vector.<ClipVertex> = MakeClipPointVector();
        private static var s_clipPoints2:Vector.<ClipVertex> = MakeClipPointVector();
        private static var s_edgeAO:Vector.<int> = new Vector.<int>(1);
        private static var s_edgeBO:Vector.<int> = new Vector.<int>(1);
        private static var s_localTangent:b2Vec2 = new b2Vec2();
        private static var s_localNormal:b2Vec2 = new b2Vec2();
        private static var s_planePoint:b2Vec2 = new b2Vec2();
        private static var s_normal:b2Vec2 = new b2Vec2();
        private static var s_tangent:b2Vec2 = new b2Vec2();
        private static var s_tangent2:b2Vec2 = new b2Vec2();
        private static var s_v11:b2Vec2 = new b2Vec2();
        private static var s_v12:b2Vec2 = new b2Vec2();
        // Find edge normal of max separation on A - return if separating axis is found
        // Find edge normal of max separation on B - return if separation axis is found
        // Choose reference edge as min(minA, minB)
        // Find incident edge
        // Clip
        static private var b2CollidePolyTempVec:b2Vec2 = new b2Vec2();
        // The normal points from 1 to 2


        private static var cp_cv:ClipVertex;
        private static var cp_totalRadius:Number;

        private static var cp_edgeA:int;
        private static var cp_separationA:Number;
        private static var cp_edgeB:int;
        private static var cp_separationB:Number;

        private static var cp_poly1:b2PolygonShape;
        private static var cp_poly2:b2PolygonShape;
        private static var cp_xf1:b2Transform;
        private static var cp_xf2:b2Transform;
        private static var cp_edge1:int;
        private static var cp_flip:uint;
        private static var cp_tMat:b2Mat22;
        private static var cp_incidentEdge:Vector.<ClipVertex>;

        private static var cp_count1:int;
        private static var cp_vertices1:Array;

        private static var cp_local_v11:b2Vec2;
        private static var cp_local_v12:b2Vec2;
        private static var cp_localTangent:b2Vec2;
        private static var cp_localNormal:b2Vec2;

        private static var cp_planePoint:b2Vec2;
        private static var cp_tangent:b2Vec2;
        private static var cp_tangent2:b2Vec2;
        private static var cp_normal:b2Vec2;

        private static var cp_v11:b2Vec2;
        private static var cp_v12:b2Vec2;

        private static var cp_frontOffset:Number;
        private static var cp_sideOffset1:Number;
        private static var cp_sideOffset2:Number;

        // Clip incident edge against extruded edge1 side edges.
        private static var cp_clipPoints1:Vector.<ClipVertex>;
        private static var cp_clipPoints2:Vector.<ClipVertex>;
        private static var cp_np:int;

        private static var cp_pointCount:int;
        private static var cp_i:int;
        private static var cp_separation:Number;
        private static var cp_cp:b2ManifoldPoint;
        private static var cp_tX:Number;
        private static var cp_tY:Number;

        [Inline]
        static public function CollidePolygons(manifold:b2Manifold,
                                                polyA:b2PolygonShape, xfA:b2Transform,
                                                polyB:b2PolygonShape, xfB:b2Transform):void
        {
            cp_cv = null;

            manifold.m_pointCount = 0;

            cp_totalRadius = polyA.m_radius + polyB.m_radius;

            cp_edgeA = 0;

            s_edgeAO[0] = cp_edgeA;

            cp_separationA = FindMaxSeparation(s_edgeAO, polyA, xfA, polyB, xfB);

            cp_edgeA = s_edgeAO[0];

            if(cp_separationA > cp_totalRadius)
            {
                return;
            }

            cp_edgeB = 0;

            s_edgeBO[0] = cp_edgeB;

            cp_separationB = FindMaxSeparation(s_edgeBO, polyB, xfB, polyA, xfA);

            cp_edgeB = s_edgeBO[0];

            if(cp_separationB > cp_totalRadius)
            {
                return;
            }

            cp_poly1 = null;
            cp_poly2 = null;

            cp_xf1 = null;
            cp_xf2 = null;

            cp_edge1 = 0;       // reference edge
            cp_flip = 0;

            cp_tMat = null;

            if (cp_separationB > k_relativeTol * cp_separationA + k_absoluteTol)
            {
                cp_poly1 = polyB;
                cp_poly2 = polyA;
                cp_xf1 = xfB;
                cp_xf2 = xfA;
                cp_edge1 = cp_edgeB;
                manifold.m_type = b2Manifold.e_faceB;
                cp_flip = 1;
            }
            else
            {
                cp_poly1 = polyA;
                cp_poly2 = polyB;
                cp_xf1 = xfA;
                cp_xf2 = xfB;
                cp_edge1 = cp_edgeA;
                manifold.m_type = b2Manifold.e_faceA;
                cp_flip = 0;
            }

            cp_incidentEdge = s_incidentEdge;
            FindIncidentEdge(cp_incidentEdge, cp_poly1, cp_xf1, cp_edge1, cp_poly2, cp_xf2);

            cp_count1 = cp_poly1.m_vertexCount;
            cp_vertices1 = cp_poly1.m_vertices;

            cp_local_v11 = cp_vertices1[cp_edge1];
            cp_local_v12 = null;

            if(cp_edge1 + 1 < cp_count1)
            {
                cp_local_v12 = cp_vertices1[int(cp_edge1+1)];
            }
            else
            {
                cp_local_v12 = cp_vertices1[0];
            }

            cp_localTangent = s_localTangent;
            cp_localTangent.Set(cp_local_v12.x - cp_local_v11.x, cp_local_v12.y - cp_local_v11.y);
            cp_localTangent.Normalize();

            cp_localNormal = s_localNormal;
            cp_localNormal.x = cp_localTangent.y;
            cp_localNormal.y = -cp_localTangent.x;

            cp_planePoint = s_planePoint;
            cp_planePoint.Set(0.5 * (cp_local_v11.x + cp_local_v12.x), 0.5 * (cp_local_v11.y + cp_local_v12.y));

            cp_tangent = s_tangent;
            //tangent = b2Math.b2MulMV(xf1.R, localTangent);
            cp_tMat = cp_xf1.R;
            cp_tangent.x = (cp_tMat.col1.x * cp_localTangent.x + cp_tMat.col2.x * cp_localTangent.y);
            cp_tangent.y = (cp_tMat.col1.y * cp_localTangent.x + cp_tMat.col2.y * cp_localTangent.y);

            cp_tangent2 = s_tangent2;
            cp_tangent2.x = - cp_tangent.x;
            cp_tangent2.y = - cp_tangent.y;

            cp_normal = s_normal;
            cp_normal.x = cp_tangent.y;
            cp_normal.y = -cp_tangent.x;

            //v11 = b2Math.MulX(xf1, local_v11);
            //v12 = b2Math.MulX(xf1, local_v12);
            cp_v11 = s_v11;
            cp_v12 = s_v12;

            cp_v11.x = cp_xf1.position.x + (cp_tMat.col1.x * cp_local_v11.x + cp_tMat.col2.x * cp_local_v11.y);
            cp_v11.y = cp_xf1.position.y + (cp_tMat.col1.y * cp_local_v11.x + cp_tMat.col2.y * cp_local_v11.y);

            cp_v12.x = cp_xf1.position.x + (cp_tMat.col1.x * cp_local_v12.x + cp_tMat.col2.x * cp_local_v12.y);
            cp_v12.y = cp_xf1.position.y + (cp_tMat.col1.y * cp_local_v12.x + cp_tMat.col2.y * cp_local_v12.y);

            // Face offset
            cp_frontOffset = cp_normal.x * cp_v11.x + cp_normal.y * cp_v11.y;

            // Side offsets, extended by polytope skin thickness
            cp_sideOffset1 = -cp_tangent.x * cp_v11.x - cp_tangent.y * cp_v11.y + cp_totalRadius;
            cp_sideOffset2 = cp_tangent.x * cp_v12.x + cp_tangent.y * cp_v12.y + cp_totalRadius;

            // Clip incident edge against extruded edge1 side edges.
            cp_clipPoints1 = s_clipPoints1;
            cp_clipPoints2 = s_clipPoints2;
            cp_np = 0;

            // Clip to box side 1
            //np = ClipSegmentToLine(clipPoints1, incidentEdge, -tangent, sideOffset1);
            cp_np = ClipSegmentToLine(cp_clipPoints1, cp_incidentEdge, cp_tangent2, cp_sideOffset1);

            if(cp_np < 2)
            {
                return;
            }

            // Clip to negative box side 1
            cp_np = ClipSegmentToLine(cp_clipPoints2, cp_clipPoints1,  cp_tangent, cp_sideOffset2);

            if(cp_np < 2)
            {
                return;
            }

            // Now clipPoints2 contains the clipped points.
            manifold.m_localPlaneNormal.x = cp_localNormal.x;
            manifold.m_localPlaneNormal.y = cp_localNormal.y;

            manifold.m_localPoint.x = cp_planePoint.x;
            manifold.m_localPoint.y = cp_planePoint.y;

            cp_pointCount = 0;

            for (cp_i = 0; cp_i < b2Settings.b2_maxManifoldPoints; ++cp_i)
            {
                cp_cv = cp_clipPoints2[cp_i];

                cp_separation = cp_normal.x * cp_cv.v.x + cp_normal.y * cp_cv.v.y - cp_frontOffset;

                if(cp_separation <= cp_totalRadius)
                {
                    cp_cp = manifold.m_points[cp_pointCount];

                    //cp.m_localPoint = b2Math.b2MulXT(xf2, cv.v);
                    cp_tMat = cp_xf2.R;

                    cp_tX = cp_cv.v.x - cp_xf2.position.x;
                    cp_tY = cp_cv.v.y - cp_xf2.position.y;

                    cp_cp.m_localPoint.x = (cp_tX * cp_tMat.col1.x + cp_tY * cp_tMat.col1.y);
                    cp_cp.m_localPoint.y = (cp_tX * cp_tMat.col2.x + cp_tY * cp_tMat.col2.y);
                    cp_cp.m_id.Set(cp_cv.id);
                    cp_cp.m_id.features.flip = cp_flip;

                    ++cp_pointCount;
                }
            }

            manifold.m_pointCount = cp_pointCount;
        }


        private static var cc_tMat:b2Mat22;
        private static var cc_tVec:b2Vec2;

        private static var cc_p1X:Number;
        private static var cc_p1Y:Number;

        private static var cc_p2X:Number;
        private static var cc_p2Y:Number;

        private static var cc_dX:Number;
        private static var cc_dY:Number;

        private static var cc_distSqr:Number;
        private static var cc_radius:Number;

        private static var manifoldPoint:b2ManifoldPoint;

        [Inline]
        static public function CollideCircles(manifold:b2Manifold,
                                              circle1:b2CircleShape,
                                              xf1:b2Transform,
                                              circle2:b2CircleShape,
                                              xf2:b2Transform):void
        {
            manifold.m_pointCount = 0;

            //b2Vec2 p1 = b2Mul(xf1, circle1->m_p);
            cc_tMat = xf1.R;
            cc_tVec = circle1.m_p;

            cc_p1X = xf1.position.x + (cc_tMat.col1.x * cc_tVec.x + cc_tMat.col2.x * cc_tVec.y);
            cc_p1Y = xf1.position.y + (cc_tMat.col1.y * cc_tVec.x + cc_tMat.col2.y * cc_tVec.y);

            cc_tMat = xf2.R;
            cc_tVec = circle2.m_p;

            cc_p2X = xf2.position.x + (cc_tMat.col1.x * cc_tVec.x + cc_tMat.col2.x * cc_tVec.y);
            cc_p2Y = xf2.position.y + (cc_tMat.col1.y * cc_tVec.x + cc_tMat.col2.y * cc_tVec.y);

            cc_dX = cc_p2X - cc_p1X;
            cc_dY = cc_p2Y - cc_p1Y;

            cc_distSqr = cc_dX * cc_dX + cc_dY * cc_dY;
            cc_radius = circle1.m_radius + circle2.m_radius;

            if(cc_distSqr > cc_radius * cc_radius)
            {
                return;
            }

            manifold.m_type = b2Manifold.e_circles;

            manifold.m_localPoint.x = circle1.m_p.x;
            manifold.m_localPoint.y = circle1.m_p.y;

            manifold.m_localPlaneNormal.x = b2Settings.DECIMAL_ZERO;
            manifold.m_localPlaneNormal.y = b2Settings.DECIMAL_ZERO;

            manifold.m_pointCount = 1;

            manifoldPoint = manifold.m_points[0];

            manifoldPoint.m_localPoint.x = circle2.m_p.x;
            manifoldPoint.m_localPoint.y = circle2.m_p.y;

            manifoldPoint.m_id.key = 0;

            manifoldPoint = null;
            cc_tMat = null;
            cc_tVec = null;
        }


        private static var cpc_dX:Number;
        private static var cpc_dY:Number;

        private static var cpc_tVec:b2Vec2;
        private static var cpc_tMat:b2Mat22;

        private static var cpc_cX:Number;
        private static var cpc_cY:Number;

        private static var cpc_cLocalX:Number;
        private static var cpc_cLocalY:Number;

        private static var cpc_normalIndex:int;
        private static var cpc_separation:Number;
        private static var cpc_radius:Number;
        private static var cpc_vertexCount:int;
        private static var cpc_vertices:Array;
        private static var cpc_normals:Array;

        private static var cpc_i:int;
        private static var cpc_s:Number;

        private static var cpc_vertIndex1:int;
        private static var cpc_vertIndex2:int;
        private static var cpc_v1:b2Vec2;
        private static var cpc_v2:b2Vec2;
        private static var cpc_normalv1:b2Vec2;
        private static var cpc_normalv2:b2Vec2;

        private static var cpc_u1:Number;
        private static var cpc_u2:Number;

        private static var cpc_faceCenterX:Number;
        private static var cpc_faceCenterY:Number;


        [Inline]
        static public function CollidePolygonAndCircle(manifold:b2Manifold,
                                                       polygon:b2PolygonShape,
                                                       xf1:b2Transform,
                                                       circle:b2CircleShape,
                                                       xf2:b2Transform):void
        {
            manifold.m_pointCount = 0;

            cpc_dX = 0;
            cpc_dY = 0;

            cpc_tVec = null;
            cpc_tMat = null;

            cpc_tMat = xf2.R;
            cpc_tVec = circle.m_p;

            cpc_cX = xf2.position.x + (cpc_tMat.col1.x * cpc_tVec.x + cpc_tMat.col2.x * cpc_tVec.y);
            cpc_cY = xf2.position.y + (cpc_tMat.col1.y * cpc_tVec.x + cpc_tMat.col2.y * cpc_tVec.y);

            //b2Vec2 cLocal = b2MulT(xf1, c);
            cpc_dX = cpc_cX - xf1.position.x;
            cpc_dY = cpc_cY - xf1.position.y;
            cpc_tMat = xf1.R;

            cpc_cLocalX = (cpc_dX * cpc_tMat.col1.x + cpc_dY * cpc_tMat.col1.y);
            cpc_cLocalY = (cpc_dX * cpc_tMat.col2.x + cpc_dY * cpc_tMat.col2.y);

            // Find the min separating edge.
            cpc_normalIndex = 0;
            cpc_separation = -Number.MAX_VALUE;
            cpc_radius = polygon.m_radius + circle.m_radius;

            cpc_vertexCount = polygon.m_vertexCount;
            cpc_vertices = polygon.m_vertices;
            cpc_normals = polygon.m_normals;

            for (cpc_i = 0; cpc_i < cpc_vertexCount; ++cpc_i)
            {
                cpc_tVec = cpc_vertices[cpc_i];
                cpc_dX = cpc_cLocalX - cpc_tVec.x;
                cpc_dY = cpc_cLocalY - cpc_tVec.y;
                cpc_tVec = cpc_normals[cpc_i];

                cpc_s = cpc_tVec.x * cpc_dX + cpc_tVec.y * cpc_dY;

                if(cpc_s > cpc_radius)
                {
                    return;
                }

                if(cpc_s > cpc_separation)
                {
                    cpc_separation = cpc_s;
                    cpc_normalIndex = cpc_i;
                }
            }

            // Vertices that subtend the incident face
            cpc_vertIndex1 = cpc_normalIndex;
            cpc_vertIndex2 = cpc_vertIndex1 + 1 < cpc_vertexCount ? cpc_vertIndex1 + 1 : 0;

            cpc_v1 = cpc_vertices[cpc_vertIndex1];
            cpc_v2 = cpc_vertices[cpc_vertIndex2];

            // If the center is inside the polygon ...
            if (cpc_separation < Number.MIN_VALUE)
            {
                manifold.m_pointCount = 1;
                manifold.m_type = b2Manifold.e_faceA;
                manifold.m_localPlaneNormal.SetV(cpc_normals[cpc_normalIndex]);
                manifold.m_localPoint.x = 0.5 * (cpc_v1.x + cpc_v2.x);
                manifold.m_localPoint.y = 0.5 * (cpc_v1.y + cpc_v2.y);

                manifoldPoint = manifold.m_points[0];

                manifoldPoint.m_localPoint.x = circle.m_p.x;
                manifoldPoint.m_localPoint.y = circle.m_p.y;

                manifoldPoint.m_id.key = 0;

                manifoldPoint = null;
                return;
            }

            // Project the circle center onto the edge segment.
            cpc_u1 = (cpc_cLocalX - cpc_v1.x) * (cpc_v2.x - cpc_v1.x) + (cpc_cLocalY - cpc_v1.y) * (cpc_v2.y - cpc_v1.y);
            cpc_u2 = (cpc_cLocalX - cpc_v2.x) * (cpc_v1.x - cpc_v2.x) + (cpc_cLocalY - cpc_v2.y) * (cpc_v1.y - cpc_v2.y);

            if(cpc_u1 <= b2Settings.DECIMAL_ZERO)
            {
                if ((cpc_cLocalX - cpc_v1.x)*(cpc_cLocalX - cpc_v1.x)+(cpc_cLocalY - cpc_v1.y)*(cpc_cLocalY - cpc_v1.y) > cpc_radius * cpc_radius)
                {
                    return;
                }

                manifold.m_pointCount = 1;
                manifold.m_type = b2Manifold.e_faceA;
                manifold.m_localPlaneNormal.x = cpc_cLocalX - cpc_v1.x;
                manifold.m_localPlaneNormal.y = cpc_cLocalY - cpc_v1.y;
                manifold.m_localPlaneNormal.Normalize();

                manifold.m_localPoint.x = cpc_v1.x;
                manifold.m_localPoint.y = cpc_v1.y;

                manifoldPoint = manifold.m_points[0];

                manifoldPoint.m_localPoint.x = circle.m_p.x;
                manifoldPoint.m_localPoint.y = circle.m_p.y;

                manifoldPoint.m_id.key = 0;
            }
            else if (cpc_u2 <= 0)
            {
                if ((cpc_cLocalX - cpc_v2.x)*(cpc_cLocalX - cpc_v2.x)+(cpc_cLocalY - cpc_v2.y)*(cpc_cLocalY - cpc_v2.y) > cpc_radius * cpc_radius)
                {
                    return;
                }

                manifold.m_pointCount = 1;
                manifold.m_type = b2Manifold.e_faceA;
                manifold.m_localPlaneNormal.x = cpc_cLocalX - cpc_v2.x;
                manifold.m_localPlaneNormal.y = cpc_cLocalY - cpc_v2.y;

                manifold.m_localPlaneNormal.Normalize();

                manifold.m_localPoint.x = cpc_v2.x;
                manifold.m_localPoint.y = cpc_v2.y;

                manifoldPoint = manifold.m_points[0];

                manifoldPoint.m_localPoint.x = circle.m_p.x;
                manifoldPoint.m_localPoint.y = circle.m_p.y;

                manifoldPoint.m_id.key = 0;
            }
            else
            {
                cpc_faceCenterX = 0.5 * (cpc_v1.x + cpc_v2.x);
                cpc_faceCenterY = 0.5 * (cpc_v1.y + cpc_v2.y);

                cpc_normalv1 = cpc_normals[cpc_vertIndex1];

                cpc_separation = (cpc_cLocalX - cpc_faceCenterX) * cpc_normalv1.x + (cpc_cLocalY - cpc_faceCenterY) * cpc_normalv1.y;

                if (cpc_separation > cpc_radius)
                {
                    return;
                }

                manifold.m_pointCount = 1;
                manifold.m_type = b2Manifold.e_faceA;

                manifold.m_localPlaneNormal.x = cpc_normalv1.x;
                manifold.m_localPlaneNormal.y = cpc_normalv1.y;

                manifold.m_localPlaneNormal.Normalize();

                manifold.m_localPoint.x = cpc_faceCenterX;
                manifold.m_localPoint.y = cpc_faceCenterY;

                manifoldPoint = manifold.m_points[0];

                manifoldPoint.m_localPoint.x = circle.m_p.x;
                manifoldPoint.m_localPoint.y = circle.m_p.y;

                manifoldPoint.m_id.key = 0;
            }

            manifoldPoint = null;
        }


        private static var to_t1:b2Vec2;
        private static var to_t2:b2Vec2;

        private static var to_d1X:Number;
        private static var to_d1Y:Number;


        private static var to_d2X:Number;
        private static var to_d2Y:Number;


        [Inline]
        static public function TestOverlap(a:b2AABB, b:b2AABB):Boolean
        {
            to_t1 = b.lowerBound;
            to_t2 = a.upperBound;

            //d1 = b2Math.SubtractVV(b.lowerBound, a.upperBound);
            to_d1X = to_t1.x - to_t2.x;
            to_d1Y = to_t1.y - to_t2.y;

            //d2 = b2Math.SubtractVV(a.lowerBound, b.upperBound);
            to_t1 = a.lowerBound;
            to_t2 = b.upperBound;

            to_d2X = to_t1.x - to_t2.x;
            to_d2Y = to_t1.y - to_t2.y;

            if (to_d1X > b2Settings.DECIMAL_ZERO || to_d1Y > b2Settings.DECIMAL_ZERO)
            {
                return false;
            }

            if (to_d2X > b2Settings.DECIMAL_ZERO || to_d2Y > b2Settings.DECIMAL_ZERO)
            {
                return false;
            }

            return true;
        }
    }
}