Learn how to explore a hexagonal grid and find a goal point with greedy best first search in this pathfinding tutorial. You will use Processing and Java to code the greedy best first search pathfinding algorithm and create a graphical demo so you can see it in action.

Introduction

The greedy best first search algorithm will explore the nodes closest to the goal point first. This means it is capable of finding the goal quickly when exploring less complex maps. However it doesn’t always pick the shortest path, since it will pick the first path it stumbles across.

In the last tutorial, we generated a hexagonal grid in Processing. This tutorial will add code to the existing code from the last tutorial. So make sure you follow the last tutorial first if you want to see a graphical demo of the pathfinding in action. Or read on if you just want to learn about the greedy best first search algorithm.

Open Processing

Open Processing, then add the “Collections” import. We will use this to reverse the path list later in the pathfinding algorithm.

import java.util.Collections;

Next add the “Comparator” import. We will use this to compare the goal score of different nodes.

import java.util.Comparator;

Finally add the import for the priority queue collection. We will use this to keep our nodes sorted by distance to the goal.

import java.util.PriorityQueue;

Next add two variables for the start and goal node.

Node startNode = null;
Node goalNode = null;

Now we need a list for the nodes we have explored. These will be used for the graphical demo.

ArrayList<Node> exploredNodes = new ArrayList<Node>();
int currentExploredSize = 0;

Then add a list for the path the algorithm has taken.

ArrayList<Node> path = new ArrayList<Node>();
int currentPathSize = 0;

Then add a variable for controlling the timing of the graphical demo.

int startTime = 0;

Math

Create a new tab called “Math”. Then create three functions for calculating the squared distance. These will be handy for calculating the the squared distance to the goal node.

float SquaredDistance(float x, float y)
{
  return x*x + y*y;
}

float SquaredDistance(float x1, float y1, float x2, float y2)
{
  return SquaredDistance(x1 - x2, y2 - y1);
}

float SquaredDistance(PVector a, PVector b)
{
  return SquaredDistance(a.x - b.x, a.y - b.y);
}

Node

Next open the “Node” tab and add three new variables to the “Node” class.

Whether a node is explored or not.

boolean explored = false;

The previous node which accessed this node.

Node parent = null;

The score to the goal. The lower the value, the higher the priority.

float goalScore = Float.POSITIVE_INFINITY;

Node Comparator

Next add a comparator for the Node class. This will allow us to compare the goal score of two nodes.

class NodeGoalScoreComparator implements Comparator<Node>
{
  @Override
  public int compare(Node a, Node b)
  {
    return Float.compare(a.goalScore, b.goalScore);
  }
}

Pathfinding

Create a new tab called “Pathfinding”. Then create a new greedy best first search function.

The function takes a start node, a goal node and a path list, which will be used to store the path the algorithm takes.

boolean GreedyBestFirstSearch(Node start, Node goal, ArrayList<Node> path)
{
}

Inside the function, first clear the path list and explored list.

path.clear();
exploredNodes.clear();

Next create a variable which will record the number of moves taken by the algorithm.

int moves = 0;

Then check if the start node is the goal node. If it is, then we have found the goal. Add either node to the path and return true.

if(start == goal)
{
  path.add(goal);
  println("Goal is start node");
  return true;
}

Next mark the start node as explored.

start.explored = true;

Now create a priority queue which uses the comparator we created earlier. The queue will be sorted by the goal scores of the nodes. Lowest value is first in the queue.

PriorityQueue<Node> queue = new PriorityQueue<Node>(10, new NodeGoalScoreComparator());
queue.add(start);

Next set the goal score of the start node, by calculating the squared distance using the start position and goal position. We used the squared distance since it is more efficient than using a squared root, and we don’t need to know the actual distance.

start.goalScore = SquaredDistance(start.position, goal.position);

Then create a current node variable, which will store the currently polled node from the queue.

Node current = null;

Next create a while loop which checks if the queue is empty.

while(!queue.isEmpty())
{
}

Inside the while loop, increment the number of moves taken.

++moves;

Then get the first node in the queue.

current = queue.poll();

Then add the current node to the explored nodes list.

exploredNodes.add(current);

Next we will loop over all the adjacent nodes in the current node.

for(Node node : current.adjacentNodes)
{
}

Inside the for loop, first check if the node is already explored. If it is, then we will continue and ignore it.

if(node.explored) continue;

Next we will set the node explored variable to true.

node.explored = true;

Then we will set the node’s parent to the current node.

node.parent = current;

Next check if we have found the goal node.

if(node == goal)
{
}

Inside the if statement, we will first print the number of moves taken to the console.

println("Goal has been found in " + moves + " moves");

Then we will add the node to the explored nodes list.

exploredNodes.add(node);

Next we will set the current node to the node from the for loop.

current = node;

Now we will use a while loop to backtrack through all the parents of the current node until we reach the start point. Remember the start point doesn’t have a parent.

while(current != null)
{
  path.add(current);
  current = current.parent;
}

