C# 實現 A* 算法 (4) — 核心路徑規劃邏輯

AStarRoutePlanner 核心實現

整合前文基礎結構 (AStarNode, RoutePlanData, CompassDirections, GeometryHelper, ICostGetter)，實現遞迴搜尋主邏輯：

public class AStarRoutePlanner
{
    private int _lineCount = 10;      // 地圖高度 (Y)
    private int _columnCount = 10;    // 地圖寬度 (X)
    private ICostGetter _costGetter = new SimpleCostGetter();
    private bool[][] _obstacles;      // 障礙物 [Column][Line]

    public AStarRoutePlanner() : this(10, 10, new SimpleCostGetter()) { }
    public AStarRoutePlanner(int lines, int cols, ICostGetter getter)
    {
        _lineCount = lines; _columnCount = cols; _costGetter = getter;
        InitializeObstacles();
    }

    private void InitializeObstacles()
    {
        _obstacles = new bool[_columnCount][];
        for (int i = 0; i < _columnCount; i++)
            _obstacles[i] = new bool[_lineCount]; // 預設 false = 可通行
    }

    public void Initialize(IList obstaclePoints)
    {
        foreach (Point pt in obstaclePoints)
            _obstacles[pt.X][pt.Y] = true;
    }

    public IList Plan(Point start, Point destination)
    {
        var map = new Rectangle(0, 0, _columnCount, _lineCount);
        if (!map.Contains(start) || !map.Contains(destination))
            throw new Exception("起點或終點超出地圖範圍！");

        var data = new RoutePlanData(map, destination);
        var startNode = new AStarNode(start, null, 0, 0);
        data.OpenedList.Add(startNode);
        return DoPlan(data, startNode);
    }

    private IList DoPlan(RoutePlanData data, AStarNode current)
    {
        foreach (var dir in CompassDirectionsHelper.GetAllCompassDirections())
        {
            Point next = GeometryHelper.GetAdjacentPoint(current.Location, dir);
            if (!data.CellMap.Contains(next)) continue;           // 超出地圖
            if (_obstacles[next.X][next.Y]) continue;             // 障礙物

            int costG = _costGetter.GetCost(current.Location, dir);
            int costH = Math.Abs(next.X - data.Destination.X) +
                        Math.Abs(next.Y - data.Destination.Y);    // 曼哈頓距離

            if (costH == 0) // 到達終點
            {
                var route = new List { data.Destination, current.Location };
                var node = current;
                while (node.PreviousNode != null)
                {
                    route.Insert(0, node.PreviousNode.Location);
                    node = node.PreviousNode;
                }
                return route;
            }

            var exist = GetNodeOnLocation(next, data);
            if (exist != null)
            {
                if (exist.CostG > costG)
                    exist.ResetPreviousNode(current, costG); // 發現更優路徑
            }
            else
            {
                data.OpenedList.Add(new AStarNode(next, current, costG, costH));
            }
        }

        // 當前節點移入關閉列表
        data.OpenedList.Remove(current);
        data.ClosedList.Add(current);

        var nextNode = GetMinCostNode(data.OpenedList);
        if (nextNode == null) return null; // 無路可達

        return DoPlan(data, nextNode); // 遞迴處理下一個最低 F 節點
    }

    private AStarNode GetNodeOnLocation(Point loc, RoutePlanData data)
    {
        foreach (var n in data.OpenedList) if (n.Location == loc) return n;
        foreach (var n in data.ClosedList) if (n.Location == loc) return n;
        return null;
    }

    private AStarNode GetMinCostNode(IList opened)
    {
        if (opened.Count == 0) return null;
        AStarNode min = opened[0];
        foreach (var n in opened) if (n.CostF < min.CostF) min = n;
        return min;
    }
}

關鍵流程

1. 從起點節點開始，展開 8 方向鄰居。
2. 跳過障礙物與地圖外鄰居。
3. 計算 G (實際代價) 與 H (曼哈頓啟發式)。
4. 若鄰居為終點 → 回溯構建路徑並返回。
5. 若鄰居已在開放/關閉列表：比較 G，較優則更新父節點。
6. 否則加入開放列表。
7. 當前節點移入關閉列表，從開放列表取 F 最小節點遞迴。
8. 開放列表為空 → 無路可達，返回 null。