Skip to content

Instantly share code, notes, and snippets.

@SquidLord
Last active June 17, 2024 08:21
Show Gist options
  • Save SquidLord/4741746 to your computer and use it in GitHub Desktop.
Save SquidLord/4741746 to your computer and use it in GitHub Desktop.
egps: A* pathfinding library for Minecraft ComputerCraft turtles.
-- This library provide high level turtle movement functions.
--
-- Before being able to use them, you should start the GPS with egps.startGPS()
-- then get your current location with egps.setLocationFromGPS().
-- egps.forward(), egps.back(), egps.up(), egps.down(), egps.turnLeft(), egps.turnRight()
-- replace the standard turtle functions.
-- If you need to use the standard functions, you
-- should call egps.setLocationFromGPS() again before using any egps functions.
-- Gist at: https://gist.github.com/SquidLord/4741746
-- The MIT License (MIT)
-- Copyright (c) 2012 Alexander Williams
-- 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.
-- THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
-- IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
-- FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
-- AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
-- LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
-- OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
-- SOFTWARE.
function empty(table)
for _, value in pairs(table) do
if value ~= nil then
return false
end
end
return true
end
-- Cache of the current turtle position and direction
local cachedX, cachedY, cachedZ, cachedDir
-- Directions
North, West, South, East, Up, Down = 0, 1, 2, 3, 4, 5
local shortNames = {[North] = "N", [West] = "W", [South] = "S",
[East] = "E", [Up] = "U", [Down] = "D" }
local deltas = {[North] = {0, 0, -1}, [West] = {-1, 0, 0}, [South] = {0, 0, 1},
[East] = {1, 0, 0}, [Up] = {0, 1, 0}, [Down] = {0, -1, 0}}
-- cache world geometry
local cachedWorld = {}
-- A* parameters
local stopAt, nilCost = 1500, 1000
----------------------------------------
-- detectAll
--
-- function: Detect in all possible directions
--
function detectAll()
local F, U, D = deltas[cachedDir], deltas[Up], deltas[Down]
cachedWorld[cachedX..":"..cachedY..":"..cachedZ] = 0
if not turtle.detect() then cachedWorld[(cachedX + F[1])..":"..(cachedY + F[2])..":"..(cachedZ + F[3])] = 0 end
if not turtle.detectUp() then cachedWorld[(cachedX + U[1])..":"..(cachedY + U[2])..":"..(cachedZ + U[3])] = 0 end
if not turtle.detectDown() then cachedWorld[(cachedX + D[1])..":"..(cachedY + D[2])..":"..(cachedZ + D[3])] = 0 end
end
----------------------------------------
-- forward
--
-- function: Move the turtle forward if possible and put the result in cache
-- return: boolean "success"
--
function forward()
local D = deltas[cachedDir]
local x, y, z = cachedX + D[1], cachedY + D[2], cachedZ + D[3]
local idx_pos = x..":"..y..":"..z
if turtle.forward() then
cachedX, cachedY, cachedZ = x, y, z
detectAll()
return true
else
cachedWorld[idx_pos] = (turtle.detect() and 1 or 0.5)
return false
end
end
----------------------------------------
-- back
--
-- function: Move the turtle backward if possible and put the result in cache
-- return: boolean "success"
--
function back()
local D = deltas[cachedDir]
local x, y, z = cachedX - D[1], cachedY - D[2], cachedZ - D[3]
local idx_pos = x..":"..y..":"..z
if turtle.back() then
cachedX, cachedY, cachedZ = x, y, z
detectAll()
return true
else
cachedWorld[idx_pos] = 0.5
return false
end
end
----------------------------------------
-- up
--
-- function: Move the turtle up if possible and put the result in cache
-- return: boolean "success"
--
function up()
local D = deltas[Up]
local x, y, z = cachedX + D[1], cachedY + D[2], cachedZ + D[3]
local idx_pos = x..":"..y..":"..z
if turtle.up() then
cachedX, cachedY, cachedZ = x, y, z
detectAll()
return true
else
cachedWorld[idx_pos] = (turtle.detectUp() and 1 or 0.5)
return false
end
end
----------------------------------------
-- down
--
-- function: Move the turtle down if possible and put the result in cache
-- return: boolean "success"
--
function down()
local D = deltas[Down]
local x, y, z = cachedX + D[1], cachedY + D[2], cachedZ + D[3]
local idx_pos = x..":"..y..":"..z
if turtle.down() then
cachedX, cachedY, cachedZ = x, y, z
detectAll()
return true
else
cachedWorld[idx_pos] = (turtle.detectDown() and 1 or 0.5)
return false
end
end
----------------------------------------
-- turnLeft
--
-- function: Turn the turtle to the left and put the result in cache
-- return: boolean "success"
--
function turnLeft()
cachedDir = (cachedDir + 1) % 4
turtle.turnLeft()
detectAll()
return true
end
----------------------------------------
-- turnRight
--
-- function: Turn the turtle to the right and put the result in cache
-- return: boolean "success"
--
function turnRight()
cachedDir = (cachedDir + 3) % 4
turtle.turnRight()
detectAll()
return true
end
----------------------------------------
-- turnTo
--
-- function: Turn the turtle to the choosen direction and put the result in cache
-- input: number _targetDir
-- return: boolean "success"
--
function turnTo(_targetDir)
if _targetDir == cachedDir then
return true
elseif ((_targetDir - cachedDir + 4) % 4) == 1 then
turnLeft()
elseif ((cachedDir - _targetDir + 4) % 4) == 1 then
turnRight()
else
turnLeft()
turnLeft()
end
return true
end
----------------------------------------
-- clearWorld
--
-- function: Clear the world cache
--
function clearWorld()
cachedWorld = {}
detectAll()
end
----------------------------------------
-- heuristic_cost_estimate
--
-- function: A* heuristic
-- input: X, Y, Z of the 2 points
-- return: Manhattan distance between the 2 points
--
local function heuristic_cost_estimate(x1, y1, z1, x2, y2, z2)
return math.abs(x2 - x1) + math.abs(y2 - y1) + math.abs(z2 - z1)
end
----------------------------------------
-- reconstruct_path
--
-- function: A* path reconstruction
-- input: A* visited nodes and goal
-- return: List of movement to be executed
--
local function reconstruct_path(_cameFrom, _currentNode)
if _cameFrom[_currentNode] ~= nil then
local dir, nextNode = _cameFrom[_currentNode][1], _cameFrom[_currentNode][2]
local path = reconstruct_path(_cameFrom, nextNode)
table.insert(path, dir)
return path
else
return {}
end
end
----------------------------------------
-- a_star
--
-- function: A* path finding
-- input: start and goal coordinates
-- return: List of movement to be executed
--
local function a_star(x1, y1, z1, x2, y2, z2, discover)
discover = discover or 1
local start, idx_start = {x1, y1, z1}, x1..":"..y1..":"..z1
local goal, idx_goal = {x2, y2, z2}, x2..":"..y2..":"..z2
if (cachedWorld[idx_goal] or 0) == 0 then
local openset, closedset, cameFrom, g_score, f_score, tries = {}, {}, {}, {}, {}, 0
openset[idx_start] = start
g_score[idx_start] = 0
f_score[idx_start] = heuristic_cost_estimate(x1, y1, z1, x2, y2, z2)
while not empty(openset) do
local current, idx_current
local cur_f = 9999999
for idx_cur, cur in pairs(openset) do
if cur ~= nil and f_score[idx_cur] <= cur_f then
idx_current, current, cur_f = idx_cur, cur, f_score[idx_cur]
end
end
if idx_current == idx_goal then
return reconstruct_path(cameFrom, idx_goal)
end
-- no more than 500 moves
if cur_f >= stopAt then
break
end
openset[idx_current] = nil
closedset[idx_current] = true
local x3, y3, z3 = current[1], current[2], current[3]
for dir = 0, 5 do -- for all direction find the neighbor of the current position
local D = deltas[dir]
local x4, y4, z4 = x3 + D[1], y3 + D[2], z3 + D[3]
local neighbor, idx_neighbor = {x4, y4, z4}, x4..":"..y4..":"..z4
if (cachedWorld[idx_neighbor] or 0) == 0 then -- if its free or unknow
if closedset[idx_neighbor] == nil then
local tentative_g_score = g_score[idx_current] + ((cachedWorld[idx_neighbor] == nil) and discover or 1)
if openset[idx_neighbor] == nil or tentative_g_score <= g_score[idx_neighbor] then
cameFrom[idx_neighbor] = {dir, idx_current}
g_score[idx_neighbor] = tentative_g_score
f_score[idx_neighbor] = tentative_g_score + heuristic_cost_estimate(x4, y4, z4, x2, y2, z2)
openset[idx_neighbor] = neighbor
end
end
end
end
end
end
print("no path found")
return {}
end
----------------------------------------
-- moveTo
--
-- function: Move the turtle to the choosen coordinates in the world
-- input: X, Y, Z and direction of the goal
-- return: boolean "success"
--
function moveTo(_targetX, _targetY, _targetZ, _targetDir, changeDir, discover)
changeDir = (changeDir == nil) and true or changeDir
while cachedX ~= _targetX or cachedY ~= _targetY or cachedZ ~= _targetZ do
local path = a_star(cachedX, cachedY, cachedZ, _targetX, _targetY, _targetZ, discover)
if #path == 0 then
return false
end
for i, dir in ipairs(path) do
if dir == Up then
if not up() then
break
end
elseif dir == Down then
if not down() then
break
end
else
turnTo(dir)
if not forward() then
break
end
end
end
end
if changeDir then
turnTo(_targetDir)
end
return true
end
----------------------------------------
-- discoverWorld
--
-- function: Move the turtle to the choosen coordinates in the world
-- input: size of the cuboid to check
--
function discoverWorld(_range)
local x, y, z, d = locate()
-- Try to go to every location in the cuboid
for r = 1, _range do
for dx = -r, r do
for dy = -r, r do
for dz = -r, r do
local idx_goal = (x+dx)..":"..(y+dy)..":"..(z+dz)
if cachedWorld[idx_goal] == nil then
moveTo(x+dx, y+dy, z+dz, cachedDir, false, nilCost)
sleep(0.01)
end
end
end
end
end
-- Go back to the starting point
moveTo(x, y, z, d)
end
----------------------------------------
-- setLocation
--
-- function: Set the current X, Y, Z and direction of the turtle
--
function setLocation(x, y, z, d)
cacheX, cacheY, cacheZ, cacheDir = x, y, z, d
return cacheX, cacheY, cacheZ, cacheDir
end
----------------------------------------
-- startGPS
--
-- function: Open the rednet network
-- return: boolean "success"
--
function startGPS()
local netOpen, modemSide = false, nil
for _, side in pairs(rs.getSides()) do -- for all sides
if peripheral.getType(side) == "modem" then -- find the modem
modemSide = side
if rednet.isOpen(side) then -- check its status
netOpen = true
break
end
end
end
if not netOpen then -- if the rednet network is not open
if modemSide then -- and we found a modem, open the rednet network
rednet.open(modemSide)
else
print("No modem found")
return false
end
end
return true
end
----------------------------------------
-- setGPSLocation
--
-- function: Retrieve the turtle GPS position and direction (if possible)
-- return: current X, Y, Z and direction of the turtle
--
function setLocationFromGPS()
if startGPS() then
-- get the current position
cachedX, cachedY, cachedZ = gps.locate(4, false)
cachedDir = nil
-- determine the current direction
for tries = 0, 3 do -- try to move in one direction
if turtle.forward() then
local newX, _, newZ = gps.locate(4, false) -- get the new position
turtle.back() -- and go back
-- deduce the curent direction
if newZ < cachedZ then
cachedDir = North -- North
elseif newZ > cachedZ then
cachedDir = South -- South
elseif newX < cachedX then
cachedDir = West -- West
elseif newX > cachedX then
cachedDir = East -- East
end
-- Cancel out the tries
turnTo((cachedDir - tries + 4) % 4)
-- exit the loop
break
else -- try in another direction
tries = tries + 1
turtle.turnLeft()
end
end
if cachedDir == nil then
print("Could not determine direction")
end
-- Return the current turtle position
return cachedX, cachedY, cachedZ, cachedDir
end
end
----------------------------------------
-- getLocation
--
-- function: Retrieve the cached turtle position and direction
-- return: cached X, Y, Z and direction of the turtle
--
function getLocation()
return cachedX, cachedY, cachedZ, cachedDir
end
@NevesAngelesLeo
Copy link

Hi i was super interested in this library because im working on an "Mister Handy-like" OS for computercraft turtles and an already working pathfinding would speed up things. But im having some problems, maybe im doing something wrong but, when i run egps.setLocationFromGPS() it makes 3 turns but fail to move to determine directions and proceeds to say that "Could not determine direction". I would be very gratefull if you could shed some light into this :)

@SquidLord
Copy link
Author

It's been about 16 billion years since I wrote this code, but my guess would be that if you try to set the initialization location from GPS and it simply turns in place three times before erroring out – the GPS module is not returning data properly OR the turtle is not actually able to move forward.

Note that the code requires that it be able to move in order to change the GPS coordinate. If it can't move, it can't compare new data.

@NevesAngelesLeo
Copy link

Hey you were right. I fixed my mods an then it started working normally. Thank ypu so much for this, i will be sure to give you the credits!

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment