Пример #1
0
        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));
        }
Пример #3
0
        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));
 }