joeld98

A* in computercraft

Aug 10th, 2016
90
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. local function log(msg) -- adapted from: https://github.com/rxi/log.lua
  2.   if not os.loadAPI("rwt") then
  3.         if not shell.run("pastebin", "get", "TNLjCCKq", "rwt") then
  4.             print("Could not load Time API")
  5.         else
  6.             os.loadAPI("rwt")
  7.         end
  8.   end
  9.   logTime = rwt.getTime("utc",-4,dateTime24_mmddyyyy,10)
  10.   local fp = io.open("seek.log", "a")
  11.   local str = string.format("[%s]%s\n",logTime, msg)
  12.       fp:write(str)
  13.       fp:close()
  14. end
  15.  
  16. local function saveTable(table,fileName)
  17.   local hWrite = fs.open(fileName, "w")
  18.   hWrite.write(textutils.serialize(table))
  19.   hWrite.close()
  20. end
  21.  
  22. Node = {  --Thanks to Karmakilledthecat for the function tutorials
  23.   x=0, y=0, z=0, parentx=0,parenty=0,parentz=0,parentg=0,startx=0,starty=0,startz = 0,endz=0,endx=0,endy=0,endz=0,g=0,h=0,f=0, w = 0, state = "",block_type = "", meta = {},
  24.    
  25.   new = function(nodeVector,startVector,endVector,parentNode)
  26.    if  nodeVector then
  27.       if type(nodeVector) ~= "table" then
  28.         error("vector expected",2)
  29.       elseif not nodeVector.x then
  30.         error("vector expected",2)
  31.       end
  32.     else
  33.       error("vector expected",2)
  34.     end
  35.     if  startVector then
  36.       if type(startVector) ~= "table" then
  37.         error("vector expected",2)
  38.       elseif not startVector.x then
  39.         error("vector expected",2)
  40.       end
  41.     else
  42.       error("vector expected",2)
  43.     end
  44.     if  endVector then
  45.       if type(endVector) ~= "table" then
  46.         error("vector expected",2)
  47.       elseif not endVector.x then
  48.         error("vector expected",2)
  49.       end
  50.     else
  51.       error("vector expected",2)
  52.     end  
  53.     if parentNode then
  54.       if type(parentNode) ~= "table" then
  55.         error("node expected",2)
  56.       elseif not parentNode.x then
  57.         error("node expected",2)
  58.       end
  59.     else
  60.      parentNode = { x = 0, y = 0, z = 0, g = 0, w = 0}  
  61.      --parentNode.x = startVector.x
  62.      --parentNode.y = startVector.y
  63.      --parentNode.z = startVector.z
  64.      parentNode.g = math.abs(( nodeVector.x - startVector.x) + (nodeVector.y - startVector.y) + (nodeVector.z-startVector.z)) + 10 -- so that parent cost is never 0 on first pass
  65.     end
  66.    
  67.     local n = {}
  68.     setmetatable(n,Node.meta)  
  69.     for k,v in pairs(Node) do
  70.       n[k] = v
  71.     end
  72.     n.x = nodeVector.x
  73.     n.y = nodeVector.y
  74.     n.z = nodeVector.z
  75.     n.parentx = parentNode.x
  76.     n.parenty = parentNode.y
  77.     n.parentz = parentNode.z
  78.     n.parentg = parentNode.g
  79.     n.startx = startVector.x
  80.     n.starty = startVector.y
  81.     n.startz = startVector.z
  82.     n.endx = endVector.x
  83.     n.endy = endVector.y
  84.     n.endz = endVector.z
  85.     n.g = math.abs((n.x-n.parentx) + (n.y-n.parenty) + (n.z-n.parentz)) + parentNode.g
  86.     n.h = math.abs((n.x-n.endx) + (n.y-n.endy) + (n.z-n.endz))
  87.     if n.parentx == 0 and n.parenty == 0 and n.parentz == 0 then
  88.     n.state = "closed"  
  89.     elseif n.g == 0 or n.h == 0 then
  90.       n.state = "closed"
  91.     else
  92.       n.state = "open"
  93.     end
  94.     n.f = n.g + n.h + n.w
  95.     return n
  96.   end,
  97.  
  98.   closeNode = function(n)
  99.     n.state = "closed"    
  100.   end,
  101.  
  102.   openNode = function(n)
  103.   if  n then
  104.       if type(n) ~= "table" then
  105.         error("Node expected",2)
  106.       elseif not n.x then
  107.         error("Node expected",2)
  108.       end
  109.     else
  110.       error("Node expected",2)
  111.     end
  112.     n.state = "open"    
  113.   end,
  114.  
  115.   calc = function(n)
  116.    n.g = math.abs((n.x-n.parentx) + (n.y-n.parenty) + (n.z-n.parentz)) + n.parentg
  117.    n.h = math.abs((n.x-n.endx) + (n.y-n.endy) + (n.z-n.endz))
  118.    n.f = n.g + n.h + n.w
  119.   end,
  120.  
  121.   raise = function(n)
  122.    n.w = n.w + 1
  123.    n.f = n.g + n.h + n.w
  124.   end,
  125.  
  126.   decrease = function(n)
  127.     if n.w >=1 then
  128.       n.w = n.w -1
  129.     end
  130.     n.f = n.g + n.h + n.w-1
  131.   end,
  132.  
  133.   serialize = function(n,filename)
  134.   end,
  135.  
  136.   unserialize = function(n)
  137.  
  138.   end,
  139. }
  140.  
  141. Node.meta.__tostring = function(n)
  142.   return "("..n.x..","..n.y..","..n.z..")".."("..n.parentx..","..n.parenty..","..n.parentz..")".."("..n.g..")".."("..n.h..")".."("..n.f..")".."("..n.state..")"
  143. end
  144.  
  145. local function getFacingDirection()
  146.   log("Get Facing Direction Called")
  147.   local function findClearBlock() -- returns the number of turns you must turn right to find a lear block at the current level
  148.   local turnsToClear = 0
  149.   while turtle.inspect() and turnsToClear <=4 do
  150.     turtle.turnRight()
  151.     turnsToClear = turnsToClear + 1
  152.   end
  153.   if turnsToClear >= 4 then
  154.     return nil
  155.   else
  156.   end
  157.   if turnsToClear ~=4 and turnsToClear ~=0 then
  158.     for faceHome = 1, turnsToClear do
  159.       turtle.turnLeft()
  160.     end
  161.   end
  162.   return turnsToClear
  163.   end
  164.   local startLoc = vector.new(gps.locate())
  165.   local dirLoc
  166.   local facingDir = 0
  167.   local facingVector
  168.   if turtle.forward() then
  169.     dirLoc = vector.new(gps.locate())
  170.     turtle.back()
  171.     facingVector = dirLoc - startLoc
  172.     facingDir = ((facingVector.x + math.abs(facingVector.x) * 2) + (facingVector.z + math.abs(facingVector.z) * 3))
  173.   else
  174.     local tCount = findClearBlock()
  175.     if tCount == nil then
  176.       return nil
  177.     elseif tCount == 2 then
  178.       turtle.back()
  179.       dirLoc = vector.new(gps.locate())
  180.       turtle.forward()
  181.       facingVector = startLoc - dirLoc
  182.       facingDir = (((facingVector.x + math.abs(facingVector.x) * 2) + (facingVector.z + math.abs(facingVector.z) * 3)))
  183.     elseif tCount == 1 then
  184.       turtle.turnRight()
  185.       turtle.forward()
  186.       turtle.turnLeft()
  187.       dirLoc = vector.new(gps.locate())
  188.       turtle.turnLeft()
  189.       turtle.forward()
  190.       turtle.turnRight()
  191.       facingVector = dirLoc - startLoc
  192.       facingDir = (((facingVector.x + math.abs(facingVector.x) * 2) + (facingVector.z + math.abs(facingVector.z) * 3))) - 1
  193.     elseif
  194.       tCount == 3 then
  195.       turtle.turnLeft()
  196.       turtle.forward()
  197.       turtle.turnRight()
  198.       dirLoc = vector.new(gps.locate())
  199.       turtle.turnRight()
  200.       turtle.forward()
  201.       turtle.turnLeft()
  202.       facingVector = dirLoc - startLoc
  203.       facingDir = ((facingVector.x + math.abs(facingVector.x) * 2) + (facingVector.z + math.abs(facingVector.z) * 3)) + 1
  204.       if facingDir > 4 then
  205.         facingDir = 1
  206.       end
  207.     end
  208.   end
  209.   return facingDir
  210. end
  211.  
  212. local function seekDestination(currentVector, targetVector,safeHeight, startVector)
  213.   local nodeList = {}
  214.   local bestMove
  215.   local hDisplacement
  216.   local function getNodeData() --get Neighbor details
  217.     log("GetNodeData Called")
  218.     local pNode = Node.new(currentVector,startVector,targetVector) --
  219.     log(string.format("pNode set to: %s", tostring(pNode)))
  220.     local fNode, bNode, lNode, rNode, uNode, dNode
  221.     fNode = Node.new(currentVector,startVector,targetVector,pNode) --Initialize each neighbor as local
  222.     bNode = Node.new(currentVector,startVector,targetVector,pNode)
  223.     lNode = Node.new(currentVector,startVector,targetVector,pNode)
  224.     rNode = Node.new(currentVector,startVector,targetVector,pNode)
  225.     uNode = Node.new(currentVector,startVector,targetVector,pNode)
  226.     dNode = Node.new(currentVector,startVector,targetVector,pNode)
  227.     facingDirection = getFacingDirection() -- need this to calculate neighbor offsets and move commands
  228.     if facingDirection == 1 then -- when facing west
  229.     if currentVector.x <= 0 then -- and we are in negative X coordinates
  230.       fNode.x = currentVector.x - 1 -- the node in front of us is our current x value - 1
  231.       bNode.x = currentVector.x + 1 -- the node in behind our current x value is + 1
  232.     else
  233.       fNode.x = currentVector.x + 1 -- when x is a possitive coordinate front is +1
  234.       bNode.x = currentVector.x - 1 -- and back is -1
  235.     end
  236.     if currentVector.z <= 0 then  
  237.       lNode.z = currentVector.z + 1  -- when z is negative left is +1
  238.       rNode.z = currentVector.z - 1  -- and right is -1
  239.     else
  240.       lNode.z = currentVector.z - 1  -- and the opposite when positive
  241.       rNode.z = currentVector.z + 1
  242.     end
  243.     elseif facingDirection == 2 then -- and so on for each direction. 0 and 3 are mirrors as are 2 and 4
  244.     if currentVector.x <= 0 then
  245.       fNode.x = currentVector.x + 1
  246.       bNode.x = currentVector.x - 1
  247.     else
  248.       fNode.x = currentVector.x - 1
  249.       bNode.x = currentVector.x + 1
  250.     end
  251.     if currentVector.z <=0 then
  252.       lNode.z = currentVector.z - 1
  253.       rNode.z = currentVector.z + 1
  254.     else
  255.       lNode.z = currentVector.z + 1
  256.       rNode.z = currentVector.z - 1
  257.     end
  258.     elseif facingDirection == 3 then
  259.     if currentVector.x <= 0 then
  260.       fNode.x = currentVector.x + 1
  261.       bNode.x = currentVector.x - 1
  262.     else
  263.       fNode.x = currentVector.x - 1
  264.       bNode.x = currentVector.x + 1
  265.     end
  266.     if currentVector.z <=0 then
  267.       lNode.z = currentVector.z - 1
  268.       rNode.z = currentVector.z + 1
  269.     else
  270.       lNode.z = currentVector.z + 1
  271.       rNode.z = currentVector.z - 1
  272.     end
  273.     elseif facingDirection == 4 then
  274.     if currentVector.x <= 0 then
  275.       fNode.x = currentVector.x - 1
  276.       bNode.x = currentVector.x + 1
  277.     else
  278.       fNode.x = currentVector.x + 1
  279.       bNode.x = currentVector.x - 1
  280.     end
  281.     if currentVector.z <=0 then
  282.       lNode.z = currentVector.z + 1
  283.       rNode.z = currentVector.z - 1
  284.     else
  285.       lNode.z = currentVector.z - 1
  286.       rNode.z = currentVector.z + 1
  287.     end
  288.     end
  289.     uNode.y = currentVector.y + 1 -- up is up
  290.     dNode.y = currentVector.y - 1 -- down is down no matter which way you are facing.
  291.     local fResult,fInspect = turtle.inspect() -- inspect Left, up and down at no cost.
  292.     local uResult,uInspect = turtle.inspectUp() -- using inspect instead of detect to capture block type
  293.     local dResult,dInspect = turtle.inspectDown() -- might need it later
  294.     turtle.turnRight()
  295.     local rResult,rInspect = turtle.inspect()  -- time/move cost but no fuel cost so get them all
  296.     turtle.turnRight()
  297.     local bResult,bInspect = turtle.inspect()
  298.     turtle.turnRight()
  299.     local lResult,lInspect = turtle.inspect()
  300.     turtle.turnRight()
  301.     if fResult == true then -- if any block is found close the node and keep the block type
  302.         fNode:closeNode()
  303.         fNode.block_type = fInspect.name
  304.     end
  305.     if uResult == true then
  306.       uNode:closeNode()
  307.       uNode.block_type = uInspect.name
  308.     end
  309.     if dResult == true then
  310.       dNode:closeNode()
  311.       dNode.block_type = dInspect.name
  312.     end
  313.     if rResult == true then
  314.       rNode:closeNode()
  315.       rNode.block_type = rInspect.name
  316.     end
  317.     if bResult == true then
  318.       bNode:closeNode()
  319.       bNode.block_type = bInspect.name
  320.     end
  321.     if lResult == true then
  322.       lNode:closeNode()
  323.       lNode.block_type = lInspect.name  
  324.     end
  325.     fNode:calc()  -- recalculate the scores now that actual coordinates are set
  326.     uNode:raise() -- make up down cost a littke more
  327.     uNode:calc()
  328.     dNode:raise() -- make up down cost a little more
  329.     dNode:calc()
  330.     rNode:calc()
  331.     bNode:calc()
  332.     lNode:calc()
  333.     parentNodeKey = string.format("%d,%d,%d", pNode.x, pNode.y, pNode.z) -- want the coordinates as the primay key
  334.     fNodeKey = string.format("%d,%d,%d", fNode.x, fNode.y, fNode.z)
  335.     uNodeKey = string.format("%d,%d,%d", uNode.x, uNode.y, uNode.z)
  336.     dNodeKey = string.format("%d,%d,%d", dNode.x, dNode.y, dNode.z)
  337.     rNodeKey = string.format("%d,%d,%d", rNode.x, rNode.y, rNode.z)
  338.     bNodeKey = string.format("%d,%d,%d", bNode.x, bNode.y, bNode.z)
  339.     lNodeKey = string.format("%d,%d,%d", lNode.x, lNode.y, lNode.z)
  340.  
  341.     if nodeList[parentNodeKey] == nil then -- if the prmary key is not in the table
  342.       nodeList[parentNodeKey] = pNode -- add key and set the node table by reference <- I suspect this is the problem
  343.       nodeList[parentNodeKey].state = "closed" -- close the node now that "state" exists
  344.     elseif nodeList[parentNodeKey].state == "open" then
  345.       nodeList[parentNodeKey].state = "closed" -- if it is already there just close it
  346.     end
  347.     if nodeList[fNodeKey] == nil or nodeList[fNodeKey].state == "open" then -- for the rest just add or update if open
  348.         nodeList[fNodeKey] = fNode
  349.     end
  350.     if nodeList[uNodeKey] == nil or nodeList[uNodeKey].state == "open" then
  351.         nodeList[uNodeKey] = uNode
  352.     end  
  353.     if nodeList[dNodeKey] == nil or nodeList[dNodeKey].state == "open" then
  354.         nodeList[dNodeKey] = dNode
  355.     end
  356.     if nodeList[rNodeKey] == nil or nodeList[rNodeKey].state == "open" then
  357.        nodeList[rNodeKey] = rNode
  358.     end
  359.     if nodeList[bNodeKey] == nil or nodeList[bNodeKey].state == "open" then
  360.        nodeList[bNodeKey] = bNode
  361.     end
  362.     if nodeList[lNodeKey] == nil or nodeList[lNodeKey].state == "open" then
  363.         nodeList[lNodeKey] = lNode
  364.     end
  365.     --[[for k, v in pairs(nodeList) do -- just cheking that table loaded as expected
  366.         log(string.format("nodeList Key:%s Value:%s", tostring(k), tostring(v)))
  367.         if type(v) then
  368.             for sk, sv in pairs(v) do
  369.                 log(string.format("nodeList Key:%s Value:%s", tostring(sk), tostring(sv)))
  370.             end
  371.         end
  372.     end --]]
  373.     return nodeList
  374.   end
  375.  --*************************************************************************************
  376.  local function getBestNode(nodeData)
  377.     for vNode, nTable in pairs(nodeData) do
  378.         if nTable.state == "open" then -- only consider open nodes
  379.             if bestMove == nil then -- grab first open node to have something to compare to
  380.                 log(string.format("Setting bestMove to:%s", tostring(vNode)))
  381.                 bestMove = vNode
  382.             end
  383.             if nTable.f < nodeList[bestMove].f then -- if this node is better than the best seen  
  384.                --nodeList[bestMove].state = "closed" -- close current best <- Might be part of the problem
  385.                bestMove = vNode -- set this node as the new best choice
  386.             elseif nTable.f == nodeList[bestMove].f then -- if nodes costs are equal
  387.                if nTable.g < nodeList[bestMove].g then -- use distance to destination as a tie breaker
  388.                 --nodeList[bestMove].state = "closed" --close the old best
  389.                 bestMove = vNode -- set the new best
  390.                end
  391.             else
  392.             end
  393.         else
  394.         end
  395.     end
  396.     if nodeList[bestMove].state == "open" then -- make sure the best node is open before finishing
  397.         local bmVector = vector.new(nodeList[bestMove].x,nodeList[bestMove].y,nodeList[bestMove].z)
  398.         return bmVector -- returning node as vector. passing as indect to nodeList did not work
  399.     else return false -- if bestMove is not open let caller know.
  400.     end
  401.   end
  402.  local function gotoNode(bnOffset, currentVector)  
  403.   facingDirection = getFacingDirection()  -- using to pick move forward or back
  404.   local moves
  405.   if facingDirection == 1 then
  406.     if bnOffset.x > 0 then
  407.       for moves = 1,math.abs(bnOffset.x) do --offsets should always be 1 but will move more if backtracking
  408.         turtle.forward()
  409.       end
  410.     else
  411.       for moves = 1,math.abs(bnOffset.x) do
  412.         turtle.back()
  413.       end
  414.     end
  415.     if bnOffset.z < 0 then
  416.       turtle.turnLeft()
  417.       for moves = 1,math.abs(bnOffset.z) do
  418.         turtle.forward()
  419.       end
  420.       turtle.turnRight()
  421.     else
  422.       turtle.turnRight()
  423.       for moves = 1,math.abs(bnOffset.z) do
  424.         turtle.forward()
  425.       end
  426.       turtle.turnLeft()
  427.     end
  428.     if bnOffset.y < 0 then
  429.       for moves = 1,math.abs(bnOffset.y) do
  430.         turtle.down()
  431.       end
  432.     elseif bnOffset.y > 0 then
  433.       for moves = 1,math.abs(bnOffset.z) do
  434.         turtle.up()
  435.       end
  436.     end
  437.   elseif facingDirection == 2 then
  438.     if bnOffset.z < 0 then
  439.       for moves = 1,math.abs(bnOffset.z) do
  440.          turtle.back()
  441.       end
  442.     elseif bnOffset.z > 0 then
  443.       for moves = 1,math.abs(bnOffset.z) do
  444.         turtle.forward()
  445.       end
  446.     end
  447.     if bnOffset.x < 0 then
  448.       turtle.turnLeft()
  449.       for moves = 1,math.abs(bnOffset.x) do
  450.         turtle.forward()
  451.       end
  452.       turtle.turnRight()
  453.     elseif bnOffset.x > 0 then
  454.       turtle.turnRight()
  455.       for moves = 1,math.abs(bnOffset.x) do
  456.         turtle.forward()
  457.       end
  458.       turtle.turnLeft()
  459.     end
  460.     if bnOffset.y < 0 then
  461.       for moves = 1,math.abs(bnOffset.y) do
  462.         turtle.down()
  463.       end
  464.     elseif bnOffset.y > 0 then
  465.       for moves = 1,math.abs(bnOffset.z) do
  466.         turtle.up()
  467.       end
  468.     end
  469.   elseif facingDirection == 3 then
  470.     if bnOffset.x > 0 then
  471.       for moves = 1,math.abs(bnOffset.x) do
  472.         turtle.forward()
  473.       end
  474.     else
  475.       for moves = 1,math.abs(bnOffset.x) do
  476.         turtle.back()
  477.       end
  478.     end
  479.     if bnOffset.z < 0 then
  480.       turtle.turnLeft()
  481.       for moves = 1,math.abs(bnOffset.z) do
  482.         turtle.forward()
  483.       end
  484.       turtle.turnRight()
  485.     else
  486.       turtle.turnRight()
  487.       for moves = 1,math.abs(bnOffset.z) do
  488.         turtle.forward()
  489.       end
  490.       turtle.turnLeft()
  491.     end
  492.     if bnOffset.y < 0 then
  493.       for moves = 1,math.abs(bnOffset.y) do
  494.         turtle.down()
  495.       end
  496.     elseif bnOffset.y > 0 then
  497.       for moves = 1,math.abs(bnOffset.z) do
  498.         turtle.up()
  499.       end
  500.     end
  501.   elseif facingDirection == 4 then
  502.   if bnOffset.z < 0 then
  503.       for moves = 1,math.abs(bnOffset.z) do
  504.         turtle.forward()
  505.       end
  506.     elseif bnOffset.z > 0 then
  507.       for moves = 1,math.abs(bnOffset.z) do
  508.         turtle.back()
  509.       end
  510.     end
  511.     if bnOffset.x < 0 then
  512.       turtle.turnLeft()
  513.       for moves = 1,math.abs(bnOffset.x) do
  514.         turtle.forward()
  515.       end
  516.       turtle.turnRight()
  517.     elseif bnOffset.x > 0 then
  518.       turtle.turnRight()
  519.       for moves = 1,math.abs(bnOffset.x) do
  520.         turtle.forward()
  521.       end
  522.       turtle.turnLeft()
  523.     end
  524.     if bnOffset.y < 0 then
  525.       for moves = 1,math.abs(bnOffset.y) do
  526.         turtle.down()
  527.       end
  528.     elseif bnOffset.y > 0 then
  529.       for moves = 1,math.abs(bnOffset.z) do
  530.         turtle.up()
  531.       end
  532.     end
  533.   end
  534.  end
  535.  
  536.  local moveCount = 0
  537.  while currentVector ~= targetVector and moveCount < 20 do -- limit number of moves so we don't have to chase turtle
  538.     moveCount = moveCount + 1
  539.     log(string.format("Starting move #%d", moveCount))
  540.     log(string.format("Current Location is :%s", tostring(currentVector)))
  541.     local iNodes = getNodeData() -- this seems to work. Tested all directions with and without blocks on 3 sides
  542.     log(string.format("getNodeData returned %s", tostring(iNodes)))
  543.     local oVect = getBestNode(iNodes) -- I think the problem is in here. After a couple move returns false.
  544.     log(string.format("getBestNode returned %s", tostring(oVect)))
  545.     if oVect then
  546.         log("Calculating MestMove Offset")
  547.         local bestOffset = oVect - currentVector
  548.         log(string.format("Offset of %s beween bestmove:%s and current%s", tostring(bestOffset),tostring(oVect), tostring(currentVector)))
  549.         local bmResult = gotoNode(bestOffset, currentVector)
  550.         log(string.format("gotoNode returned %s", tostring(bmResult)))
  551.     else
  552.         log("No Best Node Returned")
  553.     end
  554.     currentVector = vector.new(gps.locate(5))
  555.     log(string.format("new current Vector set to %s", tostring(currentVector)))
  556.  
  557.  end
  558. end
  559. --************Starting Seek*************
  560. cVector = vector.new(gps.locate(5))
  561. sVector = cVector
  562. targetVector = vector.new(-12, 76,-562)
  563. m = seekDestination(cVector,targetVector, 86, sVector )
RAW Paste Data