2.4 A* Search

A* is almost exactly like Dijkstra’s Algorithm, except we add in a heuristic. Note that the code for the algorithm isn’t specific to grids. Knowledge about grids is in the graph class (SquareGrids in this case) and in the heuristic function. Replace those two and you can use the A* algorithm code with any other graph structure.

inline double heuristic(SquareGrid::Location a, SquareGrid::Location b) {
  int x1, y1, x2, y2;
  tie (x1, y1) = a;
  tie (x2, y2) = b;
  return abs(x1 - x2) + abs(y1 - y2);
}

and

template<typename Graph>
void a_star_search
  (const Graph& graph,
   typename Graph::Location start,
   typename Graph::Location goal,
   unordered_map<typename Graph::Location, typename Graph::Location>& came_from,
   unordered_map<typename Graph::Location, double>& cost_so_far)
{
  typedef typename Graph::Location Location;
  PriorityQueue<Location, double> frontier;
  frontier.put(start, 0);

  came_from[start] = start;
  cost_so_far[start] = 0;
  
  while (!frontier.empty()) {
    auto current = frontier.get();

    if (current == goal) {
      break;
    }

    for (auto next : graph.neighbors(current)) {
      double new_cost = cost_so_far[current] + graph.cost(current, next);
      if (!cost_so_far.count(next) || new_cost < cost_so_far[next]) {
        cost_so_far[next] = new_cost;
        double priority = new_cost + heuristic(next, goal);
        frontier.put(next, priority);
        came_from[next] = current;
      }
    }
  }
}

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.

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

int main() {
  GridWithWeights grid = make_diagram4();
  SquareGrid::Location start{1, 4};
  SquareGrid::Location goal{8, 5};
  unordered_map<SquareGrid::Location, SquareGrid::Location> came_from;
  unordered_map<SquareGrid::Location, double> cost_so_far;
  a_star_search(grid, start, goal, came_from, cost_so_far);
  draw_grid(grid, 2, nullptr, &came_from);
  std::cout << std::endl;
  draw_grid(grid, 3, &cost_so_far, nullptr);
  std::cout << std::endl;
  vector<SquareGrid::Location> path = reconstruct_path(start, goal, came_from);
  draw_grid(grid, 3, nullptr, nullptr, &path);
}

2.4.1 Straighter paths

If you implement this code in your own project you might find that some of the paths aren’t as “straight” as you’d like. This is normal. When using grids, especially grids where every step has the same movement cost, you end up with ties: many paths have exactly the same cost. A* ends up picking one of the many short paths, and very often it doesn’t look good to you. The quick hack is to break the ties, but it’s not entirely satisfactory. The better approach is to change the map representation, which makes A* a lot faster, and also produces straighter, better looking paths. However, that only works for mostly-static maps where every step has the same movement cost. For the demos on my page, I’m using a quick hack, but it only works with my slow priority queue. If you switch to a faster priority queue you’ll need a different quick hack.

2.4.2 TODO Heuristic function should be template parameter

I should make the heuristic into a template parameter instead of being a global function.