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
{
    /// This holds contact filtering data.
    public struct Filter
    {
        /// The collision category bits. Normally you would just set one bit.
        public UInt16 categoryBits;

        /// The collision mask bits. This states the categories that this
        /// Shape would accept for collision.
        public UInt16 maskBits;

        /// Collision groups allow a certain group of objects to never collide (negative)
        /// or always collide (positive). Zero means no collision group. Non-zero group
        /// filtering always wins against the mask bits.
        public Int16 groupIndex;
    };

    /// A fixture definition is used to create a fixture. This class defines an
    /// abstract fixture definition. You can reuse fixture definitions safely.
    public class FixtureDef
    {
	    /// The constructor sets the default fixture definition values.
        public FixtureDef()
	    {
		    shape = null;
		    userData = null;
		    friction = 0.2f;
		    restitution = 0.0f;
		    density = 0.0f;
		    filter.categoryBits = 0x0001;
		    filter.maskBits = 0xFFFF;
		    filter.groupIndex = 0;
		    isSensor = false;
	    }

	    /// The Shape, this must be set. The Shape will be cloned, so you
	    /// can create the Shape on the stack.
        public Shape shape;

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

	    /// The friction coefficient, usually in the range [0,1].
        public float friction;

	    /// The restitution (elasticity) usually in the range [0,1].
        public float restitution;

	    /// The density, usually in kg/m^2.
        public float density;

	    /// A sensor Shape collects contact information but never generates a collision
	    /// response.
        public bool isSensor;

	    /// Contact filtering data.
        public Filter filter;
    };

    /// This proxy is used internally to connect fixtures to the broad-phase.
    public class FixtureProxy
    {
        public AABB aabb;
        public Fixture fixture;
        public int childIndex;
        public int proxyId;
    };

    /// A fixture is used to attach a Shape to a body for collision detection. A fixture
    /// inherits its transform from its parent. Fixtures hold additional non-geometric data
    /// such as friction, collision filters, etc.
    /// Fixtures are created via Body.CreateFixture.
    /// @warning you cannot reuse fixtures.
    public class Fixture
    {
	    /// Get the type of the child Shape. You can use this to down cast to the concrete Shape.
	    /// @return the Shape type.
	    public ShapeType ShapeType
        {
            get { return _shape.ShapeType; }
        }

	    /// Get the child Shape. You can modify the child Shape, however you should not change the
	    /// number of vertices because this will crash some collision caching mechanisms.
	    public Shape GetShape()
        {
            return _shape;
        }

	    /// Is this fixture a sensor (non-solid)?
	    /// @return the true if the Shape is a sensor.
	    public bool IsSensor()
        {
            return _isSensor;
        }

	    /// Set if this fixture is a sensor.
	    public void SetSensor(bool sensor)
        {
	        _isSensor = sensor;
        }

        /// Set the contact filtering data. This will not update contacts until the next time
        /// step when either parent body is active and awake.
	    public void SetFilterData(ref Filter filter)
        {
            _filter = filter;

	        if (_body == null)
	        {
		        return;
	        }

	        // Flag associated contacts for filtering.
	        ContactEdge edge = _body.GetContactList();
	        while (edge != null)
	        {
		        Contact contact = edge.Contact;
		        Fixture fixtureA = contact.GetFixtureA();
		        Fixture fixtureB = contact.GetFixtureB();
		        if (fixtureA == this || fixtureB == this)
		        {
			        contact.FlagForFiltering();
		        }

                edge = edge.Next;
	        }
        }

	    /// Get the contact filtering data.
	    public void GetFilterData(out Filter filter)
        {
            filter = _filter;
        }

	    /// Get the parent body of this fixture. This is null if the fixture is not attached.
	    /// @return the parent body.
	    public Body GetBody()
        {
            return _body;
        }

	    /// Get the next fixture in the parent body's fixture list.
	    /// @return the next Shape.
	    public Fixture GetNext()
        {
            return _next;
        }

	    /// Get the user data that was assigned in the fixture definition. Use this to
	    /// store your application specific data.
	    public object GetUserData()
        {
            return _userData;
        }

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

        public void SetDensity(float density)
        {
	        Debug.Assert(MathUtils.IsValid(density) && density >= 0.0f);
	        _density = density;
        }

        public float GetDensity()
        {
	        return _density;
        }

	    /// Test a point for containment in this fixture.
	    /// @param xf the Shape world transform.
	    /// @param p a point in world coordinates.
	    public bool TestPoint(Vector2 p)
        {
            Transform xf;
            _body.GetTransform(out xf);
            return _shape.TestPoint(ref xf, p);
        }

        /// Cast a ray against this Shape.
	    /// @param output the ray-cast results.
	    /// @param input the ray-cast input parameters.
        public bool RayCast(out RayCastOutput output, ref RayCastInput input, int childIndex)
        {
            Transform xf;
            _body.GetTransform(out xf);
            return _shape.RayCast(out output, ref input, ref xf, childIndex);
        }

	    /// Get the mass data for this fixture. The mass data is based on the density and
	    /// the Shape. The rotational inertia is about the Shape's origin.
	    public void GetMassData(out MassData massData)
        {
            _shape.ComputeMass(out massData, _density);
        }

	    /// Get the coefficient of friction.
	    public float GetFriction()
        {
            return _friction;
        }

	    /// Set the coefficient of friction.
	    public void SetFriction(float friction)
        {
            _friction = friction;
        }

	    /// Get the coefficient of restitution.
	    public float GetRestitution()
        {
            return _restitution;
        }

	    /// Set the coefficient of restitution.
	    public void SetRestitution(float restitution)
        {
            _restitution = restitution;
        }

        /// Get the fixture's AABB. This AABB may be enlarge and/or stale.
	    /// If you need a more accurate AABB, compute it using the Shape and
	    /// the body transform.
	    public void GetAABB(out AABB aabb, int childIndex)
        {
            Debug.Assert(0 <= childIndex && childIndex < _proxyCount);
            aabb = _proxies[childIndex].aabb;
        }

	    internal Fixture()
        {
            _userData = null;
	        _body = null;
	        _next = null;
	        _proxyId = BroadPhase.NullProxy;
	        _shape = null;
        }

	    // We need separation create/destroy functions from the constructor/destructor because
	    // the destructor cannot access the allocator or broad-phase (no destructor arguments allowed by C++).
	    internal void Create(Body body, FixtureDef def)
        {
            _userData = def.userData;
	        _friction = def.friction;
	        _restitution = def.restitution;

	        _body = body;
	        _next = null;

	        _filter = def.filter;

	        _isSensor = def.isSensor;

	        _shape = def.shape.Clone();

            // Reserve proxy space
	        int childCount = _shape.GetChildCount();
	        _proxies = new FixtureProxy[childCount];
	        for (int i = 0; i < childCount; ++i)
	        {
                _proxies[i] = new FixtureProxy();
		        _proxies[i].fixture = null;
		        _proxies[i].proxyId = BroadPhase.NullProxy;
	        }
	        _proxyCount = 0;

            _density = def.density;
        }

	    internal void Destroy()
        {
            // The proxies must be destroyed before calling this.
            Debug.Assert(_proxyCount == 0);

            // Free the proxy array.
            _proxies = null;

            _shape = null;
        }

        // These support body activation/deactivation.
	    internal void CreateProxies(BroadPhase broadPhase, ref Transform xf)
        {
            Debug.Assert(_proxyCount == 0);

            // Create proxies in the broad-phase.
            _proxyCount = _shape.GetChildCount();

            for (int i = 0; i < _proxyCount; ++i)
            {
                FixtureProxy proxy = _proxies[i];
                _shape.ComputeAABB(out proxy.aabb, ref xf, i);
                proxy.fixture = this;
                proxy.childIndex = i;
                proxy.proxyId = broadPhase.CreateProxy(ref proxy.aabb, proxy);

                _proxies[i] = proxy;
            }
        }

        internal void DestroyProxies(BroadPhase broadPhase)
        {
	        // Destroy proxies in the broad-phase.
	        for (int i = 0; i < _proxyCount; ++i)
	        {
                broadPhase.DestroyProxy(_proxies[i].proxyId);
		        _proxies[i].proxyId = BroadPhase.NullProxy;
	        }

	        _proxyCount = 0;
        }

        internal void Synchronize(BroadPhase broadPhase, ref Transform transform1, ref Transform transform2)
        {
            if (_proxyCount == 0)
            {
                return;
            }

            for (int i = 0; i < _proxyCount; ++i)
            {
                FixtureProxy proxy = _proxies[i];

                // Compute an AABB that covers the swept Shape (may miss some rotation effect).
                AABB aabb1, aabb2;
                _shape.ComputeAABB(out aabb1, ref transform1, proxy.childIndex);
                _shape.ComputeAABB(out aabb2, ref transform2, proxy.childIndex);

                proxy.aabb.Combine(ref aabb1, ref aabb2);

                Vector2 displacement = transform2.Position - transform1.Position;

                broadPhase.MoveProxy(proxy.proxyId, ref proxy.aabb, displacement);
            }

        }

        internal AABB _aabb;

        internal FixtureProxy[] _proxies;
        internal int _proxyCount;

        internal float _density;

	    internal Fixture _next;
	    internal Body _body;

	    internal Shape _shape;

	    internal float _friction;
	    internal float _restitution;

	    internal int _proxyId;
	    internal Filter _filter;

	    internal bool _isSensor;

	    internal object _userData;
    };

}


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
{
    /// This is an internal class.
    class Island
    {
        public Island() { }

        public void Reset(int bodyCapacity, int contactCapacity, int jointCapacity, IContactListener listener)
        {
            _bodyCapacity = bodyCapacity;
	        _contactCapacity = contactCapacity;
	        _jointCapacity	 = jointCapacity;
	        _bodyCount = 0;
	        _jointCount = 0;

	        _listener = listener;

            if (_bodies == null || _bodies.Length < bodyCapacity)
            {
                _bodies = new Body[bodyCapacity];
            }

            if (_contacts == null || _contacts.Length < contactCapacity)
            {
                _contacts = new Contact[contactCapacity * 2];
            }

            if (_joints == null || _joints.Length < jointCapacity)
            {
                _joints = new Joint[jointCapacity * 2];
            }
        }

	    public void Clear()
	    {
		    _bodyCount = 0;
	        _contactCount = 0;
		    _jointCount = 0;
	    }

	    public void Solve(ref TimeStep step, Vector2 gravity, bool allowSleep)
        {
            // Integrate velocities and apply damping.
	        for (int i = 0; i < _bodyCount; ++i)
	        {
		        Body b = _bodies[i];

                if (b.GetType() != BodyType.Dynamic)
                {
                    continue;
                }

		        // Integrate velocities.
		        b._linearVelocity += step.dt * (gravity + b._invMass * b._force);
		        b._angularVelocity += step.dt * b._invI * b._torque;

		        // Apply damping.
		        // ODE: dv/dt + c * v = 0
		        // Solution: v(t) = v0 * exp(-c * t)
		        // Time step: v(t + dt) = v0 * exp(-c * (t + dt)) = v0 * exp(-c * t) * exp(-c * dt) = v * exp(-c * dt)
		        // v2 = exp(-c * dt) * v1
		        // Taylor expansion:
		        // v2 = (1.0f - c * dt) * v1
		        b._linearVelocity *= MathUtils.Clamp(1.0f - step.dt * b._linearDamping, 0.0f, 1.0f);
		        b._angularVelocity *= MathUtils.Clamp(1.0f - step.dt * b._angularDamping, 0.0f, 1.0f);
	        }

            // Partition contacts so that contacts with static bodies are solved last.
	        int i1 = -1;
	        for (int i2 = 0; i2 < _contactCount; ++i2)
	        {
		        Fixture fixtureA = _contacts[i2].GetFixtureA();
		        Fixture fixtureB = _contacts[i2].GetFixtureB();
		        Body bodyA = fixtureA.GetBody();
		        Body bodyB = fixtureB.GetBody();
                bool nonStatic = bodyA.GetType() != BodyType.Static && bodyB.GetType() != BodyType.Static;
		        if (nonStatic)
		        {
			        ++i1;
			        //b2Swap(_contacts[i1], _contacts[i2]);
                    Contact temp = _contacts[i1];
                    _contacts[i1] = _contacts[i2];
                    _contacts[i2] = temp;
		        }
	        }

	        // Initialize velocity constraints.
            _contactSolver.Reset(_contacts, _contactCount, step.dtRatio);
	        _contactSolver.WarmStart();

	        for (int i = 0; i < _jointCount; ++i)
	        {
		        _joints[i].InitVelocityConstraints(ref step);
	        }

	        // Solve velocity constraints.
	        for (int i = 0; i < step.velocityIterations; ++i)
	        {
		        for (int j = 0; j < _jointCount; ++j)
		        {
			        _joints[j].SolveVelocityConstraints(ref step);
		        }

                _contactSolver.SolveVelocityConstraints();
	        }

	        // Post-solve (store impulses for warm starting).
            _contactSolver.StoreImpulses();

	        // Integrate positions.
	        for (int i = 0; i < _bodyCount; ++i)
	        {
		        Body b = _bodies[i];

                if (b.GetType() == BodyType.Static)
                {
                    continue;
                }

		        // Check for large velocities.
		        Vector2 translation = step.dt * b._linearVelocity;
		        if (Vector2.Dot(translation, translation) > Settings.b2_maxTranslationSquared)
		        {
			        float ratio = Settings.b2_maxTranslation / translation.Length();
                    b._linearVelocity *= ratio;
		        }

		        float rotation = step.dt * b._angularVelocity;
		        if (rotation * rotation > Settings.b2_maxRotationSquared)
		        {
                    float ratio = Settings.b2_maxRotation / Math.Abs(rotation);
                    b._angularVelocity *= ratio;
		        }

		        // Store positions for continuous collision.
		        b._sweep.c0 = b._sweep.c;
		        b._sweep.a0 = b._sweep.a;

		        // Integrate
		        b._sweep.c += step.dt * b._linearVelocity;
		        b._sweep.a += step.dt * b._angularVelocity;

		        // Compute new transform
		        b.SynchronizeTransform();

		        // Note: shapes are synchronized later.
	        }

	        // Iterate over constraints.
            for (int i = 0; i < step.positionIterations; ++i)
	        {
                bool contactsOkay = _contactSolver.SolvePositionConstraints(Settings.b2_contactBaumgarte);

		        bool jointsOkay = true;
		        for (int j = 0; j < _jointCount; ++j)
		        {
			        bool jointOkay = _joints[j].SolvePositionConstraints(Settings.b2_contactBaumgarte);
			        jointsOkay = jointsOkay && jointOkay;
		        }

		        if (contactsOkay && jointsOkay)
		        {
			        // Exit early if the position errors are small.
			        break;
		        }
	        }

            Report(_contactSolver._constraints);

	        if (allowSleep)
	        {
		        float minSleepTime = Settings.b2_maxFloat;

		        const float linTolSqr = Settings.b2_linearSleepTolerance * Settings.b2_linearSleepTolerance;
		        const float angTolSqr = Settings.b2_angularSleepTolerance * Settings.b2_angularSleepTolerance;

		        for (int i = 0; i < _bodyCount; ++i)
		        {
			        Body b = _bodies[i];
			        if (b.GetType() == BodyType.Static)
			        {
				        continue;
			        }

			        if ((b._flags & BodyFlags.AutoSleep) == 0)
			        {
				        b._sleepTime = 0.0f;
				        minSleepTime = 0.0f;
			        }

			        if ((b._flags & BodyFlags.AutoSleep) == 0 ||
				        b._angularVelocity * b._angularVelocity > angTolSqr ||
				        Vector2.Dot(b._linearVelocity, b._linearVelocity) > linTolSqr)
			        {
				        b._sleepTime = 0.0f;
				        minSleepTime = 0.0f;
			        }
			        else
			        {
				        b._sleepTime += step.dt;
				        minSleepTime = Math.Min(minSleepTime, b._sleepTime);
			        }
		        }

		        if (minSleepTime >= Settings.b2_timeToSleep)
		        {
			        for (int i = 0; i < _bodyCount; ++i)
			        {
				        Body b = _bodies[i];
                        b.SetAwake(false);
			        }
		        }
	        }
        }

	    public void Add(Body body)
	    {
		    Debug.Assert(_bodyCount < _bodyCapacity);
		    body._islandIndex = _bodyCount;
		    _bodies[_bodyCount++] = body;
	    }

	    public void Add(Contact contact)
	    {
            Debug.Assert(_contactCount < _contactCapacity);
            _contacts[_contactCount++] = contact;
        }

	    public void Add(Joint joint)
	    {
		    Debug.Assert(_jointCount < _jointCapacity);
		    _joints[_jointCount++] = joint;
	    }

	    public void Report(ContactConstraint[] constraints)
        {
            if (_listener == null)
	        {
		        return;
	        }

            for (int i = 0; i < _contactCount; ++i)
	        {
		        Contact c = _contacts[i];

                ContactConstraint cc = constraints[i];

		        ContactImpulse impulse = new ContactImpulse();
		        for (int j = 0; j < cc.pointCount; ++j)
		        {
			        impulse.normalImpulses[j] = cc.points[j].normalImpulse;
			        impulse.tangentImpulses[j] = cc.points[j].tangentImpulse;
		        }

		        _listener.PostSolve(c, ref impulse);
	        }
        }

        private ContactSolver _contactSolver = new ContactSolver();
	    public IContactListener _listener;

	    public Body[] _bodies;
        public Contact[] _contacts;
	    public Joint[] _joints;

	    public int _bodyCount;
        public int _contactCount;
	    public int _jointCount;

	    public int _bodyCapacity;
	    public int _contactCapacity;
	    public int _jointCapacity;

	    public int _positionIterationCount;
    };
}

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

namespace Box2D.XNA
{
    /// Distance joint definition. This requires defining an
    /// anchor point on both bodies and the non-zero length of the
    /// distance joint. The definition uses local anchor points
    /// so that the initial configuration can violate the constraint
    /// slightly. This helps when saving and loading a game.
    /// @warning Do not use a zero or short length.
    public class DistanceJointDef : JointDef
    {
	    public DistanceJointDef()
	    {
		    type = JointType.Distance;
		    localAnchorA = new Vector2(0.0f, 0.0f);
		    localAnchorB = new Vector2(0.0f, 0.0f);
		    length = 1.0f;
		    frequencyHz = 0.0f;
		    dampingRatio = 0.0f;
	    }

	    /// Initialize the bodies, anchors, and length using the world
	    /// anchors.
        // 1-D rained system
        // m (v2 - v1) = lambda
        // v2 + (beta/h) * x1 + gamma * lambda = 0, gamma has units of inverse mass.
        // x2 = x1 + h * v2

        // 1-D mass-damper-spring system
        // m (v2 - v1) + h * d * v2 + h * k *

        // C = norm(p2 - p1) - L
        // u = (p2 - p1) / norm(p2 - p1)
        // Cdot = dot(u, v2 + cross(w2, r2) - v1 - cross(w1, r1))
        // J = [-u -cross(r1, u) u cross(r2, u)]
        // K = J * invM * JT
        //   = invMass1 + invI1 * cross(r1, u)^2 + invMass2 + invI2 * cross(r2, u)^2
	    public void Initialize(Body b1, Body b2,
					    Vector2 anchor1, Vector2 anchor2)
        {
	        bodyA = b1;
	        bodyB = b2;
	        localAnchorA = bodyA.GetLocalPoint(anchor1);
	        localAnchorB = bodyB.GetLocalPoint(anchor2);
	        Vector2 d = anchor2 - anchor1;
	        length = d.Length();
        }

	    /// The local anchor point relative to body1's origin.
	    public Vector2 localAnchorA;

	    /// The local anchor point relative to body2's origin.
	    public Vector2 localAnchorB;

        /// The natural length between the anchor points.
	    public float length;

        /// The mass-spring-damper frequency in Hertz.
	    public float frequencyHz;

	    /// The damping ratio. 0 = no damping, 1 = critical damping.
	    public float dampingRatio;
    };

    /// A distance joint rains two points on two bodies
    /// to remain at a fixed distance from each other. You can view
    /// this as a massless, rigid rod.
    public class DistanceJoint : Joint
    {
        /// Set/get the natural length.
        /// Manipulating the length can lead to non-physical behavior when the frequency is zero.
        public void SetLength(float length)
        {
            _length = length;
        }

        public float GetLength()
        {
            return _length;
        }

	    // Set/get frequency in Hz.
        public void SetFrequency(float hz)
        {
            _frequencyHz = hz;
        }

        public float GetFrequency()
        {
            return _frequencyHz;
        }

	    // Set/get damping ratio.
        public void SetDampingRatio(float ratio)
        {
            _dampingRatio = ratio;
        }

        public float GetDampingRatio()
        {
            return _dampingRatio;
        }

	    public override Vector2 GetAnchorA()
        {
            return _bodyA.GetWorldPoint(_localAnchor1);
        }

        public override Vector2 GetAnchorB()
        {
	        return _bodyB.GetWorldPoint(_localAnchor2);
        }

        public override Vector2 GetReactionForce(float inv_dt)
        {
	        Vector2 F = (inv_dt * _impulse) * _u;
	        return F;
        }

        public override float GetReactionTorque(float inv_dt)
        {
	        return 0.0f;
        }

	    internal DistanceJoint(DistanceJointDef def)
            : base(def)
        {
	        _localAnchor1 = def.localAnchorA;
	        _localAnchor2 = def.localAnchorB;
	        _length = def.length;
	        _frequencyHz = def.frequencyHz;
	        _dampingRatio = def.dampingRatio;
	        _impulse = 0.0f;
	        _gamma = 0.0f;
	        _bias = 0.0f;
        }

        internal override void InitVelocityConstraints(ref TimeStep step)
        {
	        Body b1 = _bodyA;
	        Body b2 = _bodyB;

            Transform xf1, xf2;
            b1.GetTransform(out xf1);
            b2.GetTransform(out xf2);

	        // Compute the effective mass matrix.
            Vector2 r1 = MathUtils.Multiply(ref xf1.R, _localAnchor1 - b1.GetLocalCenter());
            Vector2 r2 = MathUtils.Multiply(ref xf2.R, _localAnchor2 - b2.GetLocalCenter());
	        _u = b2._sweep.c + r2 - b1._sweep.c - r1;

	        // Handle singularity.
	        float length = _u.Length();
	        if (length > Settings.b2_linearSlop)
	        {
		        _u *= 1.0f / length;
	        }
	        else
	        {
		        _u = new Vector2(0.0f, 0.0f);
	        }

	        float cr1u = MathUtils.Cross(r1, _u);
	        float cr2u = MathUtils.Cross(r2, _u);
	        float invMass = b1._invMass + b1._invI * cr1u * cr1u + b2._invMass + b2._invI * cr2u * cr2u;
	        Debug.Assert(invMass > Settings.b2_epsilon);
            _mass = invMass != 0.0f ? 1.0f / invMass : 0.0f;

	        if (_frequencyHz > 0.0f)
	        {
		        float C = length - _length;

		        // Frequency
		        float omega = 2.0f * Settings.b2_pi * _frequencyHz;

		        // Damping coefficient
		        float d = 2.0f * _mass * _dampingRatio * omega;

		        // Spring stiffness
		        float k = _mass * omega * omega;

		        // magic formulas
                _gamma = step.dt * (d + step.dt * k);
                _gamma = _gamma != 0.0f ? 1.0f / _gamma : 0.0f;
                _bias = C * step.dt * k * _gamma;

                _mass = invMass + _gamma;
                _mass = _mass != 0.0f ? 1.0f / _mass : 0.0f;
	        }

	        if (step.warmStarting)
	        {
		        // Scale the impulse to support a variable time step.
		        _impulse *= step.dtRatio;

		        Vector2 P = _impulse * _u;
		        b1._linearVelocity -= b1._invMass * P;
		        b1._angularVelocity -= b1._invI * MathUtils.Cross(r1, P);
		        b2._linearVelocity += b2._invMass * P;
		        b2._angularVelocity += b2._invI * MathUtils.Cross(r2, P);
	        }
	        else
	        {
		        _impulse = 0.0f;
	        }
        }

        internal override void SolveVelocityConstraints(ref TimeStep step)
        {
	        Body b1 = _bodyA;
	        Body b2 = _bodyB;

            Transform xf1, xf2;
            b1.GetTransform(out xf1);
            b2.GetTransform(out xf2);

            Vector2 r1 = MathUtils.Multiply(ref xf1.R, _localAnchor1 - b1.GetLocalCenter());
            Vector2 r2 = MathUtils.Multiply(ref xf2.R, _localAnchor2 - b2.GetLocalCenter());

	        // Cdot = dot(u, v + cross(w, r))
	        Vector2 v1 = b1._linearVelocity + MathUtils.Cross(b1._angularVelocity, r1);
	        Vector2 v2 = b2._linearVelocity + MathUtils.Cross(b2._angularVelocity, r2);
	        float Cdot = Vector2.Dot(_u, v2 - v1);

	        float impulse = -_mass * (Cdot + _bias + _gamma * _impulse);
	        _impulse += impulse;

	        Vector2 P = impulse * _u;
	        b1._linearVelocity -= b1._invMass * P;
	        b1._angularVelocity -= b1._invI * MathUtils.Cross(r1, P);
	        b2._linearVelocity += b2._invMass * P;
	        b2._angularVelocity += b2._invI * MathUtils.Cross(r2, P);
        }

        internal override bool SolvePositionConstraints(float baumgarte)
        {
	        if (_frequencyHz > 0.0f)
	        {
		        // There is no position correction for soft distance constraints.
		        return true;
	        }

	        Body b1 = _bodyA;
	        Body b2 = _bodyB;

            Transform xf1, xf2;
            b1.GetTransform(out xf1);
            b2.GetTransform(out xf2);

	        Vector2 r1 = MathUtils.Multiply(ref xf1.R, _localAnchor1 - b1.GetLocalCenter());
	        Vector2 r2 = MathUtils.Multiply(ref xf2.R, _localAnchor2 - b2.GetLocalCenter());

	        Vector2 d = b2._sweep.c + r2 - b1._sweep.c - r1;

	        float length = d.Length();

            if (length == 0.0f)
                return true;

            d /= length;
	        float C = length - _length;
	        C = MathUtils.Clamp(C, -Settings.b2_maxLinearCorrection, Settings.b2_maxLinearCorrection);

	        float impulse = -_mass * C;
	        _u = d;
	        Vector2 P = impulse * _u;

	        b1._sweep.c -= b1._invMass * P;
	        b1._sweep.a -= b1._invI * MathUtils.Cross(r1, P);
	        b2._sweep.c += b2._invMass * P;
	        b2._sweep.a += b2._invI * MathUtils.Cross(r2, P);

	        b1.SynchronizeTransform();
	        b2.SynchronizeTransform();

	        return Math.Abs(C) < Settings.b2_linearSlop;
        }

	    internal Vector2 _localAnchor1;
	    internal Vector2 _localAnchor2;
	    internal Vector2 _u;
	    internal float _frequencyHz;
	    internal float _dampingRatio;
	    internal float _gamma;
	    internal float _bias;
	    internal float _impulse;
	    internal float _mass;
	    internal float _length;
    };
}


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

using Microsoft.Xna.Framework;

namespace Box2D.XNA
{
    /// Friction joint definition.
    public class FrictionJointDef : JointDef
    {
        public FrictionJointDef()
	    {
		    type = JointType.Friction;
	    }

        // Point-to-point constraint
        // Cdot = v2 - v1
        //      = v2 + cross(w2, r2) - v1 - cross(w1, r1)
        // J = [-I -r1_skew I r2_skew ]
        // Identity used:
        // w k % (rx i + ry j) = w * (-ry i + rx j)

        // Angle constraint
        // Cdot = w2 - w1
        // J = [0 0 -1 0 0 1]
        // K = invI1 + invI2
	    public void Initialize(Body b1, Body b2,
					    Vector2 anchor1, Vector2 anchor2)
        {
	        bodyA = b1;
	        bodyB = b2;
	        localAnchorA = bodyA.GetLocalPoint(anchor1);
	        localAnchorB = bodyB.GetLocalPoint(anchor2);
        }

	    /// The local anchor point relative to body1's origin.
	    public Vector2 localAnchorA;

	    /// The local anchor point relative to body2's origin.
	    public Vector2 localAnchorB;

        /// The maximum friction force in N.
	    public float maxForce;

        /// The maximum friction torque in N-m.
	    public float maxTorque;
    };

    /// Friction joint. This is used for top-down friction.
    /// It provides 2D translational friction and angular friction.
    public class FrictionJoint : Joint
    {
	    public override Vector2 GetAnchorA()
        {
            return _bodyA.GetWorldPoint(_localAnchor1);
        }

        public override Vector2 GetAnchorB()
        {
	        return _bodyB.GetWorldPoint(_localAnchor2);
        }

        public override Vector2 GetReactionForce(float inv_dt)
        {
	        Vector2 F = (inv_dt * _linearImpulse);
	        return F;
        }

        public override float GetReactionTorque(float inv_dt)
        {
            float F = (inv_dt * _angularImpulse);
            return F;
        }

        public void SetMaxForce(float force)
        {
            _maxForce = force;
        }

        public float GetMaxForce()
        {
            return _maxForce;
        }

        public void SetMaxTorque(float torque)
        {
            _maxTorque = torque;
        }

        public float GetMaxTorque()
        {
            return _maxTorque;
        }

	    internal FrictionJoint(FrictionJointDef def)
            : base(def)
        {
	        _localAnchor1 = def.localAnchorA;
	        _localAnchor2 = def.localAnchorB;
            _maxForce = def.maxForce;
            _maxTorque = def.maxTorque;
        }

        internal override void InitVelocityConstraints(ref TimeStep step)
        {
	        Body bA = _bodyA;
	        Body bB = _bodyB;

            Transform xfA, xfB;
            bA.GetTransform(out xfA);
            bB.GetTransform(out xfB);

	        // Compute the effective mass matrix.
            Vector2 rA = MathUtils.Multiply(ref xfA.R, _localAnchor1 - bA.GetLocalCenter());
            Vector2 rB = MathUtils.Multiply(ref xfB.R, _localAnchor2 - bB.GetLocalCenter());

            // J = [-I -r1_skew I r2_skew]
	        //     [ 0       -1 0       1]
	        // r_skew = [-ry; rx]

	        // Matlab
	        // K = [ mA+r1y^2*iA+mB+r2y^2*iB,  -r1y*iA*r1x-r2y*iB*r2x,          -r1y*iA-r2y*iB]
	        //     [  -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB,           r1x*iA+r2x*iB]
	        //     [          -r1y*iA-r2y*iB,           r1x*iA+r2x*iB,                   iA+iB]

	        float mA = bA._invMass, mB = bB._invMass;
	        float iA = bA._invI, iB = bB._invI;

	        Mat22 K1 = new Mat22();
	        K1.col1.X = mA + mB;	K1.col2.X = 0.0f;
	        K1.col1.Y = 0.0f;		K1.col2.Y = mA + mB;

            Mat22 K2 = new Mat22();
	        K2.col1.X =  iA * rA.Y * rA.Y;	K2.col2.X = -iA * rA.X * rA.Y;
	        K2.col1.Y = -iA * rA.X * rA.Y;	K2.col2.Y =  iA * rA.X * rA.X;

            Mat22 K3 = new Mat22();
	        K3.col1.X =  iB * rB.Y * rB.Y;	K3.col2.X = -iB * rB.X * rB.Y;
	        K3.col1.Y = -iB * rB.X * rB.Y;	K3.col2.Y =  iB * rB.X * rB.X;

            Mat22 K12;
            Mat22.Add(ref K1, ref K2, out K12);
            Mat22 K;
            Mat22.Add(ref K12, ref K3, out K);
	        _linearMass = K.GetInverse();

	        _angularMass = iA + iB;
	        if (_angularMass > 0.0f)
	        {
		        _angularMass = 1.0f / _angularMass;
	        }

	        if (step.warmStarting)
	        {
		        // Scale impulses to support a variable time step.
		        _linearImpulse *= step.dtRatio;
		        _angularImpulse *= step.dtRatio;

		        Vector2 P = new Vector2(_linearImpulse.X, _linearImpulse.Y);

		        bA._linearVelocity -= mA * P;
		        bA._angularVelocity -= iA * (MathUtils.Cross(rA, P) + _angularImpulse);

		        bB._linearVelocity += mB * P;
		        bB._angularVelocity += iB * (MathUtils.Cross(rB, P) + _angularImpulse);
	        }
	        else
	        {
                _linearImpulse = Vector2.Zero;
		        _angularImpulse = 0.0f;
	        }
        }

