public void Contains() { var pq = new PriorityQueue<object>(); for (int i = 0; i < 20; i++) pq.Enqueue(i); Assert.IsTrue(pq.Contains(0)); Assert.IsTrue(pq.Contains(14)); Assert.IsTrue(pq.Contains(19)); Assert.IsFalse(pq.Contains(-1)); Assert.IsFalse(pq.Contains(20)); pq.Dequeue(); Assert.IsFalse(pq.Contains(19)); }
public override IEnumerator FindPath(Vector3 startPos, Vector3 targetPos, DistanceHeuristic distanceType, bool simplified) { this.myProperties.sw = new Stopwatch(); this.myProperties.sw.Start(); #region Init Node startTile = this.myProperties.myGraph.GetNode(startPos); Node targetTile = this.myProperties.myGraph.GetNode(targetPos); Vector3[] wayPoints = new Vector3[0]; bool success = false; //List<Tile> open = new List<Tile>(); PriorityQueue<Node> open = new PriorityQueue<Node>(this.myProperties.myGraph.Size); HashSet<Node> close = new HashSet<Node>(); open.Enqueue(startTile); //Add the starting tile to be processed #endregion Init //if (startTile.walkable) { while (open.Count > 0) { Node currentTile = open.Dequeue(); //Set the currentTile to the next elem in open //If we got to the target, the create the path to it //and exit the loop if (currentTile == targetTile) { this.myProperties.sw.Stop(); print("A*: " + this.myProperties.sw.ElapsedMilliseconds + " ms"); success = true; break; } close.Add(currentTile); // foreach (Node adjacent in this.myProperties.myGraph.GetAdjacents(currentTile)) { //Ignore the adjacent neightbor which is already evaluated if (!adjacent.walkable || close.Contains(adjacent)) continue; //Length of this path this.myProperties.tentativeGScore = currentTile.gScore + this.GetDistance(currentTile, adjacent, distanceType); //Find new tiles if (this.myProperties.tentativeGScore < adjacent.gScore || !open.Contains(adjacent)) { adjacent.gScore = this.myProperties.tentativeGScore; adjacent.hScore = this.GetDistance(adjacent, targetTile, distanceType); adjacent.myParent = currentTile; if (!open.Contains(adjacent)) open.Enqueue(adjacent); else open.UpdateElement(adjacent); } } } yield return new WaitForSeconds(0.0000001f); if (success) { wayPoints = CreatePath(startTile, targetTile, simplified); } this.myProperties.myManager.DoneProcessing(wayPoints, success); //} }