#+TITLE: Implementation of A*
# #+DATE: <2014-07-06>
#+OPTIONS: toc:2
#+PROPERTY: header-args :exports both :results output :wrap example :eval never-export
#+filetags: pathfinding a-star
#+updated: <2020-10>

#+begin_export html
<address>Jul 2014, then Feb 2016, Nov 2018, Oct 2020</address>
#+end_export

#+begin_export html
<style>
    /* Need 80 columns for code, 90 for output, and with these extra margins it was too small, so I decreased font size. */
    pre {
        font-size: 0.8em;
        letter-spacing: -0.05em;
    }
    pre.src {
        white-space: pre;
        border: 0.5em solid hsl(60, 20%, 95%);
        padding: 0.5em 1em;
        margin-bottom: 0;
        box-shadow: inset 0 0 0.125em 1px hsla(60, 10%, 50%, 0.5);
    }
    pre.example {
        font-size: 0.7em;
        line-height: 1.2;
        background: hsl(60, 20%, 95%);
        padding-left: 1em;
        padding-bottom: 0.25em;
        border-top: none;
        border-bottom: none;
        white-space: pre;
    }

    /* "Syntax highlighting" for output ascii grids */
    pre.example span { display: inline-block; width: 2.75ch; text-align: center; color: hsl(220, 10%, 50%); }
    pre.example strong { background: hsl(30, 20%, 85%); color: hsl(30, 20%, 60%); }
    pre.example b { background: hsl(180, 20%, 85%); }
    pre.example i { background: hsl(180, 100%, 80%); }

    /* Make the code sections wider to handle 80 columns */
    section > .org-src-container, .org-src-container + pre.example {
        width: calc(1.1 * var(--body-width));
    }
    .org-src-container > pre {
        width: unset;
    }

    /* Remove the margin between the input and output sections */
    .org-src-container + pre.example {
        margin-top: 0;
    }
</style>
#+end_export

#+begin_note
This article is a companion guide to my [[./introduction.html][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.
#+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 remove type annotations, change the =super()= call, and change the =print= function to work with Python 2.

** 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 graph location (see [[../grids/graphs.html][this tutorial]]). A /weighted/ graph also gives a =cost= of moving along an edge.
- Locations :: a simple value (int, string, tuple, etc.) that /labels/ locations in the graph. These are not necessarily locations on the map. They may include additional information such as direction, fuel, lane, or inventory, depending on the problem being solved.
- Search :: an algorithm that takes a graph, a starting graph location, and optionally a goal graph location, and calculates some useful information (reached, parent pointer, distance) for some or all graph locations.
- Queue :: a data structure used by the search algorithm to decide the order in which to process the graph locations.

#+begin_src python :tangle yes :exports none :main no
# Sample code from https://www.redblobgames.com/pathfinding/a-star/
# Copyright 2014 Red Blob Games <redblobgames@gmail.com>
#
# Feel free to use this code in your own projects, including commercial projects
# License: Apache v2.0 <http://www.apache.org/licenses/LICENSE-2.0.html>

from __future__ import annotations
# some of these types are deprecated: https://www.python.org/dev/peps/pep-0585/
from typing import Protocol, Iterator, Tuple, TypeVar, Optional
T = TypeVar('T')
#+end_src

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:

#+begin_src python :tangle yes :exports code :results none :main no
Location = TypeVar('Location')
class Graph(Protocol):
    def neighbors(self, id: Location) -> list[Location]: pass
#+end_src

I'm using Python's [[https://peps.python.org/pep-0526/][type hints]] to try to make it easier to understand which variables hold a =list=, a =dict=, a =Location=, etc. =Graph= is the /interface/ that the search algorithms will want. Here's an implementation go to with it:

#+begin_src python :tangle yes :exports code :results none :main no
class SimpleGraph:
    def __init__(self):
        self.edges: dict[Location, list[Location]] = {}
    
    def neighbors(self, id: Location) -> list[Location]:
        return self.edges[id]
#+end_src

Yes, 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 =Location= type, and then use arrays (Python lists) or hash tables (Python dicts) 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 start with an example map with both two-way and one-way links:

#+include: "implementation-example-graph.svg" export html

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 =Location= type is a letter A, B, C, D, E, or F.

#+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 -> C; C -> B;
  C -> D; D -> C;
  C -> F;
  D -> E;
  E -> F;
}
#+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': ['C'],
    'C': ['B', 'D', 'F'],
    'D': ['C', 'E'],
    'E': ['F'],
    'F': [],
}
#+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) -> bool:
        return not self.elements
    
    def put(self, x: T):
        self.elements.append(x)
    
    def get(self) -> T:
        return self.elements.popleft()
#+end_src

This queue class is 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(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')
#+end_src

#+RESULTS:
#+begin_example
Reachable from A:
  Visiting A
  Visiting B
  Visiting C
  Visiting D
  Visiting F
  Visiting E
Reachable from E:
  Visiting E
  Visiting F
#+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):
    r = " . "
    if 'number' in style and id in style['number']: r = " %-2d" % 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 = " > "
        if x2 == x1 - 1: r = " < "
        if y2 == y1 + 1: r = " v "
        if y2 == y1 - 1: r = " ^ "
    if 'path' in style and id in style['path']:   r = " @ "
    if 'start' in style and id == style['start']: r = " A "
    if 'goal' in style and id == style['goal']:   r = " Z "
    if id in graph.walls: r = "###"
    return r

def draw_grid(graph, **style):
    print("___" * graph.width)
    for y in range(graph.height):
        for x in range(graph.width):
            print("%s" % draw_tile(graph, (x, y), style), end="")
        print()
    print("~~~" * graph.width)

# 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 =GridLocation= being a tuple =(x: int, y: int)=. In this map, the locations ("states") in the graph are the same as locations on the game map, but in many problems graph locations are not the same as map locations. Instead of storing the edges explicitly, I'll calculate them in the =neighbors= function. In many problems it's better to store them explicitly.

#+begin_src python :tangle yes :exports code :results none :main no
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
#+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 =reached= (True/False) to =came_from= (location):

#+begin_src python
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)
#+end_src
#+RESULTS:
#+begin_example
__________________________________________________________________________________________
 >  >  >  v  v  v  v  v  v  v  v  v  v  v  v  v  <  <  <  <  < ###### v  v  v  v  v  v  v 
 >  >  >  >  v  v  v  v  v  v  v  v  v  v  v  <  <  <  <  <  < ###### >  v  v  v  v  v  v 
 >  >  >  >  >  v  v  v  v  v  v  v  v  v  <  <  <  <  <  <  < ###### >  >  v  v  v  v  v 
 >  >  ^ ###### v  v  v  v  v  v  v  v  <  <  <  <  <  <  <  < ###### >  >  >  v  v  v  v 
 >  ^  ^ ###### v  v  v  v  v  v  v  < ###### ^  <  <  <  <  < ###### >  >  >  v  v  v  v 
 ^  ^  ^ ###### >  v  v  v  v  v  <  < ###### ^  ^  <  <  <  < ############### v  v  v  < 
 ^  ^  ^ ###### >  >  v  v  v  <  <  < ###### ^  ^  ^  <  <  < ############### v  v  <  < 
 v  v  v ###### >  >  >  A  <  <  <  < ###### ^  ^  ^  ^  <  <  <  <  <  <  <  <  <  <  < 
 v  v  v ###### >  >  ^  ^  ^  <  <  < ###### ^  ^  ^  ^  ^  <  <  <  <  <  <  <  <  <  < 
 v  v  v ###### >  ^  ^  ^  ^  ^  <  < ###### ^  ^  ^  ^  ^  ^  <  <  <  <  <  <  <  <  < 
 >  v  v ###### ^  ^  ^  ^  ^  ^  ^  < ###### ^  ^  ^  ^  ^  ^  ^  <  <  <  <  <  <  <  < 
 >  >  v ###### ^  ^  ^  ^  ^  ^  ^  ^ ###### ^  ^  ^  ^  ^  ^  ^  ^  <  <  <  <  <  <  < 
 >  >  >  >  >  ^  ^  ^  ^  ^  ^  ^  ^ ###### ^  ^  ^  ^  ^  ^  ^  ^  ^  <  <  <  <  <  < 
 >  >  >  >  ^  ^  ^  ^  ^  ^  ^  ^  ^ ###### ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  <  <  <  <  < 
 >  >  >  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^ ###### ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  <  <  <  < 
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
#+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, 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*:

#+begin_src python :prologue
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] = None
    
    while not frontier.empty():
        current: Location = frontier.get()
        
        if current == goal: # early exit
            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

start = (8, 7)
goal = (17, 2)
parents = breadth_first_search(g, start, goal)
draw_grid(g, point_to=parents, start=start, goal=goal)
#+end_src

#+RESULTS:
#+begin_example
__________________________________________________________________________________________
 .  >  >  v  v  v  v  v  v  v  v  v  v  v  v  v  <  .  .  .  . ###### .  .  .  .  .  .  . 
 >  >  >  >  v  v  v  v  v  v  v  v  v  v  v  <  <  <  .  .  . ###### .  .  .  .  .  .  . 
 >  >  >  >  >  v  v  v  v  v  v  v  v  v  <  <  <  Z  .  .  . ###### .  .  .  .  .  .  . 
 >  >  ^ ###### v  v  v  v  v  v  v  v  <  <  <  <  <  <  .  . ###### .  .  .  .  .  .  . 
 .  ^  ^ ###### v  v  v  v  v  v  v  < ###### ^  <  <  .  .  . ###### .  .  .  .  .  .  . 
 .  .  ^ ###### >  v  v  v  v  v  <  < ###### ^  ^  .  .  .  . ############### .  .  .  . 
 .  .  . ###### >  >  v  v  v  <  <  < ###### ^  .  .  .  .  . ############### .  .  .  . 
 .  .  . ###### >  >  >  A  <  <  <  < ###### .  .  .  .  .  .  .  .  .  .  .  .  .  .  . 
 .  .  . ###### >  >  ^  ^  ^  <  <  < ###### .  .  .  .  .  .  .  .  .  .  .  .  .  .  . 
 .  .  v ###### >  ^  ^  ^  ^  ^  <  < ###### .  .  .  .  .  .  .  .  .  .  .  .  .  .  . 
 .  v  v ###### ^  ^  ^  ^  ^  ^  ^  < ###### .  .  .  .  .  .  .  .  .  .  .  .  .  .  . 
 >  >  v ###### ^  ^  ^  ^  ^  ^  ^  ^ ###### .  .  .  .  .  .  .  .  .  .  .  .  .  .  . 
 >  >  >  >  >  ^  ^  ^  ^  ^  ^  ^  ^ ###### .  .  .  .  .  .  .  .  .  .  .  .  .  .  . 
 >  >  >  >  ^  ^  ^  ^  ^  ^  ^  ^  ^ ###### .  .  .  .  .  .  .  .  .  .  .  .  .  .  . 
 .  >  >  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^ ###### .  .  .  .  .  .  .  .  .  .  .  .  .  .  . 
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
#+end_example

You can see that the algorithm stops when it finds the goal =Z=. Early exit is also useful for   [[href:../early-exit/][problems other than standard pathfinding]].


** 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
:PROPERTIES:
:CUSTOM_ID: python-graph-with-weights
:END:

A regular graph tells me the =neighbors= of each node. A /weighted/ graph also tells me the cost of moving along each edge. 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=. Here's the interface:

#+begin_src python :tangle yes :exports code :results none :main no
class WeightedGraph(Graph):
    def cost(self, from_id: Location, to_id: Location) -> float: pass
#+end_src

Let's implement the interface with a grid that uses grid locations and stores the weights in a dict:

#+begin_src python :tangle yes :exports code :results none :main no
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)
#+end_src

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 include the movement costs in the value returned by the =neighbors= function.

#+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
:PROPERTIES:
:CUSTOM_ID: python-queue-with-priorities
:END:

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: 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]
#+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

Note that Python now has a =queue.PriorityQueue= class you can use directly instead of this wrapper. The API is slightly different.

*** Search
:PROPERTIES:
:CUSTOM_ID: python-search
:END:

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 reached. (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: 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
                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 :exports none :results none :main no
# thanks to @m1sp <Jaiden Mispy> for this simpler version of
# reconstruct_path that doesn't have duplicate entries
#+end_src

#+begin_src python :tangle yes :main no
def reconstruct_path(came_from: dict[Location, Location],
                     start: Location, goal: Location) -> list[Location]:

    current: Location = goal
    path: list[Location] = []
    if goal not in came_from: # no path was found
        return []
    while current != start:
        path.append(current)
        current = came_from[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 *
start, goal = (1, 4), (8, 3)
came_from, cost_so_far = dijkstra_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))
#+end_src

#+RESULTS:
#+begin_example
______________________________
 v  v  <  <  <  <  <  <  <  < 
 v  v  <  <  <  ^  ^  <  <  < 
 v  v  <  <  <  <  ^  ^  <  < 
 v  v  <  <  <  <  <  ^  Z  . 
 >  A  <  <  <  <  .  .  .  . 
 ^  ^  <  <  <  <  .  .  .  . 
 ^  ^  <  <  <  <  <  .  .  . 
 ^ ######### ^  <  v  v  .  . 
 ^ ######### v  v  v  <  <  . 
 ^  <  <  <  <  <  <  <  <  . 
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

______________________________
 .  @  @  @  @  @  @  .  .  . 
 .  @  .  .  .  .  @  @  .  . 
 .  @  .  .  .  .  .  @  @  . 
 .  @  .  .  .  .  .  .  @  . 
 .  @  .  .  .  .  .  .  .  . 
 .  .  .  .  .  .  .  .  .  . 
 .  .  .  .  .  .  .  .  .  . 
 . ######### .  .  .  .  .  . 
 . ######### .  .  .  .  .  . 
 .  .  .  .  .  .  .  .  .  . 
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
#+end_example

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 =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.

*** No path
:PROPERTIES:
:CUSTOM_ID: python-no-path
:END:

There's a tricky case — what if there's no path? Let's try a wall that completely blocks the left and right sides from each other.

#+begin_src python :tangle yes :exports none :results none :main no
diagram_nopath = GridWithWeights(10, 10)
diagram_nopath.walls = [(5, row) for row in range(10)]
#+end_src

#+begin_src python
from implementation import *
start, goal = (1, 4), (8, 3)
came_from, cost_so_far = dijkstra_search(diagram_nopath, start, goal)
draw_grid(diagram_nopath, point_to=came_from, start=start, goal=goal)
# reconstruct_path(came_from, start=start, goal=goal) will be []
#+end_src

#+results:
#+begin_example
______________________________
 v  v  <  <  < ### .  .  .  . 
 v  v  <  <  < ### .  .  .  . 
 v  v  <  <  < ### .  .  .  . 
 v  v  <  <  < ### .  .  Z  . 
 >  A  <  <  < ### .  .  .  . 
 ^  ^  <  <  < ### .  .  .  . 
 ^  ^  <  <  < ### .  .  .  . 
 ^  ^  <  <  < ### .  .  .  . 
 ^  ^  <  <  < ### .  .  .  . 
 ^  ^  <  <  < ### .  .  .  . 
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
#+end_example

The search algorithm will try to explore as much as it can but it just can't get from ~A~ to ~Z~. We can detect this in =reconstruct_path= because =goal= will not be in the =came_from= map. It can still be /slow/ though, as the search algorithm has to explore every nook and cranny before realizing there's no path. If you can, pre-process the map with [[https://en.wikipedia.org/wiki/Connected-component_labeling][connected component labeling]] to determine whether there's a path /before/ running graph search.

*** Distance fields
:PROPERTIES:
:CUSTOM_ID: python-distance-field
:END:

Collecting distances instead of directions gives us a /distance field/. Here's an example of computing the distance from the start location ~A~, with no goal:

#+begin_src python
from implementation import *
start, goal = (1, 4), None
came_from, cost_so_far = dijkstra_search(diagram4, start, goal)
draw_grid(diagram4, number=cost_so_far, start=start)
#+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  A  1  6  11 16 21 20 15 16
 2  1  2  7  12 17 22 21 16 17
 3  2  3  4  9  14 19 16 17 18
 4 ######### 14 19 18 15 16 17
 5 ######### 15 16 13 14 15 16
 6  7  8  9  10 11 12 13 14 15
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
#+end_example

Distance fields be useful for some [[href:../distance-to-any/][variants of pathfinding]]. For these I'll often run the search algorithm /without/ early exit, or with [[href:../early-exit/][a different type of early exit]].

** A* Search
:PROPERTIES:
:CUSTOM_ID: python-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 (=GridWithWeights=), the locations, and in the =heuristic= function. Replace those three and you can use the A* algorithm code with any other graph structure.

#+begin_src python :tangle yes :main no
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
#+end_src

Let's try it out:

#+begin_src python
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))
#+end_src

#+RESULTS:
#+begin_example
______________________________
 v  v  v  v  <  <  <  <  <  < 
 v  v  v  v  <  ^  ^  <  <  < 
 v  v  v  v  <  <  ^  ^  <  < 
 >  v  <  <  <  <  .  ^  Z  . 
 >  A  <  <  <  .  .  .  .  . 
 ^  ^  ^  <  <  .  .  .  .  . 
 ^  ^  ^  <  <  .  .  .  .  . 
 ^ ######### .  .  .  .  .  . 
 . ######### .  .  .  .  .  . 
 .  .  .  .  .  .  .  .  .  . 
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

______________________________
 .  .  .  @  @  @  @  .  .  . 
 .  .  .  @  .  .  @  @  .  . 
 .  .  .  @  .  .  .  @  @  . 
 .  @  @  @  .  .  .  .  @  . 
 .  @  .  .  .  .  .  .  .  . 
 .  .  .  .  .  .  .  .  .  . 
 .  .  .  .  .  .  .  .  .  . 
 . ######### .  .  .  .  .  . 
 . ######### .  .  .  .  .  . 
 .  .  .  .  .  .  .  .  .  . 
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
#+end_example

Here are the distances it calculated:

#+begin_src python
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)
#+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 Z  . 
 1  A  1  6  11 .  .  .  .  . 
 2  1  2  7  12 .  .  .  .  . 
 3  2  3  4  9  .  .  .  .  . 
 4 ######### .  .  .  .  .  . 
 . ######### .  .  .  .  .  . 
 .  .  .  .  .  .  .  .  .  . 
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
#+end_example