        internal override void SolveVelocityConstraints(ref TimeStep step)
        {
	        Body bA = _bodyA;
	        Body bB = _bodyB;

            Vector2 vA = bA._linearVelocity;
            float wA = bA._angularVelocity;
            Vector2 vB = bB._linearVelocity;
            float wB = bB._angularVelocity;

            float mA = bA._invMass, mB = bB._invMass;
            float iA = bA._invI, iB = bB._invI;

            Transform xfA, xfB;
            bA.GetTransform(out xfA);
            bB.GetTransform(out xfB);

            Vector2 rA = MathUtils.Multiply(ref xfA.R, _localAnchor1 - bA.GetLocalCenter());
            Vector2 rB = MathUtils.Multiply(ref xfB.R, _localAnchor2 - bB.GetLocalCenter());

            // Solve angular friction
            {
                float Cdot = wB - wA;
                float impulse = -_angularMass * Cdot;

                float oldImpulse = _angularImpulse;
                float maxImpulse = step.dt * _maxTorque;
                _angularImpulse = MathUtils.Clamp(_angularImpulse + impulse, -maxImpulse, maxImpulse);
                impulse = _angularImpulse - oldImpulse;

                wA -= iA * impulse;
                wB += iB * impulse;
            }

            // Solve linear friction
            {
                Vector2 Cdot = vB + MathUtils.Cross(wB, rB) - vA - MathUtils.Cross(wA, rA);

                Vector2 impulse = -MathUtils.Multiply(ref _linearMass, Cdot);
                Vector2 oldImpulse = _linearImpulse;
                _linearImpulse += impulse;

                float maxImpulse = step.dt * _maxForce;

                if (_linearImpulse.LengthSquared() > maxImpulse * maxImpulse)
                {
                    _linearImpulse.Normalize();
                    _linearImpulse *= maxImpulse;
                }

                impulse = _linearImpulse - oldImpulse;

                vA -= mA * impulse;
                wA -= iA * MathUtils.Cross(rA, impulse);

                vB += mB * impulse;
                wB += iB * MathUtils.Cross(rB, impulse);
            }

            bA._linearVelocity = vA;
            bA._angularVelocity = wA;
            bB._linearVelocity = vB;
            bB._angularVelocity = wB;
        }

        internal override bool SolvePositionConstraints(float baumgarte)
        {
            return true;
        }

	    internal Vector2 _localAnchor1;
	    internal Vector2 _localAnchor2;
        internal Mat22 _linearMass;
        internal float _angularMass;
	    internal Vector2 _linearImpulse;
	    internal float _angularImpulse;
	    internal float _maxForce;
	    internal float _maxTorque;
    };
}


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

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

namespace Box2D.XNA
{
    /// Gear joint definition. This definition requires two existing
    /// revolute or prismatic joints (any combination will work).
    /// The provided joints must attach a dynamic body to a static body.
    public class GearJointDef : JointDef
    {
	    public GearJointDef()
	    {
		    type = JointType.Gear;
		    joint1 = null;
		    joint2 = null;
		    ratio = 1.0f;
	    }

	    /// The first revolute/prismatic joint attached to the gear joint.
	    public Joint joint1;

	    /// The second revolute/prismatic joint attached to the gear joint.
	    public Joint joint2;

	    /// The gear ratio.
	    /// @see GearJoint for explanation.
	    public float ratio;
    };

    /// A gear joint is used to connect two joints together. Either joint
    /// can be a revolute or prismatic joint. You specify a gear ratio
    /// to bind the motions together:
    /// coordinate1 + ratio * coordinate2 = ant
    /// The ratio can be negative or positive. If one joint is a revolute joint
    /// and the other joint is a prismatic joint, then the ratio will have units
    /// of length or units of 1/length.
    /// @warning The revolute and prismatic joints must be attached to
    /// fixed bodies (which must be body1 on those joints).
    public class GearJoint : Joint
    {
	    public override Vector2 GetAnchorA()
        {
	        return _bodyA.GetWorldPoint(_localAnchor1);
        }

	    public override Vector2 GetAnchorB()
        {
            return _bodyB.GetWorldPoint(_localAnchor2);
        }

	    public override Vector2 GetReactionForce(float inv_dt)
        {
            Vector2 P = _impulse * _J.linearB;
	        return inv_dt * P;
        }

	    public override float GetReactionTorque(float inv_dt)
        {
            Transform xf1;
            _bodyB.GetTransform(out xf1);

            Vector2 r = MathUtils.Multiply(ref xf1.R, _localAnchor2 - _bodyB.GetLocalCenter());
	        Vector2 P = _impulse * _J.linearB;
	        float L = _impulse * _J.angularB - MathUtils.Cross(r, P);
	        return inv_dt * L;
        }

	    /// Get the gear ratio.
	    public float GetRatio()
        {
            return _ratio;
        }

        public void SetRatio(float ratio)
        {
            Debug.Assert(MathUtils.IsValid(ratio));
            _ratio = ratio;
        }

	    internal GearJoint(GearJointDef def)
            : base(def)
        {
	        JointType type1 = def.joint1.JointType;
            JointType type2 = def.joint2.JointType;

	        Debug.Assert(type1 == JointType.Revolute || type1 == JointType.Prismatic);
	        Debug.Assert(type2 == JointType.Revolute || type2 == JointType.Prismatic);
	        Debug.Assert(def.joint1.GetBodyA().GetType() == BodyType.Static);
            Debug.Assert(def.joint2.GetBodyA().GetType() == BodyType.Static);

	        _revolute1 = null;
	        _prismatic1 = null;
	        _revolute2 = null;
	        _prismatic2 = null;

	        float coordinate1, coordinate2;

	        _ground1 = def.joint1.GetBodyA();
	        _bodyA = def.joint1.GetBodyB();
	        if (type1 == JointType.Revolute)
	        {
		        _revolute1 = (RevoluteJoint)def.joint1;
		        _groundAnchor1 = _revolute1._localAnchor1;
		        _localAnchor1 = _revolute1._localAnchor2;
		        coordinate1 = _revolute1.GetJointAngle();
	        }
	        else
	        {
		        _prismatic1 = (PrismaticJoint)def.joint1;
		        _groundAnchor1 = _prismatic1._localAnchor1;
		        _localAnchor1 = _prismatic1._localAnchor2;
		        coordinate1 = _prismatic1.GetJointTranslation();
	        }

	        _ground2 = def.joint2.GetBodyA();
	        _bodyB = def.joint2.GetBodyB();
	        if (type2 == JointType.Revolute)
	        {
		        _revolute2 = (RevoluteJoint)def.joint2;
		        _groundAnchor2 = _revolute2._localAnchor1;
		        _localAnchor2 = _revolute2._localAnchor2;
		        coordinate2 = _revolute2.GetJointAngle();
	        }
	        else
	        {
		        _prismatic2 = (PrismaticJoint)def.joint2;
		        _groundAnchor2 = _prismatic2._localAnchor1;
		        _localAnchor2 = _prismatic2._localAnchor2;
		        coordinate2 = _prismatic2.GetJointTranslation();
	        }

	        _ratio = def.ratio;

	        _ant = coordinate1 + _ratio * coordinate2;

	        _impulse = 0.0f;
        }

	    internal override void InitVelocityConstraints(ref TimeStep step)
        {
	        Body g1 = _ground1;
	        Body g2 = _ground2;
	        Body b1 = _bodyA;
	        Body b2 = _bodyB;

	        float K = 0.0f;
            _J.SetZero();

	        if (_revolute1 != null)
	        {
		        _J.angularA = -1.0f;
		        K += b1._invI;
	        }
	        else
	        {
                Transform xf1, xfg1;
                b1.GetTransform(out xf1);
                g1.GetTransform(out xfg1);

		        Vector2 ug = MathUtils.Multiply(ref xfg1.R, _prismatic1._localXAxis1);
		        Vector2 r = MathUtils.Multiply(ref xf1.R, _localAnchor1 - b1.GetLocalCenter());
		        float crug = MathUtils.Cross(r, ug);
		        _J.linearA = -ug;
		        _J.angularA = -crug;
		        K += b1._invMass + b1._invI * crug * crug;
	        }

	        if (_revolute2 != null)
	        {
		        _J.angularB = -_ratio;
		        K += _ratio * _ratio * b2._invI;
	        }
	        else
	        {
                Transform xfg1, xf2;
                g1.GetTransform(out xfg1);
                b2.GetTransform(out xf2);

		        Vector2 ug = MathUtils.Multiply(ref xfg1.R, _prismatic2._localXAxis1);
		        Vector2 r = MathUtils.Multiply(ref xf2.R, _localAnchor2 - b2.GetLocalCenter());
		        float crug = MathUtils.Cross(r, ug);
		        _J.linearB = -_ratio * ug;
		        _J.angularB = -_ratio * crug;
		        K += _ratio * _ratio * (b2._invMass + b2._invI * crug * crug);
	        }

	        // Compute effective mass.
	        Debug.Assert(K > 0.0f);
            _mass = K > 0.0f ? 1.0f / K : 0.0f;

	        if (step.warmStarting)
	        {
		        // Warm starting.
		        b1._linearVelocity += b1._invMass * _impulse * _J.linearA;
		        b1._angularVelocity += b1._invI * _impulse * _J.angularA;
		        b2._linearVelocity += b2._invMass * _impulse * _J.linearB;
		        b2._angularVelocity += b2._invI * _impulse * _J.angularB;
	        }
	        else
	        {
		        _impulse = 0.0f;
	        }
        }

	    internal override void SolveVelocityConstraints(ref TimeStep step)
        {
	        Body b1 = _bodyA;
	        Body b2 = _bodyB;

	        float Cdot = _J.Compute(	b1._linearVelocity, b1._angularVelocity,
								        b2._linearVelocity, b2._angularVelocity);

	        float impulse = _mass * (-Cdot);
	        _impulse += impulse;

	        b1._linearVelocity += b1._invMass * impulse * _J.linearA;
	        b1._angularVelocity += b1._invI * impulse * _J.angularA;
	        b2._linearVelocity += b2._invMass * impulse * _J.linearB;
	        b2._angularVelocity += b2._invI * impulse * _J.angularB;
        }

	    internal override bool SolvePositionConstraints(float baumgarte)
        {
            float linearError = 0.0f;

	        Body b1 = _bodyA;
	        Body b2 = _bodyB;

	        float coordinate1, coordinate2;
	        if (_revolute1 != null)
	        {
		        coordinate1 = _revolute1.GetJointAngle();
	        }
	        else
	        {
		        coordinate1 = _prismatic1.GetJointTranslation();
	        }

            if (_revolute2 != null)
	        {
		        coordinate2 = _revolute2.GetJointAngle();
	        }
	        else
	        {
		        coordinate2 = _prismatic2.GetJointTranslation();
	        }

	        float C = _ant - (coordinate1 + _ratio * coordinate2);

	        float impulse = _mass * (-C);

	        b1._sweep.c += b1._invMass * impulse * _J.linearA;
	        b1._sweep.a += b1._invI * impulse * _J.angularA;
	        b2._sweep.c += b2._invMass * impulse * _J.linearB;
	        b2._sweep.a += b2._invI * impulse * _J.angularB;

	        b1.SynchronizeTransform();
	        b2.SynchronizeTransform();

	        // TODO_ERIN not implemented
	        return linearError < Settings.b2_linearSlop;
        }

