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
{
    /// The body type.
    /// static: zero mass, zero velocity, may be manually moved
    /// kinematic: zero mass, non-zero velocity set by user, moved by solver
    /// dynamic: positive mass, non-zero velocity determined by forces, moved by solver
    public enum BodyType
    {
        Static,
        Kinematic,
        Dynamic,
    }

    /// A body definition holds all the data needed to construct a rigid body.
    /// You can safely re-use body definitions. Shapes are added to a body after construction.
    public class BodyDef
    {
        /// This constructor sets the body definition default values.
        public BodyDef()
        {
            userData = null;
            position = new Vector2(0.0f, 0.0f);
            angle = 0.0f;
            linearVelocity = new Vector2(0.0f, 0.0f);
            angularVelocity = 0.0f;
            linearDamping = 0.0f;
            angularDamping = 0.0f;
            allowSleep = true;
            awake = true;
            fixedRotation = false;
            bullet = false;
            type = BodyType.Static;
		    active = true;
            inertiaScale = 1.0f;
        }

        /// The body type: static, kinematic, or dynamic.
        /// Note: if a dynamic body would have zero mass, the mass is set to one.
        public BodyType type;

        /// The world position of the body. Avoid creating bodies at the origin
        /// since this can lead to many overlapping shapes.
        public Vector2 position;

        /// The world angle of the body in radians.
        public float angle;

        /// The linear velocity of the body's origin in world co-ordinates.
        public Vector2 linearVelocity;

        /// The angular velocity of the body.
        public float angularVelocity;

        /// Linear damping is use to reduce the linear velocity. The damping parameter
        /// can be larger than 1.0f but the damping effect becomes sensitive to the
        /// time step when the damping parameter is large.
        public float linearDamping;

        /// Angular damping is use to reduce the angular velocity. The damping parameter
        /// can be larger than 1.0f but the damping effect becomes sensitive to the
        /// time step when the damping parameter is large.
        public float angularDamping;

        /// Set this flag to false if this body should never fall asleep. Note that
        /// this increases CPU usage.
        public bool allowSleep;

        /// Is this body awake or sleeping?
        public bool awake;

        /// Should this body be prevented from rotating? Useful for characters.
        public bool fixedRotation;

        /// Is this a fast moving body that should be prevented from tunneling through
        /// other moving bodies? Note that all bodies are prevented from tunneling through
        /// kinematic and static bodies. This setting is only considered on dynamic bodies.
        /// @warning You should use this flag sparingly since it increases processing time.
        public bool bullet;

        /// Does this body start out active?
	    public bool active;

        /// Use this to store application specific body data.
        public object userData;

        /// Experimental: scales the inertia tensor.
        public float inertiaScale;
    };

    [Flags]
    public enum BodyFlags
    {
        None            = 0,
        Island          = (1 << 0),
        Awake           = (1 << 1),
        AutoSleep       = (1 << 2),
        Bullet          = (1 << 3),
        FixedRotation   = (1 << 4),
        Active          = (1 << 5),
        Toi             = (1 << 6),
    }

    public class Body
    {

        /// Set the type of this body. This may alter the mass and velocity.
	    public void SetType(BodyType type)
        {
            if (_type == type)
            {
                return;
            }

            _type = type;

            ResetMassData();

            if (_type == BodyType.Static)
            {
                _linearVelocity = Vector2.Zero;
                _angularVelocity = 0.0f;
            }

            SetAwake(true);

            _force = Vector2.Zero;
            _torque = 0.0f;

            // Since the body type changed, we need to flag contacts for filtering.
            for (ContactEdge ce = _contactList; ce != null; ce = ce.Next)
            {
                ce.Contact.FlagForFiltering();
            }
        }

	    /// Get the type of this body.
        public new BodyType GetType()
        {
            return _type;
        }

	    /// Creates a fixture and attach it to this body. Use this function if you need
	    /// to set some fixture parameters, like friction. Otherwise you can create the
	    /// fixture directly from a shape.
	    /// If the density is non-zero, this function automatically updates the mass of the body.
	    /// Contacts are not created until the next time step.
	    /// @param def the fixture definition.
	    /// @warning This function is locked during callbacks.
	    public Fixture CreateFixture(FixtureDef def)
        {
            Debug.Assert(_world.IsLocked == false);
	        if (_world.IsLocked == true)
	        {
		        return null;
	        }

            Fixture fixture = new Fixture();
            fixture.Create(this, def);

            if ((_flags & BodyFlags.Active) == BodyFlags.Active)
            {
                BroadPhase broadPhase = _world._contactManager._broadPhase;
                fixture.CreateProxies(broadPhase, ref _xf);
            }

	        fixture._next = _fixtureList;
	        _fixtureList = fixture;
	        ++_fixtureCount;

	        fixture._body = this;

            // Adjust mass properties if needed.
            if (fixture._density > 0.0f)
            {
                ResetMassData();
            }

            // Let the world know we have a new fixture. This will cause new contacts
            // to be created at the beginning of the next time step.
	        _world._flags |= WorldFlags.NewFixture;

	        return fixture;
        }

	    /// Creates a fixture from a shape and attach it to this body.
	    /// This is a convenience function. Use FixtureDef if you need to set parameters
	    /// like friction, restitution, user data, or filtering.
	    /// If the density is non-zero, this function automatically updates the mass of the body.
	    /// @param shape the shape to be cloned.
	    /// @param density the shape density (set to zero for static bodies).
	    /// @warning This function is locked during callbacks.
	    public Fixture CreateFixture(Shape shape, float density)
        {
            FixtureDef def = new FixtureDef();
            def.shape = shape;
            def.density = density;

            return CreateFixture(def);
        }

	    /// Destroy a fixture. This removes the fixture from the broad-phase and
	    /// destroys all contacts associated with this fixture. This will
        /// automatically adjust the mass of the body if the body is dynamic and the
	    /// fixture has positive density.
	    /// All fixtures attached to a body are implicitly destroyed when the body is destroyed.
	    /// @param fixture the fixture to be removed.
	    /// @warning This function is locked during callbacks.
	    public void DestroyFixture(Fixture fixture)
        {
            Debug.Assert(_world.IsLocked == false);
	        if (_world.IsLocked == true)
	        {
		        return;
	        }

	        Debug.Assert(fixture._body == this);

	        // Remove the fixture from this body's singly linked list.
	        Debug.Assert(_fixtureCount > 0);
	        Fixture node = _fixtureList;
	        bool found = false;
	        while (node != null)
	        {
		        if (node == fixture)
		        {
                    _fixtureList = fixture._next;
			        found = true;
			        break;
		        }

		        node = node._next;
	        }

	        // You tried to remove a shape that is not attached to this body.
	        Debug.Assert(found);

	        // Destroy any contacts associated with the fixture.
	        ContactEdge edge = _contactList;
	        while (edge != null)
	        {
		        Contact c = edge.Contact;
		        edge = edge.Next;

		        Fixture fixtureA = c.GetFixtureA();
		        Fixture fixtureB = c.GetFixtureB();

		        if (fixture == fixtureA || fixture == fixtureB)
		        {
			        // This destroys the contact and removes it from
			        // this body's contact list.
			        _world._contactManager.Destroy(c);
		        }
	        }

            if ((_flags & BodyFlags.Active) == BodyFlags.Active)
            {
                Debug.Assert(fixture._proxyId != BroadPhase.NullProxy);

                BroadPhase broadPhase = _world._contactManager._broadPhase;
                fixture.DestroyProxies(broadPhase);
            }

	        fixture.Destroy();
	        fixture._body = null;
	        fixture._next = null;

            --_fixtureCount;

            ResetMassData();
        }

	    /// Set the position of the body's origin and rotation.
	    /// This breaks any contacts and wakes the other bodies.
        /// Manipulating a body's transform may cause non-physical behavior.
	    /// @param position the world position of the body's local origin.
	    /// @param angle the world rotation in radians.
	    public void SetTransform(Vector2 position, float angle)
        {
            Debug.Assert(_world.IsLocked == false);
	        if (_world.IsLocked == true)
	        {
		        return;
	        }

	        _xf.R.Set(angle);
	        _xf.Position = position;

	        _sweep.c0 = _sweep.c = MathUtils.Multiply(ref _xf, _sweep.localCenter);
	        _sweep.a0 = _sweep.a = angle;

	        BroadPhase broadPhase = _world._contactManager._broadPhase;
	        for (Fixture f = _fixtureList; f != null; f = f._next)
	        {
		        f.Synchronize(broadPhase, ref _xf, ref _xf);
	        }

	        _world._contactManager.FindNewContacts();
        }

        // For teleporting a body without considering new contacts immediately.
        public void SetTransformIgnoreContacts(Vector2 position, float angle)
        {
            Debug.Assert(_world.IsLocked == false);
            if (_world.IsLocked == true)
            {
                return;
            }

            _xf.R.Set(angle);
            _xf.Position = position;

            _sweep.c0 = _sweep.c = MathUtils.Multiply(ref _xf, _sweep.localCenter);
            _sweep.a0 = _sweep.a = angle;

            BroadPhase broadPhase = _world._contactManager._broadPhase;
            for (Fixture f = _fixtureList; f != null; f = f._next)
            {
                f.Synchronize(broadPhase, ref _xf, ref _xf);
            }
        }

	    /// Get the body transform for the body's origin.
	    /// @return the world transform of the body's origin.
	    public void GetTransform(out Transform xf)
        {
            xf = _xf;
        }

	    /// Get the world body origin position.
	    /// @return the world position of the body's origin.
	    public Vector2 GetPosition()
        {
            return _xf.Position;
        }

	    /// Get the angle in radians.
	    /// @return the current world rotation angle in radians.
	    public float GetAngle()
        {
            return _sweep.a;
        }

	    /// Get the world position of the center of mass.
	    public Vector2 GetWorldCenter()
        {
            return _sweep.c;
        }

	    /// Get the local position of the center of mass.
	    public Vector2 GetLocalCenter()
        {
            return _sweep.localCenter;
        }

	    /// Set the linear velocity of the center of mass.
	    /// @param v the new linear velocity of the center of mass.
	    public void SetLinearVelocity(Vector2 v)
        {
            if (_type == BodyType.Static)
            {
                return;
            }

            if (Vector2.Dot(v,v) > 0.0f)
            {
                SetAwake(true);
            }

            _linearVelocity = v;
        }

	    /// Get the linear velocity of the center of mass.
	    /// @return the linear velocity of the center of mass.
	    public Vector2 GetLinearVelocity()
        {
            return _linearVelocity;
        }

	    /// Set the angular velocity.
	    /// @param omega the new angular velocity in radians/second.
	    public void SetAngularVelocity(float w)
        {
            if (_type == BodyType.Static)
            {
                return;
            }

            if (w*w > 0.0f)
            {
                SetAwake(true);
            }

            _angularVelocity = w;
        }

	    /// Get the angular velocity.
	    /// @return the angular velocity in radians/second.
	    public float GetAngularVelocity()
        {
            return _angularVelocity;
        }

	    /// Apply a force at a world point. If the force is not
	    /// applied at the center of mass, it will generate a torque and
	    /// affect the angular velocity. This wakes up the body.
	    /// @param force the world force vector, usually in Newtons (N).
	    /// @param point the world position of the point of application.
	    public void ApplyForce(Vector2 force, Vector2 point)
        {
            if (_type == BodyType.Dynamic)
            {
                if (IsAwake() == false)
                {
                    SetAwake(true);
                }

                _force += force;
                _torque += MathUtils.Cross(point - _sweep.c, force);
            }
        }

	    /// Apply a torque. This affects the angular velocity
	    /// without affecting the linear velocity of the center of mass.
	    /// This wakes up the body.
	    /// @param torque about the z-axis (out of the screen), usually in N-m.
	    public void ApplyTorque(float torque)
        {
            if (_type == BodyType.Dynamic)
            {
                if (IsAwake() == false)
                {
                    SetAwake(true);
                }

                _torque += torque;
            }
        }

	    /// Apply an impulse at a point. This immediately modifies the velocity.
	    /// It also modifies the angular velocity if the point of application
	    /// is not at the center of mass. This wakes up the body.
	    /// @param impulse the world impulse vector, usually in N-seconds or kg-m/s.
	    /// @param point the world position of the point of application.
	    public void ApplyLinearImpulse(Vector2 impulse, Vector2 point)
        {
            if (_type != BodyType.Dynamic)
            {
                return;
            }
            if (IsAwake() == false)
            {
                SetAwake(true);
            }
            _linearVelocity += _invMass * impulse;
            _angularVelocity += _invI * MathUtils.Cross(point - _sweep.c, impulse);
        }

        /// Apply an angular impulse.
        /// @param impulse the angular impulse in units of kg*m*m/s
        public void ApplyAngularImpulse(float impulse)
        {
            if (_type != BodyType.Dynamic)
            {
                return;
            }

            if (IsAwake() == false)
            {
                SetAwake(true);
            }

            _angularVelocity += _invI * impulse;
        }

	    /// Get the total mass of the body.
	    /// @return the mass, usually in kilograms (kg).
	    public float GetMass()
        {
            return _mass;
        }

        /// Get the rotational inertia of the body about the local origin.
	    /// @return the rotational inertia, usually in kg-m^2.
	    public float GetInertia()
        {
            return _I + _mass * Vector2.Dot(_sweep.localCenter, _sweep.localCenter);
        }

        /// Get the mass data of the body.
	    /// @return a struct containing the mass, inertia and center of the body.
	    public void GetMassData(out MassData massData)
        {
            massData = new MassData();
            massData.mass = _mass;
            massData.I = _I + _mass * Vector2.Dot(_sweep.localCenter, _sweep.localCenter);
            massData.center = _sweep.localCenter;
        }

        /// Set the mass properties to override the mass properties of the fixtures.
 	    /// Note that this changes the center of mass position.
        /// Note that creating or destroying fixtures can also alter the mass.
        /// This function has no effect if the body isn't dynamic.
        /// @param massData the mass properties.
        public void SetMassData(ref MassData massData)
        {
	        Debug.Assert(_world.IsLocked == false);
	        if (_world.IsLocked == true)
	        {
		        return;
	        }

            if (_type != BodyType.Dynamic)
            {
                return;
            }

	        _invMass = 0.0f;
	        _I = 0.0f;
	        _invI = 0.0f;

	        _mass = massData.mass;

	        if (_mass <= 0.0f)
	        {
                _mass = 1.0f;
	        }

            _invMass = 1.0f / _mass;

	        if (massData.I > 0.0f && (_flags & BodyFlags.FixedRotation) == 0)
	        {
		        _I = massData.I - _mass * Vector2.Dot(massData.center, massData.center);
                Debug.Assert(_I > 0.0f);
		        _invI = 1.0f / _I;
	        }

	        // Move center of mass.
	        Vector2 oldCenter = _sweep.c;
	        _sweep.localCenter = massData.center;
	        _sweep.c0 = _sweep.c = MathUtils.Multiply(ref _xf, _sweep.localCenter);

	        // Update center of mass velocity.
            _linearVelocity += MathUtils.Cross(_angularVelocity, _sweep.c - oldCenter);
        }

        /// This resets the mass properties to the sum of the mass properties of the fixtures.
        /// This normally does not need to be called unless you called SetMassData to override
        /// the mass and you later want to reset the mass.
        public void ResetMassData()
        {
	        // Compute mass data from shapes. Each shape has its own density.
	        _mass = 0.0f;
	        _invMass = 0.0f;
	        _I = 0.0f;
	        _invI = 0.0f;
            _sweep.localCenter = Vector2.Zero;

            // Static and kinematic bodies have zero mass.
            if (_type == BodyType.Static || _type == BodyType.Kinematic)
            {
                _sweep.c0 = _sweep.c = _xf.Position;
                return;
            }

            Debug.Assert(_type == BodyType.Dynamic);

            // Accumulate mass over all fixtures.
	        Vector2 center = Vector2.Zero;
	        for (Fixture f = _fixtureList; f != null; f = f._next)
	        {
                if (f._density == 0.0f)
                {
                    continue;
                }

                MassData massData;
                f.GetMassData(out massData);
		        _mass += massData.mass;
		        center += massData.mass * massData.center;
		        _I += massData.I;
	        }

	        // Compute center of mass.
	        if (_mass > 0.0f)
	        {
		        _invMass = 1.0f / _mass;
		        center *= _invMass;
	        }
            else
            {
                // Force all dynamic bodies to have a positive mass.
                _mass = 1.0f;
                _invMass = 1.0f;
            }

	        if (_I > 0.0f && (_flags & BodyFlags.FixedRotation) == 0)
	        {
		        // Center the inertia about the center of mass.
		        _I -= _mass * Vector2.Dot(center, center);

		        Debug.Assert(_I > 0.0f);
		        _invI = 1.0f / _I;
	        }
	        else
	        {
		        _I = 0.0f;
		        _invI = 0.0f;
	        }

	        // Move center of mass.
	        Vector2 oldCenter = _sweep.c;
	        _sweep.localCenter = center;
	        _sweep.c0 = _sweep.c = MathUtils.Multiply(ref _xf, _sweep.localCenter);

	        // Update center of mass velocity.
	        _linearVelocity += MathUtils.Cross(_angularVelocity, _sweep.c - oldCenter);
        }

	    /// Get the world coordinates of a point given the local coordinates.
	    /// @param localPoint a point on the body measured relative the the body's origin.
	    /// @return the same point expressed in world coordinates.
	    public Vector2 GetWorldPoint(Vector2 localPoint)
        {
            return MathUtils.Multiply(ref _xf, localPoint);
        }

	    /// Get the world coordinates of a vector given the local coordinates.
	    /// @param localVector a vector fixed in the body.
	    /// @return the same vector expressed in world coordinates.
	    public Vector2 GetWorldVector(Vector2 localVector)
        {
            return MathUtils.Multiply(ref _xf.R, localVector);
        }

	    /// Gets a local point relative to the body's origin given a world point.
	    /// @param a point in world coordinates.
	    /// @return the corresponding local point relative to the body's origin.
	    public Vector2 GetLocalPoint(Vector2 worldPoint)
        {
            return MathUtils.MultiplyT(ref _xf, worldPoint);
        }

	    /// Gets a local vector given a world vector.
	    /// @param a vector in world coordinates.
	    /// @return the corresponding local vector.
	    public Vector2 GetLocalVector(Vector2 worldVector)
        {
            return MathUtils.MultiplyT(ref _xf.R, worldVector);
        }

	    /// Get the world linear velocity of a world point attached to this body.
	    /// @param a point in world coordinates.
	    /// @return the world velocity of a point.
	    public Vector2 GetLinearVelocityFromWorldPoint(Vector2 worldPoint)
        {
            return _linearVelocity + MathUtils.Cross(_angularVelocity, worldPoint - _sweep.c);
        }

	    /// Get the world velocity of a local point.
	    /// @param a point in local coordinates.
	    /// @return the world velocity of a point.
	    public Vector2 GetLinearVelocityFromLocalPoint(Vector2 localPoint)
        {
            return GetLinearVelocityFromWorldPoint(GetWorldPoint(localPoint));
        }

	    /// Get the linear damping of the body.
	    public float GetLinearDamping()
        {
            return _linearDamping;
        }

	    /// Set the linear damping of the body.
	    public void SetLinearDamping(float linearDamping)
        {
            _linearDamping = linearDamping;
        }

	    /// Get the angular damping of the body.
	    public float GetAngularDamping()
        {
            return _angularDamping;
        }

	    /// Set the angular damping of the body.
	    public void SetAngularDamping(float angularDamping)
        {
            _angularDamping = angularDamping;
        }

	    /// Is this body treated like a bullet for continuous collision detection?
	    public bool IsBullet
        {
            get
            {
                return (_flags & BodyFlags.Bullet) == BodyFlags.Bullet;
            }
        }

	    /// Should this body be treated like a bullet for continuous collision detection?
	    public void SetBullet(bool flag)
        {
            if (flag)
	        {
		        _flags |= BodyFlags.Bullet;
	        }
	        else
	        {
		        _flags &= ~BodyFlags.Bullet;
	        }
        }

        /// You can disable sleeping on this body. If you disable sleeping, the
	    /// body will be woken.
	    void SetSleepingAllowed(bool flag)
        {
            if (flag)
            {
                _flags |= BodyFlags.AutoSleep;
            }
            else
            {
                _flags &= ~BodyFlags.AutoSleep;
                SetAwake(true);
            }

        }

	    /// Is this body allowed to sleep
	    public bool IsSleepingAllowed
        {
            get
            {
                return (_flags & BodyFlags.AutoSleep) == BodyFlags.AutoSleep;
            }
        }

	    /// You can disable sleeping on this body.
	    public void AllowSleeping(bool flag)
        {
            if (flag)
	        {
		        _flags |= BodyFlags.AutoSleep;
	        }
	        else
	        {
		        _flags &= ~BodyFlags.AutoSleep;
		        SetAwake(true);
	        }
        }

	    /// Set the sleep state of the body. A sleeping body has very
	    /// low CPU cost.
	    /// @param flag set to true to put body to sleep, false to wake it.
	    public void SetAwake(bool flag)
        {
            if (flag)
            {
                if ((_flags & BodyFlags.Awake) == 0)
                {
                    _flags |= BodyFlags.Awake;
                    _sleepTime = 0.0f;
                }
            }
            else
            {
                _flags &= ~BodyFlags.Awake;
                _sleepTime = 0.0f;
                _linearVelocity = Vector2.Zero;
                _angularVelocity = 0.0f;
                _force = Vector2.Zero;
                _torque = 0.0f;
            }
        }

        /// Get the sleeping state of this body.
	    /// @return true if the body is sleeping.
	    public bool IsAwake()
        {
            return (_flags & BodyFlags.Awake) == BodyFlags.Awake;
        }

	    /// Set the active state of the body. An inactive body is not
	    /// simulated and cannot be collided with or woken up.
	    /// If you pass a flag of true, all fixtures will be added to the
	    /// broad-phase.
	    /// If you pass a flag of false, all fixtures will be removed from
	    /// the broad-phase and all contacts will be destroyed.
	    /// Fixtures and joints are otherwise unaffected. You may continue
	    /// to create/destroy fixtures and joints on inactive bodies.
	    /// Fixtures on an inactive body are implicitly inactive and will
	    /// not participate in collisions, ray-casts, or queries.
	    /// Joints connected to an inactive body are implicitly inactive.
	    /// An inactive body is still owned by a b2World object and remains
	    /// in the body list.
        public void SetActive(bool flag)
        {
            if (flag == IsActive())
            {
                return;
            }

            if (flag)
            {
                _flags |= BodyFlags.Active;

                // Create all proxies.
                BroadPhase broadPhase = _world._contactManager._broadPhase;
                for (Fixture f = _fixtureList; f != null; f = f._next)
                {
                    f.CreateProxies(broadPhase, ref _xf);
                }

                // Contacts are created the next time step.
            }
            else
            {
                _flags &= ~BodyFlags.Active;

                // Destroy all proxies.
                BroadPhase broadPhase = _world._contactManager._broadPhase;
                for (Fixture f = _fixtureList; f != null; f = f._next)
                {
                    f.DestroyProxies(broadPhase);
                }

                // Destroy the attached contacts.
                ContactEdge ce = _contactList;
                while (ce != null)
                {
                    ContactEdge ce0 = ce;
                    ce = ce.Next;
                    _world._contactManager.Destroy(ce0.Contact);
                }
                _contactList = null;
            }
        }

	    /// Get the active state of the body.
	    public bool IsActive()
        {
            return (_flags & BodyFlags.Active) == BodyFlags.Active;
        }

	    /// Set this body to have fixed rotation. This causes the mass
	    /// to be reset.
        public void SetFixedRotation(bool flag)
        {
            if (flag)
            {
                _flags |= BodyFlags.FixedRotation;
            }
            else
            {
                _flags &= ~BodyFlags.FixedRotation;
            }

            ResetMassData();
        }

	    /// Does this body have fixed rotation?
        public bool IsFixedRotation()
        {
            return (_flags & BodyFlags.FixedRotation) == BodyFlags.FixedRotation;
        }

	    public Fixture GetFixtureList()
        {
            return _fixtureList;
        }

	    /// Get the list of all joints attached to this body.
	    public JointEdge GetJointList()
        {
            return _jointList;
        }

	    /// Get the list of all contacts attached to this body.
	    /// @warning this list changes during the time step and you may
	    /// miss some collisions if you don't use ContactListener.
	    public ContactEdge GetContactList()
        {
            return _contactList;
        }

        /// Get the next body in the world's body list.
	    public Body GetNext()
        {
            return _next;
        }

	    /// Get the user data pointer that was provided in the body definition.
	    public object GetUserData()
        {
            return _userData;
        }

	    /// Set the user data. Use this to store your application specific data.
	    public void SetUserData(object data)
        {
            _userData = data;
        }

	    /// Get the parent world of this body.
	    public World GetWorld()
        {
            return _world;
        }

        internal Body(BodyDef bd, World world)
        {
            Debug.Assert(bd.position.IsValid());
            Debug.Assert(bd.linearVelocity.IsValid());
            Debug.Assert(MathUtils.IsValid(bd.angle));
            Debug.Assert(MathUtils.IsValid(bd.angularVelocity));
            Debug.Assert(MathUtils.IsValid(bd.inertiaScale) && bd.inertiaScale >= 0.0f);
            Debug.Assert(MathUtils.IsValid(bd.angularDamping) && bd.angularDamping >= 0.0f);
            Debug.Assert(MathUtils.IsValid(bd.linearDamping) && bd.linearDamping >= 0.0f);

	        if (bd.bullet)
	        {
		        _flags |= BodyFlags.Bullet;
	        }
	        if (bd.fixedRotation)
	        {
		        _flags |= BodyFlags.FixedRotation;
	        }
	        if (bd.allowSleep)
	        {
		        _flags |= BodyFlags.AutoSleep;
	        }
            if (bd.awake)
            {
                _flags |= BodyFlags.Awake;
            }
            if (bd.active)
            {
                _flags |= BodyFlags.Active;
            }

	        _world = world;

	        _xf.Position = bd.position;
	        _xf.R.Set(bd.angle);

	        _sweep.a0 = _sweep.a = bd.angle;
	        _sweep.c0 = _sweep.c = MathUtils.Multiply(ref _xf, _sweep.localCenter);

	        _linearVelocity = bd.linearVelocity;
	        _angularVelocity = bd.angularVelocity;

	        _linearDamping = bd.linearDamping;
	        _angularDamping = bd.angularDamping;

            _type = bd.type;
            if (_type == BodyType.Dynamic)
            {
                _mass = 1.0f;
                _invMass = 1.0f;
            }

	        _userData = bd.userData;
        }

        internal void SynchronizeFixtures()
        {
            Transform xf1 = new Transform();
	        xf1.R.Set(_sweep.a0);
	        xf1.Position = _sweep.c0 - MathUtils.Multiply(ref xf1.R, _sweep.localCenter);

	        BroadPhase broadPhase = _world._contactManager._broadPhase;
	        for (Fixture f = _fixtureList; f != null; f = f._next)
	        {
		        f.Synchronize(broadPhase, ref xf1, ref _xf);
	        }
        }

	    internal void SynchronizeTransform()
        {
            _xf.R.Set(_sweep.a);
	        _xf.Position = _sweep.c - MathUtils.Multiply(ref _xf.R, _sweep.localCenter);
        }

	    // This is used to prevent connected bodies from colliding.
	    // It may lie, depending on the collideConnected flag.
	    internal bool ShouldCollide(Body other)
        {
            // At least one body should be dynamic.
            if (_type != BodyType.Dynamic && other._type != BodyType.Dynamic)
            {
                return false;
            }

            // Does a joint prevent collision?
            for (JointEdge jn = _jointList; jn != null; jn = jn.Next)
	        {
		        if (jn.Other == other)
		        {
                    if (jn.Joint._collideConnected == false)
                    {
                        return false;
                    }
		        }
	        }

	        return true;
        }

	    internal void Advance(float t)
        {
            // Advance to the new safe time.
	        _sweep.Advance(t);
	        _sweep.c = _sweep.c0;
	        _sweep.a = _sweep.a0;
	        SynchronizeTransform();
        }

        /// <summary>
        /// Get the world body origin position.
        /// </summary>
        /// <returns>Return the world position of the body's origin.</returns>
        public Vector2 Position
        {
            get
            {
                return _xf.Position;
            }
            set
            {
                SetTransform(value, Rotation);
            }
        }

        /// <summary>
        /// Get the angle in radians.
        /// </summary>
        /// <returns>Return the current world rotation angle in radians.</returns>
        public float Rotation
        {
            get
            {
                return _sweep.a;
            }
            set
            {
                SetTransform(Position, value);
            }
        }

        internal BodyFlags _flags;
        internal BodyType _type;

        internal int _islandIndex;

        internal Transform _xf;		// the body origin transform
        internal Sweep _sweep;	// the swept motion for CCD

        internal Vector2 _linearVelocity;
        internal float _angularVelocity;

        internal Vector2 _force;
        internal float _torque;

        internal World _world;
        internal Body _prev;
        internal Body _next;

        internal Fixture _fixtureList;
        internal int _fixtureCount;

        internal JointEdge _jointList;
        internal ContactEdge _contactList;

        internal float _mass, _invMass;

        // Rotational inertia about the center of mass.
        internal float _I, _invI;

        internal float _linearDamping;
        internal float _angularDamping;

        internal float _sleepTime;

        internal object _userData;

        internal float _intertiaScale;
    }
}


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

