|
defmodule Astar do |
|
@start_pos {0, 0} |
|
@end_pos {4, 4} |
|
|
|
@maze [ |
|
[0, 0, 0, 0, 0], |
|
[0, 1, 0, 0, 0], |
|
[0, 1, 0, 0, 0], |
|
[0, 1, 0, 1, 0], |
|
[0, 0, 0, 0, 0] |
|
] |
|
|
|
def distance(nil, _), do: 0 |
|
|
|
def distance({x1, y1}, {x2, y2}) do |
|
abs(x1 - x2) + abs(y1 - y2) |
|
end |
|
|
|
def g(current_pos) do |
|
distance(current_pos, @start_pos) |
|
end |
|
|
|
def h(current_pos) do |
|
:math.pow(abs(elem(current_pos, 0) - elem(@end_pos, 0)), 2) + |
|
:math.pow(abs(elem(current_pos, 1) - elem(@end_pos, 1)), 2) |
|
end |
|
|
|
def f(%{pos: current_pos}) do |
|
g(current_pos) + h(current_pos) |
|
end |
|
|
|
def get_pos({x, y}), do: Enum.at(@maze, y) |> Enum.at(x) |
|
|
|
def valid_node(%{pos: {x, y}}) do |
|
x >= 0 && x < length(Enum.at(@maze, 0)) && y >= 0 && y < length(@maze) |
|
end |
|
|
|
def neighbours(%{pos: {x1, y1}}) do |
|
[{-1, -1}, {0, -1}, {1, -1}, {-1, 0}, {1, 0}, {-1, 1}, {0, 1}, {1, 1}] |
|
|> Enum.map(fn {x2, y2} -> %{pos: {x1 + x2, y1 + y2}, g: nil, h: nil, f: nil} end) |
|
|> Enum.filter(&Astar.valid_node/1) |
|
end |
|
|
|
def solve do |
|
open_list = [%{pos: @start_pos, g: 0, h: 0, f: 0}] |
|
|
|
closed_list = [] |
|
|
|
solve(open_list, closed_list) |
|
end |
|
|
|
def tap(a) do |
|
IO.puts("TAP ...") |
|
IO.inspect(a) |
|
|
|
a |
|
end |
|
|
|
def solve(open_list, closed_list) do |
|
case {open_list, closed_list} do |
|
{[], _} -> |
|
IO.puts("DONE - no path") |
|
|
|
{_, [%{pos: @end_pos} | _]} -> |
|
IO.puts("DONE - path!") |
|
IO.puts("PATH: ") |
|
|
|
closed_list |
|
|> Enum.reverse() |
|
|> Enum.map(& &1.pos) |
|
|> Enum.map(&"(#{elem(&1, 0)}, #{elem(&1, 1)})") |
|
|> Enum.join(" -> ") |
|
|> IO.puts() |
|
|
|
_ -> |
|
current_node = Enum.min_by(open_list, & &1.f) |
|
|
|
new_open_list = Enum.reject(open_list, &(&1.pos == current_node.pos)) |
|
updated_closed_list = [current_node | closed_list] |
|
|
|
IO.inspect(Enum.reverse(Enum.map(updated_closed_list, & &1.pos))) |
|
IO.puts("CURRENT POS #{elem(current_node.pos, 0)}-#{elem(current_node.pos, 1)}") |
|
|
|
IO.puts("NEIGHBOURS") |
|
IO.inspect(neighbours(current_node)) |
|
|
|
IO.puts("NEW OPEN LIST") |
|
IO.inspect(new_open_list) |
|
|
|
current_node_neighbours = |
|
neighbours(current_node) |
|
|> Enum.reject(fn neighbour -> |
|
Enum.member?(Enum.map(updated_closed_list, & &1.pos), neighbour.pos) |
|
end) |
|
|> tap |
|
|> Enum.reject(&(get_pos(&1.pos) == 1)) |
|
|> Enum.map( |
|
&%{&1 | g: current_node.g + distance(&1.pos, current_node.pos), h: h(&1.pos)} |
|
) |
|
|> Enum.map(&%{&1 | f: &1.g + &1.h}) |
|
|> Enum.map(fn neighbour -> |
|
case Enum.member?(Enum.map(open_list, & &1.pos), neighbour.pos) do |
|
true -> |
|
case neighbour.g > Enum.find(open_list, &(&1.pos == neighbour.pos)) do |
|
true -> |
|
nil |
|
|
|
false -> |
|
neighbour |
|
end |
|
|
|
false -> |
|
neighbour |
|
end |
|
end) |
|
|> Enum.reject(&(&1 == nil)) |
|
|> Enum.sort(fn a, b -> a.h < b.h end) |
|
|> tap |
|
|
|
solve(new_open_list ++ current_node_neighbours, updated_closed_list) |
|
end |
|
|
|
:ok |
|
end |
|
end |
|
|
|
Astar.solve() |