C# AStar 算法 - 实际应用

在基本实现中 我们的理想化 是方格坐标的，但是实际应用可能是跨点的坐标，所以需要以 路段为核心，并且路段也支持了 方向的限制。

如果处于转弯的话，也优化了转弯的权重，尽量少转弯。

其中距离计算可以根据需要自行修改。

namespace Project.Utils
{
    public class AStarDirectionControlHelp
    { 

        public static void GetPathText()
        {
            try
            {
                var pathfinder = new AStarPathfinder();

                // 添加路段
                AddRoad(ref pathfinder);

                Console.WriteLine("=== A*算法复杂路况测试 ===\n");
                Stopwatch stopwatch = new Stopwatch();

                // 测试0: 随机测试
                Console.WriteLine("测试0: 随机测试");
                stopwatch.Start();
                var path0 = pathfinder.FindPath((0, 0), (3, 2));
                stopwatch.Stop();
                PrintResult(path0);
                Console.WriteLine("测试0: 耗时" + stopwatch.ElapsedMilliseconds + "ms");

                // 测试1: 正向反向限制测试 (0,0) -> (4,0)
                Console.WriteLine("测试1: 正向反向限制测试 (0,0) -> (4,0)");
                stopwatch.Start();
                var path1 = pathfinder.FindPath((0, 0), (4, 0));
                stopwatch.Stop();
                PrintResult(path1);
                Console.WriteLine("测试1: 耗时" + stopwatch.ElapsedMilliseconds + "ms");

                // 测试2: 反向测试 (4,0) -> (0,0) - 应该失败因为有反向限制
                Console.WriteLine("测试2: 反向测试 (4,0) -> (0,0)");
                var path2 = pathfinder.FindPath((4, 0), (0, 0));
                PrintResult(path2);

                // 测试3: 转弯优化测试 (0,0) -> (4,4) - 应该选择转弯少的路径
                Console.WriteLine("测试3: 转弯优化测试 (0,0) -> (4,4)");
                var path3 = pathfinder.FindPath((0, 0), (4, 4));
                PrintResult(path3);

                // 测试4: 断路测试 (1,1) -> (2,2) - 应该失败
                Console.WriteLine("测试4: 断路测试 (1,1) -> (2,2)");
                var path4 = pathfinder.FindPath((1, 1), (2, 2));
                PrintResult(path4);

                // 测试5: 复杂路径测试 (0,0) -> (3,4)
                Console.WriteLine("测试5: 复杂路径测试 (0,0) -> (3,4)");
                var path5 = pathfinder.FindPath((0, 0), (3, 4));
                PrintResult(path5);
            }
            catch (Exception ex)
            {
                Console.WriteLine("异常" + ex.Message,ex);
            }
        }
        static void PrintResult(List<(int, int)> path)
        {
            if (path.Count > 0)
            {
                Console.WriteLine($"找到路径 (长度: {path.Count}): {string.Join(" \n ", path)}");
            }
            else
            {
                Console.WriteLine("未找到路径\n");
            }
            Console.WriteLine();
        }
        private static void AddRoad(ref AStarPathfinder pathfinder)
        {
            // 构建5x5测试地图
            // 水平道路
            pathfinder.AddRoadSegment(new RoadSegment((0, 0), (1, 0), RoadDirection.Bidirectional));
            pathfinder.AddRoadSegment(new RoadSegment((1, 0), (2, 0), RoadDirection.Forward));  // 只能正向
            pathfinder.AddRoadSegment(new RoadSegment((2, 0), (3, 0), RoadDirection.Bidirectional)); // 只能反向
            pathfinder.AddRoadSegment(new RoadSegment((3, 0), (4, 0), RoadDirection.Bidirectional));

            // 垂直道路 - 制造转弯
            pathfinder.AddRoadSegment(new RoadSegment((1, 0), (1, 1), RoadDirection.Bidirectional));
            pathfinder.AddRoadSegment(new RoadSegment((1, 1), (1, 2), RoadDirection.Bidirectional));
            pathfinder.AddRoadSegment(new RoadSegment((1, 2), (1, 3), RoadDirection.Bidirectional));
            pathfinder.AddRoadSegment(new RoadSegment((1, 3), (1, 4), RoadDirection.Bidirectional));

            pathfinder.AddRoadSegment(new RoadSegment((3, 0), (3, 1), RoadDirection.Bidirectional));
            pathfinder.AddRoadSegment(new RoadSegment((3, 1), (3, 2), RoadDirection.Bidirectional));
            pathfinder.AddRoadSegment(new RoadSegment((3, 2), (3, 3), RoadDirection.Bidirectional));
            pathfinder.AddRoadSegment(new RoadSegment((3, 3), (3, 4), RoadDirection.Bidirectional));

            pathfinder.AddRoadSegment(new RoadSegment((3, 4), (4, 4), RoadDirection.Bidirectional));

            // 顶部水平连接
            pathfinder.AddRoadSegment(new RoadSegment((1, 4), (2, 4), RoadDirection.Bidirectional));
            pathfinder.AddRoadSegment(new RoadSegment((2, 4), (3, 4), RoadDirection.Bidirectional));

            // 断路测试 - (2,2)位置没有连接
        }
    }

    public enum RoadDirection
    {
        /// <summary>
        /// 双向
        /// </summary>
        Bidirectional,
        /// <summary>
        /// 正向
        /// </summary>
        Forward,
        /// <summary>
        /// 反向
        /// </summary>
        Backward
    }

    public class RoadSegment
    {
        /// <summary>
        /// 起点坐标
        /// </summary>
        public (int x, int y) From { get; set; }

        /// <summary>
        /// 终点坐标
        /// </summary>
        public (int x, int y) To { get; set; }

        /// <summary>
        /// 道路方向限制
        /// </summary>
        public RoadDirection Direction { get; set; }

        /// <summary>
        /// 通行代价，默认1.0
        /// </summary>
        public double Cost { get; set; } = 1.0;

        public RoadSegment((int, int) from, (int, int) to, RoadDirection direction = RoadDirection.Bidirectional, double cost = 1.0)
        {
            From = from;
            To = to;
            Direction = direction;
            Cost = cost;
        }

        public bool CanTraverse((int, int) from, (int, int) to)
        {
            // 正向通行检查
            if (from == From && to == To)
            {
                return Direction == RoadDirection.Bidirectional || Direction == RoadDirection.Forward;
            }
            // 反向通行检查 
            else if (from == To && to == From)
            {
                return Direction == RoadDirection.Bidirectional || Direction == RoadDirection.Backward;
            }
            // 其他情况都不允许通行
            return false;
        }
    }

    public class Node : IComparable<Node>
    {
        public (int x, int y) Position { get; set; }
        public double G { get; set; } = double.MaxValue;  // 从起点到当前节点的实际代价
        public double H { get; set; } = 0;                // 启发式代价
        public double F => G + H;                         // 总代价
        public Node Parent { get; set; }
        public (int, int)? DirectionFromParent { get; set; }  // 从父节点到当前节点的方向

        public int CompareTo(Node other)
        {
            return F.CompareTo(other.F);
        }
    }

    public class AStarPathfinder
    {
        // 整个道路网络图
        private Dictionary<(int, int), List<RoadSegment>> _roadMap;

        public AStarPathfinder()
        {
            _roadMap = new Dictionary<(int, int), List<RoadSegment>>();
        }

        /// <summary>
        /// 构建道路网络
        /// </summary>
        /// <param name="segment"></param>
        public void AddRoadSegment(RoadSegment segment)
        {
            // 为起点添加路段
            if (!_roadMap.ContainsKey(segment.From))
                _roadMap[segment.From] = new List<RoadSegment>();
            _roadMap[segment.From].Add(segment);

            // 如果是双向的，也为终点添加反向路段
            if (segment.Direction == RoadDirection.Bidirectional)
            {
                if (!_roadMap.ContainsKey(segment.To))
                    _roadMap[segment.To] = new List<RoadSegment>();
                _roadMap[segment.To].Add(new RoadSegment(segment.To, segment.From, RoadDirection.Bidirectional, segment.Cost));
            }
        }

