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

        public override void InitVelocityConstraints(SolverData data)
        {
            IndexA = BodyA.IslandIndex;
            IndexB = BodyB.IslandIndex;
            LocalCenterA.Set(BodyA.Sweep.LocalCenter);
            LocalCenterB.Set(BodyB.Sweep.LocalCenter);
            InvMassA = BodyA.InvMass;
            InvMassB = BodyB.InvMass;
            InvIA = BodyA.InvI;
            InvIB = BodyB.InvI;

            Vec2 cA = data.Positions[IndexA].C;
            float aA = data.Positions[IndexA].A;
            Vec2 vA = data.Velocities[IndexA].V;
            float wA = data.Velocities[IndexA].W;

            Vec2 cB = data.Positions[IndexB].C;
            float aB = data.Positions[IndexB].A;
            Vec2 vB = data.Velocities[IndexB].V;
            float wB = data.Velocities[IndexB].W;

            Rot qA = Pool.PopRot();
            Rot qB = Pool.PopRot();
            Vec2 d = Pool.PopVec2();
            Vec2 temp = Pool.PopVec2();
            Vec2 rA = Pool.PopVec2();
            Vec2 rB = Pool.PopVec2();

            qA.Set(aA);
            qB.Set(aB);

            // Compute the effective masses.
            Rot.MulToOutUnsafe(qA, d.Set(LocalAnchorA).SubLocal(LocalCenterA), rA);
            Rot.MulToOutUnsafe(qB, d.Set(LocalAnchorB).SubLocal(LocalCenterB), rB);
            d.Set(cB).SubLocal(cA).AddLocal(rB).SubLocal(rA);

            float mA = InvMassA, mB = InvMassB;
            float iA = InvIA, iB = InvIB;

            // Compute motor Jacobian and effective mass.
            {
                Rot.MulToOutUnsafe(qA, LocalXAxisA, Axis);
                temp.Set(d).AddLocal(rA);
                A1 = Vec2.Cross(temp, Axis);
                A2 = Vec2.Cross(rB, Axis);

                MotorMass = mA + mB + iA * A1 * A1 + iB * A2 * A2;
                if (MotorMass > 0.0f)
                {
                    MotorMass = 1.0f / MotorMass;
                }
            }

            // Prismatic constraint.
            {
                Rot.MulToOutUnsafe(qA, LocalYAxisA, Perp);

                temp.Set(d).AddLocal(rA);
                S1 = Vec2.Cross(temp, Perp);
                S2 = Vec2.Cross(rB, Perp);

                float k11 = mA + mB + iA * S1 * S1 + iB * S2 * S2;
                float k12 = iA * S1 + iB * S2;
                float k13 = iA * S1 * A1 + iB * S2 * A2;
                float k22 = iA + iB;
                if (k22 == 0.0f)
                {
                    // For bodies with fixed rotation.
                    k22 = 1.0f;
                }
                float k23 = iA * A1 + iB * A2;
                float k33 = mA + mB + iA * A1 * A1 + iB * A2 * A2;

                K.Ex.Set(k11, k12, k13);
                K.Ey.Set(k12, k22, k23);
                K.Ez.Set(k13, k23, k33);
            }

            // Compute motor and limit terms.
            if (m_limitEnabled)
            {

                float jointTranslation = Vec2.Dot(Axis, d);
                if (MathUtils.Abs(UpperTranslation - LowerTranslation) < 2.0f * Settings.LINEAR_SLOP)
                {
                    LimitState = LimitState.Equal;
                }
                else if (jointTranslation <= LowerTranslation)
                {
                    if (LimitState != LimitState.AtLower)
                    {
                        LimitState = LimitState.AtLower;
                        Impulse.Z = 0.0f;
                    }
                }
                else if (jointTranslation >= UpperTranslation)
                {
                    if (LimitState != LimitState.AtUpper)
                    {
                        LimitState = LimitState.AtUpper;
                        Impulse.Z = 0.0f;
                    }
                }
                else
                {
                    LimitState = LimitState.Inactive;
                    Impulse.Z = 0.0f;
                }
            }
            else
            {
                LimitState = LimitState.Inactive;
                Impulse.Z = 0.0f;
            }

            if (m_motorEnabled == false)
            {
                MotorImpulse = 0.0f;
            }

            if (data.Step.WarmStarting)
            {
                // Account for variable time step.
                Impulse.MulLocal(data.Step.DtRatio);
                MotorImpulse *= data.Step.DtRatio;

                Vec2 P = Pool.PopVec2();
                temp.Set(Axis).MulLocal(MotorImpulse + Impulse.Z);
                P.Set(Perp).MulLocal(Impulse.X).AddLocal(temp);

                float LA = Impulse.X * S1 + Impulse.Y + (MotorImpulse + Impulse.Z) * A1;
                float LB = Impulse.X * S2 + Impulse.Y + (MotorImpulse + Impulse.Z) * A2;

                vA.X -= mA * P.X;
                vA.Y -= mA * P.Y;
                wA -= iA * LA;

                vB.X += mB * P.X;
                vB.Y += mB * P.Y;
                wB += iB * LB;

                Pool.PushVec2(1);
            }
            else
            {
                Impulse.SetZero();
                MotorImpulse = 0.0f;
            }

            data.Velocities[IndexA].V.Set(vA);
            data.Velocities[IndexA].W = wA;
            data.Velocities[IndexB].V.Set(vB);
            data.Velocities[IndexB].W = wB;

            Pool.PushRot(2);
            Pool.PushVec2(4);
        }