public IList <ILevelTile> CalculatePath(ILevelTile from, ILevelTile to)
{
target = to;
openNodes = new FibonacciHeap <float>();
closedNodes = new HashSet <AStarNode>();
tilesToNodes = new Dictionary <ILevelTile, AStarNode>();
AStarNode startNode = new AStarNode(from, heuristic.GetDistance(from.CenterPos, to.CenterPos));
startNode.cost = 0;
tilesToNodes.Add(from, startNode);
openNodes.Insert(startNode);
do
{
AStarNode currNode = (AStarNode)openNodes.ExtractMin();
if (currNode.tile.Equals(to))
{
return(GetPathToNode(currNode));
}
closedNodes.Add(currNode);
ExpandNode(currNode);
} while (openNodes.IsEmpty() == false);
// No path found
return(null);
}
/// <summary>
/// Implementation of uniform-cost search algorithm using a Fibonacci heap data structure to minimize computing times
/// </summary>
/// <param name="source">The node on which to start the search</param>
/// <param name="destination">The node we try to find the shortest path to, starting from source</param>
public List <int> LeastCostPath(int source, int destination)
{
var Predecessor = new Dictionary <int, int>();
var Distance = new Dictionary <int, double>();
var Frontier = new FibonacciHeap <int, double>(0);
Frontier.Insert(new FibonacciHeapNode <int, double>(source, 0));
var Explored = new List <int>();
Predecessor.Add(source, -1); //value of -1 indicates this node has no predecessors
while (true)
{
if (Frontier.IsEmpty())
{
throw new Exception("LeastCostPath: Failed to find path between source (" + source + ") and destination (" + destination + ").");
}
var minNode = Frontier.RemoveMin();
if (minNode.Data == destination)
{
List <int> LCP = new List <int> {
minNode.Data
};
int pred = Predecessor[minNode.Data];
while (pred != -1)
{
LCP.Add(pred);
pred = Predecessor[pred];
}
LCP.Reverse();
return(LCP);
}
Explored.Add(minNode.Data);
foreach (int neighbor in this.GetNeighbors(minNode.Data))
{
if (!Explored.Contains(neighbor))
{
var neighborCost = minNode.Key + AdjacencyMatrix[minNode.Data, neighbor];
Frontier.Insert(new FibonacciHeapNode <int, double>(neighbor, neighborCost));
if (Distance.TryGetValue(neighbor, out double cost))
{
if (neighborCost < cost)
{
Predecessor[neighbor] = minNode.Data;
Distance[neighbor] = neighborCost;
}
}
else
{
Predecessor.Add(neighbor, minNode.Data);
Distance.Add(neighbor, neighborCost);
}
}
}
}
}
/// <summary>
/// Implementation of Yen's algorithm which finds the shortest path between nodes, and then the
/// K-1 shortest deviations from this path. Paths returned will be simple and loopless
/// </summary>
/// <param name="K">The number of shortest paths to find.</param>
/// <param name="source">The node on which to start the search</param>
/// <param name="destination">The node we try to find the shortest path to, starting from source</param>
/// <returns></returns>
public List <List <int> > KShortestPaths(int K, int source, int destination)
{
List <List <int> > ShortestPaths = new List <List <int> >();
var PotentialPaths = new FibonacciHeap <List <int>, double>(0);
ShortestPaths.Add(LeastCostPath(source, destination));
//now find next K-1 shortest paths
foreach (int k in Enumerable.Range(1, K - 1))
{
//The spur node ranges from the first node to the next to last node in the previous k-shortest path.
int spurNodeCount = ShortestPaths[k - 1].Count - 1;
foreach (int i in Enumerable.Range(0, spurNodeCount))
{
int spurNode = ShortestPaths[k - 1][i];
List <int> rootPath = ShortestPaths[k - 1].GetRange(0, i + 1);
PSO_WeightedGraph AlteredGraph = this.Clone();
//temporarily remove edges to avoid retracing our steps
foreach (List <int> shortPath in ShortestPaths)
{
if (rootPath.SequenceEqual(shortPath.Take(i + 1)))
{
AlteredGraph.AdjacencyMatrix[shortPath[i], shortPath[i + 1]] = 0;
}
}
//To avoid looping back over a previous path, we disconnect nodes in the root path (except the spur node)
//by setting the weights of the edges that connect them to the graph to 0
foreach (int x in Enumerable.Range(0, Size).Where(a => a != spurNode & rootPath.Contains(a)))
{
var v = Vector <double> .Build.Sparse(Size);
AlteredGraph.AdjacencyMatrix.SetColumn(x, v);
AlteredGraph.AdjacencyMatrix.SetRow(x, v);
}
//build spur path and connect the spur path to the root
List <int> spurPath = new List <int>();
//finding the least cost path may fail due to removing the edges above; just ignore and continue
try { spurPath = AlteredGraph.LeastCostPath(spurNode, destination); }
catch (Exception ex) { break; }
List <int> totalPath = rootPath;
totalPath.AddRange(spurPath.Where(node => node != spurNode).ToList());
PotentialPaths.Insert(new FibonacciHeapNode <List <int>, double>(totalPath, this.PathCost(totalPath)));
}
if (PotentialPaths.IsEmpty())
{
break;
}
ShortestPaths.Add(PotentialPaths.RemoveMin().Data);
}
return(ShortestPaths);
}
public void EmptyHeapDecreaseKey_DoesNothing()
{
FibonacciHeap <int> heap = new FibonacciHeap <int>();
FibonacciNode <int> node = new FibonacciNode <int>(3);
heap.DecreaseKey(node, 2);
NUnit.Framework.Assert.True(heap.IsEmpty());
NUnit.Framework.Assert.AreEqual(default(int), heap.GetMin());
}
public static DijkstraTile[,] Dijkstra(IEnumerable <Point> start, int width, int height, double maxDist, Func <Point, Point, double> length, Func <Point, IEnumerable <Point> > neighbors)
{
var dijkstraMap = new DijkstraTile[width, height];
var nodeMap = new FibonacciHeapNode <DijkstraTile, double> [width, height];
var heap = new FibonacciHeap <DijkstraTile, double>(0);
for (int x = 0; x < width; x++)
{
for (int y = 0; y < height; y++)
{
Point tile = new Point(x, y);
bool isStart = start.Contains(tile);
DijkstraTile dTile = new DijkstraTile(tile, isStart ? 0 : double.PositiveInfinity, isStart ? 0 : double.PositiveInfinity);
var node = new FibonacciHeapNode <DijkstraTile, double>(dTile, dTile.Distance);
dijkstraMap[x, y] = dTile;
nodeMap[x, y] = node;
heap.Insert(node);
}
}
while (!heap.IsEmpty())
{
var node = heap.RemoveMin();
var dTile = node.Data;
if (dTile.Distance >= maxDist)
{
break;
}
foreach (var neighbor in neighbors(dTile.Tile))
{
if (neighbor.X < 0 || neighbor.Y < 0 || neighbor.X >= width || neighbor.Y >= height)
{
continue;
}
var nodeNeighbor = nodeMap[neighbor.X, neighbor.Y];
var dNeighbor = nodeNeighbor.Data;
double newDist = dTile.Distance + length(dTile.Tile, dNeighbor.Tile);
if (newDist < dNeighbor.Distance)
{
dNeighbor.Distance = newDist;
dNeighbor.Previous = dTile;
dNeighbor.MoveDistance = dTile.MoveDistance + 1;
heap.DecreaseKey(nodeNeighbor, dNeighbor.Distance);
}
}
}
return(dijkstraMap);
}
public void UnconsolidatedHeapIsEmpty_False()
{
FibonacciHeap <int> heap = new FibonacciHeap <int>();
IList <int> input = new List <int>()
{
0,
28,
-13,
80
};
foreach (int value in input)
{
heap.Insert(value);
}
NUnit.Framework.Assert.IsFalse(heap.IsEmpty());
}
public List <Tile> FindReachableTiles(Unit unit, Tile center, int ap)
{
List <Tile> tiles = new List <Tile>();
Dictionary <Tile, FibonacciHeapNode <TileMeta> > tileMap = new Dictionary <Tile, FibonacciHeapNode <TileMeta> >();
FibonacciHeap <TileMeta> heap = new FibonacciHeap <TileMeta>();
tileMap.Add(center, heap.Push(new TileMeta(center, 0)));
while (!heap.IsEmpty())
{
TileMeta tileMeta = heap.Pop().Value;
Tile tile = tileMeta.tile;
tiles.Add(tile);
int previousCost = tileMeta.value;
foreach (Vector2Int gridPosition in GetAdjacentGridPositions(tile.x, tile.y))
{
tile = GetTile(gridPosition);
if (IsAccessible(unit, tile))
{
int cost = previousCost + unit.Statistics.CalculateEffectiveFatigue(tile.Cost, FatigueType.Movement);
if (tileMap.ContainsKey(tile))
{
FibonacciHeapNode <TileMeta> node = tileMap[tile];
if (cost < node.Value.value)
{
heap.Decrement(node, new TileMeta(tile, cost));
}
}
else if (cost <= ap)
{
tileMap.Add(tile, heap.Push(new TileMeta(tile, cost)));
}
}
}
}
return(tiles);
}
public void min()
{
var heap = new FibonacciHeap <int, double>(0);
var node = new FibonacciHeapNode <int, double>(-1, 0);
heap.Insert(node);
while (!heap.IsEmpty())
{
node = heap.Min();
heap.RemoveMin();
int n = node.Data;
double timee = node.Key;
if (n == -2)
{
continue;
}
var con = parameter[n];
foreach (var item in con)
{
//ListValueData.Add(item);
int n1 = item.Key; // el node el connected beha
double t1 = item.Value.Key; // el weight 3ala el edge
double oldtime = ans[n1].Value.Key;
double dist = item.Value.Value.Key;
if (t1 + timee < oldtime)
{
var vip = new
KeyValuePair <int, KeyValuePair <double, double> >(n, new KeyValuePair <double, double>(t1 + timee, dist));
ans[n1] = vip;
var node2 = new FibonacciHeapNode <int, double>(n1, t1 + timee);
heap.Insert(node2);
}
}
}
}
public static IDijkstraMap Dijkstra(IEnumerable <Point> start, IEnumerable <Point> end, int width, int height, Rectangle activeArea, double maxDist, ICostMap costMap, IEnumerable <Point> neighbors)
{
Stopwatch stopwatch = Stopwatch.StartNew();
bool hasEnds = end.Any();
HashSet <Point> ends = new HashSet <Point>(end);
FibonacciHeap <DijkstraTile, double> heap = new FibonacciHeap <DijkstraTile, double>(0);
if (activeArea.X < 0)
{
activeArea.X = 0;
}
if (activeArea.Y < 0)
{
activeArea.Y = 0;
}
if (activeArea.Width > width - 1)
{
activeArea.Width = width - 1;
}
if (activeArea.Height > height - 1)
{
activeArea.Height = height - 1;
}
IDijkstraMap dijkstraMap = new DijkstraMap(width, height, heap, start);
int i = 0;
while (!heap.IsEmpty() && (!hasEnds || ends.Count > 0))
{
var node = heap.RemoveMin();
var dTile = node.Data;
if (dTile.Distance >= maxDist)
{
break;
}
if (ends.Contains(dTile.Tile))
{
ends.Remove(dTile.Tile);
}
i++;
foreach (var neighbor in neighbors.Select(o => dTile.Tile + o))
{
if (!activeArea.Contains(neighbor.X, neighbor.Y) /*neighbor.X < 0 || neighbor.Y < 0 || neighbor.X >= width || neighbor.Y >= height*/)
{
continue;
}
var nodeNeighbor = dijkstraMap.GetNode(neighbor.X, neighbor.Y);
var dNeighbor = nodeNeighbor.Data;
double newDist = dTile.Distance + costMap.GetCost(dNeighbor.Tile);
if (newDist < dNeighbor.Distance)
{
dNeighbor.Distance = newDist;
dNeighbor.Previous = dTile;
dNeighbor.MoveDistance = dTile.MoveDistance + 1;
heap.DecreaseKey(nodeNeighbor, dNeighbor.Distance);
}
}
}
Console.WriteLine($"Dijkstra ({i} iterations) took: {stopwatch.ElapsedTicks} ({(float)stopwatch.ElapsedTicks / i})");
return(dijkstraMap);
}
public bool IsEmpty()
{
return(Heap.IsEmpty());
}
public static Path <T> FindPath <T>(INavGrid <T> navGrid, T start, T destination, AccessibilityPredicate IsAccessible) where T : IEquatable <T>
{
Vector2Int startIndices = navGrid.GetGridPosition(start);
Vector2Int destinationIndices = navGrid.GetGridPosition(destination);
if (!IsAccessible(startIndices.x, startIndices.y) || !IsAccessible(destinationIndices.x, destinationIndices.y))
{
return(null);
}
if (start.Equals(destination))
{
return(new Path <T>(start));
}
int length = navGrid.Length;
int width = navGrid.Width;
bool[,] closedList = new bool[length, width];
AStarTile <T>[,] tiles = new AStarTile <T> [length, width];
for (int x = 0; x < length; x++)
{
for (int y = 0; y < width; y++)
{
tiles[x, y].indices = new Vector2Int(x, y);
tiles[x, y].previous = new Vector2Int(-1, -1);
tiles[x, y].f = float.MaxValue;
tiles[x, y].g = float.MaxValue;
tiles[x, y].h = float.MaxValue;
}
}
FibonacciHeap <AStarTile <T> > openList = new FibonacciHeap <AStarTile <T> >();
Dictionary <Vector2Int, FibonacciHeapNode <AStarTile <T> > > openListMap = new Dictionary <Vector2Int, FibonacciHeapNode <AStarTile <T> > >();
tiles[startIndices.x, startIndices.y].indices = startIndices;
tiles[startIndices.x, startIndices.y].previous = startIndices;
tiles[startIndices.x, startIndices.y].f = 0;
tiles[startIndices.x, startIndices.y].g = 0;
tiles[startIndices.x, startIndices.y].h = 0;
openListMap.Add(startIndices, openList.Push(tiles[startIndices.x, startIndices.y]));
while (!openList.IsEmpty())
{
AStarTile <T> current = openList.Pop().Value;
Vector2Int currentIndices = current.indices;
int x = currentIndices.x;
int y = currentIndices.y;
closedList[x, y] = true;
List <Vector2Int> adjacentGridPositions = navGrid.GetAdjacentGridPositions(x, y);
for (int i = 0; i < adjacentGridPositions.Count; i++)
{
Vector2Int neighborIndices = adjacentGridPositions[i];
int xi = neighborIndices.x;
int yi = neighborIndices.y;
if (neighborIndices == destinationIndices)
{
tiles[xi, yi].previous = currentIndices;
LinkedList <T> wayPoints = new LinkedList <T>();
while (neighborIndices != startIndices)
{
wayPoints.AddFirst(navGrid.GetTile(neighborIndices));
neighborIndices = tiles[neighborIndices.x, neighborIndices.y].previous;
}
return(new Path <T>(start, wayPoints));
}
if (closedList[xi, yi] || !IsAccessible(xi, yi))
{
continue;
}
float gNew = current.g + MathUtility.ManhattanDistance(xi, yi, x, y);
float hNew = MathUtility.ManhattanDistance(xi, yi, destinationIndices.x, destinationIndices.y);
float fNew = gNew + hNew;
if (tiles[xi, yi].f < fNew)
{
continue;
}
tiles[xi, yi].previous = currentIndices;
tiles[xi, yi].g = gNew;
tiles[xi, yi].h = hNew;
tiles[xi, yi].f = fNew;
if (!openListMap.ContainsKey(neighborIndices))
{
openListMap.Add(neighborIndices, openList.Push(tiles[xi, yi]));
}
else
{
openList.Decrement(openListMap[neighborIndices], tiles[xi, yi]);
}
}
}
return(null);
}
public void EmptyHeapIsEmpty_True()
{
FibonacciHeap <int> heap = new FibonacciHeap <int>();
NUnit.Framework.Assert.True(heap.IsEmpty());
}
public static List<Path> NearestTerminals(Vertex from, Graph graph, int n)
{
List<Path> foundPaths = new List<Path>();
HashSet<Vertex> visited = new HashSet<Vertex>();
Dictionary<Vertex, FibonacciHeap<int, Vertex>.Node> nodes = new Dictionary<Vertex, FibonacciHeap<int, Vertex>.Node>();
FibonacciHeap<int, Vertex> labels = new FibonacciHeap<int, Vertex>();
Dictionary<Vertex, Edge> comingFrom = new Dictionary<Vertex, Edge>();
if (graph.Terminals.Contains(from))
foundPaths.Add(new Path(from));
// Initialize labels.
foreach (var vertex in graph.Vertices)
{
var node = labels.Add(vertex == from ? 0 : int.MaxValue, vertex);
nodes.Add(vertex, node);
comingFrom.Add(vertex, null);
}
while (!labels.IsEmpty() && foundPaths.Count < n)
{
var currentNode = labels.ExtractMin();
var current = currentNode.Value;
// Consider all edges ending in unvisited neighbours
var edges =
graph.GetEdgesForVertex(current).Where(x => !visited.Contains(x.Other(current)));
// Update labels on the other end.
foreach (var edge in edges)
{
if (currentNode.Key + edge.Cost < nodes[edge.Other(current)].Key)
{
labels.DecreaseKey(nodes[edge.Other(current)], currentNode.Key + edge.Cost);
comingFrom[edge.Other(current)] = edge;
}
}
visited.Add(current);
if (graph.Terminals.Contains(current) && current != from)
{
// Now travel back, to find the actual path
List<Edge> pathEdges = new List<Edge>();
Vertex pathVertex = current;
while (pathVertex != from)
{
pathEdges.Add(comingFrom[pathVertex]);
pathVertex = comingFrom[pathVertex].Other(pathVertex);
}
pathEdges.Reverse();
Path path = new Path(from);
path.Edges.AddRange(pathEdges);
foundPaths.Add(path);
}
}
return foundPaths;
}
/// <summary> /// Whether this queue is empty /// </summary> public bool Empty() => heap.IsEmpty();
/// <summary>
/// Computes the most efficient path in the specified graph between the
/// specified pathfinding nodes using the A* algorithm.
/// </summary>
/// <param name="graph">Pathfinding graph to look at.</param>
/// <param name="start">Starting node.</param>
/// <param name="finish">Finish node.</param>
/// <returns>
/// List of pathfinding nodes representing the shortest path, if there is one,
/// and null otherwise.
/// </returns>
/// <typeparam name="T">Type of the pathfinding nodes.</typeparam>
/// <exception cref="ArgumentNullException">
/// Passed graph or start or finish node is <c>null</c>.
/// </exception>
public static List <T> FindPath <T>(IWeightedGraph <T> graph, T start, T finish) where T : IAStarNode
{
if (graph == null)
{
throw new ArgumentNullException("graph", "The passed pathfinding graph mustn't be null.");
}
if (start == null)
{
throw new ArgumentNullException("start", "The passed start node mustn't be null.");
}
if (finish == null)
{
throw new ArgumentNullException("finish", "The passed finish mustn't be null.");
}
// --- Initialization ---
// Initialize variables to check for the algorithm to terminate.
bool algorithmComplete = false;
bool algorithmAborted = false;
// Initialize list to choose the next node of the path from.
IPriorityQueue <T> openList = new FibonacciHeap <T>();
IPriorityQueueItem <T>[] fibHeapItems = new FibonacciHeapItem <T> [graph.VertexCount];
HashSet <T> dirtyNodes = new HashSet <T>();
// Initialize queue to hold the nodes along the path to the finish.
Queue <T> closedList = new Queue <T>();
// Declare current node to work on in order to calculate the path.
// Declare list to hold the neighbors of the current node.
// Add starting node to open list.
fibHeapItems[start.Index] = openList.Insert(start, 0);
dirtyNodes.Add(start);
start.Discovered = true;
// --- A* Pathfinding Algorithm ---
while ((!algorithmComplete) && (!algorithmAborted))
{
// Get the node with the lowest F score in the open list.
T currentNode = openList.DeleteMin().Item;
// Drop that node from the open list and add it to the closed list.
closedList.Enqueue(currentNode);
currentNode.Visited = true;
// We're done if the target node is added to the closed list.
if (currentNode.Equals(finish))
{
algorithmComplete = true;
break;
}
// Otherwise, get all adjacent nodes.
List <T> neighbors = graph.ListOfAdjacentVertices(currentNode);
// Add all nodes that aren't already on the open or closed list to the open list.
foreach (T node in neighbors)
{
// Ignore nodes in the closed list.
if (!node.Visited)
{
if (!node.Discovered)
{
// The parent node is the previous node on the path to the finish.
node.ParentNode = currentNode;
// The G score of the node is calculated by adding the G score
// of the parent node and the movement cost of the path between
// the node and the current node.
// In other words: The G score of the node is the total cost of the
// path between the starting node and this one.
node.G = node.ParentNode.G + graph.GetEdgeWeight(node, (T)node.ParentNode);
// The H score of the node is calculated by heuristically
// estimating the movement cost from the node to the finish.
// In other words: The H score of the node is the total remaining
// cost of the path between this node and the finish.
node.H = node.EstimateHeuristicMovementCost(finish);
// The F score of the node is calculated by adding the G and H scores.
// In other words: The F score is an indicator that tells whether this
// node should be crossed on the path to the finish, or not.
node.F = node.G + node.H;
// Add to open list.
fibHeapItems[node.Index] = openList.Insert(node, node.F);
dirtyNodes.Add(node);
node.Discovered = true;
}
else
{
// Node is already in open list!
// Check if the new path to this node is a better one.
if (currentNode.G + graph.GetEdgeWeight(node, currentNode) < node.G)
{
// G cost of new path is lower!
// Change parent node to current node.
node.ParentNode = currentNode;
// Recalculate F and G costs.
node.G = node.ParentNode.G + graph.GetEdgeWeight(node, (T)node.ParentNode);
node.F = node.G + node.H;
openList.DecreaseKeyTo(fibHeapItems[node.Index], node.F);
}
}
}
}
// We've failed to find a path if the open list is empty.
if (openList.IsEmpty())
{
algorithmAborted = true;
}
}
// Return the path to the finish, if there is one.
if (algorithmComplete)
{
// Generate path through parent pointers using a stack.
Stack <T> s = new Stack <T>();
T node = finish;
while (!node.Equals(start))
{
s.Push(node);
node = (T)node.ParentNode;
}
// Generate path in right order.
List <T> path = new List <T>();
while (s.Count > 0)
{
path.Add(s.Pop());
}
// Cleanup pathing.
foreach (T dirtyNode in dirtyNodes)
{
dirtyNode.Reset();
}
return(path);
}
else
{
// Cleanup pathing.
foreach (T dirtyNode in dirtyNodes)
{
dirtyNode.Reset();
}
return(null);
}
}
