예제 #1
0
        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);
                            }
                        }
                    }
                }
            }
        }
예제 #2
0
        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);
                            }
                        }
                    }
                }
            }
        }
예제 #3
0
        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);
                }
            }
        }
예제 #4
0
        /// <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);
        }