//public void StartFindReachableArea(Vector3 _startPos, float _range) //{ // StartCoroutine(FindReachableArea(_startPos, _range)); //} IEnumerator FindPath(Vector3 startPos, Vector3 endPos) { Vector3[] nodePositions = new Vector3[0]; bool pathFound = false; Node startNode = m_Grid.NodeFromWorldPoint(startPos); Node endNode = m_Grid.NodeFromWorldPoint(endPos); if (startNode.m_Passable && endNode.m_Passable) { Heap <Node> frontier = new Heap <Node>(m_Grid.NumberOfNodes); HashSet <Node> explored = new HashSet <Node>(); frontier.AddItem(startNode); while (frontier.Count > 0) { Node node = frontier.FetchFirst(); explored.Add(node); if (node == endNode) { pathFound = true; break; } foreach (Node neighbour in m_Grid.GetNeighbours(node)) { if (!neighbour.m_Passable || explored.Contains(neighbour)) { continue; } int movementCost = node.m_GCost + GetDistance(node, neighbour) + neighbour.m_MovementPenaly; if (movementCost < neighbour.m_GCost || !frontier.ContainsItem(neighbour)) { neighbour.m_GCost = movementCost; neighbour.m_HCost = GetDistance(neighbour, endNode); neighbour.m_Parent = node; if (!frontier.ContainsItem(neighbour)) { frontier.AddItem(neighbour); frontier.UpdateItem(neighbour); } } } } } yield return(null); if (pathFound) { nodePositions = RetracePath(startNode, endNode); } m_PathRequestHandler.FinishedSearchingForPath(nodePositions, pathFound); }
static void TestHeap() { int[] heapVals = new int[NUM_VALUES] { 10, 5, 30, 15, 20, 40, 60, 25, 50, 35, 45, 65, 70, 75, 55 }; Console.Write("Creating heap of default size (10)\n"); Heap pile = new Heap(); // load the heap with values Console.Write("Now filling it with 15 values, should cause doubling of size\n\n"); for (int i = 0; i < NUM_VALUES; i++) { pile.AddItem(heapVals[i]); } // remove values, should be in ascending order Console.Write("Now removing values to see if properly ordered\n"); Console.Write(" In order s/b: 5 10 15 20 25 30 35 40 45 50 55 60 65 70 75\n"); Console.Write(" Actual order: "); for (int i = 0; i < NUM_VALUES; i++) { Console.Write(pile.GetItem() + " "); } Console.Write("\n\n"); // now test for taking one too many Console.Write("Now testing for exception on removing\n"); try { pile.GetItem(); Console.Write("Should have failed, but did not\n"); } catch (IndexOutOfRangeException ex) { Console.Write("Caught error message: " + ex.Message + "\n"); } Console.Write("\nDone with testing heap\n\n"); }
public List <GridTile> FindPath(GridTile startPosition, GridTile targetPosition) { _pathFound = false; GridTile startTile = startPosition; GridTile targetTile = targetPosition; if (startTile != null && targetTile != null) { _openSet = new Heap <GridTile>(_levelTiles.Count); _closedSet = new Heap <GridTile>(_levelTiles.Count); _openSet.AddItem(startTile); while (_openSet.Count > 0) { GridTile currentTile = _openSet.RemoveFirstItem(); _closedSet.AddItem(currentTile); if (currentTile == targetTile) { _pathFound = true; break; } foreach (GridTile neighbour in GetNeighbourTiles(currentTile)) { if (_closedSet.Contains(neighbour)) { continue; } if (neighbour.Walkable == false || (neighbour.IsLocked == true && neighbour != targetTile)) { _closedSet.AddItem(neighbour); continue; } int newMovementCostToNeighbour = currentTile.GCost + GetDistance(currentTile, neighbour); if (newMovementCostToNeighbour < neighbour.GCost || !_openSet.Contains(neighbour)) { neighbour.GCost = newMovementCostToNeighbour; neighbour.HCost = GetDistance(neighbour, targetTile); neighbour.Parent = currentTile; if (!_openSet.Contains(neighbour)) { _openSet.AddItem(neighbour); } else { _openSet.UpdateItem(neighbour); } } } } } if (_pathFound) { _waypoints = RetracePath(startTile, targetTile); return(_waypoints); } return(new List <GridTile>()); }