public AStarNode(Point loc, AStarNode previous, int _costG, int _costH)
{
this.location = loc;
this.previousNode = previous;
this.costG = _costG;
this.costH = _costH;
}
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);
}
/// <summary>
/// ResetPreviousNode 当从起点到达本节点有更优的路径时，调用该方法采用更优的路径。
/// </summary>
public void ResetPreviousNode(AStarNode previous, int _costG)
{
this.previousNode = previous;
this.costG = _costG;
}
/// <summary>
/// ResetPreviousNode 当从起点到达本节点有更优的路径时，调用该方法采用更优的路径。
/// </summary>
public void ResetPreviousNode(AStarNode previous, int _costG)
{
this.previousNode = previous;
this.costG = _costG;
}
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);
}
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));
}