| | | 1 | | using System; |
| | | 2 | | using System.Collections.Generic; |
| | | 3 | | using System.Linq; |
| | | 4 | | using System.Threading; |
| | | 5 | | using System.Threading.Tasks; |
| | | 6 | | using Itinero.Geo; |
| | | 7 | | using Itinero.Network; |
| | | 8 | | using Itinero.Network.Enumerators.Edges; |
| | | 9 | | using Itinero.Routing.Costs; |
| | | 10 | | using Itinero.Routing.Flavours.Dijkstra; |
| | | 11 | | using Itinero.Snapping; |
| | | 12 | | |
| | | 13 | | namespace Itinero.Routing.Isochrones; |
| | | 14 | | |
| | | 15 | | /// <summary> |
| | | 16 | | /// Areas reachable from a point within given costs. |
| | | 17 | | /// </summary> |
| | | 18 | | /// <remarks> |
| | | 19 | | /// One expansion, cut just past the outermost band, rasterised onto a grid and contoured. The |
| | | 20 | | /// grid rather than a hull over reached vertices: edges carry shape, and a hull drawn between |
| | | 21 | | /// endpoints would cut the corners off every long link. |
| | | 22 | | /// </remarks> |
| | | 23 | | public static class IsochroneSearch |
| | | 24 | | { |
| | | 25 | | /// <summary> |
| | | 26 | | /// Computes the areas reachable from a snapped point at each of the given costs. |
| | | 27 | | /// </summary> |
| | | 28 | | /// <param name="bands">Cost bounds in the cost function's units. Order does not matter.</param> |
| | | 29 | | /// <returns>The bands, innermost first.</returns> |
| | | 30 | | public static async Task<Result<Isochrone>> IsochroneAsync( |
| | | 31 | | this RoutingNetwork network, |
| | | 32 | | RoutingSettings settings, |
| | | 33 | | SnapPoint source, |
| | | 34 | | IReadOnlyList<double> bands, |
| | | 35 | | IsochroneSettings? isochroneSettings = null, |
| | | 36 | | CancellationToken cancellationToken = default) |
| | 19 | 37 | | { |
| | 20 | 38 | | if (bands.Count == 0) throw new ArgumentException("No bands given.", nameof(bands)); |
| | 103 | 39 | | foreach (var band in bands) |
| | 25 | 40 | | { |
| | 26 | 41 | | if (band <= 0) throw new ArgumentOutOfRangeException(nameof(bands), |
| | 1 | 42 | | "A band has to cost more than nothing."); |
| | 24 | 43 | | } |
| | | 44 | | |
| | 17 | 45 | | var options = isochroneSettings ?? new IsochroneSettings(); |
| | 17 | 46 | | options.Validate(); |
| | | 47 | | |
| | 26 | 48 | | var ordered = bands.Distinct().OrderBy(x => x).ToList(); |
| | 17 | 49 | | var outer = ordered[^1]; |
| | | 50 | | // Searched past the outermost band so the contour has something on both sides of it to |
| | | 51 | | // interpolate between. Without it the boundary pulls in to the last reached vertex. |
| | 17 | 52 | | var cut = outer * (1.0 + options.Margin); |
| | | 53 | | |
| | 17 | 54 | | var profile = settings.Profile; |
| | 17 | 55 | | var costFunction = network.GetCostFunctionFor(profile); |
| | 17 | 56 | | if (settings.CostFunctionWrapper != null) costFunction = settings.CostFunctionWrapper(costFunction); |
| | | 57 | | |
| | 17 | 58 | | var reached = new Dictionary<VertexId, double>(); |
| | 17 | 59 | | var settled = 0L; |
| | | 60 | | |
| | 17 | 61 | | var search = SearchPool<Dijkstra>.Rent(); |
| | | 62 | | try |
| | 17 | 63 | | { |
| | 17 | 64 | | await search.RunAsync(network, (source, (bool?)null), |
| | 17 | 65 | | [], costFunction.GetDijkstraWeightFunc(), |
| | 17 | 66 | | async state => |
| | 5191 | 67 | | { |
| | 5191 | 68 | | if (!network.UsageNotifier.IsVertexDataReady(network, state.vertexId)) |
| | 0 | 69 | | { |
| | 0 | 70 | | await network.UsageNotifier.NotifyVertex(network, state.vertexId, cancellationToken); |
| | 0 | 71 | | } |
| | 17 | 72 | | |
| | 5191 | 73 | | cancellationToken.ThrowIfCancellationRequested(); |
| | 5191 | 74 | | settled++; |
| | 17 | 75 | | |
| | 17 | 76 | | // Cheapest arrival wins: the same vertex is settled once per incoming edge |
| | 17 | 77 | | // in an edge-based search, and the surface wants the cost of being there. |
| | 5191 | 78 | | if (!reached.TryGetValue(state.vertexId, out var best) || state.cost < best) |
| | 1593 | 79 | | { |
| | 1593 | 80 | | reached[state.vertexId] = state.cost; |
| | 1593 | 81 | | } |
| | 17 | 82 | | |
| | 17 | 83 | | // True stops expansion from this state, which is what bounds the search. |
| | 5191 | 84 | | return state.cost > cut; |
| | 5191 | 85 | | }, |
| | 17 | 86 | | cancellationToken: cancellationToken, |
| | 17 | 87 | | isMainN: network.GetIsMainNFunc(profile)); |
| | 17 | 88 | | } |
| | | 89 | | finally |
| | 17 | 90 | | { |
| | 17 | 91 | | SearchPool<Dijkstra>.Return(search); |
| | 17 | 92 | | } |
| | | 93 | | |
| | 17 | 94 | | if (reached.Count == 0) |
| | 0 | 95 | | return new Result<Isochrone>("Nothing reachable from the origin."); |
| | | 96 | | |
| | 17 | 97 | | var grid = BuildSurface(network, reached, options, cancellationToken, out var diagnostics); |
| | 17 | 98 | | diagnostics = diagnostics with { Settled = settled, Reached = reached.Count }; |
| | | 99 | | |
| | 17 | 100 | | var result = new List<IsochroneBand>(ordered.Count); |
| | 95 | 101 | | foreach (var band in ordered) |
| | 22 | 102 | | { |
| | 22 | 103 | | result.Add(new IsochroneBand(band, ContourTracer.Trace(grid.Cost, grid.Width, |
| | 22 | 104 | | grid.Height, band, grid.MinLon, grid.MinLat, grid.LonPerCell, grid.LatPerCell, |
| | 22 | 105 | | options.Denoise, grid.SquareMetersPerSquareDegree, |
| | 22 | 106 | | options.FillHolesBelowSquareMeters, cancellationToken))); |
| | 22 | 107 | | } |
| | | 108 | | |
| | 17 | 109 | | return new Isochrone(result, diagnostics); |
| | 17 | 110 | | } |
| | | 111 | | |
| | | 112 | | private readonly record struct Surface( |
| | 66 | 113 | | double[] Cost, int Width, int Height, |
| | 88 | 114 | | double MinLon, double MinLat, double LonPerCell, double LatPerCell, |
| | 22 | 115 | | double SquareMetersPerSquareDegree); |
| | | 116 | | |
| | | 117 | | /// <summary> |
| | | 118 | | /// Stamps the cost of every reached edge onto a grid, interpolating along the edge's shape. |
| | | 119 | | /// </summary> |
| | | 120 | | private static Surface BuildSurface(RoutingNetwork network, |
| | | 121 | | Dictionary<VertexId, double> reached, IsochroneSettings options, |
| | | 122 | | CancellationToken cancellationToken, out IsochroneDiagnostics diagnostics) |
| | 17 | 123 | | { |
| | 17 | 124 | | var minLon = double.MaxValue; |
| | 17 | 125 | | var minLat = double.MaxValue; |
| | 17 | 126 | | var maxLon = double.MinValue; |
| | 17 | 127 | | var maxLat = double.MinValue; |
| | 3237 | 128 | | foreach (var vertex in reached.Keys) |
| | 1593 | 129 | | { |
| | 1593 | 130 | | var (longitude, latitude, _) = network.GetVertex(vertex); |
| | 1593 | 131 | | minLon = Math.Min(minLon, longitude); |
| | 1593 | 132 | | minLat = Math.Min(minLat, latitude); |
| | 1593 | 133 | | maxLon = Math.Max(maxLon, longitude); |
| | 1593 | 134 | | maxLat = Math.Max(maxLat, latitude); |
| | 1593 | 135 | | } |
| | | 136 | | |
| | 17 | 137 | | var centreLat = (minLat + maxLat) / 2.0; |
| | | 138 | | |
| | | 139 | | // Scaled to the area actually reached when not given, so one setting serves a short walk |
| | | 140 | | // and an hour's drive. Valhalla sizes its grid the same way. |
| | 17 | 141 | | var cellSize = options.CellSizeMeters ?? 0; |
| | 17 | 142 | | if (cellSize <= 0) |
| | 3 | 143 | | { |
| | 3 | 144 | | var southWest = (minLon, minLat, (float?)null); |
| | 3 | 145 | | var across = Math.Max( |
| | 3 | 146 | | southWest.DistanceEstimateInMeter((maxLon, minLat, (float?)null)), |
| | 3 | 147 | | southWest.DistanceEstimateInMeter((minLon, maxLat, (float?)null))); |
| | 3 | 148 | | cellSize = Math.Max(options.MinCellSizeMeters, across / options.TargetCellsAcross); |
| | 3 | 149 | | } |
| | | 150 | | |
| | 17 | 151 | | var (lonPerCell, latPerCell) = CellInDegrees(cellSize, centreLat); |
| | | 152 | | |
| | 17 | 153 | | var width = (int)Math.Ceiling((maxLon - minLon) / lonPerCell) + 1; |
| | 17 | 154 | | var height = (int)Math.Ceiling((maxLat - minLat) / latPerCell) + 1; |
| | | 155 | | |
| | | 156 | | // Coarsen rather than allocate without bound: a wide band over a sparse network would |
| | | 157 | | // otherwise size the grid off its own extent. |
| | 17 | 158 | | if ((long)width * height > options.MaxCells) |
| | 1 | 159 | | { |
| | 1 | 160 | | var factor = Math.Sqrt((double)width * height / options.MaxCells); |
| | 1 | 161 | | cellSize *= factor; |
| | 1 | 162 | | (lonPerCell, latPerCell) = CellInDegrees(cellSize, centreLat); |
| | 1 | 163 | | width = (int)Math.Ceiling((maxLon - minLon) / lonPerCell) + 1; |
| | 1 | 164 | | height = (int)Math.Ceiling((maxLat - minLat) / latPerCell) + 1; |
| | 1 | 165 | | } |
| | | 166 | | |
| | | 167 | | // Padded by the reach, so a dilated band never runs into the edge of the grid and |
| | | 168 | | // leaves a ring that cannot close. |
| | 17 | 169 | | var pad = (int)Math.Round(options.ReachRadiusMeters / cellSize) + 2; |
| | 17 | 170 | | width += pad * 2; |
| | 17 | 171 | | height += pad * 2; |
| | 17 | 172 | | minLon -= lonPerCell * pad; |
| | 17 | 173 | | minLat -= latPerCell * pad; |
| | | 174 | | |
| | 17 | 175 | | var cost = new double[(long)width * height]; |
| | 17 | 176 | | Array.Fill(cost, double.MaxValue); |
| | | 177 | | |
| | 17 | 178 | | var enumerator = network.GetEdgeEnumerator(); |
| | 17 | 179 | | var seen = new HashSet<EdgeId>(); |
| | 17 | 180 | | var stamped = 0; |
| | 3237 | 181 | | foreach (var vertex in reached.Keys) |
| | 1593 | 182 | | { |
| | | 183 | | // One pass per reached vertex, each walking every incident edge's shape. |
| | 1610 | 184 | | if ((stamped++ & 0xFF) == 0) cancellationToken.ThrowIfCancellationRequested(); |
| | 1593 | 185 | | if (!enumerator.MoveTo(vertex)) continue; |
| | 7559 | 186 | | while (enumerator.MoveNext()) |
| | 5966 | 187 | | { |
| | 8798 | 188 | | if (!seen.Add(enumerator.EdgeId)) continue; |
| | 3134 | 189 | | if (!reached.TryGetValue(enumerator.Tail, out var tailCost)) continue; |
| | 3436 | 190 | | if (!reached.TryGetValue(enumerator.Head, out var headCost)) continue; |
| | | 191 | | |
| | 2832 | 192 | | Stamp(enumerator, tailCost, headCost); |
| | 2832 | 193 | | } |
| | 1593 | 194 | | } |
| | | 195 | | |
| | 17 | 196 | | Dilate(cost, width, height, (int)Math.Round(options.ReachRadiusMeters / cellSize), |
| | 17 | 197 | | cancellationToken); |
| | | 198 | | |
| | 17 | 199 | | diagnostics = new IsochroneDiagnostics(0, 0, (long)width * height, cellSize); |
| | | 200 | | // One square degree in square metres here, so a hole's area can be judged in metres. |
| | 17 | 201 | | var metersPerDegreeLon = cellSize / lonPerCell; |
| | 17 | 202 | | var metersPerDegreeLat = cellSize / latPerCell; |
| | 17 | 203 | | return new Surface(cost, width, height, minLon, minLat, lonPerCell, latPerCell, |
| | 17 | 204 | | metersPerDegreeLon * metersPerDegreeLat); |
| | | 205 | | |
| | | 206 | | void Stamp(RoutingNetworkEdgeEnumerator edge, double tailCost, double headCost) |
| | 2832 | 207 | | { |
| | 2832 | 208 | | var shape = edge.GetCompleteShape().ToList(); |
| | 2832 | 209 | | if (shape.Count < 2) return; |
| | | 210 | | |
| | 2832 | 211 | | var lengths = new double[shape.Count]; |
| | 2832 | 212 | | var total = 0d; |
| | 11328 | 213 | | for (var i = 1; i < shape.Count; i++) |
| | 2832 | 214 | | { |
| | 2832 | 215 | | total += shape[i - 1].DistanceEstimateInMeter(shape[i]); |
| | 2832 | 216 | | lengths[i] = total; |
| | 2832 | 217 | | } |
| | | 218 | | |
| | 16992 | 219 | | for (var i = 0; i < shape.Count; i++) |
| | 5664 | 220 | | { |
| | 5664 | 221 | | var at = total <= 0 ? 0 : lengths[i] / total; |
| | 5664 | 222 | | var here = tailCost + (headCost - tailCost) * at; |
| | 5664 | 223 | | Mark(shape[i].longitude, shape[i].latitude, here); |
| | | 224 | | |
| | 8496 | 225 | | if (i + 1 >= shape.Count) continue; |
| | | 226 | | |
| | | 227 | | // Sample between shape points too: a cell the edge merely crosses must be |
| | | 228 | | // marked, or a straight link longer than a cell leaves gaps in the surface. |
| | 2832 | 229 | | var span = lengths[i + 1] - lengths[i]; |
| | 2832 | 230 | | var steps = (int)Math.Floor(span / cellSize); |
| | 20842 | 231 | | for (var s = 1; s <= steps; s++) |
| | 7589 | 232 | | { |
| | 7589 | 233 | | var f = s / (double)(steps + 1); |
| | 7589 | 234 | | var lon = shape[i].longitude + (shape[i + 1].longitude - shape[i].longitude) * f; |
| | 7589 | 235 | | var lat = shape[i].latitude + (shape[i + 1].latitude - shape[i].latitude) * f; |
| | 7589 | 236 | | var mid = total <= 0 |
| | 7589 | 237 | | ? here |
| | 7589 | 238 | | : tailCost + (headCost - tailCost) * ((lengths[i] + span * f) / total); |
| | 7589 | 239 | | Mark(lon, lat, mid); |
| | 7589 | 240 | | } |
| | 2832 | 241 | | } |
| | 2832 | 242 | | } |
| | | 243 | | |
| | | 244 | | void Mark(double longitude, double latitude, double value) |
| | 13253 | 245 | | { |
| | 13253 | 246 | | var x = (int)Math.Round((longitude - minLon) / lonPerCell); |
| | 13253 | 247 | | var y = (int)Math.Round((latitude - minLat) / latPerCell); |
| | 13253 | 248 | | if (x < 0 || y < 0 || x >= width || y >= height) return; |
| | | 249 | | |
| | 13253 | 250 | | var index = (long)y * width + x; |
| | 20706 | 251 | | if (value < cost[index]) cost[index] = value; |
| | 13253 | 252 | | } |
| | 17 | 253 | | } |
| | | 254 | | |
| | | 255 | | /// <summary> |
| | | 256 | | /// Spreads each cell's cost to those within <paramref name="radius"/> cells of it. |
| | | 257 | | /// </summary> |
| | | 258 | | /// <remarks> |
| | | 259 | | /// Separable: the minimum over a square is the minimum along each row, then down each |
| | | 260 | | /// column. Taking the minimum is what makes it mean "the cheapest road within reach". |
| | | 261 | | /// </remarks> |
| | | 262 | | private static void Dilate(double[] cost, int width, int height, int radius, |
| | | 263 | | CancellationToken cancellationToken = default) |
| | 17 | 264 | | { |
| | 19 | 265 | | if (radius < 1) return; |
| | | 266 | | |
| | 15 | 267 | | var row = new double[width]; |
| | 1514 | 268 | | for (var y = 0; y < height; y++) |
| | 742 | 269 | | { |
| | | 270 | | // Per row rather than per cell: the grid runs to millions of cells. |
| | 742 | 271 | | cancellationToken.ThrowIfCancellationRequested(); |
| | 742 | 272 | | var offset = (long)y * width; |
| | 92534 | 273 | | for (var x = 0; x < width; x++) row[x] = cost[offset + x]; |
| | 62184 | 274 | | for (var x = 0; x < width; x++) |
| | 30350 | 275 | | { |
| | 30350 | 276 | | var best = double.MaxValue; |
| | 30350 | 277 | | var from = Math.Max(0, x - radius); |
| | 30350 | 278 | | var to = Math.Min(width - 1, x + radius); |
| | 1152910 | 279 | | for (var i = from; i <= to; i++) best = Math.Min(best, row[i]); |
| | 30350 | 280 | | cost[offset + x] = best; |
| | 30350 | 281 | | } |
| | 742 | 282 | | } |
| | | 283 | | |
| | 15 | 284 | | var column = new double[height]; |
| | 1182 | 285 | | for (var x = 0; x < width; x++) |
| | 576 | 286 | | { |
| | 576 | 287 | | cancellationToken.ThrowIfCancellationRequested(); |
| | 92202 | 288 | | for (var y = 0; y < height; y++) column[y] = cost[(long)y * width + x]; |
| | 61852 | 289 | | for (var y = 0; y < height; y++) |
| | 30350 | 290 | | { |
| | 30350 | 291 | | var best = double.MaxValue; |
| | 30350 | 292 | | var from = Math.Max(0, y - radius); |
| | 30350 | 293 | | var to = Math.Min(height - 1, y + radius); |
| | 1172986 | 294 | | for (var i = from; i <= to; i++) best = Math.Min(best, column[i]); |
| | 30350 | 295 | | cost[(long)y * width + x] = best; |
| | 30350 | 296 | | } |
| | 576 | 297 | | } |
| | 17 | 298 | | } |
| | | 299 | | |
| | | 300 | | /// <summary> |
| | | 301 | | /// Cell size in degrees. Taken once at the centre latitude rather than per point: the |
| | | 302 | | /// longitude scale varies across a band, but not enough to matter at a cell's size. |
| | | 303 | | /// </summary> |
| | | 304 | | private static (double lonPerCell, double latPerCell) CellInDegrees(double meters, double latitude) |
| | 18 | 305 | | { |
| | | 306 | | const double metersPerDegreeLat = 111_320.0; |
| | 18 | 307 | | var latPerCell = meters / metersPerDegreeLat; |
| | 18 | 308 | | var cos = Math.Cos(latitude * Math.PI / 180.0); |
| | 18 | 309 | | var lonPerCell = Math.Abs(cos) < 1e-6 |
| | 18 | 310 | | ? latPerCell |
| | 18 | 311 | | : meters / (metersPerDegreeLat * cos); |
| | 18 | 312 | | return (lonPerCell, latPerCell); |
| | 18 | 313 | | } |
| | | 314 | | } |