pathFinder

local WORLD, SIDE = {}, {{1,0,0},{-1,0,0},{0,0,1},{0,0,-1},{0,1,0},{0,-1,0}}
local HUGE, UNPK, INSRT = math.huge, table.unpack, table.insert

local function hash(x, y, z)
  x, z = x+134217728, z+134217728
  return y|(x<<8)|(z<<36)
end

local function unhash(h)
  return ((h&68719476480)>>8)-134217728, h&255, ((h&-68719476736)>>36)-134217728
end

local function delta(x1, y1, z1, x2, y2, z2)
  return (x1-x2)^2+(y1-y2)^2+(z1-z2)^2
end

local function getPath(sX, sY, sZ, tX, tY, tZ)
  local S, T, d = hash(tX, tY, tZ), hash(sX, sY, sZ), delta(sX, sY, sZ, tX, tY, tZ)
  if not WORLD[S] or not WORLD[T] then return nil end
  local OPEN, CLOSED, PATH = {[S]={d,d}}, {}, {}
  local cur, f, cX, cY, cZ, pX, pY, pZ, a, b, ch
  while true do
    cur, d, f = nil, HUGE, HUGE
    for k, v in pairs(OPEN) do
      if v[1] < d and v[2] < f then
        d, f = v[1], v[2]
        cur = k
      end
    end
    if cur then
      OPEN[cur], CLOSED[cur] = nil, {UNPK(OPEN[cur])}
      cX, cY, cZ = unhash(cur)
      for s = 1, #SIDE do
        pX, pY, pZ = cX+SIDE[s][1], cY+SIDE[s][2], cZ+SIDE[s][3]
        ch = hash(pX, pY, pZ)
        if WORLD[ch] and not CLOSED[ch] then
          a, b = delta(pX, pY, pZ, sX, sY, sZ), delta(pX, pY, pZ, tX, tY, tZ)
          OPEN[ch] = {a, b, cur}
          if ch == T then
            node = ch
            for i, j in pairs(OPEN) do
              CLOSED[i] = {UNPK(j)}
            end
            OPEN = nil
            while node ~= S do
              INSRT(PATH, {unhash(node)})
              if CLOSED[node] then
                node, CLOSED[node] = CLOSED[node][3], nil
              end
            end
            INSRT(PATH, {tX, tY, tZ})
            return PATH
          end
        end
      end
    else
      return nil
    end
  end
  if os.clock() > 0.5 then
    return nil
  end
end

for x = 1, 100 do
  for y = 1, 100 do
    WORLD[hash(x, y, 0)] = true
  end
end
g = getPath(1, 1, 0, 100, 100, 0)
if g then
  for i = 1, #g do
    print(UNPK(g[i]))
  end
end

--[[
x500 12.7
x400 7.4
x300 4.3
x200 2.4
x100 0.1
]]