SHARE
TWEET

Naive Lua AStar

upvoid Feb 15th, 2013 540 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. -- See Benchmarks.txt for details on this test
  2.  
  3. function performAStar(repetitions)
  4.     map = [[S   XXX               X
  5.     XX      XXXXXXXXXXXXX X
  6.            XX             X
  7.     X     XXX        XXXXXX
  8.             XXXX   XXXXXXXX
  9.     XXXXXXX    X          X
  10.     XX         X   XXXXX  X
  11.               XX     XX   X
  12.     XXXXXXXXXXX           X
  13.     XX          XXXXXXXXX X
  14.            XXXXXX         X
  15.     X     XXX        XXXXXX
  16.             XXXX   XXXXXXXX
  17.     XXXXXXX    X          X
  18.     XX         X   XXXXX  X
  19.               XX     XX   X
  20.     XXXXX XXXXX           X
  21.     XX          XXXX XXXX X
  22.            XXXXXX         X
  23.     X     XXX        XXXXXX
  24.             XXXX   XXXXXXXX
  25.     XXXXX XX   X          X
  26.     XX         X   XXXXX  X
  27.               XX     XX   X
  28.     XXXXX XXXXXXXXXXXX    X
  29.                   G XX    X
  30.     XXXXXXXXXXXXXXXXXXXXXXX]];
  31.  
  32.     for i=0,repetitions do
  33.         nodes, start, goal = parseMap(map);
  34.  
  35.         if aStar(start, goal) then
  36.             reconstructPath(goal);
  37.         else
  38.             io.write("No valid path found!");
  39.         end
  40.     end
  41. end
  42.  
  43. function parseMap(map)
  44.  
  45.     local x = 1;
  46.     local y = 1;
  47.     local width=1;
  48.     local height=1;
  49.  
  50.     local nodes = {};
  51.     local start;
  52.     local goal;
  53.  
  54.     -- create all nodes and find start and goal
  55.     for i=1,#map do
  56.         char = string.byte(map, i);
  57.         if char == string.byte("\n") then -- linebreak?
  58.             y = y+1;
  59.             width = x-1;
  60.             x = 1;
  61.         else
  62.             if nodes[x] == nil then
  63.                 nodes[x] = {};
  64.             end
  65.             if char ~= string.byte("X") then --accessible node?
  66.                 -- create a new node with the current coordinates
  67.                 local node = {};
  68.                 node.x = x;
  69.                 node.y = y;
  70.                 node.g = 1.0/0.0; -- initialize these values with infinity
  71.                 node.f = 1.0/0.0;
  72.                 node.neighbors = {};
  73.                 node.previous = nil;
  74.  
  75.                 --check if this node is start or goal
  76.                 if char == string.byte("S") then
  77.                     start = node;
  78.                 elseif char == string.byte("G") then
  79.                     goal = node;
  80.                 end
  81.  
  82.                 -- save the node in our list, indexed by its coordinates
  83.                 nodes[x][y] = node;
  84.  
  85.             end
  86.             x = x+1;
  87.  
  88.         end
  89.  
  90.     end
  91.     height = y;
  92.  
  93.     -- connect the nodes
  94.     for x=1, width do
  95.         for y=1, height do
  96.             if nodes[x][y] ~= nil then
  97.                 -- up
  98.                 if y > 1 and nodes[x][y - 1] ~= nil then
  99.                     table.insert(nodes[x][y].neighbors, nodes[x][y - 1]);
  100.                 end
  101.  
  102.                 -- up right
  103.                 if y > 1 and x < width and nodes[x + 1][y - 1] ~= nil then
  104.                     table.insert(nodes[x][y].neighbors, nodes[x + 1][y - 1]);
  105.                 end
  106.  
  107.                 -- right
  108.                 if x < width and nodes [x + 1][y] ~= nil then
  109.                     table.insert(nodes[x][y].neighbors, nodes[x + 1][y]);
  110.                 end
  111.  
  112.                 -- right down
  113.                 if x < width and y < height and nodes[x + 1][y + 1] ~= nil then
  114.                     table.insert(nodes[x][y].neighbors, nodes[x + 1][y + 1]);
  115.                 end
  116.  
  117.                 -- down
  118.                 if y < height and nodes[x][y + 1] ~= nil then
  119.                     table.insert(nodes[x][y].neighbors, nodes[x][y + 1]);
  120.                 end
  121.  
  122.                 -- down left
  123.                 if y < height and x > 1 and nodes[x - 1][y + 1] ~= nil then
  124.                     table.insert(nodes[x][y].neighbors, nodes[x - 1][y + 1]);
  125.                 end
  126.  
  127.                 -- left
  128.                 if x > 1 and nodes[x - 1][y] ~= nil then
  129.                     table.insert(nodes[x][y].neighbors, nodes[x - 1][y]);
  130.                 end
  131.  
  132.                 -- left up
  133.                 if x > 1 and y > 1 and nodes[x - 1][y - 1] ~= nil then
  134.                     table.insert(nodes[x][y].neighbors, nodes[x - 1][y - 1]);
  135.                 end
  136.             end
  137.         end
  138.     end
  139.  
  140.     return nodes, start, goal;
  141.  
  142. end
  143.  
  144. -- returns true iff there is a path between start and goal.
  145. -- the 'previous' value is set accordingly for each node on the path
  146. function aStar(start, goal)
  147.     local closedSet = {};
  148.     local openSet = { start };
  149.     start.g = 0.0;
  150.     start.f = h(start, goal);
  151.     while #openSet > 0 do
  152.         local current = findBest(openSet);
  153.  
  154.         table.insert(closedSet, current);
  155.  
  156.         if current == goal then
  157.             return true;
  158.         end
  159.  
  160.         for index,neighbor in next,current.neighbors,nil do
  161.             if not hasNode(neighbor, closedSet) then
  162.                 local newG = current.g + h(current, neighbor);
  163.  
  164.  
  165.                 if newG < neighbor.g then
  166.                     neighbor.g = newG;
  167.                     neighbor.f = newG + h(neighbor, goal)
  168.                     neighbor.previous = current;
  169.                 end
  170.  
  171.                 if not hasNode(neighbor, openSet) then
  172.                     table.insert(openSet, neighbor)
  173.                 end
  174.  
  175.             end
  176.         end
  177.  
  178.     end
  179.  
  180.     return false;
  181.  
  182. end
  183.  
  184. --save the length of the result to prevent the compiler omptimizing away anything important
  185. resultLen = 0;
  186.  
  187. -- pack the path saved in the nodes into a string
  188. function reconstructPath(goal)
  189.     local pathString = "";
  190.  
  191.     node = goal;
  192.  
  193.     while node ~= nil do
  194.         pathString = "("..(node.x)..","..(node.y)..")"..pathString;
  195.         node = node.previous;
  196.     end
  197.  
  198.     resultLen = #pathString;
  199.  
  200.     return pathString;
  201.  
  202. end
  203.  
  204. -- the heuristic used is simply the distance between the nodes
  205. function h(current, goal)
  206.     local dx = current.x - goal.x;
  207.     local dy = current.y - goal.y;
  208.  
  209.     return math.sqrt(dx*dx + dy*dy);
  210. end
  211.  
  212.  
  213.  
  214. --search and remove the node with the best f value in a given set
  215. function findBest(nodes)
  216.     local best = nil;
  217.     local bestF = 1.0/0.0;
  218.     local bestIndex = 0;
  219.     for index,node in next,nodes,nil do
  220.         if node.f < bestF then
  221.             best = node;
  222.             bestF = node.f;
  223.             bestIndex = index;
  224.         end
  225.     end
  226.  
  227.     if best ~= nil then
  228.         table.remove(nodes, bestIndex);
  229.     end
  230.  
  231.     return best;
  232. end
  233.  
  234. -- returns true iff haystack contains needle
  235. function hasNode(needle, haystack)
  236.     for index,node in next,haystack,nil do
  237.         if node == needle then
  238.             return true;
  239.         end
  240.     end
  241.     return false;
  242. end
RAW Paste Data
Pastebin PRO Summer Special!
Get 40% OFF on Pastebin PRO accounts!
Top