Adding initial files

axios/Dynamics/Joints/AngleJoint.cs

@@ -0,0 +1,93 @@
+using System;
+using System.Diagnostics;
+using Microsoft.Xna.Framework;
+
+namespace FarseerPhysics.Dynamics.Joints
+{
+    /// <summary>
+    /// Maintains a fixed angle between two bodies
+    /// </summary>
+    public class AngleJoint : Joint
+    {
+        public float BiasFactor;
+        public float MaxImpulse;
+        public float Softness;
+        private float _bias;
+        private float _jointError;
+        private float _massFactor;
+        private float _targetAngle;
+
+        internal AngleJoint()
+        {
+            JointType = JointType.Angle;
+        }
+
+        public AngleJoint(Body bodyA, Body bodyB)
+            : base(bodyA, bodyB)
+        {
+            JointType = JointType.Angle;
+            TargetAngle = 0;
+            BiasFactor = .2f;
+            Softness = 0f;
+            MaxImpulse = float.MaxValue;
+        }
+
+        public float TargetAngle
+        {
+            get { return _targetAngle; }
+            set
+            {
+                if (value != _targetAngle)
+                {
+                    _targetAngle = value;
+                    WakeBodies();
+                }
+            }
+        }
+
+        public override Vector2 WorldAnchorA
+        {
+            get { return BodyA.Position; }
+        }
+
+        public override Vector2 WorldAnchorB
+        {
+            get { return BodyB.Position; }
+            set { Debug.Assert(false, "You can't set the world anchor on this joint type."); }
+        }
+
+        public override Vector2 GetReactionForce(float inv_dt)
+        {
+            //TODO
+            //return _inv_dt * _impulse;
+            return Vector2.Zero;
+        }
+
+        public override float GetReactionTorque(float inv_dt)
+        {
+            return 0;
+        }
+
+        internal override void InitVelocityConstraints(ref TimeStep step)
+        {
+            _jointError = (BodyB.Sweep.A - BodyA.Sweep.A - TargetAngle);
+
+            _bias = -BiasFactor * step.inv_dt * _jointError;
+
+            _massFactor = (1 - Softness) / (BodyA.InvI + BodyB.InvI);
+        }
+
+        internal override void SolveVelocityConstraints(ref TimeStep step)
+        {
+            float p = (_bias - BodyB.AngularVelocity + BodyA.AngularVelocity) * _massFactor;
+            BodyA.AngularVelocity -= BodyA.InvI * Math.Sign(p) * Math.Min(Math.Abs(p), MaxImpulse);
+            BodyB.AngularVelocity += BodyB.InvI * Math.Sign(p) * Math.Min(Math.Abs(p), MaxImpulse);
+        }
+
+        internal override bool SolvePositionConstraints()
+        {
+            //no position solving for this joint
+            return true;
+        }
+    }
+}

axios/Dynamics/Joints/DistanceJoint.cs

@@ -0,0 +1,286 @@
+/*
+* Farseer Physics Engine based on Box2D.XNA port:
+* Copyright (c) 2010 Ian Qvist
+* 
+* 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 FarseerPhysics.Common;
+using Microsoft.Xna.Framework;
+
+namespace FarseerPhysics.Dynamics.Joints
+{
+    // 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
+
+    /// <summary>
+    /// 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.
+    /// </summary>
+    public class DistanceJoint : Joint
+    {
+        /// <summary>
+        /// The local anchor point relative to bodyA's origin.
+        /// </summary>
+        public Vector2 LocalAnchorA;
+
+        /// <summary>
+        /// The local anchor point relative to bodyB's origin.
+        /// </summary>
+        public Vector2 LocalAnchorB;
+
+        private float _bias;
+        private float _gamma;
+        private float _impulse;
+        private float _mass;
+        private float _tmpFloat1;
+        private Vector2 _tmpVector1;
+        private Vector2 _u;
+
+        internal DistanceJoint()
+        {
+            JointType = JointType.Distance;
+        }
+
+        /// <summary>
+        /// This requires defining an
+        /// anchor point on both bodies and the non-zero length of the
+        /// distance joint. If you don't supply a length, the local anchor points
+        /// is used 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.
+        /// </summary>
+        /// <param name="bodyA">The first body</param>
+        /// <param name="bodyB">The second body</param>
+        /// <param name="localAnchorA">The first body anchor</param>
+        /// <param name="localAnchorB">The second body anchor</param>
+        public DistanceJoint(Body bodyA, Body bodyB, Vector2 localAnchorA, Vector2 localAnchorB)
+            : base(bodyA, bodyB)
+        {
+            JointType = JointType.Distance;
+
+            LocalAnchorA = localAnchorA;
+            LocalAnchorB = localAnchorB;
+
+            Vector2 d = WorldAnchorB - WorldAnchorA;
+            Length = d.Length();
+        }
+
+        /// <summary>
+        /// The natural length between the anchor points.
+        /// Manipulating the length can lead to non-physical behavior when the frequency is zero.
+        /// </summary>
+        public float Length { get; set; }
+
+        /// <summary>
+        /// The mass-spring-damper frequency in Hertz.
+        /// </summary>
+        public float Frequency { get; set; }
+
+        /// <summary>
+        /// The damping ratio. 0 = no damping, 1 = critical damping.
+        /// </summary>
+        public float DampingRatio { get; set; }
+
+        public override sealed Vector2 WorldAnchorA
+        {
+            get { return BodyA.GetWorldPoint(LocalAnchorA); }
+        }
+
+        public override sealed Vector2 WorldAnchorB
+        {
+            get { return BodyB.GetWorldPoint(LocalAnchorB); }
+            set { Debug.Assert(false, "You can't set the world anchor on this joint type."); }
+        }
+
+        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 override void InitVelocityConstraints(ref TimeStep step)
+        {
+            Body b1 = BodyA;
+            Body b2 = BodyB;
+
+            // Compute the effective mass matrix.
+            Vector2 r1 = MathUtils.Multiply(ref b1.Xf.R, LocalAnchorA - b1.LocalCenter);
+            Vector2 r2 = MathUtils.Multiply(ref b2.Xf.R, LocalAnchorB - b2.LocalCenter);
+            _u = b2.Sweep.C + r2 - b1.Sweep.C - r1;
+
+            // Handle singularity.
+            float length = _u.Length();
+            if (length > Settings.LinearSlop)
+            {
+                _u *= 1.0f / length;
+            }
+            else
+            {
+                _u = Vector2.Zero;
+            }
+
+            float cr1u, cr2u;
+            MathUtils.Cross(ref r1, ref _u, out cr1u);
+            MathUtils.Cross(ref r2, ref _u, out cr2u);
+            float invMass = b1.InvMass + b1.InvI * cr1u * cr1u + b2.InvMass + b2.InvI * cr2u * cr2u;
+            Debug.Assert(invMass > Settings.Epsilon);
+            _mass = invMass != 0.0f ? 1.0f / invMass : 0.0f;
+
+            if (Frequency > 0.0f)
+            {
+                float C = length - Length;
+
+                // Frequency
+                float omega = 2.0f * Settings.Pi * Frequency;
+
+                // 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 (Settings.EnableWarmstarting)
+            {
+                // Scale the impulse to support a variable time step.
+                _impulse *= step.dtRatio;
+
+                Vector2 P = _impulse * _u;
+                b1.LinearVelocityInternal -= b1.InvMass * P;
+                MathUtils.Cross(ref r1, ref P, out _tmpFloat1);
+                b1.AngularVelocityInternal -= b1.InvI * /* r1 x P */ _tmpFloat1;
+                b2.LinearVelocityInternal += b2.InvMass * P;
+                MathUtils.Cross(ref r2, ref P, out _tmpFloat1);
+                b2.AngularVelocityInternal += b2.InvI * /* r2 x P */ _tmpFloat1;
+            }
+            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, LocalAnchorA - b1.LocalCenter);
+            Vector2 r2 = MathUtils.Multiply(ref xf2.R, LocalAnchorB - b2.LocalCenter);
+
+            // Cdot = dot(u, v + cross(w, r))
+            MathUtils.Cross(b1.AngularVelocityInternal, ref r1, out _tmpVector1);
+            Vector2 v1 = b1.LinearVelocityInternal + _tmpVector1;
+            MathUtils.Cross(b2.AngularVelocityInternal, ref r2, out _tmpVector1);
+            Vector2 v2 = b2.LinearVelocityInternal + _tmpVector1;
+            float Cdot = Vector2.Dot(_u, v2 - v1);
+
+            float impulse = -_mass * (Cdot + _bias + _gamma * _impulse);
+            _impulse += impulse;
+
+            Vector2 P = impulse * _u;
+            b1.LinearVelocityInternal -= b1.InvMass * P;
+            MathUtils.Cross(ref r1, ref P, out _tmpFloat1);
+            b1.AngularVelocityInternal -= b1.InvI * _tmpFloat1;
+            b2.LinearVelocityInternal += b2.InvMass * P;
+            MathUtils.Cross(ref r2, ref P, out _tmpFloat1);
+            b2.AngularVelocityInternal += b2.InvI * _tmpFloat1;
+        }
+
+        internal override bool SolvePositionConstraints()
+        {
+            if (Frequency > 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, LocalAnchorA - b1.LocalCenter);
+            Vector2 r2 = MathUtils.Multiply(ref xf2.R, LocalAnchorB - b2.LocalCenter);
+
+            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.MaxLinearCorrection, Settings.MaxLinearCorrection);
+
+            float impulse = -_mass * C;
+            _u = d;
+            Vector2 P = impulse * _u;
+
+            b1.Sweep.C -= b1.InvMass * P;
+            MathUtils.Cross(ref r1, ref P, out _tmpFloat1);
+            b1.Sweep.A -= b1.InvI * _tmpFloat1;
+            b2.Sweep.C += b2.InvMass * P;
+            MathUtils.Cross(ref r2, ref P, out _tmpFloat1);
+            b2.Sweep.A += b2.InvI * _tmpFloat1;
+
+            b1.SynchronizeTransform();
+            b2.SynchronizeTransform();
+
+            return Math.Abs(C) < Settings.LinearSlop;
+        }
+    }
+}

axios/Dynamics/Joints/FixedAngleJoint.cs

@@ -0,0 +1,84 @@
+using System;
+using System.Diagnostics;
+using Microsoft.Xna.Framework;
+
+namespace FarseerPhysics.Dynamics.Joints
+{
+    public class FixedAngleJoint : Joint
+    {
+        public float BiasFactor;
+        public float MaxImpulse;
+        public float Softness;
+        private float _bias;
+        private float _jointError;
+        private float _massFactor;
+        private float _targetAngle;
+
+        public FixedAngleJoint(Body bodyA)
+            : base(bodyA)
+        {
+            JointType = JointType.FixedAngle;
+            TargetAngle = 0;
+            BiasFactor = .2f;
+            Softness = 0f;
+            MaxImpulse = float.MaxValue;
+        }
+
+        public float TargetAngle
+        {
+            get { return _targetAngle; }
+            set
+            {
+                if (value != _targetAngle)
+                {
+                    _targetAngle = value;
+                    WakeBodies();
+                }
+            }
+        }
+
+        public override Vector2 WorldAnchorA
+        {
+            get { return BodyA.Position; }
+        }
+
+        public override Vector2 WorldAnchorB
+        {
+            get { return BodyA.Position; }
+            set { Debug.Assert(false, "You can't set the world anchor on this joint type."); }
+        }
+
+        public override Vector2 GetReactionForce(float inv_dt)
+        {
+            //TODO
+            //return _inv_dt * _impulse;
+            return Vector2.Zero;
+        }
+
+        public override float GetReactionTorque(float inv_dt)
+        {
+            return 0;
+        }
+
+        internal override void InitVelocityConstraints(ref TimeStep step)
+        {
+            _jointError = BodyA.Sweep.A - TargetAngle;
+
+            _bias = -BiasFactor * step.inv_dt * _jointError;
+
+            _massFactor = (1 - Softness) / (BodyA.InvI);
+        }
+
+        internal override void SolveVelocityConstraints(ref TimeStep step)
+        {
+            float p = (_bias - BodyA.AngularVelocity) * _massFactor;
+            BodyA.AngularVelocity += BodyA.InvI * Math.Sign(p) * Math.Min(Math.Abs(p), MaxImpulse);
+        }
+
+        internal override bool SolvePositionConstraints()
+        {
+            //no position solving for this joint
+            return true;
+        }
+    }
+}

axios/Dynamics/Joints/FixedDistanceJoint.cs

