| | | 1 | | using System; |
| | | 2 | | using System.Collections.Generic; |
| | | 3 | | using System.Linq; |
| | | 4 | | using System.Threading; |
| | | 5 | | using System.Threading.Tasks; |
| | | 6 | | using Itinero.Network; |
| | | 7 | | using Itinero.Network.Enumerators.Edges; |
| | | 8 | | using Itinero.Routes.Paths; |
| | | 9 | | using Itinero.Routing.Costs; |
| | | 10 | | using Itinero.Snapping; |
| | | 11 | | |
| | | 12 | | namespace Itinero.Routing.Flavours.Dijkstra.Bidirectional; |
| | | 13 | | |
| | | 14 | | |
| | | 15 | | internal class BidirectionalDijkstra |
| | | 16 | | { |
| | | 17 | | private readonly RoutingNetwork _routingNetwork; |
| | | 18 | | private readonly BidirectionalDijkstraForward _forward; |
| | | 19 | | private readonly BidirectionalDijkstraBackward _backward; |
| | | 20 | | private ICostFunction _costFunction; |
| | | 21 | | internal (uint forward, uint backward, double cost, Path? singleHopPath) _best; |
| | | 22 | | |
| | 27 | 23 | | internal BidirectionalDijkstra(RoutingNetwork routingNetwork) |
| | 27 | 24 | | { |
| | 27 | 25 | | _routingNetwork = routingNetwork; |
| | | 26 | | |
| | 27 | 27 | | _backward = new BidirectionalDijkstraBackward(this); |
| | 27 | 28 | | _forward = new BidirectionalDijkstraForward(this); |
| | 27 | 29 | | } |
| | | 30 | | |
| | | 31 | | [ThreadStatic] |
| | | 32 | | private static BidirectionalDijkstra? _cached; |
| | | 33 | | |
| | | 34 | | public static BidirectionalDijkstra ForNetwork(RoutingNetwork routingNetwork) |
| | 59 | 35 | | { |
| | 59 | 36 | | var cached = _cached; |
| | 59 | 37 | | if (cached != null && cached._routingNetwork == routingNetwork) |
| | 32 | 38 | | { |
| | 32 | 39 | | return cached; |
| | | 40 | | } |
| | | 41 | | |
| | 27 | 42 | | cached = new BidirectionalDijkstra(routingNetwork); |
| | 27 | 43 | | _cached = cached; |
| | 27 | 44 | | return cached; |
| | 59 | 45 | | } |
| | | 46 | | |
| | | 47 | | public async Task<(Path? path, double cost)> RunAsync(SnapPoint origin, |
| | | 48 | | SnapPoint destination, ICostFunction costFunction, Func<VertexId, Task<bool>>? settled = null, |
| | | 49 | | Func<VertexId, Task<bool>>? queued = null, CancellationToken cancellationToken = default) |
| | 59 | 50 | | { |
| | 59 | 51 | | _costFunction = costFunction; |
| | | 52 | | |
| | 59 | 53 | | _forward.Clear(); |
| | 59 | 54 | | _backward.Clear(); |
| | | 55 | | |
| | 59 | 56 | | _best = (uint.MaxValue, uint.MaxValue, double.MaxValue, null); |
| | 59 | 57 | | if (_routingNetwork.TrySingleHop(origin, destination, costFunction, out var singleHopPath, out var singleHopCost |
| | 47 | 58 | | { |
| | 47 | 59 | | _best = (uint.MaxValue, uint.MaxValue, singleHopCost, singleHopPath); |
| | 47 | 60 | | } |
| | | 61 | | |
| | 59 | 62 | | _backward.Push(costFunction, destination, false); |
| | 59 | 63 | | _forward.Push(costFunction, origin, true); |
| | | 64 | | |
| | 59 | 65 | | var forwardDone = false; |
| | 59 | 66 | | var backwardDone = false; |
| | 59 | 67 | | var forwardCost = 0d; |
| | 59 | 68 | | var backwardCost = 0d; |
| | 315 | 69 | | while (!forwardDone || !backwardDone) |
| | 312 | 70 | | { |
| | 312 | 71 | | cancellationToken.ThrowIfCancellationRequested(); |
| | | 72 | | |
| | 312 | 73 | | if (!forwardDone) |
| | 312 | 74 | | { |
| | 312 | 75 | | var (p, v, c) = _forward.Pop(); |
| | 312 | 76 | | forwardCost = c; |
| | 312 | 77 | | if (p != uint.MaxValue) |
| | 309 | 78 | | { |
| | 309 | 79 | | if (_backward.TryGetVisit(v.vertex, out var backwardVisit)) |
| | 21 | 80 | | { |
| | 21 | 81 | | var cost = c + backwardVisit.cost; |
| | 21 | 82 | | if (cost < _best.cost && |
| | 21 | 83 | | this.CanTurn(p, backwardVisit.p)) |
| | 0 | 84 | | { |
| | 0 | 85 | | _best = (p, backwardVisit.p, cost, null); |
| | 0 | 86 | | } |
| | 21 | 87 | | } |
| | | 88 | | |
| | 613 | 89 | | if (settled != null) await settled(v.vertex); |
| | | 90 | | |
| | 309 | 91 | | if (!_forward.Step(p, v, c)) forwardDone = true; |
| | 309 | 92 | | } |
| | | 93 | | else |
| | 3 | 94 | | { |
| | 3 | 95 | | forwardDone = true; |
| | 3 | 96 | | } |
| | 312 | 97 | | } |
| | 312 | 98 | | if (!backwardDone) |
| | 312 | 99 | | { |
| | 312 | 100 | | var (p, v, c) = _backward.Pop(); |
| | 312 | 101 | | backwardCost = c; |
| | 312 | 102 | | if (p != uint.MaxValue) |
| | 309 | 103 | | { |
| | 309 | 104 | | if (_forward.TryGetVisit(v.vertex, out var forwardVisit)) |
| | 66 | 105 | | { |
| | 66 | 106 | | var cost = c + forwardVisit.cost; |
| | 66 | 107 | | if (cost < _best.cost && |
| | 66 | 108 | | this.CanTurn(forwardVisit.p, p)) |
| | 6 | 109 | | { |
| | 6 | 110 | | _best = (forwardVisit.p, p, cost, null); |
| | 6 | 111 | | } |
| | 66 | 112 | | } |
| | | 113 | | |
| | 613 | 114 | | if (settled != null) await settled(v.vertex); |
| | | 115 | | |
| | 309 | 116 | | if (!_backward.Step(p, v, c)) backwardDone = true; |
| | 309 | 117 | | } |
| | | 118 | | else |
| | 3 | 119 | | { |
| | 3 | 120 | | backwardDone = true; |
| | 3 | 121 | | } |
| | 312 | 122 | | } |
| | | 123 | | |
| | 368 | 124 | | if (_best.cost < (forwardCost + backwardCost)) break; |
| | 256 | 125 | | } |
| | | 126 | | |
| | 62 | 127 | | if (_best.cost >= double.MaxValue) return (null, double.MaxValue); |
| | 103 | 128 | | if (_best.forward == uint.MaxValue) return (_best.singleHopPath, _best.cost); |
| | | 129 | | |
| | 9 | 130 | | var forwardPath = _forward.GetPathToVisit(_best.forward); |
| | 9 | 131 | | var backwardPath = _backward.GetPathToVisit(_best.backward); |
| | 9 | 132 | | forwardPath.Append(backwardPath.InvertDirection()); |
| | | 133 | | |
| | 9 | 134 | | forwardPath.Offset1 = forwardPath.First.direction ? origin.Offset : (ushort)(ushort.MaxValue - origin.Offset); |
| | 9 | 135 | | forwardPath.Offset2 = forwardPath.Last.direction |
| | 9 | 136 | | ? destination.Offset |
| | 9 | 137 | | : (ushort)(ushort.MaxValue - destination.Offset); |
| | | 138 | | |
| | 9 | 139 | | return (forwardPath, _best.cost); |
| | 59 | 140 | | } |
| | | 141 | | |
| | | 142 | | private bool CanTurn(uint forwardPointer, uint backwardPointer) |
| | 15 | 143 | | { |
| | 15 | 144 | | var (vertex, _, _, _, _) = _forward.GetVisit(forwardPointer); |
| | 15 | 145 | | var forwardPrevious = _forward.GetPreviousEdges(forwardPointer).ToList(); |
| | 15 | 146 | | if (forwardPrevious.Count == 0) return true; // not a turn. |
| | 15 | 147 | | var backwardPrevious = _backward.GetPreviousEdges(backwardPointer) |
| | 32 | 148 | | .Select(x => x.edge).ToList(); |
| | 15 | 149 | | if (backwardPrevious.Count == 0) return true; // not a turn. |
| | 15 | 150 | | return _forward.CanTurn(vertex, forwardPrevious, backwardPrevious); |
| | 15 | 151 | | } |
| | | 152 | | |
| | | 153 | | internal class BidirectionalDijkstraForward : DijkstraAlgorithm |
| | | 154 | | { |
| | | 155 | | private readonly BidirectionalDijkstra _bidirectionalDijkstra; |
| | | 156 | | |
| | | 157 | | internal BidirectionalDijkstraForward(BidirectionalDijkstra bidirectionalDijkstra) |
| | 27 | 158 | | : base(bidirectionalDijkstra._routingNetwork) |
| | 27 | 159 | | { |
| | 27 | 160 | | _bidirectionalDijkstra = bidirectionalDijkstra; |
| | 27 | 161 | | } |
| | | 162 | | |
| | | 163 | | |
| | | 164 | | protected override bool OnQueued(uint visit, EdgeId edge, (double cost, double turnCost) edgeCost, VertexId vert |
| | 660 | 165 | | { |
| | | 166 | | // check if the neighbor vertex is already settled by the backward search |
| | 660 | 167 | | if (_bidirectionalDijkstra._backward.TryGetVisit(vertex, out var backwardVisit)) |
| | 29 | 168 | | { |
| | | 169 | | // reject U-turns: forward and backward must not arrive via the same edge |
| | 29 | 170 | | var backwardEdge = _bidirectionalDijkstra._backward.GetVisit(backwardVisit.p).edge; |
| | 38 | 171 | | if (edge == backwardEdge) return true; |
| | | 172 | | |
| | 20 | 173 | | var combinedCost = totalCost + backwardVisit.cost; |
| | 20 | 174 | | if (combinedCost < _bidirectionalDijkstra._best.cost) |
| | 2 | 175 | | { |
| | 2 | 176 | | _bidirectionalDijkstra._best = (visit, backwardVisit.p, combinedCost, null); |
| | 2 | 177 | | } |
| | 20 | 178 | | } |
| | | 179 | | |
| | 651 | 180 | | return true; |
| | 660 | 181 | | } |
| | | 182 | | |
| | | 183 | | protected override bool OnSettled(uint visit, VertexId vertex, double cost) |
| | 309 | 184 | | { |
| | 309 | 185 | | return true; |
| | 309 | 186 | | } |
| | | 187 | | |
| | | 188 | | protected override (double cost, double turnCost) GetCost(RoutingNetworkEdgeEnumerator edgeEnumerator, PreviousE |
| | 717 | 189 | | { |
| | 717 | 190 | | return _bidirectionalDijkstra._costFunction.GetCost(edgeEnumerator, true, previousEdges); |
| | 717 | 191 | | } |
| | | 192 | | |
| | | 193 | | protected override (double cost, double turnCost) GetCost(RoutingNetworkEdgeEnumerator edgeEnumerator, IEnumerab |
| | 14 | 194 | | { |
| | 14 | 195 | | return _bidirectionalDijkstra._costFunction.GetCost(edgeEnumerator, true, previousEdges); |
| | 14 | 196 | | } |
| | | 197 | | } |
| | | 198 | | |
| | | 199 | | internal class BidirectionalDijkstraBackward : DijkstraAlgorithm |
| | | 200 | | { |
| | | 201 | | private readonly BidirectionalDijkstra _bidirectionalDijkstra; |
| | | 202 | | |
| | | 203 | | internal BidirectionalDijkstraBackward(BidirectionalDijkstra bidirectionalDijkstra) |
| | 27 | 204 | | : base(bidirectionalDijkstra._routingNetwork) |
| | 27 | 205 | | { |
| | 27 | 206 | | _bidirectionalDijkstra = bidirectionalDijkstra; |
| | 27 | 207 | | } |
| | | 208 | | |
| | | 209 | | |
| | | 210 | | protected override bool OnQueued(uint visit, EdgeId edge, (double cost, double turnCost) edgeCost, VertexId vert |
| | 658 | 211 | | { |
| | | 212 | | // check if the neighbor vertex is already settled by the forward search |
| | 658 | 213 | | if (_bidirectionalDijkstra._forward.TryGetVisit(vertex, out var forwardVisit)) |
| | 29 | 214 | | { |
| | | 215 | | // reject U-turns: forward and backward must not arrive via the same edge |
| | 29 | 216 | | var forwardEdge = _bidirectionalDijkstra._forward.GetVisit(forwardVisit.p).edge; |
| | 38 | 217 | | if (edge == forwardEdge) return true; |
| | | 218 | | |
| | 20 | 219 | | var combinedCost = totalCost + forwardVisit.cost; |
| | 20 | 220 | | if (combinedCost < _bidirectionalDijkstra._best.cost) |
| | 1 | 221 | | { |
| | 1 | 222 | | _bidirectionalDijkstra._best = (forwardVisit.p, visit, combinedCost, null); |
| | 1 | 223 | | } |
| | 20 | 224 | | } |
| | | 225 | | |
| | 649 | 226 | | return true; |
| | 658 | 227 | | } |
| | | 228 | | |
| | | 229 | | protected override bool OnSettled(uint visit, VertexId vertex, double cost) |
| | 309 | 230 | | { |
| | 309 | 231 | | return true; |
| | 309 | 232 | | } |
| | | 233 | | |
| | | 234 | | protected override (double cost, double turnCost) GetCost(RoutingNetworkEdgeEnumerator edgeEnumerator, PreviousE |
| | 720 | 235 | | { |
| | 720 | 236 | | return _bidirectionalDijkstra._costFunction.GetCost(edgeEnumerator, false, previousEdges); |
| | 720 | 237 | | } |
| | | 238 | | |
| | | 239 | | protected override (double cost, double turnCost) GetCost(RoutingNetworkEdgeEnumerator edgeEnumerator, IEnumerab |
| | 0 | 240 | | { |
| | 0 | 241 | | return _bidirectionalDijkstra._costFunction.GetCost(edgeEnumerator, false, previousEdges); |
| | 0 | 242 | | } |
| | | 243 | | } |
| | | 244 | | } |