123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144 |
- 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; // 返回总距离
- }
- }
|