/// <summary>
/// Project on route and return the next entry index and coordinate.
/// </summary>
/// <param name="route"></param>
/// <param name="coordinates"></param>
/// <returns></returns>
private KeyValuePair <int, GeoCoordinate> ProjectOn(Route route, GeoCoordinate coordinates)
{
double distance = double.MaxValue;
GeoCoordinate closest = null;
int closestIdx = -1;
List <GeoCoordinate> points = route.GetPoints();
for (int idx = 0; idx < points.Count - 1; idx++)
{
GeoCoordinateLine line = new GeoCoordinateLine(points[idx], points[idx + 1], true, true);
PointF2D projectedPoint = line.ProjectOn(coordinates);
GeoCoordinate projected;
double currentDistance;
if (projectedPoint != null)
{
projected = new GeoCoordinate(projectedPoint[1], projectedPoint[0]);
currentDistance = coordinates.Distance(projected);
if (currentDistance < distance)
{
closest = projected;
closestIdx = idx + 1;
distance = currentDistance;
}
}
projected = points[idx];
currentDistance = coordinates.Distance(projected);
if (currentDistance < distance)
{
closest = projected;
closestIdx = idx;
distance = currentDistance;
}
}
return(new KeyValuePair <int, GeoCoordinate>(closestIdx, closest));
}
/// <summary>
/// Calculates the direction.
/// </summary>
/// <param name="from"></param>
/// <param name="to"></param>
/// <returns></returns>
public static DirectionEnum Calculate(GeoCoordinate from, GeoCoordinate to)
{
double offset = 0.01;
// calculate the angle with the horizontal and vertical axes.
GeoCoordinate horizonal_from = new GeoCoordinate(from.Latitude, from.Longitude + offset);
GeoCoordinate vertical_from = new GeoCoordinate(from.Latitude + offset, from.Longitude);
// create line.
GeoCoordinateLine line = new GeoCoordinateLine(from, to);
GeoCoordinateLine vertical_line = new GeoCoordinateLine(from,vertical_from);
GeoCoordinateLine horizontal_line= new GeoCoordinateLine(from,horizonal_from);
// calculate angle.
Degree horizontal_angle = line.Direction.Angle(horizontal_line.Direction);
Degree vertical_angle = line.Direction.Angle(vertical_line.Direction);
if (vertical_angle < new Degree(22.5)
|| vertical_angle >= new Degree(360- 22.5))
{ // north
return DirectionEnum.North;
}
else if (vertical_angle >= new Degree(22.5)
&& vertical_angle < new Degree(90 - 22.5))
{ // north-east.
return DirectionEnum.NorthEast;
}
else if (vertical_angle >= new Degree(90 - 22.5)
&& vertical_angle < new Degree(90 + 22.5))
{ // east.
return DirectionEnum.East;
}
else if (vertical_angle >= new Degree(90 + 22.5)
&& vertical_angle < new Degree(180 - 22.5))
{ // south-east.
return DirectionEnum.SouthEast;
}
else if (vertical_angle >= new Degree(180 - 22.5)
&& vertical_angle < new Degree(180 + 22.5))
{ // south
return DirectionEnum.South;
}
else if (vertical_angle >= new Degree(180 + 22.5)
&& vertical_angle < new Degree(270 - 22.5))
{ // south-west.
return DirectionEnum.SouthWest;
}
else if (vertical_angle >= new Degree(270 - 22.5)
&& vertical_angle < new Degree(270 + 22.5))
{ // south-west.
return DirectionEnum.West;
}
else if (vertical_angle >= new Degree(270 + 22.5)
&& vertical_angle < new Degree(360-22.5))
{ // south-west.
return DirectionEnum.NorhtWest;
}
throw new ArgumentOutOfRangeException();
}
/// <summary>
/// Calculates the direction.
/// </summary>
/// <param name="from"></param>
/// <param name="to"></param>
/// <returns></returns>
public static DirectionEnum Calculate(GeoCoordinate from, GeoCoordinate to)
{
double offset = 0.01;
// calculate the angle with the horizontal and vertical axes.
//GeoCoordinate horizonal_from = new GeoCoordinate(from.Latitude, from.Longitude + offset);
GeoCoordinate vertical_from = new GeoCoordinate(from.Latitude + offset, from.Longitude);
// create line.
GeoCoordinateLine line = new GeoCoordinateLine(from, to);
GeoCoordinateLine vertical_line = new GeoCoordinateLine(from, vertical_from);
//GeoCoordinateLine horizontal_line= new GeoCoordinateLine(from,horizonal_from);
// calculate angle.
//Degree horizontal_angle = line.Direction.Angle(horizontal_line.Direction);
Degree vertical_angle = line.Direction.Angle(vertical_line.Direction);
if (vertical_angle < new Degree(22.5) ||
vertical_angle >= new Degree(360 - 22.5))
{ // north
return(DirectionEnum.North);
}
else if (vertical_angle >= new Degree(22.5) &&
vertical_angle < new Degree(90 - 22.5))
{ // north-east.
return(DirectionEnum.NorthEast);
}
else if (vertical_angle >= new Degree(90 - 22.5) &&
vertical_angle < new Degree(90 + 22.5))
{ // east.
return(DirectionEnum.East);
}
else if (vertical_angle >= new Degree(90 + 22.5) &&
vertical_angle < new Degree(180 - 22.5))
{ // south-east.
return(DirectionEnum.SouthEast);
}
else if (vertical_angle >= new Degree(180 - 22.5) &&
vertical_angle < new Degree(180 + 22.5))
{ // south
return(DirectionEnum.South);
}
else if (vertical_angle >= new Degree(180 + 22.5) &&
vertical_angle < new Degree(270 - 22.5))
{ // south-west.
return(DirectionEnum.SouthWest);
}
else if (vertical_angle >= new Degree(270 - 22.5) &&
vertical_angle < new Degree(270 + 22.5))
{ // south-west.
return(DirectionEnum.West);
}
else if (vertical_angle >= new Degree(270 + 22.5) &&
vertical_angle < new Degree(360 - 22.5))
{ // south-west.
return(DirectionEnum.NorhtWest);
}
throw new ArgumentOutOfRangeException();
}
/// <summary>
/// Calculates the relative direction.
/// </summary>
/// <param name="from"></param>
/// <param name="along"></param>
/// <param name="to"></param>
/// <returns></returns>
public static RelativeDirection Calculate(GeoCoordinate from, GeoCoordinate along, GeoCoordinate to)
{
RelativeDirection direction = new RelativeDirection();
double margin = 65;
double straight_on = 10;
double turn_back = 5;
GeoCoordinateLine line_from = new GeoCoordinateLine(from, along);
GeoCoordinateLine line_to = new GeoCoordinateLine(along, to);
Degree angle = line_from.Direction.Angle(line_to.Direction);
if (angle >= new Degree(360 - straight_on)
|| angle < new Degree(straight_on))
{
direction.Direction = RelativeDirectionEnum.StraightOn;
}
else if (angle >= new Degree(straight_on)
&& angle < new Degree(90 - margin))
{
direction.Direction = RelativeDirectionEnum.SlightlyLeft;
}
else if (angle >= new Degree(90 - margin)
&& angle < new Degree(90 + margin))
{
direction.Direction = RelativeDirectionEnum.Left;
}
else if (angle >= new Degree(90 + margin)
&& angle < new Degree(180 - turn_back))
{
direction.Direction = RelativeDirectionEnum.SharpLeft;
}
else if (angle >= new Degree(180 - turn_back)
&& angle < new Degree(180 + turn_back))
{
direction.Direction = RelativeDirectionEnum.TurnBack;
}
else if (angle >= new Degree(180 + turn_back)
&& angle < new Degree(270-margin))
{
direction.Direction = RelativeDirectionEnum.SharpRight;
}
else if (angle >= new Degree(270 - margin)
&& angle < new Degree(270 + margin))
{
direction.Direction = RelativeDirectionEnum.Right;
}
else if (angle >= new Degree(270 + margin)
&& angle < new Degree(360- straight_on))
{
direction.Direction = RelativeDirectionEnum.SlightlyRight;
}
//direction.Direction = RelativeDirectionEnum.StraightOn;
direction.Angle = angle;
return direction;
}
/// <summary>
/// Calculates the relative direction.
/// </summary>
/// <param name="from"></param>
/// <param name="along"></param>
/// <param name="to"></param>
/// <returns></returns>
public static RelativeDirection Calculate(GeoCoordinate from, GeoCoordinate along, GeoCoordinate to)
{
RelativeDirection direction = new RelativeDirection();
double margin = 65;
double straight_on = 10;
double turn_back = 5;
GeoCoordinateLine line_from = new GeoCoordinateLine(from, along);
GeoCoordinateLine line_to = new GeoCoordinateLine(along, to);
Degree angle = line_from.Direction.Angle(line_to.Direction);
if (angle >= new Degree(360 - straight_on) ||
angle < new Degree(straight_on))
{
direction.Direction = RelativeDirectionEnum.StraightOn;
}
else if (angle >= new Degree(straight_on) &&
angle < new Degree(90 - margin))
{
direction.Direction = RelativeDirectionEnum.SlightlyLeft;
}
else if (angle >= new Degree(90 - margin) &&
angle < new Degree(90 + margin))
{
direction.Direction = RelativeDirectionEnum.Left;
}
else if (angle >= new Degree(90 + margin) &&
angle < new Degree(180 - turn_back))
{
direction.Direction = RelativeDirectionEnum.SharpLeft;
}
else if (angle >= new Degree(180 - turn_back) &&
angle < new Degree(180 + turn_back))
{
direction.Direction = RelativeDirectionEnum.TurnBack;
}
else if (angle >= new Degree(180 + turn_back) &&
angle < new Degree(270 - margin))
{
direction.Direction = RelativeDirectionEnum.SharpRight;
}
else if (angle >= new Degree(270 - margin) &&
angle < new Degree(270 + margin))
{
direction.Direction = RelativeDirectionEnum.Right;
}
else if (angle >= new Degree(270 + margin) &&
angle < new Degree(360 - straight_on))
{
direction.Direction = RelativeDirectionEnum.SlightlyRight;
}
//direction.Direction = RelativeDirectionEnum.StraightOn;
direction.Angle = angle;
return(direction);
}
/// <summary>
/// Calculates the relative direction.
/// </summary>
/// <param name="from"></param>
/// <param name="along"></param>
/// <param name="to"></param>
/// <returns></returns>
public static RelativeDirection Calculate(GeoCoordinate from, GeoCoordinate along, GeoCoordinate to)
{
var direction = new RelativeDirection();
var margin = 65.0;
var straight_on = 10.0;
var turn_back = 5.0;
var lineFrom = new GeoCoordinateLine(from, along);
var lineTo = new GeoCoordinateLine(along, to);
var angle = lineFrom.Direction.Angle(lineTo.Direction);
if (angle >= new Degree(360 - straight_on) ||
angle < new Degree(straight_on))
{
direction.Direction = RelativeDirectionEnum.StraightOn;
}
else if (angle >= new Degree(straight_on) &&
angle < new Degree(90 - margin))
{
direction.Direction = RelativeDirectionEnum.SlightlyLeft;
}
else if (angle >= new Degree(90 - margin) &&
angle < new Degree(90 + margin))
{
direction.Direction = RelativeDirectionEnum.Left;
}
else if (angle >= new Degree(90 + margin) &&
angle < new Degree(180 - turn_back))
{
direction.Direction = RelativeDirectionEnum.SharpLeft;
}
else if (angle >= new Degree(180 - turn_back) &&
angle < new Degree(180 + turn_back))
{
direction.Direction = RelativeDirectionEnum.TurnBack;
}
else if (angle >= new Degree(180 + turn_back) &&
angle < new Degree(270 - margin))
{
direction.Direction = RelativeDirectionEnum.SharpRight;
}
else if (angle >= new Degree(270 - margin) &&
angle < new Degree(270 + margin))
{
direction.Direction = RelativeDirectionEnum.Right;
}
else if (angle >= new Degree(270 + margin) &&
angle < new Degree(360 - straight_on))
{
direction.Direction = RelativeDirectionEnum.SlightlyRight;
}
direction.Angle = angle;
return(direction);
}
/// <summary>
/// Calculates the relative direction.
/// </summary>
/// <param name="from"></param>
/// <param name="along"></param>
/// <param name="to"></param>
/// <returns></returns>
public static RelativeDirection Calculate(GeoCoordinate from, GeoCoordinate along, GeoCoordinate to)
{
var direction = new RelativeDirection();
var margin = 65.0;
var straight_on = 10.0;
var turn_back = 5.0;
var lineFrom = new GeoCoordinateLine(from, along);
var lineTo = new GeoCoordinateLine(along, to);
var angle = lineFrom.Direction.Angle(lineTo.Direction);
if (angle >= new Degree(360 - straight_on)
|| angle < new Degree(straight_on))
{
direction.Direction = RelativeDirectionEnum.StraightOn;
}
else if (angle >= new Degree(straight_on)
&& angle < new Degree(90 - margin))
{
direction.Direction = RelativeDirectionEnum.SlightlyLeft;
}
else if (angle >= new Degree(90 - margin)
&& angle < new Degree(90 + margin))
{
direction.Direction = RelativeDirectionEnum.Left;
}
else if (angle >= new Degree(90 + margin)
&& angle < new Degree(180 - turn_back))
{
direction.Direction = RelativeDirectionEnum.SharpLeft;
}
else if (angle >= new Degree(180 - turn_back)
&& angle < new Degree(180 + turn_back))
{
direction.Direction = RelativeDirectionEnum.TurnBack;
}
else if (angle >= new Degree(180 + turn_back)
&& angle < new Degree(270-margin))
{
direction.Direction = RelativeDirectionEnum.SharpRight;
}
else if (angle >= new Degree(270 - margin)
&& angle < new Degree(270 + margin))
{
direction.Direction = RelativeDirectionEnum.Right;
}
else if (angle >= new Degree(270 + margin)
&& angle < new Degree(360- straight_on))
{
direction.Direction = RelativeDirectionEnum.SlightlyRight;
}
direction.Angle = angle;
return direction;
}
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 static RelativeDirection Calculate(GeoCoordinate from, GeoCoordinate along, GeoCoordinate to)
{
RelativeDirection relativeDirection = new RelativeDirection();
double num1 = 65.0;
double num2 = 10.0;
double num3 = 5.0;
Radian radian = new GeoCoordinateLine(from, along).Direction.Angle(new GeoCoordinateLine(along, to).Direction);
if ((Degree)radian >= new Degree(360.0 - num2) || (Degree)radian < new Degree(num2))
{
relativeDirection.Direction = RelativeDirectionEnum.StraightOn;
}
else if ((Degree)radian >= new Degree(num2) && (Degree)radian < new Degree(90.0 - num1))
{
relativeDirection.Direction = RelativeDirectionEnum.SlightlyLeft;
}
else if ((Degree)radian >= new Degree(90.0 - num1) && (Degree)radian < new Degree(90.0 + num1))
{
relativeDirection.Direction = RelativeDirectionEnum.Left;
}
else if ((Degree)radian >= new Degree(90.0 + num1) && (Degree)radian < new Degree(180.0 - num3))
{
relativeDirection.Direction = RelativeDirectionEnum.SharpLeft;
}
else if ((Degree)radian >= new Degree(180.0 - num3) && (Degree)radian < new Degree(180.0 + num3))
{
relativeDirection.Direction = RelativeDirectionEnum.TurnBack;
}
else if ((Degree)radian >= new Degree(180.0 + num3) && (Degree)radian < new Degree(270.0 - num1))
{
relativeDirection.Direction = RelativeDirectionEnum.SharpRight;
}
else if ((Degree)radian >= new Degree(270.0 - num1) && (Degree)radian < new Degree(270.0 + num1))
{
relativeDirection.Direction = RelativeDirectionEnum.Right;
}
else if ((Degree)radian >= new Degree(270.0 + num1) && (Degree)radian < new Degree(360.0 - num2))
{
relativeDirection.Direction = RelativeDirectionEnum.SlightlyRight;
}
relativeDirection.Angle = (Degree)radian;
return(relativeDirection);
}
/// <summary>
/// Calculates the closest point on the route.
/// </summary>
/// <param name="coordinates"></param>
/// <returns></returns>
public GeoCoordinate ProjectOn(GeoCoordinate coordinates)
{
double distance = double.MaxValue;
GeoCoordinate closests = null;
List <GeoCoordinate> points = this.GetPoints();
for (int idx = 0; idx < points.Count - 1; idx++)
{
GeoCoordinateLine line = new GeoCoordinateLine(points[idx], points[idx + 1]);
PointF2D projectedPoint = line.ProjectOn(coordinates);
GeoCoordinate projected = new GeoCoordinate(projectedPoint[1], projectedPoint[0]);
double currentDistance = coordinates.Distance(projected);
if (currentDistance < distance)
{
closests = projected;
distance = currentDistance;
}
}
return(closests);
}
public static DirectionEnum Calculate(GeoCoordinate from, GeoCoordinate to)
{
double num = 0.01;
GeoCoordinate point2 = new GeoCoordinate(from.Latitude + num, from.Longitude);
Radian radian = new GeoCoordinateLine(from, to).Direction.Angle(new GeoCoordinateLine(from, point2).Direction);
if ((Degree)radian < new Degree(22.5) || (Degree)radian >= new Degree(337.5))
{
return(DirectionEnum.North);
}
if ((Degree)radian >= new Degree(22.5) && (Degree)radian < new Degree(67.5))
{
return(DirectionEnum.NorthEast);
}
if ((Degree)radian >= new Degree(67.5) && (Degree)radian < new Degree(112.5))
{
return(DirectionEnum.East);
}
if ((Degree)radian >= new Degree(112.5) && (Degree)radian < new Degree(157.5))
{
return(DirectionEnum.SouthEast);
}
if ((Degree)radian >= new Degree(157.5) && (Degree)radian < new Degree(202.5))
{
return(DirectionEnum.South);
}
if ((Degree)radian >= new Degree(202.5) && (Degree)radian < new Degree(247.5))
{
return(DirectionEnum.SouthWest);
}
if ((Degree)radian >= new Degree(247.5) && (Degree)radian < new Degree(292.5))
{
return(DirectionEnum.West);
}
if ((Degree)radian >= new Degree(292.5) && (Degree)radian < new Degree(337.5))
{
return(DirectionEnum.NorthWest);
}
throw new ArgumentOutOfRangeException();
}
/// <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>
public SearchClosestResult SearchClosest(IBasicRouterDataSource <TEdgeData> graph, IRoutingInterpreter interpreter, Vehicle vehicle,
GeoCoordinate coordinate, float delta, IEdgeMatcher matcher, TagsCollection pointTags, bool verticesOnly)
{
var closestWithMatch = new SearchClosestResult(double.MaxValue, 0);
var closestWithoutMatch = new SearchClosestResult(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.
KeyValuePair <uint, KeyValuePair <uint, TEdgeData> >[] arcs = graph.GetArcs(searchBox);
if (!verticesOnly)
{ // find both closest arcs and vertices.
// loop over all.
foreach (KeyValuePair <uint, KeyValuePair <uint, TEdgeData> > arc in arcs)
{
TagsCollection arcTags = graph.TagsIndex.Get(arc.Value.Value.Tags);
bool 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(arc.Key, out fromLatitude, out fromLongitude) &&
graph.GetVertex(arc.Value.Key, out toLatitude, out toLongitude))
{ // return the vertex.
var fromCoordinates = new GeoCoordinate(fromLatitude, fromLongitude);
distance = coordinate.Distance(fromCoordinates);
if (distance < 0.00001)
{ // the distance is smaller than the tolerance value.
closestWithoutMatch = new SearchClosestResult(
distance, arc.Key);
if (matcher == null ||
(pointTags == null || pointTags.Count == 0) ||
matcher.MatchWithEdge(vehicle, pointTags, arcTags))
{
closestWithMatch = new SearchClosestResult(
distance, arc.Key);
break;
}
}
if (distance < closestWithoutMatch.Distance)
{ // the distance is smaller for the without match.
closestWithoutMatch = new SearchClosestResult(
distance, arc.Key);
}
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(arc.Value.Value.Tags)))
{
closestWithMatch = new SearchClosestResult(
distance, arc.Key);
}
}
var toCoordinates = new GeoCoordinate(toLatitude, toLongitude);
distance = coordinate.Distance(toCoordinates);
if (distance < closestWithoutMatch.Distance)
{ // the distance is smaller for the without match.
closestWithoutMatch = new SearchClosestResult(
distance, arc.Value.Key);
}
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(
distance, arc.Value.Key);
}
}
// create a line.
double distanceTotal = fromCoordinates.Distance(toCoordinates);
if (distanceTotal > 0)
{ // the from/to are not the same location.
var line = new GeoCoordinateLine(fromCoordinates, toCoordinates, true, true);
distance = line.Distance(coordinate);
if (distance < closestWithoutMatch.Distance)
{ // the distance is smaller.
PointF2D projectedPoint =
line.ProjectOn(coordinate);
// calculate the position.
if (projectedPoint != null)
{ // calculate the distance
double distancePoint = fromCoordinates.Distance(projectedPoint);
double position = distancePoint / distanceTotal;
closestWithoutMatch = new SearchClosestResult(
distance, arc.Key, arc.Value.Key, position);
}
}
if (distance < closestWithMatch.Distance)
{
PointF2D projectedPoint =
line.ProjectOn(coordinate);
// calculate the position.
if (projectedPoint != null)
{ // calculate the distance
double distancePoint = fromCoordinates.Distance(projectedPoint);
double position = distancePoint / distanceTotal;
if (matcher == null ||
(pointTags == null || pointTags.Count == 0) ||
matcher.MatchWithEdge(vehicle, pointTags, arcTags))
{
closestWithMatch = new SearchClosestResult(
distance, arc.Key, arc.Value.Key, position);
}
}
}
}
}
}
}
}
else
{ // only find closest vertices.
// loop over all.
foreach (KeyValuePair <uint, KeyValuePair <uint, TEdgeData> > arc in arcs)
{
float fromLatitude, fromLongitude;
float toLatitude, toLongitude;
if (graph.GetVertex(arc.Key, out fromLatitude, out fromLongitude) &&
graph.GetVertex(arc.Value.Key, out toLatitude, out toLongitude))
{
var vertexCoordinate = new GeoCoordinate(fromLatitude, fromLongitude);
double distance = coordinate.Distance(vertexCoordinate);
if (distance < closestWithoutMatch.Distance)
{ // the distance found is closer.
closestWithoutMatch = new SearchClosestResult(
distance, arc.Key);
}
vertexCoordinate = new GeoCoordinate(toLatitude, toLongitude);
distance = coordinate.Distance(vertexCoordinate);
if (distance < closestWithoutMatch.Distance)
{ // the distance found is closer.
closestWithoutMatch = new SearchClosestResult(
distance, arc.Value.Key);
}
}
}
}
// return the best result.
if (closestWithMatch.Distance < double.MaxValue)
{
return(closestWithMatch);
}
return(closestWithoutMatch);
}
/// <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);
}
public static bool ProjectOn(this GeometricGraph graph, GeometricEdge edge, float latitude, float longitude, out float projectedLatitude, out float projectedLongitude, out float projectedDistanceFromFirst, out int projectedShapeIndex, out float distanceToProjected, out float totalLength)
{
distanceToProjected = float.MaxValue;
projectedDistanceFromFirst = 0.0f;
projectedLatitude = float.MaxValue;
projectedLongitude = float.MaxValue;
projectedShapeIndex = -1;
ICoordinate coordinate1 = (ICoordinate)graph.GetVertex(edge.From);
ShapeBase shape = edge.Shape;
IEnumerator <ICoordinate> enumerator = (IEnumerator <ICoordinate>)null;
if (shape != null)
{
enumerator = shape.GetEnumerator();
enumerator.Reset();
}
float num1 = 0.0f;
int num2 = -1;
while (true)
{
bool flag = true;
ICoordinate coordinate2;
if (enumerator != null && enumerator.MoveNext())
{
coordinate2 = enumerator.Current;
}
else
{
flag = false;
coordinate2 = (ICoordinate)graph.GetVertex(edge.To);
}
PointF2D pointF2D = new GeoCoordinateLine(new GeoCoordinate((double)coordinate1.Latitude, (double)coordinate1.Longitude), new GeoCoordinate((double)coordinate2.Latitude, (double)coordinate2.Longitude), true, true).ProjectOn((PointF2D) new GeoCoordinate((double)latitude, (double)longitude));
if (pointF2D != (PointF2D)null)
{
double num3 = GeoCoordinate.DistanceEstimateInMeter(pointF2D[1], pointF2D[0], (double)latitude, (double)longitude);
if (num3 < (double)distanceToProjected)
{
distanceToProjected = (float)num3;
projectedLatitude = (float)pointF2D[1];
projectedLongitude = (float)pointF2D[0];
projectedDistanceFromFirst = num1 + (float)GeoCoordinate.DistanceEstimateInMeter((double)projectedLatitude, (double)projectedLongitude, (double)coordinate1.Latitude, (double)coordinate1.Longitude);
projectedShapeIndex = num2 + 1;
}
}
if (flag)
{
num1 += (float)GeoCoordinate.DistanceEstimateInMeter((double)coordinate1.Latitude, (double)coordinate1.Longitude, (double)coordinate2.Latitude, (double)coordinate2.Longitude);
++num2;
coordinate1 = coordinate2;
}
else
{
break;
}
}
GeoCoordinateSimple vertex = graph.GetVertex(edge.To);
totalLength = num1 + (float)GeoCoordinate.DistanceEstimateInMeter((double)coordinate1.Latitude, (double)coordinate1.Longitude, (double)vertex.Latitude, (double)vertex.Longitude);
return((double)distanceToProjected != 3.40282346638529E+38);
}
/// <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);
}
public static List <uint> SearchCloserThan(this GeometricGraph graph, float latitude, float longitude, float offset, float maxDistanceMeter, Func <GeometricEdge, bool> isOk)
{
HashSet <uint> uintSet1 = new HashSet <uint>();
GeoCoordinate geoCoordinate = new GeoCoordinate((double)latitude, (double)longitude);
HashSet <uint> uintSet2 = graph.Search(latitude, longitude, offset);
GeometricGraph.EdgeEnumerator edgeEnumerator = graph.GetEdgeEnumerator();
foreach (uint vertex1 in uintSet2)
{
GeoCoordinateSimple vertex2 = graph.GetVertex(vertex1);
if (GeoCoordinate.DistanceEstimateInMeter((double)latitude, (double)longitude, (double)vertex2.Latitude, (double)vertex2.Longitude) < (double)maxDistanceMeter)
{
edgeEnumerator.MoveTo(vertex1);
while (edgeEnumerator.MoveNext())
{
if (isOk(edgeEnumerator.Current))
{
uintSet1.Add(edgeEnumerator.Id);
break;
}
}
}
}
GeoCoordinateBox geoCoordinateBox = new GeoCoordinateBox(new GeoCoordinate((double)latitude, (double)longitude).OffsetWithDirection((Meter)((double)maxDistanceMeter), DirectionEnum.NorthWest), new GeoCoordinate((double)latitude, (double)longitude).OffsetWithDirection((Meter)((double)maxDistanceMeter), DirectionEnum.SouthEast));
HashSet <uint> uintSet3 = new HashSet <uint>();
foreach (uint vertex1 in uintSet2)
{
GeoCoordinateSimple vertex2 = graph.GetVertex(vertex1);
if (edgeEnumerator.MoveTo(vertex1) && edgeEnumerator.HasData)
{
while (edgeEnumerator.MoveNext())
{
if (!uintSet3.Contains(edgeEnumerator.Id))
{
uintSet3.Add(edgeEnumerator.Id);
bool flag = isOk == null;
ICoordinate coordinate = (ICoordinate)vertex2;
ShapeBase shapeBase = edgeEnumerator.Shape;
if (shapeBase != null)
{
if (edgeEnumerator.DataInverted)
{
shapeBase = shapeBase.Reverse();
}
IEnumerator <ICoordinate> enumerator = shapeBase.GetEnumerator();
enumerator.Reset();
while (enumerator.MoveNext())
{
ICoordinate current = enumerator.Current;
if (GeoCoordinate.DistanceEstimateInMeter((double)current.Latitude, (double)current.Longitude, (double)latitude, (double)longitude) < (double)maxDistanceMeter)
{
if (!flag && isOk(edgeEnumerator.Current))
{
flag = true;
}
if (flag)
{
uintSet1.Add(edgeEnumerator.Id);
}
}
if (geoCoordinateBox.IntersectsPotentially((double)coordinate.Longitude, (double)coordinate.Latitude, (double)current.Longitude, (double)current.Latitude))
{
PointF2D pointF2D = new GeoCoordinateLine(new GeoCoordinate((double)coordinate.Latitude, (double)coordinate.Longitude), new GeoCoordinate((double)current.Latitude, (double)current.Longitude), true, true).ProjectOn((PointF2D)geoCoordinate);
if (pointF2D != (PointF2D)null && GeoCoordinate.DistanceEstimateInMeter(pointF2D[1], pointF2D[0], (double)latitude, (double)longitude) < (double)maxDistanceMeter)
{
if (!flag && isOk(edgeEnumerator.Current))
{
flag = true;
}
if (flag)
{
uintSet1.Add(edgeEnumerator.Id);
}
}
}
coordinate = current;
}
}
ICoordinate vertex3 = (ICoordinate)graph.GetVertex(edgeEnumerator.To);
if (geoCoordinateBox.IntersectsPotentially((double)coordinate.Longitude, (double)coordinate.Latitude, (double)vertex3.Longitude, (double)vertex3.Latitude))
{
PointF2D pointF2D = new GeoCoordinateLine(new GeoCoordinate((double)coordinate.Latitude, (double)coordinate.Longitude), new GeoCoordinate((double)vertex3.Latitude, (double)vertex3.Longitude), true, true).ProjectOn((PointF2D)geoCoordinate);
if (pointF2D != (PointF2D)null && GeoCoordinate.DistanceEstimateInMeter(pointF2D[1], pointF2D[0], (double)latitude, (double)longitude) < (double)maxDistanceMeter)
{
if (!flag && isOk(edgeEnumerator.Current))
{
flag = true;
}
if (flag)
{
uintSet1.Add(edgeEnumerator.Id);
}
}
}
}
}
}
}
return(new List <uint>((IEnumerable <uint>)uintSet1));
}
public static uint[] SearchClosestEdges(this GeometricGraph graph, float latitude, float longitude, float latitudeOffset, float longitudeOffset, float maxDistanceMeter, Func <GeometricEdge, bool>[] isOks)
{
GeoCoordinate geoCoordinate = new GeoCoordinate((double)latitude, (double)longitude);
HashSet <uint> uintSet1 = graph.Search(latitude - latitudeOffset, longitude - longitudeOffset, latitude + latitudeOffset, longitude + longitudeOffset);
uint[] numArray1 = new uint[isOks.Length];
double[] numArray2 = new double[isOks.Length];
for (int index = 0; index < numArray1.Length; ++index)
{
numArray1[index] = uint.MaxValue;
numArray2[index] = (double)maxDistanceMeter;
}
GeometricGraph.EdgeEnumerator edgeEnumerator = graph.GetEdgeEnumerator();
foreach (uint vertex1 in uintSet1)
{
GeoCoordinateSimple vertex2 = graph.GetVertex(vertex1);
double num = GeoCoordinate.DistanceEstimateInMeter((double)latitude, (double)longitude, (double)vertex2.Latitude, (double)vertex2.Longitude);
for (int index = 0; index < isOks.Length; ++index)
{
if (num < numArray2[index])
{
edgeEnumerator.MoveTo(vertex1);
while (edgeEnumerator.MoveNext())
{
if (isOks[index](edgeEnumerator.Current))
{
numArray2[index] = num;
numArray1[index] = edgeEnumerator.Id;
break;
}
}
}
}
}
GeoCoordinateBox[] boxes = new GeoCoordinateBox[isOks.Length];
for (int index = 0; index < boxes.Length; ++index)
{
boxes[index] = new GeoCoordinateBox(new GeoCoordinate((double)latitude, (double)longitude).OffsetWithDirection((Meter)((double)maxDistanceMeter), DirectionEnum.NorthWest), new GeoCoordinate((double)latitude, (double)longitude).OffsetWithDirection((Meter)((double)maxDistanceMeter), DirectionEnum.SouthEast));
}
HashSet <uint> uintSet2 = new HashSet <uint>();
foreach (uint vertex1 in uintSet1)
{
GeoCoordinateSimple vertex2 = graph.GetVertex(vertex1);
if (edgeEnumerator.MoveTo(vertex1) && edgeEnumerator.HasData)
{
while (edgeEnumerator.MoveNext())
{
if (!uintSet2.Contains(edgeEnumerator.Id))
{
uintSet2.Add(edgeEnumerator.Id);
bool[] flagArray = new bool[isOks.Length];
for (int index = 0; index < isOks.Length; ++index)
{
flagArray[index] = isOks[index] == null;
}
ICoordinate coordinate = (ICoordinate)vertex2;
ShapeBase shapeBase = edgeEnumerator.Shape;
if (shapeBase != null)
{
if (edgeEnumerator.DataInverted)
{
shapeBase = shapeBase.Reverse();
}
IEnumerator <ICoordinate> enumerator = shapeBase.GetEnumerator();
enumerator.Reset();
while (enumerator.MoveNext())
{
ICoordinate current = enumerator.Current;
double num1 = GeoCoordinate.DistanceEstimateInMeter((double)current.Latitude, (double)current.Longitude, (double)latitude, (double)longitude);
for (int index = 0; index < numArray1.Length; ++index)
{
if (num1 < numArray2[index])
{
if (!flagArray[index] && isOks[index](edgeEnumerator.Current))
{
flagArray[index] = true;
}
if (flagArray[index])
{
numArray2[index] = num1;
numArray1[index] = edgeEnumerator.Id;
boxes[index] = new GeoCoordinateBox(new GeoCoordinate((double)latitude, (double)longitude).OffsetWithDirection((Meter)numArray2[index], DirectionEnum.NorthWest), new GeoCoordinate((double)latitude, (double)longitude).OffsetWithDirection((Meter)numArray2[index], DirectionEnum.SouthEast));
}
}
}
if (boxes.AnyIntersectsPotentially((double)coordinate.Longitude, (double)coordinate.Latitude, (double)current.Longitude, (double)current.Latitude))
{
PointF2D pointF2D = new GeoCoordinateLine(new GeoCoordinate((double)coordinate.Latitude, (double)coordinate.Longitude), new GeoCoordinate((double)current.Latitude, (double)current.Longitude), true, true).ProjectOn((PointF2D)geoCoordinate);
if (pointF2D != (PointF2D)null)
{
double num2 = GeoCoordinate.DistanceEstimateInMeter(pointF2D[1], pointF2D[0], (double)latitude, (double)longitude);
for (int index = 0; index < numArray1.Length; ++index)
{
if (num2 < numArray2[index])
{
if (!flagArray[index] && isOks[index](edgeEnumerator.Current))
{
flagArray[index] = true;
}
if (flagArray[index])
{
numArray2[index] = num2;
numArray1[index] = edgeEnumerator.Id;
boxes[index] = new GeoCoordinateBox(new GeoCoordinate((double)latitude, (double)longitude).OffsetWithDirection((Meter)numArray2[index], DirectionEnum.NorthWest), new GeoCoordinate((double)latitude, (double)longitude).OffsetWithDirection((Meter)numArray2[index], DirectionEnum.SouthEast));
}
}
}
}
}
coordinate = current;
}
}
ICoordinate vertex3 = (ICoordinate)graph.GetVertex(edgeEnumerator.To);
if (boxes.AnyIntersectsPotentially((double)coordinate.Longitude, (double)coordinate.Latitude, (double)vertex3.Longitude, (double)vertex3.Latitude))
{
PointF2D pointF2D = new GeoCoordinateLine(new GeoCoordinate((double)coordinate.Latitude, (double)coordinate.Longitude), new GeoCoordinate((double)vertex3.Latitude, (double)vertex3.Longitude), true, true).ProjectOn((PointF2D)geoCoordinate);
if (pointF2D != (PointF2D)null)
{
double num = GeoCoordinate.DistanceEstimateInMeter(pointF2D[1], pointF2D[0], (double)latitude, (double)longitude);
for (int index = 0; index < isOks.Length; ++index)
{
if (num < numArray2[index])
{
if (!flagArray[index] && isOks[index](edgeEnumerator.Current))
{
flagArray[index] = true;
}
if (flagArray[index])
{
numArray2[index] = num;
numArray1[index] = edgeEnumerator.Id;
boxes[index] = new GeoCoordinateBox(new GeoCoordinate((double)latitude, (double)longitude).OffsetWithDirection((Meter)numArray2[index], DirectionEnum.NorthWest), new GeoCoordinate((double)latitude, (double)longitude).OffsetWithDirection((Meter)numArray2[index], DirectionEnum.SouthEast));
}
}
}
}
}
}
}
}
}
return(numArray1);
}
/// <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);
}
