Calculation of distance from Cartesian point to ellipsoid

I tried today to implement a glsl compatible way to get from a Cartesian point distance to ellipsoid.

Because Ellipsoid.WGS84.cartesianToCartographic is to complicated to calculate for each point (and I don't need all values), I tried another way:

// Calculate angle between XY plane and p
var angle = Math.asin(Math.abs(p.z) / Cesium.Cartesian3.magnitude(p));
// Map angle to ellipsoid and get length
var ellipsoidHeight = Math.sqrt(Math.pow(Cesium.Ellipsoid.WGS84._radii.x * Math.cos(angle), 2.0) + Math.pow(Cesium.Ellipsoid.WGS84._radii.z * Math.sin(angle), 2.0));

Now I expected following condition is true (or nearly)

Cesium.Ellipsoid.WGS84.cartesianToCartographic(p).height === Cesium.Cartesian3.magnitude(p) - ellipsoidHeight

But this is not the case. The difference with my test coordinates are around 25 meter. So I compared following condition:

Cesium.Cartesian3.magnitude(Cesium.Ellipsoid.WGS84.scaleToGeodeticSurface(p)) === ellipsoidHeight

But already there I get the difference. So miss I something on my calculation or is cartesianToCartographic wrong?

It seems like the difference is Cesium computes cartesianToCartographic with geodetic latitude whereas you are computing basically the geocentric distance.

Possibly helpful image: