有了上述這些基礎結構 ，我們便可以開始實現算法的核心功能了：
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
}
