C# PriorityQueue.DequeueValue方法代码示例

    // Run A* and update the path of nodes we want to travel
    private void ComputePath(Vector3 startPos, Vector3 targetPos)
    {
        Node3D startNode = graph.NearestNode(startPos);
        Node3D targetNode = graph.NearestNode(targetPos);

        HashSet<Node3D> visited = new HashSet<Node3D>();
        PriorityQueue<float, Node3D> frontier = new PriorityQueue<float, Node3D>();
        frontier.Enqueue(0f, startNode);

        // initialize map of parents (in-edge neighbor to each vertex) for path reconstruction
        Node3D[,,] parents = new Node3D[graph.xgrid + 1, graph.ygrid + 1, graph.zgrid + 1];
        parents[startNode.x, startNode.y, startNode.z] = startNode;

        // initialize costs
        float[,,] costs = new float[graph.xgrid + 1, graph.ygrid + 1, graph.zgrid + 1];
        for (int i = 0; i < graph.xgrid; i++) {
            for (int j = 0; j < graph.ygrid; j++) {
                for (int k = 0; k < graph.zgrid; k++) {
                    if (i == startNode.x && j == startNode.y && k == startNode.z) {
                        costs[i, j, k] = 0f;
                    } else {
                        costs[i, j, k] = -1f;
                    }
                }
            }
        }

        bool foundPath = false;
        Node3D current;
        while (!frontier.IsEmpty) {
            current = frontier.DequeueValue();
            if (current == targetNode) {
                foundPath = true;
                break;
            }
            visited.Add(current);

            Node3D[] neighbors = graph.neighbors[current.x][current.y][current.z];
            for (int i = 0; i < neighbors.Length; i++) {
                if (visited.Contains(neighbors[i])) {
                    continue;
                }

                float cost = costs[current.x, current.y, current.z] + graph.dist;
                // If we found a better cost/distance, add to frontier
                if (costs[neighbors[i].x, neighbors[i].y, neighbors[i].z] < 0f || cost < costs[neighbors[i].x, neighbors[i].y, neighbors[i].z]) {
                    costs[neighbors[i].x, neighbors[i].y, neighbors[i].z] = cost;
                    parents[neighbors[i].x, neighbors[i].y, neighbors[i].z] = current;
                    float heuristic = HeuristicValue(neighbors[i], targetNode);
                    frontier.Enqueue(cost + heuristic, neighbors[i]);
                }
            }
        }

        // Either we found a path, or finished searching with no results
        if (!foundPath) {
            return;
        }

        // Reconstruct path using parents
        current = targetNode;
        List<Vector3> newPath = new List<Vector3>();
        newPath.Add(graph.WorldPosition(current));
        while (current != startNode) {
            current = parents[current.x, current.y, current.z];
            newPath.Add(graph.WorldPosition(current));
        }

        // Smooth path by deleting unneeded nodes (going in reverse)
        path.Clear();
        int p0 = newPath.Count - 1;
        int p1 = newPath.Count - 2;
        path.Add(newPath[p0]);

        RaycastHit hit = new RaycastHit();
        while (p1 >= 0) {
            float dist = Vector3.Distance(newPath[p0], newPath[p1]);
            Vector3 dir = newPath[p1] - newPath[p0];
            Vector3 normal = Vector3.Normalize(Vector3.Cross(dir, Vector3.up)) * 0.5f;
            if (p1 == 0 || newPath[p0].y != newPath[p1].y || newPath[p0].y > 2f || newPath[p1].y > 2f || p0 - p1 > 3 ||
               (Physics.Raycast(newPath[p0], dir, out hit, dist) && hit.collider.tag == graph.collisionTag) ||
               (Physics.Raycast(newPath[p0] + normal, dir, out hit, dist) && hit.collider.tag == graph.collisionTag) ||
               (Physics.Raycast(newPath[p0] - normal, dir, out hit, dist) && hit.collider.tag == graph.collisionTag)) {
                path.Add(newPath[p1 + 1]);
                p0 = p1;
            }
            p1--;
        }
    }

    public Node[] FindPath(Node source, Node destination)
    {
        //Initialize single source
        float[] d = new float[nodos.Count];

        for (int i = 0; i < d.Length; i++) {
            d[i] = Mathf.Infinity;
        }
        d[node2int[source]] = 0.0f;
        Node[] p = new Node[nodos.Count];

        //Dijkstra
        PriorityQueue<float, Node> q = new PriorityQueue<float, Node>(nodos.Count);

        foreach(Node n in nodos){
            q.Add(new KeyValuePair<float, Node>(d[node2int[n]], n));
        }

        while(q.Count > 0){
            Node u = q.DequeueValue();
            for(int i = 0; i < nodos.Count; i++){
                if(i != node2int[u] && adj[node2int[u], i] != Mathf.Infinity){
                    //Relaxation
                    if(d[i] > d[node2int[u]] + adj[node2int[u], i]){
                        //Debug.Log("Contiene" + q.Contains(new KeyValuePair<float, Node>(d[i], nodos[i])));
                        q.Remove(new KeyValuePair<float, Node>(d[i], nodos[i]));

                        d[i] = d[node2int[u]] + adj[node2int[u], i];
                        p[i] = u;
                        //Debug.Log(i + " " + d[i]);
                        //Update values
                        q.Add(new KeyValuePair<float, Node>(d[i], nodos[i]));
                    }
                }
            }
            //Debug.Log("Iteration" + node2int[u]);
        }

        Stack<Node> path = new Stack<Node>();

        Node prev = destination;

        while(prev != source){
            if(d[node2int[prev]] == Mathf.Infinity){
                throw new Exception("Unreachable node");
            }
            path.Push(prev);
            prev = p[node2int[prev]];
        }

        path.Push(source);
        return path.ToArray();
    }