private static List <PathNode> RefineAbstractPath(HierarchicalMap map, List <PathNode> path, int level, ref int maxRefineCount)
{
var result = new List <PathNode>();
if (path.Count == 0)
{
return(result);
}
int calculatedPaths = 0;
result.Add(new PathNode(path[0].Id, path[0].Pos, path[0].Level - 1));
for (int i = 1; i < path.Count; i++)
{
if (path[i].Level == level && path[i].Level == path[i - 1].Level && map.BelongToSameCluster(path[i].Pos, path[i - 1].Pos, level) && calculatedPaths < maxRefineCount)
{
var interPath = GetPath(map, path[i - 1].Id, path[i].Id, level - 1, false);
result.AddRange(interPath);
calculatedPaths++;
}
else
{
result.Add(new PathNode(path[i].Id, path[i].Pos, path[i].Level - 1));
}
}
maxRefineCount -= calculatedPaths;
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);
}
static void Main(string[] args)
{
Console.WriteLine("Presse Enter");
Console.ReadKey();
HierarchicalMap.RelativeScales = new int[] { 2, 2 };
var map = new HierarchicalMap(10, 10, new Random(0));
//map.SetTestPortals();
//map.CreatePaths();
//map.CoverPathsWithZones(3,2);
map.SpawnZoneAtClusterPosition(3, 2);
map.PrintToConsole();
Console.ReadLine();
map.SpawnZoneAtClusterPosition(3, 2, null, true);
map.PrintToConsole();
Console.ReadLine();
map.SpawnZoneAtClusterPosition(3, 2, null, true);
map.PrintToConsole();
Console.ReadLine();
map.SpawnZoneAtClusterPosition(3, 2, null, true);
map.PrintToConsole();
Console.ReadLine();
map.CreateSubMaps();
map.PrintMasterToConsole();
map.PrintMasterToBitmap(ConfigurationManager.AppSettings["BitmapOutput"]);
for (var i = 0; i < map.flatZones.Count(); i++)
{
var submap = map.flatZones[i].SubMap;
submap.PrintMasterToBitmap(ConfigurationManager.AppSettings["BitmapOutput"].Replace(".bmp", i.ToString() + ".bmp"));
}
}
private static List <PathNode> SearchImpl(HierarchicalMap map, int startAbsId, int goalAbsId, int maxSearchLevel, int maxRefineCount = int.MaxValue)
{
//找出最高层的抽象路径
List <PathNode> path = GetPath(map, startAbsId, goalAbsId, maxSearchLevel, true);
if (path.Count == 0)
{
return(path);
}
//一层层具化路径直到最低层抽象
for (int level = maxSearchLevel; level > 1; level--)
{
if (maxRefineCount <= 0)
{
break;
}
path = RefineAbstractPath(map, path, level, ref maxRefineCount);
}
//找到具体的路径
path = AbstractPathToConcretePath(map, path, maxRefineCount);
return(path);
}
public List <AbstractPathNode> RefineAbstractPath(HierarchicalMap map, List <AbstractPathNode> path, int level, int maxPathsToRefine = int.MaxValue)
{
var refinedAbstractPath = new List <AbstractPathNode>();
var calculatedPaths = 0;
if (path.Count == 0)
{
return(refinedAbstractPath);
}
refinedAbstractPath.Add(new AbstractPathNode(path[0].Id, level - 1));
for (var i = 1; i < path.Count; i++)
{
if (path[i].Level == level && path[i].Level == path[i - 1].Level &&
map.BelongToSameCluster(path[i].Id, path[i - 1].Id, level) && calculatedPaths < maxPathsToRefine)
{
var interNodePath = GetPath(map, path[i - 1].Id, path[i].Id, level - 1, false);
for (int j = 1; j < interNodePath.Count; j++)
{
refinedAbstractPath.Add(interNodePath[j]);
}
calculatedPaths++;
}
else
{
refinedAbstractPath.Add(new AbstractPathNode(path[i].Id, level - 1));
}
}
return(refinedAbstractPath);
}
/// <summary>
/// 恢复寻路新增节点导致的变化
/// </summary>
private void RestoreNodeBackup(HierarchicalMap map, int nodeId, NodeBackup backup)
{
AbstractGraph graph = map.AbstractGraph;
AbstractNode node = graph.GetNode(nodeId);
//恢复节点的级别
node.Level = backup.Level;
//恢复相关的边
graph.RemoveEdgeFromAndToNode(nodeId);
foreach (var edge in backup.Edges)
{
int targetNodeId = edge.TargetNodeId;
var targetNode = map.GetAbstractNode(targetNodeId);
AbstractEdge abstractEdge = HPADemo.Instance.CreateEdge(node.Pos, targetNode.Pos, edge.Level, edge.IsInterEdge);
abstractEdge.Init(targetNodeId, edge.Cost, edge.Level, edge.IsInterEdge);
abstractEdge.SetInnerLowerLevelPath(edge.InnerLowerLevelPath);
graph.AddEdge(nodeId, abstractEdge);
edge.InnerLowerLevelPath?.Reverse();
abstractEdge = HPADemo.Instance.CreateEdge(targetNode.Pos, node.Pos, edge.Level, edge.IsInterEdge);
abstractEdge.Init(nodeId, edge.Cost, edge.Level, edge.IsInterEdge);
abstractEdge.SetInnerLowerLevelPath(edge.InnerLowerLevelPath);
graph.AddEdge(targetNodeId, abstractEdge);
}
m_backupDict.Remove(nodeId);
}
private void RestoreNodeBackup(HierarchicalMap map, Id <AbstractNode> nodeId)
{
var abstractGraph = map.AbstractGraph;
var nodeBackup = nodeBackups[nodeId];
var nodeInfo = abstractGraph.GetNodeInfo(nodeId);
nodeInfo.Level = nodeBackup.Level;
abstractGraph.RemoveEdgesFromAndToNode(nodeId);
abstractGraph.AddNode(nodeId, nodeInfo);
foreach (var edge in nodeBackup.Edges)
{
var targetNodeId = edge.TargetNodeId;
map.AddEdge(nodeId, targetNodeId, edge.Info.Cost,
edge.Info.Level, edge.Info.IsInterClusterEdge,
edge.Info.InnerLowerLevelPath != null ? new List <Id <AbstractNode> >(edge.Info.InnerLowerLevelPath) : null);
edge.Info.InnerLowerLevelPath?.Reverse();
map.AddEdge(targetNodeId, nodeId, edge.Info.Cost,
edge.Info.Level, edge.Info.IsInterClusterEdge, edge.Info.InnerLowerLevelPath);
}
nodeBackups.Remove(nodeId);
}
public int InsertAbstractNode(HierarchicalMap map, Vector2Int concretePos)
{
int abstractId = InsertAbstractNodeInClusterLevel(map, concretePos);
map.AddHierarchicalEdgesForAbstractNode(abstractId);
return(abstractId);
}
public void RemoveAbstractNode(HierarchicalMap map, Id <AbstractNode> nodeId, int stal)
{
var abstractGraph = map.AbstractGraph;
if (m_stalUsed[stal])
{
// The node was an existing entrance point in the graph. Restore it with
// the information we kept when inserting
var nodeInfo = abstractGraph.GetNodeInfo(nodeId);
nodeInfo.Level = m_stalLevel[stal];
abstractGraph.RemoveEdgesFromAndToNode(nodeId);
abstractGraph.AddNode(nodeId, nodeInfo);
foreach (var edge in m_stalEdges[stal])
{
var targetNodeId = edge.TargetNodeId;
map.AddEdge(nodeId, targetNodeId, edge.Info.Cost,
edge.Info.Level, edge.Info.IsInterClusterEdge);
map.AddEdge(targetNodeId, nodeId, edge.Info.Cost,
edge.Info.Level, edge.Info.IsInterClusterEdge);
}
}
else
{
// Just delete the node from the graph
var currentNodeInfo = abstractGraph.GetNodeInfo(nodeId);
var clusterId = currentNodeInfo.ClusterId;
var cluster = map.Clusters[clusterId.IdValue];
cluster.RemoveLastEntranceRecord();
map.ConcreteNodeIdToAbstractNodeIdMap.Remove(currentNodeInfo.ConcreteNodeId);
abstractGraph.RemoveEdgesFromAndToNode(nodeId);
abstractGraph.RemoveLastNode();
}
}
public void UpdateGraph(ICellMap map, IEnumerable <ICellFragment> obstacles, HierarchicalMap graph, NeighbourMode neighbourMode)
{
foreach (var obstacle in obstacles)
{
UpdateGraph(map, obstacle, graph, neighbourMode);
}
}
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>
/// Refines all the nodes that belong to a certain level to a lower level
/// </summary>
public List<PathNode> RefineAbstractPath(HierarchicalMap map, List<PathNode> path, int level, int maxPathsToRefine = int.MaxValue)
{
var result = new List<PathNode>();
var calculatedPaths = 0;
for (var i = 0; i < path.Count - 1; i++)
{
// if the two consecutive points belong to the same cluster, compute the path between them and
// add the resulting nodes of that path to the list
if (path[i].Level == path[i + 1].Level && path[i].Level == level &&
map.BelongToSameCluster(path[i].Id, path[i + 1].Id, level) && calculatedPaths < maxPathsToRefine)
{
var tmp = this.PerformSearch(map, path[i].Id, path[i + 1].Id, level - 1, false)
.Select(n => new PathNode(n, level - 1))
.ToList();
result.AddRange(tmp);
calculatedPaths++;
// When we have calculated a path between 2 nodes, the next path in the search
// will be an interEdge node. We can safely skip it
i++;
}
else
result.Add(path[i]);
}
// make sure last elem is added
if (result[result.Count - 1].Id != path[path.Count - 1].Id)
result.Add(path[path.Count - 1]);
return result;
}
public static Room ToRoom(this HierarchicalMap hMap, int scale = 1)
{
///MAYBE THIS IS MESSED UP //confirming other stuff first.
var room = new Room();
room.Height = hMap._MapHeight * scale;
room.Width = hMap._MapWidth * scale;
if (room.Height < room.Width && hMap._Portals.Any(x => x.direction.Y == 1))
{
var breaka = "here";
}
room.DistanceBetweenPathAndEdge = 2;//scale - 1;
room.portals = new List <Portal>();
room.FromMap = hMap;
foreach (var portal in hMap._Portals)
{
if (portal.destination == null)
{
continue;
}
if (portal.point.X != 0 && portal.point.X != hMap._MapWidth - 1 && portal.point.Y != 0 && portal.point.Y != hMap._MapHeight - 1)
{
continue;
}
var x = portal.point.X * scale;
var y = portal.point.Y * scale;
if (portal.direction.X == 0)
{
x += (int)Math.Floor((double)scale / 2);
}
else
{
y += (int)Math.Floor((double)scale / 2);
}
//if (portal.direction.X == 0 && x == 0)
// x = room.DistanceBetweenPathAndEdge;
//if (portal.direction.Y == 0 && y == 0)
// y = room.DistanceBetweenPathAndEdge;
if (portal.direction.X == 0 && y != 0)
{
y = (hMap._MapHeight * scale) - 1;
}
else if (portal.direction.Y == 0 && x != 0)
{
x = (hMap._MapWidth * scale) - 1;
}
var p = new Portal()
{
point = new System.Drawing.Point(x, y),
direction = portal.direction
};
p.fromPortal = portal;
portal.portalOffsetFromRoom = new System.Drawing.Point(x, y);
room.portals.Add(p);
}
return(room);
}
public Id <AbstractNode> InsertAbstractNode(HierarchicalMap map, Position pos, int start)
{
var nodeId = Id <ConcreteNode> .From(pos.Y *map.Width + pos.X);
var result = InsertStal(map, nodeId, pos, start);
InsertStalHEdges(map, nodeId);
return(result);
}
public Id <AbstractNode> InsertAbstractNode(HierarchicalMap map, Position pos)
{
var nodeId = Id <ConcreteNode> .From(pos.Y *map.Width + pos.X);
var abstractNodeId = InsertNodeIntoHierarchicalMap(map, nodeId, pos);
map.AddHierarchicalEdgesForAbstractNode(abstractNodeId);
return(abstractNodeId);
}
private static void RemoveNode(HierarchicalMap graph, HierarchicalGraphNode node)
{
foreach (var child in graph.Nodes)
{
child.Remove(node);
}
graph.Nodes.Remove(node);
}
private HierarchicalMap GenerateTwoLevelMap(ICellMap cellMap, NeighbourMode neighbourMode,
int levelZeroClusterSize)
{
CellCluster[,] clusterMatrix = CreateClusters(cellMap, levelZeroClusterSize);
GeneratedClusters = clusterMatrix;
HierarchicalMap levelOneMap = new HierarchicalMap(levelZeroClusterSize, levelZeroClusterSize);
for (int i = 0; i < clusterMatrix.GetLength(0); i++)
{
for (int j = 0; j < clusterMatrix.GetLength(1); j++)
{
CellCluster currentCluster = clusterMatrix[i, j];
int prevI = i - 1;
int prevJ = j - 1;
int nextJ = j + 1;
if (prevI >= 0)
{
CellCluster neighbourCluster = clusterMatrix[prevI, j];
ProceedNeighbourClusters(currentCluster, neighbourCluster, levelOneMap);
}
if (prevJ >= 0)
{
CellCluster neighbourCluster = clusterMatrix[i, prevJ];
ProceedNeighbourClusters(currentCluster, neighbourCluster, levelOneMap);
}
if (prevJ >= 0 && prevI >= 0 && neighbourMode == NeighbourMode.SidesAndDiagonals)
{
CellCluster neighbourCluster = clusterMatrix[prevI, prevJ];
ProceedNeighbourClusters(currentCluster, neighbourCluster, levelOneMap);
}
if (nextJ < cellMap.Height && prevI >= 0 && neighbourMode == NeighbourMode.SidesAndDiagonals)
{
CellCluster neighbourCluster = clusterMatrix[prevI, nextJ];
ProceedNeighbourClusters(currentCluster, neighbourCluster, levelOneMap);
}
}
}
for (int i = 0; i < clusterMatrix.GetLength(0); i++)
{
for (int j = 0; j < clusterMatrix.GetLength(1); j++)
{
_currentCellCluster = clusterMatrix[i, j];
clusterMatrix[i, j].CalculatePaths(neighbourMode, OnCellClusterCellViewed);
}
}
levelOneMap.ZeroLevelClusters = clusterMatrix;
return(levelOneMap);
}
public void RemoveAbstractNode(HierarchicalMap map, Id <AbstractNode> nodeId)
{
if (nodeBackups.ContainsKey(nodeId))
{
RestoreNodeBackup(map, nodeId);
}
else
{
map.RemoveAbstractNode(nodeId);
}
}
/// <summary>
/// Inserts a node and creates edges around the local points of the cluster it the
/// node we try to insert belongs to at each level
/// </summary>
private static void InsertStalHEdges(HierarchicalMap map, Id <ConcreteNode> concreteNodeId)
{
var abstractNodeId = map.ConcreteNodeIdToAbstractNodeIdMap[concreteNodeId];
var abstractNodeInfo = map.AbstractGraph.GetNodeInfo(abstractNodeId);
var oldLevel = abstractNodeInfo.Level;
abstractNodeInfo.Level = map.MaxLevel;
for (var level = oldLevel + 1; level <= map.MaxLevel; level++)
{
map.AddEdgesToOtherEntrancesInCluster(abstractNodeInfo, level);
}
}
public List<PathNode> DoHierarchicalSearch(HierarchicalMap map, int startNodeId, int targetNodeId, int maxSearchLevel, int maxPathsToRefine = int.MaxValue)
{
var path = this.PerformSearch(map, startNodeId, targetNodeId, maxSearchLevel, true)
.Select(n => new PathNode(n, maxSearchLevel)).ToList();
if (path.Count == 0) return path;
for (var level = maxSearchLevel; level > 1; level--)
path = this.RefineAbstractPath(map, path, level, maxPathsToRefine);
return path;
}
private void Generate()
{
HierarchicalMapFactory factory = new HierarchicalMapFactory();
HierarchicalMap hierarchicalMap = factory.CreateHierarchicalMap(m_concreteMap, m_clusterSize, m_maxLevel);
List <PathNode> path = HierarchicalSearch.Search(factory, hierarchicalMap, m_maxLevel, m_startNode.Pos, m_goalNode.Pos);
for (int i = 0; i < path.Count; i++)
{
var node = m_concreteMap.Get(path[i].Pos);
node.SetSearchType(SearchType.Path);
}
}
// insert a new node, such as start or target, to the abstract graph and
// returns the id of the newly created node in the abstract graph
// x and y are the positions where I want to put the node
private Id <AbstractNode> InsertNodeIntoHierarchicalMap(HierarchicalMap map, Id <ConcreteNode> concreteNodeId, Position pos)
{
// If the node already existed (for instance, it was the an entrance point already
// existing in the graph, we need to keep track of the previous status in order
// to be able to restore it once we delete this STAL
if (map.ConcreteNodeIdToAbstractNodeIdMap.ContainsKey(concreteNodeId))
{
var existingAbstractNodeId = map.ConcreteNodeIdToAbstractNodeIdMap[concreteNodeId];
var nodeBackup = new NodeBackup(
map.AbstractGraph.GetNodeInfo(existingAbstractNodeId).Level,
map.GetNodeEdges(concreteNodeId));
nodeBackups[existingAbstractNodeId] = nodeBackup;
return(map.ConcreteNodeIdToAbstractNodeIdMap[concreteNodeId]);
}
var cluster = map.FindClusterForPosition(pos);
// create global entrance
var abstractNodeId = Id <AbstractNode> .From(map.NrNodes);
var entrance = cluster.AddEntrance(abstractNodeId, new Position(pos.X - cluster.Origin.X, pos.Y - cluster.Origin.Y));
cluster.UpdatePathsForLocalEntrance(entrance);
map.ConcreteNodeIdToAbstractNodeIdMap[concreteNodeId] = abstractNodeId;
var info = new AbstractNodeInfo(
abstractNodeId,
1,
cluster.Id,
pos,
concreteNodeId);
map.AbstractGraph.AddNode(abstractNodeId, info);
foreach (var entrancePoint in cluster.EntrancePoints)
{
if (cluster.AreConnected(abstractNodeId, entrancePoint.AbstractNodeId))
{
map.AddEdge(
entrancePoint.AbstractNodeId,
abstractNodeId,
cluster.GetDistance(entrancePoint.AbstractNodeId, abstractNodeId));
map.AddEdge(
abstractNodeId,
entrancePoint.AbstractNodeId,
cluster.GetDistance(abstractNodeId, entrancePoint.AbstractNodeId));
}
}
return(abstractNodeId);
}
/// <summary>
/// 移除寻路时新增的抽象节点
/// </summary>
public void RemoveAbstractNode(HierarchicalMap map, int nodeId)
{
NodeBackup backup;
if (m_backupDict.TryGetValue(nodeId, out backup))
{
RestoreNodeBackup(map, nodeId, backup); //如果寻路时新增的节点在图中原本就存在，则重置相关数据
}
else
{
map.RemoveAbstractNode(nodeId); //如果的确新增了节点，则直接移除该节点
}
}
public void UpdateGraph(ICellMap map, ICellFragment cellFragment, HierarchicalMap graph, NeighbourMode neighbourMode)
{
var clusterMatrix = graph.ZeroLevelClusters;
int clusterWidth = graph.ClusterDefaultWidth;
int clusterHeight = graph.ClusterDefaultHeight;
Vector2Int fragmentSize = new Vector2Int(cellFragment.Width, cellFragment.Height);
Vector2Int leftBottomClusterPosition = GetContainingClusterPosition(clusterMatrix, clusterWidth, clusterHeight, cellFragment.LeftBottom);
Vector2Int rightTopClusterPosition = GetContainingClusterPosition(clusterMatrix, clusterWidth, clusterHeight, cellFragment.LeftBottom + fragmentSize);
int iMin = leftBottomClusterPosition.X;
int iMax = rightTopClusterPosition.X;
int jMin = leftBottomClusterPosition.Y;
int jMax = rightTopClusterPosition.Y;
for (int i = iMin; i <= iMax; i++)
{
for (int j = jMin; j <= jMax; j++)
{
CellCluster currentCluster = clusterMatrix[i, j];
int prevI = i - 1;
int prevJ = j - 1;
int nextJ = j + 1;
if (prevI >= iMin)
{
CellCluster neighbourCluster = clusterMatrix[prevI, j];
RemoveTransitionNodes(currentCluster, neighbourCluster, graph);
ProceedNeighbourClusters(currentCluster, neighbourCluster, graph);
}
if (prevJ >= jMin)
{
CellCluster neighbourCluster = clusterMatrix[i, prevJ];
RemoveTransitionNodes(currentCluster, neighbourCluster, graph);
ProceedNeighbourClusters(currentCluster, neighbourCluster, graph);
}
}
}
for (int i = iMin; i <= iMax; i++)
{
for (int j = jMin; j <= jMax; j++)
{
CellCluster currentCluster = clusterMatrix[i, j];
currentCluster.CalculatePaths(neighbourMode, null);
}
}
}
private void CreateEdgesNP(List <Entrance> entrances, List <Cluster> clusters)
{
foreach (var entrance in entrances)
{
CreateEntranceEdges(entrance, HierarchicalMap.Type);
}
//foreach (var cluster in clusters)
//{
// cluster.CreateIntraClusterEdges();
// CreateIntraClusterEdges(cluster);
//}
HierarchicalMap.CreateHierarchicalEdges();
}
public List <AbstractPathNode> DoHierarchicalSearch(HierarchicalMap map, Id <AbstractNode> startNodeId, Id <AbstractNode> targetNodeId, int maxSearchLevel, int maxPathsToRefine = int.MaxValue)
{
List <AbstractPathNode> path = GetPath(map, startNodeId, targetNodeId, maxSearchLevel, true);
if (path.Count == 0)
{
return(path);
}
for (var level = maxSearchLevel; level > 1; level--)
{
path = RefineAbstractPath(map, path, level, maxPathsToRefine);
}
return(path);
}
public static List <PathNode> Search(HierarchicalMapFactory factory, HierarchicalMap map, int maxLevel, Vector2Int startPos, Vector2Int goalPos, int maxRefineCount = int.MaxValue)
{
//首先插入起点和终点到图中
var startAbsId = factory.InsertAbstractNode(map, startPos);
var goalAbsId = factory.InsertAbstractNode(map, goalPos);
var path = SearchImpl(map, startAbsId, goalAbsId, maxLevel, maxRefineCount);
//可以在这里应用路径平滑
//搜索完毕后就移除起点和终点（如果常用，也可以留着）
factory.RemoveAbstractNode(map, goalAbsId);
factory.RemoveAbstractNode(map, startAbsId);
return(path);
}
/// <summary>
/// 插入节点到Cluster内，并构建相关节点和边到图中
/// </summary>
private int InsertAbstractNodeInClusterLevel(HierarchicalMap map, Vector2Int concretePos)
{
AbstractNode abstractNode = map.GetAbstractNode(concretePos);
//如果要插入的位置已经存在节点，则保存相关信息，以便之后删除时恢复
if (abstractNode != null)
{
NodeBackup backup = new NodeBackup(abstractNode.Level, abstractNode.Edges.Values.ToList());
if (m_backupDict.ContainsKey(abstractNode.Id))
{
Debug.LogError("已经存在一个NodeBackup，逻辑上出错了");
}
m_backupDict[abstractNode.Id] = backup;
return(abstractNode.Id);
}
//把节点插入到Cluster内
int abstractId = map.NodeCount();
var cluster = map.FindClusterForPosition(concretePos);
var entrance = cluster.AddEntrancePoint(abstractId, m_concreteMap.Get(concretePos));
cluster.AddIntraEdgesData(entrance);
//把节点插入到图中
abstractNode = HPADemo.Instance.CreateAbstractNode(1, concretePos);
abstractNode.Init(abstractId, 1, cluster.Id, concretePos);
map.AddAbstractNode(abstractNode);
map.AbstractGraph.AddNode(abstractNode);
//把该节点相关的边插入到图中
foreach (var otherEntrance in cluster.EntrancePoints)
{
if (cluster.IsConnected(abstractId, otherEntrance.AbstractId))
{
float distance = cluster.Distance(abstractId, otherEntrance.AbstractId);
AbstractEdge edge = HPADemo.Instance.CreateEdge(concretePos, otherEntrance.ConcreteNode.Pos, 1, false);
edge.Init(otherEntrance.AbstractId, distance, 1, false);
map.AbstractGraph.AddEdge(abstractId, edge);
edge = HPADemo.Instance.CreateEdge(otherEntrance.ConcreteNode.Pos, concretePos, 1, false);
edge.Init(abstractId, distance, 1, false);
map.AbstractGraph.AddEdge(otherEntrance.AbstractId, edge);
}
}
return(abstractId);
}
public void CreateHierarchicalMapNP(ConcreteMap concreteMap, int clusterSize, int maxLevel, EntranceStyle style)
{
ClusterSize = clusterSize;
EntranceStyle = style;
MaxLevel = maxLevel;
ConcreteMap = concreteMap;
HierarchicalMap = new HierarchicalMap(concreteMap, clusterSize, maxLevel);
List <Entrance> entrances;
List <Cluster> clusters;
CreateEntrancesAndClusters(out entrances, out clusters);
HierarchicalMap.Clusters = clusters;
CreateAbstractNodes(entrances);
CreateEdgesNP(entrances, clusters);
}
public HierarchicalMap CreateHierarchicalMap(ConcreteMap concreteMap, int clusterSize, int maxLevel)
{
m_concreteMap = concreteMap;
m_clusterSize = clusterSize;
m_maxLevel = maxLevel;
m_hierarchicalMap = new HierarchicalMap(concreteMap, clusterSize, maxLevel);
List <Cluster> clusters = new List <Cluster>();
List <Entrance> entrances = new List <Entrance>();
CreateClustersAndEntrances(clusters, entrances);
m_hierarchicalMap.InitClusters(clusters);
CreateAbstractNodes(entrances);
CreateEdges(clusters, entrances);
return(m_hierarchicalMap);
}
private static void ProceedNeighbourClusters(CellCluster currentCluster, CellCluster neighbourCluster,
HierarchicalMap graph)
{
List <Vector2Int> currentPoints = new List <Vector2Int>(2);
List <Vector2Int> neighbourPoints = new List <Vector2Int>(2);
GetTransitionCells(currentCluster, neighbourCluster, currentPoints, neighbourPoints);
for (var k = 0; k < currentPoints.Count; k++)
{
var currentPoint = currentPoints[k];
var neighbourPoint = neighbourPoints[k];
HierarchicalGraphNode transitionNodeCurrent, transitionNodeNeighbour;
transitionNodeCurrent =
(HierarchicalGraphNode)currentCluster.TransitionNodes.FirstOrDefault(node =>
node.Position == currentPoint);
transitionNodeNeighbour =
(HierarchicalGraphNode)neighbourCluster.TransitionNodes.FirstOrDefault(node =>
node.Position == neighbourPoint);
if (transitionNodeCurrent == null)
{
transitionNodeCurrent = new HierarchicalGraphNode();
currentCluster.TransitionNodes.Add(transitionNodeCurrent);
graph.Nodes.Add(transitionNodeCurrent);
}
if (transitionNodeNeighbour == null)
{
transitionNodeNeighbour = new HierarchicalGraphNode();
neighbourCluster.TransitionNodes.Add(transitionNodeNeighbour);
graph.Nodes.Add(transitionNodeNeighbour);
}
transitionNodeCurrent.SetWeight(transitionNodeNeighbour, 1);
transitionNodeNeighbour.SetWeight(transitionNodeCurrent, 1);
transitionNodeCurrent.Position = currentPoint;
transitionNodeNeighbour.Position = neighbourPoint;
transitionNodeCurrent.ParentCluster = currentCluster;
transitionNodeNeighbour.ParentCluster = neighbourCluster;
}
}
public HierarchicalMap CreateHierarchicalMap(ConcreteMap concreteMap, int clusterSize, int maxLevel, EntranceStyle style)
{
_clusterSize = clusterSize;
_entranceStyle = style;
_maxLevel = maxLevel;
_concreteMap = concreteMap;
_hierarchicalMap = new HierarchicalMap(concreteMap, clusterSize, maxLevel);
List <Entrance> entrances;
List <Cluster> clusters;
CreateEntrancesAndClusters(out entrances, out clusters);
_hierarchicalMap.Clusters = clusters;
CreateAbstractNodes(entrances);
CreateEdges(entrances, clusters);
return(_hierarchicalMap);
}
public List<int> PerformSearch(HierarchicalMap map, int startNodeId, int targetNodeId, int level, bool mainSearch)
{
var search = new AStar();
map.SetCurrentLevel(level);
var nodeInfo = map.AbstractGraph.GetNodeInfo(startNodeId);
if (mainSearch)
map.SetCurrentCluster(nodeInfo.Position, map.MaxLevel + 1);
else
map.SetCurrentCluster(nodeInfo.Position, level + 1);
// TODO: This could be perfectly replaced by cached paths in the clusters!
var path = search.FindPath(map, startNodeId, targetNodeId);
if (path.PathCost == -1)
{
// No path found
return new List<int>();
}
var result = path.PathNodes;
result.Reverse();
return result;
}
public List<PathNode> AbstractPathToLowLevelPath(HierarchicalMap map, List<PathNode> absPath, int width, int maxPathsToCalculate = int.MaxValue)
{
var result = new List<PathNode>(absPath.Count * 10);
if (absPath.Count == 0) return result;
var calculatedPaths = 0;
var lastAbsNodeId = absPath[0].Id;
for (var j = 1; j < absPath.Count; j++)
{
var currentAbsNodeId = absPath[j].Id;
var currentNodeInfo = map.AbstractGraph.GetNodeInfo(currentAbsNodeId);
var lastNodeInfo = map.AbstractGraph.GetNodeInfo(lastAbsNodeId);
// We cannot compute a low level path from a level which is higher than lvl 1
// (obvious...) therefore, ignore any non-refined path
if (absPath[j].Level > 1)
{
result.Add(absPath[j]);
continue;
}
var eClusterId = currentNodeInfo.ClusterId;
var leClusterId = lastNodeInfo.ClusterId;
if (eClusterId == leClusterId && calculatedPaths < maxPathsToCalculate)
{
// insert the local solution into the global one
// var cluster = map.GetCluster(eClusterId);
//var localpos1 = cluster.GetLocalPosition(lastNodeInfo.LocalEntranceId);
//var localpos2 = cluster.GetLocalPosition(currentNodeInfo.LocalEntranceId);
if (lastNodeInfo.LocalEntranceId != currentNodeInfo.LocalEntranceId)
{
var cluster = map.GetCluster(eClusterId);
var localPath = cluster.GetPath(lastNodeInfo.LocalEntranceId, currentNodeInfo.LocalEntranceId)
.Select(
localId =>
{
int localPoint = LocalClusterId2GlobalId(localId, cluster, width);
return new PathNode(localPoint, 0);
});
result.AddRange(localPath);
calculatedPaths++;
}
}
else
{
var lastVal = lastNodeInfo.CenterId;
var currentVal = currentNodeInfo.CenterId;
if (result[result.Count - 1].Id != lastVal)
result.Add(new PathNode(lastVal, 0));
result.Add(new PathNode(currentVal, 0));
}
lastAbsNodeId = currentAbsNodeId;
}
return result;
}
