A-star velocity fails, example

a-star-velocity-demo.lua

local BinaryHeap

local PriorityQueue = setmetatable({
    empty = function(self)
        return self.length == 0
    end,
    insert = function(self, value, priority)
        assert(type(priority) == "number")
        self.length = self.length + 1
        table.insert(self, {value, priority})
    end,
    pop = function(self)
        self.length = self.length - 1
        table.sort(self, function(a, b) return a[2] > b[2] end)
        return unpack(table.remove(self))
    end
},
{__call = function(PriorityQueue)
    return setmetatable({length=0}, PriorityQueue)
end})

PriorityQueue.__index = PriorityQueue

local function reversed(t)
    local ret = {}
    local count = #t
    for i, v in ipairs(t) do
        ret[count + 1 - i] = v
    end
    return ret
end

function traverse(start, goal, opt)
    assert(opt)
    local satisfy = opt.satisfy or function(a, b) return a == b end
    local paths = opt.paths
    local cost = opt.cost
    local hash = opt.hash or tostring
    local heuristic = opt.heuristic or function(a, b)
        local score = 0
        for k, v in pairs(a) do
            local difference = a[k] - b[k]
            score = score + difference * difference
        end
        return math.sqrt(score)
    end
    local ending
    assert(paths)
    assert(cost)

    local frontier = PriorityQueue()
    frontier:insert(start, 0)
    local came_from = {
        [hash(start)] = nil
    }
    local cost_so_far = {
        [hash(start)] = 0
    }
    while not frontier:empty() do
        local current, priority = frontier:pop()
        if satisfy(current, goal) then
            ending = current
            break
        end
        for i, path in ipairs(paths(current)) do
            local new_cost = cost_so_far[hash(current)] + cost(current, path)
            local follow = path.transition(current)
            local follow_hash = hash(follow)

            if not cost_so_far[follow_hash] or 
                    new_cost < cost_so_far[follow_hash] then
                cost_so_far[follow_hash] = new_cost
                local priority = new_cost + heuristic(follow, goal)
                frontier:insert(follow, priority)
                came_from[hash(follow)] = {current, path}
            end
        end
    end
    local method = {}
    local current, path = ending
    while hash(current) ~= hash(start) do
        current, path = unpack(came_from[hash(current)])
        table.insert(method, path)
    end
    return reversed(method)
end

local function update(current, new)
    for k, v in pairs(current) do
        if new[k] == nil then
            new[k] = v
        end
    end
    return new
end


local edges = {
    accel = {
        transition = function (current) return
            update(current, {
                v = current.v + 1,
            })
        end,
        cost = function(current)
            return 0
        end
    },
    decel = {
        transition = function (current) return
            update(current, {
                v = current.v - 1,
            })
        end,
        cost = function(current)
            return 0
        end
    },
    wait = {
        -- filter = function(current) return current.v > 99 end,
        transition = function (current) return
            update(current, {
                x = current.x + current.v
            })
        end,
        cost = function(current)
            return 1
        end
    },
}

local start = {v=0, x=0}
local method = traverse(start, {v=0, x=100}, {
    hash=function(current)
        return current.v..","..current.x
    end,
    satisfy = function(current, goal)
        return current.v == goal.v and current.x == goal.x
    end,
    cost = function(current, path)
        if type(path.cost) == "number" then
            return path.cost
        end
        return path.cost(current)
    end,
    paths = function(current)
        local all = {}
        for name, edge in pairs(edges) do
            edge.name = name
            if not edge.filter or edge.filter(current) then
                table.insert(all, edge)
            end
        end
        table.sort(all, function(a, b)
            return a.name < b.name
        end)
        return all
    end
})