Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- function pathFind(Map) -- Map is a 2-dimensional matrix with uniform width and height (a rectangle)
- local height, width = 0, 0
- local start, finish, open, closed, order, current = {}, {}, {}, {}, {}, {}
- -- Format: open[x .. "-" .. y] = {{x, y}, ParentNode, fCost}
- for i, v in pairs(Map) do
- height = height + 1
- for j, k in pairs(v) do
- if height == 1 then
- width = width + 1
- end
- if k == 2 then
- start = {j, i}
- elseif k == 3 then
- finish = {j, i}
- end
- end
- end
- local function getPrCost(Node, ParentNode)
- if not ParentNode then return 0 end
- if Node[1] - ParentNode[1] == 0 or Node[2] - ParentNode[2] == 0 then
- return 10
- else
- return 14
- end
- end
- local function getSeCost(Node)
- return math.floor((((Node[1] - finish[1])^2 + (Node[2] - finish[2])^2)) ^ .5 * 10 + .5)
- end
- local function getSurroundingNodes(Node, ParentCost)
- local x, y = Node[1], Node[2]
- for i = -1, 1 do
- for j = -1, 1 do
- if x + i > width or y + j > height or x + i <= 0 or y + j <= 0 or (Map[y + j][x + i] and Map[y + j][x +i] == 1) or closed[x + i .. "-" .. y + j] then
- open[x + i .. "-" .. y + j] = nil
- closed[x + i .. "-" .. y + j] = true
- elseif open[x + i .. "-" .. y + j] then
- if ParentCost + getPrCost({x + i, y + j}, Node) < getPrCost({x + i, y + j}, open[x + i .. "-" .. y + j][2]) then
- open[x + i .. "-" .. y + j] = {{x + i, y + j}, Node, getPrCost({x + i, y + j}, Node) + getSeCost({x + i, y + j})}
- end
- else
- open[x + i .. "-" .. y + j] = {{x + i, y + j}, Node, getPrCost({x + i, y + j}, Node) + getSeCost({x + i, y + j})}
- end
- end
- end
- end
- current = start
- --open[start[1] .. "-" .. start[2]] = {{start[1], start[2]}, nil, 0}
- while (true) do
- if current[1] == start[1] and current[2] == start[2] then
- localCost = 0
- else
- local localCost = getPrCost(current, open[current[1] .. "-" .. current[2]][2])
- end
- open[current[1] .. "-" .. current[2]] = nil
- closed[current[1] .. "-" .. current[2]] = true
- if current[1] == finish[1] and current[2] == finish[2] then print("Search complete.") return order end
- getSurroundingNodes(current, localCost)
- local closest, lowest
- for i, v in pairs(open) do
- if not closest or v[3] < lowest then
- closest = v[1]
- lowest = v[3]
- end
- end
- if not closest then print("No path.") return {} end
- table.insert(order, {closest[1], closest[2]})
- current = closest
- end
- end
- local order = pathFind {
- {2, 0, 0},
- {0, 1, 0},
- {0, 0, 3}
- }
- for i, v in pairs(order) do
- print("{" .. v[1] .. ", " .. v[2] .. "}")
- end
Advertisement
Add Comment
Please, Sign In to add comment