| | | 1 | | using System; |
| | | 2 | | using System.Collections.Generic; |
| | | 3 | | using Itinero.Geo; |
| | | 4 | | using Itinero.Geo.Directions; |
| | | 5 | | using Itinero.Network; |
| | | 6 | | using Itinero.Network.Enumerators.Edges; |
| | | 7 | | using Itinero.Network.Search; |
| | | 8 | | using Itinero.Profiles; |
| | | 9 | | |
| | | 10 | | namespace Itinero.Snapping; |
| | | 11 | | |
| | | 12 | | /// <summary> |
| | | 13 | | /// Extension methods related to snapping and snap points. |
| | | 14 | | /// </summary> |
| | | 15 | | public static class SnapPointExtensions |
| | | 16 | | { |
| | | 17 | | /// <summary> |
| | | 18 | | /// Returns the location on the given edge using the given offset. |
| | | 19 | | /// </summary> |
| | | 20 | | /// <param name="routerDb">The router db.</param> |
| | | 21 | | /// <param name="snapPoint">The snap point.</param> |
| | | 22 | | /// <returns>The location on the network.</returns> |
| | | 23 | | public static (double longitude, double latitude, float? e) LocationOnNetwork(this SnapPoint snapPoint, |
| | | 24 | | RoutingNetwork routerDb) |
| | 9 | 25 | | { |
| | 9 | 26 | | var enumerator = routerDb.GetEdgeEnumerator(); |
| | 9 | 27 | | enumerator.MoveTo(snapPoint.EdgeId); |
| | | 28 | | |
| | 9 | 29 | | return enumerator.LocationOnEdge(snapPoint.Offset); |
| | 9 | 30 | | } |
| | | 31 | | |
| | | 32 | | /// <summary> |
| | | 33 | | /// Returns the edge direction that best aligns with the direction based on the degrees with the meridian clockwise. |
| | | 34 | | /// </summary> |
| | | 35 | | /// <param name="snapPoint">The snap point.</param> |
| | | 36 | | /// <param name="routerDb">The router db.</param> |
| | | 37 | | /// <param name="direction">The direction.</param> |
| | | 38 | | /// <param name="distance">The distance to average the edge angle over in the range of ]0,∞[.</param> |
| | | 39 | | /// <param name="tolerance">The tolerance in range of ]0,90], default 90, determining what is considered forward or |
| | | 40 | | /// If the difference in angle is too big null is returned.</param> |
| | | 41 | | /// <returns>The direction on the edge at the location of the snap point that best matches the given direction. |
| | | 42 | | /// Returns null if the difference is too big relative to the tolerance or the edge is too small to properly calcula |
| | | 43 | | public static bool? Direction(this SnapPoint snapPoint, RoutingNetwork routerDb, DirectionEnum direction, |
| | | 44 | | double distance = 10, double tolerance = 90) |
| | 5 | 45 | | { |
| | 5 | 46 | | return snapPoint.DirectionFromAngle(routerDb, (double)direction, out _, distance, tolerance); |
| | 5 | 47 | | } |
| | | 48 | | |
| | | 49 | | /// <summary> |
| | | 50 | | /// Returns the edge direction that best aligns with the direction based on the degrees with the meridian clockwise. |
| | | 51 | | /// </summary> |
| | | 52 | | /// <param name="snapPoint">The snap point.</param> |
| | | 53 | | /// <param name="routerDb">The router db.</param> |
| | | 54 | | /// <param name="direction">The direction.</param> |
| | | 55 | | /// <param name="difference">The difference in degrees in the range of ]-180,180].</param> |
| | | 56 | | /// <param name="distance">The distance to average the edge angle over in the range of ]0,∞[.</param> |
| | | 57 | | /// <param name="tolerance">The tolerance in range of ]0,90], default 90, determining what is considered forward or |
| | | 58 | | /// If the difference in angle is too big null is returned.</param> |
| | | 59 | | /// <returns>The direction on the edge at the location of the snap point that best matches the given direction. |
| | | 60 | | /// Returns null if the difference is too big relative to the tolerance or the edge is too small to properly calcula |
| | | 61 | | public static bool? Direction(this SnapPoint snapPoint, RoutingNetwork routerDb, DirectionEnum direction, |
| | | 62 | | out double difference, double distance = 10, double tolerance = 90) |
| | 0 | 63 | | { |
| | 0 | 64 | | return snapPoint.DirectionFromAngle(routerDb, (double)direction, out difference, distance, tolerance); |
| | 0 | 65 | | } |
| | | 66 | | |
| | | 67 | | /// <summary> |
| | | 68 | | /// Returns the edge direction that best aligns with the direction based on the degrees with the meridian clockwise. |
| | | 69 | | /// </summary> |
| | | 70 | | /// <param name="snapPoint">The snap point.</param> |
| | | 71 | | /// <param name="routerDb">The router db.</param> |
| | | 72 | | /// <param name="degreesMeridian">The angle in degrees with the meridian clockwise.</param> |
| | | 73 | | /// <param name="difference">The difference in degrees in the range of ]-180,180].</param> |
| | | 74 | | /// <param name="distance">The distance to average the edge angle over in the range of ]0,∞[.</param> |
| | | 75 | | /// <param name="tolerance">The tolerance in range of ]0,90], default 90, determining what is considered forward or |
| | | 76 | | /// If the difference in angle is too big null is returned.</param> |
| | | 77 | | /// <returns>The direction on the edge at the location of the snap point that best matches the given direction. |
| | | 78 | | /// Returns null if the difference is too big relative to the tolerance or the edge is too small to properly calcula |
| | | 79 | | public static bool? DirectionFromAngle(this SnapPoint snapPoint, RoutingNetwork routerDb, |
| | | 80 | | double degreesMeridian, out double difference, double distance = 10, |
| | | 81 | | double tolerance = 90) |
| | 5 | 82 | | { |
| | 5 | 83 | | if (tolerance <= 0 || tolerance > 90) |
| | 0 | 84 | | { |
| | 0 | 85 | | throw new ArgumentOutOfRangeException(nameof(tolerance), "The tolerance has to be in range of ]0,90]"); |
| | | 86 | | } |
| | | 87 | | |
| | 5 | 88 | | var angle = snapPoint.Angle(routerDb, distance); |
| | 5 | 89 | | if (!angle.HasValue) |
| | 0 | 90 | | { |
| | 0 | 91 | | difference = 0; |
| | 0 | 92 | | return null; |
| | | 93 | | } |
| | | 94 | | |
| | 5 | 95 | | difference = degreesMeridian - angle.Value; |
| | 5 | 96 | | if (difference > 180) |
| | 0 | 97 | | { |
| | 0 | 98 | | difference -= 360; |
| | 0 | 99 | | } |
| | | 100 | | |
| | 5 | 101 | | if ((difference >= 0 && difference <= tolerance) || |
| | 5 | 102 | | (difference < 0 && difference >= -tolerance)) |
| | 4 | 103 | | { |
| | | 104 | | // forward, according to the tolerance. |
| | 4 | 105 | | return true; |
| | | 106 | | } |
| | | 107 | | |
| | 1 | 108 | | var reverseTolerance = 180 - tolerance; |
| | 1 | 109 | | if ((difference >= 0 && difference >= reverseTolerance) || |
| | 1 | 110 | | (difference < 0 && difference <= reverseTolerance)) |
| | 1 | 111 | | { |
| | | 112 | | // backward, according to the tolerance. |
| | 1 | 113 | | return false; |
| | | 114 | | } |
| | | 115 | | |
| | 0 | 116 | | return null; |
| | 5 | 117 | | } |
| | | 118 | | |
| | | 119 | | /// <summary> |
| | | 120 | | /// Returns the angle in degrees at the given snap point over a given distance. |
| | | 121 | | /// </summary> |
| | | 122 | | /// <param name="snapPoint">The snap point.</param> |
| | | 123 | | /// <param name="routerDb">The router db.</param> |
| | | 124 | | /// <param name="distance">The distance to average the edge angle over in the range of ]0,∞[.</param> |
| | | 125 | | /// <returns>The angle in degrees with the meridian clockwise.</returns> |
| | | 126 | | public static double? Angle(this SnapPoint snapPoint, RoutingNetwork routerDb, double distance = 10) |
| | 5 | 127 | | { |
| | 5 | 128 | | if (distance <= 0) |
| | 0 | 129 | | { |
| | 0 | 130 | | throw new ArgumentOutOfRangeException(nameof(distance), "The distance has to be in the range ]0,∞["); |
| | | 131 | | } |
| | | 132 | | |
| | 5 | 133 | | var edgeEnumerator = routerDb.GetEdgeEnumerator(); |
| | 5 | 134 | | if (!edgeEnumerator.MoveTo(snapPoint.EdgeId, true)) |
| | 0 | 135 | | { |
| | 0 | 136 | | throw new ArgumentException($"Cannot find edge in {nameof(SnapPoint)}: {snapPoint}"); |
| | | 137 | | } |
| | | 138 | | |
| | | 139 | | // determine the first and last point on the edge |
| | | 140 | | // to calculate the angle for. |
| | 5 | 141 | | var edgeLength = edgeEnumerator.EdgeLength(); |
| | 5 | 142 | | var distanceOffset = distance / edgeLength * ushort.MaxValue; |
| | 5 | 143 | | var offset1 = (ushort)0; |
| | 5 | 144 | | var offset2 = ushort.MaxValue; |
| | 5 | 145 | | if (distanceOffset <= ushort.MaxValue) |
| | 5 | 146 | | { |
| | | 147 | | // not the entire edge. |
| | | 148 | | // round offsets to beginning/end of edge. |
| | 5 | 149 | | offset1 = (ushort)Math.Max(0, |
| | 5 | 150 | | snapPoint.Offset - distanceOffset); |
| | 5 | 151 | | offset2 = (ushort)Math.Min(ushort.MaxValue, |
| | 5 | 152 | | snapPoint.Offset + distanceOffset); |
| | | 153 | | |
| | | 154 | | // if both are at the same location make sure to at least |
| | | 155 | | // convert the smallest possible section of the edge. |
| | 5 | 156 | | if (offset2 - offset1 == 0) |
| | 0 | 157 | | { |
| | 0 | 158 | | if (offset1 > 0) |
| | 0 | 159 | | { |
| | 0 | 160 | | offset1--; |
| | 0 | 161 | | } |
| | | 162 | | |
| | 0 | 163 | | if (offset2 < ushort.MaxValue) |
| | 0 | 164 | | { |
| | 0 | 165 | | offset2++; |
| | 0 | 166 | | } |
| | 0 | 167 | | } |
| | 5 | 168 | | } |
| | | 169 | | |
| | | 170 | | // calculate the locations. |
| | 5 | 171 | | var location1 = edgeEnumerator.LocationOnEdge(offset1); |
| | 5 | 172 | | var location2 = edgeEnumerator.LocationOnEdge(offset2); |
| | | 173 | | |
| | 5 | 174 | | if (location1.DistanceEstimateInMeter(location2) < .1) |
| | 0 | 175 | | { // distance too small, edge to short. |
| | 0 | 176 | | return null; |
| | | 177 | | } |
| | | 178 | | |
| | | 179 | | // calculate and return angle. |
| | 5 | 180 | | var toNorth = (location1.longitude, location1.latitude + 0.001f, (float?)null); |
| | 5 | 181 | | var angleRadians = DirectionCalculator.Angle(location2, location1, toNorth); |
| | 5 | 182 | | return angleRadians.ToDegrees().NormalizeDegrees(); |
| | 5 | 183 | | } |
| | | 184 | | |
| | | 185 | | /// <summary> |
| | | 186 | | /// Gets the vertex for the snap point, but only if it represents an exact vertex. |
| | | 187 | | /// </summary> |
| | | 188 | | /// <param name="snapPoint">The snap point, representing a vertex.</param> |
| | | 189 | | /// <param name="routingNetwork">The routing network.</param> |
| | | 190 | | /// <returns>The vertex.</returns> |
| | | 191 | | public static VertexId GetVertex(this SnapPoint snapPoint, RoutingNetwork routingNetwork) |
| | 0 | 192 | | { |
| | 0 | 193 | | if (!snapPoint.IsVertex) throw new ArgumentOutOfRangeException(nameof(snapPoint), "Snap point is not a vertex"); |
| | | 194 | | |
| | 0 | 195 | | var enumerator = routingNetwork.GetEdgeEnumerator(); |
| | 0 | 196 | | if (!enumerator.MoveTo(snapPoint.EdgeId)) |
| | 0 | 197 | | throw new Exception("Edge not found, has snap point been created on the given network?"); |
| | | 198 | | |
| | 0 | 199 | | if (snapPoint.Offset == 0) |
| | 0 | 200 | | { |
| | 0 | 201 | | return enumerator.Tail; |
| | | 202 | | } |
| | | 203 | | |
| | 0 | 204 | | return enumerator.Head; |
| | 0 | 205 | | } |
| | | 206 | | } |