public void ConsolidatedHeapDecreaseKey_CorrectCuts()
{
FibonacciHeap <int> heap = new FibonacciHeap <int>();
IList <FibonacciNode <int> > input = new List <FibonacciNode <int> >()
{
new FibonacciNode <int>(0),
new FibonacciNode <int>(28),
new FibonacciNode <int>(-13),
new FibonacciNode <int>(80),
new FibonacciNode <int>(3),
new FibonacciNode <int>(7),
new FibonacciNode <int>(-7),
new FibonacciNode <int>(42),
new FibonacciNode <int>(-11),
new FibonacciNode <int>(12)
};
IList <int> expectedElementOrder = new List <int>()
{
7,
-8,
-11,
12,
-42,
80,
-1,
-3,
0
};
foreach (FibonacciNode <int> value in input)
{
heap.Insert(value);
}
heap.ExtractMin();
// A decrease key with no structural changes
heap.DecreaseKey(input[6], -8);
// Normal cuts with parent marked
heap.DecreaseKey(input[7], -42);
heap.DecreaseKey(input[4], -1);
// Double cascading cut
heap.DecreaseKey(input[1], -3);
IList <int> result = heap.GetAllValues();
for (int i = 0; i < expectedElementOrder.Count; ++i)
{
NUnit.Framework.Assert.IsTrue(expectedElementOrder[i] == result[i]);
}
NUnit.Framework.Assert.IsTrue(heap.GetMin() == -42);
}
public static int Dijkstra(Vertex from, Vertex to, Graph graph)
{
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>();
// Initialize labels.
foreach (var vertex in graph.Vertices)
{
var n = labels.Add(vertex == from ? 0 : int.MaxValue, vertex);
nodes.Add(vertex, n);
}
int currentLabel = int.MaxValue;
while (!visited.Contains(to))
{
var currentNode = labels.ExtractMin();
var current = currentNode.Value;
currentLabel = currentNode.Key;
// 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);
}
visited.Add(current);
}
return currentLabel;
}
/// <summary>
/// 更新结点的值，新值更小才能成功更新
/// </summary>
public void Decrease(TElement item, TPriority priority)
{
if (fibonacciNodeDic.ContainsKey(item))
{
heap.DecreaseKey(fibonacciNodeDic[item], priority);
}
}
public static void DecreaseKey_EmptyHeap_ThrowsCorrectException()
{
var heap = new FibonacciHeap <int>();
var item = new FHeapNode <int>(1);
Assert.Throws <ArgumentException>(() => heap.DecreaseKey(item, 0));
}
public static void DecreaseKey_TryIncreaseKey_ThrowsCorrectException()
{
var heap = new FibonacciHeap <int>();
var item = heap.Push(1);
Assert.Throws <InvalidOperationException>(() => heap.DecreaseKey(item, 2));
}
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 void DecreaseKey_NonEmptyHeap_PreservesHeapStructure()
{
var heap = new FibonacciHeap <int>();
for (var i = 11; i < 20; i++)
{
heap.Push(i);
}
var item = heap.Push(10);
for (var i = 0; i < 10; i++)
{
heap.Push(i);
}
var bigItem = heap.Push(20);
heap.DecreaseKey(item, -1);
Assert.AreEqual(heap.Pop(), -1);
var currentVal = -1;
for (var i = 0; i < 10; i++)
{
var newVal = heap.Pop();
Assert.True(currentVal < newVal);
currentVal = newVal;
}
heap.DecreaseKey(bigItem, -1);
Assert.AreEqual(heap.Pop(), -1);
currentVal = -1;
for (var i = 0; i < 9; i++)
{
var newVal = heap.Pop();
Assert.True(currentVal < newVal);
currentVal = newVal;
}
}
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 void UnconsolidatedHeapDecreaseKey_NoException()
{
FibonacciHeap <int> heap = new FibonacciHeap <int>();
IList <int> input = new List <int>()
{
0,
28,
};
heap.DecreaseKey(null, 3);
}
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 Path DijkstraPath(Vertex from, Vertex to, Graph graph)
{
HashSet<Vertex> visited = new HashSet<Vertex>();
Dictionary<Vertex, FibonacciHeap<int, Vertex>.Node> nodes =
new Dictionary<Vertex, FibonacciHeap<int, Vertex>.Node>();
Dictionary<Vertex, Edge> comingFrom = new Dictionary<Vertex, Edge>();
FibonacciHeap<int, Vertex> labels = new FibonacciHeap<int, Vertex>();
// Initialize labels.
foreach (var vertex in graph.Vertices)
{
var n = labels.Add(vertex == from ? 0 : int.MaxValue, vertex);
nodes.Add(vertex, n);
comingFrom.Add(vertex, null);
}
while (!visited.Contains(to))
{
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);
}
// Now travel back, to find the actual path
List<Edge> pathEdges = new List<Edge>();
Vertex pathVertex = to;
while (pathVertex != from)
{
pathEdges.Add(comingFrom[pathVertex]);
pathVertex = comingFrom[pathVertex].Other(pathVertex);
}
pathEdges.Reverse();
Path path = new Path(from);
path.Edges.AddRange(pathEdges);
return path;
}
private void ExpandNode(AStarNode node)
{
ISet <ILevelTile> neighbours = graphProvider.GetConnectedNavPoints(node.tile);
foreach (ILevelTile neighbour in neighbours)
{
// Obtain node for neighbour point
if (tilesToNodes.TryGetValue(neighbour, out AStarNode neighbourNode) == false)
{
neighbourNode = new AStarNode(neighbour, heuristic.GetDistance(neighbour.CenterPos, target.CenterPos));
tilesToNodes.Add(neighbour, neighbourNode);
}
if (closedNodes.Contains(neighbourNode))
{
continue;
}
float newCost = node.cost + Vector2.Distance(node.tile.CenterPos, neighbour.CenterPos);
if (openNodes.Contains(neighbourNode))
{
if (newCost >= neighbourNode.cost)
{
continue;
}
}
neighbourNode.predecessor = node;
neighbourNode.cost = newCost;
float fVal = neighbourNode.cost + neighbourNode.heuristicValue;
if (openNodes.Contains(neighbourNode))
{
openNodes.DecreaseKey(neighbourNode, fVal);
}
else
{
neighbourNode.key = fVal;
openNodes.Insert(neighbourNode);
}
}
}
public void UnconsolidatedHeapDecreaseKeyMinSame_CorrectValue()
{
FibonacciNode <int> node = new FibonacciNode <int>(-3);
FibonacciHeap <int> heap = new FibonacciHeap <int>();
IList <int> input = new List <int>()
{
0,
28,
-13,
80
};
heap.Insert(node);
foreach (int value in input)
{
heap.Insert(value);
}
heap.DecreaseKey(node, -4);
NUnit.Framework.Assert.IsTrue(heap.GetMin() == -13);
}
public void TestDecreaseKey()
{
// Build heap
FibonacciHeap <int, double> heap = new FibonacciHeap <int, double>();
heap.Insert(0, 0.9);
heap.Insert(1, 17.9);
heap.Insert(2, 0.333);
heap.Insert(3, 2.7);
heap.Insert(4, 42);
// Force tree consolidation by removing one node
heap.ExtractMinimum();
// Decrease key and check result
heap.DecreaseKey(3, 0.5);
Assert.AreEqual(3, heap.Minimum);
Assert.AreEqual(4, heap.Count);
// Extract min element again
heap.ExtractMinimum();
Assert.AreEqual(0, heap.Minimum);
}
public void TestFibonacciHeap()
{
FibonacciHeap <int, double> heap = new FibonacciHeap <int, double>();
heap.Insert(0, double.MaxValue);
heap.Insert(1, double.MaxValue);
heap.Insert(2, double.MaxValue);
heap.Insert(3, double.MaxValue);
heap.Insert(4, double.MaxValue);
// Starting point: vertex 0
heap.DecreaseKey(0, 0);
Assert.AreEqual(0, heap.ExtractMinimum());
// Update edge costs from vertex 0
heap.DecreaseKey(1, 0.2);
heap.DecreaseKey(2, 0.1);
heap.DecreaseKey(3, 0.3);
// Remove vertex 2
Assert.AreEqual(2, heap.ExtractMinimum());
// Update edge costs from vertex 2
heap.DecreaseKey(1, 0.1);
// Remove vertex 1
Assert.AreEqual(1, heap.ExtractMinimum());
// Update edge costs from vertex 1
heap.DecreaseKey(3, 0.2);
heap.DecreaseKey(4, 1.0);
// Remove last verteces
Assert.AreEqual(3, heap.ExtractMinimum());
Assert.AreEqual(4, heap.ExtractMinimum());
Assert.AreEqual(true, heap.Empty);
}
public void DecreaseKey(T value, TKey key)
{
var node = HeapLookup[value];
Heap.DecreaseKey(node, key);
}
private void ProcessData(Tuple <Stack <byte>, Stack <Stack <int> > > data)
{
var sw = Stopwatch.StartNew();
var commands = data.Item1;
var arguments = data.Item2;
int argOffset = 0;
int listOffset = 0;
// Prepare the heap
var heap = new FibonacciHeap <int, int>();
int insertCount = GetNextArg(ref argOffset, arguments, ref listOffset);
NodeItem <int, int>[] insertedNodes = new NodeItem <int, int> [insertCount];
int deleteDepthCount = 0;
int deleteCount = 0;
// Do insert commands
int id = 0, key = 0;
NodeItem <int, int> node;
foreach (byte command in commands)
{
if (command != DelKey)
{
id = GetNextArg(ref argOffset, arguments, ref listOffset);
key = GetNextArg(ref argOffset, arguments, ref listOffset);
}
switch (command)
{
case InsKey:
node = heap.Add(key, id);
Debug.Assert(insertedNodes[id] == null);
insertedNodes[id] = node;
break;
case DelKey:
node = heap.PeekMin();
Debug.Assert(insertedNodes[node.Value] != null);
insertedNodes[node.Value] = null;
heap.DeleteMin();
deleteDepthCount += heap.LastConsolidateDepth;
deleteCount++;
break;
case DecKey:
node = insertedNodes[id];
if (node == null || key > node.Key)
{
break;
}
heap.DecreaseKey(node, key);
break;
}
}
// Cleanup and store the measurements
//heap.Clear(); // Disassembles tree node pointers .... not a good idea with a GC...
sw.Stop();
float avgDeleteDepthCount = 0;
if (deleteCount > 0)
{
avgDeleteDepthCount = deleteDepthCount / (float)deleteCount;
}
lock (_results)
_results.Add(deleteCount, avgDeleteDepthCount);
Interlocked.Increment(ref _currentJobsDone);
Log("{0}/{1} done/waiting :: {2:F} sec :: {3} inserts :: {4}/{5:F} deletes/delete depth average",
_currentJobsDone,
Buffer.WaitingItemCount,
sw.ElapsedMilliseconds * 0.001,
insertCount,
deleteCount,
avgDeleteDepthCount);
}
public override IPath FindPath(Grid grid, Location start, Location goal)
{
OnStarted();
Statistics.Reset();
Statistics.TotalGridNodes = grid.Width * grid.Height;
Statistics.StartTimer();
if (!ValidateGoal(grid, goal))
{
var p = new Path(start, start);
p.PushBack(start);
OnPathNotFound();
return(p);
}
var openList = new FibonacciHeap <uint, Location>(0);
var isClosed = new bool[grid.Width, grid.Height];
var soFarCost = new uint[grid.Width, grid.Height];
var parents = new Location?[grid.Width, grid.Height];
var queueNode = new IPair <uint, Location> [grid.Width, grid.Height];
var hotspot = start;
queueNode[start.X, start.Y] = openList.Push(0, hotspot);
var adjacents = new Location[8];
while (hotspot != goal)
{
Statistics.UpdateMaximumOpenNodes((uint)openList.Count);
if (openList.Count <= 0)
{
OnPathNotFound();
return(null);
}
hotspot = openList.Pop().Value;
isClosed[hotspot.X, hotspot.Y] = true;
Statistics.AddClosedNode();
SetAdjacentArray(adjacents, hotspot);
for (var i = 0; i < adjacents.Length; i++)
{
var pos = adjacents[i];
if (ProbeMode == EProbeMode.FourDirections &&
pos.X != hotspot.X &&
pos.Y != hotspot.Y)
{
continue;
}
if (grid.InBounds(pos) &&
hotspot != pos &&
grid[pos] &&
!isClosed[pos.X, pos.Y])
{
if (!HasCornerBetween(grid, hotspot, pos) &&
!HasDiagonalWall(grid, hotspot, pos))
{
if (queueNode[pos.X, pos.Y] == null)
{
parents[pos.X, pos.Y] = hotspot;
var scoreH = CalculateHeuristicCost(pos, goal);
var scoreG = soFarCost[hotspot.X, hotspot.Y] + CalculateDistance(pos, hotspot);
var score = scoreH + scoreG;
queueNode[pos.X, pos.Y] = openList.Push(score, pos);
soFarCost[pos.X, pos.Y] = scoreG;
Statistics.AddOpenedNode();
}
else
{
if (!parents[pos.X, pos.Y].HasValue)
{
continue;
}
var currentParent = parents[pos.X, pos.Y].Value;
var currentScoreG = soFarCost[currentParent.X, currentParent.Y]
+ CalculateDistance(pos, currentParent);
var newScoreG = soFarCost[hotspot.X, hotspot.Y]
+ CalculateDistance(pos, hotspot);
if (newScoreG < currentScoreG)
{
parents[pos.X, pos.Y] = hotspot;
soFarCost[pos.X, pos.Y] = newScoreG;
var score = newScoreG + CalculateHeuristicCost(pos, goal);
openList.DecreaseKey(queueNode[pos.X, pos.Y], score);
}
}
}
}
OnIteration();
Statistics.AddIteration();
}
}
Statistics.PathCost = soFarCost[hotspot.X, hotspot.Y];
var inverter = new Stack <Location>();
Location?aux = hotspot;
while (aux.HasValue)
{
inverter.Push(aux.Value);
aux = parents[aux.Value.X, aux.Value.Y];
}
var path = new Path(start, goal);
while (inverter.Count > 0)
{
path.PushBack(inverter.Pop());
}
OnPathFound();
Statistics.StopTimer();
Statistics.PathLength = path.Size;
return(path);
}
public void DecreasePriority(T data, TKey priority)
{
Heap.DecreaseKey(ObjectToHeapNodeMapping[data], priority);
}
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 static void DijkstraBetweenPoints(LiveWireGraph graph, double[,] distanceMap, out List <Point> path, out List <int> segmentIndices, out TargetPathPoint pathIndex, Dictionary <Int32Point, int> pathPoints, params Point[] points)
{
// Which mode are we operating in
bool toPathMode = pathPoints != null;
// Initialisations
pathIndex = new TargetPathPoint(default(Int32Point), -1);
Int32Point currentPoint = (Int32Point)points.First();
int width = graph.Width;
int height = graph.Height;
int nodeCount = width * height;
double[] distances = new double[nodeCount];
segmentIndices = new List <int>();
bool[] visited = new bool[nodeCount];
Int32Point?[][] previous = new Int32Point?[width][];
for (int i = 0; i < width; i++)
{
previous[i] = new Int32Point?[height];
}
DateTime startTime = DateTime.Now;
List <Point> outputPoints = null;
// Loop for one extra in paths mode
int pointsLoopLength = toPathMode ? points.Length + 1 : points.Length;
for (int pointIndex = 1; pointIndex < pointsLoopLength; pointIndex++)
{
// Loop Initialisations
Int32Point targetPoint; // = (Int32Point)points[pointIndex];
bool toPaths = false;
if (!toPathMode || pointIndex < points.Length)
{
targetPoint = (Int32Point)points[pointIndex];
}
else
{
targetPoint = default(Int32Point);
toPaths = true;
}
FibonacciHeap <double, Int32Point> unvisitedheap = new FibonacciHeap <double, Int32Point>();
Node <double, Int32Point>[] nodeindex = new Node <double, Int32Point> [width * height];
for (int x = 0; x < nodeCount; x++)
{
distances[x] = double.MaxValue;
visited[x] = false;
}
for (int x = 0; x < width; x++)
{
for (int y = 0; y < height; y++)
{
previous[x][y] = null;
}
}
distances[currentPoint.Y * width + currentPoint.X] = 0;
Int32Point pN = new Int32Point(currentPoint.X, currentPoint.Y);
Node <double, Int32Point> pixelNode = new Node <double, Int32Point>(0, pN);
unvisitedheap.Insert(pixelNode);
nodeindex[currentPoint.Y * width + currentPoint.X] = pixelNode;
// Dijkstra's Algorithm between sourcePoint and nextPoint
while (unvisitedheap.Count > 0)
{
// Get next priority point u
Node <double, Int32Point> n = unvisitedheap.Minimum();
unvisitedheap.RemoveMinimum();
Int32Point u = n.value;
int uIndex = u.Y * width + u.X;
// if dist[u] == infinity then break
if (distances[uIndex] == double.MaxValue)
{
break;
}
if (!toPathMode)
{
// if u == target we are done
if (u.X == targetPoint.X && u.Y == targetPoint.Y)
{
break;
}
}
else
{
if (!toPaths)
{
if (u.X == targetPoint.X && u.Y == targetPoint.Y)
{
break;
}
}
else
{
// Search hash set rather than target point
if (pathPoints.ContainsKey(u))
{
pathIndex = new TargetPathPoint(u, pathPoints[u]);
break;
}
}
}
// for each possible neighbour v
for (int i = 0; i < 8; i++)
{
double distance;
Int32Point?nV = GetNeighbour8(u, i, graph, out distance);
if (nV == null)
{
continue;
}
Int32Point v = nV.Value;
int vIndex = v.Y * width + v.X;
if (visited[vIndex])
{
continue;
}
double newDistance = distances[uIndex] + distance;
double remainingDistance = (0.03 * Math.Sqrt(Math.Pow(v.X - targetPoint.X, 2.0) + Math.Pow(v.Y - targetPoint.Y, 2.0))) + 2 * (Math.Pow(distanceMap[v.X, v.Y], 2));
if (newDistance < distances[vIndex])
{
// Adjust V and re-prioritise
Node <double, Int32Point> vNode = nodeindex[vIndex];
if (vNode == null)
{
Int32Point p = new Int32Point(v.X, v.Y);
vNode = new Node <double, Int32Point>(newDistance + remainingDistance, p);
unvisitedheap.Insert(vNode);
nodeindex[vIndex] = vNode;
distances[vIndex] = newDistance;
previous[v.X][v.Y] = u;
}
else
{
distances[vIndex] = newDistance;
previous[v.X][v.Y] = u;
unvisitedheap.DecreaseKey(vNode, newDistance + remainingDistance);
}
}
}
visited[uIndex] = true;
}
// Finalisation
List <Point> shortestPathPoints = new List <Point>();
Point f = default(Point);
if (!toPathMode)
{
f = (Point)targetPoint;
}
else
{
if (toPaths)
{
f = (Point)pathIndex.Point;
}
else
{
f = (Point)targetPoint;
}
}
shortestPathPoints.Add(f);
int fIndex = (int)f.Y * width + (int)f.X;
while (previous[(int)f.X][(int)f.Y] != null)
{
shortestPathPoints.Insert(0, (Point)previous[(int)f.X][(int)f.Y]);
f = (Point)previous[(int)f.X][(int)f.Y];
fIndex = (int)f.Y * width + (int)f.X;
}
// Append to outputPoints
if (outputPoints == null)
{
outputPoints = shortestPathPoints;
}
else
{
segmentIndices.Add(outputPoints.Count);
foreach (Point p in shortestPathPoints)
{
outputPoints.Add(p);
}
}
// Preparation for next loop
if (!toPathMode)
{
currentPoint = (Int32Point)points[pointIndex];
}
else if (!toPaths)
{
currentPoint = (Int32Point)points[pointIndex];
}
}
path = outputPoints;
}
static void TestCase5()
{
var root1 = Node(7)
.AddChild(Node(24)
.AddChild(Node(26).Marked()
.AddChild(Node(35)))
.AddChild(Node(46)))
.AddChild(Node(17)
.AddChild(Node(30)))
.AddChild(Node(23));
var root2 = Node(18).Marked()
.AddChild(Node(21)
.AddChild(Node(52)))
.AddChild(Node(39).Marked());
var root3 = Node(38).AddChild(Node(41));
var heap = new FibonacciHeap<int>(int.MinValue, Comparer<int>.Default, new[] {root1, root2, root3});
var node1 = root1.Children.ElementAt(0).Children.ElementAt(1);
var node2 = root1.Children.ElementAt(0).Children.ElementAt(0).Children.ElementAt(0);
heap.DecreaseKey(node1, 15);
heap.DecreaseKey(node2, 5);
var expRoots = new[]
{
15, 5, 26, 24, 7, 18, 38
};
var actualRoots = heap.Roots.Select(x => x.Data).ToList();
Assert(expRoots.Length == actualRoots.Count);
for (var i = 0; i < expRoots.Length; i++)
Assert(actualRoots.Contains(expRoots[i]));
}
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;
}
public static Dictionary<Vertex, Path> DijkstraPathToAll(Vertex from, Graph graph, bool onlyTerminals)
{
Dictionary<Vertex, Edge> comingFrom = new Dictionary<Vertex, Edge>();
HashSet<Vertex> visited = new HashSet<Vertex>();
Dictionary<Vertex, FibonacciHeap<int, Vertex>.Node> nodes =
new Dictionary<Vertex, FibonacciHeap<int, Vertex>.Node>();
Dictionary<Vertex, Path> paths = new Dictionary<Vertex, Path>();
FibonacciHeap<int, Vertex> labels = new FibonacciHeap<int, Vertex>();
// Initialize labels.
foreach (var vertex in graph.Vertices)
{
var n = labels.Add(vertex == from ? 0 : int.MaxValue, vertex);
nodes.Add(vertex, n);
}
while (paths.Count < (onlyTerminals ? graph.Terminals.Count - 1 : graph.NumberOfVertices - 1))
{
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 (current != from && (!onlyTerminals || graph.Terminals.Contains(current)))
{
// Travel back the 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);
paths[current] = path;
}
}
return paths;
}
public void decreaseKey(FibNode node, int newPriority)
{
heap.DecreaseKey(node, newPriority);
}