How avoid obstacles in a A* movement system?

The_NinjaBR0 · July 13, 2024, 1:16am

My Code:

local Players = game:GetService("Players")
local UserInputService = game:GetService("UserInputService")
local RunService = game:GetService("RunService")
local ReplicatedStorage = game:GetService("ReplicatedStorage")

local player = Players.LocalPlayer
local mouse = player:GetMouse()
local character = player.Character or player.CharacterAdded:Wait()
local humanoid = character:WaitForChild("Humanoid")
local rootPart = character:WaitForChild("HumanoidRootPart")
local blockTemplate = ReplicatedStorage:FindFirstChild("BlockTemplate") 

local gridSize = 8
local previewBlock = blockTemplate:Clone()
previewBlock.Transparency = 0.5
previewBlock.CanCollide = false
previewBlock.Parent = workspace

local moving = false

local function roundToGrid(value, gridSize)
	return math.floor(value / gridSize + 0.5) * gridSize
end

local function heuristic(a, b)
	return (a - b).Magnitude
end


local function getNeighbors(node, gridSize)
	local neighbors = {}
	local directions = {
		Vector3.new(1, 0, 0),
		Vector3.new(-1, 0, 0),
		Vector3.new(0, 0, 1),
		Vector3.new(0, 0, -1)
	}

	for _, direction in ipairs(directions) do
		local neighbor = node + direction * gridSize
		table.insert(neighbors, neighbor)
	end

	return neighbors
end

local function aStar(startPos, goalPos, gridSize)
	local openSet = {[startPos] = true}
	local cameFrom = {}
	local gScore = {[startPos] = 0}
	local fScore = {[startPos] = heuristic(startPos, goalPos)}

	while next(openSet) do
		local current
		local lowestFScore = math.huge

		for node in pairs(openSet) do
			if fScore[node] and fScore[node] < lowestFScore then
				lowestFScore = fScore[node]
				current = node
			end
		end

		if current == goalPos then
			local path = {}
			while cameFrom[current] do
				table.insert(path, 1, current)
				current = cameFrom[current]
			end
			return path
		end

		openSet[current] = nil

		for _, neighbor in ipairs(getNeighbors(current, gridSize)) do
			local tentativeGScore = gScore[current] + heuristic(current, neighbor)

			if not gScore[neighbor] or tentativeGScore < gScore[neighbor] then
				cameFrom[neighbor] = current
				gScore[neighbor] = tentativeGScore
				fScore[neighbor] = gScore[neighbor] + heuristic(neighbor, goalPos)
				openSet[neighbor] = true
			end
		end
		wait()
	end

	return nil 
end

local function moveToGridPosition(gridPosition)
	if moving then
		humanoid:MoveTo(rootPart.Position)
		moving = false
	end

	local startPos = Vector3.new(
		roundToGrid(rootPart.Position.X, gridSize),
		roundToGrid(rootPart.Position.Y, gridSize),
		roundToGrid(rootPart.Position.Z, gridSize)
	)

	local path = aStar(startPos, gridPosition, gridSize)

	if path then
		moving = true
		for _, waypoint in ipairs(path) do
			if not moving then break end
			humanoid:MoveTo(waypoint)
			humanoid.MoveToFinished:Wait()
			wait()
		end
		moving = false
	else
		warn("Path not found!")
	end
end

UserInputService.InputBegan:Connect(function(input, gameProcessed)
	if input.UserInputType == Enum.UserInputType.MouseButton1 and not gameProcessed then
		local mousePos = mouse.Hit.p
		local gridPos = Vector3.new(
			roundToGrid(mousePos.X, gridSize),
			roundToGrid(mousePos.Y, gridSize),
			roundToGrid(mousePos.Z, gridSize)
		)
		moveToGridPosition(gridPos)
	end
end)

local function onUpdate()
	local mouseRay = mouse.UnitRay
	local raycastParams = RaycastParams.new()
	raycastParams.FilterDescendantsInstances = {workspace.Grid}
	raycastParams.FilterType = Enum.RaycastFilterType.Include
	raycastParams.IgnoreWater = true

	local raycastResult = workspace:Raycast(mouseRay.Origin, mouseRay.Direction * 1000, raycastParams)
	if raycastResult then
		local hitPos = raycastResult.Position
		local gridPos = Vector3.new(
			roundToGrid(hitPos.X, gridSize),
			roundToGrid(hitPos.Y, gridSize),
			roundToGrid(hitPos.Z, gridSize)
		)
		previewBlock.Position = gridPos
	end
end

RunService.RenderStepped:Connect(onUpdate)

JamesBlossoms · July 13, 2024, 1:18am

Is there a reason you’re not using PathFindingService?

The_NinjaBR0 · July 13, 2024, 1:19am

It’s because my movement is done on a grid (8x8), and I didn’t think pathfindingservice supports this.

JamesBlossoms · July 13, 2024, 1:20am

I would try using Raycasting to determine if there is an obstacle. This is just an example.

local function isPositionClear(position)
    -- Perform a raycast to check if the position is clear of obstacles
    local raycastResult = workspace:Raycast(position, Vector3.new(0, -1, 0), RaycastParams.new())
    return raycastResult == nil  -- No obstacle if raycast doesn't hit anything
end

local function getNeighbors(node, gridSize)
    local neighbors = {}
    local directions = {
        Vector3.new(1, 0, 0),
        Vector3.new(-1, 0, 0),
        Vector3.new(0, 0, 1),
        Vector3.new(0, 0, -1)
    }

    for _, direction in ipairs(directions) do
        local neighbor = node + direction * gridSize

        -- Check if the neighbor position is clear of obstacles
        if isPositionClear(neighbor) then
            table.insert(neighbors, neighbor)
        end
    end

    return neighbors
end

The_NinjaBR0 · July 13, 2024, 1:33am

I tried this way but it didn’t work

local Players = game:GetService("Players")
local UserInputService = game:GetService("UserInputService")
local RunService = game:GetService("RunService")
local ReplicatedStorage = game:GetService("ReplicatedStorage")

local player = Players.LocalPlayer
local mouse = player:GetMouse()
local character = player.Character or player.CharacterAdded:Wait()
local humanoid = character:WaitForChild("Humanoid")
local rootPart = character:WaitForChild("HumanoidRootPart")
local blockTemplate = ReplicatedStorage:FindFirstChild("BlockTemplate") 

local gridSize = 8
local previewBlock = blockTemplate:Clone()
previewBlock.Transparency = 0.5
previewBlock.CanCollide = false
previewBlock.Parent = workspace

local moving = false

local function roundToGrid(value, gridSize)
	return math.floor(value / gridSize + 0.5) * gridSize
end

local function heuristic(a, b)
	return (a - b).Magnitude
end

local function isPositionClear(position)
	local params = RaycastParams.new()
	params.FilterType = Enum.RaycastFilterType.Exclude
	params.FilterDescendantsInstances = workspace.Obstacles:GetChildren()
	params.IgnoreWater = true
	local raycastResult = workspace:Raycast(position, Vector3.new(0, -1, 0), params)
	return not raycastResult
end

local function getNeighbors(node, gridSize)
	local neighbors = {}
	local directions = {
		Vector3.new(1, 0, 0),
		Vector3.new(-1, 0, 0),
		Vector3.new(0, 0, 1),
		Vector3.new(0, 0, -1)
	}

	for _, direction in ipairs(directions) do
		
		local neighbor = node + direction * gridSize
		print(isPositionClear(neighbor))
		table.insert(neighbors, neighbor)
	end

	return neighbors
end

