pathFinder2

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, PRS = math.huge, table.unpack, table.insert, pairs

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

-- FRONT/MARKED = { {x, y, z, d_total, d_target, parent}, ... }
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 FRONT, MARKED, PATH, DIR = {[S]={tX, tY, tZ, d, d, S}}, {}, {}, 1
  local c, t, cX, cY, cZ, pX, pY, pZ, a, b, ch, N
  while true do
    c, t, d = nil, HUGE, HUGE
    for i, j in PRS(FRONT) do
      if j[4] < t and j[5] < d then
        c, t, d = i, j[4], j[5]
      end
    end
    if c then
      MARKED[c], FRONT[c] = {UNPK(FRONT[c])}, nil
      cX, cY, cZ = MARKED[c][1], MARKED[c][2], MARKED[c][3]
      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 MARKED[ch] and not FRONT[ch] then
          a, b = delta(pX, pY, pZ, sX, sY, sZ), delta(pX, pY, pZ, tX, tY, tZ)
          if (s > 0 and s < 5) and s ~= DIR then
            DIR, a = s, b-a
          end
          FRONT[ch] = {pX, pY, pZ, a, b, c}
          if ch == T then
            N = ch
            for i, j in PRS(FRONT) do
              MARKED[i] = {UNPK(j)}
            end
            FRONT = nil
            while N ~= S do
              INSRT(PATH, {unhash(N)})
              if MARKED[N] then
                N, MARKED[N] = MARKED[N][6], 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, 10 do
  for y = 1, 10 do
    if math.random(0, 3) ~= 0 then
      WORLD[hash(x, y, 0)] = true
    end
  end
end
local g = getPath(1, 1, 0, 10, 10, 0)
if not g then
  print('PATH NOT FOUND')
end
local a,c=''
for x = 1, 10 do
  for y = 1, 10 do
    for i = 1, #g do
      if g[i][1] == x and g[i][2] == y then
        c=true
      end
    end
    if c then
      a, c = a..'o', nil
    else
      if WORLD[hash(x, y, 0)] then
        a = a..' '
      else
        a = a..'x'
      end
    end
  end
  a = a..'\n'
end
print(a)
]]