| | | 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.Network.Search.Reachability; |
| | | 8 | | using Itinero.Routing.DataStructures; |
| | | 9 | | using Itinero.Routing.Flavours.Dijkstra; |
| | | 10 | | using Itinero.Routing.Flavours.Dijkstra.Bidirectional; |
| | | 11 | | using Itinero.Snapping; |
| | | 12 | | |
| | | 13 | | namespace Itinero.Routing.Corridors; |
| | | 14 | | |
| | | 15 | | /// <summary> |
| | | 16 | | /// Extracts a cost ellipse around a route: every edge that lies on some origin-to-destination |
| | | 17 | | /// path costing no more than a multiple of the optimum. |
| | | 18 | | /// </summary> |
| | | 19 | | /// <remarks> |
| | | 20 | | /// Out of the route's own search: its two halves meet in the middle, so neither spans the ellipse |
| | | 21 | | /// alone, and lifting the ceiling to the budget keeps what the search would examine anyway. |
| | | 22 | | /// </remarks> |
| | | 23 | | public static class RouteCorridorSearch |
| | | 24 | | { |
| | | 25 | | /// <summary> |
| | | 26 | | /// Resolves the route and extracts the corridor around it, in one search. |
| | | 27 | | /// </summary> |
| | | 28 | | /// <param name="costFactor"> |
| | | 29 | | /// The budget as a multiple of the optimum. 1.0 admits only optimal paths. |
| | | 30 | | /// </param> |
| | | 31 | | /// <returns> |
| | | 32 | | /// The route and the corridor around it. The route is handed back because the caller needs |
| | | 33 | | /// the endpoints and cost functions that were actually used, and this call consumed them. |
| | | 34 | | /// </returns> |
| | | 35 | | /// <remarks> |
| | | 36 | | /// At termination <c>budget <= forwardFrontier + backwardFrontier</c>, so one half or the |
| | | 37 | | /// other relaxed every transition of a within-budget path: their union prices them all. |
| | | 38 | | /// </remarks> |
| | | 39 | | public static async Task<Result<(ResolvedRoute Route, RouteCorridor Corridor)>> RouteCorridorAsync( |
| | | 40 | | this RoutingNetwork network, |
| | | 41 | | RoutingSettings settings, |
| | | 42 | | (double longitude, double latitude) origin, |
| | | 43 | | (double longitude, double latitude) destination, |
| | | 44 | | ReachabilityBounds bounds, |
| | | 45 | | double searchBoxMeters, |
| | | 46 | | double costFactor, |
| | | 47 | | CancellationToken cancellationToken = default, |
| | | 48 | | double maxLocalDetour = double.MaxValue) |
| | 25 | 49 | | { |
| | 25 | 50 | | Check(costFactor, maxLocalDetour); |
| | | 51 | | |
| | 23 | 52 | | var collector = new TransitionCollector(); |
| | 23 | 53 | | var resolved = await network.ResolveAsync(settings, origin, destination, bounds, |
| | 23 | 54 | | searchBoxMeters, costFactor, collector.OnForward, collector.OnBackward, cancellationToken); |
| | 23 | 55 | | if (resolved.IsError) |
| | 0 | 56 | | return new Result<(ResolvedRoute, RouteCorridor)>(resolved.ErrorMessage); |
| | | 57 | | |
| | 23 | 58 | | return Corridor(network, resolved.Value, costFactor, collector, maxLocalDetour, cancellationToken); |
| | 23 | 59 | | } |
| | | 60 | | |
| | | 61 | | /// <summary> |
| | | 62 | | /// Extracts the corridor around a route between two points already snapped, in one search. |
| | | 63 | | /// </summary> |
| | | 64 | | /// <param name="costFactor"> |
| | | 65 | | /// The budget as a multiple of the optimum. 1.0 admits only optimal paths. |
| | | 66 | | /// </param> |
| | | 67 | | /// <returns>The route and the corridor around it.</returns> |
| | | 68 | | /// <remarks> |
| | | 69 | | /// Takes the same engine the ordinary one-to-one route takes, so the corridor is around the |
| | | 70 | | /// route the router itself would return. Snapping is the caller's. |
| | | 71 | | /// </remarks> |
| | | 72 | | public static async Task<Result<(ResolvedRoute Route, RouteCorridor Corridor)>> RouteCorridorAsync( |
| | | 73 | | this RoutingNetwork network, |
| | | 74 | | RoutingSettings settings, |
| | | 75 | | SnapPoint source, |
| | | 76 | | SnapPoint target, |
| | | 77 | | double costFactor, |
| | | 78 | | CancellationToken cancellationToken = default, |
| | | 79 | | double maxLocalDetour = double.MaxValue) |
| | 7 | 80 | | { |
| | 7 | 81 | | Check(costFactor, maxLocalDetour); |
| | | 82 | | |
| | 7 | 83 | | var collector = new TransitionCollector(); |
| | 7 | 84 | | var resolved = await network.SearchAsync(settings, source, target, costFactor, |
| | 7 | 85 | | collector.OnForward, collector.OnBackward, cancellationToken); |
| | 7 | 86 | | if (resolved.IsError) |
| | 0 | 87 | | return new Result<(ResolvedRoute, RouteCorridor)>(resolved.ErrorMessage); |
| | | 88 | | |
| | 7 | 89 | | return Corridor(network, resolved.Value, costFactor, collector, maxLocalDetour, cancellationToken); |
| | 7 | 90 | | } |
| | | 91 | | |
| | | 92 | | private static void Check(double costFactor, double maxLocalDetour) |
| | 32 | 93 | | { |
| | 33 | 94 | | if (costFactor < 1.0) throw new ArgumentOutOfRangeException(nameof(costFactor), |
| | 1 | 95 | | "A corridor cannot be tighter than the optimum it is measured against."); |
| | 32 | 96 | | if (maxLocalDetour < 1.0) throw new ArgumentOutOfRangeException(nameof(maxLocalDetour), |
| | 1 | 97 | | "A detour cannot be required to beat the stretch it replaces."); |
| | 30 | 98 | | } |
| | | 99 | | |
| | | 100 | | private static Result<(ResolvedRoute Route, RouteCorridor Corridor)> Corridor( |
| | | 101 | | RoutingNetwork network, ResolvedRoute route, double costFactor, |
| | | 102 | | TransitionCollector collector, double maxLocalDetour, CancellationToken cancellationToken) |
| | 30 | 103 | | { |
| | 30 | 104 | | return new Result<(ResolvedRoute, RouteCorridor)>((route, Assemble(network, route, |
| | 30 | 105 | | route.Cost * costFactor, collector.Transitions, collector.Relaxed, maxLocalDetour, |
| | 30 | 106 | | cancellationToken))); |
| | 30 | 107 | | } |
| | | 108 | | |
| | | 109 | | /// <summary> |
| | | 110 | | /// Collects the states the two halves relaxed, in path order. |
| | | 111 | | /// </summary> |
| | | 112 | | /// <remarks> |
| | | 113 | | /// Arrive at Turn over Prev, then traverse Next. Neither half reports in that convention, so |
| | | 114 | | /// take the state triple only and price it later; the halves' overlap then dedupes free. |
| | | 115 | | /// </remarks> |
| | | 116 | | private sealed class TransitionCollector |
| | | 117 | | { |
| | 696 | 118 | | public HashSet<(EdgeId Prev, VertexId Turn, EdgeId Next)> Transitions { get; } = new(); |
| | | 119 | | |
| | 1302 | 120 | | public int Relaxed { get; private set; } |
| | | 121 | | |
| | | 122 | | public void OnForward(EdgeId fromEdge, VertexId turnVertex, EdgeId toEdge) |
| | 318 | 123 | | { |
| | 318 | 124 | | this.Relaxed++; |
| | 318 | 125 | | this.Transitions.Add((fromEdge, turnVertex, toEdge)); |
| | 318 | 126 | | } |
| | | 127 | | |
| | | 128 | | public void OnBackward(EdgeId fromEdge, VertexId turnVertex, EdgeId toEdge) |
| | 318 | 129 | | { |
| | | 130 | | // Against travel the roles swap: in path order the route arrives at turnVertex over |
| | | 131 | | // the edge being relaxed onto, and leaves over the one the half arrived by. |
| | 318 | 132 | | this.Relaxed++; |
| | 318 | 133 | | this.Transitions.Add((toEdge, turnVertex, fromEdge)); |
| | 318 | 134 | | } |
| | | 135 | | } |
| | | 136 | | |
| | | 137 | | /// <summary> |
| | | 138 | | /// Prices every transition in path order, then applies the corridor's own filter and trim. |
| | | 139 | | /// </summary> |
| | | 140 | | private static RouteCorridor Assemble( |
| | | 141 | | RoutingNetwork network, |
| | | 142 | | ResolvedRoute route, |
| | | 143 | | double budget, |
| | | 144 | | HashSet<(EdgeId Prev, VertexId Turn, EdgeId Next)> transitions, |
| | | 145 | | int relaxed, |
| | | 146 | | double maxLocalDetour, |
| | | 147 | | CancellationToken cancellationToken = default) |
| | 30 | 148 | | { |
| | 30 | 149 | | var probe = network.GetEdgeEnumerator(); |
| | 30 | 150 | | var orders = network.GetEdgeEnumerator(); |
| | 30 | 151 | | var costFunction = route.ForwardCostFunction; |
| | 30 | 152 | | var lengths = new Dictionary<EdgeId, double>(); |
| | | 153 | | |
| | 30 | 154 | | var states = new List<CorridorState?> { null, null }; |
| | 30 | 155 | | var nodes = new Dictionary<(EdgeId, VertexId), int>(); |
| | 30 | 156 | | var byVertex = new Dictionary<VertexId, List<int>>(); |
| | 30 | 157 | | var tails = new List<int>(transitions.Count); |
| | 30 | 158 | | var heads = new List<int>(transitions.Count); |
| | 30 | 159 | | var costs = new List<double>(transitions.Count); |
| | | 160 | | |
| | 30 | 161 | | var priced = 0; |
| | 922 | 162 | | foreach (var (prev, turn, next) in transitions) |
| | 416 | 163 | | { |
| | | 164 | | // As long as the search that produced it, so a cancelled request must not keep |
| | | 165 | | // pricing. Masked: the check is cheap but this runs millions of times. |
| | 445 | 166 | | if ((priced++ & 0x3FF) == 0) cancellationToken.ThrowIfCancellationRequested(); |
| | | 167 | | |
| | | 168 | | // Position on Next with Turn as its tail: that is the direction the path travels it. |
| | 416 | 169 | | if (!probe.MoveTo(next, true)) continue; |
| | 416 | 170 | | if (probe.Tail != turn && !(probe.MoveTo(next, false) && probe.Tail == turn)) continue; |
| | | 171 | | |
| | 416 | 172 | | var prevOrder = OrderAt(orders, prev, turn); |
| | 416 | 173 | | var previous = prevOrder.HasValue |
| | 416 | 174 | | ? PreviousEdgeEnumerable.ForEdge(prev, prevOrder) |
| | 416 | 175 | | : default; |
| | 416 | 176 | | var (canAccess, _, _, cost, turnCost) = |
| | 416 | 177 | | costFunction.Get(probe, tailToHead: true, previous); |
| | 416 | 178 | | if (!canAccess || cost is >= double.MaxValue or <= 0) continue; |
| | 416 | 179 | | if (turnCost is >= double.MaxValue or < 0) continue; |
| | | 180 | | |
| | | 181 | | // Which way Prev was travelled follows from where it ends, and it ends at the turn. |
| | 416 | 182 | | var prevForward = orders.MoveTo(prev, true) && orders.Head == turn; |
| | 416 | 183 | | Add(Node(new CorridorState(prev, prevForward, turn)), |
| | 416 | 184 | | Node(new CorridorState(next, probe.Forward, probe.Head)), |
| | 416 | 185 | | cost + turnCost); |
| | 416 | 186 | | } |
| | | 187 | | |
| | 30 | 188 | | var sourcePartials = AddSourcePartials(); |
| | 30 | 189 | | var destinationPartials = AddDestinationPartials(); |
| | | 190 | | |
| | 30 | 191 | | var fromOrigin = Distances(tails, heads, costs, states.Count, budget, |
| | 30 | 192 | | RouteCorridor.SourceNode, forwards: true); |
| | 30 | 193 | | var toDestination = Distances(tails, heads, costs, states.Count, budget, |
| | 30 | 194 | | RouteCorridor.TargetNode, forwards: false); |
| | | 195 | | |
| | | 196 | | // Edges reachable only by a detour absurd for what it replaces; the trip-wide budget |
| | | 197 | | // cannot see those. Both fields are recomputed after, since they are published as exact. |
| | 30 | 198 | | bool[]? alive = null; |
| | 30 | 199 | | if (maxLocalDetour < double.MaxValue) |
| | 2 | 200 | | { |
| | 2 | 201 | | alive = LocallyReasonable(tails, heads, costs, states.Count, fromOrigin, toDestination, |
| | 2 | 202 | | maxLocalDetour); |
| | 2 | 203 | | fromOrigin = Distances(tails, heads, costs, states.Count, budget, |
| | 2 | 204 | | RouteCorridor.SourceNode, forwards: true, alive); |
| | 2 | 205 | | toDestination = Distances(tails, heads, costs, states.Count, budget, |
| | 2 | 206 | | RouteCorridor.TargetNode, forwards: false, alive); |
| | 2 | 207 | | } |
| | | 208 | | |
| | 30 | 209 | | var surviving = new List<int>(); |
| | 30 | 210 | | var originEdges = 0; |
| | 30 | 211 | | var interiorEdges = 0; |
| | 30 | 212 | | var destinationEdges = 0; |
| | 1118 | 213 | | for (var i = 0; i < tails.Count; i++) |
| | 529 | 214 | | { |
| | 533 | 215 | | if (alive != null && !alive[i]) continue; |
| | | 216 | | |
| | 525 | 217 | | var before = fromOrigin[tails[i]]; |
| | 525 | 218 | | var after = toDestination[heads[i]]; |
| | 771 | 219 | | if (double.IsPositiveInfinity(before) || double.IsPositiveInfinity(after)) continue; |
| | 303 | 220 | | if (before + costs[i] + after > budget) continue; |
| | | 221 | | |
| | 284 | 222 | | if (tails[i] == RouteCorridor.SourceNode) originEdges++; |
| | 255 | 223 | | else if (heads[i] == RouteCorridor.TargetNode) destinationEdges++; |
| | 197 | 224 | | else interiorEdges++; |
| | 255 | 225 | | surviving.Add(i); |
| | 255 | 226 | | } |
| | | 227 | | |
| | 30 | 228 | | var connected = Trim(tails, heads, surviving, states.Count); |
| | 30 | 229 | | var edges = new List<CorridorEdge>(connected.Count); |
| | 855 | 230 | | foreach (var i in connected) edges.Add(Materialise(i)); |
| | 30 | 231 | | var (compactStates, compactEdges, compactFromOrigin, compactToDestination) = |
| | 30 | 232 | | Compact(states, edges, fromOrigin, toDestination); |
| | | 233 | | |
| | 30 | 234 | | var optimalLength = 0d; |
| | 440 | 235 | | foreach (var (edge, _, offset1, offset2) in route.Path) |
| | 175 | 236 | | { |
| | 175 | 237 | | optimalLength += Length(edge) * ((offset2 - offset1) / (double)ushort.MaxValue); |
| | 175 | 238 | | } |
| | | 239 | | |
| | 30 | 240 | | return new RouteCorridor(compactStates, compactEdges, route.Cost, optimalLength, |
| | 30 | 241 | | route.Settled, relaxed, surviving.Count - connected.Count, |
| | 30 | 242 | | originEdges, interiorEdges, destinationEdges, compactFromOrigin, compactToDestination); |
| | | 243 | | |
| | | 244 | | // Rebuilt rather than captured from the seeds: a candidate retry re-seeds, and those |
| | | 245 | | // seeds belong to a snap point this route no longer uses. |
| | | 246 | | HashSet<int> AddSourcePartials() |
| | 30 | 247 | | { |
| | 30 | 248 | | var partials = new HashSet<int>(); |
| | 210 | 249 | | foreach (var forward in new[] { true, false }) |
| | 60 | 250 | | { |
| | 60 | 251 | | if (!probe.MoveTo(route.Source.EdgeId, forward)) continue; |
| | | 252 | | |
| | 60 | 253 | | var (canAccess, _, _, cost, _) = costFunction.Get(probe, tailToHead: true); |
| | 60 | 254 | | if (!canAccess || cost <= 0) continue; |
| | | 255 | | |
| | 60 | 256 | | partials.Add(tails.Count); |
| | 60 | 257 | | Add(RouteCorridor.SourceNode, |
| | 60 | 258 | | Node(new CorridorState(route.Source.EdgeId, probe.Forward, probe.Head)), |
| | 60 | 259 | | cost * (forward ? 1 - route.Source.OffsetFactor() : route.Source.OffsetFactor())); |
| | 60 | 260 | | } |
| | | 261 | | |
| | 30 | 262 | | return partials; |
| | 30 | 263 | | } |
| | | 264 | | |
| | | 265 | | // Into the destination, from every state that can turn onto the target edge. The turn is |
| | | 266 | | // charged here because it exists only once the two sides are composed. |
| | | 267 | | Dictionary<int, bool> AddDestinationPartials() |
| | 30 | 268 | | { |
| | 30 | 269 | | var partials = new Dictionary<int, bool>(); |
| | 30 | 270 | | var backwardCostFunction = route.BackwardCostFunction; |
| | 210 | 271 | | foreach (var forward in new[] { true, false }) |
| | 60 | 272 | | { |
| | 60 | 273 | | if (!probe.MoveTo(route.Target.EdgeId, forward)) continue; |
| | | 274 | | |
| | 60 | 275 | | var (canAccess, _, _, cost, _) = backwardCostFunction.Get(probe, tailToHead: false); |
| | 60 | 276 | | if (!canAccess || cost <= 0) continue; |
| | | 277 | | |
| | 60 | 278 | | var at = probe.Head; |
| | 60 | 279 | | if (!byVertex.TryGetValue(at, out var arrivals)) continue; |
| | | 280 | | |
| | 60 | 281 | | var partialCost = |
| | 60 | 282 | | cost * (forward ? 1 - route.Target.OffsetFactor() : route.Target.OffsetFactor()); |
| | 406 | 283 | | foreach (var node in arrivals) |
| | 113 | 284 | | { |
| | 113 | 285 | | var arrival = states[node]!.Value; |
| | 173 | 286 | | if (arrival.Edge == route.Target.EdgeId) continue; // U-turn onto the target. |
| | | 287 | | |
| | 53 | 288 | | var turnCost = SearchHalf.TurnCostBetween(orders, backwardCostFunction, |
| | 53 | 289 | | arrival.Edge, OrderAt(orders, arrival.Edge, at), route.Target.EdgeId, forward); |
| | 53 | 290 | | if (turnCost is >= double.MaxValue or < 0) continue; |
| | | 291 | | |
| | 53 | 292 | | partials[tails.Count] = forward; |
| | 53 | 293 | | Add(node, RouteCorridor.TargetNode, partialCost + turnCost); |
| | 53 | 294 | | } |
| | 60 | 295 | | } |
| | | 296 | | |
| | 30 | 297 | | return partials; |
| | 30 | 298 | | } |
| | | 299 | | |
| | | 300 | | CorridorEdge Materialise(int i) |
| | 255 | 301 | | { |
| | 255 | 302 | | if (destinationPartials.TryGetValue(i, out var intoForward)) |
| | 29 | 303 | | { |
| | | 304 | | // intoForward is the orientation the backward half probed the target edge with, |
| | | 305 | | // and it reached the turn at that edge's head — so travel runs the other way. |
| | 29 | 306 | | return new CorridorEdge(tails[i], heads[i], costs[i], |
| | 29 | 307 | | Partial(route.Target.EdgeId, intoForward, route.Target), |
| | 29 | 308 | | route.Target.EdgeId, !intoForward, |
| | 29 | 309 | | intoForward ? CorridorHalf.TargetBackwardFromHead : CorridorHalf.TargetForwardFromTail); |
| | | 310 | | } |
| | | 311 | | |
| | 226 | 312 | | var state = states[heads[i]]!.Value; |
| | 226 | 313 | | if (sourcePartials.Contains(i)) |
| | 29 | 314 | | { |
| | 29 | 315 | | return new CorridorEdge(tails[i], heads[i], costs[i], |
| | 29 | 316 | | Partial(state.Edge, state.Forward, route.Source), state.Edge, state.Forward, |
| | 29 | 317 | | state.Forward ? CorridorHalf.SourceForwardToHead : CorridorHalf.SourceBackwardToTail); |
| | | 318 | | } |
| | | 319 | | |
| | 197 | 320 | | return new CorridorEdge(tails[i], heads[i], costs[i], Length(state.Edge), |
| | 197 | 321 | | state.Edge, state.Forward, CorridorHalf.None); |
| | 255 | 322 | | } |
| | | 323 | | |
| | | 324 | | void Add(int tail, int head, double cost) |
| | 529 | 325 | | { |
| | 529 | 326 | | tails.Add(tail); |
| | 529 | 327 | | heads.Add(head); |
| | 529 | 328 | | costs.Add(cost); |
| | 529 | 329 | | } |
| | | 330 | | |
| | | 331 | | int Node(CorridorState state) |
| | 892 | 332 | | { |
| | 892 | 333 | | var key = (state.Edge, state.Vertex); |
| | 1358 | 334 | | if (nodes.TryGetValue(key, out var index)) return index; |
| | | 335 | | |
| | 426 | 336 | | index = states.Count; |
| | 426 | 337 | | states.Add(state); |
| | 426 | 338 | | nodes[key] = index; |
| | 426 | 339 | | if (!byVertex.TryGetValue(state.Vertex, out var at)) |
| | 249 | 340 | | { |
| | 249 | 341 | | at = new List<int>(2); |
| | 249 | 342 | | byVertex[state.Vertex] = at; |
| | 249 | 343 | | } |
| | 426 | 344 | | at.Add(index); |
| | 426 | 345 | | return index; |
| | 892 | 346 | | } |
| | | 347 | | |
| | | 348 | | double Length(EdgeId edge) |
| | 430 | 349 | | { |
| | 657 | 350 | | if (lengths.TryGetValue(edge, out var length)) return length; |
| | | 351 | | |
| | 203 | 352 | | length = probe.MoveTo(edge, true) ? probe.EdgeLength() : 0d; |
| | 203 | 353 | | lengths[edge] = length; |
| | 203 | 354 | | return length; |
| | 430 | 355 | | } |
| | | 356 | | |
| | | 357 | | // The same fraction of the edge the partial's cost was scaled by. |
| | | 358 | | double Partial(EdgeId edge, bool towardHead, SnapPoint snap) => |
| | 58 | 359 | | Length(edge) * (towardHead ? 1 - snap.OffsetFactor() : snap.OffsetFactor()); |
| | 30 | 360 | | } |
| | | 361 | | |
| | | 362 | | /// The order of <paramref name="edge"/> at <paramref name="vertex"/>, whichever end that is. |
| | | 363 | | private static byte? OrderAt( |
| | | 364 | | RoutingNetworkEdgeEnumerator enumerator, EdgeId edge, VertexId vertex) |
| | 469 | 365 | | { |
| | 469 | 366 | | if (!enumerator.MoveTo(edge, true)) return null; |
| | | 367 | | |
| | 469 | 368 | | return enumerator.Head == vertex ? enumerator.HeadOrder |
| | 469 | 369 | | : enumerator.Tail == vertex ? enumerator.TailOrder |
| | 469 | 370 | | : null; |
| | 469 | 371 | | } |
| | | 372 | | |
| | | 373 | | /// <summary> |
| | | 374 | | /// Drops every edge that does not lie on some origin-to-destination path inside the corridor. |
| | | 375 | | /// </summary> |
| | | 376 | | /// <remarks> |
| | | 377 | | /// The cost filter proves a continuation exists in the network, not in the corridor, so a |
| | | 378 | | /// transition can price inside the budget with nothing to continue into. Not visibly wrong. |
| | | 379 | | /// </remarks> |
| | | 380 | | private static List<int> Trim( |
| | | 381 | | List<int> tails, List<int> heads, List<int> candidates, int nodeCount) |
| | 30 | 382 | | { |
| | 30 | 383 | | var outgoing = new List<int>[nodeCount]; |
| | 30 | 384 | | var incoming = new List<int>[nodeCount]; |
| | 600 | 385 | | foreach (var e in candidates) |
| | 255 | 386 | | { |
| | 255 | 387 | | (outgoing[tails[e]] ??= new List<int>()).Add(e); |
| | 255 | 388 | | (incoming[heads[e]] ??= new List<int>()).Add(e); |
| | 255 | 389 | | } |
| | | 390 | | |
| | 30 | 391 | | var fromOrigin = Reach(RouteCorridor.SourceNode, outgoing, forwards: true); |
| | 30 | 392 | | var toDestination = Reach(RouteCorridor.TargetNode, incoming, forwards: false); |
| | | 393 | | |
| | 30 | 394 | | var kept = new List<int>(candidates.Count); |
| | 600 | 395 | | foreach (var e in candidates) |
| | 255 | 396 | | { |
| | 510 | 397 | | if (fromOrigin[tails[e]] && toDestination[heads[e]]) kept.Add(e); |
| | 255 | 398 | | } |
| | | 399 | | |
| | 30 | 400 | | return kept; |
| | | 401 | | |
| | | 402 | | bool[] Reach(int start, List<int>[] adjacency, bool forwards) |
| | 60 | 403 | | { |
| | 60 | 404 | | var seen = new bool[nodeCount]; |
| | 60 | 405 | | var stack = new Stack<int>(); |
| | 60 | 406 | | seen[start] = true; |
| | 60 | 407 | | stack.Push(start); |
| | 554 | 408 | | while (stack.Count > 0) |
| | 494 | 409 | | { |
| | 2502 | 410 | | foreach (var e in adjacency[stack.Pop()] ?? []) |
| | 510 | 411 | | { |
| | 510 | 412 | | var next = forwards ? heads[e] : tails[e]; |
| | 586 | 413 | | if (seen[next]) continue; |
| | | 414 | | |
| | 434 | 415 | | seen[next] = true; |
| | 434 | 416 | | stack.Push(next); |
| | 434 | 417 | | } |
| | 494 | 418 | | } |
| | | 419 | | |
| | 60 | 420 | | return seen; |
| | 60 | 421 | | } |
| | 30 | 422 | | } |
| | | 423 | | |
| | | 424 | | /// <summary> |
| | | 425 | | /// Shortest paths over the traced graph, out of a node or into it. |
| | | 426 | | /// </summary> |
| | | 427 | | /// <remarks> |
| | | 428 | | /// Over the traced graph, not either half's settled costs: each may only enter the pocket it |
| | | 429 | | /// started in, but their union holds every within-budget path in full. |
| | | 430 | | /// </remarks> |
| | | 431 | | private static double[] Distances( |
| | | 432 | | List<int> tails, List<int> heads, List<double> costs, int nodeCount, double budget, |
| | | 433 | | int start, bool forwards, bool[]? alive = null) |
| | 64 | 434 | | { |
| | 64 | 435 | | var adjacency = new List<int>[nodeCount]; |
| | 2380 | 436 | | for (var e = 0; e < tails.Count; e++) |
| | 1126 | 437 | | { |
| | 1134 | 438 | | if (alive != null && !alive[e]) continue; |
| | | 439 | | |
| | 1118 | 440 | | (adjacency[forwards ? tails[e] : heads[e]] ??= new List<int>()).Add(e); |
| | 1118 | 441 | | } |
| | | 442 | | |
| | 64 | 443 | | var distances = new double[nodeCount]; |
| | 64 | 444 | | Array.Fill(distances, double.PositiveInfinity); |
| | 64 | 445 | | distances[start] = 0; |
| | | 446 | | |
| | 64 | 447 | | var queue = new BinaryHeap<int>(); |
| | 64 | 448 | | queue.Push(start, 0); |
| | 64 | 449 | | var settled = new bool[nodeCount]; |
| | | 450 | | |
| | 853 | 451 | | while (queue.Count > 0) |
| | 789 | 452 | | { |
| | 789 | 453 | | var node = queue.Pop(out var cost); |
| | 824 | 454 | | if (settled[node]) continue; |
| | | 455 | | |
| | 754 | 456 | | settled[node] = true; |
| | 754 | 457 | | if (cost > budget) break; |
| | | 458 | | |
| | 3910 | 459 | | foreach (var e in adjacency[node] ?? []) |
| | 824 | 460 | | { |
| | 824 | 461 | | var next = forwards ? heads[e] : tails[e]; |
| | 824 | 462 | | var candidate = cost + costs[e]; |
| | 923 | 463 | | if (candidate >= distances[next] || candidate > budget) continue; |
| | | 464 | | |
| | 725 | 465 | | distances[next] = candidate; |
| | 725 | 466 | | queue.Push(next, candidate); |
| | 725 | 467 | | } |
| | 754 | 468 | | } |
| | | 469 | | |
| | 64 | 470 | | return distances; |
| | 64 | 471 | | } |
| | | 472 | | |
| | | 473 | | |
| | | 474 | | /// <summary> |
| | | 475 | | /// Which edges are reachable without a local detour worse than <paramref name="maxDetour"/> |
| | | 476 | | /// times the stretch it replaces. Per edge, so a flow model with no single path can use it. |
| | | 477 | | /// </summary> |
| | | 478 | | /// <remarks> |
| | | 479 | | /// O(1) an edge: its cheapest path leaves the optimum at A and rejoins at B, both on the |
| | | 480 | | /// optimum, so the replaced stretch costs <c>d_D(A) - d_D(B)</c> with no path walked. |
| | | 481 | | /// </remarks> |
| | | 482 | | private static bool[] LocallyReasonable( |
| | | 483 | | List<int> tails, List<int> heads, List<double> costs, int nodeCount, |
| | | 484 | | double[] fromOrigin, double[] toDestination, double maxDetour) |
| | 2 | 485 | | { |
| | 2 | 486 | | var alive = new bool[tails.Count]; |
| | 2 | 487 | | Array.Fill(alive, true); |
| | | 488 | | |
| | | 489 | | // The predecessor and successor each field's descent would pick, per node. |
| | 2 | 490 | | var toward = new int[nodeCount]; |
| | 2 | 491 | | var onward = new int[nodeCount]; |
| | 2 | 492 | | Array.Fill(toward, -1); |
| | 2 | 493 | | Array.Fill(onward, -1); |
| | 72 | 494 | | for (var e = 0; e < tails.Count; e++) |
| | 34 | 495 | | { |
| | 58 | 496 | | if (Realises(fromOrigin, heads[e], tails[e], costs[e])) toward[heads[e]] = e; |
| | 54 | 497 | | if (Realises(toDestination, tails[e], heads[e], costs[e])) onward[tails[e]] = e; |
| | 34 | 498 | | } |
| | | 499 | | |
| | | 500 | | // The optimum: descend to the destination from the origin. Its nodes are the ones a |
| | | 501 | | // detour can be measured against. |
| | 2 | 502 | | var onOptimum = new bool[nodeCount]; |
| | 2 | 503 | | var at = RouteCorridor.SourceNode; |
| | 28 | 504 | | for (var guard = 0; at != RouteCorridor.TargetNode && guard <= tails.Count; guard++) |
| | 12 | 505 | | { |
| | 12 | 506 | | onOptimum[at] = true; |
| | 12 | 507 | | var step = onward[at]; |
| | 12 | 508 | | if (step < 0) return alive; // no optimum to compare against; keep everything |
| | | 509 | | |
| | 12 | 510 | | at = heads[step]; |
| | 12 | 511 | | } |
| | | 512 | | |
| | 2 | 513 | | onOptimum[RouteCorridor.TargetNode] = true; |
| | | 514 | | |
| | 2 | 515 | | var ancestor = Nearest(toward, tails, heads, onOptimum, towardOrigin: true); |
| | 2 | 516 | | var descendant = Nearest(onward, tails, heads, onOptimum, towardOrigin: false); |
| | | 517 | | |
| | 72 | 518 | | for (var e = 0; e < tails.Count; e++) |
| | 34 | 519 | | { |
| | 34 | 520 | | var a = ancestor[tails[e]]; |
| | 34 | 521 | | var b = descendant[heads[e]]; |
| | 52 | 522 | | if (a < 0 || b < 0) continue; |
| | | 523 | | |
| | 16 | 524 | | var replaced = toDestination[a] - toDestination[b]; |
| | 16 | 525 | | var detour = (fromOrigin[tails[e]] - fromOrigin[a]) + costs[e] |
| | 16 | 526 | | + (toDestination[heads[e]] - toDestination[b]); |
| | 16 | 527 | | if (detour <= 0) continue; |
| | | 528 | | |
| | | 529 | | // Leaving and rejoining at the same point replaces nothing, which no factor excuses. |
| | 16 | 530 | | alive[e] = replaced > 0 && detour <= replaced * maxDetour; |
| | 16 | 531 | | } |
| | | 532 | | |
| | 2 | 533 | | return alive; |
| | | 534 | | |
| | | 535 | | static bool Realises(double[] field, int from, int to, double cost) => |
| | 68 | 536 | | !double.IsPositiveInfinity(field[from]) && !double.IsPositiveInfinity(field[to]) && |
| | 68 | 537 | | Math.Abs(field[to] + cost - field[from]) < 1e-6; |
| | | 538 | | |
| | | 539 | | // Walk each node to the first optimum node its descent reaches, memoised, so the whole |
| | | 540 | | // map costs one pass rather than one walk per edge. |
| | | 541 | | int[] Nearest(int[] step, List<int> t, List<int> h, bool[] optimum, bool towardOrigin) |
| | 4 | 542 | | { |
| | 4 | 543 | | var nearest = new int[nodeCount]; |
| | 4 | 544 | | Array.Fill(nearest, -2); |
| | 212 | 545 | | for (var n = 0; n < nodeCount; n++) Resolve(n); |
| | | 546 | | |
| | 4 | 547 | | return nearest; |
| | | 548 | | |
| | | 549 | | int Resolve(int node) |
| | 88 | 550 | | { |
| | 108 | 551 | | if (nearest[node] != -2) return nearest[node]; |
| | 96 | 552 | | if (optimum[node]) return nearest[node] = node; |
| | | 553 | | |
| | 40 | 554 | | nearest[node] = -1; // breaks a cycle rather than recursing into it |
| | 40 | 555 | | var e = step[node]; |
| | 60 | 556 | | if (e < 0) return nearest[node] = -1; |
| | | 557 | | |
| | 20 | 558 | | return nearest[node] = Resolve(towardOrigin ? t[e] : h[e]); |
| | 88 | 559 | | } |
| | 4 | 560 | | } |
| | 2 | 561 | | } |
| | | 562 | | |
| | | 563 | | /// <summary> |
| | | 564 | | /// Renumbers the nodes the surviving edges actually use, dropping the rest. |
| | | 565 | | /// </summary> |
| | | 566 | | /// <remarks> |
| | | 567 | | /// Numbering happens while the trace is assembled, so most entries do not survive the filter; |
| | | 568 | | /// the gaps would otherwise make every caller walk nodes no edge refers to. |
| | | 569 | | /// </remarks> |
| | | 570 | | private static (List<CorridorState?> states, List<CorridorEdge> edges, |
| | | 571 | | List<double> fromOrigin, List<double> toDestination) |
| | | 572 | | Compact(List<CorridorState?> states, List<CorridorEdge> edges, |
| | | 573 | | double[] fromOrigin, double[] toDestination) |
| | 30 | 574 | | { |
| | 30 | 575 | | var renumbered = new int[states.Count]; |
| | 30 | 576 | | Array.Fill(renumbered, -1); |
| | 30 | 577 | | renumbered[RouteCorridor.SourceNode] = RouteCorridor.SourceNode; |
| | 30 | 578 | | renumbered[RouteCorridor.TargetNode] = RouteCorridor.TargetNode; |
| | | 579 | | |
| | 30 | 580 | | var compactStates = new List<CorridorState?> { null, null }; |
| | 30 | 581 | | var compactFromOrigin = new List<double> |
| | 30 | 582 | | { fromOrigin[RouteCorridor.SourceNode], fromOrigin[RouteCorridor.TargetNode] }; |
| | 30 | 583 | | var compactToDestination = new List<double> |
| | 30 | 584 | | { toDestination[RouteCorridor.SourceNode], toDestination[RouteCorridor.TargetNode] }; |
| | 30 | 585 | | var compactEdges = new List<CorridorEdge>(edges.Count); |
| | 600 | 586 | | foreach (var edge in edges) |
| | 255 | 587 | | { |
| | 255 | 588 | | compactEdges.Add(edge with { Tail = Renumber(edge.Tail), Head = Renumber(edge.Head) }); |
| | 255 | 589 | | } |
| | | 590 | | |
| | 30 | 591 | | return (compactStates, compactEdges, compactFromOrigin, compactToDestination); |
| | | 592 | | |
| | | 593 | | int Renumber(int node) |
| | 510 | 594 | | { |
| | 832 | 595 | | if (renumbered[node] >= 0) return renumbered[node]; |
| | | 596 | | |
| | 188 | 597 | | renumbered[node] = compactStates.Count; |
| | 188 | 598 | | compactStates.Add(states[node]); |
| | 188 | 599 | | compactFromOrigin.Add(fromOrigin[node]); |
| | 188 | 600 | | compactToDestination.Add(toDestination[node]); |
| | 188 | 601 | | return renumbered[node]; |
| | 510 | 602 | | } |
| | 30 | 603 | | } |
| | | 604 | | } |