/// <summary>
/// 找路径
/// </summary>
/// <param name="routeData"></param>
/// <param name="currentNode">当前节点</param>
/// <param name="costGetter">从起点导航到本节点的代价 计算公式</param>
/// <returns></returns>
private IList<Point> FindPath(RouteData routeData, AStarNode currentNode, ICostGetter costGetter)
{
IList<CompassDirections> allCompassDirections = CompassDirectionsHelper.GetAllCompassDirections();
for (int i = 0; i < allCompassDirections.Count; i++)
{
CompassDirections direction = allCompassDirections[i];
Point nextPoint = GeometryHelper.GetAdjacentPoint(currentNode.Location, direction);
//if (地图之外) 或 如果非寻路点 跳过
//if (!Usable(nextPoint, routeData.CellMap)) continue;
if (!CanPass(nextPoint)) continue;
int costG = costGetter.GetCost(currentNode.Location, direction);//获取消费G值
int costH = Mathf.Abs(nextPoint.R - routeData.Destination.R) + Mathf.Abs(nextPoint.C - routeData.Destination.C);//H耗值
//下一个到目的点 寻路结束
if (costH == 0)
{
IList<Point> routeList = new List<Point>();
AStarNode desNode = currentNode;
routeList.Add(nextPoint);
routeList.Insert(0, currentNode.Location);
while (desNode.PreviousNode != null)
{
//从子节点往父节点逐个加到list中
routeList.Insert(0, desNode.PreviousNode.Location);
desNode = desNode.PreviousNode;
}
return routeList;
}
AStarNode exitNode = GetNodeOnLocation(nextPoint, routeData);
if (null != exitNode)
{
//在open或者close列表中存在该节点 则与当前节点比较G 如果有更短的路劲则更新该位置原始路径
if (exitNode.CostG > costG)
{
exitNode.ResetPreviousNode(currentNode, costG);
}
}
else
{
AStarNode newNode = new AStarNode(nextPoint, currentNode, costG, costH);
routeData.OpenedList.Add(newNode);
}
}
routeData.OpenedList.Remove(currentNode);
routeData.ClosedList.Add(currentNode);
//找最短路劲
AStarNode minCostNode = GetMinCostNode(routeData.OpenedList);
if (null == minCostNode)
return null;
//对开放列表中的下一个代价最小的节点作递归调用
return FindPath(routeData, minCostNode, costGetter);
}
public AStarRoutePlanner(int _lineCount, int _columnCount, ICostGetter _costGetter)
{
this.lineCount = _lineCount;
this.columnCount = _columnCount;
this.costGetter = _costGetter;
this.InitializeObstacles();
}
public AStarRoutePlanner(int _lineCount, int _columnCount, ICostGetter _costGetter)
{
this.lineCount = _lineCount;
this.columnCount = _columnCount;
this.costGetter = _costGetter;
this.InitializeObstacles();
}