Skip to content
Snippets Groups Projects
Select Git revision
  • 713dd66b832b1f001c7eaa76ad9b1a3fbbe1e380
  • master default
  • labcomm2006
  • typedefs
  • anders.blomdell
  • typeref
  • pragma
  • compiler-refactoring
  • labcomm2013
  • v2014.4
  • v2006.0
  • v2014.3
  • v2014.2
  • v2014.1
  • v2014.0
  • v2013.0
16 results

run

Blame
  • 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)