private List <AbstractPathNode> GetPath(HierarchicalMap map, Id <AbstractNode> startNodeId, Id <AbstractNode> targetNodeId, int level, bool mainSearch)
{
map.SetCurrentLevelForSearches(level);
var nodeInfo = map.AbstractGraph.GetNodeInfo(startNodeId);
// TODO: This could be perfectly replaced by cached paths in the clusters!
Path <AbstractNode> path;
if (!mainSearch)
{
map.SetCurrentClusterByPositionAndLevel(nodeInfo.Position, level + 1);
var edgeInfo = map.AbstractGraph.GetEdges(startNodeId)[targetNodeId].Info;
path = new Path <AbstractNode>(edgeInfo.InnerLowerLevelPath, edgeInfo.Cost);
}
else
{
map.SetAllMapAsCurrentCluster();
var search = new AStar <AbstractNode>(map, startNodeId, targetNodeId);
path = search.FindPath();
}
if (path.PathCost == -1)
{
return(new List <AbstractPathNode>());
}
var result = new List <AbstractPathNode>(path.PathNodes.Count);
foreach (Id <AbstractNode> abstractNodeId in path.PathNodes)
{
result.Add(new AbstractPathNode(abstractNodeId, level));
}
return(result);
}
/// <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);
}
