Box2DX.Dynamics.PrismaticJoint.InitVelocityConstraints C# (CSharp) Метод

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

            // You cannot create a prismatic joint between bodies that
            // both have fixed rotation.
            Box2DXDebug.Assert(b1._invI > 0.0f || b2._invI > 0.0f);

            _localCenter1 = b1.GetLocalCenter();
            _localCenter2 = b2.GetLocalCenter();

            XForm xf1 = b1.GetXForm();
            XForm xf2 = b2.GetXForm();

            // Compute the effective masses.
            Vec2 r1 = Box2DX.Common.Math.Mul(xf1.R, _localAnchor1 - _localCenter1);
            Vec2 r2 = Box2DX.Common.Math.Mul(xf2.R, _localAnchor2 - _localCenter2);
            Vec2 d = b2._sweep.C + r2 - b1._sweep.C - r1;

            _invMass1 = b1._invMass;
            _invI1 = b1._invI;
            _invMass2 = b2._invMass;
            _invI2 = b2._invI;

            // Compute motor Jacobian and effective mass.
            {
                _axis = Box2DX.Common.Math.Mul(xf1.R, _localXAxis1);
                _a1 = Vec2.Cross(d + r1, _axis);
                _a2 = Vec2.Cross(r2, _axis);

                _motorMass = _invMass1 + _invMass2 + _invI1 * _a1 * _a1 + _invI2 * _a2 * _a2;
                Box2DXDebug.Assert(_motorMass > Settings.FLT_EPSILON);
                _motorMass = 1.0f / _motorMass;
            }

            // Prismatic constraint.
            {
                _perp = Box2DX.Common.Math.Mul(xf1.R, _localYAxis1);

                _s1 = Vec2.Cross(d + r1, _perp);
                _s2 = Vec2.Cross(r2, _perp);

                float m1 = _invMass1, m2 = _invMass2;
                float i1 = _invI1, i2 = _invI2;

                float k11 = m1 + m2 + i1 * _s1 * _s1 + i2 * _s2 * _s2;
                float k12 = i1 * _s1 + i2 * _s2;
                float k13 = i1 * _s1 * _a1 + i2 * _s2 * _a2;
                float k22 = i1 + i2;
                float k23 = i1 * _a1 + i2 * _a2;
                float k33 = m1 + m2 + i1 * _a1 * _a1 + i2 * _a2 * _a2;

                _K.Col1.Set(k11, k12, k13);
                _K.Col2.Set(k12, k22, k23);
                _K.Col3.Set(k13, k23, k33);
            }

            // Compute motor and limit terms.
            if (_enableLimit)
            {
                float jointTranslation = Vec2.Dot(_axis, d);
                if (Box2DX.Common.Math.Abs(_upperTranslation - _lowerTranslation) < 2.0f * Settings.LinearSlop)
                {
                    _limitState = LimitState.EqualLimits;
                }
                else if (jointTranslation <= _lowerTranslation)
                {
                    if (_limitState != LimitState.AtLowerLimit)
                    {
                        _limitState = LimitState.AtLowerLimit;
                        _impulse.Z = 0.0f;
                    }
                }
                else if (jointTranslation >= _upperTranslation)
                {
                    if (_limitState != LimitState.AtUpperLimit)
                    {
                        _limitState = LimitState.AtUpperLimit;
                        _impulse.Z = 0.0f;
                    }
                }
                else
                {
                    _limitState = LimitState.InactiveLimit;
                    _impulse.Z = 0.0f;
                }
            }
            else
            {
                _limitState = LimitState.InactiveLimit;
            }

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

            if (step.WarmStarting)
            {
                // Account for variable time step.
                _impulse *= step.DtRatio;
                _motorImpulse *= step.DtRatio;

                Vec2 P = _impulse.X * _perp + (_motorImpulse + _impulse.Z) * _axis;
                float L1 = _impulse.X * _s1 + _impulse.Y + (_motorImpulse + _impulse.Z) * _a1;
                float L2 = _impulse.X * _s2 + _impulse.Y + (_motorImpulse + _impulse.Z) * _a2;

                b1._linearVelocity -= _invMass1 * P;
                b1._angularVelocity -= _invI1 * L1;

                b2._linearVelocity += _invMass2 * P;
                b2._angularVelocity += _invI2 * L2;
            }
            else
            {
                _impulse.SetZero();
                _motorImpulse = 0.0f;
            }
        }