public void RemoveAbstractNode(HierarchicalMap map, int 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.RemoveEdgesFromNode(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.IsInterEdge);
map.AddEdge(targetNodeId, nodeId, edge.Info.Cost,
edge.Info.Level, edge.Info.IsInterEdge);
}
}
else
{
// Just delete the node from the graph
var currentNodeInfo = abstractGraph.GetNodeInfo(nodeId);
var clusterId = currentNodeInfo.ClusterId;
var cluster = map.Clusters[clusterId];
cluster.RemoveLastEntranceRecord();
map.AbsNodeIds[currentNodeInfo.CenterId] = Constants.NO_NODE;
abstractGraph.RemoveEdgesFromNode(nodeId);
abstractGraph.RemoveLastNode();
}
}
public int InsertAbstractNode(HierarchicalMap map, Position pos, int start)
{
var nodeId = pos.Y * map.Width + pos.X;
var result = InsertStal(map, nodeId, pos, start);
InsertStalHEdges(map, nodeId);
return result;
}
public void CreateHierarchicalMap(ConcreteMap concreteMap, int clusterSize, int maxLevel, EntranceStyle style)
{
this.ClusterSize = clusterSize;
this.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, clusters);
CreateEdges(entrances, clusters);
}
private static List <IPathNode> HierarchicalSearch(HierarchicalMap hierarchicalMap, int maxLevel, ConcreteMap concreteMap, Position startPosition, Position endPosition)
{
var factory = new HierarchicalMapFactory();
var startAbsNode = factory.InsertAbstractNode(hierarchicalMap, startPosition);
var targetAbsNode = factory.InsertAbstractNode(hierarchicalMap, endPosition);
var maxPathsToRefine = int.MaxValue;
var hierarchicalSearch = new HierarchicalSearch();
var abstractPath = hierarchicalSearch.DoHierarchicalSearch(hierarchicalMap, startAbsNode, targetAbsNode, maxLevel, maxPathsToRefine);
var path = hierarchicalSearch.AbstractPathToLowLevelPath(hierarchicalMap, abstractPath, hierarchicalMap.Width, maxPathsToRefine);
var smoother = new SmoothWizard(concreteMap, path);
path = smoother.SmoothPath();
factory.RemoveAbstractNode(hierarchicalMap, targetAbsNode);
factory.RemoveAbstractNode(hierarchicalMap, startAbsNode);
return(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 int InsertStal(HierarchicalMap map, int nodeId, Position pos, int start)
{
// 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.AbsNodeIds[nodeId] != Constants.NO_NODE)
{
m_stalLevel[start] = map.AbstractGraph.GetNodeInfo(map.AbsNodeIds[nodeId]).Level;
m_stalEdges[start] = map.GetNodeEdges(nodeId);
m_stalUsed[start] = true;
return map.AbsNodeIds[nodeId];
}
m_stalUsed[start] = false;
var cluster = map.FindClusterForPosition(pos);
// create global entrance
var absNodeId = map.NrNodes;
var localEntranceIdx = cluster.AddEntrance(absNodeId, new Position(pos.X - cluster.Origin.X, pos.Y - cluster.Origin.Y));
cluster.UpdatePaths(localEntranceIdx);
map.AbsNodeIds[nodeId] = absNodeId;
var info = new AbsTilingNodeInfo(
absNodeId,
1,
cluster.Id,
pos,
nodeId,
localEntranceIdx);
map.AbstractGraph.AddNode(absNodeId, info);
// add new edges to the abstract graph
for (var k = 0; k < cluster.GetNrEntrances() - 1; k++)
{
if (cluster.AreConnected(localEntranceIdx, k))
{
map.AddEdge(
cluster.GetGlobalAbsNodeId(k),
cluster.GetGlobalAbsNodeId(localEntranceIdx),
cluster.GetDistance(localEntranceIdx, k));
map.AddEdge(
cluster.GetGlobalAbsNodeId(localEntranceIdx),
cluster.GetGlobalAbsNodeId(k),
cluster.GetDistance(k, localEntranceIdx));
}
}
return absNodeId;
}
/// <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, int nodeId)
{
var abstractNodeId = map.AbsNodeIds[nodeId];
var nodeInfo = map.AbstractGraph.GetNodeInfo(abstractNodeId);
var oldLevel = nodeInfo.Level;
nodeInfo.Level = map.MaxLevel;
for (var level = oldLevel + 1; level <= map.MaxLevel; level++)
{
map.SetCurrentLevel(level - 1);
map.SetCurrentCluster(nodeInfo.Position, level);
var clusterRectangle = map.GetCurrentClusterRectangle();
var currentClusterY0 = clusterRectangle.Origin.Y;
var currentClusterY1 = clusterRectangle.Origin.Y + clusterRectangle.Size.Height;
var currentClusterX0 = clusterRectangle.Origin.X;
var currentClusterX1 = clusterRectangle.Origin.X + clusterRectangle.Size.Width;
for (var y = currentClusterY0; y <= currentClusterY1; y++)
for (var x = currentClusterX0; x <= currentClusterX1; x++)
{
var nodeId2 = y * map.Width + x;
var abstractNodeId2 = map.AbsNodeIds[nodeId2];
AddEdgesBetweenAbstractNodes(map, abstractNodeId, abstractNodeId2, level);
}
}
}
/// <summary>
/// Adds an edge between two abstract nodes for a given level
/// </summary>
private static void AddEdgesBetweenAbstractNodes(HierarchicalMap map, int absNodeId1, int absNodeId2, int level)
{
if (absNodeId1 == absNodeId2 || !IsValidAbstractNode(map, absNodeId2, level))
return;
var search = new AStar();
var path = search.FindPath(map, absNodeId1, absNodeId2);
if (path.PathCost >= 0)
{
map.AddEdge(absNodeId1, absNodeId2, path.PathCost, level, false);
map.AddEdge(absNodeId2, absNodeId1, path.PathCost, level, false);
}
}
private static bool IsValidAbstractNode(HierarchicalMap map, int abstractNode, int level)
{
if (abstractNode == Constants.NO_NODE)
return false;
var nodeInfo1 = map.AbstractGraph.GetNodeInfo(abstractNode);
if (nodeInfo1.Level < level)
return false;
return true;
}
private static void PrintFormatted(List <char> chars, ConcreteMap concreteMap, HierarchicalMap hierarchicalGraph, int clusterSize, List <Position> path)
{
for (var y = 0; y < concreteMap.Height; y++)
{
if (y % clusterSize == 0)
{
Console.WriteLine("---------------------------------------------------------");
}
for (var x = 0; x < concreteMap.Width; x++)
{
Console.ForegroundColor = ConsoleColor.White;
if (x % clusterSize == 0)
{
Console.Write('|');
}
var nodeId = concreteMap.GetNodeIdFromPos(x, y);
var hasAbsNode = hierarchicalGraph.AbstractGraph.Nodes.SingleOrDefault(n => n.Info.ConcreteNodeId == nodeId);
if (hasAbsNode != null)
{
switch (hasAbsNode.Info.Level)
{
case 1: Console.ForegroundColor = ConsoleColor.Red;
break;
case 2: Console.ForegroundColor = ConsoleColor.DarkGreen;
break;
}
}
Console.Write(path.Any(node => node.X == x && node.Y == y) ? 'X' : chars[nodeId.IdValue]);
}
Console.WriteLine();
}
}
public static void PrintFormatted(ConcreteMap concreteMap, HierarchicalMap hierarchicalGraph, int clusterSize, List <Position> path)
{
PrintFormatted(GetCharVector(concreteMap), concreteMap, hierarchicalGraph, clusterSize, path);
}
private static void PrintFormatted(List<char> chars, ConcreteMap concreteMap, HierarchicalMap hierarchicalGraph, int clusterSize, List<Position> path)
{
for (var y = 0; y < concreteMap.Height; ++y)
{
if (y % clusterSize == 0) Console.WriteLine("---------------------------------------------------------");
for (var x = 0; x < concreteMap.Width; ++x)
{
Console.ForegroundColor = ConsoleColor.White;
if (x % clusterSize == 0) Console.Write('|');
var nodeId = concreteMap.GetNodeIdFromPos(x, y);
var hasAbsNode = hierarchicalGraph.AbstractGraph.Nodes.FirstOrDefault(n => n.Info.CenterId == nodeId);
if (hasAbsNode != null)
switch (hasAbsNode.Info.Level)
{
case 1: Console.ForegroundColor = ConsoleColor.Red;
break;
case 2: Console.ForegroundColor = ConsoleColor.DarkGreen;
break;
}
Console.Write(path.Any(n => n.X == x && n.Y == y) ? 'X' : chars[nodeId]);
}
Console.WriteLine();
}
}
public static void PrintFormatted(ConcreteMap concreteMap, HierarchicalMap hierarchicalGraph, int clusterSize, List<Position> path)
{
PrintFormatted(GetCharVector(concreteMap), concreteMap, hierarchicalGraph, clusterSize, path);
}
private static List<Position> HierarchicalSearch(HierarchicalMap hierarchicalMap, int maxLevel, ConcreteMap concreteMap)
{
// Hierarchical pathfinding
var factory = new AbstractMapFactory();
var startAbsNode = factory.InsertAbstractNode(hierarchicalMap, StartPosition, 0);
var targetAbsNode = factory.InsertAbstractNode(hierarchicalMap, EndPosition, 1);
var maxPathsToRefine = int.MaxValue;
var hierarchicalSearch = new HierarchicalSearch();
var abstractPath = hierarchicalSearch.DoHierarchicalSearch(hierarchicalMap, startAbsNode, targetAbsNode, maxLevel, maxPathsToRefine);
var path = hierarchicalSearch.AbstractPathToLowLevelPath(hierarchicalMap, abstractPath, hierarchicalMap.Width, maxPathsToRefine);
factory.RemoveAbstractNode(hierarchicalMap, targetAbsNode, 1);
factory.RemoveAbstractNode(hierarchicalMap, startAbsNode, 0);
//var smoother = new SmoothWizard(concreteMap, path);
//path = smoother.SmoothPath();
return path.Select(p => concreteMap.Graph.GetNodeInfo(p.Id).Position).ToList();
//return path.Select(n => n.Level == 0 ? concreteMap.Graph.GetNodeInfo(n.Id).Position : hierarchicalMap.AbstractGraph.GetNodeInfo(n.Id).Position).ToList();
}
