This article is a companion guide to my introduction to A*, where I explain how the algorithms work. On this page I show how to implement Breadth-First Search, Dijkstra’s Algorithm, Greedy Best-First Search, and A*. I try to keep the code here simple. Graph search is a family of related algorithms. There are lots of variants of the algorithms, and lots of variants in implementation. Treat the code on this page as a starting point, not as a final version of the algorithm that works for all situations. 1 Python Implementation#I explain most of the code below. There are a few extra bits that you can find in implementation.py. These use Python 3 so if you use Python 2, you will need to remove type annotations, change the 1.1. Breadth First SearchLet’s implement Breadth First Search in Python. The main article shows the Python code for the search algorithm, but we also need to define the graph it works on. These are the abstractions I’ll use:
In the main article, I focused on search. On this page, I’ll fill in the rest of the details to make complete working programs. Let’s start with a graph. What does a graph look like? It’s a location type along with a class with a method to get neighboring locations: Location = TypeVar('Location') class Graph(Protocol): def neighbors(self, id: Location) -> List[Location]: pass
class SimpleGraph: def __init__(self): self.edges: Dict[Location, List[Location]] = {} def neighbors(self, id: Location) -> List[Location]: return self.edges[id] Yep, that’s all we need! You may be asking, where’s the Node object? The answer is: I rarely use a node object. I find it simpler to use integers, strings, or tuples as the Note that the edges are directed: we can have an edge from A to B without also having an edge from B to A. In game maps most edges are bidirectional but sometimes there are one-way doors or jumps off cliffs that are expressed as directed edges. Let’s start with an example map with both two-way and one-way links: Part of turning a map into a graph is choosing which locations to mark. Here I decided to mark each horizontal platform as a location. We can represent this example in a graph where the For each location I need a list of which locations it leads to: example_graph = SimpleGraph() example_graph.edges = { 'A': ['B'], 'B': ['C'], 'C': ['B', 'D', 'F'], 'D': ['C', 'E'], 'E': ['F'], 'F': [], } Before we can use it with a search algorithm, we need to make a queue: import collections class Queue: def __init__(self): self.elements = collections.deque() def empty(self) -> bool: return not self.elements def put(self, x: T): self.elements.append(x) def get(self) -> T: return self.elements.popleft() This queue class is a wrapper around the built-in Let’s try the example graph with this queue and the breadth-first search algorithm code from the main article: from implementation import * def breadth_first_search(graph: Graph, start: Location): # print out what we find frontier = Queue() frontier.put(start) reached: Dict[Location, bool] = {} reached[start] = True while not frontier.empty(): current: Location = frontier.get() print(" Visiting %s" % current) for next in graph.neighbors(current): if next not in reached: frontier.put(next) reached[next] = True print('Reachable from A:') breadth_first_search(example_graph, 'A') print('Reachable from E:') breadth_first_search(example_graph, 'E') Reachable from A: Visiting A Visiting B Visiting C Visiting D Visiting F Visiting E Reachable from E: Visiting E Visiting F Grids can be expressed as graphs too. I’ll now define a new graph called GridLocation = Tuple[int, int] class SquareGrid: def __init__(self, width: int, height: int): self.width = width self.height = height self.walls: List[GridLocation] = [] def in_bounds(self, id: GridLocation) -> bool: (x, y) = id return 0 <= x < self.width and 0 <= y < self.height def passable(self, id: GridLocation) -> bool: return id not in self.walls def neighbors(self, id: GridLocation) -> Iterator[GridLocation]: (x, y) = id neighbors = [(x+1, y), (x-1, y), (x, y-1), (x, y+1)] # E W N S # see "Ugly paths" section for an explanation: if (x + y) % 2 == 0: neighbors.reverse() # S N W E results = filter(self.in_bounds, neighbors) results = filter(self.passable, results) return results Let’s try it out with the first grid in the main article: from implementation import * g = SquareGrid(30, 15) g.walls = DIAGRAM1_WALLS # long list, [(21, 0), (21, 2), ...] draw_grid(g) . . . . . . . . . . . . . . . . . . . . . ###### . . . . . . . . . . . . . . . . . . . . . . . . . . . . ###### . . . . . . . . . . . . . . . . . . . . . . . . . . . . ###### . . . . . . . . . . ###### . . . . . . . . . . . . . . . . ###### . . . . . . . . . . ###### . . . . . . . . ###### . . . . . . ###### . . . . . . . . . . ###### . . . . . . . . ###### . . . . . . ############### . . . . . . . ###### . . . . . . . . ###### . . . . . . ############### . . . . . . . ###### . . . . . . . . ###### . . . . . . . . . . . . . . . . . . ###### . . . . . . . . ###### . . . . . . . . . . . . . . . . . . ###### . . . . . . . . ###### . . . . . . . . . . . . . . . . . . ###### . . . . . . . . ###### . . . . . . . . . . . . . . . . . . ###### . . . . . . . . ###### . . . . . . . . . . . . . . . . . . . . . . . . . . . . ###### . . . . . . . . . . . . . . . . . . . . . . . . . . . . ###### . . . . . . . . . . . . . . . . . . . . . . . . . . . . ###### . . . . . . . . . . . . . . . In order to reconstruct paths we need to store the location of where we came from, so I’ve renamed from implementation import * def breadth_first_search(graph: Graph, start: Location): frontier = Queue() frontier.put(start) came_from: Dict[Location, Optional[Location]] = {} came_from[start] = None while not frontier.empty(): current: Location = frontier.get() for next in graph.neighbors(current): if next not in came_from: frontier.put(next) came_from[next] = current return came_from g = SquareGrid(30, 15) g.walls = DIAGRAM1_WALLS start = (8, 7) parents = breadth_first_search(g, start) draw_grid(g, point_to=parents, start=start) →→→↓↓↓↓↓↓↓↓↓↓↓↓↓←←←←←######↓↓↓↓↓↓↓ →→→→↓↓↓↓↓↓↓↓↓↓↓←←←←←←######→↓↓↓↓↓↓ →→→→→↓↓↓↓↓↓↓↓↓←←←←←←←######→→↓↓↓↓↓ →→↑######↓↓↓↓↓↓↓↓←←←←←←←←######→→→↓↓↓↓ →↑↑######↓↓↓↓↓↓↓←######↑←←←←←######→→→↓↓↓↓ ↑↑↑######→↓↓↓↓↓←←######↑↑←←←←###############↓↓↓← ↑↑↑######→→↓↓↓←←←######↑↑↑←←←###############↓↓←← ↓↓↓######→→→ A ←←←←######↑↑↑↑←←←←←←←←←←← ↓↓↓######→→↑↑↑←←←######↑↑↑↑↑←←←←←←←←←← ↓↓↓######→↑↑↑↑↑←←######↑↑↑↑↑↑←←←←←←←←← →↓↓######↑↑↑↑↑↑↑←######↑↑↑↑↑↑↑←←←←←←←← →→↓######↑↑↑↑↑↑↑↑######↑↑↑↑↑↑↑↑←←←←←←← →→→→→↑↑↑↑↑↑↑↑######↑↑↑↑↑↑↑↑↑←←←←←← →→→→↑↑↑↑↑↑↑↑↑######↑↑↑↑↑↑↑↑↑↑←←←←← →→→↑↑↑↑↑↑↑↑↑↑######↑↑↑↑↑↑↑↑↑↑↑←←←← Some implementations use internal storage, creating a Node object to hold 1.2. Early ExitFollowing the code from the main article, we need to add an if statement to the main loop. This test is optional for Breadth First Search or Dijkstra’s Algorithm and effectively required for Greedy Best-First Search and A*: from implementation import * def breadth_first_search(graph: Graph, start: Location, goal: Location): frontier = Queue() frontier.put(start) came_from: Dict[Location, Optional[Location]] = {} came_from[start] = while not frontier.empty(): if current == goal: for next in graph.neighbors(current): if next frontier.put(next) came_from[next return g g.walls = DIAGRAM1_WALLS start = (8, 7) goal = (17, 2) draw_grid(g, point_to=parents, start=start, goal=goal) . →→↓↓↓↓↓↓↓↓↓↓↓↓↓← . . . . ######→→→→↓↓↓↓↓↓↓↓↓↓↓←←← . . . ######→→→→→↓↓↓↓↓↓↓↓↓←←← Z . . . ######→→↑######↓↓↓↓↓↓↓↓←←←←←← . . ###### . ↑↑######↓↓↓↓↓↓↓←######↑←← . . . ###### . . ↑######→↓↓↓↓↓←←######↑↑ . . . . ############### . . . . . . . ######→→↓↓↓←←←######↑ . . . . . ############### . . . . . . . ######→→→ A ←←←←###### . . . . . . . . . . . . . . . . . . ######→→↑↑↑←←←###### . . . . . . . . . . . . . . . . . ↓######→↑↑↑↑↑←←###### . . . . . . . . . . . . . . . . ↓↓######↑↑↑↑↑↑↑←###### . . . . . . . . . . . . . . . →→↓######↑↑↑↑↑↑↑↑###### . . . . . . . . . . . . . . . →→→→→↑↑↑↑↑↑↑↑###### . . . . . . . . . . . . . . . →→→→↑↑↑↑↑↑↑↑↑###### . . . . . . . . . . . . . . . . →→↑↑↑↑↑↑↑↑↑↑###### . . . . . . . . . . . . . . . You can see that the algorithm stops when it finds the goal 1.3. Dijkstra’s AlgorithmThis is what adds complexity to graph search, because we’re going to start processing locations in a better order than “first in, first out”. What do we need to change?
1.3.1. Graph with weightsA regular graph tells me the class WeightedGraph(Graph): def cost(self, from_id: Location, to_id: Location) -> float: pass Let’s implement the interface with a grid that uses grid locations and stores the weights in a dict: class GridWithWeights(SquareGrid): def __init__(self, width: int, height: int): super().__init__(width, height) self.weights: Dict[GridLocation, float] = {} def cost(self, from_node: GridLocation, to_node: GridLocation) -> float: return self.weights.get(to_node, 1) In this forest map I chose to make movement depend only on 1.3.2. Queue with prioritiesA priority queue associates with each item a number called a “priority”. When returning an item, it picks the one with the lowest number.
Here’s a reasonably fast priority queue that uses binary heaps, but does not support reprioritize. To get the right ordering, we’ll use tuples (priority, item). When an element is inserted that is already in the queue, we’ll have a duplicate; I’ll explain why that’s ok in the Optimization section. import heapq class PriorityQueue: def __init__(self): self.elements: List[Tuple[float, T]] = [] def empty(self) -> bool: return not self.elements def put(self, item: T, priority: float heapq.heappush(self.elements, (priority, item)) def get(self) -> T: return heapq.heappop(self.elements)[1] 1.3.3. SearchHere’s a tricky bit about the implementation: once we add movement costs it’s possible to visit a location again, with a better This forest map is from the main page. def dijkstra_search(graph frontier = PriorityQueue() frontier.put(start, 0) came_from: Dict[Location, Optional[Location]] = {} cost_so_far: Dict[Location, float] = {} came_from[start] = None cost_so_far[start] = 0 while not frontier.empty(): current: Location = frontier.get() if current == goal: break for next in new_cost = cost_so_far[current] + graph.cost(current, next) if next not in cost_so_far or new_cost < cost_so_far[next]: cost_so_far[next] = new_cost priority = new_cost frontier.put(next, priority) came_from[next] = current return came_from, cost_so_far Finally, after searching I need to build the path: def reconstruct_path(came_from: Dict[Location, Location], start: Location, goal: Location) -> List[Location]: current: Location = goal path: List[Location] = [] while current != start: path.append(current) current = came_from[current] path.append(start) # optional path.reverse() # optional return path Although paths are best thought of as a sequence of edges, it’s convenient to store them as a sequence of nodes. To build the path, start at the end and follow the Let’s try it out: from implementation import * start, goal = (1, 4), (8, 3) came_from, cost_so_far draw_grid(diagram4, point_to=came_from, start=start, goal=goal) print draw_grid(diagram4, path=reconstruct_path(came_from, start=start, goal=goal)) ↓↓←←←←←←←← ↓↓←←←↑↑←←← ↓↓←←←←↑↑←← ↓↓←←←←←↑ Z → A ←←←← . . . . ↑↑←←←← . . . . ↑↑←←←←← . . . ↑#########↑←↓↓ . . ↑#########↓↓↓←← . ↑←←←←←←←← . . @ @ @ @ @ @ . @ . . . . @ @ . @ . . . . . @ @ . @ . . . . . . @ . @ . . . . . . . . . . . . . . . . . . . . . ######### . ######### . . . . . . . . . . The first output shows the vector field; the second shows the path. Why is the path going up and over? Remember that this is the forest example from the main page, where the middle of the map has a big forest that’s slow to move through. The shortest path goes around the forest, not through it. The line Collecting distances instead of directions gives us a distance field, which can be useful for some applications: from implementation import * start, goal = (1, 4), (8, 3) came_from, cost_so_far = dijkstra_search(diagram4, start, goal) draw_grid(diagram4, number=cost_so_far, start=start, goal=goal) 5 4 5 6 7 8 9 10 11 12 4 3 4 5 10 13 10 11 12 13 3 2 3 4 9 14 15 12 13 14 2 1 2 3 8 13 18 17 Z . 1 A 1 6 11 16 . . . . 2 1 2 7 12 17 . . . . 3 2 3 4 9 14 19 . . . 4 ######### 14 19 18 15 . . 5 ######### 15 16 13 14 15 . 6 7 8 9 10 11 12 13 14 . We can simplify the logic by making 1.4. A* SearchA* is almost exactly like Dijkstra’s Algorithm, except we add in a heuristic. Note that the code for the algorithm isn’t specific to grids. Knowledge about grids is in the graph class ( def heuristic(a: GridLocation, b: GridLocation) -> float: (x1, y1) = a (x2, y2) = b return abs(x1 - x2) + abs(y1 - y2) def a_star_search(graph: WeightedGraph, start: Location, goal: Location): frontier = PriorityQueue() frontier.put(start, 0) came_from: Dict[Location, Optional[Location]] = {} cost_so_far: Dict[Location, float] = {} came_from[start] = None cost_so_far[start] = 0 while not frontier.empty(): current: Location = frontier.get() if current == goal: break for next in graph.neighbors(current): new_cost = cost_so_far[current] + graph.cost(current, next) if next not in cost_so_far or new_cost < cost_so_far[next]: cost_so_far[next] = new_cost priority = new_cost + heuristic(next, goal) frontier.put(next, priority) came_from[next] = current return came_from, cost_so_far Let’s try it out: from implementation import * start, goal = (1, 4), (8, 3) came_from, cost_so_far = a_star_search(diagram4, start, goal) draw_grid(diagram4, point_to=came_from, start=start, goal=goal) print() draw_grid(diagram4, path=reconstruct_path(came_from, start=start, goal=goal)) ↓↓↓↓←←←←←← ↓↓↓↓←↑↑←←← ↓↓↓↓←←↑↑←← →↓←←←← . ↑ Z . → A ←←← . . . . . ↑↑↑←← . . . . . ↑↑↑←← . . . . . ↑######### . . . . . . . ######### . . . . . . . . . . . . . . . . . . . @ @ @ @ . . . . . . @ . . @ @ . . . . . @ . . . @ @ . . @ @ @ . . . . @ . . @ . . . . . . . . . . . . . . . . . . . . . . . . . . . . . ######### . . . . . . . ######### . . . . . . . . . . . . . . . . Here are the distances it calculated: from implementation import * start, goal = (1, 4), (8, 3) came_from, cost_so_far = a_star_search(diagram4, start, goal) draw_grid(diagram4, number=cost_so_far, start=start, goal=goal) 5 4 5 6 7 8 9 10 11 12 4 3 4 5 10 13 10 11 12 13 3 2 3 4 9 14 15 12 13 14 2 1 2 3 8 13 . 17 Z . 1 A 1 6 11 . . . . . 2 1 2 7 12 . . . . . 3 2 3 4 9 . . . . . 4 ######### . . . . . . . ######### . . . . . . . . . . . . . . . . And that’s it! We’ve implemented graphs, grids, Breadth First Search, Dijkstra’s Algorithm, and A*. 1.4.1. Straighter pathsIf you implement this code in your own project you might find that some of the paths aren’t as “straight” as you’d like. This is normal. When using grids, especially grids where every step has the same movement cost, you end up with ties: many paths have exactly the same cost. A* ends up picking one of the many short paths, and often it won’t look good to you. I list some solutions in a later section. 2 C++ Implementation#Note: some of the sample code needs to include redblobgames/pathfinding/a-star/implementation.cpp to run. I am using C++14 for this code so some of it will need to be changed if you use an older version of the C++ standard. The code here is meant for the tutorial and is not production-quality; there’s a section at the end with tips on making it better. 2.1. Breadth First SearchLet’s implement Breadth First Search in C++. These are the components we need:
In the main article, I focused on search. On this page, I’ll fill in the rest of the details to make complete working programs. Let’s start with a graph where the locations are struct SimpleGraph { std::unordered_map<char, std::vector<char> > edges; std::vector<char> neighbors(char id) { return edges[id]; } }; Note that the edges are directed: we can have an edge from A to B without also having an edge from B to A. In game maps most edges are bidirectional but sometimes there are one-way doors or jumps off cliffs that are expressed as directed edges. Let’s start with an example map with both two-way and one-way links: Part of turning a map into a graph is choosing which locations to mark. Here I decided to mark each horizontal platform as a location. We can represent this example in a graph where the SimpleGraph example_graph {{ {'A', {'B'}}, {'B', {'C'}}, {'C', {'B', 'D', 'F'}}, {'D', {'C', 'E'}}, {'E', {'F'}}, {'F', {}}, }}; The C++ standard library already includes a queue class. We now have a graph ( #include "redblobgames/pathfinding/a-star/implementation.cpp" void breadth_first_search(SimpleGraph graph, char start) { std::queue<char> frontier; frontier.push(start); std::unordered_set<char> reached; reached.insert(start); while (!frontier.empty()) { char current = frontier.front(); frontier.pop(); std::cout << " Visiting " << current << '\n'; for (char next : graph.neighbors(current)) { if (reached.find(next) == reached.end()) { frontier.push(next); reached.insert(next); } } } } int main() { std::cout << "Reachable from A:\n"; breadth_first_search(example_graph, 'A'); std::cout << "Reachable from E:\n"; breadth_first_search(example_graph, 'E'); } Reachable from A: Visiting A Visiting B Visiting C Visiting D Visiting F Visiting E Reachable from E: Visiting E Visiting F Grids can be expressed as graphs too. I’ll now define a new graph called struct GridLocation { int x, y; }; namespace std { /* implement hash function so we can put GridLocation into an unordered_set */ template <> struct hash<GridLocation> { std::size_t operator()(const GridLocation& id) const noexcept { return std::hash<int>()(id.x ^ (id.y << 4)); } }; } struct SquareGrid { static std::array<GridLocation, 4> DIRS; int width, height; std::unordered_set<GridLocation> walls; SquareGrid(int width_, int height_) : width(width_), height(height_) {} bool in_bounds(GridLocation id) const { return 0 <= id.x && id.x < width && 0 <= id.y && id.y < height; } bool passable(GridLocation id) const { return walls.find(id) == walls.end(); } std::vector<GridLocation> neighbors(GridLocation id) const { std::vector<GridLocation> results; for (GridLocation dir : DIRS) { GridLocation next{id.x + dir.x, id.y + dir.y}; if (in_bounds(next) && passable(next)) { results.push_back(next); } } if ((id.x + id.y) % 2 == 0) { // see "Ugly paths" section for an explanation: std::reverse(results.begin(), results.end()); } return results; } }; std::array<GridLocation, 4> SquareGrid::DIRS = { /* East, West, North, South */ GridLocation{1, 0}, GridLocation{-1, 0}, GridLocation{0, -1}, GridLocation{0, 1} }; In the helper file #include "redblobgames/pathfinding/a-star/implementation.cpp" int main() { SquareGrid grid = make_diagram1(); draw_grid(grid); } . . . . . . . . . . . . . . . . . . . . . ###### . . . . . . . . . . . . . . . . . . . . . . . . . . . . ###### . . . . . . . . . . . . . . . . . . . . . . . . . . . . ###### . . . . . . . . . . ###### . . . . . . . . . . . . . . . . ###### . . . . . . . . . . ###### . . . . . . . . ###### . . . . . . ###### . . . . . . . . . . ###### . . . . . . . . ###### . . . . . . ############### . . . . . . . ###### . . . . . . . . ###### . . . . . . ############### . . . . . . . ###### . . . . . . . . ###### . . . . . . . . . . . . . . . . . . ###### . . . . . . . . ###### . . . . . . . . . . . . . . . . . . ###### . . . . . . . . ###### . . . . . . . . . . . . . . . . . . ###### . . . . . . . . ###### . . . . . . . . . . . . . . . . . . ###### . . . . . . . . ###### . . . . . . . . . . . . . . . . . . . . . . . . . . . . ###### . . . . . . . . . . . . . . . . . . . . . . . . . . . . ###### . . . . . . . . . . . . . . . . . . . . . . . . . . . . ###### . . . . . . . . . . . . . . . Let’s try Breadth First Search again, keeping track of #include "redblobgames/pathfinding/a-star/implementation.cpp" template<typename Location, typename Graph> std::unordered_map<Location, Location> breadth_first_search(Graph graph, Location start) { std::queue<Location> frontier; frontier.push(start); std::unordered_map<Location, Location> came_from; came_from[start] = start; while (!frontier.empty()) { Location current = frontier.front(); frontier.pop(); for (Location next : graph.neighbors(current)) { if (came_from.find(next) == came_from.end()) { frontier.push(next); came_from[next] = current; } } } return came_from; } int main() { SquareGrid grid = make_diagram1(); GridLocation start{7, 8}; auto parents = breadth_first_search(grid, start); draw_grid(grid, nullptr, &parents, nullptr, &start); } →→→↓↓↓↓↓↓↓↓↓↓↓↓↓←←←←←######↓↓↓↓↓↓↓ →→→→↓↓↓↓↓↓↓↓↓↓↓←←←←←←######→↓↓↓↓↓↓ →→→→→↓↓↓↓↓↓↓↓↓←←←←←←←######→→↓↓↓↓↓ →→↑######↓↓↓↓↓↓↓↓←←←←←←←←######→→→↓↓↓↓ →↑↑######↓↓↓↓↓↓↓←######↑←←←←←######→→→↓↓↓↓ ↑↑↑######↓↓↓↓↓↓←←######↑↑←←←←###############↓↓↓← ↓↓↓######↓↓↓↓↓←←←######↑↑↑←←←###############↓↓←← ↓↓↓######→↓↓↓←←←←######↑↑↑↑←←←←←←←←←←← ↓↓↓######→→ A ←←←←←######↑↑↑↑↑←←←←←←←←←← ↓↓↓######→↑↑↑←←←←######↑↑↑↑↑↑←←←←←←←←← →↓↓######↑↑↑↑↑←←←######↑↑↑↑↑↑↑←←←←←←←← →→↓######↑↑↑↑↑↑←←######↑↑↑↑↑↑↑↑←←←←←←← →→→→→↑↑↑↑↑↑↑←######↑↑↑↑↑↑↑↑↑←←←←←← →→→→↑↑↑↑↑↑↑↑↑######↑↑↑↑↑↑↑↑↑↑←←←←← →→→↑↑↑↑↑↑↑↑↑↑######↑↑↑↑↑↑↑↑↑↑↑←←←← Some implementations use internal storage, creating a Node object to hold 2.2. Early ExitBreadth First Search and Dijkstra’s Algorithm will explore the entire map by default. If we’re looking for a path to a single, point we can add if (current == goal) to exit the loop as soon as we find the path. #include "redblobgames/pathfinding/a-star/implementation.cpp" template<typename Location, typename Graph> std::unordered_map<Location, Location> breadth_first_search(Graph graph, Location start, Location goal) { std::queue<Location> frontier; frontier.push(start); std::unordered_map<Location, Location> came_from; came_from[start] = start; while (!frontier.empty()) { Location current = frontier.front(); frontier.pop(); if (current == goal) { break; } for (Location next : graph.neighbors(current)) { if (came_from.find(next) == came_from.end()) { frontier.push(next); came_from[next] = current; } } } return came_from; } int main() { GridLocation start{8, 7}, goal{17, 2}; SquareGrid grid = make_diagram1(); auto came_from = breadth_first_search(grid, start, goal); draw_grid(grid, nullptr, &came_from, nullptr, &start, &goal); } . →→↓↓↓↓↓↓↓↓↓↓↓↓↓← . . . . ###### . . . . . . . →→→→↓↓↓↓↓↓↓↓↓↓↓←←← . . . ###### . . . . . . . →→→→→↓↓↓↓↓↓↓↓↓←←← Z . . . ###### . . . . . . . →→↑######↓↓↓↓↓↓↓↓←←←←←← . . ###### . . . . . . . . ↑↑######↓↓↓↓↓↓↓←######↑←← . . . ###### . . . . . . . . . ↑######→↓↓↓↓↓←←######↑↑ . . . . ############### . . . . . . . ######→→↓↓↓←←←######↑ . . . . . ############### . . . . . . . ######→→→ A ←←←←###### . . . . . . . . . . . . . . . . . . ######→→↑↑↑←←←###### . . . . . . . . . . . . . . . . . ↓######→↑↑↑↑↑←←###### . . . . . . . . . . . . . . . . ↓↓######↑↑↑↑↑↑↑←###### . . . . . . . . . . . . . . . →→↓######↑↑↑↑↑↑↑↑###### . . . . . . . . . . . . . . . →→→→→↑↑↑↑↑↑↑↑###### . . . . . . . . . . . . . . . →→→→↑↑↑↑↑↑↑↑↑###### . . . . . . . . . . . . . . . . →→↑↑↑↑↑↑↑↑↑↑###### . . . . . . . . . . . . . . . In the output we can see that the algorithm did not explore the entire map, but stopped early. 2.3. Dijkstra’s AlgorithmThis is what adds complexity to graph search, because we’re going to start processing locations in a better order than “first in, first out”. What do we need to change?
2.3.1. Graph with weightsA regular graph tells me the struct GridWithWeights: SquareGrid { std::unordered_set<GridLocation> forests; GridWithWeights(int w, int h): SquareGrid(w, h) {} double cost(GridLocation from_node, GridLocation to_node) const { return forests.find(to_node) != forests.end()? 5 : 1; } }; 2.3.2. Queue with prioritiesWe need a priority queue. C++ offers a template<typename T, typename priority_t> struct PriorityQueue { typedef std::pair<priority_t, T> PQElement; std::priority_queue<PQElement, std::vector<PQElement>, std::greater<PQElement>> elements; inline bool empty() const { return elements.empty(); } inline void put(T item, priority_t priority) { elements.emplace(priority, item); } T get() { T best_item = elements.top().second; elements.pop(); return best_item; } }; In this sample code I’m wrapping the C++ 2.3.3. SearchSee the forest map from the main page. template<typename Location, typename Graph> void dijkstra_search (Graph graph, Location start, Location goal, std::unordered_map<Location, Location>& came_from, std::unordered_map<Location, double>& cost_so_far) { PriorityQueue<Location, double> frontier; frontier.put(start, 0); came_from[start] = start; cost_so_far[start] = 0; while (!frontier.empty()) { Location current = frontier.get(); if (current == goal) { break; } for (Location next : graph.neighbors(current)) { double new_cost = cost_so_far[current] + graph.cost(current, next); if (cost_so_far.find(next) == cost_so_far.end() || new_cost < cost_so_far[next]) { cost_so_far[next] = new_cost; came_from[next] = current; frontier.put(next, new_cost); } } } } The types of the Finally, after searching I need to build the path: template< path.push_back(current); current = came_from[current]; } path.push_back(start); // optional std::reverse(path.begin(), path.end()); return path; } Although paths are best thought of as a sequence of edges, it’s convenient to store them as a sequence of nodes. To build the path, start at the end and follow the Let’s try it out: #include "redblobgames/pathfinding/a-star/implementation.cpp" int main dijkstra_search(grid, start, goal, came_from, cost_so_far); draw_grid(grid, nullptr, &came_from, nullptr, &start, &goal); std::cout << '\n'; std::vector<GridLocation> path = reconstruct_path(start, goal, came_from); draw_grid(grid, nullptr, nullptr, &path, &start, &goal); std::cout << '\n'; draw_grid(grid, &cost_so_far, nullptr, nullptr, &start, &goal); } ↓↓←←←←←←←← ↓↓←←←↑↑←←← ↓↓←←←←↑↑←← ↓↓←←←←←↑ Z → A ←←←← . . . . ↑↑←←←← . . . . ↑↑←←←←← . . . ↑#########↑←↓↓ . . ↑#########↓↓↓←← . ↑←←←←←←←← . . @ @ @ @ @ @ . @ . . . . @ @ . . . @ . . . . . @ @ . . @ . . . . . . Z . . A . . . . . . . . . . . . . . . . . . . . . . . . . . . . . ######### . . . . . . . ######### . . . . . . . . . . . . . . . . 5 4 5 6 7 8 9 10 11 12 4 3 4 5 10 13 10 11 12 13 3 2 3 4 9 14 15 12 13 14 2 1 2 3 8 13 18 17 Z . 1 A 1 6 11 16 . . . . 2 1 2 7 12 17 . . . . 3 2 3 4 9 14 19 . . . 4 ######### 14 19 18 15 . . 5 ######### 15 16 13 14 15 . 6 7 8 9 10 11 12 13 14 . Why is the path going up and over? Remember that this is the forest example from the main page, where the middle of the map has a big forest that’s slow to move through. The shortest path goes around the forest, not through it. The results are not always the same as the Python version because I’m using the built-in priority queues in C++ and Python. These may order equal-valued nodes differently. This is something you’ll run into if using grids. There are many equally short paths, and the pathfinder will find one of them, not necessarily the one that looks the best to your eye. 2.4. A* SearchA* is almost exactly like Dijkstra’s Algorithm, except we add in a heuristic. Note that the code for the algorithm isn’t specific to grids. Knowledge about grids is in the graph class ( inline double heuristic(GridLocation a, GridLocation b) { return std::abs(a.x - b.x) + std::abs(a.y - b.y); } template<typename Location, typename Graph> void a_star_search (Graph graph, Location start, Location goal, std::unordered_map<Location, Location>& came_from, std::unordered_map<Location, double>& cost_so_far) { PriorityQueue<Location, double> frontier; frontier.put(start, 0); came_from[start] = start; cost_so_far[start] = 0; while (!frontier.empty()) { Location current = frontier.get(); if (current == goal) { break; } for (Location next : graph.neighbors(current)) { double new_cost = cost_so_far[current] + graph.cost(current, next); if (cost_so_far.find(next) == cost_so_far.end() || new_cost < cost_so_far[next]) { cost_so_far[next] = new_cost; double priority = new_cost + heuristic(next, goal); frontier.put(next, priority); came_from[next] = current; } } } } The type of the Minor note: It would be more correct to write Let’s try it out: #include "redblobgames/pathfinding/a-star/implementation.cpp" int main() { GridWithWeights grid = make_diagram4(); GridLocation start{1, 4}, goal{8, 3}; std::unordered_map<GridLocation, GridLocation> came_from; std::unordered_map<GridLocation, double> cost_so_far; a_star_search(grid, start, goal, came_from, cost_so_far); draw_grid(grid, nullptr, &came_from, nullptr, &start, &goal); std::cout << '\n'; std::vector<GridLocation> path = reconstruct_path(start, goal, came_from); draw_grid(grid, nullptr, nullptr, &path, &start, &goal); std::cout << '\n'; draw_grid(grid, &cost_so_far, nullptr, nullptr, &start, &goal); } ↓↓↓↓←←←←←← ↓↓↓↓←↑↑←←← ↓↓↓↓←←↑↑←← →↓←←←← . ↑ Z . → A ←←← . . . . . ↑↑↑←← . . . . . ↑↑↑←← . . . . . ↑######### . . . . . . . ######### . . . . . . . . . . . . . . . . . . . @ @ @ @ . . . . . . @ . . @ @ . . . . . @ . . . @ @ . . @ @ @ . . . . Z . . A . . . . . . . . . . . . . . . . . . . . . . . . . . . . . ######### . . . . . . . ######### . . . . . . . . . . . . . . . . 5 4 5 6 7 8 9 10 11 12 4 3 4 5 10 13 10 11 12 13 3 2 3 4 9 14 15 12 13 14 2 1 2 3 8 13 . 17 Z . 1 A 1 6 11 . . . . . 2 1 2 7 12 . . . . . 3 2 3 4 9 . . . . . 4 ######### . . . . . . . ######### . . . . . . . . . . . . . . . . And that’s it! We’ve implemented graphs, grids, Breadth First Search, Dijkstra’s Algorithm, and A*. 2.4.1. Straighter pathsIf you implement this code in your own project you might find that some of the paths aren’t as “straight” as you’d like. This is normal. When using grids, especially grids where every step has the same movement cost, you end up with ties: many paths have exactly the same cost. A* ends up picking one of the many short paths, and often it won’t look good to you. I list some solutions in a later section. 2.5. Production codeThe C++ code I’ve shown above is simplified to make it easier to follow the algorithm and data structures. In practice there are many things you’d want to do differently:
Here’s how the A* code might look different with some (but not all) of these changes: template<typename Graph> void a_star_search (Graph graph, typename Graph::Location start, typename Graph::Location goal, std::function<typename Graph::cost_t(typename Graph::Location a, typename Graph::Location b)> heuristic, std::unordered_map<typename Graph::Location, typename Graph::Location>& came_from, std::unordered_map<typename Graph::Location, typename Graph::cost_t>& cost_so_far) { typedef typename Graph::Location Location; typedef typename Graph::cost_t cost_t; PriorityQueue<Location, cost_t> frontier; std::vector<Location> neighbors; frontier.put(start, cost_t(0)); came_from[start] = start; cost_so_far[start] = cost_t(0); while (!frontier.empty()) { typename Location current = frontier.get(); if (current == goal) { break; } graph.get_neighbors(current, neighbors); for (Location next : neighbors) { cost_t new_cost = cost_so_far[current] + graph.cost(current, next); if (cost_so_far.find(next) == cost_so_far.end() || new_cost < cost_so_far[next]) { cost_so_far[next] = new_cost; cost_t priority = new_cost + heuristic(next, goal); frontier.put(next, priority); came_from[next] = current; } } } } I wanted the code on this page to be about the algorithms and data structures and not about the C++ optimizations so I tried to show simple code instead of fast or abstract code. 3 C# Implementation#These were my first C# programs so they might not be idiomatic or stylistically proper. These examples aren’t as complete as the Python and C++ sections, but I hope they’re helpful. Here’s a simple graph, and Breadth First Search: using System; using System.Collections.Generic; public class Graph<Location> { // NameValueCollection would be a reasonable alternative here, if // you're always using string location types public Dictionary<Location, Location[]> edges = new Dictionary<Location, Location[]>(); public Location[] Neighbors(Location id) { return edges[id]; } }; class BreadthFirstSearch { static void Search(Graph<string> graph, string start) { var frontier = new Queue<string>(); frontier.Enqueue(start); var reached = new HashSet<string>(); reached.Add(start); while (frontier.Count > 0) { var current = frontier.Dequeue(); Console.WriteLine("Visiting {0}", current); foreach (var next in graph.Neighbors(current)) { if (!reached.Contains(next)) { frontier.Enqueue(next); reached.Add(next); } } } } static void Main() { Graph<string> g = new Graph<string>(); g.edges = new Dictionary<string, string[]> { { "A", new [] { "B" } }, { "B", new [] { "A", "C", "D" } }, { "C", new [] { "A" } }, { "D", new [] { "E", "A" } }, { "E", new [] { "B" } } }; Search(g, "A"); } } Here’s a graph representing a grid with weighted edges (the forest and walls example from the main page): using System; using System.Collections.Generic; // A* needs only a WeightedGraph and a location type L, and does *not* // have to be a grid. However, in the example code I am using a grid. public interface WeightedGraph<L> { double Cost(Location a, Location b); IEnumerable<Location> Neighbors(Location id); } public struct Location { // Implementation notes: I am using the default Equals but it can // be slow. You'll probably want to override both Equals and // GetHashCode in a real project. public readonly int x, y; public Location(int x, int y) { this.x = x; this.y = y; } } public class SquareGrid : WeightedGraph<Location> { // Implementation notes: I made the fields public for convenience, // but in a real project you'll probably want to follow standard // style and make them private. public static readonly Location[] DIRS = new [] { new Location(1, 0), new Location(0, -1), new Location(-1, 0), new Location(0, 1) }; public int width, height; public HashSet<Location> walls = new HashSet<Location>(); public HashSet<Location> forests = new HashSet<Location>(); public SquareGrid(int width, int height) { this.width = width; this.height = height; } public bool InBounds(Location id) { return 0 <= id.x && id.x < width && 0 <= id.y && id.y < height; } public bool Passable(Location id) { return !walls.Contains(id); } public double Cost(Location a, Location b) { return forests.Contains(b) ? 5 : 1; } public IEnumerable<Location> Neighbors(Location id) { foreach (var dir in DIRS) { Location next = new Location(id.x + dir.x, id.y + dir.y); if (InBounds(next) && Passable(next)) { yield return next; } } } } public class PriorityQueue<T> { // I'm using an unsorted array for this example, but ideally this // would be a binary heap. There's an open issue for adding a binary // heap to the standard C# library: https://github.com/dotnet/corefx/issues/574 // // Until then, find a binary heap class: // * https://github.com/BlueRaja/High-Speed-Priority-Queue-for-C-Sharp // * http:///articles/2012/11/01/priority-queues-with-c.aspx // * http://xfleury./graphsearch.html // * http:///questions/102398/priority-queue-in-net private List<Tuple<T, double>> elements = new List<Tuple<T, double>>(); public int Count { get { return elements.Count; } } public void Enqueue(T item, double priority) { elements.Add(Tuple.Create(item, priority)); } public T Dequeue() { int bestIndex = 0; for (int i = 0; i < elements.Count; i++) { if (elements[i].Item2 < elements[bestIndex].Item2) { bestIndex = i; } } T bestItem = elements[bestIndex].Item1; elements.RemoveAt(bestIndex); return bestItem; } } /* NOTE about types: in the main article, in the Python code I just * use numbers for costs, heuristics, and priorities. In the C++ code * I use a typedef for this, because you might want int or double or * another type. In this C# code I use double for costs, heuristics, * and priorities. You can use an int if you know your values are * always integers, and you can use a smaller size number if you know * the values are always small. */ public class AStarSearch { public Dictionary<Location, Location> cameFrom = new Dictionary<Location, Location>(); public Dictionary<Location, double> costSoFar = new Dictionary<Location, double>(); // Note: a generic version of A* would abstract over Location and // also Heuristic static public double Heuristic(Location a, Location b) { return Math.Abs(a.x - b.x) + Math.Abs(a.y - b.y); } public AStarSearch(WeightedGraph<Location> graph, Location start, Location goal) { var frontier = new PriorityQueue<Location>(); frontier.Enqueue(start, 0); cameFrom[start] = start; costSoFar[start] = 0; while (frontier.Count > 0) { var current = frontier.Dequeue(); if (current.Equals(goal)) { break; } foreach (var next in graph.Neighbors(current)) { double newCost = costSoFar[current] + graph.Cost(current, next); if (!costSoFar.ContainsKey(next) || newCost < costSoFar[next]) { costSoFar[next] = newCost; double priority = newCost + Heuristic(next, goal); frontier.Enqueue(next, priority); cameFrom[next] = current; } } } } } public class Test { static void DrawGrid(SquareGrid grid, AStarSearch astar) { // Print out the cameFrom array for (var y = 0; y < 10; y++) { for (var x = 0; x < 10; x++) { Location id = new Location(x, y); Location ptr = id; if (!astar.cameFrom.TryGetValue(id, out ptr)) { ptr = id; } if (grid.walls.Contains(id)) { Console.Write("##"); } else if (ptr.x == x+1) { Console.Write("\u2192 "); } else if (ptr.x == x-1) { Console.Write("\u2190 "); } else if (ptr.y == y+1) { Console.Write("\u2193 "); } else if (ptr.y == y-1) { Console.Write("\u2191 "); } else { Console.Write("* "); } } Console.WriteLine(); } } static void Main() { // Make "diagram 4" from main article var grid = new SquareGrid(10, 10); for (var x = 1; x < 4; x++) { for (var y = 7; y < 9; y++) { grid.walls.Add(new Location(x, y)); } } grid.forests = new HashSet<Location> { new Location(3, 4), new Location(3, 5), new Location(4, 1), new Location(4, 2), new Location(4, 3), new Location(4, 4), new Location(4, 5), new Location(4, 6), new Location(4, 7), new Location(4, 8), new Location(5, 1), new Location(5, 2), new Location(5, 3), new Location(5, 4), new Location(5, 5), new Location(5, 6), new Location(5, 7), new Location(5, 8), new Location(6, 2), new Location(6, 3), new Location(6, 4), new Location(6, 5), new Location(6, 6), new Location(6, 7), new Location(7, 3), new Location(7, 4), new Location(7, 5) }; // Run A* var astar = new AStarSearch(grid, new Location(1, 4), new Location(8, 5)); DrawGrid(grid, astar); } } I haven’t worked with C# much but the structure of the code is the same for my Python and C++ examples, and you can use that same structure in C#. 4 Algorithm changes#The version of Dijkstra’s Algorithm and A* on my pages is slightly different from what you’ll see in an algorithms or AI textbook. The pure version of Dijkstra’s Algorithm starts the priority queue with all nodes, and does not have early exit. It uses a “decrease-key” operation in the queue. It’s fine in theory. But in practice…
This variant is sometimes called “Uniform Cost Search”. See Wikipedia to see the pseudocode, or read Felner’s paper [PDF] to see justifications for these changes. There are three further differences between my version and what you might find elsewhere. These apply to both Dijkstra’s Algorithm and A*:
If you have more suggestions for simplifications that preserve performance, please let me know! 5 Optimizations#For the code I present here, I’ve been focusing on simplicity and generality rather than performance. First make it work, then make it fast. Many of the optimizations I use in real projects are specific to the project, so instead of presenting optimal code, here are some ideas to pursue for your own project: 5.1. GraphThe biggest optimization you can make is to explore fewer nodes. My #1 recommendation is that if you’re using a grid map, consider using a non-grid pathfinding graph. It’s not always feasible but it’s worth looking at. If your graph has a simple structure (e.g. a grid), calculate the neighbors in a function. If it’s a more complex structure (either a non-grid, or a grid with lots of walls, like a maze), store the neighbors in a data structure. You can also save a bit of copying by reusing the neighbors array. Instead of returning a new one each time, allocate it once in the search code and pass it into the graph’s neighbors method. 5.2. QueueBreadth First Search uses a simple queue instead of the priority queue needed by the other algorithms. Queues are simpler and faster than priority queues. In exchange, the other algorithms usually explore fewer nodes. In most game maps, exploring fewer nodes is worth the slowdown from the other algorithms. There are some maps though where you don’t save much, and it might be better to use Breadth First Search. For queues, use a deque instead of an array. A deque allows fast insertion and removal on either end, whereas an array is fast only at one end. In Python, see collections.deque; in C++, see the deque container. However, it turns out breadth first search doesn’t even need a queue! The queue only contains nodes with distance from implementation import * def breadth_first_search(graph: Graph, start: Location): currentfrontier = [] nextfrontier = [] currentfrontier.append(start) reached: Dict[Location, bool] = {} reached[start] = True while currentfrontier: for current in currentfrontier: for next in graph.neighbors(current): if next not in reached: nextfrontier.append(next) reached[next] = True # optimization: swap and clear currentfrontier, nextfrontier = nextfrontier, currentfrontier nextfrontier.clear() print('Reachable from A:') breadth_first_search(example_graph, 'A') print('Reachable from E:') breadth_first_search(example_graph, 'E') This uses two arrays instead of a queue, making Breadth First Search run faster than Dijkstra’s Algorithm when you don’t have varying edge weights. In my Javascript projects, it runs at over 1,000,000 nodes per second. 5.3. Priority QueueFor priority queues, use a binary heap instead of an array or sorted array. A binary heap allows fast insertion and removal, whereas an array is fast at one or the other but not both. In Python, see heapq; in C++, see the priority_queue container. In Python, the Queue and PriorityQueue classes I presented above are so simple that you might consider inlining the methods into the search algorithm. I don’t know if this buys you much; I need to measure it. The C++ versions are going to be inlined. In Dijkstra’s Algorithm, note that the priority queue’s priority is stored twice, once in the priority queue and once in The paper “Priority Queues and Dijkstra’s Algorithm” by Chen, Chowdhury, Ramachandran, Lan Roche, Tong suggests optimizing the structure of Dijkstra’s Algorithm by not reprioritizing, and it also suggests looking at pairing heaps and other data structures. If you’re considering using something other than a binary heap, first measure the size of your frontier and how often you reprioritize. Profile the code and see if the priority queue is the bottleneck. My gut feeling is that bucketing is promising. Just as bucket sort and radix sort can be useful alternatives to quicksort when the keys are integers, we have an even better situation with Dijkstra’s Algorithm and A*. The priorities in Dijkstra’s Algorithm are incredibly narrow. If the lowest element in the queue has priority I have more note about priority queue data structures here. 5.4. SearchThe heuristic adds complexity and cpu time. The goal though is to explore fewer nodes. In some maps (such as mazes), the heuristic may not add much information, and it may be better to use a simpler algorithm without a heuristic guide. Some people use an inadmissible (overestimating) heuristic to speed up A* search. This seems reasonable. I haven’t looked closely into its implications though. I believe (but don’t know for sure) that some already-reached elements may need to be visited again even after they’ve been taken out of the frontier. Some implementations always insert a new node into the open set, even if it’s already there. You can avoid the potentially expensive step of checking whether the node is already in the open set. This will make your open set bigger/slower and you’ll also end up evaluating more nodes than necessary. If the open-set test is expensive, it might still be worth it. However, in the code I’ve presented, I made the test cheap and I don’t use this approach. Some implementations don’t test whether a new node is better than an existing node in the open set. This avoids a potentially expensive check. However, it also can lead to a bug. For some types of maps, you will not find the shortest path when you skip this test. In the code I’ve presented, I check this ( 5.5. Integer locationsIf your graph uses integers as locations, consider using a simple array instead of a hash table for vector<uint16_t> reached(1 + maximum_node_id/16); … size_t index = node_id/16; uint16_t bitmask = 1u << (node_id & 0xf); if (!(reached[index] & bitmask) || new_cost < cost_so_far[next]) { reached[index] |= bitmask; … } If you run only one search at a time, you can statically allocate and then reuse these arrays from one invocation to the next. Then keep an array of all indices that have been assigned to the bit vector, and then reset those on exit. For example: static vector<uint16_t> reached(1 + maximum_node_id/16); static vector<size_t> indices_to_clear; … size_t index = node_id/16; uint16_t bitmask = 1u << (node_id & 0xf); if (!(reached[index] & bitmask) || new_cost < cost_so_far[next]) { if (!reached[index]) { indices_to_clear.push_back(index); } reached[index] |= bitmask; … } … for (size_t index : indices_to_clear) { reached[index] = 0; } indices_to_clear.clear(); (Caveat: I haven’t used or tested this code) 6 Troubleshooting#6.1. TODO No pathsSometimes there just isn’t a path from the start to the goal. These graph search algorithms will search and search until they run out of nodes to explore. At the end, 6.2. Wrong pathsIf you’re not getting a shortest path, try testing:
6.3. Ugly pathsThe most common question I get when people run pathfinding on a grid is why don’t my paths look straight? On a grid with uniform movement costs, there can be more than one shortest path of the same length. For example, in a 4-way movement grid, moving south 2 and east 2 could be any of these:
The hacks don’t work as well as the other three approaches but they’re easy to implement, so I’ll describe them here: 6.3.1. Checkerboard neighbor orderBreadth First Search is sensitive to the order in which it explores the neighbors of a tile. We normally go through the neighbors in a fixed order. Since Breadth First Search uses a first-in-first-out queue, it will pick the first path to a node. If moving south 2 and east 2, there are many ways to get there: from implementation import * test_with_custom_order([(+1, 0), (0, -1), (-1, 0), (0, +1)]) →→→→→→→→↓↓↓↓↓↓↓↓↓↓↓↓↓###### . . . . . . . →→→→→→→→↓↓↓↓↓↓↓↓↓↓↓↓↓###### . . . ↓ . . . →→→→→→→→↓↓↓↓↓↓↓↓↓↓↓↓↓###### . . →↓ Z ↓ . ↑↑↑######→→→↓↓↓↓ @ @ @ @ @ @ @ @ @ ###### . →→↓ @ ↓↓ ↑↑↑######→→→↓↓↓↓ @ ######↑↑↑↑↑ @ ######→→→↓ @ ↓↓ ↑↑↑######→→→↓↓↓↓ @ ######↑↑↑↑↑ @ ###############↓ @ ↓↓ ↑↑↑######→→→↓↓↓↓ @ ######↑↑↑↑↑ @ ###############↓ @ ↓↓ ↑↑↑######→→→ A @ @ @ @ ######↑↑↑↑↑ @ @ @ @ @ @ @ @ ←← →→↓######↑↑↑↑↑↑↑↑######↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑ →→↓######↑↑↑↑↑↑↑↑######↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑ →→↓######↑↑↑↑↑↑↑↑######↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑ →→↓######↑↑↑↑↑↑↑↑######↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑ →→→→→↑↑↑↑↑↑↑↑######↑↑↑↑↑↑↑↑↑↑↑↑↑↑ . ↑↑↑↑↑↑↑↑↑↑↑↑↑######↑↑↑↑↑↑↑↑↑↑↑↑ . . . ↑↑↑↑↑↑↑↑↑↑↑↑↑######↑↑↑↑↑↑↑↑↑↑↑ . . . . It moves east as far as possible before considering north or south. What if North came in the list before East? Here’s the output with the order South, West, North, East: from implementation import * test_with_custom_order([(0, +1), (-1, 0), (0, -1), (+1, 0)]) ↓↓↓↓↓↓↓↓↓←←←←←←←←←←←←###### . . . ↓ . . . ↓↓↓↓↓↓↓↓↓←←←←←←←←←←←←###### . . ↓↓← . . →→→→→↓↓↓↓←←←←←←←←←←←←###### . ↓↓ @ Z . . →→↑######↓↓↓ @ @ @ @ @ @ @ @ ←←←←←######↓↓↓ @ ←← . →→↑######↓↓↓ @ ←←←←###### @ ←←←←←######→→→ @ ←←← →→↑######↓↓↓ @ ←←←←###### @ ←←←←←############### @ ←←← →→↑######↓↓↓ @ ←←←←###### @ ←←←←←############### @ ←←← ↓↓↓######→→→ A ←←←←###### @ @ @ @ @ @ @ @ @ @ @ @ ←←← ↓↓↓######→→→↑←←←←######↑←←←←←←←←←←←←←← ↓↓↓######→→→↑←←←←######↑←←←←←←←←←←←←←← ↓↓↓######→→→↑←←←←######↑←←←←←←←←←←←←←← ↓↓↓######→→→↑←←←←######↑←←←←←←←←←←←←←← →→→→→→→→↑←←←←######↑←←←←←←←←←←←←← . →→→→→→→→↑←←←←######↑←←←←←←←←←←←← . . →→→→→→→→↑←←←←######↑←←←←←←←←←←← . . . Let’s try South, East, West, North: from implementation import * test_with_custom_order([(0, +1), (+1, 0), (-1, 0), (0, -1)]) ↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓###### . . . . . . . ↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓###### . . . ↓ . . . →→→→→↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓###### . . ↓↓ Z ↓ . →→↑######↓↓↓↓↓↓↓ @ @ @ @ ←←←←←###### . ↓↓↓ @ ↓↓ →→↑######↓↓↓↓↓↓↓ @ ###### @ ←←←←←######→→→↓ @ ↓↓ →→↑######↓↓↓↓↓↓↓ @ ###### @ ←←←←←###############↓ @ ↓↓ →→↑######↓↓↓↓↓↓↓ @ ###### @ ←←←←←###############↓ @ ↓↓ ↓↓↓######→→→ A @ @ @ @ ###### @ @ @ @ @ @ @ @ @ @ @ @ @ ←← ↓↓↓######→→→↑←←←←######↑←←←←←←←←←←←←←← ↓↓↓######→→→↑←←←←######↑←←←←←←←←←←←←←← ↓↓↓######→→→↑←←←←######↑←←←←←←←←←←←←←← ↓↓↓######→→→↑←←←←######↑←←←←←←←←←←←←←← →→→→→→→→↑←←←←######↑←←←←←←←←←←←←← . →→→→→→→→↑←←←←######↑←←←←←←←←←←←← . . →→→→→→→→↑←←←←######↑←←←←←←←←←←← . . . No help. Any fixed order of neighbors will lead to long horizontal and vertical sections of the path. Here’s the hack for Breadth First Search: in the graph class, make the list of neighbors depend on
The result is the path alternating between vertical and horizontal steps: from implementation import * test_with_custom_order(None) →→→↓↓↓↓↓↓↓↓↓↓↓↓↓←←←←←###### . . . . . . . →→→→↓↓↓↓↓↓↓↓↓↓↓←←←←←←###### . . . ↓ . . . →→→→→↓↓↓↓↓↓↓↓↓←←←←←←←###### . . ↓↓ Z ↓ . →→↑######↓↓↓↓↓↓↓ @ @ @ @ ←←←←←###### . →→↓ @ ↓↓ →↑↑######↓↓↓↓↓↓ @ @ ###### @ @ ←←←←######→→→↓ @ ↓↓ ↑↑↑######→↓↓↓↓ @ @ ←######↑ @ @ ←←←###############↓ @ ↓← ↑↑↑######→→↓↓ @ @ ←←######↑↑ @ @ ←←###############↓ @ ←← ↓↓↓######→→→ A @ ←←←######↑↑↑ @ @ @ @ @ @ @ @ @ @ ←← ↓↓↓######→→↑↑↑←←←######↑↑↑↑↑←←←←←←←←←← ↓↓↓######→↑↑↑↑↑←←######↑↑↑↑↑↑←←←←←←←←← →↓↓######↑↑↑↑↑↑↑←######↑↑↑↑↑↑↑←←←←←←←← →→↓######↑↑↑↑↑↑↑↑######↑↑↑↑↑↑↑↑←←←←←←← →→→→→↑↑↑↑↑↑↑↑######↑↑↑↑↑↑↑↑↑←←←←← . →→→→↑↑↑↑↑↑↑↑↑######↑↑↑↑↑↑↑↑↑↑←←← . . →→→↑↑↑↑↑↑↑↑↑↑######↑↑↑↑↑↑↑↑↑↑↑← . . . Here’s the code: class SquareGrid: … def neighbors(self, id: GridLocation) -> Iterator[GridLocation]: (x, y) = id neighbors = [(x+1, y), (x-1, y), (x, y-1), (x, y+1)] # E W N S if (x + y) % 2 == 0: neighbors.reverse() # change to S N W E results = filter(self.in_bounds, neighbors) results = filter(self.passable, results) return results This is a quick hack but it works with 4-way movement to make Breadth First Search paths look better. I used this hack on these tutorial pages. (Note: I came up with this hack for these tutorial pages; if you’ve seen a good reference please send it to me.) 6.3.2. Checkerboard movement costsThe direction order hack above works with Breadth First Search, but does it work with A*? Let’s try: from implementation import * g = GridWithWeights(30, 15) g.walls = DIAGRAM1_WALLS start, goal = (8, 7), (27, 2) came_from, cost_so_far = a_star_search(g, start, goal) draw_grid(g, point_to=came_from, path=reconstruct_path(came_from, start, goal), start=start, goal=goal) . . . . . ↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓###### . . . . . . . . . . . ↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓###### . . . ↓ . . . . . . →→→→→↓←←←←←←←←←←←←###### . . → @ Z . . . . . ######→→→ @ @ @ @ @ @ @ @ @ @ @ @ @ ###### . . → @ ← . . . . . ######→→→ @ ←←←←######↑↑↑↑↑ @ ###### . . → @ ← . . . . . ######→→→ @ ←←←←######↑↑↑↑↑ @ ############### @ ← . . . . . ######→→→ @ ←←←←######↑↑↑↑↑ @ ############### @ ← . . . . . ######→→→ A ←←←←######↑↑↑↑↑ @ @ @ @ @ @ @ ← . . . . . ######↑↑↑↑↑↑↑↑######↑↑↑↑↑↑↑↑↑↑↑↑ . . . . . . ######↑↑↑↑↑↑↑↑###### . . . . . . . . . . . . . . . . . . ###### . ↑↑↑↑↑↑↑###### . . . . . . . . . . . . . . . . . . ###### . . ↑↑↑↑↑↑###### . . . . . . . . . . . . . . . . . . . . . . . ↑↑↑↑↑###### . . . . . . . . . . . . . . . . . . . . . . . . . . . . ###### . . . . . . . . . . . . . . . . . . . . . . . . . . . . ###### . . . . . . . . . . . . . . . No, it doesn’t. It changes the insertion order for the queue, but Dijkstra’s Algorithm and A* use a priority queue that follows priority order instead of the insertion order. The priority in Dijkstra’s Algorithm uses the movement cost; the priority in A* uses both the movement cost and the heuristic. We need to modify either the movement cost or the heuristic to change the priority order. Here’s the hack for A* and Dijkstra’s Algorithm: in the graph class, make the movement cost depend on
from implementation import * g = GridWithAdjustedWeights(30, 15) g.walls = DIAGRAM1_WALLS start, goal = (8, 7), (27, 2) came_from, cost_so_far = a_star_search(g, start, goal) draw_grid(g, point_to=came_from, path=reconstruct_path(came_from, start, goal), start=start, goal=goal) . . . . . ↓↓↓↓↓↓↓↓↓↓↓←↓←↓←###### . . . . . . . . . . . ↓↓↓↓↓↓↓↓↓↓↓←↓←↓←↓###### . . . . . . . . . . →→↓→↓↓↓↓↓↓↓←←←←←←←###### . . . ↓ Z . . . . . ######→↓↓↓↓↓↓ @ @ @ @ @ @ ←←←###### . . →↓ @ ← . . . . ######↓→↓↓↓↓ @ @ ######↑← @ @ ↑←###### . . →↓ @ ← . . . . ######→↓→↓↓ @ @ ←######↑↑← @ @ ↑###############↓ @ ← . . . . ######→→↓↓ @ @ ←←######↑↑↑← @ @ ###############↓ @ ← . . . . ######→→→ A @ ←←←######↑↑↑↑← @ @ @ @ @ @ @ @ ← . . . . ######→→↑↑↑←↑←######↑↑↑↑↑↑↑↑↑↑↑↑↑ . . . . . ######→↑↑↑↑↑←↑###### . . . . . . . . . . . . . . . . . . ###### . ↑↑↑↑↑↑←###### . . . . . . . . . . . . . . . . . . ###### . . ↑↑↑↑↑↑###### . . . . . . . . . . . . . . . . . . . . . . . ↑↑↑↑↑###### . . . . . . . . . . . . . . . . . . . . . . . . . . . . ###### . . . . . . . . . . . . . . . . . . . . . . . . . . . . ###### . . . . . . . . . . . . . . . This works nicely with 4-way movement. Here’s the code: class GridWithAdjustedWeights(GridWithWeights): def cost(self, from_node, to_node): prev_cost = super().cost(from_node, to_node) nudge = 0 (x1, y1) = from_node (x2, y2) = to_node if (x1 + y1) % 2 == 0 and x2 != x1: nudge = 1 if (x1 + y1) % 2 == 1 and y2 != y1: nudge = 1 return prev_cost + 0.001 * nudge This is a quick hack but it works with 4-way movement to make Dijkstra’s Algorithm and A* paths look better. (Note: I came up with this hack for these tutorial pages; if you’ve seen this idea elsewhere please send me a reference so I can add it to the page.) 6.3.3. 8-way movementThe above two hacks work for 4-way movement. What if you have 8-way movement? If all 8 directions have the same movement cost, we can end up with a path that takes diagonals when it seems like it shouldn’t: from implementation import * test_with_custom_order([(-1, -1), (-1, +1), (+1, -1), (+1, +1), (+1, 0), (0, -1), (-1, 0), (0, +1)]) ↓→↓↓↓↓↓↓↓↓↓↓↓↓↓↓←↓←↓←###### . . . . . . . →↑→↓↓↓↓↓↓↓↓↓↓↓ @ ←↑←↑←↑######↓↓↓↓↓↓↓ ↑→↑→↓↓↓↓↓↓↓↓↓ @ ← @ ←↑←↑←######↓↓↓↓ Z ↓↓ →↑↑######↓↓↓↓↓↓↓ @ ←↑← @ ←↑←↑######→↓↓↓ @ ↓↓ ↑↑↑######↓↓↓↓↓↓ @ ←######↑← @ ←↑←######↑→↓ @ ↓↓↓ ↑↑↑######→↓↓↓↓ @ ←↑######↑↑← @ ←↑###############↓ @ ↓← ↑↑↑######↑→↓↓ @ ←↑←######↑↑↑← @ ←############### @ ↓←↑ ↑↑↑######→↑→ A ←↑←↑######↑↑↑↑← @ ← @ ← @ @ ↓←↑← ↓↓↓######↑→↑↑↑←↑←######↑↑↑↑↑← @ ← @ ←↑←↑←↑ ↓↓↓######→↑↑↑↑↑←↑######↑↑↑↑↑↑←↑←↑←↑←↑← ↓↓↓######↑↑↑↑↑↑↑←######↑↑↑↑↑↑↑←↑←↑←↑←↑ →↓↓######↑↑↑↑↑↑↑↑######↑↑↑↑↑↑↑↑←↑←↑←↑← ↑→↓→↑↑↑↑↑↑↑↑↑######↑↑↑↑↑↑↑↑↑←↑←↑←↑ →↑→↑↑↑↑↑↑↑↑↑↑######↑↑↑↑↑↑↑↑↑↑←↑←↑← ↑→↑↑↑↑↑↑↑↑↑↑↑######↑↑↑↑↑↑↑↑↑↑↑←↑←↑ The 4-way tie-breaking hacks can be extended to work here:
After using one of these hacks, the path will look like this: from implementation import * test_with_custom_order([(+1, 0), (0, -1), (-1, 0), (0, +1), (-1, -1), (-1, +1), (+1, -1), (+1, +1)]) ↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓###### . . . . . . . ↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓###### . ↓↓↓↓ . . →→→→↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓######↓↓↓↓ Z ↓↓ ↑↑↑######↓↓↓↓↓↓↓↓ @ @ @ @ @ ←←←######↓↓↓ @ ↓↓↓ ↑↑↑######↓↓↓↓↓↓↓ @ ######↑↑↑ @ ↑↑######→→↓ @ ↓↓↓ ↑↑↑######↓↓↓↓↓↓ @ ↓######↑↑↑↑ @ ↑############### @ ↓↓↓ ↑↑↑######↓↓↓↓↓ @ ↓↓######↑↑↑↑↑ @ ############### @ ↓↓↓ ↑↑↑######→→→ A @ ←←←######↑↑↑↑↑↑ @ @ @ @ @ ←←←← ↓↓↓######↑↑↑↑↑↑↑↑######↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑ ↓↓↓######↑↑↑↑↑↑↑↑######↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑ ↓↓↓######↑↑↑↑↑↑↑↑######↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑ ↓↓↓######↑↑↑↑↑↑↑↑######↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑ →→→→↑↑↑↑↑↑↑↑↑######↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑ ↑↑↑↑↑↑↑↑↑↑↑↑↑######↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑ ↑↑↑↑↑↑↑↑↑↑↑↑↑######↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑ These hacks are easy to implement and give reasonable paths for grids. However, for even better paths, try the approaches listed in the Ugly paths section. 7 Vocabulary#Algorithms textbooks often use mathematical notation with single-letter variable names. On these pages I’ve tried to use more descriptive variable names. Correspondences:
The OPEN, CLOSED, and reached sets are sets of states. In my code they do not have their own data structures, but instead are contained as part of other data structures:
We can reason about the OPEN, CLOSED, and reached sets even though they’re not stored in a separate data structure. 8 More reading#
|
|