Pathfinding

A* Path finding algorithm

Why?

Path finding is all about finding a route through a maze. From a given start point to a given end point. It will find its way past obsticles, and discover the most direct route.

This has applications in a number of areas from computer games to real world directions.

Background

The Maze

We want to think about how we go about representing a maze. It will be a 2-D grid with an x, y value, and indicators for walls, start and end point.

Just like the Game of Life example, lets try having a structure for the cells and one for the maze.

defmodule Cell do
  defstruct x: nil, y: nil, type: nil

  @wall_type :wall
  @end_type :end_cell
  @start_type :start_cell

  def wall_type do
    @wall_type
  end

  def end_type do
    @end_type
  end

  def is_wall(cell) do
    cell.type == @wall_type
  end

  def is_end(cell) do
    cell.type == @end_type
  end

  def is_start(cell) do
    cell.type == @start_type
  end
end

defmodule Maze do
  defstruct cells: [], x: 0, y: 0

  def initialise_maze(x, y, start_coords, end_coords, walls) do
    cells =
      for i <- y..1,
          j <- 1..x,
          do: %Cell{x: i, y: j, type: find_type({j, i}, start_coords, end_coords, walls)}

    %Maze{x: x, y: y, cells: cells}
  end

  def find_type(current, start_coords, _end_coords, _walls) when current == start_coords do
    :start_cell
  end

  def find_type(current, _start_coords, end_coords, _walls) when current == end_coords do
    :end_cell
  end

  def find_type(current, _start_coords, _end_coords, walls) do
    if Enum.any?(walls, fn x -> x == current end) do
      :wall
    else
      nil
    end
  end

  def print_maze(maze) do
    maze.cells
    |> Enum.map(fn x -> draw_cell(x) end)
    |> Enum.chunk_every(maze.x)
  end

  def draw_cell(cell) do
    case cell.type do
      :wall -> "⬛️"
      :end_cell -> "🌟"
      :start_cell -> "🟢"
      _ -> "⬜️"
    end
  end
end

Lets start with a simple 8 by 8 board and initialise it with a start and stop position and a simple wall.

Maze.initialise_maze(8, 8, {2, 2}, {7, 6}, [{3, 3}, {3, 4}, {4, 4}])
|> Maze.print_maze()

The algorithm itself

We want to start at the first cell and evaluate each of its immediate neighbours and come up with a score for how good that cell is.

$$F = G + H$$

This is saying the fitness of a cell is:

G is the distance between the current node and the start node.

H is the heuristic — estimated distance from the current node to the end node.

See Easy A* (star) Pathfinding

Lets see if we can calulate this value.

defmodule Maze do
  defstruct cells: [], x: 0, y: 0

  def initialise_maze(x, y, start_coords, end_coords, walls) do
    cells =
      for i <- y..1,
          j <- 1..x,
          do: %Cell{x: i, y: j, type: find_type({j, i}, start_coords, end_coords, walls)}

    %Maze{x: x, y: y, cells: cells}
  end

  def calc_fitness(cell, maze) do
    start_cell = find_start(maze)
    end_cell = find_end(maze)
    g = abs(cell.x - start_cell.x) + abs(cell.y - start_cell.y)
    IO.inspect(g)
    h = Integer.pow(end_cell.x - cell.x, 2) + Integer.pow(end_cell.y - cell.y, 2)
    IO.inspect(h)
    g + h
  end

  def get_neighbours(cell, maze) do
    maze.cells
    |> Enum.filter(fn x ->
      x != cell and x.x >= cell.x - 1 and x.x <= cell.x + 1 and x.y >= cell.y - 1 and
        x.y <= cell.y + 1
    end)
  end

  def find_start(maze) do
    maze.cells |> Enum.find(fn cell -> cell.type == :start_cell end)
  end

  def find_end(maze) do
    maze.cells |> Enum.find(fn cell -> cell.type == :end_cell end)
  end

  def find_type(current, start_coords, _end_coords, _walls) when current == start_coords do
    :start_cell
  end

  def find_type(current, _start_coords, end_coords, _walls) when current == end_coords do
    :end_cell
  end

  def find_type(current, _start_coords, _end_coords, walls) do
    if Enum.any?(walls, fn x -> x == current end) do
      :wall
    else
      nil
    end
  end

  def print_maze(maze) do
    maze.cells
    |> Enum.map(fn x -> draw_cell(x) end)
    |> Enum.chunk_every(maze.x)
  end

  def draw_cell(cell) do
    case cell.type do
      :wall -> "⬛️"
      :end_cell -> "🌟"
      :start_cell -> "🟢"
      _ -> "⬜️"
    end
  end
