SimulationServer/Models/SimulationCommon/FlightPlanner.cs

namespace SimulationCommon;

public class FlightPlanner
    {
        private const double EarthRadius = 6371000; // 地球半径，单位：米
        private const double RadiansPerDegree = Math.PI / 180.0; // 角度转弧度的转换常数

        private static Dictionary<(int, int), List<Point3D>> gridCells; // 网格单元
        private static double cellSize; // 网格单元大小

       
        // 规划飞行路径
        public static List<Point3D> PlanFlightPath(Point3D start, Point3D end, List<Point3D> areaPoints)
        {
            InitializeGrid(areaPoints); // 初始化网格

            var path = new List<Point3D> { start }; // 初始化路径，包含起点
            var current = FindClosestPoint(start); // 找到起点最近的点
            var target = FindClosestPoint(end);   // 找到终点最近的点

            // 循环查找路径直到到达目标点
            while (current != target)
            {
                path.Add(current); // 添加当前点到路径
                current = FindNextPoint(current, target); // 找到下一个点
            }

            path.Add(target); // 添加目标点
            path.Add(end);    // 添加终点
                              // 对路径中除第一个和最后一个点的高度加 300
            for (int j = 1; j < path.Count - 1; j++)
            {
                path[j].Z += 300;
            }
            return path; // 返回完整路径
        }

        // 初始化网格
        private static void InitializeGrid(List<Point3D> points)
        {
            cellSize = 0.01; // 根据数据密度调整此值
            gridCells = new Dictionary<(int, int), List<Point3D>>(); // 初始化网格字典

            foreach (var point in points)
            {
                var cellX = (int)(point.Lon / cellSize); // 计算网格 X 坐标
                var cellY = (int)(point.Lat / cellSize); // 计算网格 Y 坐标
                var key = (cellX, cellY);

                if (!gridCells.ContainsKey(key))
                {
                    gridCells[key] = new List<Point3D>(); // 如果网格不存在，初始化列表
                }
                gridCells[key].Add(point); // 将点添加到对应的网格单元
            }
        }

        // 找到离目标点最近的点
        private static Point3D FindClosestPoint(Point3D target)
        {
            var cellX = (int)(target.Lon / cellSize); // 计算目标点的网格 X 坐标
            var cellY = (int)(target.Lat / cellSize); // 计算目标点的网格 Y 坐标

            var nearbyPoints = new List<Point3D>(); // 存储附近点
            for (int i = -1; i <= 1; i++)
            {
                for (int j = -1; j <= 1; j++)
                {
                    var key = (cellX + i, cellY + j);
                    if (gridCells.TryGetValue(key, out var cellPoints))
                    {
                        nearbyPoints.AddRange(cellPoints); // 添加附近网格的点
                    }
                }
            }

            // 根据距离排序并返回最近的点
            return nearbyPoints.OrderBy(p => DistanceSquared(p, target)).First();
        }

        // 找到下一个最佳点
        private static Point3D FindNextPoint(Point3D current, Point3D target)
        {
            var cellX = (int)(current.Lon / cellSize); // 当前点的网格 X 坐标
            var cellY = (int)(current.Lat / cellSize); // 当前点的网格 Y 坐标

            var bestPoint = current; // 初始化最佳点为当前点
            var bestDistance = double.MaxValue; // 初始化最佳距离为最大值

            // 遍历附近的网格点
            for (int i = -1; i <= 1; i++)
            {
                for (int j = -1; j <= 1; j++)
                {
                    var key = (cellX + i, cellY + j);
                    if (gridCells.TryGetValue(key, out var cellPoints))
                    {
                        foreach (var point in cellPoints)
                        {
                            if (point != current) // 不考虑当前点
                            {
                                var distanceToCurrent = DistanceSquared(point, current); // 当前点到候选点的距离
                                var distanceToTarget = DistanceSquared(point, target); // 候选点到目标点的距离
                                var totalDistance = distanceToCurrent + distanceToTarget; // 总距离

                                if (totalDistance < bestDistance) // 如果找到更短的距离
                                {
                                    bestDistance = totalDistance; // 更新最佳距离
                                    bestPoint = point; // 更新最佳点
                                }
                            }
                        }
                    }
                }
            }

            return bestPoint; // 返回最佳点
        }

        // 计算两个点之间的平方距离
        private static double DistanceSquared(Point3D a, Point3D b)
        {
            var dLon = (b.Lon - a.Lon) * RadiansPerDegree; // 经度差
            var dLat = (b.Lat - a.Lat) * RadiansPerDegree; // 纬度差
            var lat1 = a.Lat * RadiansPerDegree; // 点 A 的纬度弧度
            var lat2 = b.Lat * RadiansPerDegree; // 点 B 的纬度弧度

            var x = dLon * Math.Cos((lat1 + lat2) / 2); // X 轴距离
            var y = dLat; // Y 轴距离

            return EarthRadius * EarthRadius * (x * x + y * y); // 返回平方距离
        }

        // 计算路径总距离
        public static double CalculatePathDistance(List<Point3D> path)
        {
            double totalDistance = 0; // 初始化总距离
            for (int i = 0; i < path.Count - 1; i++)
            {
                totalDistance += Math.Sqrt(DistanceSquared(path[i], path[i + 1])); // 累加每段距离
            }
            return totalDistance; // 返回总距离
        }
    }