/// <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);
}
/// <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);
}
/// <summary>
/// 判断边是否可用于某层级
/// </summary>
private static bool IsValidEdgeForLevel(AbstractEdge edge, int level)
{
if (edge.IsInterEdge)
{
return(edge.Level >= level);
}
else
{
return(edge.Level == level);
}
}
public void AddEdge(AbstractEdge edge)
{
if (Edges.TryGetValue(edge.TargetNodeId, out AbstractEdge originEdge))
{
if (originEdge.Level < edge.Level)
{
Edges[edge.TargetNodeId] = edge;
originEdge.Release();
}
}
else
{
Edges[edge.TargetNodeId] = edge;
}
}
/// <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 virtual bool Equals(AbstractEdge <PointType> edge1, AbstractEdge <PointType> edge2)
{
if (edge2 == null && edge1 == null)
{
return(true);
}
else if (edge1 == null | edge2 == null)
{
return(false);
}
else if (edge1.StartPoint.Equals(edge2.StartPoint) && edge1.EndPoint.Equals(edge2.EndPoint))
{
return(true);
}
else
{
return(false);
}
}
/// <summary>
/// 构建指定层级节点间的边
/// </summary>
private void AddEdgesBetweenAbstractNodes(int absId1, int absId2, int level)
{
var node1 = AbstractGraph.GetNode(absId1);
var node2 = AbstractGraph.GetNode(absId2);
var planner = new PathPlanner(this, null);
var path = planner.Search(node1, node2);
if (path != null && path.Nodes.Count > 0)
{
AbstractEdge edge = HPADemo.Instance.CreateEdge(node1.Pos, node2.Pos, level, false);
edge.Init(absId2, path.Cost, level, false);
edge.SetInnerLowerLevelPath(path.Nodes);
AbstractGraph.AddEdge(absId1, edge);
path.Nodes.Reverse();
edge = HPADemo.Instance.CreateEdge(node2.Pos, node1.Pos, level, false);
edge.Init(absId1, path.Cost, level, false);
edge.SetInnerLowerLevelPath(path.Nodes);
AbstractGraph.AddEdge(absId2, edge);
}
}
public void AddEdge(int srcId, AbstractEdge edge)
{
Nodes[srcId].AddEdge(edge);
}
public virtual int GetHashCode(AbstractEdge <PointType> obj)
{
return(0);
//return obj.GetHashCode();
}
