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

        internal override void InitVelocityConstraints(ref TimeStep step)
        {
            Body b1 = BodyA;

            if (_enableMotor || _enableLimit)
            {
                // You cannot create a rotation limit between bodies that
                // both have fixed rotation.
                Debug.Assert(b1.InvI > 0.0f /* || b2._invI > 0.0f*/);
            }

            // Compute the effective mass matrix.
            Transform xf1;
            b1.GetTransform(out xf1);

            Vector2 r1 = MathUtils.Multiply(ref xf1.R, LocalAnchorA - b1.LocalCenter);
            Vector2 r2 = _worldAnchor; // MathUtils.Multiply(ref xf2.R, LocalAnchorB - b2.LocalCenter);

            // J = [-I -r1_skew I r2_skew]
            //     [ 0       -1 0       1]
            // r_skew = [-ry; rx]

            // Matlab
            // K = [ m1+r1y^2*i1+m2+r2y^2*i2,  -r1y*i1*r1x-r2y*i2*r2x,          -r1y*i1-r2y*i2]
            //     [  -r1y*i1*r1x-r2y*i2*r2x, m1+r1x^2*i1+m2+r2x^2*i2,           r1x*i1+r2x*i2]
            //     [          -r1y*i1-r2y*i2,           r1x*i1+r2x*i2,                   i1+i2]

            float m1 = b1.InvMass;
            const float m2 = 0;
            float i1 = b1.InvI;
            const float i2 = 0;

            _mass.Col1.X = m1 + m2 + r1.Y*r1.Y*i1 + r2.Y*r2.Y*i2;
            _mass.Col2.X = -r1.Y*r1.X*i1 - r2.Y*r2.X*i2;
            _mass.Col3.X = -r1.Y*i1 - r2.Y*i2;
            _mass.Col1.Y = _mass.Col2.X;
            _mass.Col2.Y = m1 + m2 + r1.X*r1.X*i1 + r2.X*r2.X*i2;
            _mass.Col3.Y = r1.X*i1 + r2.X*i2;
            _mass.Col1.Z = _mass.Col3.X;
            _mass.Col2.Z = _mass.Col3.Y;
            _mass.Col3.Z = i1 + i2;

            _motorMass = i1 + i2;
            if (_motorMass > 0.0f)
            {
                _motorMass = 1.0f/_motorMass;
            }

            if (_enableMotor == false)
            {
                _motorImpulse = 0.0f;
            }

            if (_enableLimit)
            {
                float jointAngle = 0 - b1.Sweep.A - ReferenceAngle;
                if (Math.Abs(_upperAngle - _lowerAngle) < 2.0f*Settings.AngularSlop)
                {
                    _limitState = LimitState.Equal;
                }
                else if (jointAngle <= _lowerAngle)
                {
                    if (_limitState != LimitState.AtLower)
                    {
                        _impulse.Z = 0.0f;
                    }
                    _limitState = LimitState.AtLower;
                }
                else if (jointAngle >= _upperAngle)
                {
                    if (_limitState != LimitState.AtUpper)
                    {
                        _impulse.Z = 0.0f;
                    }
                    _limitState = LimitState.AtUpper;
                }
                else
                {
                    _limitState = LimitState.Inactive;
                    _impulse.Z = 0.0f;
                }
            }
            else
            {
                _limitState = LimitState.Inactive;
            }

            if (Settings.EnableWarmstarting)
            {
                // Scale impulses to support a variable time step.
                _impulse *= step.dtRatio;
                _motorImpulse *= step.dtRatio;

                Vector2 P = new Vector2(_impulse.X, _impulse.Y);

                b1.LinearVelocityInternal -= m1*P;
                b1.AngularVelocityInternal -= i1*(MathUtils.Cross(r1, P) + _motorImpulse + _impulse.Z);
            }
            else
            {
                _impulse = Vector3.Zero;
                _motorImpulse = 0.0f;
            }
        }