And that's it! We've implemented graphs, grids, Breadth First Search, Dijkstra's Algorithm, and A*.

*** Straighter paths
:PROPERTIES:
:CUSTOM_ID: python-straighter-paths
:END:

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 often *it won't look good to you*. 
I list [[Ugly paths][some solutions]] in a later section.


* C++ Implementation
:PROPERTIES:
:CUSTOM_ID: cpp
:END:

Note: some of the sample code needs to include [[./implementation.cpp][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.

** Breadth First Search
:PROPERTIES:
:CUSTOM_ID: cpp-breadth-first
:END:

Let's implement Breadth First Search in C++. These are the components we need:

- Graph :: a data structure that can tell me the =neighbors= for each graph location (see [[../grids/graphs.html][this tutorial]]). A /weighted/ graph can also tell me the =cost= of moving along an edge.
- Locations :: a simple value (int, string, tuple, etc.) that /labels/ locations in the graph. These are not necessarily locations on the map. They may include additional information such as direction, fuel, lane, or inventory, depending on the problem being solved.
- Search :: an algorithm that takes a graph, a starting graph location, and optionally a goal graph location, and calculates some useful information (reached, parent pointer, distance) for some or all graph locations.
- Queue :: a data structure used by the search algorithm to decide the order in which to process the graph 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* where the locations are =char=:

#+begin_src cpp :tangle yes :exports none :main no
/*
 Sample code from https://www.redblobgames.com/pathfinding/a-star/
 Copyright 2014 Red Blob Games <redblobgames@gmail.com>
 
 Feel free to use this code in your own projects, including commercial projects
 License: Apache v2.0 <http://www.apache.org/licenses/LICENSE-2.0.html>
*/

#include <iostream>
#include <iomanip>
#include <unordered_map>
#include <unordered_set>
#include <array>
#include <vector>
#include <utility>
#include <queue>
#include <tuple>
#include <algorithm>
#include <cstdlib>
#+end_src


#+begin_src cpp :tangle yes :main no
struct SimpleGraph {
  std::unordered_map<char, std::vector<char> > edges;

  std::vector<char> neighbors(char id) {
    return edges[id];
  }
};
#+end_src

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:

#+include: "implementation-example-graph.svg" export html

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 =Location= type is a letter A, B, C, D, E, or F.

[[file:implementation-example-graph.png]]

#+begin_src cpp :tangle yes :main no
SimpleGraph example_graph {{
    {'A', {'B'}},
    {'B', {'C'}},
    {'C', {'B', 'D', 'F'}},
    {'D', {'C', 'E'}},
    {'E', {'F'}},
    {'F', {}},
  }};
#+end_src

The C++ standard library already includes a queue class. We now have a graph (=SimpleGraph=), locations (=char=), and a queue (=std::queue=). Now we can try Breadth First Search:

#+begin_src cpp
#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');
}
#+end_src

#+RESULTS:
#+begin_example
Reachable from A:
  Visiting A
  Visiting B
  Visiting C
  Visiting D
  Visiting F
  Visiting E
Reachable from E:
  Visiting E
  Visiting F
#+end_example

Grids can be expressed as graphs too. I'll now define a new *graph* called =SquareGrid=, with *locations* structs with two ints. In this map, the locations ("states") in the graph are the same as locations on the game map, but in many problems graph locations are not the same as map locations. Instead of storing the edges explicitly, I'll calculate them in the =neighbors= function. In many problems it's better to store them explicitly.

#+begin_src cpp :tangle yes :main no
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 {
    // NOTE: better to use something like boost hash_combine
    return std::hash<int>()(id.x ^ (id.y << 16));
  }
};
}


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}
};
#+end_src


#+begin_src cpp :tangle yes :exports none :main no
// Helpers for GridLocation

bool operator == (GridLocation a, GridLocation b) {
  return a.x == b.x && a.y == b.y;
}

bool operator != (GridLocation a, GridLocation b) {
  return !(a == b);
}

bool operator < (GridLocation a, GridLocation b) {
  return std::tie(a.x, a.y) < std::tie(b.x, b.y);
}

std::basic_iostream<char>::basic_ostream& operator<<(std::basic_iostream<char>::basic_ostream& out, const GridLocation& loc) {
  out << '(' << loc.x << ',' << loc.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<class Graph>
void draw_grid(const Graph& graph,
               std::unordered_map<GridLocation, double>* distances=nullptr,
               std::unordered_map<GridLocation, GridLocation>* point_to=nullptr,
               std::vector<GridLocation>* path=nullptr,
               GridLocation* start=nullptr,
               GridLocation* goal=nullptr) {
  const int field_width = 3;
  std::cout << std::string(field_width * graph.width, '_') << '\n';
  for (int y = 0; y != graph.height; ++y) {
    for (int x = 0; x != graph.width; ++x) {
      GridLocation id {x, y};
      if (graph.walls.find(id) != graph.walls.end()) {
        std::cout << std::string(field_width, '#');
      } else if (start && id == *start) {
        std::cout << " A ";
      } else if (goal && id == *goal) {
        std::cout << " Z ";
      } else if (path != nullptr && find(path->begin(), path->end(), id) != path->end()) {
        std::cout << " @ ";
      } else if (point_to != nullptr && point_to->count(id)) {
        GridLocation next = (*point_to)[id];
        if (next.x == x + 1) { std::cout << " > "; }
        else if (next.x == x - 1) { std::cout << " < "; }
        else if (next.y == y + 1) { std::cout << " v "; }
        else if (next.y == y - 1) { std::cout << " ^ "; }
        else { std::cout << " * "; }
      } else if (distances != nullptr && distances->count(id)) {
        std::cout << ' ' << std::left << std::setw(field_width - 1) << (*distances)[id];
      } else {
        std::cout << " . ";
      }
    }
    std::cout << '\n';
  }
  std::cout << std::string(field_width * graph.width, '~') << '\n';
}
#+end_src

In the helper file =implementation.cpp= I defined a function to make 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(GridLocation{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);
}
#+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<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);
}
#+end_src

#+RESULTS:
#+begin_example
__________________________________________________________________________________________
 >  >  >  v  v  v  v  v  v  v  v  v  v  v  v  v  <  <  <  <  < ###### v  v  v  v  v  v  v 
 >  >  >  >  v  v  v  v  v  v  v  v  v  v  v  <  <  <  <  <  < ###### >  v  v  v  v  v  v 
 >  >  >  >  >  v  v  v  v  v  v  v  v  v  <  <  <  <  <  <  < ###### >  >  v  v  v  v  v 
 >  >  ^ ###### v  v  v  v  v  v  v  v  <  <  <  <  <  <  <  < ###### >  >  >  v  v  v  v 
 >  ^  ^ ###### v  v  v  v  v  v  v  < ###### ^  <  <  <  <  < ###### >  >  >  v  v  v  v 
 ^  ^  ^ ###### v  v  v  v  v  v  <  < ###### ^  ^  <  <  <  < ############### v  v  v  < 
 v  v  v ###### v  v  v  v  v  <  <  < ###### ^  ^  ^  <  <  < ############### v  v  <  < 
 v  v  v ###### >  v  v  v  <  <  <  < ###### ^  ^  ^  ^  <  <  <  <  <  <  <  <  <  <  < 
 v  v  v ###### >  >  A  <  <  <  <  < ###### ^  ^  ^  ^  ^  <  <  <  <  <  <  <  <  <  < 
 v  v  v ###### >  ^  ^  ^  <  <  <  < ###### ^  ^  ^  ^  ^  ^  <  <  <  <  <  <  <  <  < 
 >  v  v ###### ^  ^  ^  ^  ^  <  <  < ###### ^  ^  ^  ^  ^  ^  ^  <  <  <  <  <  <  <  < 
 >  >  v ###### ^  ^  ^  ^  ^  ^  <  < ###### ^  ^  ^  ^  ^  ^  ^  ^  <  <  <  <  <  <  < 
 >  >  >  >  >  ^  ^  ^  ^  ^  ^  ^  < ###### ^  ^  ^  ^  ^  ^  ^  ^  ^  <  <  <  <  <  < 
 >  >  >  >  ^  ^  ^  ^  ^  ^  ^  ^  ^ ###### ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  <  <  <  <  < 
 >  >  >  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^ ###### ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  <  <  <  < 
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
#+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.

** Early Exit
:PROPERTIES:
:CUSTOM_ID: cpp-early-exit
:END:

Breadth 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. 

#+begin_src cpp
#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);
}
#+end_src

