A* Search Algorithm

As I mentioned in my last post, I read the article “A* search: what’s in a name?” by James W. Davis and Jeff Hachtel. If interested, you may find the article in Communications of the ACM, December 2019, Vol. 63 No. 1, Pages 36-37. The article deals with the name of a search algorithm that was originally published in 1968 by Peter Hart, Nils Nilsson and Bertram Raphael of Stanford Research Institute.

My interest was not much about the name but to be able to experiment and understand the algorithm. There are so many algorithms in Computer Science that adding one more to my list is always welcomed. I do not plan on memorizing the algorithm but if the time comes when I might need to search a graph, I will have one more choice in my toolkit.

I started by doing a Google search on the algorithm and reading a few articles. The one that seems to describe it reasonably well is “A* search algorithm – Wikipedia”. In addition the “Implementing A Star (A*) Search Algorithm in Java for Path finding in Games” by Sylvain Saurel is worth watching.

The code is written in Java using the Visual Studio Code IDE.

heuristicCosts:
2   1   0
3   2   1
4   3   2

grid:
0   0   EC
0   BC  0
SC  0   0
0: Open Cell BC: Blocked Cell EC : End Cell CS: Start Cell

updateCostIfNeeded <<< current: (2,0) t: (1,0) tFinalCost: 13 t.finalCost: 13       
updateCostIfNeeded <<< current: (2,0) t: (2,1) tFinalCost: 13 t.finalCost: 13       

updateCostIfNeeded <<< current: (1,0) t: (0,0) tFinalCost: 25 t.finalCost: 25       
updateCostIfNeeded <<< current: (1,0) t: (0,1) tFinalCost: 28 t.finalCost: 28       
updateCostIfNeeded <<< current: (1,0) t: (2,1) tFinalCost: 30 t.finalCost: 13       

updateCostIfNeeded <<< current: (2,1) t: (1,2) tFinalCost: 28 t.finalCost: 28       
updateCostIfNeeded <<< current: (2,1) t: (2,2) tFinalCost: 25 t.finalCost: 25       

updateCostIfNeeded <<< current: (0,0) t: (0,1) tFinalCost: 36 t.finalCost: 28       

updateCostIfNeeded <<< current: (2,2) t: (1,2) tFinalCost: 36 t.finalCost: 28       

updateCostIfNeeded <<< current: (1,2) t: (0,2) tFinalCost: 38 t.finalCost: 38       
updateCostIfNeeded <<< current: (1,2) t: (0,1) tFinalCost: 43 t.finalCost: 28       

updateCostIfNeeded <<< current: (0,1) t: (0,2) tFinalCost: 38 t.finalCost: 38 closedCells: T T T T F T T T T scores: 25 28 38 13 BC 28 0 13 25 path: (2,0) -> (2,1) -> (1,2) -> (0,2)

grid:
0   0   EC
0   BC  X
SC  X   0

This is a very simple 3 x 3 graph. It seems that the algorithm initializes an array with what is known as the Manhattan Distance or Taxicab Geometry. For example, if we start at the lower left corner (2, 0) and want to compute the distance to the end corner at (0, 2) we would count 2 cells in the vertical direction and another 2 in the horizontal direction for a total of 2 + 2 == 4.

The grid illustrates that the start cell (SC) is located at (2, 0) and the end cell (EC) is at (0, 2). In addition we have a blocked cell (BC) at (1, 1).

It seems that the search makes use of the method updateCostIfNeeded() which is called multiple times. Let’s take the start cell (2, 0) and attempt to figure out the calculation that is going on. From (2, 0) we can only move up or right. The final cost would be the same (13 in this case). By looking at cells (0, 0) and (2, 2) as expected the cost should also be the same given that there are no other shorter or longer paths. You can visualize this better by looking at the scores after the closed cells.

