Created
October 17, 2025 20:16
-
-
Save dgxo/deb5ae0ca58942e143ae6326011ee63b to your computer and use it in GitHub Desktop.
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| local function binaryHeapInsert(heap, node) | |
| table.insert(heap, node) | |
| local index = #heap | |
| while index > 1 do | |
| local parent = math.floor(index / 2) | |
| if heap[parent].fCost > heap[index].fCost then | |
| heap[parent], heap[index] = heap[index], heap[parent] | |
| index = parent | |
| else | |
| break | |
| end | |
| end | |
| end | |
| local function binaryHeapExtract(heap) | |
| if #heap == 0 then return nil end | |
| local root = heap[1] | |
| heap[1] = heap[#heap] | |
| table.remove(heap, #heap) | |
| local index = 1 | |
| while index * 2 <= #heap do | |
| local smallest = index | |
| local left = index * 2 | |
| local right = index * 2 + 1 | |
| if heap[left].fCost < heap[smallest].fCost then smallest = left end | |
| if right <= #heap and heap[right].fCost < heap[smallest].fCost then smallest = right end | |
| if smallest ~= index then | |
| heap[index], heap[smallest] = heap[smallest], heap[index] | |
| index = smallest | |
| else | |
| break | |
| end | |
| end | |
| return root | |
| end | |
| return function(startPart, targetPosition, cellSize, maxPathLength, heuristic, debugCallback) | |
| cellSize = cellSize or 4 | |
| maxPathLength = maxPathLength or 1000 | |
| heuristic = heuristic or function(a, b) | |
| return math.abs(a.X - b.X) + math.abs(a.Y - b.Y) + math.abs(a.Z - b.Z) | |
| end | |
| local startPosition = startPart.Position | |
| local function gridKey(position) | |
| return math.floor(position.X / cellSize) .. "," .. math.floor(position.Y / cellSize) .. "," .. math.floor(position.Z / cellSize) | |
| end | |
| local function isObstacle(position) | |
| local ray = Ray.new(position, Vector3.new(0, 0.1, 0)) | |
| local hit, _ = workspace:FindPartOnRay(ray, startPart) | |
| return hit and hit:HasTag("Obstacle") | |
| end | |
| local openSet = {} | |
| local closedSet = {} | |
| local gCosts = {} | |
| local fCosts = {} | |
| local cameFrom = {} | |
| local startKey = gridKey(startPosition) | |
| local targetKey = gridKey(targetPosition) | |
| gCosts[startKey] = 0 | |
| fCosts[startKey] = heuristic(startPosition, targetPosition) | |
| binaryHeapInsert(openSet, { | |
| position = startPosition, | |
| key = startKey, | |
| gCost = 0, | |
| fCost = fCosts[startKey] | |
| }) | |
| while #openSet > 0 do | |
| local current = binaryHeapExtract(openSet) | |
| if not current then break end | |
| if debugCallback then | |
| debugCallback(current.position) | |
| end | |
| if current.key == targetKey then | |
| local path = {} | |
| local key = targetKey | |
| while cameFrom[key] do | |
| table.insert(path, 1, current.position) | |
| key = cameFrom[key] | |
| for nodeKey, nodeData in pairs(gCosts) do | |
| if nodeKey == key then | |
| current = { position = Vector3.new(tonumber(nodeKey:match("([^,]+)")), tonumber(nodeKey:match(",([^,]+)")), tonumber(nodeKey:match(",([^,]+)$"))), key = key } | |
| break | |
| end | |
| end | |
| end | |
| table.insert(path, 1, startPosition) | |
| return path | |
| end | |
| closedSet[current.key] = true | |
| for dx = -1, 1 do | |
| for dy = -1, 1 do | |
| for dz = -1, 1 do | |
| if dx == 0 and dy == 0 and dz == 0 then continue end | |
| local neighborPos = current.position + Vector3.new(dx * cellSize, dy * cellSize, dz * cellSize) | |
| local neighborKey = gridKey(neighborPos) | |
| if closedSet[neighborKey] or isObstacle(neighborPos) then continue end | |
| local tentativeGCost = gCosts[current.key] + (current.position - neighborPos).Magnitude | |
| if gCosts[neighborKey] and tentativeGCost >= gCosts[neighborKey] then continue end | |
| if #path and tentativeGCost / cellSize > maxPathLength then continue end | |
| cameFrom[neighborKey] = current.key | |
| gCosts[neighborKey] = tentativeGCost | |
| fCosts[neighborKey] = tentativeGCost + heuristic(neighborPos, targetPosition) | |
| binaryHeapInsert(openSet, { | |
| position = neighborPos, | |
| key = neighborKey, | |
| gCost = tentativeGCost, | |
| fCost = fCosts[neighborKey] | |
| }) | |
| end | |
| end | |
| end | |
| end | |
| return nil | |
| end |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment