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

        public override bool SolvePositionConstraints(SolverData data)
        {
            if (FrequencyHz > 0.0f)
            {
                return true;
            }
            Rot qA = Pool.PopRot();
            Rot qB = Pool.PopRot();
            Vec2 rA = Pool.PopVec2();
            Vec2 rB = Pool.PopVec2();
            Vec2 u = Pool.PopVec2();

            Vec2 cA = data.Positions[IndexA].C;
            float aA = data.Positions[IndexA].A;
            Vec2 cB = data.Positions[IndexB].C;
            float aB = data.Positions[IndexB].A;

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

            Rot.MulToOutUnsafe(qA, u.Set(LocalAnchorA).SubLocal(LocalCenterA), rA);
            Rot.MulToOutUnsafe(qB, u.Set(LocalAnchorB).SubLocal(LocalCenterB), rB);
            u.Set(cB).AddLocal(rB).SubLocal(cA).SubLocal(rA);

            float length = u.Normalize();
            float C = length - Length;
            C = MathUtils.Clamp(C, -Settings.MAX_LINEAR_CORRECTION, Settings.MAX_LINEAR_CORRECTION);

            float impulse = (-Mass) * C;
            float Px = impulse * u.X;
            float Py = impulse * u.Y;

            cA.X -= InvMassA * Px;
            cA.Y -= InvMassA * Py;
            aA -= InvIA * (rA.X * Py - rA.Y * Px);
            cB.X += InvMassB * Px;
            cB.Y += InvMassB * Py;
            aB += InvIB * (rB.X * Py - rB.Y * Px);

            data.Positions[IndexA].C.Set(cA);
            data.Positions[IndexA].A = aA;
            data.Positions[IndexB].C.Set(cB);
            data.Positions[IndexB].A = aB;

            Pool.PushVec2(3);
            Pool.PushRot(2);

            return MathUtils.Abs(C) < Settings.LINEAR_SLOP;
        }