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”，


效果如下
  

还有一个问题有待解决：
如果目标节点本身为障碍物，或是目标节点被障碍物完全包围，如何给出一条路径，使其尽量逼近目标节点？

 

源码下载请至置顶页~~