C# Heap.Add方法代码示例

	IEnumerator FindPath(Vector3 startPos, Vector3 targetPos) 
	{
		
		Stopwatch sw = new Stopwatch();
		sw.Start();
		
		Vector3[] waypoints = new Vector3[0];
		bool pathSuccess = false;
		
		Node startNode = grid.NodeFromWorldPoint(startPos);
		Node targetNode = grid.NodeFromWorldPoint(targetPos);
		
		if (startNode.walkable && targetNode.walkable) 
		{
			Heap<Node> openSet = new Heap<Node>(grid.MaxSize);
			HashSet<Node> closedSet = new HashSet<Node>();
			openSet.Add(startNode);
			
			while (openSet.Count > 0) 
			{
				Node currentNode = openSet.RemoveFirst();
				closedSet.Add(currentNode);
				
				if (currentNode == targetNode) 
				{
					sw.Stop();
					print ("Path found: " + sw.ElapsedMilliseconds + " ms");
					pathSuccess = true;
					break;
				}
				
				foreach (Node neighbour in grid.GetNeighbours(currentNode)) 
				{
					if (!neighbour.walkable || closedSet.Contains(neighbour)) 
					{
						continue;
					}
					
					int newMovementCostToNeighbour = currentNode.gCost + GetDistance(currentNode, neighbour) + neighbour.movementPenalty;
					if (newMovementCostToNeighbour < neighbour.gCost || !openSet.Contains(neighbour)) 
					{
						neighbour.gCost = newMovementCostToNeighbour;
						neighbour.hCost = GetDistance(neighbour, targetNode);
						neighbour.parent = currentNode;
						
						if (!openSet.Contains(neighbour))
							openSet.Add(neighbour);
						else
							openSet.UpdateItem (neighbour);
					}
				}
			}
		}
		yield return null;
		if (pathSuccess) {
			waypoints = RetracePath(startNode,targetNode);
		}
		requestManager.FinishedProcessingPath(waypoints,pathSuccess);
		
	}

	void FindPath(Vector3 startPos, Vector3 targetPos) {  // performs A* search on the grid to find a path
        startPos = new Vector3(startPos.x + 8.0f, 0, startPos.z - 2.0f); // offsets
        targetPos = new Vector3(targetPos.x + 8.0f, 0, targetPos.z - 2.0f); // offsets

        Node startNode = grid.GetNodeFromWorldPoint(startPos);
        Node targetNode = grid.GetNodeFromWorldPoint(targetPos);

        if (targetNode.walkable == false) return; // don't try to path find if we're on an unwalkable area

		Heap<Node> openSet = new Heap<Node>(grid.MaxSize); 
        HashSet<Node> closedSet = new HashSet<Node>();

        openSet.Add(startNode);
        while(openSet.Count > 0) { // we still have nodes 
			Node currentNode = openSet.pop();
            closedSet.Add(currentNode);
            if(currentNode == targetNode) { // we've found exit
                RetracePath(startNode, targetNode);
                path = backTrackPath(startNode, targetNode);
                return;
            }
            foreach(Node n in grid.GetNeighbours(currentNode)) {
                if (!n.walkable || closedSet.Contains(n)) continue;
                int newMovementCostToNeighbour = currentNode.gCost + GetDistance(currentNode, n);
                if(newMovementCostToNeighbour < n.gCost || !openSet.contains(n)) {
                    n.gCost = newMovementCostToNeighbour;
                    n.hCost = GetDistance(n, targetNode);
                    n.parent = currentNode;

                    if (!openSet.contains(n)) openSet.Add(n); // add our neighbour into open set
					else openSet.UpdateItem(n);
                }
            }
        }
    }

        public void Simple()
        {
            var heap = new Heap<int>(HeapType.Minimum)
                           {
                               5
                           };

            Assert.AreEqual(heap.Count, 1);
            Assert.IsFalse(heap.IsEmpty);
            Assert.AreEqual(heap.Root, 5);

            heap.Add(2);
            Assert.AreEqual(heap.Count, 2);
            Assert.IsFalse(heap.IsEmpty);
            Assert.AreEqual(heap.Root, 2);

            heap.Add(3);
            Assert.AreEqual(heap.Count, 3);
            Assert.IsFalse(heap.IsEmpty);
            Assert.AreEqual(heap.Root, 2);

            Assert.AreEqual(heap.RemoveRoot(), 2);

            heap.Add(1);
            Assert.AreEqual(heap.Count, 3);
            Assert.IsFalse(heap.IsEmpty);
            Assert.AreEqual(heap.Root, 1);
        }

    /* 1 Implement a class PriorityQueue<T> based
     * on the data structure "binary heap".
     * */
    static void Main(string[] args)
    {
        var heap = new Heap<int>();
        heap.Add(1);
        heap.Add(2);
        heap.Add(3);

        Debug.Assert(heap.SameContents(new[] { 1, 2, 3 }));
        Console.WriteLine(string.Join(",", heap));

        Debug.Assert(heap.ChopHead() == 3);
        Debug.Assert(heap.ChopHead() == 2);
        Debug.Assert(heap.ChopHead() == 1);
        Debug.Assert(heap.IsEmpty);

        // higher string means lower priority
        var pqueue = new PriorityQueue<string, string>((s1, s2) => -s1.CompareTo(s2));

        pqueue.Enqueue("18:00", "Buy food");
        pqueue.Enqueue("06:00", "Walk dog");
        pqueue.Enqueue("21:00", "Do homework");
        pqueue.Enqueue("09:00", "Go to work");
        pqueue.Enqueue("21:00", "Drink beer");

        Debug.Assert(pqueue.Count == 5);

        Debug.Assert(pqueue.Dequeue() == "Walk dog");
        Debug.Assert(pqueue.Dequeue() == "Go to work");
        Debug.Assert(pqueue.Dequeue() == "Buy food");
        Debug.Assert(new[] { "Do homework", "Drink beer" }.Contains(pqueue.Dequeue()));
        Debug.Assert(new[] { "Do homework", "Drink beer" }.Contains(pqueue.Dequeue()));
    }

 public void FindPath(Grid _grid)
 {
     Node start = _grid.StartNode;
     Node end = _grid.EndNode;
     open = new Heap<Node>(_grid.GridMaxSize);
     close = new HashSet<Node>();
     open.Add(start);
     while (open.Count > 0)
     {
         Node current = open.GetFirst();
         if (current.GridBlock.Equals(end.GridBlock))
             return;
         foreach(Node p in _grid.GetNeighbours(current))
         {
             if (p.GridBlock.Type != GridBlock.BlockType.Obstacle || close.Contains(p))
                 continue;                                      
             int gCost = current.gCost + GetDistance(current, p);
             if(gCost < current.gCost || !open.Contains(p))
             {
                 p.gCost = gCost;
                 p.hCost = GetDistance(current, p);
                 p.Parent = current;
                 if (!open.Contains(p))
                     open.Add(p);
             }
         }
     }
 }

        List<Point> FindRoadPath(Road a, Road b, RoadType T)
        {
            AstarObject[,] Set = new AstarObject[14, 9];
            for (int x = 0; x < 14; x++)
            {
                for (int y = 0; y < 9; y++)
                {
                    Set[x, y] = new AstarObject(x, y, this);
                }
            }

            Heap<AstarObject> OpenSet = new Heap<AstarObject>(14 * 9);
            HashSet<AstarObject> ClosedSet = new HashSet<AstarObject>();
            AstarObject Start = Set[a.X, a.Y];
            AstarObject End = Set[b.X, b.Y];
            OpenSet.Add(Start);

            while (OpenSet.Count > 0)
            {
                AstarObject CurrentLocation = OpenSet.RemoveFirst();

                ClosedSet.Add(CurrentLocation);

                if (CurrentLocation == End)
                {
                    return RetracePath(Start, End);
                    //Retracepath and stuff.
                }

                List<AstarObject> Neighbours = GetNeighbours(CurrentLocation, ref Set, NeighbourhoodType.Neumann, MapsizeXR, MapsizeYR);
                foreach (AstarObject neighbour in Neighbours)
                {
                    if (neighbour.RType != T || ClosedSet.Contains(neighbour))
                    {
                        continue;
                    }

                    int newMovementCostToNeighbour = CurrentLocation.gCost + GetDistance(CurrentLocation, neighbour);
                    if (newMovementCostToNeighbour < neighbour.gCost || !OpenSet.Contains(neighbour))
                    {
                        neighbour.gCost = newMovementCostToNeighbour;
                        neighbour.hCost = GetDistance(neighbour, End);
                        neighbour.parent = CurrentLocation;

                        if (!OpenSet.Contains(neighbour))
                        {
                            OpenSet.Add(neighbour);
                        }
                        else
                        {
                            OpenSet.UpdateItem(neighbour);
                        }

                    }

                }

            }
            return new List<Point>();
        }

    public void aStar(Vector3 startPos, Vector3 endPos)
    {
        Stopwatch watch = new Stopwatch();
        watch.Start();

        //List<Node_K> openSet = new List<Node_K>();
        Heap<Node_K> openSet = new Heap<Node_K>(AIarea.GetMaxSize);
        HashSet<Node_K> closedSet = new HashSet<Node_K>();
        Node_K sourceNode = AIarea.getNodeAtPos(startPos);
        Node_K targetNode = AIarea.getNodeAtPos(endPos);
        openSet.Add(sourceNode);

        while(openSet.Count > 0)
        {
            Node_K currentNode = openSet.RemoveFirst();
            //Node_K currentNode = openSet[0];
            //for (int i = 1; i < openSet.Count; i++)
            //{
            //    if (openSet[i].fCost < currentNode.fCost || openSet[i].fCost == currentNode.fCost && openSet[i].hCost < currentNode.hCost)
            //    {
            //        currentNode = openSet[i];
            //    }
            //}

            //openSet.Remove(currentNode);
            closedSet.Add(currentNode);

            if(currentNode == targetNode)
            {
                reversePath(sourceNode, targetNode);
                watch.Stop();
                time = watch.ElapsedMilliseconds.ToString();
                print("Astar " + watch.ElapsedMilliseconds + "ms");
                return;
            }

            List<Node_K> neighbors = new List<Node_K>();
            neighbors = AIarea.neighbors(currentNode);
            for (int i = 0; i < neighbors.Count; i++)
            {
                if (neighbors[i].walkable == false || closedSet.Contains(neighbors[i]))
                    continue;

                int cost = currentNode.gCost + moveCost(currentNode, neighbors[i]);
                if (!openSet.Contains(neighbors[i]) || (cost < neighbors[i].gCost))
                {
                    neighbors[i].gCost = cost;
                    neighbors[i].hCost = moveCost(neighbors[i], targetNode);
                    neighbors[i].parent = currentNode;

                    if (!openSet.Contains(neighbors[i]))
                        openSet.Add(neighbors[i]);
                    else
                        openSet.UpdateItem(neighbors[i]);
                }
            }
        }
    }

    //Generate path from start to destination using A*
    public Path findPath(Vector3 startPos, Vector3 targetPos, int steps) {
        //Convert 3D Positions to GridNodes
        GridNode startNode = grid.getNode(startPos);
        GridNode targetNode = grid.getNode(targetPos);
        //Exit if nodes are out of bounds
        if (startNode == null || targetNode == null)
        {
            return null;
        }

        //Instantiate Open and Closed set DataStructures for A* Algorithm
        HashSet<GridNode> closedSet = new HashSet<GridNode>();
        Heap<GridNode> openSet = new Heap<GridNode>(grid.MaxSize);        
        openSet.Add(startNode); //Add start node to the open set

        int count = 0;

        //Iterate through as long as openset is not empty
        while(openSet.Count > 0 && count++ < steps) {
            //Store first node in collection 
            GridNode currentNode = openSet.RemoveFirst();
            closedSet.Add(currentNode);

            //Check if arrived at target node
            if(currentNode == targetNode) {
                //Retrace and get coordinates
                List<GridNode> points = retracePath(startNode, targetNode);
                //Convert to a path
                return getPath(points);
            }

            //Iterate through all neighbor nodes
            foreach(GridNode neighbor in grid.GetNeighbors(currentNode)) {
                //Skip node if not walkable or already used
                if (!neighbor.isWalkable() || closedSet.Contains(neighbor))
                {
                    continue;
                }

                //Calculate the move cost to the neighbor nodes
                int moveCost = currentNode.gCost + GridNode.getDistance(currentNode, neighbor);

                //Calculate neighbor's costs
                if(moveCost < neighbor.gCost || !openSet.Contains(neighbor)) {
                    //Set Costs
                    neighbor.gCost = moveCost;
                    neighbor.hCost = GridNode.getDistance(neighbor, targetNode);
                    neighbor.parent = currentNode;

                    //If the open set doesn't already contain the neighboring node, add it
                    if (!openSet.Contains(neighbor))
                        openSet.Add(neighbor);
                }
            }
        }
        //Unable to find a path
        return null;
    }

    IEnumerator FindPath(Vector3 startPos, Vector3 targetPos)
    {
        Vector3[] waypoints = new Vector3[0];
        bool pathSuccess = false;

        Node startNode = grid.NodeFromWorlPoint (startPos);
        Node targetNode = grid.NodeFromWorlPoint (targetPos);

        if (startNode.Walkable && targetNode.Walkable)
        {
            Heap<Node> openSet = new Heap<Node>(grid.MaxSize);
            HashSet<Node> closedSet = new HashSet<Node>();
            openSet.Add(startNode);

            while (openSet.Count > 0)
            {
                Node currentNode = openSet.RemoveFirst();
                closedSet.Add(currentNode);

                if (currentNode == targetNode)
                {
                    pathSuccess = true;
                    break;
                }

                foreach (Node neighbour in grid.GetNeighbours(currentNode))
                {
                    if (!neighbour.Walkable || closedSet.Contains(neighbour))
                        continue;
                    int newMovementCostToNeighbour = currentNode.gCost + GetDistance(currentNode, neighbour);
                    if (newMovementCostToNeighbour < neighbour.gCost || !openSet.Contains(neighbour))
                    {
                        neighbour.gCost = newMovementCostToNeighbour;
                        neighbour.hCost = GetDistance(neighbour, targetNode);
                        neighbour.Parent = currentNode;

                        if (!openSet.Contains(neighbour))
                        {
                            openSet.Add(neighbour);
                        }
                        else
                            openSet.UpdateItem(neighbour);
                    }
                }
            }
            yield return null;
            if (pathSuccess)
            {
                waypoints = RetracePath(startNode, targetNode);
            }
            requestManager.FinishedProcessingPath(waypoints, pathSuccess);
        } else {
            requestManager.FinishedProcessingPath(waypoints, pathSuccess);
            yield break;
        }
    }

	//A* implementation
	public static IEnumerator CalculatePath(PathNode startNode, PathNode endNode, PathNode[] allNodes, Action<Vector2[], bool> callback){

		#if DEBUG
		Stopwatch sw = new Stopwatch();
		sw.Start();
		#endif

		var openList = new Heap<PathNode>(allNodes.Length);
		var closedList = new HashSet<PathNode>();
		var success = false;

		openList.Add(startNode);

		while (openList.Count > 0){

			var currentNode = openList.RemoveFirst();
			closedList.Add(currentNode);

			if (currentNode == endNode){
				
				#if DEBUG
				sw.Stop();
				//UnityEngine.Debug.Log("Path Found: " + sw.ElapsedMilliseconds + " ms.");
				#endif

				success = true;
				break;
			}

			
			foreach (var neighbour in currentNode.links.Select( index => allNodes[index] )){

				if (closedList.Contains(neighbour))
					continue;

				var costToNeighbour = currentNode.gCost + GetDistance( currentNode, neighbour );
				if (costToNeighbour < neighbour.gCost || !openList.Contains(neighbour) ){
					neighbour.gCost = costToNeighbour;
					neighbour.hCost = GetDistance(neighbour, endNode);
					neighbour.parent = currentNode;

					if (!openList.Contains(neighbour)){
						openList.Add(neighbour);
						openList.UpdateItem(neighbour);
					}
				}
			}
		}

		yield return null;
		if (success){
			callback( RetracePath(startNode, endNode), true );
		} else {
			callback( new Vector2[0], false );
		}
	}

        public void AddExample()
        {
            var heap = new Heap<string>(HeapType.Minimum);
            heap.Add("cat");
            heap.Add("dog");
            heap.Add("canary");

            // There should be 3 items in the heap.
            Assert.AreEqual(3, heap.Count);
        }

    Vector2[] FindPath(Vector2 from, Vector2 to)
    {
        Stopwatch sw = new Stopwatch();
        sw.Start();

        Vector2[] waypoints = new Vector2[0];
        bool pathSuccess = false;

        Node startNode = grid.NodeFromWorldPoint(from);
        Node targetNode = grid.NodeFromWorldPoint(to);
        startNode.parent = startNode;

        if (startNode.walkable && targetNode.walkable) {
            Heap<Node> openSet = new Heap<Node>(grid.MaxSize);
            HashSet<Node> closedSet = new HashSet<Node>();
            openSet.Add(startNode);

            while (openSet.Count > 0) {
                Node currentNode = openSet.RemoveFirst();
                closedSet.Add(currentNode);

                if (currentNode == targetNode) {
                    sw.Stop();
                    print ("Path found: " + sw.ElapsedMilliseconds + " ms");
                    pathSuccess = true;
                    break;
                }

                foreach (Node neighbour in grid.GetNeighbours(currentNode)) {
                    if (!neighbour.walkable || closedSet.Contains(neighbour)) {
                        continue;
                    }

                    int newMovementCostToNeighbour = currentNode.gCost + GetDistance(currentNode, neighbour)+TurningCost(currentNode,neighbour);
                    if (newMovementCostToNeighbour < neighbour.gCost || !openSet.Contains(neighbour)) {
                        neighbour.gCost = newMovementCostToNeighbour;
                        neighbour.hCost = GetDistance(neighbour, targetNode);
                        neighbour.parent = currentNode;

                        if (!openSet.Contains(neighbour))
                            openSet.Add(neighbour);
                        else
                            openSet.UpdateItem(neighbour);
                    }
                }
            }
        }

        if (pathSuccess) {
            waypoints = RetracePath(startNode,targetNode);
        }

        return waypoints;
    }

    void FindPath(Vector2 startPos, Vector2 targetPos)
    {
        Stopwatch sw = new Stopwatch();
        sw.Start();

        Node startNode = grid.NodeFromWorldPoint(startPos);
        Node targetNode = grid.NodeFromWorldPoint(targetPos);

        Heap<Node> openSet = new Heap<Node>(grid.MaxSize);
        HashSet<Node> closedSet = new HashSet<Node>();
        openSet.Add(startNode);

        while(openSet.Count >0 )
        {

            Node currentNode = openSet.RemoveFirst();
            closedSet.Add(currentNode);

            if (currentNode== targetNode){
                sw.Stop();
                print("Path found" + sw.ElapsedMilliseconds + "ms");
                RetracePath(startNode,targetNode);

                return;

            }

            //check if neighbour is walkable
            foreach(Node neighbour in grid.GetNeighbours(currentNode))
            {
                if(!neighbour.walkable || closedSet.Contains(neighbour))
                {
                    continue;
                }

                int newMovementCostToNeighbour = currentNode.gCost + GetDistance(currentNode, neighbour);

                if( newMovementCostToNeighbour < neighbour.gCost || !openSet.Contains(neighbour)){

                    neighbour.gCost = newMovementCostToNeighbour;
                    neighbour.hCost = GetDistance(neighbour, targetNode);
                    neighbour.parent = currentNode;

                    if (!openSet.Contains(neighbour))
                        openSet.Add(neighbour);
                    else
                       openSet.UpdateItem(neighbour);

                }

            }
        }
    }

    IEnumerator FindPath(Vector3 startPos, Vector3 targetPos)
    {
        //Create and start stopwatch to log method completion time
        Stopwatch sw = new Stopwatch();
        sw.Start();

        //Initialize waypoints array
        Vector3[] waypoints = new Vector3[0];
        bool pathSuccess = false;

        //Determine the start and target node based on the Gameobjects Vector 3 positions
        Node startNode = grid.NodeFromWorldPoint(startPos);
        Node targetNode = grid.NodeFromWorldPoint(targetPos);
        if(startNode.walkable && targetNode.walkable) {
            Heap<Node> openSet = new Heap<Node>(grid.MaxSize); //Holds all the nodes in the grid
            HashSet<Node> closedSet = new HashSet<Node>(); //Used to store nodes that are
            openSet.Add(startNode);

            while (openSet.Count > 0) {
                Node currentNode = openSet.RemoveFirst();
                closedSet.Add(currentNode);

                if (currentNode == targetNode) {
                    sw.Stop();
                    print("Path Found: " + sw.ElapsedMilliseconds + " ms");
                    pathSuccess = true;
                    break;
                }

                foreach(Node neighbour in grid.GetNeighbours(currentNode)) {
                    if(!neighbour.walkable || closedSet.Contains(neighbour)) {
                        continue;
                    }

                    int newMovementCostToNeighbour = currentNode.gCost + GetDistance(currentNode, neighbour) + neighbour.movementPenalty;
                    if(newMovementCostToNeighbour < neighbour.gCost || !openSet.Contains(neighbour)) {
                        neighbour.gCost = newMovementCostToNeighbour;
                        neighbour.hCost = GetDistance(neighbour, targetNode);
                        neighbour.parent = currentNode;
                        if (!openSet.Contains(neighbour))
                            openSet.Add(neighbour);
                        else
                            openSet.UpdateItem(neighbour);
                    }
                }
            }
        }
        yield return null;
        if (pathSuccess) {
            waypoints = RetracePath(startNode, targetNode);
        }
        requestManager.FinishedProcessingPath(waypoints, pathSuccess);
    }

    public bool FindPathImmediate(GridNode start, GridNode end, List<List<GridNode>> grid, out List<GridNode> path)
    {
        path = new List<GridNode>();
        bool success = false;
        if (start.walkable && end.walkable)
        {
            Heap<GridNode> openSet = new Heap<GridNode>(grid.Count * grid[0].Count);
            HashSet<GridNode> closedSet = new HashSet<GridNode>();
            openSet.Add(start);

            while (openSet.Count > 0)
            {
                GridNode currentNode = openSet.RemoveFirst();
                closedSet.Add(currentNode);

                //path found
                if (currentNode == end)
                {
                    success = true;
                    break;
                }

                foreach (GridNode neighbour in currentNode.GetNeighbours())
                {
                    if (!neighbour.walkable || closedSet.Contains(neighbour))
                        continue;

                    int newNeighbourMoveCost = currentNode.gCost + GetDistanceManhatten(currentNode, neighbour);
                    if (newNeighbourMoveCost < neighbour.gCost || !openSet.Contains(neighbour))
                    {
                        neighbour.gCost = newNeighbourMoveCost;
                        neighbour.hCost = GetDistanceManhatten(neighbour, end);
                        neighbour.parent = currentNode;

                        if (!openSet.Contains(neighbour))
                            openSet.Add(neighbour);
                        else
                            openSet.UpdateItem(neighbour);
                    }
                }
            }
        }
        if (success)
        {
            path = RetracePath(start, end);
        }

        return success;
    }