C# 實作 A* 尋路演算法：網格地圖最佳路徑規劃完整指南

A* 核心公式

f(n) = g(n) + h(n)

• g(n)：從起點到節點 n 的實際代價
• h(n)：從 n 到終點的估計代價 (啟發式函數，必須可接受：不高估實際代價)
• f(n)：優先佇列排序依據

網格地圖資料結構

public enum Direction { None=0, N=1, NE=2, E=3, SE=4, S=5, SW=6, W=7, NW=8 }

public interface ICostProvider
{
    int GetCost(Point from, Direction dir);
}

public class SimpleCostProvider : ICostProvider
{
    public int GetCost(Point from, Direction dir) => dir switch
    {
        Direction.N or Direction.S or Direction.E or Direction.W => 10, // 直線
        _ => 14 // 斜線 ≈ 10 * √2
    };
}

public class AStarNode
{
    public Point Loc { get; }
    public AStarNode Parent { get; private set; }
    public int G { get; private set; }
    public int H { get; }
    public int F => G + H;

    public AStarNode(Point loc, AStarNode parent, int g, int h)
    {
        Loc = loc; Parent = parent; G = g; H = h;
    }

    public void UpdateParent(AStarNode newParent, int newG)
    {
        Parent = newParent; G = newG;
    }
}

public class GridMap
{
    public int Width { get; }
    public int Height { get; }
    private readonly bool[,] _obstacles; // true = 障礙
    public GridMap(int w, int h) { Width = w; Height = h; _obstacles = new bool[w, h]; }
    public void SetObstacle(Point p, bool blocked) => _obstacles[p.X, p.Y] = blocked;
    public bool IsWalkable(Point p) => p.X >= 0 && p.X < Width && p.Y >= 0 && p.Y < Height && !_obstacles[p.X, p.Y];
}

A* 主流程

public class AStarPlanner
{
    private readonly GridMap _map;
    private readonly ICostProvider _cost;

    public AStarPlanner(GridMap map, ICostProvider cost = null)
    {
        _map = map; _cost = cost ?? new SimpleCostProvider();
    }

    public List FindPath(Point start, Point goal)
    {
        if (!_map.IsWalkable(start) || !_map.IsWalkable(goal)) return null;

        var open = new PriorityQueue();
        var closed = new HashSet();
        var startNode = new AStarNode(start, null, 0, Heuristic(start, goal));
        open.Enqueue(startNode, startNode.F);

        while (open.Count > 0)
        {
            var current = open.Dequeue();
            if (current.Loc == goal) return ReconstructPath(current);

            closed.Add(current.Loc);

            foreach (Direction dir in Enum.GetValues())
            {
                if (dir == Direction.None) continue;
                Point next = GetNeighbor(current.Loc, dir);
                if (!_map.IsWalkable(next) || closed.Contains(next)) continue;

                int newG = current.G + _cost.GetCost(current.Loc, dir);
                var existing = open.UnorderedItems.FirstOrDefault(x => x.Element.Loc == next);
                
                if (existing.Element == null)
                {
                    var node = new AStarNode(next, current, newG, Heuristic(next, goal));
                    open.Enqueue(node, node.F);
                }
                else if (newG < existing.Element.G)
                {
                    existing.Element.UpdateParent(current, newG);
                    // PriorityQueue 無 DecreaseKey，重新入隊 (舊節點會被 closed 擋住)
                    open.Enqueue(existing.Element, existing.Element.F);
                }
            }
        }
        return null; // 找不到路徑
    }

    private static int Heuristic(Point a, Point b) => Math.Abs(a.X - b.X) + Math.Abs(a.Y - b.Y); // 曼哈頓距離

    private static Point GetNeighbor(Point p, Direction d) => d switch
    {
        Direction.N => new Point(p.X, p.Y - 1),
        Direction.NE => new Point(p.X + 1, p.Y - 1),
        Direction.E => new Point(p.X + 1, p.Y),
        Direction.SE => new Point(p.X + 1, p.Y + 1),
        Direction.S => new Point(p.X, p.Y + 1),
        Direction.SW => new Point(p.X - 1, p.Y + 1),
        Direction.W => new Point(p.X - 1, p.Y),
        Direction.NW => new Point(p.X - 1, p.Y - 1),
        _ => p
    };

    private static List ReconstructPath(AStarNode node)
    {
        var path = new List();
        while (node != null) { path.Add(node.Loc); node = node.Parent; }
        path.Reverse();
        return path;
    }
}

使用範例

var map = new GridMap(10, 10);
// 設定障礙：第 4 行 x=2..6
for (int x = 2; x <= 6; x++) map.SetObstacle(new Point(x, 4), true);

var planner = new AStarPlanner(map);
var path = planner.FindPath(new Point(3, 3), new Point(4, 6));

Console.WriteLine("路徑: " + string.Join(" -> ", path));
// 輸出: (3,3) -> (2,3) -> (1,3) -> (1,4) -> (1,5) -> (2,5) -> (2,6) -> (3,6) -> (4,6)

效能優化關鍵點

總結

A* 結合實際代價與啟發式估計，在網格地圖尋路中兼具最優解保證與高效能。C# 透過 PriorityQueue、HashSet、struct Point 等現代 API 可實作出簡潔高效的尋路元件，廣泛應用於遊戲、機器人、物流規劃等領域。