/// <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);
}
/// <summary>
/// 获取路径
/// </summary>
/// <param name="startPoint">开始点</param>
/// <param name="destination">结束点</param>
/// <param name="maps">二维地图</param>
/// <returns></returns>
public IList<Point> GetPath(Point startPoint, Point destination)
{
if (!Usable(startPoint) || !Usable(destination))
{
Debug.Log(string.Format("此处为非寻路点:({0},{1}),type={2}",destination.R,destination.C,maps[destination.R,destination.C].GetMapType().ToString()));
return null;
}
//RouteData routData = new RouteData(maps, destination);
RouteData routData = new RouteData(destination);
ICostGetter costGetter = new SimpleCostGetter();
AStarNode startNode = new AStarNode(startPoint, null, 0, 0);
routData.OpenedList.Add(startNode);
AStarNode currentNode = startNode;
return FindPath(routData, currentNode, costGetter);
}
/// <summary>
/// 是否在 列表中
/// </summary>
/// <param name="p"></param>
/// <param name="route"></param>
/// <returns></returns>
private AStarNode GetNodeOnLocation(Point p, RouteData route)
{
for (int i = 0; i < route.OpenedList.Count; i++)
{
if (route.OpenedList[i].Location.R == p.R && route.OpenedList[i].Location.C == p.C) return route.OpenedList[i];
}
for (int j = 0; j < route.ClosedList.Count; j++)
{
if (route.ClosedList[j].Location.R == p.R && route.ClosedList[j].Location.C == p.C) return route.ClosedList[j];
}
return null;
}