A* search algorithm

class AStarGraph {
 
    has barriers = [
        [2,4],[2,5],[2,6],[3,6],[4,6],[5,6],[5,5],[5,4],[5,3],[5,2],[4,2],[3,2]
    ]
 
    method heuristic(start, goal) {
        var (D1 = 1, D2 = 1)
        var dx = abs(start[0] - goal[0])
        var dy = abs(start[1] - goal[1])
        (D1 * (dx + dy)) + ((D2 - 2*D1) * Math.min(dx, dy))
    }
 
    method get_vertex_neighbours(pos) {
        gather {
            for dx, dy in [[1,0],[-1,0],[0,1],[0,-1],[1,1],[-1,1],[1,-1],[-1,-1]] {
                var x2 = (pos[0] + dx)
                var y2 = (pos[1] + dy)
                (x2<0 || x2>7 || y2<0 || y2>7) && next
                take([x2, y2])
            }
        }
    }
 
    method move_cost(_a, b) {
        barriers.contains(b) ? 100 : 1
    }
}
 
func AStarSearch(start, end, graph) {
 
    var G = Hash()
    var F = Hash()
 
    G{start} = 0
    F{start} = graph.heuristic(start, end)
 
    var closedVertices = []
    var openVertices = [start]
    var cameFrom = Hash()
 
    while (openVertices) {
 
        var current = nil
        var currentFscore = Inf
 
        for pos in openVertices {
            if (F{pos} < currentFscore) {
                currentFscore = F{pos}
                current = pos
            }
        }
 
        if (current == end) {
            var path = [current]
            while (cameFrom.contains(current)) {
                current = cameFrom{current}
                path << current
            }
            path.flip!
            return (path, F{end})
        }
 
        openVertices.remove(current)
        closedVertices.append(current)
 
        for neighbour in (graph.get_vertex_neighbours(current)) {
            if (closedVertices.contains(neighbour)) {
                next
            }
            var candidateG = (G{current} + graph.move_cost(current, neighbour))
 
            if (!openVertices.contains(neighbour)) {
                openVertices.append(neighbour)
            }
            elsif (candidateG >= G{neighbour}) {
                next
            }
 
            cameFrom{neighbour} = current
            G{neighbour} = candidateG
            var H = graph.heuristic(neighbour, end)
            F{neighbour} = (G{neighbour} + H)
        }
    }
 
    die "A* failed to find a solution"
}
 
var graph = AStarGraph()
var (route, cost) = AStarSearch([0,0], [7,7], graph)
 
var w = 10
var h = 10
 
var grid = h.of { w.of { "." } }
for y in (^h) { grid[y][0] = "█"; grid[y][-1] = "█" }
for x in (^w) { grid[0][x] = "█"; grid[-1][x] = "█" }
 
for x,y in (graph.barriers) { grid[x+1][y+1] = "█" }
for x,y in (route)          { grid[x+1][y+1] = "x" }
 
grid.each { .join.say }
 
say "Path cost #{cost}: #{route}"

Output:

██████████
█x.......█
█.x......█
█..x.███.█
█.x█...█.█
█.x█...█.█
█.x█████.█
█..xxxxx.█
█.......x█
██████████
Path cost 11: [[0, 0], [1, 1], [2, 2], [3, 1], [4, 1], [5, 1], [6, 2], [6, 3], [6, 4], [6, 5], [6, 6], [7, 7]]

Last updated