	    internal Body _ground1;
	    internal Body _ground2;

	    // One of these is null.
	    internal RevoluteJoint _revolute1;
	    internal PrismaticJoint _prismatic1;

	    // One of these is null.
	    internal RevoluteJoint _revolute2;
	    internal PrismaticJoint _prismatic2;

	    internal Vector2 _groundAnchor1;
	    internal Vector2 _groundAnchor2;

	    internal Vector2 _localAnchor1;
	    internal Vector2 _localAnchor2;

	    internal Jacobian _J;

	    internal float _ant;
	    internal float _ratio;

	    // Effective mass
	    internal float _mass;

	    // Impulse for accumulation/warm starting.
	    internal float _impulse;
    };
}


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

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

namespace Box2D.XNA
{
    public enum JointType
    {
        Unknown,
        Revolute,
        Prismatic,
        Distance,
        Pulley,
        Mouse,
        Gear,
        Line,
        Weld,
        Friction,
        MaxDistance,
    };

    public enum LimitState
    {
	    Inactive,
	    AtLower,
	    AtUpper,
	    Equal,
    };

    internal struct Jacobian
    {
	    public Vector2 linearA;
	    public float angularA;
	    public Vector2 linearB;
	    public float angularB;

	    public void SetZero()
        {
	        linearA = Vector2.Zero; angularA = 0.0f;
	        linearB = Vector2.Zero; angularB = 0.0f;
        }

	    public void Set(Vector2 x1, float a1, Vector2 x2, float a2)
        {
	        linearA = x1; angularA = a1;
	        linearB = x2; angularB = a2;
        }

	    public float Compute(Vector2 x1, float a1, Vector2 x2, float a2)
        {
            return Vector2.Dot(linearA, x1) + angularA * a1 + Vector2.Dot(linearB, x2) + angularB * a2;
        }
    };

     /// A joint edge is used to connect bodies and joints together
    /// in a joint graph where each body is a node and each joint
    /// is an edge. A joint edge belongs to a doubly linked list
    /// maintained in each attached body. Each joint has two joint
    /// nodes, one for each attached body.
    public class JointEdge
    {
        /// <summary>
        /// Provides quick access to the other body attached.
        /// </summary>
        public Body Other;

        /// <summary>
        /// The joint.
        /// </summary>
        public Joint Joint;

        /// <summary>
        /// The previous joint edge in the body's joint list.
        /// </summary>
        public JointEdge Prev;

        /// <summary>
        /// The next joint edge in the body's joint list.
        /// </summary>
        public JointEdge Next;
    }

    public class JointDef
    {
	    /// The joint type is set automatically for concrete joint types.
        internal JointType type;

	    /// Use this to attach application specific data to your joints.
        public object userData;

	    /// The first attached body.
        public Body bodyA;

	    /// The second attached body.
        public Body bodyB;

	    /// Set this flag to true if the attached bodies should collide.
	    public bool collideConnected;
    }

    public abstract class Joint
    {
	    /// Get the type of the concrete joint.
	    public JointType JointType
        {
            get
            {
                return _type;
            }
        }

	    /// Get the first body attached to this joint.
	    public Body GetBodyA()
        {
            return _bodyA;
        }

	    /// Get the second body attached to this joint.
	    public Body GetBodyB()
        {
            return _bodyB;
        }

	    /// Get the anchor point on body1 in world coordinates.
	    public abstract Vector2 GetAnchorA();

	    /// Get the anchor point on body2 in world coordinates.
	    public abstract Vector2 GetAnchorB();

        /// Get the reaction force on body2 at the joint anchor in Newtons.
	    public abstract Vector2 GetReactionForce(float inv_dt);

        /// Get the reaction torque on body2 in N*m.
	    public abstract float GetReactionTorque(float inv_dt);

	    /// Get the next joint the world joint list.
	    public Joint GetNext()
        {
	        return _next;
        }

	    /// Get the user data pointer.
	    public object GetUserData()
        {
            return _userData;
        }

	    /// Set the user data pointer.
	    public void SetUserData(object data)
        {
            _userData = data;
        }

        /// Short-cut function to determine if either body is inactive.
        public bool IsActive()
        {
            return _bodyA.IsActive() && _bodyB.IsActive();

        }

