[core] Fix PGS friction cone.

[duburcqa]

Enforcing ||Ft||<µ Fn independently using Fx/y = min(Fx/y, sqrt(µ Fn^2 - Fy/x^2)) for x and y axes is ill-posed and can be proven to not converge to the solution. It clamping must be done jointly x and y axes (x, y) *= min(1.0, ||(x, y)|| / µ Fn) to converge.