| | 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.Enumerators.Edges; |
| | 8 | | using Itinero.Snapping; |
| | 9 | |
|
| | 10 | | namespace Itinero.Network.Search.Edges; |
| | 11 | |
|
| | 12 | | internal static class EdgeSearch |
| | 13 | | { |
| | 14 | | /// <summary> |
| | 15 | | /// Returns the closest edge to the center of the given box that has at least one vertex inside the given box. |
| | 16 | | /// </summary> |
| | 17 | | /// <param name="network">The network.</param> |
| | 18 | | /// <param name="searchBox">The box to search in.</param> |
| | 19 | | /// <param name="maxDistance">The maximum distance of any snap point returned relative to the center of the search b |
| | 20 | | /// <param name="edgeChecker">Used to determine if an edge is acceptable or not. If null any edge will be accepted.< |
| | 21 | | /// <param name="cancellationToken">The cancellation token.</param> |
| | 22 | | /// <returns>The closest edge to the center of the box inside the given box.</returns> |
| | 23 | | public static async Task<SnapPoint> SnapInBoxAsync(this RoutingNetwork network, |
| | 24 | | ((double longitude, double, float? e) topLeft, (double longitude, double latitude, float? e) bottomRight) |
| | 25 | | searchBox, |
| | 26 | | IEdgeChecker? edgeChecker = null, double maxDistance = double.MaxValue, CancellationToken cancellationToken = de |
| 25 | 27 | | { |
| 25 | 28 | | var edgeEnumerator = network.SearchEdgesInBox(searchBox); |
| 25 | 29 | | var center = searchBox.Center(); |
| | 30 | |
|
| | 31 | | const double exactTolerance = 1; |
| 25 | 32 | | var bestDistance = maxDistance; |
| 25 | 33 | | (EdgeId edgeId, ushort offset) bestSnapPoint = (EdgeId.Empty, ushort.MaxValue); |
| 55 | 34 | | while (edgeEnumerator.MoveNext()) |
| 50 | 35 | | { |
| 70 | 36 | | if (bestDistance <= 0) break; // break when exact on an edge. |
| | 37 | |
|
| | 38 | | // search for the local snap point that improves the current best snap point. |
| 30 | 39 | | (EdgeId edgeId, double offset) localSnapPoint = (EdgeId.Empty, 0); |
| 30 | 40 | | var isAcceptable = edgeChecker == null ? (bool?)true : null; |
| 30 | 41 | | var completeShape = edgeEnumerator.GetCompleteShape(); |
| 30 | 42 | | var length = 0.0; |
| 30 | 43 | | using (var completeShapeEnumerator = completeShape.GetEnumerator()) |
| 30 | 44 | | { |
| 30 | 45 | | completeShapeEnumerator.MoveNext(); |
| 30 | 46 | | var previous = completeShapeEnumerator.Current; |
| | 47 | |
|
| | 48 | | // start with the first location. |
| 30 | 49 | | var distance = previous.DistanceEstimateInMeter(center); |
| 30 | 50 | | if (distance < bestDistance) |
| 17 | 51 | | { |
| 17 | 52 | | isAcceptable ??= edgeChecker!.IsAcceptable(edgeEnumerator) ?? |
| 17 | 53 | | await edgeChecker.RunCheckAsync(edgeEnumerator, cancellati |
| 17 | 54 | | if (!isAcceptable.Value) |
| 0 | 55 | | { |
| 0 | 56 | | continue; |
| | 57 | | } |
| | 58 | |
|
| 17 | 59 | | if (distance < exactTolerance) |
| 12 | 60 | | { |
| 12 | 61 | | distance = 0; |
| 12 | 62 | | } |
| | 63 | |
|
| 17 | 64 | | bestDistance = distance; |
| 17 | 65 | | localSnapPoint = (edgeEnumerator.EdgeId, 0); |
| 17 | 66 | | } |
| | 67 | |
|
| | 68 | | // loop over all pairs. |
| 92 | 69 | | while (completeShapeEnumerator.MoveNext()) |
| 62 | 70 | | { |
| 62 | 71 | | var current = completeShapeEnumerator.Current; |
| | 72 | |
|
| 62 | 73 | | var segmentLength = previous.DistanceEstimateInMeter(current); |
| | 74 | |
|
| | 75 | | // first check the actual current location, it may be an exact match. |
| 62 | 76 | | distance = current.DistanceEstimateInMeter(center); |
| 62 | 77 | | if (distance < bestDistance) |
| 26 | 78 | | { |
| 26 | 79 | | isAcceptable ??= edgeChecker!.IsAcceptable(edgeEnumerator) ?? |
| 26 | 80 | | await edgeChecker.RunCheckAsync(edgeEnumerator, cancellationToken); |
| 26 | 81 | | if (!isAcceptable.Value) |
| 0 | 82 | | { |
| 0 | 83 | | break; |
| | 84 | | } |
| | 85 | |
|
| 26 | 86 | | if (distance < exactTolerance) |
| 9 | 87 | | { |
| 9 | 88 | | distance = 0; |
| 9 | 89 | | } |
| | 90 | |
|
| 26 | 91 | | bestDistance = distance; |
| 26 | 92 | | localSnapPoint = (edgeEnumerator.EdgeId, length + segmentLength); |
| 26 | 93 | | } |
| | 94 | |
|
| | 95 | | // update length. |
| 62 | 96 | | var startLength = length; |
| 62 | 97 | | length += segmentLength; |
| | 98 | |
|
| | 99 | | // TODO: figure this out, there has to be a way to not project every segment. |
| | 100 | | // // check if we even need to check. |
| | 101 | | // var previousDistance = previous.DistanceEstimateInMeter(center); |
| | 102 | | // var shapePointDistance = current.DistanceEstimateInMeter(center); |
| | 103 | | // if (previousDistance + segmentLength > bestDistance && |
| | 104 | | // shapePointDistance + segmentLength > bestDistance) |
| | 105 | | // { |
| | 106 | | // continue; |
| | 107 | | // } |
| | 108 | |
|
| | 109 | | // project on line segment. |
| 62 | 110 | | var line = (previous, current); |
| 62 | 111 | | var originalPrevious = previous; |
| 62 | 112 | | previous = current; |
| 62 | 113 | | if (bestDistance <= 0) |
| 36 | 114 | | { |
| | 115 | | // we need to continue, we need the total length. |
| 36 | 116 | | continue; |
| | 117 | | } |
| | 118 | |
|
| 26 | 119 | | var projected = line.ProjectOn(center); |
| 26 | 120 | | if (!projected.HasValue) |
| 17 | 121 | | { |
| 17 | 122 | | continue; |
| | 123 | | } |
| | 124 | |
|
| 9 | 125 | | distance = projected.Value.DistanceEstimateInMeter(center); |
| 9 | 126 | | if (!(distance < bestDistance)) |
| 0 | 127 | | { |
| 0 | 128 | | continue; |
| | 129 | | } |
| | 130 | |
|
| 9 | 131 | | isAcceptable ??= edgeChecker!.IsAcceptable(edgeEnumerator) ?? |
| 9 | 132 | | await edgeChecker.RunCheckAsync(edgeEnumerator, cancellationToken); |
| 9 | 133 | | if (!isAcceptable.Value) |
| 0 | 134 | | { |
| 0 | 135 | | break; |
| | 136 | | } |
| | 137 | |
|
| 9 | 138 | | if (distance < exactTolerance) |
| 3 | 139 | | { |
| 3 | 140 | | distance = 0; |
| 3 | 141 | | } |
| | 142 | |
|
| 9 | 143 | | bestDistance = distance; |
| 9 | 144 | | localSnapPoint = (edgeEnumerator.EdgeId, |
| 9 | 145 | | startLength + originalPrevious.DistanceEstimateInMeter(projected.Value)); |
| 9 | 146 | | } |
| 30 | 147 | | } |
| | 148 | |
|
| | 149 | | // move to the nex edge if no better point was found. |
| 30 | 150 | | if (localSnapPoint.edgeId == EdgeId.Empty) |
| 0 | 151 | | { |
| 0 | 152 | | continue; |
| | 153 | | } |
| | 154 | |
|
| | 155 | | // calculate the actual offset. |
| 30 | 156 | | var offset = ushort.MaxValue; |
| 30 | 157 | | if (localSnapPoint.offset < length) |
| 21 | 158 | | { |
| 21 | 159 | | if (localSnapPoint.offset <= 0) |
| 14 | 160 | | { |
| 14 | 161 | | offset = 0; |
| 14 | 162 | | } |
| | 163 | | else |
| 7 | 164 | | { |
| 7 | 165 | | offset = (ushort)(localSnapPoint.offset / length * ushort.MaxValue); |
| 7 | 166 | | } |
| 21 | 167 | | } |
| | 168 | |
|
| | 169 | | // invert offset if edge is reversed. |
| 30 | 170 | | if (!edgeEnumerator.Forward) |
| 4 | 171 | | { |
| 4 | 172 | | offset = (ushort)(ushort.MaxValue - offset); |
| 4 | 173 | | } |
| | 174 | |
|
| 30 | 175 | | bestSnapPoint = (localSnapPoint.edgeId, offset); |
| 30 | 176 | | } |
| | 177 | |
|
| 25 | 178 | | return new SnapPoint(bestSnapPoint.edgeId, bestSnapPoint.offset); |
| 25 | 179 | | } |
| | 180 | |
|
| | 181 | | /// <summary> |
| | 182 | | /// Snaps all points in the given box that could potentially be snapping points. |
| | 183 | | /// </summary> |
| | 184 | | /// <param name="routerDb"></param> |
| | 185 | | /// <param name="searchBox">The box to search in.</param> |
| | 186 | | /// <param name="maxDistance">The maximum distance of any snap point returned relative to the center of the search b |
| | 187 | | /// <param name="edgeChecker">Used to determine if an edge is acceptable or not. If null any edge will be accepted.< |
| | 188 | | /// <param name="nonOrthogonalEdges">When true the best potential location on each edge is returned, when false only |
| | 189 | | /// <param name="cancellationToken">The cancellation token.</param> |
| | 190 | | /// <returns>All edges that could potentially be relevant snapping points, not only the closest.</returns> |
| | 191 | | public static async IAsyncEnumerable<SnapPoint> SnapAllInBoxAsync(this RoutingNetwork routerDb, |
| | 192 | | ((double longitude, double latitude, float? e) topLeft, (double longitude, double latitude, float? e) |
| | 193 | | bottomRight) searchBox, |
| | 194 | | IEdgeChecker? edgeChecker = null, bool nonOrthogonalEdges = true, double maxDistance = double.MaxValue, Cancella |
| 2 | 195 | | { |
| 2 | 196 | | var edges = new HashSet<EdgeId>(); |
| | 197 | |
|
| 2 | 198 | | var edgeEnumerator = routerDb.SearchEdgesInBox(searchBox); |
| 2 | 199 | | var center = searchBox.Center(); |
| | 200 | |
|
| 14 | 201 | | while (edgeEnumerator.MoveNext()) |
| 12 | 202 | | { |
| 18 | 203 | | if (!edges.Add(edgeEnumerator.EdgeId)) continue; |
| | 204 | |
|
| | 205 | | // search for the best snap point for the current edge. |
| 6 | 206 | | (EdgeId edgeId, double offset, bool isOrthoganal, double distance) bestEdgeSnapPoint = |
| 6 | 207 | | (EdgeId.Empty, 0, false, maxDistance); |
| 6 | 208 | | var isAcceptable = edgeChecker == null ? (bool?)true : null; |
| 6 | 209 | | var completeShape = edgeEnumerator.GetCompleteShape(); |
| 6 | 210 | | var length = 0.0; |
| 6 | 211 | | using (var completeShapeEnumerator = completeShape.GetEnumerator()) |
| 6 | 212 | | { |
| 6 | 213 | | completeShapeEnumerator.MoveNext(); |
| 6 | 214 | | var previous = completeShapeEnumerator.Current; |
| | 215 | |
|
| | 216 | | // start with the first location. |
| 6 | 217 | | var distance = previous.DistanceEstimateInMeter(center); |
| 6 | 218 | | if (distance < bestEdgeSnapPoint.distance) |
| 6 | 219 | | { |
| 6 | 220 | | isAcceptable ??= edgeChecker!.IsAcceptable(edgeEnumerator) ?? await edgeChecker.RunCheckAsync(edgeEn |
| 6 | 221 | | if (!isAcceptable.Value) |
| 0 | 222 | | { |
| 0 | 223 | | continue; |
| | 224 | | } |
| | 225 | |
|
| 6 | 226 | | bestEdgeSnapPoint = (edgeEnumerator.EdgeId, 0, false, distance); |
| 6 | 227 | | } |
| | 228 | |
|
| | 229 | | // loop over all pairs. |
| 12 | 230 | | while (completeShapeEnumerator.MoveNext()) |
| 6 | 231 | | { |
| 6 | 232 | | var current = completeShapeEnumerator.Current; |
| | 233 | |
|
| 6 | 234 | | var segmentLength = previous.DistanceEstimateInMeter(current); |
| | 235 | |
|
| | 236 | | // first check the actual current location, it may be an exact match. |
| 6 | 237 | | distance = current.DistanceEstimateInMeter(center); |
| 6 | 238 | | if (distance < bestEdgeSnapPoint.distance) |
| 4 | 239 | | { |
| 4 | 240 | | isAcceptable ??= edgeChecker!.IsAcceptable(edgeEnumerator) ?? await edgeChecker.RunCheckAsync(ed |
| 4 | 241 | | if (!isAcceptable.Value) |
| 0 | 242 | | { |
| 0 | 243 | | break; |
| | 244 | | } |
| | 245 | |
|
| 4 | 246 | | bestEdgeSnapPoint = (edgeEnumerator.EdgeId, length + segmentLength, false, distance); |
| 4 | 247 | | } |
| | 248 | |
|
| | 249 | | // update length. |
| 6 | 250 | | var startLength = length; |
| 6 | 251 | | length += segmentLength; |
| | 252 | |
|
| | 253 | | // project on line segment. |
| 6 | 254 | | var line = (previous, current); |
| 6 | 255 | | var originalPrevious = previous; |
| 6 | 256 | | previous = current; |
| | 257 | |
|
| 6 | 258 | | var projected = line.ProjectOn(center); |
| 6 | 259 | | if (projected.HasValue) |
| 4 | 260 | | { |
| 4 | 261 | | distance = projected.Value.DistanceEstimateInMeter(center); |
| 4 | 262 | | if (distance < bestEdgeSnapPoint.distance) |
| 4 | 263 | | { |
| 4 | 264 | | isAcceptable ??= edgeChecker!.IsAcceptable(edgeEnumerator) ?? await edgeChecker.RunCheckAsyn |
| 4 | 265 | | if (isAcceptable.Value) |
| 4 | 266 | | { |
| 4 | 267 | | bestEdgeSnapPoint = (edgeEnumerator.EdgeId, |
| 4 | 268 | | startLength + originalPrevious.DistanceEstimateInMeter(projected.Value), |
| 4 | 269 | | true, distance); |
| 4 | 270 | | } |
| 4 | 271 | | } |
| 4 | 272 | | } |
| 6 | 273 | | } |
| 6 | 274 | | } |
| | 275 | |
|
| | 276 | | // move to the nex edge if no snap point was found. |
| 6 | 277 | | if (bestEdgeSnapPoint.edgeId == EdgeId.Empty) |
| 0 | 278 | | { |
| 0 | 279 | | continue; |
| | 280 | | } |
| | 281 | |
|
| | 282 | | // check type and return if needed. |
| 6 | 283 | | var returnEdge = bestEdgeSnapPoint.isOrthoganal || nonOrthogonalEdges; |
| 6 | 284 | | if (!returnEdge) |
| 1 | 285 | | { |
| 1 | 286 | | continue; |
| | 287 | | } |
| | 288 | |
|
| | 289 | | // calculate the actual offset. |
| 5 | 290 | | var offset = ushort.MaxValue; |
| 5 | 291 | | if (bestEdgeSnapPoint.offset < length) |
| 5 | 292 | | { |
| 5 | 293 | | if (bestEdgeSnapPoint.offset <= 0) |
| 1 | 294 | | { |
| 1 | 295 | | offset = 0; |
| 1 | 296 | | } |
| | 297 | | else |
| 4 | 298 | | { |
| 4 | 299 | | offset = (ushort)(bestEdgeSnapPoint.offset / length * ushort.MaxValue); |
| 4 | 300 | | } |
| 5 | 301 | | } |
| | 302 | |
|
| | 303 | | // invert offset if edge is reversed. |
| 5 | 304 | | if (!edgeEnumerator.Forward) |
| 0 | 305 | | { |
| 0 | 306 | | offset = (ushort)(ushort.MaxValue - offset); |
| 0 | 307 | | } |
| | 308 | |
|
| 5 | 309 | | yield return new SnapPoint(bestEdgeSnapPoint.edgeId, offset); |
| 5 | 310 | | } |
| 2 | 311 | | } |
| | 312 | |
|
| | 313 | | /// <summary> |
| | 314 | | /// Returns the closest vertex to the center of the given box. |
| | 315 | | /// </summary> |
| | 316 | | /// <param name="network">The network.</param> |
| | 317 | | /// <param name="searchBox">The box to search in.</param> |
| | 318 | | /// <param name="maxDistance">The maximum distance of any vertex returned relative to the center of the search box.< |
| | 319 | | /// <param name="edgeChecker">Used to determine if an edge is acceptable or not. If null any edge will be accepted.< |
| | 320 | | /// <param name="cancellationToken">The cancellation token.</param> |
| | 321 | | /// <returns>The closest edge to the center of the box inside the given box.</returns> |
| | 322 | | public static async Task<VertexId> SnapToVertexInBoxAsync(this RoutingNetwork network, |
| | 323 | | ((double longitude, double latitude, float? e) topLeft, (double longitude, double latitude, float? e) |
| | 324 | | bottomRight) searchBox, IEdgeChecker? edgeChecker = null, double maxDistance = double.MaxValue, Cancellation |
| 0 | 325 | | { |
| 0 | 326 | | var center = searchBox.Center(); |
| 0 | 327 | | var closestDistance = maxDistance; |
| 0 | 328 | | var closest = VertexId.Empty; |
| | 329 | |
|
| 0 | 330 | | var vertices = network.SearchVerticesInBox(searchBox); |
| 0 | 331 | | var edgeEnumerator = network.GetEdgeEnumerator(); |
| 0 | 332 | | foreach (var (vertex, location) in vertices) |
| 0 | 333 | | { |
| 0 | 334 | | var d = center.DistanceEstimateInMeter(location); |
| 0 | 335 | | if (d > closestDistance) continue; |
| | 336 | |
|
| 0 | 337 | | if (edgeChecker == null) |
| 0 | 338 | | { |
| 0 | 339 | | closest = vertex; |
| 0 | 340 | | closestDistance = d; |
| 0 | 341 | | continue; |
| | 342 | | } |
| | 343 | |
|
| 0 | 344 | | edgeEnumerator.MoveTo(vertex); |
| 0 | 345 | | while (edgeEnumerator.MoveNext()) |
| 0 | 346 | | { |
| 0 | 347 | | if (!(edgeChecker.IsAcceptable(edgeEnumerator) ?? await edgeChecker.RunCheckAsync(edgeEnumerator, cancel |
| | 348 | |
|
| 0 | 349 | | closest = vertex; |
| 0 | 350 | | closestDistance = d; |
| 0 | 351 | | break; |
| | 352 | | } |
| 0 | 353 | | } |
| | 354 | |
|
| 0 | 355 | | return closest; |
| 0 | 356 | | } |
| | 357 | |
|
| | 358 | | /// <summary> |
| | 359 | | /// Snaps all vertices in the given box that could potentially be snapping vertices. |
| | 360 | | /// </summary> |
| | 361 | | /// <param name="network">The network.</param> |
| | 362 | | /// <param name="searchBox">The box to search in.</param> |
| | 363 | | /// <param name="maxDistance">The maximum distance of any vertex returned relative to the center of the search box.< |
| | 364 | | /// <param name="edgeChecker">Used to determine if an edge is acceptable or not. If null any edge will be accepted.< |
| | 365 | | /// <param name="cancellationToken">The cancellation token.</param> |
| | 366 | | /// <returns>All the vertices within the box with at least one acceptable edge.</returns> |
| | 367 | | public static async IAsyncEnumerable<VertexId> SnapToAllVerticesInBoxAsync(this RoutingNetwork network, |
| | 368 | | ((double longitude, double latitude, float? e) topLeft, (double longitude, double latitude, float? e) |
| | 369 | | bottomRight) searchBox, |
| | 370 | | IEdgeChecker? edgeChecker = null, double maxDistance = double.MaxValue, CancellationToken cancellationToken = de |
| 0 | 371 | | { |
| 0 | 372 | | var center = searchBox.Center(); |
| 0 | 373 | | var vertices = network.SearchVerticesInBox(searchBox); |
| 0 | 374 | | var edgeEnumerator = network.GetEdgeEnumerator(); |
| 0 | 375 | | foreach (var (vertex, location) in vertices) |
| 0 | 376 | | { |
| 0 | 377 | | edgeEnumerator.MoveTo(vertex); |
| | 378 | |
|
| 0 | 379 | | var d = center.DistanceEstimateInMeter(location); |
| 0 | 380 | | if (d > maxDistance) continue; |
| | 381 | |
|
| 0 | 382 | | while (edgeEnumerator.MoveNext()) |
| 0 | 383 | | { |
| 0 | 384 | | if (edgeChecker != null && !(edgeChecker.IsAcceptable(edgeEnumerator) ?? await edgeChecker.RunCheckAsync |
| | 385 | |
|
| 0 | 386 | | yield return vertex; |
| 0 | 387 | | break; |
| | 388 | | } |
| 0 | 389 | | } |
| 0 | 390 | | } |
| | 391 | |
|
| | 392 | | /// <summary> |
| | 393 | | /// Enumerates all edges that have at least one vertex in the given bounding box. |
| | 394 | | /// </summary> |
| | 395 | | /// <param name="network">The network.</param> |
| | 396 | | /// <param name="box">The box to enumerate in.</param> |
| | 397 | | /// <returns>An enumerator with all the vertices and their location.</returns> |
| | 398 | | public static IEdgeEnumerator<RoutingNetwork> SearchEdgesInBox(this RoutingNetwork network, |
| | 399 | | ((double longitude, double latitude, float? e) topLeft, (double longitude, double latitude, float? e) |
| | 400 | | bottomRight) box) |
| 29 | 401 | | { |
| 29 | 402 | | var vertices = network.SearchVerticesInBox(box); |
| 91 | 403 | | return new VertexEdgeEnumerator(network, vertices.Select((i) => i.vertex)); |
| 29 | 404 | | } |
| | 405 | | } |