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