Box2DX.Dynamics.GearJoint.InitVelocityConstraints C# (CSharp) Method

        internal override void InitVelocityConstraints(TimeStep step)
        {
            Body g1 = _ground1;
            Body g2 = _ground2;
            Body b1 = _body1;
            Body b2 = _body2;

            float K = 0.0f;
            _J.SetZero();

            if (_revolute1!=null)
            {
                _J.Angular1 = -1.0f;
                K += b1._invI;
            }
            else
            {
                Vec2 ug = Common.Math.Mul(g1.GetXForm().R, _prismatic1._localXAxis1);
                Vec2 r = Common.Math.Mul(b1.GetXForm().R, _localAnchor1 - b1.GetLocalCenter());
                float crug = Vec2.Cross(r, ug);
                _J.Linear1 = -ug;
                _J.Angular1 = -crug;
                K += b1._invMass + b1._invI * crug * crug;
            }

            if (_revolute2!=null)
            {
                _J.Angular2 = -_ratio;
                K += _ratio * _ratio * b2._invI;
            }
            else
            {
                Vec2 ug = Common.Math.Mul(g2.GetXForm().R, _prismatic2._localXAxis1);
                Vec2 r = Common.Math.Mul(b2.GetXForm().R, _localAnchor2 - b2.GetLocalCenter());
                float crug = Vec2.Cross(r, ug);
                _J.Linear2 = -_ratio * ug;
                _J.Angular2 = -_ratio * crug;
                K += _ratio * _ratio * (b2._invMass + b2._invI * crug * crug);
            }

            // Compute effective mass.
            Box2DXDebug.Assert(K > 0.0f);
            _mass = 1.0f / K;

            if (step.WarmStarting)
            {
                // Warm starting.
                b1._linearVelocity += b1._invMass * _impulse * _J.Linear1;
                b1._angularVelocity += b1._invI * _impulse * _J.Angular1;
                b2._linearVelocity += b2._invMass * _impulse * _J.Linear2;
                b2._angularVelocity += b2._invI * _impulse * _J.Angular2;
            }
            else
            {
                _impulse = 0.0f;
            }
        }