Box2DX.Dynamics.DistanceJoint.SolvePositionConstraints C# (CSharp) Method

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

            Body b1 = _body1;
            Body b2 = _body2;

            Vector2 r1 = b1.GetTransform().TransformDirection(_localAnchor1 - b1.GetLocalCenter());
            Vector2 r2 = b2.GetTransform().TransformDirection(_localAnchor2 - b2.GetLocalCenter());

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

            float length = d.magnitude;
            d.Normalize();
            float C = length - _length;
            C = Mathf.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 * r1.Cross(P);
            b2._sweep.C += b2._invMass * P;
            b2._sweep.A += b2._invI * r2.Cross(P);

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

            return System.Math.Abs(C) < Settings.LinearSlop;
        }