Sanya-ips/ipslib/Ips.Library.Basic/Geo/GeoUtil.cs

using System;

namespace Ips.Library.Basic
{
    public static class GeoUtil
    {
        public const double WGS84a = 6378137.0;
        public const double f = 1.0 / 298.257224;
        public const double WGS84b = WGS84a - WGS84a * f;
        public const double WGS84a2 = WGS84a * WGS84a;
        public const double WGS84b2 = WGS84b * WGS84b;
        public const double C = 299792458.0;
        public const double R = 6378141.4;


        public static double[] LLA2XYZ(double lon, double lat, double alt)
        {
            lat = Math.PI * lat / 180.0;
            lon = Math.PI * lon / 180.0;

            double cosLat = Math.Cos(lat);
            double sinLat = Math.Sin(lat);

            double cosLong = Math.Cos(lon);
            double sinLong = Math.Sin(lon);

            double c = 1 / Math.Sqrt(cosLat * cosLat + (1 - f) * (1 - f) * sinLat * sinLat);
            double s = (1 - f) * (1 - f) * c;

            double x = (WGS84a * c + alt) * cosLat * cosLong;
            double y = (WGS84a * c + alt) * cosLat * sinLong;
            double z = (WGS84a * s + alt) * sinLat;

            return new double[3] { x, y, z };
        }

        public static double[] LLA2XYZ(double[] lla)
        {
            return LLA2XYZ(lla[0], lla[1], lla[2]);
        }

        public static double[] XYZ2LLA(double x, double y, double z)
        {
            double ea = Math.Sqrt((WGS84a * WGS84a - WGS84b * WGS84b) / (WGS84a * WGS84a));
            double eb = Math.Sqrt((WGS84a * WGS84a - WGS84b * WGS84b) / (WGS84b * WGS84b));
            double p = Math.Sqrt(x * x + y * y);

            double theta = Math.Atan2(z * WGS84a, p * WGS84b);
            double lon = Math.Atan2(y, x);
            double lat = Math.Atan2(z + eb * eb * WGS84b * Math.Pow(Math.Sin(theta), 3),
                                    p - ea * ea * WGS84a * Math.Pow(Math.Cos(theta), 3));
            double N = WGS84a / Math.Sqrt(1 - ea * ea * Math.Sin(lat) * Math.Sin(lat));
            double alt = p / Math.Cos(lat) - N;

            return new double[3] { lon * (180.0 / Math.PI), lat * (180.0 / Math.PI), alt };
        }

        public static double[] XYZ2LLA(double[] xyz)
        {
            return XYZ2LLA(xyz[0], xyz[1], xyz[2]);
        }

        public static double Distince(double x1, double y1, double z1, double x2, double y2, double z2)
        {
            double x = (x1 - x2);
            double y = (y1 - y2);
            double z = (z1 - z2);
            double dist = Math.Sqrt(x * x + y * y + z * z);
            return dist;
        }

        public static double Distince(double[] xyzOne, double[] xyzTwo)
        {
            return Distince(xyzOne[0], xyzOne[1], xyzOne[2], xyzTwo[0], xyzTwo[1], xyzTwo[2]);
        }

        public static double DistinceLla(double lon1, double lat1, double alt1, double lon2, double lat2, double alt2)
        {
            var xyz1 = LLA2XYZ(lon1, lat1, alt1);
            var xyz2 = LLA2XYZ(lon2, lat2, alt2);
            return Distince(xyz1, xyz2);
        }

        public static double DistinceLla(double[] lla1, double[] lla2)
        {
            double[] xyz1 = LLA2XYZ(lla1), xyz2 = LLA2XYZ(lla2);
            return Distince(xyz1, xyz2);
        }

        public static double CalcDt(double x1, double y1, double z1, double x2, double y2, double z2)
        {
            var dis = Distince(x1, y1, z1, x2, y2, z2);
            return (dis / C) * 1e6;
        }

        public static double CalcDt(double[] xyzOne, double[] xyzTwo)
        {
            return CalcDt(xyzOne[0], xyzOne[1], xyzOne[2], xyzTwo[0], xyzTwo[1], xyzTwo[2]);
        }

        public static double CalcDtoCenter(double[] tarLla, double[] satXyz, double[] antLla)
        {
            double[] tarXyz = LLA2XYZ(tarLla);
            double[] antXyz = LLA2XYZ(antLla);

            double disTarSat = Distince(tarXyz, satXyz);
            double disSatAnt = Distince(satXyz, antXyz);

            return -1 * (disTarSat + disSatAnt) / C * 1e6;
        }

        public static double CalcDtoCenter(double[] tarLla, double[] mSatXyz, double[] aSatXyz, double[] mAntLla, double[] aAntLla)
        {
            double[] tarXyz = LLA2XYZ(tarLla);
            double[] mAntXyz = LLA2XYZ(mAntLla);
            double[] aAntXyz = LLA2XYZ(aAntLla);

            double mDisTarSatAnt = Distince(tarXyz, mSatXyz) + Distince(mSatXyz, mAntXyz);
            double aDisTarSatAnt = Distince(tarXyz, aSatXyz) + Distince(aSatXyz, aAntXyz);

            return -1 * (aDisTarSatAnt - mDisTarSatAnt) / C * 1e6;
        }

