merlin11188

A* Algorithm

Dec 11th, 2011
122
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
Lua 2.58 KB | None | 0 0
  1. function pathFind(Map) -- Map is a 2-dimensional matrix with uniform width and height (a rectangle)
  2.     local height, width = 0, 0
  3.     local start, finish, open, closed, order, current = {}, {}, {}, {}, {}, {}
  4.     -- Format: open[x .. "-" .. y] = {{x, y}, ParentNode, fCost}
  5.     for i, v in pairs(Map) do
  6.         height = height + 1
  7.         for j, k in pairs(v) do
  8.             if height == 1 then
  9.                 width = width + 1
  10.             end
  11.             if k == 2 then
  12.                 start = {j, i}
  13.             elseif k == 3 then
  14.                 finish = {j, i}
  15.             end
  16.         end
  17.     end
  18.    
  19.     local function getPrCost(Node, ParentNode)
  20.         if not ParentNode then return 0 end
  21.         if Node[1] - ParentNode[1] == 0 or Node[2] - ParentNode[2] == 0 then
  22.             return 10
  23.         else
  24.             return 14
  25.         end
  26.     end
  27.  
  28.     local function getSeCost(Node)
  29.         return math.floor((((Node[1] - finish[1])^2 + (Node[2] - finish[2])^2)) ^ .5 * 10 + .5)
  30.     end
  31.        
  32.     local function getSurroundingNodes(Node, ParentCost)
  33.         local x, y = Node[1], Node[2]
  34.         for i = -1, 1 do
  35.             for j = -1, 1 do
  36.                 if x + i > width or y + j > height or x + i <= 0 or y + j <= 0 or (Map[y + j][x + i] and Map[y + j][x +i] == 1) or closed[x + i .. "-" .. y + j] then
  37.                     open[x + i .. "-" .. y + j] = nil
  38.                     closed[x + i .. "-" .. y + j] = true
  39.                 elseif open[x + i .. "-" .. y + j] then
  40.                     if ParentCost + getPrCost({x + i, y + j}, Node) < getPrCost({x + i, y + j}, open[x + i .. "-" .. y + j][2]) then
  41.                         open[x + i .. "-" .. y + j] = {{x + i, y + j}, Node, getPrCost({x + i, y + j}, Node) + getSeCost({x + i, y + j})}
  42.                     end
  43.                 else
  44.                     open[x + i .. "-" .. y + j] = {{x + i, y + j}, Node, getPrCost({x + i, y + j}, Node) + getSeCost({x + i, y + j})}
  45.                 end
  46.             end
  47.         end
  48.     end
  49.    
  50.     current = start
  51.     --open[start[1] .. "-" .. start[2]] = {{start[1], start[2]}, nil, 0}
  52.     while (true) do
  53.         if current[1] == start[1] and current[2] == start[2] then
  54.             localCost = 0
  55.         else
  56.             local localCost = getPrCost(current, open[current[1] .. "-" .. current[2]][2])
  57.         end
  58.         open[current[1] .. "-" .. current[2]] = nil
  59.         closed[current[1] .. "-" .. current[2]] = true
  60.         if current[1] == finish[1] and current[2] == finish[2] then print("Search complete.") return order end
  61.         getSurroundingNodes(current, localCost)
  62.         local closest, lowest
  63.         for i, v in pairs(open) do
  64.             if not closest or v[3] < lowest then
  65.                 closest = v[1]
  66.                 lowest = v[3]
  67.             end
  68.         end
  69.         if not closest then print("No path.") return {} end
  70.         table.insert(order, {closest[1], closest[2]})
  71.         current = closest
  72.     end
  73. end
  74.  
  75. local order = pathFind {
  76.     {2, 0, 0},
  77.     {0, 1, 0},
  78.     {0, 0, 3}
  79. }
  80.        
  81. for i, v in pairs(order) do
  82.     print("{" .. v[1] .. ", " .. v[2] .. "}")
  83. end
  84.  
Advertisement
Add Comment
Please, Sign In to add comment