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 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));
}
static GeoDatum()
{
var wgs84a = 6378137.0;
var wgs84g = 1 / 298.257223563;
var wgs84e = Math.Sqrt(1 - BasicMath.Sqr(1 - wgs84g));
_wgs84 = new GeoDatum(wgs84a, wgs84e);
}
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 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 static RotoTranslation3 Enu(this GeoPoint2 origin, GeoDatum datum)
{
return(GeoPoint3Utils.Enu((GeoPoint3)origin, datum));
}
public static Vector3 ToEcef(this GeoPoint2 point, GeoDatum datum)
{
return(GeoPoint3Utils.ToEcef((GeoPoint3)point, datum));
}
