internal void CreatePatensyGraph(int layerId)
{
_patensyGraph = new QuickGraph.UndirectedGraph<GraphNode, QuickGraph.Edge<GraphNode>>(false);
_weightsDictionary = new Dictionary<QuickGraph.Edge<GraphNode>, double>();
if (Math.Max(this.Width, this.Height) > 128)
{
double power = Math.Log((double)this.Width * this.Height / 1500, 2) / 2;
power = Math.Ceiling(power);
if (power > 7)
{
power = 7;
}
if (power < 4)
{
power = 4;
}
_searchWayAreaSize = (int)Math.Pow(2, power);
}
else
{
//Дополнитеный уровень иерархии при поиске не используется
_searchWayAreaSize = 0;
return;
}
//Создаем вершины
for (int j = 0; j < Math.Ceiling((double)this.Height / _searchWayAreaSize); j++)
{
for (int i = 0; i < Math.Ceiling((double)this.Width / _searchWayAreaSize); i++)
{
if ((j + 1) * _searchWayAreaSize < this.Height)
{
int width = (this.Width - i * _searchWayAreaSize) >= _searchWayAreaSize ? _searchWayAreaSize : this.Width - i * _searchWayAreaSize;
var cells = this.GetCells(i * _searchWayAreaSize, (j + 1) * _searchWayAreaSize - 1, width, 2);
GraphNode node = null;
for (int k = 0; k < width; k++)
{
//if ((cells[k, 0].StaticValue == 1 || cells[k, 1].StaticValue == 1) && cells[k, 0].StaticValue != 0 && cells[k, 1].StaticValue != 0)
if (cells[k, 0].StaticValue > 0x00 || cells[k, 1].StaticValue > 0x00)
{
if (node == null)
{
node = new GraphNode(i, j, i, j + 1)
{
SourceWP = new WayPoint(i * _searchWayAreaSize + k, (j + 1) * _searchWayAreaSize - 1, layerId, 1, 1),
TargetWP = new WayPoint(i * _searchWayAreaSize + k, (j + 1) * _searchWayAreaSize, layerId, 1, 1),
Volume = cells[k, 0].StaticValue < cells[k, 1].StaticValue ? cells[k, 0].StaticValue : cells[k, 1].StaticValue
};
}
else
{
node.SourceWP.Width++;
node.TargetWP.Width++;
node.Volume += cells[k, 0].StaticValue < cells[k, 1].StaticValue ? cells[k, 0].StaticValue : cells[k, 1].StaticValue;
}
}
else
{
if (node != null)
{
_patensyGraph.AddVertex(node);
node = null;
}
}
}
if (node != null)
{
_patensyGraph.AddVertex(node);
node = null;
}
}
if ((i + 1) * _searchWayAreaSize < this.Width)
{
int height = (this.Height - j * _searchWayAreaSize) >= _searchWayAreaSize ? _searchWayAreaSize : this.Height - j * _searchWayAreaSize;
var cells = GetCells((i + 1) * _searchWayAreaSize - 1, j * _searchWayAreaSize, 2, height);
GraphNode node = null;
for (int k = 0; k < height; k++)
{
//if ((cells[0, k].StaticValue == 1 || cells[1, k].StaticValue == 1) && cells[0, k].StaticValue != 0 && cells[1, k].StaticValue != 0)
if (cells[0, k].StaticValue > 0x00 || cells[1, k].StaticValue > 0x00)
{
if (node == null)
{
node = new GraphNode(i, j, i + 1, j)
{
SourceWP = new WayPoint((i + 1) * _searchWayAreaSize - 1, j * _searchWayAreaSize + k, layerId, 1, 1),
TargetWP = new WayPoint((i + 1) * _searchWayAreaSize, j * _searchWayAreaSize + k, layerId, 1, 1),
Volume = cells[0, k].StaticValue < cells[1, k].StaticValue ? cells[0, k].StaticValue : cells[1, k].StaticValue
};
}
else
{
node.SourceWP.Height++;
node.TargetWP.Height++;
node.Volume += cells[0, k].StaticValue < cells[1, k].StaticValue ? cells[0, k].StaticValue : cells[1, k].StaticValue;
}
}
else
{
if (node != null)
{
_patensyGraph.AddVertex(node);
node = null;
}
}
}
if (node != null)
{
_patensyGraph.AddVertex(node);
node = null;
}
}
}
}
//Создаем ребра
for (int j = 0; j < this.Height / _searchWayAreaSize + 1; j++)
{
for (int i = 0; i < this.Width / _searchWayAreaSize + 1; i++)
{
Point areaIndex = new Point(i, j);
List<GraphNode> nodes = new List<GraphNode>(from node in _patensyGraph.Vertices
where (node.SourceArea == areaIndex || node.TargetArea == areaIndex)
select node);
while (nodes.Count() > 1)
{
var first = nodes.FirstOrDefault();
WayPoint firstWp = first.SourceArea == areaIndex ? first.SourceWP : first.TargetWP;
nodes.Remove(first);
foreach (var node in nodes)
{
WayPoint nextWp = node.SourceArea == areaIndex ? node.SourceWP : node.TargetWP;
//Point toP = node.Center;
//if (first.FromArea == areaIndex)
//{
// if (first.FromArea.X == first.ToArea.X)
// {
// fromP.Offset(0, -1);
// }
// else
// {
// fromP.Offset(-1, 0);
// }
//}
//if (node.FromArea == areaIndex)
//{
// if (node.FromArea.X == node.ToArea.X)
// {
// toP.Offset(0, -1);
// }
// else
// {
// toP.Offset(-1, 0);
// }
//}
int routeLenght;
if (HasRoute(first, node, out routeLenght))
{
var forwardEgde = new QuickGraph.Edge<GraphNode>(first, node);
_patensyGraph.AddEdge(forwardEgde);
_weightsDictionary.Add(forwardEgde, routeLenght);
//var backwardEgde = new QuickGraph.Edge<GraphNode>(node, first);
//_patensyGraph.AddEdge(backwardEgde);
//_weightsDictionary.Add(backwardEgde, routeLenght);
}
}
}
}
}
}
internal void CreatePatensyGraph(int layerId)
{
_patensyGraph = new QuickGraph.UndirectedGraph <GraphNode, QuickGraph.Edge <GraphNode> >(false);
_weightsDictionary = new Dictionary <QuickGraph.Edge <GraphNode>, double>();
if (Math.Max(this.Width, this.Height) > 128)
{
double power = Math.Log((double)this.Width * this.Height / 1500, 2) / 2;
power = Math.Ceiling(power);
if (power > 7)
{
power = 7;
}
if (power < 4)
{
power = 4;
}
_searchWayAreaSize = (int)Math.Pow(2, power);
}
else
{
//Дополнитеный уровень иерархии при поиске не используется
_searchWayAreaSize = 0;
return;
}
//Создаем вершины
for (int j = 0; j < Math.Ceiling((double)this.Height / _searchWayAreaSize); j++)
{
for (int i = 0; i < Math.Ceiling((double)this.Width / _searchWayAreaSize); i++)
{
if ((j + 1) * _searchWayAreaSize < this.Height)
{
int width = (this.Width - i * _searchWayAreaSize) >= _searchWayAreaSize ? _searchWayAreaSize : this.Width - i * _searchWayAreaSize;
var cells = this.GetCells(i * _searchWayAreaSize, (j + 1) * _searchWayAreaSize - 1, width, 2);
GraphNode node = null;
for (int k = 0; k < width; k++)
{
//if ((cells[k, 0].StaticValue == 1 || cells[k, 1].StaticValue == 1) && cells[k, 0].StaticValue != 0 && cells[k, 1].StaticValue != 0)
if (cells[k, 0].StaticValue > 0x00 || cells[k, 1].StaticValue > 0x00)
{
if (node == null)
{
node = new GraphNode(i, j, i, j + 1)
{
SourceWP = new WayPoint(i * _searchWayAreaSize + k, (j + 1) * _searchWayAreaSize - 1, layerId, 1, 1),
TargetWP = new WayPoint(i * _searchWayAreaSize + k, (j + 1) * _searchWayAreaSize, layerId, 1, 1),
Volume = cells[k, 0].StaticValue < cells[k, 1].StaticValue ? cells[k, 0].StaticValue : cells[k, 1].StaticValue
};
}
else
{
node.SourceWP.Width++;
node.TargetWP.Width++;
node.Volume += cells[k, 0].StaticValue < cells[k, 1].StaticValue ? cells[k, 0].StaticValue : cells[k, 1].StaticValue;
}
}
else
{
if (node != null)
{
_patensyGraph.AddVertex(node);
node = null;
}
}
}
if (node != null)
{
_patensyGraph.AddVertex(node);
node = null;
}
}
if ((i + 1) * _searchWayAreaSize < this.Width)
{
int height = (this.Height - j * _searchWayAreaSize) >= _searchWayAreaSize ? _searchWayAreaSize : this.Height - j * _searchWayAreaSize;
var cells = GetCells((i + 1) * _searchWayAreaSize - 1, j * _searchWayAreaSize, 2, height);
GraphNode node = null;
for (int k = 0; k < height; k++)
{
//if ((cells[0, k].StaticValue == 1 || cells[1, k].StaticValue == 1) && cells[0, k].StaticValue != 0 && cells[1, k].StaticValue != 0)
if (cells[0, k].StaticValue > 0x00 || cells[1, k].StaticValue > 0x00)
{
if (node == null)
{
node = new GraphNode(i, j, i + 1, j)
{
SourceWP = new WayPoint((i + 1) * _searchWayAreaSize - 1, j * _searchWayAreaSize + k, layerId, 1, 1),
TargetWP = new WayPoint((i + 1) * _searchWayAreaSize, j * _searchWayAreaSize + k, layerId, 1, 1),
Volume = cells[0, k].StaticValue < cells[1, k].StaticValue ? cells[0, k].StaticValue : cells[1, k].StaticValue
};
}
else
{
node.SourceWP.Height++;
node.TargetWP.Height++;
node.Volume += cells[0, k].StaticValue < cells[1, k].StaticValue ? cells[0, k].StaticValue : cells[1, k].StaticValue;
}
}
else
{
if (node != null)
{
_patensyGraph.AddVertex(node);
node = null;
}
}
}
if (node != null)
{
_patensyGraph.AddVertex(node);
node = null;
}
}
}
}
//Создаем ребра
for (int j = 0; j < this.Height / _searchWayAreaSize + 1; j++)
{
for (int i = 0; i < this.Width / _searchWayAreaSize + 1; i++)
{
Point areaIndex = new Point(i, j);
List <GraphNode> nodes = new List <GraphNode>(from node in _patensyGraph.Vertices
where (node.SourceArea == areaIndex || node.TargetArea == areaIndex)
select node);
while (nodes.Count() > 1)
{
var first = nodes.FirstOrDefault();
WayPoint firstWp = first.SourceArea == areaIndex ? first.SourceWP : first.TargetWP;
nodes.Remove(first);
foreach (var node in nodes)
{
WayPoint nextWp = node.SourceArea == areaIndex ? node.SourceWP : node.TargetWP;
//Point toP = node.Center;
//if (first.FromArea == areaIndex)
//{
// if (first.FromArea.X == first.ToArea.X)
// {
// fromP.Offset(0, -1);
// }
// else
// {
// fromP.Offset(-1, 0);
// }
//}
//if (node.FromArea == areaIndex)
//{
// if (node.FromArea.X == node.ToArea.X)
// {
// toP.Offset(0, -1);
// }
// else
// {
// toP.Offset(-1, 0);
// }
//}
int routeLenght;
if (HasRoute(first, node, out routeLenght))
{
var forwardEgde = new QuickGraph.Edge <GraphNode>(first, node);
_patensyGraph.AddEdge(forwardEgde);
_weightsDictionary.Add(forwardEgde, routeLenght);
//var backwardEgde = new QuickGraph.Edge<GraphNode>(node, first);
//_patensyGraph.AddEdge(backwardEgde);
//_weightsDictionary.Add(backwardEgde, routeLenght);
}
}
}
}
}
}
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);
}