Select Git revision
Forked from
Anders Blomdell / LabComm
Source project has a limited visibility.
astar.jl 5.02 KiB
using DataStructures: heappop!, heappush!, isheap, heapify!
import Base: ==, .==, show, isequal, isless
using Base.Test
const Coord = Tuple{Int,Int}
abstract type AbstractNode end
struct StartNode <: AbstractNode end
struct GoalNode <: AbstractNode end
struct UnknownNode <: AbstractNode end
mutable struct MatrixNode <: AbstractNode
c::Coord
f::Float64
g::Float64
cost::Float64
parent::AbstractNode
end
show(io::IO,n::MatrixNode) = print(io,"MatrixNode(c=",n.c,", f,g=",round(n.f,3),",",round(n.g,3),")")
==(a::MatrixNode,b::MatrixNode) = a.c == b.c
.==(a::MatrixNode,b::MatrixNode) = a.c == b.c
isless(a::MatrixNode,b::MatrixNode) = isless(a.f, b.f)
distance(a,b) = b.cost + √((a.c[1]-b.c[1])^2 + (a.c[2]-b.c[2])^2)
function neighbors(current::MatrixNode,G)::Vector{MatrixNode}
N = size(G,1)
i,j = current.c
irange = max(1,i-1):min(N,i+1)
jrange = max(1,j-1):min(N,j+1)
nodes = G[irange,jrange] |> vec
return nodes
end
function reconstruct_path(current, path)
push!(path,current)
if current.parent == StartNode()
return reverse(path)
else
return reconstruct_path(current.parent, path)
end
end
reconstruct_path(current) = reconstruct_path(current, typeof(current)[])
# Generate graph (matrix)
function astar{T}(G::Matrix{T},startc,goalc)
time0 = tic()
start = G[startc...]
start.c = startc
start.f = heuristic(startc,goalc)
start.g = 0.
start.parent = StartNode()
goal = G[goalc...]
goal.c = goalc
goal.f = Inf
goal.g = Inf
goal.parent = GoalNode()
open = Vector{T}(); sizehint!(open,length(G)÷10)
closed = Set{T}(); sizehint!(closed,length(G)÷10)
heappush!(open,start)
while !isempty(open)
current = heappop!(open)
if current == goal
path = reconstruct_path(current)
# println("A* found the optimal path, length: $(length(path)), cost: ",path[end].g, " , time: ", toq())
return path
end
push!(closed, current)
for neighbor in neighbors(current,G)
if neighbor ∈ closed
continue
end
g_new = current.g + distance(current, neighbor)
if g_new >= neighbor.g
continue
end
neighbor.parent = current
neighbor.g = g_new
neighbor.f = neighbor.g + heuristic(neighbor.c, goal.c)
if neighbor ∉ open
heappush!(open, neighbor)
# @assert isheap(open)
else
heapify!(open) # This has to be done here since f was updated above.
# TODO: this can probably be changed for percolate_up!, since f can only be changed for the better
end
end
end
println("A* failed to find a feasible path :(")
return nothing
end
function assign_costs!{T<:AbstractNode}(G::AbstractMatrix{T},costs)
for (g,c) in zip(G,costs)
g.cost = c
end
end
function run_astar(N)
G = MatrixNode[MatrixNode((i,j),Inf,Inf,rand(1:50), UnknownNode()) for i = 1:N, j=1:N]
nf = 5
costs = conv2(Float64.(abs.(randn(N,N))), ones(nf,nf)/nf^2)[nf÷2:end-nf÷2,nf÷2:end-nf÷2]
costs = costs[1:N,1:N]
# costs[20:22,10:40] = 50
# for i = 2:N-1
# costs[i,i-1] *= 0.9
# end
# for i = N÷2:N-10
# costs[i,i+10] *= 0.8
# end
assign_costs!(G,costs)
startc = (1,1)
goalc = (N-1,N)
t = @elapsed path = astar(G,startc,goalc)
t
end
function test_and_plot(label)
t = run_astar(100)
Nvec = round.(Int, logspace(1, log10(400), 20) )
Nmat = Nvec
times = map(run_astar,Nmat)
Alog = Float64.(Nmat.^(0:1)')
Alog[:,2:end] = log.(Alog[:,2:end])
xlog = Alog\log.(times)
scatter!(Nmat,times, lab=label, yscale=:log10, xscale=:log10,
xlabel="Problem size", ylabel="Execution time [s]")
# plot!(Nmat, exp.(Alog*xlog), lab="Log-Model fit")
gui()
end
plot()
heuristic(a,b)::Float64 = √((a[1]-b[1])^2 + (a[2]-b[2])^2)
test_and_plot("L2")
heuristic(a,b)::Float64 = 0.5*√((a[1]-b[1])^2 + (a[2]-b[2])^2)
test_and_plot("0.5 L2")
heuristic(a,b)::Float64 = 0.1*√((a[1]-b[1])^2 + (a[2]-b[2])^2)
test_and_plot("0.1 L2")
heuristic(a,b)::Float64 = ((a[1]-b[1])^2 + (a[2]-b[2])^2)^(1/4)
test_and_plot("sqrt(L2)")
heuristic(a,b)::Float64 = log(abs(a[1]-b[1])) + log(abs(a[2]-b[2]))
test_and_plot("log-manhattan")
# costs = [n.cost for n in G]
# pathx = [n.c[1] for n in path]
# pathy = [n.c[2] for n in path]
# heatmap(log(costs))
# scatter!(pathy,pathx, c=:red)
# # scatter!(pathx,pathy, c=:blue)
# gui()
# Tests ========================================================================
# @test path[1].parent == StartNode()
# @test all(isa(node.parent, MatrixNode) for node in path[2:end])
# @test all(path[i].f <= path[i+1].f for i = 1:length(path)-1)
# @test all(path[i].g <= path[i+1].g for i = 1:length(path)-1)
# @test all(n.f >= n.g for n in path)
# @test all(abs(pathx[i] - pathx[i+1]) <= 1 for i = 1:length(pathx)-1)
# @test all(abs(pathy[i] - pathy[i+1]) <= 1 for i = 1:length(pathy)-1)