Box2D.Dynamics.Joints.MouseJoint.SolveVelocityConstraints C# (CSharp) Метод

        public override void SolveVelocityConstraints(SolverData data)
        {
            Vec2 vB = data.Velocities[IndexB].V;
            float wB = data.Velocities[IndexB].W;

            // Cdot = v + cross(w, r)
            Vec2 Cdot = Pool.PopVec2();
            Vec2.CrossToOutUnsafe(wB, RB, Cdot);
            Cdot.AddLocal(vB);

            Vec2 impulse = Pool.PopVec2();
            Vec2 temp = Pool.PopVec2();

            temp.Set(m_impulse).MulLocal(m_gamma).AddLocal(m_C).AddLocal(Cdot).NegateLocal();
            Mat22.MulToOutUnsafe(m_mass, temp, impulse);

            Vec2 oldImpulse = temp;
            oldImpulse.Set(m_impulse);
            m_impulse.AddLocal(impulse);
            float maxImpulse = data.Step.Dt * m_maxForce;
            if (m_impulse.LengthSquared() > maxImpulse * maxImpulse)
            {
                m_impulse.MulLocal(maxImpulse / m_impulse.Length());
            }
            impulse.Set(m_impulse).SubLocal(oldImpulse);

            vB.X += InvMassB * m_impulse.X;
            vB.Y += InvMassB * m_impulse.Y;
            wB += InvIB * Vec2.Cross(RB, impulse);

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

            Pool.PushVec2(3);
        }