在基本实现中 我们的理想化 是方格坐标的,但是实际应用可能是跨点的坐标,所以需要以 路段为核心,并且路段也支持了 方向的限制。
如果处于转弯的话,也优化了转弯的权重,尽量少转弯。
其中距离计算可以根据需要自行修改。

1 namespace Project.Utils 2 { 3 public class AStarDirectionControlHelp 4 { 5 6 public static void GetPathText() 7 { 8 try 9 { 10 var pathfinder = new AStarPathfinder(); 11 12 // 添加路段 13 AddRoad(ref pathfinder); 14 15 Console.WriteLine("=== A*算法复杂路况测试 ===\n"); 16 Stopwatch stopwatch = new Stopwatch(); 17 18 // 测试0: 随机测试 19 Console.WriteLine("测试0: 随机测试"); 20 stopwatch.Start(); 21 var path0 = pathfinder.FindPath((0, 0), (3, 2)); 22 stopwatch.Stop(); 23 PrintResult(path0); 24 Console.WriteLine("测试0: 耗时" + stopwatch.ElapsedMilliseconds + "ms"); 25 26 // 测试1: 正向反向限制测试 (0,0) -> (4,0) 27 Console.WriteLine("测试1: 正向反向限制测试 (0,0) -> (4,0)"); 28 stopwatch.Start(); 29 var path1 = pathfinder.FindPath((0, 0), (4, 0)); 30 stopwatch.Stop(); 31 PrintResult(path1); 32 Console.WriteLine("测试1: 耗时" + stopwatch.ElapsedMilliseconds + "ms"); 33 34 // 测试2: 反向测试 (4,0) -> (0,0) - 应该失败因为有反向限制 35 Console.WriteLine("测试2: 反向测试 (4,0) -> (0,0)"); 36 var path2 = pathfinder.FindPath((4, 0), (0, 0)); 37 PrintResult(path2); 38 39 // 测试3: 转弯优化测试 (0,0) -> (4,4) - 应该选择转弯少的路径 40 Console.WriteLine("测试3: 转弯优化测试 (0,0) -> (4,4)"); 41 var path3 = pathfinder.FindPath((0, 0), (4, 4)); 42 PrintResult(path3); 43 44 // 测试4: 断路测试 (1,1) -> (2,2) - 应该失败 45 Console.WriteLine("测试4: 断路测试 (1,1) -> (2,2)"); 46 var path4 = pathfinder.FindPath((1, 1), (2, 2)); 47 PrintResult(path4); 48 49 // 测试5: 复杂路径测试 (0,0) -> (3,4) 50 Console.WriteLine("测试5: 复杂路径测试 (0,0) -> (3,4)"); 51 var path5 = pathfinder.FindPath((0, 0), (3, 4)); 52 PrintResult(path5); 53 } 54 catch (Exception ex) 55 { 56 Console.WriteLine("异常" + ex.Message,ex); 57 } 58 } 59 static void PrintResult(List<(int, int)> path) 60 { 61 if (path.Count > 0) 62 { 63 Console.WriteLine($"找到路径 (长度: {path.Count}): {string.Join(" \n ", path)}"); 64 } 65 else 66 { 67 Console.WriteLine("未找到路径\n"); 68 } 69 Console.WriteLine(); 70 } 71 private static void AddRoad(ref AStarPathfinder pathfinder) 72 { 73 // 构建5x5测试地图 74 // 水平道路 75 pathfinder.AddRoadSegment(new RoadSegment((0, 0), (1, 0), RoadDirection.Bidirectional)); 76 pathfinder.AddRoadSegment(new RoadSegment((1, 0), (2, 0), RoadDirection.Forward)); // 只能正向 77 pathfinder.AddRoadSegment(new RoadSegment((2, 0), (3, 0), RoadDirection.Bidirectional)); // 只能反向 78 pathfinder.AddRoadSegment(new RoadSegment((3, 0), (4, 0), RoadDirection.Bidirectional)); 79 80 // 垂直道路 - 制造转弯 81 pathfinder.AddRoadSegment(new RoadSegment((1, 0), (1, 1), RoadDirection.Bidirectional)); 82 pathfinder.AddRoadSegment(new RoadSegment((1, 1), (1, 2), RoadDirection.Bidirectional)); 83 pathfinder.AddRoadSegment(new RoadSegment((1, 2), (1, 3), RoadDirection.Bidirectional)); 84 pathfinder.AddRoadSegment(new RoadSegment((1, 3), (1, 4), RoadDirection.Bidirectional)); 85 86 pathfinder.AddRoadSegment(new RoadSegment((3, 0), (3, 1), RoadDirection.Bidirectional)); 87 pathfinder.AddRoadSegment(new RoadSegment((3, 1), (3, 2), RoadDirection.Bidirectional)); 88 pathfinder.AddRoadSegment(new RoadSegment((3, 2), (3, 3), RoadDirection.Bidirectional)); 89 pathfinder.AddRoadSegment(new RoadSegment((3, 3), (3, 4), RoadDirection.Bidirectional)); 90 91 pathfinder.AddRoadSegment(new RoadSegment((3, 4), (4, 4), RoadDirection.Bidirectional)); 92 93 // 顶部水平连接 94 pathfinder.AddRoadSegment(new RoadSegment((1, 4), (2, 4), RoadDirection.Bidirectional)); 95 pathfinder.AddRoadSegment(new RoadSegment((2, 4), (3, 4), RoadDirection.Bidirectional)); 96 97 // 断路测试 - (2,2)位置没有连接 98 } 99 } 100 101 public enum RoadDirection 102 { 103 /// <summary> 104 /// 双向 105 /// </summary> 106 Bidirectional, 107 /// <summary> 108 /// 正向 109 /// </summary> 110 Forward, 111 /// <summary> 112 /// 反向 113 /// </summary> 114 Backward 115 } 116 117 public class RoadSegment 118 { 119 /// <summary> 120 /// 起点坐标 121 /// </summary> 122 public (int x, int y) From { get; set; } 123 124 /// <summary> 125 /// 终点坐标 126 /// </summary> 127 public (int x, int y) To { get; set; } 128 129 /// <summary> 130 /// 道路方向限制 131 /// </summary> 132 public RoadDirection Direction { get; set; } 133 134 /// <summary> 135 /// 通行代价,默认1.0 136 /// </summary> 137 public double Cost { get; set; } = 1.0; 138 139 public RoadSegment((int, int) from, (int, int) to, RoadDirection direction = RoadDirection.Bidirectional, double cost = 1.0) 140 { 141 From = from; 142 To = to; 143 Direction = direction; 144 Cost = cost; 145 } 146 147 public bool CanTraverse((int, int) from, (int, int) to) 148 { 149 // 正向通行检查 150 if (from == From && to == To) 151 { 152 return Direction == RoadDirection.Bidirectional || Direction == RoadDirection.Forward; 153 } 154 // 反向通行检查 155 else if (from == To && to == From) 156 { 157 return Direction == RoadDirection.Bidirectional || Direction == RoadDirection.Backward; 158 } 159 // 其他情况都不允许通行 160 return false; 161 } 162 } 163 164 public class Node : IComparable<Node> 165 { 166 public (int x, int y) Position { get; set; } 167 public double G { get; set; } = double.MaxValue; // 从起点到当前节点的实际代价 168 public double H { get; set; } = 0; // 启发式代价 169 public double F => G + H; // 总代价 170 public Node Parent { get; set; } 171 public (int, int)? DirectionFromParent { get; set; } // 从父节点到当前节点的方向 172 173 public int CompareTo(Node other) 174 { 175 return F.CompareTo(other.F); 176 } 177 } 178 179 public class AStarPathfinder 180 { 181 // 整个道路网络图 182 private Dictionary<(int, int), List<RoadSegment>> _roadMap; 183 184 public AStarPathfinder() 185 { 186 _roadMap = new Dictionary<(int, int), List<RoadSegment>>(); 187 } 188 189 /// <summary> 190 /// 构建道路网络 191 /// </summary> 192 /// <param name="segment"></param> 193 public void AddRoadSegment(RoadSegment segment) 194 { 195 // 为起点添加路段 196 if (!_roadMap.ContainsKey(segment.From)) 197 _roadMap[segment.From] = new List<RoadSegment>(); 198 _roadMap[segment.From].Add(segment); 199 200 // 如果是双向的,也为终点添加反向路段 201 if (segment.Direction == RoadDirection.Bidirectional) 202 { 203 if (!_roadMap.ContainsKey(segment.To)) 204 _roadMap[segment.To] = new List<RoadSegment>(); 205 _roadMap[segment.To].Add(new RoadSegment(segment.To, segment.From, RoadDirection.Bidirectional, segment.Cost)); 206 } 207 } 208 209 /// <summary> 210 /// 核心算法 211 /// </summary> 212 /// <param name="start">起点</param> 213 /// <param name="goal">终点</param> 214 /// <returns></returns> 215 public List<(int, int)> FindPath((int, int) start, (int, int) goal) 216 { 217 // 确保起点和终点都在道路网络中 218 if (!_roadMap.ContainsKey(start) || !_roadMap.ContainsKey(goal)) 219 return new List<(int, int)>(); 220 221 var openSet = new PriorityQueue<Node, double>(); // 待探索节点 222 var closedSet = new Dictionary<(int, int), Node>(); // 已探索节点 223 var allNodes = new Dictionary<(int, int), Node>(); // 所有节点记录 224 225 // 初始化起点 226 var startNode = new Node 227 { 228 Position = start, 229 G = 0, 230 H = CalculateHeuristic(start, goal), 231 DirectionFromParent = null 232 }; 233 234 allNodes[start] = startNode; 235 openSet.Enqueue(startNode, startNode.F); 236 237 while (openSet.Count > 0) 238 { 239 var currentNode = openSet.Dequeue(); // 取出F值最小的节点 240 241 242 if (currentNode.Position.Equals(goal)) // 到达目标点 243 return ReconstructPath(currentNode); 244 245 closedSet[currentNode.Position] = currentNode; // 标记为已探索 246 247 // 遍历所有相邻节点 248 foreach (var neighbor in GetNeighbors(currentNode.Position)) 249 { 250 // 跳过已探索节点 251 if (closedSet.ContainsKey(neighbor.To)) continue; 252 253 // 检查道路可通行性 254 if (!_roadMap.ContainsKey(currentNode.Position)) continue; 255 256 // 检查路段是否可通行 257 var road = _roadMap[currentNode.Position] 258 .FirstOrDefault(r => r.CanTraverse(currentNode.Position, neighbor.To)); 259 260 if (road == null) continue; 261 262 // 计算转弯代价 263 double turnCost = CalculateTurnCost(currentNode, neighbor.To); 264 double tentativeG = currentNode.G + road.Cost + turnCost; 265 266 if (!allNodes.ContainsKey(neighbor.To)) 267 { 268 allNodes[neighbor.To] = new Node { Position = neighbor.To }; 269 } 270 271 var neighborNode = allNodes[neighbor.To]; 272 273 // 更新邻居节点 274 if (tentativeG < neighborNode.G) 275 { 276 // 更新路径和代价 277 neighborNode.Parent = currentNode; 278 neighborNode.G = tentativeG; 279 neighborNode.H = CalculateHeuristic(neighbor.To, goal); 280 neighborNode.DirectionFromParent = GetDirection(currentNode.Position, neighbor.To); 281 282 // 加入待探索队列 283 if (!openSet.UnorderedItems.Any(item => item.Element.Position.Equals(neighbor.To))) 284 { 285 openSet.Enqueue(neighborNode, neighborNode.F); 286 } 287 } 288 } 289 } 290 291 return new List<(int, int)>(); // 未找到路径 292 } 293 294 /// <summary> 295 /// 获取邻居 296 /// </summary> 297 private List<RoadSegment> GetNeighbors((int, int) position) 298 { 299 if (_roadMap.ContainsKey(position)) 300 return _roadMap[position]; 301 return new List<RoadSegment>(); 302 } 303 304 /// <summary> 305 /// 启发函数 306 /// </summary> 307 private double CalculateHeuristic((int, int) a, (int, int) b) 308 { 309 string type = "euclidean"; 310 int x1 = a.Item1; 311 int y1 = a.Item2; 312 int x2 = b.Item1; 313 int y2 = b.Item2; 314 return type.ToLower() switch 315 { 316 // 欧几里得距离 317 "euclidean" => (float)Math.Sqrt(Math.Pow(x1 - x2, 2) + Math.Pow(y1 - y2, 2)), 318 // 切比雪夫距离 319 "chebyshev" => Math.Max(Math.Abs(x1 - x2), Math.Abs(y1 - y2)), 320 // 曼哈顿距离 321 _ => Math.Abs(x1 - x2) + Math.Abs(y1 - y2) 322 }; 323 } 324 325 private (int, int) GetDirection((int, int) from, (int, int) to) 326 { 327 return (to.Item1 - from.Item1, to.Item2 - from.Item2); 328 } 329 330 /// <summary> 331 /// 转弯代价 332 /// </summary> 333 private double CalculateTurnCost(Node currentNode, (int, int) nextPosition) 334 { 335 if (currentNode.Parent == null || currentNode.DirectionFromParent == null) 336 return 0; 337 338 var currentDirection = GetDirection(currentNode.Position, nextPosition); 339 var previousDirection = currentNode.DirectionFromParent; 340 341 // 如果方向改变,增加转弯代价 342 if (currentDirection != previousDirection) 343 { 344 return 2.0; // 转弯代价 345 } 346 347 return 0; 348 } 349 350 /// <summary> 351 /// 重建路径 352 /// </summary> 353 private List<(int, int)> ReconstructPath(Node goalNode) 354 { 355 var path = new List<(int, int)>(); 356 var current = goalNode; 357 358 while (current != null) 359 { 360 path.Add(current.Position); 361 current = current.Parent; 362 } 363 364 path.Reverse(); 365 return path; 366 } 367 } 368 369 // 优先队列实现(简化版) 370 public class PriorityQueue<TElement, TPriority> where TPriority : IComparable<TPriority> 371 { 372 private readonly List<(TElement Element, TPriority Priority)> _elements = new List<(TElement, TPriority)>(); 373 374 public int Count => _elements.Count; 375 376 public void Enqueue(TElement element, TPriority priority) 377 { 378 _elements.Add((element, priority)); 379 _elements.Sort((x, y) => x.Priority.CompareTo(y.Priority)); 380 } 381 382 public TElement Dequeue() 383 { 384 if (_elements.Count == 0) 385 throw new InvalidOperationException("Queue is empty"); 386 387 var item = _elements[0]; 388 _elements.RemoveAt(0); 389 return item.Element; 390 } 391 392 public IEnumerable<(TElement Element, TPriority Priority)> UnorderedItems => _elements; 393 } 394 395 396 }
代码中有详细的注释 可以参考
代码大部分为AI生成,仅仅记录使用。