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));
}