Box2D.Dynamics.Joints.PulleyJoint.InitVelocityConstraints C# (CSharp) Method

        public override void InitVelocityConstraints(SolverData data)
        {
            IndexA = BodyA.IslandIndex;
            IndexB = BodyB.IslandIndex;
            m_localCenterA.Set(BodyA.Sweep.LocalCenter);
            m_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 temp = Pool.PopVec2();

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

            // Compute the effective masses.
            Rot.MulToOutUnsafe(qA, temp.Set(LocalAnchorA).SubLocal(m_localCenterA), RA);
            Rot.MulToOutUnsafe(qB, temp.Set(LocalAnchorB).SubLocal(m_localCenterB), RB);

            m_uA.Set(cA).AddLocal(RA).SubLocal(m_groundAnchorA);
            m_uB.Set(cB).AddLocal(RB).SubLocal(m_groundAnchorB);

            float lengthA = m_uA.Length();
            float lengthB = m_uB.Length();

            if (lengthA > 10f * Settings.LINEAR_SLOP)
            {
                m_uA.MulLocal(1.0f / lengthA);
            }
            else
            {
                m_uA.SetZero();
            }

            if (lengthB > 10f * Settings.LINEAR_SLOP)
            {
                m_uB.MulLocal(1.0f / lengthB);
            }
            else
            {
                m_uB.SetZero();
            }

            // Compute effective mass.
            float ruA = Vec2.Cross(RA, m_uA);
            float ruB = Vec2.Cross(RB, m_uB);

            float mA = InvMassA + InvIA * ruA * ruA;
            float mB = InvMassB + InvIB * ruB * ruB;

            m_mass = mA + m_ratio * m_ratio * mB;

            if (m_mass > 0.0f)
            {
                m_mass = 1.0f / m_mass;
            }

            if (data.Step.WarmStarting)
            {

                // Scale impulses to support variable time steps.
                m_impulse *= data.Step.DtRatio;

                // Warm starting.
                Vec2 PA = Pool.PopVec2();
                Vec2 PB = Pool.PopVec2();

                PA.Set(m_uA).MulLocal(-m_impulse);
                PB.Set(m_uB).MulLocal((-m_ratio) * m_impulse);

                vA.X += InvMassA * PA.X;
                vA.Y += InvMassA * PA.Y;
                wA += InvIA * Vec2.Cross(RA, PA);
                vB.X += InvMassB * PB.X;
                vB.Y += InvMassB * PB.Y;
                wB += InvIB * Vec2.Cross(RB, PB);

                Pool.PushVec2(2);
            }
            else
            {
                m_impulse = 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.PushVec2(1);
            Pool.PushRot(2);
        }