C# PriorityQueue.Push方法代码示例

 public void TestMethodSizeAndPush()
 {
     PriorityQueue<double> testQueue = new PriorityQueue<double>();
     testQueue.Push(1, 2);
     testQueue.Push(2, 2);
     testQueue.Push(1, 3);
     Assert.IsTrue(testQueue.Size() == 3);
 }

 public void TestMethodPushAndTop()
 {
     PriorityQueue<double> testQueue = new PriorityQueue<double>();
     testQueue.Push(1, 2);
     testQueue.Push(2, 2);
     testQueue.Push(1, 3);
     Assert.IsTrue(testQueue.Top() == 2);
 }

        public static void Main()
        {
            var queue = new PriorityQueue<int>(new MyComperer());
            queue.Push(5);
            queue.Push(1);
            queue.Push(2);
            queue.Push(8);

            while (queue.Size > 0)
            {
                Console.WriteLine(queue.Pop());
            }
        }

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

    //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 void AStar(Vector3 start, Vector3 end, OnPathComputed callback)
        {
            // Reset all scores and sets membership
            Reset();

            Node startNode = getClosestNode(start);
            Node endNode = getClosestNode(end);

            if (startNode == null || endNode == null) {
                callback(new Path());
                return;
            }

            PriorityQueue<Node> openSet = new PriorityQueue<Node>();

            openSet.Push(startNode); startNode.InOpenSet = true;

            Dictionary<Node, Node> parentPath = new Dictionary<Node, Node>();

            startNode.GScore = 0;
            startNode.FScore = startNode.GScore + heuristic(startNode, endNode);

            Path path = new Path();
            Node current = startNode;

            while (openSet.Count() > 0) {
                current = openSet.Pop(); // automatically do the remove part
                current.InOpenSet = false;

                if (current.Id == endNode.Id) {
                    path.Add(current.Position);
                    while (parentPath.ContainsKey(current)) {
                        current = parentPath[current];
                        path.Add(current.Position);
                    }

                    path.Reverse();
                    callback(path);
                    return;
                }

                current.InClosedSet = true;

                List<Node> neighbors = getNeighbors(current);

                for (int i = 0; i < neighbors.Count; ++i) {
                    Node neighbor = neighbors[i];
                    if (neighbor.InClosedSet) continue;

                    float gAttempt = current.GScore + euclidian(current, neighbor);

                    if (!neighbor.InOpenSet || gAttempt <= neighbor.GScore) {
                        parentPath[neighbor] = current;
                        neighbor.GScore = gAttempt;
                        neighbor.FScore = neighbor.GScore + heuristic(neighbor, endNode);
                        if (!neighbor.InOpenSet) {
                            openSet.Push(neighbor);
                            neighbor.InOpenSet = true;
                        }
                    }

                } // end loop neighbors

            }
        }

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

        // This isn't really A* at all. But whatevs.
        public List<Coordinate> CustomAStar(Coordinate start, Coordinate end, int iterationLimit = 200)
        {
            Dictionary<Coordinate, bool> closed = new Dictionary<Coordinate, bool>();
            Dictionary<Coordinate, AStarEntry> queueReverseLookup = new Dictionary<Coordinate, AStarEntry>();

            Coordinate? closestStart = this.FindClosestAStarCell(start, 10);
            Coordinate? closestEnd = this.FindClosestFilledCell(end);

            if (!closestStart.HasValue || !closestEnd.HasValue)
                return null;
            else
            {
                start = closestStart.Value;
                end = closestEnd.Value;
            }

            Vector3 endPos = this.GetRelativePosition(end);

            PriorityQueue<AStarEntry> queue = new PriorityQueue<AStarEntry>(new LambdaComparer<AStarEntry>((x, y) => x.ToGoal.CompareTo(y.ToGoal)));
            AStarEntry firstEntry = new AStarEntry { Coordinate = start, SoFar = 0, ToGoal = (this.GetRelativePosition(start) - endPos).Length() };
            queue.Push(firstEntry);
            queueReverseLookup.Add(start, firstEntry);
            int iteration = 0;
            while (queue.Count > 0)
            {
                AStarEntry entry = queue.Pop();

                if (iteration == iterationLimit
                    || (Math.Abs(entry.Coordinate.X - end.X) <= 1
                    && Math.Abs(entry.Coordinate.Y - end.Y) <= 1
                    && Math.Abs(entry.Coordinate.Z - end.Z) <= 1))
                    return this.constructPath(entry);

                queueReverseLookup.Remove(entry.Coordinate);
                try
                {
                    closed.Add(entry.Coordinate, true);
                }
                catch (ArgumentException)
                {
                    continue;
                }

                foreach (Direction d in DirectionExtensions.Directions)
                {
                    Coordinate next = entry.Coordinate.Move(d);
                    if ((entry.Parent == null || !next.Equivalent(entry.Parent.Coordinate)) && !closed.ContainsKey(next))
                    {
                        Map.CellState state = this[next];
                        if (state.ID == 0)
                        {
                            // This is an empty cell
                            // We can still use it if it's adjacent to a full cell
                            if (this[next.Move(0, 0, 1)].ID == 0
                                && this[next.Move(0, 1, 0)].ID == 0
                                && this[next.Move(0, 1, 1)].ID == 0
                                && this[next.Move(1, 0, 0)].ID == 0
                                && this[next.Move(1, 0, 1)].ID == 0
                                && this[next.Move(1, 1, 0)].ID == 0
                                && this[next.Move(1, 1, 1)].ID == 0
                                && this[next.Move(0, 0, -1)].ID == 0
                                && this[next.Move(0, -1, 0)].ID == 0
                                && this[next.Move(0, -1, -1)].ID == 0
                                && this[next.Move(-1, 0, 0)].ID == 0
                                && this[next.Move(-1, 0, 1)].ID == 0
                                && this[next.Move(-1, -1, 0)].ID == 0
                                && this[next.Move(-1, -1, -1)].ID == 0)
                                continue;
                        }
                        else if (state.Permanent)
                            continue;

                        float tentativeGScore = entry.SoFar + 1;

                        AStarEntry newEntry;
                        if (queueReverseLookup.TryGetValue(next, out newEntry))
                        {
                            if (newEntry.SoFar < tentativeGScore)
                                continue;
                        }

                        if (newEntry == null)
                        {
                            newEntry = new AStarEntry { Coordinate = next, Parent = entry, SoFar = tentativeGScore, ToGoal = (this.GetRelativePosition(next) - endPos).Length() };
                            queue.Push(newEntry);
                            queueReverseLookup.Add(next, newEntry);
                        }
                        else
                            newEntry.SoFar = tentativeGScore;
                    }
                }
                iteration++;
            }

            return null;
        }