C# PriorityQueue.ExtractMin方法代码示例

        /// <summary>
        /// The dijkstra's shortest path procedure for a directed &quot; non-negative &quot; weighted graphs.
        /// </summary>
        /// <param name="directedGraph">
        /// a directed graph containing a set V of n vertices 
        /// and a set E of m directed edges with nonnegative weights.
        /// </param>
        /// <param name="sourceVertex">a source vertex in V.</param>
        /// <remarks>
        /// Result:
        ///     For each non-source vertex v in V, shortest[v] is the weight sp(s,v) of a shortest path from s to v and pred(v) is the vertex
        ///     preceding v on some shortest path.
        ///     For the source vertex s, shortest(s) = 0 and pred(s) = NULL.
        ///     If there is no path from s to v, then shortest[v] = infinity, and pred(v) = NULL.
        /// </remarks>
        public void Compute(IGraph directedGraph, int sourceVertex)
        {
            Graph.Shortest = new double[directedGraph.N];
            Graph.Predecessors = new int?[directedGraph.N];

            for (int i = 0; i < directedGraph.N; ++i)
            {
                Graph.Shortest[i] = double.PositiveInfinity;
                Graph.Predecessors[i] = null;
            }

            Graph.Shortest[sourceVertex] = 0;

            var queue = new PriorityQueue<int>();

            foreach (var v in directedGraph.Vertices)
                queue.Insert(v);

            while (queue.Count > 0)
            {
                var u = queue.ExtractMin();
                double tempShortest;

                foreach (var v in directedGraph[u])
                {
                    tempShortest = Shortest[v];

                    Graph.Relax(directedGraph, u, v);

                    if (tempShortest != Shortest[v])
                        queue.DecreaseKey(v);
                }
            }
        }

    public static List<int> DijkstraAlgorithm(Dictionary<Node, Dictionary<Node, int>> graph, Dictionary<int, Node> nodes, int sourceNode, int destinationNode)
    {
        int[] previous = new int[graph.Count];
        bool[] visited = new bool[graph.Count];
        PriorityQueue<Node> priorityQueue = new PriorityQueue<Node>();
        var startNode = nodes[sourceNode];
        startNode.DistanceFromStart = 100000;

        for (int i = 0; i < previous.Length; i++)
        {
            previous[i] = -1;
        }

        priorityQueue.Enqueue(startNode);

        while (priorityQueue.Count > 0)
        {
            var currentNode = priorityQueue.ExtractMin();

            if (currentNode.Index == destinationNode)
            {
                break;
            }

            foreach (var edge in graph[currentNode])
            {
                if (!visited[edge.Key.Index])
                {
                    priorityQueue.Enqueue(edge.Key);
                    visited[edge.Key.Index] = true;
                }

                var distance = ((double)currentNode.DistanceFromStart / 100000) * ((double)edge.Value / 100000) * 100000;
                if (distance > edge.Key.DistanceFromStart && edge.Key.Index != sourceNode)
                {
                    edge.Key.DistanceFromStart = (int)distance;
                    previous[edge.Key.Index] = currentNode.Index;
                    priorityQueue.DecreaseKey(edge.Key);
                }
            }
        }

        if (previous[destinationNode] == -1)
        {
            return null;
        }

        List<int> path = new List<int>();
        int current = destinationNode;
        while (current != -1)
        {
            path.Add(current);
            current = previous[current];
        }

        path.Reverse();
        return path;
    }

        public LinkedList<Vector3> FindPathTo(Node end)
        {
            HashSet<Node> closedSet = new HashSet<Node>();
            PriorityQueue openSet = new PriorityQueue();
            Dictionary<Node, int> g_score = new Dictionary<Node, int>();
            Dictionary<Node, Node> cameFrom = new Dictionary<Node, Node>();

            g_score.Add(this, 0);
            openSet.Insert(heuristicDistance(this, end), this);

            while (openSet.Length > 0)
            {
                Node currentNode = openSet.ExtractMin();

                if (currentNode == end)
                {
                    LinkedList<Vector3> path = ReconstructPath(cameFrom, end);
                    return path;
                }

                closedSet.Add(currentNode);

                if (currentNode.neighbours != null && currentNode.neighbours.Length > 0)
                {
                    foreach (var neighbour in currentNode.neighbours)
                    {
                        if (neighbour.cost >= 100)
                            continue;

                        int tentative_g_score = g_score[currentNode] + RealDistance(currentNode, neighbour);

                        if (closedSet.Contains(neighbour))
                        {
                            if(tentative_g_score >= g_score[currentNode])
                                continue;
                            else
                            {
                                Debug.Log("Remove");
                                closedSet.Remove(currentNode);
                            }
                        }

                        if (openSet.Contains(neighbour))
                        {
                            if (tentative_g_score >= g_score[neighbour])
                                continue;
                            else
                                openSet.DecreaseKey(tentative_g_score + heuristicDistance(neighbour, end), neighbour);
                        }
                        else
                            openSet.Insert(tentative_g_score + heuristicDistance(neighbour, end), neighbour);

                        cameFrom[neighbour] = currentNode;
                        g_score[neighbour] = tentative_g_score;
                    }
                }
            }

            return null;
        }