sidef/scripts/RosettaCode/A_star_algorithm.sf

#!/usr/bin/ruby

#
## https://rosettacode.org/wiki/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}"

assert_eq(route, [[0, 0], [1, 1], [2, 2], [3, 1], [4, 1], [5, 1], [6, 2], [6, 3], [6, 4], [6, 5], [6, 6], [7, 7]])