joeld98

A* in computercraft

Aug 10th, 2016
100
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

Adblocker detected! Please consider disabling it...

We've detected AdBlock Plus or some other adblocking software preventing Pastebin.com from fully loading.

We don't have any obnoxious sound, or popup ads, we actively block these annoying types of ads!

Please add Pastebin.com to your ad blocker whitelist or disable your adblocking software.

×