The search seems to return a path from the lower left corner moving right, then diagonally up and finally up to reach the end cell at the upper right corner. The grid shows the selected path. The algorithm could as well have chosen the similar that using up, diagonal and right. Given that both paths are equivalent the resulting one is just an artifact of the implementation.

  /**
   * Test scaffolding.
   */
  public static void main(String[] args) {

    // **** ****
    // AStar aStar = new AStar(5, 5, 0, 0, 3, 2,
    // new int[][] { { 0, 4 }, { 2, 2 }, { 3, 1 }, { 3, 3 }, { 2, 1 }, { 2, 3 } });
    // AStar aStar = new AStar(7, 6, 6, 0, 0, 5,
    // new int[][] { { 4, 0 }, { 5, 1 }, { 0, 4 }, { 1, 5 }, { 2, 1 }, { 2, 2 }, {
    // 2, 3 }, { 3, 3 }, { 4, 3 } });
    AStar aStar = new AStar(3, 3, 2, 0, 0, 2, new int[][] { { 1, 1 } });

    // **** display the grid of cells ****
    aStar.displayGrid();

    // **** search the grid for a path from the start to the end cell ****
    aStar.search();

    // **** display the closed cells ****
    aStar.displayClosedCells();

    // **** display the scores ****
    aStar.displayScores();

    // **** display solution path (if any) ****
    aStar.displayPath();
  }

We start by instantiating the AStar class passing the following arguments:

Arguments Description
width, height Width and height dimensions for the grid.
startI, startJ Coordinates for the start cell.
endI, endJ Coordinates for the end cell.
blocked cells Array of coordinates for blocked cells.

The last call to the constructor that was used for the screen capture indicates a 3 x 3 grid with the start cell at (2, 0), the end cell at (0, 2) and the only blocked cell at (1, 1). Please feel free to change different arguments in order to better understand how the algorithm works.

/**
 * Cell class used to populate the grid.
 */
class Cell {

  // **** members ****
  public int i; // row coordinate (vertical)
  public int j; // column coordinate (horizontal)

  public Cell parent; // parent for this cell

  public int heuristicCost;
  public int finalCost;

  public boolean solution; // cell is part of the solution path

  /**
   * Constructor.
   */
  public Cell(int i, int j) {
    this.i = i;
    this.j = j;
  }

  /**
   * Return string representation of Cell. (more fields to come)
   */
  public String toString() {
    return "(" + this.i + "," + this.j + ")";
  }
}

The Cell class is used to hold information needed by the algorithm for each unblocked cell. There is a reference to the parent which will be used to construct the path from the end to the start cell. A couple of variables hold the heuristic and final costs for the cell. The solution flag is used to determine which cells are parts of the solution path.

import java.util.PriorityQueue;
import java.util.Stack;

/**
 * A* (AStar) selects the path that minimizes:
 *
 * f(n) = g(n) + h(n)
 *
 * where n is the next node on the path, g(n) is the cost of the path from the
 * start node to n, and h(n) is a heuristic function that estimates the cost of
 * the cheapest path from n to the goal.
 */
public class AStar {

  // **** cost for diagonal and vertical / horizontal moves ****
  public static final int DIAGONAL_COST = 14;
  public static final int V_H_COST = 10;

  // **** cells for the grid ****
  private Cell[][] grid;

  // **** priority queue for open cells
  // open cells: set of noded to be evaluated
  // we insert cells with lowest cost first ****
  private PriorityQueue<Cell> openCells;

  // **** closed cells: flags if cell has already been evaluated evaluated ****
  private boolean[][] closedCells;

  // **** start cell coordinates ****
  private int startI;
  private int startJ;

  // **** end cell coordinates ****
  private int endI;
  private int endJ;
  
  // ???? each method in this class will be described separately ????
  
}

The AStar class has a couple constants which will be used to estimate the heuristic costs of the algorithm. You can use larger grids with larger values if needed. I would suggest trying different values to better understand how these ones were selected. The values should be larger than what is expected in the vertical and horizontal and in the diagonal based on the Manhattan distance.

