blunty666

aStar

Sep 5th, 2015
328
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. if not pQueue then
  2.     if not os.loadAPI("pQueue") then
  3.         error("could not load pQueue API")
  4.     end
  5. end
  6.  
  7. -- a very basic map API used to store node information
  8. local mapMethods = {
  9.     get = function(self, tVector)
  10.         if self.map[tVector.x] and self.map[tVector.x][tVector.y] then
  11.             return self.map[tVector.x][tVector.y][tVector.z]
  12.         end
  13.     end,
  14.     set = function(self, tVector, value)
  15.         if not self.map[tVector.x] then
  16.             self.map[tVector.x] = {}
  17.         end
  18.         if not self.map[tVector.x][tVector.y] then
  19.             self.map[tVector.x][tVector.y] = {}
  20.         end
  21.         self.map[tVector.x][tVector.y][tVector.z] = value
  22.         return self.map[tVector.x][tVector.y][tVector.z]
  23.     end,
  24.     getOrSet = function(self, tVector, value)
  25.         if self.map[tVector.x] and self.map[tVector.x][tVector.y] and self.map[tVector.x][tVector.y][tVector.z] ~= nil then
  26.             return self.map[tVector.x][tVector.y][tVector.z], false
  27.         else
  28.             return self:set(tVector, value), true
  29.         end
  30.     end,
  31. }
  32. local mapMetatable = {__index = mapMethods}
  33.  
  34. function newMap()
  35.     return setmetatable({map = {}}, mapMetatable)
  36. end
  37.  
  38. local function makePath(nodes, start, startEnd, goalStart, goal)
  39.     local current, path = startEnd, {}
  40.     while not vectorEquals(current, start) do
  41.         table.insert(path, current)
  42.         current = nodes:get(current)[1]
  43.     end
  44.     current = goalStart
  45.     while not vectorEquals(current, goal) do
  46.         table.insert(path, 1, current)
  47.         current = nodes:get(current)[1]
  48.     end
  49.     table.insert(path, 1, goal)
  50.     return path
  51. end
  52.  
  53. function vectorEquals(a, b) -- the comparison function used in pQueue
  54.     return a.x == b.x and a.y == b.y and a.z == b.z
  55. end
  56.  
  57. local posZ = vector.new(0, 0, 1)
  58. local negX = vector.new(-1, 0, 0)
  59. local negZ = vector.new(0, 0, -1)
  60. local posX = vector.new(1, 0, 0)
  61. local posY = vector.new(0, 1, 0)
  62. local negY = vector.new(0, -1, 0)
  63. function adjacent(u)
  64.     return {
  65.         u + posZ,
  66.         u + negX,
  67.         u + negZ,
  68.         u + posX,
  69.         u + posY,
  70.         u + negY,
  71.     }
  72. end
  73.    
  74. function distance(a, b) -- 1-norm/manhattan metric
  75.     return math.abs(a.x - b.x) + math.abs(a.y - b.y) + math.abs(a.z - b.z)
  76. end
  77.  
  78. function compute(distanceFunction, start, goal)
  79.    
  80.     if type(distanceFunction) ~= "function" then
  81.         error("aStar new: distanceFunction must be of type function", 2)
  82.     end
  83.        
  84.     local distanceFunc = distanceFunction -- is this necessary?
  85.  
  86.     -- node data structure is {parent node, true cost from startNode/goalNode, whether in closed list, search direction this node was found in, whether in open list}
  87.     local nodes = newMap()
  88.     nodes:set(start, {start + vector.new(0, 0, -1), 0, false, true, true})
  89.     nodes:set(goal, {goal + vector.new(0, 0, -1), 0, false, false, true})
  90.  
  91.     local openStartSet = pQueue.new()
  92.     openStartSet:insert(start, distance(start, goal))
  93.     local openGoalSet = pQueue.new()
  94.     openGoalSet:insert(goal, distance(start, goal))
  95.  
  96.     local yieldCount = 0
  97.     local activeOpenSet, pendingOpenSet = openStartSet, openGoalSet
  98.     local forwardSearch, lastNode, switch = true, false, false
  99.    
  100.     local current, currNode, parent
  101.     local baseCost
  102.     local newCost
  103.     local nbrNode, newNode
  104.     local preHeuristic
  105.  
  106.     while not openStartSet:isEmpty() and not openGoalSet:isEmpty() do
  107.  
  108.         --yield every so often to avoid getting timed out
  109.         yieldCount = yieldCount + 1
  110.         if yieldCount > 200 then
  111.             os.pullEvent(os.queueEvent("yield"))
  112.             yieldCount = 0
  113.         end
  114.  
  115.         if switch then --switch the search direction
  116.             activeOpenSet, pendingOpenSet = pendingOpenSet, activeOpenSet
  117.             forwardSearch = not forwardSearch
  118.             lastNode = false
  119.         end
  120.  
  121.         current = activeOpenSet:pop()
  122.         currNode = nodes:get(current)
  123.         parent = current - currNode[1]
  124.            
  125.         currNode[3], currNode[5], switch = true, false, true
  126.            
  127.         for _, neighbour in ipairs(adjacent(current)) do
  128.            
  129.             baseCost = distanceFunc(current, neighbour)
  130.             if baseCost < math.huge then -- if not graph:get(neighbour) then
  131.            
  132.                 newCost = currNode[2] + baseCost
  133.            
  134.                 nbrNode, newNode = nodes:getOrSet(neighbour, {current, newCost, false, forwardSearch, false})
  135.                 if switch and ((not lastNode) or vectorEquals(lastNode, neighbour)) then
  136.                     switch = false
  137.                 end
  138.  
  139.                 if not newNode then
  140.                     if forwardSearch ~= nbrNode[4] then -- nbrNode has been discovered in the opposite search direction
  141.                         if nbrNode[3] then -- and is in the closed list so has been expanded already
  142.                             return makePath(nodes, start, (forwardSearch and current) or neighbour, (forwardSearch and neighbour) or current, goal)
  143.                         end
  144.                     elseif newCost < nbrNode[2] then
  145.                         if nbrNode[5] then
  146.                             activeOpenSet:remove(neighbour, vectorEquals)
  147.                             nbrNode[5] = false
  148.                         end
  149.                         nbrNode[3] = false
  150.                     end
  151.                 end
  152.  
  153.                 if (newNode or (forwardSearch ~= nbrNode[4] and not nbrNode[5] and not nbrNode[3])) and newCost < math.huge then
  154.                     nbrNode[1] = current
  155.                     nbrNode[2] = newCost
  156.                     nbrNode[4] = currNode[4]
  157.                     nbrNode[5] = true
  158.                     preHeuristic = distance(neighbour, (forwardSearch and goal) or start)
  159.                     activeOpenSet:insert(neighbour, newCost + preHeuristic + 0.0001*(preHeuristic + parent.length(parent:cross(neighbour - current))))
  160.                 end
  161.             end
  162.         end
  163.         lastNode = current
  164.     end
  165.     return false
  166. end
RAW Paste Data