Convert Data

How to Convert from Cartesian to Spherical Coordinates via WGS84 Conventions

/*
	https://possiblywrong.wordpress.com/2014/02/14/when-approximate-is-better-than-exact/
  	This formula take 0.8 of the time of the other formula.
  	It is faster than the other formula.
  */
  
  #define	F64_WGS84_EARTH_RADIUS	6378137.0	// WGS 84 semi-major axis (https://en.wikipedia.org/wiki/Geodetic_datum)
  
  void CartesianToSphericalWGS84(f64 & longitude, f64 & latitude, f32 & altitude, f64 x, f64 y, f64 z)
  {
  	// Derived parameters
  
  	const f64 e2 = 0.0066943799901413165;	// f * (2 - f);
  	const f64 a1 = 42697.672707179969;		// a * e2;
  	const f64 a2 = 1823091254.6094613;		// a1 * a1;
  	const f64 a3 = 142.91722289827430;		// a1 * e2 / 2;
  	const f64 a4 = 4557728136.5236530;		// 2.5 * a2;
  	const f64 a5 = 42840.589930078240;		// a1 + a3;
  	const f64 a6 = 0.99330562000985867;		// 1 - e2;
  
  	// Convert ECEF (meters) to LLA (radians and meters).
  
  	// Olson, D. K., Converting Earth-Centered, Earth-Fixed Coordinates to
  	// Geodetic Coordinates, IEEE Transactions on Aerospace and Electronic
  	// Systems, 32 (1996) 473-476.
  
  	const f64 w = CMath::Sqrt(x * x + y * y);
  	const f64 zp = CMath::Abs(z);
  	const f64 w2 = w * w;
  	const f64 r2 = z * z + w2;
  	const f64 r = CMath::Sqrt(r2);
  	const f64 s2 = z * z / r2;
  	const f64 c2 = w2 / r2;
  
  	f64 u = a2 / r;
  	f64 v = a3 - a4 / r;
  
  	f64 c;
  	f64 s;
  	f64 ss;
  
  	if (c2 > 0.3)
  	{
  		s = (zp / r) * (1 + c2 * (a1 + u + s2 * v) / r);
  		latitude = CMath::ArcSin(s);
  		ss = s * s;
  		c = CMath::Sqrt(1 - ss);
  	}
  	else
  	{
  		c = (w / r) * (1 - s2 * (a5 - u - c2 * v) / r);
  		latitude = CMath::ArcCos(c);
  		ss = 1 - c * c;
  		s = CMath::Sqrt(ss);
  	}
  
  	const f64 g = 1 - e2 * ss;
  	const f64 rg = F64_WGS84_EARTH_RADIUS / CMath::Sqrt(g);
  	const f64 rf = a6 * rg;
  
  	u = w - rg * c;
  	v = zp - rf * s;
  
  	const f64 f = c * u + s * v;
  	const f64 m = c * v - s * u;
  	const f64 p = m / (rf / g + f);
  
  	latitude += p;
  
  	if (z < 0.0)
  	{
  		latitude = -latitude;
  	}
  
  	longitude = CMath::ArcTan(y, x);
  	altitude = StaticCast(f32, f + m * p / 2);
  }