using System;

namespace Box2D.XNA
{
    public class ContactManager
    {
        internal ContactManager()
        {
            _addPair = AddPair;

            _contactList = null;
            _contactCount = 0;
            ContactFilter = new DefaultContactFilter();
            ContactListener = new DefaultContactListener();
        }

	    // Broad-phase callback.
        internal void AddPair(FixtureProxy proxyA, FixtureProxy proxyB)
        {
            Fixture fixtureA = proxyA.fixture;
            Fixture fixtureB = proxyB.fixture;

            int indexA = proxyA.childIndex;
            int indexB = proxyB.childIndex;

	        Body bodyA = fixtureA.GetBody();
	        Body bodyB = fixtureB.GetBody();

	        // Are the fixtures on the same body?
	        if (bodyA == bodyB)
	        {
		        return;
	        }

	        // Does a contact already exist?
	        ContactEdge edge = bodyB.GetContactList();
	        while (edge != null)
	        {
		        if (edge.Other == bodyA)
		        {
			        Fixture fA = edge.Contact.GetFixtureA();
			        Fixture fB = edge.Contact.GetFixtureB();
                    int iA = edge.Contact.GetChildIndexA();
                    int iB = edge.Contact.GetChildIndexB();

                    if (fA == fixtureA && fB == fixtureB && iA == indexA && iB == indexB)
                    {
                        // A contact already exists.
                        return;
                    }

                    if (fA == fixtureB && fB == fixtureA && iA == indexB && iB == indexA)
                    {
				        // A contact already exists.
				        return;
			        }
		        }

		        edge = edge.Next;
	        }

            // Does a joint override collision? Is at least one body dynamic?
            if (bodyB.ShouldCollide(bodyA) == false)
	        {
		        return;
	        }

	        // Check user filtering.
            if (ContactFilter != null && ContactFilter.ShouldCollide(fixtureA, fixtureB) == false)
	        {
		        return;
	        }

	        // Call the factory.
	        Contact c = Contact.Create(fixtureA, indexA, fixtureB, indexB);

	        // Contact creation may swap fixtures.
	        fixtureA = c.GetFixtureA();
	        fixtureB = c.GetFixtureB();
            indexA = c.GetChildIndexA();
            indexB = c.GetChildIndexB();
	        bodyA = fixtureA.GetBody();
	        bodyB = fixtureB.GetBody();

	        // Insert into the world.
	        c._prev = null;
	        c._next = _contactList;
	        if (_contactList != null)
	        {
		        _contactList._prev = c;
	        }
	        _contactList = c;

	        // Connect to island graph.

	        // Connect to body A
	        c._nodeA.Contact = c;
	        c._nodeA.Other = bodyB;

	        c._nodeA.Prev = null;
	        c._nodeA.Next = bodyA._contactList;
	        if (bodyA._contactList != null)
	        {
		        bodyA._contactList.Prev = c._nodeA;
	        }
	        bodyA._contactList = c._nodeA;

	        // Connect to body B
	        c._nodeB.Contact = c;
	        c._nodeB.Other = bodyA;

	        c._nodeB.Prev = null;
	        c._nodeB.Next = bodyB._contactList;
	        if (bodyB._contactList != null)
	        {
		        bodyB._contactList.Prev = c._nodeB;
	        }
	        bodyB._contactList = c._nodeB;

	        ++_contactCount;
        }