@@ -0,0 +1,255 @@
+/*
+* Farseer Physics Engine based on Box2D.XNA port:
+* Copyright (c) 2010 Ian Qvist
+* 
+* 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 FarseerPhysics.Common;
+using Microsoft.Xna.Framework;
+
+namespace FarseerPhysics.Dynamics.Joints
+{
+    // 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
+
+    /// <summary>
+    /// 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.
+    /// </summary>
+    public class FixedDistanceJoint : Joint
+    {
+        /// <summary>
+        /// The local anchor point relative to bodyA's origin.
+        /// </summary>
+        public Vector2 LocalAnchorA;
+
+        private float _bias;
+        private float _gamma;
+        private float _impulse;
+        private float _mass;
+        private Vector2 _u;
+        private Vector2 _worldAnchorB;
+
+        /// <summary>
+        /// This requires defining an
+        /// anchor point on both bodies and the non-zero length of the
+        /// distance joint. If you don't supply a length, the local anchor points
+        /// is used 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.
+        /// </summary>
+        /// <param name="body">The body.</param>
+        /// <param name="bodyAnchor">The body anchor.</param>
+        /// <param name="worldAnchor">The world anchor.</param>
+        public FixedDistanceJoint(Body body, Vector2 bodyAnchor, Vector2 worldAnchor)
+            : base(body)
+        {
+            JointType = JointType.FixedDistance;
+
+            LocalAnchorA = bodyAnchor;
+            _worldAnchorB = worldAnchor;
+
+            //Calculate the length
+            Vector2 d = WorldAnchorB - WorldAnchorA;
+            Length = d.Length();
+        }
+
+        /// <summary>
+        /// The natural length between the anchor points.
+        /// Manipulating the length can lead to non-physical behavior when the frequency is zero.
+        /// </summary>
+        public float Length { get; set; }
+
+        /// <summary>
+        /// The mass-spring-damper frequency in Hertz.
+        /// </summary>
+        public float Frequency { get; set; }
+
+        /// <summary>
+        /// The damping ratio. 0 = no damping, 1 = critical damping.
+        /// </summary>
+        public float DampingRatio { get; set; }
+
+        public override sealed Vector2 WorldAnchorA
+        {
+            get { return BodyA.GetWorldPoint(LocalAnchorA); }
+        }
+
+        public override sealed Vector2 WorldAnchorB
+        {
+            get { return _worldAnchorB; }
+            set { _worldAnchorB = value; }
+        }
+
+        public override Vector2 GetReactionForce(float invDt)
+        {
+            return (invDt * _impulse) * _u;
+        }
+
+        public override float GetReactionTorque(float invDt)
+        {
+            return 0.0f;
+        }
+
+        internal override void InitVelocityConstraints(ref TimeStep step)
+        {
+            Body b1 = BodyA;
+
+            Transform xf1;
+            b1.GetTransform(out xf1);
+
+            // Compute the effective mass matrix.
+            Vector2 r1 = MathUtils.Multiply(ref xf1.R, LocalAnchorA - b1.LocalCenter);
+            Vector2 r2 = _worldAnchorB;
+            _u = r2 - b1.Sweep.C - r1;
+
+            // Handle singularity.
+            float length = _u.Length();
+            if (length > Settings.LinearSlop)
+            {
+                _u *= 1.0f / length;
+            }
+            else
+            {
+                _u = Vector2.Zero;
+            }
+
+            float cr1u = MathUtils.Cross(r1, _u);
+            float cr2u = MathUtils.Cross(r2, _u);
+            float invMass = b1.InvMass + b1.InvI * cr1u * cr1u + 0 * cr2u * cr2u;
+            Debug.Assert(invMass > Settings.Epsilon);
+            _mass = invMass != 0.0f ? 1.0f / invMass : 0.0f;
+
+            if (Frequency > 0.0f)
+            {
+                float C = length - Length;
+
+                // Frequency
+                float omega = 2.0f * Settings.Pi * Frequency;
+
+                // 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 (Settings.EnableWarmstarting)
+            {
+                // Scale the impulse to support a variable time step.
+                _impulse *= step.dtRatio;
+
+                Vector2 P = _impulse * _u;
+                b1.LinearVelocityInternal -= b1.InvMass * P;
+                b1.AngularVelocityInternal -= b1.InvI * MathUtils.Cross(r1, P);
+            }
+            else
+            {
+                _impulse = 0.0f;
+            }
+        }
+
+        internal override void SolveVelocityConstraints(ref TimeStep step)
+        {
+            Body b1 = BodyA;
+
+            Transform xf1;
+            b1.GetTransform(out xf1);
+
+            Vector2 r1 = MathUtils.Multiply(ref xf1.R, LocalAnchorA - b1.LocalCenter);
+
+            // Cdot = dot(u, v + cross(w, r))
+            Vector2 v1 = b1.LinearVelocityInternal + MathUtils.Cross(b1.AngularVelocityInternal, r1);
+            Vector2 v2 = Vector2.Zero;
+            float Cdot = Vector2.Dot(_u, v2 - v1);
+
+            float impulse = -_mass * (Cdot + _bias + _gamma * _impulse);
+            _impulse += impulse;
+
+            Vector2 P = impulse * _u;
+            b1.LinearVelocityInternal -= b1.InvMass * P;
+            b1.AngularVelocityInternal -= b1.InvI * MathUtils.Cross(r1, P);
+        }
+
+        internal override bool SolvePositionConstraints()
+        {
+            if (Frequency > 0.0f)
+            {
+                // There is no position correction for soft distance constraints.
+                return true;
+            }
+
+            Body b1 = BodyA;
+
+            Transform xf1;
+            b1.GetTransform(out xf1);
+
+            Vector2 r1 = MathUtils.Multiply(ref xf1.R, LocalAnchorA - b1.LocalCenter);
+            Vector2 r2 = _worldAnchorB;
+
+            Vector2 d = 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.MaxLinearCorrection, Settings.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);
+
+            b1.SynchronizeTransform();
+
+            return Math.Abs(C) < Settings.LinearSlop;
+        }
+    }
+}

axios/Dynamics/Joints/FixedFrictionJoint.cs

@@ -0,0 +1,227 @@
+/*
+* Farseer Physics Engine based on Box2D.XNA port:
+* Copyright (c) 2010 Ian Qvist
+* 
+* 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 FarseerPhysics.Common;
+using Microsoft.Xna.Framework;
+
+namespace FarseerPhysics.Dynamics.Joints
+{
+    // 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
+
+    /// <summary>
+    /// Friction joint. This is used for top-down friction.
+    /// It provides 2D translational friction and angular friction.
+    /// </summary>
+    public class FixedFrictionJoint : Joint
+    {
+        public Vector2 LocalAnchorA;
+
+        /// <summary>
+        /// The maximum friction force in N.
+        /// </summary>
+        public float MaxForce;
+
+        /// <summary>
+        /// The maximum friction torque in N-m.
+        /// </summary>
+        public float MaxTorque;
+
+        private float _angularImpulse;
+        private float _angularMass;
+        private Vector2 _linearImpulse;
+        private Mat22 _linearMass;
+
+        public FixedFrictionJoint(Body body, Vector2 localAnchorA)
+            : base(body)
+        {
+            JointType = JointType.FixedFriction;
+            LocalAnchorA = localAnchorA;
+
+            //Setting default max force and max torque
+            const float gravity = 10.0f;
+
+            // For a circle: I = 0.5 * m * r * r ==> r = sqrt(2 * I / m)
+            float radius = (float)Math.Sqrt(2.0 * (body.Inertia / body.Mass));
+
+            MaxForce = body.Mass * gravity;
+            MaxTorque = body.Mass * radius * gravity;
+        }
+
+        public override Vector2 WorldAnchorA
+        {
+            get { return BodyA.GetWorldPoint(LocalAnchorA); }
+        }
+
+        public override Vector2 WorldAnchorB
+        {
+            get { return Vector2.Zero; }
+            set { Debug.Assert(false, "You can't set the world anchor on this joint type."); }
+        }
+
+        public override Vector2 GetReactionForce(float invDT)
+        {
+            return invDT * _linearImpulse;
+        }
+
+        public override float GetReactionTorque(float invDT)
+        {
+            return invDT * _angularImpulse;
+        }
+
+        internal override void InitVelocityConstraints(ref TimeStep step)
+        {
+            Body bA = BodyA;
+
+            Transform xfA;
+            bA.GetTransform(out xfA);
+
+            // Compute the effective mass matrix.
+            Vector2 rA = MathUtils.Multiply(ref xfA.R, LocalAnchorA - bA.LocalCenter);
+
+            // 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;
+            float iA = bA.InvI;
+
+            Mat22 K1 = new Mat22();
+            K1.Col1.X = mA;
+            K1.Col2.X = 0.0f;
+            K1.Col1.Y = 0.0f;
+            K1.Col2.Y = mA;
+
+            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 K12;
+            Mat22.Add(ref K1, ref K2, out K12);
+
+            _linearMass = K12.Inverse;
+
+            _angularMass = iA;
+            if (_angularMass > 0.0f)
+            {
+                _angularMass = 1.0f / _angularMass;
+            }
+
+            if (Settings.EnableWarmstarting)
+            {
+                // Scale impulses to support a variable time step.
+                _linearImpulse *= step.dtRatio;
+                _angularImpulse *= step.dtRatio;
+
+                Vector2 P = new Vector2(_linearImpulse.X, _linearImpulse.Y);
+
+                bA.LinearVelocityInternal -= mA * P;
+                bA.AngularVelocityInternal -= iA * (MathUtils.Cross(rA, P) + _angularImpulse);
+            }
+            else
+            {
+                _linearImpulse = Vector2.Zero;
+                _angularImpulse = 0.0f;
+            }
+        }
+
+        internal override void SolveVelocityConstraints(ref TimeStep step)
+        {
+            Body bA = BodyA;
+
+            Vector2 vA = bA.LinearVelocityInternal;
+            float wA = bA.AngularVelocityInternal;
+
+            float mA = bA.InvMass;
+            float iA = bA.InvI;
+
+            Transform xfA;
+            bA.GetTransform(out xfA);
+
+            Vector2 rA = MathUtils.Multiply(ref xfA.R, LocalAnchorA - bA.LocalCenter);
+
+            // Solve angular friction
+            {
+                float Cdot = -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;
+            }
+
+            // Solve linear friction
+            {
+                Vector2 Cdot = -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);
+            }
+
+            bA.LinearVelocityInternal = vA;
+            bA.AngularVelocityInternal = wA;
+        }
+
+        internal override bool SolvePositionConstraints()
+        {
+            return true;
+        }
+    }
+}

axios/Dynamics/Joints/FixedLineJoint.cs

@@ -0,0 +1,413 @@
+/*
+* Farseer Physics Engine based on Box2D.XNA port:
+* Copyright (c) 2010 Ian Qvist
+* 
+* 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 FarseerPhysics.Common;
+using Microsoft.Xna.Framework;
+
+namespace FarseerPhysics.Dynamics.Joints
+{
+    public class FixedLineJoint : Joint
+    {
+        private Vector2 _ax, _ay;
+        private float _bias;
+        private bool _enableMotor;
+        private float _gamma;
+        private float _impulse;
+        private Vector2 _localXAxis;
+        private Vector2 _localYAxisA;
+        private float _mass;
+        private float _maxMotorTorque;
+        private float _motorImpulse;
+        private float _motorMass;
+        private float _motorSpeed;
+
+        private float _sAx;
+        private float _sAy;
+        private float _sBx;
+        private float _sBy;
+
+        private float _springImpulse;
+        private float _springMass;
+
+        // Linear constraint (point-to-line)
+        // d = pB - pA = xB + rB - xA - rA
+        // C = dot(ay, d)
+        // Cdot = dot(d, cross(wA, ay)) + dot(ay, vB + cross(wB, rB) - vA - cross(wA, rA))
+        //      = -dot(ay, vA) - dot(cross(d + rA, ay), wA) + dot(ay, vB) + dot(cross(rB, ay), vB)
+        // J = [-ay, -cross(d + rA, ay), ay, cross(rB, ay)]
+
+        // Spring linear constraint
+        // C = dot(ax, d)
+        // Cdot = = -dot(ax, vA) - dot(cross(d + rA, ax), wA) + dot(ax, vB) + dot(cross(rB, ax), vB)
+        // J = [-ax -cross(d+rA, ax) ax cross(rB, ax)]
+
+        // Motor rotational constraint
+        // Cdot = wB - wA
+        // J = [0 0 -1 0 0 1]
+
+        internal FixedLineJoint() { JointType = JointType.FixedLine; }
+
+        public FixedLineJoint(Body body, Vector2 worldAnchor, Vector2 axis)
+            : base(body)
+        {
+            JointType = JointType.FixedLine;
+
+            BodyB = BodyA;
+
+            LocalAnchorA = worldAnchor;
+            LocalAnchorB = BodyB.GetLocalPoint(worldAnchor);
+            LocalXAxis = axis;
+        }
+
+        public Vector2 LocalAnchorA { get; set; }
+
+        public Vector2 LocalAnchorB { get; set; }
+
+        public override Vector2 WorldAnchorA
+        {
+            get { return LocalAnchorA; }
+        }
+
+        public override Vector2 WorldAnchorB
+        {
+            get { return BodyA.GetWorldPoint(LocalAnchorB); }
+            set { Debug.Assert(false, "You can't set the world anchor on this joint type."); }
+        }
+
+        public float JointTranslation
+        {
+            get
+            {
+                Body bA = BodyA;
+                Body bB = BodyB;
+
+                Vector2 pA = bA.GetWorldPoint(LocalAnchorA);
+                Vector2 pB = bB.GetWorldPoint(LocalAnchorB);
+                Vector2 d = pB - pA;
+                Vector2 axis = bA.GetWorldVector(LocalXAxis);
+
+                float translation = Vector2.Dot(d, axis);
+                return translation;
+            }
+        }
+
+        public float JointSpeed
+        {
+            get
+            {
+                float wA = BodyA.AngularVelocityInternal;
+                float wB = BodyB.AngularVelocityInternal;
+                return wB - wA;
+            }
+        }
+
+        public bool MotorEnabled
+        {
+            get { return _enableMotor; }
+            set
+            {
+                BodyA.Awake = true;
+                BodyB.Awake = true;
+                _enableMotor = value;
+            }
+        }
+
+        public float MotorSpeed
+        {
+            set
+            {
+                BodyA.Awake = true;
+                BodyB.Awake = true;
+                _motorSpeed = value;
+            }
+            get { return _motorSpeed; }
+        }
+
+        public float MaxMotorTorque
+        {
+            set
+            {
+                BodyA.Awake = true;
+                BodyB.Awake = true;
+                _maxMotorTorque = value;
+            }
+            get { return _maxMotorTorque; }
+        }
+
+        public float Frequency { get; set; }
+
+        public float DampingRatio { get; set; }
+
+        public Vector2 LocalXAxis
+        {
+            get { return _localXAxis; }
+            set
+            {
+                _localXAxis = value;
+                _localYAxisA = MathUtils.Cross(1.0f, _localXAxis);
+            }
+        }
+
+        public override Vector2 GetReactionForce(float invDt)
+        {
+            return invDt * (_impulse * _ay + _springImpulse * _ax);
+        }
+
+        public override float GetReactionTorque(float invDt)
+        {
+            return invDt * _motorImpulse;
+        }
+
+        internal override void InitVelocityConstraints(ref TimeStep step)
+        {
+            Body bB = BodyB;
+
+            LocalCenterA = Vector2.Zero;
+            LocalCenterB = bB.LocalCenter;
+
+            Transform xfB;
+            bB.GetTransform(out xfB);
+
+            // Compute the effective masses.
+            Vector2 rA = LocalAnchorA;
+            Vector2 rB = MathUtils.Multiply(ref xfB.R, LocalAnchorB - LocalCenterB);
+            Vector2 d = bB.Sweep.C + rB - rA;
+
+            InvMassA = 0.0f;
+            InvIA = 0.0f;
+            InvMassB = bB.InvMass;
+            InvIB = bB.InvI;
+
+            // Point to line constraint
+            {
+                _ay = _localYAxisA;
+                _sAy = MathUtils.Cross(d + rA, _ay);
+                _sBy = MathUtils.Cross(rB, _ay);
+
+                _mass = InvMassA + InvMassB + InvIA * _sAy * _sAy + InvIB * _sBy * _sBy;
+
+                if (_mass > 0.0f)
+                {
+                    _mass = 1.0f / _mass;
+                }
+            }
+
+            // Spring constraint
+            _springMass = 0.0f;
+            if (Frequency > 0.0f)
+            {
+                _ax = LocalXAxis;
+                _sAx = MathUtils.Cross(d + rA, _ax);
+                _sBx = MathUtils.Cross(rB, _ax);
+
+                float invMass = InvMassA + InvMassB + InvIA * _sAx * _sAx + InvIB * _sBx * _sBx;
+
+                if (invMass > 0.0f)
+                {
+                    _springMass = 1.0f / invMass;
+
+                    float C = Vector2.Dot(d, _ax);
+
+                    // Frequency
+                    float omega = 2.0f * Settings.Pi * Frequency;
+
+                    // Damping coefficient
+                    float da = 2.0f * _springMass * DampingRatio * omega;
+
+                    // Spring stiffness
+                    float k = _springMass * omega * omega;
+
+                    // magic formulas
+                    _gamma = step.dt * (da + step.dt * k);
+                    if (_gamma > 0.0f)
+                    {
+                        _gamma = 1.0f / _gamma;
+                    }
+
+                    _bias = C * step.dt * k * _gamma;
+
+                    _springMass = invMass + _gamma;
+                    if (_springMass > 0.0f)
+                    {
+                        _springMass = 1.0f / _springMass;
+                    }
+                }
+            }
+            else
+            {
+                _springImpulse = 0.0f;
+                _springMass = 0.0f;
+            }
+
+            // Rotational motor
+            if (_enableMotor)
+            {
+                _motorMass = InvIA + InvIB;
+                if (_motorMass > 0.0f)
+                {
+                    _motorMass = 1.0f / _motorMass;
+                }
+            }
+            else
+            {
+                _motorMass = 0.0f;
+                _motorImpulse = 0.0f;
+            }
+
+            if (Settings.EnableWarmstarting)
+            {
+                // Account for variable time step.
+                _impulse *= step.dtRatio;
+                _springImpulse *= step.dtRatio;
+                _motorImpulse *= step.dtRatio;
+
+                Vector2 P = _impulse * _ay + _springImpulse * _ax;
+                float LB = _impulse * _sBy + _springImpulse * _sBx + _motorImpulse;
+
+                bB.LinearVelocityInternal += InvMassB * P;
+                bB.AngularVelocityInternal += InvIB * LB;
+            }
+            else
+            {
+                _impulse = 0.0f;
+                _springImpulse = 0.0f;
+                _motorImpulse = 0.0f;
+            }
+        }
+
+        internal override void SolveVelocityConstraints(ref TimeStep step)
+        {
+            Body bB = BodyB;
+
+            Vector2 vA = Vector2.Zero;
+            float wA = 0.0f;
+            Vector2 vB = bB.LinearVelocityInternal;
+            float wB = bB.AngularVelocityInternal;
+
+            // Solve spring constraint
+            {
+                float Cdot = Vector2.Dot(_ax, vB - vA) + _sBx * wB - _sAx * wA;
+                float impulse = -_springMass * (Cdot + _bias + _gamma * _springImpulse);
+                _springImpulse += impulse;
+
+                Vector2 P = impulse * _ax;
+                float LA = impulse * _sAx;
+                float LB = impulse * _sBx;
+
+                vA -= InvMassA * P;
+                wA -= InvIA * LA;
+
+                vB += InvMassB * P;
+                wB += InvIB * LB;
+            }
+
+            // Solve rotational motor constraint
+            {
+                float Cdot = wB - wA - _motorSpeed;
+                float impulse = -_motorMass * Cdot;
+
+                float oldImpulse = _motorImpulse;
+                float maxImpulse = step.dt * _maxMotorTorque;
+                _motorImpulse = MathUtils.Clamp(_motorImpulse + impulse, -maxImpulse, maxImpulse);
+                impulse = _motorImpulse - oldImpulse;
+
+                wA -= InvIA * impulse;
+                wB += InvIB * impulse;
+            }
+
+            // Solve point to line constraint
+            {
+                float Cdot = Vector2.Dot(_ay, vB - vA) + _sBy * wB - _sAy * wA;
+                float impulse = _mass * (-Cdot);
+                _impulse += impulse;
+
+                Vector2 P = impulse * _ay;
+                float LB = impulse * _sBy;
+
+                vB += InvMassB * P;
+                wB += InvIB * LB;
+            }
+
+            bB.LinearVelocityInternal = vB;
+            bB.AngularVelocityInternal = wB;
+        }
+
+        internal override bool SolvePositionConstraints()
+        {
+            Body bB = BodyB;
+
+            Vector2 xA = Vector2.Zero;
+            const float angleA = 0.0f;
+
+            Vector2 xB = bB.Sweep.C;
+            float angleB = bB.Sweep.A;
+
+            Mat22 RA = new Mat22(angleA);
+            Mat22 RB = new Mat22(angleB);
+
+            Vector2 rA = MathUtils.Multiply(ref RA, LocalAnchorA - LocalCenterA);
+            Vector2 rB = MathUtils.Multiply(ref RB, LocalAnchorB - LocalCenterB);
+            Vector2 d = xB + rB - xA - rA;
+
+            Vector2 ay = MathUtils.Multiply(ref RA, _localYAxisA);
+
+            float sBy = MathUtils.Cross(rB, ay);
+
+            float C = Vector2.Dot(d, ay);
+
+            float k = InvMassA + InvMassB + InvIA * _sAy * _sAy + InvIB * _sBy * _sBy;
+
+            float impulse;
+            if (k != 0.0f)
+            {
+                impulse = -C / k;
+            }
+            else
+            {
+                impulse = 0.0f;
+            }
+
+            Vector2 P = impulse * ay;
+            float LB = impulse * sBy;
+
+            xB += InvMassB * P;
+            angleB += InvIB * LB;
+
+            // TODO_ERIN remove need for this.
+            bB.Sweep.C = xB;
+            bB.Sweep.A = angleB;
+            bB.SynchronizeTransform();
+
+            return Math.Abs(C) <= Settings.LinearSlop;
+        }
+
+        public float GetMotorTorque(float invDt)
+        {
+            return invDt * _motorImpulse;
+        }
+    }
+}

axios/Dynamics/Joints/FixedMouseJoint.cs

@@ -0,0 +1,209 @@
+/*
+* Farseer Physics Engine based on Box2D.XNA port:
+* Copyright (c) 2010 Ian Qvist
+* 
+* 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 FarseerPhysics.Common;
+using Microsoft.Xna.Framework;
+
+namespace FarseerPhysics.Dynamics.Joints
+{
+    /// <summary>
+    /// 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.
+    /// </summary>
+    public class FixedMouseJoint : Joint
+    {
+        public Vector2 LocalAnchorA;
+        private Vector2 _C; // position error
+        private float _beta;
+        private float _gamma;
+        private Vector2 _impulse;
+        private Mat22 _mass; // effective mass for point-to-point constraint.
+
+        private Vector2 _worldAnchor;
+
+        /// <summary>
+        /// This requires a world target point,
+        /// tuning parameters, and the time step.
+        /// </summary>
+        /// <param name="body">The body.</param>
+        /// <param name="worldAnchor">The target.</param>
+        public FixedMouseJoint(Body body, Vector2 worldAnchor)
+            : base(body)
+        {
+            JointType = JointType.FixedMouse;
+            Frequency = 5.0f;
+            DampingRatio = 0.7f;
+
+            Debug.Assert(worldAnchor.IsValid());
+
+            Transform xf1;
+            BodyA.GetTransform(out xf1);
+
+            _worldAnchor = worldAnchor;
+            LocalAnchorA = BodyA.GetLocalPoint(worldAnchor);
+        }
+
+        public override Vector2 WorldAnchorA
+        {
+            get { return BodyA.GetWorldPoint(LocalAnchorA); }
+        }
+
+        public override Vector2 WorldAnchorB
+        {
+            get { return _worldAnchor; }
+            set
+            {
+                BodyA.Awake = true;
+                _worldAnchor = value;
+            }
+        }
+
+        /// <summary>
+        /// 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).
+        /// </summary>
+        public float MaxForce { get; set; }
+
+        /// <summary>
+        /// The response speed.
+        /// </summary>
+        public float Frequency { get; set; }
+
+        /// <summary>
+        /// The damping ratio. 0 = no damping, 1 = critical damping.
+        /// </summary>
+        public float DampingRatio { get; set; }
+
+        public override Vector2 GetReactionForce(float inv_dt)
+        {
+            return inv_dt * _impulse;
+        }
+
+        public override float GetReactionTorque(float inv_dt)
+        {
+            return inv_dt * 0.0f;
+        }
+
+        internal override void InitVelocityConstraints(ref TimeStep step)
+        {
+            Body b = BodyA;
+
+            float mass = b.Mass;
+
+            // Frequency
+            float omega = 2.0f * Settings.Pi * Frequency;
+
+            // 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.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, LocalAnchorA - b.LocalCenter);
+
+            // 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.Inverse;
+
+            _C = b.Sweep.C + r - _worldAnchor;
+
+            // Cheat with some damping
+            b.AngularVelocityInternal *= 0.98f;
+
+            // Warm starting.
+            _impulse *= step.dtRatio;
+            b.LinearVelocityInternal += invMass * _impulse;
+            b.AngularVelocityInternal += invI * MathUtils.Cross(r, _impulse);
+        }
+
+        internal override void SolveVelocityConstraints(ref TimeStep step)
+        {
+            Body b = BodyA;
+
+            Transform xf1;
+            b.GetTransform(out xf1);
+
+            Vector2 r = MathUtils.Multiply(ref xf1.R, LocalAnchorA - b.LocalCenter);
+
+            // Cdot = v + cross(w, r)
+            Vector2 Cdot = b.LinearVelocityInternal + MathUtils.Cross(b.AngularVelocityInternal, 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.LinearVelocityInternal += b.InvMass * impulse;
+            b.AngularVelocityInternal += b.InvI * MathUtils.Cross(r, impulse);
+        }
+
+        internal override bool SolvePositionConstraints()
+        {
+            return true;
+        }
+    }
+}

axios/Dynamics/Joints/FixedPrismaticJoint.cs

@@ -0,0 +1,636 @@
+/*
+* Farseer Physics Engine based on Box2D.XNA port:
+* Copyright (c) 2010 Ian Qvist
+* 
+* 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 FarseerPhysics.Common;
+using Microsoft.Xna.Framework;
+
+namespace FarseerPhysics.Dynamics.Joints
+{
+    // Linear constraint (point-to-line)
+    // d = p2 - p1 = x2 + r2 - x1 - r1
+    // C = dot(perp, d)
+    // Cdot = dot(d, cross(w1, perp)) + dot(perp, v2 + cross(w2, r2) - v1 - cross(w1, r1))
+    //      = -dot(perp, v1) - dot(cross(d + r1, perp), w1) + dot(perp, v2) + dot(cross(r2, perp), v2)
+    // J = [-perp, -cross(d + r1, perp), perp, cross(r2,perp)]
+    //
+    // Angular constraint
+    // C = a2 - a1 + a_initial
+    // Cdot = w2 - w1
+    // J = [0 0 -1 0 0 1]
+    //
+    // K = J * invM * JT
+    //
+    // J = [-a -s1 a s2]
+    //     [0  -1  0  1]
+    // a = perp
+    // s1 = cross(d + r1, a) = cross(p2 - x1, a)
+    // s2 = cross(r2, a) = cross(p2 - x2, a)
+    // Motor/Limit linear constraint
+    // C = dot(ax1, d)
+    // Cdot = = -dot(ax1, v1) - dot(cross(d + r1, ax1), w1) + dot(ax1, v2) + dot(cross(r2, ax1), v2)
+    // J = [-ax1 -cross(d+r1,ax1) ax1 cross(r2,ax1)]
+    // Block Solver
+    // We develop a block solver that includes the joint limit. This makes the limit stiff (inelastic) even
+    // when the mass has poor distribution (leading to large torques about the joint anchor points).
+    //
+    // The Jacobian has 3 rows:
+    // J = [-uT -s1 uT s2] // linear
+    //     [0   -1   0  1] // angular
+    //     [-vT -a1 vT a2] // limit
+    //
+    // u = perp
+    // v = axis
+    // s1 = cross(d + r1, u), s2 = cross(r2, u)
+    // a1 = cross(d + r1, v), a2 = cross(r2, v)
+    // M * (v2 - v1) = JT * df
+    // J * v2 = bias
+    //
+    // v2 = v1 + invM * JT * df
+    // J * (v1 + invM * JT * df) = bias
+    // K * df = bias - J * v1 = -Cdot
+    // K = J * invM * JT
+    // Cdot = J * v1 - bias
+    //
+    // Now solve for f2.
+    // df = f2 - f1
+    // K * (f2 - f1) = -Cdot
+    // f2 = invK * (-Cdot) + f1
+    //
+    // Clamp accumulated limit impulse.
+    // lower: f2(3) = max(f2(3), 0)
+    // upper: f2(3) = min(f2(3), 0)
+    //
+    // Solve for correct f2(1:2)
+    // K(1:2, 1:2) * f2(1:2) = -Cdot(1:2) - K(1:2,3) * f2(3) + K(1:2,1:3) * f1
+    //                       = -Cdot(1:2) - K(1:2,3) * f2(3) + K(1:2,1:2) * f1(1:2) + K(1:2,3) * f1(3)
+    // K(1:2, 1:2) * f2(1:2) = -Cdot(1:2) - K(1:2,3) * (f2(3) - f1(3)) + K(1:2,1:2) * f1(1:2)
+    // f2(1:2) = invK(1:2,1:2) * (-Cdot(1:2) - K(1:2,3) * (f2(3) - f1(3))) + f1(1:2)
+    //
+    // Now compute impulse to be applied:
+    // df = f2 - f1
+
+    /// <summary>
+    /// A prismatic joint. This joint provides one degree of freedom: translation
+    /// along an axis fixed in body1. Relative rotation is prevented. 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.
+    /// </summary>
+    public class FixedPrismaticJoint : Joint
+    {
+        private Mat33 _K;
+        private float _a1, _a2;
+        private Vector2 _axis;
+        private bool _enableLimit;
+        private bool _enableMotor;
+        private Vector3 _impulse;
+        private LimitState _limitState;
+        private Vector2 _localXAxis1;
+        private Vector2 _localYAxis1;
+        private float _lowerTranslation;
+        private float _maxMotorForce;
+        private float _motorMass; // effective mass for motor/limit translational constraint.
+        private float _motorSpeed;
+        private Vector2 _perp;
+        private float _refAngle;
+        private float _s1, _s2;
+        private float _upperTranslation;
+
+        /// <summary>
+        /// 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.
+        /// </summary>
+        /// <param name="body">The body.</param>
+        /// <param name="worldAnchor">The anchor.</param>
+        /// <param name="axis">The axis.</param>
+        public FixedPrismaticJoint(Body body, Vector2 worldAnchor, Vector2 axis)
+            : base(body)
+        {
+            JointType = JointType.FixedPrismatic;
+
+            BodyB = BodyA;
+
+            LocalAnchorA = worldAnchor;
+            LocalAnchorB = BodyB.GetLocalPoint(worldAnchor);
+
+            _localXAxis1 = axis;
+            _localYAxis1 = MathUtils.Cross(1.0f, _localXAxis1);
+            _refAngle = BodyB.Rotation;
+
+            _limitState = LimitState.Inactive;
+        }
+
+        public Vector2 LocalAnchorA { get; set; }
+
+        public Vector2 LocalAnchorB { get; set; }
+
+        public override Vector2 WorldAnchorA
+        {
+            get { return LocalAnchorA; }
+        }
+
+        public override Vector2 WorldAnchorB
+        {
+            get { return BodyA.GetWorldPoint(LocalAnchorB); }
+            set { Debug.Assert(false, "You can't set the world anchor on this joint type."); }
+        }
+
+        /// <summary>
+        /// Get the current joint translation, usually in meters.
+        /// </summary>
+        /// <value></value>
+        public float JointTranslation
+        {
+            get
+            {
+                Vector2 d = BodyB.GetWorldPoint(LocalAnchorB) - LocalAnchorA;
+                Vector2 axis = _localXAxis1;
+
+                return Vector2.Dot(d, axis);
+            }
+        }
+
+        /// <summary>
+        /// Get the current joint translation speed, usually in meters per second.
+        /// </summary>
+        /// <value></value>
+        public float JointSpeed
+        {
+            get
+            {
+                Transform xf2;
+                BodyB.GetTransform(out xf2);
+
+                Vector2 r1 = LocalAnchorA;
+                Vector2 r2 = MathUtils.Multiply(ref xf2.R, LocalAnchorB - BodyB.LocalCenter);
+                Vector2 p1 = r1;
+                Vector2 p2 = BodyB.Sweep.C + r2;
+                Vector2 d = p2 - p1;
+                Vector2 axis = _localXAxis1;
+
+                Vector2 v1 = Vector2.Zero;
+                Vector2 v2 = BodyB.LinearVelocityInternal;
+                const float w1 = 0.0f;
+                float w2 = BodyB.AngularVelocityInternal;
+
+                float speed = Vector2.Dot(d, MathUtils.Cross(w1, axis)) +
+                              Vector2.Dot(axis, v2 + MathUtils.Cross(w2, r2) - v1 - MathUtils.Cross(w1, r1));
+                return speed;
+            }
+        }
+
+        /// <summary>
+        /// Is the joint limit enabled?
+        /// </summary>
+        /// <value><c>true</c> if [limit enabled]; otherwise, <c>false</c>.</value>
+        public bool LimitEnabled
+        {
+            get { return _enableLimit; }
+            set
+            {
+                Debug.Assert(BodyA.FixedRotation == false, "Warning: limits does currently not work with fixed rotation");
+
+                WakeBodies();
+                _enableLimit = value;
+            }
+        }
+
+        /// <summary>
+        /// Get the lower joint limit, usually in meters.
+        /// </summary>
+        /// <value></value>
+        public float LowerLimit
+        {
+            get { return _lowerTranslation; }
+            set
+            {
+                WakeBodies();
+                _lowerTranslation = value;
+            }
+        }
+
+        /// <summary>
+        /// Get the upper joint limit, usually in meters.
+        /// </summary>
+        /// <value></value>
+        public float UpperLimit
+        {
+            get { return _upperTranslation; }
+            set
+            {
+                WakeBodies();
+                _upperTranslation = value;
+            }
+        }
+
+        /// <summary>
+        /// Is the joint motor enabled?
+        /// </summary>
+        /// <value><c>true</c> if [motor enabled]; otherwise, <c>false</c>.</value>
+        public bool MotorEnabled
+        {
+            get { return _enableMotor; }
+            set
+            {
+                WakeBodies();
+                _enableMotor = value;
+            }
+        }
+
+        /// <summary>
+        /// Set the motor speed, usually in meters per second.
+        /// </summary>
+        /// <value>The speed.</value>
+        public float MotorSpeed
+        {
+            set
+            {
+                WakeBodies();
+                _motorSpeed = value;
+            }
+            get { return _motorSpeed; }
+        }
+
+        /// <summary>
+        /// Set the maximum motor force, usually in N.
+        /// </summary>
+        /// <value>The force.</value>
+        public float MaxMotorForce
+        {
+            set
+            {
+                WakeBodies();
+                _maxMotorForce = value;
+            }
+        }
+
+        /// <summary>
+        /// Get the current motor force, usually in N.
+        /// </summary>
+        /// <value></value>
+        public float MotorForce { get; set; }
+
+        public Vector2 LocalXAxis1
+        {
+            get { return _localXAxis1; }
+            set
+            {
+                _localXAxis1 = value;
+                _localYAxis1 = MathUtils.Cross(1.0f, _localXAxis1);
+            }
+        }
+
+        public override Vector2 GetReactionForce(float inv_dt)
+        {
+            return inv_dt * (_impulse.X * _perp + (MotorForce + _impulse.Z) * _axis);
+        }
+
+        public override float GetReactionTorque(float inv_dt)
+        {
+            return inv_dt * _impulse.Y;
+        }
+
+        internal override void InitVelocityConstraints(ref TimeStep step)
+        {
+            Body bB = BodyB;
+
+            LocalCenterA = Vector2.Zero;
+            LocalCenterB = bB.LocalCenter;
+
+            Transform xf2;
+            bB.GetTransform(out xf2);
+
+            // Compute the effective masses.
+            Vector2 r1 = LocalAnchorA;
+            Vector2 r2 = MathUtils.Multiply(ref xf2.R, LocalAnchorB - LocalCenterB);
+            Vector2 d = bB.Sweep.C + r2 - /* b1._sweep.Center - */ r1;
+
+            InvMassA = 0.0f;
+            InvIA = 0.0f;
+            InvMassB = bB.InvMass;
+            InvIB = bB.InvI;
+
+            // Compute motor Jacobian and effective mass.
+            {
+                _axis = _localXAxis1;
+                _a1 = MathUtils.Cross(d + r1, _axis);
+                _a2 = MathUtils.Cross(r2, _axis);
+
+                _motorMass = InvMassA + InvMassB + InvIA * _a1 * _a1 + InvIB * _a2 * _a2;
+
+                if (_motorMass > Settings.Epsilon)
+                {
+                    _motorMass = 1.0f / _motorMass;
+                }
+            }
+
+            // Prismatic constraint.
+            {
+                _perp = _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 + i2 * _s2;
+                float k13 = i1 * _s1 * _a1 + i2 * _s2 * _a2;
+                float k22 = i1 + i2;
+                float k23 = i1 * _a1 + i2 * _a2;
+                float k33 = m1 + m2 + i1 * _a1 * _a1 + i2 * _a2 * _a2;
+
+                _K.Col1 = new Vector3(k11, k12, k13);
+                _K.Col2 = new Vector3(k12, k22, k23);
+                _K.Col3 = new Vector3(k13, k23, k33);
+            }
+
+            // Compute motor and limit terms.
+            if (_enableLimit)
+            {
+                float jointTranslation = Vector2.Dot(_axis, d);
+                if (Math.Abs(_upperTranslation - _lowerTranslation) < 2.0f * Settings.LinearSlop)
+                {
+                    _limitState = LimitState.Equal;
+                }
+                else if (jointTranslation <= _lowerTranslation)
+                {
+                    if (_limitState != LimitState.AtLower)
+                    {
+                        _limitState = LimitState.AtLower;
+                        _impulse.Z = 0.0f;
+                    }
+                }
+                else if (jointTranslation >= _upperTranslation)
+                {
+                    if (_limitState != LimitState.AtUpper)
+                    {
+                        _limitState = LimitState.AtUpper;
+                        _impulse.Z = 0.0f;
+                    }
+                }
+                else
+                {
+                    _limitState = LimitState.Inactive;
+                    _impulse.Z = 0.0f;
+                }
+            }
+            else
+            {
+                _limitState = LimitState.Inactive;
+            }
+
+            if (_enableMotor == false)
+            {
+                MotorForce = 0.0f;
+            }
+
+            if (Settings.EnableWarmstarting)
+            {
+                // Account for variable time step.
+                _impulse *= step.dtRatio;
+                MotorForce *= step.dtRatio;
+
+                Vector2 P = _impulse.X * _perp + (MotorForce + _impulse.Z) * _axis;
+                float L2 = _impulse.X * _s2 + _impulse.Y + (MotorForce + _impulse.Z) * _a2;
+
+                bB.LinearVelocityInternal += InvMassB * P;
+                bB.AngularVelocityInternal += InvIB * L2;
+            }
+            else
+            {
+                _impulse = Vector3.Zero;
+                MotorForce = 0.0f;
+            }
+        }
+
+        internal override void SolveVelocityConstraints(ref TimeStep step)
+        {
+            Body bB = BodyB;
+
+            Vector2 v1 = Vector2.Zero;
+            float w1 = 0.0f;
+            Vector2 v2 = bB.LinearVelocityInternal;
+            float w2 = bB.AngularVelocityInternal;
+
+            // 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 = MotorForce;
+                float maxImpulse = step.dt * _maxMotorForce;
+                MotorForce = MathUtils.Clamp(MotorForce + impulse, -maxImpulse, maxImpulse);
+                impulse = MotorForce - 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;
+            }
+
+            Vector2 Cdot1 = new Vector2(Vector2.Dot(_perp, v2 - v1) + _s2 * w2 - _s1 * w1, w2 - 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;
+                Vector3 Cdot = new Vector3(Cdot1.X, Cdot1.Y, Cdot2);
+
+                Vector3 f1 = _impulse;
+                Vector3 df = _K.Solve33(-Cdot);
+                _impulse += df;
+
+                if (_limitState == LimitState.AtLower)
+                {
+                    _impulse.Z = Math.Max(_impulse.Z, 0.0f);
+                }
+                else if (_limitState == LimitState.AtUpper)
+                {
+                    _impulse.Z = Math.Min(_impulse.Z, 0.0f);
+                }
+
+                // f2(1:2) = invK(1:2,1:2) * (-Cdot(1:2) - K(1:2,3) * (f2(3) - f1(3))) + f1(1:2)
+                Vector2 b = -Cdot1 - (_impulse.Z - f1.Z) * new Vector2(_K.Col3.X, _K.Col3.Y);
+                Vector2 f2r = _K.Solve22(b) + new Vector2(f1.X, f1.Y);
+                _impulse.X = f2r.X;
+                _impulse.Y = f2r.Y;
+
+                df = _impulse - f1;
+
+                Vector2 P = df.X * _perp + df.Z * _axis;
+                float L2 = df.X * _s2 + df.Y + df.Z * _a2;
+
+                v2 += InvMassB * P;
+                w2 += InvIB * L2;
+            }
+            else
+            {
+                // Limit is inactive, just solve the prismatic constraint in block form.
+                Vector2 df = _K.Solve22(-Cdot1);
+                _impulse.X += df.X;
+                _impulse.Y += df.Y;
+
+                Vector2 P = df.X * _perp;
+                float L2 = df.X * _s2 + df.Y;
+
+                v2 += InvMassB * P;
+                w2 += InvIB * L2;
+            }
+
+            bB.LinearVelocityInternal = v2;
+            bB.AngularVelocityInternal = w2;
+        }
+
+        internal override bool SolvePositionConstraints()
+        {
+            //Body b1 = BodyA;
+            Body b2 = BodyB;
+
+            Vector2 c1 = Vector2.Zero; // b1._sweep.Center;
+            float a1 = 0.0f; // b1._sweep.Angle;
+
+            Vector2 c2 = b2.Sweep.C;
+            float a2 = b2.Sweep.A;
+
+            // Solve linear limit constraint.
+            float linearError = 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, LocalAnchorA - LocalCenterA);
+            Vector2 r2 = MathUtils.Multiply(ref R2, LocalAnchorB - 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.LinearSlop)
+                {
+                    // Prevent large angular corrections
+                    C2 = MathUtils.Clamp(translation, -Settings.MaxLinearCorrection, Settings.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.LinearSlop,
+                                         -Settings.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.LinearSlop, 0.0f,
+                                         Settings.MaxLinearCorrection);
+                    linearError = translation - _upperTranslation;
+                    active = true;
+                }
+            }
+
+            _perp = MathUtils.Multiply(ref R1, _localYAxis1);
+
+            _s1 = MathUtils.Cross(d + r1, _perp);
+            _s2 = MathUtils.Cross(r2, _perp);
+
+            Vector3 impulse;
+            Vector2 C1 = new Vector2(Vector2.Dot(_perp, d), a2 - a1 - _refAngle);
+
+            linearError = Math.Max(linearError, Math.Abs(C1.X));
+            float angularError = Math.Abs(C1.Y);
+
+            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 + i2 * _s2;
+                float k13 = i1 * _s1 * _a1 + i2 * _s2 * _a2;
+                float k22 = i1 + i2;
+                float k23 = i1 * _a1 + i2 * _a2;
+                float k33 = m1 + m2 + i1 * _a1 * _a1 + i2 * _a2 * _a2;
+
+                _K.Col1 = new Vector3(k11, k12, k13);
+                _K.Col2 = new Vector3(k12, k22, k23);
+                _K.Col3 = new Vector3(k13, k23, k33);
+
+                Vector3 C = new Vector3(-C1.X, -C1.Y, -C2);
+                impulse = _K.Solve33(C); // negated above
+            }
+            else
+            {
+                float m1 = InvMassA, m2 = InvMassB;
+                float i1 = InvIA, i2 = InvIB;
+
+                float k11 = m1 + m2 + i1 * _s1 * _s1 + i2 * _s2 * _s2;
+                float k12 = i1 * _s1 + i2 * _s2;
+                float k22 = i1 + i2;
+
+                _K.Col1 = new Vector3(k11, k12, 0.0f);
+                _K.Col2 = new Vector3(k12, k22, 0.0f);
+
+                Vector2 impulse1 = _K.Solve22(-C1);
+                impulse.X = impulse1.X;
+                impulse.Y = impulse1.Y;
+                impulse.Z = 0.0f;
+            }
+
+            Vector2 P = impulse.X * _perp + impulse.Z * _axis;
+            float L2 = impulse.X * _s2 + impulse.Y + impulse.Z * _a2;
+
+            c2 += InvMassB * P;
+            a2 += InvIB * L2;
+
+            // TODO_ERIN remove need for this.
+            b2.Sweep.C = c2;
+            b2.Sweep.A = a2;
+            b2.SynchronizeTransform();
+
+            return linearError <= Settings.LinearSlop && angularError <= Settings.AngularSlop;
+        }
+    }
+}

