joeld98

A* in computercraft - Rev 4

Aug 21st, 2016
77
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1.  
  2. local function getFacingDirection()
  3.     local function findClearBlock()
  4.     local turnsToClear = 0
  5.     while turtle.inspect() and turnsToClear <=4 do
  6.       turtle.turnRight()
  7.       turnsToClear = turnsToClear + 1
  8.     end
  9.     if turnsToClear >= 4 then
  10.       return nil
  11.     else
  12.     end
  13.     if turnsToClear ~=4 and turnsToClear ~=0 then
  14.       for faceHome = 1, turnsToClear do
  15.         turtle.turnLeft()
  16.       end
  17.     end
  18.     return turnsToClear
  19.     end
  20.     local startLoc = vector.new(gps.locate())
  21.     local dirLoc
  22.     local facingDir = 0
  23.     local facingVector
  24.     if turtle.forward() then
  25.     dirLoc = vector.new(gps.locate())
  26.     turtle.back()
  27.     facingVector = dirLoc - startLoc
  28.     facingDir = ((facingVector.x + math.abs(facingVector.x) * 2) + (facingVector.z + math.abs(facingVector.z) * 3))
  29.     else
  30.     local tCount = findClearBlock()
  31.     if tCount == nil then
  32.       return nil
  33.     elseif tCount == 2 then
  34.       turtle.back()
  35.       dirLoc = vector.new(gps.locate())
  36.       turtle.forward()
  37.       facingVector = startLoc - dirLoc
  38.       facingDir = (((facingVector.x + math.abs(facingVector.x) * 2) + (facingVector.z + math.abs(facingVector.z) * 3)))
  39.     elseif tCount == 1 then
  40.       turtle.turnRight()
  41.       turtle.forward()
  42.       turtle.turnLeft()
  43.       dirLoc = vector.new(gps.locate())
  44.       turtle.turnLeft()
  45.       turtle.forward()
  46.       turtle.turnRight()
  47.       facingVector = dirLoc - startLoc
  48.       facingDir = (((facingVector.x + math.abs(facingVector.x) * 2) + (facingVector.z + math.abs(facingVector.z) * 3))) - 1
  49.     elseif
  50.       tCount == 3 then
  51.       turtle.turnLeft()
  52.       turtle.forward()
  53.       turtle.turnRight()
  54.       dirLoc = vector.new(gps.locate())
  55.       turtle.turnRight()
  56.       turtle.forward()
  57.       turtle.turnLeft()
  58.       facingVector = dirLoc - startLoc
  59.       facingDir = ((facingVector.x + math.abs(facingVector.x) * 2) + (facingVector.z + math.abs(facingVector.z) * 3)) + 1
  60.       if facingDir > 4 then
  61.       facingDir = 1
  62.       end
  63.     end
  64.     end
  65.     return facingDir
  66. end
  67.  
  68. local function rightTurn()
  69.     if not facingDirection then
  70.         facingDirection = getFacingDirection()
  71.     end
  72.     turtle.turnRight()
  73.     facingDirection = facingDirection + 1
  74.     if facingDirection > 4 then
  75.         facingDirection = 1
  76.     end
  77. end
  78.  
  79. local function leftTurn()
  80.     if not facingDirection then
  81.         facingDirection = getFacingDirection()
  82.     end
  83.     turtle.turnLeft()
  84.     facingDirection = facingDirection - 1
  85.     if facingDirection < 1 then
  86.         facingDirection = 4
  87.     end
  88. end
  89.  
  90.  
  91. local function getNodeData(currentVector, targetVector , startVector, parentVector)
  92.   local cNode, cNodeKey = nil
  93.   local fNode, fNodeKey, fInspect, fResult = nil
  94.   local uNode, uNodeKey, uInspect,uResult = nil
  95.   local dNode, dNodeKey, dInspect, dResult = nil
  96.   local rNode, rNodeKey, rInspect, rResult = nil
  97.   local bNode, bNodeKey, bInspect, bResult = nil
  98.   local lNode, lNodeKey, lInspect, lResult = nil  
  99.   local fuNode, fuNodeKey = nil
  100.   local ruNode, ruNodeKey = nil
  101.   local luNode, luNodeKey = nil
  102.   local buNode, buNodeKey = nil
  103.   local fdNode, fdNodeKey = nil
  104.   local rdNode, rdNodeKey = nil
  105.   local bdode, bdNodeKey = nil
  106.   local bdrNode, bdNodeKey = nil
  107.   local frNode, frNodeKey = nil
  108.   local rrNode, rrNodeKey = nil
  109.   local flNode, flNodeKey = nil
  110.   local lrNode, lrNodeKey = nil
  111.   local fruNode, fruNodeKey = nil
  112.   local rruNode, rruNodeKey = nil
  113.   local fluNode, fluNodeKey = nil
  114.   local lruNode, lruNodeKey = nil
  115.   local frdNode, frdNodeKey = nil
  116.   local rrdNode, rrdNodeKey = nil
  117.   local fldNode, fldNodeKey = nil
  118.   local lrdNode, lrdNodeKey = nil
  119.   cNode = {x = 0, y = 0, z = 0}
  120.  
  121.   fNode = {x = 0, y = 0, z = 0}
  122.   uNode = {x = 0, y = 0, z = 0}
  123.   dNode = {x = 0, y = 0, z = 0}  
  124.   rNode = {x = 0, y = 0, z = 0}
  125.   bNode = {x = 0, y = 0, z = 0}
  126.   lNode = {x = 0, y = 0, z = 0}
  127.   frNode = {x = 0, y = 0, z = 0}
  128.   fruNode = {x = 0, y = 0, z = 0}
  129.   frdNode = {x = 0, y = 0, z = 0}
  130.   fuNode = {x = 0, y = 0, z = 0}
  131.   fdNode = {x = 0, y = 0, z = 0}  
  132.   flNode = {x = 0, y = 0, z = 0}
  133.   fluNode = {x = 0, y = 0, z = 0}
  134.   fldNode = {x = 0, y = 0, z = 0}
  135.   ruNode = {x = 0, y = 0, z = 0}
  136.   rdNode = {x = 0, y = 0, z = 0}
  137.   rrNode = {x = 0, y = 0, z = 0}
  138.   rruNode = {x = 0, y = 0, z = 0}  
  139.   rrdNode = {x = 0, y = 0, z = 0}  
  140.   buNode = {x = 0, y = 0, z = 0}
  141.   bdNode = {x = 0, y = 0, z = 0}       
  142.   lrNode = {x = 0, y = 0, z = 0}
  143.   lruNode = {x = 0, y = 0, z = 0}
  144.   lrdNode = {x = 0, y = 0, z = 0}
  145.   luNode = {x = 0, y = 0, z = 0}
  146.   ldNode = {x = 0, y = 0, z = 0}
  147.  
  148.   if not facingDirection then
  149.     facingDirection = getFacingDirection()  
  150.   end
  151.   uNode.x = currentVector.x
  152.   uNode.z = currentVector.z
  153.   uNode.y = currentVector.y + 1  
  154.   dNode.x = currentVector.x
  155.   dNode.z = currentVector.z
  156.   dNode.y = currentVector.y - 1
  157.   cNodeKey = string.format("%d,%d,%d", currentVector.x, currentVector.y, currentVector.z)
  158.   if facingDirection == 1 then  
  159.     if currentVector.x <= 0 then  
  160.       fNode.x = currentVector.x - 1
  161.       bNode.x = currentVector.x + 1
  162.       lNode.x = currentVector.x
  163.       rNode.x = currentVector.x
  164.     else
  165.       fNode.x = currentVector.x + 1
  166.       bNode.x = currentVector.x - 1
  167.       lNode.x = currentVector.x
  168.       rNode.x = currentVector.x
  169.     end
  170.     if currentVector.z <= 0 then  
  171.       lNode.z = currentVector.z + 1
  172.       rNode.z = currentVector.z - 1  
  173.       fNode.z = currentVector.z
  174.       bNode.z = currentVector.z
  175.     else
  176.       lNode.z = currentVector.z - 1  
  177.       rNode.z = currentVector.z + 1
  178.       fNode.z = currentVector.z
  179.       bNode.z = currentVector.z
  180.     end
  181.     fNode.y = currentVector.y
  182.     bNode.y = currentVector.y
  183.     lNode.y = currentVector.y
  184.     rNode.y = currentVector.y
  185.     fNodeKey = string.format("%d,%d,%d", fNode.x, fNode.y, fNode.z)
  186.     frNodeKey = string.format("%d,%d,%d", fNode.x, fNode.y, rNode.z)
  187.     rNodeKey = string.format("%d,%d,%d", rNode.x, rNode.y, rNode.z)
  188.     rrNodeKey = string.format("%d,%d,%d", bNode.x, bNode.y, rNode.z)
  189.     bNodeKey = string.format("%d,%d,%d", bNode.x, bNode.y, bNode.z)
  190.     lrNodeKey = string.format("%d,%d,%d", bNode.x, lNode.y, lNode.z)
  191.     lNodeKey = string.format("%d,%d,%d", lNode.x, lNode.y, lNode.z)
  192.     flNodeKey = string.format("%d,%d,%d", fNode.x, lNode.y, lNode.z)
  193.     uNodeKey = string.format("%d,%d,%d", uNode.x, uNode.y, uNode.z)
  194.     dNodeKey = string.format("%d,%d,%d", dNode.x, dNode.y, dNode.z)
  195.     fuNodeKey = string.format("%d,%d,%d", fNode.x, uNode.y, fNode.z)
  196.     fruNodeKey = string.format("%d,%d,%d", fNode.x, uNode.y, rNode.z)
  197.     ruNodeKey = string.format("%d,%d,%d", rNode.x, uNode.y, rNode.z)
  198.     rruNodeKey = string.format("%d,%d,%d", bNode.x, uNode.y, rNode.z)
  199.     buNodeKey = string.format("%d,%d,%d", bNode.x, uNode.y, bNode.z)
  200.     lruNodeKey = string.format("%d,%d,%d", bNode.x, uNode.y, lNode.z)
  201.     luNodeKey = string.format("%d,%d,%d", lNode.x, uNode.y, lNode.z)
  202.     fluNodeKey = string.format("%d,%d,%d", fNode.x, uNode.y, fNode.z)
  203.     fdNodeKey = string.format("%d,%d,%d", fNode.x, dNode.y, fNode.z)
  204.     frdNodeKey = string.format("%d,%d,%d", fNode.x, dNode.y, rNode.z)
  205.     rdNodeKey = string.format("%d,%d,%d", rNode.x, dNode.y, rNode.z)
  206.     rrdNodeKey = string.format("%d,%d,%d", bNode.x, dNode.y, rNode.z)
  207.     bdNodeKey = string.format("%d,%d,%d", bNode.x, dNode.y, bNode.z)
  208.     lrdNodeKey = string.format("%d,%d,%d", bNode.x, dNode.y, lNode.z)
  209.     ldNodeKey = string.format("%d,%d,%d", lNode.x, dNode.y, lNode.z)
  210.     fldNodeKey = string.format("%d,%d,%d", fNode.x, dNode.y, lNode.z)
  211.   elseif facingDirection == 2 then  
  212.     if currentVector.x <= 0 then
  213.       rNode.x = currentVector.x + 1
  214.       lNode.x = currentVector.x - 1
  215.       fNode.x = currentVector.x
  216.       bNode.x = currentVector.x
  217.     else
  218.       rNode.x = currentVector.x - 1
  219.       lNode.x = currentVector.x + 1
  220.       fNode.x = currentVector.x
  221.       bNode.x = currentVector.x
  222.     end
  223.     if currentVector.z <=0 then
  224.       fNode.z = currentVector.z - 1
  225.       bNode.z = currentVector.z + 1
  226.       lNode.z = currentVector.z
  227.       rNode.z = currentVector.z
  228.     else
  229.       fNode.z = currentVector.z + 1
  230.       bNode.z = currentVector.z - 1
  231.       lNode.z = currentVector.z
  232.       rNode.z = currentVector.z
  233.     end
  234.     fNode.y = currentVector.y
  235.     bNode.y = currentVector.y
  236.     lNode.y = currentVector.y
  237.     rNode.y = currentVector.y
  238.     fNodeKey = string.format("%d,%d,%d", fNode.x, fNode.y, fNode.z)
  239.     frNodeKey = string.format("%d,%d,%d", rNode.x, fNode.y, fNode.z)
  240.     rNodeKey = string.format("%d,%d,%d", rNode.x, rNode.y, rNode.z)
  241.     rrNodeKey = string.format("%d,%d,%d", rNode.x, bNode.y, bNode.z)
  242.     bNodeKey = string.format("%d,%d,%d", bNode.x, bNode.y, bNode.z)
  243.     lrNodeKey = string.format("%d,%d,%d", lNode.x, lNode.y, bNode.z)
  244.     lNodeKey = string.format("%d,%d,%d", lNode.x, lNode.y, lNode.z)
  245.     flNodeKey = string.format("%d,%d,%d", lNode.x, lNode.y, fNode.z)
  246.     uNodeKey = string.format("%d,%d,%d", uNode.x, uNode.y, uNode.z)
  247.     dNodeKey = string.format("%d,%d,%d", dNode.x, dNode.y, dNode.z)
  248.     fuNodeKey = string.format("%d,%d,%d", fNode.x, uNode.y, fNode.z)
  249.     fruNodeKey = string.format("%d,%d,%d", rNode.x, uNode.y, fNode.z)
  250.     ruNodeKey = string.format("%d,%d,%d", rNode.x, uNode.y, rNode.z)
  251.     rruNodeKey = string.format("%d,%d,%d", rNode.x, uNode.y, bNode.z)
  252.     buNodeKey = string.format("%d,%d,%d", bNode.x, uNode.y, bNode.z)
  253.     lruNodeKey = string.format("%d,%d,%d", lNode.x, uNode.y, bNode.z)
  254.     luNodeKey = string.format("%d,%d,%d", lNode.x, uNode.y, lNode.z)
  255.     fluNodeKey = string.format("%d,%d,%d", lNode.x, uNode.y, fNode.z)
  256.     fdNodeKey = string.format("%d,%d,%d", fNode.x, dNode.y, fNode.z)
  257.     frdNodeKey = string.format("%d,%d,%d", rNode.x, dNode.y, fNode.z)
  258.     rdNodeKey = string.format("%d,%d,%d", rNode.x, dNode.y, rNode.z)
  259.     rrdNodeKey = string.format("%d,%d,%d", rNode.x, dNode.y, bNode.z)
  260.     bdNodeKey = string.format("%d,%d,%d", bNode.x, dNode.y, bNode.z)
  261.     lrdNodeKey = string.format("%d,%d,%d", lNode.x, dNode.y, bNode.z)
  262.     ldNodeKey = string.format("%d,%d,%d", lNode.x, dNode.y, lNode.z)
  263.     fldNodeKey = string.format("%d,%d,%d", lNode.x, dNode.y, fNode.z)
  264.   elseif facingDirection == 3 then
  265.     if currentVector.x <= 0 then
  266.       fNode.x = currentVector.x + 1
  267.       bNode.x = currentVector.x - 1
  268.       lNode.x = currentVector.x
  269.       rNode.x = currentVector.x
  270.     else
  271.       fNode.x = currentVector.x - 1
  272.       bNode.x = currentVector.x + 1
  273.       lNode.x = currentVector.x
  274.       rNode.x = currentVector.x
  275.     end
  276.     if currentVector.z <=0 then
  277.       lNode.z = currentVector.z - 1
  278.       rNode.z = currentVector.z + 1
  279.       fNode.z = currentVector.z
  280.       bNode.z = currentVector.z
  281.     else
  282.       lNode.z = currentVector.z + 1
  283.       rNode.z = currentVector.z - 1
  284.       fNode.z = currentVector.z
  285.       bNode.z = currentVector.z
  286.     end
  287.     fNode.y = currentVector.y
  288.     bNode.y = currentVector.y
  289.     lNode.y = currentVector.y
  290.     rNode.y = currentVector.y
  291.     fNodeKey = string.format("%d,%d,%d", fNode.x, fNode.y, fNode.z)
  292.     frNodeKey = string.format("%d,%d,%d", fNode.x, fNode.y, rNode.z)
  293.     rNodeKey = string.format("%d,%d,%d", rNode.x, rNode.y, rNode.z)
  294.     rrNodeKey = string.format("%d,%d,%d", bNode.x, bNode.y, rNode.z)
  295.     bNodeKey = string.format("%d,%d,%d", bNode.x, bNode.y, bNode.z)
  296.     lrNodeKey = string.format("%d,%d,%d", bNode.x, lNode.y, lNode.z)
  297.     lNodeKey = string.format("%d,%d,%d", lNode.x, lNode.y, lNode.z)
  298.     flNodeKey = string.format("%d,%d,%d", fNode.x, lNode.y, lNode.z)
  299.     uNodeKey = string.format("%d,%d,%d", uNode.x, uNode.y, uNode.z)
  300.     dNodeKey = string.format("%d,%d,%d", dNode.x, dNode.y, dNode.z)
  301.     fuNodeKey = string.format("%d,%d,%d", fNode.x, uNode.y, fNode.z)
  302.     fruNodeKey = string.format("%d,%d,%d", fNode.x, uNode.y, rNode.z)
  303.     ruNodeKey = string.format("%d,%d,%d", rNode.x, uNode.y, rNode.z)
  304.     rruNodeKey = string.format("%d,%d,%d", bNode.x, uNode.y, rNode.z)
  305.     buNodeKey = string.format("%d,%d,%d", bNode.x, uNode.y, bNode.z)
  306.     lruNodeKey = string.format("%d,%d,%d", bNode.x, uNode.y, lNode.z)
  307.     luNodeKey = string.format("%d,%d,%d", lNode.x, uNode.y, lNode.z)
  308.     fluNodeKey = string.format("%d,%d,%d", fNode.x, uNode.y, fNode.z)
  309.     fdNodeKey = string.format("%d,%d,%d", fNode.x, dNode.y, fNode.z)
  310.     frdNodeKey = string.format("%d,%d,%d", fNode.x, dNode.y, rNode.z)
  311.     rdNodeKey = string.format("%d,%d,%d", rNode.x, dNode.y, rNode.z)
  312.     rrdNodeKey = string.format("%d,%d,%d", bNode.x, dNode.y, rNode.z)
  313.     bdNodeKey = string.format("%d,%d,%d", bNode.x, dNode.y, bNode.z)
  314.     lrdNodeKey = string.format("%d,%d,%d", bNode.x, dNode.y, lNode.z)
  315.     ldNodeKey = string.format("%d,%d,%d", lNode.x, dNode.y, lNode.z)
  316.     fldNodeKey = string.format("%d,%d,%d", fNode.x, dNode.y, lNode.z)
  317.  
  318.   elseif facingDirection == 4 then
  319.     if currentVector.x <= 0 then
  320.       lNode.x = currentVector.x + 1
  321.       rNode.x = currentVector.x - 1
  322.       fNode.x = currentVector.x
  323.       bNode.x = currentVector.x
  324.       else
  325.       rNode.x = currentVector.x + 1
  326.       lNode.x = currentVector.x - 1
  327.       fNode.x = currentVector.x
  328.       bNode.x = currentVector.x
  329.     end
  330.     if currentVector.z >=0 then
  331.       fNode.z = currentVector.z - 1
  332.       bNode.z = currentVector.z + 1
  333.       lNode.z = currentVector.z
  334.       rNode.z = currentVector.z
  335.       else
  336.       fNode.z = currentVector.z + 1
  337.       bNode.z = currentVector.z - 1
  338.       lNode.z = currentVector.z
  339.       rNode.z = currentVector.z
  340.     end
  341.     fNode.y = currentVector.y
  342.     bNode.y = currentVector.y
  343.     lNode.y = currentVector.y
  344.     rNode.y = currentVector.y
  345.     fNodeKey = string.format("%d,%d,%d", fNode.x, fNode.y, fNode.z)
  346.     frNodeKey = string.format("%d,%d,%d", rNode.x, fNode.y, fNode.z)
  347.     rNodeKey = string.format("%d,%d,%d", rNode.x, rNode.y, rNode.z)
  348.     rrNodeKey = string.format("%d,%d,%d", rNode.x, bNode.y, bNode.z)
  349.     bNodeKey = string.format("%d,%d,%d", bNode.x, bNode.y, bNode.z)
  350.     lrNodeKey = string.format("%d,%d,%d", lNode.x, lNode.y, bNode.z)
  351.     lNodeKey = string.format("%d,%d,%d", lNode.x, lNode.y, lNode.z)
  352.     flNodeKey = string.format("%d,%d,%d", lNode.x, lNode.y, fNode.z)
  353.     uNodeKey = string.format("%d,%d,%d", uNode.x, uNode.y, uNode.z)
  354.     dNodeKey = string.format("%d,%d,%d", dNode.x, dNode.y, dNode.z)
  355.     fuNodeKey = string.format("%d,%d,%d", fNode.x, uNode.y, fNode.z)
  356.     fruNodeKey = string.format("%d,%d,%d", rNode.x, uNode.y, fNode.z)
  357.     ruNodeKey = string.format("%d,%d,%d", rNode.x, uNode.y, rNode.z)
  358.     rruNodeKey = string.format("%d,%d,%d", rNode.x, uNode.y, bNode.z)
  359.     buNodeKey = string.format("%d,%d,%d", bNode.x, uNode.y, bNode.z)
  360.     lruNodeKey = string.format("%d,%d,%d", lNode.x, uNode.y, bNode.z)
  361.     luNodeKey = string.format("%d,%d,%d", lNode.x, uNode.y, lNode.z)
  362.     fluNodeKey = string.format("%d,%d,%d", lNode.x, uNode.y, fNode.z)
  363.     fdNodeKey = string.format("%d,%d,%d", fNode.x, dNode.y, fNode.z)
  364.     frdNodeKey = string.format("%d,%d,%d", rNode.x, dNode.y, fNode.z)
  365.     rdNodeKey = string.format("%d,%d,%d", rNode.x, dNode.y, rNode.z)
  366.     rrdNodeKey = string.format("%d,%d,%d", rNode.x, dNode.y, bNode.z)
  367.     bdNodeKey = string.format("%d,%d,%d", bNode.x, dNode.y, bNode.z)
  368.     lrdNodeKey = string.format("%d,%d,%d", lNode.x, dNode.y, bNode.z)
  369.     ldNodeKey = string.format("%d,%d,%d", lNode.x, dNode.y, lNode.z)
  370.     fldNodeKey = string.format("%d,%d,%d", lNode.x, dNode.y, fNode.z)
  371.   end
  372.   if not parentVector then
  373.     pNodeKey = string.format("%d,%d,%d", currentVector.x, currentVector.y, currentVector.z)
  374.   else
  375.     pNodeKey = string.format("%d,%d,%d", parentVector.x, parentVector.y, parentVector.z)
  376.   end
  377.   if not nodeList[pNodeKey] then -- this should only happen on the first pass when seekDestination is called. Our last pass currentVector is passed as our parent thereafter.
  378.     nodeList[pNodeKey] = {}
  379.     nodeList[pNodeKey].x = currentVector.x
  380.     nodeList[pNodeKey].y = currentVector.y
  381.     nodeList[pNodeKey].z = currentVector.z
  382.     nodeList[pNodeKey].g = 10
  383.     nodeList[pNodeKey].startx = startVector.x
  384.     nodeList[pNodeKey].starty = startVector.y
  385.     nodeList[pNodeKey].startz = startVector.z
  386.     nodeList[pNodeKey].endx = targetVector.x
  387.     nodeList[pNodeKey].endy = targetVector.y
  388.     nodeList[pNodeKey].endz = targetVector.z
  389.     nodeList[pNodeKey].h = math.abs(nodeList[pNodeKey].x - nodeList[pNodeKey].endx) +  math.abs(nodeList[pNodeKey].y-nodeList[pNodeKey].endy) + math.abs(nodeList[pNodeKey].z-nodeList[pNodeKey].endz)
  390.     nodeList[pNodeKey].f = nodeList[pNodeKey].g + nodeList[pNodeKey].h
  391.     nodeList[pNodeKey].state = "closed"
  392.   elseif nodeList[pNodeKey].state == "open" then
  393.     nodeList[pNodeKey].state = "closed"
  394.   end
  395.   if not nodeList[cNodeKey] then -- this should only happen on the first pass when seekDestination is called. Parent and Current are equal on this pass. One of our last pass nodes should become our current node thereafter.
  396.     nodeList[cNodeKey] = {}
  397.     nodeList[cNodeKey].x = currentVector.x
  398.     nodeList[cNodeKey].y = currentVector.y
  399.     nodeList[cNodeKey].z = currentVector.z
  400.     nodeList[cNodeKey].g = 10
  401.     nodeList[cNodeKey].startx = startVector.x
  402.     nodeList[cNodeKey].starty = startVector.y
  403.     nodeList[cNodeKey].startz = startVector.z
  404.     nodeList[cNodeKey].endx = targetVector.x
  405.     nodeList[cNodeKey].endy = targetVector.y
  406.     nodeList[cNodeKey].endz = targetVector.z
  407.     nodeList[cNodeKey].h = math.abs(nodeList[pNodeKey].x - nodeList[cNodeKey].endx) +  math.abs(nodeList[pNodeKey].y-nodeList[cNodeKey].endy) + math.abs(nodeList[pNodeKey].z-nodeList[cNodeKey].endz)
  408.     nodeList[cNodeKey].f = nodeList[cNodeKey].g + nodeList[cNodeKey].h
  409.     nodeList[cNodeKey].state = "closed"
  410.   elseif nodeList[cNodeKey].state == "open" then
  411.     nodeList[cNodeKey].state = "closed"
  412.   end
  413.   if not nodeList[fNodeKey] then
  414.     nodeList[fNodeKey] = {}
  415.     nodeList[fNodeKey].x = fNode.x
  416.     nodeList[fNodeKey].y = fNode.y
  417.     nodeList[fNodeKey].z = fNode.z
  418.     nodeList[fNodeKey].startx = startVector.x
  419.     nodeList[fNodeKey].starty = startVector.y
  420.     nodeList[fNodeKey].startz = startVector.z
  421.     nodeList[fNodeKey].endx = targetVector.x
  422.     nodeList[fNodeKey].endy = targetVector.y
  423.     nodeList[fNodeKey].endz = targetVector.z
  424.     nodeList[fNodeKey].g = math.abs((nodeList[cNodeKey].x-nodeList[fNodeKey].x) + (nodeList[cNodeKey].y-nodeList[fNodeKey].y) + (nodeList[cNodeKey].z-nodeList[fNodeKey].z)) + nodeList[pNodeKey].g
  425.     nodeList[fNodeKey].h = math.abs(nodeList[fNodeKey].x - nodeList[fNodeKey].endx) +  math.abs(nodeList[fNodeKey].y-nodeList[fNodeKey].endy) + math.abs(nodeList[fNodeKey].z-nodeList[fNodeKey].endz)
  426.     nodeList[fNodeKey].f = nodeList[fNodeKey].g + nodeList[fNodeKey].h
  427.     nodeList[fNodeKey].state = "open"
  428.   elseif nodeList[fNodeKey].state == "open" then
  429.     --put logic here to recalc g if needed.
  430.   end
  431.   if not nodeList[uNodeKey] then
  432.     nodeList[uNodeKey] = {}
  433.     nodeList[uNodeKey].x = uNode.x
  434.     nodeList[uNodeKey].y = uNode.y
  435.     nodeList[uNodeKey].z = uNode.z
  436.     nodeList[uNodeKey].startx = startVector.x
  437.     nodeList[uNodeKey].starty = startVector.y
  438.     nodeList[uNodeKey].startz = startVector.z
  439.     nodeList[uNodeKey].endx = targetVector.x
  440.     nodeList[uNodeKey].endy = targetVector.y
  441.     nodeList[uNodeKey].endz = targetVector.z
  442.     nodeList[uNodeKey].g = math.abs((nodeList[pNodeKey].x-nodeList[uNodeKey].x) + (nodeList[pNodeKey].y-nodeList[uNodeKey].y) + (nodeList[pNodeKey].z-nodeList[uNodeKey].z)) + nodeList[pNodeKey].g
  443.     nodeList[uNodeKey].h = math.abs(nodeList[uNodeKey].x - nodeList[uNodeKey].endx) +  math.abs(nodeList[uNodeKey].y-nodeList[uNodeKey].endy) + math.abs(nodeList[uNodeKey].z-nodeList[uNodeKey].endz)
  444.     nodeList[uNodeKey].f = nodeList[uNodeKey].g + nodeList[uNodeKey].h
  445.     nodeList[uNodeKey].state = "open"
  446.   elseif nodeList[uNodeKey].state == "open" then
  447.     --put logic here to recalc g if needed.
  448.   end
  449.   if not nodeList[dNodeKey] then
  450.     nodeList[dNodeKey] = {}
  451.     nodeList[dNodeKey].x = dNode.x
  452.     nodeList[dNodeKey].y = dNode.y
  453.     nodeList[dNodeKey].z = dNode.z
  454.     nodeList[dNodeKey].startx = startVector.x
  455.     nodeList[dNodeKey].starty = startVector.y
  456.     nodeList[dNodeKey].startz = startVector.z
  457.     nodeList[dNodeKey].endx = targetVector.x
  458.     nodeList[dNodeKey].endy = targetVector.y
  459.     nodeList[dNodeKey].endz = targetVector.z
  460.     nodeList[dNodeKey].g = math.abs((nodeList[pNodeKey].x-nodeList[dNodeKey].x) + (nodeList[pNodeKey].y-nodeList[dNodeKey].y) + (nodeList[pNodeKey].z-nodeList[dNodeKey].z)) + nodeList[pNodeKey].g
  461.     nodeList[dNodeKey].h = math.abs(nodeList[dNodeKey].x - nodeList[dNodeKey].endx) +  math.abs(nodeList[dNodeKey].y-nodeList[dNodeKey].endy) + math.abs(nodeList[dNodeKey].z-nodeList[dNodeKey].endz)
  462.     nodeList[dNodeKey].f = nodeList[dNodeKey].g + nodeList[dNodeKey].h
  463.     nodeList[dNodeKey].state = "open"
  464.   elseif nodeList[dNodeKey].state == "open" then
  465.     --put logic here to recalc g if needed.
  466.   end
  467.   if not nodeList[rNodeKey] then
  468.     nodeList[rNodeKey] = {}
  469.     nodeList[rNodeKey].x = rNode.x
  470.     nodeList[rNodeKey].y = rNode.y
  471.     nodeList[rNodeKey].z = rNode.z
  472.     nodeList[rNodeKey].startx = startVector.x
  473.     nodeList[rNodeKey].starty = startVector.y
  474.     nodeList[rNodeKey].startz = startVector.z
  475.     nodeList[rNodeKey].endx = targetVector.x
  476.     nodeList[rNodeKey].endy = targetVector.y
  477.     nodeList[rNodeKey].endz = targetVector.z
  478.     nodeList[rNodeKey].g = math.abs((nodeList[pNodeKey].x-nodeList[rNodeKey].x) + (nodeList[pNodeKey].y-nodeList[rNodeKey].y) + (nodeList[pNodeKey].z-nodeList[rNodeKey].z)) + nodeList[pNodeKey].g
  479.     nodeList[rNodeKey].h = math.abs(nodeList[rNodeKey].x - nodeList[rNodeKey].endx) +  math.abs(nodeList[rNodeKey].y-nodeList[rNodeKey].endy) + math.abs(nodeList[rNodeKey].z-nodeList[rNodeKey].endz)
  480.     nodeList[rNodeKey].f = nodeList[rNodeKey].g + nodeList[rNodeKey].h
  481.     nodeList[rNodeKey].state = "open"
  482.   elseif nodeList[rNodeKey].state == "open" then
  483.     --put logic here to recalc g if needed.
  484.   end
  485.   if not nodeList[bNodeKey] then
  486.     nodeList[bNodeKey] = {}
  487.     nodeList[bNodeKey].x = bNode.x
  488.     nodeList[bNodeKey].y = bNode.y
  489.     nodeList[bNodeKey].z = bNode.z
  490.     nodeList[bNodeKey].startx = startVector.x
  491.     nodeList[bNodeKey].starty = startVector.y
  492.     nodeList[bNodeKey].startz = startVector.z
  493.     nodeList[bNodeKey].endx = targetVector.x
  494.     nodeList[bNodeKey].endy = targetVector.y
  495.     nodeList[bNodeKey].endz = targetVector.z
  496.     nodeList[bNodeKey].g = math.abs((nodeList[pNodeKey].x-nodeList[bNodeKey].x) + (nodeList[pNodeKey].y-nodeList[bNodeKey].y) + (nodeList[pNodeKey].z-nodeList[bNodeKey].z)) + nodeList[pNodeKey].g
  497.     nodeList[bNodeKey].h = math.abs(nodeList[bNodeKey].x - nodeList[bNodeKey].endx) +  math.abs(nodeList[bNodeKey].y-nodeList[bNodeKey].endy) + math.abs(nodeList[bNodeKey].z-nodeList[bNodeKey].endz)
  498.     nodeList[bNodeKey].f = nodeList[bNodeKey].g + nodeList[bNodeKey].h
  499.     nodeList[bNodeKey].state = "open"
  500.   elseif nodeList[bNodeKey].state == "open" then
  501.     --put logic here to recalc g if needed.
  502.   end
  503.   if not nodeList[lNodeKey] then
  504.     nodeList[lNodeKey] = {}
  505.     nodeList[lNodeKey].x = lNode.x
  506.     nodeList[lNodeKey].y = lNode.y
  507.     nodeList[lNodeKey].z = lNode.z
  508.     nodeList[lNodeKey].startx = startVector.x
  509.     nodeList[lNodeKey].starty = startVector.y
  510.     nodeList[lNodeKey].startz = startVector.z
  511.     nodeList[lNodeKey].endx = targetVector.x
  512.     nodeList[lNodeKey].endy = targetVector.y
  513.     nodeList[lNodeKey].endz = targetVector.z
  514.     nodeList[lNodeKey].g = math.abs((nodeList[pNodeKey].x-nodeList[lNodeKey].x) + (nodeList[pNodeKey].y-nodeList[lNodeKey].y) + (nodeList[pNodeKey].z-nodeList[lNodeKey].z)) + nodeList[pNodeKey].g
  515.     nodeList[lNodeKey].h = math.abs(nodeList[lNodeKey].x - nodeList[lNodeKey].endx) +  math.abs(nodeList[lNodeKey].y-nodeList[lNodeKey].endy) + math.abs(nodeList[lNodeKey].z-nodeList[lNodeKey].endz)
  516.     nodeList[lNodeKey].f = nodeList[lNodeKey].g + nodeList[lNodeKey].h
  517.     nodeList[lNodeKey].state = "open"
  518.   elseif nodeList[lNodeKey].state == "open" then
  519.     --put logic here to recalc g if needed.
  520.   end
  521.   if nodeList[fuNodeKey] then
  522.     nodeList[fuNodeKey].g = nodeList[fuNodeKey].g + nodeList[cNodeKey].g
  523.     nodeList[fuNodeKey].f = nodeList[fuNodeKey].g + nodeList[fuNodeKey].h
  524.   end
  525.   if nodeList[fdNodeKey] then
  526.     nodeList[fdNodeKey].g = nodeList[fdNodeKey].g + nodeList[cNodeKey].g
  527.     nodeList[fdNodeKey].f = nodeList[fdNodeKey].g + nodeList[fdNodeKey].h
  528.   end
  529.   if nodeList[frNodeKey] then
  530.     nodeList[frNodeKey].g = nodeList[frNodeKey].g + nodeList[cNodeKey].g
  531.     nodeList[frNodeKey].f = nodeList[frNodeKey].g + nodeList[frNodeKey].h
  532.   end
  533.   if nodeList[fruNodeKey] then
  534.     nodeList[fruNodeKey].g = nodeList[fruNodeKey].g + nodeList[cNodeKey].g
  535.     nodeList[fruNodeKey].f = nodeList[fruNodeKey].g + nodeList[fruNodeKey].h
  536.   end
  537.   if nodeList[frdNodeKey] then
  538.     nodeList[frdNodeKey].g = nodeList[frdNodeKey].g + nodeList[cNodeKey].g
  539.     nodeList[frdNodeKey].f = nodeList[frdNodeKey].g + nodeList[frdNodeKey].h
  540.   end  
  541.   if nodeList[ruNodeKey] then
  542.     nodeList[ruNodeKey].g = nodeList[ruNodeKey].g + nodeList[cNodeKey].g
  543.     nodeList[ruNodeKey].f = nodeList[ruNodeKey].g + nodeList[ruNodeKey].h
  544.   end
  545.   if nodeList[rdNodeKey] then
  546.     nodeList[rdNodeKey].g = nodeList[rdNodeKey].g + nodeList[cNodeKey].g
  547.     nodeList[rdNodeKey].f = nodeList[rdNodeKey].g + nodeList[rdNodeKey].h
  548.   end  
  549.   if nodeList[rrNodeKey] then
  550.     nodeList[rrNodeKey].g = nodeList[rrNodeKey].g + nodeList[cNodeKey].g
  551.     nodeList[rrNodeKey].f = nodeList[rrNodeKey].g + nodeList[rrNodeKey].h
  552.   end
  553.   if nodeList[rruNodeKey] then
  554.     nodeList[rruNodeKey].g = nodeList[rruNodeKey].g + nodeList[cNodeKey].g
  555.     nodeList[rruNodeKey].f = nodeList[rruNodeKey].g + nodeList[rruNodeKey].h
  556.   end
  557.   if nodeList[rrdNodeKey] then
  558.     nodeList[rrdNodeKey].g = nodeList[rrdNodeKey].g + nodeList[cNodeKey].g
  559.     nodeList[rrdNodeKey].f = nodeList[rrdNodeKey].g + nodeList[rrdNodeKey].h
  560.   end  
  561.   if nodeList[buNodeKey] then
  562.     nodeList[buNodeKey].g = nodeList[buNodeKey].g + nodeList[cNodeKey].g
  563.     nodeList[buNodeKey].f = nodeList[buNodeKey].g + nodeList[buNodeKey].h  
  564.   end
  565.   if nodeList[bdNodeKey] then
  566.     nodeList[bdNodeKey].g = nodeList[bdNodeKey].g + nodeList[cNodeKey].g
  567.     nodeList[bdNodeKey].f = nodeList[bdNodeKey].g + nodeList[bdNodeKey].h  
  568.   end
  569.   if nodeList[lrNodeKey] then
  570.     nodeList[lrNodeKey].g = nodeList[lrNodeKey].g + nodeList[cNodeKey].g
  571.     nodeList[lrNodeKey].f = nodeList[lrNodeKey].g + nodeList[lrNodeKey].h  
  572.   end
  573.   if nodeList[lruNodeKey] then
  574.     nodeList[lruNodeKey].g = nodeList[lruNodeKey].g + nodeList[cNodeKey].g
  575.     nodeList[lruNodeKey].f = nodeList[lruNodeKey].g + nodeList[lruNodeKey].h  
  576.   end  
  577.   if nodeList[lrdNodeKey] then
  578.     nodeList[lrdNodeKey].g = nodeList[lrdNodeKey].g + nodeList[cNodeKey].g
  579.     nodeList[lrdNodeKey].f = nodeList[lrdNodeKey].g + nodeList[lrdNodeKey].h  
  580.   end
  581.   if nodeList[luNodeKey] then
  582.     nodeList[luNodeKey].g = nodeList[luNodeKey].g + nodeList[cNodeKey].g
  583.     nodeList[luNodeKey].f = nodeList[luNodeKey].g + nodeList[luNodeKey].h  
  584.   end
  585.   if nodeList[ldNodeKey] then
  586.     nodeList[ldNodeKey].g = nodeList[ldNodeKey].g + nodeList[cNodeKey].g
  587.     nodeList[ldNodeKey].f = nodeList[ldNodeKey].g + nodeList[ldNodeKey].h  
  588.   end  
  589.   if nodeList[flNodeKey] then
  590.     nodeList[flNodeKey].g = nodeList[flNodeKey].g + nodeList[cNodeKey].g
  591.     nodeList[flNodeKey].f = nodeList[flNodeKey].g + nodeList[flNodeKey].h
  592.   end
  593.   if nodeList[fluNodeKey] then
  594.     nodeList[fluNodeKey].g = nodeList[fluNodeKey].g + nodeList[cNodeKey].g
  595.     nodeList[fluNodeKey].f = nodeList[fluNodeKey].g + nodeList[fluNodeKey].h
  596.   end
  597.   if nodeList[fldNodeKey] then
  598.     nodeList[fldNodeKey].g = nodeList[fldNodeKey].g + nodeList[cNodeKey].g
  599.     nodeList[fldNodeKey].f = nodeList[fldNodeKey].g + nodeList[fldNodeKey].h
  600.   end  
  601.   local fResult,fInspect = turtle.inspect()
  602.   local uResult,uInspect = turtle.inspectUp()
  603.   local dResult,dInspect = turtle.inspectDown()
  604.   rightTurn()
  605.   local rResult,rInspect = turtle.inspect()
  606.   rightTurn()
  607.   local bResult,bInspect = turtle.inspect()
  608.   rightTurn()
  609.   local lResult,lInspect = turtle.inspect()
  610.   rightTurn()
  611.   if fResult == true then
  612.     nodeList[fNodeKey].state = "closed"
  613.     nodeList[fNodeKey].block_type = fInspect.name
  614.   end
  615.   if uResult == true then
  616.     nodeList[uNodeKey].state = "closed"
  617.     nodeList[uNodeKey].block_type = uInspect.name
  618.   end
  619.   if dResult == true then
  620.     nodeList[dNodeKey].state = "closed"
  621.     nodeList[dNodeKey].block_type = dInspect.name
  622.   end
  623.   if rResult == true then
  624.     nodeList[rNodeKey].state = "closed"
  625.     nodeList[rNodeKey].block_type = rInspect.name
  626.   end
  627.   if bResult == true then
  628.     nodeList[bNodeKey].state = "closed"
  629.     nodeList[bNodeKey].block_type = bInspect.name
  630.   end
  631.   if lResult == true then
  632.     nodeList[lNodeKey].state = "closed"
  633.     nodeList[lNodeKey].block_type = lInspect.name  
  634.   end
  635.   return nodeList[parentNodeKey], true
  636. end
  637.  
  638. local function getBestNode(currentVector, targetVector , startVector)
  639.   local bestMove = nil
  640.   for vNode, nTable in pairs(nodeList) do
  641.     if nTable.state == "open" then  
  642.       if bestMove == nil then  
  643.         bestMove = vNode
  644.       end
  645.       if nTable.f < nodeList[bestMove].f then  
  646.         bestMove = vNode
  647.       elseif nTable.f == nodeList[bestMove].f then  
  648.         if nTable.h < nodeList[bestMove].h then  
  649.           bestMove = vNode
  650.         elseif nTable.h == nodeList[bestMove].h then
  651.           if nodeList[bestMove].y > currentVector.y then
  652.             if nTable.y == currentVector.y then
  653.               bestMove = vNode
  654.             end
  655.           end
  656.         end
  657.       else
  658.       end
  659.     else
  660.     end
  661.   end
  662.   if nodeList[bestMove].state == "open" then
  663.     local bmVector = vector.new(nodeList[bestMove].x,nodeList[bestMove].y,nodeList[bestMove].z)
  664.     return bmVector
  665.   else
  666.     return false
  667.   end
  668. end
  669.  
  670. local function gotoNode(bnOffset, currentVector)  
  671.   if not facingDirection then
  672.     facingDirection = getFacingDirection()  -- using to pick move forward or back
  673.   end
  674.   local moves
  675.   if facingDirection == 1 then
  676.     if bnOffset.x < 0 then
  677.       for moves = 1,math.abs(bnOffset.x) do
  678.         turtle.forward()
  679.       end
  680.     else
  681.       for moves = 1,math.abs(bnOffset.x) do
  682.         turtle.back()
  683.       end
  684.     end
  685.     if bnOffset.z > 0 then
  686.       leftTurn()
  687.       for moves = 1,math.abs(bnOffset.z) do
  688.           turtle.forward()
  689.         end
  690.       rightTurn()
  691.     else
  692.       rightTurn()
  693.         for moves = 1,math.abs(bnOffset.z) do
  694.           turtle.forward()
  695.         end
  696.       leftTurn()
  697.     end
  698.     if bnOffset.y < 0 then
  699.       for moves = 1,math.abs(bnOffset.y) do
  700.         turtle.down()
  701.       end
  702.     elseif bnOffset.y > 0 then
  703.       for moves = 0 ,math.abs(bnOffset.z) do
  704.         turtle.up()
  705.       end
  706.     end
  707.   elseif facingDirection == 2 then
  708.     if bnOffset.z > 0 then
  709.       for moves = 1,math.abs(bnOffset.z) do
  710.         turtle.back()
  711.       end
  712.     elseif bnOffset.z < 0 then
  713.       for moves = 1,math.abs(bnOffset.z) do
  714.         turtle.forward()
  715.       end
  716.     end
  717.     if bnOffset.x < 0 then
  718.       leftTurn()
  719.       for moves = 1,math.abs(bnOffset.x) do
  720.         turtle.forward()
  721.       end
  722.       rightTurn()
  723.     elseif bnOffset.x > 0 then
  724.       rightTurn()
  725.       for moves = 1,math.abs(bnOffset.x) do
  726.         turtle.forward()
  727.       end
  728.       leftTurn()
  729.     end
  730.     if bnOffset.y < 0 then
  731.       for moves = 1,math.abs(bnOffset.y) do
  732.         turtle.down()
  733.       end
  734.     elseif bnOffset.y > 0 then
  735.       for moves = 0,math.abs(bnOffset.z) do
  736.         turtle.up()
  737.       end
  738.     end
  739.   elseif facingDirection == 3 then
  740.     if bnOffset.x > 0 then
  741.       for moves = 1,math.abs(bnOffset.x) do
  742.         turtle.forward()
  743.       end
  744.     else
  745.       for moves = 1,math.abs(bnOffset.x) do
  746.         turtle.back()
  747.       end
  748.     end
  749.     if bnOffset.z < 0 then
  750.       leftTurn()
  751.       for moves = 1,math.abs(bnOffset.z) do
  752.         turtle.forward()
  753.       end
  754.       rightTurn()
  755.     else
  756.       rightTurn()
  757.       for moves = 1,math.abs(bnOffset.z) do
  758.         turtle.forward()
  759.       end
  760.       leftTurn()
  761.     end
  762.     if bnOffset.y < 0 then
  763.       for moves = 1,math.abs(bnOffset.y) do
  764.         turtle.down()
  765.       end
  766.     elseif bnOffset.y > 0 then
  767.       for moves = 0 ,math.abs(bnOffset.z) do
  768.         turtle.up()
  769.       end
  770.     end
  771.   elseif facingDirection == 4 then
  772.     if bnOffset.z < 0 then
  773.       for moves = 1,math.abs(bnOffset.z) do
  774.         turtle.back()        
  775.       end
  776.     elseif bnOffset.z > 0 then
  777.       for moves = 1,math.abs(bnOffset.z) do
  778.         turtle.forward()        
  779.       end
  780.     end
  781.     if bnOffset.x > 0 then
  782.       leftTurn()
  783.       for moves = 1,math.abs(bnOffset.x) do
  784.         turtle.forward()
  785.       end
  786.       rightTurn()
  787.     elseif bnOffset.x < 0 then
  788.       rightTurn()
  789.       for moves = 1,math.abs(bnOffset.x) do
  790.         turtle.forward()
  791.       end
  792.       leftTurn()
  793.     end
  794.     if bnOffset.y < 0 then
  795.       for moves = 1,math.abs(bnOffset.y) do
  796.         turtle.down()
  797.       end
  798.     elseif bnOffset.y > 0 then
  799.       for moves = 0,math.abs(bnOffset.z) do
  800.         turtle.up()
  801.       end
  802.     end
  803.   end
  804.  
  805. end
  806.  
  807. local function seekDestination(currentVector, targetVector)
  808.   local hDisplacement
  809.   local moveCount = 0
  810.   local startVector = currentVector
  811.   local parentVector = currentVector
  812.   repeat
  813.     moveCount = moveCount + 1  
  814.     local lastParentNode, nodeDataStatus = getNodeData(currentVector, targetVector, startVector, parentVector)
  815.     local bestMoveVector = getBestNode(currentVector, targetVector , startVector)
  816.     if bestMoveVector then
  817.       local bestMoveOffset = bestMoveVector - currentVector
  818.       local bmResult = gotoNode(bestMoveOffset, currentVector)
  819.       parentVector = currentVector
  820.     end
  821.     currentVector = vector.new(gps.locate(5))
  822.   until currentVector.x == targetVector.x and currentVector.y == targetVector.y and currentVector.z == targetVector.z
  823. end
  824. --************Starting Seek*************
  825. facingDirection  = getFacingDirection()
  826. nodeList = {}
  827. cVector = vector.new(gps.locate(5))
  828. sVector = cVector
  829. targetVector = vector.new(-12, 75,-562)
  830. m = seekDestination(cVector,targetVector,sVector)
RAW Paste Data