A*尋路演算法的C#實現

詳細講述了A*尋路演算法，有下列地方值得學習
1. 不要用AStarPathNode來構造PathFinder的Matrix，Matrix僅僅是byte[,],需要對Matrix中某個元素進行處理時再構造相應的AStarPathNode
2. 對每個Matrix中的元素取值不僅僅0和1，而是代表其權重，0表示障礙物，1表示平地，n表示高山，沼澤難以行走的地方，Weight越大越難以通過。節點G值會根據Weight進行計算。
     newG = parentNode.G + mGrid[newNode.X, newNode.Y];
3. 使用Closed List，用於判斷是否達到尋路次數的上限。如果Closed List中的節點數達到某個數值(Search Limit)，就停止尋路。
4. 設定A* 演算法的參數，
    Search Limit ：如果Closed List中的節點數達到某個數值(Search Limit)，就停止尋路。
    Heuristic Formula：使用不同的演算法來計算節點的H值,確省為Manhattan演算法。
    Diagonals：是否處理對角線上的相鄰節點
       如果Diagonals = false，就意味著找到的路徑都是直線
     

     

     HeavyDiagonals：是否認為走對角線比走直線更遠，表現在演算法上就是:
       if(HeavyDiagonals)
           newNodeGValue = parentNode.G + Matrix[newNode.X, newNodeY]*2.41; //???為什麼不是根號2~~1.41
       else
           newNodeGValue = parentNode.G + Matrix[newNode.X, newNodeY]
     
   
    Punish Change Direction：是否希望找到的路徑看起來比較平滑，如果新節點和其父節點的走向與
父節點和父節點的父節點之間的走向不符，則增加新節點的G值。這裡要注意newG += 20。//為什麼是20???
    演算法為:
     if (PunishChangeDirection)
           mHoriz = (parentNode.X - parentNode.PX);   //???似乎還應該加上mVert = (parentNode.Y - parentNode.PY)
     newNodeGValue = //計算GValue
      if (PunishChangeDirection)
      {
           if ((newNode.X - parentNode.X) != 0)
           {
               if (mHoriz == 0)
                    newG += 20;
           }
           if ((newNode.Y - parentNode.Y) != 0)
           {
                  if (mHoriz != 0)                                //???在這裡使用mVert
                      newG += 20;
           }
       }
     

    Tie Breaker: 避免出現殊途同歸，輕微的改變H值
   if (mTieBreaker)
{
      int dx1 = parentNode.X - end.X;
      int dy1 = parentNode.Y - end.Y;
      int dx2 = start.X - end.X;
      int dy2 = start.Y - end.Y;
      int cross = Math.Abs(dx1 * dy2 - dx2 * dy1);
      newNode.H = (int) (newNode.H + cross * 0.001);
   }
不同的組合在不同的環境下具有最好的效能
5. 對相鄰節點的處理很巧妙
    用 direction = new sbyte[8,2]{ {0,-1} , {1,0}, {0,1}, {-1,0}, {1,-1}, {1,1}, {-1,1}, {-1,-1}}; 分別表示上，右，下，左，右上，右下，左下，左上這八個相鄰Node的座標x,y值的位移。
    比如：current node座標為x,y，這八個節點的座標可以這樣獲得
    for(int i=0; i< 8 ; i ++)
    {
         newNodeX = x + direction[i,0];
         newNodeX = x + direction[i,1];
    }

6. 不使用普通的array list儲存open node，否則，找出F值最小的open node會花去大量的時間，而是引入了Priority Queue作為open node的容器。把排序的複雜度分解在Push和Pop中，保證每次Pop出的都是最小的元素

7. 與Path Find無關。背景工作執行緒與UI的互動
public delegate void PathFinderDebugHandler(int fromX, int fromY, int x, int y, PathFinderNodeType type, int totalCost, int cost);

public class PathFinder : IPathFinder
{
    public event PathFinderDebugHandler PathFinderDebug;

}

//-------- Form
mPathFinder.PathFinderDebug += new PathFinderDebugHandler(PathFinderDebug);   //handle Event

//用delegate來進行跨線程調用
private delegate void PathFinderDebugDelegate(int parentX, int parentY, int x, int y, PathFinderNodeType type, int totalCost, int cost);

private void PathFinderDebug(int parentX, int parentY, int x, int y, PathFinderNodeType type, int totalCost, int cost)
{
    if (InvokeRequired)
    {
        Invoke(new PathFinderDebugDelegate(PathFinderDebug), new object[]{parentX, parentY, x, y, type, totalCost, cost});
        return;
    }

    PnlGUI.DrawDebug(parentX, parentY, x, y, type, totalCost, cost);
}

對本文我進行了下列修改：
1.在遍曆當前節點的相鄰節點時，需要看這個相鄰節點是否已經在open list 或close list中，如果某個相鄰節點已經在OpenList中，就要看如果把這個相鄰節點的父節點修改為當前節點，是否會降低這個相鄰節點的G值，原文中使用了for 迴圈來檢查這個節點是否在list中
int foundInOpenIndex = -1;
for(int j=0; j<mOpen.Count; j++)
{
    if (mOpen[j].X == newNode.X && mOpen[j].Y == newNode.Y)
    {
        foundInOpenIndex = j;
        break;
    }
}
if (foundInOpenIndex != -1 && mOpen[foundInOpenIndex].G <= newG)
    continue;

int foundInCloseIndex = -1;
for(int j=0; j<mClose.Count; j++)
{
    if (mClose[j].X == newNode.X && mClose[j].Y == newNode.Y)
    {
        foundInCloseIndex = j;
        break;
    }
}
if (foundInCloseIndex != -1 && mClose[foundInCloseIndex].G <= newG)
    continue;

為了提高效能，我給儲存open node的Queue中加入
private Dictionary<Point, int> pointDictionary = new Dictionary<Point, int>();
並使用Dictionary<Point, AStarNode> closedNodes 儲存close node，
以便使用x，y座標可以在open list或closed list中找到相應的Node

2.為了避免出現這種”穿牆”的情況，我加入了一個條件”CrossDiagonalBar”，


效果如下
  

還有一個問題有待解決：
如果目標節點本身為障礙物，或是目標節點被障礙物完全包圍，如何給出一條路徑，使其盡量逼近目標節點？

 

源碼下載請至置頂頁~~