public static bool ClosestPoints(ISupportMappable support1, ISupportMappable support2, ref JMatrix orientation1,
            ref JMatrix orientation2, ref JVector position1, ref JVector position2,
            out JVector p1, out JVector p2, out JVector normal)
        {
            VoronoiSimplexSolver simplexSolver = simplexSolverPool.GetNew();
            simplexSolver.Reset();

            p1 = p2 = JVector.Zero;

            JVector r = position1 - position2;
            JVector w, v;

            JVector supVertexA;
            JVector rn, vn;

            rn = JVector.Negate(r);

            SupportMapTransformed(support1, ref orientation1, ref position1, ref rn, out supVertexA);

            JVector supVertexB;
            SupportMapTransformed(support2, ref orientation2, ref position2, ref r, out supVertexB);

            v = supVertexA - supVertexB;

            normal = JVector.Zero;

            int iter = 0;

            float distSq = v.LengthSquared();
            float epsilon = 0.00001f;

            while ((distSq > epsilon) && (iter++ < MaxIterations))
            {
                IterationsTaken = iter;
                vn = JVector.Negate(v);
                SupportMapTransformed(support1, ref orientation1, ref position1, ref vn, out supVertexA);
                SupportMapTransformed(support2, ref orientation2, ref position2, ref v, out supVertexB);
                w = supVertexA - supVertexB;

                if (!simplexSolver.InSimplex(w)) simplexSolver.AddVertex(w, supVertexA, supVertexB);
                if (simplexSolver.Closest(out v))
                {
                    distSq = v.LengthSquared();
                    normal = v;
                }
                else distSq = 0.0f;
            }

            simplexSolver.ComputePoints(out p1, out p2);

            if (normal.LengthSquared() > JMath.Epsilon * JMath.Epsilon)
                normal.Normalize();

            simplexSolverPool.GiveBack(simplexSolver);

            return true;
        }

 public FixedLinearSpring(RigidBody body, JVector localAnchor, JVector worldAnchor, float springConstant, float dampingConstant)
 {
     Body = body;
     LocalAnchor = localAnchor;
     WorldAnchor = worldAnchor;
     SpringConstant = springConstant;
     DampingConstant = dampingConstant;
     Length = (worldAnchor - Body.LocalToWorld(localAnchor)).Length();
 }

        /// <summary>
        /// Discrete Box vs Box test. Very fast. No contact info. NOTE: ensure UpdateAxes is called for each BoxShape prior.
        /// </summary>
        /// <param name="A">BoxShape A.</param>
        /// <param name="PA">BoxShape A's position.</param>
        /// <param name="B">BoxShape B.</param>
        /// <param name="PB">BoxShape B's position.</param>
        /// <returns></returns>
        public static bool BoxBoxTest(ref BoxShape A, ref JVector PA, ref BoxShape B, ref JVector PB)
        {
            JVector T = PB - PA;

            // cache scaled local axes
            JVector Ax = A.halfSize.X * A.xAxis;
            JVector Ay = A.halfSize.Y * A.yAxis;
            JVector Bx = B.halfSize.X * B.xAxis;
            JVector By = B.halfSize.Y * B.yAxis;

            // axis to test
            JVector L = A.xAxis;
            float TL = Math.Abs(T * L);

            float a = Math.Abs((Ax) * L) + Math.Abs((Ay) * L);
            float b = Math.Abs((Bx) * L) + Math.Abs((By) * L);

            if (TL > (a + b))
                return false;

            // axis to test
            L = A.yAxis;
            TL = Math.Abs(T * L);

            a = Math.Abs((Ax) * L) + Math.Abs((Ay) * L);
            b = Math.Abs((Bx) * L) + Math.Abs((By) * L);

            if (TL > (a + b))
                return false;

            // axis to test
            L = B.xAxis;
            TL = Math.Abs(T * L);

            a = Math.Abs((Ax) * L) + Math.Abs((Ay) * L);
            b = Math.Abs((Bx) * L) + Math.Abs((By) * L);

            if (TL > (a + b))
                return false;

            // axis to test
            L = B.yAxis;
            TL = Math.Abs(T * L);

            a = Math.Abs((Ax) * L) + Math.Abs((Ay) * L);
            b = Math.Abs((Bx) * L) + Math.Abs((By) * L);

            if (TL > (a + b))
                return false;

            // if all axes overlap collision exists
            return true;
        }

        public static void SupportMapTransformed(ISupportMappable support,
            ref JMatrix orientation, ref JVector position, ref JVector direction, out JVector result)
        {
            // THIS IS *THE* HIGH FREQUENCY CODE OF THE COLLLISION PART OF THE ENGINE
            result.X = ((direction.X * orientation.M11) + (direction.Y * orientation.M12));
            result.Y = ((direction.X * orientation.M21) + (direction.Y * orientation.M22));

            support.SupportMapping(ref result, out result);

            float x = ((result.X * orientation.M11) + (result.Y * orientation.M21));
            float y = ((result.X * orientation.M12) + (result.Y * orientation.M22));

            result.X = position.X + x;
            result.Y = position.Y + y;
        }

        /// <summary>
        /// SupportMapping. Finds the point in the shape furthest away from the given direction.
        /// Imagine a plane with a normal in the search direction. Now move the plane along the normal
        /// until the plane does not intersect the shape. The last intersection point is the result.
        /// </summary>
        /// <param name="direction">The direction.</param>
        /// <param name="result">The result.</param>
        public override void SupportMapping(ref JVector direction, out JVector result)
        {
            float maxDotProduct = float.MinValue;
            int maxIndex = 0;
            float dotProduct;

            for (int i = 0; i < vertices.Count; i++)
            {
                dotProduct = JVector.Dot(vertices[i], direction);
                if (dotProduct > maxDotProduct)
                {
                    maxDotProduct = dotProduct;
                    maxIndex = i;
                }
            }

            result = vertices[maxIndex] - this.shifted;
        }

        public static bool CircleCircleTest(JVector centerA, float radiusA, JVector centerB, float radiusB, out JVector pointA, out JVector pointB, out JVector normal, out float distance)
        {
            // ||A-B|| - (r1+r2) < 0
            float d = JVector.Distance(centerA, centerB);
            float r = radiusA + radiusB;

            distance = d - r;

            normal = (centerA - centerB) / d;

            //penetrationVector = normal * distance;

            // calculate closest 2 points
            pointA = JVector.Negate(normal) * radiusA + centerA;
            pointB = normal * radiusB + centerB;

            if (distance < 0.0f)
                return true;
            else
                return false;
        }

        public static bool CircleCapsuleTest(JVector centerA, float radiusA, JVector centerB, JVector axis, float length, float radiusB, out JVector pointA, out JVector pointB, out JVector normal, out float distance)
        {
            // get capsule endpoints
            var p0 = centerB - axis * (length * 0.5f);
            var p1 = centerB + axis * (length * 0.5f);

            // get vector from endpoint to circle
            var D = centerA - p0;

            // project vector onto axis and clamp
            var d = JVector.Dot(D, axis);
            d = JMath.Clamp(d, 0, length);

            // get point on axis
            var R = p0 + axis * d;

            // distance
            var b = Math.Abs((centerA - R).Length());

            normal = (centerA - R) / b;

            // calculate closest 2 points
            var RH = JVector.Normalize(centerA - R);

            pointA = JVector.Negate(RH) * radiusA + centerA;
            pointB = RH * radiusB + R;

            normal.Negate();

            distance = b - (radiusA + radiusB);

            //
            if (b < radiusA + radiusB)
                return true;
            return false;
        }

 void DrawPoly(List<JVector> poly, JVector pos, JMatrix o, Color color)
 {
     for (int i = 0; i < poly.Count - 1; i++)
     {
         JVector a = JVector.Transform(poly[i], o * JMatrix.CreateTranslation(pos));
         JVector b = JVector.Transform(poly[i + 1], o * JMatrix.CreateTranslation(pos));
         DebugDrawer.DrawLine(a, b, color);
     }
     JVector c = JVector.Transform(poly[0], o * JMatrix.CreateTranslation(pos));
     JVector d = JVector.Transform(poly[poly.Count - 1], o * JMatrix.CreateTranslation(pos));
     DebugDrawer.DrawLine(c, d, color);
 }

        protected override void Update(GameTime gameTime)
        {
            // Allows the game to exit
            if (GamePad.GetState(PlayerIndex.One).Buttons.Back == ButtonState.Pressed)
                this.Exit();

            KeyboardState keys = Keyboard.GetState();
            JVector moveVector = JVector.Zero;
            float amountOfMovement = 0.05f;

            if (keys.IsKeyDown(Keys.Right))
                moveVector.X += amountOfMovement;
            if (keys.IsKeyDown(Keys.Left))
                moveVector.X -= amountOfMovement;
            if (keys.IsKeyDown(Keys.Down))
                moveVector.Y -= amountOfMovement;
            if (keys.IsKeyDown(Keys.Up))
                moveVector.Y += amountOfMovement;

            PB += moveVector;
            OA = (float)gameTime.TotalGameTime.TotalSeconds * 0.1f;
            OB = (float)gameTime.TotalGameTime.TotalSeconds * -0.1f;

            DrawBox(A, PA, OA, Color.Blue * 0.25f);
            DrawBox(B, PB, OB, Color.Green * 0.25f);

            float t = 0.0f;
            JVector[] CA = new JVector[2], CB = new JVector[2];
            int NumContacts = 0;

            sw.Start();

            JMatrix OAM = JMatrix.CreateRotationZ(OA);
            JMatrix OBM = JMatrix.CreateRotationZ(OB);

            for (int i = 0; i < 1; i++)
            {
                A.UpdateAxes(OA);
                B.UpdateAxes(OB);

                //hit = Collision.BoxBoxTest(ref A, ref PA, ref B, ref PB);
                //AV = new List<JVector> { A.GetCorner(0), A.GetCorner(1), A.GetCorner(2), A.GetCorner(3) };
                //BV = new List<JVector> { B.GetCorner(0), B.GetCorner(1), B.GetCorner(2), B.GetCorner(3) };

                //hit = SAT.Collide(ref AV, ref PA, ref VA, ref OAM,
                //    ref BV, ref PB, ref VB, ref OBM,
                //    ref normal, ref t);

                //if (hit)
                //{
                //    SAT.FindContacts(ref AV, ref PA, ref VA, ref OAM,
                //        ref BV, ref PB, ref VB, ref OBM,
                //        ref normal, t, out CA, out CB, out NumContacts);

                //    normal.Normalize();
                //}

                hit = Collision.BoxBoxTestContact(ref A, ref PA, ref OAM, ref B, ref PB, ref OBM,
                    out normal, out t, out CA, out CB, out NumContacts);

                penetration = t;
                iterations = NumContacts;
            }

            sw.Stop();
            ticks = sw.ElapsedTicks / 1;
            sw.Reset();

            if (hit)
            {
                //DrawBox(A, PA + normal * (t * 0.5f), OA, Color.Blue);
                //DrawBox(B, PB - normal * (t * 0.5f), OB, Color.Green);

                for (int i = 0; i < NumContacts; i++)
                {
                    DebugDrawer.DrawPoint(CA[i]);// + normal * (t * 0.5f));
                    DebugDrawer.DrawPoint(CB[i]);// - normal * (t * 0.5f));
                }
            }

            base.Update(gameTime);
        }

 public static Vector2 ToXNAVector2(JVector vector)
 {
     return new Vector2(vector.X, vector.Y);
 }

        public override bool Raycast(JVector rayOrigin, JVector rayDirection, RaycastCallback raycast, out RigidBody body, out JVector normal, out float fraction)
        {
            throw new NotImplementedException();
            //body = null; normal = JVector.Zero; fraction = float.MaxValue;

            //JVector tempNormal; float tempFraction;
            //bool result = false;

            //// TODO: This can be done better in CollisionSystemPersistenSAP
            //foreach (IBroadphaseEntity e in bodyList)
            //{
            //    if (e is SoftBody)
            //    {
            //        SoftBody softBody = e as SoftBody;
            //        foreach (RigidBody b in softBody.VertexBodies)
            //        {
            //            if (this.Raycast(b, rayOrigin, rayDirection, out tempNormal, out tempFraction))
            //            {
            //                if (tempFraction < fraction && (raycast == null || raycast(b, tempNormal, tempFraction)))
            //                {
            //                    body = b;
            //                    normal = tempNormal;
            //                    fraction = tempFraction;
            //                    result = true;
            //                }
            //            }
            //        }
            //    }
            //    else
            //    {
            //        RigidBody b = e as RigidBody;

            //        if (this.Raycast(b, rayOrigin, rayDirection, out tempNormal, out tempFraction))
            //        {
            //            if (tempFraction < fraction && (raycast == null || raycast(b, tempNormal, tempFraction)))
            //            {
            //                body = b;
            //                normal = tempNormal;
            //                fraction = tempFraction;
            //                result = true;
            //            }
            //        }
            //    }
            //}

            //return result;
        }

 static JVector()
 {
     One = new JVector(1, 1);
     Zero = new JVector(0, 0);
     Left = new JVector(1, 0);
     Right = new JVector(-1, 0);
     Up = new JVector(0, 1);
     Down = new JVector(0, -1);
     MinValue = new JVector(float.MinValue);
     MaxValue = new JVector(float.MaxValue);
     Arbitrary = new JVector(1, 1);
     InternalZero = Zero;
 }

        /// <summary>
        /// Transforms a vector by the transposed of the given Matrix.
        /// </summary>
        /// <param name="position">The vector to transform.</param>
        /// <param name="matrix">The transform matrix.</param>
        /// <param name="result">The transformed vector.</param>
        public static void TransposedTransform(ref JVector position, ref JMatrix matrix, out JVector result)
        {
            float num0 = ((position.X * matrix.M11) + (position.Y * matrix.M12));
            float num1 = ((position.X * matrix.M21) + (position.Y * matrix.M22));

            result.X = num0;
            result.Y = num1;
        }

 /// <summary>
 /// Multiply a vector with a factor.
 /// </summary>
 /// <param name="value1">The vector to multiply.</param>
 /// <param name="scaleFactor">The scale factor.</param>
 /// <param name="result">Returns the multiplied vector.</param>
 public static void Multiply(ref JVector value1, float scaleFactor, out JVector result)
 {
     result.X = value1.X * scaleFactor;
     result.Y = value1.Y * scaleFactor;
 }

 public static JVector Transform(JVector position, JMatrix matrix)
 {
     JVector result;
     JVector.Transform(ref position, ref matrix, out result);
     return result;
 }

 /// <summary>
 /// Inverses the direction of a vector.
 /// </summary>
 /// <param name="value">The vector to inverse.</param>
 /// <returns>The negated vector.</returns>
 public static JVector Negate(JVector value)
 {
     JVector result;
     JVector.Negate(ref value, out result);
     return result;
 }

 internal static JVector Clamp(JVector v, float len)
 {
     return (JVector.Dot(v, v) > len * len) ? JVector.Multiply(JVector.Normalize(v), len) : v;
 }

 public JVector ScreenToWorldSpace(JVector position)
 {
     var t = this.Game.GraphicsDevice.Viewport.Unproject(new Vector3(position.X, position.Y, 0), Projection, View, Matrix.Identity);
     return new JVector(t.X, t.Y);
 }

 public static JVector Normalize(JVector value)
 {
     JVector result;
     JVector.Normalize(ref value, out result);
     return result;
 }

 public static Vector3 ToXNAVector3(JVector vector)
 {
     return new Vector3(vector.X, vector.Y, 0.0f);
 }