axios/Dynamics/Joints/FixedRevoluteJoint.cs

@@ -0,0 +1,541 @@
+/*
+* Farseer Physics Engine based on Box2D.XNA port:
+* Copyright (c) 2010 Ian Qvist
+* 
+* 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 FarseerPhysics.Common;
+using Microsoft.Xna.Framework;
+
+namespace FarseerPhysics.Dynamics.Joints
+{
+    /// <summary>
+    /// A revolute joint rains to bodies to share a common point while they
+    /// are free to rotate about the point. The relative rotation about the shared
+    /// point is the joint angle. You can limit the relative rotation with
+    /// a joint limit that specifies a lower and upper angle. You can use a motor
+    /// to drive the relative rotation about the shared point. A maximum motor torque
+    /// is provided so that infinite forces are not generated.
+    /// </summary>
+    public class FixedRevoluteJoint : Joint
+    {
+        private bool _enableLimit;
+        private bool _enableMotor;
+        private Vector3 _impulse;
+        private LimitState _limitState;
+        private float _lowerAngle;
+        private Mat33 _mass; // effective mass for point-to-point constraint.
+        private float _maxMotorTorque;
+        private float _motorImpulse;
+        private float _motorMass; // effective mass for motor/limit angular constraint.
+        private float _motorSpeed;
+        private float _upperAngle;
+        private Vector2 _worldAnchor;
+
+        /// <summary>
+        /// Initialize the bodies, anchors, and reference angle using the world
+        /// anchor.
+        /// This requires defining an
+        /// anchor point where the bodies are joined. The definition
+        /// uses local anchor points so that the initial configuration
+        /// can violate the constraint slightly. You also need to
+        /// specify the initial relative angle for joint limits. This
+        /// helps when saving and loading a game.
+        /// The local anchor points are measured from the body's origin
+        /// rather than the center of mass because:
+        /// 1. you might not know where the center of mass will be.
+        /// 2. if you add/remove shapes from a body and recompute the mass,
+        /// the joints will be broken.
+        /// </summary>
+        /// <param name="body">The body.</param>
+        /// <param name="bodyAnchor">The body anchor.</param>
+        /// <param name="worldAnchor">The world anchor.</param>
+        public FixedRevoluteJoint(Body body, Vector2 bodyAnchor, Vector2 worldAnchor)
+            : base(body)
+        {
+            JointType = JointType.FixedRevolute;
+
+            // Changed to local coordinates.
+            LocalAnchorA = bodyAnchor;
+            _worldAnchor = worldAnchor;
+
+            ReferenceAngle = -BodyA.Rotation;
+
+            _impulse = Vector3.Zero;
+
+            _limitState = LimitState.Inactive;
+        }
+
+        public override Vector2 WorldAnchorA
+        {
+            get { return BodyA.GetWorldPoint(LocalAnchorA); }
+        }
+
+        public override Vector2 WorldAnchorB
+        {
+            get { return _worldAnchor; }
+            set { _worldAnchor = value; }
+        }
+
+        public Vector2 LocalAnchorA { get; set; }
+
+        public float ReferenceAngle { get; set; }
+
+        /// <summary>
+        /// Get the current joint angle in radians.
+        /// </summary>
+        /// <value></value>
+        public float JointAngle
+        {
+            get { return BodyA.Sweep.A - ReferenceAngle; }
+        }
+
+        /// <summary>
+        /// Get the current joint angle speed in radians per second.
+        /// </summary>
+        /// <value></value>
+        public float JointSpeed
+        {
+            get { return BodyA.AngularVelocityInternal; }
+        }
+
+        /// <summary>
+        /// Is the joint limit enabled?
+        /// </summary>
+        /// <value><c>true</c> if [limit enabled]; otherwise, <c>false</c>.</value>
+        public bool LimitEnabled
+        {
+            get { return _enableLimit; }
+            set
+            {
+                WakeBodies();
+                _enableLimit = value;
+            }
+        }
+
+        /// <summary>
+        /// Get the lower joint limit in radians.
+        /// </summary>
+        /// <value></value>
+        public float LowerLimit
+        {
+            get { return _lowerAngle; }
+            set
+            {
+                WakeBodies();
+                _lowerAngle = value;
+            }
+        }
+
+        /// <summary>
+        /// Get the upper joint limit in radians.
+        /// </summary>
+        /// <value></value>
+        public float UpperLimit
+        {
+            get { return _upperAngle; }
+            set
+            {
+                WakeBodies();
+                _upperAngle = value;
+            }
+        }
+
+        /// <summary>
+        /// Is the joint motor enabled?
+        /// </summary>
+        /// <value><c>true</c> if [motor enabled]; otherwise, <c>false</c>.</value>
+        public bool MotorEnabled
+        {
+            get { return _enableMotor; }
+            set
+            {
+                WakeBodies();
+                _enableMotor = value;
+            }
+        }
+
+        /// <summary>
+        /// Set the motor speed in radians per second.
+        /// </summary>
+        /// <value>The speed.</value>
+        public float MotorSpeed
+        {
+            set
+            {
+                WakeBodies();
+                _motorSpeed = value;
+            }
+            get { return _motorSpeed; }
+        }
+
+        /// <summary>
+        /// Set the maximum motor torque, usually in N-m.
+        /// </summary>
+        /// <value>The torque.</value>
+        public float MaxMotorTorque
+        {
+            set
+            {
+                WakeBodies();
+                _maxMotorTorque = value;
+            }
+            get { return _maxMotorTorque; }
+        }
+
+        /// <summary>
+        /// Get the current motor torque, usually in N-m.
+        /// </summary>
+        /// <value></value>
+        public float MotorTorque
+        {
+            get { return _motorImpulse; }
+            set
+            {
+                WakeBodies();
+                _motorImpulse = value;
+            }
+        }
+
+        public override Vector2 GetReactionForce(float inv_dt)
+        {
+            return inv_dt * new Vector2(_impulse.X, _impulse.Y);
+        }
+
+        public override float GetReactionTorque(float inv_dt)
+        {
+            return inv_dt * _impulse.Z;
+        }
+
+        internal override void InitVelocityConstraints(ref TimeStep step)
+        {
+            Body b1 = BodyA;
+
+            if (_enableMotor || _enableLimit)
+            {
+                // You cannot create a rotation limit between bodies that
+                // both have fixed rotation.
+                Debug.Assert(b1.InvI > 0.0f /* || b2._invI > 0.0f*/);
+            }
+
+            // Compute the effective mass matrix.
+            Transform xf1;
+            b1.GetTransform(out xf1);
+
+            Vector2 r1 = MathUtils.Multiply(ref xf1.R, LocalAnchorA - b1.LocalCenter);
+            Vector2 r2 = _worldAnchor; // MathUtils.Multiply(ref xf2.R, LocalAnchorB - b2.LocalCenter);
+
+            // J = [-I -r1_skew I r2_skew]
+            //     [ 0       -1 0       1]
+            // r_skew = [-ry; rx]
+
+            // Matlab
+            // K = [ m1+r1y^2*i1+m2+r2y^2*i2,  -r1y*i1*r1x-r2y*i2*r2x,          -r1y*i1-r2y*i2]
+            //     [  -r1y*i1*r1x-r2y*i2*r2x, m1+r1x^2*i1+m2+r2x^2*i2,           r1x*i1+r2x*i2]
+            //     [          -r1y*i1-r2y*i2,           r1x*i1+r2x*i2,                   i1+i2]
+
+            float m1 = b1.InvMass;
+            const float m2 = 0;
+            float i1 = b1.InvI;
+            const float i2 = 0;
+
+            _mass.Col1.X = m1 + m2 + r1.Y * r1.Y * i1 + r2.Y * r2.Y * i2;
+            _mass.Col2.X = -r1.Y * r1.X * i1 - r2.Y * r2.X * i2;
+            _mass.Col3.X = -r1.Y * i1 - r2.Y * i2;
+            _mass.Col1.Y = _mass.Col2.X;
+            _mass.Col2.Y = m1 + m2 + r1.X * r1.X * i1 + r2.X * r2.X * i2;
+            _mass.Col3.Y = r1.X * i1 + r2.X * i2;
+            _mass.Col1.Z = _mass.Col3.X;
+            _mass.Col2.Z = _mass.Col3.Y;
+            _mass.Col3.Z = i1 + i2;
+
+            _motorMass = i1 + i2;
+            if (_motorMass > 0.0f)
+            {
+                _motorMass = 1.0f / _motorMass;
+            }
+
+            if (_enableMotor == false)
+            {
+                _motorImpulse = 0.0f;
+            }
+
+            if (_enableLimit)
+            {
+                float jointAngle = 0 - b1.Sweep.A - ReferenceAngle;
+                if (Math.Abs(_upperAngle - _lowerAngle) < 2.0f * Settings.AngularSlop)
+                {
+                    _limitState = LimitState.Equal;
+                }
+                else if (jointAngle <= _lowerAngle)
+                {
+                    if (_limitState != LimitState.AtLower)
+                    {
+                        _impulse.Z = 0.0f;
+                    }
+                    _limitState = LimitState.AtLower;
+                }
+                else if (jointAngle >= _upperAngle)
+                {
+                    if (_limitState != LimitState.AtUpper)
+                    {
+                        _impulse.Z = 0.0f;
+                    }
+                    _limitState = LimitState.AtUpper;
+                }
+                else
+                {
+                    _limitState = LimitState.Inactive;
+                    _impulse.Z = 0.0f;
+                }
+            }
+            else
+            {
+                _limitState = LimitState.Inactive;
+            }
+
+            if (Settings.EnableWarmstarting)
+            {
+                // Scale impulses to support a variable time step.
+                _impulse *= step.dtRatio;
+                _motorImpulse *= step.dtRatio;
+
+                Vector2 P = new Vector2(_impulse.X, _impulse.Y);
+
+                b1.LinearVelocityInternal -= m1 * P;
+                b1.AngularVelocityInternal -= i1 * (MathUtils.Cross(r1, P) + _motorImpulse + _impulse.Z);
+            }
+            else
+            {
+                _impulse = Vector3.Zero;
+                _motorImpulse = 0.0f;
+            }
+        }
+
+        internal override void SolveVelocityConstraints(ref TimeStep step)
+        {
+            Body b1 = BodyA;
+
+            Vector2 v1 = b1.LinearVelocityInternal;
+            float w1 = b1.AngularVelocityInternal;
+            Vector2 v2 = Vector2.Zero;
+            const float w2 = 0;
+
+            float m1 = b1.InvMass;
+            float i1 = b1.InvI;
+
+            // Solve motor constraint.
+            if (_enableMotor && _limitState != LimitState.Equal)
+            {
+                float Cdot = w2 - w1 - _motorSpeed;
+                float impulse = _motorMass * (-Cdot);
+                float oldImpulse = _motorImpulse;
+                float maxImpulse = step.dt * _maxMotorTorque;
+                _motorImpulse = MathUtils.Clamp(_motorImpulse + impulse, -maxImpulse, maxImpulse);
+                impulse = _motorImpulse - oldImpulse;
+
+                w1 -= i1 * impulse;
+            }
+
+            // Solve limit constraint.
+            if (_enableLimit && _limitState != LimitState.Inactive)
+            {
+                Transform xf1;
+                b1.GetTransform(out xf1);
+
+                Vector2 r1 = MathUtils.Multiply(ref xf1.R, LocalAnchorA - b1.LocalCenter);
+                Vector2 r2 = _worldAnchor;
+
+                // Solve point-to-point constraint
+                Vector2 Cdot1 = v2 + MathUtils.Cross(w2, r2) - v1 - MathUtils.Cross(w1, r1);
+                float Cdot2 = w2 - w1;
+                Vector3 Cdot = new Vector3(Cdot1.X, Cdot1.Y, Cdot2);
+
+                Vector3 impulse = _mass.Solve33(-Cdot);
+
+                if (_limitState == LimitState.Equal)
+                {
+                    _impulse += impulse;
+                }
+                else if (_limitState == LimitState.AtLower)
+                {
+                    float newImpulse = _impulse.Z + impulse.Z;
+                    if (newImpulse < 0.0f)
+                    {
+                        Vector2 reduced = _mass.Solve22(-Cdot1);
+                        impulse.X = reduced.X;
+                        impulse.Y = reduced.Y;
+                        impulse.Z = -_impulse.Z;
+                        _impulse.X += reduced.X;
+                        _impulse.Y += reduced.Y;
+                        _impulse.Z = 0.0f;
+                    }
+                }
+                else if (_limitState == LimitState.AtUpper)
+                {
+                    float newImpulse = _impulse.Z + impulse.Z;
+                    if (newImpulse > 0.0f)
+                    {
+                        Vector2 reduced = _mass.Solve22(-Cdot1);
+                        impulse.X = reduced.X;
+                        impulse.Y = reduced.Y;
+                        impulse.Z = -_impulse.Z;
+                        _impulse.X += reduced.X;
+                        _impulse.Y += reduced.Y;
+                        _impulse.Z = 0.0f;
+                    }
+                }
+
+                Vector2 P = new Vector2(impulse.X, impulse.Y);
+
+                v1 -= m1 * P;
+                w1 -= i1 * (MathUtils.Cross(r1, P) + impulse.Z);
+            }
+            else
+            {
+                Transform xf1;
+                b1.GetTransform(out xf1);
+
+                Vector2 r1 = MathUtils.Multiply(ref xf1.R, LocalAnchorA - b1.LocalCenter);
+                Vector2 r2 = _worldAnchor;
+
+                // Solve point-to-point constraint
+                Vector2 Cdot = v2 + MathUtils.Cross(w2, r2) - v1 - MathUtils.Cross(w1, r1);
+                Vector2 impulse = _mass.Solve22(-Cdot);
+
+                _impulse.X += impulse.X;
+                _impulse.Y += impulse.Y;
+
+                v1 -= m1 * impulse;
+                w1 -= i1 * MathUtils.Cross(r1, impulse);
+            }
+
+            b1.LinearVelocityInternal = v1;
+            b1.AngularVelocityInternal = w1;
+        }
+
+        internal override bool SolvePositionConstraints()
+        {
+            // TODO_ERIN block solve with limit. COME ON ERIN
+
+            Body b1 = BodyA;
+
+            float angularError = 0.0f;
+            float positionError;
+
+            // Solve angular limit constraint.
+            if (_enableLimit && _limitState != LimitState.Inactive)
+            {
+                float angle = 0 - b1.Sweep.A - ReferenceAngle;
+                float limitImpulse = 0.0f;
+
+                if (_limitState == LimitState.Equal)
+                {
+                    // Prevent large angular corrections
+                    float C = MathUtils.Clamp(angle - _lowerAngle, -Settings.MaxAngularCorrection,
+                                              Settings.MaxAngularCorrection);
+                    limitImpulse = -_motorMass * C;
+                    angularError = Math.Abs(C);
+                }
+                else if (_limitState == LimitState.AtLower)
+                {
+                    float C = angle - _lowerAngle;
+                    angularError = -C;
+
+                    // Prevent large angular corrections and allow some slop.
+                    C = MathUtils.Clamp(C + Settings.AngularSlop, -Settings.MaxAngularCorrection, 0.0f);
+                    limitImpulse = -_motorMass * C;
+                }
+                else if (_limitState == LimitState.AtUpper)
+                {
+                    float C = angle - _upperAngle;
+                    angularError = C;
+
+                    // Prevent large angular corrections and allow some slop.
+                    C = MathUtils.Clamp(C - Settings.AngularSlop, 0.0f, Settings.MaxAngularCorrection);
+                    limitImpulse = -_motorMass * C;
+                }
+
+                b1.Sweep.A -= b1.InvI * limitImpulse;
+
+                b1.SynchronizeTransform();
+            }
+
+            // Solve point-to-point constraint.
+            {
+                Transform xf1;
+                b1.GetTransform(out xf1);
+
+                Vector2 r1 = MathUtils.Multiply(ref xf1.R, LocalAnchorA - b1.LocalCenter);
+                Vector2 r2 = _worldAnchor;
+
+                Vector2 C = Vector2.Zero + r2 - b1.Sweep.C - r1;
+                positionError = C.Length();
+
+                float invMass1 = b1.InvMass;
+                const float invMass2 = 0;
+                float invI1 = b1.InvI;
+                const float invI2 = 0;
+
+                // Handle large detachment.
+                const float k_allowedStretch = 10.0f * Settings.LinearSlop;
+                if (C.LengthSquared() > k_allowedStretch * k_allowedStretch)
+                {
+                    // Use a particle solution (no rotation).
+                    Vector2 u = C;
+                    u.Normalize();
+                    float k = invMass1 + invMass2;
+                    Debug.Assert(k > Settings.Epsilon);
+                    float m = 1.0f / k;
+                    Vector2 impulse2 = m * (-C);
+                    const float k_beta = 0.5f;
+                    b1.Sweep.C -= k_beta * invMass1 * impulse2;
+
+                    C = Vector2.Zero + r2 - b1.Sweep.C - r1;
+                }
+
+                Mat22 K1 = new Mat22(new Vector2(invMass1 + invMass2, 0.0f), new Vector2(0.0f, invMass1 + invMass2));
+                Mat22 K2 = new Mat22(new Vector2(invI1 * r1.Y * r1.Y, -invI1 * r1.X * r1.Y),
+                                     new Vector2(-invI1 * r1.X * r1.Y, invI1 * r1.X * r1.X));
+                Mat22 K3 = new Mat22(new Vector2(invI2 * r2.Y * r2.Y, -invI2 * r2.X * r2.Y),
+                                     new Vector2(-invI2 * r2.X * r2.Y, invI2 * r2.X * r2.X));
+
+                Mat22 Ka;
+                Mat22.Add(ref K1, ref K2, out Ka);
+
+                Mat22 K;
+                Mat22.Add(ref Ka, ref K3, out K);
+
+                Vector2 impulse = K.Solve(-C);
+
+                b1.Sweep.C -= b1.InvMass * impulse;
+                b1.Sweep.A -= b1.InvI * MathUtils.Cross(r1, impulse);
+
+                b1.SynchronizeTransform();
+            }
+
+            return positionError <= Settings.LinearSlop && angularError <= Settings.AngularSlop;
+        }
+    }
+}

axios/Dynamics/Joints/FrictionJoint.cs

@@ -0,0 +1,249 @@
+/*
+* Farseer Physics Engine based on Box2D.XNA port:
+* Copyright (c) 2010 Ian Qvist
+* 
+* 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 FarseerPhysics.Common;
+using Microsoft.Xna.Framework;
+
+namespace FarseerPhysics.Dynamics.Joints
+{
+    // 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
+
+    /// <summary>
+    /// Friction joint. This is used for top-down friction.
+    /// It provides 2D translational friction and angular friction.
+    /// </summary>
+    public class FrictionJoint : Joint
+    {
+        public Vector2 LocalAnchorA;
+        public Vector2 LocalAnchorB;
+        private float _angularImpulse;
+        private float _angularMass;
+        private Vector2 _linearImpulse;
+        private Mat22 _linearMass;
+
+        internal FrictionJoint()
+        {
+            JointType = JointType.Friction;
+        }
+
+        public FrictionJoint(Body bodyA, Body bodyB, Vector2 localAnchorA, Vector2 localAnchorB)
+            : base(bodyA, bodyB)
+        {
+            JointType = JointType.Friction;
+            LocalAnchorA = localAnchorA;
+            LocalAnchorB = localAnchorB;
+        }
+
+        public override Vector2 WorldAnchorA
+        {
+            get { return BodyA.GetWorldPoint(LocalAnchorA); }
+        }
+
+        public override Vector2 WorldAnchorB
+        {
+            get { return BodyB.GetWorldPoint(LocalAnchorB); }
+            set { Debug.Assert(false, "You can't set the world anchor on this joint type."); }
+        }
+
+        /// <summary>
+        /// The maximum friction force in N.
+        /// </summary>
+        public float MaxForce { get; set; }
+
+        /// <summary>
+        /// The maximum friction torque in N-m.
+        /// </summary>
+        public float MaxTorque { get; set; }
+
+        public override Vector2 GetReactionForce(float inv_dt)
+        {
+            return inv_dt * _linearImpulse;
+        }
+
+        public override float GetReactionTorque(float inv_dt)
+        {
+            return inv_dt * _angularImpulse;
+        }
+
+        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, LocalAnchorA - bA.LocalCenter);
+            Vector2 rB = MathUtils.Multiply(ref xfB.R, LocalAnchorB - bB.LocalCenter);
+
+            // 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.Inverse;
+
+            _angularMass = iA + iB;
+            if (_angularMass > 0.0f)
+            {
+                _angularMass = 1.0f / _angularMass;
+            }
+
+            if (Settings.EnableWarmstarting)
+            {
+                // Scale impulses to support a variable time step.
+                _linearImpulse *= step.dtRatio;
+                _angularImpulse *= step.dtRatio;
+
+                Vector2 P = new Vector2(_linearImpulse.X, _linearImpulse.Y);
+
+                bA.LinearVelocityInternal -= mA * P;
+                bA.AngularVelocityInternal -= iA * (MathUtils.Cross(rA, P) + _angularImpulse);
+
+                bB.LinearVelocityInternal += mB * P;
+                bB.AngularVelocityInternal += 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.LinearVelocityInternal;
+            float wA = bA.AngularVelocityInternal;
+            Vector2 vB = bB.LinearVelocityInternal;
+            float wB = bB.AngularVelocityInternal;
+
+            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, LocalAnchorA - bA.LocalCenter);
+            Vector2 rB = MathUtils.Multiply(ref xfB.R, LocalAnchorB - bB.LocalCenter);
+
+            // 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.LinearVelocityInternal = vA;
+            bA.AngularVelocityInternal = wA;
+            bB.LinearVelocityInternal = vB;
+            bB.AngularVelocityInternal = wB;
+        }
+
+        internal override bool SolvePositionConstraints()
+        {
+            return true;
+        }
+    }
+}

axios/Dynamics/Joints/GearJoint.cs

@@ -0,0 +1,350 @@
+/*
+* Farseer Physics Engine based on Box2D.XNA port:
+* Copyright (c) 2010 Ian Qvist
+* 
+* 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 FarseerPhysics.Common;
+using Microsoft.Xna.Framework;
+
+namespace FarseerPhysics.Dynamics.Joints
+{
+    /// <summary>
+    /// 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).
+    /// </summary>
+    public class GearJoint : Joint
+    {
+        private Jacobian _J;
+
+        private float _ant;
+        private FixedPrismaticJoint _fixedPrismatic1;
+        private FixedPrismaticJoint _fixedPrismatic2;
+        private FixedRevoluteJoint _fixedRevolute1;
+        private FixedRevoluteJoint _fixedRevolute2;
+        private float _impulse;
+        private float _mass;
+        private PrismaticJoint _prismatic1;
+        private PrismaticJoint _prismatic2;
+        private RevoluteJoint _revolute1;
+        private RevoluteJoint _revolute2;
+
+        /// <summary>
+        /// Requires two existing revolute or prismatic joints (any combination will work).
+        /// The provided joints must attach a dynamic body to a static body.
+        /// </summary>
+        /// <param name="jointA">The first joint.</param>
+        /// <param name="jointB">The second joint.</param>
+        /// <param name="ratio">The ratio.</param>
+        public GearJoint(Joint jointA, Joint jointB, float ratio)
+            : base(jointA.BodyA, jointA.BodyB)
+        {
+            JointType = JointType.Gear;
+            JointA = jointA;
+            JointB = jointB;
+            Ratio = ratio;
+
+            JointType type1 = jointA.JointType;
+            JointType type2 = jointB.JointType;
+
+            // Make sure its the right kind of joint
+            Debug.Assert(type1 == JointType.Revolute ||
+                         type1 == JointType.Prismatic ||
+                         type1 == JointType.FixedRevolute ||
+                         type1 == JointType.FixedPrismatic);
+            Debug.Assert(type2 == JointType.Revolute ||
+                         type2 == JointType.Prismatic ||
+                         type2 == JointType.FixedRevolute ||
+                         type2 == JointType.FixedPrismatic);
+
+            // In the case of a prismatic and revolute joint, the first body must be static.
+            if (type1 == JointType.Revolute || type1 == JointType.Prismatic)
+                Debug.Assert(jointA.BodyA.BodyType == BodyType.Static);
+            if (type2 == JointType.Revolute || type2 == JointType.Prismatic)
+                Debug.Assert(jointB.BodyA.BodyType == BodyType.Static);
+
+            float coordinate1 = 0.0f, coordinate2 = 0.0f;
+
+            switch (type1)
+            {
+                case JointType.Revolute:
+                    BodyA = jointA.BodyB;
+                    _revolute1 = (RevoluteJoint)jointA;
+                    LocalAnchor1 = _revolute1.LocalAnchorB;
+                    coordinate1 = _revolute1.JointAngle;
+                    break;
+                case JointType.Prismatic:
+                    BodyA = jointA.BodyB;
+                    _prismatic1 = (PrismaticJoint)jointA;
+                    LocalAnchor1 = _prismatic1.LocalAnchorB;
+                    coordinate1 = _prismatic1.JointTranslation;
+                    break;
+                case JointType.FixedRevolute:
+                    BodyA = jointA.BodyA;
+                    _fixedRevolute1 = (FixedRevoluteJoint)jointA;
+                    LocalAnchor1 = _fixedRevolute1.LocalAnchorA;
+                    coordinate1 = _fixedRevolute1.JointAngle;
+                    break;
+                case JointType.FixedPrismatic:
+                    BodyA = jointA.BodyA;
+                    _fixedPrismatic1 = (FixedPrismaticJoint)jointA;
+                    LocalAnchor1 = _fixedPrismatic1.LocalAnchorA;
+                    coordinate1 = _fixedPrismatic1.JointTranslation;
+                    break;
+            }
+
+            switch (type2)
+            {
+                case JointType.Revolute:
+                    BodyB = jointB.BodyB;
+                    _revolute2 = (RevoluteJoint)jointB;
+                    LocalAnchor2 = _revolute2.LocalAnchorB;
+                    coordinate2 = _revolute2.JointAngle;
+                    break;
+                case JointType.Prismatic:
+                    BodyB = jointB.BodyB;
+                    _prismatic2 = (PrismaticJoint)jointB;
+                    LocalAnchor2 = _prismatic2.LocalAnchorB;
+                    coordinate2 = _prismatic2.JointTranslation;
+                    break;
+                case JointType.FixedRevolute:
+                    BodyB = jointB.BodyA;
+                    _fixedRevolute2 = (FixedRevoluteJoint)jointB;
+                    LocalAnchor2 = _fixedRevolute2.LocalAnchorA;
+                    coordinate2 = _fixedRevolute2.JointAngle;
+                    break;
+                case JointType.FixedPrismatic:
+                    BodyB = jointB.BodyA;
+                    _fixedPrismatic2 = (FixedPrismaticJoint)jointB;
+                    LocalAnchor2 = _fixedPrismatic2.LocalAnchorA;
+                    coordinate2 = _fixedPrismatic2.JointTranslation;
+                    break;
+            }
+
+            _ant = coordinate1 + Ratio * coordinate2;
+        }
+
+        public override Vector2 WorldAnchorA
+        {
+            get { return BodyA.GetWorldPoint(LocalAnchor1); }
+        }
+
+        public override Vector2 WorldAnchorB
+        {
+            get { return BodyB.GetWorldPoint(LocalAnchor2); }
+            set { Debug.Assert(false, "You can't set the world anchor on this joint type."); }
+        }
+
+        /// <summary>
+        /// The gear ratio.
+        /// </summary>
+        public float Ratio { get; set; }
+
+        /// <summary>
+        /// The first revolute/prismatic joint attached to the gear joint.
+        /// </summary>
+        public Joint JointA { get; set; }
+
+        /// <summary>
+        /// The second revolute/prismatic joint attached to the gear joint.
+        /// </summary>
+        public Joint JointB { get; set; }
+
+        public Vector2 LocalAnchor1 { get; private set; }
+        public Vector2 LocalAnchor2 { get; private set; }
+
+        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.LocalCenter);
+            Vector2 P = _impulse * _J.LinearB;
+            float L = _impulse * _J.AngularB - MathUtils.Cross(r, P);
+            return inv_dt * L;
+        }
+
+        internal override void InitVelocityConstraints(ref TimeStep step)
+        {
+            Body b1 = BodyA;
+            Body b2 = BodyB;
+
+            float K = 0.0f;
+            _J.SetZero();
+
+            if (_revolute1 != null || _fixedRevolute1 != null)
+            {
+                _J.AngularA = -1.0f;
+                K += b1.InvI;
+            }
+            else
+            {
+                Vector2 ug;
+                if (_prismatic1 != null)
+                    ug = _prismatic1.LocalXAxis1; // MathUtils.Multiply(ref xfg1.R, _prismatic1.LocalXAxis1);
+                else
+                    ug = _fixedPrismatic1.LocalXAxis1; // MathUtils.Multiply(ref xfg1.R, _prismatic1.LocalXAxis1);
+
+                Transform xf1 /*, xfg1*/;
+                b1.GetTransform(out xf1);
+                //g1.GetTransform(out xfg1);
+
+
+                Vector2 r = MathUtils.Multiply(ref xf1.R, LocalAnchor1 - b1.LocalCenter);
+                float crug = MathUtils.Cross(r, ug);
+                _J.LinearA = -ug;
+                _J.AngularA = -crug;
+                K += b1.InvMass + b1.InvI * crug * crug;
+            }
+
+            if (_revolute2 != null || _fixedRevolute2 != null)
+            {
+                _J.AngularB = -Ratio;
+                K += Ratio * Ratio * b2.InvI;
+            }
+            else
+            {
+                Vector2 ug;
+                if (_prismatic2 != null)
+                    ug = _prismatic2.LocalXAxis1; // MathUtils.Multiply(ref xfg1.R, _prismatic1.LocalXAxis1);
+                else
+                    ug = _fixedPrismatic2.LocalXAxis1; // MathUtils.Multiply(ref xfg1.R, _prismatic1.LocalXAxis1);
+
+                Transform /*xfg1,*/ xf2;
+                //g1.GetTransform(out xfg1);
+                b2.GetTransform(out xf2);
+
+                Vector2 r = MathUtils.Multiply(ref xf2.R, LocalAnchor2 - b2.LocalCenter);
+                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 (Settings.EnableWarmstarting)
+            {
+                // Warm starting.
+                b1.LinearVelocityInternal += b1.InvMass * _impulse * _J.LinearA;
+                b1.AngularVelocityInternal += b1.InvI * _impulse * _J.AngularA;
+                b2.LinearVelocityInternal += b2.InvMass * _impulse * _J.LinearB;
+                b2.AngularVelocityInternal += 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.LinearVelocityInternal, b1.AngularVelocityInternal,
+                                    b2.LinearVelocityInternal, b2.AngularVelocityInternal);
+
+            float impulse = _mass * (-Cdot);
+            _impulse += impulse;
+
+            b1.LinearVelocityInternal += b1.InvMass * impulse * _J.LinearA;
+            b1.AngularVelocityInternal += b1.InvI * impulse * _J.AngularA;
+            b2.LinearVelocityInternal += b2.InvMass * impulse * _J.LinearB;
+            b2.AngularVelocityInternal += b2.InvI * impulse * _J.AngularB;
+        }
+
+        internal override bool SolvePositionConstraints()
+        {
+            const float linearError = 0.0f;
+
+            Body b1 = BodyA;
+            Body b2 = BodyB;
+
+            float coordinate1 = 0.0f, coordinate2 = 0.0f;
+            if (_revolute1 != null)
+            {
+                coordinate1 = _revolute1.JointAngle;
+            }
+            else if (_fixedRevolute1 != null)
+            {
+                coordinate1 = _fixedRevolute1.JointAngle;
+            }
+            else if (_prismatic1 != null)
+            {
+                coordinate1 = _prismatic1.JointTranslation;
+            }
+            else if (_fixedPrismatic1 != null)
+            {
+                coordinate1 = _fixedPrismatic1.JointTranslation;
+            }
+
+            if (_revolute2 != null)
+            {
+                coordinate2 = _revolute2.JointAngle;
+            }
+            else if (_fixedRevolute2 != null)
+            {
+                coordinate2 = _fixedRevolute2.JointAngle;
+            }
+            else if (_prismatic2 != null)
+            {
+                coordinate2 = _prismatic2.JointTranslation;
+            }
+            else if (_fixedPrismatic2 != null)
+            {
+                coordinate2 = _fixedPrismatic2.JointTranslation;
+            }
+
+            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.LinearSlop;
+        }
+    }
+}

axios/Dynamics/Joints/Joint.cs

@@ -0,0 +1,282 @@
+/*
+* Farseer Physics Engine based on Box2D.XNA port:
+* Copyright (c) 2010 Ian Qvist
+* 
+* 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 FarseerPhysics.Dynamics.Joints
+{
+    public enum JointType
+    {
+        Revolute,
+        Prismatic,
+        Distance,
+        Pulley,
+        Gear,
+        Line,
+        Weld,
+        Friction,
+        Slider,
+        Angle,
+        Rope,
+        FixedMouse,
+        FixedRevolute,
+        FixedDistance,
+        FixedLine,
+        FixedPrismatic,
+        FixedAngle,
+        FixedFriction,
+    }
+
+    public enum LimitState
+    {
+        Inactive,
+        AtLower,
+        AtUpper,
+        Equal,
+    }
+
+    internal struct Jacobian
+    {
+        public float AngularA;
+        public float AngularB;
+        public Vector2 LinearA;
+        public Vector2 LinearB;
+
+        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;
+        }
+    }
+
+    /// <summary>
+    /// 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.
+    /// </summary>
+    public sealed class JointEdge
+    {
+        /// <summary>
+        /// The joint.
+        /// </summary>
+        public Joint Joint;
+
+        /// <summary>
+        /// The next joint edge in the body's joint list.
+        /// </summary>
+        public JointEdge Next;
+
+        /// <summary>
+        /// Provides quick access to the other body attached.
+        /// </summary>
+        public Body Other;
+
+        /// <summary>
+        /// The previous joint edge in the body's joint list.
+        /// </summary>
+        public JointEdge Prev;
+    }
+
+    public abstract class Joint
+    {
+        /// <summary>
+        /// The Breakpoint simply indicates the maximum Value the JointError can be before it breaks.
+        /// The default value is float.MaxValue
+        /// </summary>
+        public float Breakpoint = float.MaxValue;
+
+        internal JointEdge EdgeA = new JointEdge();
+        internal JointEdge EdgeB = new JointEdge();
+        public bool Enabled = true;
+        protected float InvIA;
+        protected float InvIB;
+        protected float InvMassA;
+        protected float InvMassB;
+        internal bool IslandFlag;
+        protected Vector2 LocalCenterA, LocalCenterB;
+
+        protected Joint()
+        {
+        }
+
+        protected Joint(Body body, Body bodyB)
+        {
+            Debug.Assert(body != bodyB);
+
+            BodyA = body;
+            BodyB = bodyB;
+
+            //Connected bodies should not collide by default
+            CollideConnected = false;
+        }
+
+        /// <summary>
+        /// Constructor for fixed joint
+        /// </summary>
+        protected Joint(Body body)
+        {
+            BodyA = body;
+
+            //Connected bodies should not collide by default
+            CollideConnected = false;
+        }
+
+        /// <summary>
+        /// Gets or sets the type of the joint.
+        /// </summary>
+        /// <value>The type of the joint.</value>
+        public JointType JointType { get; protected set; }
+
+        /// <summary>
+        /// Get the first body attached to this joint.
+        /// </summary>
+        /// <value></value>
+        public Body BodyA { get; set; }
+
+        /// <summary>
+        /// Get the second body attached to this joint.
+        /// </summary>
+        /// <value></value>
+        public Body BodyB { get; set; }
+
+        /// <summary>
+        /// Get the anchor point on body1 in world coordinates.
+        /// </summary>
+        /// <value></value>
+        public abstract Vector2 WorldAnchorA { get; }
+
+        /// <summary>
+        /// Get the anchor point on body2 in world coordinates.
+        /// </summary>
+        /// <value></value>
+        public abstract Vector2 WorldAnchorB { get; set; }
+
+        /// <summary>
+        /// Set the user data pointer.
+        /// </summary>
+        /// <value>The data.</value>
+        public object UserData { get; set; }
+
+        /// <summary>
+        /// Short-cut function to determine if either body is inactive.
+        /// </summary>
+        /// <value><c>true</c> if active; otherwise, <c>false</c>.</value>
+        public bool Active
+        {
+            get { return BodyA.Enabled && BodyB.Enabled; }
+        }
+
+        /// <summary>
+        /// Set this flag to true if the attached bodies should collide.
+        /// </summary>
+        public bool CollideConnected { get; set; }
+
+        /// <summary>
+        /// Fires when the joint is broken.
+        /// </summary>
+        public event Action<Joint, float> Broke;
+
+        /// <summary>
+        /// Get the reaction force on body2 at the joint anchor in Newtons.
+        /// </summary>
+        /// <param name="inv_dt">The inv_dt.</param>
+        /// <returns></returns>
+        public abstract Vector2 GetReactionForce(float inv_dt);
+
+        /// <summary>
+        /// Get the reaction torque on body2 in N*m.
+        /// </summary>
+        /// <param name="inv_dt">The inv_dt.</param>
+        /// <returns></returns>
+        public abstract float GetReactionTorque(float inv_dt);
+
+        protected void WakeBodies()
+        {
+            BodyA.Awake = true;
+            if (BodyB != null)
+            {
+                BodyB.Awake = true;
+            }
+        }
+
+        /// <summary>
+        /// Return true if the joint is a fixed type.
+        /// </summary>
+        public bool IsFixedType()
+        {
+            return JointType == JointType.FixedRevolute ||
+                   JointType == JointType.FixedDistance ||
+                   JointType == JointType.FixedPrismatic ||
+                   JointType == JointType.FixedLine ||
+                   JointType == JointType.FixedMouse ||
+                   JointType == JointType.FixedAngle ||
+                   JointType == JointType.FixedFriction;
+        }
+
+        internal abstract void InitVelocityConstraints(ref TimeStep step);
+
+        internal void Validate(float invDT)
+        {
+            if (!Enabled)
+                return;
+
+            float jointError = GetReactionForce(invDT).Length();
+            if (Math.Abs(jointError) <= Breakpoint)
+                return;
+
+            Enabled = false;
+
+            if (Broke != null)
+                Broke(this, jointError);
+        }
+
+        internal abstract void SolveVelocityConstraints(ref TimeStep step);
+
+        /// <summary>
+        /// Solves the position constraints.
+        /// </summary>
+        /// <returns>returns true if the position errors are within tolerance.</returns>
+        internal abstract bool SolvePositionConstraints();
+    }
+}

axios/Dynamics/Joints/LineJoint.cs

@@ -0,0 +1,436 @@
+/*
+* Farseer Physics Engine based on Box2D.XNA port:
+* Copyright (c) 2010 Ian Qvist
+* 
+* 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 FarseerPhysics.Common;
+using Microsoft.Xna.Framework;
+
+namespace FarseerPhysics.Dynamics.Joints
+{
+    public class LineJoint : Joint
+    {
+        private Vector2 _ax, _ay;
+        private float _bias;
+        private bool _enableMotor;
+        private float _gamma;
+        private float _impulse;
+        private Vector2 _localXAxis;
+        private Vector2 _localYAxisA;
+        private float _mass;
+        private float _maxMotorTorque;
+        private float _motorImpulse;
+        private float _motorMass;
+        private float _motorSpeed;
+
+        private float _sAx;
+        private float _sAy;
+        private float _sBx;
+        private float _sBy;
+
+        private float _springImpulse;
+        private float _springMass;
+
+        // Linear constraint (point-to-line)
+        // d = pB - pA = xB + rB - xA - rA
+        // C = dot(ay, d)
+        // Cdot = dot(d, cross(wA, ay)) + dot(ay, vB + cross(wB, rB) - vA - cross(wA, rA))
+        //      = -dot(ay, vA) - dot(cross(d + rA, ay), wA) + dot(ay, vB) + dot(cross(rB, ay), vB)
+        // J = [-ay, -cross(d + rA, ay), ay, cross(rB, ay)]
+
+        // Spring linear constraint
+        // C = dot(ax, d)
+        // Cdot = = -dot(ax, vA) - dot(cross(d + rA, ax), wA) + dot(ax, vB) + dot(cross(rB, ax), vB)
+        // J = [-ax -cross(d+rA, ax) ax cross(rB, ax)]
+
+        // Motor rotational constraint
+        // Cdot = wB - wA
+        // J = [0 0 -1 0 0 1]
+
+        internal LineJoint()
+        {
+            JointType = JointType.Line;
+        }
+
+        public LineJoint(Body bA, Body bB, Vector2 anchor, Vector2 axis)
+            : base(bA, bB)
+        {
+            JointType = JointType.Line;
+
+            LocalAnchorA = bA.GetLocalPoint(anchor);
+            LocalAnchorB = bB.GetLocalPoint(anchor);
+            LocalXAxis = bA.GetLocalVector(axis);
+        }
+
+        public Vector2 LocalAnchorA { get; set; }
+
+        public Vector2 LocalAnchorB { get; set; }
+
+        public override Vector2 WorldAnchorA
+        {
+            get { return BodyA.GetWorldPoint(LocalAnchorA); }
+        }
+
+        public override Vector2 WorldAnchorB
+        {
+            get { return BodyB.GetWorldPoint(LocalAnchorB); }
+            set { Debug.Assert(false, "You can't set the world anchor on this joint type."); }
+        }
+
+        public float JointTranslation
+        {
+            get
+            {
+                Body bA = BodyA;
+                Body bB = BodyB;
+
+                Vector2 pA = bA.GetWorldPoint(LocalAnchorA);
+                Vector2 pB = bB.GetWorldPoint(LocalAnchorB);
+                Vector2 d = pB - pA;
+                Vector2 axis = bA.GetWorldVector(LocalXAxis);
+
+                float translation = Vector2.Dot(d, axis);
+                return translation;
+            }
+        }
+
+        public float JointSpeed
+        {
+            get
+            {
+                float wA = BodyA.AngularVelocityInternal;
+                float wB = BodyB.AngularVelocityInternal;
+                return wB - wA;
+            }
+        }
+
+        public bool MotorEnabled
+        {
+            get { return _enableMotor; }
+            set
+            {
+                BodyA.Awake = true;
+                BodyB.Awake = true;
+                _enableMotor = value;
+            }
+        }
+
+        public float MotorSpeed
+        {
+            set
+            {
+                BodyA.Awake = true;
+                BodyB.Awake = true;
+                _motorSpeed = value;
+            }
+            get { return _motorSpeed; }
+        }
+
+        public float MaxMotorTorque
+        {
+            set
+            {
+                BodyA.Awake = true;
+                BodyB.Awake = true;
+                _maxMotorTorque = value;
+            }
+            get { return _maxMotorTorque; }
+        }
+
+        public float Frequency { get; set; }
+
+        public float DampingRatio { get; set; }
+
+        public Vector2 LocalXAxis
+        {
+            get { return _localXAxis; }
+            set
+            {
+                _localXAxis = value;
+                _localYAxisA = MathUtils.Cross(1.0f, _localXAxis);
+            }
+        }
+
+        public override Vector2 GetReactionForce(float invDt)
+        {
+            return invDt * (_impulse * _ay + _springImpulse * _ax);
+        }
+
+        public override float GetReactionTorque(float invDt)
+        {
+            return invDt * _motorImpulse;
+        }
+
+        internal override void InitVelocityConstraints(ref TimeStep step)
+        {
+            Body bA = BodyA;
+            Body bB = BodyB;
+
+            LocalCenterA = bA.LocalCenter;
+            LocalCenterB = bB.LocalCenter;
+
+            Transform xfA;
+            bA.GetTransform(out xfA);
+            Transform xfB;
+            bB.GetTransform(out xfB);
+
+            // Compute the effective masses.
+            Vector2 rA = MathUtils.Multiply(ref xfA.R, LocalAnchorA - LocalCenterA);
+            Vector2 rB = MathUtils.Multiply(ref xfB.R, LocalAnchorB - LocalCenterB);
+            Vector2 d = bB.Sweep.C + rB - bA.Sweep.C - rA;
+
+            InvMassA = bA.InvMass;
+            InvIA = bA.InvI;
+            InvMassB = bB.InvMass;
+            InvIB = bB.InvI;
+
+            // Point to line constraint
+            {
+                _ay = MathUtils.Multiply(ref xfA.R, _localYAxisA);
+                _sAy = MathUtils.Cross(d + rA, _ay);
+                _sBy = MathUtils.Cross(rB, _ay);
+
+                _mass = InvMassA + InvMassB + InvIA * _sAy * _sAy + InvIB * _sBy * _sBy;
+
+                if (_mass > 0.0f)
+                {
+                    _mass = 1.0f / _mass;
+                }
+            }
+
+            // Spring constraint
+            _springMass = 0.0f;
+            if (Frequency > 0.0f)
+            {
+                _ax = MathUtils.Multiply(ref xfA.R, LocalXAxis);
+                _sAx = MathUtils.Cross(d + rA, _ax);
+                _sBx = MathUtils.Cross(rB, _ax);
+
+                float invMass = InvMassA + InvMassB + InvIA * _sAx * _sAx + InvIB * _sBx * _sBx;
+
+                if (invMass > 0.0f)
+                {
+                    _springMass = 1.0f / invMass;
+
+                    float C = Vector2.Dot(d, _ax);
+
+                    // Frequency
+                    float omega = 2.0f * Settings.Pi * Frequency;
+
+                    // Damping coefficient
+                    float da = 2.0f * _springMass * DampingRatio * omega;
+
+                    // Spring stiffness
+                    float k = _springMass * omega * omega;
+
+                    // magic formulas
+                    _gamma = step.dt * (da + step.dt * k);
+                    if (_gamma > 0.0f)
+                    {
+                        _gamma = 1.0f / _gamma;
+                    }
+
+                    _bias = C * step.dt * k * _gamma;
+
+                    _springMass = invMass + _gamma;
+                    if (_springMass > 0.0f)
+                    {
+                        _springMass = 1.0f / _springMass;
+                    }
+                }
+            }
+            else
+            {
+                _springImpulse = 0.0f;
+                _springMass = 0.0f;
+            }
+
+            // Rotational motor
+            if (_enableMotor)
+            {
+                _motorMass = InvIA + InvIB;
+                if (_motorMass > 0.0f)
+                {
+                    _motorMass = 1.0f / _motorMass;
+                }
+            }
+            else
+            {
+                _motorMass = 0.0f;
+                _motorImpulse = 0.0f;
+            }
+
+            if (Settings.EnableWarmstarting)
+            {
+                // Account for variable time step.
+                _impulse *= step.dtRatio;
+                _springImpulse *= step.dtRatio;
+                _motorImpulse *= step.dtRatio;
+
+                Vector2 P = _impulse * _ay + _springImpulse * _ax;
+                float LA = _impulse * _sAy + _springImpulse * _sAx + _motorImpulse;
+                float LB = _impulse * _sBy + _springImpulse * _sBx + _motorImpulse;
+
+                bA.LinearVelocityInternal -= InvMassA * P;
+                bA.AngularVelocityInternal -= InvIA * LA;
+
+                bB.LinearVelocityInternal += InvMassB * P;
+                bB.AngularVelocityInternal += InvIB * LB;
+            }
+            else
+            {
+                _impulse = 0.0f;
+                _springImpulse = 0.0f;
+                _motorImpulse = 0.0f;
+            }
+        }
+
+        internal override void SolveVelocityConstraints(ref TimeStep step)
+        {
+            Body bA = BodyA;
+            Body bB = BodyB;
+
+            Vector2 vA = bA.LinearVelocity;
+            float wA = bA.AngularVelocityInternal;
+            Vector2 vB = bB.LinearVelocityInternal;
+            float wB = bB.AngularVelocityInternal;
+
+            // Solve spring constraint
+            {
+                float Cdot = Vector2.Dot(_ax, vB - vA) + _sBx * wB - _sAx * wA;
+                float impulse = -_springMass * (Cdot + _bias + _gamma * _springImpulse);
+                _springImpulse += impulse;
+
+                Vector2 P = impulse * _ax;
+                float LA = impulse * _sAx;
+                float LB = impulse * _sBx;
+
+                vA -= InvMassA * P;
+                wA -= InvIA * LA;
+
+                vB += InvMassB * P;
+                wB += InvIB * LB;
+            }
+
+            // Solve rotational motor constraint
+            {
+                float Cdot = wB - wA - _motorSpeed;
+                float impulse = -_motorMass * Cdot;
+
+                float oldImpulse = _motorImpulse;
+                float maxImpulse = step.dt * _maxMotorTorque;
+                _motorImpulse = MathUtils.Clamp(_motorImpulse + impulse, -maxImpulse, maxImpulse);
+                impulse = _motorImpulse - oldImpulse;
+
+                wA -= InvIA * impulse;
+                wB += InvIB * impulse;
+            }
+
+            // Solve point to line constraint
+            {
+                float Cdot = Vector2.Dot(_ay, vB - vA) + _sBy * wB - _sAy * wA;
+                float impulse = _mass * (-Cdot);
+                _impulse += impulse;
+
+                Vector2 P = impulse * _ay;
+                float LA = impulse * _sAy;
+                float LB = impulse * _sBy;
+
+                vA -= InvMassA * P;
+                wA -= InvIA * LA;
+
+                vB += InvMassB * P;
+                wB += InvIB * LB;
+            }
+
+            bA.LinearVelocityInternal = vA;
+            bA.AngularVelocityInternal = wA;
+            bB.LinearVelocityInternal = vB;
+            bB.AngularVelocityInternal = wB;
+        }
+
+        internal override bool SolvePositionConstraints()
+        {
+            Body bA = BodyA;
+            Body bB = BodyB;
+
+            Vector2 xA = bA.Sweep.C;
+            float angleA = bA.Sweep.A;
+
+            Vector2 xB = bB.Sweep.C;
+            float angleB = bB.Sweep.A;
+
+            Mat22 RA = new Mat22(angleA);
+            Mat22 RB = new Mat22(angleB);
+
+            Vector2 rA = MathUtils.Multiply(ref RA, LocalAnchorA - LocalCenterA);
+            Vector2 rB = MathUtils.Multiply(ref RB, LocalAnchorB - LocalCenterB);
+            Vector2 d = xB + rB - xA - rA;
+
+            Vector2 ay = MathUtils.Multiply(ref RA, _localYAxisA);
+
+            float sAy = MathUtils.Cross(d + rA, ay);
+            float sBy = MathUtils.Cross(rB, ay);
+
+            float C = Vector2.Dot(d, ay);
+
+            float k = InvMassA + InvMassB + InvIA * _sAy * _sAy + InvIB * _sBy * _sBy;
+
+            float impulse;
+            if (k != 0.0f)
+            {
+                impulse = -C / k;
+            }
+            else
+            {
+                impulse = 0.0f;
+            }
+
+            Vector2 P = impulse * ay;
+            float LA = impulse * sAy;
+            float LB = impulse * sBy;
+
+            xA -= InvMassA * P;
+            angleA -= InvIA * LA;
+            xB += InvMassB * P;
+            angleB += InvIB * LB;
+
+            // TODO_ERIN remove need for this.
+            bA.Sweep.C = xA;
+            bA.Sweep.A = angleA;
+            bB.Sweep.C = xB;
+            bB.Sweep.A = angleB;
+            bA.SynchronizeTransform();
+            bB.SynchronizeTransform();
+
+            return Math.Abs(C) <= Settings.LinearSlop;
+        }
+
+        public float GetMotorTorque(float invDt)
+        {
+            return invDt * _motorImpulse;
+        }
+    }
+}

axios/Dynamics/Joints/PrismaticJoint.cs

@@ -0,0 +1,677 @@
+/*
+* Farseer Physics Engine based on Box2D.XNA port:
+* Copyright (c) 2010 Ian Qvist
+* 
+* 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 FarseerPhysics.Common;
+using Microsoft.Xna.Framework;
+
+namespace FarseerPhysics.Dynamics.Joints
+{
+    // Linear constraint (point-to-line)
+    // d = p2 - p1 = x2 + r2 - x1 - r1
+    // C = dot(perp, d)
+    // Cdot = dot(d, cross(w1, perp)) + dot(perp, v2 + cross(w2, r2) - v1 - cross(w1, r1))
+    //      = -dot(perp, v1) - dot(cross(d + r1, perp), w1) + dot(perp, v2) + dot(cross(r2, perp), v2)
+    // J = [-perp, -cross(d + r1, perp), perp, cross(r2,perp)]
+    //
+    // Angular constraint
+    // C = a2 - a1 + a_initial
+    // Cdot = w2 - w1
+    // J = [0 0 -1 0 0 1]
+    //
+    // K = J * invM * JT
+    //
+    // J = [-a -s1 a s2]
+    //     [0  -1  0  1]
+    // a = perp
+    // s1 = cross(d + r1, a) = cross(p2 - x1, a)
+    // s2 = cross(r2, a) = cross(p2 - x2, a)
+    // Motor/Limit linear constraint
+    // C = dot(ax1, d)
+    // Cdot = = -dot(ax1, v1) - dot(cross(d + r1, ax1), w1) + dot(ax1, v2) + dot(cross(r2, ax1), v2)
+    // J = [-ax1 -cross(d+r1,ax1) ax1 cross(r2,ax1)]
+    // Block Solver
+    // We develop a block solver that includes the joint limit. This makes the limit stiff (inelastic) even
+    // when the mass has poor distribution (leading to large torques about the joint anchor points).
+    //
+    // The Jacobian has 3 rows:
+    // J = [-uT -s1 uT s2] // linear
+    //     [0   -1   0  1] // angular
+    //     [-vT -a1 vT a2] // limit
+    //
+    // u = perp
+    // v = axis
+    // s1 = cross(d + r1, u), s2 = cross(r2, u)
+    // a1 = cross(d + r1, v), a2 = cross(r2, v)
+    // M * (v2 - v1) = JT * df
+    // J * v2 = bias
+    //
+    // v2 = v1 + invM * JT * df
+    // J * (v1 + invM * JT * df) = bias
+    // K * df = bias - J * v1 = -Cdot
+    // K = J * invM * JT
+    // Cdot = J * v1 - bias
+    //
+    // Now solve for f2.
+    // df = f2 - f1
+    // K * (f2 - f1) = -Cdot
+    // f2 = invK * (-Cdot) + f1
+    //
+    // Clamp accumulated limit impulse.
+    // lower: f2(3) = max(f2(3), 0)
+    // upper: f2(3) = min(f2(3), 0)
+    //
+    // Solve for correct f2(1:2)
+    // K(1:2, 1:2) * f2(1:2) = -Cdot(1:2) - K(1:2,3) * f2(3) + K(1:2,1:3) * f1
+    //                       = -Cdot(1:2) - K(1:2,3) * f2(3) + K(1:2,1:2) * f1(1:2) + K(1:2,3) * f1(3)
+    // K(1:2, 1:2) * f2(1:2) = -Cdot(1:2) - K(1:2,3) * (f2(3) - f1(3)) + K(1:2,1:2) * f1(1:2)
+    // f2(1:2) = invK(1:2,1:2) * (-Cdot(1:2) - K(1:2,3) * (f2(3) - f1(3))) + f1(1:2)
+    //
+    // Now compute impulse to be applied:
+    // df = f2 - f1
+
+    /// <summary>
+    /// A prismatic joint. This joint provides one degree of freedom: translation
+    /// along an axis fixed in body1. Relative rotation is prevented. 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.
+    /// </summary>
+    public class PrismaticJoint : Joint
+    {
+        public Vector2 LocalAnchorA;
+
+        public Vector2 LocalAnchorB;
+        private Mat33 _K;
+        private float _a1, _a2;
+        private Vector2 _axis;
+        private bool _enableLimit;
+        private bool _enableMotor;
+        private Vector3 _impulse;
+        private LimitState _limitState;
+        private Vector2 _localXAxis1;
+        private Vector2 _localYAxis1;
+        private float _lowerTranslation;
+        private float _maxMotorForce;
+        private float _motorImpulse;
+        private float _motorMass; // effective mass for motor/limit translational constraint.
+        private float _motorSpeed;
+        private Vector2 _perp;
+        private float _refAngle;
+        private float _s1, _s2;
+        private float _upperTranslation;
+
+        internal PrismaticJoint()
+        {
+            JointType = JointType.Prismatic;
+        }
+
+        /// <summary>
+        /// 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.
+        /// </summary>
+        /// <param name="bodyA">The first body.</param>
+        /// <param name="bodyB">The second body.</param>
+        /// <param name="localAnchorA">The first body anchor.</param>
+        /// <param name="localAnchorB">The second body anchor.</param>
+        /// <param name="axis">The axis.</param>
+        public PrismaticJoint(Body bodyA, Body bodyB, Vector2 localAnchorA, Vector2 localAnchorB, Vector2 axis)
+            : base(bodyA, bodyB)
+        {
+            JointType = JointType.Prismatic;
+
+            LocalAnchorA = localAnchorA;
+            LocalAnchorB = localAnchorB;
+
+            _localXAxis1 = BodyA.GetLocalVector(axis);
+            _localYAxis1 = MathUtils.Cross(1.0f, _localXAxis1);
+            _refAngle = BodyB.Rotation - BodyA.Rotation;
+
+            _limitState = LimitState.Inactive;
+        }
+
+        public override Vector2 WorldAnchorA
+        {
+            get { return BodyA.GetWorldPoint(LocalAnchorA); }
+        }
+
+        public override Vector2 WorldAnchorB
+        {
+            get { return BodyB.GetWorldPoint(LocalAnchorB); }
+            set { Debug.Assert(false, "You can't set the world anchor on this joint type."); }
+        }
+
+        /// <summary>
+        /// Get the current joint translation, usually in meters.
+        /// </summary>
+        /// <value></value>
+        public float JointTranslation
+        {
+            get
+            {
+                Vector2 d = BodyB.GetWorldPoint(LocalAnchorB) - BodyA.GetWorldPoint(LocalAnchorA);
+                Vector2 axis = BodyA.GetWorldVector(ref _localXAxis1);
+
+                return Vector2.Dot(d, axis);
+            }
+        }
+
+        /// <summary>
+        /// Get the current joint translation speed, usually in meters per second.
+        /// </summary>
+        /// <value></value>
+        public float JointSpeed
+        {
+            get
+            {
+                Transform xf1, xf2;
+                BodyA.GetTransform(out xf1);
+                BodyB.GetTransform(out xf2);
+
+                Vector2 r1 = MathUtils.Multiply(ref xf1.R, LocalAnchorA - BodyA.LocalCenter);
+                Vector2 r2 = MathUtils.Multiply(ref xf2.R, LocalAnchorB - BodyB.LocalCenter);
+                Vector2 p1 = BodyA.Sweep.C + r1;
+                Vector2 p2 = BodyB.Sweep.C + r2;
+                Vector2 d = p2 - p1;
+                Vector2 axis = BodyA.GetWorldVector(ref _localXAxis1);
+
+                Vector2 v1 = BodyA.LinearVelocityInternal;
+                Vector2 v2 = BodyB.LinearVelocityInternal;
+                float w1 = BodyA.AngularVelocityInternal;
+                float w2 = BodyB.AngularVelocityInternal;
+
+                float speed = Vector2.Dot(d, MathUtils.Cross(w1, axis)) +
+                              Vector2.Dot(axis, v2 + MathUtils.Cross(w2, r2) - v1 - MathUtils.Cross(w1, r1));
+                return speed;
+            }
+        }
+
+        /// <summary>
+        /// Is the joint limit enabled?
+        /// </summary>
+        /// <value><c>true</c> if [limit enabled]; otherwise, <c>false</c>.</value>
+        public bool LimitEnabled
+        {
+            get { return _enableLimit; }
+            set
+            {
+                Debug.Assert(BodyA.FixedRotation == false || BodyB.FixedRotation == false,
+                             "Warning: limits does currently not work with fixed rotation");
+
+                WakeBodies();
+                _enableLimit = value;
+            }
+        }
+
+        /// <summary>
+        /// Get the lower joint limit, usually in meters.
+        /// </summary>
+        /// <value></value>
+        public float LowerLimit
+        {
+            get { return _lowerTranslation; }
+            set
+            {
+                WakeBodies();
+                _lowerTranslation = value;
+            }
+        }
+
+        /// <summary>
+        /// Get the upper joint limit, usually in meters.
+        /// </summary>
+        /// <value></value>
+        public float UpperLimit
+        {
+            get { return _upperTranslation; }
+            set
+            {
+                WakeBodies();
+                _upperTranslation = value;
+            }
+        }
+
+        /// <summary>
+        /// Is the joint motor enabled?
+        /// </summary>
+        /// <value><c>true</c> if [motor enabled]; otherwise, <c>false</c>.</value>
+        public bool MotorEnabled
+        {
+            get { return _enableMotor; }
+            set
+            {
+                WakeBodies();
+                _enableMotor = value;
+            }
+        }
+
+        /// <summary>
+        /// Set the motor speed, usually in meters per second.
+        /// </summary>
+        /// <value>The speed.</value>
+        public float MotorSpeed
+        {
+            set
+            {
+                WakeBodies();
+                _motorSpeed = value;
+            }
+            get { return _motorSpeed; }
+        }
+
+        /// <summary>
+        /// Set the maximum motor force, usually in N.
+        /// </summary>
+        /// <value>The force.</value>
+        public float MaxMotorForce
+        {
+            get { return _maxMotorForce; }
+            set
+            {
+                WakeBodies();
+                _maxMotorForce = value;
+            }
+        }
+
+        /// <summary>
+        /// Get the current motor force, usually in N.
+        /// </summary>
+        /// <value></value>
+        public float MotorForce
+        {
+            get { return _motorImpulse; }
+            set { _motorImpulse = value; }
+        }
+
+        public Vector2 LocalXAxis1
+        {
+            get { return _localXAxis1; }
+            set
+            {
+                _localXAxis1 = BodyA.GetLocalVector(value);
+                _localYAxis1 = MathUtils.Cross(1.0f, _localXAxis1);
+            }
+        }
+
+        public float ReferenceAngle
+        {
+            get { return _refAngle; }
+            set { _refAngle = value; }
+        }
+
+        public override Vector2 GetReactionForce(float inv_dt)
+        {
+            return inv_dt * (_impulse.X * _perp + (_motorImpulse + _impulse.Z) * _axis);
+        }
+
+        public override float GetReactionTorque(float inv_dt)
+        {
+            return inv_dt * _impulse.Y;
+        }
+
+        internal override void InitVelocityConstraints(ref TimeStep step)
+        {
+            Body b1 = BodyA;
+            Body b2 = BodyB;
+
+            LocalCenterA = b1.LocalCenter;
+            LocalCenterB = b2.LocalCenter;
+
+            Transform xf1, xf2;
+            b1.GetTransform(out xf1);
+            b2.GetTransform(out xf2);
+
+            // Compute the effective masses.
+            Vector2 r1 = MathUtils.Multiply(ref xf1.R, LocalAnchorA - LocalCenterA);
+            Vector2 r2 = MathUtils.Multiply(ref xf2.R, LocalAnchorB - 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.Epsilon)
+                {
+                    _motorMass = 1.0f / _motorMass;
+                }
+            }
+
+            // 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 + i2 * _s2;
+                float k13 = i1 * _s1 * _a1 + i2 * _s2 * _a2;
+                float k22 = i1 + i2;
+                float k23 = i1 * _a1 + i2 * _a2;
+                float k33 = m1 + m2 + i1 * _a1 * _a1 + i2 * _a2 * _a2;
+
+                _K.Col1 = new Vector3(k11, k12, k13);
+                _K.Col2 = new Vector3(k12, k22, k23);
+                _K.Col3 = new Vector3(k13, k23, k33);
+            }
+
+            // Compute motor and limit terms.
+            if (_enableLimit)
+            {
+                float jointTranslation = Vector2.Dot(_axis, d);
+                if (Math.Abs(_upperTranslation - _lowerTranslation) < 2.0f * Settings.LinearSlop)
+                {
+                    _limitState = LimitState.Equal;
+                }
+                else if (jointTranslation <= _lowerTranslation)
+                {
+                    if (_limitState != LimitState.AtLower)
+                    {
+                        _limitState = LimitState.AtLower;
+                        _impulse.Z = 0.0f;
+                    }
+                }
+                else if (jointTranslation >= _upperTranslation)
+                {
+                    if (_limitState != LimitState.AtUpper)
+                    {
+                        _limitState = LimitState.AtUpper;
+                        _impulse.Z = 0.0f;
+                    }
+                }
+                else
+                {
+                    _limitState = LimitState.Inactive;
+                    _impulse.Z = 0.0f;
+                }
+            }
+            else
+            {
+                _limitState = LimitState.Inactive;
+            }
+
+            if (_enableMotor == false)
+            {
+                _motorImpulse = 0.0f;
+            }
+
+            if (Settings.EnableWarmstarting)
+            {
+                // Account for variable time step.
+                _impulse *= step.dtRatio;
+                _motorImpulse *= step.dtRatio;
+
+                Vector2 P = _impulse.X * _perp + (_motorImpulse + _impulse.Z) * _axis;
+                float L1 = _impulse.X * _s1 + _impulse.Y + (_motorImpulse + _impulse.Z) * _a1;
+                float L2 = _impulse.X * _s2 + _impulse.Y + (_motorImpulse + _impulse.Z) * _a2;
+
+                b1.LinearVelocityInternal -= InvMassA * P;
+                b1.AngularVelocityInternal -= InvIA * L1;
+
+                b2.LinearVelocityInternal += InvMassB * P;
+                b2.AngularVelocityInternal += InvIB * L2;
+            }
+            else
+            {
+                _impulse = Vector3.Zero;
+                _motorImpulse = 0.0f;
+            }
+        }
+
+        internal override void SolveVelocityConstraints(ref TimeStep step)
+        {
+            Body b1 = BodyA;
+            Body b2 = BodyB;
+
+            Vector2 v1 = b1.LinearVelocityInternal;
+            float w1 = b1.AngularVelocityInternal;
+            Vector2 v2 = b2.LinearVelocityInternal;
+            float w2 = b2.AngularVelocityInternal;
+
+            // 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;
+            }
+
+            Vector2 Cdot1 = new Vector2(Vector2.Dot(_perp, v2 - v1) + _s2 * w2 - _s1 * w1, w2 - 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;
+                Vector3 Cdot = new Vector3(Cdot1.X, Cdot1.Y, Cdot2);
+
+                Vector3 f1 = _impulse;
+                Vector3 df = _K.Solve33(-Cdot);
+                _impulse += df;
+
+                if (_limitState == LimitState.AtLower)
+                {
+                    _impulse.Z = Math.Max(_impulse.Z, 0.0f);
+                }
+                else if (_limitState == LimitState.AtUpper)
+                {
+                    _impulse.Z = Math.Min(_impulse.Z, 0.0f);
+                }
+
+                // f2(1:2) = invK(1:2,1:2) * (-Cdot(1:2) - K(1:2,3) * (f2(3) - f1(3))) + f1(1:2)
+                Vector2 b = -Cdot1 - (_impulse.Z - f1.Z) * new Vector2(_K.Col3.X, _K.Col3.Y);
+                Vector2 f2r = _K.Solve22(b) + new Vector2(f1.X, f1.Y);
+                _impulse.X = f2r.X;
+                _impulse.Y = f2r.Y;
+
+                df = _impulse - f1;
+
+                Vector2 P = df.X * _perp + df.Z * _axis;
+                float L1 = df.X * _s1 + df.Y + df.Z * _a1;
+                float L2 = df.X * _s2 + df.Y + df.Z * _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.
+                Vector2 df = _K.Solve22(-Cdot1);
+                _impulse.X += df.X;
+                _impulse.Y += df.Y;
+
+                Vector2 P = df.X * _perp;
+                float L1 = df.X * _s1 + df.Y;
+                float L2 = df.X * _s2 + df.Y;
+
+                v1 -= InvMassA * P;
+                w1 -= InvIA * L1;
+
+                v2 += InvMassB * P;
+                w2 += InvIB * L2;
+            }
+
+            b1.LinearVelocityInternal = v1;
+            b1.AngularVelocityInternal = w1;
+            b2.LinearVelocityInternal = v2;
+            b2.AngularVelocityInternal = w2;
+        }
+
+        internal override bool SolvePositionConstraints()
+        {
+            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;
+            bool active = false;
+            float C2 = 0.0f;
+
+            Mat22 R1 = new Mat22(a1);
+            Mat22 R2 = new Mat22(a2);
+
+            Vector2 r1 = MathUtils.Multiply(ref R1, LocalAnchorA - LocalCenterA);
+            Vector2 r2 = MathUtils.Multiply(ref R2, LocalAnchorB - 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.LinearSlop)
+                {
+                    // Prevent large angular corrections
+                    C2 = MathUtils.Clamp(translation, -Settings.MaxLinearCorrection, Settings.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.LinearSlop,
+                                         -Settings.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.LinearSlop, 0.0f,
+                                         Settings.MaxLinearCorrection);
+                    linearError = translation - _upperTranslation;
+                    active = true;
+                }
+            }
+
+            _perp = MathUtils.Multiply(ref R1, _localYAxis1);
+
+            _s1 = MathUtils.Cross(d + r1, _perp);
+            _s2 = MathUtils.Cross(r2, _perp);
+
+            Vector3 impulse;
+            Vector2 C1 = new Vector2(Vector2.Dot(_perp, d), a2 - a1 - ReferenceAngle);
+
+            linearError = Math.Max(linearError, Math.Abs(C1.X));
+            float angularError = Math.Abs(C1.Y);
+
+            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 + i2 * _s2;
+                float k13 = i1 * _s1 * _a1 + i2 * _s2 * _a2;
+                float k22 = i1 + i2;
+                float k23 = i1 * _a1 + i2 * _a2;
+                float k33 = m1 + m2 + i1 * _a1 * _a1 + i2 * _a2 * _a2;
+
+                _K.Col1 = new Vector3(k11, k12, k13);
+                _K.Col2 = new Vector3(k12, k22, k23);
+                _K.Col3 = new Vector3(k13, k23, k33);
+
+                Vector3 C = new Vector3(-C1.X, -C1.Y, -C2);
+                impulse = _K.Solve33(C); // negated above
+            }
+            else
+            {
+                float m1 = InvMassA, m2 = InvMassB;
+                float i1 = InvIA, i2 = InvIB;
+
+                float k11 = m1 + m2 + i1 * _s1 * _s1 + i2 * _s2 * _s2;
+                float k12 = i1 * _s1 + i2 * _s2;
+                float k22 = i1 + i2;
+
+                _K.Col1 = new Vector3(k11, k12, 0.0f);
+                _K.Col2 = new Vector3(k12, k22, 0.0f);
+
+                Vector2 impulse1 = _K.Solve22(-C1);
+                impulse.X = impulse1.X;
+                impulse.Y = impulse1.Y;
+                impulse.Z = 0.0f;
+            }
+
+            Vector2 P = impulse.X * _perp + impulse.Z * _axis;
+            float L1 = impulse.X * _s1 + impulse.Y + impulse.Z * _a1;
+            float L2 = impulse.X * _s2 + impulse.Y + impulse.Z * _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.LinearSlop && angularError <= Settings.AngularSlop;
+        }
+    }
+}

axios/Dynamics/Joints/PulleyJoint.cs

@@ -0,0 +1,507 @@
+/*
+* Farseer Physics Engine based on Box2D.XNA port:
+* Copyright (c) 2010 Ian Qvist
+* 
+* 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 FarseerPhysics.Common;
+using Microsoft.Xna.Framework;
+
+namespace FarseerPhysics.Dynamics.Joints
+{
+    /// <summary>
+    /// The pulley joint is connected to two bodies and two fixed ground points.
+    /// The pulley supports a ratio such that:
+    /// length1 + ratio * length2 <!--<-->= ant
+    /// Yes, the force transmitted is scaled by the ratio.
+    /// The pulley also enforces a maximum length limit on both sides. This is
+    /// useful to prevent one side of the pulley hitting the top.
+    /// </summary>
+    public class PulleyJoint : Joint
+    {
+        /// <summary>
+        /// Get the first ground anchor.
+        /// </summary>
+        /// <value></value>
+        public Vector2 GroundAnchorA;
+
+        /// <summary>
+        /// Get the second ground anchor.
+        /// </summary>
+        /// <value></value>
+        public Vector2 GroundAnchorB;
+
+        public Vector2 LocalAnchorA;
+        public Vector2 LocalAnchorB;
+
+        public float MinPulleyLength = 2.0f;
+        private float _ant;
+        private float _impulse;
+        private float _lengthA;
+        private float _lengthB;
+        private float _limitImpulse1;
+        private float _limitImpulse2;
+        private float _limitMass1;
+        private float _limitMass2;
+        private LimitState _limitState1;
+        private LimitState _limitState2;
+        private float _maxLengthA;
+        private float _maxLengthB;
+
+        // Effective masses
+        private float _pulleyMass;
+        private LimitState _state;
+        private Vector2 _u1;
+        private Vector2 _u2;
+
+        internal PulleyJoint()
+        {
+            JointType = JointType.Pulley;
+        }
+
+        /// <summary>
+        /// Initialize the bodies, anchors, lengths, max lengths, and ratio using the world anchors.
+        /// This requires two ground anchors,
+        /// two dynamic body anchor points, max lengths for each side,
+        /// and a pulley ratio.
+        /// </summary>
+        /// <param name="bodyA">The first body.</param>
+        /// <param name="bodyB">The second body.</param>
+        /// <param name="groundAnchorA">The ground anchor for the first body.</param>
+        /// <param name="groundAnchorB">The ground anchor for the second body.</param>
+        /// <param name="localAnchorA">The first body anchor.</param>
+        /// <param name="localAnchorB">The second body anchor.</param>
+        /// <param name="ratio">The ratio.</param>
+        public PulleyJoint(Body bodyA, Body bodyB,
+                           Vector2 groundAnchorA, Vector2 groundAnchorB,
+                           Vector2 localAnchorA, Vector2 localAnchorB,
+                           float ratio)
+            : base(bodyA, bodyB)
+        {
+            JointType = JointType.Pulley;
+
+            GroundAnchorA = groundAnchorA;
+            GroundAnchorB = groundAnchorB;
+            LocalAnchorA = localAnchorA;
+            LocalAnchorB = localAnchorB;
+
+            Vector2 d1 = BodyA.GetWorldPoint(localAnchorA) - groundAnchorA;
+            _lengthA = d1.Length();
+
+            Vector2 d2 = BodyB.GetWorldPoint(localAnchorB) - groundAnchorB;
+            _lengthB = d2.Length();
+
+            Debug.Assert(ratio != 0.0f);
+            Debug.Assert(ratio > Settings.Epsilon);
+            Ratio = ratio;
+
+            float C = _lengthA + Ratio * _lengthB;
+
+            MaxLengthA = C - Ratio * MinPulleyLength;
+            MaxLengthB = (C - MinPulleyLength) / Ratio;
+
+            _ant = _lengthA + Ratio * _lengthB;
+
+            MaxLengthA = Math.Min(MaxLengthA, _ant - Ratio * MinPulleyLength);
+            MaxLengthB = Math.Min(MaxLengthB, (_ant - MinPulleyLength) / Ratio);
+
+            _impulse = 0.0f;
+            _limitImpulse1 = 0.0f;
+            _limitImpulse2 = 0.0f;
+        }
+
+        public override Vector2 WorldAnchorA
+        {
+            get { return BodyA.GetWorldPoint(LocalAnchorA); }
+        }
+
+        public override Vector2 WorldAnchorB
+        {
+            get { return BodyB.GetWorldPoint(LocalAnchorB); }
+            set { Debug.Assert(false, "You can't set the world anchor on this joint type."); }
+        }
+
+        /// <summary>
+        /// Get the current length of the segment attached to body1.
+        /// </summary>
+        /// <value></value>
+        public float LengthA
+        {
+            get
+            {
+                Vector2 d = BodyA.GetWorldPoint(LocalAnchorA) - GroundAnchorA;
+                return d.Length();
+            }
+            set { _lengthA = value; }
+        }
+
+        /// <summary>
+        /// Get the current length of the segment attached to body2.
+        /// </summary>
+        /// <value></value>
+        public float LengthB
+        {
+            get
+            {
+                Vector2 d = BodyB.GetWorldPoint(LocalAnchorB) - GroundAnchorB;
+                return d.Length();
+            }
+            set { _lengthB = value; }
+        }
+
+        /// <summary>
+        /// Get the pulley ratio.
+        /// </summary>
+        /// <value></value>
+        public float Ratio { get; set; }
+
+        public float MaxLengthA
+        {
+            get { return _maxLengthA; }
+            set { _maxLengthA = value; }
+        }
+
+        public float MaxLengthB
+        {
+            get { return _maxLengthB; }
+            set { _maxLengthB = value; }
+        }
+
+        public override Vector2 GetReactionForce(float inv_dt)
+        {
+            Vector2 P = _impulse * _u2;
+            return inv_dt * P;
+        }
+
+        public override float GetReactionTorque(float inv_dt)
+        {
+            return 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);
+
+            Vector2 r1 = MathUtils.Multiply(ref xf1.R, LocalAnchorA - b1.LocalCenter);
+            Vector2 r2 = MathUtils.Multiply(ref xf2.R, LocalAnchorB - b2.LocalCenter);
+
+            Vector2 p1 = b1.Sweep.C + r1;
+            Vector2 p2 = b2.Sweep.C + r2;
+
+            Vector2 s1 = GroundAnchorA;
+            Vector2 s2 = GroundAnchorB;
+
+            // Get the pulley axes.
+            _u1 = p1 - s1;
+            _u2 = p2 - s2;
+
+            float length1 = _u1.Length();
+            float length2 = _u2.Length();
+
+            if (length1 > Settings.LinearSlop)
+            {
+                _u1 *= 1.0f / length1;
+            }
+            else
+            {
+                _u1 = Vector2.Zero;
+            }
+
+            if (length2 > Settings.LinearSlop)
+            {
+                _u2 *= 1.0f / length2;
+            }
+            else
+            {
+                _u2 = Vector2.Zero;
+            }
+
+            float C = _ant - length1 - Ratio * length2;
+            if (C > 0.0f)
+            {
+                _state = LimitState.Inactive;
+                _impulse = 0.0f;
+            }
+            else
+            {
+                _state = LimitState.AtUpper;
+            }
+
+            if (length1 < MaxLengthA)
+            {
+                _limitState1 = LimitState.Inactive;
+                _limitImpulse1 = 0.0f;
+            }
+            else
+            {
+                _limitState1 = LimitState.AtUpper;
+            }
+
+            if (length2 < MaxLengthB)
+            {
+                _limitState2 = LimitState.Inactive;
+                _limitImpulse2 = 0.0f;
+            }
+            else
+            {
+                _limitState2 = LimitState.AtUpper;
+            }
+
+            // Compute effective mass.
+            float cr1u1 = MathUtils.Cross(r1, _u1);
+            float cr2u2 = MathUtils.Cross(r2, _u2);
+
+            _limitMass1 = b1.InvMass + b1.InvI * cr1u1 * cr1u1;
+            _limitMass2 = b2.InvMass + b2.InvI * cr2u2 * cr2u2;
+            _pulleyMass = _limitMass1 + Ratio * Ratio * _limitMass2;
+            Debug.Assert(_limitMass1 > Settings.Epsilon);
+            Debug.Assert(_limitMass2 > Settings.Epsilon);
+            Debug.Assert(_pulleyMass > Settings.Epsilon);
+            _limitMass1 = 1.0f / _limitMass1;
+            _limitMass2 = 1.0f / _limitMass2;
+            _pulleyMass = 1.0f / _pulleyMass;
+
+            if (Settings.EnableWarmstarting)
+            {
+                // Scale impulses to support variable time steps.
+                _impulse *= step.dtRatio;
+                _limitImpulse1 *= step.dtRatio;
+                _limitImpulse2 *= step.dtRatio;
+
+                // Warm starting.
+                Vector2 P1 = -(_impulse + _limitImpulse1) * _u1;
+                Vector2 P2 = (-Ratio * _impulse - _limitImpulse2) * _u2;
+                b1.LinearVelocityInternal += b1.InvMass * P1;
+                b1.AngularVelocityInternal += b1.InvI * MathUtils.Cross(r1, P1);
+                b2.LinearVelocityInternal += b2.InvMass * P2;
+                b2.AngularVelocityInternal += b2.InvI * MathUtils.Cross(r2, P2);
+            }
+            else
+            {
+                _impulse = 0.0f;
+                _limitImpulse1 = 0.0f;
+                _limitImpulse2 = 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, LocalAnchorA - b1.LocalCenter);
+            Vector2 r2 = MathUtils.Multiply(ref xf2.R, LocalAnchorB - b2.LocalCenter);
+
+            if (_state == LimitState.AtUpper)
+            {
+                Vector2 v1 = b1.LinearVelocityInternal + MathUtils.Cross(b1.AngularVelocityInternal, r1);
+                Vector2 v2 = b2.LinearVelocityInternal + MathUtils.Cross(b2.AngularVelocityInternal, r2);
+
+                float Cdot = -Vector2.Dot(_u1, v1) - Ratio * Vector2.Dot(_u2, v2);
+                float impulse = _pulleyMass * (-Cdot);
+                float oldImpulse = _impulse;
+                _impulse = Math.Max(0.0f, _impulse + impulse);
+                impulse = _impulse - oldImpulse;
+
+                Vector2 P1 = -impulse * _u1;
+                Vector2 P2 = -Ratio * impulse * _u2;
+                b1.LinearVelocityInternal += b1.InvMass * P1;
+                b1.AngularVelocityInternal += b1.InvI * MathUtils.Cross(r1, P1);
+                b2.LinearVelocityInternal += b2.InvMass * P2;
+                b2.AngularVelocityInternal += b2.InvI * MathUtils.Cross(r2, P2);
+            }
+
+            if (_limitState1 == LimitState.AtUpper)
+            {
+                Vector2 v1 = b1.LinearVelocityInternal + MathUtils.Cross(b1.AngularVelocityInternal, r1);
+
+                float Cdot = -Vector2.Dot(_u1, v1);
+                float impulse = -_limitMass1 * Cdot;
+                float oldImpulse = _limitImpulse1;
+                _limitImpulse1 = Math.Max(0.0f, _limitImpulse1 + impulse);
+                impulse = _limitImpulse1 - oldImpulse;
+
+                Vector2 P1 = -impulse * _u1;
+                b1.LinearVelocityInternal += b1.InvMass * P1;
+                b1.AngularVelocityInternal += b1.InvI * MathUtils.Cross(r1, P1);
+            }
+
+            if (_limitState2 == LimitState.AtUpper)
+            {
+                Vector2 v2 = b2.LinearVelocityInternal + MathUtils.Cross(b2.AngularVelocityInternal, r2);
+
+                float Cdot = -Vector2.Dot(_u2, v2);
+                float impulse = -_limitMass2 * Cdot;
+                float oldImpulse = _limitImpulse2;
+                _limitImpulse2 = Math.Max(0.0f, _limitImpulse2 + impulse);
+                impulse = _limitImpulse2 - oldImpulse;
+
+                Vector2 P2 = -impulse * _u2;
+                b2.LinearVelocityInternal += b2.InvMass * P2;
+                b2.AngularVelocityInternal += b2.InvI * MathUtils.Cross(r2, P2);
+            }
+        }
+
+        internal override bool SolvePositionConstraints()
+        {
+            Body b1 = BodyA;
+            Body b2 = BodyB;
+
+            Vector2 s1 = GroundAnchorA;
+            Vector2 s2 = GroundAnchorB;
+
+            float linearError = 0.0f;
+
+            if (_state == LimitState.AtUpper)
+            {
+                Transform xf1, xf2;
+                b1.GetTransform(out xf1);
+                b2.GetTransform(out xf2);
+
+                Vector2 r1 = MathUtils.Multiply(ref xf1.R, LocalAnchorA - b1.LocalCenter);
+                Vector2 r2 = MathUtils.Multiply(ref xf2.R, LocalAnchorB - b2.LocalCenter);
+
+                Vector2 p1 = b1.Sweep.C + r1;
+                Vector2 p2 = b2.Sweep.C + r2;
+
+                // Get the pulley axes.
+                _u1 = p1 - s1;
+                _u2 = p2 - s2;
+
+                float length1 = _u1.Length();
+                float length2 = _u2.Length();
+
+                if (length1 > Settings.LinearSlop)
+                {
+                    _u1 *= 1.0f / length1;
+                }
+                else
+                {
+                    _u1 = Vector2.Zero;
+                }
+
+                if (length2 > Settings.LinearSlop)
+                {
+                    _u2 *= 1.0f / length2;
+                }
+                else
+                {
+                    _u2 = Vector2.Zero;
+                }
+
+                float C = _ant - length1 - Ratio * length2;
+                linearError = Math.Max(linearError, -C);
+
+                C = MathUtils.Clamp(C + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0.0f);
+                float impulse = -_pulleyMass * C;
+
+                Vector2 P1 = -impulse * _u1;
+                Vector2 P2 = -Ratio * impulse * _u2;
+
+                b1.Sweep.C += b1.InvMass * P1;
+                b1.Sweep.A += b1.InvI * MathUtils.Cross(r1, P1);
+                b2.Sweep.C += b2.InvMass * P2;
+                b2.Sweep.A += b2.InvI * MathUtils.Cross(r2, P2);
+
+                b1.SynchronizeTransform();
+                b2.SynchronizeTransform();
+            }
+
+            if (_limitState1 == LimitState.AtUpper)
+            {
+                Transform xf1;
+                b1.GetTransform(out xf1);
+
+                Vector2 r1 = MathUtils.Multiply(ref xf1.R, LocalAnchorA - b1.LocalCenter);
+                Vector2 p1 = b1.Sweep.C + r1;
+
+                _u1 = p1 - s1;
+                float length1 = _u1.Length();
+
+                if (length1 > Settings.LinearSlop)
+                {
+                    _u1 *= 1.0f / length1;
+                }
+                else
+                {
+                    _u1 = Vector2.Zero;
+                }
+
+                float C = MaxLengthA - length1;
+                linearError = Math.Max(linearError, -C);
+                C = MathUtils.Clamp(C + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0.0f);
+                float impulse = -_limitMass1 * C;
+
+                Vector2 P1 = -impulse * _u1;
+                b1.Sweep.C += b1.InvMass * P1;
+                b1.Sweep.A += b1.InvI * MathUtils.Cross(r1, P1);
+
+                b1.SynchronizeTransform();
+            }
+
+            if (_limitState2 == LimitState.AtUpper)
+            {
+                Transform xf2;
+                b2.GetTransform(out xf2);
+
+                Vector2 r2 = MathUtils.Multiply(ref xf2.R, LocalAnchorB - b2.LocalCenter);
+                Vector2 p2 = b2.Sweep.C + r2;
+
+                _u2 = p2 - s2;
+                float length2 = _u2.Length();
+
+                if (length2 > Settings.LinearSlop)
+                {
+                    _u2 *= 1.0f / length2;
+                }
+                else
+                {
+                    _u2 = Vector2.Zero;
+                }
+
+                float C = MaxLengthB - length2;
+                linearError = Math.Max(linearError, -C);
+                C = MathUtils.Clamp(C + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0.0f);
+                float impulse = -_limitMass2 * C;
+
+                Vector2 P2 = -impulse * _u2;
+                b2.Sweep.C += b2.InvMass * P2;
+                b2.Sweep.A += b2.InvI * MathUtils.Cross(r2, P2);
+
+                b2.SynchronizeTransform();
+            }
+
+            return linearError < Settings.LinearSlop;
+        }
+    }
+}

axios/Dynamics/Joints/RevoluteJoint.cs

@@ -0,0 +1,595 @@
+/*
+* Farseer Physics Engine based on Box2D.XNA port:
+* Copyright (c) 2010 Ian Qvist
+* 
+* 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 FarseerPhysics.Common;
+using Microsoft.Xna.Framework;
+
+namespace FarseerPhysics.Dynamics.Joints
+{
+    /// <summary>
+    /// A revolute joint rains to bodies to share a common point while they
+    /// are free to rotate about the point. The relative rotation about the shared
+    /// point is the joint angle. You can limit the relative rotation with
+    /// a joint limit that specifies a lower and upper angle. You can use a motor
+    /// to drive the relative rotation about the shared point. A maximum motor torque
+    /// is provided so that infinite forces are not generated.
+    /// </summary>
+    public class RevoluteJoint : Joint
+    {
+        public Vector2 LocalAnchorA;
+
+        public Vector2 LocalAnchorB;
+        private bool _enableLimit;
+        private bool _enableMotor;
+        private Vector3 _impulse;
+        private LimitState _limitState;
+        private float _lowerAngle;
+        private Mat33 _mass; // effective mass for point-to-point constraint.
+        private float _maxMotorTorque;
+        private float _motorImpulse;
+        private float _motorMass; // effective mass for motor/limit angular constraint.
+        private float _motorSpeed;
+        private float _referenceAngle;
+        private float _tmpFloat1;
+        private Vector2 _tmpVector1, _tmpVector2;
+        private float _upperAngle;
+
+        internal RevoluteJoint()
+        {
+            JointType = JointType.Revolute;
+        }
+
+        /// <summary>
+        /// Initialize the bodies and local anchor.
+        /// This requires defining an
+        /// anchor point where the bodies are joined. The definition
+        /// uses local anchor points so that the initial configuration
+        /// can violate the constraint slightly. You also need to
+        /// specify the initial relative angle for joint limits. This
+        /// helps when saving and loading a game.
+        /// The local anchor points are measured from the body's origin
+        /// rather than the center of mass because:
+        /// 1. you might not know where the center of mass will be.
+        /// 2. if you add/remove shapes from a body and recompute the mass,
+        /// the joints will be broken.
+        /// </summary>
+        /// <param name="bodyA">The first body.</param>
+        /// <param name="bodyB">The second body.</param>
+        /// <param name="localAnchorA">The first body anchor.</param>
+        /// <param name="localAnchorB">The second anchor.</param>
+        public RevoluteJoint(Body bodyA, Body bodyB, Vector2 localAnchorA, Vector2 localAnchorB)
+            : base(bodyA, bodyB)
+        {
+            JointType = JointType.Revolute;
+
+            // Changed to local coordinates.
+            LocalAnchorA = localAnchorA;
+            LocalAnchorB = localAnchorB;
+
+            ReferenceAngle = BodyB.Rotation - BodyA.Rotation;
+
+            _impulse = Vector3.Zero;
+
+            _limitState = LimitState.Inactive;
+        }
+
+        public override Vector2 WorldAnchorA
+        {
+            get { return BodyA.GetWorldPoint(LocalAnchorA); }
+        }
+
+        public override Vector2 WorldAnchorB
+        {
+            get { return BodyB.GetWorldPoint(LocalAnchorB); }
+            set { Debug.Assert(false, "You can't set the world anchor on this joint type."); }
+        }
+
+        public float ReferenceAngle
+        {
+            get { return _referenceAngle; }
+            set
+            {
+                WakeBodies();
+                _referenceAngle = value;
+            }
+        }
+
+        /// <summary>
+        /// Get the current joint angle in radians.
+        /// </summary>
+        /// <value></value>
+        public float JointAngle
+        {
+            get { return BodyB.Sweep.A - BodyA.Sweep.A - ReferenceAngle; }
+        }
+
+        /// <summary>
+        /// Get the current joint angle speed in radians per second.
+        /// </summary>
+        /// <value></value>
+        public float JointSpeed
+        {
+            get { return BodyB.AngularVelocityInternal - BodyA.AngularVelocityInternal; }
+        }
+
+        /// <summary>
+        /// Is the joint limit enabled?
+        /// </summary>
+        /// <value><c>true</c> if [limit enabled]; otherwise, <c>false</c>.</value>
+        public bool LimitEnabled
+        {
+            get { return _enableLimit; }
+            set
+            {
+                WakeBodies();
+                _enableLimit = value;
+            }
+        }
+
+        /// <summary>
+        /// Get the lower joint limit in radians.
+        /// </summary>
+        /// <value></value>
+        public float LowerLimit
+        {
+            get { return _lowerAngle; }
+            set
+            {
+                WakeBodies();
+                _lowerAngle = value;
+            }
+        }
+
+        /// <summary>
+        /// Get the upper joint limit in radians.
+        /// </summary>
+        /// <value></value>
+        public float UpperLimit
+        {
+            get { return _upperAngle; }
+            set
+            {
+                WakeBodies();
+                _upperAngle = value;
+            }
+        }
+
+        /// <summary>
+        /// Is the joint motor enabled?
+        /// </summary>
+        /// <value><c>true</c> if [motor enabled]; otherwise, <c>false</c>.</value>
+        public bool MotorEnabled
+        {
+            get { return _enableMotor; }
+            set
+            {
+                WakeBodies();
+                _enableMotor = value;
+            }
+        }
+
+        /// <summary>
+        /// Set the motor speed in radians per second.
+        /// </summary>
+        /// <value>The speed.</value>
+        public float MotorSpeed
+        {
+            set
+            {
+                WakeBodies();
+                _motorSpeed = value;
+            }
+            get { return _motorSpeed; }
+        }
+
+        /// <summary>
+        /// Set the maximum motor torque, usually in N-m.
+        /// </summary>
+        /// <value>The torque.</value>
+        public float MaxMotorTorque
+        {
+            set
+            {
+                WakeBodies();
+                _maxMotorTorque = value;
+            }
+            get { return _maxMotorTorque; }
+        }
+
+        /// <summary>
+        /// Get the current motor torque, usually in N-m.
+        /// </summary>
+        /// <value></value>
+        public float MotorTorque
+        {
+            get { return _motorImpulse; }
+            set
+            {
+                WakeBodies();
+                _motorImpulse = value;
+            }
+        }
+
+        public override Vector2 GetReactionForce(float inv_dt)
+        {
+            Vector2 P = new Vector2(_impulse.X, _impulse.Y);
+            return inv_dt * P;
+        }
+
+        public override float GetReactionTorque(float inv_dt)
+        {
+            return inv_dt * _impulse.Z;
+        }
+
+        internal override void InitVelocityConstraints(ref TimeStep step)
+        {
+            Body b1 = BodyA;
+            Body b2 = BodyB;
+
+            if (_enableMotor || _enableLimit)
+            {
+                // You cannot create a rotation limit between bodies that
+                // both have fixed rotation.
+                Debug.Assert(b1.InvI > 0.0f || b2.InvI > 0.0f);
+            }
+
+            // Compute the effective mass matrix.
+            /*Transform xf1, xf2;
+            b1.GetTransform(out xf1);
+            b2.GetTransform(out xf2);*/
+
+            Vector2 r1 = MathUtils.Multiply(ref b1.Xf.R, LocalAnchorA - b1.LocalCenter);
+            Vector2 r2 = MathUtils.Multiply(ref b2.Xf.R, LocalAnchorB - b2.LocalCenter);
+
+            // J = [-I -r1_skew I r2_skew]
+            //     [ 0       -1 0       1]
+            // r_skew = [-ry; rx]
+
+            // Matlab
+            // K = [ m1+r1y^2*i1+m2+r2y^2*i2,  -r1y*i1*r1x-r2y*i2*r2x,          -r1y*i1-r2y*i2]
+            //     [  -r1y*i1*r1x-r2y*i2*r2x, m1+r1x^2*i1+m2+r2x^2*i2,           r1x*i1+r2x*i2]
+            //     [          -r1y*i1-r2y*i2,           r1x*i1+r2x*i2,                   i1+i2]
+
+            float m1 = b1.InvMass, m2 = b2.InvMass;
+            float i1 = b1.InvI, i2 = b2.InvI;
+
+            _mass.Col1.X = m1 + m2 + r1.Y * r1.Y * i1 + r2.Y * r2.Y * i2;
+            _mass.Col2.X = -r1.Y * r1.X * i1 - r2.Y * r2.X * i2;
+            _mass.Col3.X = -r1.Y * i1 - r2.Y * i2;
+            _mass.Col1.Y = _mass.Col2.X;
+            _mass.Col2.Y = m1 + m2 + r1.X * r1.X * i1 + r2.X * r2.X * i2;
+            _mass.Col3.Y = r1.X * i1 + r2.X * i2;
+            _mass.Col1.Z = _mass.Col3.X;
+            _mass.Col2.Z = _mass.Col3.Y;
+            _mass.Col3.Z = i1 + i2;
+
+            _motorMass = i1 + i2;
+            if (_motorMass > 0.0f)
+            {
+                _motorMass = 1.0f / _motorMass;
+            }
+
+            if (_enableMotor == false)
+            {
+                _motorImpulse = 0.0f;
+            }
+
+            if (_enableLimit)
+            {
+                float jointAngle = b2.Sweep.A - b1.Sweep.A - ReferenceAngle;
+                if (Math.Abs(_upperAngle - _lowerAngle) < 2.0f * Settings.AngularSlop)
+                {
+                    _limitState = LimitState.Equal;
+                }
+                else if (jointAngle <= _lowerAngle)
+                {
+                    if (_limitState != LimitState.AtLower)
+                    {
+                        _impulse.Z = 0.0f;
+                    }
+                    _limitState = LimitState.AtLower;
+                }
+                else if (jointAngle >= _upperAngle)
+                {
+                    if (_limitState != LimitState.AtUpper)
+                    {
+                        _impulse.Z = 0.0f;
+                    }
+                    _limitState = LimitState.AtUpper;
+                }
+                else
+                {
+                    _limitState = LimitState.Inactive;
+                    _impulse.Z = 0.0f;
+                }
+            }
+            else
+            {
+                _limitState = LimitState.Inactive;
+            }
+
+            if (Settings.EnableWarmstarting)
+            {
+                // Scale impulses to support a variable time step.
+                _impulse *= step.dtRatio;
+                _motorImpulse *= step.dtRatio;
+
+                Vector2 P = new Vector2(_impulse.X, _impulse.Y);
+
+                b1.LinearVelocityInternal -= m1 * P;
+                MathUtils.Cross(ref r1, ref P, out _tmpFloat1);
+                b1.AngularVelocityInternal -= i1 * ( /* r1 x P */_tmpFloat1 + _motorImpulse + _impulse.Z);
+
+                b2.LinearVelocityInternal += m2 * P;
+                MathUtils.Cross(ref r2, ref P, out _tmpFloat1);
+                b2.AngularVelocityInternal += i2 * ( /* r2 x P */_tmpFloat1 + _motorImpulse + _impulse.Z);
+            }
+            else
+            {
+                _impulse = Vector3.Zero;
+                _motorImpulse = 0.0f;
+            }
+        }
+
+        internal override void SolveVelocityConstraints(ref TimeStep step)
+        {
+            Body b1 = BodyA;
+            Body b2 = BodyB;
+
+            Vector2 v1 = b1.LinearVelocityInternal;
+            float w1 = b1.AngularVelocityInternal;
+            Vector2 v2 = b2.LinearVelocityInternal;
+            float w2 = b2.AngularVelocityInternal;
+
+            float m1 = b1.InvMass, m2 = b2.InvMass;
+            float i1 = b1.InvI, i2 = b2.InvI;
+
+            // Solve motor constraint.
+            if (_enableMotor && _limitState != LimitState.Equal)
+            {
+                float Cdot = w2 - w1 - _motorSpeed;
+                float impulse = _motorMass * (-Cdot);
+                float oldImpulse = _motorImpulse;
+                float maxImpulse = step.dt * _maxMotorTorque;
+                _motorImpulse = MathUtils.Clamp(_motorImpulse + impulse, -maxImpulse, maxImpulse);
+                impulse = _motorImpulse - oldImpulse;
+
+                w1 -= i1 * impulse;
+                w2 += i2 * impulse;
+            }
+
+            // Solve limit constraint.
+            if (_enableLimit && _limitState != LimitState.Inactive)
+            {
+                /*Transform xf1, xf2;
+                b1.GetTransform(out xf1);
+                b2.GetTransform(out xf2);*/
+
+                Vector2 r1 = MathUtils.Multiply(ref b1.Xf.R, LocalAnchorA - b1.LocalCenter);
+                Vector2 r2 = MathUtils.Multiply(ref b2.Xf.R, LocalAnchorB - b2.LocalCenter);
+
+                // Solve point-to-point constraint
+                MathUtils.Cross(w2, ref r2, out _tmpVector2);
+                MathUtils.Cross(w1, ref r1, out _tmpVector1);
+                Vector2 Cdot1 = v2 + /* w2 x r2 */ _tmpVector2 - v1 - /* w1 x r1 */ _tmpVector1;
+                float Cdot2 = w2 - w1;
+                Vector3 Cdot = new Vector3(Cdot1.X, Cdot1.Y, Cdot2);
+
+                Vector3 impulse = _mass.Solve33(-Cdot);
+
+                if (_limitState == LimitState.Equal)
+                {
+                    _impulse += impulse;
+                }
+                else if (_limitState == LimitState.AtLower)
+                {
+                    float newImpulse = _impulse.Z + impulse.Z;
+                    if (newImpulse < 0.0f)
+                    {
+                        Vector2 reduced = _mass.Solve22(-Cdot1);
+                        impulse.X = reduced.X;
+                        impulse.Y = reduced.Y;
+                        impulse.Z = -_impulse.Z;
+                        _impulse.X += reduced.X;
+                        _impulse.Y += reduced.Y;
+                        _impulse.Z = 0.0f;
+                    }
+                }
+                else if (_limitState == LimitState.AtUpper)
+                {
+                    float newImpulse = _impulse.Z + impulse.Z;
+                    if (newImpulse > 0.0f)
+                    {
+                        Vector2 reduced = _mass.Solve22(-Cdot1);
+                        impulse.X = reduced.X;
+                        impulse.Y = reduced.Y;
+                        impulse.Z = -_impulse.Z;
+                        _impulse.X += reduced.X;
+                        _impulse.Y += reduced.Y;
+                        _impulse.Z = 0.0f;
+                    }
+                }
+
+                Vector2 P = new Vector2(impulse.X, impulse.Y);
+
+                v1 -= m1 * P;
+                MathUtils.Cross(ref r1, ref P, out _tmpFloat1);
+                w1 -= i1 * ( /* r1 x P */_tmpFloat1 + impulse.Z);
+
+                v2 += m2 * P;
+                MathUtils.Cross(ref r2, ref P, out _tmpFloat1);
+                w2 += i2 * ( /* r2 x P */_tmpFloat1 + impulse.Z);
+            }
+            else
+            {
+                /*Transform xf1, xf2;
+                b1.GetTransform(out xf1);
+                b2.GetTransform(out xf2);*/
+
+                _tmpVector1 = LocalAnchorA - b1.LocalCenter;
+                _tmpVector2 = LocalAnchorB - b2.LocalCenter;
+                Vector2 r1 = MathUtils.Multiply(ref b1.Xf.R, ref _tmpVector1);
+                Vector2 r2 = MathUtils.Multiply(ref b2.Xf.R, ref _tmpVector2);
+
+                // Solve point-to-point constraint
+                MathUtils.Cross(w2, ref r2, out _tmpVector2);
+                MathUtils.Cross(w1, ref r1, out _tmpVector1);
+                Vector2 Cdot = v2 + /* w2 x r2 */ _tmpVector2 - v1 - /* w1 x r1 */ _tmpVector1;
+                Vector2 impulse = _mass.Solve22(-Cdot);
+
+                _impulse.X += impulse.X;
+                _impulse.Y += impulse.Y;
+
+                v1 -= m1 * impulse;
+                MathUtils.Cross(ref r1, ref impulse, out _tmpFloat1);
+                w1 -= i1 * /* r1 x impulse */ _tmpFloat1;
+
+                v2 += m2 * impulse;
+                MathUtils.Cross(ref r2, ref impulse, out _tmpFloat1);
+                w2 += i2 * /* r2 x impulse */ _tmpFloat1;
+            }
+
+            b1.LinearVelocityInternal = v1;
+            b1.AngularVelocityInternal = w1;
+            b2.LinearVelocityInternal = v2;
+            b2.AngularVelocityInternal = w2;
+        }
+
+        internal override bool SolvePositionConstraints()
+        {
+            // TODO_ERIN block solve with limit. COME ON ERIN
+
+            Body b1 = BodyA;
+            Body b2 = BodyB;
+
+            float angularError = 0.0f;
+            float positionError;
+
+            // Solve angular limit constraint.
+            if (_enableLimit && _limitState != LimitState.Inactive)
+            {
+                float angle = b2.Sweep.A - b1.Sweep.A - ReferenceAngle;
+                float limitImpulse = 0.0f;
+
+                if (_limitState == LimitState.Equal)
+                {
+                    // Prevent large angular corrections
+                    float C = MathUtils.Clamp(angle - _lowerAngle, -Settings.MaxAngularCorrection,
+                                              Settings.MaxAngularCorrection);
+                    limitImpulse = -_motorMass * C;
+                    angularError = Math.Abs(C);
+                }
+                else if (_limitState == LimitState.AtLower)
+                {
+                    float C = angle - _lowerAngle;
+                    angularError = -C;
+
+                    // Prevent large angular corrections and allow some slop.
+                    C = MathUtils.Clamp(C + Settings.AngularSlop, -Settings.MaxAngularCorrection, 0.0f);
+                    limitImpulse = -_motorMass * C;
+                }
+                else if (_limitState == LimitState.AtUpper)
+                {
+                    float C = angle - _upperAngle;
+                    angularError = C;
+
+                    // Prevent large angular corrections and allow some slop.
+                    C = MathUtils.Clamp(C - Settings.AngularSlop, 0.0f, Settings.MaxAngularCorrection);
+                    limitImpulse = -_motorMass * C;
+                }
+
+                b1.Sweep.A -= b1.InvI * limitImpulse;
+                b2.Sweep.A += b2.InvI * limitImpulse;
+
+                b1.SynchronizeTransform();
+                b2.SynchronizeTransform();
+            }
+
+            // Solve point-to-point constraint.
+            {
+                /*Transform xf1, xf2;
+                b1.GetTransform(out xf1);
+                b2.GetTransform(out xf2);*/
+
+                Vector2 r1 = MathUtils.Multiply(ref b1.Xf.R, LocalAnchorA - b1.LocalCenter);
+                Vector2 r2 = MathUtils.Multiply(ref b2.Xf.R, LocalAnchorB - b2.LocalCenter);
+
+                Vector2 C = b2.Sweep.C + r2 - b1.Sweep.C - r1;
+                positionError = C.Length();
+
+                float invMass1 = b1.InvMass, invMass2 = b2.InvMass;
+                float invI1 = b1.InvI, invI2 = b2.InvI;
+
+                // Handle large detachment.
+                const float k_allowedStretch = 10.0f * Settings.LinearSlop;
+                if (C.LengthSquared() > k_allowedStretch * k_allowedStretch)
+                {
+                    // Use a particle solution (no rotation).
+                    Vector2 u = C;
+                    u.Normalize();
+                    float k = invMass1 + invMass2;
+                    Debug.Assert(k > Settings.Epsilon);
+                    float m = 1.0f / k;
+                    Vector2 impulse2 = m * (-C);
+                    const float k_beta = 0.5f;
+                    b1.Sweep.C -= k_beta * invMass1 * impulse2;
+                    b2.Sweep.C += k_beta * invMass2 * impulse2;
+
+                    C = b2.Sweep.C + r2 - b1.Sweep.C - r1;
+                }
+
+                Mat22 K1 = new Mat22(new Vector2(invMass1 + invMass2, 0.0f), new Vector2(0.0f, invMass1 + invMass2));
+                Mat22 K2 = new Mat22(new Vector2(invI1 * r1.Y * r1.Y, -invI1 * r1.X * r1.Y),
+                                     new Vector2(-invI1 * r1.X * r1.Y, invI1 * r1.X * r1.X));
+                Mat22 K3 = new Mat22(new Vector2(invI2 * r2.Y * r2.Y, -invI2 * r2.X * r2.Y),
+                                     new Vector2(-invI2 * r2.X * r2.Y, invI2 * r2.X * r2.X));
+
+                Mat22 Ka;
+                Mat22.Add(ref K1, ref K2, out Ka);
+
+                Mat22 K;
+                Mat22.Add(ref Ka, ref K3, out K);
+
+
+                Vector2 impulse = K.Solve(-C);
+
+                b1.Sweep.C -= b1.InvMass * impulse;
+                MathUtils.Cross(ref r1, ref impulse, out _tmpFloat1);
+                b1.Sweep.A -= b1.InvI * /* r1 x impulse */ _tmpFloat1;
+
+                b2.Sweep.C += b2.InvMass * impulse;
+                MathUtils.Cross(ref r2, ref impulse, out _tmpFloat1);
+                b2.Sweep.A += b2.InvI * /* r2 x impulse */ _tmpFloat1;
+
+                b1.SynchronizeTransform();
+                b2.SynchronizeTransform();
+            }
+
+            return positionError <= Settings.LinearSlop && angularError <= Settings.AngularSlop;
+        }
+    }
+}

axios/Dynamics/Joints/RopeJoint.cs

@@ -0,0 +1,239 @@
+/*
+* Copyright (c) 2006-2010 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 FarseerPhysics.Common;
+using Microsoft.Xna.Framework;
+
+namespace FarseerPhysics.Dynamics.Joints
+{
+    // Limit:
+    // C = norm(pB - pA) - L
+    // u = (pB - pA) / norm(pB - pA)
+    // Cdot = dot(u, vB + cross(wB, rB) - vA - cross(wA, rA))
+    // J = [-u -cross(rA, u) u cross(rB, u)]
+    // K = J * invM * JT
+    //   = invMassA + invIA * cross(rA, u)^2 + invMassB + invIB * cross(rB, u)^2
+
+    /// <summary>
+    /// A rope joint enforces a maximum distance between two points
+    /// on two bodies. It has no other effect.
+    /// Warning: if you attempt to change the maximum length during
+    /// the simulation you will get some non-physical behavior.
+    /// A model that would allow you to dynamically modify the length
+    /// would have some sponginess, so I chose not to implement it
+    /// that way. See b2DistanceJoint if you want to dynamically
+    /// control length.
+    /// </summary>
+    public class RopeJoint : Joint
+    {
+        public Vector2 LocalAnchorA;
+        public Vector2 LocalAnchorB;
+
+        private float _impulse;
+        private float _length;
+
+        private float _mass;
+        private Vector2 _rA, _rB;
+        private LimitState _state;
+        private Vector2 _u;
+
+        internal RopeJoint()
+        {
+            JointType = JointType.Rope;
+        }
+
+        public RopeJoint(Body bodyA, Body bodyB, Vector2 localAnchorA, Vector2 localAnchorB)
+            : base(bodyA, bodyB)
+        {
+            JointType = JointType.Rope;
+            LocalAnchorA = localAnchorA;
+            LocalAnchorB = localAnchorB;
+
+            Vector2 d = WorldAnchorB - WorldAnchorA;
+            MaxLength = d.Length();
+
+            _mass = 0.0f;
+            _impulse = 0.0f;
+            _state = LimitState.Inactive;
+            _length = 0.0f;
+        }
+
+        /// Get the maximum length of the rope.
+        public float MaxLength { get; set; }
+
+        public LimitState State
+        {
+            get { return _state; }
+        }
+
+        public override sealed Vector2 WorldAnchorA
+        {
+            get { return BodyA.GetWorldPoint(LocalAnchorA); }
+        }
+
+        public override sealed Vector2 WorldAnchorB
+        {
+            get { return BodyB.GetWorldPoint(LocalAnchorB); }
+            set { Debug.Assert(false, "You can't set the world anchor on this joint type."); }
+        }
+
+        public override Vector2 GetReactionForce(float invDt)
+        {
+            return (invDt * _impulse) * _u;
+        }
+
+        public override float GetReactionTorque(float invDt)
+        {
+            return 0;
+        }
+
+        internal override void InitVelocityConstraints(ref TimeStep step)
+        {
+            Body bA = BodyA;
+            Body bB = BodyB;
+
+            Transform xf1;
+            bA.GetTransform(out xf1);
+
+            Transform xf2;
+            bB.GetTransform(out xf2);
+
+            _rA = MathUtils.Multiply(ref xf1.R, LocalAnchorA - bA.LocalCenter);
+            _rB = MathUtils.Multiply(ref xf2.R, LocalAnchorB - bB.LocalCenter);
+
+            // Rope axis
+            _u = bB.Sweep.C + _rB - bA.Sweep.C - _rA;
+
+            _length = _u.Length();
+
+            float C = _length - MaxLength;
+            if (C > 0.0f)
+            {
+                _state = LimitState.AtUpper;
+            }
+            else
+            {
+                _state = LimitState.Inactive;
+            }
+
+            if (_length > Settings.LinearSlop)
+            {
+                _u *= 1.0f / _length;
+            }
+            else
+            {
+                _u = Vector2.Zero;
+                _mass = 0.0f;
+                _impulse = 0.0f;
+                return;
+            }
+
+            // Compute effective mass.
+            float crA = MathUtils.Cross(_rA, _u);
+            float crB = MathUtils.Cross(_rB, _u);
+            float invMass = bA.InvMass + bA.InvI * crA * crA + bB.InvMass + bB.InvI * crB * crB;
+
+            _mass = invMass != 0.0f ? 1.0f / invMass : 0.0f;
+
+            if (Settings.EnableWarmstarting)
+            {
+                // Scale the impulse to support a variable time step.
+                _impulse *= step.dtRatio;
+
+                Vector2 P = _impulse * _u;
+                bA.LinearVelocity -= bA.InvMass * P;
+                bA.AngularVelocity -= bA.InvI * MathUtils.Cross(_rA, P);
+                bB.LinearVelocity += bB.InvMass * P;
+                bB.AngularVelocity += bB.InvI * MathUtils.Cross(_rB, P);
+            }
+            else
+            {
+                _impulse = 0.0f;
+            }
+        }
+
+        internal override void SolveVelocityConstraints(ref TimeStep step)
+        {
+            Body bA = BodyA;
+            Body bB = BodyB;
+
+            // Cdot = dot(u, v + cross(w, r))
+            Vector2 vA = bA.LinearVelocity + MathUtils.Cross(bA.AngularVelocity, _rA);
+            Vector2 vB = bB.LinearVelocity + MathUtils.Cross(bB.AngularVelocity, _rB);
+            float C = _length - MaxLength;
+            float Cdot = Vector2.Dot(_u, vB - vA);
+
+            // Predictive constraint.
+            if (C < 0.0f)
+            {
+                Cdot += step.inv_dt * C;
+            }
+
+            float impulse = -_mass * Cdot;
+            float oldImpulse = _impulse;
+            _impulse = Math.Min(0.0f, _impulse + impulse);
+            impulse = _impulse - oldImpulse;
+
+            Vector2 P = impulse * _u;
+            bA.LinearVelocity -= bA.InvMass * P;
+            bA.AngularVelocity -= bA.InvI * MathUtils.Cross(_rA, P);
+            bB.LinearVelocity += bB.InvMass * P;
+            bB.AngularVelocity += bB.InvI * MathUtils.Cross(_rB, P);
+        }
+
+        internal override bool SolvePositionConstraints()
+        {
+            Body bA = BodyA;
+            Body bB = BodyB;
+
+            Transform xf1;
+            bA.GetTransform(out xf1);
+
+            Transform xf2;
+            bB.GetTransform(out xf2);
+
+            Vector2 rA = MathUtils.Multiply(ref xf1.R, LocalAnchorA - bA.LocalCenter);
+            Vector2 rB = MathUtils.Multiply(ref xf2.R, LocalAnchorB - bB.LocalCenter);
+
+            Vector2 u = bB.Sweep.C + rB - bA.Sweep.C - rA;
+
+
+            float length = u.Length();
+            u.Normalize();
+
+            float C = length - MaxLength;
+
+            C = MathUtils.Clamp(C, 0.0f, Settings.MaxLinearCorrection);
+
+            float impulse = -_mass * C;
+            Vector2 P = impulse * u;
+
+            bA.Sweep.C -= bA.InvMass * P;
+            bA.Sweep.A -= bA.InvI * MathUtils.Cross(rA, P);
+            bB.Sweep.C += bB.InvMass * P;
+            bB.Sweep.A += bB.InvI * MathUtils.Cross(rB, P);
+
+            bA.SynchronizeTransform();
+            bB.SynchronizeTransform();
+
+            return length - MaxLength < Settings.LinearSlop;
+        }
+    }
+}

axios/Dynamics/Joints/SliderJoint.cs

@@ -0,0 +1,298 @@
+/*
+* Farseer Physics Engine based on Box2D.XNA port:
+* Copyright (c) 2010 Ian Qvist
+* 
+* 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 FarseerPhysics.Common;
+using Microsoft.Xna.Framework;
+
+namespace FarseerPhysics.Dynamics.Joints
+{
+    /// <summary>
+    /// A distance joint contrains two points on two bodies
+    /// to remain at a fixed distance from each other. You can view
+    /// this as a massless, rigid rod.
+    /// </summary>
+    public class SliderJoint : Joint
+    {
+        // 1-D constrained 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 Vector2 LocalAnchorA;
+
+        public Vector2 LocalAnchorB;
+        private float _bias;
+        private float _gamma;
+        private float _impulse;
+        private float _mass;
+        private Vector2 _u;
+
+        internal SliderJoint()
+        {
+            JointType = JointType.Slider;
+        }
+
+        /// <summary>
+        /// Initializes a new instance of the <see cref="SliderJoint"/> class.
+        /// Warning: Do not use a zero or short length.
+        /// </summary>
+        /// <param name="bodyA">The first body.</param>
+        /// <param name="bodyB">The second body.</param>
+        /// <param name="localAnchorA">The first body anchor.</param>
+        /// <param name="localAnchorB">The second body anchor.</param>
+        /// <param name="minLength">The minimum length between anchorpoints</param>
+        /// <param name="maxlength">The maximum length between anchorpoints.</param>
+        public SliderJoint(Body bodyA, Body bodyB, Vector2 localAnchorA, Vector2 localAnchorB, float minLength,
+                           float maxlength)
+            : base(bodyA, bodyB)
+        {
+            JointType = JointType.Slider;
+
+            LocalAnchorA = localAnchorA;
+            LocalAnchorB = localAnchorB;
+            MaxLength = maxlength;
+            MinLength = minLength;
+        }
+
+        /// <summary>
+        /// The maximum length between the anchor points.
+        /// </summary>
+        /// <value>The length.</value>
+        public float MaxLength { get; set; }
+
+        /// <summary>
+        /// The minimal length between the anchor points.
+        /// </summary>
+        /// <value>The length.</value>
+        public float MinLength { get; set; }
+
+        /// <summary>
+        /// The mass-spring-damper frequency in Hertz.
+        /// </summary>
+        /// <value>The frequency.</value>
+        public float Frequency { get; set; }
+
+        /// <summary>
+        /// The damping ratio. 0 = no damping, 1 = critical damping.
+        /// </summary>
+        /// <value>The damping ratio.</value>
+        public float DampingRatio { get; set; }
+
+        public override Vector2 WorldAnchorA
+        {
+            get { return BodyA.GetWorldPoint(LocalAnchorA); }
+        }
+
+        public override Vector2 WorldAnchorB
+        {
+            get { return BodyB.GetWorldPoint(LocalAnchorB); }
+            set { Debug.Assert(false, "You can't set the world anchor on this joint type."); }
+        }
+
+        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 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, LocalAnchorA - b1.LocalCenter);
+            Vector2 r2 = MathUtils.Multiply(ref xf2.R, LocalAnchorB - b2.LocalCenter);
+            _u = b2.Sweep.C + r2 - b1.Sweep.C - r1;
+
+            // Handle singularity.
+            float length = _u.Length();
+
+            if (length < MaxLength && length > MinLength)
+            {
+                return;
+            }
+
+            if (length > Settings.LinearSlop)
+            {
+                _u *= 1.0f / length;
+            }
+            else
+            {
+                _u = Vector2.Zero;
+            }
+
+            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.Epsilon);
+            _mass = invMass != 0.0f ? 1.0f / invMass : 0.0f;
+
+            if (Frequency > 0.0f)
+            {
+                float C = length - MaxLength;
+
+                // Frequency
+                float omega = 2.0f * Settings.Pi * Frequency;
+
+                // 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 (Settings.EnableWarmstarting)
+            {
+                // Scale the impulse to support a variable time step.
+                _impulse *= step.dtRatio;
+
+                Vector2 P = _impulse * _u;
+                b1.LinearVelocityInternal -= b1.InvMass * P;
+                b1.AngularVelocityInternal -= b1.InvI * MathUtils.Cross(r1, P);
+                b2.LinearVelocityInternal += b2.InvMass * P;
+                b2.AngularVelocityInternal += 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, LocalAnchorA - b1.LocalCenter);
+            Vector2 r2 = MathUtils.Multiply(ref xf2.R, LocalAnchorB - b2.LocalCenter);
+
+            Vector2 d = b2.Sweep.C + r2 - b1.Sweep.C - r1;
+
+            float length = d.Length();
+
+            if (length < MaxLength && length > MinLength)
+            {
+                return;
+            }
+
+            // Cdot = dot(u, v + cross(w, r))
+            Vector2 v1 = b1.LinearVelocityInternal + MathUtils.Cross(b1.AngularVelocityInternal, r1);
+            Vector2 v2 = b2.LinearVelocityInternal + MathUtils.Cross(b2.AngularVelocityInternal, r2);
+            float Cdot = Vector2.Dot(_u, v2 - v1);
+
+            float impulse = -_mass * (Cdot + _bias + _gamma * _impulse);
+            _impulse += impulse;
+
+            Vector2 P = impulse * _u;
+            b1.LinearVelocityInternal -= b1.InvMass * P;
+            b1.AngularVelocityInternal -= b1.InvI * MathUtils.Cross(r1, P);
+            b2.LinearVelocityInternal += b2.InvMass * P;
+            b2.AngularVelocityInternal += b2.InvI * MathUtils.Cross(r2, P);
+        }
+
+        internal override bool SolvePositionConstraints()
+        {
+            if (Frequency > 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, LocalAnchorA - b1.LocalCenter);
+            Vector2 r2 = MathUtils.Multiply(ref xf2.R, LocalAnchorB - b2.LocalCenter);
+
+            Vector2 d = b2.Sweep.C + r2 - b1.Sweep.C - r1;
+
+            float length = d.Length();
+
+            if (length < MaxLength && length > MinLength)
+            {
+                return true;
+            }
+
+            if (length == 0.0f)
+                return true;
+
+            d /= length;
+            float C = length - MaxLength;
+            C = MathUtils.Clamp(C, -Settings.MaxLinearCorrection, Settings.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.LinearSlop;
+        }
+    }
+}

axios/Dynamics/Joints/WeldJoint.cs

@@ -0,0 +1,263 @@
+/*
+* Farseer Physics Engine based on Box2D.XNA port:
+* Copyright (c) 2010 Ian Qvist
+* 
+* 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 FarseerPhysics.Common;
+using Microsoft.Xna.Framework;
+
+namespace FarseerPhysics.Dynamics.Joints
+{
+    // Point-to-point constraint
+    // C = p2 - p1
+    // 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
+    // C = angle2 - angle1 - referenceAngle
+    // Cdot = w2 - w1
+    // J = [0 0 -1 0 0 1]
+    // K = invI1 + invI2
+
+    /// <summary>
+    /// A weld joint essentially glues two bodies together. A weld joint may
+    /// distort somewhat because the island constraint solver is approximate.
+    /// </summary>
+    public class WeldJoint : Joint
+    {
+        public Vector2 LocalAnchorA;
+        public Vector2 LocalAnchorB;
+        private Vector3 _impulse;
+        private Mat33 _mass;
+
+        internal WeldJoint()
+        {
+            JointType = JointType.Weld;
+        }
+
+        /// <summary>
+        /// You need to specify a local anchor point
+        /// where they are attached and the relative body angle. The position
+        /// of the anchor point is important for computing the reaction torque.
+        /// You can change the anchor points relative to bodyA or bodyB by changing LocalAnchorA
+        /// and/or LocalAnchorB.
+        /// </summary>
+        /// <param name="bodyA">The first body</param>
+        /// <param name="bodyB">The second body</param>
+        /// <param name="localAnchorA">The first body anchor.</param>
+        /// <param name="localAnchorB">The second body anchor.</param>
+        public WeldJoint(Body bodyA, Body bodyB, Vector2 localAnchorA, Vector2 localAnchorB)
+            : base(bodyA, bodyB)
+        {
+            JointType = JointType.Weld;
+
+            LocalAnchorA = localAnchorA;
+            LocalAnchorB = localAnchorB;
+            ReferenceAngle = BodyB.Rotation - BodyA.Rotation;
+        }
+
+        public override Vector2 WorldAnchorA
+        {
+            get { return BodyA.GetWorldPoint(LocalAnchorA); }
+        }
+
+        public override Vector2 WorldAnchorB
+        {
+            get { return BodyB.GetWorldPoint(LocalAnchorB); }
+            set { Debug.Assert(false, "You can't set the world anchor on this joint type."); }
+        }
+
+        /// <summary>
+        /// The body2 angle minus body1 angle in the reference state (radians).
+        /// </summary>
+        public float ReferenceAngle { get; private set; }
+
+        public override Vector2 GetReactionForce(float inv_dt)
+        {
+            return inv_dt * new Vector2(_impulse.X, _impulse.Y);
+        }
+
+        public override float GetReactionTorque(float inv_dt)
+        {
+            return inv_dt * _impulse.Z;
+        }
+
+        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, LocalAnchorA - bA.LocalCenter);
+            Vector2 rB = MathUtils.Multiply(ref xfB.R, LocalAnchorB - bB.LocalCenter);
+
+            // 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;
+
+            _mass.Col1.X = mA + mB + rA.Y * rA.Y * iA + rB.Y * rB.Y * iB;
+            _mass.Col2.X = -rA.Y * rA.X * iA - rB.Y * rB.X * iB;
+            _mass.Col3.X = -rA.Y * iA - rB.Y * iB;
+            _mass.Col1.Y = _mass.Col2.X;
+            _mass.Col2.Y = mA + mB + rA.X * rA.X * iA + rB.X * rB.X * iB;
+            _mass.Col3.Y = rA.X * iA + rB.X * iB;
+            _mass.Col1.Z = _mass.Col3.X;
+            _mass.Col2.Z = _mass.Col3.Y;
+            _mass.Col3.Z = iA + iB;
+
+            if (Settings.EnableWarmstarting)
+            {
+                // Scale impulses to support a variable time step.
+                _impulse *= step.dtRatio;
+
+                Vector2 P = new Vector2(_impulse.X, _impulse.Y);
+
+                bA.LinearVelocityInternal -= mA * P;
+                bA.AngularVelocityInternal -= iA * (MathUtils.Cross(rA, P) + _impulse.Z);
+
+                bB.LinearVelocityInternal += mB * P;
+                bB.AngularVelocityInternal += iB * (MathUtils.Cross(rB, P) + _impulse.Z);
+            }
+            else
+            {
+                _impulse = Vector3.Zero;
+            }
+        }
+
+        internal override void SolveVelocityConstraints(ref TimeStep step)
+        {
+            Body bA = BodyA;
+            Body bB = BodyB;
+
+            Vector2 vA = bA.LinearVelocityInternal;
+            float wA = bA.AngularVelocityInternal;
+            Vector2 vB = bB.LinearVelocityInternal;
+            float wB = bB.AngularVelocityInternal;
+
+            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, LocalAnchorA - bA.LocalCenter);
+            Vector2 rB = MathUtils.Multiply(ref xfB.R, LocalAnchorB - bB.LocalCenter);
+
+            //  Solve point-to-point constraint
+            Vector2 Cdot1 = vB + MathUtils.Cross(wB, rB) - vA - MathUtils.Cross(wA, rA);
+            float Cdot2 = wB - wA;
+            Vector3 Cdot = new Vector3(Cdot1.X, Cdot1.Y, Cdot2);
+
+            Vector3 impulse = _mass.Solve33(-Cdot);
+            _impulse += impulse;
+
+            Vector2 P = new Vector2(impulse.X, impulse.Y);
+
+            vA -= mA * P;
+            wA -= iA * (MathUtils.Cross(rA, P) + impulse.Z);
+
+            vB += mB * P;
+            wB += iB * (MathUtils.Cross(rB, P) + impulse.Z);
+
+            bA.LinearVelocityInternal = vA;
+            bA.AngularVelocityInternal = wA;
+            bB.LinearVelocityInternal = vB;
+            bB.AngularVelocityInternal = wB;
+        }
+
+        internal override bool SolvePositionConstraints()
+        {
+            Body bA = BodyA;
+            Body bB = BodyB;
+
+            float mA = bA.InvMass, mB = bB.InvMass;
+            float iA = bA.InvI, iB = bB.InvI;
+
+            Transform xfA;
+            Transform xfB;
+            bA.GetTransform(out xfA);
+            bB.GetTransform(out xfB);
+
+            Vector2 rA = MathUtils.Multiply(ref xfA.R, LocalAnchorA - bA.LocalCenter);
+            Vector2 rB = MathUtils.Multiply(ref xfB.R, LocalAnchorB - bB.LocalCenter);
+
+            Vector2 C1 = bB.Sweep.C + rB - bA.Sweep.C - rA;
+            float C2 = bB.Sweep.A - bA.Sweep.A - ReferenceAngle;
+
+            // Handle large detachment.
+            const float k_allowedStretch = 10.0f * Settings.LinearSlop;
+            float positionError = C1.Length();
+            float angularError = Math.Abs(C2);
+            if (positionError > k_allowedStretch)
+            {
+                iA *= 1.0f;
+                iB *= 1.0f;
+            }
+
+            _mass.Col1.X = mA + mB + rA.Y * rA.Y * iA + rB.Y * rB.Y * iB;
+            _mass.Col2.X = -rA.Y * rA.X * iA - rB.Y * rB.X * iB;
+            _mass.Col3.X = -rA.Y * iA - rB.Y * iB;
+            _mass.Col1.Y = _mass.Col2.X;
+            _mass.Col2.Y = mA + mB + rA.X * rA.X * iA + rB.X * rB.X * iB;
+            _mass.Col3.Y = rA.X * iA + rB.X * iB;
+            _mass.Col1.Z = _mass.Col3.X;
+            _mass.Col2.Z = _mass.Col3.Y;
+            _mass.Col3.Z = iA + iB;
+
+            Vector3 C = new Vector3(C1.X, C1.Y, C2);
+
+            Vector3 impulse = _mass.Solve33(-C);
+
+            Vector2 P = new Vector2(impulse.X, impulse.Y);
+
+            bA.Sweep.C -= mA * P;
+            bA.Sweep.A -= iA * (MathUtils.Cross(rA, P) + impulse.Z);
+
+            bB.Sweep.C += mB * P;
+            bB.Sweep.A += iB * (MathUtils.Cross(rB, P) + impulse.Z);
+
+            bA.SynchronizeTransform();
+            bB.SynchronizeTransform();
+
+            return positionError <= Settings.LinearSlop && angularError <= Settings.AngularSlop;
+        }
+    }
+}