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); }