private IEnumerable <AStarNode> GetNeighbors(int x, int y) { List <AStarNode> neighbors = new List <AStarNode>(); bool hasLeft = x > 0 && x <= _aStarGrid.Width - 1; bool hasRight = x >= 0 && x < _aStarGrid.Width - 1; bool hasTop = y > 0 && y <= _aStarGrid.Height - 1; bool hasBottom = y >= 0 && y < _aStarGrid.Height - 1; // Check for left. if (hasLeft) { AStarNode left = _aStarGrid.GetNodeAt(x - 1, y); if (left.Walkable) { neighbors.Add(left); } // Check top left diagonal. if (Diagonal && hasTop) { AStarNode topLeft = _aStarGrid.GetNodeAt(x - 1, y - 1); if (topLeft.Walkable) { neighbors.Add(topLeft); } } } // Check for right. if (hasRight) { AStarNode right = _aStarGrid.GetNodeAt(x + 1, y); if (right.Walkable) { neighbors.Add(right); } // Check top right diagonal. if (Diagonal && hasTop) { AStarNode topRight = _aStarGrid.GetNodeAt(x + 1, y - 1); if (topRight.Walkable) { neighbors.Add(topRight); } } } // Check for top. if (hasTop) { AStarNode top = _aStarGrid.GetNodeAt(x, y - 1); if (top.Walkable) { neighbors.Add(top); } } // Check for bottom. // ReSharper disable once InvertIf if (hasBottom) { AStarNode bottom = _aStarGrid.GetNodeAt(x, y + 1); if (bottom.Walkable) { neighbors.Add(bottom); } // Check bottom left diagonal. if (Diagonal && hasLeft) { AStarNode bottomLeft = _aStarGrid.GetNodeAt(x - 1, y + 1); if (bottomLeft.Walkable) { neighbors.Add(bottomLeft); } } // Check bottom right diagonal. // ReSharper disable once InvertIf if (Diagonal && hasRight) { AStarNode bottomRight = _aStarGrid.GetNodeAt(x + 1, y + 1); if (bottomRight.Walkable) { neighbors.Add(bottomRight); } } } return(neighbors); }
/// <summary> /// Update the path. Executes a single step of path finding until the final path has been found. /// </summary> /// <returns>The current path in progress, or null if there is no path.</returns> public AStarNode[] Update() { // Loop while there are nodes in the open set, if there are none left and a path hasn't been found then there is no path. if (_openSet.Count > 0) { // Get the node with the lowest score. (F) AStarNode current = _openSet.OrderBy(x => x.F).First(); // Check if the current node is the end, in which case the path has been found. if (current.Equals(_end)) { Finished = true; } else { // Update sets. _openSet.Remove(current); _closedSet.Add(current); // Get neighbors of current. IEnumerable <AStarNode> neighbors = GetNeighbors(current.X, current.Y); foreach (AStarNode neighbor in neighbors) { // Check if the neighbor is done with, in which case we skip. if (_closedSet.Contains(neighbor)) { continue; } // Get the tentative distance between the current and the neighbor. Using 1 as distance. int tentativeG = current.G + 1; // Check if the neighbor is being evaluated. if (_openSet.Contains(neighbor)) { // Check if we have found a more efficient way to the neighbor node. if (tentativeG < neighbor.G) { neighbor.G = tentativeG; } } else { // Assign the calculated distance and add the node to the open set. neighbor.G = tentativeG; _openSet.Add(neighbor); } neighbor.H = HeuristicFunction(neighbor, _end); neighbor.CameFrom = current; } } // Return the path as currently found. List <AStarNode> path = new List <AStarNode> { current }; // Trace current's ancestry. while (current.CameFrom != null) { path.Add(current.CameFrom); current = current.CameFrom; } // Reverse so the goal isn't at the 0 index but is last node. path.Reverse(); LastFoundPath = path.ToArray(); return(LastFoundPath); } Finished = true; return(LastFoundPath); }