end

test_cell = %Cell{x: 2, y: 3}
maze = Maze.initialise_maze(8, 8, {2, 2}, {8, 8}, [])

Maze.calc_fitness(test_cell, maze)

# Lets try and get all the neighbours for a given cell

Maze.get_neighbours(test_cell, maze)
|> Enum.map(fn x -> Maze.calc_fitness(x, maze) end)

Algorithm steps

1) Add the starting square (or node) to the open list.

2) Repeat the following:

A) Look for the lowest F cost square on the open list. We refer to this as the current square.

B) Switch it to the closed list.

C) For each of the 8 squares adjacent to this current square

• If it is not walkable or if it is on the closed list, ignore it. Otherwise do the following.
• If it isn’t on the open list, add it to the open list. Make the current square the parent of this square. Record the F, G, and H costs of the square.
• If it is on the open list already, check to see if this path to that square is better, using G cost as the measure. A lower G cost means that this is a better path. If so, change the parent of the square to the current square, and recalculate the G and F scores of the square. If you are keeping your open list sorted by F score, you may need to resort the list to account for the change.

D) Stop when you:

Add the target square to the closed list, in which case the path has been found, or Fail to find the target square, and the open list is empty. In this case, there is no path.

3) Save the path. Working backwards from the target square, go from each square to its parent square until you reach the starting square. That is your path.

There is alot going on there, so lets start with running the maze with an open and closed list. We also want to keep track of the parent of the node, so we have a chain of NodeA -> NodeB -> NodeC

defmodule RunMaze do
  def run(maze) do
    start_node = Maze.find_start(maze)
    iterate(maze, [start_node], [], [])
  end

  # End conditon - an empty open_list
  def iterate(maze, [], closed_list, path) do
    path
  end

  def iterate(maze, open_list, closed_list, path) do
    # Look for the lowest F cost square on the open list. We refer to this as the current square
  end
end

Because we are going to be using the fitness terms F, G and H alot I am going to redefine the cell structure to attach them to the cell, to make it easy to record and sort by. The Maze calculation will now record these values when caclulated.

defmodule Cell do
  defstruct x: nil, y: nil, type: nil, f: nil, g: nil, h: nil

  def is_walkable?(cell) do
    cell.type != :wall
  end
end

defmodule Maze do
  defstruct cells: [], x: 0, y: 0

  def initialise_maze(x, y, start_coords, end_coords, walls) do
    cells =
      for i <- y..1,
          j <- 1..x,
          do: %Cell{x: i, y: j, type: find_type({j, i}, start_coords, end_coords, walls)}

    %Maze{x: x, y: y, cells: cells}
  end

  def calc_fitness(cell, maze) do
    start_cell = find_start(maze)
    end_cell = find_end(maze)
    g = abs(cell.x - start_cell.x) + abs(cell.y - start_cell.y)
    h = Integer.pow(end_cell.x - cell.x, 2) + Integer.pow(end_cell.y - cell.y, 2)
    %Cell{x: cell.x, y: cell.y, type: cell.type, f: g + h, g: g, h: h}
  end

  def get_neighbours(cell, maze) do
    maze.cells
    |> Enum.filter(fn x ->
      x != cell and x.x >= cell.x - 1 and x.x <= cell.x + 1 and x.y >= cell.y - 1 and
        x.y <= cell.y + 1
    end)
  end

  def find_start(maze) do
    maze.cells |> Enum.find(fn cell -> cell.type == :start_cell end)
  end

  def find_end(maze) do
    maze.cells |> Enum.find(fn cell -> cell.type == :end_cell end)
  end

  def find_type(current, start_coords, _end_coords, _walls) when current == start_coords do
    :start_cell
  end

  def find_type(current, _start_coords, end_coords, _walls) when current == end_coords do
    :end_cell
  end

  def find_type(current, _start_coords, _end_coords, walls) do
    if Enum.any?(walls, fn x -> x == current end) do
      :wall
    else
      nil
    end
  end

  def print_maze(maze) do
    maze.cells
    |> Enum.map(fn x -> draw_cell(x) end)
    |> Enum.chunk_every(maze.x)
  end

  def draw_cell(cell) do
    case cell.type do
      :wall -> "⬛️"
      :end_cell -> "🌟"
      :start_cell -> "🟢"
      _ -> "⬜️"
    end
  end
end

defmodule RunMaze do
  def run(maze) do
    start_node =
      Maze.find_start(maze)
      |> Maze.calc_fitness(maze)

    iterate(maze, [start_node], [], [])
  end

  # End conditon - an empty open_list
  def iterate(maze, [], closed_list, path) do
    path
  end

  def iterate(maze, open_list, closed_list, path) do
    IO.inspect(open_list)
    # Look for the lowest F cost square on the open list. We refer to this as the current square
  end
end

Maze.initialise_maze(8, 8, {2, 2}, {8, 8}, []) |> RunMaze.run()

We have the beginning of the algorithm, we have started by adding the start node and calculated the fitness values, lets try running it a few steps by getting the neighbours and calculating their fitness.

Remember the core loop is :

A) Look for the lowest F cost square on the open list. We refer to this as the current square.

B) Switch it to the closed list.

C) For each of the 8 squares adjacent to this current square

• If it is not walkable or if it is on the closed list, ignore it. Otherwise do the following.
• If it isn’t on the open list, add it to the open list. Make the current square the parent of this square. Record the F, G, and H costs of the square.
• If it is on the open list already, check to see if this path to that square is better, using G cost as the measure. A lower G cost means that this is a better path. If so, change the parent of the square to the current square, and recalculate the G and F scores of the square. If you are keeping your open list sorted by F score, you may need to resort the list to account for the change.

defmodule RunMaze do
  def run(maze) do
    start_node =
      Maze.find_start(maze)
      |> Maze.calc_fitness(maze)

    iterate(maze, [start_node], [], [])
  end

  # End conditon - an empty open_list
  def iterate(_maze, [], _closed_list, path) do
    IO.inspect("Number fo steps : #{length(path)}")
    path
  end

  def iterate(maze, open_list, closed_list, path) do
    # Look for the lowest F cost square on the open list. We refer to this as the current square

    # This does the path finding, but it does not produce the optimum result, as we often have equal 
    # fitness values, so I want to make a function that will sort by fitness (F), but in case of a tie, 
    # sorts by distance G
    # current_cell = open_list |> Enum.sort_by(fn x -> x.f end, &<=/2) |> List.first()

    current_cell = open_list |> get_current_cell()

    # Switch it to the closed list.
    # We need to find the current_cell in the open list, pop it out and add to the closed list
    current_cell_index = open_list |> Enum.find_index(fn x -> x == current_cell end)
    {_cs, open_list} = List.pop_at(open_list, current_cell_index)
    closed_list = closed_list ++ [current_cell]

    # For each of the 8 squares adjacent to this current square
    neighbours =
      Maze.get_neighbours(current_cell, maze)
      |> Enum.map(fn cell -> Maze.calc_fitness(cell, maze) end)

    new_open_list =
      neighbours
      |> Enum.map(fn cell -> should_add_cell(cell, open_list, closed_list) end)
      |> Enum.filter(fn cell -> !is_nil(cell) end)

    if current_cell.type == :end_cell do
      IO.inspect("Found end cell")
      iterate(maze, [], [], path ++ [current_cell])
    else
      iterate(maze, open_list ++ new_open_list, closed_list, path ++ [current_cell])
    end
  end

  def get_current_cell(open_list) do
    # If we have equal fitness cells then organise by (G)
    potential_cell = open_list |> Enum.sort_by(fn x -> x.f end, &amp;<=/2) |> List.first()

    if Enum.count(open_list, fn x -> x.f == potential_cell.f end) > 1 do
      open_list
      |> Enum.filter(fn cell -> cell.f == potential_cell.f end)
      # Contrary to what is written above, we actually want to reduce the distance to the goal,
      # So I am actually taking the smallest h value
      |> Enum.sort_by(fn x -> x.h end, &amp;<=/2)
      |> List.first()
    else
      potential_cell
    end
  end

  def should_add_cell(cell, open_list, closed_list) do
    # If it is not walkable or if it is on the closed list, ignore it. If it isn’t on the open list 
    # add it to the open list.
    if !is_in_list(cell, open_list) and Cell.is_walkable?(cell) and !is_in_list(cell, closed_list) do
      cell
    else
      nil
    end
  end

  def is_in_list(entry, list) do
    Enum.any?(list, fn x -> x == entry end)
  end
end

Maze.initialise_maze(8, 8, {1, 1}, {8, 8}, [{3, 3}]) |> RunMaze.run()

Note: This is not a complete implementation.

Instead of the step -

• If it is on the open list already, check to see if this path to that square is better, using G cost as the measure. A lower G cost means that this is a better path. If so, change the parent of the square to the current square, and recalculate the G and F scores of the square. If you are keeping your open list sorted by F score, you may need to resort the list to account for the change

We just look at the “best” node from the open list and head in that direction.
I may come back to this in the future. It would be good to visualise the path, by showing the nodes visited. Thsi would mean a new node type and updating the node in the maze list to reflect the new status.