Optimization for Jump Point Search Pathfinding code

Help and Feedback Code Review
,
#1

Hello, everyone.

I’ve been working on a real-time strategy game, and have struggled with what I consider to be one of the most important aspects of an RTS: pathfinding. I went from using Roblox’s default pathfinding service, to creating a custom A* pathfinding script, to “upgrading” the A* algorithm to Jump Point Search. However, the results have been disheartening. Using JPS, it seems to perform worse than when I was using A*.

Below is an image of the map I’m currently using to test pathfinding.
test map

The map is 100 * 100 studs in size. A uniform grid consisting of 2 * 2 stud-sized nodes populate the map, creating a 50 * 50 grid. Nodes placed on the edges of cliffs are marked as blocked.

On this map, creating a path usually takes longest when the starting point is on the right corner, and the goal is at the top corner (the highest platform on the map). Originally, calculating a path from these two points using JPS could take as much as .2 seconds!

I spent the last couple days researching the Forum and the Developer Hub for resources on optimization, and made several changes to how my code was structured. Now, in the worst-case scenario, creating a path takes about 0.08 seconds. While it is a major improvement, the delay remains noticeable. On top of this, I anticipate that maps in the final product of this game would be much larger than 100 * 100.

I’ve already considered yet another way to fix pathfinding for my game. But before I make another massive overhaul to the pathfinding mechanics, I would like to hear any of your thoughts in regards to my code, and if it could be optimized even further. (For the sake of readability, I’ve also removed several comments that were kept solely for debugging purposes.)

--[[
JPS Pathfinding Script
Cyber_Star00
1/16/2021
]]

--[[
Sources:
	Daniel Harabor and Alban Grastien's papers:
		Online Graph Pruning for Pathfinding on Grid Maps:
		http://users.cecs.anu.edu.au/~dharabor/data/papers/harabor-grastien-aaai11.pdf
	JavaScript Pathfinding Visualizer:
		https://qiao.github.io/PathFinding.js/visual/
	Source code of said Visualizer:
		https://github.com/qiao/PathFinding.js
]]

--[[
MIT License

© 2011-2012 Xueqiao Xu <xueqiaoxu@gmail.com>

Permission is hereby granted, free of charge, to any person obtaining a copy of this software and
associated documentation files (the "Software"), to deal in the Software without restriction,
including without limitation the rights to use, copy, modify, merge, publish, distribute,
sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in all copies or substantial
portions of the Software.
]]

local BLOCK_SIZE = 2
local OFFSET = BLOCK_SIZE / 2

local newVector3 = Vector3.new
local insert = table.insert

local Pathfinder = {}

local gScore = {}; --Dictionary
local fScore = {}; --Dictionary
local hScore = {}; --Dictionary
local openNodes = {}; --List
local closedNodes = {}; --List
local parentOf = {}; --Dictionary

local function drawLine(pointA, pointB)
	local attachmentA = Instance.new("Attachment")
	attachmentA.Parent = workspace.Platforms.Baseplate
	attachmentA.WorldPosition = pointA
	local attachmentB = Instance.new("Attachment")
	attachmentB.Parent = workspace.Platforms.Baseplate
	attachmentB.WorldPosition = pointB
	local line = Instance.new("Beam")
	line.Attachment0 = attachmentA
	line.Attachment1 = attachmentB
	line.Width0 = 0.5
	line.Width1 = 0.5
	line.FaceCamera = true
	line.Transparency = NumberSequence.new(0)
	line.Color = ColorSequence.new(Color3.fromRGB(75, 75, 75))
	line.Parent = workspace.Platforms.Baseplate
end

--Use a dictionary to store and index nodes, as this is much faster than using
--Instance:FindFirstChild()
local Grid = require(script.Grid)
local getNode = Grid.GetNode
local isBlocked = Grid.IsBlocked

local function getDirection(vectorA, vectorB)
	--[[
	Return the normalized direction of travel from one point to another.
	
	param vectorA: The starting point.
	param vectorB: The ending point.
	]]
	return newVector3(
		((vectorA.X - vectorB.X) / math.max(math.abs(vectorA.X - vectorB.X), 1)) * BLOCK_SIZE,
		0,
		((vectorA.Z - vectorB.Z) / math.max(math.abs(vectorA.Z - vectorB.Z), 1)) * BLOCK_SIZE
	)
end

local function heuristic(vectorA, vectorB)
	--Manhattan distance on a square grid
	return math.abs(vectorB.X - vectorA.X) + math.abs(vectorB.Z - vectorA.Z)
end

local function octileHeuristic(x, z)
	local f = math.sqrt(2) - 1;
	if x < z then
		return f * x + z
	else
		return f * z + x
	end
end

local function snapTo(number, step)
	return number - (number % step)
end

local function reconstructPath(startNode, goalNode)
	--Return a table of Vector3s
	--Clear any preexisting lines
	local path = {}
	local currentNode = goalNode
	repeat
		insert(path, 1, currentNode)
		currentNode = parentOf[currentNode]
	until not currentNode or currentNode == startNode
	return path
end

local function jump(nodeX, nodeZ, parentX, parentZ, goalNode)
	--[[
	Search recursively in the direction (parent -> child), stopping only when a jump point is found.
 	
 	param nodePosition:
 	param parentNodePosition:
 	param goalNode:
	]]
	local direction = getDirection(newVector3(nodeX, 0, nodeZ), newVector3(parentX, 0, parentZ))
	--Make some abbreviated variables
	local directionX, directionZ = direction.X, direction.Z
	
	--local case1 = getNode(newVector3(nodeX, 0, nodeZ))
	if isBlocked(nodeX, nodeZ) == true then
		return nil
	end
	
	--if trackJumpRecursion == true then
		--Indicate that this node has been tested
	--end
	
	if getNode(nodeX, nodeZ) == goalNode then
		return newVector3(nodeX, 0, nodeZ)
	end
	
	local xd = nodeX + directionX
	local xdn = nodeX - directionX
	local xb = nodeX + BLOCK_SIZE
	local xbn = nodeX - BLOCK_SIZE
	local zd = nodeZ + directionZ
	local zdn = nodeZ - directionZ
	local zb = nodeZ + BLOCK_SIZE
	local zbn = nodeZ - BLOCK_SIZE
	
	--Check for forced neighbors along the diagonal (if the direction is diagonal)
	if directionX ~= 0 and directionZ ~= 0 then
		--Moving diagonally, check for vertical / horizontal jump points
		if jump(xd, nodeZ, nodeX, nodeZ, goalNode)
			or
			jump(nodeX, zd, nodeX, nodeZ, goalNode)
		then
			return newVector3(nodeX, 0, nodeZ)
		end
		
	--Check for forced neighbors horizontally
	elseif directionX ~= 0 then
		if (
			isBlocked(nodeX, zbn) == false
			and
			isBlocked(xdn, zbn) == true
			)
				or
			(
			isBlocked(nodeX, zb) == false
			and
			isBlocked(xdn, zb) == true
			)
		then
			return newVector3(nodeX, 0, nodeZ)
		end
	--Check for forced neighbors vertically
	elseif directionZ ~= 0 then
		if (
			isBlocked(xbn, nodeZ) == false
			and
			isBlocked(xbn, zdn) == true
			)
			or
			(
			isBlocked(xb, nodeZ) == false
			and
			isBlocked(xb, zdn) == true
			)
		then
			return newVector3(nodeX, 0, nodeZ)
		end
	end
	
	--Moving diagonally, make sure one of the vertical / horizontal neighbors is open to allow the
	--path.
	if (
		isBlocked(xd, nodeZ) == false
		and
		isBlocked(nodeX, zd) == false
		)
	then
		return jump(xd, zd, nodeX, nodeZ, goalNode)	
	else
		return nil
	end
end

local function getNeighbors(x, z)
	--[[
	Get the neighbors of the given node.
	
	   offsets (S):  diagonalOffsets (D):
	  +---+---+---+    +---+---+---+
	  |   | 0 |   |    | 0 |   | 1 |
	  +---+---+---+    +---+---+---+
	  | 3 |   | 1 |    |   |   |   |
	  +---+---+---+    +---+---+---+
	  |   | 2 |   |    | 3 |   | 2 |
	  +---+---+---+    +---+---+---+
	
	if offsets[i] is valid, then
	diagonalOffsets[i] and
	diagonalOffsets[(i + 1) % 4] is valid.
	]]
	--print("Getting neighbors of the given node")
	local neighbors = {}
	local s0, s1, s2, s3 = false, false, false, false --"Guilty until proven innocent"
	local d0, d1, d2, d3 = false, false, false, false
	
	-- ↑
	if isBlocked(x, z - BLOCK_SIZE) == false then
		insert(neighbors, getNode(x, z - BLOCK_SIZE))
		s0 = true
	end
	-- →
	if isBlocked(x + BLOCK_SIZE, z) == false then
		insert(neighbors, getNode(x + BLOCK_SIZE, z))
		s1 = true
	end
	-- ↓
	if isBlocked(x, z + BLOCK_SIZE) == false then
		insert(neighbors, getNode(x, z + BLOCK_SIZE))
		s2 = true
	end
	-- ←
	if isBlocked(x - BLOCK_SIZE, z) == false then
		insert(neighbors, getNode(x - BLOCK_SIZE, z))
		s3 = true
	end
	
	d0 = s3 and s0
	d1 = s0 and s1
	d2 = s1 and s2
	d3 = s2 and s3
	-- ↖
	if d0 == true and isBlocked(x - BLOCK_SIZE, z - BLOCK_SIZE) == false then
		insert(neighbors, getNode(x - BLOCK_SIZE, z - BLOCK_SIZE))
	end
	-- ↗
	if d1 == true and isBlocked(x + BLOCK_SIZE, z - BLOCK_SIZE) == false then
		insert(neighbors, getNode(x + BLOCK_SIZE, z - BLOCK_SIZE))
	end
	-- ↘
	if d2 == true and isBlocked(x + BLOCK_SIZE, z + BLOCK_SIZE) == false then
		insert(neighbors, getNode(x + BLOCK_SIZE, z + BLOCK_SIZE))
	end
	-- ↙
	if d3 == true and isBlocked(x - BLOCK_SIZE, z + BLOCK_SIZE) == false then
		insert(neighbors, getNode(x - BLOCK_SIZE, z + BLOCK_SIZE))
	end
	return neighbors
end

local function prune(node)
	local parentNode = parentOf[node]
	local neighborNodes = {}
	
	local nodeX, nodeZ = node.Position.X, node.Position.Z
	--Directed pruning; can ignore most neighbors unless forced.
	if parentNode then
		--Get the normalized direction of travel
		local direction = getDirection(node.Position, parentNode.Position)
		local directionX, directionZ = direction.X, direction.Z
		--Search diagonally (if the direction is diagonal)
		if directionX ~= 0 and directionZ ~= 0 then
			--print(getNode(newVector3(nodeX, 0, nodeZ + direction.Z)).Blocked.Value,
			--	getNode(newVector3(nodeX + direction.X, 0, nodeZ)).Blocked.Value
			--)
			
			if isBlocked(nodeX, nodeZ + directionZ) == false then
				insert(neighborNodes, getNode(nodeX, nodeZ + directionZ))
			end
			if isBlocked(nodeX + direction.X, nodeZ) == false then
				insert(neighborNodes, getNode(nodeX + directionX, nodeZ))
			end
			if isBlocked(nodeX, nodeZ + directionZ) == false
				and
				isBlocked(nodeX + directionX, nodeZ) == false
			then
				insert(neighborNodes, getNode(nodeX + directionX, nodeZ + directionZ))
			end
		--Search horizontally / vertically (based on the direction of travel)
		else
			local isNextNodeBlocked
			--Search horizontally
			if directionX ~= 0 then
				isNextNodeBlocked = isBlocked(nodeX + directionX, nodeZ)
				local isTopBlocked = isBlocked(nodeX, nodeZ + BLOCK_SIZE)
				local isBottomBlocked = isBlocked(nodeX, nodeZ - BLOCK_SIZE)
				if isNextNodeBlocked == false then
					insert(neighborNodes, getNode(nodeX + directionX, nodeZ))
					if isTopBlocked == false then
						insert(neighborNodes, getNode(nodeX + directionX, nodeZ + BLOCK_SIZE))
					end
					if isBottomBlocked == false then
						insert(neighborNodes, getNode(nodeX + directionX, nodeZ - BLOCK_SIZE))
					end
				end
				if isTopBlocked == false then
					insert(neighborNodes, getNode(nodeX, nodeZ + BLOCK_SIZE))
				end
				if isBottomBlocked == false then
					insert(neighborNodes, getNode(nodeX, nodeZ - BLOCK_SIZE))
				end
			--Search vertically
			elseif directionZ ~= 0 then
				isNextNodeBlocked = isBlocked(nodeX, nodeZ + directionZ)
				local isRightBlocked = isBlocked(nodeX + BLOCK_SIZE, nodeZ)
				local isLeftBlocked = isBlocked(nodeX - BLOCK_SIZE, nodeZ)
				if isNextNodeBlocked == false then
					insert(neighborNodes, getNode(nodeX, nodeZ + directionZ))
					if isRightBlocked == false then
						insert(neighborNodes, getNode(nodeX + BLOCK_SIZE, nodeZ + directionZ))
					end
					if isLeftBlocked == false then
						insert(neighborNodes, getNode(nodeX - BLOCK_SIZE, nodeZ + directionZ))
					end
				end
				if isRightBlocked == false then
					insert(neighborNodes, getNode(nodeX + BLOCK_SIZE, nodeZ))
				end
				if isLeftBlocked == false then
					insert(neighborNodes, getNode(nodeX - BLOCK_SIZE, nodeZ))
				end
			end
		end
	--No parent node found; return all neighbors
	else
		neighborNodes = getNeighbors(nodeX, nodeZ)
	end
	return neighborNodes
end

local function identifySuccessors(node, goalNode)
	--[[
	Identify successors for the given node. Runs a jump point search in the direction of each
	available neighbor, adding any points found to the open list.
	
	param node:
	param goalNode:
	]]
	local neighbors = prune(node)
	local nodePosition = node.Position
	local nodeX, nodeZ = nodePosition.X, nodePosition.Z
	for i = 1, #neighbors do
		local neighborPosition = neighbors[i].Position
		local jumpPoint = jump(neighborPosition.X, neighborPosition.Z, nodeX, nodeZ, goalNode)
		if jumpPoint then
			local jumpNode = getNode(jumpPoint.X, jumpPoint.Z)
			--if isBlocked(jumpPoint.X, jumpPoint.Z) == true then
			--	continue
			--end
			if table.find(closedNodes, jumpNode) then
				continue
			end
			--Include distance, as the parent node may not be immediately adjacent
			local distance = octileHeuristic(math.abs(jumpPoint.X - nodeX), math.abs(jumpPoint.Z - nodeZ))
			--local distance = heuristic(jumpPoint, node)
			local nextGScore = gScore[node] + distance
			if (not table.find(openNodes, jumpNode)) or (nextGScore < gScore[jumpNode]) then
				gScore[jumpNode] = nextGScore
				hScore[jumpNode] = hScore[jumpNode] or heuristic(goalNode.Position, jumpPoint)
				fScore[jumpNode] = gScore[jumpNode] + hScore[jumpNode]
				parentOf[jumpNode] = node
				
				--Add the jump node to the open list if it has not been added, already.
				--print("Adding jump node")
				if not table.find(openNodes, jumpNode) then
					insert(openNodes, jumpNode)
				end
			end
		else
		end
	end
	table.sort(openNodes, function(a, b)
		return fScore[a] < fScore[b]
	end)
end

function Pathfinder:CreatePath(startVector, goalVector)
	--[[
	Find and return a path from one point to another
	
	param startVector: the starting point for the path.
	param goalVector: the point the path is trying to reach.
	]]
	--Reset the path data
	closedNodes = {}
	fScore = {}
	gScore = {}
	hScore = {}
	openNodes = {}
	parentOf = {}
	
	--Update the vectors given so that they are snapped to the grid's measurments
	startVector = newVector3(snapTo(startVector.X, BLOCK_SIZE) + OFFSET, 0, snapTo(startVector.Z, BLOCK_SIZE) + OFFSET)
	goalVector = newVector3(snapTo(goalVector.X, BLOCK_SIZE) + OFFSET, 0, snapTo(goalVector.Z, BLOCK_SIZE) + OFFSET)
	--print(startVector, goalVector) --DEBUG
	--Convert the given Vector3s into strings so that we can index the closest available nodes
	local startNode = getNode(startVector.X, startVector.Z)
	local goalNode = getNode(goalVector.X, goalVector.Z)
	--print(startNode.Position, goalNode.Position) --DEBUG
	
	--Set the G score and F score of the startNode to 0
	gScore[startNode] = 0
	fScore[startNode] = 0
	--Put the start node in the open list
	insert(openNodes, startNode)
	
	while #openNodes > 0 do
		
		local nodeWithLowestFScore = table.remove(openNodes, 1)
		
		--Remove the node with the lowest F score from the openNodes list
		--table.remove(openNodes, currentNodeIndex)
		--Add said node to the closedNodes list to indicate it has already been evaluated
		insert(closedNodes, nodeWithLowestFScore)
		
		if nodeWithLowestFScore == goalNode then
			return true, reconstructPath(startNode, goalNode)
		end
		
		identifySuccessors(nodeWithLowestFScore, goalNode)
	end
	
	--Complete path not found
	return false, {}
end

return Pathfinder

This ModuleScript does require another sub-module called “Grid.” If needed, I can also provide the code to that.

4 Likes
Custom Pathfinding Just Isn't Working
#2

You are using a 2x2 grid, so I’m guessing this will be the approximate size for a standard unit. This is good for fine-grained detection along short distances, but want happens when we try to move to an invalid point? Checking your entire grid isn’t viable.

One solution is to group the terrain and store connections (aka ramps) to it’s neighbors. Kinda hard to explain so here’s a pic.
GroupPath

Each group can be thought of as one of your collision grids. The white dots represent connections between groups.

So, instead of checking connections between plots on the grid, you can check connections between groups. This makes determining if we have a valid path super quick, and allows units to move before the full path is computed.

1 Like
#3

Nice, more code for me to yoink

4 Likes