astar.jl

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)