Learn how to explore a triangle grid and find a goal point with the A-star search algorithm in this pathfinding tutorial. You will use Processing and Java to code the A-star pathfinding algorithm and create a graphical demo so you can see it in action.
Introduction
The A-star pathfinding algorithm finds the shortest path from a start node to a goal node in a node graph (assuming the path exists). It does this using two heuristics.
The first heuristic measures the distance from the current node to the start node. The second heuristic measures the distance from the current node to the goal node. The combined scores of these two heuristics can be used to steer the A-star algorithm towards optimal nodes.
In the last tutorial, we generated a triangular 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 A-star search algorithm.
Open Processing
Open Processing, then add the collections import. We will use this to reverse the path list later on.
import java.util.Collections;
Next add the comparator import, which will be used to sort a priority queue.
import java.util.Comparator;
Then add the priority queue import.
import java.util.PriorityQueue;
Next add a list for the explored nodes.
ArrayList<Node> exploredNodes = new ArrayList<Node>();
int currentExploredSize = 0;
Then add a list for the path the algorithm takes.
ArrayList<Node> path = new ArrayList<Node>();
int currentPathSize = 0;
Now add a timer variable, which we will use to control the animation speed of the graphical demo.
int startTime = 0;
Lastly, we need two node variables. One for the start node and one for the goal node.
Node start = null;
Node goal = null;
Node
First go to the Node tab and add these variables to the Node class.
Node parent = null;
float startScore = Float.POSITIVE_INFINITY;
float goalScore = Float.POSITIVE_INFINITY;
The parent variable will be used to remember the previous node which accessed this node.
The start and goal score variables will be used to steer the A-star algorithm towards optimal nodes.
Next add a Comparator for the Node class. It will compare the goal scores of two nodes.
class NodeGoalScoreComparator implements Comparator<Node>
{
@Override
public int compare(Node a, Node b)
{
return Float.compare(a.goalScore, b.goalScore);
}
}
Math
Next go to the Math tab. Then add these three distance functions.
float Distance(float x, float y)
{
return sqrt(x*x + y*y);
}
float Distance(float x1, float y1, float x2, float y2)
{
return Distance(x1 - x2, y1 - y2);
}
float Distance(PVector a, PVector b)
{
return Distance(a.x - b.x, a.y - b.y);
}
We will use these functions to compare the distance between the current node and the start node, and the current node and the goal node.
Pathfinding
Create a new tab called ‘Pathfinding’. Then create a function for the A-star algorithm.
boolean AStarSearch(Node start, Node goal, ArrayList<Node> path, ArrayList<Node> exploredNodes)
{
}
The function takes a start node, a goal node, a list for the path the algorithm takes, and a list for the nodes explored (which is used by the graphical demo).
The algorithm is based on the pseudocode from Wikipedia.
Inside the function, first clear the path and explored nodes lists.
path.clear();
exploredNodes.clear();
Next create a variable to track the number of moves taken by the algorithm.
int moves = 0;
Then set the start and goal score for the start node.
start.startScore = 0.0f;
start.goalScore = Distance(start.position, goal.position);
Next create a priority queue, which uses the Comparator we created earlier. The Comparator will compare the goal score of two nodes. Every time we add a node to the priority queue, it will be sorted so that the node with the lowest score is always at the front.
PriorityQueue<Node> frontier = new PriorityQueue<Node>(10, new NodeGoalScoreComparator());
Now add the start node to the priority queue.
frontier.add(start);
Next create a Node variable to store the current node.
Node current = null;
Then create a new variable for calculating a new score.
float tentativeScore = 0;
Now we will create a while loop. The loop will continue until the priority queue is empty.
while(!frontier.isEmpty())
{
}
Inside the while loop, increment the number of moves taken.
++moves;
Then retrieve and remove the first node in the queue.
current = frontier.poll();
Add the current node to the explored nodes list.
exploredNodes.add(current);
Next we will check if we have reached the goal. If we have, then we will print the number of moves taken.
Then we will loop through all the nodes using the parent node variable stored in each node, to find the path taken. This will need to be reversed, since we are starting from the goal node. Then we just need to return true to exit the function.
if(current == goal)
{
println("Goal has been found in " + moves + " moves");
while(current != null)
{
path.add(current);
current = current.parent;
}
Collections.reverse(path);
return true;
}
If we haven’t found the goal, then we will loop through the adjacent nodes of the current node.
for(Node node : current.adjacentNodes)
{
}
Inside the for loop, we first calculate the score of the current node and the loop node.
tentativeScore = current.startScore + Distance(current.position, node.position);
Next we check if the score is less than the loop node’s start score.
if (tentativeScore < node.startScore)
{
}
If it is, then we will record the current node.
node.parent = current;
Then we will set the start score.
node.startScore = tentativeScore;
Next we will set the goal score. Using the existing score and the distance to the goal node from this node.
node.goalScore = tentativeScore + Distance(node.position, goal.position);
Now we will add the node to the priority queue, if it is not already in the priority queue.
boolean duplicate = false;
for(Node f : frontier)
{
if(f == node)
{
duplicate = true;
break;
}
}
if(!duplicate)
{
frontier.add(node);
}
Finally, all we need to do is exit the function. Outside of the while loop, we will print the number of moves taken and a message saying we failed to find the goal. Then we will return false.
println("Failed to find the goal in", moves, "moves");
return false;
Here is the full algorithm:
boolean AStarSearch(Node start, Node goal, ArrayList<Node> path, ArrayList<Node> exploredNodes)
{
path.clear();
exploredNodes.clear();
int moves = 0;
start.startScore = 0.0f;
start.goalScore = Distance(start.position, goal.position);
PriorityQueue<Node> frontier = new PriorityQueue<Node>(10, new NodeGoalScoreComparator());
frontier.add(start);
Node current = null;
float tentativeScore = 0;
while(!frontier.isEmpty())
{
++moves;
current = frontier.poll();
exploredNodes.add(current);
if(current == goal)
{
println("Goal has been found in " + moves + " moves");
while(current != null)
{
path.add(current);
current = current.parent;
}
Collections.reverse(path);
return true;
}
for(Node node : current.adjacentNodes)
{
tentativeScore = current.startScore + Distance(current.position, node.position);
if (tentativeScore < node.startScore)
{
// This path to neighbour is better than any previous one. Record it!
node.parent = current;
node.startScore = tentativeScore;
node.goalScore = tentativeScore + Distance(node.position, goal.position);
boolean duplicate = false;
for(Node f : frontier)
{
if(f == node)
{
duplicate = true;
break;
}
}
if(!duplicate)
{
frontier.add(node);
}
}
}
}
println("Failed to find the goal in", moves, "moves");
return false;
}
Setup
Go back to the main tab. Then update the setup function.
We first set the window size. Then set the number of columns and rows. We use this to generate a triangle grid. Then we create the walls using the random seed value. And finally connect the nodes.
Then we will pick a start and a goal node from the node list.
Next we will run the A-star algorithm. Then we will set the start time to the current time since the program started in milliseconds.
void setup()
{
size(720, 720);
final int columns = 20;
final int rows = 10;
GenerateTriangleGrid(columns, rows);
final int seed = 130;
CreateWalls(seed, columns, rows);
ConnectNodes();
// Pick start and goal nodes
start = nodeList.get(0);
goal = nodeList.get(nodeList.size()-1);
AStarSearch(start, goal, path, exploredNodes);
startTime = millis();
}
Keyboard
Next create a key pressed function. You can use this to restart the animation by pressing the ‘R’ key on your keyboard.
void keyPressed()
{
if(key == 'r' || key == 'R')
{
currentExploredSize = 0;
currentPathSize = 0;
startTime = millis();
}
}
Triangle
Next go to the triangle tab and create a new render function inside the Triangle class.
void Render(float red, float green, float blue)
{
fill(red, green, blue);
PVector a = vertices.get(0);
PVector b = vertices.get(1);
PVector c = vertices.get(2);
triangle(a.x, a.y, b.x, b.y, c.x, c.y);
}
We will use this to draw the explored and path triangles in different colours.
Render Path
Next go back to the main tab. Then create a function which renders the explored nodes and the current path. The function will update one of two index variables every 100 milliseconds. These variables are used to control how many explored nodes or path nodes are rendered at once.
Then we will use these two variables to render the current number of explored nodes and current number of path nodes in different colours.
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 nodes the pathfinding algorithm explores
stroke(10);
strokeWeight(2);
fill(20, 200, 200);
Node node = null;
for(int i=0; i<currentExploredSize; ++i)
{
node = exploredNodes.get(i);
node.triangle.Render(20, 200, 200);
}
// Draw the path the pathfinding algorithm takes
stroke(10);
strokeWeight(2);
fill(200, 200, 20);
for(int i=0; i<currentPathSize; ++i)
{
node = path.get(i);
node.triangle.Render(200, 200, 20);
}
}
Render Start and Goal
Create another render function which renders the start and goal nodes as circles.
void RenderStartAndGoal()
{
// Draw start and goal nodes as circles
stroke(10);
strokeWeight(2);
fill(20, 255, 10);
final float radius = triangleSize / 4;
circle(start.position.x, start.position.y, radius);
fill(255, 10, 20);
circle(goal.position.x, goal.position.y, radius);
}
Draw
Next edit the draw function. Call the new render functions and optionally comment out the render functions you don’t want to see.
void draw()
{
background(200);
RenderTriangles();
RenderPath();
//RenderConnections();
//RenderCircles();
RenderStartAndGoal();
}
Finally you just need to run the sketch, and you should see a cool graphical demo of the pathfinding in action!
Conclusion
Thanks for reading this tutorial. You have learned how to explore a triangle grid and find a goal point. You used Processing and Java to code the A-star pathfinding algorithm, and then coded a graphical demo so you can see the pathfinding in action.
Agate DRAGON Games 