local function aStar(startPos, goalPos, gridSize)
	local openSet = {[startPos] = true}
	local cameFrom = {}
	local gScore = {[startPos] = 0}
	local fScore = {[startPos] = heuristic(startPos, goalPos)}

	while next(openSet) do
		local current
		local lowestFScore = math.huge

		for node in pairs(openSet) do
			if fScore[node] and fScore[node] < lowestFScore then
				lowestFScore = fScore[node]
				current = node
			end
		end

		if current == goalPos then
			local path = {}
			while cameFrom[current] do
				table.insert(path, 1, current)
				current = cameFrom[current]
			end
			return path
		end

		openSet[current] = nil

		for _, neighbor in ipairs(getNeighbors(current, gridSize)) do
			local tentativeGScore = gScore[current] + heuristic(current, neighbor)

			if not gScore[neighbor] or tentativeGScore < gScore[neighbor] then
				cameFrom[neighbor] = current
				gScore[neighbor] = tentativeGScore
				fScore[neighbor] = gScore[neighbor] + heuristic(neighbor, goalPos)
				openSet[neighbor] = true
			end
		end
		wait()
	end

	return nil 
end

local function moveToGridPosition(gridPosition)
	if moving then
		humanoid:MoveTo(rootPart.Position)
		moving = false
	end

	local startPos = Vector3.new(
		roundToGrid(rootPart.Position.X, gridSize),
		roundToGrid(rootPart.Position.Y, gridSize),
		roundToGrid(rootPart.Position.Z, gridSize)
	)

	local path = aStar(startPos, gridPosition, gridSize)

	if path then
		moving = true
		for _, waypoint in ipairs(path) do
			if not moving then break end
			humanoid:MoveTo(waypoint)
			humanoid.MoveToFinished:Wait()
			wait()
		end
		moving = false
	else
		warn("Path not found!")
	end
end

UserInputService.InputBegan:Connect(function(input, gameProcessed)
	if input.UserInputType == Enum.UserInputType.MouseButton1 and not gameProcessed then
		local mousePos = mouse.Hit.p
		local gridPos = Vector3.new(
			roundToGrid(mousePos.X, gridSize),
			roundToGrid(mousePos.Y, gridSize),
			roundToGrid(mousePos.Z, gridSize)
		)
		moveToGridPosition(gridPos)
	end
end)

local function onUpdate()
	local mouseRay = mouse.UnitRay
	local raycastParams = RaycastParams.new()
	raycastParams.FilterDescendantsInstances = {workspace.Grid}
	raycastParams.FilterType = Enum.RaycastFilterType.Include
	raycastParams.IgnoreWater = true

	local raycastResult = workspace:Raycast(mouseRay.Origin, mouseRay.Direction * 1000, raycastParams)
	if raycastResult then
		local hitPos = raycastResult.Position
		local gridPos = Vector3.new(
			roundToGrid(hitPos.X, gridSize),
			roundToGrid(hitPos.Y, gridSize),
			roundToGrid(hitPos.Z, gridSize)
		)
		previewBlock.Position = gridPos
	end
end

RunService.RenderStepped:Connect(onUpdate)

Uhsleep91 · July 13, 2024, 1:40am

I feel like the best way to do this would be to properly calculate the walkable graph that A* will use. It looks like in getNeighbors, you’re always adding every direction as a neighbor even if there’s an obstacle in that direction? Would it be better to check if there is an obstacle in that direction and if so, don’t add it to the list of neighbors?

Edit: I just realized that’s exactly what @JamesBlossoms did lol. Not sure why that didn’t work unless isPositionClear wasn’t working as intended.

Edit 2:
Wait, based on the most recent code you posted, you’re still adding the direction as a neighbor every time?

for _, direction in ipairs(directions) do
	local neighbor = node + direction * gridSize
	print(isPositionClear(neighbor))
	table.insert(neighbors, neighbor)
end

The_NinjaBR0 · July 13, 2024, 1:15pm

In this case, I still continue adding it because I only added the print to check the return, which in the video I showed that always returns true even with objects in the path.

The_NinjaBR0 · July 13, 2024, 6:02pm

I managed to fix it, after some refactoring and testing I made the system work the way I wanted. For those who are still curious:

NodeModule:

local Node = {}
Node.__index = Node

function Node.new(position, parent)
	return setmetatable({
		Position = position,
		Parent = parent,
		Cost = 0,
	}, Node)
end

return Node

ServerScript:

