| | | 1 | | using System; |
| | | 2 | | using System.Collections.Generic; |
| | | 3 | | using System.Linq; |
| | | 4 | | using System.Runtime.CompilerServices; |
| | | 5 | | using System.Threading; |
| | | 6 | | using System.Threading.Tasks; |
| | | 7 | | using Itinero.Network; |
| | | 8 | | using Itinero.Routes.Paths; |
| | | 9 | | using Itinero.Routing.DataStructures; |
| | | 10 | | using Itinero.Snapping; |
| | | 11 | | |
| | | 12 | | [assembly: InternalsVisibleTo("Itinero.Tests")] |
| | | 13 | | [assembly: InternalsVisibleTo("Itinero.Tests.Benchmarks")] |
| | | 14 | | [assembly: InternalsVisibleTo("Itinero.Tests.Functional")] |
| | | 15 | | |
| | | 16 | | namespace Itinero.Routing.Flavours.Dijkstra; |
| | | 17 | | |
| | | 18 | | /// <summary> |
| | | 19 | | /// An edge-based dijkstra implementation. |
| | | 20 | | /// |
| | | 21 | | /// When <c>isMainN</c> is supplied to <see cref="RunAsync"/>, the search is access-aware: |
| | | 22 | | /// each label carries a sticky <c>leftMain</c> bit that flips true on the first |
| | | 23 | | /// main → non-main transition, after which the search refuses to relax onto any |
| | | 24 | | /// edge classified as main-N. This enforces the "L-edge is fine iff it is the |
| | | 25 | | /// only route to destination/origin" rule structurally — main-N may only host |
| | | 26 | | /// a contiguous middle segment of the path. See the design note for the five |
| | | 27 | | /// valid path shapes this admits. |
| | | 28 | | /// |
| | | 29 | | /// When <c>isMainN</c> is null the search reduces to the classic edge-based |
| | | 30 | | /// Dijkstra: <c>leftMain</c> stays false everywhere and no relaxations are |
| | | 31 | | /// rejected on access grounds. |
| | | 32 | | /// </summary> |
| | | 33 | | internal class Dijkstra |
| | | 34 | | { |
| | 25 | 35 | | private readonly PathTree _tree = new(); |
| | 25 | 36 | | private readonly HashSet<(EdgeId edgeId, VertexId vertexId, bool leftMain)> _visits = new(); |
| | | 37 | | // Carries g (cost so far) because the heap is ordered by f = g + h; relaxation |
| | | 38 | | // and target costs need g, and recovering it at pop time would cost a tile lookup. |
| | 25 | 39 | | private readonly BinaryHeap<(uint pointer, EdgeId edgeId, VertexId vertexId, bool leftMain, double g)> _heap = new() |
| | | 40 | | |
| | | 41 | | public async Task<(Path? path, double cost)> RunAsync(RoutingNetwork network, SnapPoint source, |
| | | 42 | | SnapPoint target, |
| | | 43 | | DijkstraWeightFunc getDijkstraWeight, |
| | | 44 | | Func<(EdgeId edgeId, VertexId vertexId, double cost), Task<bool>>? settled = null, |
| | | 45 | | Func<(EdgeId edgeId, VertexId vertexId), Task<bool>>? queued = null, |
| | | 46 | | CancellationToken cancellationToken = default, |
| | | 47 | | IsMainNFunc? isMainN = null, |
| | | 48 | | HeuristicFunc? heuristic = null) |
| | 0 | 49 | | { |
| | 0 | 50 | | var paths = await this.RunAsync(network, (source, null), new[] { (target, (bool?)null) }, getDijkstraWeight, |
| | 0 | 51 | | settled, queued, cancellationToken, isMainN, heuristic); |
| | | 52 | | |
| | 0 | 53 | | return paths.Length < 1 ? (null, double.MaxValue) : paths[0]; |
| | 0 | 54 | | } |
| | | 55 | | |
| | | 56 | | public async Task<(Path? path, double cost)> RunAsync(RoutingNetwork network, |
| | | 57 | | (SnapPoint sp, bool? direction) source, |
| | | 58 | | (SnapPoint sp, bool? direction) target, |
| | | 59 | | DijkstraWeightFunc getDijkstraWeight, |
| | | 60 | | Func<(EdgeId edgeId, VertexId vertexId, double cost), Task<bool>>? settled = null, |
| | | 61 | | Func<(EdgeId edgeId, VertexId vertexId), Task<bool>>? queued = null, |
| | | 62 | | CancellationToken cancellationToken = default, |
| | | 63 | | IsMainNFunc? isMainN = null, |
| | | 64 | | HeuristicFunc? heuristic = null) |
| | 22 | 65 | | { |
| | 22 | 66 | | var paths = await this.RunAsync(network, source, new[] { target }, getDijkstraWeight, settled, queued, cancellat |
| | | 67 | | |
| | 22 | 68 | | return paths.Length < 1 ? (null, double.MaxValue) : paths[0]; |
| | 22 | 69 | | } |
| | | 70 | | |
| | | 71 | | public async Task<(Path? path, double cost)[]> RunAsync(RoutingNetwork network, SnapPoint source, |
| | | 72 | | IReadOnlyList<SnapPoint> targets, |
| | | 73 | | DijkstraWeightFunc getDijkstraWeight, |
| | | 74 | | Func<(EdgeId edgeId, VertexId vertexId, double cost), Task<bool>>? settled = null, |
| | | 75 | | Func<(EdgeId edgeId, VertexId vertexId), Task<bool>>? queued = null, |
| | | 76 | | CancellationToken cancellationToken = default, |
| | | 77 | | IsMainNFunc? isMainN = null, |
| | | 78 | | HeuristicFunc? heuristic = null) |
| | 11542 | 79 | | { |
| | 11542 | 80 | | var directedTargets = new (SnapPoint sp, bool? direction)[targets.Count]; |
| | 402636 | 81 | | for (var i = 0; i < targets.Count; i++) |
| | 189776 | 82 | | { |
| | 189776 | 83 | | directedTargets[i] = (targets[i], null); |
| | 189776 | 84 | | } |
| | | 85 | | |
| | 11542 | 86 | | return await this.RunAsync(network, (source, null), directedTargets, |
| | 11542 | 87 | | getDijkstraWeight, settled, queued, cancellationToken, isMainN, heuristic); |
| | 11542 | 88 | | } |
| | | 89 | | |
| | | 90 | | /// <summary> |
| | | 91 | | /// |
| | | 92 | | /// </summary> |
| | | 93 | | /// <param name="network"></param> |
| | | 94 | | /// <param name="source"></param> |
| | | 95 | | /// <param name="targets"></param> |
| | | 96 | | /// <param name="getDijkstraWeight"></param> |
| | | 97 | | /// <param name="settled"> |
| | | 98 | | /// Called once per state whose minimal cost is final, with that cost. Return true to stop |
| | | 99 | | /// expanding from it — which is how a search is cut at a cost bound, as an isochrone does. |
| | | 100 | | /// </param> |
| | | 101 | | /// <param name="queued">This callback is called before an edge is loaded. Should not be used to influence route pla |
| | | 102 | | /// <param name="isMainN">Optional access-aware predicate. When supplied the search tracks a |
| | | 103 | | /// sticky <c>leftMain</c> bit and rejects any relaxation back onto a main-N edge after the |
| | | 104 | | /// first main → non-main transition.</param> |
| | | 105 | | /// <returns></returns> |
| | | 106 | | /// <exception cref="Exception"></exception> |
| | | 107 | | public async Task<(Path? path, double cost)[]> RunAsync(RoutingNetwork network, |
| | | 108 | | (SnapPoint sp, bool? direction) source, |
| | | 109 | | IReadOnlyList<(SnapPoint sp, bool? direction)> targets, |
| | | 110 | | DijkstraWeightFunc getDijkstraWeight, |
| | | 111 | | Func<(EdgeId edgeId, VertexId vertexId, double cost), Task<bool>>? settled = null, |
| | | 112 | | Func<(EdgeId edgeId, VertexId vertexId), Task<bool>>? queued = null, |
| | | 113 | | CancellationToken cancellationToken = default, |
| | | 114 | | IsMainNFunc? isMainN = null, |
| | | 115 | | HeuristicFunc? heuristic = null) |
| | 11588 | 116 | | { |
| | | 117 | | static double GetWorst((uint pointer, double cost)[] targets) |
| | 133862 | 118 | | { |
| | | 119 | | // No targets means nothing to improve on, so there is no cost at which the search can |
| | | 120 | | // stop early: it runs until the heap empties or `settled` prunes it. That is the |
| | | 121 | | // isochrone case. |
| | 133879 | 122 | | if (targets.Length == 0) return double.MaxValue; |
| | | 123 | | |
| | 133845 | 124 | | var worst = 0d; |
| | 776532 | 125 | | for (var i = 0; i < targets.Length; i++) |
| | 383953 | 126 | | { |
| | 383953 | 127 | | if (!(targets[i].cost > worst)) |
| | 154873 | 128 | | { |
| | 154873 | 129 | | continue; |
| | | 130 | | } |
| | | 131 | | |
| | 229080 | 132 | | worst = targets[i].cost; |
| | 229080 | 133 | | if (worst >= double.MaxValue) |
| | 129532 | 134 | | { |
| | 129532 | 135 | | break; |
| | | 136 | | } |
| | 99548 | 137 | | } |
| | | 138 | | |
| | 133845 | 139 | | return worst; |
| | 133862 | 140 | | } |
| | | 141 | | |
| | | 142 | | // Cost still to pay from the edge's head; 0 without a heuristic, making the |
| | | 143 | | // search plain Dijkstra. HeadLocation is memoised, so this adds no tile lookup. |
| | | 144 | | double Estimate(Itinero.Network.Enumerators.Edges.RoutingNetworkEdgeEnumerator e) |
| | 422635 | 145 | | { |
| | 845270 | 146 | | if (heuristic == null) return 0d; |
| | | 147 | | |
| | 0 | 148 | | var (longitude, latitude, _) = e.HeadLocation; |
| | 0 | 149 | | return heuristic(longitude, latitude); |
| | 422635 | 150 | | } |
| | | 151 | | |
| | 11588 | 152 | | var enumerator = network.GetEdgeEnumerator(); |
| | | 153 | | |
| | 11588 | 154 | | _tree.Clear(); |
| | 11588 | 155 | | _visits.Clear(); |
| | 11588 | 156 | | _heap.Clear(); |
| | | 157 | | |
| | | 158 | | // add sources. leftMain starts false on every source: the search has not yet |
| | | 159 | | // transitioned out of main-N (or has not yet entered, when source is non-main). |
| | 11588 | 160 | | var sourceForwardVisit = uint.MaxValue; |
| | 11588 | 161 | | if (source.Forward()) |
| | 11588 | 162 | | { |
| | | 163 | | // add forward. |
| | 11588 | 164 | | if (!enumerator.MoveTo(source.sp.EdgeId, true)) |
| | 0 | 165 | | { |
| | 0 | 166 | | throw new Exception($"Edge in source {source} not found!"); |
| | | 167 | | } |
| | | 168 | | |
| | 11588 | 169 | | var sourceFwd = getDijkstraWeight(enumerator, default(PreviousEdgeEnumerable)); |
| | 11588 | 170 | | if (sourceFwd.cost > 0) |
| | 11588 | 171 | | { |
| | | 172 | | // can traverse edge in the forward direction. |
| | 11588 | 173 | | var sourceOffsetCostForward = sourceFwd.cost * (1 - source.sp.OffsetFactor()); |
| | 11588 | 174 | | sourceForwardVisit = |
| | 11588 | 175 | | _tree.AddVisit(enumerator, leftMain: false, localAccess: sourceFwd.localAccess, uint.MaxValue); |
| | 11588 | 176 | | _heap.Push((sourceForwardVisit, enumerator.EdgeId, enumerator.Head, false, sourceOffsetCostForward), |
| | 11588 | 177 | | sourceOffsetCostForward + Estimate(enumerator)); |
| | 11588 | 178 | | } |
| | 11588 | 179 | | } |
| | | 180 | | |
| | 11588 | 181 | | var sourceBackwardVisit = uint.MaxValue; |
| | 11588 | 182 | | if (source.Backward()) |
| | 11583 | 183 | | { |
| | | 184 | | // add backward. |
| | 11583 | 185 | | if (!enumerator.MoveTo(source.sp.EdgeId, false)) |
| | 0 | 186 | | { |
| | 0 | 187 | | throw new Exception($"Edge in source {source} not found!"); |
| | | 188 | | } |
| | | 189 | | |
| | 11583 | 190 | | var sourceBwd = getDijkstraWeight(enumerator, default(PreviousEdgeEnumerable)); |
| | 11583 | 191 | | if (sourceBwd.cost > 0) |
| | 8360 | 192 | | { |
| | | 193 | | // can traverse edge in the backward direction. |
| | 8360 | 194 | | var sourceOffsetCostBackward = sourceBwd.cost * source.sp.OffsetFactor(); |
| | 8360 | 195 | | sourceBackwardVisit = |
| | 8360 | 196 | | _tree.AddVisit(enumerator, leftMain: false, localAccess: sourceBwd.localAccess, uint.MaxValue); |
| | 8360 | 197 | | _heap.Push((sourceBackwardVisit, enumerator.EdgeId, enumerator.Head, false, sourceOffsetCostBackward), |
| | 8360 | 198 | | sourceOffsetCostBackward + Estimate(enumerator)); |
| | 8360 | 199 | | } |
| | 11583 | 200 | | } |
| | | 201 | | |
| | | 202 | | // add targets. |
| | 11588 | 203 | | var bestTargets = new (uint pointer, double cost)[targets.Count]; |
| | 11588 | 204 | | var targetsPerVertex = new Dictionary<VertexId, List<int>>(); |
| | 402798 | 205 | | for (var t = 0; t < targets.Count; t++) |
| | 189811 | 206 | | { |
| | 189811 | 207 | | bestTargets[t] = (uint.MaxValue, double.MaxValue); |
| | 189811 | 208 | | var target = targets[t]; |
| | | 209 | | |
| | 189811 | 210 | | if (target.Forward()) |
| | 189808 | 211 | | { |
| | | 212 | | // add forward. |
| | 189808 | 213 | | if (!enumerator.MoveTo(target.sp.EdgeId, true)) |
| | 0 | 214 | | { |
| | 0 | 215 | | throw new Exception($"Edge in target {target} not found!"); |
| | | 216 | | } |
| | | 217 | | |
| | 189808 | 218 | | var targetCostForward = getDijkstraWeight(enumerator, default(PreviousEdgeEnumerable)) |
| | 189808 | 219 | | .cost; |
| | 189808 | 220 | | if (targetCostForward > 0) |
| | 189808 | 221 | | { |
| | 189808 | 222 | | if (!targetsPerVertex.TryGetValue(enumerator.Tail, out var targetsAtVertex)) |
| | 110335 | 223 | | { |
| | 110335 | 224 | | targetsAtVertex = new List<int>(); |
| | 110335 | 225 | | targetsPerVertex[enumerator.Tail] = targetsAtVertex; |
| | 110335 | 226 | | } |
| | | 227 | | |
| | 189808 | 228 | | targetsAtVertex.Add(t); |
| | 189808 | 229 | | } |
| | 189808 | 230 | | } |
| | | 231 | | |
| | 189811 | 232 | | if (target.Backward()) |
| | 189809 | 233 | | { |
| | | 234 | | // add backward. |
| | 189809 | 235 | | if (!enumerator.MoveTo(target.sp.EdgeId, false)) |
| | 0 | 236 | | { |
| | 0 | 237 | | throw new Exception($"Edge in source {source} not found!"); |
| | | 238 | | } |
| | | 239 | | |
| | 189809 | 240 | | var targetCostBackward = |
| | 189809 | 241 | | getDijkstraWeight(enumerator, default(PreviousEdgeEnumerable)).cost; |
| | 189809 | 242 | | if (targetCostBackward > 0) |
| | 102031 | 243 | | { |
| | 102031 | 244 | | if (!targetsPerVertex.TryGetValue(enumerator.Tail, out var targetsAtVertex)) |
| | 68899 | 245 | | { |
| | 68899 | 246 | | targetsAtVertex = new List<int>(); |
| | 68899 | 247 | | targetsPerVertex[enumerator.Tail] = targetsAtVertex; |
| | 68899 | 248 | | } |
| | | 249 | | |
| | 102031 | 250 | | targetsAtVertex.Add(t); |
| | 102031 | 251 | | } |
| | 189809 | 252 | | } |
| | | 253 | | |
| | | 254 | | // consider paths 'within' a single edge. |
| | 189811 | 255 | | if (source.sp.EdgeId != target.sp.EdgeId) |
| | 180026 | 256 | | { |
| | 180026 | 257 | | continue; |
| | | 258 | | } |
| | | 259 | | |
| | 9785 | 260 | | if (source.sp.Offset == target.sp.Offset) |
| | 5092 | 261 | | { |
| | | 262 | | // source and target are identical. |
| | 5092 | 263 | | if (sourceForwardVisit != uint.MaxValue && |
| | 5092 | 264 | | target.Forward()) |
| | 5091 | 265 | | { |
| | 5091 | 266 | | bestTargets[t] = (sourceForwardVisit, 0); |
| | 5091 | 267 | | } |
| | 1 | 268 | | else if (sourceBackwardVisit != uint.MaxValue && |
| | 1 | 269 | | target.Backward()) |
| | 0 | 270 | | { |
| | 0 | 271 | | bestTargets[t] = (sourceForwardVisit, 0); |
| | 0 | 272 | | } |
| | 5092 | 273 | | } |
| | 4693 | 274 | | else if (source.sp.Offset < target.sp.Offset && |
| | 4693 | 275 | | source.Forward() && target.Forward()) |
| | 2865 | 276 | | { |
| | | 277 | | // the source is earlier in the direction of the edge |
| | | 278 | | // and the edge can be traversed in this direction. |
| | 2865 | 279 | | if (!enumerator.MoveTo(source.sp.EdgeId, true)) |
| | 0 | 280 | | { |
| | 0 | 281 | | throw new Exception($"Edge in source {source} not found!"); |
| | | 282 | | } |
| | | 283 | | |
| | 2865 | 284 | | var weight = getDijkstraWeight(enumerator, default(PreviousEdgeEnumerable)).cost * |
| | 2865 | 285 | | (target.sp.OffsetFactor() - source.sp.OffsetFactor()); |
| | 2865 | 286 | | bestTargets[t] = (sourceForwardVisit, weight); |
| | 2865 | 287 | | } |
| | 1828 | 288 | | else if (source.sp.Offset > target.sp.Offset && |
| | 1828 | 289 | | source.Backward() && target.Backward()) |
| | 1825 | 290 | | { |
| | | 291 | | // the source is earlier against the direction of the edge |
| | | 292 | | // and the edge can be traversed in this direction. |
| | 1825 | 293 | | if (!enumerator.MoveTo(source.sp.EdgeId, false)) |
| | 0 | 294 | | { |
| | 0 | 295 | | throw new Exception($"Edge in source {source} not found!"); |
| | | 296 | | } |
| | | 297 | | |
| | 1825 | 298 | | var weight = getDijkstraWeight(enumerator, default(PreviousEdgeEnumerable)).cost * |
| | 1825 | 299 | | (source.sp.OffsetFactor() - target.sp.OffsetFactor()); |
| | 1825 | 300 | | bestTargets[t] = (sourceBackwardVisit, weight); |
| | 1825 | 301 | | } |
| | 9785 | 302 | | } |
| | | 303 | | |
| | | 304 | | // update worst target cost. |
| | 11588 | 305 | | var worstTargetCost = GetWorst(bestTargets); |
| | | 306 | | |
| | | 307 | | // keep going until heap is empty. |
| | 318597 | 308 | | while (_heap.Count > 0) |
| | 311513 | 309 | | { |
| | 311513 | 310 | | cancellationToken.ThrowIfCancellationRequested(); |
| | | 311 | | |
| | | 312 | | // dequeue new visit. Dedup includes leftMain because the same (edge,vertex) |
| | | 313 | | // is reachable both with leftMain=false and leftMain=true; the former is |
| | | 314 | | // strictly more permissive but both can occur in the search. |
| | 311513 | 315 | | var currentEntry = _heap.Pop(out var currentCost); |
| | 406695 | 316 | | while (_visits.Contains((currentEntry.edgeId, currentEntry.vertexId, currentEntry.leftMain))) |
| | 97173 | 317 | | { |
| | | 318 | | // visited before, skip. |
| | 97173 | 319 | | if (_heap.Count == 0) |
| | 1991 | 320 | | { |
| | 1991 | 321 | | currentEntry = (uint.MaxValue, default, default, false, 0d); |
| | 1991 | 322 | | break; |
| | | 323 | | } |
| | | 324 | | |
| | 95182 | 325 | | currentEntry = _heap.Pop(out currentCost); |
| | 95182 | 326 | | } |
| | | 327 | | |
| | 311513 | 328 | | var currentPointer = currentEntry.pointer; |
| | 311513 | 329 | | if (currentPointer == uint.MaxValue) |
| | 1991 | 330 | | { |
| | 1991 | 331 | | break; |
| | | 332 | | } |
| | | 333 | | |
| | | 334 | | // only call GetVisitWithState after the visited check passes. |
| | 309522 | 335 | | var currentVisit = _tree.GetVisitWithState(currentPointer); |
| | | 336 | | |
| | | 337 | | // log visit. |
| | 309522 | 338 | | if (currentVisit.previousPointer != uint.MaxValue) |
| | 290128 | 339 | | { |
| | 290128 | 340 | | _visits.Add((currentEntry.edgeId, currentEntry.vertexId, currentEntry.leftMain)); |
| | 290128 | 341 | | } |
| | | 342 | | |
| | 309522 | 343 | | if (settled != null && await settled((currentEntry.edgeId, currentEntry.vertexId, currentCost))) |
| | 52058 | 344 | | { |
| | | 345 | | // the best cost to this edge has already been found; current visit can not improve this anymore so we c |
| | 52058 | 346 | | continue; |
| | | 347 | | } |
| | | 348 | | |
| | | 349 | | // check if the search needs to stop. |
| | 257464 | 350 | | if (currentCost > worstTargetCost) |
| | 2513 | 351 | | { |
| | | 352 | | // impossible to improve on cost to any target. |
| | 2513 | 353 | | break; |
| | | 354 | | } |
| | | 355 | | |
| | | 356 | | // check neighbours. |
| | 254951 | 357 | | if (!enumerator.MoveTo(currentVisit.vertex)) |
| | 0 | 358 | | { |
| | | 359 | | // no edges, move on! |
| | 0 | 360 | | continue; |
| | | 361 | | } |
| | | 362 | | |
| | | 363 | | // check if this is a target. |
| | 254951 | 364 | | if (!targetsPerVertex.TryGetValue(currentVisit.vertex, out var targetsAtVertex)) |
| | 115834 | 365 | | { |
| | 115834 | 366 | | targetsAtVertex = null; |
| | 115834 | 367 | | } |
| | | 368 | | |
| | 1066791 | 369 | | while (enumerator.MoveNext()) |
| | 811840 | 370 | | { |
| | | 371 | | // filter out if u-turns or visits on the same edge. |
| | 811840 | 372 | | var neighbourEdge = enumerator.EdgeId; |
| | 811840 | 373 | | if (neighbourEdge == currentVisit.edge) |
| | 254951 | 374 | | { |
| | 254951 | 375 | | continue; |
| | | 376 | | } |
| | | 377 | | |
| | | 378 | | // gets the cost of the current edge. |
| | 556889 | 379 | | var (neighbourCost, turnCost, neighbourLocalAccess) = |
| | 556889 | 380 | | getDijkstraWeight(enumerator, new PreviousEdgeEnumerable(_tree, currentPointer)); |
| | 556889 | 381 | | if (neighbourCost is >= double.MaxValue or <= 0) |
| | 154190 | 382 | | { |
| | 154190 | 383 | | continue; |
| | | 384 | | } |
| | | 385 | | |
| | 402699 | 386 | | if (turnCost is >= double.MaxValue or < 0) |
| | 11 | 387 | | { |
| | 11 | 388 | | continue; |
| | | 389 | | } |
| | | 390 | | |
| | | 391 | | // Access-aware state-bit logic. When isMainN is null this block is a no-op: |
| | | 392 | | // newLeftMain stays false and no relaxation is rejected. |
| | 402688 | 393 | | var newLeftMain = false; |
| | 402688 | 394 | | if (isMainN != null) |
| | 402667 | 395 | | { |
| | 402667 | 396 | | var prevMain = IsMain(currentVisit.edge, currentVisit.localAccess, currentVisit.leftMain, isMainN); |
| | 402667 | 397 | | var neighbourMain = IsMain(neighbourEdge, neighbourLocalAccess, currentVisit.leftMain, isMainN); |
| | | 398 | | |
| | | 399 | | // Rule: once leftMain is true we may not relax onto a main-N edge again. |
| | | 400 | | // That would mean re-entering main-N after having departed, i.e. an L-edge |
| | | 401 | | // (or non-main-N pocket) used as through-traffic between two main-N segments. |
| | 402668 | 402 | | if (currentVisit.leftMain && neighbourMain) continue; |
| | | 403 | | |
| | | 404 | | // newLeftMain is sticky and flips on the first main-N → non-main-N transition. |
| | 402666 | 405 | | newLeftMain = currentVisit.leftMain || (prevMain && !neighbourMain); |
| | 402666 | 406 | | } |
| | | 407 | | |
| | | 408 | | // if the vertex has targets, check if this edge is a match. |
| | 402687 | 409 | | var neighbourPointer = uint.MaxValue; |
| | 402687 | 410 | | if (targetsAtVertex != null) |
| | 221010 | 411 | | { |
| | | 412 | | // only consider targets when found for the 'from' vertex. |
| | | 413 | | // and when this in not a u-turn. |
| | 1638960 | 414 | | foreach (var t in targetsAtVertex) |
| | 487965 | 415 | | { |
| | 487965 | 416 | | var target = targets[t]; |
| | 487965 | 417 | | if (target.sp.EdgeId != neighbourEdge) |
| | 297180 | 418 | | { |
| | 297180 | 419 | | continue; |
| | | 420 | | } |
| | | 421 | | |
| | | 422 | | // check directions. |
| | 190785 | 423 | | if (enumerator.Forward && !target.Forward()) |
| | 0 | 424 | | { |
| | 0 | 425 | | continue; |
| | | 426 | | } |
| | | 427 | | |
| | 190785 | 428 | | if (!enumerator.Forward && !target.Backward()) |
| | 0 | 429 | | { |
| | 0 | 430 | | continue; |
| | | 431 | | } |
| | | 432 | | |
| | | 433 | | // there is a target on this edge, calculate the cost. |
| | | 434 | | // calculate the cost from the 'from' vertex to the target. |
| | 190785 | 435 | | var targetCost = enumerator.Forward |
| | 190785 | 436 | | ? neighbourCost * target.sp.OffsetFactor() |
| | 190785 | 437 | | : neighbourCost * (1 - target.sp.OffsetFactor()); |
| | | 438 | | // this is the case where the target is on this edge |
| | | 439 | | // and there is a path to 'from' before. |
| | 190785 | 440 | | targetCost += currentEntry.g; |
| | | 441 | | |
| | 190785 | 442 | | targetCost += turnCost; |
| | | 443 | | |
| | | 444 | | // if this is an improvement, use it! |
| | 190785 | 445 | | var targetBestCost = bestTargets[t].cost; |
| | 190785 | 446 | | if (!(targetCost < targetBestCost)) |
| | 68511 | 447 | | { |
| | 68511 | 448 | | continue; |
| | | 449 | | } |
| | | 450 | | |
| | | 451 | | // this is an improvement. |
| | 122274 | 452 | | neighbourPointer = _tree.AddVisit(enumerator, newLeftMain, neighbourLocalAccess, currentPointer) |
| | 122274 | 453 | | bestTargets[t] = (neighbourPointer, targetCost); |
| | | 454 | | |
| | | 455 | | // update worst. |
| | 122274 | 456 | | worstTargetCost = GetWorst(bestTargets); |
| | 122274 | 457 | | } |
| | 221010 | 458 | | } |
| | | 459 | | |
| | 402687 | 460 | | if (queued != null && |
| | 402687 | 461 | | await queued((enumerator.EdgeId, enumerator.Head))) |
| | 0 | 462 | | { |
| | | 463 | | // don't queue this edge if the queued function returns true. |
| | 0 | 464 | | continue; |
| | | 465 | | } |
| | | 466 | | |
| | | 467 | | // add visit if not added yet. |
| | 402687 | 468 | | if (neighbourPointer == uint.MaxValue) |
| | 280416 | 469 | | { |
| | 280416 | 470 | | neighbourPointer = |
| | 280416 | 471 | | _tree.AddVisit(enumerator, newLeftMain, neighbourLocalAccess, currentPointer); |
| | 280416 | 472 | | } |
| | | 473 | | |
| | | 474 | | // add visit to heap. |
| | 402687 | 475 | | var neighbourG = neighbourCost + currentEntry.g + turnCost; |
| | 402687 | 476 | | _heap.Push((neighbourPointer, enumerator.EdgeId, enumerator.Head, newLeftMain, neighbourG), |
| | 402687 | 477 | | neighbourG + Estimate(enumerator)); |
| | 402687 | 478 | | } |
| | 254951 | 479 | | } |
| | | 480 | | |
| | 11588 | 481 | | var paths = new (Path? path, double cost)[targets.Count]; |
| | 402798 | 482 | | for (var p = 0; p < paths.Length; p++) |
| | 189811 | 483 | | { |
| | 189811 | 484 | | var bestTarget = bestTargets[p]; |
| | 189811 | 485 | | if (bestTarget.pointer == uint.MaxValue) |
| | 59497 | 486 | | { |
| | 59497 | 487 | | paths[p] = (null, double.MaxValue); |
| | 59497 | 488 | | continue; |
| | | 489 | | } |
| | | 490 | | |
| | | 491 | | // build resulting path. |
| | 130314 | 492 | | var path = new Path(network); |
| | 130314 | 493 | | var visit = _tree.GetVisit(bestTarget.pointer); |
| | | 494 | | |
| | | 495 | | // path is at least two edges. |
| | 711685 | 496 | | while (true) |
| | 711685 | 497 | | { |
| | 711685 | 498 | | if (visit.previousPointer == uint.MaxValue) |
| | 130314 | 499 | | { |
| | 130314 | 500 | | enumerator.MoveTo(visit.edge); |
| | 130314 | 501 | | path.Prepend(visit.edge, visit.forward); |
| | 130314 | 502 | | break; |
| | | 503 | | } |
| | | 504 | | |
| | 581371 | 505 | | path.Prepend(visit.edge, visit.forward); |
| | 581371 | 506 | | visit = _tree.GetVisit(visit.previousPointer); |
| | 581371 | 507 | | } |
| | | 508 | | |
| | | 509 | | // add the offsets. |
| | 130314 | 510 | | var target = targets[p]; |
| | 130314 | 511 | | path.Offset1 = path.First.direction ? source.sp.Offset : (ushort)(ushort.MaxValue - source.sp.Offset); |
| | 130314 | 512 | | path.Offset2 = path.Last.direction |
| | 130314 | 513 | | ? target.sp.Offset |
| | 130314 | 514 | | : (ushort)(ushort.MaxValue - target.sp.Offset); |
| | | 515 | | |
| | 130314 | 516 | | paths[p] = (path, bestTarget.cost); |
| | 130314 | 517 | | } |
| | | 518 | | |
| | | 519 | | |
| | 11588 | 520 | | return paths; |
| | 11588 | 521 | | } |
| | | 522 | | |
| | | 523 | | /// <summary> |
| | | 524 | | /// Hybrid "is main-N?" predicate. Consults <paramref name="isMainN"/> first; when that |
| | | 525 | | /// returns null (the tile isn't classified yet) falls back to <c>!leftMain</c>. The fallback |
| | | 526 | | /// is path-dependent: while we still believe we are in main (<c>leftMain == false</c>) an |
| | | 527 | | /// unclassified edge is treated as main, so no spurious leftMain transition fires; once we |
| | | 528 | | /// have left main, an unclassified edge is treated as non-main, so no spurious re-entry |
| | | 529 | | /// rejection fires. |
| | | 530 | | /// </summary> |
| | | 531 | | private static bool IsMain(EdgeId edgeId, bool localAccess, bool leftMain, IsMainNFunc isMainN) |
| | 805334 | 532 | | { |
| | 805334 | 533 | | var verdict = isMainN(edgeId, localAccess); |
| | 805334 | 534 | | return verdict ?? !leftMain; |
| | 805334 | 535 | | } |
| | | 536 | | |
| | | 537 | | /// <summary> |
| | | 538 | | /// Gets a fresh Dijkstra instance for this call. Was previously a [ThreadStatic] |
| | | 539 | | /// reuse, but that's unsafe under async/await + ThreadPool reuse: thread A starts |
| | | 540 | | /// a routing call on instance D, awaits a callback, returns to the pool, picks up |
| | | 541 | | /// a second routing call — Default returns the same D, and now two routing calls |
| | | 542 | | /// mutate D's _heap/_visits/_tree concurrently → "concurrent update" crash. |
| | | 543 | | /// Allocating three small collections per call is cheap vs. the routing work. |
| | | 544 | | /// </summary> |
| | 24 | 545 | | public static Dijkstra Default => new(); |
| | | 546 | | } |