	    internal static Joint Create(JointDef def)
        {
	        Joint joint = null;

	        switch (def.type)
	        {
	        case JointType.Distance:
		        {
			        joint = new DistanceJoint((DistanceJointDef)def);
		        }
		        break;

	        case JointType.Mouse:
		        {
			        joint = new MouseJoint((MouseJointDef)def);
		        }
		        break;

	        case JointType.Prismatic:
		        {
			        joint = new PrismaticJoint((PrismaticJointDef)def);
		        }
		        break;

	        case JointType.Revolute:
		        {
			        joint = new RevoluteJoint((RevoluteJointDef)def);
		        }
		        break;

	        case JointType.Pulley:
		        {
			        joint = new PulleyJoint((PulleyJointDef)def);
		        }
		        break;

	        case JointType.Gear:
		        {
			        joint = new GearJoint((GearJointDef)def);
		        }
		        break;

	        case JointType.Line:
		        {
			        joint = new LineJoint((LineJointDef)def);
		        }
		        break;

            case JointType.Weld:
                {
                    joint = new WeldJoint((WeldJointDef)def);
                }
                break;
            case JointType.Friction:
                {
                    joint = new FrictionJoint((FrictionJointDef)def);
                }
                break;
            case JointType.MaxDistance:
                {
                    joint = new MaxDistanceJoint((MaxDistanceJointDef)def);
                }
                break;

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

	        return joint;
        }

	    protected Joint(JointDef def)
        {
            Debug.Assert(def.bodyA != def.bodyB);

	        _type = def.type;
	        _bodyA = def.bodyA;
	        _bodyB = def.bodyB;
	        _collideConnected = def.collideConnected;
	        _userData = def.userData;

            _edgeA = new JointEdge();
            _edgeB = new JointEdge();
        }

	    internal abstract void InitVelocityConstraints(ref TimeStep step);
	    internal abstract void SolveVelocityConstraints(ref TimeStep step);

	    // This returns true if the position errors are within tolerance.
	    internal abstract bool SolvePositionConstraints(float baumgarte);

	    internal JointType _type;
	    internal Joint _prev;
	    internal Joint _next;
	    internal JointEdge _edgeA;
	    internal JointEdge _edgeB;
	    internal Body _bodyA;
	    internal Body _bodyB;

	    internal bool _islandFlag;
	    internal bool _collideConnected;

	    internal object _userData;

	    // Cache here per time step to reduce cache misses.
	    internal Vector2 _localCenterA, _localCenterB;
	    internal float _invMassA, _invIA;
	    internal float _invMassB, _invIB;
    }
}


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
{
    /// Line joint definition. This requires defining a line of
    /// motion using an axis and an anchor point. The definition uses local
    /// anchor points and a local axis so that the initial configuration
    /// can violate the constraint slightly. The joint translation is zero
    /// when the local anchor points coincide in world space. Using local
    /// anchors and a local axis helps when saving and loading a game.
    public class LineJointDef : JointDef
    {
	    public LineJointDef()
	    {
		    type = JointType.Line;
		    localAnchorA = Vector2.Zero;
		    localAnchorB = Vector2.Zero;
		    localAxisA = new Vector2(1.0f, 0.0f);
		    enableLimit = false;
		    lowerTranslation = 0.0f;
		    upperTranslation = 0.0f;
		    enableMotor = false;
		    maxMotorForce = 0.0f;
		    motorSpeed = 0.0f;
	    }

	    /// Initialize the bodies, anchors, axis, and reference angle using the world
	    /// anchor and world axis.
        public void Initialize(Body b1, Body b2, Vector2 anchor, Vector2 axis)
        {
	        bodyA = b1;
	        bodyB = b2;
	        localAnchorA = bodyA.GetLocalPoint(anchor);
	        localAnchorB = bodyB.GetLocalPoint(anchor);
	        localAxisA = bodyA.GetLocalVector(axis);
        }

	    /// The local anchor point relative to body1's origin.
	    public Vector2 localAnchorA;

	    /// The local anchor point relative to body2's origin.
	    public Vector2 localAnchorB;

	    /// The local translation axis in body1.
	    public Vector2 localAxisA;

	    /// Enable/disable the joint limit.
	    public bool enableLimit;

	    /// The lower translation limit, usually in meters.
	    public float lowerTranslation;

	    /// The upper translation limit, usually in meters.
	    public float upperTranslation;

	    /// Enable/disable the joint motor.
	    public bool enableMotor;

	    /// The maximum motor torque, usually in N-m.
	    public float maxMotorForce;

	    /// The desired motor speed in radians per second.
	    public float motorSpeed;
    };

    /// A line joint. This joint provides two degrees of freedom: translation
    /// along an axis fixed in body1 and rotation in the plane. You can use a
    /// joint limit to restrict the range of motion and a joint motor to drive
    /// the motion or to model joint friction.
    public class LineJoint : Joint
    {
	    public override Vector2 GetAnchorA()
        {
            return _bodyA.GetWorldPoint(_localAnchor1);
        }

	    public override Vector2 GetAnchorB()
        {
            return _bodyB.GetWorldPoint(_localAnchor2);
        }

	    public override Vector2 GetReactionForce(float inv_dt)
        {
            return inv_dt * (_impulse.X * _perp + (_motorImpulse + _impulse.Y) * _axis);
        }

        public override float GetReactionTorque(float inv_dt)
        {
	        return 0.0f;
        }

	    /// Get the current joint translation, usually in meters.
	    public float GetJointTranslation()
        {
	        Body b1 = _bodyA;
	        Body b2 = _bodyB;

	        Vector2 p1 = b1.GetWorldPoint(_localAnchor1);
	        Vector2 p2 = b2.GetWorldPoint(_localAnchor2);
	        Vector2 d = p2 - p1;
	        Vector2 axis = b1.GetWorldVector(_localXAxis1);

	        float translation = Vector2.Dot(d, axis);
	        return translation;
        }

	    /// Get the current joint translation speed, usually in meters per second.
	    public float GetJointSpeed()
        {
            Body b1 = _bodyA;
	        Body b2 = _bodyB;
            Transform xf1, xf2;
            b1.GetTransform(out xf1);
            b2.GetTransform(out xf2);

	        Vector2 r1 = MathUtils.Multiply(ref xf1.R, _localAnchor1 - b1.GetLocalCenter());
	        Vector2 r2 = MathUtils.Multiply(ref xf2.R, _localAnchor2 - b2.GetLocalCenter());
	        Vector2 p1 = b1._sweep.c + r1;
	        Vector2 p2 = b2._sweep.c + r2;
	        Vector2 d = p2 - p1;
	        Vector2 axis = b1.GetWorldVector(_localXAxis1);

	        Vector2 v1 = b1._linearVelocity;
	        Vector2 v2 = b2._linearVelocity;
	        float w1 = b1._angularVelocity;
	        float w2 = b2._angularVelocity;

	        float speed = Vector2.Dot(d, MathUtils.Cross(w1, axis)) + Vector2.Dot(axis, v2 + MathUtils.Cross(w2, r2) - v1 - MathUtils.Cross(w1, r1));
	        return speed;
        }

	    /// Is the joint limit enabled?
	    public bool IsLimitEnabled()
        {
            return _enableLimit;
        }

	    /// Enable/disable the joint limit.
	    public void EnableLimit(bool flag)
        {
	        _bodyA.SetAwake(true);
	        _bodyB.SetAwake(true);
	        _enableLimit = flag;
        }

	    /// Get the lower joint limit, usually in meters.
	    public float GetLowerLimit()
        {
            return _lowerTranslation;
        }

	    /// Get the upper joint limit, usually in meters.
	    public float GetUpperLimit()
        {
            return _upperTranslation;
        }

	    /// Set the joint limits, usually in meters.
	    public void SetLimits(float lower, float upper)
        {
	        Debug.Assert(lower <= upper);
	        _bodyA.SetAwake(true);
	        _bodyB.SetAwake(true);
	        _lowerTranslation = lower;
	        _upperTranslation = upper;
        }

	    /// Is the joint motor enabled?
	    public bool IsMotorEnabled()
        {
            return _enableMotor;
        }

	    /// Enable/disable the joint motor.
	    public void EnableMotor(bool flag)
        {
            _bodyA.SetAwake(true);
	        _bodyB.SetAwake(true);
	        _enableMotor = flag;
        }

	    /// Set the motor speed, usually in meters per second.
	    public void SetMotorSpeed(float speed)
        {
	        _bodyA.SetAwake(true);
	        _bodyB.SetAwake(true);
	        _motorSpeed = speed;
        }

	    /// Get the motor speed, usually in meters per second.
	    public float GetMotorSpeed()
        {
            return _motorSpeed;
        }

        public float GetMaxMotorForce()
        {
            return _maxMotorForce;
        }

	    /// Set the maximum motor force, usually in N.
	    public void SetMaxMotorForce(float force)
        {
	        _bodyA.SetAwake(true);
	        _bodyB.SetAwake(true);
	        _maxMotorForce = force;
        }

	    /// Get the current motor force, usually in N.
	    public float GetMotorForce()
        {
	        return _motorImpulse;
        }

	    internal LineJoint(LineJointDef def)
            : base (def)
        {
	        _localAnchor1 = def.localAnchorA;
	        _localAnchor2 = def.localAnchorB;
	        _localXAxis1 = def.localAxisA;
	        _localYAxis1 = MathUtils.Cross(1.0f, _localXAxis1);

	        _impulse = Vector2.Zero;
	        _motorMass = 0.0f;
	        _motorImpulse = 0.0f;

	        _lowerTranslation = def.lowerTranslation;
	        _upperTranslation = def.upperTranslation;
	        _maxMotorForce = def.maxMotorForce;
	        _motorSpeed = def.motorSpeed;
	        _enableLimit = def.enableLimit;
	        _enableMotor = def.enableMotor;
	        _limitState = LimitState.Inactive;

	        _axis = Vector2.Zero;
	        _perp = Vector2.Zero;
        }

	    internal override void InitVelocityConstraints(ref TimeStep step)
        {
            Body b1 = _bodyA;
	        Body b2 = _bodyB;

	        _localCenterA = b1.GetLocalCenter();
	        _localCenterB = b2.GetLocalCenter();

            Transform xf1, xf2;
            b1.GetTransform(out xf1);
            b2.GetTransform(out xf2);

	        // Compute the effective masses.
	        Vector2 r1 = MathUtils.Multiply(ref xf1.R, _localAnchor1 - _localCenterA);
	        Vector2 r2 = MathUtils.Multiply(ref xf2.R, _localAnchor2 - _localCenterB);
	        Vector2 d = b2._sweep.c + r2 - b1._sweep.c - r1;

	        _invMassA = b1._invMass;
	        _invIA = b1._invI;
	        _invMassB = b2._invMass;
	        _invIB = b2._invI;

	        // Compute motor Jacobian and effective mass.
	        {
		        _axis = MathUtils.Multiply(ref xf1.R, _localXAxis1);
		        _a1 = MathUtils.Cross(d + r1, _axis);
		        _a2 = MathUtils.Cross(r2, _axis);

		        _motorMass = _invMassA + _invMassB + _invIA * _a1 * _a1 + _invIB * _a2 * _a2;
                if (_motorMass > Settings.b2_epsilon)
                {
                    _motorMass = 1.0f / _motorMass;
                }
                else
                {
                    _motorMass = 0.0f;
                }
	        }

	        // Prismatic constraint.
	        {
		        _perp = MathUtils.Multiply(ref xf1.R, _localYAxis1);

		        _s1 = MathUtils.Cross(d + r1, _perp);
		        _s2 = MathUtils.Cross(r2, _perp);

		        float m1 = _invMassA, m2 = _invMassB;
		        float i1 = _invIA, i2 = _invIB;

		        float k11 = m1 + m2 + i1 * _s1 * _s1 + i2 * _s2 * _s2;
		        float k12 = i1 * _s1 * _a1 + i2 * _s2 * _a2;
		        float k22 = m1 + m2 + i1 * _a1 * _a1 + i2 * _a2 * _a2;

		        _K.col1 = new Vector2(k11, k12);
                _K.col2 = new Vector2(k12, k22);
	        }

	        // Compute motor and limit terms.
	        if (_enableLimit)
	        {
		        float jointTranslation = Vector2.Dot(_axis, d);
		        if (Math.Abs(_upperTranslation - _lowerTranslation) < 2.0f * Settings.b2_linearSlop)
		        {
			        _limitState = LimitState.Equal;
		        }
		        else if (jointTranslation <= _lowerTranslation)
		        {
			        if (_limitState != LimitState.AtLower)
			        {
				        _limitState = LimitState.AtLower;
				        _impulse.Y = 0.0f;
			        }
		        }
		        else if (jointTranslation >= _upperTranslation)
		        {
			        if (_limitState != LimitState.AtUpper)
			        {
				        _limitState = LimitState.AtUpper;
				        _impulse.Y = 0.0f;
			        }
		        }
		        else
		        {
			        _limitState = LimitState.Inactive;
			        _impulse.Y = 0.0f;
		        }
	        }
	        else
	        {
		        _limitState = LimitState.Inactive;
	        }

	        if (_enableMotor == false)
	        {
		        _motorImpulse = 0.0f;
	        }

	        if (step.warmStarting)
	        {
		        // Account for variable time step.
		        _impulse *= step.dtRatio;
		        _motorImpulse *= step.dtRatio;

		        Vector2 P = _impulse.X * _perp + (_motorImpulse + _impulse.Y) * _axis;
		        float L1 = _impulse.X * _s1 + (_motorImpulse + _impulse.Y) * _a1;
		        float L2 = _impulse.X * _s2 + (_motorImpulse + _impulse.Y) * _a2;

		        b1._linearVelocity -= _invMassA * P;
		        b1._angularVelocity -= _invIA * L1;

		        b2._linearVelocity += _invMassB * P;
		        b2._angularVelocity += _invIB * L2;
	        }
	        else
	        {
		        _impulse = Vector2.Zero;
		        _motorImpulse = 0.0f;
	        }
        }

	    internal override void SolveVelocityConstraints(ref TimeStep step)
        {
	        Body b1 = _bodyA;
	        Body b2 = _bodyB;

	        Vector2 v1 = b1._linearVelocity;
	        float w1 = b1._angularVelocity;
	        Vector2 v2 = b2._linearVelocity;
	        float w2 = b2._angularVelocity;

	        // Solve linear motor constraint.
	        if (_enableMotor && _limitState != LimitState.Equal)
	        {
		        float Cdot = Vector2.Dot(_axis, v2 - v1) + _a2 * w2 - _a1 * w1;
		        float impulse = _motorMass * (_motorSpeed - Cdot);
		        float oldImpulse = _motorImpulse;
		        float maxImpulse = step.dt * _maxMotorForce;
		        _motorImpulse = MathUtils.Clamp(_motorImpulse + impulse, -maxImpulse, maxImpulse);
		        impulse = _motorImpulse - oldImpulse;

		        Vector2 P = impulse * _axis;
		        float L1 = impulse * _a1;
		        float L2 = impulse * _a2;

		        v1 -= _invMassA * P;
		        w1 -= _invIA * L1;

		        v2 += _invMassB * P;
		        w2 += _invIB * L2;
	        }

	        float Cdot1 = Vector2.Dot(_perp, v2 - v1) + _s2 * w2 - _s1 * w1;

	        if (_enableLimit && _limitState != LimitState.Inactive)
	        {
		        // Solve prismatic and limit constraint in block form.
		        float Cdot2 = Vector2.Dot(_axis, v2 - v1) + _a2 * w2 - _a1 * w1;
		        Vector2 Cdot = new Vector2(Cdot1, Cdot2);

		        Vector2 f1 = _impulse;
		        Vector2 df =  _K.Solve(-Cdot);
		        _impulse += df;

		        if (_limitState == LimitState.AtLower)
		        {
			        _impulse.Y = Math.Max(_impulse.Y, 0.0f);
		        }
		        else if (_limitState == LimitState.AtUpper)
		        {
                    _impulse.Y = Math.Min(_impulse.Y, 0.0f);
		        }

		        // f2(1) = invK(1,1) * (-Cdot(1) - K(1,2) * (f2(2) - f1(2))) + f1(1)
		        float b = -Cdot1 - (_impulse.Y - f1.Y) * _K.col2.X;

                float f2r;
                if (_K.col1.X != 0.0f)
                {
                    f2r = b / _K.col1.X + f1.X;
                }
                else
                {
                    f2r = f1.X;
                }

		        _impulse.X = f2r;

		        df = _impulse - f1;

		        Vector2 P = df.X * _perp + df.Y * _axis;
		        float L1 = df.X * _s1 + df.Y * _a1;
		        float L2 = df.X * _s2 + df.Y * _a2;

		        v1 -= _invMassA * P;
		        w1 -= _invIA * L1;

		        v2 += _invMassB * P;
		        w2 += _invIB * L2;
	        }
	        else
	        {
		        // Limit is inactive, just solve the prismatic constraint in block form.

                float df;
                if (_K.col1.X != 0.0f)
                {
                    df = -Cdot1 / _K.col1.X;
                }
                else
                {
                    df = 0.0f;
                }

		        _impulse.X += df;

		        Vector2 P = df * _perp;
		        float L1 = df * _s1;
		        float L2 = df * _s2;

		        v1 -= _invMassA * P;
		        w1 -= _invIA * L1;

		        v2 += _invMassB * P;
		        w2 += _invIB * L2;
	        }

	        b1._linearVelocity = v1;
	        b1._angularVelocity = w1;
	        b2._linearVelocity = v2;
	        b2._angularVelocity = w2;
        }

	    internal override bool SolvePositionConstraints(float baumgarte)
        {
	        Body b1 = _bodyA;
	        Body b2 = _bodyB;

	        Vector2 c1 = b1._sweep.c;
	        float a1 = b1._sweep.a;

	        Vector2 c2 = b2._sweep.c;
	        float a2 = b2._sweep.a;

	        // Solve linear limit constraint.
	        float linearError = 0.0f, angularError = 0.0f;
	        bool active = false;
	        float C2 = 0.0f;

            Mat22 R1 = new Mat22(a1);
            Mat22 R2 = new Mat22(a2);

	        Vector2 r1 = MathUtils.Multiply(ref R1, _localAnchor1 - _localCenterA);
	        Vector2 r2 = MathUtils.Multiply(ref R2, _localAnchor2 - _localCenterB);
	        Vector2 d = c2 + r2 - c1 - r1;

	        if (_enableLimit)
	        {
		        _axis = MathUtils.Multiply(ref R1, _localXAxis1);

		        _a1 = MathUtils.Cross(d + r1, _axis);
		        _a2 = MathUtils.Cross(r2, _axis);

		        float translation = Vector2.Dot(_axis, d);
		        if (Math.Abs(_upperTranslation - _lowerTranslation) < 2.0f * Settings.b2_linearSlop)
		        {
			        // Prevent large angular corrections
			        C2 = MathUtils.Clamp(translation, -Settings.b2_maxLinearCorrection, Settings.b2_maxLinearCorrection);
			        linearError = Math.Abs(translation);
			        active = true;
		        }
		        else if (translation <= _lowerTranslation)
		        {
			        // Prevent large linear corrections and allow some slop.
			        C2 = MathUtils.Clamp(translation - _lowerTranslation + Settings.b2_linearSlop, -Settings.b2_maxLinearCorrection, 0.0f);
			        linearError = _lowerTranslation - translation;
			        active = true;
		        }
		        else if (translation >= _upperTranslation)
		        {
			        // Prevent large linear corrections and allow some slop.
			        C2 = MathUtils.Clamp(translation - _upperTranslation - Settings.b2_linearSlop, 0.0f, Settings.b2_maxLinearCorrection);
			        linearError = translation - _upperTranslation;
			        active = true;
		        }
	        }

	        _perp = MathUtils.Multiply(ref R1, _localYAxis1);

	        _s1 = MathUtils.Cross(d + r1, _perp);
	        _s2 = MathUtils.Cross(r2, _perp);

	        Vector2 impulse;
	        float C1;
	        C1 = Vector2.Dot(_perp, d);

	        linearError = Math.Max(linearError, Math.Abs(C1));
	        angularError = 0.0f;

	        if (active)
	        {
		        float m1 = _invMassA, m2 = _invMassB;
		        float i1 = _invIA, i2 = _invIB;

		        float k11 = m1 + m2 + i1 * _s1 * _s1 + i2 * _s2 * _s2;
		        float k12 = i1 * _s1 * _a1 + i2 * _s2 * _a2;
		        float k22 = m1 + m2 + i1 * _a1 * _a1 + i2 * _a2 * _a2;

                _K.col1 = new Vector2(k11, k12);
		        _K.col2 = new Vector2(k12, k22);

		        Vector2 C = new Vector2(-C1, -C2);

		        impulse = _K.Solve(C); //note i inverted above
	        }
	        else
	        {
		        float m1 = _invMassA, m2 = _invMassB;
		        float i1 = _invIA, i2 = _invIB;

		        float k11 = m1 + m2 + i1 * _s1 * _s1 + i2 * _s2 * _s2;

                float impulse1;
                if (k11 != 0.0f)
                {
                    impulse1 = -C1 / k11;
                }
                else
                {
                    impulse1 = 0.0f;
                }

		        impulse.X = impulse1;
		        impulse.Y = 0.0f;
	        }

	        Vector2 P = impulse.X * _perp + impulse.Y * _axis;
	        float L1 = impulse.X * _s1 + impulse.Y * _a1;
	        float L2 = impulse.X * _s2 + impulse.Y * _a2;

	        c1 -= _invMassA * P;
	        a1 -= _invIA * L1;
	        c2 += _invMassB * P;
	        a2 += _invIB * L2;

	        // TODO_ERIN remove need for this.
	        b1._sweep.c = c1;
	        b1._sweep.a = a1;
	        b2._sweep.c = c2;
	        b2._sweep.a = a2;
	        b1.SynchronizeTransform();
	        b2.SynchronizeTransform();

	        return linearError <= Settings.b2_linearSlop && angularError <= Settings.b2_angularSlop;
        }

	    internal Vector2 _localAnchor1;
	    internal Vector2 _localAnchor2;
	    internal Vector2 _localXAxis1;
	    internal Vector2 _localYAxis1;

	    internal Vector2 _axis, _perp;
	    internal float _s1, _s2;
	    internal float _a1, _a2;

	    internal Mat22 _K;
	    internal Vector2 _impulse;

	    internal float _motorMass;			// effective mass for motor/limit translational constraint.
	    internal float _motorImpulse;

	    internal float _lowerTranslation;
	    internal float _upperTranslation;
	    internal float _maxMotorForce;
	    internal float _motorSpeed;

	    internal bool _enableLimit;
	    internal bool _enableMotor;
	    internal LimitState _limitState;
    };
}


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

using Microsoft.Xna.Framework;
using System.Diagnostics;
using System;
namespace Box2D.XNA
{
    /// Distance joint definition. This requires defining an
    /// anchor point on both bodies and the non-zero length of the
    /// distance joint. The definition uses local anchor points
    /// so that the initial configuration can violate the constraint
    /// slightly. This helps when saving and loading a game.
    /// @warning Do not use a zero or short length.
    public class MaxDistanceJointDef : JointDef
    {
        public MaxDistanceJointDef()
	    {
		    type = JointType.MaxDistance;
		    localAnchorA = new Vector2(0.0f, 0.0f);
		    localAnchorB = new Vector2(0.0f, 0.0f);
		    length = 1.0f;
		    frequencyHz = 0.0f;
		    dampingRatio = 0.0f;
	    }

