| | | 1 | | using System; |
| | | 2 | | using System.Collections.Generic; |
| | | 3 | | using System.Threading; |
| | | 4 | | using System.Threading.Tasks; |
| | | 5 | | using Itinero.Geo; |
| | | 6 | | using Itinero.Network; |
| | | 7 | | using Itinero.Network.Enumerators.Edges; |
| | | 8 | | using Itinero.Network.Search.Edges; |
| | | 9 | | using Itinero.Network.Search.Reachability; |
| | | 10 | | using Itinero.Network.Tiles; |
| | | 11 | | using Itinero.Routes.Paths; |
| | | 12 | | using Itinero.Routing.Costs; |
| | | 13 | | using Itinero.Routing.Flavours.Dijkstra; |
| | | 14 | | using Itinero.Routing.Flavours.Dijkstra.Bidirectional; |
| | | 15 | | using Itinero.Snapping; |
| | | 16 | | |
| | | 17 | | namespace Itinero.Routing; |
| | | 18 | | |
| | | 19 | | /// <summary> |
| | | 20 | | /// Routes between two locations, doing the snapping and the routing as one operation. |
| | | 21 | | /// </summary> |
| | | 22 | | /// <remarks> |
| | | 23 | | /// Each candidate is tried by expanding a <see cref="SearchHalf"/> from it, and that same half is |
| | | 24 | | /// what the search continues with, so the connectivity check costs nothing the route would not. |
| | | 25 | | /// </remarks> |
| | | 26 | | /// <remarks> |
| | | 27 | | /// One-to-one only: a half is consumed by the search that adopts it, so sharing an origin across |
| | | 28 | | /// destinations would mean copying it. |
| | | 29 | | /// </remarks> |
| | | 30 | | public static class ReachabilityRouter |
| | | 31 | | { |
| | | 32 | | |
| | | 33 | | /// <summary> |
| | | 34 | | /// Calculates a path between two locations. |
| | | 35 | | /// </summary> |
| | | 36 | | /// <param name="network">The network.</param> |
| | | 37 | | /// <param name="settings">Routing settings; carries the profile.</param> |
| | | 38 | | /// <param name="origin">Where the route starts, as longitude/latitude.</param> |
| | | 39 | | /// <param name="destination">Where it ends.</param> |
| | | 40 | | /// <param name="bounds">How much network counts as connected, and the work ceiling.</param> |
| | | 41 | | /// <param name="searchBoxMeters">Half-size of the box searched for candidate edges.</param> |
| | | 42 | | /// <param name="cancellationToken">Cancellation.</param> |
| | | 43 | | /// <returns>The path, and the two snap points it was built between.</returns> |
| | | 44 | | public static async Task<Result<ReachabilityRoute>> PathAsync( |
| | | 45 | | this RoutingNetwork network, |
| | | 46 | | RoutingSettings settings, |
| | | 47 | | (double longitude, double latitude) origin, |
| | | 48 | | (double longitude, double latitude) destination, |
| | | 49 | | ReachabilityBounds bounds, |
| | | 50 | | double searchBoxMeters, |
| | | 51 | | CancellationToken cancellationToken = default) |
| | | 52 | | { |
| | | 53 | | var resolved = await network.ResolveAsync(settings, origin, destination, bounds, |
| | | 54 | | searchBoxMeters, pruneFactor: 1.0, onForwardRelaxed: null, onBackwardRelaxed: null, |
| | | 55 | | cancellationToken); |
| | | 56 | | if (resolved.IsError) return new Result<ReachabilityRoute>(resolved.ErrorMessage); |
| | | 57 | | |
| | | 58 | | var r = resolved.Value; |
| | | 59 | | return new ReachabilityRoute(r.Path, r.Source, r.Target); |
| | | 60 | | } |
| | | 61 | | |
| | | 62 | | /// <summary> |
| | | 63 | | /// Resolves, and keeps relaxing past the best meeting so a caller can map the region around |
| | | 64 | | /// the route out of the same search. Collected here because a pruned transition's tail is |
| | | 65 | | /// settled and never expanded again, so the band holding the alternatives cannot be re-found. |
| | | 66 | | /// </summary> |
| | | 67 | | /// <param name="pruneFactor">How far past the best meeting to go. 1.0 is an ordinary route.</param> |
| | | 68 | | internal static async Task<Result<ResolvedRoute>> ResolveAsync( |
| | | 69 | | this RoutingNetwork network, |
| | | 70 | | RoutingSettings settings, |
| | | 71 | | (double longitude, double latitude) origin, |
| | | 72 | | (double longitude, double latitude) destination, |
| | | 73 | | ReachabilityBounds bounds, |
| | | 74 | | double searchBoxMeters, |
| | | 75 | | double pruneFactor, |
| | | 76 | | RelaxedCallback? onForwardRelaxed, |
| | | 77 | | RelaxedCallback? onBackwardRelaxed, |
| | | 78 | | CancellationToken cancellationToken = default) |
| | | 79 | | { |
| | | 80 | | var profile = settings.Profile; |
| | | 81 | | var costFunction = network.GetCostFunctionFor(profile); |
| | | 82 | | if (settings.CostFunctionWrapper != null) costFunction = settings.CostFunctionWrapper(costFunction); |
| | | 83 | | |
| | | 84 | | var accessChecker = (IEdgeChecker)network.Snap([profile], s => |
| | | 85 | | { |
| | | 86 | | s.CheckIslands = false; |
| | | 87 | | s.CheckCanStopOn = false; |
| | | 88 | | s.OffsetInMeter = searchBoxMeters; |
| | | 89 | | s.OffsetInMeterMax = searchBoxMeters; |
| | | 90 | | s.MaxDistance = double.MaxValue; |
| | | 91 | | }); |
| | | 92 | | |
| | | 93 | | await network.UsageNotifier.NotifyBox(network, BoxFor(origin, searchBoxMeters), cancellationToken); |
| | | 94 | | await network.UsageNotifier.NotifyBox(network, BoxFor(destination, searchBoxMeters), cancellationToken); |
| | | 95 | | |
| | | 96 | | await using var originCandidates = network.NearestCandidatesAsync( |
| | | 97 | | BoxFor(origin, searchBoxMeters), accessChecker, searchBoxMeters, cancellationToken) |
| | | 98 | | .GetAsyncEnumerator(cancellationToken); |
| | | 99 | | await using var destinationCandidates = network.NearestCandidatesAsync( |
| | | 100 | | BoxFor(destination, searchBoxMeters), accessChecker, searchBoxMeters, cancellationToken) |
| | | 101 | | .GetAsyncEnumerator(cancellationToken); |
| | | 102 | | |
| | | 103 | | // Edges no longer worth snapping to, per direction, from the settled set of a half that |
| | | 104 | | // ran out of network: that set is closed, so every edge in it leads exactly as far. |
| | | 105 | | var deadForOrigin = new HashSet<EdgeId>(); |
| | | 106 | | var deadForDestination = new HashSet<EdgeId>(); |
| | | 107 | | |
| | | 108 | | var sourceSnap = await NextSnapAsync(network, costFunction, originCandidates, deadForOrigin); |
| | | 109 | | if (sourceSnap == null) |
| | | 110 | | { |
| | | 111 | | return new Result<ResolvedRoute>(FormattableString.Invariant( |
| | | 112 | | $"Could not snap origin to connected network: {origin.longitude},{origin.latitude}")); |
| | | 113 | | } |
| | | 114 | | |
| | | 115 | | var targetSnap = await NextSnapAsync(network, costFunction, destinationCandidates, deadForDestination); |
| | | 116 | | if (targetSnap == null) |
| | | 117 | | { |
| | | 118 | | return new Result<ResolvedRoute>(FormattableString.Invariant( |
| | | 119 | | $"Could not snap destination to connected network: {destination.longitude},{destination.latitude}")); |
| | | 120 | | } |
| | | 121 | | |
| | | 122 | | SearchHalf? forwardHalf = null; |
| | | 123 | | SearchHalf? backwardHalf = null; |
| | | 124 | | |
| | | 125 | | // A stranded candidate gets a second attempt with its pocket relabelled before we give up |
| | | 126 | | // on it and move further away. One attempt per candidate; null means no pocket yet. |
| | | 127 | | ICostFunction? forwardCostFn = null; |
| | | 128 | | ICostFunction? backwardCostFn = null; |
| | | 129 | | var forwardPocketTried = false; |
| | | 130 | | var backwardPocketTried = false; |
| | | 131 | | var pocketBound = network.IslandManager.MaxIslandSize; |
| | | 132 | | |
| | | 133 | | var maxBox = settings.MaxBoxFor(network, [sourceSnap.Value, targetSnap.Value]); |
| | | 134 | | |
| | | 135 | | // Goal direction, the same balanced potential the island-detection router uses. It depends |
| | | 136 | | // on both snap locations, so a candidate advance invalidates it and it is rebuilt below. |
| | | 137 | | var potential = IRouterOneToOneExtensions.BuildBalancedPotential( |
| | | 138 | | network, profile, sourceSnap.Value, targetSnap.Value); |
| | | 139 | | |
| | | 140 | | ReachabilityCounters.CountRoute(); |
| | | 141 | | var settledBefore = 0L; |
| | | 142 | | var routeSettled = 0L; |
| | | 143 | | var retried = false; |
| | | 144 | | |
| | | 145 | | try |
| | | 146 | | { |
| | | 147 | | while (true) |
| | | 148 | | { |
| | | 149 | | cancellationToken.ThrowIfCancellationRequested(); |
| | | 150 | | |
| | | 151 | | // A half whose endpoint did not move is kept, settled set and frontier intact, and |
| | | 152 | | // carries on from where it stopped. Only the end that failed starts over. |
| | | 153 | | forwardHalf ??= NewHalf(network, forwardCostFn ?? costFunction, sourceSnap.Value, true); |
| | | 154 | | backwardHalf ??= NewHalf(network, backwardCostFn ?? costFunction, targetSnap.Value, false); |
| | | 155 | | |
| | | 156 | | var (path, pathCost) = await RouteAsync(forwardHalf, backwardHalf, |
| | | 157 | | sourceSnap.Value, targetSnap.Value, potential); |
| | | 158 | | |
| | | 159 | | // Settled counts are cumulative per half and a surviving half keeps growing across |
| | | 160 | | // retries, so charge the increment rather than the total. |
| | | 161 | | var settledNow = (long)forwardHalf.Settled.Count + backwardHalf.Settled.Count; |
| | | 162 | | var settledDelta = settledNow >= settledBefore ? settledNow - settledBefore : settledNow; |
| | | 163 | | ReachabilityCounters.CountSettled(settledDelta); |
| | | 164 | | routeSettled += settledDelta; |
| | | 165 | | settledBefore = settledNow; |
| | | 166 | | |
| | | 167 | | if (path != null) |
| | | 168 | | { |
| | | 169 | | return new ResolvedRoute(path, pathCost, sourceSnap.Value, targetSnap.Value, |
| | | 170 | | forwardCostFn ?? costFunction, backwardCostFn ?? costFunction, routeSettled); |
| | | 171 | | } |
| | | 172 | | |
| | | 173 | | var originStranded = Stranded(forwardHalf, bounds.Threshold); |
| | | 174 | | var destinationStranded = Stranded(backwardHalf, bounds.Threshold); |
| | | 175 | | |
| | | 176 | | // Both halves got past the threshold and still did not meet: there is plenty of |
| | | 177 | | // network at both ends and it does not join up. Trying other edges nearby cannot |
| | | 178 | | // change that. |
| | | 179 | | if (!originStranded && !destinationStranded) |
| | | 180 | | { |
| | | 181 | | return new Result<ResolvedRoute>("Path not found"); |
| | | 182 | | } |
| | | 183 | | |
| | | 184 | | if (originStranded && !forwardPocketTried) |
| | | 185 | | { |
| | | 186 | | forwardPocketTried = true; |
| | | 187 | | ReachabilityCounters.CountPocketComposed(); |
| | | 188 | | var pocket = LocalPocket.Compose(network, costFunction, |
| | | 189 | | SettledEdges(forwardHalf), asOrigin: true, |
| | | 190 | | componentAllowance: pocketBound, budget: pocketBound); |
| | | 191 | | |
| | | 192 | | if (!pocket.Abandoned) |
| | | 193 | | { |
| | | 194 | | // Same candidate, second attempt: the pocket's untagged members now read as |
| | | 195 | | // local-access, so the half can cross them to reach its way out. |
| | | 196 | | forwardCostFn = new PocketCostFunction(costFunction, pocket.Members); |
| | | 197 | | forwardHalf = null; |
| | | 198 | | retried = true; |
| | | 199 | | continue; |
| | | 200 | | } |
| | | 201 | | |
| | | 202 | | ReachabilityCounters.CountPocketAbandoned(); |
| | | 203 | | } |
| | | 204 | | |
| | | 205 | | if (originStranded) |
| | | 206 | | { |
| | | 207 | | foreach (var edge in SettledEdges(forwardHalf)) deadForOrigin.Add(edge); |
| | | 208 | | sourceSnap = await NextSnapAsync(network, costFunction, originCandidates, deadForOrigin); |
| | | 209 | | if (sourceSnap == null) |
| | | 210 | | { |
| | | 211 | | return new Result<ResolvedRoute>(FormattableString.Invariant( |
| | | 212 | | $"Could not snap origin to connected network: {origin.longitude},{origin.latitude}")); |
| | | 213 | | } |
| | | 214 | | |
| | | 215 | | ReachabilityCounters.CountOriginAdvance(); |
| | | 216 | | ReachabilityCounters.CountWasted(forwardHalf.Settled.Count); |
| | | 217 | | retried = true; |
| | | 218 | | forwardHalf = null; |
| | | 219 | | forwardCostFn = null; |
| | | 220 | | forwardPocketTried = false; |
| | | 221 | | } |
| | | 222 | | |
| | | 223 | | if (destinationStranded && !backwardPocketTried) |
| | | 224 | | { |
| | | 225 | | backwardPocketTried = true; |
| | | 226 | | ReachabilityCounters.CountPocketComposed(); |
| | | 227 | | var pocket = LocalPocket.Compose(network, costFunction, |
| | | 228 | | SettledEdges(backwardHalf), asOrigin: false, |
| | | 229 | | componentAllowance: pocketBound, budget: pocketBound); |
| | | 230 | | |
| | | 231 | | if (!pocket.Abandoned) |
| | | 232 | | { |
| | | 233 | | backwardCostFn = new PocketCostFunction(costFunction, pocket.Members); |
| | | 234 | | backwardHalf = null; |
| | | 235 | | retried = true; |
| | | 236 | | continue; |
| | | 237 | | } |
| | | 238 | | |
| | | 239 | | ReachabilityCounters.CountPocketAbandoned(); |
| | | 240 | | } |
| | | 241 | | |
| | | 242 | | if (destinationStranded) |
| | | 243 | | { |
| | | 244 | | foreach (var edge in SettledEdges(backwardHalf)) deadForDestination.Add(edge); |
| | | 245 | | targetSnap = await NextSnapAsync(network, costFunction, destinationCandidates, deadForDestination); |
| | | 246 | | if (targetSnap == null) |
| | | 247 | | { |
| | | 248 | | return new Result<ResolvedRoute>(FormattableString.Invariant( |
| | | 249 | | $"Could not snap destination to connected network: {destination.longitude},{destination.lati |
| | | 250 | | } |
| | | 251 | | |
| | | 252 | | ReachabilityCounters.CountDestinationAdvance(); |
| | | 253 | | ReachabilityCounters.CountWasted(backwardHalf.Settled.Count); |
| | | 254 | | retried = true; |
| | | 255 | | backwardHalf = null; |
| | | 256 | | backwardCostFn = null; |
| | | 257 | | backwardPocketTried = false; |
| | | 258 | | } |
| | | 259 | | |
| | | 260 | | maxBox = settings.MaxBoxFor(network, [sourceSnap.Value, targetSnap.Value]); |
| | | 261 | | |
| | | 262 | | // An endpoint moved, so the goal direction did too. The surviving half's frontier |
| | | 263 | | // is keyed on the old one; the search re-keys it from stored costs on the next |
| | | 264 | | // call, which is why rebuilding here rather than adjusting in place is safe. |
| | | 265 | | potential = IRouterOneToOneExtensions.BuildBalancedPotential( |
| | | 266 | | network, profile, sourceSnap.Value, targetSnap.Value); |
| | | 267 | | } |
| | | 268 | | } |
| | | 269 | | finally |
| | | 270 | | { |
| | | 271 | | // Every exit from the loop lands here, including the failures — a route that ran |
| | | 272 | | // out of candidates is exactly the kind the retry accounting must not omit. |
| | | 273 | | ReachabilityCounters.CountRouteOutcome(retried, routeSettled); |
| | | 274 | | } |
| | | 275 | | |
| | | 276 | | async Task<(Path? path, double cost)> RouteAsync(SearchHalf fwd, SearchHalf bwd, |
| | | 277 | | SnapPoint from, SnapPoint to, HeuristicFunc? goalDirection) |
| | | 278 | | { |
| | | 279 | | ReachabilityCounters.CountSearch(); |
| | | 280 | | var search = SearchPool<BidirectionalDijkstra>.Rent(); |
| | | 281 | | try |
| | | 282 | | { |
| | | 283 | | return await search.ContinueAsync(network, fwd, bwd, from, to, costFunction, |
| | | 284 | | SettleAsync, cancellationToken, isMainN: null, potential: goalDirection, |
| | | 285 | | localAccessRule: true, |
| | | 286 | | forwardCostFunction: forwardCostFn, backwardCostFunction: backwardCostFn, |
| | | 287 | | pruneFactor: pruneFactor, |
| | | 288 | | onForwardRelaxed: onForwardRelaxed, onBackwardRelaxed: onBackwardRelaxed); |
| | | 289 | | } |
| | | 290 | | finally |
| | | 291 | | { |
| | | 292 | | SearchPool<BidirectionalDijkstra>.Return(search); |
| | | 293 | | } |
| | | 294 | | } |
| | | 295 | | |
| | | 296 | | async Task<bool> SettleAsync(VertexId v) |
| | | 297 | | { |
| | | 298 | | if (!network.UsageNotifier.IsVertexDataReady(network, v)) |
| | | 299 | | { |
| | | 300 | | await network.UsageNotifier.NotifyVertex(network, v, cancellationToken); |
| | | 301 | | } |
| | | 302 | | |
| | | 303 | | if (cancellationToken.IsCancellationRequested) return false; |
| | | 304 | | if (maxBox == null) return false; |
| | | 305 | | |
| | | 306 | | return !maxBox.Value.Overlaps(network.GetVertex(v)); |
| | | 307 | | } |
| | | 308 | | } |
| | | 309 | | |
| | | 310 | | /// A half seeded at a snap point. |
| | | 311 | | private static SearchHalf NewHalf(RoutingNetwork network, ICostFunction costFunction, |
| | | 312 | | SnapPoint snap, bool isForwardHalf) |
| | | 313 | | { |
| | | 314 | | var half = new SearchHalf(isForwardHalf); |
| | | 315 | | half.PushTerminal(network.GetEdgeEnumerator(), snap, costFunction, potential: null); |
| | | 316 | | return half; |
| | | 317 | | } |
| | | 318 | | |
| | | 319 | | /// <summary> |
| | | 320 | | /// Whether this half ran out of network rather than being stopped by anything else. |
| | | 321 | | /// </summary> |
| | | 322 | | /// <remarks> |
| | | 323 | | /// An empty heap below the threshold means everything reachable has been seen and there is |
| | | 324 | | /// not enough of it — the same verdict the candidate evaluation would reach. |
| | | 325 | | /// </remarks> |
| | | 326 | | private static bool Stranded(SearchHalf half, int threshold) => |
| | | 327 | | half.Heap.Count == 0 && half.Settled.Count < threshold; |
| | | 328 | | |
| | | 329 | | /// The box searched for candidate edges around a location. |
| | | 330 | | private static ((double longitude, double, float? e) topLeft, |
| | | 331 | | (double longitude, double latitude, float? e) bottomRight) BoxFor( |
| | | 332 | | (double longitude, double latitude) location, double searchBoxMeters) |
| | | 333 | | { |
| | | 334 | | (double longitude, double latitude, float? e) at = (location.longitude, location.latitude, null); |
| | | 335 | | return at.BoxAround(searchBoxMeters); |
| | | 336 | | } |
| | | 337 | | |
| | | 338 | | /// <summary> |
| | | 339 | | /// The next candidate that is not already known to be hopeless. |
| | | 340 | | /// </summary> |
| | | 341 | | /// <remarks> |
| | | 342 | | /// Skips everything the failed search settled: that set is closed and under threshold, so one |
| | | 343 | | /// failed search dismisses the whole dead end rather than one edge of it. |
| | | 344 | | /// </remarks> |
| | | 345 | | private static async Task<SnapPoint?> NextSnapAsync( |
| | | 346 | | RoutingNetwork network, |
| | | 347 | | ICostFunction costFunction, |
| | | 348 | | IAsyncEnumerator<SnapPoint> candidates, |
| | | 349 | | HashSet<EdgeId> dead) |
| | | 350 | | { |
| | | 351 | | var enumerator = network.GetEdgeEnumerator(); |
| | | 352 | | while (await candidates.MoveNextAsync()) |
| | | 353 | | { |
| | | 354 | | var candidate = candidates.Current; |
| | | 355 | | if (dead.Contains(candidate.EdgeId)) continue; |
| | | 356 | | |
| | | 357 | | // Only "can the profile use this edge at all". Whether it leads anywhere is settled by |
| | | 358 | | // the search itself, and only paid for when it turns out not to. |
| | | 359 | | if (!Routable(enumerator, costFunction, candidate.EdgeId)) continue; |
| | | 360 | | |
| | | 361 | | return candidate; |
| | | 362 | | } |
| | | 363 | | |
| | | 364 | | return null; |
| | | 365 | | } |
| | | 366 | | |
| | | 367 | | /// Whether the (masked) cost function can traverse this edge at all. |
| | | 368 | | private static bool Routable(RoutingNetworkEdgeEnumerator enumerator, ICostFunction routable, |
| | | 369 | | EdgeId edgeId) |
| | | 370 | | { |
| | | 371 | | foreach (var forward in new[] { true, false }) |
| | | 372 | | { |
| | | 373 | | if (!enumerator.MoveTo(edgeId, forward)) continue; |
| | | 374 | | |
| | | 375 | | var (canAccess, _, _, cost, _) = routable.Get(enumerator, tailToHead: true); |
| | | 376 | | if (canAccess && cost > 0) return true; |
| | | 377 | | } |
| | | 378 | | |
| | | 379 | | return false; |
| | | 380 | | } |
| | | 381 | | |
| | | 382 | | /// The distinct edges a half has settled. |
| | | 383 | | private static HashSet<EdgeId> SettledEdges(SearchHalf half) |
| | | 384 | | { |
| | | 385 | | var edges = new HashSet<EdgeId>(); |
| | | 386 | | foreach (var (edge, _) in half.Settled) edges.Add(edge); |
| | | 387 | | return edges; |
| | | 388 | | } |
| | | 389 | | |
| | | 390 | | |
| | | 391 | | } |
| | | 392 | | |
| | | 393 | | /// <summary> |
| | | 394 | | /// A path, together with the snap points chosen for it. |
| | | 395 | | /// </summary> |
| | | 396 | | /// <remarks> |
| | | 397 | | /// The snap points come back with the path because nobody else computed them — snapping happened |
| | | 398 | | /// inside, as part of deciding which candidate edges a route can use. |
| | | 399 | | /// </remarks> |
| | | 400 | | /// <param name="Path">The path found.</param> |
| | | 401 | | /// <param name="Source">Where the origin snapped.</param> |
| | | 402 | | /// <param name="Target">Where the destination snapped.</param> |
| | 1 | 403 | | public readonly record struct ReachabilityRoute(Path Path, SnapPoint Source, SnapPoint Target); |
| | | 404 | | |
| | | 405 | | /// <summary> |
| | | 406 | | /// A route plus what it was resolved against: the endpoints snapped to in the end, and the cost |
| | | 407 | | /// functions those searches actually ran with — not the profile's, which may have been wrapped. |
| | | 408 | | /// </summary> |
| | | 409 | | /// <param name="Cost">As the search computed it; re-summing the path would have to reapply turn |
| | | 410 | | /// costs and any wrapper in the same order.</param> |
| | | 411 | | /// <param name="Settled">States both halves settled, across candidate retries. Diagnostic.</param> |
| | | 412 | | public readonly record struct ResolvedRoute( |
| | | 413 | | Path Path, |
| | | 414 | | double Cost, |
| | | 415 | | SnapPoint Source, |
| | | 416 | | SnapPoint Target, |
| | | 417 | | ICostFunction ForwardCostFunction, |
| | | 418 | | ICostFunction BackwardCostFunction, |
| | | 419 | | long Settled); |