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); }