C#實現AStar尋路算法

AStar尋路算法是一種在一個靜態路網中尋找最短路徑的算法，也是在游戲開發中最常用到的尋路算法之一；最近剛好需要用到尋路算法，因此把自己的實現過程記錄下來。

先直接上可視化之后的效果圖，圖中黑色方格代表障礙物，綠色的方格代表最終路線，紅色方格為關閉列表，藍色方格為開啟列表；關於這一部分我會在稍后詳細敘述。（可視化的實現部分我就不討論了，這一篇主要說一下算法的實現）

 

一、算法原理

在描述具體算法邏輯之前，需要先理解幾個基本概念：

• • 啟發式搜索：聽起來很炫酷，其實很簡單；想象你在一個九宮格的中間，你現在需要走到九宮格的右上角；這個時候你的第一步有四個選擇：上下左右。雖然你還不知道具體路徑怎么走，但你知道左邊和下邊距離終點似乎更遠，所以你會優先選擇先往右走或者先往上走。這就是啟發式搜索——優先搜索最有可能產生最佳路徑的節點；通過啟發式搜索，可以有效減少不必要的搜索。
  • 估價函數：上面說到啟發式搜索會優先搜索最有可能產生最佳路徑的節點，那么估價函數的作用就是對節點與終點的距離進行預估。預估距離最短的那個節點，就是目前最有可能產生最佳路徑的節點。
  • 開啟列表：當估價函數對一個節點估價完畢后，這個節點就會被放入開啟列表中。那么開啟列表中的所有節點就是下一步所有可能被搜索的節點。
  • 關閉列表：當算法對開啟列表中最有可能是最佳路徑的節點搜索完畢后，會將這個節點放入關閉列表。那么關閉列表中的所有節點就是已經搜索完的所有路線的節點。

接下來，我用一個簡單的例子來描述算法的邏輯。

 

首先，假設我們有一個4*4的方格，左下角坐標（0，0），右上角坐標（3，3），黑色格子為障礙物；這個時候時候我們需要尋找點（0，1）到點（3，1）的最短路線；這個時候我們把起點加入開啟列表（藍色格子），即所有下一步可能被搜索的節點。

 

接下來我們要對開啟列表進行搜索了，這個時候開啟列表只有一個格子即起點，因此我們對起點進行搜索：

1. 這個時候我們把起點作為當前節點（即當前正在搜索的節點），然后找出當前節點所有下一步可能的節點，把他們加入開啟列表（藍色格子），表示這些都是下一步可能被搜索的節點。
2. 這個時候我們要對所有新增的節點用估價函數進行估價，估價函數的表達式為f(n)=g(n)+h(n)， 其中g(h)代表節點n到起點的實際距離，h(n)即啟發值，代表節點n到終點的預估距離，以節點（0，2）為例子：
   • g：可以看到，（0，2）從起點往上移動了一格，假設一個格子邊長為10，那么當前格子到起點的距離為10，因此g值為10，我們把它標在格子左下角。
   • h：h有很多種估價的方式，在這里我們就直接取 忽略障礙物直接走到終點的距離；如圖所示，該節點若要直接走到終點，它的路線為：右-右-右下，那么它的h值就是：10+10+14 = 34；我們把它標記在格子右下角。
   • f：f為包含當前節點的路線的預估總距離，即g+h = 10+34 = 44，我們把它標在格子左上角。
   • 父節點：父節點表示當前節點的估價是由哪個節點產生的，或者當前節點是從哪個節點走過來的；因此它的父節點為（0，1），我們用一個箭頭指向（0，1），表示（0，1）是（0，2）的父節點。
3. 對所有下一步可能的節點估價完畢后，我們將當前節點即起點從開啟列表移動到關閉列表中（紅色方格），表示我們對這個節點已經搜索完畢；結果如下圖所示。

 

接下來我們繼續對開啟列表進行搜索，經過上一步后，我們的開啟列表有三個節點；這個時候我們尋找f值最小的節點對它進行搜索，可以看到坐標為（1，0）的節點有着最小的f值38；因此節點（1，0）是目前最有可能產生最佳路線的節點，我們把它作為當前節點進行搜索：

1. 這個時候我們重復上一步的動作，找出當前節點所有下一步可能的節點，並對它們進行估價
2. 在找出節點的過程中，我們發現它左邊的節點是它下一步可能的新節點（0，0）之一，但是左邊的節點已經存在於開啟列表中了；那我們是否讓新節點覆蓋舊節點呢？這個時候我們如果對新產生的節點（0，0）進行估價，我們會發現新產生的節點h值不變，g值為24，f值為24+34 = 58；我們發現新節點的f值58大於舊節點的f值44，那么我們可以知道新節點所產生的路線總距離大於老節點產生的路線的總距離，因此我們選擇保留老節點。（當然如果我們發現新節點所產生的f值小於老節點所產生的f值，我們就要用新節點覆蓋老節點，因為新節點所產生的路線總距離更近）
3. 估價完畢后，我們對把當前節點從開啟列表移動到關閉列表中（紅色方格），結果如下圖

 

這個時候我們發現最小f值的節點有兩個，我們隨便選一個（2，1）繼續搜索，可以得到下圖的結果：

 

這個時候依然有兩個節點f值最小，我們隨便選一個（3，1），把它作為當前節點，繼續搜索；這個時候我們發現當前節點位置就是終點的位置，我們按照箭頭線路走到起點，於是我們終於找到了路線，如下圖所示：

　　

 

於是我們可以總結一下Astar尋路算法的算法邏輯（偽代碼）：

　　new openList；

　　new closeList；

　　openList.Add(start); //把起點加入開啟列表

　　　　loop{

　　　　　　currentNode = lowest f cost in openList；//把開啟列表中f值最低的節點作為當前節點

　　　　　　if (currentNode == end) return; //找到路線

　　　　　　foreach(neighbour in currentNode.Neighbours){ //對所有當前節點的相鄰節點進行循環

　　　　　　　　if (closeList.Contains(neighbour) or neighbour == obstacle ) continue; //跳過關閉列表中的節點和障礙物節點

　　　　　　　　if( new fCost <= old fCost || !openList.Contains(neighbour) ){ //如果新節點的f值小於老節點的f值，用新節點替換老節點

　　　　　　　　　　neighbour.fCost = new fCost;

　　　　　　　　　　neighbour.parent = currentNode;

　　　　　　　　　　if(!openList.Contains(neighbour)) openList.Add(neighbour);//如果新節點不在開啟列表，將其加入開啟列表

　　　　　　　　}

　　　　　　}

　　　　}

 

二、算法實現

首先，在路徑網格中每一個網格都有一個對應的坐標，因此我創建了一個Point2類用來表示網格坐標

//A class used to store the position information
public class Point2  3 {  4     public Point2(int x, int y)  5  {  6         this.x = x;  7         this.y = y;  8  }  9 
    public int x { get; set; } 11 
    public int y { get; set; } 13 
    public override bool Equals(object obj) 15  { 16         return this.x == (obj as Point2).x && this.y == (obj as Point2).y; 17  } 18 
    public override int GetHashCode() 20  { 21         return x ^ (y * 256); 22  } 23 
    public override string ToString() 25  { 26         return x + "," + y; 27  } 28 
    public static bool operator ==(Point2 a, Point2 b) 30  { 31         return a.Equals(b); 32  } 33 
    public static bool operator !=(Point2 a, Point2 b) 35  { 36         return !a.Equals(b); 37  } 38 }

 

　　接下來，我創建了一個PathNode類用來記錄單個節點的信息

 

public class PathNode  2 {  3     public PathNode(bool isWall, Point2 position)  4  {  5         this.isWall = isWall;  6         this.position = position;  7  }  8 
    public readonly Point2 position; 10 
    public bool isWall { get; set; } 12 
    public PathNode parent { get; set; } 14 
    public int gCost { get; set; } 16 
    public int hCost { get; set; } 18 
    public int fCost { 20         get { 21             return gCost + hCost; 22  } 23  } 24 
    public override bool Equals(object obj) 26  { 27         PathNode node = obj as PathNode; 28         return node.isWall == this.isWall && node.gCost == this.gCost && node.hCost == this.hCost && node.parent == this.parent && this.position == node.position; 29  } 30 
    public override int GetHashCode() 32  { 33         return gCost ^ (hCost * 128) + position.GetHashCode(); 34  } 35 
    public static bool operator ==(PathNode a, PathNode b) 37  { 38         return a.Equals(b); 39  } 40 
    public static bool operator !=(PathNode a, PathNode b) 42  { 43         return !a.Equals(b); 44  } 45 }

 

　　最后是我們的PathGrid類，通過創建該類的實例來創建一個網格信息，其中包含了網格大小以及所有障礙物信息。使用中輸入起點信息和終點信息可以返回路徑信息。關於代碼部分，由於Astar算法中開銷最大的部分是開啟列表和關閉列表的維護以及在開啟列表中尋找f值最低的部分。因此我用C#的SortedDictionary額外創建了一個開啟列表用於查詢f值最低的節點。當然算法還存在很大的優化空間，不過像32*32這樣的小的網格中已經夠用了。

using System.Collections.Generic;  2 using System.Linq;  3 using System;  4 
public class PathGrid  6 {  7     private SortedDictionary<int, List<Point2>> openTree = new SortedDictionary<int, List<Point2>>();  8 
    private HashSet<Point2> openSet = new HashSet<Point2>();  10     private HashSet<Point2> closeSet = new HashSet<Point2>();  11     private Dictionary<Point2, PathNode> allNodes = new Dictionary<Point2, PathNode>();  12 
    private Point2 endPos;  14     private Point2 gridSize;  15 
    private List<Point2> currentPath;  17 
    //這一部分在實際尋路中並不需要，只是為了方便外部程序實現尋路可視化
    public HashSet<Point2> GetCloseList()  20  {  21         return closeSet;  22  }  23 
    //這一部分在實際尋路中並不需要，只是為了方便外部程序實現尋路可視化
    public HashSet<Point2> GetOpenList()  26  {  27         return openSet;  28  }  29 
    //這一部分在實際尋路中並不需要，只是為了方便外部程序實現尋路可視化
    public List<Point2> GetCurrentPath()  32  {  33         return currentPath;  34  }  35 
    //新建一個PathGrid，包含了網格大小和障礙物信息
    public PathGrid(int x, int y, List<Point2> walls)  38  {  39         gridSize = new Point2(x, y);  40         for (int i = 0; i < x; i++) {  41             for (int j = 0; j < y; j++) {  42                 Point2 newPos = new Point2(i, j);  43                 allNodes.Add(newPos, new PathNode(walls.Contains(newPos), newPos));  44  }  45  }  46  }  47 
    //尋路主要邏輯，通過調用該方法來獲取路徑信息，由一串Point2代表
    public List<Point2> FindPath(Point2 beginPos, Point2 endPos)  50  {  51         List<Point2> result = new List<Point2>();  52 
        this.endPos = endPos;  54         Point2 currentPos = beginPos;  55  openSet.Add(currentPos);  56 
        while (!currentPos.Equals(this.endPos)) {  58  UpdatePath(currentPos);  59             if (openSet.Count == 0) return null;  60 
            currentPos = openTree.First().Value.First();  62  }  63 
        Point2 path = currentPos;  65 
        while (!path.Equals(beginPos)) {  67  result.Add(path);  68             path = allNodes[path].parent.position;  69             currentPath = result;  70  }  71 
 result.Add(beginPos);  73         return result;  74  }  75 
    //尋路
    private void UpdatePath(Point2 currentPos)  78  {  79  closeSet.Add(currentPos);  80  RemoveOpen(currentPos, allNodes[currentPos]);  81         List<Point2> neighborNodes = FindNeighbor(currentPos);  82         foreach (Point2 nodePos in neighborNodes) {  83 
            PathNode newNode = new PathNode(false, nodePos);  85             newNode.parent = allNodes[currentPos];  86 
            int g;  88             int h;  89 
            g = currentPos.x == nodePos.x || currentPos.y == nodePos.y ? 10 : 14;  91 
            int xMoves = Math.Abs(nodePos.x - endPos.x);  93             int yMoves = Math.Abs(nodePos.y - endPos.y);  94 
            int min = Math.Min(xMoves, yMoves);  96             int max = Math.Max(xMoves, yMoves);  97             h = min * 14 + (max - min) * 10;  98 

            newNode.gCost = g + newNode.parent.gCost; 101             newNode.hCost = h; 102 
            PathNode originNode = allNodes[nodePos]; 104 
            if (openSet.Contains(nodePos)) { 106                 if (newNode.fCost < originNode.fCost) { 107  UpdateNode(newNode, originNode); 108  } 109             } else { 110                 allNodes[nodePos] = newNode; 111  AddOpen(nodePos, newNode); 112  } 113  } 114  } 115 
    //將舊節點更新為新節點
    private void UpdateNode(PathNode newNode, PathNode oldNode) 118  { 119         Point2 nodePos = newNode.position; 120         int oldCost = oldNode.fCost; 121         allNodes[nodePos] = newNode; 122         List<Point2> sameCost; 123 
        if (openTree.TryGetValue(oldCost, out sameCost)) { 125  sameCost.Remove(nodePos); 126             if (sameCost.Count == 0) openTree.Remove(oldCost); 127  } 128 
        if (openTree.TryGetValue(newNode.fCost, out sameCost)) { 130  sameCost.Add(nodePos); 131         } else { 132             sameCost = new List<Point2> { nodePos }; 133  openTree.Add(newNode.fCost, sameCost); 134  } 135  } 136 
    //將目標節點移出開啟列表
    private void RemoveOpen(Point2 pos, PathNode node) 139  { 140  openSet.Remove(pos); 141         List<Point2> sameCost; 142         if (openTree.TryGetValue(node.fCost, out sameCost)) { 143  sameCost.Remove(pos); 144             if (sameCost.Count == 0) openTree.Remove(node.fCost); 145  } 146  } 147 
    //將目標節點加入開啟列表
    private void AddOpen(Point2 pos, PathNode node) 150  { 151  openSet.Add(pos); 152         List<Point2> sameCost; 153         if (openTree.TryGetValue(node.fCost, out sameCost)) { 154  sameCost.Add(pos); 155         } else { 156             sameCost = new List<Point2> { pos }; 157  openTree.Add(node.fCost, sameCost); 158  } 159  } 160 
    //找到某節點的所有相鄰節點
    private List<Point2> FindNeighbor(Point2 nodePos) 163  { 164         List<Point2> result = new List<Point2>(); 165 
        for (int x = -1; x < 2; x++) { 167             for (int y = -1; y < 2; y++) { 168                 if (x == 0 && y == 0) continue; 169 
                Point2 currentPos = new Point2(nodePos.x + x, nodePos.y + y); 171 
                if (currentPos.x >= gridSize.x || currentPos.y >= gridSize.y || currentPos.x < 0 || currentPos.y < 0) continue; //out of bondary
                if (closeSet.Contains(currentPos)) continue; // already in the close list
                if (allNodes[currentPos].isWall) continue;  // the node is a wall

 result.Add(currentPos); 177  } 178  } 179 
        return result; 181  } 182 }