        /// <summary>
        /// 核心算法
        /// </summary>
        /// <param name="start">起点</param>
        /// <param name="goal">终点</param>
        /// <returns></returns>
        public List<(int, int)> FindPath((int, int) start, (int, int) goal)
        {
            // 确保起点和终点都在道路网络中
            if (!_roadMap.ContainsKey(start) || !_roadMap.ContainsKey(goal))
                return new List<(int, int)>();

            var openSet = new PriorityQueue<Node, double>();    // 待探索节点
            var closedSet = new Dictionary<(int, int), Node>(); // 已探索节点
            var allNodes = new Dictionary<(int, int), Node>();  // 所有节点记录

            // 初始化起点
            var startNode = new Node
            {
                Position = start,
                G = 0,
                H = CalculateHeuristic(start, goal),
                DirectionFromParent = null
            };

            allNodes[start] = startNode;
            openSet.Enqueue(startNode, startNode.F);

            while (openSet.Count > 0)
            {
                var currentNode = openSet.Dequeue(); // 取出F值最小的节点

                
                if (currentNode.Position.Equals(goal)) // 到达目标点
                    return ReconstructPath(currentNode);

                closedSet[currentNode.Position] = currentNode; // 标记为已探索

                // 遍历所有相邻节点
                foreach (var neighbor in GetNeighbors(currentNode.Position))
                {
                    // 跳过已探索节点
                    if (closedSet.ContainsKey(neighbor.To)) continue;

                    // 检查道路可通行性
                    if (!_roadMap.ContainsKey(currentNode.Position)) continue;

                    // 检查路段是否可通行
                    var road = _roadMap[currentNode.Position]
                        .FirstOrDefault(r => r.CanTraverse(currentNode.Position, neighbor.To));

                    if (road == null) continue;

                    // 计算转弯代价
                    double turnCost = CalculateTurnCost(currentNode, neighbor.To);
                    double tentativeG = currentNode.G + road.Cost + turnCost;

                    if (!allNodes.ContainsKey(neighbor.To))
                    {
                        allNodes[neighbor.To] = new Node { Position = neighbor.To };
                    }

                    var neighborNode = allNodes[neighbor.To];

                    // 更新邻居节点
                    if (tentativeG < neighborNode.G)
                    {
                        // 更新路径和代价
                        neighborNode.Parent = currentNode;
                        neighborNode.G = tentativeG;
                        neighborNode.H = CalculateHeuristic(neighbor.To, goal);
                        neighborNode.DirectionFromParent = GetDirection(currentNode.Position, neighbor.To);

                        // 加入待探索队列
                        if (!openSet.UnorderedItems.Any(item => item.Element.Position.Equals(neighbor.To)))
                        {
                            openSet.Enqueue(neighborNode, neighborNode.F);
                        }
                    }
                }
            }

            return new List<(int, int)>(); // 未找到路径
        }

        /// <summary>
        /// 获取邻居
        /// </summary>
        private List<RoadSegment> GetNeighbors((int, int) position)
        {
            if (_roadMap.ContainsKey(position))
                return _roadMap[position];
            return new List<RoadSegment>();
        }

        /// <summary>
        /// 启发函数
        /// </summary>
        private double CalculateHeuristic((int, int) a, (int, int) b)
        {
            string type = "euclidean";
            int x1 = a.Item1;
            int y1 = a.Item2;
            int x2 = b.Item1;
            int y2 = b.Item2;
            return type.ToLower() switch
            {
                // 欧几里得距离
                "euclidean" => (float)Math.Sqrt(Math.Pow(x1 - x2, 2) + Math.Pow(y1 - y2, 2)),
                // 切比雪夫距离
                "chebyshev" => Math.Max(Math.Abs(x1 - x2), Math.Abs(y1 - y2)),
                // 曼哈顿距离
                _ => Math.Abs(x1 - x2) + Math.Abs(y1 - y2)
            };
        }

        private (int, int) GetDirection((int, int) from, (int, int) to)
        {
            return (to.Item1 - from.Item1, to.Item2 - from.Item2);
        }

        /// <summary>
        /// 转弯代价
        /// </summary>
        private double CalculateTurnCost(Node currentNode, (int, int) nextPosition)
        {
            if (currentNode.Parent == null || currentNode.DirectionFromParent == null)
                return 0;

            var currentDirection = GetDirection(currentNode.Position, nextPosition);
            var previousDirection = currentNode.DirectionFromParent;

            // 如果方向改变，增加转弯代价
            if (currentDirection != previousDirection)
            {
                return 2.0; // 转弯代价
            }

            return 0;
        }

        /// <summary>
        /// 重建路径
        /// </summary>
        private List<(int, int)> ReconstructPath(Node goalNode)
        {
            var path = new List<(int, int)>();
            var current = goalNode;

            while (current != null)
            {
                path.Add(current.Position);
                current = current.Parent;
            }

            path.Reverse();
            return path;
        }
    }

    // 优先队列实现（简化版）
    public class PriorityQueue<TElement, TPriority> where TPriority : IComparable<TPriority>
    {
        private readonly List<(TElement Element, TPriority Priority)> _elements = new List<(TElement, TPriority)>();

        public int Count => _elements.Count;

        public void Enqueue(TElement element, TPriority priority)
        {
            _elements.Add((element, priority));
            _elements.Sort((x, y) => x.Priority.CompareTo(y.Priority));
        }

        public TElement Dequeue()
        {
            if (_elements.Count == 0)
                throw new InvalidOperationException("Queue is empty");

            var item = _elements[0];
            _elements.RemoveAt(0);
            return item.Element;
        }

        public IEnumerable<(TElement Element, TPriority Priority)> UnorderedItems => _elements;
    }


}

代码中有详细的注释 可以参考

 

代码大部分为AI生成，仅仅记录使用。