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