/// <summary>
/// Returns the longitude, latitude and height from XYZ-coords on a given world reference ellipsoid.
/// </summary>
/// <param name="xyz">Vector with xyz.</param>
/// <param name="ellipsoid">GeoEllipsoid from the GeoConsts class.</param>
/// <returns>Position vector with Lon,Lat,Hei</returns>
public static V3d LonLatHeightFromXyz(V3d xyz, GeoEllipsoid ellipsoid)
{
// this implementation follows the Bowring Method (85).
// It might be extended to the newer Toms Method (99), but is delivers suitable results.
double a = ellipsoid.A;
double b = ellipsoid.B;
// Distance between x and y.
double W = (xyz.X * xyz.X + xyz.Y * xyz.Y).Sqrt();
// apprx phi
double PHI = System.Math.Atan((xyz.Z * a) / (W * b));
double eq = ellipsoid.EQ;
double e2q = ellipsoid.E2Q;// (eq / (1.0 - eq)).Sqrt();
double lam = System.Math.Atan(xyz.Y / xyz.X);
double sinPhi = (PHI).Sin();
double sinPhiTo3 = sinPhi * sinPhi * sinPhi;
double cosPhi = (PHI).Cos();
double cosPhiTo3 = cosPhi * cosPhi * cosPhi;
double phi = System.Math.Atan((xyz.Z + e2q * b * sinPhiTo3) / (W - eq * a * cosPhiTo3));
double sinphi = phi.Sin();
// curvature in the prime vertical
double Rv = a / (1.0 - eq * sinphi * sinphi).Sqrt();
double cosphi = phi.Cos();
double h = W / cosphi - Rv;
double lat = Conversion.DegreesFromRadians(phi); // phi * 180 / pi;
double lon = Conversion.DegreesFromRadians(lam); // lam * 180 / pi;
return(new V3d(lon, lat, h));
}
public static V3d GaussKruegerPlaneToEllipsoid(
V3d rightHeightAlt, GeoEllipsoid ellipsoid, double referenceMeridan
)
{
double a = ellipsoid.A;
double b = ellipsoid.B;
double Y = rightHeightAlt.X;
double X = rightHeightAlt.Y + 5000000;
double n = (a - b) / (a + b);
double nTo2 = n * n;
double nTo3 = nTo2 * n;
double nTo4 = nTo3 * n;
double nTo5 = nTo4 * n;
double c1 = (a + b) / 2.0 * (1.0 + 1.0 / 4.0 * nTo2 * 1.0 / 64.0 * nTo4);
double c2 = 3.0 / 2.0 * n - 27.0 / 32.0 * nTo3 - 269.0 / 512.0 * nTo5;
double c3 = 21.0 / 16.0 * nTo2 - 55.0 / 32 * nTo4;
double c4 = 151.0 / 96.0 * nTo3 - 417.0 / 128.0 * nTo5;
double c5 = 1097.0 / 512.0 * nTo4;
double c0 = X / c1;
//latitude footprint in radians
double fPHI = c0 + c2 * (2.0 * c0).Sin() + c3 * (4.0 * c0).Sin() + c4 * (6.0 * c0).Sin() + c5 * (8.0 * c0).Sin();
//% second nummerical eccentrencity
double e2q = ellipsoid.E2Q;
//% auxilary quantity 1
double cosfPHI = fPHI.Cos();
double nuTo2 = e2q * cosfPHI * cosfPHI;
double nuTo4 = nuTo2 * nuTo2;
//%radius of the curvature in prime vertical
double N = (a * a) / (b * (1.0 + nuTo2).Sqrt());
double Nto2 = N * N;
double Nto3 = Nto2 * N;
double Nto4 = Nto3 * N;
double Nto5 = Nto4 * N;
double Nto6 = Nto5 * N;
double Nto7 = Nto6 * N;
double Nto8 = Nto7 * N;
//% auxilary quantity 2
double t = fPHI.Tan();
// t to powers
double tTo2 = t * t;
double tTo4 = tTo2 * tTo2;
double tTo6 = tTo2 * tTo4;
double Yto2 = Y * Y;
double Yto3 = Yto2 * Y;
double Yto4 = Yto3 * Y;
double Yto5 = Yto4 * Y;
double Yto6 = Yto5 * Y;
double Yto7 = Yto6 * Y;
double Yto8 = Yto7 * Y;
double p1 = fPHI + (t / (2.0 * Nto2)) * (-1.0 - nuTo2) * Yto2;
double p2 = (t / (24.0 * Nto4)) * (5.0 + 3.0 * tTo2 + 6.0 * nuTo2 - 6 * tTo2 * nuTo2 - 3.0 * nuTo4 - 9.0 * tTo2 * nuTo4) * Yto4;
double p3 = (t / (720.0 * Nto6)) * (-61.0 - 90.0 * tTo2 - 45.0 * tTo4 - 107.0 * nuTo2 + 162.0 * tTo2 * nuTo2 + 45.0 * tTo4 * nuTo2) * Yto6;
double p4 = (t / (40320.0 * Nto8)) * (1385.0 + 3633.0 * tTo2 + 4045 * tTo4 + 1575 * tTo6) * Yto8;
double phi = p1 + p2 + p3 + p4;
double l1 = Y / (cosfPHI * N);
double l2 = ((-1.0 - 2.0 * tTo2 - nuTo2) * Yto3) / (6.0 * Nto3 * cosfPHI);
double l3 = (5.0 + 28.0 * tTo2 + 24 * tTo4 + 6.0 * nuTo2 + 8.0 * tTo2 * nuTo2) * Yto5 / (120.0 * Nto5 * cosfPHI);
double l4 = (-61.0 - 662 * tTo2 - 1320 * tTo4 - 720 * tTo6) * Yto7 / (5040.0 * Nto7 * cosfPHI);
double lam = l1 + l2 + l3 + l4;
double lat = Conversion.DegreesFromRadians(phi);
double lon = Conversion.DegreesFromRadians(lam) + referenceMeridan;
return(new V3d(lon, lat, rightHeightAlt.Z));
}