Finally we just need to reverse the path list since it starts from the goal node, and return true.

Collections.reverse(path);
return true;

Next, outside of the if statement which checks for the goal.

if(node == goal)

If we haven’t reached the goal, then we need to set the goal score for the node using it’s position and the goal position. Then add it to the queue. The queue will keep the elements sorted by using the goal score. The lowest goal score will be at the front of the queue.

node.goalScore = SquaredDistance(node.position, goal.position);
queue.add(node);

If this while loop ends without finding the goal.

while(!queue.isEmpty())

Then we will print the number of moves taken and return false.

println("Failed to find the goal in", moves, "moves");
return false;

Full algorithm code:

boolean GreedyBestFirstSearch(Node start, Node goal, ArrayList<Node> path)
{
  path.clear();
  exploredNodes.clear();
  int moves = 0;
  
  if(start == goal)
  {
    path.add(goal);
    println("Goal is start node");
    return true;
  }
  
  start.explored = true;
  PriorityQueue<Node> queue = new PriorityQueue<Node>(10, new NodeGoalScoreComparator());
  queue.add(start);
  start.goalScore = SquaredDistance(start.position, goal.position);
  Node current = null;
  while(!queue.isEmpty())
  {
    ++moves;
    current = queue.poll();
    exploredNodes.add(current);
    for(Node node : current.adjacentNodes)
    {
      if(node.explored) continue;
      node.explored = true;
      node.parent = current;
      if(node == goal)
      {
        println("Goal has been found in " + moves + " moves");
        exploredNodes.add(node);
        current = node;
        while(current != null)
        {
          path.add(current);
          current = current.parent;
        }
        Collections.reverse(path);
        return true;
      }
      node.goalScore = SquaredDistance(node.position, goal.position);
      queue.add(node);
    }
  }
  
  println("Failed to find the goal in", moves, "moves");
  return false;
}

Setup

Go back to the main tab and edit the “setup” function.

void setup()
{
  size(720, 720);
  
  CreateHexagonGrid(columns, rows, hexagonRadius);
  CreateWalls();
  ConnectNodes(hexagonRadius);
  
  startNode = nodeList.get(48);
  goalNode = nodeList.get(203);
  
  GreedyBestFirstSearch(startNode, goalNode, path);
  
  startTime = millis();
}

We set the start node and goal node to a random valid index which we like. Then we call the greedy best first search function. Then set the start time to the current time in milliseconds from when the program started.

Render Path

Create a new function for rendering the path.

void RenderPath()
{
  // Control size of path
  if(millis() - startTime > 100)
  {
    startTime = millis();
    
    if(currentExploredSize == exploredNodes.size())
    {
      currentPathSize = min(++currentPathSize, path.size());
    }
    
    currentExploredSize = min(++currentExploredSize, exploredNodes.size());
  }
  
  // Draw the explored nodes
  strokeWeight(1);
  stroke(40);
  fill(20, 200, 200);
  Node node = null;
  for(int i=0; i<currentExploredSize; ++i)
  {
    node = exploredNodes.get(i);
    RenderHexagon(node.position, hexagonRadius);
  }
  
  // Draw the path taken
  strokeWeight(1);
  stroke(40);
  fill(200, 200, 20);
  for(int i=0; i<currentPathSize; ++i)
  {
    node = path.get(i);
    RenderHexagon(node.position, hexagonRadius);
  }
}

The function increments the current path and current explored node indices every 100 milliseconds. Explored nodes are prioritised over path nodes.

Then we render both the explored nodes and the path nodes, using the current index values.

Render Start and Goal

Next we will render the start and goal nodes as small circles.

void RenderStartAndGoal()
{
  // Draw start and goal nodes as circles
  strokeWeight(2);
  stroke(0);
  fill(20, 255, 10);
  final float extent = hexagonRadius / 2;
  circle(startNode.position.x, startNode.position.y, extent);
  fill(255, 10, 20);
  circle(goalNode.position.x, goalNode.position.y, extent);
}

Draw

Edit the “draw” function.

void draw()
{
  background(50);
  RenderHexagonGrid();
  RenderPath();
  RenderStartAndGoal();
}

We now call the render path and render start and goal functions.

Keyboard

Add this code to restart the animation using the “R” key.

void keyPressed()
{
  if(key == 'r' || key == 'R')
  {
    currentExploredSize = 0;
    currentPathSize = 0;
    startTime = millis();
  }
}

Finally you just need to run the sketch, and you should see a cool graphical demo of the pathfinding in action!

Exploring All Nodes

If you want to see the greedy best first search algorithm explore every connected node in the grid. Then remove the check which exits after finding the goal in the greedy best first search function.

if(node == goal)
{
}

Conclusion

Thanks for reading this tutorial. You have learned how to explore a hexagonal grid and find a goal point. You used Processing and Java to code the greedy best first search pathfinding algorithm, and then coded a graphical demo so you can see the pathfinding in action.