private IList <Point> DoPlan(RoutePlanData routePlanData, AStarNode currenNode)
{
IList <CompassDirections> allCompassDirections = CompassDirectionsHelper.GetAllCompassDirections();
foreach (CompassDirections directions in allCompassDirections)
{
Point adjacentPoint = GeometryHelper.GetAdjacentPoint(currenNode.Location, directions);
if (routePlanData.CellMap.Contains(adjacentPoint) && !this.obstacles[adjacentPoint.X][adjacentPoint.Y])
{
int cost = this.costGetter.GetCost(currenNode.Location, directions);
int introduced15 = Math.Abs((int)(adjacentPoint.X - routePlanData.Destination.X));
int num2 = introduced15 + Math.Abs((int)(adjacentPoint.Y - routePlanData.Destination.Y));
if (num2 == 0)
{
IList <Point> list2 = new List <Point>();
list2.Add(routePlanData.Destination);
list2.Insert(0, currenNode.Location);
for (AStarNode node = currenNode; node.PreviousNode != null; node = node.PreviousNode)
{
list2.Insert(0, node.PreviousNode.Location);
}
return(list2);
}
AStarNode nodeOnLocation = this.GetNodeOnLocation(adjacentPoint, routePlanData);
if (nodeOnLocation != null)
{
if (nodeOnLocation.CostG > cost)
{
nodeOnLocation.ResetPreviousNode(currenNode, cost);
}
}
else
{
AStarNode item = new AStarNode(adjacentPoint, currenNode, cost, num2);
routePlanData.OpenedList.Add(item);
}
}
}
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));
}