Box2D.Dynamics.Joints.DistanceJoint.SolveVelocityConstraints C# (CSharp) Method

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

            Vec2 vpA = Pool.PopVec2();
            Vec2 vpB = Pool.PopVec2();

            // Cdot = dot(u, v + cross(w, r))
            Vec2.CrossToOutUnsafe(wA, RA, vpA);
            vpA.AddLocal(vA);
            Vec2.CrossToOutUnsafe(wB, RB, vpB);
            vpB.AddLocal(vB);
            float Cdot = Vec2.Dot(U, vpB.SubLocal(vpA));

            float impulse = (-Mass) * (Cdot + Bias + Gamma * Impulse);
            Impulse += impulse;

            float Px = impulse * U.X;
            float Py = impulse * U.Y;

            vA.X -= InvMassA * Px;
            vA.Y -= InvMassA * Py;
            wA -= InvIA * (RA.X * Py - RA.Y * Px);
            vB.X += InvMassB * Px;
            vB.Y += InvMassB * Py;
            wB += InvIB * (RB.X * Py - RB.Y * Px);

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

            Pool.PushVec2(2);
        }