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

        internal override bool SolvePositionConstraints(float baumgarte)
        {
            // TODO_ERIN block solve with limit.

            Body b1 = _body1;
            Body b2 = _body2;

            float angularError = 0.0f;
            float positionError = 0.0f;

            // Solve angular limit constraint.
            if (_enableLimit && _limitState !=  LimitState.InactiveLimit)
            {
                float angle = b2._sweep.A - b1._sweep.A - _referenceAngle;
                float limitImpulse = 0.0f;

                if (_limitState == LimitState.EqualLimits)
                {
                    // Prevent large angular corrections
                    float C = Box2DXMath.Clamp(angle, -Settings.MaxAngularCorrection, Settings.MaxAngularCorrection);
                    limitImpulse = -_motorMass * C;
                    angularError = Box2DXMath.Abs(C);
                }
                else if (_limitState == LimitState.AtLowerLimit)
                {
                    float C = angle - _lowerAngle;
                    angularError = -C;

                    // Prevent large angular corrections and allow some slop.
                    C = Box2DXMath.Clamp(C + Settings.AngularSlop, -Settings.MaxAngularCorrection, 0.0f);
                    limitImpulse = -_motorMass * C;
                }
                else if (_limitState == LimitState.AtUpperLimit)
                {
                    float C = angle - _upperAngle;
                    angularError = C;

                    // Prevent large angular corrections and allow some slop.
                    C = Box2DXMath.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.
            {
                Vec2 r1 = Box2DXMath.Mul(b1.GetXForm().R, _localAnchor1 - b1.GetLocalCenter());
                Vec2 r2 = Box2DXMath.Mul(b2.GetXForm().R, _localAnchor2 - b2.GetLocalCenter());

                Vec2 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.
                float k_allowedStretch = 10.0f * Settings.LinearSlop;
                if (C.LengthSquared() > k_allowedStretch * k_allowedStretch)
                {
                    // Use a particle solution (no rotation).
                    Vec2 u = C; u.Normalize();
                    float k = invMass1 + invMass2;
                    Box2DXDebug.Assert(k > Settings.FLT_EPSILON);
                    float m = 1.0f / k;
                    Vec2 impulse = m * (-C);
                    float k_beta = 0.5f;
                    b1._sweep.C -= k_beta * invMass1 * impulse;
                    b2._sweep.C += k_beta * invMass2 * impulse;

                    C = b2._sweep.C + r2 - b1._sweep.C - r1;
                }

                Mat22 K1 = new Mat22();
                K1.Col1.X = invMass1 + invMass2; K1.Col2.X = 0.0f;
                K1.Col1.Y = 0.0f; K1.Col2.Y = invMass1 + invMass2;

                Mat22 K2 = new Mat22();
                K2.Col1.X = invI1 * r1.Y * r1.Y; K2.Col2.X = -invI1 * r1.X * r1.Y;
                K2.Col1.Y = -invI1 * r1.X * r1.Y; K2.Col2.Y = invI1 * r1.X * r1.X;

                Mat22 K3 = new Mat22();
                K3.Col1.X = invI2 * r2.Y * r2.Y; K3.Col2.X = -invI2 * r2.X * r2.Y;
                K3.Col1.Y = -invI2 * r2.X * r2.Y; K3.Col2.Y = invI2 * r2.X * r2.X;

                Mat22 K = K1 + K2 + K3;
                Vec2 impulse_ = K.Solve(-C);

                b1._sweep.C -= b1._invMass * impulse_;
                b1._sweep.A -= b1._invI * Vec2.Cross(r1, impulse_);

                b2._sweep.C += b2._invMass * impulse_;
                b2._sweep.A += b2._invI * Vec2.Cross(r2, impulse_);

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

            return positionError <= Settings.LinearSlop && angularError <= Settings.AngularSlop;
        }