Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- -- ********************************************************************************** --
- -- ** ** --
- -- ** A-Star algorithm for 3d dimensional volume ** --
- -- ** ** --
- -- ** http://en.wikipedia.org/wiki/A*_search_algorithm ** --
- -- ** ---------------------------------------------------- ** --
- -- ** ** --
- -- ** Developed to use in program "KrutoyTurtle" ** --
- -- ** http://computercraft.ru/topic/48-stroitelnaia-sistema-krutoyturtle/ ** --
- -- ** ** --
- -- ********************************************************************************** --
- -- Redefine global function for faster acces
- local abs,pairs,floor,time,insert,remove,sleep,time = math.abs,pairs,math.floor,os.time,table.insert,table.remove,os.sleep,os.time
- -- Heuristic estimate.
- local function heuristic_cost_estimate(p1,p2)
- return abs(p1.x-p2.x) + abs(p1.y-p2.y) + abs(p1.z-p2.z)
- end
- -- ********************************************************************************** --
- -- ** Utils ** --
- -- ********************************************************************************** --
- -- 3d array metatable
- -- Use arr3d() as constructor, arr(x,y,z) as acces and arr:set(x,y,z,v) as set value
- if not arr3d then arr3d = function() return setmetatable({
- set = function(t,x,y,z,v)
- t[z] = t[z] or {}
- t[z][y] = t[z][y] or {}
- t[z][y][x] = v
- end,
- setVolume = function(t, x1,y1,z1,x2,y2,z2, v)
- for z=z1, z2 do
- for y=y1, y2 do
- for x=x1, x2 do
- t:set(x,y,z, v)
- end
- end
- end
- end
- }, { __call = function(t, x, y, z)
- if not t[z] or not t[z][y] then return nil end
- return t[z][y][x]
- end
- })end end
- -- Checking time for avoid yeld exception
- local chtime
- local function checktime()
- -- os.queueEvent("")
- -- coroutine.yield()
- if not chtime then chtime=time() end
- local ntime=time()
- if ntime-chtime>0.05 or ntime-chtime<0 then
- sleep(0)
- chtime=ntime
- end
- end
- -- ********************************************************************************** --
- -- ** BreadCrumb ** --
- -- ********************************************************************************** --
- local BreadCrumb = {}
- BreadCrumb.__index = BreadCrumb
- function BreadCrumb.new(x,y,z, parent)
- local self = setmetatable({}, BreadCrumb)
- self.pos = {x=x,y=y,z=z}
- self.next = parent
- self.cost = math.huge
- self.onClosedList = false
- self.onOpenList = false
- return self
- end
- function BreadCrumb:Equals(b)
- return b.pos.x == self.pos.x and b.pos.y == self.pos.y and b.pos.z == self.pos.z
- end
- -- ********************************************************************************** --
- -- ** Path Finder ** --
- -- ********************************************************************************** --
- -- Neigtbours of current point
- local surrounding = {
- {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},
- }
- -- Method that switfly finds the best path from p_start to end.
- -- The starting breadcrumb traversable via .next to the end or nil if there is no path
- local function AStarFindPath(world, p_start, p_end)
- -- note we just flip p_start and end here so you don't have to.
- p_end, p_start = p_start, p_end
- -- Destination point is bloked
- if world(p_start.x, p_start.y, p_start.z) then return end
- if p_start.x == p_end.x and p_start.y == p_end.y and p_start.z == p_end.z then
- return { p_start }
- end
- local openList = {}
- local brWorld = arr3d()
- local current= BreadCrumb.new(p_start.x,p_start.y,p_start.z)
- current.cost = heuristic_cost_estimate(current.pos,p_end)
- local finish = BreadCrumb.new(p_end.x,p_end.y,p_end.z)
- brWorld:set(current.pos.x,current.pos.y,current.pos.z,current)
- insert(openList,current)
- while #openList > 0 do
- --Find best item and switch it to the 'closedList'
- current = remove(openList)
- current.onClosedList = true
- --Find neighbours
- for k,v in pairs(surrounding) do
- local tmpX,tmpY,tmpZ = current.pos.x + v.x, current.pos.y + v.y, current.pos.z + v.z
- if not world(tmpX,tmpY,tmpZ) then
- --Check if we've already examined a neighbour, if not create a new node for it.
- local node
- if brWorld(tmpX,tmpY,tmpZ) == nil then
- node = BreadCrumb.new(tmpX,tmpY,tmpZ)
- brWorld:set(tmpX,tmpY,tmpZ,node)
- else
- node = brWorld(tmpX,tmpY,tmpZ)
- end
- --If the node is not on the 'closedList' check it's new score, keep the best
- if node.onClosedList == false then
- local cost = heuristic_cost_estimate(node.pos,p_end)
- if cost < node.cost then
- node.cost = cost
- node.next = current
- end
- --If the node wasn't on the openList yet, add it
- if node.onOpenList == false then
- --Check to see if we're done
- if node:Equals(finish) == true then
- node.next = current
- return node
- end
- node.onOpenList = true
- -- Sort and add to open list
- local pos = #openList
- while pos > 0 and openList[pos].cost <= node.cost do
- pos = pos - 1
- end
- insert(openList,pos+1,node)
- end
- end
- end
- end
- checktime() -- Check yelding time for computerCraft
- end
- return nil --no path found
- end
- return {
- findPath = AStarFindPath,
- arr3d = arr3d,
- }
- -- ********************************************************************************** --
- -- ** Usage ** --
- -- ********************************************************************************** --
- --[[
- local pathfinder = require "pathfinder"
- -- Create new world as 3d array
- local world = pathfinder.arr3d()
- -- Block a cell, make it impassable. Indexes from [0]
- -- Indexes is [x][y][z]
- world:set(1,4,3,true)
- local p_start = {x=1,y=2,z=3} -- Start point.
- local p_end = {x=1,y=6,z=3} -- End point
- -- Main path find function
- -- Return the first bread crumb of path
- local crumb = pathfinder.findPath(world, p_start, p_end)
- if crumb == nil then
- print('Path not found')
- else
- io.write('['.. crumb.pos.x..","..crumb.pos.y..","..crumb.pos.z.."]->")
- -- BreadCrumbs is connected list. To get next point in path use crumb.next
- while crumb.next ~= nil do
- crumb = crumb.next
- io.write('['.. crumb.pos.x..","..crumb.pos.y..","..crumb.pos.z..(crumb.next and "]->" or "]"))
- end
- print()
- end
- ]]--
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement