/// <summary>
/// 获得指定层的路径
/// </summary>
private static List <PathNode> GetPath(HierarchicalMap map, int startAbsId, int goalAbsId, int level, bool isMainSearch)
{
map.SetCurrentLevelForSearch(level);
AbstractNode startAbsNode = map.AbstractGraph.GetNode(startAbsId);
AbstractNode goalAbsNode = map.AbstractGraph.GetNode(goalAbsId);
Path path;
if (isMainSearch)
{
map.SetAllMapAsCurrentCluster();
var planner = new PathPlanner(map, null);
path = planner.Search(startAbsNode, goalAbsNode);
}
else
{
map.SetCurrentClusterAndLevel(startAbsNode.Pos, level + 1); //因为每一层节点都保存的是下一层的路径，所以要通过上一层来找到当层的路径
AbstractEdge edge = map.AbstractGraph.GetNode(startAbsNode.Id).Edges[goalAbsNode.Id];
path = new Path(edge.InnerLowerLevelPath, edge.Cost);
}
if (path == null)
{
return(new List <PathNode>());
}
var result = new List <PathNode>(path.Nodes.Count);
foreach (var node in path.Nodes)
{
result.Add(new PathNode(node.Id, node.Pos, level));
}
return(result);
}
