/// <summary>
        /// Computes a shortest paths tree from the specified sourceVertex to every other vertex in the edge-weighted directed graph
        /// </summary>
        /// <param name="graph">The edge-weighted directed graph</param>
        /// <param name="sourceVertex">The source vertex to compute the shortest paths tree from</param>
        /// <exception cref="ArgumentOutOfRangeException">Throws an ArgumentOutOfRangeException if an edge weight is negative</exception>
        /// <exception cref="ArgumentNullException">Thrown if EdgeWeightedDigraph is null</exception>
        public DijkstraShortestPath(EdgeWeightedDigraph graph, int sourceVertex)
        {
            if (graph == null)
            {
                throw new ArgumentNullException("graph", "EdgeWeightedDigraph cannot be null");
            }

            foreach (DirectedEdge edge in graph.Edges())
            {
                if (edge.Weight < 0)
                {
                    throw new ArgumentOutOfRangeException(string.Format("Edge: '{0}' has negative weight", edge));
                }
            }

            _distanceTo = new double[graph.NumberOfVertices];
            _edgeTo     = new DirectedEdge[graph.NumberOfVertices];
            for (int v = 0; v < graph.NumberOfVertices; v++)
            {
                _distanceTo[v] = Double.PositiveInfinity;
            }
            _distanceTo[sourceVertex] = 0.0;

            _priorityQueue = new IndexMinPriorityQueue <double>(graph.NumberOfVertices);
            _priorityQueue.Insert(sourceVertex, _distanceTo[sourceVertex]);
            while (!_priorityQueue.IsEmpty())
            {
                int v = _priorityQueue.DeleteMin();
                foreach (DirectedEdge edge in graph.Adjacent(v))
                {
                    Relax(edge);
                }
            }
        }
예제 #2
0
        private DijkstraShortestPath(EdgeWeightedDigraph graph, int sourceVertex, int?destinationVertex)
        {
            if (graph == null)
            {
                throw new ArgumentNullException(nameof(graph), "EdgeWeightedDigraph cannot be null");
            }

            foreach (DirectedEdge edge in graph.Edges())
            {
                if (edge.Weight < 0)
                {
                    throw new ArgumentOutOfRangeException($"Edge: '{edge}' has negative weight");
                }
            }

            _distanceTo = new double[graph.NumberOfVertices];
            _edgeTo     = new DirectedEdge[graph.NumberOfVertices];
            for (int v = 0; v < graph.NumberOfVertices; v++)
            {
                _distanceTo[v] = double.PositiveInfinity;
            }
            _distanceTo[sourceVertex] = 0.0;

            _priorityQueue = new IndexMinPriorityQueue <double>(graph.NumberOfVertices);
            _priorityQueue.Insert(sourceVertex, _distanceTo[sourceVertex]);
            while (!_priorityQueue.IsEmpty())
            {
                int v = _priorityQueue.DeleteMin();

                if (destinationVertex.HasValue && v == destinationVertex.Value)
                {
                    return;
                }

                foreach (DirectedEdge edge in graph.Adjacent(v))
                {
                    Relax(edge);
                }
            }
        }
예제 #3
0
        /// <summary>
        /// Returns an List of Cells representing a shortest path from the specified source to the specified destination
        /// </summary>
        /// <param name="source">The source Cell to find a shortest path from</param>
        /// <param name="destination">The destination Cell to find a shortest path to</param>
        /// <param name="map">The Map on which to find the shortest path between Cells</param>
        /// <returns>List of Cells representing a shortest path from the specified source to the specified destination</returns>
        public List <TCell> FindPath(TCell source, TCell destination, IMap <TCell> map)
        {
            // OPEN = the set of nodes to be evaluated
            IndexMinPriorityQueue <PathNode> openNodes = new IndexMinPriorityQueue <PathNode>(map.Height * map.Width);

            // CLOSED = the set of nodes already evaluated
            bool[] isNodeClosed = new bool[map.Height * map.Width];

            // add the start node to OPEN
            openNodes.Insert(map.IndexFor(source), new PathNode
            {
                DistanceFromStart        = 0,
                HeuristicDistanceFromEnd = CalculateDistance(source, destination, _diagonalCost),
                X      = source.X,
                Y      = source.Y,
                Parent = null
            });

            PathNode currentNode;

            // loop
            while (true)
            {
                // current = node in OPEN with the lowest f_cost
                if (openNodes.Size < 1)
                {
                    return(null);
                }
                currentNode = openNodes.MinKey();
                // remove current from OPEN
                int currentIndex = openNodes.DeleteMin();
                // add current to CLOSED
                isNodeClosed[currentIndex] = true;

                ICell currentCell = map.CellFor(currentIndex);
                // if current is the target node the path has been found
                if (currentCell.Equals(destination))
                {
                    break;
                }

                // foreach neighbor of the current node
                bool includeDiagonals = _diagonalCost.HasValue;
                foreach (TCell neighbor in map.GetAdjacentCells(currentCell.X, currentCell.Y, includeDiagonals))
                {
                    int neighborIndex = map.IndexFor(neighbor);
                    // if neighbor is not walkable or neighbor is in CLOSED
                    if (neighbor.IsWalkable == false || isNodeClosed[neighborIndex])
                    {
                        // skip to the next neighbor
                        continue;
                    }

                    bool isNeighborInOpen = openNodes.Contains(neighborIndex);

                    // if neighbor is in OPEN
                    if (isNeighborInOpen)
                    {
                        // if new path to neighbor is shorter
                        PathNode neighborNode = openNodes.KeyAt(neighborIndex);
                        double   newDistance  = currentNode.DistanceFromStart + 1;
                        if (newDistance < neighborNode.DistanceFromStart)
                        {
                            // update neighbor distance
                            neighborNode.DistanceFromStart = newDistance;
                            // set parent of neighbor to current
                            neighborNode.Parent = currentNode;
                        }
                    }
                    else // if neighbor is not in OPEN
                    {
                        // set f_cost of neighbor
                        // set parent of neighbor to current
                        PathNode neighborNode = new PathNode
                        {
                            DistanceFromStart        = currentNode.DistanceFromStart + 1,
                            HeuristicDistanceFromEnd = CalculateDistance(source, destination, _diagonalCost),
                            X      = neighbor.X,
                            Y      = neighbor.Y,
                            Parent = currentNode
                        };
                        // add neighbor to OPEN
                        openNodes.Insert(neighborIndex, neighborNode);
                    }
                }
            }

            List <TCell> path = new List <TCell>();

            path.Add(map.GetCell(currentNode.X, currentNode.Y));
            while (currentNode.Parent != null)
            {
                currentNode = currentNode.Parent;
                path.Add(map.GetCell(currentNode.X, currentNode.Y));
            }

            path.Reverse();
            return(path);
        }