C# PriorityQueue.queue方法代码示例

        // <summary>
        // Given two Node objects, determines a path between the startNode and the endNode
        // </summary>
        // <param name="startNode"> the start Node </param>
        // <param name="endNode"> the end Node </param>
        public void determinePath(GenerateGraph.Node startNode, GenerateGraph.Node endNode)
        {
            if (startNode == null || endNode == null)
                return;
            PriorityQueue<GenerateGraph.Node> pq = new PriorityQueue<GenerateGraph.Node>(map.nodes.Count);
            pq.queue(startNode);
            Dictionary<GenerateGraph.Node, GenerateGraph.Node> came_from = new Dictionary<GenerateGraph.Node, GenerateGraph.Node>();
            Dictionary<GenerateGraph.Node, float> cost_so_far = new Dictionary<GenerateGraph.Node, float> ();
            came_from.Add(startNode, null);
            cost_so_far.Add (startNode, 0);

            Dictionary<GenerateGraph.Node, int> nodeToId = new Dictionary<GenerateGraph.Node, int>();
            for (int i = 0; i < map.nodes.Count; i++) {
                nodeToId.Add (map.nodes[i], i);
            }
            GenerateGraph.Node current;

            while (pq.getSize() > 0) {
                current = pq.dequeue();
                print(current.point.ToString());
                print("Neighbor Count: " + current.neighbors.Count);
                if (current.Equals(endNode)) {
                    print ("True");
                    break;
                }

                for (int i = 0; i < current.neighbors.Count; i++) {
                    print("Current Neighbor: " + current.neighbors[i].point.ToString());
                    float new_cost = cost_so_far[current] +
                        distanceBetweenNodes(current, current.neighbors[i]);
                    if (cost_so_far.ContainsKey(current.neighbors[i]) == false ||
                        new_cost < cost_so_far[current.neighbors[i]]) {
                        cost_so_far[current.neighbors[i]] = new_cost;
                        current.neighbors[i].priority = new_cost;
                        pq.queue(current.neighbors[i]);
                        came_from[current.neighbors[i]] = current;
                    }
                }
            }

            //Put nodes of the path into the list
            path = new List<GenerateGraph.Node> ();
            GenerateGraph.Node currentNode = endNode;
            path.Add (currentNode);
            while (currentNode.Equals(startNode) == false) {
                currentNode = came_from[currentNode];
                path.Add (currentNode);
            }

            path.Reverse();
            print ("Path Nodes: ");
            for (int i = 0; i < path.Count; i++) {
                print(nodeToId[path[i]] + "\n");
            }
        }

    // <summary>
    // Performs an A* traversal from the start node to the end node; however, the end node
    // only provides a direction for the path finding, as once the current node in the
    // traversal is pathLength away from the start node, the traversal is complete
    // </summary>
    protected override void ThreadFunction()
    {
        PriorityQueue<Node> open = new PriorityQueue<Node> (PathPlanningDataStructures.graph.Size ());
        HashSet<Node> closed = new HashSet<Node> ();
        Dictionary<Node, Node> came_from = new Dictionary<Node, Node> ();
        Dictionary<Node, float> cost_so_far = new Dictionary<Node, float> ();
        came_from.Add (startNode, null);
        cost_so_far.Add (startNode, 0);
        Debug.Log ("the start node: " + startNode);
        open.queue (PathPlanningDataStructures.heuristic.Estimate (startNode), startNode);
        while (open.getSize() > 0) {
            Node current = open.dequeue ();

            if (current.Equals (PathPlanningDataStructures.graph.endNode) ||
                Node.distanceBetweenNodes (startNode, current) >= pathLength) {
                if (came_from[current] == null) {
                    came_from[current] = startNode;
                }
                destinationNode = current;
                break;
            }

            foreach (Node n in current.neighbors) {
                float graph_cost = cost_so_far [current] + Node.distanceBetweenNodes (current, n);
                if (cost_so_far.ContainsKey (n) == false || graph_cost < cost_so_far [n]) {
                    cost_so_far [n] = graph_cost;
                    float priority = graph_cost + PathPlanningDataStructures.heuristic.Estimate (n);
                    open.queue (priority, n);
                    came_from [n] = current;
                }
            }
        }

        //Put nodes of the path into the list
        pathWayPoints = new List<Vector3> ();
        Node currentNode = destinationNode;
        pathWayPoints.Add (currentNode.position);
        while (currentNode.Equals(startNode) == false) {
            currentNode = came_from [currentNode];
            pathWayPoints.Add (currentNode.position);
            Debug.Log (currentNode.ToString());
        }
        pathWayPoints.Reverse ();
    }

    public static List<Node> GetPath(Vector3 start)
    {
        Node startNode = graph.getClosestNode(start);
        PriorityQueue<Node> open = new PriorityQueue<Node>(graph.Size());
        HashSet<Node> closed = new HashSet<Node>();
        Dictionary<Node, Node> came_from = new Dictionary<Node, Node>();
        Dictionary<Node, float> cost_so_far = new Dictionary<Node, float>();
        came_from.Add(startNode, null);
        cost_so_far.Add(startNode, 0);
        open.queue(heuristic.Estimate(startNode), startNode);

        while (open.getSize() > 0) {
            Node current = open.dequeue();

            if (current.Equals(graph.endNode)) {
                break;
            }

            foreach (Node n in current.neighbors) {

                float graph_cost = cost_so_far[current] + Node.distanceBetweenNodes(current, n);

                if (cost_so_far.ContainsKey(n) == false ||  graph_cost < cost_so_far[n]) {
                    cost_so_far[n] = graph_cost;
                    float priority = graph_cost + heuristic.Estimate(n);
                    open.queue(priority, n);
                    came_from[n] = current;
                }
            }
        }

        //Put nodes of the path into the list
        List<Node> path = new List<Node> ();
        Node currentNode = graph.endNode;
        path.Add (currentNode);
        while (currentNode.Equals(startNode) == false) {
            currentNode = came_from[currentNode];
            path.Add (currentNode);
        }
        path.Reverse();
        return path;
    }

    public HeuristicD(GenerateGraph graph)
    {
        heuristicCost = new Dictionary<Node, float>();

        PriorityQueue<Node> pq = new PriorityQueue<Node>(graph.Size ());
        Dictionary<Node, float> cost_so_far = new Dictionary<Node, float> ();
        pq.queue(0.0f, graph.endNode);
        cost_so_far.Add (graph.endNode, 0.0f);
        while (pq.getSize() > 0) {
            Node current = pq.dequeue();
            heuristicCost[current] = cost_so_far[current];
            for (int i = 0; i < current.neighbors.Count; i++) {
                float new_cost = cost_so_far[current] + Node.distanceBetweenNodes(current, current.neighbors[i]);
                if (!cost_so_far.ContainsKey(current.neighbors[i]) || new_cost < cost_so_far[current.neighbors[i]]) {
                    cost_so_far[current.neighbors[i]] = new_cost;
                    pq.queue(new_cost, current.neighbors[i]);
                }
            }
        }
    }