/// <summary>
/// Updates the tracker with the given location.
/// </summary>
/// <param name="location">The measured location.</param>
public void Track(GeoCoordinate location)
{
// set the current location.
_currentPosition = location;
// calculate the total distance.
var previous = new GeoCoordinate(_route.Segments[0].Latitude, _route.Segments[0].Longitude);;
var totalDistance = 0.0;
for (int idx = 1; idx < _route.Segments.Length; idx++)
{
GeoCoordinate next = new GeoCoordinate(_route.Segments[idx].Latitude, _route.Segments[idx].Longitude);
totalDistance = totalDistance + previous.DistanceReal(next).Value;
previous = next;
}
double totalTime = _route.TotalTime;
// project onto the route.
int entryIdx;
_route.ProjectOn(_currentPosition, out _currentRoutePosition, out entryIdx, out _distanceFromStart, out _timeFromStart);
_distanceToEnd = totalDistance - _distanceFromStart;
_timeToEnd = totalTime - _timeFromStart.Value;
// find the next instruction.
_nextInstructionIdx = -1;
for (int instructionIdx = 0; instructionIdx < _instructions.Count; instructionIdx++)
{
var instruction = _instructions[instructionIdx];
if (instruction.LastSegmentIdx > entryIdx)
{ // stop here!
_nextInstructionIdx = instructionIdx;
break;
}
}
if (_nextInstructionIdx < 0)
{ // no instruction was found after the entryIdx: assume last instruction.
_nextInstructionIdx = _instructions.Count - 1;
}
// calculate the distance to the next instruction.
previous = _currentRoutePosition;
var distance = 0.0;
for (int idx = entryIdx + 1; idx <= _instructions[_nextInstructionIdx].LastSegmentIdx && idx < _route.Segments.Length; idx++)
{
var next = (new GeoCoordinate(_route.Segments[idx].Latitude, _route.Segments[idx].Longitude));
distance = distance + previous.DistanceReal(next).Value;
previous = next;
}
_distanceNextInstruction = distance;
}
public void GeoCoordinateTest()
{
var from = new GeoCoordinate(50.0589533, -122.9576373);
var to = new GeoCoordinate(50.058952331543, -122.957633972168);
Meter m = from.DistanceReal(to);
}
/// <summary>
/// Builds a dummy route (as the crow flies) for segments of a route not found.
/// </summary>
/// <returns></returns>
public virtual Route BuildDummyRoute(Vehicle vehicle, GeoCoordinate coordinate1, GeoCoordinate coordinate2)
{
var route = new Route();
route.Vehicle = vehicle.UniqueName;
var segments = new RouteSegment[2];
segments[0] = new RouteSegment();
segments[0].Distance = 0;
segments[0].Time = 0;
segments[0].Type = RouteSegmentType.Start;
segments[0].Vehicle = vehicle.UniqueName;
segments[0].Latitude = (float)coordinate1.Latitude;
segments[0].Longitude = (float)coordinate1.Longitude;
var distance = coordinate1.DistanceReal(coordinate2).Value;
var timeEstimage = distance / (vehicle.MaxSpeed().Value) * 3.6;
var tags = new TagsCollection();
tags.Add("route", "not_found");
segments[1] = new RouteSegment();
segments[1].Distance = distance;
segments[1].Time = timeEstimage;
segments[1].Type = RouteSegmentType.Stop;
segments[1].Vehicle = vehicle.UniqueName;
segments[1].Latitude = (float)coordinate2.Latitude;
segments[1].Longitude = (float)coordinate2.Longitude;
segments[1].Tags = RouteTagsExtensions.ConvertFrom(tags);
route.Segments = segments;
return(route);
}
/// <summary>
/// Updates the tracker with the given location.
/// </summary>
/// <param name="location">The measured location.</param>
public void Track(GeoCoordinate location)
{
// project onto the route.
KeyValuePair <int, GeoCoordinate> projectedResult = this.ProjectOn(_route, location);
// set the current/route position.
_currentPosition = location;
_currentRoutePosition = projectedResult.Value;
// find the next instruction.
for (int instructionIdx = 0; instructionIdx < _instructions.Count; instructionIdx++)
{
Instruction instruction = _instructions[instructionIdx];
if (instruction.EntryIdx >= projectedResult.Key)
{ // stop here!
_nextInstructionIdx = instructionIdx;
break;
}
}
// calculate the distance to the next instruction.
GeoCoordinate previous = (new GeoCoordinate(_route.Entries[projectedResult.Key].Latitude,
_route.Entries[projectedResult.Key].Longitude));
Meter distance = previous.DistanceReal(projectedResult.Value);
for (int idx = projectedResult.Key; idx < _instructions[_nextInstructionIdx].EntryIdx - 1; idx++)
{
GeoCoordinate next = (new GeoCoordinate(_route.Entries[idx + 1].Latitude, _route.Entries[idx + 1].Longitude));
distance = distance + previous.DistanceReal(next);
previous = next;
}
_distanceNextInstruction = distance;
// calculate the distance from start.
previous = (new GeoCoordinate(_route.Entries[0].Latitude, _route.Entries[0].Longitude));
distance = 0;
for (int idx = 0; idx < projectedResult.Key - 1; idx++)
{
GeoCoordinate next = (new GeoCoordinate(_route.Entries[idx + 1].Latitude, _route.Entries[idx + 1].Longitude));
distance = distance + previous.DistanceReal(next);
previous = next;
}
distance = distance + previous.DistanceReal(projectedResult.Value);
_distanceFromStart = distance;
}
/// <summary>
/// Encodes a bearing based on the list of coordinates and the BEARDIST parameter.
/// </summary>
/// <param name="coordinates"></param>
/// <returns></returns>
public static Degree EncodeBearing(List <GeoCoordinate> coordinates)
{
var distance = 0.0;
var previous = coordinates[0];
PointF2D bearingPosition = null;
for (int idx = 1; idx < coordinates.Count; idx++)
{
var current = new GeoCoordinate(coordinates[idx].Latitude, coordinates[idx].Longitude);
var currentSegmentDistance = current.DistanceReal(previous).Value;
var currentDistance = currentSegmentDistance + distance;
if (currentDistance > Parameters.BEARDIST)
{ // the coordinate to calculate the beardist is in this segment!
// calculate where.
var relativeDistance = Parameters.BEARDIST - distance;
var relativeOffset = relativeDistance / currentSegmentDistance;
bearingPosition = previous + ((current - previous) * relativeOffset);
break;
}
distance = currentDistance;
previous = current;
}
if (bearingPosition == null)
{ // use the toCoordinate as the last 'current'.
// if edge is too short use target coordinate.
bearingPosition = coordinates[coordinates.Count - 1];
}
// calculate offset.
var offset = (bearingPosition - coordinates[0]);
// convert offset to meters.
var north = new VectorF2D(0, 1); // north.
var xMeters = new GeoCoordinate(coordinates[0].Latitude, coordinates[0].Longitude + offset[0]).DistanceReal(
coordinates[0]).Value;
if (offset[0] < 0)
{ // invert offset.
xMeters = -xMeters;
}
var yMeters = new GeoCoordinate(coordinates[0].Latitude + offset[1], coordinates[0].Longitude).DistanceReal(
coordinates[0]).Value;
if (offset[1] < 0)
{ // invert offset.
yMeters = -yMeters;
}
var direction = new VectorF2D(xMeters, yMeters).Normalize();
return(direction.Angle(north));
}
/// <summary>
/// Invalidates the map center.
/// </summary>
private void InvalidateMapCenter()
{
if (_autoInvalidate)
{
if (_previousRenderingMapCenter == null ||
_previousRenderingMapCenter.DistanceReal(_mapCenter).Value > 40)
{
// TODO: update this with a more resonable measure depending on the zoom.
this.Change(false);
_previousRenderingMapCenter = _mapCenter;
}
}
}
public void TestGeoCoordinateOffsetRandom()
{
var generator = new OsmSharp.Math.Random.RandomGenerator(10124613);
for (int idx = 0; idx < 1000; idx++)
{
GeoCoordinate start = new GeoCoordinate(51, 4.8);
GeoCoordinate offset = start.OffsetRandom(generator, 20);
double distance = offset.DistanceReal(start).Value;
Assert.IsTrue(distance <= 20.001);
}
}
public static bool ProjectOn(this Route route, GeoCoordinate coordinates, out GeoCoordinate projectedCoordinates, out int entryIndex, out Meter distanceFromStart, out Second timeFromStart)
{
double num1 = double.MaxValue;
distanceFromStart = (Meter)0.0;
timeFromStart = (Second)0.0;
double num2 = 0.0;
projectedCoordinates = (GeoCoordinate)null;
entryIndex = -1;
List <GeoCoordinate> points = route.GetPoints();
for (int index = 0; index < points.Count - 1; ++index)
{
PointF2D pointF2D = new GeoCoordinateLine(points[index], points[index + 1], true, true).ProjectOn((PointF2D)coordinates);
if (pointF2D != (PointF2D)null)
{
GeoCoordinate point = new GeoCoordinate(pointF2D[1], pointF2D[0]);
double num3 = coordinates.Distance(point);
if (num3 < num1)
{
projectedCoordinates = point;
entryIndex = index;
num1 = num3;
double num4 = point.DistanceReal(points[index]).Value;
distanceFromStart = (Meter)(num2 + num4);
}
}
GeoCoordinate point1 = points[index];
double num5 = coordinates.Distance(point1);
if (num5 < num1)
{
projectedCoordinates = point1;
entryIndex = index;
num1 = num5;
distanceFromStart = (Meter)num2;
}
num2 += points[index].DistanceReal(points[index + 1]).Value;
}
GeoCoordinate point2 = points[points.Count - 1];
if (coordinates.Distance(point2) < num1)
{
projectedCoordinates = point2;
entryIndex = points.Count - 1;
distanceFromStart = (Meter)num2;
}
return(true);
}
public void TestGeoCoordinateOffsetEstimate()
{
GeoCoordinate coord1 = new GeoCoordinate(51, 4.8);
Meter distance = 10000;
GeoCoordinate coord3 = coord1.OffsetWithDistances(distance);
GeoCoordinate coord3_lat = new GeoCoordinate(coord3.Latitude, coord1.Longitude);
GeoCoordinate coord3_lon = new GeoCoordinate(coord1.Latitude, coord3.Longitude);
Meter distance_lat = coord3_lat.DistanceReal(coord1);
Meter distance_lon = coord3_lon.DistanceReal(coord1);
Assert.AreEqual(distance.Value, distance_lat.Value, 0.001);
Assert.AreEqual(distance.Value, distance_lon.Value, 0.001);
}
/// <summary>
/// Generates an arc and it's next point from the current aggregated point.
/// </summary>
/// <param name="route"></param>
/// <param name="previous"></param>
/// <param name="current"></param>
/// <param name="next"></param>
/// <returns></returns>
internal AggregatedArc CreateArcAndPoint(Route route, AggregatedRoutePoint previous,
AggregatedRoutePoint current, AggregatedRoutePoint next)
{
// create the arc.
var a = new AggregatedArc();
a.Name = current.Segment.Name;
a.Names = current.Segment.Names.ConvertTo();
a.Tags = current.Segment.Tags.ConvertToTagsCollection();
a.Vehicle = string.IsNullOrWhiteSpace(route.Vehicle) ? current.Segment.Vehicle : route.Vehicle;
if (previous != null)
{
var previousCoordinate = new GeoCoordinate(previous.Segment.Latitude, previous.Segment.Longitude);
var currentCoordinate = new GeoCoordinate(current.Segment.Latitude, current.Segment.Longitude);
var distance = previousCoordinate.DistanceReal(currentCoordinate);
a.Distance = distance;
}
// create the point.
var p = new AggregatedPoint();
p.Location = new GeoCoordinate(current.Segment.Latitude, current.Segment.Longitude);
p.Points = new List <PointPoi>();
p.SegmentIdx = current.SegmentIndex;
if (previous != null && next != null && next.Segment != null)
{
var previousCoordinate = new GeoCoordinate(previous.Segment.Latitude, previous.Segment.Longitude);
var nextCoordinate = new GeoCoordinate(next.Segment.Latitude, next.Segment.Longitude);
p.Angle = RelativeDirectionCalculator.Calculate(previousCoordinate, p.Location, nextCoordinate);
}
if (current.Segment.SideStreets != null && current.Segment.SideStreets.Length > 0)
{
p.ArcsNotTaken = new List <KeyValuePair <RelativeDirection, AggregatedArc> >();
foreach (var sideStreet in current.Segment.SideStreets)
{
var side = new AggregatedArc();
side.Name = sideStreet.Name;
side.Names = sideStreet.Names.ConvertTo();
side.Tags = sideStreet.Tags.ConvertToTagsCollection();
RelativeDirection sideDirection = null;
if (previous != null)
{
var previousCoordinate = new GeoCoordinate(previous.Segment.Latitude, previous.Segment.Longitude);
var nextCoordinate = new GeoCoordinate(sideStreet.Latitude, sideStreet.Longitude);
sideDirection = RelativeDirectionCalculator.Calculate(previousCoordinate, p.Location, nextCoordinate);
}
p.ArcsNotTaken.Add(new KeyValuePair <RelativeDirection, AggregatedArc>(sideDirection, side));
}
}
if (current.Segment.Points != null)
{
foreach (var routePoint in current.Segment.Points)
{
var poi = new PointPoi();
poi.Name = routePoint.Name;
poi.Tags = routePoint.Tags.ConvertTo();
poi.Location = new GeoCoordinate(routePoint.Latitude, routePoint.Longitude);
var previousCoordinate = new GeoCoordinate(previous.Segment.Latitude, previous.Segment.Longitude);
var currentCoordinate = new GeoCoordinate(current.Segment.Latitude, current.Segment.Longitude);
poi.Angle = RelativeDirectionCalculator.Calculate(previousCoordinate, currentCoordinate, poi.Location);
p.Points.Add(poi);
}
}
// link the arc to the point.
a.Next = p;
return(a);
}
/// <summary>
/// Generates an arc and it's next point from the current aggregated point.
/// </summary>
/// <param name="previous"></param>
/// <param name="current"></param>
/// <param name="next"></param>
/// <returns></returns>
internal AggregatedArc CreateArcAndPoint(AggregatedRoutePoint previous, AggregatedRoutePoint current, AggregatedRoutePoint next)
{
// create the arc.
AggregatedArc a = new AggregatedArc();
a.Name = current.Entry.WayFromName;
a.Names = current.Entry.WayFromNames.ConvertTo();
a.Tags = current.Entry.Tags.ConvertToTagsCollection();
if (previous != null)
{
GeoCoordinate previous_coordinate =
new GeoCoordinate(previous.Entry.Latitude, previous.Entry.Longitude);
GeoCoordinate current_coordinate = new GeoCoordinate(current.Entry.Latitude, current.Entry.Longitude);
Meter distance = previous_coordinate.DistanceReal(current_coordinate);
a.Distance = distance;
}
// create the point.
AggregatedPoint p = new AggregatedPoint();
p.Location = new GeoCoordinate(current.Entry.Latitude, current.Entry.Longitude);
p.Points = new List <PointPoi>();
if (previous != null && next != null && next.Entry != null)
{
GeoCoordinate previous_coordinate =
new GeoCoordinate(previous.Entry.Latitude, previous.Entry.Longitude);
GeoCoordinate next_coordinate =
new GeoCoordinate(next.Entry.Latitude, next.Entry.Longitude);
p.Angle = RelativeDirectionCalculator.Calculate(previous_coordinate, p.Location, next_coordinate);
}
if (current.Entry.SideStreets != null && current.Entry.SideStreets.Length > 0)
{
p.ArcsNotTaken = new List <KeyValuePair <RelativeDirection, AggregatedArc> >();
foreach (RoutePointEntrySideStreet side_street in current.Entry.SideStreets)
{
AggregatedArc side = new AggregatedArc();
side.Name = side_street.WayName;
side.Names = side_street.WayNames.ConvertTo();
side.Tags = side_street.Tags.ConvertToTagsCollection();
RelativeDirection side_direction = null;
if (previous != null)
{
GeoCoordinate previous_coordinate =
new GeoCoordinate(previous.Entry.Latitude, previous.Entry.Longitude);
GeoCoordinate next_coordinate =
new GeoCoordinate(side_street.Latitude, side_street.Longitude);
side_direction = RelativeDirectionCalculator.Calculate(previous_coordinate, p.Location, next_coordinate);
}
p.ArcsNotTaken.Add(new KeyValuePair <RelativeDirection, AggregatedArc>(side_direction, side));
}
}
if (current.Entry.Points != null)
{
foreach (RoutePoint route_point in current.Entry.Points)
{
PointPoi poi = new PointPoi();
poi.Name = route_point.Name;
poi.Tags = route_point.Tags.ConvertTo();
poi.Location = new GeoCoordinate(route_point.Latitude, route_point.Longitude);
GeoCoordinate previous_coordinate =
new GeoCoordinate(previous.Entry.Latitude, previous.Entry.Longitude);
GeoCoordinate current_coordinate = new GeoCoordinate(current.Entry.Latitude, current.Entry.Longitude);
poi.Angle = RelativeDirectionCalculator.Calculate(previous_coordinate, current_coordinate, poi.Location);
p.Points.Add(poi);
}
}
// link the arc to the point.
a.Next = p;
return(a);
}
/// <summary>
/// Calculates the closest point on the route relative to the given coordinate.
/// </summary>
/// <returns></returns>
public bool ProjectOn(GeoCoordinate coordinates, out GeoCoordinate projectedCoordinates, out int entryIndex, out Meter distanceFromStart, out Second timeFromStart)
{
double distance = double.MaxValue;
distanceFromStart = 0;
timeFromStart = 0;
double currentDistanceFromStart = 0;
projectedCoordinates = null;
entryIndex = -1;
// loop over all points and try to project onto the line segments.
GeoCoordinate projected;
double currentDistance;
var points = this.GetPoints();
for (int idx = 0; idx < points.Count - 1; idx++)
{
var line = new GeoCoordinateLine(points[idx], points[idx + 1], true, true);
var projectedPoint = line.ProjectOn(coordinates);
if (projectedPoint != null)
{ // there was a projected point.
projected = new GeoCoordinate(projectedPoint[1], projectedPoint[0]);
currentDistance = coordinates.Distance(projected);
if (currentDistance < distance)
{ // this point is closer.
projectedCoordinates = projected;
entryIndex = idx;
distance = currentDistance;
// calculate distance/time.
double localDistance = projected.DistanceReal(points[idx]).Value;
distanceFromStart = currentDistanceFromStart + localDistance;
if (this.HasTimes && idx > 0)
{ // there should be proper timing information.
double timeToSegment = this.Segments[idx].Time;
double timeToNextSegment = this.Segments[idx + 1].Time;
timeFromStart = timeToSegment + ((timeToNextSegment - timeToSegment) * (localDistance / line.LengthReal.Value));
}
}
}
// check first point.
projected = points[idx];
currentDistance = coordinates.Distance(projected);
if (currentDistance < distance)
{ // this point is closer.
projectedCoordinates = projected;
entryIndex = idx;
distance = currentDistance;
distanceFromStart = currentDistanceFromStart;
if (this.HasTimes)
{ // there should be proper timing information.
timeFromStart = this.Segments[idx].Time;
}
}
// update distance from start.
currentDistanceFromStart = currentDistanceFromStart + points[idx].DistanceReal(points[idx + 1]).Value;
}
// check last point.
projected = points[points.Count - 1];
currentDistance = coordinates.Distance(projected);
if (currentDistance < distance)
{ // this point is closer.
projectedCoordinates = projected;
entryIndex = points.Count - 1;
distance = currentDistance;
distanceFromStart = currentDistanceFromStart;
if (this.HasTimes)
{ // there should be proper timing information.
timeFromStart = this.Segments[points.Count - 1].Time;
}
}
return(true);
}
/// <summary>
/// Searches the data for a point on an edge closest to the given coordinate.
/// </summary>
/// <param name="graph"></param>
/// <param name="vehicle"></param>
/// <param name="coordinate"></param>
/// <param name="delta"></param>
/// <param name="matcher"></param>
/// <param name="pointTags"></param>
/// <param name="interpreter"></param>
/// <param name="verticesOnly"></param>
/// <param name="parameters"></param>
public SearchClosestResult <TEdgeData> SearchClosest(IBasicRouterDataSource <TEdgeData> graph, IRoutingInterpreter interpreter, Vehicle vehicle,
GeoCoordinate coordinate, float delta, IEdgeMatcher matcher, TagsCollectionBase pointTags, bool verticesOnly, Dictionary <string, object> parameters)
{
Meter distanceEpsilon = .1; // 10cm is the tolerance to distinguish points.
var closestWithMatch = new SearchClosestResult <TEdgeData>(double.MaxValue, 0);
var closestWithoutMatch = new SearchClosestResult <TEdgeData>(double.MaxValue, 0);
double searchBoxSize = delta;
// create the search box.
var searchBox = new GeoCoordinateBox(new GeoCoordinate(
coordinate.Latitude - searchBoxSize, coordinate.Longitude - searchBoxSize),
new GeoCoordinate(
coordinate.Latitude + searchBoxSize, coordinate.Longitude + searchBoxSize));
// get the arcs from the data source.
var arcs = graph.GetEdges(searchBox);
if (!verticesOnly)
{ // find both closest arcs and vertices.
// loop over all.
while (arcs.MoveNext())
{
if (!graph.TagsIndex.Contains(arcs.EdgeData.Tags))
{ // skip this edge, no valid tags found.
continue;
}
var arcTags = graph.TagsIndex.Get(arcs.EdgeData.Tags);
var canBeTraversed = vehicle.CanTraverse(arcTags);
if (canBeTraversed)
{ // the edge can be traversed.
// test the two points.
float fromLatitude, fromLongitude;
float toLatitude, toLongitude;
double distance;
if (graph.GetVertex(arcs.Vertex1, out fromLatitude, out fromLongitude) &&
graph.GetVertex(arcs.Vertex2, out toLatitude, out toLongitude))
{ // return the vertex.
var fromCoordinates = new GeoCoordinate(fromLatitude, fromLongitude);
distance = coordinate.DistanceReal(fromCoordinates).Value;
ICoordinateCollection coordinates;
ICoordinate[] coordinatesArray = null;
if (!graph.GetEdgeShape(arcs.Vertex1, arcs.Vertex2, out coordinates))
{
coordinates = null;
}
if (coordinates != null)
{
coordinatesArray = coordinates.ToArray();
}
if (distance < distanceEpsilon.Value)
{ // the distance is smaller than the tolerance value.
closestWithoutMatch = new SearchClosestResult <TEdgeData>(
distance, arcs.Vertex1);
if (matcher == null ||
(pointTags == null || pointTags.Count == 0) ||
matcher.MatchWithEdge(vehicle, pointTags, arcTags))
{
closestWithMatch = new SearchClosestResult <TEdgeData>(
distance, arcs.Vertex1);
break;
}
}
if (distance < closestWithoutMatch.Distance)
{ // the distance is smaller for the without match.
closestWithoutMatch = new SearchClosestResult <TEdgeData>(
distance, arcs.Vertex1);
}
if (distance < closestWithMatch.Distance)
{ // the distance is smaller for the with match.
if (matcher == null ||
(pointTags == null || pointTags.Count == 0) ||
matcher.MatchWithEdge(vehicle, pointTags, graph.TagsIndex.Get(arcs.EdgeData.Tags)))
{
closestWithMatch = new SearchClosestResult <TEdgeData>(
distance, arcs.Vertex1);
}
}
var toCoordinates = new GeoCoordinate(toLatitude, toLongitude);
distance = coordinate.DistanceReal(toCoordinates).Value;
if (distance < closestWithoutMatch.Distance)
{ // the distance is smaller for the without match.
closestWithoutMatch = new SearchClosestResult <TEdgeData>(
distance, arcs.Vertex2);
}
if (distance < closestWithMatch.Distance)
{ // the distance is smaller for the with match.
if (matcher == null ||
(pointTags == null || pointTags.Count == 0) ||
matcher.MatchWithEdge(vehicle, pointTags, arcTags))
{
closestWithMatch = new SearchClosestResult <TEdgeData>(
distance, arcs.Vertex2);
}
}
// search along the line.
var distanceTotal = 0.0;
var previous = fromCoordinates;
var arcValueValueCoordinates = arcs.Intermediates;
if (arcValueValueCoordinates != null)
{ // calculate distance along all coordinates.
var arcValueValueCoordinatesArray = arcValueValueCoordinates.ToArray();
for (int idx = 0; idx < arcValueValueCoordinatesArray.Length; idx++)
{
var current = new GeoCoordinate(arcValueValueCoordinatesArray[idx].Latitude, arcValueValueCoordinatesArray[idx].Longitude);
distanceTotal = distanceTotal + current.DistanceReal(previous).Value;
previous = current;
}
}
distanceTotal = distanceTotal + toCoordinates.DistanceReal(previous).Value;
if (distanceTotal > 0)
{ // the from/to are not the same location.
// loop over all edges that are represented by this arc (counting intermediate coordinates).
previous = fromCoordinates;
GeoCoordinateLine line;
var distanceToSegment = 0.0;
if (arcValueValueCoordinates != null)
{
var arcValueValueCoordinatesArray = arcValueValueCoordinates.ToArray();
for (int idx = 0; idx < arcValueValueCoordinatesArray.Length; idx++)
{
var current = new GeoCoordinate(
arcValueValueCoordinatesArray[idx].Latitude, arcValueValueCoordinatesArray[idx].Longitude);
line = new GeoCoordinateLine(previous, current, true, true);
distance = line.DistanceReal(coordinate).Value;
if (distance < closestWithoutMatch.Distance)
{ // the distance is smaller.
var projectedPoint = line.ProjectOn(coordinate);
// calculate the position.
if (projectedPoint != null)
{ // calculate the distance
var distancePoint = previous.DistanceReal(new GeoCoordinate(projectedPoint)).Value + distanceToSegment;
var position = distancePoint / distanceTotal;
closestWithoutMatch = new SearchClosestResult <TEdgeData>(
distance, arcs.Vertex1, arcs.Vertex2, position, arcs.EdgeData, coordinatesArray);
}
}
if (distance < closestWithMatch.Distance)
{
var projectedPoint = line.ProjectOn(coordinate);
// calculate the position.
if (projectedPoint != null)
{ // calculate the distance
var distancePoint = previous.DistanceReal(new GeoCoordinate(projectedPoint)).Value + distanceToSegment;
var position = distancePoint / distanceTotal;
if (matcher == null ||
(pointTags == null || pointTags.Count == 0) ||
matcher.MatchWithEdge(vehicle, pointTags, arcTags))
{
closestWithMatch = new SearchClosestResult <TEdgeData>(
distance, arcs.Vertex1, arcs.Vertex2, position, arcs.EdgeData, coordinatesArray);
}
}
}
// add current segment distance to distanceToSegment for the next segment.
distanceToSegment = distanceToSegment + line.LengthReal.Value;
// set previous.
previous = current;
}
}
// check the last segment.
line = new GeoCoordinateLine(previous, toCoordinates, true, true);
distance = line.DistanceReal(coordinate).Value;
if (distance < closestWithoutMatch.Distance)
{ // the distance is smaller.
var projectedPoint = line.ProjectOn(coordinate);
// calculate the position.
if (projectedPoint != null)
{ // calculate the distance
double distancePoint = previous.DistanceReal(new GeoCoordinate(projectedPoint)).Value + distanceToSegment;
double position = distancePoint / distanceTotal;
closestWithoutMatch = new SearchClosestResult <TEdgeData>(
distance, arcs.Vertex1, arcs.Vertex2, position, arcs.EdgeData, coordinatesArray);
}
}
if (distance < closestWithMatch.Distance)
{
var projectedPoint = line.ProjectOn(coordinate);
// calculate the position.
if (projectedPoint != null)
{ // calculate the distance
double distancePoint = previous.DistanceReal(new GeoCoordinate(projectedPoint)).Value + distanceToSegment;
double position = distancePoint / distanceTotal;
if (matcher == null ||
(pointTags == null || pointTags.Count == 0) ||
matcher.MatchWithEdge(vehicle, pointTags, arcTags))
{
closestWithMatch = new SearchClosestResult <TEdgeData>(
distance, arcs.Vertex1, arcs.Vertex2, position, arcs.EdgeData, coordinatesArray);
}
}
}
}
}
}
}
}
else
{ // only find closest vertices.
// loop over all.
while (arcs.MoveNext())
{
float fromLatitude, fromLongitude;
float toLatitude, toLongitude;
if (graph.GetVertex(arcs.Vertex1, out fromLatitude, out fromLongitude) &&
graph.GetVertex(arcs.Vertex2, out toLatitude, out toLongitude))
{
var vertexCoordinate = new GeoCoordinate(fromLatitude, fromLongitude);
double distance = coordinate.DistanceReal(vertexCoordinate).Value;
if (distance < closestWithoutMatch.Distance)
{ // the distance found is closer.
closestWithoutMatch = new SearchClosestResult <TEdgeData>(
distance, arcs.Vertex1);
}
vertexCoordinate = new GeoCoordinate(toLatitude, toLongitude);
distance = coordinate.DistanceReal(vertexCoordinate).Value;
if (distance < closestWithoutMatch.Distance)
{ // the distance found is closer.
closestWithoutMatch = new SearchClosestResult <TEdgeData>(
distance, arcs.Vertex2);
}
var arcValueValueCoordinates = arcs.Intermediates;
if (arcValueValueCoordinates != null)
{ // search over intermediate points.
var arcValueValueCoordinatesArray = arcValueValueCoordinates.ToArray();
for (int idx = 0; idx < arcValueValueCoordinatesArray.Length; idx++)
{
vertexCoordinate = new GeoCoordinate(
arcValueValueCoordinatesArray[idx].Latitude,
arcValueValueCoordinatesArray[idx].Longitude);
distance = coordinate.DistanceReal(vertexCoordinate).Value;
if (distance < closestWithoutMatch.Distance)
{ // the distance found is closer.
closestWithoutMatch = new SearchClosestResult <TEdgeData>(
distance, arcs.Vertex1, arcs.Vertex2, idx, arcs.EdgeData, arcValueValueCoordinatesArray);
}
}
}
}
}
}
// return the best result.
if (closestWithMatch.Distance < double.MaxValue)
{
return(closestWithMatch);
}
return(closestWithoutMatch);
}
/// <summary>
/// Searches the data for a point on an edge closest to the given coordinate.
/// </summary>
/// <param name="graph"></param>
/// <param name="vehicle"></param>
/// <param name="coordinate"></param>
/// <param name="delta"></param>
/// <param name="matcher"></param>
/// <param name="pointTags"></param>
/// <param name="interpreter"></param>
/// <param name="verticesOnly"></param>
/// <param name="parameters"></param>
public SearchClosestResult <TEdgeData> SearchClosest(IRoutingAlgorithmData <TEdgeData> graph, IRoutingInterpreter interpreter, Vehicle vehicle,
GeoCoordinate coordinate, float delta, IEdgeMatcher matcher, TagsCollectionBase pointTags, bool verticesOnly, Dictionary <string, object> parameters)
{
Meter distanceEpsilon = .1; // 10cm is the tolerance to distinguish points.
var closestWithMatch = new SearchClosestResult <TEdgeData>(double.MaxValue, 0);
GeoCoordinateBox closestWithMatchBox = null;
var closestWithoutMatch = new SearchClosestResult <TEdgeData>(double.MaxValue, 0);
GeoCoordinateBox closestWithoutMatchBox = null;
double searchBoxSize = delta;
// create the search box.
var searchBox = new GeoCoordinateBox(new GeoCoordinate(
coordinate.Latitude - searchBoxSize, coordinate.Longitude - searchBoxSize),
new GeoCoordinate(
coordinate.Latitude + searchBoxSize, coordinate.Longitude + searchBoxSize));
// get the arcs from the data source.
var edges = graph.GetEdges(searchBox);
if (!verticesOnly)
{ // find both closest arcs and vertices.
// loop over all.
while (edges.MoveNext())
{
//if (!graph.TagsIndex.Contains(edges.EdgeData.Tags))
//{ // skip this edge, no valid tags found.
// continue;
//}
// test the two points.
float fromLatitude, fromLongitude;
float toLatitude, toLongitude;
double distance;
if (graph.GetVertex(edges.Vertex1, out fromLatitude, out fromLongitude) &&
graph.GetVertex(edges.Vertex2, out toLatitude, out toLongitude))
{ // return the vertex.
var vertex1Coordinate = new GeoCoordinate(fromLatitude, fromLongitude);
var vertex2Coordinate = new GeoCoordinate(toLatitude, toLongitude);
if (edges.EdgeData.ShapeInBox)
{ // ok, check if it is needed to even check this edge.
var edgeBox = new GeoCoordinateBox(vertex1Coordinate, vertex2Coordinate);
var edgeBoxOverlap = false;
if (closestWithoutMatchBox == null ||
closestWithoutMatchBox.Overlaps(edgeBox))
{ // edge box overlap.
edgeBoxOverlap = true;
}
else if (closestWithMatchBox == null ||
closestWithMatchBox.Overlaps(edgeBox))
{ // edge box overlap.
edgeBoxOverlap = true;
}
if (!edgeBoxOverlap)
{ // no overlap, impossible this edge is a candidate.
continue;
}
}
var arcTags = graph.TagsIndex.Get(edges.EdgeData.Tags);
var canBeTraversed = vehicle.CanTraverse(arcTags);
if (canBeTraversed)
{ // the edge can be traversed.
distance = coordinate.DistanceEstimate(vertex1Coordinate).Value;
if (distance < distanceEpsilon.Value)
{ // the distance is smaller than the tolerance value.
var diff = coordinate - vertex1Coordinate;
closestWithoutMatchBox = new GeoCoordinateBox(new GeoCoordinate(coordinate + diff),
new GeoCoordinate(coordinate - diff));
closestWithoutMatch = new SearchClosestResult <TEdgeData>(
distance, edges.Vertex1);
if (matcher == null ||
(pointTags == null || pointTags.Count == 0) ||
matcher.MatchWithEdge(vehicle, pointTags, arcTags))
{
closestWithMatchBox = new GeoCoordinateBox(new GeoCoordinate(coordinate + diff),
new GeoCoordinate(coordinate - diff));
closestWithMatch = new SearchClosestResult <TEdgeData>(
distance, edges.Vertex1);
break;
}
}
if (distance < closestWithoutMatch.Distance)
{ // the distance is smaller for the without match.
var diff = coordinate - vertex1Coordinate;
closestWithoutMatchBox = new GeoCoordinateBox(new GeoCoordinate(coordinate + diff),
new GeoCoordinate(coordinate - diff));
closestWithoutMatch = new SearchClosestResult <TEdgeData>(
distance, edges.Vertex1);
}
if (distance < closestWithMatch.Distance)
{ // the distance is smaller for the with match.
if (matcher == null ||
(pointTags == null || pointTags.Count == 0) ||
matcher.MatchWithEdge(vehicle, pointTags, graph.TagsIndex.Get(edges.EdgeData.Tags)))
{
var diff = coordinate - vertex1Coordinate;
closestWithMatchBox = new GeoCoordinateBox(new GeoCoordinate(coordinate + diff),
new GeoCoordinate(coordinate - diff));
closestWithMatch = new SearchClosestResult <TEdgeData>(
distance, edges.Vertex1);
}
}
distance = coordinate.DistanceEstimate(vertex2Coordinate).Value;
if (distance < closestWithoutMatch.Distance)
{ // the distance is smaller for the without match.
var diff = coordinate - vertex2Coordinate;
closestWithoutMatchBox = new GeoCoordinateBox(new GeoCoordinate(coordinate + diff),
new GeoCoordinate(coordinate - diff));
closestWithoutMatch = new SearchClosestResult <TEdgeData>(
distance, edges.Vertex2);
}
if (distance < closestWithMatch.Distance)
{ // the distance is smaller for the with match.
if (matcher == null ||
(pointTags == null || pointTags.Count == 0) ||
matcher.MatchWithEdge(vehicle, pointTags, arcTags))
{
var diff = coordinate - vertex2Coordinate;
closestWithMatchBox = new GeoCoordinateBox(new GeoCoordinate(coordinate + diff),
new GeoCoordinate(coordinate - diff));
closestWithMatch = new SearchClosestResult <TEdgeData>(
distance, edges.Vertex2);
}
}
// search along the line.
var coordinatesArray = new ICoordinate[0];
var distanceTotal = 0.0;
var arcValueValueCoordinates = edges.Intermediates;
if (arcValueValueCoordinates != null)
{ // calculate distance along all coordinates.
coordinatesArray = arcValueValueCoordinates.ToArray();
}
// loop over all edges that are represented by this arc (counting intermediate coordinates).
var previous = vertex1Coordinate;
GeoCoordinateLine line;
var distanceToSegment = 0.0;
if (arcValueValueCoordinates != null)
{
for (int idx = 0; idx < coordinatesArray.Length; idx++)
{
var current = new GeoCoordinate(
coordinatesArray[idx].Latitude, coordinatesArray[idx].Longitude);
var edgeBox = new GeoCoordinateBox(previous, current);
var edgeBoxOverlap = false;
if (closestWithoutMatchBox == null ||
closestWithoutMatchBox.Overlaps(edgeBox))
{ // edge box overlap.
edgeBoxOverlap = true;
}
else if (closestWithMatchBox == null ||
closestWithMatchBox.Overlaps(edgeBox))
{ // edge box overlap.
edgeBoxOverlap = true;
}
if (edgeBoxOverlap)
{ // overlap, possible this edge is a candidate.
line = new GeoCoordinateLine(previous, current, true, true);
distance = line.DistanceReal(coordinate).Value;
if (distance < closestWithoutMatch.Distance)
{ // the distance is smaller.
var projectedPoint = line.ProjectOn(coordinate);
// calculate the position.
if (projectedPoint != null)
{ // calculate the distance.
if (distanceTotal == 0)
{ // calculate total distance.
var pCoordinate = vertex1Coordinate;
for (int cIdx = 0; cIdx < coordinatesArray.Length; cIdx++)
{
var cCoordinate = new GeoCoordinate(coordinatesArray[cIdx].Latitude, coordinatesArray[cIdx].Longitude);
distanceTotal = distanceTotal + cCoordinate.DistanceReal(pCoordinate).Value;
pCoordinate = cCoordinate;
}
distanceTotal = distanceTotal + vertex2Coordinate.DistanceReal(pCoordinate).Value;
}
var distancePoint = previous.DistanceReal(new GeoCoordinate(projectedPoint)).Value + distanceToSegment;
var position = distancePoint / distanceTotal;
var diff = coordinate - new GeoCoordinate(projectedPoint);
closestWithoutMatchBox = new GeoCoordinateBox(new GeoCoordinate(coordinate + diff),
new GeoCoordinate(coordinate - diff));
closestWithoutMatch = new SearchClosestResult <TEdgeData>(
distance, edges.Vertex1, edges.Vertex2, position, edges.EdgeData, coordinatesArray);
}
}
if (distance < closestWithMatch.Distance)
{
var projectedPoint = line.ProjectOn(coordinate);
// calculate the position.
if (projectedPoint != null)
{ // calculate the distance
if (distanceTotal == 0)
{ // calculate total distance.
var pCoordinate = vertex1Coordinate;
for (int cIdx = 0; cIdx < coordinatesArray.Length; cIdx++)
{
var cCoordinate = new GeoCoordinate(coordinatesArray[cIdx].Latitude, coordinatesArray[cIdx].Longitude);
distanceTotal = distanceTotal + cCoordinate.DistanceReal(pCoordinate).Value;
pCoordinate = cCoordinate;
}
distanceTotal = distanceTotal + vertex2Coordinate.DistanceReal(pCoordinate).Value;
}
var distancePoint = previous.DistanceReal(new GeoCoordinate(projectedPoint)).Value + distanceToSegment;
var position = distancePoint / distanceTotal;
if (matcher == null ||
(pointTags == null || pointTags.Count == 0) ||
matcher.MatchWithEdge(vehicle, pointTags, arcTags))
{
var diff = coordinate - new GeoCoordinate(projectedPoint);
closestWithMatchBox = new GeoCoordinateBox(new GeoCoordinate(coordinate + diff),
new GeoCoordinate(coordinate - diff));
closestWithMatch = new SearchClosestResult <TEdgeData>(
distance, edges.Vertex1, edges.Vertex2, position, edges.EdgeData, coordinatesArray);
}
}
}
}
// add current segment distance to distanceToSegment for the next segment.
distanceToSegment = distanceToSegment + previous.DistanceEstimate(current).Value;
// set previous.
previous = current;
}
}
// check the last segment.
line = new GeoCoordinateLine(previous, vertex2Coordinate, true, true);
distance = line.DistanceReal(coordinate).Value;
if (distance < closestWithoutMatch.Distance)
{ // the distance is smaller.
var projectedPoint = line.ProjectOn(coordinate);
// calculate the position.
if (projectedPoint != null)
{ // calculate the distance
if (distanceTotal == 0)
{ // calculate total distance.
var pCoordinate = vertex1Coordinate;
for (int cIdx = 0; cIdx < coordinatesArray.Length; cIdx++)
{
var cCoordinate = new GeoCoordinate(coordinatesArray[cIdx].Latitude, coordinatesArray[cIdx].Longitude);
distanceTotal = distanceTotal + cCoordinate.DistanceReal(pCoordinate).Value;
pCoordinate = cCoordinate;
}
distanceTotal = distanceTotal + vertex2Coordinate.DistanceReal(pCoordinate).Value;
}
double distancePoint = previous.DistanceReal(new GeoCoordinate(projectedPoint)).Value + distanceToSegment;
double position = distancePoint / distanceTotal;
var diff = coordinate - new GeoCoordinate(projectedPoint);
closestWithoutMatchBox = new GeoCoordinateBox(new GeoCoordinate(coordinate + diff),
new GeoCoordinate(coordinate - diff));
closestWithoutMatch = new SearchClosestResult <TEdgeData>(
distance, edges.Vertex1, edges.Vertex2, position, edges.EdgeData, coordinatesArray);
}
}
if (distance < closestWithMatch.Distance)
{
var projectedPoint = line.ProjectOn(coordinate);
// calculate the position.
if (projectedPoint != null)
{ // calculate the distance
if (distanceTotal == 0)
{ // calculate total distance.
var pCoordinate = vertex1Coordinate;
for (int cIdx = 0; cIdx < coordinatesArray.Length; cIdx++)
{
var cCoordinate = new GeoCoordinate(coordinatesArray[cIdx].Latitude, coordinatesArray[cIdx].Longitude);
distanceTotal = distanceTotal + cCoordinate.DistanceReal(pCoordinate).Value;
pCoordinate = cCoordinate;
}
distanceTotal = distanceTotal + vertex2Coordinate.DistanceReal(pCoordinate).Value;
}
double distancePoint = previous.DistanceReal(new GeoCoordinate(projectedPoint)).Value + distanceToSegment;
double position = distancePoint / distanceTotal;
if (matcher == null ||
(pointTags == null || pointTags.Count == 0) ||
matcher.MatchWithEdge(vehicle, pointTags, arcTags))
{
var diff = coordinate - new GeoCoordinate(projectedPoint);
closestWithMatchBox = new GeoCoordinateBox(new GeoCoordinate(coordinate + diff),
new GeoCoordinate(coordinate - diff));
closestWithMatch = new SearchClosestResult <TEdgeData>(
distance, edges.Vertex1, edges.Vertex2, position, edges.EdgeData, coordinatesArray);
}
}
}
}
}
}
}
else
{ // only find closest vertices.
// loop over all.
while (edges.MoveNext())
{
float fromLatitude, fromLongitude;
float toLatitude, toLongitude;
if (graph.GetVertex(edges.Vertex1, out fromLatitude, out fromLongitude) &&
graph.GetVertex(edges.Vertex2, out toLatitude, out toLongitude))
{
var vertexCoordinate = new GeoCoordinate(fromLatitude, fromLongitude);
double distance = coordinate.DistanceReal(vertexCoordinate).Value;
if (distance < closestWithoutMatch.Distance)
{ // the distance found is closer.
var diff = coordinate - vertexCoordinate;
closestWithoutMatchBox = new GeoCoordinateBox(new GeoCoordinate(coordinate + diff),
new GeoCoordinate(coordinate - diff));
closestWithoutMatch = new SearchClosestResult <TEdgeData>(
distance, edges.Vertex1);
}
vertexCoordinate = new GeoCoordinate(toLatitude, toLongitude);
distance = coordinate.DistanceReal(vertexCoordinate).Value;
if (distance < closestWithoutMatch.Distance)
{ // the distance found is closer.
var diff = coordinate - vertexCoordinate;
closestWithoutMatchBox = new GeoCoordinateBox(new GeoCoordinate(coordinate + diff),
new GeoCoordinate(coordinate - diff));
closestWithoutMatch = new SearchClosestResult <TEdgeData>(
distance, edges.Vertex2);
}
var arcValueValueCoordinates = edges.Intermediates;
if (arcValueValueCoordinates != null)
{ // search over intermediate points.
var arcValueValueCoordinatesArray = arcValueValueCoordinates.ToArray();
for (int idx = 0; idx < arcValueValueCoordinatesArray.Length; idx++)
{
vertexCoordinate = new GeoCoordinate(
arcValueValueCoordinatesArray[idx].Latitude,
arcValueValueCoordinatesArray[idx].Longitude);
distance = coordinate.DistanceReal(vertexCoordinate).Value;
if (distance < closestWithoutMatch.Distance)
{ // the distance found is closer.
var diff = coordinate - vertexCoordinate;
closestWithoutMatchBox = new GeoCoordinateBox(new GeoCoordinate(coordinate + diff),
new GeoCoordinate(coordinate - diff));
closestWithoutMatch = new SearchClosestResult <TEdgeData>(
distance, edges.Vertex1, edges.Vertex2, idx, edges.EdgeData, arcValueValueCoordinatesArray);
}
}
}
}
}
}
// return the best result.
if (closestWithMatch.Distance < double.MaxValue)
{
return(closestWithMatch);
}
return(closestWithoutMatch);
}