local Node = require(game.ReplicatedStorage:WaitForChild("Node"))

local pathParts = {}
local lastStartPos, lastGoalPos

local function heuristic(a, b)
	return math.abs(a.X - b.X) + math.abs(a.Y - b.Y) + math.abs(a.Z - b.Z)
end

local function isWalkable(position)
	local part = workspace:FindPartOnRay(Ray.new(position + Vector3.new(0, 5, 0), Vector3.new(0, -10, 0)))
	return part == nil or part.Name ~= "Wall"
end

local function snapToGrid(position)
	local gridSize = 4
	return Vector3.new(
		math.round(position.X / gridSize) * gridSize,
		position.Y,
		math.round(position.Z / gridSize) * gridSize
	)
end

local function aStarSearch(start, goal)
	local openSet = {}
	local closedSet = {}
	local gScores = {}
	local fScores = {}

	local startNode = Node.new(snapToGrid(start), nil)
	gScores[startNode.Position] = 0
	fScores[startNode.Position] = heuristic(startNode.Position, snapToGrid(goal))

	openSet[startNode.Position] = startNode

	while next(openSet) do
		local currentNode = nil
		local lowestF = math.huge

		for _, node in pairs(openSet) do
			if fScores[node.Position] < lowestF then
				lowestF = fScores[node.Position]
				currentNode = node
			end
		end

		openSet[currentNode.Position] = nil

		if currentNode.Position == snapToGrid(goal) then
			local path = {}
			while currentNode do
				table.insert(path, 1, currentNode.Position)
				currentNode = currentNode.Parent
			end
			return path
		end

		closedSet[currentNode.Position] = true

		for _, direction in ipairs({
			Vector3.new(4, 0, 0), Vector3.new(-4, 0, 0),
			Vector3.new(0, 0, 4), Vector3.new(0, 0, -4)
		}) do
			local neighborPos = snapToGrid(Vector3.new(currentNode.Position.X + direction.X, start.Y, currentNode.Position.Z + direction.Z))

			if isWalkable(neighborPos) and not closedSet[neighborPos] then
				local neighbor = Node.new(neighborPos, currentNode)
				local tentative_gScore = gScores[currentNode.Position] + 1

				if not openSet[neighbor.Position] then
					openSet[neighbor.Position] = neighbor
				elseif tentative_gScore >= (gScores[neighbor.Position] or math.huge) then
					continue
				end

				gScores[neighbor.Position] = tentative_gScore
				fScores[neighbor.Position] = tentative_gScore + heuristic(neighbor.Position, snapToGrid(goal))
			end
		end
	end

	return nil
end

local function createPart(position, name)
	local part = Instance.new("Part")
	part.Size = Vector3.new(4, 4, 4)
	part.Material = Enum.Material.SmoothPlastic
	part.Transparency = 0.5
	part.Position = position
	part.Anchored = true
	part.Name = name
	part.CanCollide = false
	part.Parent = workspace
	return part
end

local function updatePath()
	local startBlock = workspace:FindFirstChild("Start")
	local finalBlock = workspace:FindFirstChild("Final")

	if not startBlock or not finalBlock then
		print("Start or Final block not found.")
		return
	end

	local start = snapToGrid(startBlock.Position)
	local goal = snapToGrid(finalBlock.Position)
	if lastStartPos ~= start or lastGoalPos ~= goal then
		for _, part in ipairs(pathParts) do
			part:Destroy()
		end
		pathParts = {}

		lastStartPos = start
		lastGoalPos = goal

		local path = aStarSearch(start, goal)

		if path then
			for i, step in ipairs(path) do
				local part = createPart(step, "StepPart_" .. i)
				table.insert(pathParts, part)
				print("Step: ", step)
			end
		else
			print("No path found.")
		end
	end
end

game:GetService("RunService").Heartbeat:Connect(updatePath)

Showcase Video:

Edit: For those who want to see the difference between the most famous algorithms:

system · July 27, 2024, 6:02pm

This topic was automatically closed 14 days after the last reply. New replies are no longer allowed.