#+RESULTS:
#+begin_example
__________________________________________________________________________________________
 .  >  >  v  v  v  v  v  v  v  v  v  v  v  v  v  <  .  .  .  . ###### .  .  .  .  .  .  . 
 >  >  >  >  v  v  v  v  v  v  v  v  v  v  v  <  <  <  .  .  . ###### .  .  .  .  .  .  . 
 >  >  >  >  >  v  v  v  v  v  v  v  v  v  <  <  <  Z  .  .  . ###### .  .  .  .  .  .  . 
 >  >  ^ ###### v  v  v  v  v  v  v  v  <  <  <  <  <  <  .  . ###### .  .  .  .  .  .  . 
 .  ^  ^ ###### v  v  v  v  v  v  v  < ###### ^  <  <  .  .  . ###### .  .  .  .  .  .  . 
 .  .  ^ ###### >  v  v  v  v  v  <  < ###### ^  ^  .  .  .  . ############### .  .  .  . 
 .  .  . ###### >  >  v  v  v  <  <  < ###### ^  .  .  .  .  . ############### .  .  .  . 
 .  .  . ###### >  >  >  A  <  <  <  < ###### .  .  .  .  .  .  .  .  .  .  .  .  .  .  . 
 .  .  . ###### >  >  ^  ^  ^  <  <  < ###### .  .  .  .  .  .  .  .  .  .  .  .  .  .  . 
 .  .  v ###### >  ^  ^  ^  ^  ^  <  < ###### .  .  .  .  .  .  .  .  .  .  .  .  .  .  . 
 .  v  v ###### ^  ^  ^  ^  ^  ^  ^  < ###### .  .  .  .  .  .  .  .  .  .  .  .  .  .  . 
 >  >  v ###### ^  ^  ^  ^  ^  ^  ^  ^ ###### .  .  .  .  .  .  .  .  .  .  .  .  .  .  . 
 >  >  >  >  >  ^  ^  ^  ^  ^  ^  ^  ^ ###### .  .  .  .  .  .  .  .  .  .  .  .  .  .  . 
 >  >  >  >  ^  ^  ^  ^  ^  ^  ^  ^  ^ ###### .  .  .  .  .  .  .  .  .  .  .  .  .  .  . 
 .  >  >  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^ ###### .  .  .  .  .  .  .  .  .  .  .  .  .  .  . 
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
#+end_example

You can see that the algorithm stops when it finds the goal =Z=. Early exit is also useful for   [[href:../early-exit/][problems other than standard pathfinding]].

** Dijkstra's Algorithm
:PROPERTIES:
:CUSTOM_ID: cpp-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
:PROPERTIES:
:CUSTOM_ID: cpp-graph-with-weights
:END:

A regular graph tells me the =neighbors= of each node. A /weighted/ graph also tells me the cost of moving along each edge. 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. Here's a grid with a list of forest tiles, which will have movement cost 5:

#+begin_src cpp :tangle yes :main no
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;
  }
};
#+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 GridLocation L;
  grid.forests = std::unordered_set<GridLocation> {
    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
:PROPERTIES:
:CUSTOM_ID: cpp-queue-with-priorities
:END:

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<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;
  }
};
#+end_src

#+begin_src cpp :exports none
#include "redblobgames/pathfinding/a-star/implementation.cpp"

int main() {
  PriorityQueue<char, int> 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() << '\n';
  }
}
#+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
:PROPERTIES:
:CUSTOM_ID: cpp-search
:END:

See [[./introduction.html#dijkstra][the forest map from the main page]].

#+begin_src cpp :tangle yes :main no
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);
      }
    }
  }
}
#+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<typename Location>
std::vector<Location> reconstruct_path(
   Location start, Location goal,
   std::unordered_map<Location, Location> came_from
) {
  std::vector<Location> path;
  Location current = goal;
  if (came_from.find(goal) == came_from.end()) {
    return path; // no path can be found
  }
  while (current != start) {
    path.push_back(current);
    current = came_from[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();
  GridLocation start{1, 4}, goal{8, 3};
  std::unordered_map<GridLocation, GridLocation> came_from;
  std::unordered_map<GridLocation, double> cost_so_far;
  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);
}
#+end_src

#+RESULTS:
#+begin_example
______________________________
 v  v  <  <  <  <  <  <  <  < 
 v  v  <  <  <  ^  ^  <  <  < 
 v  v  <  <  <  <  ^  ^  <  < 
 v  v  <  <  <  <  <  ^  Z  . 
 >  A  <  <  <  <  .  .  .  . 
 ^  ^  <  <  <  <  .  .  .  . 
 ^  ^  <  <  <  <  <  .  .  . 
 ^ ######### ^  <  v  v  .  . 
 ^ ######### v  v  v  <  <  . 
 ^  <  <  <  <  <  <  <  <  . 
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

______________________________
 .  @  @  @  @  @  @  .  .  . 
 .  @  .  .  .  .  @  @  .  . 
 .  @  .  .  .  .  .  @  @  . 
 .  @  .  .  .  .  .  .  Z  . 
 .  A  .  .  .  .  .  .  .  . 
 .  .  .  .  .  .  .  .  .  . 
 .  .  .  .  .  .  .  .  .  . 
 . ######### .  .  .  .  .  . 
 . ######### .  .  .  .  .  . 
 .  .  .  .  .  .  .  .  .  . 
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
#+end_example

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.

*** No path
:PROPERTIES:
:CUSTOM_ID: cpp-no-path
:END:

There's a tricky case — what if there's no path? Let's try a wall that completely blocks the left and right sides from each other.

#+begin_src cpp :tangle yes :exports none :results none :main no
GridWithWeights make_diagram_nopath() {
  GridWithWeights grid(10, 10);
  add_rect(grid, 5, 0, 6, 10);
  return grid;
}
#+end_src

#+begin_src cpp
#include "redblobgames/pathfinding/a-star/implementation.cpp"

int main() {
  GridWithWeights grid = make_diagram_nopath();
  GridLocation start{1, 4}, goal{8, 3};
  std::unordered_map<GridLocation, GridLocation> came_from;
  std::unordered_map<GridLocation, double> cost_so_far;
  dijkstra_search(grid, start, goal, came_from, cost_so_far);
  draw_grid(grid, nullptr, &came_from, nullptr, &start, &goal);
  // reconstruct_path(start, goal, came_from) returns an empty vector
}
#+end_src

#+results:
#+begin_example
______________________________
 v  v  <  <  < ### .  .  .  . 
 v  v  <  <  < ### .  .  .  . 
 v  v  <  <  < ### .  .  .  . 
 v  v  <  <  < ### .  .  Z  . 
 >  A  <  <  < ### .  .  .  . 
 ^  ^  <  <  < ### .  .  .  . 
 ^  ^  <  <  < ### .  .  .  . 
 ^  ^  <  <  < ### .  .  .  . 
 ^  ^  <  <  < ### .  .  .  . 
 ^  ^  <  <  < ### .  .  .  . 
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
#+end_example

The search algorithm will try to explore as much as it can but it just can't get from ~A~ to ~Z~. We can detect this in =reconstruct_path= because =goal= will not be in the =came_from= map. It can still be /slow/ though, as the search algorithm has to explore every nook and cranny before realizing there's no path. If you can, pre-process the map with [[https://en.wikipedia.org/wiki/Connected-component_labeling][connected component labeling]] to determine whether there's a path /before/ running graph search.

*** Distance fields
:PROPERTIES:
:CUSTOM_ID: cpp-distance-field
:END:

Collecting distances instead of directions gives us a /distance field/. Here's an example of computing the distance from the start location ~A~ with a dummy value for the goal ~Z~:

#+begin_src cpp
#include "redblobgames/pathfinding/a-star/implementation.cpp"

int main() {
  GridWithWeights grid = make_diagram4();
  GridLocation start{1, 4}, goal{-1, -1};
  std::unordered_map<GridLocation, GridLocation> came_from;
  std::unordered_map<GridLocation, double> cost_so_far;
  dijkstra_search(grid, start, goal, came_from, cost_so_far);
  draw_grid(grid, &cost_so_far, nullptr, nullptr, &start, &goal);
}
#+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  A  1  6  11 16 21 20 15 16
 2  1  2  7  12 17 22 21 16 17
 3  2  3  4  9  14 19 16 17 18
 4 ######### 14 19 18 15 16 17
 5 ######### 15 16 13 14 15 16
 6  7  8  9  10 11 12 13 14 15
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
#+end_example

Distance fields can be useful for some [[href:../distance-to-any/][variants of pathfinding]]. For these I'll often run the search algorithm /without/ early exit, or with [[href:../early-exit/][a different type of early exit]].

** 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 (=GridWithWeights=), the locations (=Location= struct), and in the =heuristic= function. Replace those three and you can use the A* algorithm code with any other graph structure.

#+begin_src cpp :tangle yes :main no
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;
      }
    }
  }
}
#+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.

Let's try it out:

#+begin_src cpp
#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);
}
#+end_src

#+RESULTS:
#+begin_example
______________________________
 v  v  v  v  <  <  <  <  <  < 
 v  v  v  v  <  ^  ^  <  <  < 
 v  v  v  v  <  <  ^  ^  <  < 
 >  v  <  <  <  <  .  ^  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 ######### .  .  .  .  .  . 
 . ######### .  .  .  .  .  . 
 .  .  .  .  .  .  .  .  .  . 
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
#+end_example

And that's it! We've implemented graphs, grids, Breadth First Search, Dijkstra's Algorithm, and A*.

*** Straighter paths
:PROPERTIES:
:CUSTOM_ID: cpp-straighter-paths
:END:

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 often *it won't look good to you*. 
I list [[Ugly paths][some solutions]] in a later section.

** Production code
:PROPERTIES:
:CUSTOM_ID: cpp-production
:END:

