/// Runs one iteration of the search. private void ProcessNext() { // Should only call this while there's still work to do. if (_open.Count == 0) { throw new ApplicationException(); } var start = _open.Dequeue(); var distance = _distances[start.x, start.y]; // Update the neighbor's distances. foreach (var dir in Direction.All) { var here = start + dir; var bounds = new Rect(0, 0, _distances.GetUpperBound(0), _distances.GetUpperBound(1)); if (!bounds.Contains(here)) { continue; } // Ignore tiles we've already reached. if (_distances[here.x, here.y] != _unknown) { continue; } // Can't reach impassable tiles. var tile = _stage[here + _offset]; var canEnter = tile.IsPassable || (tile.IsTraversable && _canOpenDoors); // Can't walk through other actors. if (!_ignoreActors && _stage.ActorAt(here + _offset) != null) { canEnter = false; } if (!canEnter) { _distances[here.x, here.y] = _unreachable; continue; } _distances[here.x, here.y] = distance + 1; _open.Enqueue(here); _found.Add(here); } }
public static PathNode _findPath(Stage stage, VectorBase start, VectorBase end, int maxLength, bool canOpenDoors) { //var AStarState = stage.Appearances; var logicCount = 0; //GameBoard.Debugger.Instance.LogToDisk(string.Format("Processing _findPath")); // TODO: More optimal data structure. var startPath = new PathNode(null, Direction.None, start, 0, heuristic(start, end)); var open = new List <PathNode> { startPath }; var closed = new List <VectorBase>(); while (open.Count > 0) { logicCount++; // Debugger the state to disc.. //if (logicCount % 1 == 0) //{ // stage.Debugger(AStarState, "AStar Calculations"); //} if (logicCount >= 50) { return(null); } // Pull out the best potential candidate. var lastIndex = open.Count - 1; var current = open[lastIndex]; open.RemoveAt(lastIndex); //GameBoard.Debugger.Instance.LogToDisk(string.Format(" * current = [" + current.pos.x + "," + current.pos.y + "] at " + current.cost)); if ((current.pos.x == end.x && current.pos.y == end.y) || (current.cost > Option.AStarFloorCost * maxLength)) { // Found the path. return(current); } closed.Add(current.pos); foreach (var dir in Direction.All) { //GameBoard.Debugger.Instance.LogToDisk(string.Format(" * testing = [" + dir.x + "," + dir.y + "]")); var neighbor = current.pos + dir; //GameBoard.Debugger.Instance.LogToDisk(string.Format(" * neighbor = [" + neighbor.x + "," + neighbor.y + "]")); // Skip impassable tiles. if (!stage[neighbor].IsTraversable) { //GameBoard.Debugger.Instance.LogToDisk(string.Format(" * traversable - N")); continue; } //GameBoard.Debugger.Instance.LogToDisk(string.Format(" * traversable - Y ")); // Given how far the current tile is, how far is each neighbor? var stepCost = Option.AStarFloorCost; if (stage[neighbor].Type.OpensTo != null) { if (canOpenDoors) { // One to open the door and one to enter the tile. stepCost = Option.AStarFloorCost * 2; } else { // Even though the monster can't open doors, we don't consider it // totally impassable because there's a chance the door will be // opened by someone else. stepCost = Option.AStarDoorCost; } } else if (stage.ActorAt(neighbor) != null) { stepCost = Option.AStarOccupiedCost; } var cost = current.cost + stepCost; //GameBoard.Debugger.Instance.LogToDisk(string.Format(" * cost = [" + cost + "]")); // See if we just found a better path to a tile we're already // considering. If so, remove the old one and replace it (below) with // this new better path. var inOpen = false; for (var i = 0; i < open.Count; i++) { var alreadyOpen = open[i]; if (alreadyOpen.pos.x == neighbor.x && alreadyOpen.pos.y == neighbor.y) { if (alreadyOpen.cost > cost) { open.RemoveAt(i); i--; } else { inOpen = true; } break; } } //GameBoard.Debugger.Instance.LogToDisk(string.Format(" * inOpen = [" + inOpen + "]")); var inClosed = closed.Any(v => v.x == neighbor.x && v.y == neighbor.y); //GameBoard.Debugger.Instance.LogToDisk(string.Format(" * inClosed = [" + inClosed + "]")); // If we have a new path, add it. if (!inOpen && !inClosed) { var guess = cost + heuristic(neighbor, end); var path = new PathNode(current, dir, neighbor, cost, guess); //GameBoard.Debugger.Instance.LogToDisk(string.Format(" * new Path = [" + path.pos.x + "," + path.pos.y + "] at " + path.cost)); //AStarState[path.pos.x + 1, path.pos.y + 1].Glyph = path.cost.ToString("00"); // Insert it in sorted order (such that the best node is at the *end* // of the list for easy removal). var inserted = false; for (var i = open.Count - 1; i >= 0; i--) { if (open[i].guess > guess) { open.Insert(i + 1, path); inserted = true; break; } } // If we didn't find a node to put it after, put it at the front. if (!inserted) { open.Insert(0, path); } } } } // No path. return(null); }