pathfind.lua

--simulated program for OpenComputers Robots
-- OC:robot = require("robot")
cords = {112, 21,398, 0}
dest  = {103, 64, 85, 2}
steps = 0

N=0 S=2 -- +z -z
E=1 W=3 -- +x -x

function ChangeDirection(dir)
 while cords[4] ~= dir do
  robot.turnRight()
  cords[4] = (cords[4]+1)%4
 end
end

function run() -- OC:*remove this line*
 while not(cords[1]==dest[1] and cords[2]==dest[2] and cords[3]==dest[3]) do
  print("\\My Cords are: ["..tostring(cords[1])..", "..tostring(cords[2])..", "..tostring(cords[3]).."] |  Dir:"..tostring(cords[4]))
  print(" \\My current distance is: "..tostring(math.sqrt(math.pow((dest[1]-cords[1]),2)+math.pow((dest[2]-cords[2]),2)+math.pow((cords[3]-dest[3]),2))))
  distsum = {
   {math.sqrt(math.pow((dest[1]-(cords[1]+1)),2)+math.pow((dest[2]- cords[2]   ),2)+math.pow((dest[3]- cords[3]   ),2)),"+x"},
   {math.sqrt(math.pow((dest[1]-(cords[1]-1)),2)+math.pow((dest[2]- cords[2]   ),2)+math.pow((dest[3]- cords[3]   ),2)),"-x"},
   {math.sqrt(math.pow((dest[1]- cords[1]   ),2)+math.pow((dest[2]-(cords[2]+1)),2)+math.pow((dest[3]- cords[3]   ),2)),"+y"},
   {math.sqrt(math.pow((dest[1]- cords[1]   ),2)+math.pow((dest[2]-(cords[2]-1)),2)+math.pow((dest[3]- cords[3]   ),2)),"-y"},
   {math.sqrt(math.pow((dest[1]- cords[1]   ),2)+math.pow((dest[2]- cords[2]   ),2)+math.pow((dest[3]-(cords[3]+1)),2)),"+z"},
   {math.sqrt(math.pow((dest[1]- cords[1]   ),2)+math.pow((dest[2]- cords[2]   ),2)+math.pow((dest[3]-(cords[3]-1)),2)),"-z"},
  } table.sort(distsum, function(a,b) return a[1]<b[1] end)
  print("  \\My best directions is on the "..tostring(distsum[1][2]).." axis distance: "..tostring(distsum[1][1]))
  i=1
  while true do
   if distsum[i][2] == "+x" then ChangeDirection(E); if robot.forward() == true then cords[1]=cords[1]+1; break end end
   if distsum[i][2] == "-x" then ChangeDirection(S); if robot.forward() == true then cords[1]=cords[1]-1; break end end
   if distsum[i][2] == "+y" then                     if robot.up()      == true then cords[2]=cords[2]+1; break end end
   if distsum[i][2] == "-y" then                     if robot.down()    == true then cords[2]=cords[2]-1; break end end
   if distsum[i][2] == "+z" then ChangeDirection(N); if robot.forward() == true then cords[3]=cords[3]+1; break end end
   if distsum[i][2] == "-z" then ChangeDirection(W); if robot.forward() == true then cords[3]=cords[3]-1; break end end
   print("  /My Cords are: ["..tostring(cords[1])..", "..tostring(cords[2])..", "..tostring(cords[3]).."] |  Dir:"..tostring(cords[4]))
   print(" /My current distance is: "..tostring(math.sqrt(math.pow((cords[1]-dest[1]),2)+math.pow((cords[2]-dest[2]),2)+math.pow((cords[3]-dest[3]),2))))
   i=i+1; print("/ Redirecting...")
  end steps=steps+1
 end ChangeDirection(dest[4])
 print("\\My Cords are: ["..tostring(cords[1])..", "..tostring(cords[2])..", "..tostring(cords[3]).."] | Dir:"..tostring(cords[4]))
 print(" \\Number of Steps: "..tostring(steps))
end -- OC:*from here and below remove*

--simulating blocks
blocks = {
 ["111,22,390"] = 1, ["112,22,390"] = 1, ["112,22,389"] = 1,
 ["111,21,390"] = 1, ["112,21,390"] = 1, ["112,21,389"] = 1,
 ["111,20,390"] = 1, ["112,20,390"] = 1, ["112,20,389"] = 1,
}
--simulating robot
robot = {
 ["facing"] = 0,
 ["turnLeft"] =  function() robot.facing=(robot.facing-1)%4 end,
 ["turnRight"] = function() robot.facing=(robot.facing+1)%4 end,
 ["forward"] = function()
  for i,k in pairs(blocks) do
   if     robot.facing == E then
    if tostring(cords[1]+1)..","..tostring(cords[2])..","..tostring(cords[3]  ) == i then cords[1]=cords[1]+1; return false end
   elseif robot.facing == W then
    if tostring(cords[1]-1)..","..tostring(cords[2])..","..tostring(cords[3]  ) == i then cords[1]=cords[1]-1; return false end
   elseif robot.facing == N then
    if tostring(cords[1]  )..","..tostring(cords[2])..","..tostring(cords[3]+1) == i then cords[3]=cords[3]+1; return false end
   elseif robot.facing == S then
    if tostring(cords[1]  )..","..tostring(cords[2])..","..tostring(cords[3]-1) == i then cords[3]=cords[3]-1; return false end
   end
  end return true
 end,
 ["back"] = function()
  for i,k in pairs(blocks) do
   if     robot.facing == E then
    if tostring(cords[1]+1)..","..tostring(cords[2])..","..tostring(cords[3]  ) == i then cords[1]=cords[1]-1; return false end
   elseif robot.facing == W then
    if tostring(cords[1]-1)..","..tostring(cords[2])..","..tostring(cords[3]  ) == i then cords[1]=cords[1]+1; return false end
   elseif robot.facing == N then
    if tostring(cords[1]  )..","..tostring(cords[2])..","..tostring(cords[3]+1) == i then cords[3]=cords[3]-1; return false end
   elseif robot.facing == S then
    if tostring(cords[1]  )..","..tostring(cords[2])..","..tostring(cords[3]-1) == i then cords[3]=cords[3]+1; return false end
   end
  end return true
 end,
 ["up"] = function()
  for i,k in pairs(blocks) do
   if tostring(cords[1])..","..tostring(cords[2]+1)..","..tostring(cords[3]) == i then
    return false
   end
  end return true
 end,
 ["down"] = function()
  for i,k in pairs(blocks) do
   if tostring(cords[1])..","..tostring(cords[2]-1)..","..tostring(cords[3]) == i then
    return false
   end
  end return true
 end,
}
run()