The 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:

- inlining small functions
- the =Location= parameter should be part of the =Graph=
- the cost could be int or double, and should be part of the =Graph=
- use =array= instead of =unordered_set= if the ids are dense integers, and reset these values on exit instead of initializing on entry
- pass larger data structures by reference instead of by value
- return larger data structures in out parameters instead of returning them, or use move constructors (for example, the vector returned from the =neighbors= function)
- the heuristic can vary and should be a template parameter to the A* function so that it can be inlined

Here's how the A* code might look different with some (but not all) of these changes:

#+begin_src cpp :main no
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;
      }
    }
  }
}
#+end_src

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.

#+begin_comment
Rust Implementation

Duplicating all the code on the page is getting kind of annoying. Is it time to switch to a dynamic version?

https://docs.rs/pathfinding/0.2.2/src/pathfinding/astar.rs.html#75-135

Javascript implementation

Will node inside babel let me import from third party modules like flatqueue?
#+end_comment


* 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: "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: "cs/AStar.cs" src csharp

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#.

* Algorithm changes
:PROPERTIES:
:CUSTOM_ID: algorithm
:END:

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…

1. By starting the priority with only the start node, we can keep it small, which makes it faster and use less memory.
2. With early exit, we almost never need to insert all the nodes into the queue, and we can return the path as soon as it's found.
3. By not putting all nodes into the queue at the start, most of the time we can use a cheap insert operation instead of the more expensive decrease-key operation.
4. By not putting all nodes into the queue at the start, we can handle situations where we do not even know all the nodes, or where the number of nodes is infinite.

This variant is sometimes called “Uniform Cost Search”. See [[https://en.wikipedia.org/wiki/Dijkstra%27s_algorithm#Practical_optimizations_and_infinite_graphs][Wikipedia]] to see the pseudocode, or read [[https://www.aaai.org/ocs/index.php/SOCS/SOCS11/paper/viewFile/4017/4357][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*:

5. [@5] I eliminate the check for a node being in the frontier with a higher cost. By not checking, I end up with duplicate elements in the frontier. /The algorithm still works./ It will revisit some locations more than necessary (but rarely, in my experience, as long as the heuristic is admissible). The code is simpler and it allows me to use a simpler and faster priority queue that does not support the decrease-key operation. The paper [[https://www3.cs.stonybrook.edu/~rezaul/papers/TR-07-54.pdf]["Priority Queues and Dijkstra’s Algorithm"]] suggests that this approach is faster in practice.
6. Instead of storing both a “closed set” and an “open set”, I have call the open set the =frontier=, and I have a =reached= flag that tells me whether it's in /either/ of those sets. I still have two sets but merging the two into =reached= simplifies the code.
7. I use hash tables instead of arrays of node objects. This eliminates the rather expensive /initialize/ step that many other implementations have. For large game maps, the initialization of those arrays is often slower than the rest of A*.

If you have more suggestions for simplifications that preserve performance, please let me know!

* 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-bfs-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 [[https://en.cppreference.com/w/cpp/container/deque][deque]] container. 

However, it turns out breadth first search doesn't even need a queue! The queue only contains nodes with distance =d= and nodes with distance =d+1=.  We can split the queue into two, one for =d= and one for =d+1=:

#+begin_src python
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')
#+end_src

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.


** Priority Queue
:PROPERTIES:
:CUSTOM_ID: optimize-queue
:END:

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 [[https://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 paper [[https://www3.cs.stonybrook.edu/~rezaul/papers/TR-07-54.pdf]["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 [[https://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.

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 (=new_cost < cost_so_far=). The test is cheap because I made it cheap to look up =cost_so_far=.

** Integer locations
:PROPERTIES:
:CUSTOM_ID: optimize-integer-ids
:END:

If your graph uses integers as locations, consider using a simple array instead of a hash table for =cost_so_far=, =reached=, =came_from=, etc. Since =reached= is an array of booleans, you can use a bit vector. Initialize the =reached= bit vector for all ids, but leave =cost_so_far= and =came_from= uninitialized. Then only initialize on the first visit.

#+begin_src cpp
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;
        …
    }
#+end_src

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:

#+begin_src cpp
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();
#+end_src

(Caveat: I haven't used or tested this code)

* Troubleshooting
:PROPERTIES:
:CUSTOM_ID: troubleshooting
:END:


** Wrong paths
: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). Try setting the heuristic to 0. If a 0 heuristic fixes the paths, your heuristic is probably wrong. If a 0 heuristic doesn't help, then your graph search algorithm code probably has a bug.
- 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. 
- The heuristic and costs need to have the same "units". Try testing A* on a map with no walls. If the heuristic and movement costs match up, the priority should be the /same/ along the entire path. If it isn't, then your heuristic and costs probably don't match up. When there are no obstacles and uniform movement costs, at each step, the heuristic should decrease by the same amount cost_so_far increases.

** Ugly paths
:PROPERTIES:
:CUSTOM_ID: troubleshooting-ugly-path
:END:

The 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: =SSEE=, =SESE=, =SEES=, =ESSE=, =ESES=, =EESS=.
The pathfinding algorithm is going to pick one, and it may not be the one you prefer. The path is /short/ but it doesn't /look/ good. What can we do to favor good looking paths, like =SESE= or =ESES=?

#+begin_src python :wrap export html :exports results
# generate the diagrams instead of writing them out by hand
def show(path):
    x, y = 0, 0
    cells = [(x, y)]
    svg = f'<rect x="{x}" y="{y}" width="1" height="1"/>'
    for dir in path:
        if dir == 'S': dx, dy = 0, 1
        if dir == 'E': dx, dy = 1, 0
        x += dx
        y += dy
        cells.append((x, y))
    return ''.join([f'<rect x="{x}" y="{y}" width="1" height="1"/>\n' for (x, y) in cells])

print('<figure>')
for path in ['SSEE', 'SESE', 'SEES', 'ESSE', 'ESES', 'EESS']:
    print('<svg viewBox="-1 0 5 4" width="15%"><g fill="hsl(0,20%,70%)" stroke="white" stroke-width="0.1">')
    print(show(path))
    print(f'</g><text text-anchor="middle" x="1.5" y="4" font-size="0.8" font-family="sans-serif">{path}</text></svg>')
print('<figcaption>Many ways to move south 2 east 2</figcaption>')
print('</figure>')
#+end_src

#+results:
#+begin_export html
<figure>
<svg viewBox="-1 0 5 4" width="15%"><g fill="hsl(0,20%,70%)" stroke="white" stroke-width="0.1">
<rect x="0" y="0" width="1" height="1"/>
<rect x="0" y="1" width="1" height="1"/>
<rect x="0" y="2" width="1" height="1"/>
<rect x="1" y="2" width="1" height="1"/>
<rect x="2" y="2" width="1" height="1"/>

</g><text text-anchor="middle" x="1.5" y="4" font-size="0.8" font-family="sans-serif">SSEE</text></svg>
<svg viewBox="-1 0 5 4" width="15%"><g fill="hsl(0,20%,70%)" stroke="white" stroke-width="0.1">
<rect x="0" y="0" width="1" height="1"/>
<rect x="0" y="1" width="1" height="1"/>
<rect x="1" y="1" width="1" height="1"/>
<rect x="1" y="2" width="1" height="1"/>
<rect x="2" y="2" width="1" height="1"/>

</g><text text-anchor="middle" x="1.5" y="4" font-size="0.8" font-family="sans-serif">SESE</text></svg>
<svg viewBox="-1 0 5 4" width="15%"><g fill="hsl(0,20%,70%)" stroke="white" stroke-width="0.1">
<rect x="0" y="0" width="1" height="1"/>
<rect x="0" y="1" width="1" height="1"/>
<rect x="1" y="1" width="1" height="1"/>
<rect x="2" y="1" width="1" height="1"/>
<rect x="2" y="2" width="1" height="1"/>

</g><text text-anchor="middle" x="1.5" y="4" font-size="0.8" font-family="sans-serif">SEES</text></svg>
<svg viewBox="-1 0 5 4" width="15%"><g fill="hsl(0,20%,70%)" stroke="white" stroke-width="0.1">
<rect x="0" y="0" width="1" height="1"/>
<rect x="1" y="0" width="1" height="1"/>
<rect x="1" y="1" width="1" height="1"/>
<rect x="1" y="2" width="1" height="1"/>
<rect x="2" y="2" width="1" height="1"/>

</g><text text-anchor="middle" x="1.5" y="4" font-size="0.8" font-family="sans-serif">ESSE</text></svg>
<svg viewBox="-1 0 5 4" width="15%"><g fill="hsl(0,20%,70%)" stroke="white" stroke-width="0.1">
<rect x="0" y="0" width="1" height="1"/>
<rect x="1" y="0" width="1" height="1"/>
<rect x="1" y="1" width="1" height="1"/>
<rect x="2" y="1" width="1" height="1"/>
<rect x="2" y="2" width="1" height="1"/>

</g><text text-anchor="middle" x="1.5" y="4" font-size="0.8" font-family="sans-serif">ESES</text></svg>
<svg viewBox="-1 0 5 4" width="15%"><g fill="hsl(0,20%,70%)" stroke="white" stroke-width="0.1">
<rect x="0" y="0" width="1" height="1"/>
<rect x="1" y="0" width="1" height="1"/>
<rect x="2" y="0" width="1" height="1"/>
<rect x="2" y="1" width="1" height="1"/>
<rect x="2" y="2" width="1" height="1"/>

</g><text text-anchor="middle" x="1.5" y="4" font-size="0.8" font-family="sans-serif">EESS</text></svg>
<figcaption>Many ways to move south 2 east 2</figcaption>
</figure>
#+end_export

- /Don't use a grid/: tell A* only the places where you might turn, instead of every grid square; [[../grids/algorithms.html][read more here]]. Bonus: switching to a non-grid usually makes A* much faster.
- /Modify the A* algorithm/ to support “any angle” paths: Theta*, Block A*, Field A*, or AnyA. See the paper [[https://scholar.google.com/scholar?cluster=8491292501067866547&hl=en&as_sdt=0,5][An Empirical Comparison of Any-Angle Path-Planning Algorithms]] from Uras & Koenig.
- /Choose/ the paths by calculating some metric that lets you pick the most pleasing path. See [[https://towardsdatascience.com/a-short-and-direct-walk-with-pascals-triangle-26a86d76f75f][Path Counting for Grid-Based Navigation]] (and [[https://www.keanw.com/2022/06/a-paper-on-our-space-analysis-algorithm-in-the-journal-of-artificial-intelligence-research.html][blog post]]) for a clever way to do this using Pascal's Triangle.
- /Straighten/ the paths using a "string pulling" algorithm: If the final path has points P, Q, R, S, and there's a straight line from P to S, then follow that straight line instead of visiting Q and R.
- /Nudge/ the paths when there's a tie towards better-looking paths, by adjusting the order of nodes in the queue. For 4-way movement I describe two hacks below. For 8-way movement, make sure your neighbors function returns the cardinal directions (N, E, S, W) earlier in the array than the diagonal directions (NW, SE, SW, NE).

The hacks don't work as well as the other three approaches but they're easy to implement, so I'll describe them here:

*** Checkerboard neighbor order
:PROPERTIES:
:CUSTOM_ID: ties-checkerboard-neighbors
:END:

Breadth 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. 

#+begin_src python :tangle yes :exports none :main no
def breadth_first_search(graph: Graph, start: Location, goal: Location):
    frontier = Queue()
    frontier.put(start)
    came_from: dict[Location, Optional[Location]] = {}
    came_from[start] = None
    
    while not frontier.empty():
        current: Location = 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
#+end_src


#+begin_src python :tangle yes :main no :exports none
class SquareGridNeighborOrder(SquareGrid):
    def neighbors(self, id):
        (x, y) = id
        neighbors = [(x + dx, y + dy) for (dx, dy) in self.NEIGHBOR_ORDER]
        results = filter(self.in_bounds, neighbors)
        results = filter(self.passable, results)
        return list(results)

def test_with_custom_order(neighbor_order):
    if neighbor_order:
        g = SquareGridNeighborOrder(30, 15)
        g.NEIGHBOR_ORDER = neighbor_order
    else:
        g = SquareGrid(30, 15)
    g.walls = DIAGRAM1_WALLS
    start, goal = (8, 7), (27, 2)
    came_from = breadth_first_search(g, start, goal)
    draw_grid(g, path=reconstruct_path(came_from, start=start, goal=goal),
              point_to=came_from, start=start, goal=goal)
#+end_src

If moving south 2 and east 2, there are many ways to get there: =SSEE=, =SESE=, =SEES=, =ESSE=, =ESES=, =EESS=. 
If East comes before South in the list of neighbors, then it will /always/ explore east before it explores south, and end up choosing =EESS=.
If South comes before East, then it will always explore south first, and end up choosing =SSEE=.
We can see that problem in this larger example where the order is East, North, West, South:

#+begin_src python :results output
from implementation import *
test_with_custom_order([(+1, 0), (0, -1), (-1, 0), (0, +1)])
#+end_src

#+results:
#+begin_example
__________________________________________________________________________________________
 >  >  >  >  >  >  >  >  v  v  v  v  v  v  v  v  v  v  v  v  v ###### .  .  .  .  .  .  . 
 >  >  >  >  >  >  >  >  v  v  v  v  v  v  v  v  v  v  v  v  v ###### .  .  .  v  .  .  . 
 >  >  >  >  >  >  >  >  v  v  v  v  v  v  v  v  v  v  v  v  v ###### .  .  >  v  Z  v  . 
 ^  ^  ^ ###### >  >  >  v  v  v  v  @  @  @  @  @  @  @  @  @ ###### .  >  >  v  @  v  v 
 ^  ^  ^ ###### >  >  >  v  v  v  v  @ ###### ^  ^  ^  ^  ^  @ ###### >  >  >  v  @  v  v 
 ^  ^  ^ ###### >  >  >  v  v  v  v  @ ###### ^  ^  ^  ^  ^  @ ############### v  @  v  v 
 ^  ^  ^ ###### >  >  >  v  v  v  v  @ ###### ^  ^  ^  ^  ^  @ ############### v  @  v  v 
 ^  ^  ^ ###### >  >  >  A  @  @  @  @ ###### ^  ^  ^  ^  ^  @  @  @  @  @  @  @  @  <  < 
 >  >  v ###### ^  ^  ^  ^  ^  ^  ^  ^ ###### ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^ 
 >  >  v ###### ^  ^  ^  ^  ^  ^  ^  ^ ###### ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^ 
 >  >  v ###### ^  ^  ^  ^  ^  ^  ^  ^ ###### ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^ 
 >  >  v ###### ^  ^  ^  ^  ^  ^  ^  ^ ###### ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^ 
 >  >  >  >  >  ^  ^  ^  ^  ^  ^  ^  ^ ###### ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  . 
 ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^ ###### ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  .  .  . 
 ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^ ###### ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  .  .  .  . 
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
#+end_example

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:

#+begin_src python :results output
from implementation import *
test_with_custom_order([(0, +1), (-1, 0), (0, -1), (+1, 0)])
#+end_src

#+results:
#+begin_example
__________________________________________________________________________________________
 v  v  v  v  v  v  v  v  v  <  <  <  <  <  <  <  <  <  <  <  < ###### .  .  .  v  .  .  . 
 v  v  v  v  v  v  v  v  v  <  <  <  <  <  <  <  <  <  <  <  < ###### .  .  v  v  <  .  . 
 >  >  >  >  >  v  v  v  v  <  <  <  <  <  <  <  <  <  <  <  < ###### .  v  v  @  Z  .  . 
 >  >  ^ ###### v  v  v  @  @  @  @  @  @  @  @  <  <  <  <  < ###### v  v  v  @  <  <  . 
 >  >  ^ ###### v  v  v  @  <  <  <  < ###### @  <  <  <  <  < ###### >  >  >  @  <  <  < 
 >  >  ^ ###### v  v  v  @  <  <  <  < ###### @  <  <  <  <  < ############### @  <  <  < 
 >  >  ^ ###### v  v  v  @  <  <  <  < ###### @  <  <  <  <  < ############### @  <  <  < 
 v  v  v ###### >  >  >  A  <  <  <  < ###### @  @  @  @  @  @  @  @  @  @  @  @  <  <  < 
 v  v  v ###### >  >  >  ^  <  <  <  < ###### ^  <  <  <  <  <  <  <  <  <  <  <  <  <  < 
 v  v  v ###### >  >  >  ^  <  <  <  < ###### ^  <  <  <  <  <  <  <  <  <  <  <  <  <  < 
 v  v  v ###### >  >  >  ^  <  <  <  < ###### ^  <  <  <  <  <  <  <  <  <  <  <  <  <  < 
 v  v  v ###### >  >  >  ^  <  <  <  < ###### ^  <  <  <  <  <  <  <  <  <  <  <  <  <  < 
 >  >  >  >  >  >  >  >  ^  <  <  <  < ###### ^  <  <  <  <  <  <  <  <  <  <  <  <  <  . 
 >  >  >  >  >  >  >  >  ^  <  <  <  < ###### ^  <  <  <  <  <  <  <  <  <  <  <  <  .  . 
 >  >  >  >  >  >  >  >  ^  <  <  <  < ###### ^  <  <  <  <  <  <  <  <  <  <  <  .  .  . 
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
#+end_example

Let's try South, East, West, North:

#+begin_src python :results output
from implementation import *
test_with_custom_order([(0, +1), (+1, 0), (-1, 0), (0, -1)])
#+end_src

#+results:
#+begin_example
__________________________________________________________________________________________
 v  v  v  v  v  v  v  v  v  v  v  v  v  v  v  v  v  v  v  v  v ###### .  .  .  .  .  .  . 
 v  v  v  v  v  v  v  v  v  v  v  v  v  v  v  v  v  v  v  v  v ###### .  .  .  v  .  .  . 
 >  >  >  >  >  v  v  v  v  v  v  v  v  v  v  v  v  v  v  v  v ###### .  .  v  v  Z  v  . 
 >  >  ^ ###### v  v  v  v  v  v  v  @  @  @  @  <  <  <  <  < ###### .  v  v  v  @  v  v 
 >  >  ^ ###### v  v  v  v  v  v  v  @ ###### @  <  <  <  <  < ###### >  >  >  v  @  v  v 
 >  >  ^ ###### v  v  v  v  v  v  v  @ ###### @  <  <  <  <  < ############### v  @  v  v 
 >  >  ^ ###### v  v  v  v  v  v  v  @ ###### @  <  <  <  <  < ############### v  @  v  v 
 v  v  v ###### >  >  >  A  @  @  @  @ ###### @  @  @  @  @  @  @  @  @  @  @  @  @  <  < 
 v  v  v ###### >  >  >  ^  <  <  <  < ###### ^  <  <  <  <  <  <  <  <  <  <  <  <  <  < 
 v  v  v ###### >  >  >  ^  <  <  <  < ###### ^  <  <  <  <  <  <  <  <  <  <  <  <  <  < 
 v  v  v ###### >  >  >  ^  <  <  <  < ###### ^  <  <  <  <  <  <  <  <  <  <  <  <  <  < 
 v  v  v ###### >  >  >  ^  <  <  <  < ###### ^  <  <  <  <  <  <  <  <  <  <  <  <  <  < 
 >  >  >  >  >  >  >  >  ^  <  <  <  < ###### ^  <  <  <  <  <  <  <  <  <  <  <  <  <  . 
 >  >  >  >  >  >  >  >  ^  <  <  <  < ###### ^  <  <  <  <  <  <  <  <  <  <  <  <  .  . 
 >  >  >  >  >  >  >  >  ^  <  <  <  < ###### ^  <  <  <  <  <  <  <  <  <  <  <  .  .  . 
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
#+end_example

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 =(x + y) % 2=:

- when 0: return the South, North, West, East neighbors
- when 1: return the East, West, North, South neighbors

The result is the path alternating between vertical and horizontal steps:

#+begin_src python :results output
from implementation import *
test_with_custom_order(None)
#+end_src

#+results:
#+begin_example
__________________________________________________________________________________________
 >  >  >  v  v  v  v  v  v  v  v  v  v  v  v  v  <  <  <  <  < ###### .  .  .  .  .  .  . 
 >  >  >  >  v  v  v  v  v  v  v  v  v  v  v  <  <  <  <  <  < ###### .  .  .  v  .  .  . 
 >  >  >  >  >  v  v  v  v  v  v  v  v  v  <  <  <  <  <  <  < ###### .  .  v  v  Z  v  . 
 >  >  ^ ###### v  v  v  v  v  v  v  @  @  @  @  <  <  <  <  < ###### .  >  >  v  @  v  v 
 >  ^  ^ ###### v  v  v  v  v  v  @  @ ###### @  @  <  <  <  < ###### >  >  >  v  @  v  v 
 ^  ^  ^ ###### >  v  v  v  v  @  @  < ###### ^  @  @  <  <  < ############### v  @  v  < 
 ^  ^  ^ ###### >  >  v  v  @  @  <  < ###### ^  ^  @  @  <  < ############### v  @  <  < 
 v  v  v ###### >  >  >  A  @  <  <  < ###### ^  ^  ^  @  @  @  @  @  @  @  @  @  @  <  < 
 v  v  v ###### >  >  ^  ^  ^  <  <  < ###### ^  ^  ^  ^  ^  <  <  <  <  <  <  <  <  <  < 
 v  v  v ###### >  ^  ^  ^  ^  ^  <  < ###### ^  ^  ^  ^  ^  ^  <  <  <  <  <  <  <  <  < 
 >  v  v ###### ^  ^  ^  ^  ^  ^  ^  < ###### ^  ^  ^  ^  ^  ^  ^  <  <  <  <  <  <  <  < 
 >  >  v ###### ^  ^  ^  ^  ^  ^  ^  ^ ###### ^  ^  ^  ^  ^  ^  ^  ^  <  <  <  <  <  <  < 
 >  >  >  >  >  ^  ^  ^  ^  ^  ^  ^  ^ ###### ^  ^  ^  ^  ^  ^  ^  ^  ^  <  <  <  <  <  . 
 >  >  >  >  ^  ^  ^  ^  ^  ^  ^  ^  ^ ###### ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  <  <  <  .  . 
 >  >  >  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^ ###### ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  <  .  .  . 
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
#+end_example

Here's the code:

#+begin_src python :results none
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
#+end_src

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.)

*** Checkerboard movement costs
:PROPERTIES:
:CUSTOM_ID: ties-checkerboard-costs
:END:

The direction order hack above works with Breadth First Search, but does it work with A*? Let's try:

#+begin_src python
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)
#+end_src

#+results:
#+begin_example
__________________________________________________________________________________________
 .  .  .  .  .  v  v  v  v  v  v  v  v  v  v  v  v  v  v  v  v ###### .  .  .  .  .  .  . 
 .  .  .  .  v  v  v  v  v  v  v  v  v  v  v  v  v  v  v  v  v ###### .  .  .  v  .  .  . 
 .  .  .  >  >  >  >  >  v  <  <  <  <  <  <  <  <  <  <  <  < ###### .  .  >  @  Z  .  . 
 .  .  . ###### >  >  >  @  @  @  @  @  @  @  @  @  @  @  @  @ ###### .  .  >  @  <  .  . 
 .  .  . ###### >  >  >  @  <  <  <  < ###### ^  ^  ^  ^  ^  @ ###### .  .  >  @  <  .  . 
 .  .  . ###### >  >  >  @  <  <  <  < ###### ^  ^  ^  ^  ^  @ ############### @  <  .  . 
 .  .  . ###### >  >  >  @  <  <  <  < ###### ^  ^  ^  ^  ^  @ ############### @  <  .  . 
 .  .  . ###### >  >  >  A  <  <  <  < ###### ^  ^  ^  ^  ^  @  @  @  @  @  @  @  <  .  . 
 .  .  . ###### ^  ^  ^  ^  ^  ^  ^  ^ ###### ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  .  .  . 
 .  .  . ###### ^  ^  ^  ^  ^  ^  ^  ^ ###### .  .  .  .  .  .  .  .  .  .  .  .  .  .  . 
 .  .  . ###### .  ^  ^  ^  ^  ^  ^  ^ ###### .  .  .  .  .  .  .  .  .  .  .  .  .  .  . 
 .  .  . ###### .  .  ^  ^  ^  ^  ^  ^ ###### .  .  .  .  .  .  .  .  .  .  .  .  .  .  . 
 .  .  .  .  .  .  .  .  ^  ^  ^  ^  ^ ###### .  .  .  .  .  .  .  .  .  .  .  .  .  .  . 
 .  .  .  .  .  .  .  .  .  .  .  .  . ###### .  .  .  .  .  .  .  .  .  .  .  .  .  .  . 
 .  .  .  .  .  .  .  .  .  .  .  .  . ###### .  .  .  .  .  .  .  .  .  .  .  .  .  .  . 
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
#+end_example

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 =(x + y) % 2=:

- when 0: make horizontal movement slightly more expensive
- when 1: make vertical movement slightly more expensive

#+begin_src python
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)
#+end_src

#+results:
#+begin_example
__________________________________________________________________________________________
 .  .  .  .  .  v  v  v  v  v  v  v  v  v  v  v  <  v  <  v  < ###### .  .  .  .  .  .  . 
 .  .  .  .  v  v  v  v  v  v  v  v  v  v  v  <  v  <  v  <  v ###### .  .  .  .  .  .  . 
 .  .  .  >  >  v  >  v  v  v  v  v  v  v  <  <  <  <  <  <  < ###### .  .  .  v  Z  .  . 
 .  .  . ###### >  v  v  v  v  v  v  @  @  @  @  @  @  <  <  < ###### .  .  >  v  @  <  . 
 .  .  . ###### v  >  v  v  v  v  @  @ ###### ^  <  @  @  ^  < ###### .  .  >  v  @  <  . 
 .  .  . ###### >  v  >  v  v  @  @  < ###### ^  ^  <  @  @  ^ ############### v  @  <  . 
 .  .  . ###### >  >  v  v  @  @  <  < ###### ^  ^  ^  <  @  @ ############### v  @  <  . 
 .  .  . ###### >  >  >  A  @  <  <  < ###### ^  ^  ^  ^  <  @  @  @  @  @  @  @  @  <  . 
 .  .  . ###### >  >  ^  ^  ^  <  ^  < ###### ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  .  . 
 .  .  . ###### >  ^  ^  ^  ^  ^  <  ^ ###### .  .  .  .  .  .  .  .  .  .  .  .  .  .  . 
 .  .  . ###### .  ^  ^  ^  ^  ^  ^  < ###### .  .  .  .  .  .  .  .  .  .  .  .  .  .  . 
 .  .  . ###### .  .  ^  ^  ^  ^  ^  ^ ###### .  .  .  .  .  .  .  .  .  .  .  .  .  .  . 
 .  .  .  .  .  .  .  .  ^  ^  ^  ^  ^ ###### .  .  .  .  .  .  .  .  .  .  .  .  .  .  . 
 .  .  .  .  .  .  .  .  .  .  .  .  . ###### .  .  .  .  .  .  .  .  .  .  .  .  .  .  . 
 .  .  .  .  .  .  .  .  .  .  .  .  . ###### .  .  .  .  .  .  .  .  .  .  .  .  .  .  . 
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
#+end_example

This works nicely with 4-way movement.

Here's the code:

#+begin_src python :tangle yes
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
#+end_src


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.)

*** 8-way movement
:PROPERTIES:
:CUSTOM_ID: ties-diagonals
:END:

The 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:

#+begin_src python :results output
from implementation import *
test_with_custom_order([(-1, -1), (-1, +1), (+1, -1), (+1, +1), (+1, 0), (0, -1), (-1, 0), (0, +1)])
#+end_src

#+results:
#+begin_example
__________________________________________________________________________________________
 v  >  v  v  v  v  v  v  v  v  v  v  v  v  v  v  <  v  <  v  < ###### .  .  .  .  .  .  . 
 >  ^  >  v  v  v  v  v  v  v  v  v  v  v  @  <  ^  <  ^  <  ^ ###### v  v  v  v  v  v  v 
 ^  >  ^  >  v  v  v  v  v  v  v  v  v  @  <  @  <  ^  <  ^  < ###### v  v  v  v  Z  v  v 
 >  ^  ^ ###### v  v  v  v  v  v  v  @  <  ^  <  @  <  ^  <  ^ ###### >  v  v  v  @  v  v 
 ^  ^  ^ ###### v  v  v  v  v  v  @  < ###### ^  <  @  <  ^  < ###### ^  >  v  @  v  v  v 
 ^  ^  ^ ###### >  v  v  v  v  @  <  ^ ###### ^  ^  <  @  <  ^ ############### v  @  v  < 
 ^  ^  ^ ###### ^  >  v  v  @  <  ^  < ###### ^  ^  ^  <  @  < ############### @  v  <  ^ 
 ^  ^  ^ ###### >  ^  >  A  <  ^  <  ^ ###### ^  ^  ^  ^  <  @  <  @  <  @  @  v  <  ^  < 
 v  v  v ###### ^  >  ^  ^  ^  <  ^  < ###### ^  ^  ^  ^  ^  <  @  <  @  <  ^  <  ^  <  ^ 
 v  v  v ###### >  ^  ^  ^  ^  ^  <  ^ ###### ^  ^  ^  ^  ^  ^  <  ^  <  ^  <  ^  <  ^  < 
 v  v  v ###### ^  ^  ^  ^  ^  ^  ^  < ###### ^  ^  ^  ^  ^  ^  ^  <  ^  <  ^  <  ^  <  ^ 
 >  v  v ###### ^  ^  ^  ^  ^  ^  ^  ^ ###### ^  ^  ^  ^  ^  ^  ^  ^  <  ^  <  ^  <  ^  < 
 ^  >  v  >  ^  ^  ^  ^  ^  ^  ^  ^  ^ ###### ^  ^  ^  ^  ^  ^  ^  ^  ^  <  ^  <  ^  <  ^ 
 >  ^  >  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^ ###### ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  <  ^  <  ^  < 
 ^  >  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^ ###### ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  <  ^  <  ^ 
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
#+end_example

The 4-way tie-breaking hacks can be extended to work here:

- Breadth First Search: make sure the cardinal neighbors (N, S, E, W) come before the diagonal neighbors (NE, NW, SE, SW).
- Dijkstra's Algorthm, A*: add a tiny movement penalty (0.001) to diagonal movements.

After using one of these hacks, the path will look like this:

#+begin_src python :results output
from implementation import *
test_with_custom_order([(+1, 0), (0, -1), (-1, 0), (0, +1), (-1, -1), (-1, +1), (+1, -1), (+1, +1)])
#+end_src

#+results:
#+begin_example
__________________________________________________________________________________________
 v  v  v  v  v  v  v  v  v  v  v  v  v  v  v  v  v  v  v  v  v ###### .  .  .  .  .  .  . 
 v  v  v  v  v  v  v  v  v  v  v  v  v  v  v  v  v  v  v  v  v ###### .  v  v  v  v  .  . 
 >  >  >  >  v  v  v  v  v  v  v  v  v  v  v  v  v  v  v  v  v ###### v  v  v  v  Z  v  v 
 ^  ^  ^ ###### v  v  v  v  v  v  v  v  @  @  @  @  @  <  <  < ###### v  v  v  @  v  v  v 
 ^  ^  ^ ###### v  v  v  v  v  v  v  @ ###### ^  ^  ^  @  ^  ^ ###### >  >  v  @  v  v  v 
 ^  ^  ^ ###### v  v  v  v  v  v  @  v ###### ^  ^  ^  ^  @  ^ ############### @  v  v  v 
 ^  ^  ^ ###### v  v  v  v  v  @  v  v ###### ^  ^  ^  ^  ^  @ ############### @  v  v  v 
 ^  ^  ^ ###### >  >  >  A  @  <  <  < ###### ^  ^  ^  ^  ^  ^  @  @  @  @  @  <  <  <  < 
 v  v  v ###### ^  ^  ^  ^  ^  ^  ^  ^ ###### ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^ 
 v  v  v ###### ^  ^  ^  ^  ^  ^  ^  ^ ###### ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^ 
 v  v  v ###### ^  ^  ^  ^  ^  ^  ^  ^ ###### ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^ 
 v  v  v ###### ^  ^  ^  ^  ^  ^  ^  ^ ###### ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^ 
 >  >  >  >  ^  ^  ^  ^  ^  ^  ^  ^  ^ ###### ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^ 
 ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^ ###### ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^ 
 ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^ ###### ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^ 
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
#+end_example

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][Ugly paths]] section.

* Vocabulary
:PROPERTIES:
:CUSTOM_ID: terminology
:END:

Algorithms textbooks often use mathematical notation with single-letter variable names. On 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 or fringe
  - locations such as =current= and =next= are called /states/ or /nodes/ and written with letters /u/, /v/

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:

- The /elements/ of the =frontier= are the OPEN set and the /priorities/ of the =frontier= are the associated priority values.
- The /keys/ of the =came_from= map are the reached set and the /values/ of the =came_from= map are the parent pointers. Alternatively, if you want to keep the costs, the /keys/ of the =cost_so_far= map are the reached set and the /values/ of the =cost_so_far= map are the costs.
- the reached set is the union of OPEN and CLOSED.

We can reason about the OPEN, CLOSED, and reached sets even though they're not stored in a separate data structure.

* More reading
:PROPERTIES:
:CUSTOM_ID: more
:END:

License: all the sample code on this page is free to use in your projects. If you need a license for it, you can treat it as Apache v2 licensed by Red Blob Games.

- Aleksander Nowak has written a *Go version* of this code at https://github.com/vyrwu/a-star-redblob
- sark has written a *Rust version* of this code at https://github.com/sarkahn/sark_pathfinding_rs

- Wikipedia links: 
  - [[https://en.wikipedia.org/wiki/Queue_(abstract_data_type)][Queue]]
  - [[https://en.wikipedia.org/wiki/Graph_(data_structure)][Graph]]
  - [[https://en.wikipedia.org/wiki/Breadth-first_search][Breadth-First Search]]
  - (Greedy) [[https://en.wikipedia.org/wiki/Best-first_search][Best-First Search]]
  - [[https://en.wikipedia.org/wiki/Dijkstra's_algorithm][Dijkstra's Algorithm]]
  - [[https://en.wikipedia.org/wiki/A*_search_algorithm][A* Algorithm]]

#+begin_src js :tangle yes :exports none
/* This code goes through the babel output and 
   adds some color to make it easier for the reader */

function htmlEscape(rawString) {
    return rawString
        .replace(/&/g, "&amp;").replace(/"/g, "&quot;")
        .replace(/</g, "&lt;").replace(/>/g, "&gt;");
}

function prettifyAsciiOutput() {
    for (let pre of document.querySelectorAll("pre.example")) {
        let output = [];
        let lines = pre.textContent.split("\n");
        /* Find sections between markers */
        for (let i = 0, start = null; i < lines.length; i++) {
            if (start === null && /^_+$/.test(lines[i])) {
                start = i;
            } else if (start !== null && /^~+$/.test(lines[i])) {
                let html = htmlEscape(lines.slice(start+1, i).join("\n"));
                html = html.replace(/ &lt; /g, "<span>←</span>");
                html = html.replace(/ &gt; /g, "<span>→</span>");
                html = html.replace(/ v /g, "<span>↓</span>");
                html = html.replace(/ \^ /g, "<span>↑</span>");
                html = html.replace(/ [AZ] /g, "<i>$&</i>");
                html = html.replace(/( @ )+/g, "<b>$&</b>");
                html = html.replace(/#+/g, "<strong>$&</strong>");
                output.push(html);
                start = null;
            } else if (start === null) {
                output.push(htmlEscape(lines[i]));
            }
        }
        pre.innerHTML = output.join("\n");
    }
}

requestAnimationFrame(prettifyAsciiOutput);
#+end_src

#+begin_export html
<x:head>
  <script src="implementation.js" defer="defer"></script>
</x:head>
#+end_export

#+begin_footer
Created {{{date(%e %b %Y)}}} with [[https://orgmode.org/][Emacs Org-mode]], from [[./implementation.org][implementation.org]]. Last modified: {{{modification-time(%e %b %Y)}}}
#+end_footer