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

        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;

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

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

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

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

            // Compute the effective mass matrix.
            Rot.MulToOutUnsafe(qA, temp.Set(m_localAnchorA).SubLocal(LocalCenterA), RA);
            Rot.MulToOutUnsafe(qB, temp.Set(m_localAnchorB).SubLocal(LocalCenterB), RB);

            // J = [-I -r1_skew I r2_skew]
            // [ 0 -1 0 1]
            // r_skew = [-ry; rx]

            // Matlab
            // K = [ mA+r1y^2*iA+mB+r2y^2*iB, -r1y*iA*r1x-r2y*iB*r2x, -r1y*iA-r2y*iB]
            // [ -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB, r1x*iA+r2x*iB]
            // [ -r1y*iA-r2y*iB, r1x*iA+r2x*iB, iA+iB]

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

            Mat22 K = Pool.PopMat22();
            K.Ex.X = mA + mB + iA * RA.Y * RA.Y + iB * RB.Y * RB.Y;
            K.Ex.Y = (-iA) * RA.X * RA.Y - iB * RB.X * RB.Y;
            K.Ey.X = K.Ex.Y;
            K.Ey.Y = mA + mB + iA * RA.X * RA.X + iB * RB.X * RB.X;

            K.InvertToOut(LinearMass);

            AngularMass = iA + iB;
            if (AngularMass > 0.0f)
            {
                AngularMass = 1.0f / AngularMass;
            }

            if (data.Step.WarmStarting)
            {
                // Scale impulses to support a variable time step.
                m_linearImpulse.MulLocal(data.Step.DtRatio);
                m_angularImpulse *= data.Step.DtRatio;

                Vec2 P = Pool.PopVec2();
                P.Set(m_linearImpulse);

                temp.Set(P).MulLocal(mA);
                vA.SubLocal(temp);
                wA -= iA * (Vec2.Cross(RA, P) + m_angularImpulse);

                temp.Set(P).MulLocal(mB);
                vB.AddLocal(temp);
                wB += iB * (Vec2.Cross(RB, P) + m_angularImpulse);

                Pool.PushVec2(1);
            }
            else
            {
                m_linearImpulse.SetZero();
                m_angularImpulse = 0.0f;
            }
            data.Velocities[IndexA].V.Set(vA);
            if (data.Velocities[IndexA].W != wA)
            {
                Debug.Assert(data.Velocities[IndexA].W != wA);
            }
            data.Velocities[IndexA].W = wA;
            data.Velocities[IndexB].V.Set(vB);
            data.Velocities[IndexB].W = wB;

            Pool.PushRot(2);
            Pool.PushVec2(1);
            Pool.PushMat22(1);
        }