We have defined the cell of grids which will be used to place cells. The priority queue will be used by the algorithm to keep the different cells in order representing the different possible trees (routes). The closedCells array prevents the algorithm processing the same cell multiple times. Finally we have the coordinates for the start and end cells.

  /**
   * Constructor.
   */
  public AStar(int width, int height, int si, int sj, int ei, int ej, int[][] blocks) {

    // **** declare the grid based on the desired width and height ****
    grid = new Cell[width][height];

    // **** ****
    closedCells = new boolean[width][height];

    // **** priority queue with comparator ****
    openCells = new PriorityQueue<Cell>((Cell c1, Cell c2) -> {
      return c1.finalCost < c2.finalCost ? -1 : c1.finalCost > c2.finalCost ? 1 : 0;
    });

    // **** set the coordinates for the start cell ****
    startCell(si, sj);

    // **** set the coordinates for the end cell ****
    endCell(ei, ej);

    // **** initialize cells ****
    for (int i = 0; i < grid.length; i++) {
      for (int j = 0; j < grid[i].length; j++) {
      
        // **** cell for this grid location ****
        grid[i][j] = new Cell(i, j);

        // **** manhattan distance from this to the end cell ****
        grid[i][j].heuristicCost = Math.abs(i - endI) + Math.abs(j - endJ);

        // **** not part of solution yet ****
        grid[i][j].solution = false;
      }
    }

    // ???? ????
    System.out.println("heuristicCosts:");
    for (int i = 0; i < grid.length; i++) {
      for (int j = 0; j < grid[i].length; j++) {
        System.out.printf("%-3d ", grid[i][j].heuristicCost);
      }
      System.out.println();
    }
    System.out.println();

    // **** *****
    grid[startI][startJ].finalCost = 0;

    // **** put blocks on the grid ****
    for (int i = 0; i < blocks.length; i++) {
      addBlockOnCell(blocks[i][0], blocks[i][1]);
    }
  }

We start by declaring the grid based on the specified dimensions. We then declare the priority queue to hold the open cells. We set the coordinates for the start and end cells.

We then loop through the grid populating it with cells. For each cell we set the coordinates, the Manhattan distance from each cell to the end cell and flag that the cell is not part of the solution path yet.

The program displays the grid showing the initial heuristic costs. We clear the final cost for the start cell. Finally we loop adding block cells to the grid.

 /**
   * Add a block to this cell in the grid. We do so by removing the cell from the
   * grid.
   */
  public void addBlockOnCell(int i, int j) {
    grid[i][j] = null;
  }

The addBlockOnCell() method removes the cell from the grid at the specified location.

  /**
   * Set start cell coordinates.
   */
  public void startCell(int i, int j) {
    startI = i;
    startJ = j;
  }

This method sets the coordinates for the start cell.

  /**
   * Set end cell coordinates.
   */
  public void endCell(int i, int j) {
    endI = i;
    endJ = j;
  }

This method sets the coordinates for the end cell.

  /**
   * Update the cost of cell t and the parent. We will use the parent to display
   * the path from the end cell back to the start cell.
   */
  public void updateCostIfNeeded(Cell current, Cell t, int cost) {

    // **** check if there is no need to update cost ****
    if ((t == null) || closedCells[t.i][t.j]) {
      return;
    }

    // ???? ????
    System.out.print("updateCostIfNeeded <<< current: " + current.toString() + " t: " + t.toString());

    // **** ****
    int tFinalCost = t.heuristicCost + cost;

    // **** determine if this cell is in the queue ****
    boolean isOpen = openCells.contains(t);

    // **** add the updated cell to the priority queue (if needed) ****
    if (!isOpen || (tFinalCost < t.finalCost)) {

      // **** update the final cost and parent to the cell ****
      t.finalCost = tFinalCost;
      t.parent = current;

      // **** add the cell to the priority queue (if needed) ****
      if (!isOpen) {
        openCells.add(t);
      }
    }

    // ???? ????
    System.out.println(" tFinalCost: " + tFinalCost + " t.finalCost: " + t.finalCost);
  }

This method determines if we need to update the cost of the specified cell. We compute the final cost for the cell. We check if the cell is not in the priority queue. We then add the cell if not in the priority queue or if the new cost is lower than the current cost.

  /**
   * Search the grid for a path from the starting to the end cell.
   */
  public void search() {

    // // ???? ????
    // System.out.println("process <<< t: " + grid[startI][startJ].toString()); // **** add start location to the open cells queue **** openCells.add(grid[startI][startJ]); // **** **** Cell current; // **** loop until queue is empty (will visit all cells) **** while (true) { // **** retrieve and remove the head cell from the queue **** current = openCells.poll(); // **** check if we are done looping **** if (current == null) { break; } // **** **** closedCells[current.i][current.j] = true; // **** check if we reached the end cell **** if (current.equals(grid[endI][endJ])) { return; } // **** temporary cell **** Cell t; // **** top 3 cells **** if (current.i - 1 >= 0) {

        // **** top cell ****
        t = grid[current.i - 1][current.j];
        updateCostIfNeeded(current, t, current.finalCost + V_H_COST);

        // **** top left diagonal cell ****
        if (current.j - 1 >= 0) {
          t = grid[current.i - 1][current.j - 1];
          updateCostIfNeeded(current, t, current.finalCost + DIAGONAL_COST);
        }

        // **** top right diagonal cell ****
        if (current.j + 1 < grid[0].length) { t = grid[current.i - 1][current.j + 1]; updateCostIfNeeded(current, t, current.finalCost + DIAGONAL_COST); } } // **** left cell **** if (current.j - 1 >= 0) {
        t = grid[current.i][current.j - 1];
        updateCostIfNeeded(current, t, current.finalCost + V_H_COST);
      }

      // **** right cell ****
      if (current.j + 1 < grid[0].length) {
        t = grid[current.i][current.j + 1];
        updateCostIfNeeded(current, t, current.finalCost + V_H_COST);
      }

      // **** bottom 3 cells ****
      if (current.i + 1 < grid.length) { // **** bottom cell **** t = grid[current.i + 1][current.j]; updateCostIfNeeded(current, t, current.finalCost + V_H_COST); // **** bottom left diagonal cell **** if (current.j - 1 >= 0) {
          t = grid[current.i + 1][current.j - 1];
          updateCostIfNeeded(current, t, current.finalCost + DIAGONAL_COST);
        }

        // **** bottom righ diagonal cell ****
        if (current.j + 1 < grid[0].length) {
          t = grid[current.i + 1][current.j + 1];
          updateCostIfNeeded(current, t, current.finalCost + DIAGONAL_COST);
        }
      }

      // ???? ????
      System.out.println();
    }
  }

The search method is used to find the path from the start cell to the end cell. This is done by using the priority queue and building paths by updating the cost of neighboring cells.

  /**
   * Display the grid of cells.
   */
  public void displayGrid() {

    // **** display label ****
    System.out.println("grid: ");

    // **** traverse the grid ****
    for (int i = 0; i < grid.length; i++) {
      for (int j = 0; j < grid[i].length; j++) {
        if ((i == startI) && (j == startJ)) {
          System.out.print("SC  "); // start cell
        } else if ((i == endI) && (j == endJ)) {
          System.out.print("EC  "); // end cell
        } else if (grid[i][j] != null) {
          System.out.printf("%-3d ", 0); // open cell
        } else {
          System.out.print("BC  "); // blocked cell
        }

      }
      System.out.println();
    }
    System.out.println("0: Open Cell BC: Blocked Cell EC : End Cell CS: Start Cell\n");
  }

The displayGrid() method displays the cells in the grid.

  /**
   * Display scores of cells.
   */
  public void displayScores() {

    // **** display a label ****
    System.out.println("scores:");

    // **** traverse the grid cells ****
    for (int i = 0; i < grid.length; i++) {
      for (int j = 0; j < grid[i].length; j++) {
        if (grid[i][j] != null) {
          System.out.printf("%-3d ", grid[i][j].finalCost);
        } else {
          System.out.print("BC  "); // blocked cell
        }
      }
      System.out.println();
    }
    System.out.println();
  }