	    /// Initialize the bodies, anchors, and length using the world
	    /// anchors.
        // 1-D rained system
        // m (v2 - v1) = lambda
        // v2 + (beta/h) * x1 + gamma * lambda = 0, gamma has units of inverse mass.
        // x2 = x1 + h * v2

        // 1-D mass-damper-spring system
        // m (v2 - v1) + h * d * v2 + h * k *

        // C = norm(p2 - p1) - L
        // u = (p2 - p1) / norm(p2 - p1)
        // Cdot = dot(u, v2 + cross(w2, r2) - v1 - cross(w1, r1))
        // J = [-u -cross(r1, u) u cross(r2, u)]
        // K = J * invM * JT
        //   = invMass1 + invI1 * cross(r1, u)^2 + invMass2 + invI2 * cross(r2, u)^2
	    public void Initialize(Body b1, Body b2,
					    Vector2 anchor1, Vector2 anchor2, float maxlength)
        {
	        bodyA = b1;
	        bodyB = b2;
	        localAnchorA = bodyA.GetLocalPoint(anchor1);
	        localAnchorB = bodyB.GetLocalPoint(anchor2);
            length = maxlength;
        }

	    /// The local anchor point relative to body1's origin.
	    public Vector2 localAnchorA;

	    /// The local anchor point relative to body2's origin.
	    public Vector2 localAnchorB;

        /// The natural length between the anchor points.
	    public float length;

        /// The mass-spring-damper frequency in Hertz.
	    public float frequencyHz;

	    /// The damping ratio. 0 = no damping, 1 = critical damping.
	    public float dampingRatio;
    };

    /// A distance joint rains two points on two bodies
    /// to remain at a fixed distance from each other. You can view
    /// this as a massless, rigid rod.
    public class MaxDistanceJoint : Joint
    {
        	// Set/get the natural length.
        public void SetLength(float length)
        {
            _length = length;
        }

        public float GetLength()
        {
            return _length;
        }

	    // Set/get frequency in Hz.
        public void SetFrequency(float hz)
        {
            _frequencyHz = hz;
        }

        public float GetFrequency()
        {
            return _frequencyHz;
        }

	    // Set/get damping ratio.
        public void SetDampingRatio(float ratio)
        {
            _dampingRatio = ratio;
        }

        public float GetDampingRatio()
        {
            return _dampingRatio;
        }

	    public override Vector2 GetAnchorA()
        {
            return _bodyA.GetWorldPoint(_localAnchor1);
        }

        public override Vector2 GetAnchorB()
        {
	        return _bodyB.GetWorldPoint(_localAnchor2);
        }

        public override Vector2 GetReactionForce(float inv_dt)
        {
	        Vector2 F = (inv_dt * _impulse) * _u;
	        return F;
        }

        public override float GetReactionTorque(float inv_dt)
        {
	        return 0.0f;
        }

        internal MaxDistanceJoint(MaxDistanceJointDef def)
            : base(def)
        {
	        _localAnchor1 = def.localAnchorA;
	        _localAnchor2 = def.localAnchorB;
	        _length = def.length;
	        _frequencyHz = def.frequencyHz;
	        _dampingRatio = def.dampingRatio;
	        _impulse = 0.0f;
	        _gamma = 0.0f;
	        _bias = 0.0f;
        }

        internal override void InitVelocityConstraints(ref TimeStep step)
        {
	        Body b1 = _bodyA;
	        Body b2 = _bodyB;

            Transform xf1, xf2;
            b1.GetTransform(out xf1);
            b2.GetTransform(out xf2);

	        // Compute the effective mass matrix.
            Vector2 r1 = MathUtils.Multiply(ref xf1.R, _localAnchor1 - b1.GetLocalCenter());
            Vector2 r2 = MathUtils.Multiply(ref xf2.R, _localAnchor2 - b2.GetLocalCenter());
	        _u = b2._sweep.c + r2 - b1._sweep.c - r1;

	        // Handle singularity.
	        float length = _u.Length();

            if (length < _length)
            {
                return ;
            }

	        if (length > Settings.b2_linearSlop)
	        {
		        _u *= 1.0f / length;
	        }
	        else
	        {
		        _u = new Vector2(0.0f, 0.0f);
	        }

	        float cr1u = MathUtils.Cross(r1, _u);
	        float cr2u = MathUtils.Cross(r2, _u);
	        float invMass = b1._invMass + b1._invI * cr1u * cr1u + b2._invMass + b2._invI * cr2u * cr2u;
	        Debug.Assert(invMass > Settings.b2_epsilon);
            _mass = invMass != 0.0f ? 1.0f / invMass : 0.0f;

	        if (_frequencyHz > 0.0f)
	        {
		        float C = length - _length;

		        // Frequency
		        float omega = 2.0f * Settings.b2_pi * _frequencyHz;

		        // Damping coefficient
		        float d = 2.0f * _mass * _dampingRatio * omega;

		        // Spring stiffness
		        float k = _mass * omega * omega;

		        // magic formulas
                _gamma = step.dt * (d + step.dt * k);
                _gamma = _gamma != 0.0f ? 1.0f / _gamma : 0.0f;
                _bias = C * step.dt * k * _gamma;

                _mass = invMass + _gamma;
                _mass = _mass != 0.0f ? 1.0f / _mass : 0.0f;
	        }

	        if (step.warmStarting)
	        {
		        // Scale the impulse to support a variable time step.
		        _impulse *= step.dtRatio;

		        Vector2 P = _impulse * _u;
		        b1._linearVelocity -= b1._invMass * P;
		        b1._angularVelocity -= b1._invI * MathUtils.Cross(r1, P);
		        b2._linearVelocity += b2._invMass * P;
		        b2._angularVelocity += b2._invI * MathUtils.Cross(r2, P);
	        }
	        else
	        {
		        _impulse = 0.0f;
	        }
        }

        internal override void SolveVelocityConstraints(ref TimeStep step)
        {
	        Body b1 = _bodyA;
	        Body b2 = _bodyB;

            Transform xf1, xf2;
            b1.GetTransform(out xf1);
            b2.GetTransform(out xf2);

            Vector2 r1 = MathUtils.Multiply(ref xf1.R, _localAnchor1 - b1.GetLocalCenter());
            Vector2 r2 = MathUtils.Multiply(ref xf2.R, _localAnchor2 - b2.GetLocalCenter());

            Vector2 d = b2._sweep.c + r2 - b1._sweep.c - r1;

            float length = d.Length();

            if (length < _length)
            {
                return;
            }

	        // Cdot = dot(u, v + cross(w, r))
	        Vector2 v1 = b1._linearVelocity + MathUtils.Cross(b1._angularVelocity, r1);
	        Vector2 v2 = b2._linearVelocity + MathUtils.Cross(b2._angularVelocity, r2);
	        float Cdot = Vector2.Dot(_u, v2 - v1);

	        float impulse = -_mass * (Cdot + _bias + _gamma * _impulse);
	        _impulse += impulse;

	        Vector2 P = impulse * _u;
	        b1._linearVelocity -= b1._invMass * P;
	        b1._angularVelocity -= b1._invI * MathUtils.Cross(r1, P);
	        b2._linearVelocity += b2._invMass * P;
	        b2._angularVelocity += b2._invI * MathUtils.Cross(r2, P);
        }

        internal override bool SolvePositionConstraints(float baumgarte)
        {
	        if (_frequencyHz > 0.0f)
	        {
		        // There is no position correction for soft distance constraints.
		        return true;
	        }

	        Body b1 = _bodyA;
	        Body b2 = _bodyB;

            Transform xf1, xf2;
            b1.GetTransform(out xf1);
            b2.GetTransform(out xf2);

	        Vector2 r1 = MathUtils.Multiply(ref xf1.R, _localAnchor1 - b1.GetLocalCenter());
	        Vector2 r2 = MathUtils.Multiply(ref xf2.R, _localAnchor2 - b2.GetLocalCenter());

	        Vector2 d = b2._sweep.c + r2 - b1._sweep.c - r1;

	        float length = d.Length();

            if (length < _length)
            {
                return true;
            }

            if (length == 0.0f)
                return true;

            d /= length;
	        float C = length - _length;
	        C = MathUtils.Clamp(C, -Settings.b2_maxLinearCorrection, Settings.b2_maxLinearCorrection);

	        float impulse = -_mass * C;
	        _u = d;
	        Vector2 P = impulse * _u;

	        b1._sweep.c -= b1._invMass * P;
	        b1._sweep.a -= b1._invI * MathUtils.Cross(r1, P);
	        b2._sweep.c += b2._invMass * P;
	        b2._sweep.a += b2._invI * MathUtils.Cross(r2, P);

	        b1.SynchronizeTransform();
	        b2.SynchronizeTransform();

	        return Math.Abs(C) < Settings.b2_linearSlop;
        }

	    internal Vector2 _localAnchor1;
	    internal Vector2 _localAnchor2;
	    internal Vector2 _u;
	    internal float _frequencyHz;
	    internal float _dampingRatio;
	    internal float _gamma;
	    internal float _bias;
	    internal float _impulse;
	    internal float _mass;
	    public float _length;
    };
}


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

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

namespace Box2D.XNA
{
/// Mouse joint definition. This requires a world target point,
/// tuning parameters, and the time step.
public class MouseJointDef : JointDef
{
	public MouseJointDef()
	{
		type = JointType.Mouse;
		target = new Vector2(0.0f, 0.0f);
		maxForce = 0.0f;
		frequencyHz = 5.0f;
		dampingRatio = 0.7f;
	}

	/// The initial world target point. This is assumed
	/// to coincide with the body anchor initially.
	public Vector2 target;

	/// The maximum constraint force that can be exerted
	/// to move the candidate body. Usually you will express
	/// as some multiple of the weight (multiplier * mass * gravity).
	public float maxForce;

	/// The response speed.
	public float frequencyHz;

	/// The damping ratio. 0 = no damping, 1 = critical damping.
	public float dampingRatio;
};

/// A mouse joint is used to make a point on a body track a
/// specified world point. This a soft constraint with a maximum
/// force. This allows the constraint to stretch and without
/// applying huge forces.
/// NOTE: this joint is not documented in the manual because it was
/// developed to be used in the testbed. If you want to learn how to
/// use the mouse joint, look at the testbed.
public class MouseJoint : Joint
{
	/// Implements Joint.
	public override Vector2 GetAnchorA()
    {
        return _target;
    }

	/// Implements Joint.
	public override Vector2 GetAnchorB()
    {
        return _bodyB.GetWorldPoint(_localAnchor);
    }

	/// Implements Joint.
	public override Vector2 GetReactionForce(float inv_dt)
    {
        return inv_dt * _impulse;
    }

	/// Implements Joint.
	public override float GetReactionTorque(float inv_dt)
    {
        return inv_dt * 0.0f;
    }

	/// Use this to update the target point.
	public void SetTarget(Vector2 target)
    {
	    if (_bodyB.IsAwake() == false)
	    {
		    _bodyB.SetAwake(true);
	    }
	    _target = target;
    }

    	public Vector2 GetTarget()
        {
            return _target;
        }

	    /// Set/get the maximum force in Newtons.
	    public void SetMaxForce(float force)
        {
            _maxForce = force;
        }

	    public float GetMaxForce()
        {
            return _maxForce;
        }

	    /// Set/get the frequency in Hertz.
	    public void SetFrequency(float hz)
        {
            _frequencyHz = hz;
        }

	    public float GetFrequency()
        {
            return _frequencyHz;
        }

	    /// Set/get the damping ratio (dimensionless).
	    public void SetDampingRatio(float ratio)
        {
            _dampingRatio = ratio;
        }

	    public float GetDampingRatio()
        {
            return _dampingRatio;
        }

	internal MouseJoint(MouseJointDef def)
        : base(def)
    {
        Debug.Assert(def.target.IsValid());
        Debug.Assert(MathUtils.IsValid(def.maxForce) && def.maxForce >= 0.0f);
        Debug.Assert(MathUtils.IsValid(def.frequencyHz) && def.frequencyHz >= 0.0f);
        Debug.Assert(MathUtils.IsValid(def.dampingRatio) && def.dampingRatio >= 0.0f);

        Transform xf1;
        _bodyB.GetTransform(out xf1);

	    _target = def.target;
	    _localAnchor = MathUtils.MultiplyT(ref xf1, _target);

	    _maxForce = def.maxForce;
	    _impulse = Vector2.Zero;

	    _frequencyHz = def.frequencyHz;
	    _dampingRatio = def.dampingRatio;

	    _beta = 0.0f;
	    _gamma = 0.0f;
    }

	internal override void InitVelocityConstraints(ref TimeStep step)
    {
	    Body b = _bodyB;

	    float mass = b.GetMass();

	    // Frequency
	    float omega = 2.0f * Settings.b2_pi * _frequencyHz;

	    // Damping coefficient
	    float d = 2.0f * mass * _dampingRatio * omega;

	    // Spring stiffness
	    float k = mass * (omega * omega);

	    // magic formulas
	    // gamma has units of inverse mass.
	    // beta has units of inverse time.
	    Debug.Assert(d + step.dt * k > Settings.b2_epsilon);

        _gamma = step.dt * (d + step.dt * k);
        if (_gamma != 0.0f)
        {
            _gamma = 1.0f / _gamma;
        }

	    _beta = step.dt * k * _gamma;

	    // Compute the effective mass matrix.
        Transform xf1;
        b.GetTransform(out xf1);
	    Vector2 r = MathUtils.Multiply(ref xf1.R, _localAnchor - b.GetLocalCenter());

	    // K    = [(1/m1 + 1/m2) * eye(2) - skew(r1) * invI1 * skew(r1) - skew(r2) * invI2 * skew(r2)]
	    //      = [1/m1+1/m2     0    ] + invI1 * [r1.Y*r1.Y -r1.X*r1.Y] + invI2 * [r1.Y*r1.Y -r1.X*r1.Y]
	    //        [    0     1/m1+1/m2]           [-r1.X*r1.Y r1.X*r1.X]           [-r1.X*r1.Y r1.X*r1.X]
	    float invMass = b._invMass;
	    float invI = b._invI;

        Mat22 K1 = new Mat22(new Vector2(invMass, 0.0f), new Vector2(0.0f, invMass));
        Mat22 K2 = new Mat22(new Vector2(invI * r.Y * r.Y, -invI * r.X * r.Y), new Vector2(-invI * r.X * r.Y, invI * r.X * r.X));

        Mat22 K;
        Mat22.Add(ref K1, ref K2, out K);

	    K.col1.X += _gamma;
	    K.col2.Y += _gamma;

	    _mass = K.GetInverse();

	    _C = b._sweep.c + r - _target;

	    // Cheat with some damping
	    b._angularVelocity *= 0.98f;

	    // Warm starting.
	    _impulse *= step.dtRatio;
	    b._linearVelocity += invMass * _impulse;
	    b._angularVelocity += invI * MathUtils.Cross(r, _impulse);
    }

	internal override void SolveVelocityConstraints(ref TimeStep step)
    {
        Body b = _bodyB;

        Transform xf1;
        b.GetTransform(out xf1);

	    Vector2 r = MathUtils.Multiply(ref xf1.R, _localAnchor - b.GetLocalCenter());

	    // Cdot = v + cross(w, r)
	    Vector2 Cdot = b._linearVelocity + MathUtils.Cross(b._angularVelocity, r);
	    Vector2 impulse = MathUtils.Multiply(ref _mass, -(Cdot + _beta * _C + _gamma * _impulse));

	    Vector2 oldImpulse = _impulse;
	    _impulse += impulse;
	    float maxImpulse = step.dt * _maxForce;
	    if (_impulse.LengthSquared() > maxImpulse * maxImpulse)
	    {
		    _impulse *= maxImpulse / _impulse.Length();
	    }
	    impulse = _impulse - oldImpulse;

	    b._linearVelocity += b._invMass * impulse;
	    b._angularVelocity += b._invI * MathUtils.Cross(r, impulse);
    }

	internal override bool SolvePositionConstraints(float baumgarte)
    {
        return true;
    }

	public Vector2 _localAnchor;
    public Vector2 _target;
    public Vector2 _impulse;

    public Mat22 _mass;		// effective mass for point-to-point constraint.
    public Vector2 _C;				// position error
    public float _maxForce;
    public float _frequencyHz;
    public float _dampingRatio;
    public float _beta;
    public float _gamma;
};
}