public static GeoPoint3 EcefToGeodetic(Vector3 ecef, GeoDatum datum)
{
const double precision2 = 1e-6;
const int maxIterations = 16;
var norm = ecef.Norm;
var phi = Math.Atan2(ecef.Y, ecef.X);
var theta0 = BasicMath.Asin(ecef.Z / ecef.Norm); // first approximation
var h0 = norm - datum.Semiminor; // first approximation
var theta = theta0;
var h = h0;
var error2 = double.PositiveInfinity;
for (var i = 0; i < maxIterations && error2 > precision2; i++)
{
var ecefPrime = new GeoPoint3(theta, phi, h).ToEcef(datum);
var normPrime = ecefPrime.Norm;
var thetaSphere = BasicMath.Asin(ecefPrime.Z / normPrime);
var hSphere = normPrime - datum.Semiminor;
theta += theta0 - thetaSphere;
h += h0 - hSphere;
error2 = Vector3.Distance2(ecef, ecefPrime);
}
return(new GeoPoint3(theta, phi, h));
}
public void Enu_is_compatible_with_numerical_differential_of_GeodeticToEcef()
{
var datum = new GeoDatum(semimajor: 6, eccentricity: 0.314);
var geodetic = GeoPoint3.InDegrees(latitude_deg: 32.067, longitude_deg: 54.783, altitude: 3.141);
var ecef = geodetic.ToEcef(datum);
var enu = geodetic.Enu(datum);
Expect(Vector3.Distance(enu.Translation, ecef), Is.LessThan(1e-10));
var eps = 1e-10;
var tolerance = 1e-5;
var geodeticN = new GeoPoint3(geodetic.Latitude + eps, geodetic.Longitude, geodetic.Altitude);
var ecefN = geodeticN.ToEcef(datum);
var enuN = enu.ApplyInv(ecefN);
Expect(Vector3.Distance(enuN.Normalized, Vector3.UnitY), Is.LessThan(tolerance));
var geodeticE = new GeoPoint3(geodetic.Latitude, geodetic.Longitude + eps, geodetic.Altitude);
var ecefE = geodeticE.ToEcef(datum);
var enuE = enu.ApplyInv(ecefE);
Expect(Vector3.Distance(enuE.Normalized, Vector3.UnitX), Is.LessThan(tolerance));
var geodeticU = new GeoPoint3(geodetic.Latitude, geodetic.Longitude, geodetic.Altitude + eps);
var ecefU = geodeticU.ToEcef(datum);
var enuU = enu.ApplyInv(ecefU);
Expect(Vector3.Distance(enuU, eps * Vector3.UnitZ), Is.LessThan(eps * 1e-4));
}
public static RotoTranslation3 Enu(this GeoPoint3 origin, GeoDatum datum)
{
var ecefOrigin = origin.ToEcef(datum);
var z = origin.EcefUp();
var y = Vector3.UnitZ - z.Z * z;
var x = y.Cross(z);
return(new RotoTranslation3(Rotation3Utils.GramSchmidt(x, y), ecefOrigin));
}
public void ToEcef_returns_correct_result()
{
// example computed using http://www.oc.nps.edu/oc2902w/coord/llhxyz.htm with 1m precision at each axis
var geodetic = GeoPoint3.InDegrees(latitude_deg: 32.067, longitude_deg: 54.783, altitude: 3.141);
var ecef = new Vector3(3119880, 4419927, 3366731);
var actualEcef = geodetic.ToEcef(GeoDatum.Wgs84);
Expect(Vector3.Distance(actualEcef, ecef), Is.LessThan(2.0));
}
public static Vector3 EcefUp(this GeoPoint3 point)
{
var theta = point.Latitude;
var phi = point.Longitude;
var sinTheta = Math.Sin(theta);
var cosTheta = Math.Cos(theta);
var sinPhi = Math.Sin(phi);
var cosPhi = Math.Cos(phi);
return(new Vector3(cosTheta * cosPhi, cosTheta * sinPhi, sinTheta));
}
public static Vector3 ToEcef(this GeoPoint3 point, GeoDatum datum)
{
var a = datum.Semimajor;
var e = datum.Eccentricity;
var theta = point.Latitude;
var phi = point.Longitude;
var h = point.Altitude;
var sinTheta = Math.Sin(theta);
var cosTheta = Math.Cos(theta);
var sinPhi = Math.Sin(phi);
var cosPhi = Math.Cos(phi);
var n = a / Math.Sqrt(1 - BasicMath.Sqr(e * sinTheta)); // distance from ellipsoid point to z-axis along ellipsoid normal
var xy = (n + h) * cosTheta;
var x = xy * cosPhi;
var y = xy * sinPhi;
var z = (n * (1 - e * e) + h) * sinTheta;
return(new Vector3(x, y, z));
}
public GeoPoint3 Deserialize()
{
return(GeoPoint3.InDegrees(Lat, Long, Alt));
}
public YamlSerialization(GeoPoint3 geoPoint3)
{
Lat = geoPoint3.Latitude_deg;
Long = geoPoint3.Longtitude_deg;
Alt = geoPoint3.Altitude;
}
