In this tutorial, we will create our own navigation system to move the agent around, instead of using Godot’s navigation system. I defined a graph & applied A-star algorithm on it. I used Godot’s built-in AStar3D helper to achieve this. This approach was more suited to the dynamic world I was working on as it gives more control over the navigation system.
This post lay foundation for our RTS unit tutorial that comes later in the series. We will create the navigation system here, which will be used later to move the unit (in separate post). The above cover image shows the final outcome of what we will eventually achieve as we proceed.
Creating Navigation System
Start by creating a folder named “NavigationSystem”. And then create a scene of the same name in this folder. Then attach a script to the scene root. Make sure to make the NavigationSystem as a singleton (auto-load) in Project Settings -> Globals. So it will be accessible globally to any other scene.
In the script, we have to do following key things:
- Define a graph spanning over the 3D world. This graph can be made in a grid-like shape, where each cell is a node, connected to 4 neighboring cells.
- We define A-star algorithm to calculate optimum path from start cell to the destination cell.
- (Optionally) we smooth the path, so it does not look like stairs; instead it should be as smooth and straight as possible.
For its implementation, we will use Godot engine’s built-in AStar3D helper node to create graph, and calculate path.
Creating an A-star Graph
A graph is a data structure used to represent points (often called nodes) and lines (connecting those nodes). This is used very often in searching problems. Since finding best path between two points is a so a searching problem, so we use it here as well.
The graph we are going to create will look like a grid, spanning over the xz-axes (it will span the horizontal plane). Thus our first task is to create a graph:
extends Node3D
class_name NavigationSystem
const GRID_SIZE := 128 * 2
const CELL_SIZE := 4 / 2
var astar: AStar3D # Global a-star instance having a graph in it
var astar_capacity: int = 0 # Total number of points in the graph, useful for scaling
func _ready():
add_to_group("navigation_system") # optional
astar = AStar3D.new()
create_grid() # create grid as soon as game starts
# Create astar grid connecting all points in graph.
# @return void
func create_grid():
# Calculate the total number of points and reserve space
var total_points = (2 * GRID_SIZE + 1) * (2 * GRID_SIZE + 1)
astar.reserve_space(total_points)
var point_id = 0
for x in range(-GRID_SIZE, GRID_SIZE + 1):
for z in range(-GRID_SIZE, GRID_SIZE + 1):
var position = Vector3(x * CELL_SIZE, 0, z * CELL_SIZE) # Assuming Y is constant/ground level
astar.add_point(point_id, position)
point_id += 1
# Connect the points
point_id = 0
for x in range(-GRID_SIZE, GRID_SIZE + 1):
for z in range(-GRID_SIZE, GRID_SIZE + 1):
# Connect to neighbors (right and below). No need for other directions due to bidirectional connections.
if x < GRID_SIZE:
astar.connect_points(point_id, point_id + 2 * GRID_SIZE + 1) # Right neighbor
if z < GRID_SIZE:
astar.connect_points(point_id, point_id + 1) # Bottom neighbor
point_id += 1Modifying the Graph Based on World Changes
The above function create a graph of size GRID_SIZE where every cell has a size CELL_SIZE. In real RTS game, there will be many obstacles, and places which are barriers (or walls). So we need to remove some of the points from the graph once it is created.
The AStar3D class in Godot docs has functions that you can use to modify the graph by removing those points which lie inside the barriers. This is very important step as you will not want your RTS units to move freely through walls.
These are the key functions you can use to remove the points from the graph:
remove_point(id: int)to remove an entire point from the graph.set_point_disabled(id: int, disabled: bool = true)to disable a point (so it acts as a temporary obstacle). Useful when buildings are placed & points inside the buildings should not be considered for path finding.
Applying A-star algorithm on Graph
Since we created a graph, our next task is to make a function to calculate the shortest path within that graph, from given start and end positions. This shortest path calculation is done using the A* algorithm:
# ... existing code ...
# Gets shortest path in graph between two points.
# @param from_position: Starting point
# @param to_position: Destination position
# @return Array of vector3 points (a path)
func get_shortest_path(from_position: Vector3, to_position: Vector3) -> Array[Vector3]:
var from_id = astar.get_closest_point(from_position)
var to_id = astar.get_closest_point(to_position)
if from_id == -1 or to_id == -1:
print_debug("Error: Start or end position is out of bounds.")
return [] # Or handle the error differently.
var path_ids = astar.get_id_path(from_id, to_id)
if path_ids.is_empty():
print_debug("Error: No path found between points.")
return [] # Or handle differently, maybe return partial path if needed.
var path_points: Array[Vector3] = []
for id in path_ids:
path_points.append(astar.get_point_position(id))
return path_points # return the path
Smoothing the A* Path
I used RDP algorithm to simplify this rough jagged path into a smooth straight path. This is the implementation of RDP algorithm:
# Simplifies a path by removing unnecessary points while preserving the general direction.
# Iterative version of the RDP algorithm.
# @param path: Array of Vector3 points (path from A* algorithm)
# @param tolerance: Minimum distance a point must have from the line to be kept
# @return Array of simplified Vector3 points
func simplify_path(path: Array[Vector3], tolerance: float) -> Array[Vector3]:
if path.size() < 3:
return path # Cannot simplify if less than 3 points
var result : Array[Vector3] = [] # Final simplified path
var stack = [] # Stack for segments to process
stack.append(Vector2(0, path.size() - 1)) # Start with the entire path
var markers = PackedInt32Array() # Marks points to keep (1 = keep, 0 = discard)
markers.resize(path.size())
# Mark the first and last points as kept
markers[0] = 1
markers[path.size() - 1] = 1
while stack.size() > 0:
var indices = stack.pop_back() # Get the current segment (start, end)
var start_index = indices.x
var end_index = indices.y
var max_distance = 0
var farthest_index = -1
# Find the point farthest from the line segment
for i in range(start_index + 1, end_index):
var distance = point_line_distance(path[i], path[start_index], path[end_index])
if distance > max_distance:
max_distance = distance
farthest_index = i
# If the farthest point exceeds the tolerance, keep it and add new segments
if max_distance > tolerance:
markers[farthest_index] = 1
stack.append(Vector2(start_index, farthest_index)) # Left segment
stack.append(Vector2(farthest_index, end_index)) # Right segment
# Collect the points marked as kept
for i in range(path.size()):
if markers[i] == 1:
result.append(path[i])
return result
# Calculates the perpendicular distance of a point from a line segment
# @param point: The point to measure distance from
# @param line_start: Start of the line segment
# @param line_end: End of the line segment
# @return The perpendicular distance
func point_line_distance(point: Vector3, line_start: Vector3, line_end: Vector3) -> float:
var line = line_end - line_start
var length_squared = line.length_squared()
if length_squared == 0:
return (point - line_start).length() # Line is a single point
# Projection of 'point' onto the line
var t = ((point - line_start).dot(line)) / length_squared
t = clamp(t, 0.0, 1.0) # Clamp to line segment bounds
var projection = line_start + t * line
return (point - projection).length()
To use the simplify_path function, you can pass the generated path from get_shortest_path in it, and the returned path will be a simplified one. I called simplify_path in get_shortest_path as follows:
# IN get_shortest_path FUNCTION:
# REPLACE:
return path_points # return the path
# WITH:
return simplify_path(path_points, 8.0) # Return simplified pathThe parameter 8.0 is the weight used to control how much simplification do we need. I found 8.0 to be a good value for me.
After simplifying the path, this is how it looks like:
Full Navigation System Code
extends Node3D
const GRID_SIZE := 128 * 2
const CELL_SIZE := 4 / 2
@export var player: Node3D
var astar: AStar3D # Global a-star instance having a graph in it
var astar_capacity: int = 0 # Total number of points in the graph, useful for scaling
func _ready():
add_to_group("navigation_system")
astar = AStar3D.new()
create_grid()
# > > > > > >
# > > > > > >
# > > > > > >
# > > > > > >
# Create astar grid connecting all points in graph.
# @return void
func create_grid():
# Calculate the total number of points and reserve space
var total_points = (2 * GRID_SIZE + 1) * (2 * GRID_SIZE + 1)
astar.reserve_space(total_points)
var point_id = 0
for x in range(-GRID_SIZE, GRID_SIZE + 1):
for z in range(-GRID_SIZE, GRID_SIZE + 1):
var position = Vector3(x * CELL_SIZE, 0, z * CELL_SIZE) # Assuming Y is constant/ground level
astar.add_point(point_id, position)
point_id += 1
# Connect the points
point_id = 0
for x in range(-GRID_SIZE, GRID_SIZE + 1):
for z in range(-GRID_SIZE, GRID_SIZE + 1):
# Connect to neighbors (right and below). No need for other directions due to bidirectional connections.
if x < GRID_SIZE:
astar.connect_points(point_id, point_id + 2 * GRID_SIZE + 1) # Right neighbor
if z < GRID_SIZE:
astar.connect_points(point_id, point_id + 1) # Bottom neighbor
point_id += 1
# > > > > > >
# > > > > > >
# > > > > > >
# > > > > > >
# Gets shortest path in graph between two points.
# @param from_position: Starting point
# @param to_position: Destination position
# @return Array of vector3 points (a path)
func get_shortest_path(from_position: Vector3, to_position: Vector3) -> Array[Vector3]:
var from_id = astar.get_closest_point(from_position)
var to_id = astar.get_closest_point(to_position)
if from_id == -1 or to_id == -1:
print_debug("Error: Start or end position is out of bounds.")
return [] # Or handle the error differently.
var path_ids = astar.get_id_path(from_id, to_id)
if path_ids.is_empty():
print_debug("Error: No path found between points.")
return [] # Or handle differently, maybe return partial path if needed.
var path_points: Array[Vector3] = []
for id in path_ids:
path_points.append(astar.get_point_position(id))
return simplify_path(path_points, 8.0) # Simplify the path
# > > > > > >
# > > > > > >
# > > > > > >
# > > > > > >
# Simplifies a path by removing unnecessary points while preserving the general direction.
# Iterative version of the RDP algorithm.
# @param path: Array of Vector3 points (path from A* algorithm)
# @param tolerance: Minimum distance a point must have from the line to be kept
# @return Array of simplified Vector3 points
func simplify_path(path: Array[Vector3], tolerance: float) -> Array[Vector3]:
if path.size() < 3:
return path # Cannot simplify if less than 3 points
var result : Array[Vector3] = [] # Final simplified path
var stack = [] # Stack for segments to process
stack.append(Vector2(0, path.size() - 1)) # Start with the entire path
var markers = PackedInt32Array() # Marks points to keep (1 = keep, 0 = discard)
markers.resize(path.size())
# Mark the first and last points as kept
markers[0] = 1
markers[path.size() - 1] = 1
while stack.size() > 0:
var indices = stack.pop_back() # Get the current segment (start, end)
var start_index = indices.x
var end_index = indices.y
var max_distance = 0
var farthest_index = -1
# Find the point farthest from the line segment
for i in range(start_index + 1, end_index):
var distance = point_line_distance(path[i], path[start_index], path[end_index])
if distance > max_distance:
max_distance = distance
farthest_index = i
# If the farthest point exceeds the tolerance, keep it and add new segments
if max_distance > tolerance:
markers[farthest_index] = 1
stack.append(Vector2(start_index, farthest_index)) # Left segment
stack.append(Vector2(farthest_index, end_index)) # Right segment
# Collect the points marked as kept
for i in range(path.size()):
if markers[i] == 1:
result.append(path[i])
return result
# Calculates the perpendicular distance of a point from a line segment
# @param point: The point to measure distance from
# @param line_start: Start of the line segment
# @param line_end: End of the line segment
# @return The perpendicular distance
func point_line_distance(point: Vector3, line_start: Vector3, line_end: Vector3) -> float:
var line = line_end - line_start
var length_squared = line.length_squared()
if length_squared == 0:
return (point - line_start).length() # Line is a single point
# Projection of 'point' onto the line
var t = ((point - line_start).dot(line)) / length_squared
t = clamp(t, 0.0, 1.0) # Clamp to line segment bounds
var projection = line_start + t * line
return (point - projection).length()
Thank you for reading <3