public T RemoveMin()
{
FibonacciHeapNode <T, TKey> popped = Heap.RemoveMin();
ObjectToHeapNodeMapping.Remove(popped.Data);
return(popped.Data);
}
/// <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);
}
/// <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);
}
}
}
}
}
public List <(TVertex Vertex, EdgeBase <TVertex> EdgeToParent)> FindShortestPathWithDijkstra(TVertex start, TVertex end)
{
var vertexInfoDict = Vertices.Select(vertex => new DijkstraVertexInfo(vertex, vertex == start ? 0 : double.PositiveInfinity))
.Select(info => new FibonacciHeapNode <DijkstraVertexInfo>(info, info.Distance))
.ToDictionary(node => node.Data.Vertex, node => node);
var heap = new FibonacciHeap <DijkstraVertexInfo>();
heap.InsertRange(vertexInfoDict.Values);
DijkstraVertexInfo endInfo = null;
while (vertexInfoDict.Count > 0)
{
var curVertexInfo = heap.RemoveMin().Data;
if (curVertexInfo.Vertex == end)
{
endInfo = curVertexInfo;
break;
}
var neighborsWithEdges = GetNeighborsWithEdges(curVertexInfo.Vertex, ignoreSelfLoops: true);
foreach (var neighborsWithEdge in neighborsWithEdges)
{
if (vertexInfoDict.TryGetValue(neighborsWithEdge.Vertex, out FibonacciHeapNode <DijkstraVertexInfo> neighborNode))
{
var alt = curVertexInfo.Distance + neighborsWithEdge.Edge.Weight ?? 1;
var neighborInfo = neighborNode.Data;
if (alt < neighborInfo.Distance)
{
heap.DecreaseKey(neighborNode, alt);
neighborInfo.Distance = alt;
neighborInfo.Parent = curVertexInfo;
neighborInfo.ParentEdge = neighborsWithEdge.Edge;
}
}
}
}
if (endInfo != null)
{
var reslut = new List <(TVertex Vertex, EdgeBase <TVertex> EdgeToParent)>();
var curVertex = endInfo;
while (curVertex != null)
{
reslut.Add((curVertex.Vertex, curVertex.ParentEdge));
curVertex = curVertex.Parent;
}
reslut.Reverse();
return(reslut);
}
return(null);
}
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 static List <Node> Dijkstras(Graph graph, Vector2 sourceVect, Vector2 goalVect)
{
Node source = null;
Node goal = null;
Node expandedNode;
List <Node> finalizedSet = new List <Node>();
FibonacciHeap <Node, float> prioQueue = new FibonacciHeap <Node, float>(0);
foreach (Node node in graph.Nodes)
{
node.Distance = 1f / 0f;
node.SetPredecessor(graph.Nodes, null);
if (node.MapPos == sourceVect)
{
source = node;
}
else
{
prioQueue.Insert(new FibonacciHeapNode <Node, float>(node, node.Distance));
}
if (node.MapPos == goalVect)
{
goal = node;
}
}
source.Distance = 0;
prioQueue.Insert(new FibonacciHeapNode <Node, float>(source, source.Distance));
while (prioQueue.Size() != 0)
{
expandedNode = prioQueue.RemoveMin().Data;
finalizedSet.Add(expandedNode);
foreach (Node node in graph.Adj[expandedNode])
{
if (node.Distance > expandedNode.Distance + 1)
{
node.Distance = expandedNode.Distance + 1;
node.SetPredecessor(graph.Nodes, expandedNode);
}
}
}
return(BuildPath(graph, source, goal));
}
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 TElement Pop()
{
return(heap.RemoveMin().Data);
}
/// <summary>
/// Removes and returns the cheapest element
/// </summary>
/// <returns></returns>
public virtual Element Pop()
{
return(heap.RemoveMin().Data);
}
public TElement Pop()
{
fibonacciNodeDic.Remove(heap.Min().Data);
return(heap.RemoveMin().Data);
}
public T RemoveMin()
{
return(Heap.RemoveMin().Data);
}
