Box2DX.Dynamics.Jacobian.Set C# (CSharp) Method

		public void Set(Vector2 x1, float a1, Vector2 x2, float a2)
		{
			Linear1 = x1; Angular1 = a1;
			Linear2 = x2; Angular2 = a2;
		}

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

            // Compute the effective masses.
            Vec2 r1 = Box2DXMath.Mul(b1.GetXForm().R, _localAnchor1 - b1.GetLocalCenter());
            Vec2 r2 = Box2DXMath.Mul(b2.GetXForm().R, _localAnchor2 - b2.GetLocalCenter());

            float invMass1 = b1._invMass, invMass2 = b2._invMass;
            float invI1 = b1._invI, invI2 = b2._invI;

            // Compute point to line constraint effective mass.
            // J = [-ay1 -cross(d+r1,ay1) ay1 cross(r2,ay1)]
            Vec2 ay1 = Box2DXMath.Mul(b1.GetXForm().R, _localYAxis1);
            Vec2 e   = b2._sweep.C + r2 - b1._sweep.C;                  // e = d + r1

            _linearJacobian.Set(-ay1, -Vec2.Cross(e, ay1), ay1, Vec2.Cross(r2, ay1));
            _linearMass = invMass1 + invI1 * _linearJacobian.Angular1 * _linearJacobian.Angular1 +
                          invMass2 + invI2 * _linearJacobian.Angular2 * _linearJacobian.Angular2;
            Box2DXDebug.Assert(_linearMass > Settings.FLT_EPSILON);
            _linearMass = 1.0f / _linearMass;

            // Compute angular constraint effective mass.
            _angularMass = invI1 + invI2;
            if (_angularMass > Settings.FLT_EPSILON)
            {
                _angularMass = 1.0f / _angularMass;
            }

            // Compute motor and limit terms.
            if (_enableLimit || _enableMotor)
            {
                // The motor and limit share a Jacobian and effective mass.
                Vec2 ax1 = Box2DXMath.Mul(b1.GetXForm().R, _localXAxis1);
                _motorJacobian.Set(-ax1, -Vec2.Cross(e, ax1), ax1, Vec2.Cross(r2, ax1));
                _motorMass = invMass1 + invI1 * _motorJacobian.Angular1 * _motorJacobian.Angular1 +
                             invMass2 + invI2 * _motorJacobian.Angular2 * _motorJacobian.Angular2;
                Box2DXDebug.Assert(_motorMass > Settings.FLT_EPSILON);
                _motorMass = 1.0f / _motorMass;

                if (_enableLimit)
                {
                    Vec2  d = e - r1;                           // p2 - p1
                    float jointTranslation = Vec2.Dot(ax1, d);
                    if (Box2DXMath.Abs(_upperTranslation - _lowerTranslation) < 2.0f * Settings.LinearSlop)
                    {
                        _limitState = LimitState.EqualLimits;
                    }
                    else if (jointTranslation <= _lowerTranslation)
                    {
                        if (_limitState != LimitState.AtLowerLimit)
                        {
                            _limitForce = 0.0f;
                        }
                        _limitState = LimitState.AtLowerLimit;
                    }
                    else if (jointTranslation >= _upperTranslation)
                    {
                        if (_limitState != LimitState.AtUpperLimit)
                        {
                            _limitForce = 0.0f;
                        }
                        _limitState = LimitState.AtUpperLimit;
                    }
                    else
                    {
                        _limitState = LimitState.InactiveLimit;
                        _limitForce = 0.0f;
                    }
                }
            }

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

            if (_enableLimit == false)
            {
                _limitForce = 0.0f;
            }

            if (step.WarmStarting)
            {
                Vec2  P1 = Settings.FORCE_SCALE(step.Dt) * (_force * _linearJacobian.Linear1 + (_motorForce + _limitForce) * _motorJacobian.Linear1);
                Vec2  P2 = Settings.FORCE_SCALE(step.Dt) * (_force * _linearJacobian.Linear2 + (_motorForce + _limitForce) * _motorJacobian.Linear2);
                float L1 = Settings.FORCE_SCALE(step.Dt) * (_force * _linearJacobian.Angular1 - _torque + (_motorForce + _limitForce) * _motorJacobian.Angular1);
                float L2 = Settings.FORCE_SCALE(step.Dt) * (_force * _linearJacobian.Angular2 + _torque + (_motorForce + _limitForce) * _motorJacobian.Angular2);

                b1._linearVelocity  += invMass1 * P1;
                b1._angularVelocity += invI1 * L1;

                b2._linearVelocity  += invMass2 * P2;
                b2._angularVelocity += invI2 * L2;
            }
            else
            {
                _force      = 0.0f;
                _torque     = 0.0f;
                _limitForce = 0.0f;
                _motorForce = 0.0f;
            }

            _limitPositionImpulse = 0.0f;
        }