Project290.Physics.Dynamics.Joints.FixedRevoluteJoint.SolvePositionConstraints C# (CSharp) Method

        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;
        }