C# PriorityQueue.Remove方法代码示例

        public void PriorityQueueRemoveTest(int length, int removeIndex)
        {
            var queue = new PriorityQueue<Tuple<int>>();
            for (int i = length - 1; i >= 0; i--)
            {
                queue.Enqueue(Tuple.Create(i));
            }

            if (removeIndex == -1)
            {
                queue.Remove(Tuple.Create(length));
                Assert.Equal(length, queue.Count);
            }
            else
            {
                queue.Remove(Tuple.Create(removeIndex));
                Assert.Equal(length - 1, queue.Count);
            }
        }

        public void Simple()
        {
            var priorityQueue = new PriorityQueue<string, int>(PriorityQueueType.Minimum) { "5" };

            Assert.AreEqual(priorityQueue.Count, 1);
            Assert.IsTrue(priorityQueue.Contains("5"));
            Assert.IsTrue(priorityQueue.Remove("5"));
            Assert.AreEqual(priorityQueue.Count, 0);

            priorityQueue.Add("6");
            priorityQueue.Add("7");
            priorityQueue.Add("2", 4);

            Assert.AreEqual(priorityQueue.Count, 3);
            Assert.IsTrue(priorityQueue.Contains("6"));
            Assert.IsTrue(priorityQueue.Contains("7"));
            Assert.IsTrue(priorityQueue.Contains("2"));

            Assert.IsFalse(priorityQueue.Remove("8"));

            Assert.IsTrue(priorityQueue.Remove("7"));
            Assert.AreEqual(priorityQueue.Count, 2);
            Assert.IsTrue(priorityQueue.Remove("2"));
            Assert.AreEqual(priorityQueue.Count, 1);
            Assert.IsTrue(priorityQueue.Remove("6"));
            Assert.AreEqual(priorityQueue.Count, 0);

            Assert.IsFalse(priorityQueue.Remove("7"));
        }

        public void WithPriority()
        {
            var priorityQueue = new PriorityQueue<string, int>(PriorityQueueType.Minimum);

            int priority;

            priorityQueue.Add("5", 3);
            Assert.AreEqual(priorityQueue.Count, 1);
            Assert.IsTrue(priorityQueue.Contains("5"));
            Assert.IsTrue(priorityQueue.Remove("5", out priority));
            Assert.AreEqual(priority, 3);

            Assert.AreEqual(priorityQueue.Count, 0);

            priorityQueue.Add("6");
            priorityQueue.Add("7");
            priorityQueue.Add("2", 4);

            Assert.AreEqual(priorityQueue.Count, 3);
            Assert.IsTrue(priorityQueue.Contains("6"));
            Assert.IsTrue(priorityQueue.Contains("7"));
            Assert.IsTrue(priorityQueue.Contains("2"));

            Assert.IsFalse(priorityQueue.Remove("8", out priority));

            Assert.IsTrue(priorityQueue.Remove("7", out priority));
            Assert.AreEqual(priority, 0);
            Assert.AreEqual(priorityQueue.Count, 2);

            Assert.IsTrue(priorityQueue.Remove("2", out priority));
            Assert.AreEqual(priority, 4);

            Assert.AreEqual(priorityQueue.Count, 1);
            Assert.IsTrue(priorityQueue.Remove("6", out priority));
            Assert.AreEqual(priority, 0);
            Assert.AreEqual(priorityQueue.Count, 0);

            Assert.IsFalse(priorityQueue.Remove("7", out priority));
        }

 public static ArrayList FindPath(Node start, Node goal)
 {
     openList = new PriorityQueue();
     openList.Push(start);
     start.nodeTotalCost = 0.0f;
     start.estimatedCost = HeuristicEstimateCost(start, goal);
     closedList = new PriorityQueue();
     Node node = null;
     while (openList.Length != 0) {
         node = openList.First();
         //Check if the current node is the goal node
         if (node.position == goal.position) {
             return CalculatePath(node);
         }
         //Create an ArrayList to store the neighboring nodes
         ArrayList neighbours = new ArrayList();
         GridManager.instance.GetNeighbours(node, neighbours);
         for (int i = 0; i < neighbours.Count; i++) {
             Node neighbourNode = (Node)neighbours[i];
             if (!closedList.Contains(neighbourNode)) {
                 float cost = HeuristicEstimateCost(node,
                         neighbourNode);
                 float totalCost = node.nodeTotalCost + cost;
                 float neighbourNodeEstCost = HeuristicEstimateCost(
                         neighbourNode, goal);
                 neighbourNode.nodeTotalCost = totalCost;
                 neighbourNode.parent = node;
                 neighbourNode.estimatedCost = totalCost +
                         neighbourNodeEstCost;
                 if (!openList.Contains(neighbourNode)) {
                     openList.Push(neighbourNode);
                 }
             }
         }
         //Push the current node to the closed list
         closedList.Push(node);
         //and remove it from openList
         openList.Remove(node);
     }
     if (node.position != goal.position) {
         Debug.LogError("Goal Not Found");
         return null;
     }
     return CalculatePath(node);
 }

    public static List<Node> findPath(Node start, Node goal)
    {
        openList = new PriorityQueue();
        openList.Push(start);
        start.nodeTotalCost = 0.0f;
        start.estimatedCost = heuristicEstimateCost(start,goal);

        closedList = new PriorityQueue();
        Node node = null;

        while (openList.Length != 0) {
            node = openList.First();

            if(node.position == goal.position){
                return calculatePath(node);
            }

            List<Node> neighbours = new List<Node>();
            gridManager.getNeighbours(node,neighbours);
            for(int i=0;i<neighbours.Count;i++){
                Node neighbour = neighbours[i];

                if(!closedList.Contains(neighbour)){
                    float cost = heuristicEstimateCost(node,neighbour);
                    float totalCost = node.nodeTotalCost + cost;
                    float neighbourEstCost = heuristicEstimateCost(neighbour,goal);
                    neighbour.nodeTotalCost = totalCost;
                    neighbour.parent = node;
                    neighbour.estimatedCost = totalCost + neighbourEstCost;
                    if(!openList.Contains(neighbour)){
                        openList.Push(neighbour);
                    }
                }

            }
            closedList.Push(node);
            openList.Remove(node);
        }
        if (node.position != goal.position) {
            Debug.LogError("Goal Not Found");
            return null;
        }
        return calculatePath(node);
    }

    static void Main()
    {
        var priQueue = new PriorityQueue<int>();

        priQueue.Add(-5);
        priQueue.Add(7);
        priQueue.Add(9);
        priQueue.Add(5);
        priQueue.Add(1);
        priQueue.Add(8);

        priQueue.Remove();

        foreach (var el in priQueue)
        {
            Console.WriteLine(el);
        }
        Console.WriteLine("Element count: {0}, Capacity: {1}", priQueue.Count, priQueue.Capacity);

        priQueue.Clear();
        Console.WriteLine("Element count: {0}, Capacity: {1}", priQueue.Count, priQueue.Capacity);
    }

        public void TestRemoval()
        {
            var priorityQueue = new PriorityQueue<int>();
            for (int i = 0; i < 1000; ++i)
            {
                priorityQueue.Enqueue(i);
            }

            priorityQueue.Remove(3);
            priorityQueue.Remove(0);
            priorityQueue.Remove(500);
            priorityQueue.Remove(251);
            priorityQueue.Remove(999);

            priorityQueue.Remove(1002);

            Assert.That(priorityQueue.Count, Is.EqualTo(1000 - 5));

            CheckPriorityQueue(priorityQueue);
        }

    //A* implementation
    public static IEnumerator CalculatePath(PathNode start, PathNode end, List<PathNode> allNodes, System.Action<List<Vector2>> callback)
    {

        int n = 0;

        PriorityQueue openList = new PriorityQueue();
        PriorityQueue closedList = new PriorityQueue();

        openList.Push(start);
        start.cost = 0;
        start.estimatedCost = HeuristicEstimate(start, end, heuristicWeight);

        PathNode currentNode = null;

        while (openList.Count != 0)
        {

            currentNode = openList.Front();
            if (currentNode == end)
                break;

            List<int> links = currentNode.links;

            for (int i = 0; i != links.Count; i++)
            {

                PathNode endNode = allNodes[links[i]];

                float incrementalCost = GetCost(currentNode, endNode);
                float endNodeCost = currentNode.cost + incrementalCost;

                if (closedList.Contains(endNode))
                {
                    if (endNode.cost <= endNodeCost)
                        continue;

                    closedList.Remove(endNode);

                }
                else if (openList.Contains(endNode))
                {
                    if (endNode.cost <= endNodeCost)
                        continue;
                }

                float endNodeHeuristic = HeuristicEstimate(endNode, end, heuristicWeight);
                endNode.cost = endNodeCost;
                endNode.parent = currentNode;
                endNode.estimatedCost = endNodeCost + endNodeHeuristic;

                if (!openList.Contains(endNode))
                    openList.Push(endNode);
            }

            closedList.Push(currentNode);
            openList.Remove(currentNode);

            n++;
            if (n > 300)
                yield return null;
        }

        if (!currentNode.Equals(end))
        {

            // Debug.LogWarning("No path found :(");
            callback(new List<Vector2>());
            yield break;

        }
        else
        {

            var path = new List<Vector2>();

            while (currentNode != null)
            {

                path.Add(currentNode.pos);
                currentNode = currentNode.parent;
            }

            path.Reverse();
            callback(path);
            yield break;
        }
    }

        public static bool GetShortestPath(GridNode i_start, GridNode i_end, bool isDiagonal, out List<GridNode> o_path, out List<GridNode> o_open)
        {
            o_path = new List<GridNode>();
            o_open = new List<GridNode>();

            IDictionary<GridNode, GridNode> path = new Dictionary<GridNode, GridNode>();
            IDictionary<GridNode, float> open = new Dictionary<GridNode, float>();

            PriorityQueue<float, GridNode> priorityQueue = new PriorityQueue<float, GridNode>(30 * 30, Comparer<float>.Default);

            priorityQueue.Add(new KeyValuePair<float, GridNode>(GetDistance(i_start, i_end, isDiagonal), i_start));
            open.Add(new KeyValuePair<GridNode, float>(i_start, 0));

            bool succeed = false;
            while (!priorityQueue.IsEmpty)
            {
                KeyValuePair<float, GridNode> current = priorityQueue.Peek();
                GridNode currentNode = current.Value;
                float currentCost;
                if (!open.TryGetValue(currentNode, out currentCost))
                {
                    Debug.Assert(false);
                }

                if (currentNode == i_end)
                {
                    succeed = true;
                    break;
                }

                foreach (GridNode neighbour in currentNode.GetNeighbors(isDiagonal))
                {
                    if (neighbour.Passable)
                    {
                        float newCost = currentCost + GetCost(currentNode, neighbour, isDiagonal);
                        float oldCost;
                        if (!open.TryGetValue(neighbour, out oldCost))
                        {
                            path.Add(neighbour, currentNode);
                            open.Add(neighbour, newCost);
                            float dist = GetDistance(neighbour, i_end, isDiagonal);
                            priorityQueue.Add(new KeyValuePair<float, GridNode>(newCost + dist, neighbour));
                        }
                        else if (newCost < oldCost)
                        {
                            path[neighbour] = currentNode;
                            open[neighbour] = newCost;
                            float dist = GetDistance(neighbour, i_end, isDiagonal);
                            priorityQueue.Remove(new KeyValuePair<float, GridNode>(oldCost + dist, neighbour));
                            priorityQueue.Add(new KeyValuePair<float, GridNode>(newCost + dist, neighbour));
                        }
                    }
                }

                priorityQueue.Remove(current);
            }

            if (succeed)
            {

                GridNode currentNode = i_end;
                o_path.Add(currentNode);
                while (path.ContainsKey(currentNode))
                {
                    currentNode = path[currentNode];
                    o_path.Add(currentNode);
                }
                o_path.Reverse();

            }

            o_open = open.Keys.ToList();

            return succeed;
        }

    public static Vector3[] FindPath(int[] start, int[] end)
    {
        PathManager mgr = GetInstance ();

        Vector3[] outPath;
        if (mgr.QueryCache (start, end, out outPath)) {
            return outPath;
        }

        int pathSearched = 0;

        GridManager grid = GridManager.GetInstance ();
        if (!grid.HasSurface (start) || !grid.HasSurface (end)) {
            return null;
        }

        List<Node> closed = new List<Node> ();
        PriorityQueue<Node> open = new PriorityQueue<Node> ();
        open.comparator = Node.Comparison;

        Vector3 goal = grid.GridCenter (end);

        Node s = new Node ();
        s.edge = null;
        s.g_score = 0.0f;
        s.h_score = mgr.SqDist (s.p, goal);
        s.f_score = s.h_score;
        s.p = grid.GridCenter (start);

        open.Add (s);

        while (!open.Empty ()) {
            Node node = open.Top;
            open.Remove (open.Top);

            // if it ends, end.
            if (node.p == goal) {
                List<Vector3> path = new List<Vector3> ();
                mgr.ReconstructPath (node, path);
                Vector3[] outpath = path.ToArray ();
                mgr.CachePath (start, end, outpath);
                return outpath;
            }

            closed.Add (node);

            List<Vector3> neighbors = mgr.Neighbors(node.p);
            foreach (Vector3 n in neighbors) {
                if (closed.FindIndex ((a) => a.p == n) >= 0) {
                    continue;
                }
                float temp_g = node.g_score + mgr.SqDist(node.p, n);
                Node f = open.Find ((a) => a.p == n);
                if (f == null) {
                    f = new Node ();
                    f.edge = node;
                    f.g_score = temp_g;
                    f.h_score = mgr.SqDist(n, goal);
                    f.f_score = f.g_score + f.h_score;
                    f.p = n;
                    open.Add (f);
                } else if (temp_g < f.g_score) {
                    f.edge = node;
                    f.g_score = temp_g;
                    f.f_score = f.g_score + f.h_score;
                    open.Update (f);
                }
            }
            if (pathSearched >= mgr.pathSearchLimit) {
                return null;
            } else {
                pathSearched ++;
            }
        }
        return null;
    }

    /// <summary>
    /// Find the path between start node and goal node using AStar Algorithm
    /// </summary>
    public static ArrayList FindPath(Shell start, Shell goal)
    {
        //Start Finding the path
        openList = new PriorityQueue();
        openList.Push(start);
        start.nodeTotalCost = 0.0f;
        start.estimatedCost = HeuristicEstimateCost(start, goal);

        closedList = new PriorityQueue();
        Shell node = null;

        while (openList.Length != 0)
        {
            node = openList.First();

            if (node.position == goal.position)
            {
                return CalculatePath(node);
            }

            ArrayList neighbours = new ArrayList();
            GridManager.instance.GetNeighbours(node, neighbours);

            #region CheckNeighbours

            //Get the Neighbours
            for (int i = 0; i < neighbours.Count; i++)
            {
                //Cost between neighbour nodes
                Shell neighbourNode = (Shell)neighbours[i];

                if (!closedList.Contains(neighbourNode))
                {
                    //Cost from current node to this neighbour node
                    float cost = HeuristicEstimateCost(node, neighbourNode);

                    //Total Cost So Far from start to this neighbour node
                    float totalCost = node.nodeTotalCost + cost;

                    //Estimated cost for neighbour node to the goal
                    float neighbourNodeEstCost = HeuristicEstimateCost(neighbourNode, goal);

                    //Assign neighbour node properties
                    neighbourNode.nodeTotalCost = totalCost;
                    neighbourNode.parent = node;
                    neighbourNode.estimatedCost = totalCost + neighbourNodeEstCost;

                    //Add the neighbour node to the list if not already existed in the list
                    if (!openList.Contains(neighbourNode))
                    {
                        openList.Push(neighbourNode);
                    }
                }
            }

            #endregion

            closedList.Push(node);
            openList.Remove(node);
        }

        //If finished looping and cannot find the goal then return null
        if (node.position != goal.position)
        {
            Debug.LogError("Goal Not Found");
            return null;
        }

        //Calculate the path based on the final node
        return CalculatePath(node);
    }

    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();
    }

        public void Remove()
        {
            var pq = new PriorityQueue<object>();

              for (int i = 0; i < 20; i++)
            pq.Enqueue(i);

              Assert.IsTrue(pq.Remove(0));
              Assert.IsTrue(pq.Remove(14));
              Assert.IsTrue(pq.Remove(19));

              Assert.IsFalse(pq.Remove(-1));
              Assert.IsFalse(pq.Remove(20));

              Assert.AreEqual(17, pq.Count);
        }

        public void Enumerator()
        {
            var pq = new PriorityQueue<int>();

              for (int i = 0; i < 10; i++)
            pq.Enqueue(i);

              var list = new List<int>();
              foreach (var i in pq)
              {
            Assert.IsTrue(!list.Contains(i));
            list.Add(i);
              }

              Assert.AreEqual(pq.Count, list.Count);

              Assert.Throws(typeof(InvalidOperationException), () =>
              {
            foreach (var i in pq)
            {
              pq.Enqueue(1);
            }
              });

              Assert.Throws(typeof(InvalidOperationException), () =>
              {
            foreach (var i in pq)
            {
              pq.Dequeue();
            }
              });

              Assert.Throws(typeof(InvalidOperationException), () =>
              {
            foreach (var i in pq)
            {
              pq.Remove(1);
            }
              });

              Assert.Throws(typeof(InvalidOperationException), () =>
              {
            foreach (var i in pq)
            {
              pq.Clear();
            }
              });
        }

    /*!!! PATHFINDING ALGORITHM IS HIDDEN HERE !!!*/
    public LinkedList<GridSquare> findPath(Vector3 pathFrom,Vector3 pathTo)
    {
        AStarNode[] aStarNodes = new AStarNode[width*height];
        bool[] closedSet = new bool[width*height];
        PriorityQueue<GridSquare> openSet = new PriorityQueue<GridSquare>(false);

        GridSquare target = this.gridSquareFromVector3(pathTo);
        GridSquare currentSquare;

        AStarNode current;

        //Initialize with path beginning
        AStarNode temp = new AStarNode(this.gridSquareFromVector3(pathFrom),target);
        aStarNodes[temp.getSquare().getHash()] = temp;
        openSet.enqueueWithPriority(temp.getSquare(),temp.getFScore());

        //While there's still items in the open set
        while ((currentSquare = openSet.dequeue()) != null) {
            //openSet stores gridsquares for efficiency reasons, so get the relevant A* node
            current = aStarNodes[currentSquare.getHash()];

            //Add node to the closed set
            closedSet[current.getSquare().getHash()] = true;

            //If the current square is the target, we have found a path and
            //can return
            if (current.getSquare () == target) {
                break;
            }

            //For every neighbor
            foreach (GridSquare s in current.getNeighbors())
            {
                //If the square is already processed, skip it
                if (closedSet[s.getHash()] == true) {
                    continue;
                }
                //This is why the open set stores GridSquares instead of AStarNodes
                if (!openSet.Contains(s)) {
                    temp = new AStarNode(s,target);
                    aStarNodes[temp.getSquare().getHash()] = temp;

                    //setParent sets the g score automatically.
                    temp.setParent(current);

                    openSet.enqueueWithPriority(temp.getSquare(),temp.getFScore());
                } else {
                    //if this is a worse path, skip it.
                    temp = aStarNodes[s.getHash ()];
                    if (current.gScore + 1 >= temp.gScore) {
                        continue;
                    }
                    //setParent sets the g score automatically.
                    temp.setParent(current);

                    //Re add to the open set.
                    openSet.Remove (temp.getSquare());
                    openSet.enqueueWithPriority(temp.getSquare(),temp.getFScore());
                }

            }
        }

        //No path was found
        if (currentSquare == null) {
            return default(LinkedList<GridSquare>);
        }

        //Reconstruct the path
        LinkedList<GridSquare> path = new LinkedList<GridSquare>();
        current = aStarNodes[target.getHash()];

        if (current == null) {
            return null;
        }

        do {
            //Add the current square to the beginning of the path
            path.AddFirst(current.getSquare());
            //Set the current node to the parent
            current = current.getParent ();
        } while (current != null);

        return path;
    }