| | 1 | | using System; |
| | 2 | | using System.Collections.Generic; |
| | 3 | | using System.Threading; |
| | 4 | | using System.Threading.Tasks; |
| | 5 | | using Itinero.Network; |
| | 6 | | using Itinero.Network.Enumerators.Edges; |
| | 7 | | using Itinero.Routes.Paths; |
| | 8 | | using Itinero.Routing.Costs; |
| | 9 | | using Itinero.Snapping; |
| | 10 | |
|
| | 11 | | namespace Itinero.Routing.Flavours.Dijkstra.Bidirectional; |
| | 12 | |
|
| | 13 | |
|
| | 14 | | internal class BidirectionalDijkstra |
| | 15 | | { |
| | 16 | | private readonly RoutingNetwork _routingNetwork; |
| | 17 | | private readonly BidirectionalDijkstraForward _forward; |
| | 18 | | private readonly BidirectionalDijkstraBackward _backward; |
| | 19 | | private ICostFunction _costFunction; |
| | 20 | |
|
| 9 | 21 | | internal BidirectionalDijkstra(RoutingNetwork routingNetwork) |
| 9 | 22 | | { |
| 9 | 23 | | _routingNetwork = routingNetwork; |
| | 24 | |
|
| 9 | 25 | | _backward = new BidirectionalDijkstraBackward(this); |
| 9 | 26 | | _forward = new BidirectionalDijkstraForward(this); |
| 9 | 27 | | } |
| | 28 | |
|
| 9 | 29 | | public static BidirectionalDijkstra ForNetwork(RoutingNetwork routingNetwork) => new(routingNetwork); |
| | 30 | |
|
| | 31 | | public async Task<(Path? path, double cost)> RunAsync(SnapPoint origin, |
| | 32 | | SnapPoint destination, ICostFunction costFunction, Func<VertexId, Task<bool>>? settled = null, |
| | 33 | | Func<VertexId, Task<bool>>? queued = null, CancellationToken cancellationToken = default) |
| 9 | 34 | | { |
| 9 | 35 | | _costFunction = costFunction; |
| | 36 | |
|
| 9 | 37 | | (uint forward, uint backward, double cost, Path? singleHopPath) best = (uint.MaxValue, uint.MaxValue, double.Max |
| 9 | 38 | | if (_routingNetwork.TrySingleHop(origin, destination, costFunction, out var singleHopPath, out var singleHopCost |
| 7 | 39 | | { |
| 7 | 40 | | best = (uint.MaxValue, uint.MaxValue, singleHopCost, singleHopPath); |
| 7 | 41 | | } |
| | 42 | |
|
| 9 | 43 | | _backward.Push(costFunction, destination, false); |
| 9 | 44 | | _forward.Push(costFunction, origin, true); |
| | 45 | |
|
| 9 | 46 | | var forwardDone = false; |
| 9 | 47 | | var backwardDone = false; |
| 9 | 48 | | var forwardCost = 0d; |
| 9 | 49 | | var backwardCost = 0d; |
| 20 | 50 | | while (!forwardDone || !backwardDone) |
| 20 | 51 | | { |
| 20 | 52 | | cancellationToken.ThrowIfCancellationRequested(); |
| | 53 | |
|
| 20 | 54 | | if (!forwardDone) |
| 20 | 55 | | { |
| 20 | 56 | | var (p, v, c) = _forward.Pop(); |
| 20 | 57 | | forwardCost = c; |
| 20 | 58 | | if (p != uint.MaxValue) |
| 20 | 59 | | { |
| 20 | 60 | | if (_backward.TryGetVisit(v.vertex, out var backwardVisit)) |
| 9 | 61 | | { |
| 9 | 62 | | var cost = c + backwardVisit.cost; |
| 9 | 63 | | if (cost < best.cost) |
| 0 | 64 | | { |
| 0 | 65 | | best = (p, backwardVisit.p, cost, null); |
| 0 | 66 | | } |
| 9 | 67 | | } |
| | 68 | |
|
| 35 | 69 | | if (settled != null) await settled(v.vertex); |
| | 70 | |
|
| 20 | 71 | | if (!_forward.Step(p, v, c)) forwardDone = true; |
| 20 | 72 | | } |
| | 73 | | else |
| 0 | 74 | | { |
| 0 | 75 | | forwardDone = true; |
| 0 | 76 | | } |
| 20 | 77 | | } |
| 20 | 78 | | if (!backwardDone) |
| 20 | 79 | | { |
| 20 | 80 | | var (p, v, c) = _backward.Pop(); |
| 20 | 81 | | backwardCost = c; |
| 20 | 82 | | if (p != uint.MaxValue) |
| 20 | 83 | | { |
| 20 | 84 | | if (_forward.TryGetVisit(v.vertex, out var forwardVisit)) |
| 11 | 85 | | { |
| 11 | 86 | | var cost = c + forwardVisit.cost; |
| 11 | 87 | | if (cost < best.cost) |
| 2 | 88 | | { |
| 2 | 89 | | best = (forwardVisit.p, p, cost, null); |
| 2 | 90 | | } |
| 11 | 91 | | } |
| | 92 | |
|
| 35 | 93 | | if (settled != null) await settled(v.vertex); |
| | 94 | |
|
| 20 | 95 | | if (!_backward.Step(p, v, c)) backwardDone = true; |
| 20 | 96 | | } |
| | 97 | | else |
| 0 | 98 | | { |
| 0 | 99 | | backwardDone = true; |
| 0 | 100 | | } |
| 20 | 101 | | } |
| | 102 | |
|
| 29 | 103 | | if (best.cost < (forwardCost + backwardCost)) break; |
| 11 | 104 | | } |
| | 105 | |
|
| 9 | 106 | | if (best.cost >= double.MaxValue) return (null, double.MaxValue); |
| 16 | 107 | | if (best.forward == uint.MaxValue) return (best.singleHopPath, best.cost); |
| | 108 | |
|
| 2 | 109 | | var forwardPath = _forward.GetPathToVisit(best.forward); |
| 2 | 110 | | var backwardPath = _backward.GetPathToVisit(best.backward); |
| 2 | 111 | | forwardPath.Append(backwardPath.InvertDirection()); |
| | 112 | |
|
| 2 | 113 | | forwardPath.Offset1 = forwardPath.First.direction ? origin.Offset : (ushort)(ushort.MaxValue - origin.Offset); |
| 2 | 114 | | forwardPath.Offset2 = forwardPath.Last.direction |
| 2 | 115 | | ? destination.Offset |
| 2 | 116 | | : (ushort)(ushort.MaxValue - destination.Offset); |
| | 117 | |
|
| 2 | 118 | | return (forwardPath, best.cost); |
| 9 | 119 | | } |
| | 120 | |
|
| | 121 | | internal class BidirectionalDijkstraForward : DijkstraAlgorithm |
| | 122 | | { |
| | 123 | | private readonly BidirectionalDijkstra _bidirectionalDijkstra; |
| | 124 | |
|
| | 125 | | internal BidirectionalDijkstraForward(BidirectionalDijkstra bidirectionalDijkstra) |
| 9 | 126 | | : base(bidirectionalDijkstra._routingNetwork) |
| 9 | 127 | | { |
| 9 | 128 | | _bidirectionalDijkstra = bidirectionalDijkstra; |
| 9 | 129 | | } |
| | 130 | |
|
| | 131 | |
|
| | 132 | | protected override bool OnQueued(uint visit, EdgeId edge, (double cost, double turnCost) edgeCost, VertexId vert |
| 2 | 133 | | { |
| 2 | 134 | | return true; |
| 2 | 135 | | } |
| | 136 | |
|
| | 137 | | protected override bool OnSettled(uint visit, VertexId vertex, double cost) |
| 20 | 138 | | { |
| 20 | 139 | | return true; |
| 20 | 140 | | } |
| | 141 | |
|
| | 142 | | protected override (double cost, double turnCost) GetCost(RoutingNetworkEdgeEnumerator edgeEnumerator, IEnumerab |
| 2 | 143 | | { |
| 2 | 144 | | return _bidirectionalDijkstra._costFunction.GetCost(edgeEnumerator, true, previousEdges); |
| 2 | 145 | | } |
| | 146 | | } |
| | 147 | |
|
| | 148 | | internal class BidirectionalDijkstraBackward : DijkstraAlgorithm |
| | 149 | | { |
| | 150 | | private readonly BidirectionalDijkstra _bidirectionalDijkstra; |
| | 151 | |
|
| | 152 | | internal BidirectionalDijkstraBackward(BidirectionalDijkstra bidirectionalDijkstra) |
| 9 | 153 | | : base(bidirectionalDijkstra._routingNetwork) |
| 9 | 154 | | { |
| 9 | 155 | | _bidirectionalDijkstra = bidirectionalDijkstra; |
| 9 | 156 | | } |
| | 157 | |
|
| | 158 | |
|
| | 159 | | protected override bool OnQueued(uint visit, EdgeId edge, (double cost, double turnCost) edgeCost, VertexId vert |
| 2 | 160 | | { |
| 2 | 161 | | return true; |
| 2 | 162 | | } |
| | 163 | |
|
| | 164 | | protected override bool OnSettled(uint visit, VertexId vertex, double cost) |
| 20 | 165 | | { |
| 20 | 166 | | return true; |
| 20 | 167 | | } |
| | 168 | |
|
| | 169 | | protected override (double cost, double turnCost) GetCost(RoutingNetworkEdgeEnumerator edgeEnumerator, IEnumerab |
| 2 | 170 | | { |
| 2 | 171 | | return _bidirectionalDijkstra._costFunction.GetCost(edgeEnumerator, false, previousEdges); |
| 2 | 172 | | } |
| | 173 | | } |
| | 174 | | } |