	    internal void FindNewContacts()
        {
            _broadPhase.UpdatePairs<FixtureProxy>(_addPair);
        }

	    internal void Destroy(Contact c)
        {
            Fixture fixtureA = c.GetFixtureA();
	        Fixture fixtureB = c.GetFixtureB();
	        Body bodyA = fixtureA.GetBody();
	        Body bodyB = fixtureB.GetBody();

            if (ContactListener != null && c.IsTouching())
	        {
		        ContactListener.EndContact(c);
	        }

	        // Remove from the world.
	        if (c._prev != null)
	        {
		        c._prev._next = c._next;
	        }

            if (c._next != null)
	        {
		        c._next._prev = c._prev;
	        }

	        if (c == _contactList)
	        {
		        _contactList = c._next;
	        }

	        // Remove from body 1
            if (c._nodeA.Prev != null)
	        {
		        c._nodeA.Prev.Next = c._nodeA.Next;
	        }

            if (c._nodeA.Next != null)
	        {
		        c._nodeA.Next.Prev = c._nodeA.Prev;
	        }

	        if (c._nodeA == bodyA._contactList)
	        {
		        bodyA._contactList = c._nodeA.Next;
	        }

	        // Remove from body 2
            if (c._nodeB.Prev != null)
	        {
		        c._nodeB.Prev.Next = c._nodeB.Next;
	        }

            if (c._nodeB.Next != null)
	        {
		        c._nodeB.Next.Prev = c._nodeB.Prev;
	        }

	        if (c._nodeB == bodyB._contactList)
	        {
		        bodyB._contactList = c._nodeB.Next;
	        }

            c.Destroy();

	        --_contactCount;
        }

