 # aStar

Sep 5th, 2015
328
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
1. if not 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)
43.     end
44.     current = goalStart
45.     while not vectorEquals(current, goal) do
46.         table.insert(path, 1, current)
47.         current = nodes:get(current)
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)
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
124.
125.         currNode, currNode, 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 + 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 then -- nbrNode has been discovered in the opposite search direction
141.                         if nbrNode 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 then
145.                         if nbrNode then
146.                             activeOpenSet:remove(neighbour, vectorEquals)
147.                             nbrNode = false
148.                         end
149.                         nbrNode = false
150.                     end
151.                 end
152.
153.                 if (newNode or (forwardSearch ~= nbrNode and not nbrNode and not nbrNode)) and newCost < math.huge then
154.                     nbrNode = current
155.                     nbrNode = newCost
156.                     nbrNode = currNode
157.                     nbrNode = 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