| | | 1 | | using System; |
| | | 2 | | using System.Collections.Generic; |
| | | 3 | | using System.Linq; |
| | | 4 | | using Itinero.Network; |
| | | 5 | | using Itinero.Network.Enumerators.Edges; |
| | | 6 | | using Itinero.Routing.DataStructures; |
| | | 7 | | |
| | | 8 | | namespace Itinero.Routing.Flavours.Dijkstra.Bidirectional; |
| | | 9 | | |
| | | 10 | | internal abstract class DijkstraAlgorithm |
| | | 11 | | { |
| | 54 | 12 | | private readonly PathTree _tree = new(); |
| | 54 | 13 | | private readonly Dictionary<VertexId, (uint p, double cost)> _settled = []; |
| | 54 | 14 | | private readonly BinaryHeap<(uint pointer, VertexId vertex)> _heap = new(); |
| | | 15 | | protected readonly RoutingNetworkEdgeEnumerator _enumerator; |
| | | 16 | | protected readonly RoutingNetwork _network; |
| | | 17 | | |
| | 54 | 18 | | protected DijkstraAlgorithm(RoutingNetwork network) |
| | 54 | 19 | | { |
| | 54 | 20 | | _network = network; |
| | | 21 | | |
| | 54 | 22 | | _enumerator = network.GetEdgeEnumerator(); |
| | 54 | 23 | | } |
| | | 24 | | |
| | 136 | 25 | | internal RoutingNetwork RoutingNetwork => _network; |
| | | 26 | | |
| | | 27 | | protected abstract bool OnQueued(uint visit, EdgeId edge, (double cost, double turnCost) edgeCost, VertexId vertex, |
| | | 28 | | |
| | | 29 | | protected abstract bool OnSettled(uint visit, VertexId vertex, double cost); |
| | | 30 | | |
| | | 31 | | protected abstract (double cost, double turnCost) GetCost(RoutingNetworkEdgeEnumerator edgeEnumerator, |
| | | 32 | | PreviousEdgeEnumerable previousEdges); |
| | | 33 | | |
| | | 34 | | protected abstract (double cost, double turnCost) GetCost(RoutingNetworkEdgeEnumerator edgeEnumerator, |
| | | 35 | | IEnumerable<(EdgeId edge, byte? turn)> previousEdges); |
| | | 36 | | |
| | | 37 | | internal bool TryGetVisit(VertexId vertex, out (uint p, double cost) visit) |
| | 1936 | 38 | | { |
| | 1936 | 39 | | return _settled.TryGetValue(vertex, out visit); |
| | 1936 | 40 | | } |
| | | 41 | | |
| | | 42 | | internal void Clear() |
| | 118 | 43 | | { |
| | 118 | 44 | | _heap.Clear(); |
| | 118 | 45 | | _tree.Clear(); |
| | 118 | 46 | | _settled.Clear(); |
| | 118 | 47 | | } |
| | | 48 | | |
| | | 49 | | internal (VertexId vertex, EdgeId edge, bool forward, byte? head, uint previousPointer) GetVisit(uint pointer) |
| | 129 | 50 | | { |
| | 129 | 51 | | return _tree.GetVisit(pointer); |
| | 129 | 52 | | } |
| | | 53 | | |
| | | 54 | | internal uint Push(EdgeId edgeId, bool forward, double cost) |
| | 198 | 55 | | { |
| | 198 | 56 | | if (!_enumerator.MoveTo(edgeId, forward)) throw new Exception($"Edge not found!"); |
| | | 57 | | |
| | 198 | 58 | | var v = _tree.AddVisit(_enumerator, uint.MaxValue); |
| | 198 | 59 | | _heap.Push((v, _enumerator.Head), cost); |
| | 198 | 60 | | return v; |
| | 198 | 61 | | } |
| | | 62 | | |
| | | 63 | | internal (uint pointer, (VertexId vertex, EdgeId edge, bool forward, byte? head, uint previousPointer) visit, double |
| | 624 | 64 | | { |
| | 624 | 65 | | var currentEntry = _heap.Pop(out var currentCost); |
| | 1258 | 66 | | while (!_settled.TryAdd(currentEntry.vertex, (currentEntry.pointer, currentCost))) |
| | 640 | 67 | | { |
| | 640 | 68 | | if (_heap.Count == 0) |
| | 6 | 69 | | { |
| | 6 | 70 | | currentEntry = (uint.MaxValue, default); |
| | 6 | 71 | | break; |
| | | 72 | | } |
| | | 73 | | |
| | 634 | 74 | | currentEntry = _heap.Pop(out currentCost); |
| | 634 | 75 | | } |
| | | 76 | | |
| | 624 | 77 | | if (currentEntry.pointer == uint.MaxValue) |
| | 6 | 78 | | { |
| | 6 | 79 | | return (uint.MaxValue, default, currentCost); |
| | | 80 | | } |
| | | 81 | | |
| | | 82 | | // only call GetVisit after the settled check passes. |
| | 618 | 83 | | var currentVisit = _tree.GetVisit(currentEntry.pointer); |
| | 618 | 84 | | return (currentEntry.pointer, currentVisit, currentCost); |
| | 624 | 85 | | } |
| | | 86 | | |
| | | 87 | | internal IEnumerable<(EdgeId edge, byte? turn)> GetPreviousEdges(uint pointer, int maxCount = 16) |
| | 30 | 88 | | { |
| | 30 | 89 | | using var previous = _tree.GetPreviousEdges(pointer).GetEnumerator(); |
| | 65 | 90 | | while (previous.MoveNext()) |
| | 35 | 91 | | { |
| | 35 | 92 | | if (maxCount <= 0) yield break; |
| | 35 | 93 | | maxCount--; |
| | | 94 | | |
| | 35 | 95 | | yield return previous.Current; |
| | 35 | 96 | | } |
| | 30 | 97 | | } |
| | | 98 | | |
| | | 99 | | internal bool CanTurn(VertexId vertex, |
| | | 100 | | IReadOnlyList<(EdgeId edge, byte? turn)> previousEdges, IReadOnlyList<EdgeId> nextEdges) |
| | 15 | 101 | | { |
| | 15 | 102 | | if (nextEdges.Count == 0) throw new Exception("Not a turn"); |
| | 15 | 103 | | if (previousEdges.Count == 0) throw new Exception("Not a turn"); |
| | | 104 | | |
| | | 105 | | // no U-turns |
| | 16 | 106 | | if (nextEdges[0] == previousEdges[0].edge) return false; |
| | | 107 | | |
| | | 108 | | // check neighbours. |
| | 14 | 109 | | var nextChecked = 0; |
| | 14 | 110 | | var edges = previousEdges.ToList(); |
| | 20 | 111 | | while (nextChecked < nextEdges.Count) |
| | 14 | 112 | | { |
| | 14 | 113 | | var nextEdge = nextEdges[nextChecked]; |
| | 14 | 114 | | var edgeFound = false; |
| | | 115 | | |
| | 14 | 116 | | if (!_enumerator.MoveTo(vertex)) return false; |
| | 16 | 117 | | while (_enumerator.MoveNext()) |
| | 16 | 118 | | { |
| | | 119 | | // filter out if U-turns or visits on the same edge. |
| | 16 | 120 | | var neighbourEdge = _enumerator.EdgeId; |
| | 18 | 121 | | if (neighbourEdge != nextEdge) continue; |
| | | 122 | | |
| | | 123 | | // gets the cost of the current edge. |
| | 14 | 124 | | var (_, turnCost) = this.GetCost(_enumerator, edges); |
| | | 125 | | |
| | 22 | 126 | | if (turnCost is >= double.MaxValue or < 0) return false; |
| | | 127 | | |
| | 6 | 128 | | edges.Add((_enumerator.EdgeId, _enumerator.HeadOrder)); |
| | 6 | 129 | | vertex = _enumerator.Head; |
| | 6 | 130 | | edgeFound = true; |
| | 6 | 131 | | break; |
| | | 132 | | } |
| | | 133 | | |
| | 6 | 134 | | if (!edgeFound) return false; |
| | | 135 | | |
| | 6 | 136 | | nextChecked++; |
| | 6 | 137 | | } |
| | | 138 | | |
| | | 139 | | // if all edges are checked, the turn is possible. |
| | 12 | 140 | | if (nextChecked >= nextEdges.Count) return true; |
| | | 141 | | |
| | | 142 | | // target edge not found as neighbour. |
| | 0 | 143 | | return false; |
| | 15 | 144 | | } |
| | | 145 | | |
| | | 146 | | internal bool Step(uint pointer, (VertexId vertex, EdgeId edge, bool forward, byte? head, uint previousPointer) visi |
| | 618 | 147 | | { |
| | | 148 | | // log settled and see if we need to continue. |
| | 618 | 149 | | if (!this.OnSettled(pointer, visit.vertex, cost)) return false; |
| | | 150 | | |
| | | 151 | | // check neighbours. |
| | 618 | 152 | | if (!_enumerator.MoveTo(visit.vertex)) return true; |
| | 2673 | 153 | | while (_enumerator.MoveNext()) |
| | 2055 | 154 | | { |
| | | 155 | | // filter out if U-turns or visits on the same edge. |
| | 2055 | 156 | | var neighbourEdge = _enumerator.EdgeId; |
| | 2673 | 157 | | if (neighbourEdge == visit.edge) continue; |
| | | 158 | | |
| | | 159 | | // gets the cost of the current edge. |
| | 1437 | 160 | | var (neighbourCost, turnCost) = this.GetCost(_enumerator, new PreviousEdgeEnumerable(_tree, pointer)); |
| | | 161 | | |
| | | 162 | | // ignore if cost is 0 or infinite. |
| | 1541 | 163 | | if (neighbourCost is >= double.MaxValue or <= 0) continue; |
| | 1348 | 164 | | if (turnCost is >= double.MaxValue or < 0) continue; |
| | | 165 | | |
| | | 166 | | // check if the vertex has to be queued. |
| | 1318 | 167 | | var totalCost = neighbourCost + cost + turnCost; |
| | | 168 | | |
| | | 169 | | // add visit if not added yet. |
| | 1318 | 170 | | var neighbourPointer = _tree.AddVisit(_enumerator, pointer); |
| | | 171 | | |
| | | 172 | | // call the on queued method and allow checking for stopping conditions. |
| | 1318 | 173 | | if (!this.OnQueued(neighbourPointer, _enumerator.EdgeId, (neighbourCost, turnCost), _enumerator.Head, totalC |
| | | 174 | | |
| | | 175 | | // add visit to heap. |
| | 1318 | 176 | | _heap.Push((neighbourPointer, _enumerator.Head), totalCost); |
| | 1318 | 177 | | } |
| | | 178 | | |
| | 618 | 179 | | return true; |
| | 618 | 180 | | } |
| | | 181 | | } |