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

        internal override bool SolvePositionConstraints(float baumgarte)
        {
            Body b1 = _body1;
            Body b2 = _body2;

            Vector2 s1 = _ground.GetTransform().position + _groundAnchor1;
            Vector2 s2 = _ground.GetTransform().position + _groundAnchor2;

            float linearError = 0.0f;

            if (_state == LimitState.AtUpperLimit)
            {
                Vector2 r1 = b1.GetTransform().TransformDirection(_localAnchor1 - b1.GetLocalCenter());
                Vector2 r2 = b2.GetTransform().TransformDirection(_localAnchor2 - b2.GetLocalCenter());

                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.magnitude;
                float length2 = _u2.magnitude;

                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 = _constant - length1 - _ratio * length2;
                linearError = Box2DXMath.Max(linearError, -C);

                C = Mathf.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 * r1.Cross(P1);
                b2._sweep.C += b2._invMass * P2;
                b2._sweep.A += b2._invI * r2.Cross(P2);

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

            if (_limitState1 == LimitState.AtUpperLimit)
            {
                Vector2 r1 = b1.GetTransform().TransformDirection(_localAnchor1 - b1.GetLocalCenter());
                Vector2 p1 = b1._sweep.C + r1;

                _u1 = p1 - s1;
                float length1 = _u1.magnitude;

                if (length1 > Settings.LinearSlop)
                {
                    _u1 *= 1.0f / length1;
                }
                else
                {
                    _u1 = Vector2.zero;
                }

                float C = _maxLength1 - length1;
                linearError = Mathf.Max(linearError, -C);
                C = Mathf.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 * r1.Cross(P1);

                b1.SynchronizeTransform();
            }

            if (_limitState2 == LimitState.AtUpperLimit)
            {
                Vector2 r2 = b2.GetTransform().TransformDirection(_localAnchor2 - b2.GetLocalCenter());
                Vector2 p2 = b2._sweep.C + r2;

                _u2 = p2 - s2;
                float length2 = _u2.magnitude;

                if (length2 > Settings.LinearSlop)
                {
                    _u2 *= 1.0f / length2;
                }
                else
                {
                    _u2 = Vector2.zero;
                }

                float C = _maxLength2 - length2;
                linearError = Box2DXMath.Max(linearError, -C);
                C = Mathf.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 * r2.Cross(P2);

                b2.SynchronizeTransform();
            }

            return linearError < Settings.LinearSlop;
        }