        public static double[] CalcDtMinMax(double tarLonMin, double tarLonMax, double tarLatMin, double tarLatMax, double tarAlt,
            double satX, double satY, double satZ,
            double antLon, double antLat, double antAlt)
        {
            double[] p1 = LLA2XYZ(tarLonMin, tarLatMin, tarAlt);
            double[] p2 = LLA2XYZ(tarLonMin, tarLatMax, tarAlt);
            double[] p3 = LLA2XYZ(tarLonMax, tarLatMin, tarAlt);
            double[] p4 = LLA2XYZ(tarLonMax, tarLatMax, tarAlt);
            double[] satXYZ = new[] { satX, satY, satZ };

            double dt1 = CalcDt(p1, satXYZ), dt2 = CalcDt(p2, satXYZ), dt3 = CalcDt(p3, satXYZ), dt4 = CalcDt(p4, satXYZ);
            double dtMax = Math.Max(Math.Max(dt1, dt2), Math.Max(dt3, dt4));

            double dtMin;
            double[] satLLA = XYZ2LLA(satX, satY, satZ);
            if (GeoUtil.InZone(satLLA[0], satLLA[1], tarLonMin, tarLonMax, tarLatMin, tarLatMax))
            {
                double satLon = satLLA[0], satLat = satLLA[1];
                double[] minLLA = new[] { satLon, satLat, 0 };
                double[] minXYZ = LLA2XYZ(minLLA);
                dtMin = CalcDt(minXYZ, satXYZ);
            }
            else
            {
                var lonRange = GeoUtil.LonRange(tarLonMax, tarLonMin);
                var latRange = GeoUtil.LatRange(tarLatMax, tarLatMin);
                var centerLLA = new double[] { tarLonMin + lonRange / 2, tarLatMin + latRange / 2, tarAlt };
                var centerXYZ = LLA2XYZ(centerLLA);
                var dtCenter = CalcDt(centerXYZ, satXYZ);
                dtMin = dtMax - (dtMax - dtCenter) * 2;
            }
            var antXYZ = LLA2XYZ(antLon, antLat, antAlt);
            var dtSatAnt = CalcDt(satXYZ, antXYZ);
            dtMin += dtSatAnt;
            dtMax += dtSatAnt;
            return new double[] { dtMin, dtMax };
        }

        public static double[] CalcDtCR(double[] tarRange, double[] satXyz, double[] antLla)
        {
            var lonRange = tarRange[3];
            var latRange = tarRange[4];
            var lonMin = tarRange[0] - lonRange / 2;
            var lonMax = tarRange[0] + lonRange / 2;
            var latMin = tarRange[1] - latRange / 2;
            var latMax = tarRange[1] + latRange / 2;

            var dtoCenter = CalcDtoCenter(tarRange, satXyz, antLla);

            double[] p1 = LLA2XYZ(lonMin, latMin, tarRange[2]);
            double[] p2 = LLA2XYZ(lonMin, latMax, tarRange[2]);
            double[] p3 = LLA2XYZ(lonMax, latMin, tarRange[2]);
            double[] p4 = LLA2XYZ(lonMax, latMax, tarRange[2]);

            double disP1Sat = Distince(p1, satXyz), disP2Sat = Distince(p2, satXyz), disP3Sat = Distince(p3, satXyz), disP4Sat = Distince(p4, satXyz);
            var disMax = Math.Max(Math.Max(disP1Sat, disP2Sat), Math.Max(disP3Sat, disP4Sat));
            var dtMax = -1 * (disMax + Distince(satXyz, LLA2XYZ(antLla))) / C * 1e6;

            var dtoRange = Math.Abs(2 * (dtMax - dtoCenter));

            return new double[] { dtoCenter, dtoRange };
        }

        public static double[] CalcDtCRByZone(double tarLonMin, double tarLonMax, double tarLatMin, double tarLatMax, double tarAlt, double[] satXyz, double[] antLla)
        {
            var lonRange = GeoUtil.LonRange(tarLonMax, tarLonMin);
            var latRange = GeoUtil.LatRange(tarLatMax, tarLatMin);
            var tarLon = GeoUtil.Lon180(tarLonMin + lonRange / 2);
            var tarLat = tarLatMin + latRange / 2;
            var tarLla = new[] { tarLon, tarLat, tarAlt };

            var dtoCenter = CalcDtoCenter(tarLla, satXyz, antLla);

            double[] p1 = LLA2XYZ(tarLonMin, tarLatMin, tarAlt);
            double[] p2 = LLA2XYZ(tarLonMin, tarLatMax, tarAlt);
            double[] p3 = LLA2XYZ(tarLonMax, tarLatMin, tarAlt);
            double[] p4 = LLA2XYZ(tarLonMax, tarLatMax, tarAlt);

            double disP1Sat = Distince(p1, satXyz), disP2Sat = Distince(p2, satXyz), disP3Sat = Distince(p3, satXyz), disP4Sat = Distince(p4, satXyz);
            var disMax = Math.Max(Math.Max(disP1Sat, disP2Sat), Math.Max(disP3Sat, disP4Sat));
            var dtMax = -1 * (disMax + Distince(satXyz, LLA2XYZ(antLla))) / C * 1e6;

            var dtoRange = Math.Abs(2 * (dtMax - dtoCenter));

            return new double[] { dtoCenter, dtoRange };
        }

        public static double[] CalcDtCR(double[] tarRange, double[] mSatXyz, double[] aSatXyz, double[] mAntLla, double[] aAntLla)
        {
            var lonRange = tarRange[3];
            var latRange = tarRange[4];
            var lonMin = tarRange[0] - lonRange / 2;
            var lonMax = tarRange[0] + lonRange / 2;
            var latMin = tarRange[1] - latRange / 2;
            var latMax = tarRange[1] + latRange / 2;
            var tarAlt = tarRange[2];

            var dtoCenter = CalcDtoCenter(tarRange, mSatXyz, aSatXyz, mAntLla, aAntLla);

            var lonStep = 0.2;
            var latStep = 0.2;

            double dtMax = double.MinValue, dtMin = double.MaxValue;
            for (double lon = lonMin; lon <= lonMax; lon += lonStep)
            {
                for (double lat = latMin; lat <= latMax; lat += latStep)
                {
                    var dt = CalcDtoCenter(new[] { lon, lat, tarAlt }, mSatXyz, aSatXyz, mAntLla, aAntLla);
                    dtMax = Math.Max(dt, dtMax);
                    dtMin = Math.Min(dt, dtMin);
                }
            }
            var dtoRange = (dtMax - dtMin) * 1.2;
            return new double[] { dtoCenter, dtoRange };

        }

        public static double CalcLeadDt(double dt1, double[] tarLla, double[] antLla, double[] ephXyz1, double[] ephXyz2)
        {
            var refTimeDt = GeoUtil.CalcDtoCenter(tarLla, ephXyz1, antLla);
            var tarTimeDt = GeoUtil.CalcDtoCenter(tarLla, ephXyz2, antLla);
            var dt2 = dt1 + (tarTimeDt - refTimeDt);
            return dt2;
        }

        public static double CalcLeadDt(double dt1, double[] tarLla, double[] mAntLla, double[] aAntLla, double[] mEph1, double[] aEph1, double[] mEph2, double[] aEph2)
        {
            var refTimeDt = GeoUtil.CalcDtoCenter(tarLla, mEph1, aEph1, mAntLla, aAntLla);
            var tarTimeDt = GeoUtil.CalcDtoCenter(tarLla, mEph2, aEph2, mAntLla, aAntLla);
            var dt2 = dt1 + (tarTimeDt - refTimeDt);
            return dt2;
        }

        public static double[] GetRange(params double?[][] ranges)
        {
            double lonMin = -180;
            double lonMax = 180;
            double latMin = -90;
            double latMax = 90;
            foreach (var range in ranges)
            {
                lonMin = Math.Max(lonMin, range?[0] ?? -180);
                lonMax = Math.Min(lonMax, range?[1] ?? 180);
                latMin = Math.Max(latMin, range?[2] ?? -90);
                latMax = Math.Min(latMax, range?[3] ?? 90);
            }
            return new double[] { lonMin, lonMax, latMin, latMax };
        }

        public static bool InZone(double tarLon, double tarLat, double lonMin, double lonMax, double latMin, double latMax)
        {
            tarLon = Lon360(tarLon);
            lonMin = Lon360(lonMin);
            lonMax = Lon360(lonMax);
            return tarLon >= lonMin && tarLon <= lonMax && tarLat >= latMin && tarLat <= latMax;
        }

        public static double LonRange(double lon1, double lon2)
        {
            lon1 = Lon360(lon1);
            lon2 = Lon360(lon2);
            return Math.Abs(lon1 - lon2);
        }

        public static double LatRange(double lat1, double lat2)
        {
            return Math.Abs(lat1 - lat2);
        }

        public static double Lon360(double lon)
        {
            return lon < 0 ? lon + 360 : lon;
        }

        public static double Lon180(double lon)
        {
            return lon > 180 ? lon - 360 : lon;
        }

        public static bool IsValidPoint(double lon, double lat)
        {
            return IsValidLon(lon) && IsValidLat(lat);
        }

        public static bool IsValidLon(double lon)
        {
            return lon > -180 && lon < 180;
        }

        public static bool IsValidLat(double lat)
        {
            return lat > -90 && lat < 90;
        }
    }
}