#+TITLE: Implementation of A*
#+DATE: <2014-07-06>
#+OPTIONS: toc:2
#+PROPERTY: results output
#+PROPERTY: exports both
#+PROPERTY: wrap example
#+begin_export html
#+end_export
#+begin_note
This article is a companion guide to my [[./introduction.html][introduction to A*]]. I show how to implement Breadth-First Search, Dijkstra's Algorithm, Greedy Best-First Search, and A*. I focused on keeping things simple.
#+end_note
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.
* Python Implementation
:PROPERTIES:
:CUSTOM_ID: python
:END:
I explain most of the code below. There are a few extra bits that you can find in [[./implementation.py][implementation.py]]. These use *Python 3* so if you use Python 2, you will need to change the =super()= call and the =print= function to the Python 2 equivalents.
** Breadth First Search
:PROPERTIES:
:CUSTOM_ID: python-breadth-first
:END:
Let'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:
- Graph :: a data structure that can tell me the neighbors for each location (see [[../grids/graphs.html][this tutorial]])
- Locations :: a simple value (int, string, tuple, etc.) that labels locations in the graph
- Search :: an algorithm that takes a graph, a starting location, and optionally a goal location, and calculates some useful information (visited, parent pointer, distance) for some or all locations
- Queue :: a data structure used by the search algorithm to decide the order in which to process the locations
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*:
#+begin_src python :tangle yes :exports none :main no
# Sample code from http://www.redblobgames.com/pathfinding/
# Copyright 2014 Red Blob Games
#
# Feel free to use this code in your own projects, including commercial projects
# License: Apache v2.0
#+end_src
#+begin_src python :tangle yes :exports code :results none :main no
class SimpleGraph:
def __init__(self):
self.edges = {}
def neighbors(self, id):
return self.edges[id]
#+end_src
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 locations, and then use arrays or hash tables that use locations as an index.
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 make an example graph, where the *locations* are letters A-E.
#+begin_src dot :cmd circo :file implementation-example-graph.png :exports results :wrap
digraph {
graph [fontname=Avenir, outputorder=edgesfirst];
node [fontname=Avenir, fontsize=12, shape=circle, style=filled, color="#aaaaaa", fillcolor="#eeeeee"];
edge [color="#999999"];
A -> B;
B -> A; B -> C; B -> D;
C -> A;
D -> E; D -> A;
E -> B;
}
#+end_src
#+RESULTS:
#+BEGIN_RESULTS
[[file:implementation-example-graph.png]]
#+END_RESULTS
For each location I need a list of which locations it leads to:
#+begin_src python :tangle yes :exports code :results none :main no
example_graph = SimpleGraph()
example_graph.edges = {
'A': ['B'],
'B': ['A', 'C', 'D'],
'C': ['A'],
'D': ['E', 'A'],
'E': ['B']
}
#+end_src
Before we can use it with a search algorithm, we need to make a *queue*:
#+begin_src python :tangle yes :exports code :results none :main no
import collections
class Queue:
def __init__(self):
self.elements = collections.deque()
def empty(self):
return len(self.elements) == 0
def put(self, x):
self.elements.append(x)
def get(self):
return self.elements.popleft()
#+end_src
This queue class is just a wrapper around the built-in =collections.deque= class. Feel free to use =deque= directly in your own code.
Let's try the example graph with this queue and the breadth-first search algorithm code from the main article:
#+begin_src python
from implementation import *
def breadth_first_search_1(graph, start):
# print out what we find
frontier = Queue()
frontier.put(start)
visited = {}
visited[start] = True
while not frontier.empty():
current = frontier.get()
print("Visiting %r" % current)
for next in graph.neighbors(current):
if next not in visited:
frontier.put(next)
visited[next] = True
breadth_first_search_1(example_graph, 'A')
#+end_src
#+RESULTS:
#+BEGIN_example
Visiting 'A'
Visiting 'B'
Visiting 'C'
Visiting 'D'
Visiting 'E'
#+END_example
#+begin_src python :tangle yes :exports none :results none :main no
# utility functions for dealing with square grids
def from_id_width(id, width):
return (id % width, id // width)
def draw_tile(graph, id, style, width):
r = "."
if 'number' in style and id in style['number']: r = "%d" % style['number'][id]
if 'point_to' in style and style['point_to'].get(id, None) is not None:
(x1, y1) = id
(x2, y2) = style['point_to'][id]
if x2 == x1 + 1: r = "\u2192"
if x2 == x1 - 1: r = "\u2190"
if y2 == y1 + 1: r = "\u2193"
if y2 == y1 - 1: r = "\u2191"
if 'start' in style and id == style['start']: r = "A"
if 'goal' in style and id == style['goal']: r = "Z"
if 'path' in style and id in style['path']: r = "@"
if id in graph.walls: r = "#" * width
return r
def draw_grid(graph, width=2, **style):
for y in range(graph.height):
for x in range(graph.width):
print("%%-%ds" % width % draw_tile(graph, (x, y), style, width), end="")
print()
# data from main article
DIAGRAM1_WALLS = [from_id_width(id, width=30) for id in [21,22,51,52,81,82,93,94,111,112,123,124,133,134,141,142,153,154,163,164,171,172,173,174,175,183,184,193,194,201,202,203,204,205,213,214,223,224,243,244,253,254,273,274,283,284,303,304,313,314,333,334,343,344,373,374,403,404,433,434]]
#+end_src
Grids can be expressed as graphs too. I'll now define a new *graph* called =SquareGrid=, with *locations* tuples (int, int). Instead of storing the edges explicitly, I'll calculate them in the =neighbors= function.
#+begin_src python :tangle yes :exports code :results none :main no
class SquareGrid:
def __init__(self, width, height):
self.width = width
self.height = height
self.walls = []
def in_bounds(self, id):
(x, y) = id
return 0 <= x < self.width and 0 <= y < self.height
def passable(self, id):
return id not in self.walls
def neighbors(self, id):
(x, y) = id
results = [(x+1, y), (x, y-1), (x-1, y), (x, y+1)]
if (x + y) % 2 == 0: results.reverse() # aesthetics
results = filter(self.in_bounds, results)
results = filter(self.passable, results)
return results
#+end_src
Let's try it out with the first grid in the main article:
#+begin_src python
from implementation import *
g = SquareGrid(30, 15)
g.walls = DIAGRAM1_WALLS # long list, [(21, 0), (21, 2), ...]
draw_grid(g)
#+end_src
#+RESULTS:
#+BEGIN_example
. . . . . . . . . . . . . . . . . . . . . ####. . . . . . .
. . . . . . . . . . . . . . . . . . . . . ####. . . . . . .
. . . . . . . . . . . . . . . . . . . . . ####. . . . . . .
. . . ####. . . . . . . . . . . . . . . . ####. . . . . . .
. . . ####. . . . . . . . ####. . . . . . ####. . . . . . .
. . . ####. . . . . . . . ####. . . . . . ##########. . . .
. . . ####. . . . . . . . ####. . . . . . ##########. . . .
. . . ####. . . . . . . . ####. . . . . . . . . . . . . . .
. . . ####. . . . . . . . ####. . . . . . . . . . . . . . .
. . . ####. . . . . . . . ####. . . . . . . . . . . . . . .
. . . ####. . . . . . . . ####. . . . . . . . . . . . . . .
. . . ####. . . . . . . . ####. . . . . . . . . . . . . . .
. . . . . . . . . . . . . ####. . . . . . . . . . . . . . .
. . . . . . . . . . . . . ####. . . . . . . . . . . . . . .
. . . . . . . . . . . . . ####. . . . . . . . . . . . . . .
#+END_example
In order to reconstruct paths we need to store the location of where we came from, so I've renamed =visited= (True/False) to =came_from= (location):
#+begin_src python
from implementation import *
def breadth_first_search_2(graph, start):
# return "came_from"
frontier = Queue()
frontier.put(start)
came_from = {}
came_from[start] = None
while not frontier.empty():
current = 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
parents = breadth_first_search_2(g, (8, 7))
draw_grid(g, width=2, point_to=parents, start=(8, 7))
#+end_src
#+RESULTS:
#+BEGIN_example
→ → → → ↓ ↓ ↓ ↓ ↓ ↓ ↓ ↓ ↓ ↓ ↓ ↓ ← ← ← ← ← ####↓ ↓ ↓ ↓ ↓ ↓ ↓
→ → → → → ↓ ↓ ↓ ↓ ↓ ↓ ↓ ↓ ↓ ↓ ← ← ← ← ← ← ####↓ ↓ ↓ ↓ ↓ ↓ ↓
→ → → → → ↓ ↓ ↓ ↓ ↓ ↓ ↓ ↓ ↓ ← ← ← ← ← ← ← ####→ ↓ ↓ ↓ ↓ ↓ ↓
→ → ↑ ####↓ ↓ ↓ ↓ ↓ ↓ ↓ ↓ ← ← ← ← ← ← ← ← ####→ → ↓ ↓ ↓ ↓ ↓
→ ↑ ↑ ####→ ↓ ↓ ↓ ↓ ↓ ↓ ← ####↑ ← ← ← ← ← ####→ → → ↓ ↓ ↓ ↓
↑ ↑ ↑ ####→ → ↓ ↓ ↓ ↓ ← ← ####↑ ↑ ← ← ← ← ##########↓ ↓ ↓ ←
↑ ↑ ↑ ####→ → → ↓ ↓ ← ← ← ####↑ ↑ ↑ ← ← ← ##########↓ ↓ ← ←
↑ ↑ ↑ ####→ → → A ← ← ← ← ####↑ ↑ ↑ ↑ ← ← ← ← ← ← ← ← ← ← ←
↓ ↓ ↓ ####→ → ↑ ↑ ↑ ← ← ← ####↑ ↑ ↑ ↑ ↑ ← ← ← ← ← ← ← ← ← ←
↓ ↓ ↓ ####→ ↑ ↑ ↑ ↑ ↑ ← ← ####↑ ↑ ↑ ↑ ↑ ↑ ← ← ← ← ← ← ← ← ←
↓ ↓ ↓ ####↑ ↑ ↑ ↑ ↑ ↑ ↑ ← ####↑ ↑ ↑ ↑ ↑ ↑ ↑ ← ← ← ← ← ← ← ←
→ ↓ ↓ ####↑ ↑ ↑ ↑ ↑ ↑ ↑ ↑ ####↑ ↑ ↑ ↑ ↑ ↑ ↑ ↑ ← ← ← ← ← ← ←
→ → → → → ↑ ↑ ↑ ↑ ↑ ↑ ↑ ↑ ####↑ ↑ ↑ ↑ ↑ ↑ ↑ ↑ ↑ ← ← ← ← ← ←
→ → → → ↑ ↑ ↑ ↑ ↑ ↑ ↑ ↑ ↑ ####↑ ↑ ↑ ↑ ↑ ↑ ↑ ↑ ↑ ↑ ← ← ← ← ←
→ → → ↑ ↑ ↑ ↑ ↑ ↑ ↑ ↑ ↑ ↑ ####↑ ↑ ↑ ↑ ↑ ↑ ↑ ↑ ↑ ↑ ↑ ← ← ← ←
#+END_example
Some implementations use /internal storage/, creating a Node object to hold =came_from= and other values for each graph node. I've instead chosen to use /external storage/, creating a single hash table to store the =came_from= for all graph nodes. If you know your map locations have integer indices, another option is to use an array to store =came_from=.
** Early Exit
:PROPERTIES:
:CUSTOM_ID: python-early-exit
:END:
Following the code from the main article, all we need to do is add an /if/ statement in 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*:
#+begin_src python :prologue
from implementation import *
def breadth_first_search_3(graph, start, goal):
frontier = Queue()
frontier.put(start)
came_from = {}
came_from[start] = None
while not frontier.empty():
current = frontier.get()
if current == goal:
break
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
parents = breadth_first_search_3(g, (8, 7), (17, 2))
draw_grid(g, width=2, point_to=parents, start=(8, 7), goal=(17, 2))
#+end_src
#+RESULTS:
#+BEGIN_example
. → → → ↓ ↓ ↓ ↓ ↓ ↓ ↓ ↓ ↓ ↓ ↓ ↓ ← . . . . ####. . . . . . .
→ → → → → ↓ ↓ ↓ ↓ ↓ ↓ ↓ ↓ ↓ ↓ ← ← ← . . . ####. . . . . . .
→ → → → → ↓ ↓ ↓ ↓ ↓ ↓ ↓ ↓ ↓ ← ← ← Z . . . ####. . . . . . .
→ → ↑ ####↓ ↓ ↓ ↓ ↓ ↓ ↓ ↓ ← ← ← ← ← ← . . ####. . . . . . .
. ↑ ↑ ####→ ↓ ↓ ↓ ↓ ↓ ↓ ← ####↑ ← ← . . . ####. . . . . . .
. . ↑ ####→ → ↓ ↓ ↓ ↓ ← ← ####↑ ↑ . . . . ##########. . . .
. . . ####→ → → ↓ ↓ ← ← ← ####↑ . . . . . ##########. . . .
. . . ####→ → → A ← ← ← ← ####. . . . . . . . . . . . . . .
. . . ####→ → ↑ ↑ ↑ ← ← ← ####. . . . . . . . . . . . . . .
. . ↓ ####→ ↑ ↑ ↑ ↑ ↑ ← ← ####. . . . . . . . . . . . . . .
. ↓ ↓ ####↑ ↑ ↑ ↑ ↑ ↑ ↑ ← ####. . . . . . . . . . . . . . .
→ ↓ ↓ ####↑ ↑ ↑ ↑ ↑ ↑ ↑ ↑ ####. . . . . . . . . . . . . . .
→ → → → → ↑ ↑ ↑ ↑ ↑ ↑ ↑ ↑ ####. . . . . . . . . . . . . . .
→ → → → ↑ ↑ ↑ ↑ ↑ ↑ ↑ ↑ ↑ ####. . . . . . . . . . . . . . .
. → → ↑ ↑ ↑ ↑ ↑ ↑ ↑ ↑ ↑ ↑ ####. . . . . . . . . . . . . . .
#+END_example
You can see that the algorithm stops when it finds the goal =Z=.
** Dijkstra's Algorithm
:PROPERTIES:
:CUSTOM_ID: python-dijkstra
:END:
This 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. The /graph/ needs to know cost of movement.
2. The /queue/ needs to return nodes in a different order.
3. The /search/ needs to keep track of these costs from the graph and give them to the queue.
*** Graph with weights
I'm going to add a =cost(from_node, to_node)= function that tells us the cost of moving from location =from_node= to its neighbor =to_node=. In this forest map I chose to make movement depend only on =to_node=, but [[http://theory.stanford.edu/~amitp/GameProgramming/MovementCosts.html][there are other types of movement that use both nodes]]. An alternate implementation would be to merge this into the =neighbors= function.
#+begin_src python :tangle yes :exports code :results none :main no
class GridWithWeights(SquareGrid):
def __init__(self, width, height):
super().__init__(width, height)
self.weights = {}
def cost(self, from_node, to_node):
return self.weights.get(to_node, 1)
#+end_src
#+begin_src python :tangle yes :exports none :results none :main no
diagram4 = GridWithWeights(10, 10)
diagram4.walls = [(1, 7), (1, 8), (2, 7), (2, 8), (3, 7), (3, 8)]
diagram4.weights = {loc: 5 for loc in [(3, 4), (3, 5), (4, 1), (4, 2),
(4, 3), (4, 4), (4, 5), (4, 6),
(4, 7), (4, 8), (5, 1), (5, 2),
(5, 3), (5, 4), (5, 5), (5, 6),
(5, 7), (5, 8), (6, 2), (6, 3),
(6, 4), (6, 5), (6, 6), (6, 7),
(7, 3), (7, 4), (7, 5)]}
#+end_src
*** Queue with priorities
A priority queue associates with each item a number called a “priority”. When returning an item, it picks the one with the lowest number.
- insert :: Add item to queue
- remove :: Remove item with the lowest number
- reprioritize :: (optional) Change an existing item's priority to a lower 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.
#+begin_src python :tangle yes :exports code :results none :main no
import heapq
class PriorityQueue:
def __init__(self):
self.elements = []
def empty(self):
return len(self.elements) == 0
def put(self, item, priority):
heapq.heappush(self.elements, (priority, item))
def get(self):
return heapq.heappop(self.elements)[1]
#+end_src
#+begin_src python :exports none
from implementation import *
pq = PriorityQueue()
pq.put('b', 5)
pq.put('c', 3)
pq.put('a', 1)
pq.put('b', 2) # duplicate
while not pq.empty():
print(pq.get())
#+end_src
#+results:
#+BEGIN_example
a
b
c
b
#+END_example
*** Search
Here's a tricky bit about the implementation: once we add movement costs it's possible to visit a location again, with a better =cost_so_far=. That means the line =if next not in came_from= won't work. Instead, have to check if the cost has gone down since the last time we visited. (In the original version of the article I wasn't checking this, but my code worked anyway; [[../posts/reprioritize.html][I wrote some notes about that bug]].)
This forest map is from [[./introduction.html#dijkstra][the main page]].
#+begin_src python :tangle yes :main no
def dijkstra_search(graph, start, goal):
frontier = PriorityQueue()
frontier.put(start, 0)
came_from = {}
cost_so_far = {}
came_from[start] = None
cost_so_far[start] = 0
while not frontier.empty():
current = 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
frontier.put(next, priority)
came_from[next] = current
return came_from, cost_so_far
#+end_src
Finally, after searching I need to build the path:
#+begin_src python :tangle yes :main no
def reconstruct_path(came_from, start, goal):
current = goal
path = [current]
while current != start:
current = came_from[current]
path.append(current)
path.append(start) # optional
path.reverse() # optional
return path
#+end_src
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 =came_from= map, which points to the previous node. When we reach start, we're done. It is the *backwards* path, so call =reverse()= at the end of =reconstruct_path= if you need it to be stored forwards. Sometimes it's actually more convenient to store it backwards. Sometimes it's useful to also store the start node in the list.
Let's try it out:
#+begin_src python
from implementation import *
came_from, cost_so_far = dijkstra_search(diagram4, (1, 4), (7, 8))
draw_grid(diagram4, width=3, point_to=came_from, start=(1, 4), goal=(7, 8))
print()
draw_grid(diagram4, width=3, number=cost_so_far, start=(1, 4), goal=(7, 8))
print()
draw_grid(diagram4, width=3, path=reconstruct_path(came_from, start=(1, 4), goal=(7, 8)))
#+end_src
#+RESULTS:
#+BEGIN_example
↓ ↓ ← ← ← ← ← ← ← ←
↓ ↓ ← ← ← ↑ ↑ ← ← ←
↓ ↓ ← ← ← ← ↑ ↑ ← ←
↓ ↓ ← ← ← ← ← ↑ ↑ .
→ A ← ← ← ← . . . .
↑ ↑ ← ← ← ← . . . .
↑ ↑ ← ← ← ← ← . . .
↑ #########↑ ← ↓ . . .
↑ #########↓ ↓ ↓ Z . .
↑ ← ← ← ← ← ← ← ← .
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 14 .
1 A 1 6 11 16 . . . .
2 1 2 7 12 17 . . . .
3 2 3 4 9 14 19 . . .
4 #########14 19 18 . . .
5 #########15 16 13 Z . .
6 7 8 9 10 11 12 13 14 .
. . . . . . . . . .
. . . . . . . . . .
. . . . . . . . . .
. . . . . . . . . .
@ @ . . . . . . . .
@ . . . . . . . . .
@ . . . . . . . . .
@ #########. . . . . .
@ #########. . @ @ . .
@ @ @ @ @ @ @ . . .
#+END_example
The line =if next not in cost_so_far or new_cost < cost_so_far[next]= could be simplified to =if new_cost < cost_so_far.get(next, Infinity)= but I didn't want to explain Python's =get()= in the main article so I left it as is. Another approach would be to use =collections.defaultdict= defaulting to infinity.
** A* Search
:PROPERTIES:
:CUSTOM_ID: python-astar
:END:
Both Greedy Best-First Search and A* use a heuristic function. The only difference is that A* uses both the heuristic and the ordering from Dijkstra's Algorithm. I'm going to show A* here.
#+begin_src python :tangle yes :main no
def heuristic(a, b):
(x1, y1) = a
(x2, y2) = b
return abs(x1 - x2) + abs(y1 - y2)
def a_star_search(graph, start, goal):
frontier = PriorityQueue()
frontier.put(start, 0)
came_from = {}
cost_so_far = {}
came_from[start] = None
cost_so_far[start] = 0
while not frontier.empty():
current = 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(goal, next)
frontier.put(next, priority)
came_from[next] = current
return came_from, cost_so_far
#+end_src
Let's try it out:
#+begin_src python
from implementation import *
came_from, cost_so_far = a_star_search(diagram4, (1, 4), (7, 8))
draw_grid(diagram4, width=3, point_to=came_from, start=(1, 4), goal=(7, 8))
print()
draw_grid(diagram4, width=3, number=cost_so_far, start=(1, 4), goal=(7, 8))
#+end_src
#+RESULTS:
#+BEGIN_example
. . . . . . . . . .
. ↓ ↓ ↓ . . . . . .
↓ ↓ ↓ ↓ ← . . . . .
↓ ↓ ↓ ← ← . . . . .
→ A ← ← ← . . . . .
→ ↑ ← ← ← . . . . .
→ ↑ ← ← ← ← . . . .
↑ #########↑ . ↓ . . .
↑ #########↓ ↓ ↓ Z . .
↑ ← ← ← ← ← ← ← . .
. . . . . . . . . .
. 3 4 5 . . . . . .
3 2 3 4 9 . . . . .
2 1 2 3 8 . . . . .
1 A 1 6 11 . . . . .
2 1 2 7 12 . . . . .
3 2 3 4 9 14 . . . .
4 #########14 . 18 . . .
5 #########15 16 13 Z . .
6 7 8 9 10 11 12 13 . .
#+END_example
*** Straighter paths
If 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 very often *it doesn't look good to you*. The quick hack is to [[http://theory.stanford.edu/~amitp/GameProgramming/Heuristics.html#breaking-ties][break the ties]], but it's not entirely satisfactory. The better approach is to [[../grids/algorithms.html][change the map representation]], which makes A* a lot faster, and also produces straighter, better looking paths. However, that only works for mostly-static maps where every step has the same movement cost. For the demos on my page, I'm using a quick hack, but it only works with my slow priority queue. If you switch to a faster priority queue you'll need a different quick hack.
* C++ Implementation
:PROPERTIES:
:CUSTOM_ID: cplusplus
:END:
Note: some of the sample code needs to include [[./implementation.cpp][redblobgames/pathfinding/a-star/implementation.cpp]] to run. I am using *C++11* for this code so some of it will need to be changed if you use an older version of the C++ standard.
** Breadth First Search
:PROPERTIES:
:CUSTOM_ID: cpp-breadth-first
:END:
First read the Python section; I have code here but I'm not including the same explanations. First, a generic graph class:
#+begin_src cpp :tangle yes :exports none :main no
/*
Sample code from http://www.redblobgames.com/pathfinding/
Copyright 2014 Red Blob Games
Feel free to use this code in your own projects, including commercial projects
License: Apache v2.0
*/
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
using std::unordered_map;
using std::unordered_set;
using std::array;
using std::vector;
using std::queue;
using std::priority_queue;
using std::pair;
using std::tuple;
using std::tie;
using std::string;
#+end_src
#+begin_src cpp :tangle yes :main no
template
struct SimpleGraph {
typedef L Location;
typedef typename vector::iterator iterator;
unordered_map > edges;
inline const vector neighbors(Location id) {
return edges[id];
}
};
#+end_src
and the same example graph used in the Python code above, using =char= for location labels:
[[file:implementation-example-graph.png]]
#+begin_src cpp :tangle yes :main no
SimpleGraph example_graph {{
{'A', {'B'}},
{'B', {'A', 'C', 'D'}},
{'C', {'A'}},
{'D', {'E', 'A'}},
{'E', {'B'}}
}};
#+end_src
Instead of defining a queue class, I'll use the one from the C++ standard library. Now we can try Breadth First Search:
#+begin_src cpp
#include "redblobgames/pathfinding/a-star/implementation.cpp"
template
void breadth_first_search(Graph graph, typename Graph::Location start) {
typedef typename Graph::Location Location;
queue frontier;
frontier.push(start);
unordered_set visited;
visited.insert(start);
while (!frontier.empty()) {
auto current = frontier.front();
frontier.pop();
std::cout << "Visiting " << current << std::endl;
for (auto& next : graph.neighbors(current)) {
if (!visited.count(next)) {
frontier.push(next);
visited.insert(next);
}
}
}
}
int main() {
breadth_first_search(example_graph, 'A');
}
#+end_src
#+RESULTS:
#+BEGIN_example
Visiting A
Visiting B
Visiting C
Visiting D
Visiting E
#+END_example
The code is slightly longer than the Python code, but it's not too bad.
How about square grids? I'll define another graph class. Note that it's not related to the previous graph class. I'm using templates, not inheritance. The graph only needs to provide a =Location= typedef and a =neighbors= function.
#+begin_src cpp :tangle yes :exports none :main no
// Helpers for SquareGrid::Location
// When using std::unordered_map, we need to have std::hash or
// provide a custom hash function in the constructor to unordered_map.
// Since I'm using std::unordered_map> I'm defining the
// hash function here. It would be nice if C++ automatically provided
// the hash function for tuple and pair, like Python does. It would
// also be nice if C++ provided something like boost::hash_combine. In
// any case, here's a simple hash function that combines x and y:
namespace std {
template <>
struct hash > {
inline size_t operator()(const tuple& location) const {
int x, y;
tie (x, y) = location;
return x * 1812433253 + y;
}
};
}
// For debugging it's useful to have an ostream::operator << so that we can write cout << foo
std::basic_iostream::basic_ostream& operator<<(std::basic_iostream::basic_ostream& out, tuple loc) {
int x, y;
tie (x, y) = loc;
out << '(' << x << ',' << y << ')';
return out;
}
// This outputs a grid. Pass in a distances map if you want to print
// the distances, or pass in a point_to map if you want to print
// arrows that point to the parent location, or pass in a path vector
// if you want to draw the path.
template
void draw_grid(const Graph& graph, int field_width,
unordered_map* distances=nullptr,
unordered_map* point_to=nullptr,
vector* path=nullptr) {
for (int y = 0; y != graph.height; ++y) {
for (int x = 0; x != graph.width; ++x) {
typename Graph::Location id {x, y};
std::cout << std::left << std::setw(field_width);
if (graph.walls.count(id)) {
std::cout << string(field_width, '#');
} else if (point_to != nullptr && point_to->count(id)) {
int x2, y2;
tie (x2, y2) = (*point_to)[id];
// TODO: how do I get setw to work with utf8?
if (x2 == x + 1) { std::cout << "\u2192 "; }
else if (x2 == x - 1) { std::cout << "\u2190 "; }
else if (y2 == y + 1) { std::cout << "\u2193 "; }
else if (y2 == y - 1) { std::cout << "\u2191 "; }
else { std::cout << "* "; }
} else if (distances != nullptr && distances->count(id)) {
std::cout << (*distances)[id];
} else if (path != nullptr && find(path->begin(), path->end(), id) != path->end()) {
std::cout << '@';
} else {
std::cout << '.';
}
}
std::cout << std::endl;
}
}
#+end_src
#+begin_src cpp :tangle yes :main no
struct SquareGrid {
typedef tuple Location;
static array DIRS;
int width, height;
unordered_set walls;
SquareGrid(int width_, int height_)
: width(width_), height(height_) {}
inline bool in_bounds(Location id) const {
int x, y;
tie (x, y) = id;
return 0 <= x && x < width && 0 <= y && y < height;
}
inline bool passable(Location id) const {
return !walls.count(id);
}
vector neighbors(Location id) const {
int x, y, dx, dy;
tie (x, y) = id;
vector results;
for (auto dir : DIRS) {
tie (dx, dy) = dir;
Location next(x + dx, y + dy);
if (in_bounds(next) && passable(next)) {
results.push_back(next);
}
}
if ((x + y) % 2 == 0) {
// aesthetic improvement on square grids
std::reverse(results.begin(), results.end());
}
return results;
}
};
array SquareGrid::DIRS {Location{1, 0}, Location{0, -1}, Location{-1, 0}, Location{0, 1}};
#+end_src
In the helper file =implementation.cpp= I defined a function to draw grids:
#+begin_src cpp :tangle yes :exports none :main no
void add_rect(SquareGrid& grid, int x1, int y1, int x2, int y2) {
for (int x = x1; x < x2; ++x) {
for (int y = y1; y < y2; ++y) {
grid.walls.insert(SquareGrid::Location { x, y });
}
}
}
SquareGrid make_diagram1() {
SquareGrid grid(30, 15);
add_rect(grid, 3, 3, 5, 12);
add_rect(grid, 13, 4, 15, 15);
add_rect(grid, 21, 0, 23, 7);
add_rect(grid, 23, 5, 26, 7);
return grid;
}
#+end_src
#+begin_src cpp
#include "redblobgames/pathfinding/a-star/implementation.cpp"
int main() {
SquareGrid grid = make_diagram1();
draw_grid(grid, 2);
}
#+end_src
#+RESULTS:
#+BEGIN_example
. . . . . . . . . . . . . . . . . . . . . ####. . . . . . .
. . . . . . . . . . . . . . . . . . . . . ####. . . . . . .
. . . . . . . . . . . . . . . . . . . . . ####. . . . . . .
. . . ####. . . . . . . . . . . . . . . . ####. . . . . . .
. . . ####. . . . . . . . ####. . . . . . ####. . . . . . .
. . . ####. . . . . . . . ####. . . . . . ##########. . . .
. . . ####. . . . . . . . ####. . . . . . ##########. . . .
. . . ####. . . . . . . . ####. . . . . . . . . . . . . . .
. . . ####. . . . . . . . ####. . . . . . . . . . . . . . .
. . . ####. . . . . . . . ####. . . . . . . . . . . . . . .
. . . ####. . . . . . . . ####. . . . . . . . . . . . . . .
. . . ####. . . . . . . . ####. . . . . . . . . . . . . . .
. . . . . . . . . . . . . ####. . . . . . . . . . . . . . .
. . . . . . . . . . . . . ####. . . . . . . . . . . . . . .
. . . . . . . . . . . . . ####. . . . . . . . . . . . . . .
#+END_example
Let's try Breadth First Search again, keeping track of =came_from=:
#+begin_src cpp
#include "redblobgames/pathfinding/a-star/implementation.cpp"
template
unordered_map
breadth_first_search(Graph graph, typename Graph::Location start) {
typedef typename Graph::Location Location;
queue frontier;
frontier.push(start);
unordered_map came_from;
came_from[start] = start;
while (!frontier.empty()) {
auto current = frontier.front();
frontier.pop();
for (auto& next : graph.neighbors(current)) {
if (!came_from.count(next)) {
frontier.push(next);
came_from[next] = current;
}
}
}
return came_from;
}
int main() {
SquareGrid grid = make_diagram1();
auto parents = breadth_first_search(grid, std::make_tuple(7, 8));
draw_grid(grid, 2, nullptr, &parents);
}
#+end_src
#+RESULTS:
#+BEGIN_example
→ → → → ↓ ↓ ↓ ↓ ↓ ↓ ↓ ↓ ↓ ↓ ↓ ↓ ← ← ← ← ← ####↓ ↓ ↓ ↓ ↓ ↓ ↓
→ → → → → ↓ ↓ ↓ ↓ ↓ ↓ ↓ ↓ ↓ ↓ ← ← ← ← ← ← ####↓ ↓ ↓ ↓ ↓ ↓ ↓
→ → → → → ↓ ↓ ↓ ↓ ↓ ↓ ↓ ↓ ↓ ← ← ← ← ← ← ← ####→ ↓ ↓ ↓ ↓ ↓ ↓
→ → ↑ ####↓ ↓ ↓ ↓ ↓ ↓ ↓ ↓ ← ← ← ← ← ← ← ← ####→ → ↓ ↓ ↓ ↓ ↓
→ ↑ ↑ ####↓ ↓ ↓ ↓ ↓ ↓ ↓ ← ####↑ ← ← ← ← ← ####→ → → ↓ ↓ ↓ ↓
↑ ↑ ↑ ####↓ ↓ ↓ ↓ ↓ ↓ ← ← ####↑ ↑ ← ← ← ← ##########↓ ↓ ↓ ←
↑ ↑ ↑ ####→ ↓ ↓ ↓ ↓ ← ← ← ####↑ ↑ ↑ ← ← ← ##########↓ ↓ ← ←
↓ ↓ ↓ ####→ → ↓ ↓ ← ← ← ← ####↑ ↑ ↑ ↑ ← ← ← ← ← ← ← ← ← ← ←
↓ ↓ ↓ ####→ → * ← ← ← ← ← ####↑ ↑ ↑ ↑ ↑ ← ← ← ← ← ← ← ← ← ←
↓ ↓ ↓ ####→ ↑ ↑ ↑ ← ← ← ← ####↑ ↑ ↑ ↑ ↑ ↑ ← ← ← ← ← ← ← ← ←
↓ ↓ ↓ ####↑ ↑ ↑ ↑ ↑ ← ← ← ####↑ ↑ ↑ ↑ ↑ ↑ ↑ ← ← ← ← ← ← ← ←
→ ↓ ↓ ####↑ ↑ ↑ ↑ ↑ ↑ ← ← ####↑ ↑ ↑ ↑ ↑ ↑ ↑ ↑ ← ← ← ← ← ← ←
→ → → → → ↑ ↑ ↑ ↑ ↑ ↑ ↑ ← ####↑ ↑ ↑ ↑ ↑ ↑ ↑ ↑ ↑ ← ← ← ← ← ←
→ → → → ↑ ↑ ↑ ↑ ↑ ↑ ↑ ↑ ↑ ####↑ ↑ ↑ ↑ ↑ ↑ ↑ ↑ ↑ ↑ ← ← ← ← ←
→ → → ↑ ↑ ↑ ↑ ↑ ↑ ↑ ↑ ↑ ↑ ####↑ ↑ ↑ ↑ ↑ ↑ ↑ ↑ ↑ ↑ ↑ ← ← ← ←
#+END_example
Some implementations use /internal storage/, creating a Node object to hold =came_from= and other values for each graph node. I've instead chosen to use /external storage/, creating a single =std::unordered_map= to store the =came_from= for all graph nodes. If you know your map locations have integer indices, another option is to use a 1D or 2D array/vector to store =came_from= and other values.
*** TODO Location struct
In this code I'm using C++ =std::tuple= because I used tuples in my Python code. However, tuples are unfamiliar to most C++ programmers. It'd be a little more /verbose/ to define a struct with {x,y}, because I would need to define the constructor, copy constructor, assignment operator, and equality comparison, along with a hash function, but that kind of code is /familar/ to most C++ programmers. I should change it.
Another option (which I use in my own code) is to encode {x,y} in an =int=. If the A* code always has a dense set of integers instead of arbitrary =Location= types, it enables some further optimizations. We can use an array for the various sets instead of hash tables. We can leave most of them uninitialized. For the arrays that have to be reinitialized each time, we can make them persistent across calls to A* (maybe using thread-local storage), reinitializing only the parts of the array that were used on the previous call. These are all more advanced techniques that I don't want to use in a beginner-level tutorial.
** Early Exit
:PROPERTIES:
:CUSTOM_ID: cpp-early-exit
:END:
As with the Python version, all we have to do is add a parameter to the function and a test to the main loop:
#+begin_src cpp
#include "redblobgames/pathfinding/a-star/implementation.cpp"
template
unordered_map
breadth_first_search(const Graph& graph,
typename Graph::Location start,
typename Graph::Location goal) {
typedef typename Graph::Location Location;
queue frontier;
frontier.push(start);
unordered_map came_from;
came_from[start] = start;
while (!frontier.empty()) {
auto current = frontier.front();
frontier.pop();
if (current == goal) {
break;
}
for (auto& next : graph.neighbors(current)) {
if (!came_from.count(next)) {
frontier.push(next);
came_from[next] = current;
}
}
}
return came_from;
}
int main() {
SquareGrid grid = make_diagram1();
auto parents = breadth_first_search(grid, SquareGrid::Location{8, 7}, SquareGrid::Location{17, 2});
draw_grid(grid, 2, nullptr, &parents);
}
#+end_src
#+RESULTS:
#+BEGIN_example
. → → → ↓ ↓ ↓ ↓ ↓ ↓ ↓ ↓ ↓ ↓ ↓ ↓ ← . . . . ####. . . . . . .
→ → → → → ↓ ↓ ↓ ↓ ↓ ↓ ↓ ↓ ↓ ↓ ← ← ← . . . ####. . . . . . .
→ → → → → ↓ ↓ ↓ ↓ ↓ ↓ ↓ ↓ ↓ ← ← ← ← . . . ####. . . . . . .
→ → ↑ ####↓ ↓ ↓ ↓ ↓ ↓ ↓ ↓ ← ← ← ← ← ← . . ####. . . . . . .
. ↑ ↑ ####→ ↓ ↓ ↓ ↓ ↓ ↓ ← ####↑ ← ← . . . ####. . . . . . .
. . ↑ ####→ → ↓ ↓ ↓ ↓ ← ← ####↑ ↑ . . . . ##########. . . .
. . . ####→ → → ↓ ↓ ← ← ← ####↑ . . . . . ##########. . . .
. . . ####→ → → * ← ← ← ← ####. . . . . . . . . . . . . . .
. . . ####→ → ↑ ↑ ↑ ← ← ← ####. . . . . . . . . . . . . . .
. . ↓ ####→ ↑ ↑ ↑ ↑ ↑ ← ← ####. . . . . . . . . . . . . . .
. ↓ ↓ ####↑ ↑ ↑ ↑ ↑ ↑ ↑ ← ####. . . . . . . . . . . . . . .
→ ↓ ↓ ####↑ ↑ ↑ ↑ ↑ ↑ ↑ ↑ ####. . . . . . . . . . . . . . .
→ → → → → ↑ ↑ ↑ ↑ ↑ ↑ ↑ ↑ ####. . . . . . . . . . . . . . .
→ → → → ↑ ↑ ↑ ↑ ↑ ↑ ↑ ↑ ↑ ####. . . . . . . . . . . . . . .
. → → ↑ ↑ ↑ ↑ ↑ ↑ ↑ ↑ ↑ ↑ ####. . . . . . . . . . . . . . .
#+END_example
** Dijkstra's Algorithm
:PROPERTIES:
:CUSTOM_ID: cpp-dijkstra
:END:
*** Graph with weights
Here's a grid with a list of forest tiles, which will have movement cost 5. In this forest map I chose to make movement depend only on =to_node=, but [[http://theory.stanford.edu/~amitp/GameProgramming/MovementCosts.html][there are other types of movement that use both nodes]].
#+begin_src cpp :tangle yes :main no
struct GridWithWeights: SquareGrid {
unordered_set forests;
GridWithWeights(int w, int h): SquareGrid(w, h) {}
double cost(Location from_node, Location to_node) const {
return forests.count(to_node) ? 5 : 1;
}
};
#+end_src
#+begin_src cpp :tangle yes :exports none :main no
GridWithWeights make_diagram4() {
GridWithWeights grid(10, 10);
add_rect(grid, 1, 7, 4, 9);
typedef SquareGrid::Location L;
grid.forests = unordered_set {
L{3, 4}, L{3, 5}, L{4, 1}, L{4, 2},
L{4, 3}, L{4, 4}, L{4, 5}, L{4, 6},
L{4, 7}, L{4, 8}, L{5, 1}, L{5, 2},
L{5, 3}, L{5, 4}, L{5, 5}, L{5, 6},
L{5, 7}, L{5, 8}, L{6, 2}, L{6, 3},
L{6, 4}, L{6, 5}, L{6, 6}, L{6, 7},
L{7, 3}, L{7, 4}, L{7, 5}
};
return grid;
}
#+end_src
*** Queue with priorities
We need a priority queue. C++ offers a =priority_queue= class that uses a binary heap but not the reprioritize operation. I'll use a pair (priority, item) for the queue elements to get the right ordering. By default, the C++ priority queue returns the maximum element first, using the =std::less= comparator; we want the minimum element instead, so I'll use the =std::greater= comparator.
#+begin_src cpp :tangle yes :main no
template
struct PriorityQueue {
typedef pair PQElement;
priority_queue,
std::greater> elements;
inline bool empty() const { return elements.empty(); }
inline void put(T item, priority_t priority) {
elements.emplace(priority, item);
}
inline T get() {
T best_item = elements.top().second;
elements.pop();
return best_item;
}
};
#+end_src
#+begin_src cpp :exports none
#include "redblobgames/pathfinding/a-star/implementation.cpp"
int main() {
PriorityQueue pq;
pq.put('b', 5);
pq.put('c', 3);
pq.put('a', 1);
pq.put('b', 2); // reprioritize
while (!pq.empty()) {
std::cout << pq.get() << std::endl;
}
}
#+end_src
#+RESULTS:
#+BEGIN_example
a
b
c
b
#+END_example
In this sample code I'm wrapping the C++ =std::priority_queue= class but I think it'd be reasonable to use that class directly without the wrapper.
*** Search
See [[./introduction.html#dijkstra][the forest map from the main page]].
#+begin_src cpp :tangle yes :main no
template
void dijkstra_search
(const Graph& graph,
typename Graph::Location start,
typename Graph::Location goal,
unordered_map& came_from,
unordered_map& cost_so_far)
{
typedef typename Graph::Location Location;
PriorityQueue frontier;
frontier.put(start, 0);
came_from[start] = start;
cost_so_far[start] = 0;
while (!frontier.empty()) {
auto current = frontier.get();
if (current == goal) {
break;
}
for (auto& next : graph.neighbors(current)) {
double new_cost = cost_so_far[current] + graph.cost(current, next);
if (!cost_so_far.count(next) || new_cost < cost_so_far[next]) {
cost_so_far[next] = new_cost;
came_from[next] = current;
frontier.put(next, new_cost);
}
}
}
}
#+end_src
The types of the =cost= variables should all match the types used in the graph. If you use =int= then you can use =int= for the cost variable and the priorities in the priority queue; if you use =double= then you should use =double= for these. In this code I used =double= but I could've used =int= and it would've worked the same. However, if your graph edge costs are doubles or if your heuristic uses doubles, then you'll need to use doubles here.
Finally, after searching I need to build the path:
#+begin_src cpp :tangle yes :main no
template
vector reconstruct_path(
Location start,
Location goal,
unordered_map& came_from
) {
vector path;
Location current = goal;
path.push_back(current);
while (current != start) {
current = came_from[current];
path.push_back(current);
}
path.push_back(start); // optional
std::reverse(path.begin(), path.end());
return path;
}
#+end_src
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 =came_from= map, which points to the previous node. When we reach start, we're done. It is the *backwards* path, so call =reverse()= at the end of =reconstruct_path= if you need it to be stored forwards. Sometimes it's actually more convenient to store it backwards. Sometimes it's useful to also store the start node in the list.
Let's try it out:
#+begin_src cpp
#include "redblobgames/pathfinding/a-star/implementation.cpp"
int main() {
GridWithWeights grid = make_diagram4();
SquareGrid::Location start{1, 4};
SquareGrid::Location goal{8, 5};
unordered_map came_from;
unordered_map cost_so_far;
dijkstra_search(grid, start, goal, came_from, cost_so_far);
draw_grid(grid, 2, nullptr, &came_from);
std::cout << std::endl;
draw_grid(grid, 3, &cost_so_far, nullptr);
std::cout << std::endl;
vector path = reconstruct_path(start, goal, came_from);
draw_grid(grid, 3, nullptr, nullptr, &path);
}
#+end_src
#+RESULTS:
#+BEGIN_example
↓ ↓ ← ← ← ← ← ← ← ←
↓ ↓ ← ← ← ↑ ↑ ← ← ←
↓ ↓ ← ← ← ← ↑ ↑ ← ←
↓ ↓ ← ← ← ← ← ↑ ↑ ←
→ * ← ← ← ← ← → ↑ ←
↑ ↑ ← ← ← ← . ↓ ↑ .
↑ ↑ ← ← ← ← ← ↓ ← .
↑ ######↑ ← ↓ ↓ ← .
↑ ######↓ ↓ ↓ ← ← ←
↑ ← ← ← ← ← ← ← ← ←
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 14 15
1 0 1 6 11 16 21 20 15 16
2 1 2 7 12 17 . 21 16 .
3 2 3 4 9 14 19 16 17 .
4 #########14 19 18 15 16 .
5 #########15 16 13 14 15 16
6 7 8 9 10 11 12 13 14 15
. @ @ @ @ @ @ . . .
. @ . . . . @ @ . .
. @ . . . . . @ @ .
. @ . . . . . . @ .
. @ . . . . . . @ .
. . . . . . . . @ .
. . . . . . . . . .
. #########. . . . . .
. #########. . . . . .
. . . . . . . . . .
#+END_example
The results are not exactly the same as the Python version because I'm using the built-in hash tables for the priority queue, and the order of iteration over hash tables is not going to be consistent.
** A* Search
:PROPERTIES:
:CUSTOM_ID: cpp-astar
:END:
A* 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 (=SquareGrids= in this case) and in the =heuristic= function. Replace those two and you can use the A* algorithm code with any other graph structure.
#+begin_src cpp :tangle yes :main no
inline double heuristic(SquareGrid::Location a, SquareGrid::Location b) {
int x1, y1, x2, y2;
tie (x1, y1) = a;
tie (x2, y2) = b;
return abs(x1 - x2) + abs(y1 - y2);
}
template
void a_star_search
(const Graph& graph,
typename Graph::Location start,
typename Graph::Location goal,
unordered_map& came_from,
unordered_map& cost_so_far)
{
typedef typename Graph::Location Location;
PriorityQueue frontier;
frontier.put(start, 0);
came_from[start] = start;
cost_so_far[start] = 0;
while (!frontier.empty()) {
auto current = frontier.get();
if (current == goal) {
break;
}
for (auto& next : graph.neighbors(current)) {
double new_cost = cost_so_far[current] + graph.cost(current, next);
if (!cost_so_far.count(next) || 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;
}
}
}
}
#+end_src
The type of the =priority= values including the type used in the priority queue should be big enough to include both the graph costs (=cost_t=) and the heuristic value. For example, if the graph costs are ints and the heuristic returns a double, then you need the priority queue to accept doubles. In this sample code I use =double= for all three (cost, heuristic, and priority), but I could've used =int= because my costs and heuristics are integer valued.
Minor note: It would be more correct to write =frontier.put(start, heuristic(start, goal))= than =frontier.put(start, 0)= but it makes no difference here because the start node's priority doesn't matter. It is the only node in the priority queue and it is selected and removed before anything else is put in there.
#+begin_src cpp
#include "redblobgames/pathfinding/a-star/implementation.cpp"
int main() {
GridWithWeights grid = make_diagram4();
SquareGrid::Location start{1, 4};
SquareGrid::Location goal{8, 5};
unordered_map came_from;
unordered_map cost_so_far;
a_star_search(grid, start, goal, came_from, cost_so_far);
draw_grid(grid, 2, nullptr, &came_from);
std::cout << std::endl;
draw_grid(grid, 3, &cost_so_far, nullptr);
std::cout << std::endl;
vector path = reconstruct_path(start, goal, came_from);
draw_grid(grid, 3, nullptr, nullptr, &path);
}
#+end_src
#+RESULTS:
#+BEGIN_example
↓ ↓ ↓ ↓ ← ← ← ← ← ←
↓ ↓ ↓ ↓ ← ↑ ↑ ← ← ←
↓ ↓ ↓ ↓ ← ← ↑ ↑ ← ←
↓ ↓ ↓ ← ← ← . ↑ ↑ ←
→ * ← ← ← ← . → ↑ ←
→ ↑ ← ← ← ← . . ↑ .
↑ ↑ ↑ ← ← ← . . . .
↑ ######↑ . . . . .
↑ ######. . . . . .
↑ . . . . . . . . .
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 14 15
1 0 1 6 11 16 . 20 15 16
2 1 2 7 12 17 . . 16 .
3 2 3 4 9 14 . . . .
4 #########14 . . . . .
5 #########. . . . . .
6 . . . . . . . . .
. . . @ @ @ @ . . .
. . . @ . . @ @ . .
. . . @ . . . @ @ .
. . @ @ . . . . @ .
. @ @ . . . . . @ .
. . . . . . . . @ .
. . . . . . . . . .
. #########. . . . . .
. #########. . . . . .
. . . . . . . . . .
#+END_example
*** Straighter paths
If 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 very often *it doesn't look good to you*. The quick hack is to [[http://theory.stanford.edu/~amitp/GameProgramming/Heuristics.html#breaking-ties][break the ties]], but it's not entirely satisfactory. The better approach is to [[../grids/algorithms.html][change the map representation]], which makes A* a lot faster, and also produces straighter, better looking paths. However, that only works for mostly-static maps where every step has the same movement cost. For the demos on my page, I'm using a quick hack, but it only works with my slow priority queue. If you switch to a faster priority queue you'll need a different quick hack.
*** TODO The neighbors() vector should be passed in
Instead of neighbors allocating a new vector each time and returning it, the A* code should allocate one vector and pass it into the neighbors function each time. This makes the code significantly faster in some of my tests.
*** TODO Remove template parameterization
The goal of this page is to have easy to understand code, and I think the template parameterization makes it overly hard to read. I could use some typedefs instead.
*** TODO Add requirements
There are two different types of graphs (unweighted and weighted) and the graph search code isn't saying which is needed where.
* C# Implementation
:PROPERTIES:
:CUSTOM_ID: csharp
:END:
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:
#+include: "~/Sites/redblobgames/pathfinding/a-star/cs/Graph.cs" src csharp
Here's a graph representing a grid with weighted edges (the forest and walls example from the main page):
#+include: "~/Sites/redblobgames/pathfinding/a-star/cs/AStar.cs" src csharp
* Optimizations
:PROPERTIES:
:CUSTOM_ID: optimizations
:END:
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:
** Graph
:PROPERTIES:
:CUSTOM_ID: optimize-graph
:END:
The biggest optimization you can make is to explore fewer nodes. My #1 recommendation is that if you're using a grid map, [[../grids/algorithms.html][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.
** Queue
:PROPERTIES:
:CUSTOM_ID: optimize-queue
:END:
Breadth 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 [[https://docs.python.org/3/library/collections.html][collections.deque]]; in C++, see the [[http://en.cppreference.com/w/cpp/container/deque][deque]] container. However, breadth first search doesn't even need a queue; it can use two vectors, swapping them when one is empty.
For 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 [[https://docs.python.org/2/library/heapq.html][heapq]]; in C++, see the [[http://en.cppreference.com/w/cpp/container/priority_queue][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 =cost_so_far=, so you could write a priority queue that gets priorities from elsewhere. I'm not sure if it's worth it.
The standard way Dijkstra's Algorithm is presented uses a reprioritizing priority queue. However, *what happens if you don't reprioritize?* You'll end up with duplicate elements in there. /The algorithm still works./ You'll revisit some locations more than you need to. The priority queue will have more elements than necessary, which slows it down, but the data structures that support reprioritization also slow down by having more elements. See the paper [[http://www.cs.sunysb.edu/~rezaul/papers/TR-07-54.pdf]["Priority Queues and Dijkstra’s Algorithm"]] by Chen, Chowdhury, Ramachandran, Lan Roche, Tong. That paper also suggests looking at [[http://en.wikipedia.org/wiki/Pairing_heap][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 =f=, then the highest element has priority =f+e= where =e= is the maximum edge weight. In the forest example, I have edge weights 1 and 5. That means all the priorities in the queue are going to be between =f= and =f+5=. Since they're all integers, /there are only six different priorities/. We could use six buckets and not sort anything at all! A* produces a wider range of priorities but it's still worth looking at. And there are fancier bucketing approaches that handle a wider range of situations.
[[http://theory.stanford.edu/~amitp/GameProgramming/ImplementationNotes.html#set-representation][I have more note about priority queue data structures here]].
** Search
:PROPERTIES:
:CUSTOM_ID: optimize-search
:END:
The 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.
If your graph uses integers as locations, consider using a simple array instead of a hash table for =cost_so_far=, =visited=, =came_from=, etc. Since =visited= is an array of booleans, you can use a bit vector. Initialize the bit vector for all ids, but leave =cost_so_far= and =came_from= uninitialized (if your language allows). Then only initialize on the first visit.
If you run only one search at a time, you can statically allocate and then reuse data structures from one invocation to the next. Instead of initializing them on entry, reset them on exit. You can use an array to keep track of which locations were modified, then reset only those. For example, if you're using a =visited[]= array initialized for 1000 nodes, but most searches visit fewer than 100 nodes, then you can keep an array of indices modified, and then reset only those 100 nodes on exiting the function instead of re-initializing all 1000 nodes when you enter the function.
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-visited 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 (=new_cost < cost_so_far=). The test is cheap because I made it cheap to look up =cost_so_far=.
* Troubleshooting
:PROPERTIES:
:CUSTOM_ID: troubleshooting
:END:
** Wrong path
:PROPERTIES:
:CUSTOM_ID: troubleshooting-wrong-path
:END:
If you're not getting a shortest path, try testing:
- Does your priority queue work correctly? Try stopping the search and dequeuing all the elements. They should all be in order.
- Does your heuristic ever overestimate the true distance? The =priority= of a new node should never be lower than the priority of its parent, unless you are overestimating the distance (you can do this but you won't get shortest paths anymore).
- In a statically typed language, the cost, heuristic, and priority values need to have compatible types. The sample code on this page works with either integers or floating point types, but not all graphs and heuristics are limited to integer values. Since priorities are the sum of costs and heuristics, the priorities will need to be floating point if /either/ costs or heuristics are floating point.
** Ugly path
:PROPERTIES:
:CUSTOM_ID: troubleshooting-ugly-path
:END:
The most common question I get when people run A* on a grid is /why don't my paths look straight?/ If you've told A* that all grid movements are equal, then there are lots of shortest paths of the same length, and it's going to pick one arbitrarily. The path is /short/ but it doesn't /look/ good.
- One solution is to /straighten/ the paths afterwards, using a "string pulling" algorithm.
- One solution is to /guide/ the paths in the right direction, by adjusting the heuristic. There are some cheap tricks that don't work in all situations; [[http://theory.stanford.edu/~amitp/GameProgramming/Heuristics.html#breaking-ties][read more here]].
- One solution is to /not use a grid/. Tell A* just the places where you might turn, instead of every grid square; [[../grids/algorithms.html][read more here]].
- One solution is a hack, but it works some of the time. When iterating through neighbors, instead of always using the same ordering (such as north, east, south, west), change the ordering on "odd" grid nodes (those where (x+y) % 2 == 1). *I use this trick on these tutorial pages.*
* More reading
:PROPERTIES:
:CUSTOM_ID: more
:END:
- Algorithms textbooks often use mathematical notation with single-letter variable names. In these pages I've tried to use more descriptive variable names. Correspondences:
- =cost= is sometimes written as /w/ or /d/ or /l/ or length
- =cost_so_far= is usually written as /g/ or /d/ or distance
- =heuristic= is usually written as /h/
- In A*, the =priority= is usually written as /f/, where /f/ = /g/ + /h/
- =came_from= is sometimes written as /π/ or parent or previous or prev
- =frontier= is usually called OPEN
- =visited= is the union of OPEN and CLOSED
- locations such as =current= and =next= are written with letters /u/, /v/
- Wikipedia links:
- [[http://en.wikipedia.org/wiki/Queue_(abstract_data_type)][Queue]]
- [[http://en.wikipedia.org/wiki/Graph_(data_structure)][Graph]]
- [[http://en.wikipedia.org/wiki/Breadth-first_search][Breadth-First Search]]
- (Greedy) [[http://en.wikipedia.org/wiki/Best-first_search][Best-First Search]]
- [[http://en.wikipedia.org/wiki/Dijkstra's_algorithm][Dijkstra's Algorithm]]
- [[http://en.wikipedia.org/wiki/A*_search_algorithm][A* Algorithm]]
#+begin_footer
Created {{{date(%e %b %Y)}}} with [[http://orgmode.org/][Emacs Org-mode]], from [[./implementation.org][implementation.org]]. Last modified: {{{modification-time(%e %b %Y)}}}
#+end_footer