private List <GraphNode> GetRoute(Point from, Point to)
{
lock (_syncObject)
{
var x = (int)Math.Floor((double)from.X / _searchWayAreaSize);
var y = (int)Math.Floor((double)from.Y / _searchWayAreaSize);
Point startAreaIndex = new Point(x, y);
var startNode = new GraphNode(startAreaIndex, startAreaIndex)
{
SourceWP = new WayPoint(from.X, from.Y),
TargetWP = new WayPoint(from.X, from.Y)
};
x = (int)Math.Floor((double)to.X / _searchWayAreaSize);
y = (int)Math.Floor((double)to.Y / _searchWayAreaSize);
Point endAreaIndex = new Point(x, y);
var endNode = new GraphNode(endAreaIndex, endAreaIndex)
{
SourceWP = new WayPoint(to.X, to.Y),
TargetWP = new WayPoint(to.X, to.Y)
};
try
{
_patensyGraph.AddVertex(startNode);
IEnumerable <GraphNode> nodesAroundStartNode =
_patensyGraph.Vertices.Where(n => n.SourceArea == startAreaIndex || n.TargetArea == startAreaIndex);
bool hasRouteFromStart = false;
foreach (var node in nodesAroundStartNode)
{
int routeLenght;
if (HasRoute(startNode, node, out routeLenght))
{
var edgeFromStart = new QuickGraph.Edge <GraphNode>(startNode, node);
_patensyGraph.AddEdge(edgeFromStart);
_weightsDictionary.Add(edgeFromStart, routeLenght);
hasRouteFromStart = true;
}
}
if (!hasRouteFromStart)
{
return(null);
}
_patensyGraph.AddVertex(endNode);
IEnumerable <GraphNode> nodesAroundEndNode =
_patensyGraph.Vertices.Where(n => n.SourceArea == endAreaIndex || n.TargetArea == endAreaIndex);
bool hasRouteToEnd = false;
foreach (var node in nodesAroundEndNode)
{
int routeLenght;
if (HasRoute(node, endNode, out routeLenght))
{
var edgeToEnd = new QuickGraph.Edge <GraphNode>(node, endNode);
_patensyGraph.AddEdge(edgeToEnd);
_weightsDictionary.Add(edgeToEnd, routeLenght);
hasRouteToEnd = true;
}
}
if (!hasRouteToEnd)
{
return(null);
}
if (_weightsDictionary.Count < _patensyGraph.EdgeCount)
{
throw new InvalidOperationException(string.Format("Не определен вес {0} ребер", _patensyGraph.EdgeCount - _weightsDictionary.Count));
}
Func <QuickGraph.Edge <GraphNode>, double> func =
AlgorithmExtensions.GetIndexer <QuickGraph.Edge <GraphNode>, double>(_weightsDictionary);
IEnumerable <QuickGraph.Edge <GraphNode> > path = null;
var tryFunc = _patensyGraph.ShortestPathsDijkstra(func, startNode);
bool result = tryFunc(endNode, out path);
if (result)
{
List <GraphNode> route = new List <GraphNode>();
foreach (var edge in path)
{
if (route.Count == 0)
{
route.Add(edge.Target);
}
else
{
if (route.Last().Equals(edge.Target))
{
route.Add(edge.Source);
}
else
{
route.Add(edge.Target);
}
}
}
return(route);
}
else
{
return(null);
}
}
finally
{
foreach (var edge in _patensyGraph.Edges.Where(e => e.Source.Equals(startNode)))
{
_weightsDictionary.Remove(edge);
}
foreach (var edge in _patensyGraph.Edges.Where(e => e.Target.Equals(endNode)))
{
_weightsDictionary.Remove(edge);
}
_patensyGraph.RemoveVertex(startNode);
_patensyGraph.RemoveVertex(endNode);
}
}
}
/// <summary>
/// Finds all routes containing the text "(auto)" and tries to auto-route them by using all non-auto routes
/// </summary>
/// <param name="updateNavObjXmlFileName"></param>
public void MakeRoutes(string updateNavObjXmlFileName)
{
XDocument docNavObj = XDocument.Load(updateNavObjXmlFileName);
var autoRoutes = new List <AutoRouteEntry>();
var idToVertexMap = new Dictionary <string, RouteVertex>();
var nameToVertexMap = new Dictionary <string, RouteVertex>();
var edgeWeightMap = new Dictionary <QuickGraph.Edge <RouteVertex>, double>();
var nonUniqueNames = new List <string>();
// Process each (non-auto) route into a "all routes" graph,
// and add auto-routes to a separate list for later processing
var graph = new QuickGraph.UndirectedGraph <RouteVertex, QuickGraph.Edge <RouteVertex> >();
foreach (var elemRoute in docNavObj.Element(nsGpx + "gpx").Elements(nsGpx + "rte"))
{
RouteVertex previousVertex = null;
// Is this an auto-route?
AutoRouteEntry autoRouteEntry = GetAutoRouteEntry(elemRoute);
if (autoRouteEntry != null)
{
autoRoutes.Add(autoRouteEntry);
continue;
}
// Process each route point in route, adding vertices and edges as we follow the route
foreach (var elemRoutePoint in elemRoute.Elements(nsGpx + "rtept"))
{
string routePointId = RouteVertex.GetElementId(elemRoutePoint);
// Get this route point's vertex (existing, or create new and add to graph)
RouteVertex elementVertex;
if (!idToVertexMap.TryGetValue(routePointId, out elementVertex))
{
elementVertex = new RouteVertex {
RoutePointElement = elemRoutePoint
};
graph.AddVertex(elementVertex);
// Add new vertex to dictionaries
idToVertexMap.Add(routePointId, elementVertex);
string elementVertexName = elementVertex.Name;
if (nameToVertexMap.ContainsKey(elementVertexName))
{
nonUniqueNames.Add(elementVertexName);
}
else
{
nameToVertexMap.Add(elementVertexName, elementVertex);
}
}
// Did we get to this vertex from another vertex? If so, add the edge and calculate its weight
if (previousVertex != null)
{
var edge = new QuickGraph.Edge <RouteVertex>(previousVertex, elementVertex);
graph.AddEdge(edge);
// Pre-calculate edge weight = distance in meters
Coordinate coordOrigin = previousVertex.Coordinate;
Coordinate coordDestination = elementVertex.Coordinate;
double weight = GeoCalculator.GetDistance(coordOrigin, coordDestination, 1, DistanceUnit.Meters);
edgeWeightMap[edge] = weight;
}
// Save current vertex as previous for next vertex
previousVertex = elementVertex;
}
}
// Find best routes for each AutoRouteEntry
foreach (AutoRouteEntry arEntry in autoRoutes)
{
// Check that auto-route uses unique names
var nonUniqueUsed = arEntry.WaypointNames.Where(x => nonUniqueNames.Contains(x)).ToList();
if (nonUniqueUsed.Count > 0)
{
SetRouteError(arEntry.RouteElement, "Non-unique waypoint name(s): " + string.Join(", ", nonUniqueUsed));
continue;
}
// Check for unknown names
var unknownWaypoints = arEntry.WaypointNames.Where(x => !nameToVertexMap.ContainsKey(x)).ToList();
if (unknownWaypoints.Count > 0)
{
SetRouteError(arEntry.RouteElement, "Unknown waypoint name(s): " + string.Join(", ", unknownWaypoints));
continue;
}
var routeElement = arEntry.RouteElement;
bool firstPart = true;
bool firstWaypoint = true;
// Process the auto route a 'pair of waypoints' at a time
for (int i = 0; i < arEntry.WaypointNames.Count - 1; i++)
{
RouteVertex start = nameToVertexMap[arEntry.WaypointNames[i]];
RouteVertex stop = nameToVertexMap[arEntry.WaypointNames[i + 1]];
var shortestPaths = graph.ShortestPathsDijkstra((x) => edgeWeightMap[x], start);
if (shortestPaths(stop, out var result))
{
// If first part of auto route, clear the existing route data
if (firstPart)
{
foreach (var deleteMe in routeElement.Elements(nsGpx + "rtept").ToList())
{
deleteMe.Remove();
}
firstPart = false;
}
RouteVertex currentPos = start;
foreach (var edge in result)
{
// Graph is not directional, so edges can be in 'reverse' direction
// We should not use current position vertex as next vertex
RouteVertex nextVertex = edge.Source != currentPos ? edge.Source : edge.Target;
// If this is the very first waypoint of the complete auto route - add it to output (if not first edge, the waypoint has already been put into the route)
if (firstWaypoint)
{
var pointElement = new XElement(currentPos.RoutePointElement);
routeElement.Add(pointElement);
firstWaypoint = false;
}
routeElement.Add(new XElement(nextVertex.RoutePointElement));
currentPos = nextVertex;
}
}
else
{
SetRouteError(arEntry.RouteElement, "No route found from: \"" + start.Name + "\" to \"" + stop.Name + "\"");
break;
}
}
}
// Overwrite original navobj file
docNavObj.Save(updateNavObjXmlFileName);
}
