FarseerPhysics.Dynamics.Joints.FixedDistanceJoint.SolveVelocityConstraints C# (CSharp) Method

        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 = new Vector2(0, 0);
            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);
        }