public static void Fill(Node position, int range, SuperNode supernode)
{
var openQueue = new Utils.PriorityQueue <VisitedNode>();
var pathNodeMap = new Dictionary <Node, VisitedNode>();
pathNodeMap[position] = new VisitedNode(position, null, 0);
openQueue.Enqueue(pathNodeMap[position], 0);
while (!openQueue.IsEmpty())
{
var current = openQueue.Dequeue();
current.GridNode.ConnectSuperNode(current.Prev != null ? current.Prev.GridNode : null, supernode, current.GScore);
foreach (var neighbour in current.GridNode.GetNeighbours())
{
if (!neighbour.To.SuperNodes.ContainsKey(supernode) && neighbour.Length + current.GScore <= range)
{
var newNode = new VisitedNode(neighbour.To, current, neighbour.Length);
if (pathNodeMap.ContainsKey(neighbour.To))
{
if (openQueue.Update(pathNodeMap[neighbour.To], pathNodeMap[neighbour.To].GScore, newNode, newNode.GScore))
{
pathNodeMap[neighbour.To] = newNode;
}
}
else
{
openQueue.Enqueue(newNode, newNode.GScore);
pathNodeMap[neighbour.To] = newNode;
}
}
}
}
}
public static Path GetPath(Dictionary <Node, float> from, List <Node> to, Path path)
{
var openSet = new Utils.PriorityQueue <VisitedNode>();
var pathNodeMap = new Dictionary <Node, VisitedNode>();
var nodesTo = new HashSet <VisitedNode>();
foreach (var node in to)
{
nodesTo.Add(new VisitedNode(node, null, 0));
}
if (nodesTo.Count == 0)
{
return(null);
}
foreach (var f in from)
{
var nodeFrom = new VisitedNode(f.Key, null, f.Value);
openSet.Enqueue(nodeFrom, nodeFrom.GetCost(nodesTo));
}
while (!openSet.IsEmpty())
{
var curNode = openSet.Dequeue();
if (nodesTo.Contains(curNode))
{
path = ReconstructPath(curNode, path);
//Debug.Log("Found path between " + nodeFrom.GridNode.Position + " and " + nodeTo.GridNode.Position + " of length: " + path.Length + " in " + (DateTime.Now-start).TotalMilliseconds + "ms.");
return(path);
}
curNode.Status = NodeStatus.Closed;
foreach (var neighbour in curNode.GridNode.GetNeighbours())
{
if (pathNodeMap.ContainsKey(neighbour.To))
{
var pathNode = pathNodeMap[neighbour.To];
if (pathNode.Status == NodeStatus.Closed)
{
continue;
}
var node = new VisitedNode(neighbour.To, curNode, neighbour.Length);
if (openSet.Update(pathNode, pathNode.GetCost(nodesTo), node, node.GetCost(nodesTo)))
{
pathNodeMap[neighbour.To] = node;
}
}
else
{
var node = new VisitedNode(neighbour.To, curNode, neighbour.Length);
openSet.Enqueue(node, node.GetCost(nodesTo));
pathNodeMap[neighbour.To] = node;
}
}
}
//Debug.Log("Couldn't find path between " + nodeFrom.GridNode + " and " + nodeTo.GridNode + " in " + (DateTime.Now - start).TotalMilliseconds + "ms.");
return(path);
}
public VisitedNode(Node node, VisitedNode prev, float cost)
{
GridNode = node;
Prev = prev;
if (prev == null)
{
GScore = cost;
}
else
{
GScore = prev.GScore + cost;
}
}
private static Path ReconstructPath(VisitedNode node, Path path)
{
var length = node.GScore;
var nodes = new List <Node>();
while (node != null)
{
nodes.Add(node.GridNode);
node = node.Prev;
}
nodes.Reverse();
path.Nodes = nodes;
path.Length = length;
return(path);
}
public static void FillNeigbours(SuperNode superNode, int gridSize)
{
var nodesToFill = superNode.GetNeighbours().Select(n => n.To).ToArray();
var openQueue = new Utils.PriorityQueue <VisitedNode>();
var pathNodeMap = new Dictionary <Node, VisitedNode>();
foreach (var childNode in superNode.ChildNodes)
{
if (childNode.SuperNodes[superNode].Length > gridSize * 0.8)
{
pathNodeMap[childNode] = new VisitedNode(childNode, null, childNode.SuperNodes[superNode].Length);
openQueue.Enqueue(pathNodeMap[childNode], pathNodeMap[childNode].GScore);
}
}
while (!openQueue.IsEmpty())
{
var current = openQueue.Dequeue();
if (!current.GridNode.SuperNodes.ContainsKey(superNode))
{
current.GridNode.ConnectSuperNode(current.Prev != null ? (current.Prev.GridNode ?? superNode) : superNode, superNode, current.GScore);
}
foreach (var neighbour in current.GridNode.GetNeighbours())
{
if (!neighbour.To.SuperNodes.ContainsKey(superNode) && neighbour.To.SuperNodes.Any(k => k.Value.Length <= gridSize && nodesToFill.Contains(k.Key)))
{
var newNode = new VisitedNode(neighbour.To, current, neighbour.Length);
if (pathNodeMap.ContainsKey(neighbour.To))
{
if (openQueue.Update(pathNodeMap[neighbour.To], pathNodeMap[neighbour.To].GScore, newNode, newNode.GScore))
{
pathNodeMap[neighbour.To] = newNode;
}
}
else
{
openQueue.Enqueue(newNode, newNode.GScore);
pathNodeMap[neighbour.To] = newNode;
}
}
}
}
}
public bool Equals(VisitedNode node)
{
return(node.GridNode.Equals(GridNode));
}
