vx13

lib/pathfinder.lua

Feb 12th, 2017
159
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. -- ********************************************************************************** --
  2. -- **                                                                              ** --
  3. -- **   A-Star algorithm for 3d dimensional volume                                 ** --
  4. -- **                                                                              ** --
  5. -- **   http://en.wikipedia.org/wiki/A*_search_algorithm                           ** --
  6. -- **   ----------------------------------------------------                       ** --
  7. -- **                                                                              ** --
  8. -- **   Developed to use in program "KrutoyTurtle"                                 ** --
  9. -- **   http://computercraft.ru/topic/48-stroitelnaia-sistema-krutoyturtle/        ** --
  10. -- **                                                                              ** --
  11. -- ********************************************************************************** --
  12.  
  13. -- Redefine global function for faster acces
  14. local abs,pairs,floor,time,insert,remove,sleep,time = math.abs,pairs,math.floor,os.time,table.insert,table.remove,os.sleep,os.time
  15.  
  16. -- Heuristic estimate.
  17. local function heuristic_cost_estimate(p1,p2)
  18.   return abs(p1.x-p2.x) + abs(p1.y-p2.y) + abs(p1.z-p2.z)
  19. end
  20.  
  21. -- ********************************************************************************** --
  22. -- **                         Utils                                                ** --
  23. -- ********************************************************************************** --
  24.  
  25. -- 3d array metatable
  26. -- Use arr3d() as constructor, arr(x,y,z) as acces and arr:set(x,y,z,v) as set value
  27. if not arr3d then arr3d = function() return setmetatable({  
  28.   set = function(t,x,y,z,v)
  29.     t[z]    = t[z]    or {}
  30.     t[z][y] = t[z][y] or {}
  31.     t[z][y][x] = v
  32.   end,
  33.   setVolume = function(t, x1,y1,z1,x2,y2,z2, v)
  34.     for z=z1, z2 do
  35.       for y=y1, y2 do
  36.         for x=x1, x2 do
  37.           t:set(x,y,z, v)
  38.         end
  39.       end
  40.     end  
  41.   end
  42.   }, { __call = function(t, x, y, z)
  43.     if not t[z] or not t[z][y] then return nil end
  44.     return t[z][y][x]
  45.   end
  46. })end end
  47.  
  48.  
  49. -- Checking time for avoid yeld exception
  50. local chtime
  51. local function checktime()
  52.   -- os.queueEvent("")
  53.   -- coroutine.yield()
  54.  
  55.   if not chtime then chtime=time() end
  56.   local ntime=time()
  57.   if ntime-chtime>0.05 or ntime-chtime<0  then
  58.     sleep(0)
  59.     chtime=ntime
  60.   end
  61. end
  62.  
  63. -- ********************************************************************************** --
  64. -- **                            BreadCrumb                                        ** --
  65. -- ********************************************************************************** --
  66.  
  67. local BreadCrumb = {}
  68. BreadCrumb.__index = BreadCrumb
  69.  
  70. function BreadCrumb.new(x,y,z, parent)
  71.   local self = setmetatable({}, BreadCrumb)
  72.   self.pos = {x=x,y=y,z=z}
  73.   self.next = parent
  74.   self.cost = math.huge
  75.   self.onClosedList = false
  76.   self.onOpenList = false
  77.   return self
  78. end
  79.  
  80. function BreadCrumb:Equals(b)
  81.     return b.pos.x == self.pos.x and b.pos.y == self.pos.y and b.pos.z == self.pos.z
  82. end
  83.  
  84. -- ********************************************************************************** --
  85. -- **                          Path Finder                                         ** --
  86. -- ********************************************************************************** --
  87.  
  88. -- Neigtbours of current point
  89. local surrounding = {
  90.   {x=0,y=0,z=-1}, {x=0,y=0,z= 1}, {x=0,y=-1,z=0}, {x=0,y= 1,z=0}, {x=-1,y=0,z=0}, {x=1, y=0,z=0},
  91. }
  92.  
  93.  
  94. -- Method that switfly finds the best path from p_start to end.
  95. -- The starting breadcrumb traversable via .next to the end or nil if there is no path
  96. local function AStarFindPath(world, p_start, p_end)
  97.     -- note we just flip p_start and end here so you don't have to.            
  98.    p_end, p_start = p_start, p_end
  99.  
  100.   -- Destination point is bloked
  101.   if world(p_start.x, p_start.y, p_start.z) then return end
  102.  
  103.   if p_start.x == p_end.x and p_start.y == p_end.y and p_start.z == p_end.z then
  104.     return { p_start }
  105.   end
  106.  
  107.   local openList = {}
  108.   local brWorld  = arr3d()
  109.  
  110.  
  111.   local current= BreadCrumb.new(p_start.x,p_start.y,p_start.z)
  112.   current.cost = heuristic_cost_estimate(current.pos,p_end)
  113.  
  114.   local finish = BreadCrumb.new(p_end.x,p_end.y,p_end.z)
  115.   brWorld:set(current.pos.x,current.pos.y,current.pos.z,current)
  116.   insert(openList,current)
  117.  
  118.  
  119.   while #openList > 0 do
  120.     --Find best item and switch it to the 'closedList'
  121.     current = remove(openList)
  122.     current.onClosedList = true
  123.  
  124.     --Find neighbours
  125.     for k,v in pairs(surrounding) do
  126.       local tmpX,tmpY,tmpZ = current.pos.x + v.x, current.pos.y + v.y, current.pos.z + v.z
  127.       if not world(tmpX,tmpY,tmpZ) then
  128.         --Check if we've already examined a neighbour, if not create a new node for it.
  129.         local node
  130.         if brWorld(tmpX,tmpY,tmpZ) == nil then
  131.           node = BreadCrumb.new(tmpX,tmpY,tmpZ)
  132.           brWorld:set(tmpX,tmpY,tmpZ,node)
  133.         else
  134.           node = brWorld(tmpX,tmpY,tmpZ)
  135.         end
  136.        
  137.         --If the node is not on the 'closedList' check it's new score, keep the best
  138.         if node.onClosedList == false then
  139.           local cost = heuristic_cost_estimate(node.pos,p_end)
  140.          
  141.           if cost < node.cost then
  142.             node.cost = cost
  143.             node.next = current
  144.           end
  145.  
  146.           --If the node wasn't on the openList yet, add it
  147.           if node.onOpenList == false then
  148.             --Check to see if we're done
  149.             if node:Equals(finish) == true then
  150.               node.next = current
  151.               return node
  152.             end
  153.             node.onOpenList = true
  154.            
  155.             -- Sort and add to open list
  156.             local pos = #openList
  157.             while pos > 0 and openList[pos].cost <= node.cost do
  158.                 pos = pos - 1
  159.             end
  160.             insert(openList,pos+1,node)
  161.           end        
  162.         end
  163.       end
  164.     end
  165.     checktime() -- Check yelding time for computerCraft
  166.   end
  167.  
  168.   return nil --no path found
  169. end
  170.  
  171. return {
  172.   findPath = AStarFindPath,
  173.   arr3d = arr3d,
  174. }
  175.  
  176. -- ********************************************************************************** --
  177. -- **                              Usage                                           ** --
  178. -- ********************************************************************************** --
  179.  
  180. --[[
  181. local pathfinder = require "pathfinder"
  182.  
  183. -- Create new world as 3d array
  184. local world = pathfinder.arr3d()
  185.  
  186. -- Block a cell, make it impassable. Indexes from [0]
  187. -- Indexes is [x][y][z]
  188. world:set(1,4,3,true)
  189.  
  190.  
  191. local p_start = {x=1,y=2,z=3} -- Start point.
  192. local p_end   = {x=1,y=6,z=3} -- End point
  193.  
  194. -- Main path find function
  195. -- Return the first bread crumb of path
  196. local crumb = pathfinder.findPath(world, p_start, p_end)
  197.  
  198. if crumb == nil then
  199.   print('Path not found')
  200. else
  201.   io.write('['.. crumb.pos.x..","..crumb.pos.y..","..crumb.pos.z.."]->")
  202.  
  203.   -- BreadCrumbs is connected list. To get next point in path use crumb.next
  204.   while crumb.next ~= nil do
  205.     crumb = crumb.next
  206.     io.write('['.. crumb.pos.x..","..crumb.pos.y..","..crumb.pos.z..(crumb.next and "]->" or "]"))
  207.   end
  208.  
  209.   print()
  210. end
  211. ]]--
RAW Paste Data