	    internal void Collide()
        {
            // Update awake contacts.
	        Contact c = _contactList;
	        while (c != null)
	        {
		        Fixture fixtureA = c.GetFixtureA();
		        Fixture fixtureB = c.GetFixtureB();
                int indexA = c.GetChildIndexA();
                int indexB = c.GetChildIndexB();
		        Body bodyA = fixtureA.GetBody();
		        Body bodyB = fixtureB.GetBody();

		        if (bodyA.IsAwake() == false && bodyB.IsAwake() == false)
		        {
			        c = c.GetNext();
			        continue;
		        }

		        // Is this contact flagged for filtering?
                if ((c._flags & ContactFlags.Filter) == ContactFlags.Filter)
		        {
                    // Should these bodies collide?
                    if (bodyB.ShouldCollide(bodyA) == false)
			        {
				        Contact cNuke = c;
				        c = cNuke.GetNext();
				        Destroy(cNuke);
				        continue;
			        }

			        // Check user filtering.
                    if (ContactFilter != null && ContactFilter.ShouldCollide(fixtureA, fixtureB) == false)
			        {
				        Contact cNuke = c;
				        c = cNuke.GetNext();
				        Destroy(cNuke);
				        continue;
			        }

			        // Clear the filtering flag.
			        c._flags &= ~ContactFlags.Filter;
		        }

                int proxyIdA = fixtureA._proxies[indexA].proxyId;
                int proxyIdB = fixtureB._proxies[indexB].proxyId;

                bool overlap = _broadPhase.TestOverlap(proxyIdA, proxyIdB);

                // Here we destroy contacts that cease to overlap in the broad-phase.
                if (overlap == false)
		        {
			        Contact cNuke = c;
			        c = cNuke.GetNext();
			        Destroy(cNuke);
			        continue;
		        }

		        // The contact persists.
		        c.Update(ContactListener);
		        c = c.GetNext();
	        }
        }

	    internal BroadPhase _broadPhase = new BroadPhase();
	    internal Contact _contactList;
	    internal int _contactCount;

        internal IContactFilter ContactFilter { get; set; }
        internal IContactListener ContactListener { get; set; }

        Action<FixtureProxy, FixtureProxy> _addPair;
    }
}

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.Collections.Generic;

namespace Box2D.XNA
{
    /// A contact edge is used to connect bodies and contacts together
    /// in a contact graph where each body is a node and each contact
    /// is an edge. A contact edge belongs to a doubly linked list
    /// maintained in each attached body. Each contact has two contact
    /// nodes, one for each attached body.
    public class ContactEdge
    {
        public Body Other;			///< provides quick access to the other body attached.
        public Contact Contact;		///< the contact
        public ContactEdge Prev;	///< the previous contact edge in the body's contact list
        public ContactEdge Next;	///< the next contact edge in the body's contact list
    };

    [Flags]
    public enum ContactFlags
    {
        None = 0,

        // Used when crawling contact graph when forming islands.
        Island = 0x0001,

        // Set when the shapes are touching.
        Touching = 0x0002,

        // This contact can be disabled (by user)
        Enabled = 0x0004,

        // This contact needs filtering because a fixture filter was changed.
        Filter = 0x0008,

        // This bullet contact had a TOI event
        BulletHit = 0x0010,
    };

    /// The class manages contact between two shapes. A contact exists for each overlapping
    /// AABB in the broad-phase (except if filtered). Therefore a contact object may exist
    /// that has no contact points.
    public class Contact
    {
        /// Get the contact manifold. Do not modify the manifold unless you understand the
        /// internals of Box2D.
	    public void GetManifold(out Manifold manifold)
        {
            manifold = _manifold;
        }

	    /// Get the world manifold.
	    public void GetWorldManifold(out WorldManifold worldManifold)
        {
            Body bodyA = _fixtureA.GetBody();
	        Body bodyB = _fixtureB.GetBody();
	        Shape shapeA = _fixtureA.GetShape();
	        Shape shapeB = _fixtureB.GetShape();

            Transform xfA, xfB;
            bodyA.GetTransform(out xfA);
            bodyB.GetTransform(out xfB);

            worldManifold = new WorldManifold(ref _manifold, ref xfA, shapeA._radius, ref xfB, shapeB._radius);
        }

        /// Is this contact touching?
	    public bool IsTouching()
        {
            return (_flags & ContactFlags.Touching) == ContactFlags.Touching;
        }

        /// Enable/disable this contact. This can be used inside the pre-solve
	    /// contact listener. The contact is only disabled for the current
	    /// time step (or sub-step in continuous collisions).
        public void SetEnabled(bool flag)
        {
            if (flag)
            {
                _flags |= ContactFlags.Enabled;
            }
            else
            {
                _flags &= ~ContactFlags.Enabled;
            }
        }

        /// Has this contact been disabled?
        public bool IsEnabled()
        {
            return (_flags & ContactFlags.Enabled) == ContactFlags.Enabled;
        }

	    /// Get the next contact in the world's contact list.
	    public Contact GetNext()
        {
            return _next;
        }

        /// Get fixture A in this contact.
	    public Fixture GetFixtureA()
        {
            return _fixtureA;
        }

	    /// Get fixture B in this contact.
	    public Fixture GetFixtureB()
        {
            return _fixtureB;
        }

        /// Get the child primitive index for fixture A.
        public int GetChildIndexA()
        {
            return _indexA;
        }

        /// Get the child primitive index for fixture B.
        public int GetChildIndexB()
        {
            return _indexB;
        }

	    /// Flag this contact for filtering. Filtering will occur the next time step.
	    public void FlagForFiltering()
        {
            _flags |= ContactFlags.Filter;
        }

        internal Contact(Fixture fA, int indexA, Fixture fB, int indexB)
        {
            Reset(fA, indexA, fB, indexB);
        }

        internal void Reset(Fixture fA, int indexA, Fixture fB, int indexB)
        {
            _flags = ContactFlags.Enabled;

	        _fixtureA = fA;
	        _fixtureB = fB;

            _indexA = indexA;
            _indexB = indexB;

	        _manifold._pointCount = 0;

	        _prev = null;
	        _next = null;

	        _nodeA.Contact = null;
	        _nodeA.Prev = null;
	        _nodeA.Next = null;
	        _nodeA.Other = null;

	        _nodeB.Contact = null;
	        _nodeB.Prev = null;
	        _nodeB.Next = null;
	        _nodeB.Other = null;

            _toiCount = 0;
        }

        // Update the contact manifold and touching status.
        // Note: do not assume the fixture AABBs are overlapping or are valid.
	    internal void Update(IContactListener listener)
        {
            Manifold oldManifold = _manifold;

            // Re-enable this contact.
            _flags |= ContactFlags.Enabled;

            bool touching = false;
            bool wasTouching = (_flags & ContactFlags.Touching) == ContactFlags.Touching;

            bool sensorA = _fixtureA.IsSensor();
            bool sensorB = _fixtureB.IsSensor();
            bool sensor = sensorA || sensorB;

	        Body bodyA = _fixtureA.GetBody();
	        Body bodyB = _fixtureB.GetBody();
	        Transform xfA; bodyA.GetTransform(out xfA);
	        Transform xfB; bodyB.GetTransform(out xfB);

	        // Is this contact a sensor?
	        if (sensor)
	        {
		        Shape shapeA = _fixtureA.GetShape();
		        Shape shapeB = _fixtureB.GetShape();
		        touching = AABB.TestOverlap(shapeA, _indexA, shapeB, _indexB, ref xfA, ref xfB);

		        // Sensors don't generate manifolds.
		        _manifold._pointCount = 0;
	        }
	        else
	        {
		        Evaluate(ref _manifold, ref xfA, ref xfB);
		        touching = _manifold._pointCount > 0;

		        // Match old contact ids to new contact ids and copy the
		        // stored impulses to warm start the solver.
		        for (int i = 0; i < _manifold._pointCount; ++i)
		        {
			        ManifoldPoint mp2 = _manifold._points[i];
			        mp2.NormalImpulse = 0.0f;
			        mp2.TangentImpulse = 0.0f;
			        ContactID id2 = mp2.Id;
                    bool found = false;

			        for (int j = 0; j < oldManifold._pointCount; ++j)
			        {
				        ManifoldPoint mp1 = oldManifold._points[j];

				        if (mp1.Id.Key == id2.Key)
				        {
					        mp2.NormalImpulse = mp1.NormalImpulse;
					        mp2.TangentImpulse = mp1.TangentImpulse;
                            found = true;
					        break;
				        }
			        }
                    if (found == false)
                    {
                        mp2.NormalImpulse = 0.0f;
                        mp2.TangentImpulse = 0.0f;
                    }

                    _manifold._points[i] = mp2;
		        }

		        if (touching != wasTouching)
		        {
			        bodyA.SetAwake(true);
			        bodyB.SetAwake(true);
		        }
	        }

	        if (touching)
	        {
		        _flags |= ContactFlags.Touching;
	        }
	        else
	        {
		        _flags &= ~ContactFlags.Touching;
	        }

	        if (wasTouching == false && touching == true && null != listener)
	        {
		        listener.BeginContact(this);
	        }

	        if (wasTouching == true && touching == false && null != listener)
	        {
		        listener.EndContact(this);
	        }

	        if (sensor == false && null != listener)
	        {
		        listener.PreSolve(this, ref oldManifold);
	        }
        }

        private static EdgeShape s_edge = new EdgeShape();

        /// Evaluate this contact with your own manifold and transforms.
        internal void Evaluate(ref Manifold manifold, ref Transform xfA, ref Transform xfB)
        {
            switch (_type)
            {
                case ContactType.Polygon:
                    Collision.CollidePolygons(ref manifold,
                            (PolygonShape)_fixtureA.GetShape(), ref xfA,
                            (PolygonShape)_fixtureB.GetShape(), ref xfB);
                    break;
                case ContactType.PolygonAndCircle:
                    Collision.CollidePolygonAndCircle(ref manifold,
                            (PolygonShape)_fixtureA.GetShape(), ref xfA,
                            (CircleShape) _fixtureB.GetShape(), ref xfB);
                    break;
                case ContactType.EdgeAndCircle:
                    Collision.CollideEdgeAndCircle(ref manifold,
                            (EdgeShape)  _fixtureA.GetShape(), ref xfA,
                            (CircleShape)_fixtureB.GetShape(), ref xfB);
                    break;
                case ContactType.EdgeAndPolygon:
                    Collision.CollideEdgeAndPolygon(ref manifold,
                            (EdgeShape)   _fixtureA.GetShape(), ref xfA,
                            (PolygonShape)_fixtureB.GetShape(), ref xfB);
                    break;
                case ContactType.LoopAndCircle:
                    var loop = (LoopShape)_fixtureA.GetShape();
                    loop.GetChildEdge(ref s_edge, _indexA);
                    Collision.CollideEdgeAndCircle(ref manifold, s_edge, ref xfA,
                            (CircleShape)_fixtureB.GetShape(), ref xfB);
                    break;
                case ContactType.LoopAndPolygon:
                    var loop2 = (LoopShape)_fixtureA.GetShape();
                    loop2.GetChildEdge(ref s_edge, _indexA);
                    Collision.CollideEdgeAndPolygon(ref manifold, s_edge, ref xfA,
                            (PolygonShape)_fixtureB.GetShape(), ref xfB);
                    break;
                case ContactType.Circle:
                    Collision.CollideCircles(ref manifold,
                            (CircleShape)_fixtureA.GetShape(), ref xfA,
                            (CircleShape)_fixtureB.GetShape(), ref xfB);
                    break;
            }
        }

        internal enum ContactType
        {
            Polygon,
            PolygonAndCircle,
            Circle,
            EdgeAndPolygon,
            EdgeAndCircle,
            LoopAndPolygon,
            LoopAndCircle,
        }

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

        internal static ContactType[,] s_registers = new ContactType[,]
        {
            {
              ContactType.Circle,
              ContactType.EdgeAndCircle,
              ContactType.PolygonAndCircle,
              ContactType.LoopAndCircle,
            },
            {
              ContactType.EdgeAndCircle,
              ContactType.EdgeAndCircle,  // 1,1 is invalid (no ContactType.Edge)
              ContactType.EdgeAndPolygon,
              ContactType.EdgeAndPolygon, // 1,3 is invalid (no ContactType.EdgeAndLoop)
            },
            {
              ContactType.PolygonAndCircle,
              ContactType.EdgeAndPolygon,
              ContactType.Polygon,
              ContactType.LoopAndPolygon,
            },
            {
              ContactType.LoopAndCircle,
              ContactType.LoopAndCircle,  // 3,1 is invalid (no ContactType.EdgeAndLoop)
              ContactType.LoopAndPolygon,
              ContactType.LoopAndPolygon, // 3,3 is invalid (no ContactType.Loop)
            },
        };

        internal static Contact Create(Fixture fixtureA, int indexA, Fixture fixtureB, int indexB)
        {
            ShapeType type1 = fixtureA.ShapeType;
	        ShapeType type2 = fixtureB.ShapeType;

	        Debug.Assert(ShapeType.Unknown < type1 && type1 < ShapeType.TypeCount);
            Debug.Assert(ShapeType.Unknown < type2 && type2 < ShapeType.TypeCount);

            Contact c;
            var pool = fixtureA._body._world._contactPool;
            if (pool.Count > 0)
            {
                c = pool.Dequeue();
                if ((type1 >= type2 || (type1 == ShapeType.Edge && type2 == ShapeType.Polygon))
                    &&
                    !(type2 == ShapeType.Edge && type1 == ShapeType.Polygon))
                {
                    c.Reset(fixtureA, indexA, fixtureB, indexB);
                }
                else
                {
                    c.Reset(fixtureB, indexB, fixtureA, indexA);
                }
            }
            else
            {
                // Edge+Polygon is non-symetrical due to the way Erin handles collision type registration.
                if ((type1 >= type2 || (type1 == ShapeType.Edge && type2 == ShapeType.Polygon))
                    &&
                    !(type2 == ShapeType.Edge && type1 == ShapeType.Polygon))
                {
                    c = new Contact(fixtureA, indexA, fixtureB, indexB);
                }
                else
                {
                    c = new Contact(fixtureB, indexB, fixtureA, indexA);
                }
            }

            c._type = Contact.s_registers[(int)type1, (int)type2];

            return c;
        }

        internal void Destroy()
        {
            _fixtureA._body._world._contactPool.Enqueue(this);
            Reset(null, 0, null, 0);
        }

        private ContactType _type;
	    internal ContactFlags _flags;

	    // World pool and list pointers.
	    internal Contact _prev;
	    internal Contact _next;

	    // Nodes for connecting bodies.
	    internal ContactEdge _nodeA = new ContactEdge();
        internal ContactEdge _nodeB = new ContactEdge();

	    internal Fixture _fixtureA;
	    internal Fixture _fixtureB;

        internal int _indexA;
        internal int _indexB;

	    internal Manifold _manifold;

	    internal int _toiCount;
    };
}


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

//#define MATH_OVERLOADS

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

namespace Box2D.XNA
{
    public struct ContactConstraintPoint
    {
        public Vector2 localPoint;
        public Vector2 rA;
        public Vector2 rB;
        public float normalImpulse;
        public float tangentImpulse;
        public float normalMass;
        public float tangentMass;
        public float velocityBias;
    };

    public struct ContactConstraint
    {
        public FixedArray2<ContactConstraintPoint> points;
        public Vector2 localNormal;
        public Vector2 localPoint;
        public Vector2 normal;
        public Mat22 normalMass;
        public Mat22 K;
        public Body bodyA;
        public Body bodyB;
        public ManifoldType type;
        public float radius;
        public float friction;
        public int pointCount;
        public Manifold manifold;
    };

    public class ContactSolver
    {
        public ContactSolver() { }

        public void Reset(Contact[] contacts, int contactCount, float impulseRatio)
        {
            _contacts = contacts;

            _constraintCount = contactCount;

            // grow the array
            if (_constraints == null || _constraints.Length < _constraintCount)
            {
                _constraints = new ContactConstraint[_constraintCount * 2];
            }

            for (int i = 0; i < _constraintCount; ++i)
	        {
		        Contact contact = contacts[i];

		        Fixture fixtureA = contact._fixtureA;
		        Fixture fixtureB = contact._fixtureB;
		        Shape shapeA = fixtureA.GetShape();
		        Shape shapeB = fixtureB.GetShape();
		        float radiusA = shapeA._radius;
		        float radiusB = shapeB._radius;
		        Body bodyA = fixtureA.GetBody();
		        Body bodyB = fixtureB.GetBody();
                Manifold manifold;
                contact.GetManifold(out manifold);

		        float friction = Settings.b2MixFriction(fixtureA.GetFriction(), fixtureB.GetFriction());
                float restitution = Settings.b2MixRestitution(fixtureA.GetRestitution(), fixtureB.GetRestitution());

		        Vector2 vA = bodyA._linearVelocity;
		        Vector2 vB = bodyB._linearVelocity;
		        float wA = bodyA._angularVelocity;
		        float wB = bodyB._angularVelocity;

		        Debug.Assert(manifold._pointCount > 0);

		        WorldManifold worldManifold = new WorldManifold(ref manifold, ref bodyA._xf, radiusA, ref bodyB._xf, radiusB);

		        ContactConstraint cc = _constraints[i];
		        cc.bodyA = bodyA;
		        cc.bodyB = bodyB;
		        cc.manifold = manifold;
		        cc.normal = worldManifold._normal;
		        cc.pointCount = manifold._pointCount;
		        cc.friction = friction;

		        cc.localNormal = manifold._localNormal;
		        cc.localPoint = manifold._localPoint;
		        cc.radius = radiusA + radiusB;
		        cc.type = manifold._type;

		        for (int j = 0; j < cc.pointCount; ++j)
		        {
			        ManifoldPoint cp = manifold._points[j];
			        ContactConstraintPoint ccp = cc.points[j];

                    ccp.normalImpulse = impulseRatio * cp.NormalImpulse;
                    ccp.tangentImpulse = impulseRatio * cp.TangentImpulse;

			        ccp.localPoint = cp.LocalPoint;

			        ccp.rA = worldManifold._points[j] - bodyA._sweep.c;
			        ccp.rB = worldManifold._points[j] - bodyB._sweep.c;

#if MATH_OVERLOADS
			        float rnA = MathUtils.Cross(ccp.rA, cc.normal);
			        float rnB = MathUtils.Cross(ccp.rB, cc.normal);
#else
                    float rnA = ccp.rA.X * cc.normal.Y - ccp.rA.Y * cc.normal.X;
                    float rnB = ccp.rB.X * cc.normal.Y - ccp.rB.Y * cc.normal.X;
#endif
			        rnA *= rnA;
			        rnB *= rnB;

			        float kNormal = bodyA._invMass + bodyB._invMass + bodyA._invI * rnA + bodyB._invI * rnB;

			        Debug.Assert(kNormal > Settings.b2_epsilon);
			        ccp.normalMass = 1.0f / kNormal;

#if MATH_OVERLOADS
			        Vector2 tangent = MathUtils.Cross(cc.normal, 1.0f);

			        float rtA = MathUtils.Cross(ccp.rA, tangent);
			        float rtB = MathUtils.Cross(ccp.rB, tangent);
#else
                    Vector2 tangent = new Vector2(cc.normal.Y, -cc.normal.X);

                    float rtA = ccp.rA.X * tangent.Y - ccp.rA.Y * tangent.X;
                    float rtB = ccp.rB.X * tangent.Y - ccp.rB.Y * tangent.X;
#endif
                    rtA *= rtA;
                    rtB *= rtB;
			        float kTangent = bodyA._invMass + bodyB._invMass + bodyA._invI * rtA + bodyB._invI * rtB;

			        Debug.Assert(kTangent > Settings.b2_epsilon);
			        ccp.tangentMass = 1.0f /  kTangent;

			        // Setup a velocity bias for restitution.
			        ccp.velocityBias = 0.0f;
			        float vRel = Vector2.Dot(cc.normal, vB + MathUtils.Cross(wB, ccp.rB) - vA - MathUtils.Cross(wA, ccp.rA));
			        if (vRel < -Settings.b2_velocityThreshold)
			        {
				        ccp.velocityBias = -restitution * vRel;
			        }

                    cc.points[j] = ccp;
		        }

		        // If we have two points, then prepare the block solver.
		        if (cc.pointCount == 2)
		        {
			        ContactConstraintPoint ccp1 = cc.points[0];
			        ContactConstraintPoint ccp2 = cc.points[1];

			        float invMassA = bodyA._invMass;
			        float invIA = bodyA._invI;
			        float invMassB = bodyB._invMass;
			        float invIB = bodyB._invI;

			        float rn1A = MathUtils.Cross(ccp1.rA, cc.normal);
			        float rn1B = MathUtils.Cross(ccp1.rB, cc.normal);
			        float rn2A = MathUtils.Cross(ccp2.rA, cc.normal);
			        float rn2B = MathUtils.Cross(ccp2.rB, cc.normal);

			        float k11 = invMassA + invMassB + invIA * rn1A * rn1A + invIB * rn1B * rn1B;
			        float k22 = invMassA + invMassB + invIA * rn2A * rn2A + invIB * rn2B * rn2B;
			        float k12 = invMassA + invMassB + invIA * rn1A * rn2A + invIB * rn1B * rn2B;

			        // Ensure a reasonable condition number.
			        const float k_maxConditionNumber = 100.0f;
			        if (k11 * k11 < k_maxConditionNumber * (k11 * k22 - k12 * k12))
			        {
				        // K is safe to invert.
				        cc.K = new Mat22(new Vector2(k11, k12), new Vector2(k12, k22));
				        cc.normalMass = cc.K.GetInverse();
			        }
			        else
			        {
				        // The constraints are redundant, just use one.
				        // TODO_ERIN use deepest?
				        cc.pointCount = 1;
			        }
		        }

                _constraints[i] = cc;
	        }
        }

        public void WarmStart()
        {
            // Warm start.
            for (int i = 0; i < _constraintCount; ++i)
            {
	            ContactConstraint c = _constraints[i];

	            Body bodyA = c.bodyA;
	            Body bodyB = c.bodyB;
	            float invMassA = bodyA._invMass;
	            float invIA = bodyA._invI;
	            float invMassB = bodyB._invMass;
	            float invIB = bodyB._invI;
	            Vector2 normal = c.normal;

#if MATH_OVERLOADS
	            Vector2 tangent = MathUtils.Cross(normal, 1.0f);
#else
                Vector2 tangent = new Vector2(normal.Y, -normal.X);
#endif

	            for (int j = 0; j < c.pointCount; ++j)
	            {
		            ContactConstraintPoint ccp = c.points[j];
#if MATH_OVERLOADS
		            Vector2 P = ccp.normalImpulse * normal + ccp.tangentImpulse * tangent;
		            bodyA._angularVelocity -= invIA * MathUtils.Cross(ccp.rA, P);
		            bodyA._linearVelocity -= invMassA * P;
		            bodyB._angularVelocity += invIB * MathUtils.Cross(ccp.rB, P);
		            bodyB._linearVelocity += invMassB * P;
#else
                    Vector2 P = new Vector2(ccp.normalImpulse * normal.X + ccp.tangentImpulse * tangent.X,
                                            ccp.normalImpulse * normal.Y + ccp.tangentImpulse * tangent.Y);
                    bodyA._angularVelocity -= invIA * (ccp.rA.X * P.Y - ccp.rA.Y * P.X);
                    bodyA._linearVelocity.X -= invMassA * P.X;
                    bodyA._linearVelocity.Y -= invMassA * P.Y;
                    bodyB._angularVelocity += invIB * (ccp.rB.X * P.Y - ccp.rB.Y * P.X);
                    bodyB._linearVelocity.X += invMassB * P.X;
                    bodyB._linearVelocity.Y += invMassB * P.Y;
#endif
                    c.points[j] = ccp;
	            }

                _constraints[i] = c;
            }
        }

        public void SolveVelocityConstraints()
        {
            for (int i = 0; i < _constraintCount; ++i)
	        {
		        ContactConstraint c = _constraints[i];
		        Body bodyA = c.bodyA;
		        Body bodyB = c.bodyB;
		        float wA = bodyA._angularVelocity;
		        float wB = bodyB._angularVelocity;
		        Vector2 vA = bodyA._linearVelocity;
		        Vector2 vB = bodyB._linearVelocity;
		        float invMassA = bodyA._invMass;
		        float invIA = bodyA._invI;
		        float invMassB = bodyB._invMass;
		        float invIB = bodyB._invI;
		        Vector2 normal = c.normal;

#if MATH_OVERLOADS
				Vector2 tangent = ZoomEngine.Physics.Common.Math.Cross(normal, 1.0f);
#else
                Vector2 tangent = new Vector2(normal.Y, -normal.X);
#endif
		        float friction = c.friction;

		        Debug.Assert(c.pointCount == 1 || c.pointCount == 2);

		        // Solve tangent constraints
		        for (int j = 0; j < c.pointCount; ++j)
		        {
			        ContactConstraintPoint ccp = c.points[j];

#if MATH_OVERLOADS
			        // Relative velocity at contact
			        Vector2 dv = vB + MathUtils.Cross(wB, ccp.rB) - vA - MathUtils.Cross(wA, ccp.rA);

			        // Compute tangent force
			        float vt = Vector2.Dot(dv, tangent);
#else
                    // Relative velocity at contact
                    Vector2 dv = new Vector2(vB.X + (-wB * ccp.rB.Y) - vA.X - (-wA * ccp.rA.Y),
                                             vB.Y + (wB * ccp.rB.X) - vA.Y - (wA * ccp.rA.X));

                    // Compute tangent force
                    float vt = dv.X * tangent.X + dv.Y * tangent.Y;
#endif
			        float lambda = ccp.tangentMass * (-vt);

			        // MathUtils.Clamp the accumulated force
			        float maxFriction = friction * ccp.normalImpulse;
			        float newImpulse = MathUtils.Clamp(ccp.tangentImpulse + lambda, -maxFriction, maxFriction);
			        lambda = newImpulse - ccp.tangentImpulse;

#if MATH_OVERLOADS
			        // Apply contact impulse
			        Vector2 P = lambda * tangent;

			        vA -= invMassA * P;
			        wA -= invIA * MathUtils.Cross(ccp.rA, P);

			        vB += invMassB * P;
			        wB += invIB * MathUtils.Cross(ccp.rB, P);
#else
                    // Apply contact impulse
                    Vector2 P = new Vector2(lambda * tangent.X, lambda * tangent.Y);

                    vA.X -= invMassA * P.X;
                    vA.Y -= invMassA * P.Y;
                    wA -= invIA * (ccp.rA.X * P.Y - ccp.rA.Y * P.X);

                    vB.X += invMassB * P.X;
                    vB.Y += invMassB * P.Y;
                    wB += invIB * (ccp.rB.X * P.Y - ccp.rB.Y * P.X);
#endif
			        ccp.tangentImpulse = newImpulse;
                    c.points[j] = ccp;
		        }

		        // Solve normal constraints
		        if (c.pointCount == 1)
		        {
			        ContactConstraintPoint ccp = c.points[0];

#if MATH_OVERLOADS
			        // Relative velocity at contact
			        Vector2 dv = vB + MathUtils.Cross(wB, ccp.rB) - vA - MathUtils.Cross(wA, ccp.rA);

			        // Compute normal impulse
			        float vn = Vector2.Dot(dv, normal);
			        float lambda = -ccp.normalMass * (vn - ccp.velocityBias);

			        // MathUtils.Clamp the accumulated impulse
			        float newImpulse = Math.Max(ccp.normalImpulse + lambda, 0.0f);
			        lambda = newImpulse - ccp.normalImpulse;

			        // Apply contact impulse
			        Vector2 P = lambda * normal;
			        vA -= invMassA * P;
			        wA -= invIA * MathUtils.Cross(ccp.rA, P);

			        vB += invMassB * P;
			        wB += invIB * MathUtils.Cross(ccp.rB, P);
#else
                    // Relative velocity at contact
                    Vector2 dv = new Vector2(vB.X + (-wB * ccp.rB.Y) - vA.X - (-wA * ccp.rA.Y),
                                             vB.Y + (wB * ccp.rB.X) - vA.Y - (wA * ccp.rA.X));

                    // Compute normal impulse
                    float vn = dv.X * normal.X + dv.Y * normal.Y;
                    float lambda = -ccp.normalMass * (vn - ccp.velocityBias);

                    // Clamp the accumulated impulse
                    float newImpulse = Math.Max(ccp.normalImpulse + lambda, 0.0f);
                    lambda = newImpulse - ccp.normalImpulse;

                    // Apply contact impulse
                    var P = new Vector2(lambda * normal.X, lambda * normal.Y);

                    vA.X -= invMassA * P.X;
                    vA.Y -= invMassA * P.Y;
                    wA -= invIA * (ccp.rA.X * P.Y - ccp.rA.Y * P.X);

                    vB.X += invMassB * P.X;
                    vB.Y += invMassB * P.Y;
                    wB += invIB * (ccp.rB.X * P.Y - ccp.rB.Y * P.X);
#endif
                    ccp.normalImpulse = newImpulse;
                    c.points[0] = ccp;
		        }
		        else
		        {
			        // Block solver developed in collaboration with Dirk Gregorius (back in 01/07 on Box2D_Lite).
			        // Build the mini LCP for this contact patch
			        //
			        // vn = A * x + b, vn >= 0, , vn >= 0, x >= 0 and vn_i * x_i = 0 with i = 1..2
			        //
			        // A = J * W * JT and J = ( -n, -r1 x n, n, r2 x n )
			        // b = vn_0 - velocityBias
			        //
			        // The system is solved using the "Total enumeration method" (s. Murty). The complementary constraint vn_i * x_i
			        // implies that we must have in any solution either vn_i = 0 or x_i = 0. So for the 2D contact problem the cases
			        // vn1 = 0 and vn2 = 0, x1 = 0 and x2 = 0, x1 = 0 and vn2 = 0, x2 = 0 and vn1 = 0 need to be tested. The first valid
			        // solution that satisfies the problem is chosen.
			        //
			        // In order to account of the accumulated impulse 'a' (because of the iterative nature of the solver which only requires
			        // that the accumulated impulse is clamped and not the incremental impulse) we change the impulse variable (x_i).
			        //
			        // Substitute:
			        //
			        // x = x' - a
			        //
			        // Plug into above equation:
			        //
			        // vn = A * x + b
			        //    = A * (x' - a) + b
			        //    = A * x' + b - A * a
			        //    = A * x' + b'
			        // b' = b - A * a;

			        ContactConstraintPoint cp1 = c.points[0];
			        ContactConstraintPoint cp2 = c.points[1];

			        Vector2 a = new Vector2(cp1.normalImpulse, cp2.normalImpulse);
			        Debug.Assert(a.X >= 0.0f && a.Y >= 0.0f);

#if MATH_OVERLOADS
			        // Relative velocity at contact
			        Vector2 dv1 = vB + MathUtils.Cross(wB, cp1.rB) - vA - MathUtils.Cross(wA, cp1.rA);
			        Vector2 dv2 = vB + MathUtils.Cross(wB, cp2.rB) - vA - MathUtils.Cross(wA, cp2.rA);

			        // Compute normal velocity
			        float vn1 = Vector2.Dot(dv1, normal);
			        float vn2 = Vector2.Dot(dv2, normal);

                    Vector2 b = new Vector2(vn1 - cp1.velocityBias, vn2 - cp2.velocityBias);
			        b -= MathUtils.Multiply(ref c.K, a);
#else
                    // Relative velocity at contact
                    Vector2 dv1 = new Vector2(vB.X + (-wB * cp1.rB.Y) - vA.X - (-wA * cp1.rA.Y),
                                              vB.Y + (wB * cp1.rB.X) - vA.Y - (wA * cp1.rA.X));
                    Vector2 dv2 = new Vector2(vB.X + (-wB * cp2.rB.Y) - vA.X - (-wA * cp2.rA.Y),
                                              vB.Y + (wB * cp2.rB.X) - vA.Y - (wA * cp2.rA.X));

                    // Compute normal velocity
                    float vn1 = dv1.X * normal.X + dv1.Y * normal.Y;
                    float vn2 = dv2.X * normal.X + dv2.Y * normal.Y;

                    Vector2 b = new Vector2(vn1 - cp1.velocityBias, vn2 - cp2.velocityBias);
                    b -= MathUtils.Multiply(ref c.K, a); // Inlining didn't help for the multiply.
#endif
                    while (true)
			        {
				        //
				        // Case 1: vn = 0
				        //
				        // 0 = A * x' + b'
				        //
				        // Solve for x':
				        //
				        // x' = - inv(A) * b'
				        //
				        Vector2 x = - MathUtils.Multiply(ref c.normalMass, b);

				        if (x.X >= 0.0f && x.Y >= 0.0f)
                        {
#if MATH_OVERLOADS
					        // Resubstitute for the incremental impulse
					        Vector2 d = x - a;

					        // Apply incremental impulse
					        Vector2 P1 = d.X * normal;
					        Vector2 P2 = d.Y * normal;
					        vA -= invMassA * (P1 + P2);
					        wA -= invIA * (MathUtils.Cross(cp1.rA, P1) + MathUtils.Cross(cp2.rA, P2));

					        vB += invMassB * (P1 + P2);
					        wB += invIB * (MathUtils.Cross(cp1.rB, P1) + MathUtils.Cross(cp2.rB, P2));
#else
                            // Resubstitute for the incremental impulse
                            Vector2 d = new Vector2(x.X - a.X, x.Y - a.Y);

                            // Apply incremental impulse
                            Vector2 P1 = new Vector2(d.X * normal.X, d.X * normal.Y);
                            Vector2 P2 = new Vector2(d.Y * normal.X, d.Y * normal.Y);
                            Vector2 P12 = new Vector2(P1.X + P2.X, P1.Y + P2.Y);

                            vA.X -= invMassA * P12.X;
                            vA.Y -= invMassA * P12.Y;
                            wA -= invIA * ((cp1.rA.X * P1.Y - cp1.rA.Y * P1.X) + (cp2.rA.X * P2.Y - cp2.rA.Y * P2.X));

                            vB.X += invMassB * P12.X;
                            vB.Y += invMassB * P12.Y;
                            wB += invIB * ((cp1.rB.X * P1.Y - cp1.rB.Y * P1.X) + (cp2.rB.X * P2.Y - cp2.rB.Y * P2.X));
#endif
					        // Accumulate
					        cp1.normalImpulse = x.X;
					        cp2.normalImpulse = x.Y;

#if B2_DEBUG_SOLVER

			                float k_errorTol = 1e-3f;

					        // Postconditions
					        dv1 = vB + MathUtils.Cross(wB, cp1.rB) - vA - MathUtils.Cross(wA, cp1.rA);
					        dv2 = vB + MathUtils.Cross(wB, cp2.rB) - vA - MathUtils.Cross(wA, cp2.rA);

					        // Compute normal velocity
					        vn1 = Vector2.Dot(dv1, normal);
					        vn2 = Vector2.Dot(dv2, normal);

					        Debug.Assert(MathUtils.Abs(vn1 - cp1.velocityBias) < k_errorTol);
					        Debug.Assert(MathUtils.Abs(vn2 - cp2.velocityBias) < k_errorTol);
#endif
                            break;
				        }

				        //
				        // Case 2: vn1 = 0 and x2 = 0
				        //
				        //   0 = a11 * x1' + a12 * 0 + b1'
				        // vn2 = a21 * x1' + a22 * 0 + b2'
				        //
				        x.X = - cp1.normalMass * b.X;
				        x.Y = 0.0f;
				        vn1 = 0.0f;
				        vn2 = c.K.col1.Y * x.X + b.Y;

				        if (x.X >= 0.0f && vn2 >= 0.0f)
				        {
#if MATH_OVERLOADS
					        // Resubstitute for the incremental impulse
					        Vector2 d = x - a;

					        // Apply incremental impulse
					        Vector2 P1 = d.X * normal;
					        Vector2 P2 = d.Y * normal;
					        vA -= invMassA * (P1 + P2);
					        wA -= invIA * (MathUtils.Cross(cp1.rA, P1) + MathUtils.Cross(cp2.rA, P2));

					        vB += invMassB * (P1 + P2);
					        wB += invIB * (MathUtils.Cross(cp1.rB, P1) + MathUtils.Cross(cp2.rB, P2));
#else
                            // Resubstitute for the incremental impulse
                            Vector2 d = new Vector2(x.X - a.X, x.Y - a.Y);

                            // Apply incremental impulse
                            Vector2 P1 = new Vector2(d.X * normal.X, d.X * normal.Y);
                            Vector2 P2 = new Vector2(d.Y * normal.X, d.Y * normal.Y);
                            Vector2 P12 = new Vector2(P1.X + P2.X, P1.Y + P2.Y);

                            vA.X -= invMassA * P12.X;
                            vA.Y -= invMassA * P12.Y;
                            wA -= invIA * ((cp1.rA.X * P1.Y - cp1.rA.Y * P1.X) + (cp2.rA.X * P2.Y - cp2.rA.Y * P2.X));

                            vB.X += invMassB * P12.X;
                            vB.Y += invMassB * P12.Y;
                            wB += invIB * ((cp1.rB.X * P1.Y - cp1.rB.Y * P1.X) + (cp2.rB.X * P2.Y - cp2.rB.Y * P2.X));
#endif
					        // Accumulate
					        cp1.normalImpulse = x.X;
					        cp2.normalImpulse = x.Y;

        #if B2_DEBUG_SOLVER
					        // Postconditions
					        dv1 = vB + MathUtils.Cross(wB, cp1.rB) - vA - MathUtils.Cross(wA, cp1.rA);

					        // Compute normal velocity
					        vn1 = Vector2.Dot(dv1, normal);

					        Debug.Assert(MathUtils.Abs(vn1 - cp1.velocityBias) < k_errorTol);
        #endif
					        break;
				        }

				        //
				        // Case 3: vn2 = 0 and x1 = 0
				        //
				        // vn1 = a11 * 0 + a12 * x2' + b1'
				        //   0 = a21 * 0 + a22 * x2' + b2'
				        //
				        x.X = 0.0f;
				        x.Y = - cp2.normalMass * b.Y;
				        vn1 = c.K.col2.X * x.Y + b.X;
				        vn2 = 0.0f;

				        if (x.Y >= 0.0f && vn1 >= 0.0f)
				        {
#if MATH_OVERLOADS
					        // Resubstitute for the incremental impulse
					        Vector2 d = x - a;

					        // Apply incremental impulse
					        Vector2 P1 = d.X * normal;
					        Vector2 P2 = d.Y * normal;
					        vA -= invMassA * (P1 + P2);
					        wA -= invIA * (MathUtils.Cross(cp1.rA, P1) + MathUtils.Cross(cp2.rA, P2));

					        vB += invMassB * (P1 + P2);
					        wB += invIB * (MathUtils.Cross(cp1.rB, P1) + MathUtils.Cross(cp2.rB, P2));
#else
                            // Resubstitute for the incremental impulse
                            Vector2 d = new Vector2(x.X - a.X, x.Y - a.Y);

                            // Apply incremental impulse
                            Vector2 P1 = new Vector2(d.X * normal.X, d.X * normal.Y);
                            Vector2 P2 = new Vector2(d.Y * normal.X, d.Y * normal.Y);
                            Vector2 P12 = new Vector2(P1.X + P2.X, P1.Y + P2.Y);

                            vA.X -= invMassA * P12.X;
                            vA.Y -= invMassA * P12.Y;
                            wA -= invIA * ((cp1.rA.X * P1.Y - cp1.rA.Y * P1.X) + (cp2.rA.X * P2.Y - cp2.rA.Y * P2.X));

                            vB.X += invMassB * P12.X;
                            vB.Y += invMassB * P12.Y;
                            wB += invIB * ((cp1.rB.X * P1.Y - cp1.rB.Y * P1.X) + (cp2.rB.X * P2.Y - cp2.rB.Y * P2.X));
#endif
					        // Accumulate
					        cp1.normalImpulse = x.X;
					        cp2.normalImpulse = x.Y;

        #if B2_DEBUG_SOLVER
					        // Postconditions
					        dv2 = vB + MathUtils.Cross(wB, cp2.rB) - vA - MathUtils.Cross(wA, cp2.rA);

					        // Compute normal velocity
					        vn2 = Vector2.Dot(dv2, normal);

					        Debug.Assert(MathUtils.Abs(vn2 - cp2.velocityBias) < k_errorTol);
        #endif
					        break;
				        }

				        //
				        // Case 4: x1 = 0 and x2 = 0
				        //
				        // vn1 = b1
				        // vn2 = b2;
				        x.X = 0.0f;
				        x.Y = 0.0f;
				        vn1 = b.X;
				        vn2 = b.Y;

				        if (vn1 >= 0.0f && vn2 >= 0.0f )
				        {
#if MATH_OVERLOADS
					        // Resubstitute for the incremental impulse
					        Vector2 d = x - a;

					        // Apply incremental impulse
					        Vector2 P1 = d.X * normal;
					        Vector2 P2 = d.Y * normal;
					        vA -= invMassA * (P1 + P2);
					        wA -= invIA * (MathUtils.Cross(cp1.rA, P1) + MathUtils.Cross(cp2.rA, P2));

					        vB += invMassB * (P1 + P2);
					        wB += invIB * (MathUtils.Cross(cp1.rB, P1) + MathUtils.Cross(cp2.rB, P2));
#else
                            // Resubstitute for the incremental impulse
                            Vector2 d = new Vector2(x.X - a.X, x.Y - a.Y);

                            // Apply incremental impulse
                            Vector2 P1 = new Vector2(d.X * normal.X, d.X * normal.Y);
                            Vector2 P2 = new Vector2(d.Y * normal.X, d.Y * normal.Y);
                            Vector2 P12 = new Vector2(P1.X + P2.X, P1.Y + P2.Y);

                            vA.X -= invMassA * P12.X;
                            vA.Y -= invMassA * P12.Y;
                            wA -= invIA * ((cp1.rA.X * P1.Y - cp1.rA.Y * P1.X) + (cp2.rA.X * P2.Y - cp2.rA.Y * P2.X));

                            vB.X += invMassB * P12.X;
                            vB.Y += invMassB * P12.Y;
                            wB += invIB * ((cp1.rB.X * P1.Y - cp1.rB.Y * P1.X) + (cp2.rB.X * P2.Y - cp2.rB.Y * P2.X));
#endif
					        // Accumulate
					        cp1.normalImpulse = x.X;
					        cp2.normalImpulse = x.Y;

					        break;
				        }

				        // No solution, give up. This is hit sometimes, but it doesn't seem to matter.
				        break;
			        }

                    c.points[0] = cp1;
                    c.points[1] = cp2;
		        }

                _constraints[i] = c;

		        bodyA._linearVelocity = vA;
		        bodyA._angularVelocity = wA;
		        bodyB._linearVelocity = vB;
		        bodyB._angularVelocity = wB;
	        }
        }

        public void StoreImpulses()
        {
            for (int i = 0; i < _constraintCount; ++i)
	        {
		        ContactConstraint c = _constraints[i];
		        Manifold m = c.manifold;

		        for (int j = 0; j < c.pointCount; ++j)
		        {
                    var pj = m._points[j];
                    var cp = c.points[j];

                    pj.NormalImpulse = cp.normalImpulse;
                    pj.TangentImpulse = cp.tangentImpulse;

                    m._points[j] = pj;
		        }

                // TODO: look for better ways of doing this.
                c.manifold = m;
                _constraints[i] = c;
                _contacts[i]._manifold = m;
	        }
        }

        public bool SolvePositionConstraints(float baumgarte)
        {
            float minSeparation = 0.0f;

            for (int i = 0; i < _constraintCount; ++i)
	        {
		        ContactConstraint c = _constraints[i];

		        Body bodyA = c.bodyA;
		        Body bodyB = c.bodyB;

		        float invMassA = bodyA._mass * bodyA._invMass;
		        float invIA = bodyA._mass * bodyA._invI;
		        float invMassB = bodyB._mass * bodyB._invMass;
		        float invIB = bodyB._mass * bodyB._invI;

		        // Solve normal constraints
		        for (int j = 0; j < c.pointCount; ++j)
		        {
                    PositionSolverManifold psm = new PositionSolverManifold(ref c, j);
                    Vector2 normal = psm._normal;

                    Vector2 point = psm._point;
                    float separation = psm._separation;

			        Vector2 rA = point - bodyA._sweep.c;
			        Vector2 rB = point - bodyB._sweep.c;

			        // Track max constraint error.
			        minSeparation = Math.Min(minSeparation, separation);

			        // Prevent large corrections and allow slop.
                    float C = MathUtils.Clamp(baumgarte *  (separation + Settings.b2_linearSlop), -Settings.b2_maxLinearCorrection, 0.0f);

                    // Compute the effective mass.
                    float rnA = MathUtils.Cross(rA, normal);
                    float rnB = MathUtils.Cross(rB, normal);
                    float K = invMassA + invMassB + invIA * rnA * rnA + invIB * rnB * rnB;

                    // Compute normal impulse
                    float impulse = K > 0.0f ? -C / K : 0.0f;

#if MATH_OVERLOADS
			        Vector2 P = impulse * normal;

			        bodyA._sweep.c -= invMassA * P;
			        bodyA._sweep.a -= invIA * MathUtils.Cross(rA, P);

			        bodyB._sweep.c += invMassB * P;
			        bodyB._sweep.a += invIB * MathUtils.Cross(rB, P);
#else
                    Vector2 P = new Vector2(impulse * normal.X, impulse * normal.Y);

                    bodyA._sweep.c.X -= invMassA * P.X;
                    bodyA._sweep.c.Y -= invMassA * P.Y;
                    bodyA._sweep.a -= invIA * (rA.X * P.Y - rA.Y * P.X);

                    bodyB._sweep.c.X += invMassB * P.X;
                    bodyB._sweep.c.Y += invMassB * P.Y;
                    bodyB._sweep.a += invIB * (rB.X * P.Y - rB.Y * P.X);
#endif
                    bodyA.SynchronizeTransform();
			        bodyB.SynchronizeTransform();
		        }
	        }

	        // We can't expect minSpeparation >= -Settings.b2_linearSlop because we don't
	        // push the separation above -Settings.b2_linearSlop.
	        return minSeparation >= -1.5f * Settings.b2_linearSlop;
        }

        public ContactConstraint[] _constraints;
        public int _constraintCount; // collection can be bigger.
        private Contact[] _contacts;
    };

    internal struct PositionSolverManifold
    {
        internal PositionSolverManifold(ref ContactConstraint cc, int index)
        {
	        Debug.Assert(cc.pointCount > 0);

	        switch (cc.type)
	        {
	        case ManifoldType.Circles:
		        {
			        Vector2 pointA = cc.bodyA.GetWorldPoint(cc.localPoint);
			        Vector2 pointB = cc.bodyB.GetWorldPoint(cc.points[0].localPoint);
			        if (Vector2.DistanceSquared(pointA, pointB) > Settings.b2_epsilon * Settings.b2_epsilon)
			        {
				        _normal = pointB - pointA;
				        _normal.Normalize();
			        }
			        else
			        {
				        _normal = new Vector2(1.0f, 0.0f);
			        }

			        _point = 0.5f * (pointA + pointB);
			        _separation = Vector2.Dot(pointB - pointA, _normal) - cc.radius;
		        }
		        break;

	        case ManifoldType.FaceA:
		        {
			        _normal = cc.bodyA.GetWorldVector(cc.localNormal);
			        Vector2 planePoint = cc.bodyA.GetWorldPoint(cc.localPoint);

			        Vector2 clipPoint = cc.bodyB.GetWorldPoint(cc.points[index].localPoint);
			        _separation = Vector2.Dot(clipPoint - planePoint, _normal) - cc.radius;
			        _point = clipPoint;
		        }
		        break;

	        case ManifoldType.FaceB:
		        {
			        _normal = cc.bodyB.GetWorldVector(cc.localNormal);
			        Vector2 planePoint = cc.bodyB.GetWorldPoint(cc.localPoint);

                    Vector2 clipPoint = cc.bodyA.GetWorldPoint(cc.points[index].localPoint);
			        _separation = Vector2.Dot(clipPoint - planePoint, _normal) - cc.radius;
			        _point = clipPoint;

                    // Ensure normal points from A to B
			        _normal = -_normal;
		        }
		        break;
            default:
                _normal = Vector2.Zero;
                _point = Vector2.Zero;
                _separation = 0.0f;
                break;
	        }
        }

        internal Vector2 _normal;
        internal Vector2 _point;
        internal float _separation;
    };
}


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
{
    internal struct TOIConstraint
    {
	    public FixedArray2<Vector2> localPoints;
        public Vector2 localNormal;
        public Vector2 localPoint;
        public ManifoldType type;
        public float radius;
        public int pointCount;
        public Body bodyA;
        public Body bodyB;
    }

    internal struct TOISolverManifold
    {
        public TOISolverManifold(ref TOIConstraint cc, int index)
	    {
		    Debug.Assert(cc.pointCount > 0);

		    switch (cc.type)
		    {
		    case ManifoldType.Circles:
			    {
				    Vector2 pointA = cc.bodyA.GetWorldPoint(cc.localPoint);
				    Vector2 pointB = cc.bodyB.GetWorldPoint(cc.localPoints[0]);
                    if ((pointA - pointB).LengthSquared() > Settings.b2_epsilon * Settings.b2_epsilon)
				    {
					    normal = pointB - pointA;
					    normal.Normalize();
				    }
				    else
				    {
					    normal = new Vector2(1.0f, 0.0f);
				    }

				    point = 0.5f * (pointA + pointB);
				    separation = Vector2.Dot(pointB - pointA, normal) - cc.radius;
			    }
			    break;

		    case ManifoldType.FaceA:
			    {
				    normal = cc.bodyA.GetWorldVector(cc.localNormal);
				    Vector2 planePoint = cc.bodyA.GetWorldPoint(cc.localPoint);

				    Vector2 clipPoint = cc.bodyB.GetWorldPoint(cc.localPoints[index]);
				    separation = Vector2.Dot(clipPoint - planePoint, normal) - cc.radius;
				    point = clipPoint;
			    }
			    break;

		    case ManifoldType.FaceB:
			    {
				    normal = cc.bodyB.GetWorldVector(cc.localNormal);
				    Vector2 planePoint = cc.bodyB.GetWorldPoint(cc.localPoint);

				    Vector2 clipPoint = cc.bodyA.GetWorldPoint(cc.localPoints[index]);
				    separation = Vector2.Dot(clipPoint - planePoint, normal) - cc.radius;
				    point = clipPoint;

				    // Ensure normal points from A to B
				    normal = -normal;
			    }
			    break;
            default:
                normal = Vector2.UnitY;
                point = Vector2.Zero;
                separation = 0.0f;
                break;
		    }
	    }

        internal Vector2 normal;
        internal Vector2 point;
        internal float separation;
    };

    internal class TOISolver
    {
        public TOISolver() { }

        public void Initialize(Contact[] contacts, int count, Body toiBody)
        {
            _count = count;
            _toiBody = toiBody;
            if (_constraints.Length < _count)
                _constraints = new TOIConstraint[Math.Max(_constraints.Length * 2, _count)];

            for (int i = 0; i < _count; ++i)
            {
                Contact contact = contacts[i];

                Fixture fixtureA = contact.GetFixtureA();
                Fixture fixtureB = contact.GetFixtureB();
                Shape shapeA = fixtureA.GetShape();
                Shape shapeB = fixtureB.GetShape();
                float radiusA = shapeA._radius;
                float radiusB = shapeB._radius;
                Body bodyA = fixtureA.GetBody();
                Body bodyB = fixtureB.GetBody();
                Manifold manifold;
                contact.GetManifold(out manifold);

                Debug.Assert(manifold._pointCount > 0);

                TOIConstraint constraint = _constraints[i];
                constraint.bodyA = bodyA;
                constraint.bodyB = bodyB;
                constraint.localNormal = manifold._localNormal;
                constraint.localPoint = manifold._localPoint;
                constraint.type = manifold._type;
                constraint.pointCount = manifold._pointCount;
                constraint.radius = radiusA + radiusB;

                for (int j = 0; j < constraint.pointCount; ++j)
                {
                    constraint.localPoints[j] = manifold._points[j].LocalPoint;
                }

                _constraints[i] = constraint;
            }
        }

        // Perform one solver iteration. Returns true if converged.
        public bool Solve(float baumgarte)
        {
            float minSeparation = 0.0f;

            for (int i = 0; i < _count; ++i)
            {
                TOIConstraint c = _constraints[i];
                Body bodyA = c.bodyA;
                Body bodyB = c.bodyB;

                float massA = bodyA._mass;
                float massB = bodyB._mass;

                // Only the TOI body should move.
                if (bodyA == _toiBody)
                {
                    massB = 0.0f;
                }
                else
                {
                    massA = 0.0f;
                }

                float invMassA = massA * bodyA._invMass;
                float invIA = massA * bodyA._invI;
                float invMassB = massB * bodyB._invMass;
                float invIB = massB * bodyB._invI;

                // Solve normal constraints
                for (int j = 0; j < c.pointCount; ++j)
                {
                    TOISolverManifold psm = new TOISolverManifold(ref c, j);

                    Vector2 normal = psm.normal;
                    Vector2 point = psm.point;
                    float separation = psm.separation;

                    Vector2 rA = point - bodyA._sweep.c;
                    Vector2 rB = point - bodyB._sweep.c;

                    // Track max constraint error.
                    minSeparation = Math.Min(minSeparation, separation);

                    // Prevent large corrections and allow slop.
                    float C = MathUtils.Clamp(baumgarte * (separation + Settings.b2_linearSlop), -Settings.b2_maxLinearCorrection, 0.0f);

                    // Compute the effective mass.
                    float rnA = MathUtils.Cross(rA, normal);
                    float rnB = MathUtils.Cross(rB, normal);
                    float K = invMassA + invMassB + invIA * rnA * rnA + invIB * rnB * rnB;

                    // Compute normal impulse
                    float impulse = K > 0.0f ? -C / K : 0.0f;

                    Vector2 P = impulse * normal;

                    bodyA._sweep.c -= invMassA * P;
                    bodyA._sweep.a -= invIA * MathUtils.Cross(rA, P);
                    bodyA.SynchronizeTransform();

                    bodyB._sweep.c += invMassB * P;
                    bodyB._sweep.a += invIB * MathUtils.Cross(rB, P);
                    bodyB.SynchronizeTransform();
                }
            }

            // We can't expect minSpeparation >= -b2_linearSlop because we don't
            // push the separation above -b2_linearSlop.
            return minSeparation >= -1.5f * Settings.b2_linearSlop;
        }

	    TOIConstraint[] _constraints = new TOIConstraint[8];
	    int _count;
	    Body _toiBody;
    }
}