blunty666

starNav

Jul 1st, 2014
613
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. if not tinyMap then
  2.     if not os.loadAPI("tinyMap") then
  3.         error("could not load tinyMap API")
  4.     end
  5. end
  6. if not aStar then
  7.     if not os.loadAPI("aStar") then
  8.         error("could not load aStar API")
  9.     end
  10. end
  11. if not location then
  12.     if not os.loadAPI("location") then
  13.         error("could not load location API")
  14.     end
  15. end
  16.  
  17. local position
  18.  
  19. local SESSION_MAX_DISTANCE_DEFAULT = 32
  20.  
  21. local SESSION_COORD_CLEAR = 1
  22. local SESSION_COORD_BLOCKED = 2
  23. local SESSION_COORD_DISTANCE = math.huge
  24. local MAIN_COORD_CLEAR = nil
  25. local MAIN_COORD_BLOCKED = 1
  26. local MAIN_COORD_DISTANCE = 1024
  27.  
  28. local sessionMidPoint
  29. local sessionMaxDistance
  30. local sessionMap
  31. local mainMap
  32.  
  33. local function sup_norm(a, b)
  34.     return math.max(math.abs(a.x - b.x), math.abs(a.y - b.y), math.abs(a.z - b.z))
  35. end
  36.  
  37. local function distanceFunc(a, b)
  38.     local sessionMapA, sessionMapB = sessionMap:get(a), sessionMap:get(b)
  39.     if sup_norm(a, sessionMidPoint) > sessionMaxDistance then
  40.         return SESSION_COORD_DISTANCE -- first coord is outside the search region
  41.     elseif sup_norm(b, sessionMidPoint) > sessionMaxDistance then
  42.         return SESSION_COORD_DISTANCE -- second coord is outside the search region
  43.     elseif sessionMapA == SESSION_COORD_BLOCKED or sessionMapB == SESSION_COORD_BLOCKED then
  44.         return SESSION_COORD_DISTANCE -- one of the coords has been found to be blocked this during this session
  45.     elseif sessionMapA == SESSION_COORD_CLEAR and sessionMapA == SESSION_COORD_CLEAR then
  46.         return aStar.distance(a, b) -- both coords has been found to be clear this during this session
  47.     elseif mainMap:get(a) or mainMap:get(b) then
  48.         return MAIN_COORD_DISTANCE
  49.     end
  50.     return aStar.distance(a, b) -- we are assuming both coords are clear
  51. end
  52.  
  53. local directions = {
  54.     [vector.new(0, 0, 1)] = 0,
  55.     [vector.new(-1, 0, 0)] = 1,
  56.     [vector.new(0, 0, -1)] = 2,
  57.     [vector.new(1, 0, 0)] = 3,
  58.     [vector.new(0, 1, 0)] = 4,
  59.     [vector.new(0, -1, 0)] = 5,
  60. }
  61.  
  62. local function deltaToDirection(delta)
  63.     for vec, dir in pairs(directions) do
  64.         if aStar.vectorEquals(delta, vec) then
  65.             return dir
  66.         end
  67.     end
  68. end
  69.  
  70. local function tryMove()
  71.     for i = 1, 4 do
  72.         if turtle.forward() then
  73.             return true
  74.         end
  75.         turtle.turnRight()
  76.     end
  77.     return false
  78. end
  79.  
  80. local function findPosition()
  81.     local move = turtle.up
  82.     while not tryMove() do
  83.         if not move() then
  84.             if move == turtle.up then
  85.                 move = turtle.down
  86.                 move()
  87.             else
  88.                 error("trapped in a ridiculous place")
  89.             end
  90.         end
  91.     end
  92.    
  93.     local p1 = {gps.locate()}
  94.     if #p1 == 3 then
  95.         p1 = vector.new(unpack(p1))
  96.     else
  97.         error("no gps signal - phase 1")
  98.     end
  99.    
  100.     if not turtle.back() then
  101.         error("couldn't move to determine direction")
  102.     end
  103.    
  104.     local p2 = {gps.locate()}
  105.     if #p2 == 3 then
  106.         p2 = vector.new(unpack(p2))
  107.     else
  108.         error("no gps signal - phase 2")
  109.     end
  110.    
  111.     local direction = deltaToDirection(p1 - p2)
  112.     if direction and direction < 4 then
  113.         return location.new(p2.x, p2.y, p2.z, direction)
  114.     else
  115.         return false
  116.     end
  117. end
  118.  
  119. local function detect(currPos, adjPos)
  120.     local direction = deltaToDirection(adjPos - currPos)
  121.     if direction then
  122.         position:setHeading(direction)
  123.         if direction == 4 then
  124.             return turtle.detectUp()
  125.         elseif direction == 5 then
  126.             return turtle.detectDown()
  127.         else
  128.             return turtle.detect()
  129.         end
  130.     end
  131.     return false
  132. end
  133.  
  134. local function inspect(currPos, adjPos)
  135.     local direction = deltaToDirection(adjPos - currPos)
  136.     if direction then
  137.         position:setHeading(direction)
  138.         if direction == 4 then
  139.             return turtle.inspectUp()
  140.         elseif direction == 5 then
  141.             return turtle.inspectDown()
  142.         else
  143.             return turtle.inspect()
  144.         end
  145.     end
  146.     return false
  147. end
  148.  
  149. local function updateCoord(coord, isBlocked)
  150.     if isBlocked then
  151.         sessionMap:set(coord, SESSION_COORD_BLOCKED)
  152.         mainMap:set(coord, MAIN_COORD_BLOCKED)
  153.     else
  154.         sessionMap:set(coord, SESSION_COORD_CLEAR)
  155.         mainMap:set(coord, MAIN_COORD_CLEAR)
  156.     end
  157. end
  158.  
  159. local function detectAll(currPos)
  160.     for _, pos in ipairs(aStar.adjacent(currPos)) do -- better order of checking directions
  161.         updateCoord(pos, detect(currPos, pos))
  162.     end
  163. end
  164.  
  165. local function findSensor()
  166.     for _, side in ipairs(peripheral.getNames()) do
  167.         if peripheral.getType(side) == "turtlesensorenvironment" then
  168.             return side
  169.         end
  170.     end
  171.     return false
  172. end
  173.  
  174. local function scan(currPos)
  175.     local sensorSide = findSensor()
  176.     if sensorSide then
  177.         local rawBlockInfo = peripheral.call(sensorSide, "sonicScan")
  178.         local sortedBlockInfo = aStar.newMap()
  179.         for _, blockInfo in ipairs(rawBlockInfo) do
  180.             sortedBlockInfo:set(currPos + vector.new(blockInfo.x, blockInfo.y, blockInfo.z), blockInfo)
  181.         end
  182.         local toCheckQueue = {}
  183.         for _, pos in ipairs(aStar.adjacent(currPos)) do
  184.             if sortedBlockInfo:get(pos) then
  185.                 table.insert(toCheckQueue, pos)
  186.             end
  187.         end
  188.         while toCheckQueue[1] do
  189.             local pos = table.remove(toCheckQueue, 1)
  190.             local blockInfo = sortedBlockInfo:get(pos)
  191.             if blockInfo.type == "AIR" then
  192.                 for _, pos2 in ipairs(aStar.adjacent(pos)) do
  193.                     local blockInfo2 = sortedBlockInfo:get(pos2)
  194.                     if blockInfo2 and not blockInfo2.checked then
  195.                         table.insert(toCheckQueue, pos2)
  196.                     end
  197.                 end
  198.                 updateCoord(pos, false)
  199.             else
  200.                 updateCoord(pos, true)
  201.             end
  202.             blockInfo.checked = true
  203.         end
  204.     else
  205.         detectAll(currPos)
  206.     end
  207.     mainMap:save()
  208. end
  209.  
  210. local function move(currPos, adjPos)
  211.     local direction = deltaToDirection(adjPos - currPos)
  212.     if direction then
  213.         position:setHeading(direction)
  214.         if direction == 4 then
  215.             return position:up()
  216.         elseif direction == 5 then
  217.             return position:down()
  218.         else
  219.             return position:forward()
  220.         end
  221.     end
  222.     return false
  223. end
  224.  
  225. function goto(x, y, z, maxDistance)
  226.     if not mainMap then
  227.         error("mainMap has not been specified")
  228.     end
  229.     if turtle.getFuelLevel() == 0 then
  230.         return false, "ran out of fuel"
  231.     end
  232.     if not position then
  233.         position = findPosition()
  234.         if not position then
  235.             return false, "couldn't determine location"
  236.         end
  237.     end
  238.    
  239.     local goal = vector.new(tonumber(x), tonumber(y), tonumber(z))
  240.  
  241.     sessionMap = aStar.newMap()
  242.     sessionMidPoint = vector.new(math.floor((goal.x + position.x)/2), math.floor((goal.y + position.y)/2), math.floor((goal.z + position.z)/2))
  243.     sessionMaxDistance = (type(maxDistance) == "number" and maxDistance) or math.max(2*sup_norm(sessionMidPoint, goal), SESSION_MAX_DISTANCE_DEFAULT)
  244.  
  245.     local path = aStar.compute(distanceFunc, position, goal)
  246.     if not path then
  247.         return false, "no known path to goal"
  248.     end
  249.     while not aStar.vectorEquals(position, goal) do
  250.         local movePos = table.remove(path)
  251.         while not move(position, movePos) do
  252.             local blockPresent, blockData = inspect(position, movePos)
  253.             local recalculate, isTurtle = false, false
  254.             if blockPresent and (blockData.name == "ComputerCraft:CC-TurtleAdvanced" or blockData.name == "ComputerCraft:CC-Turtle") then -- there is a turtle in the way
  255.                 sleep(math.random(0, 3))
  256.                 local blockPresent2, blockData2 = inspect(position, movePos)
  257.                 if blockPresent2 and (blockData2.name == "ComputerCraft:CC-TurtleAdvanced" or blockData2.name == "ComputerCraft:CC-Turtle") then -- the turtle is still there
  258.                     recalculate, isTurtle = true, true
  259.                 end
  260.             elseif blockPresent then
  261.                 recalculate = true
  262.             elseif turtle.getFuelLevel() == 0 then
  263.                 return false, "ran out of fuel"
  264.             else
  265.                 sleep(1)
  266.             end
  267.             if recalculate then
  268.                 scan(position)
  269.                 if sessionMap:get(goal) == SESSION_COORD_BLOCKED then return false, "goal is blocked" end
  270.                 path = aStar.compute(distanceFunc, position, goal)
  271.                 if not path then
  272.                     return false, "no known path to goal"
  273.                 end
  274.                 if isTurtle then
  275.                     sessionMap:set(movePos, nil)
  276.                 end
  277.                 movePos = table.remove(path)
  278.             end
  279.         end
  280.         if mainMap:get(movePos) then
  281.             mainMap:set(movePos, MAIN_COORD_CLEAR)
  282.         end
  283.     end
  284.     return true
  285. end
  286.  
  287. function setMap(mapName)
  288.     if type(mapName) ~= "string" then
  289.         error("mapName must be string")
  290.     end
  291.     mainMap = tinyMap.new(mapName)
  292. end
  293.  
  294. function getMap()
  295.     return mainMap
  296. end
  297.  
  298. function getPosition()
  299.     if position then
  300.         return position:value()
  301.     end
  302. end
RAW Paste Data