Plane.Sdk3/PlaneGcsSdk.Contract_Shared/Geography/GeographyUtils.cs

using System;

namespace Plane.Geography
{
    /// <summary>
    /// 提供地理相关计算的方法。
    /// </summary>
    public static class GeographyUtils
    {
        /// <summary>
        /// 米数转换为纬度跨度时需要乘的常数。
        /// </summary>
        public const double METERS_TO_LAT_SPAN = 1 / (GLOBE_CIRCUMFERENCE / 360);

        /// <summary>
        /// 角度转换为弧度时需要乘的常数。
        /// </summary>
        private const double DEG_TO_RAD = Math.PI / 180;

        /// <summary>
        /// 地球的半径。
        /// </summary>
        private const double EARTH_RADIUS = 6371000;

        /// <summary>
        /// 当前星球的周长。
        /// </summary>
        private const double GLOBE_CIRCUMFERENCE = GLOBE_RADIUS * 2 * Math.PI;

        /// <summary>
        /// 当前星球的半径。
        /// </summary>
        private const double GLOBE_RADIUS = EARTH_RADIUS;

        /// <summary>
        /// 计算从 (lat1, lng1) 到 (lat2, lng2) 的方向，单位为弧度。
        /// </summary>
        /// <param name="lat1">第一个点的纬度。</param>
        /// <param name="lng1">第一个点的经度。</param>
        /// <param name="lat2">第二个点的纬度。</param>
        /// <param name="lng2">第二个点的经度。</param>
        /// <returns>点 1 到点 2 的方向。</returns>
        public static double CalcDirection2D(double lat1, double lng1, double lat2, double lng2)
        {
            return Math.Atan2((lng2 - lng1) / CalcMetersToLngSpan(lat1), (lat2 - lat1) / METERS_TO_LAT_SPAN).NormalizeDirectionRad();
        }

        /// <summary>
        /// 计算空间中两点间的距离，单位为米。
        /// </summary>
        /// <param name="l1">点 1。</param>
        /// <param name="l2">点 2。</param>
        /// <returns>空间中两点间的距离，单位为米。</returns>
        public static double CalcDistance(ILocation l1, ILocation l2)
        {
            return CalcDistance(l1.Latitude, l1.Longitude, l1.Altitude, l2.Latitude, l2.Longitude, l2.Altitude);
        }
        /// <summary>
        /// 计算空间中两点间的距离，单位为米。
        /// </summary>
        /// <param name="lat1">纬度 1。</param>
        /// <param name="lng1">经度 1。</param>
        /// <param name="lat2">纬度 2。</param>
        /// <param name="lng2">经度 2。</param>
        /// <returns>空间中两点间的距离，单位为米。</returns>
        public static double CalcDistance_simple(double lat1, double lng1,  double lat2, double lng2)
        {
            double dx = lng2 - lng1;
            double dy = lat2 - lat1;
            double b = (lat1 + lat2) * 0.5;
            double Lx = (0.05 * b * b * b - 19.16 * b * b + 47.13 * b + 110966) * dx;
            double Ly = (17 * b + 110352) * dy;
            return Math.Sqrt(Lx * Lx + Ly * Ly);
        }


        /// <summary>
        /// 计算空间中两点间的距离，单位为米。
        /// </summary>
        /// <param name="lat1">纬度 1。</param>
        /// <param name="lng1">经度 1。</param>
        /// <param name="alt1">高度 1。</param>
        /// <param name="lat2">纬度 2。</param>
        /// <param name="lng2">经度 2。</param>
        /// <param name="alt2">高度 2。</param>
        /// <returns>空间中两点间的距离，单位为米。</returns>
        public static double CalcDistance_simple(double lat1, double lng1, double alt1, double lat2, double lng2,double alt2)
        {
            double dx = lng2 - lng1;
            double dy = lat2 - lat1;
            double b = (lat1 + lat2) * 0.5;
            double Lx = (0.05 * b * b * b - 19.16 * b * b + 47.13 * b + 110966) * dx;
            double Ly = (17 * b + 110352) * dy;
            double d= Math.Sqrt(Lx * Lx + Ly * Ly);
            return Math.Sqrt(Math.Pow((alt2 - alt1), 2) + Math.Pow(d, 2));
        }

        /// <summary>
        /// 计算空间中两点之间的距离，单位为米。
        /// </summary>
        /// <param name="lat1">纬度 1。</param>
        /// <param name="lng1">经度 1。</param>
        /// <param name="alt1">高度 1。</param>
        /// <param name="lat2">纬度 2。</param>
        /// <param name="lng2">经度 2。</param>
        /// <param name="alt2">高度 2。</param>
        /// <returns>空间中两点之间的距离，单位为米。</returns>
        public static double CalcDistance(double lat1, double lng1, double alt1, double lat2, double lng2, double alt2)
        {
            // generally used geo measurement function
            // var R = 6378.137; // Radius of earth in KM

            var dLat = (lat2 - lat1) * Math.PI / 180;
            var dLon = (lng2 - lng1) * Math.PI / 180;
            var a = Math.Sin(dLat / 2) * Math.Sin(dLat / 2)
                + Math.Cos(lat1 * Math.PI / 180) * Math.Cos(lat2 * Math.PI / 180) * Math.Sin(dLon / 2) * Math.Sin(dLon / 2);
            var c = 2 * Math.Atan2(Math.Sqrt(a), Math.Sqrt(1 - a));
            var d = GLOBE_RADIUS * c;
            return Math.Sqrt(Math.Pow((alt2 - alt1), 2) + Math.Pow(d, 2));
        }

        /// <summary>
        /// 计算二维平面上两个位置之间的距离，单位为米。
        /// </summary>
        /// <param name="l1">位置 1。</param>
        /// <param name="l2">位置 2。</param>
        /// <returns>空间中两点间的距离。</returns>
        public static double CalcDistance2D(ILocation2D l1, ILocation2D l2)
            => CalcDistance2D(l1.Latitude, l1.Longitude, l2.Latitude, l2.Longitude);

        /// <summary>
        /// 计算二维平面上两个位置之间的距离，单位为米。
        /// </summary>
        /// <param name="lat1">纬度 1。</param>
        /// <param name="lng1">经度 1。</param>
        /// <param name="lat2">纬度 2。</param>
        /// <param name="lng2">经度 2。</param>
        /// <returns>计算二维平面上两个位置之间的距离，单位为米。</returns>
        public static double CalcDistance2D(double lat1, double lng1, double lat2, double lng2)
            => CalcDistance(lat1, lng1, 0, lat2, lng2, 0);

        /// <summary>
        /// 计算在水平面上从指定点往指定方向移动指定距离后所在的点。
        /// </summary>
        /// <param name="lat1">出发点的纬度。</param>
        /// <param name="lng1">出发点的经度。</param>
        /// <param name="directionDegrees">移动方向，单位为角度。</param>
        /// <param name="distance">移动距离。</param>
        /// <returns>从指定点往指定方向移动指定距离后所在的点。</returns>
        public static Tuple<double, double> CalcLatLngSomeMetersAway2D(double lat1, double lng1, float directionDegrees, float distance)
        {
            var direction = directionDegrees.DegToRad();
            return Tuple.Create(
                lat1 + distance * Math.Cos(direction) * METERS_TO_LAT_SPAN,
                lng1 + distance * Math.Sin(direction) * CalcMetersToLngSpan(lat1)
            );
        }

        /// <summary>
        /// 计算在水平面上从指定点往指定方向移动指定距离后所在的点。
        /// </summary>
        /// <param name="loc1">出发点。</param>
        /// <param name="directionDegrees">移动方向，单位为角度。</param>
        /// <param name="distance">移动距离。</param>
        /// <returns>从指定点往指定方向移动指定距离后所在的点。</returns>
        public static ILocation2D CalcLatLngSomeMetersAway2D(ILocation2D loc1, float directionDegrees, float distance)
        {
            var direction = directionDegrees.DegToRad();
            return new PLLocation(
                loc1.Latitude + distance * Math.Cos(direction) * METERS_TO_LAT_SPAN,
                loc1.Longitude + distance * Math.Sin(direction) * CalcMetersToLngSpan(loc1.Latitude),
                0
            );
        }

        /// <summary>
        /// 计算在指定纬度上，米数转换为经度跨度时需要乘的值。
        /// </summary>
        /// <param name="latitude">纬度。</param>
        /// <returns>在指定纬度上，米数转换为经度跨度时需要乘的值。</returns>
        public static double CalcMetersToLngSpan(double latitude)
        {
            return METERS_TO_LAT_SPAN / Math.Cos(latitude * DEG_TO_RAD);
        }

        /// <summary>
        /// 检测同一水平面上（忽略高度）的两条线段是否相交。
        /// </summary>
        /// <param name="l1">线段 A 端点 1。</param>
        /// <param name="l2">线段 A 端点 2。</param>
        /// <param name="l3">线段 B 端点 1。</param>
        /// <param name="l4">线段 B 端点 2。</param>
        /// <returns>若相交，返回 true；否则返回 false。</returns>
        public static bool CheckCrossing2D(ILocation2D l1, ILocation2D l2, ILocation2D l3, ILocation2D l4)
            => CheckCrossing2D(l1.Latitude, l1.Longitude, l2.Latitude, l2.Longitude, l3.Latitude, l3.Longitude, l4.Latitude, l4.Longitude);

        /// <summary>
        /// 检测同一水平面上的两条线段是否相交。
        /// </summary>
        /// <param name="latA1">线段 A 端点 1 的纬度。</param>
        /// <param name="lngA1">线段 A 端点 1 的经度。</param>
        /// <param name="latA2">线段 A 端点 2 的纬度。</param>
        /// <param name="lngA2">线段 A 端点 2 的经度。</param>
        /// <param name="latB1">线段 B 端点 1 的纬度。</param>
        /// <param name="lngB1">线段 B 端点 1 的经度。</param>
        /// <param name="latB2">线段 B 端点 2 的纬度。</param>
        /// <param name="lngB2">线段 B 端点 2 的经度。</param>
        /// <returns>若相交，返回 true；否则返回 false。</returns>
        public static bool CheckCrossing2D(double latA1, double lngA1, double latA2, double lngA2,
            double latB1, double lngB1, double latB2, double lngB2)
        {
            // 线段 A(x1, y1) - B(x2, y2), 所在直线 L1 方程为 F(x, y) = 0;
            // 线段 C(x3, y3) - D(x4, y4), 所在直线 L2 方程为 G(x, y) = 0;
            // 思路:
            // (即问题的充要条件)
            //      (点 A 与点 B 在直线 L2 两侧) AND (点 C 与点 D 在直线 L1 两侧);
            // 方法:
            // 如果点 P(Xp, Yp) 不在直线 a * x + b * y + c = 0 上, 则 a* Xp + b * Yp + c <> 0;
            // 如果用两个点的坐标代入同一直线方程 a * x + b * y + c 计算出的值异号, 则两点在直线两侧;
            // 解法:
            // (G(x1, y1) * G(x2, y2) < 0) AND (F(x3, y3) * F(x4, y4) < 0)

            // 斜截式 y = kx + b
            // 即 kx - y + b = 0

            var k1 = (latA2 - latA1) / (lngA2 - lngA1);
            var b1 = latA1 - k1 * lngA1;

            var k2 = (latB2 - latB1) / (lngB2 - lngB1);
            var b2 = latB1 - k2 * lngB1;

            return k2 * lngA1 - latA1 + b2 > 0 != k2 * lngA2 - latA2 + b2 > 0 &&
                k1 * lngB1 - latB1 + b1 > 0 != k1 * lngB2 - latB2 + b1 > 0;
        }

        /// <summary>
        /// 把角度转换为弧度。
        /// </summary>
        /// <param name="value">角度值。</param>
        /// <returns>弧度值。</returns>
        public static double DegToRad(this float value)
        {
            return value * DEG_TO_RAD;
        }

        /// <summary>
        /// 把角度转换为弧度。
        /// </summary>
        /// <param name="value">角度值。</param>
        /// <returns>弧度值。</returns>
        public static double DegToRad(this double value)
        {
            return value * DEG_TO_RAD;
        }

        /// <summary>
        /// 把角度转换为弧度。
        /// </summary>
        /// <param name="value">角度值。</param>
        /// <returns>弧度值。</returns>
        public static double DegToRad(this short value)
        {
            return value * DEG_TO_RAD;
        }

        /// <summary>
        /// 把指示方向的角度数限制在 [0, 360)。
        /// </summary>
        /// <param name="value">指示方向的角度数。</param>
        /// <returns>等价的在标准范围内的角度。</returns>
        public static float NormalizeDirection(this float value)
        {
            while (value < 0)
            {
                value += 360;
            }
            while (value >= 360)
            {
                value -= 360;
            }
            return value;
        }

        /// <summary>
        /// 把指示方向的角度值限制在 [0, 360)。
        /// </summary>
        /// <param name="value">指示方向的角度值。</param>
        /// <returns>等价的在标准范围内的角度。</returns>
        public static double NormalizeDirection(this double value)
        {
            while (value < 0)
            {
                value += 360;
            }
            while (value >= 360)
            {
                value -= 360;
            }
            return value;
        }

        /// <summary>
        /// 把指示方向的弧度值限制在 [0, 2PI)。
        /// </summary>
        /// <param name="value">指示方向的弧度值。</param>
        /// <returns>等价的在标准范围内的弧度。</returns>
        public static double NormalizeDirectionRad(this double value)
        {
            while (value < 0)
            {
                value += Math.PI * 2;
            }
            while (value >= Math.PI * 2)
            {
                value -= Math.PI * 2;
            }
            return value;
        }

        /// <summary>
        /// 把弧度转换为角度。
        /// </summary>
        /// <param name="value">弧度值。</param>
        /// <returns>角度值。</returns>
        public static double RadToDeg(this float value)
        {
            return value / DEG_TO_RAD;
        }

        /// <summary>
        /// 把弧度转换为角度。
        /// </summary>
        /// <param name="value">弧度值。</param>
        /// <returns>角度值。</returns>
        public static double RadToDeg(this double value)
        {
            return value / DEG_TO_RAD;
        }
    }
}