The displayScores() method displays the scores for the cells in the grid.

  /**
   * Display the points used in the path from the start to the end cell. Then
   * display the contents of the grid.
   */
  public void displayPath() {

    // **** end cell must be a closed cell ****
    if (closedCells[endI][endJ]) {

      // **** stack to reverse the path ****
      Stack<Cell> stack = new Stack<Cell>();

      // **** start with the end cell ****
      Cell current = grid[endI][endJ];

      // **** push the current cell into the stack ****
      stack.add(current);

      // **** flag that the current cell is part of the solution path ****
      grid[current.i][current.j].solution = true;

     // **** loop until we get to the start cell ****
      while (current.parent != null) {

        // **** push the parent cell into the stack ****
        stack.add(current.parent);

        // **** flag that the parent cell is part of the solution ****
        grid[current.parent.i][current.parent.j].solution = true;

        // **** update the current reference ****
        current = current.parent;
      }

      // **** display path from start to end cell ****
      System.out.print("path: ");
      while (!stack.isEmpty()) {
        Cell c = stack.pop();
        if (stack.size() > 0) {
          System.out.print(c + " -> ");
        } else {
          System.out.print(c);
        }
      }

      // **** for the looks ****
      System.out.println("\n\ngrid:");

      // **** traverse the grid cells ****
      for (int i = 0; i < grid.length; i++) {
        for (int j = 0; j < grid[i].length; j++) {
          if ((i == startI) && (j == startJ)) {
            System.out.print("SC  "); // start cell
          } else if ((i == endI) && (j == endJ)) {
            System.out.print("EC  "); // end cell
          } else if (grid[i][j] != null) {
            System.out.printf("%-3s ", grid[i][j].solution ? "X" : "0");
          } else {
            System.out.print("BC  "); // blocked cell
          }
        }
        System.out.println();
      }
      System.out.println();
    } else {
      System.out.println("path from (" + startI + "," + startJ + ") to (" + endI + "," + endJ + ") NOT found :o(");
    }
  }

The displayPath() method does more than displaying the path from the start to the end cell. We make use of a stack since we start the traversal from the end cell.

After some checks and settings we loop using the parent reference until we get to the start cell.

For convenience we display the path from start to end (not end to start).

We then display the grid indicating the solution path.

Note that if the end point is not flagged as a closed cell we indicate that a path from the start to the end cell has not been found.

  /**
   * Display the array of closed cells.
   */
  public void displayClosedCells() {

    // **** display header ****
    System.out.println("closedCells:");

    // **** ****
    for (int i = 0; i < closedCells.length; i++) {
      for (int j = 0; j < closedCells[0].length; j++) {
        System.out.printf("%-3s ", closedCells[i][j] ? "T" : "F");
      }
      System.out.println();
    }
    System.out.println();
  }

The displayClosedCells() method display the array used to flag cells that we have processed.

The core of the algorithm is in the search() and updateCostIfNeeded() methods. Go back and see how we visit all the possible cells that are surrounding (up, diagonally up right, right, diagonally down right, down, diagonally left down, left and diagonally up left) the cell in question. We update the cost.

The entire code for this project can be found in my GitHub repository.

If you have comments or questions regarding this, or any other post in this blog, or if you would like for me to serve of assistance with any phase in the SDLC (Software Development Life Cycle) of a project associated with a product or service, please do not hesitate and leave me a note below. If you prefer, send me a message using the following address:  john.canessa@gmail.com. All messages will remain private.

Keep on reading and experimenting. It is the best way to learn, refresh your knowledge and enhance your developer toolset!

John

Twitter:  @john_canessa

Leave a Reply

Your email address will not be published. Required fields are marked *

This site uses Akismet to reduce spam. Learn how your comment data is processed.