C# PriorityQueue.Pop方法代码示例

        public static void Main()
        {
            PriorityQueue<int> queue = new PriorityQueue<int>();

            queue.Push(1);
            queue.Push(2);
            queue.Push(12);
            queue.Push(34);
            queue.Push(6);
            queue.Push(9);

            Console.WriteLine(queue.Pop());
            Console.WriteLine(queue.Pop());
        }

        public void PriorityQueueTest1()
        {
            PriorityQueue<string> a = new PriorityQueue<string>();

            a.Push("World", 7);
            a.Push("Hello", 2);
            Assert.AreEqual("Hello", a.Pop());
            Assert.AreEqual("World", a.Pop());

            a.Push("Hello", 2);
            a.Push("World", 7);
            Assert.AreEqual("Hello", a.Pop());
            Assert.AreEqual("World", a.Pop());
        }

        public static SearchResult Search(AbstractNode initialNode, IKnowledgeBase kb)
        {
            var frontier = new PriorityQueue<AbstractNode>();
            var explored = new List<AbstractState>();
            var statesSearched = 0; //Bliver kun brugt af os af ren interesse
            var end = initialNode.Target;
            frontier.Add(initialNode);
            explored.Add(initialNode.State);

            while (frontier.Count > 0)
            {
                // Chooses the lowest-cost node in the frontier
                var currentNode = frontier.Pop();
                statesSearched++;
                if (currentNode.State.Equals(end))
                    return new SearchResult(currentNode, statesSearched, true);

                var actions = kb.ActionsForNode(currentNode);
            //Explore /expand the current node
                foreach (var action in actions)
                {
                    var child = kb.Resolve(currentNode, action, end);
                    //System.Console.WriteLine("Frontier.Count: " + frontier.Count);

                    if (!explored.Contains(child.State) && !frontier.Contains(child))
                    {
                        explored.Add(child.State);
                        frontier.Add(child);

                    }
                    else if(true)
                    {
                        for (int i = 0; i < frontier.Count; i++)
                        {
                            var frontierNode = frontier[i];
                            if (frontierNode.State.Equals(child.State) && frontierNode.PathCost > child.PathCost)
                            {
                                frontier[i] = child;
                                break;
                            }
                        }
                    }
                }
            }

            return new SearchResult(null, statesSearched, false);
        }

        static int FindPath(char[,] map, int row, int col, Point start, Point end)
        {
            int[,] gscore = new int[row, col];
            int[,] fscore = new int[row, col];
            Color[,] status = new Color[row, col];

            PriorityQueue<Point> queue = new PriorityQueue<Point>();
            queue.Comparer = Compare;

            gscore[start.Y, start.X] = 0;
            start.F = fscore[start.Y, start.X] = Heuristic(start, end);
            queue.Push(start);
            status[start.Y, start.X] = Color.Grey;

            while (!queue.IsEmpty())
            {
                Point cur = queue.Pop();

                if (cur.X == end.X && cur.Y == end.Y)
                    return fscore[cur.Y, cur.X];

                foreach (var n in GetNeighbors(cur, map, row, col))
                {
                    if (status[n.Y, n.X] == Color.Black)
                        continue;

                    int tentative = gscore[cur.Y, cur.X] + 1;
                    bool useTentative = true;

                    if (status[n.Y, n.X] == Color.Grey && gscore[n.Y, n.X] <= tentative)
                        useTentative = false;

                    if (useTentative)
                    {
                        gscore[n.Y, n.X] = tentative;
                        n.F = fscore[n.Y, n.X] = tentative + Heuristic(n, end);
                        queue.Push(n);
                        n.P = cur;
                    }
                }
            }

            return int.MinValue;
        }

        public void PriorityQueueSize()
        {
            PriorityQueue<int> p = new PriorityQueue<int>();
            Assert.AreEqual(0, p.Size);

            p.Push(5, 4);
            Assert.AreEqual(1, p.Size);

            p.Pop();
            Assert.AreEqual(0, p.Size);

            p = new PriorityQueue<int>(new[] { Tuple.Create(1, 1), Tuple.Create(1, 7) });
            Assert.AreEqual(2, p.Size);

            p.Push(9, 12);
            Assert.AreEqual(3, p.Size);

            p.Pop();
            Assert.AreEqual(2, p.Size);
        }

        public static int Search(List<State> states, List<Action> actions, State start, Tile target)
        {
            var found = 0;

            PriorityQueue<BFNode> frontier = new PriorityQueue<BFNode>();
            List<State> explored = new List<State>();

            frontier.Add(new BFNode(start));

            while (frontier.Count > 0)
            {
            // Chooses the lowest-cost node in the frontier
            BFNode currentBFNode = frontier.Pop();

            // Win condition
            if (currentBFNode.State.Type.Equals(target))
            found++;

            explored.Add(currentBFNode.State);

            // Filter actions to the ones connected to the current node
            foreach (Action action in actions.Where(a => a.StateA.Equals(currentBFNode.State)))
            {
            // One of A or B will be the currentBFNode's action
            // but it won't be added to the frontier since it
            // is already in explored
            var childA = new BFNode(currentBFNode, action, action.StateA);
            var childB = new BFNode(currentBFNode, action, action.StateB);

            if (!explored.Contains(childA.State) && !frontier.Any(n => n.State == childA.State))
            frontier.Add(childA);

            if (!explored.Contains(childB.State) && !frontier.Any(n => n.State == childB.State))
            frontier.Add(childB);
            }
            }
            return found;
        }

        public void PriorityQueueTest2()
        {
            PriorityQueue<string> a = new PriorityQueue<string>(new[] { Tuple.Create("Hello", 4), Tuple.Create("World", 9), Tuple.Create("Test", 1) });
            a.Push("Test2", 2);

            Assert.AreEqual("Test", a.Pop());
            int p2;
            Assert.AreEqual("Test2", a.Pop(out p2));
            Assert.AreEqual(2, p2);

            a.Push("First", 0);
            a.Push("Second", 5);
            Assert.AreEqual("First", a.Pop());
            Assert.AreEqual("Hello", a.Pop());
            Assert.AreEqual("Second", a.Pop());
            Assert.AreEqual("World", a.Pop());
            a.Pop();
        }

        public static INode Search(List<State> states, List<Action> actions, State start, State end)
        {
            PriorityQueue<Node> frontier = new PriorityQueue<Node>();
            List<State> explored = new List<State>();

            frontier.Add(new Node(start, end));

            while (frontier.Count > 0)
            {
            // Chooses the lowest-cost node in the frontier
            Node currentNode = frontier.Pop();

            // Win condition
            if (currentNode.State.Equals(end))
            return currentNode;

            // Add currentNode to list of explored
            explored.Add(currentNode.State);

            // Filter actions to the ones connected to the current node
            foreach (Action action in actions.Where(a => a.StateA.Equals(currentNode.State) || a.StateB.Equals(currentNode.State)))
            {
            // One of A or B will be the currentNode's action
            // but it won't be added to the frontier since it
            // is already in explored
            var childA = new Node(currentNode, action, action.StateA, end);
            if (!explored.Contains(childA.State))
            frontier.Add(childA);

            var childB = new Node(currentNode, action, action.StateB, end);
            if (!explored.Contains(childB.State))
            frontier.Add(childB);
            }
            }

            return null;
        }

        public static Cell DijkstraNextCell(int startI, int startJ, int endI, int endJ, Cell[] forbidden)
        {
            if (_distMap == null)
            {
                _distMap = new double[world.Height, world.Width];
                _distPrev = new Cell[world.Height, world.Width];
            }

            var q = new PriorityQueue<Pair<double, Cell>>();
            q.Push(new Pair<double, Cell>(0.0, new Cell(endI, endJ)));
            for (var i = 0; i < world.Height; i++)
                for (var j = 0; j < world.Width; j++)
                    _distMap[i, j] = Infinity;
            _distMap[endI, endJ] = 0;
            while (q.Count > 0)
            {
                var cur = q.Top().Second;
                var minDist = -q.Top().First;
                q.Pop();

                if (minDist > _distMap[cur.I, cur.J])
                    continue;

                EnumerateNeigbours(cur, to =>
                {
                    if (!CanPass(cur.I, cur.J, to.I, to.J) || forbidden.Any(x => x.Equals(to.I, to.J)))
                        return;

                    var distTo = _distMap[cur.I, cur.J] + GetCost(to);
                    if (distTo < _distMap[to.I, to.J])
                    {
                        _distMap[to.I, to.J] = distTo;
                        _distPrev[to.I, to.J] = cur;
                        q.Push(new Pair<double, Cell>(-distTo, to));
                    }
                });
            }

            if (_distPrev[startI, startJ] == null)
            {
                if (forbidden.Length == 0)
                    throw new Exception("path not found");
                return DijkstraNextCell(startI, startJ, endI, endJ, new Cell[] {});
            }

            return _distPrev[startI, startJ];
        }

        public static String Solve( Puzzle map )
        {
            Puzzle initialNode = map;

            Puzzle end = null;
            var moves = Enum.GetValues(typeof(Puzzle.Position));

            PriorityQueue<Puzzle> nodes = new PriorityQueue<Puzzle>();

            nodes.Push(initialNode);

            while (nodes.Count != 0)
            {
                Puzzle currentNode = nodes.Top;
                nodes.Pop();

                if (currentNode.MD == 0)
                {
                    end = currentNode;
                    break;
                }

                if (currentNode.Blank != -1)
                {
                    foreach (var move in moves)
                    {
                        if (currentNode.NowStatus.answer.Count != 0)
                        {
                            Puzzle.Position now = currentNode.NowStatus.answer[currentNode.NowStatus.answer.Count - 1];
                            if (currentNode.ReversePosition(now) == (Puzzle.Position)move)
                                continue;
                        }
                        Puzzle newPuzzle = (Puzzle)currentNode.Clone();
                        if (newPuzzle.DoMove((Puzzle.Position)move))
                        {
                            newPuzzle.CalculateMD();
                            nodes.Push(newPuzzle);
                        }
                    }
                }

                if (currentNode.maxChoice != 0 && ( currentNode.NowStatus.answer.Count != 0 || currentNode.Blank == -1 ))
                {
                    for (int i = 0; i < currentNode.Data.Count(); i++)
                    {
                        if (i == currentNode.Blank)
                            continue;

                        Puzzle newPuzzle = (Puzzle)currentNode.Clone();
                        newPuzzle.Choice(i);
                        newPuzzle.CalculateMD();
                        nodes.Push(newPuzzle);
                    }
                }
            }

            if (end != null){
                return end.GetSolution();
            }
            else
                return "";
        }

 public static void UpdateDijkstraMap(PosArray<int> map,IntLocationDelegate get_cost)
 {
     PriorityQueue<cell> frontier = new PriorityQueue<cell>(c => -c.value);
     int height = map.objs.GetLength(0);
     int width = map.objs.GetLength(1);
     for(int i=0;i<height;++i){
         for(int j=0;j<width;++j){
             if(map[i,j] != U.DijkstraMin){
                 int v = map[i,j];
                 bool good = true;
                 for(int s=-1;s<=1 && good;++s){
                     for(int t=-1;t<=1 && good;++t){
                         if(i+s >= 0 && i+s < height && j+t >= 0 && j+t < width){
                             if(map[i+s,j+t] < v && map[i+s,j+t] != U.DijkstraMin){
                                 good = false;
                             }
                         }
                     }
                 }
                 if(good){ //find local minima and add them to the frontier
                     frontier.Add(new cell(i,j,v));
                 }
             }
         }
     }
     while(frontier.list.Count > 0){
         cell c = frontier.Pop();
         for(int s=-1;s<=1;++s){
             for(int t=-1;t<=1;++t){
                 if(c.row+s >= 0 && c.row+s < height && c.col+t >= 0 && c.col+t < width){
                     int cost = get_cost(c.row+s,c.col+t);
                     if(map[c.row+s,c.col+t] > c.value+cost){
                         map[c.row+s,c.col+t] = c.value+cost;
                         frontier.Add(new cell(c.row+s,c.col+t,c.value+cost));
                     }
                 }
             }
         }
     }
 }

        public void AstarRun()
        {
            Console.WriteLine("A* starts!");
            NavigateNode start = new NavigateNode(2, 2, NavigateNode.StateEnum.NAVIGABLE);
            NavigateNode end = new NavigateNode(goalX, goalY, NavigateNode.StateEnum.NAVIGABLE);

            PriorityQueue<NavigateNode> openSet = new PriorityQueue<NavigateNode>();
            PriorityQueue<NavigateNode> closeSet = new PriorityQueue<NavigateNode>();

            openSet.Add(start);

            while (!openSet.Empty) {
                // get from open set
                NavigateNode current = openSet.Pop();

                // add to close set
                closeSet.Add(current);

                // goal found
                if (current.IsSameLocation(end)) {
                    while (current.Parent != null) {
                        mMap[current.X, current.Y].State = NavigateNode.StateEnum.PATH;
                        current = current.Parent;
                    }
                    return;
                }
                else {
                    List<NavigateNode> neighbors = GetNeighbors(current);

                    foreach (NavigateNode n in neighbors) {
                        if (closeSet.IsMember(n)) {
                            continue;
                        }
                        else {
                            if (!openSet.IsMember(n)) {
                                n.Parent = current;
                                n.DirectCost = current.DirectCost + GetDirectCost(current, n);
                                n.HeuristicCost = GetHeuristicCost(n, end);
                                n.TotalCost = n.DirectCost + n.HeuristicCost;

                                // add to open set
                                openSet.Add(n);
                            }
                            else {
                                double costFromThisPathToM = current.DirectCost + GetDirectCost(current, n);
                                // we found a better path
                                if (costFromThisPathToM < n.DirectCost) {
                                    n.Parent = current;                                   // change parent to n
                                    n.DirectCost = costFromThisPathToM;             // recalculate direct cost
                                    n.TotalCost = n.HeuristicCost + n.DirectCost;   // recalculate total cost
                                }
                            }
                        }
                    }
                }
            }

            Console.WriteLine("end here?");
        }

        bool searchPath(Dictionary<string, string> pathMap, Node result)
        {
            PriorityQueue<Node> priorityQueue;
            Stack<Node> pathStack;
            priorityQueue = new PriorityQueue<Node>();
            pathStack = new Stack<Node>();

            priorityQueue.Push(this.begainNode);
            pathStack.Push(this.begainNode);

            int cycle = 0;

            while (!priorityQueue.Empty())
            {
                cycle++;
               //     Console.WriteLine("第 "+cycle.ToString()+" 步");
            //    Console.WriteLine("队列中的元素  " + priorityQueue.Count);
                Node topNode = priorityQueue.Top();

                priorityQueue.Pop();

                #region 判断是否找到目状态
                if (matched(topNode, this.targetNode))
                {
                    printState(targetNode);
                    Console.WriteLine("搜索完成");
                    printState(topNode);
                    result = topNode;
                    return true;
                }
                #endregion

                int row = topNode.row_0;
                int col = topNode.col_0;

                if (row > 0 && topNode.cannotAct != Direction.up)
                {
                    Node curNode = new Node(topNode);

               //     Console.WriteLine("当前状态");
              //      printState(topNode);
               //     Console.WriteLine(row.ToString()+" "+col.ToString()+"   空格上移后状态");

                    exchange(curNode, row, col, row - 1, col);
                    curNode.ToString();
                    curNode.cannotAct = Direction.down;
                    if (!pathMap.ContainsKey(curNode.state))
                    {
                //        printState(curNode);

                        curNode.deepth = topNode.deepth + 1;
                        curNode.value = getValue(curNode, this.targetNode);

                //        Console.WriteLine("当前代价值："+(curNode.value + curNode.deepth).ToString());

                        curNode.father = topNode;
                        curNode.row_0 = row - 1;
                        curNode.col_0 = col;
                        priorityQueue.Push(curNode);
                        pathStack.Push(curNode);
                        pathMap.Add(curNode.state, topNode.state);
                    }
                }

                if (row < 2 && topNode.cannotAct != Direction.down)
                {
                    Node curNode = new Node(topNode);

                //    Console.WriteLine("当前状态");
                //    printState(topNode);
                //    Console.WriteLine(row.ToString()+" "+col.ToString()+"    下移后状态");

                    exchange(curNode, row, col, row + 1, col);
                    curNode.ToString();
                    curNode.cannotAct = Direction.up;
                    if (!pathMap.ContainsKey(curNode.state))
                    {
                //        printState(curNode);

                        curNode.deepth = topNode.deepth + 1;
                        curNode.value = getValue(curNode, this.targetNode);

                 //       Console.WriteLine("当前代价值："+(curNode.value + curNode.deepth).ToString());

                        curNode.father = topNode;
                        curNode.row_0 = row + 1;
                        curNode.col_0 = col;
                        priorityQueue.Push(curNode);
                        pathStack.Push(curNode);
                        pathMap.Add(curNode.state, topNode.state);
                    }
                }

                if (col > 0 && topNode.cannotAct != Direction.left)
                {
                    Node curNode = new Node(topNode);

                 //   Console.WriteLine("当前状态");
                 //   printState(topNode);
                //    Console.WriteLine(row.ToString()+" "+col.ToString()+"    左移之后的状态");
//.........这里部分代码省略.........

        static void Main(string[] args)
        {
            search begainSearch = new search();

            //测试优先队列功能
            PriorityQueue<int> que;
            que = new PriorityQueue<int>();
            que.Push(12);
            que.Push(132);
            que.Push(123);
            que.Push(212);
            que.Push(322);
            que.Push(126);
            que.Push(13);
            que.Push(189);
            while (!que.Empty())
            {
                Console.Write(que.Pop().ToString()+" ");
            }
        }

        public List<Waypoint> FindPath(Waypoint start, Waypoint goal, OccupancyGrid2D og, out bool success)
        {
            List<Waypoint> path = new List<Waypoint>();

            //added by aaron (sort of a hack)
            if (og == null || goal.Coordinate.DistanceTo(start.Coordinate) == 0)
            {
                path.Add(new Waypoint(start.Coordinate, true, 0));
                success = true;
                return path;
            }

            int xIdx, yIdx;
            success = true;
            Vector2[] NESWVector = new Vector2[4];
            Vector2[] diagVector = new Vector2[4];
            bool[] NESW = new bool[4];
            Vector2 startV = start.Coordinate; // Start Vector2
            Vector2 goalV = goal.Coordinate; // Goal Vector2

            PriorityQueue open = new PriorityQueue();
            closed = new OccupancyGrid2D(resX, resY, extentX, extentY);
            opened = new OccupancyGrid2D(resX, resY, extentX, extentY);

            GetIndicies(startV.X, startV.Y, out xIdx, out yIdx);
            startV = new Vector2(xIdx, yIdx);
            GetIndicies(goalV.X, goalV.Y, out xIdx, out yIdx);
            goalV = new Vector2(xIdx, yIdx);

            Node root = new Node(goalV, goalV.DistanceTo(startV), 0, null);

            Node current = root;
            open.Push(current);

            // Do the spreading/discovering stuff until we discover a path.
            while (current.xy != startV)
            {
                if (open.q.Count == 0 || open.q.Count > MAX_OPEN)
                {
                    Console.WriteLine("Failure in DSstar. Open count is: " + open.q.Count);
                    success = false;
                    break;
                }
                current = open.Pop();

                NESWVector[0] = new Vector2(current.xy.X, current.xy.Y - 1);
                NESWVector[1] = new Vector2(current.xy.X + 1, current.xy.Y);
                NESWVector[2] = new Vector2(current.xy.X, current.xy.Y + 1);
                NESWVector[3] = new Vector2(current.xy.X - 1, current.xy.Y);

                diagVector[0] = new Vector2(current.xy.X + 1, current.xy.Y - 1);
                diagVector[1] = new Vector2(current.xy.X + 1, current.xy.Y + 1);
                diagVector[2] = new Vector2(current.xy.X - 1, current.xy.Y + 1);
                diagVector[3] = new Vector2(current.xy.X - 1, current.xy.Y - 1);

                for (int i = 0; i < 4; i++)
                {
                    if ((int)og.GetCellByIdx((int)NESWVector[i].X, (int)NESWVector[i].Y) < 255)
                    {
                        if (closed.GetCellByIdx((int)NESWVector[i].X, (int)NESWVector[i].Y) == 0)
                        {
                            NESW[i] = true;
                            if (opened.GetCellByIdx((int)NESWVector[i].X, (int)NESWVector[i].Y) == 0)
                            {
                                open.Push(new Node(NESWVector[i], NESWVector[i].DistanceTo(startV), current.h + 1
                                    + og.GetCellByIdx((int)NESWVector[i].X, (int)NESWVector[i].Y) / blurWeight, current));
                                opened.SetCellByIdx((int)NESWVector[i].X, (int)NESWVector[i].Y, 1);
                            }
                        }
                    }
                }

                for (int i = 0; i < 4; i++)
                {
                    if (NESW[i % 4] && NESW[(i + 1) % 4])
                    {
                        if (og.GetCellByIdx((int)diagVector[i].X, (int)diagVector[i].Y) < 255)
                        {
                            if (closed.GetCellByIdx((int)diagVector[i].X, (int)diagVector[i].Y) == 0)
                            {
                                if (opened.GetCellByIdx((int)diagVector[i].X, (int)diagVector[i].Y) == 0)
                                {
                                    open.Push(new Node(diagVector[i], diagVector[i].DistanceTo(startV), current.h + 1.4
                                        + og.GetCellByIdx((int)diagVector[i].X, (int)diagVector[i].Y) / blurWeight, current));
                                    opened.SetCellByIdx((int)diagVector[i].X, (int)diagVector[i].Y, 1);
                                }
                            }
                        }
                    }
                }

                for (int i = 0; i < 4; i++) NESW[i] = false;

                closed.SetCellByIdx((int) current.xy.X, (int) current.xy.Y, 1);
            }

            // Build a path using the discovered path.
            double x, y;
            Waypoint waypoint;

//.........这里部分代码省略.........