C#实现A*算法

 

 

 

原址http://www.bianceng.cn/Programming/csharp/201012/21809.htm

 

 

 

在游戏开发中，AI的最基本问题之一就是寻路算法或称路径规划算法，在三 年前，我曾实现过 基于“图算法”的最短路径规划算法，然而在游 戏中，我们通常将地图抽象为有单元格构成的矩形，如：

这个微型地图 由3*3的单元格构成，当然，实际游戏中的地图通常比它大很多，这里只是给出 一个示例。

由于游戏地图通常由单元格构成，所以，基于“图算法 ”的路径规划便不再那么适用，我们需要采用基于单元格的路径规划算法 。A*算法是如今游戏所采用的寻路算法中相当常用的一种算法，它可以保证在任 何起点和任何终点之间找到最佳的路径（如果存在的话），而且，A*算法相当有 效。

关于A*算法的原理的详细介绍，可以参考 这篇文章。当你明白A*算 法的原理之后，在来看接下来的A*算法的实现就会比较容易了。

现在， 让我们转入正题，看看如何在C#中实现A*算法。

首先，我们把地图划分 为单元格，如此，一个地图就可以由（M行*N列）个单元格构成。我们使用 AStarNode类来抽象单元格，表示一个节点，而节点的位置用Point表示，其X坐 标表示Column Index，Y坐标表示Line Index。AStarNode的定义如下：

Code

[copy to clipboard]CODE:

/// <summary>
　　/// AStarNode 用于保存规划到当前节点时的各个 Cost值以及父节点。
　　/// zhuweisky 2008.10.18
　　/// </summary>
　　public class AStarNode
　　{
　　 　　#region Ctor
　　　　public AStarNode(Point loc, AStarNode previous, int _costG, int _costH)
　　　　{
　　　　　　 this.location = loc;
　　　　　　this.previousNode = previous;
　　　　　　this.costG = _costG;
　　　　　　this.costH = _costH;
　　　　}
　　　　#endregion
　　　　#region Location
　　　　private Point location = new Point(0, 0);
　　　　/// <summary>
　　　　/// Location 节点所在的位置， 其X值代表ColumnIndex，Y值代表LineIndex
　　　　/// </summary>
　　　　public Point Location
　　　　{
　　　　　　get { return location; }
　　　　}
　　　　 #endregion
　　　　#region PreviousNode
　　　　private AStarNode previousNode = null;
　　　　/// <summary>
　　　　/// PreviousNode 父节点，即是由哪个节点导航到当前节点的。
　　　　/// </summary>
　　　　public AStarNode PreviousNode
　　　　{
　　　　　　get { return previousNode; }
　　　　}
　　　　#endregion
　　　　 #region CostF
　　　　/// <summary>
　　　　/// CostF 从起点导航经过本节点然后再到目的节点的估算总代价。
　　　　/// </summary>
　　　　public int CostF
　　　　{
　 　　　　　get
　　　　　　{
　　　　　　　　return this.costG + this.costH;
　　　　　　}
　　　　}
　　　 　#endregion
　　　　#region CostG
　　　　private int costG = 0;
　　　　/// <summary>
　　　　/// CostG 从起点导 航到本节点的代价。
　　　　/// </summary>
　　　　 public int CostG
　　　　{
　　　　　　get { return costG; }
　　　　}
　　　　#endregion
　　　　#region CostH
　　　　private int costH = 0;
　　　　/// <summary>
　　　　/// CostH 使用启发式方法估算的从本节点到目的节点的代价。
　　　　/// </summary>
　　　　public int CostH
　　 　　{
　　　　　　get { return costH; }
　　　　}
　　 　　#endregion
　　　　#region ResetPreviousNode
　　　　/// <summary>
　　　　/// ResetPreviousNode 当从起点到达本节点 有更优的路径时，调用该方法采用更优的路径。
　　　　/// </summary>
　　　　public void ResetPreviousNode(AStarNode previous, int _costG)
　　　　{
　　　　　　 this.previousNode = previous;
　　　　　　this.costG = _costG;
　　　　}
　　　　#endregion
　　　　public override string ToString()
　　　　{
　　　　　　return this.location.ToString();
　　　　}
　　}

 

/// <summary>
　　/// SimpleCostGetter ICostGetter接口的简化实 现。直线代价为10， 斜线为14。
　　/// </summary>
　　 public class SimpleCostGetter : ICostGetter
　　{
　　　　 #region ICostGetter 成员
　　　　public int GetCost(Point currentNodeLoaction, CompassDirections moveDirection)
　　　　 {
　　　　　　if (moveDirection == CompassDirections.NotSet)
　　　　　　{
　　　　　　　　return 0;
　　　　　　}
　　　　　　if (moveDirection == CompassDirections.East || moveDirection == CompassDirections.West || moveDirection == CompassDirections.South || moveDirection == CompassDirections.North)
　　　　　　{
　　　　　　　　return 10;
　　　　　 　}
　　　　　　return 14;
　　　　}
　　　　 #endregion
　　}

 

 

我们知道，如果定义上、下、左、右的 代价为1，那么斜线的代价应为根号2，为了提高计算效率，我们将根号2取近似 值为1.4，并将单位放大10倍（计算机对整数的运算比对浮点数的运算要快很多 ）。

我们还需要一个结构来保存在路径规划过程中的中间结果：

Code

[copy to clipboard]CODE:

/// <summary>
　　/// RoutePlanData 用于封装一次路径规划过程中 的规划信息。
　　/// </summary>
　　public class RoutePlanData
　　{
　　　　#region CellMap
　　　　 private Rectangle cellMap;
　　　　/// <summary>
　　 　　/// CellMap 地图的矩形大小。经过单元格标准处理。
　　　　/// </summary>
　　　　public Rectangle CellMap
　　　　 {
　　　　　　get { return cellMap; }
　　　　}
　　　 　#endregion
　　　　#region ClosedList
　　　　private IList<AStarNode> closedList = new List<AStarNode>();
　　　　/// <summary>
　　　　/// ClosedList 关闭列表，即存 放已经遍历处理过的节点。
　　　　/// </summary>
　　　 　public IList<AStarNode> ClosedList
　　　　{
　　　 　　　get { return closedList; }
　　　　}
　　　　 #endregion
　　　　#region OpenedList
　　　　private IList<AStarNode> openedList = new List<AStarNode>();
　　　　/// <summary>
　　　　/// OpenedList 开放列表，即存 放已经开发但是还未处理的节点。
　　　　/// </summary>
　　　　public IList<AStarNode> OpenedList
　　　　{
　　　　　　get { return openedList; }
　　　　}
　　　　 #endregion
　　　　#region Destination
　　　　private Point destination;
　　　　/// <summary>
　　　　/// Destination 目的节点的位置。
　　　　/// </summary>
　 　　　public Point Destination
　　　　{
　　　　　　get { return destination; }
　　　　}
　　　　#endregion
　　 　　#region Ctor
　　　　public RoutePlanData(Rectangle map, Point _destination)
　　　　{
　　　　　　this.cellMap = map;
　　　　　　this.destination = _destination;
　　　　}
　　　　#endregion
　　}

有了上述这些基础结构 ，我们便可以开始实现算法的核心功能了：

Code

[copy to clipboard]CODE:

/// <summary>
　　/// AStarRoutePlanner A*路径规划。每个单元格Cell的位置用Point表示
　 　/// F = G + H 。
　　/// G = 从起点A，沿着产生的路径，移动到网 格上指定方格的移动耗费。
　　/// H = 从网格上那个方格移动到终点B 的预估移动耗费。使用曼哈顿方法，它计算从当前格到目的格之间水平和垂直的 方格的数量总和，忽略对角线方向。
　　/// zhuweisky 2008.10.18
　　/// </summary>
　　public class AStarRoutePlanner
　　{
　　　　private int lineCount = 10;　//反映地图高度，对应 Y坐标
　　　　private int columnCount = 10; //反映地图宽度，对应X 坐标
　　　　private ICostGetter costGetter = new SimpleCostGetter();
　　　　private bool[][] obstacles = null; // 障碍物位置，维度：Column * Line
　　　　#region Ctor
　　　 　public AStarRoutePlanner() :this(10 ,10 ,new SimpleCostGetter())
　　　　{
　　　　}
　　　　public AStarRoutePlanner(int _lineCount, int _columnCount, ICostGetter _costGetter)
　　　　 {
　　　　　　this.lineCount = _lineCount;
　　　　　　 this.columnCount = _columnCount;
　　　　　　this.costGetter = _costGetter;
　　　　　　this.InitializeObstacles();
　　　 　}
　　　　/// <summary>
　　　　/// InitializeObstacles 将所有位置均标记为无障碍物。
　　　　/// </summary>
　　　　private void InitializeObstacles()
　　　　{
　　　　　　this.obstacles = new bool [this.columnCount][];
　　　　　　for (int i = 0; i < this.columnCount; i++)
　　　　　　{
　　　　　　　　 this.obstacles = new bool[this.lineCount];
　　　　　　}
　 　　　　　for (int i = 0; i < this.columnCount; i++)
　　　　 　　{
　　　　　　　　for (int j = 0; j < this.lineCount; j++)
　　　　　　　　{
　　　　　　　　　　this.obstacles[j] = false;
　　　　　　　　}
　　　　　　}
　　　　}
　　　　#endregion
　　　　#region Initialize
　　　　/// <summary>
　　　　/// Initialize 在路径规划之前先设置障碍物 位置。
　　　　/// </summary>
　　　　public void Initialize(IList<Point> obstaclePoints)
　　　　{
　　 　　　　if (obstacles != null)
　　　　　　{
　　　　　　　 　foreach (Point pt in obstaclePoints)
　　　　　　　　{
　 　　　　　　　　　this.obstacles[pt.X][pt.Y] = true;
　　　　　　 　　}
　　　　　　}
　　　　}
　　　　#endregion
　　　　#region Plan
　　　　public IList<Point> Plan(Point start, Point destination)
　　　　{
　　　　　　Rectangle map = new Rectangle(0, 0, this.columnCount, this.lineCount);
　　 　　　　if ((!map.Contains(start)) || (!map.Contains(destination)))
　　　　　　{
　　　　　　　　throw new Exception ("StartPoint or Destination not in the current map!");
　　　　　　}
　　　　　　RoutePlanData routePlanData = new RoutePlanData(map, destination);
　　　　　　AStarNode startNode = new AStarNode(start, null, 0, 0);
　　　　　　 routePlanData.OpenedList.Add(startNode);
　　　　　　AStarNode currenNode = startNode;
　　　　　　//从起始节点开始进行递归调用
　　　　　　return DoPlan(routePlanData, currenNode);
　　 　　}
　　　　#endregion
　　　　#region DoPlan
　　　 　private IList<Point> DoPlan(RoutePlanData routePlanData, AStarNode currenNode)
　　　　{
　　　　　　 IList<CompassDirections> allCompassDirections = CompassDirectionsHelper.GetAllCompassDirections();
　　　　　　 foreach (CompassDirections direction in allCompassDirections)
　 　　　　　{
　　　　　　　　Point nextCell = GeometryHelper.GetAdjacentPoint(currenNode.Location, direction);
　　　　　　　　if (!routePlanData.CellMap.Contains(nextCell)) //相邻 点已经在地图之外
　　　　　　　　{
　　　　　　　　　　 continue;
　　　　　　　　}
　　　　　　　　if (this.obstacles[nextCell.X][nextCell.Y]) //下一个Cell为障碍物
　 　　　　　　　{
　　　　　　　　　　continue;
　　　　　　　 　}
　　　　　　　　int costG = this.costGetter.GetCost (currenNode.Location, direction);
　　　　　　　　int costH = Math.Abs(nextCell.X - routePlanData.Destination.X) + Math.Abs (nextCell.Y - routePlanData.Destination.Y);
　　　　　　　　if (costH == 0) //costH为0，表示相邻点就是目的点，规划完成，构造结果路径
　　　　　　　　{
　　　　　　　　　　IList<Point> route = new List<Point>();
　　　　　　　　　　route.Add (routePlanData.Destination);
　　　　　　　　　　route.Insert(0, currenNode.Location);
　　　　　　　　　　AStarNode tempNode = currenNode;
　　　　　　　　　　while (tempNode.PreviousNode != null)
　　　　　　　　　　{
　　　　　　　　　　　　 route.Insert(0, tempNode.PreviousNode.Location);
　　　　　　　　 　　　　tempNode = tempNode.PreviousNode;
　　　　　　　　　　}
　　　　　　　　　　return route;
　　　　　　　　}
　 　　　　　　　AStarNode existNode = this.GetNodeOnLocation(nextCell, routePlanData);
　　　　　　　　if (existNode != null)
　　 　　　　　　{
　　　　　　　　　　if (existNode.CostG > costG)
　　　　　　　　　　{
　　　　　　　　　　　　//如果 新的路径代价更小，则更新该位置上的节点的原始路径
　　　　　　　　 　　　　existNode.ResetPreviousNode(currenNode, costG);
　　　　 　　　　　　}
　　　　　　　　}
　　　　　　　　else
　 　　　　　　　{
　　　　　　　　　　AStarNode newNode = new AStarNode(nextCell, currenNode, costG, costH);
　　　　　　　　　 　routePlanData.OpenedList.Add(newNode);
　　　　　　　　}
　　　　　　}
　　　　　　//将已遍历过的节点从开放列表转移到关闭 列表
　　　　　　routePlanData.OpenedList.Remove(currenNode);
　　　　　　routePlanData.ClosedList.Add(currenNode);
　　　　 　　AStarNode minCostNode = this.GetMinCostNode (routePlanData.OpenedList);
　　　　　　if (minCostNode == null) //表明从起点到终点之间没有任何通路。
　　　　　　{
　　　　 　　　　return null;
　　　　　　}
　　　　　　//对开放列表 中的下一个代价最小的节点作递归调用
　　　　　　return this.DoPlan(routePlanData, minCostNode);
　　　　}
　　　　 #endregion
　　　　#region GetNodeOnLocation
　　　　/// <summary>
　　　　/// GetNodeOnLocation 目标位置location是 否已存在于开放列表或关闭列表中
　　　　/// </summary>
　　　　private AStarNode GetNodeOnLocation(Point location, RoutePlanData routePlanData)
　　　　{
　　　　　　foreach (AStarNode temp in routePlanData.OpenedList)
　　　　　　{
　　　　　　　　if (temp.Location == location)
　　　　　　　　 {
　　　　　　　　　　return temp;
　　　　　　　　}
　 　　　　　}
　　　　　　foreach (AStarNode temp in routePlanData.ClosedList)
　　　　　　{
　　　　　　　　if (temp.Location == location)
　　　　　　　　{
　　　　　　　 　　　return temp;
　　　　　　　　}
　　　　　　}
　　 　　　　return null;
　　　　}
　　　　#endregion
　　 　　#region GetMinCostNode
　　　　/// <summary>
　　 　　/// GetMinCostNode 从开放列表中获取代价F最小的节点，以启动下一次递 归
　　　　/// </summary>
　　　　private AStarNode GetMinCostNode(IList<AStarNode> openedList)
　　　　{
　　　　　　if (openedList.Count == 0)
　　　　　　{
　　　 　　　　　return null;
　　　　　　}
　　　　　　AStarNode target = openedList[0];
　　　　　　foreach (AStarNode temp in openedList)
　　　　　　{
　　　　　　　　if (temp.CostF < target.CostF)
　　　　　　　　{
　　　　　　　　　　 target = temp;
　　　　　　　　}
　　　　　　}
　　　　 　　return target;
　　　　}
　　　　#endregion
　　}

代码中已经加了详尽的注释，要注意的有以下几点：

1.Initialize方法用于初始化障碍物的位置，所谓“障碍物 ”，是指导航时无法穿越的物体，比如，游戏中的墙、河流等。

2. 标记为红色的ResetPreviousNode方法调用处，说明，到达当前节点的当前路径 比已存在的路径代价更小，所以要选择更优的路径。

3.标记为黑体的 route.Insert(0, tempNode.PreviousNode.Location);方法调用，表示已经找到 最优路径，此处便可通过反向迭代的方式整理出从起点到终点的最终路径。

4.CostH 的计算使用曼哈顿方法，它计算从当前格到目的格之间水平和 垂直的方格的数量总和，忽略对角线方向。

5.在该算法中，至少还有一 个地方可以优化，那就是GetMinCostNode方法所实现的内容，如果在路径搜索的 过程中，我们就将OpenList中的各个节点按照Cost从小到大进行排序，那么每次 GetMinCostNode时，就只需要取出第一个节点即可，而不必在遍历OpenList中的 每一个节点了。在地图很大的时候，此法可以大幅提升算法效率。

最后 ，给出一个例子，感受一下：

Code

[copy to clipboard] CODE:

　　　　　AStarRoutePlanner aStarRoutePlanner = new AStarRoutePlanner();
　　　　　　IList<Point> obstaclePoints = new List<Point>();
　　　　　　 obstaclePoints.Add(new Point(2, 4));
　　　　　　 obstaclePoints.Add(new Point(3, 4));
　　　　　　 obstaclePoints.Add(new Point(4, 4));
　　　　　　 obstaclePoints.Add(new Point(5, 4));
　　　　　　 obstaclePoints.Add(new Point(6, 4));
　　　　　　 aStarRoutePlanner.Initialize(obstaclePoints);
　　　　　　 IList<Point> route = aStarRoutePlanner.Plan(new Point(3, 3), new Point(4, 6));

运行后，返回的route结果如下：

{3,3}, 　{2,3},　{1,3},　{1,4},　{1,5},　{2,5},　{2,6},　{3,6},　{4,6}