A* search algorithm: Difference between revisions
m (changed a related tasks name (Knight's tour).) |
m (added a link (;See also:) to a Wikipedia article, added a silent HTML comment for searches for "A star", added whitespace around barrier coördinates.) |
||
Line 1: | Line 1: | ||
{{draft task|Routing algorithms}} |
|||
⚫ | |||
<!-- A* is pronounced as: A star !--> |
|||
⚫ | |||
;Task |
;Task |
||
⚫ | Consider the problem of finding a route across the diagonal of a chess board-like 8x8 grid. The rows are numbered from 0 to 7. The columns are also numbered 0 to 7. The start position is (0, 0) and the end position is (7, 7). Movement is allow by one square in any direction including diagonals, similar to a king in chess. The standard movement cost is 1. To make things slightly harder, there is a barrier that occupy certain positions of the grid. Moving into any of the barrier positions has a cost of 100. |
||
The barrier occupies the positions (2,4), (2,5), (2,6), (3,6), (4,6), (5,6), (5,5), (5,4), (5,3), (5,2), (4,2) and (3,2). |
|||
⚫ | Consider the problem of finding a route across the diagonal of a chess board-like 8x8 grid. The rows are numbered from 0 to 7. The columns are also numbered 0 to 7. The start position is (0, 0) and the end position is (7, 7). Movement is allow by one square in any direction including diagonals, similar to a king in chess. The standard movement cost is 1. To make things slightly harder, there is a barrier that occupy certain positions of the grid. Moving into any of the barrier positions has a cost of 100. |
||
A route with the lowest cost should be found using the A* search algorithm (there are multiple optimal solutions with the same total cost). |
|||
Print the optimal route in text format, as well as the total cost of the route. |
Print the optimal route in text format, as well as the total cost of the route. |
||
Line 10: | Line 17: | ||
Note: using a heuristic score of zero is equivalent to Dijkstra's algorithm and that's kind of cheating/not really A*! |
Note: using a heuristic score of zero is equivalent to Dijkstra's algorithm and that's kind of cheating/not really A*! |
||
;See also: |
|||
* Wikipedia webpage: [https://en.wikipedia.org/wiki/A*_search_algorithm A* search algorithm]. |
|||
Revision as of 18:44, 29 January 2017
The A* search algorithm is an extension of Dijkstra's algorithm for route finding that uses heuristics to quickly find an approximate solution.
- Task
Consider the problem of finding a route across the diagonal of a chess board-like 8x8 grid. The rows are numbered from 0 to 7. The columns are also numbered 0 to 7. The start position is (0, 0) and the end position is (7, 7). Movement is allow by one square in any direction including diagonals, similar to a king in chess. The standard movement cost is 1. To make things slightly harder, there is a barrier that occupy certain positions of the grid. Moving into any of the barrier positions has a cost of 100.
The barrier occupies the positions (2,4), (2,5), (2,6), (3,6), (4,6), (5,6), (5,5), (5,4), (5,3), (5,2), (4,2) and (3,2).
A route with the lowest cost should be found using the A* search algorithm (there are multiple optimal solutions with the same total cost).
Print the optimal route in text format, as well as the total cost of the route.
Optionally, draw the optimal route and the barrier positions.
Note: using a heuristic score of zero is equivalent to Dijkstra's algorithm and that's kind of cheating/not really A*!
- See also
- Wikipedia webpage: A* search algorithm.
- Related tasks
- Knight's tour
- N-queens problem
- Solve a Hidato puzzle
- Solve a Holy Knight's tour
- Solve a Hopido puzzle
- Solve a Numbrix puzzle
- Solve the no connection puzzle
C++
<lang cpp>
- include <list>
- include <algorithm>
- include <iostream>
class point { public:
point( int a = 0, int b = 0 ) { x = a; y = b; } bool operator ==( const point& o ) { return o.x == x && o.y == y; } point operator +( const point& o ) { return point( o.x + x, o.y + y ); } int x, y;
};
class map { public:
map() { char t[8][8] = { {0, 0, 0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 1, 1, 1, 0}, {0, 0, 1, 0, 0, 0, 1, 0}, {0, 0, 1, 0, 0, 0, 1, 0}, {0, 0, 1, 1, 1, 1, 1, 0}, {0, 0, 0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0, 0, 0} }; w = h = 8; for( int r = 0; r < h; r++ ) for( int s = 0; s < w; s++ ) m[s][r] = t[r][s]; } int operator() ( int x, int y ) { return m[x][y]; } char m[8][8]; int w, h;
};
class node { public:
bool operator == (const node& o ) { return pos == o.pos; } bool operator == (const point& o ) { return pos == o; } bool operator < (const node& o ) { return dist + cost < o.dist + o.cost; } point pos, parent; int dist, cost;
};
class aStar { public:
aStar() { neighbours[0] = point( -1, -1 ); neighbours[1] = point( 1, -1 ); neighbours[2] = point( -1, 1 ); neighbours[3] = point( 1, 1 ); neighbours[4] = point( 0, -1 ); neighbours[5] = point( -1, 0 ); neighbours[6] = point( 0, 1 ); neighbours[7] = point( 1, 0 ); } int calcDist( point& p ){ // need a better heuristic int x = end.x - p.x, y = end.y - p.y; return( x * x + y * y ); } bool isValid( point& p ) { return ( p.x >-1 && p.y > -1 && p.x < m.w && p.y < m.h ); } bool existPoint( point& p, int cost ) { std::list<node>::iterator i; i = std::find( closed.begin(), closed.end(), p ); if( i != closed.end() ) { if( ( *i ).cost + ( *i ).dist < cost ) return true; else { closed.erase( i ); return false; } } i = std::find( open.begin(), open.end(), p ); if( i != open.end() ) { if( ( *i ).cost + ( *i ).dist < cost ) return true; else { open.erase( i ); return false; } } return false; } bool fillOpen( node& n ) { int stepCost, nc, dist; point neighbour;
for( int x = 0; x < 8; x++ ) { // one can make diagonals have different cost stepCost = x < 4 ? 1 : 1; neighbour = n.pos + neighbours[x]; if( neighbour == end ) return true; if( isValid( neighbour ) && m( neighbour.x, neighbour.y ) != 1 ) { nc = stepCost + n.cost; dist = calcDist( neighbour ); if( !existPoint( neighbour, nc + dist ) ) { node m; m.cost = nc; m.dist = dist; m.pos = neighbour; m.parent = n.pos; open.push_back( m ); } } } return false; } bool search( point& s, point& e, map& mp ) { node n; end = e; start = s; m = mp; n.cost = 0; n.pos = s; n.parent = 0; n.dist = calcDist( s ); open.push_back( n ); while( !open.empty() ) { //open.sort(); node n = open.front(); open.pop_front(); closed.push_back( n ); if( fillOpen( n ) ) return true; } return false; } int path( std::list<point>& path ) { path.push_front( end ); int cost = 1 + closed.back().cost; path.push_front( closed.back().pos ); point parent = closed.back().parent; for( std::list<node>::reverse_iterator i = closed.rbegin(); i != closed.rend(); i++ ) { if( ( *i ).pos == parent && !( ( *i ).pos == start ) ) { path.push_front( ( *i ).pos ); parent = ( *i ).parent; } } path.push_front( start ); return cost; } map m; point end, start; point neighbours[8]; std::list<node> open; std::list<node> closed;
};
int main( int argc, char* argv[] ) {
map m; point s, e( 7, 7 ); aStar as; if( as.search( s, e, m ) ) { std::list<point> path; int c = as.path( path ); for( int y = -1; y < 9; y++ ) { for( int x = -1; x < 9; x++ ) { if( x < 0 || y < 0 || x > 7 || y > 7 || m( x, y ) == 1 ) std::cout << char(0xdb); else { if( std::find( path.begin(), path.end(), point( x, y ) )!= path.end() ) std::cout << "x"; else std::cout << "."; } } std::cout << "\n"; } std::cout << "\nPath cost " << c << ": "; for( std::list<point>::iterator i = path.begin(); i != path.end(); i++ ) { std::cout<< "(" << ( *i ).x << ", " << ( *i ).y << ") "; } } std::cout << "\n\n"; return 0;
} </lang>
- Output:
██████████ █x.......█ █x.......█ █x...███.█ █x.█...█.█ █x.█...█.█ █.x█████.█ █..xxxx..█ █......xx█ ██████████ Path cost 11: (0, 0) (0, 1) (0, 2) (0, 3) (0, 4) (1, 5) (2, 6) (3, 6) (4, 6) (5, 6) (6, 7) (7, 7)
Racket
This code is lifted from: this blog post. Read it, it's very good.
<lang racket>#lang scribble/lp @(chunk
<graph-sig> (define-signature graph^ (node? edge? node-edges edge-src edge-cost edge-dest)))
@(chunk
<map-generation> (define (make-map N) ;; Jay's random algorithm ;; (build-matrix N N (λ (x y) (random 3))) ;; RC version (matrix [[0 0 0 0 0 0 0 0] [0 0 0 0 0 0 0 0] [0 0 0 0 1 1 1 0] [0 0 1 0 0 0 1 0] [0 0 1 0 0 0 1 0] [0 0 1 1 1 1 1 0] [0 0 0 0 0 0 0 0] [0 0 0 0 0 0 0 0]])))
@(chunk
<map-graph-rep> (struct map-node (M x y) #:transparent) (struct map-edge (src dx dy dest)))
@(chunk
<map-graph-cost> (define (edge-cost e) (match-define (map-edge _ _ _ (map-node M x y)) e) (match (matrix-ref M x y) [0 1] [1 100] [2 1000])))
@(chunk
<map-graph-edges> (define (node-edges n) (match-define (map-node M x y) n) (append* (for*/list ([dx (in-list '(1 0 -1))] [dy (in-list '(1 0 -1))] #:when (and (not (and (zero? dx) (zero? dy))) ;; RC -- allowed to move diagonally, so not this clause ;;(or (zero? dx) (zero? dy)) )) (cond [(and (<= 0 (+ dx x) (sub1 (matrix-num-cols M))) (<= 0 (+ dy y) (sub1 (matrix-num-rows M)))) (define dest (map-node M (+ dx x) (+ dy y))) (list (map-edge n dx dy dest))] [else empty])))))
@(chunk
<a-star> (define (A* graph@ initial node-cost) (define-values/invoke-unit graph@ (import) (export graph^)) (define count 0) <a-star-setup> (begin0 (let/ec esc <a-star-loop> #f) (printf "visited ~a nodes\n" count))))
@(chunk
<a-star-setup> <a-star-setup-closed> <a-star-setup-open>)
@(chunk
<a-star-setup-closed> (define node->best-path (make-hash)) (define node->best-path-cost (make-hash)) (hash-set! node->best-path initial empty) (hash-set! node->best-path-cost initial 0))
@(chunk
<a-star-setup-open> (define (node-total-estimate-cost n) (+ (node-cost n) (hash-ref node->best-path-cost n))) (define (node-cmp x y) (<= (node-total-estimate-cost x) (node-total-estimate-cost y))) (define open-set (make-heap node-cmp)) (heap-add! open-set initial))
@(chunk
<a-star-loop> (for ([x (in-heap/consume! open-set)]) (set! count (add1 count)) <a-star-loop-body>))
@(chunk
<a-star-loop-stop?> (define h-x (node-cost x)) (define path-x (hash-ref node->best-path x)) (when (zero? h-x) (esc (reverse path-x))))
@(chunk
<a-star-loop-body> <a-star-loop-stop?> (define g-x (hash-ref node->best-path-cost x)) (for ([x->y (in-list (node-edges x))]) (define y (edge-dest x->y)) <a-star-loop-per-neighbor>))
@(chunk
<a-star-loop-per-neighbor> (define new-g-y (+ g-x (edge-cost x->y))) (define old-g-y (hash-ref node->best-path-cost y +inf.0)) (when (< new-g-y old-g-y) (hash-set! node->best-path-cost y new-g-y) (hash-set! node->best-path y (cons x->y path-x)) (heap-add! open-set y)))
@(chunk
<map-display> (define map-scale 15) (define (type-color ty) (match ty [0 "yellow"] [1 "green"] [2 "red"])) (define (cell-square ty) (square map-scale "solid" (type-color ty))) (define (row-image M row) (apply beside (for/list ([col (in-range (matrix-num-cols M))]) (cell-square (matrix-ref M row col))))) (define (map-image M) (apply above (for/list ([row (in-range (matrix-num-rows M))]) (row-image M row)))))
@(chunk
<path-display-line> (define (edge-image-on e i) (match-define (map-edge (map-node _ sx sy) _ _ (map-node _ dx dy)) e) (add-line i (* (+ sy 0.5) map-scale) (* (+ sx 0.5) map-scale) (* (+ dy 0.5) map-scale) (* (+ dx 0.5) map-scale) "black")))
@(chunk
<path-display> (define (path-image M path) (foldr edge-image-on (map-image M) path)))
@(chunk
<map-graph> (define-unit map@ (import) (export graph^) (define node? map-node?) (define edge? map-edge?) (define edge-src map-edge-src) (define edge-dest map-edge-dest) <map-graph-cost> <map-graph-edges>))
@(chunk
<map-node-cost> (define ((make-node-cost GX GY) n) (match-define (map-node M x y) n) ;; Jay's #;(+ (abs (- x GX)) (abs (- y GY))) ;; RC -- diagonal movement (max (abs (- x GX)) (abs (- y GY)))))
@(chunk
<map-example> (define N 8) (define random-M (make-map N)) (define random-path (time (A* map@ (map-node random-M 0 0) (make-node-cost (sub1 N) (sub1 N))))))
@(chunk
<*> (require rackunit math/matrix racket/unit racket/match racket/list data/heap 2htdp/image racket/runtime-path) <graph-sig> <map-generation> <map-graph-rep> <map-graph> <a-star> <map-node-cost> <map-example> (printf "path is ~a long\n" (length random-path)) (printf "path is: ~a\n" (map (match-lambda [(map-edge src dx dy dest) (cons dx dy)]) random-path)) <map-display> <path-display-line> <path-display>
(path-image random-M random-path))</lang>
- Output:
visited 35 nodes cpu time: 94 real time: 97 gc time: 15 path is 11 long path is: ((1 . 1) (1 . 1) (1 . -1) (1 . 0) (1 . 0) (1 . 1) (1 . 1) (0 . 1) (-1 . 1) (1 . 1) (0 . 1)) .
A diagram is also output, but you'll need to run this in DrRacket to see it.
Python
<lang python>from __future__ import print_function import matplotlib.pyplot as plt
class AStarGraph(object): #Define a class board like grid with two barriers
def __init__(self): self.barriers = [] self.barriers.append([(2,4),(2,5),(2,6),(3,6),(4,6),(5,6),(5,5),(5,4),(5,3),(5,2),(4,2),(3,2)])
def heuristic(self, start, goal): #Use Chebyshev distance heuristic if we can move one square either #adjacent or diagonal D = 1 D2 = 1 dx = abs(start[0] - goal[0]) dy = abs(start[1] - goal[1]) return D * (dx + dy) + (D2 - 2 * D) * min(dx, dy)
def get_vertex_neighbours(self, pos): n = [] #Moves allow link a chess king for dx, dy in [(1,0),(-1,0),(0,1),(0,-1),(1,1),(-1,1),(1,-1),(-1,-1)]: x2 = pos[0] + dx y2 = pos[1] + dy if x2 < 0 or x2 > 7 or y2 < 0 or y2 > 7: continue n.append((x2, y2)) return n
def move_cost(self, a, b): for barrier in self.barriers: if b in barrier: return 100 #Extremely high cost to enter barrier squares return 1 #Normal movement cost
def AStarSearch(start, end, graph):
G = {} #Actual movement cost to each position from the start position F = {} #Estimated movement cost of start to end going via this position
#Initialize starting values G[start] = 0 F[start] = graph.heuristic(start, end)
closedVertices = set() openVertices = set([start]) cameFrom = {}
while len(openVertices) > 0: #Get the vertex in the open list with the lowest F score current = None currentFscore = None for pos in openVertices: if current is None or F[pos] < currentFscore: currentFscore = F[pos] current = pos
#Check if we have reached the goal if current == end: #Retrace our route backward path = [current] while current in cameFrom: current = cameFrom[current] path.append(current) path.reverse() return path, F[end] #Done!
#Mark the current vertex as closed openVertices.remove(current) closedVertices.add(current)
#Update scores for vertices near the current position for neighbour in graph.get_vertex_neighbours(current): if neighbour in closedVertices: continue #We have already processed this node exhaustively candidateG = G[current] + graph.move_cost(current, neighbour)
if neighbour not in openVertices: openVertices.add(neighbour) #Discovered a new vertex elif candidateG >= G[neighbour]: continue #This G score is worse than previously found
#Adopt this G score cameFrom[neighbour] = current G[neighbour] = candidateG H = graph.heuristic(neighbour, end) F[neighbour] = G[neighbour] + H
raise RuntimeError("A* failed to find a solution")
if __name__=="__main__": graph = AStarGraph() result, cost = AStarSearch((0,0), (7,7), graph) print ("route", result) print ("cost", cost) plt.plot([v[0] for v in result], [v[1] for v in result]) for barrier in graph.barriers: plt.plot([v[0] for v in barrier], [v[1] for v in barrier]) plt.xlim(-1,8) plt.ylim(-1,8) plt.show()</lang>
- Output:
route [(0, 0), (1, 1), (2, 2), (3, 1), (4, 1), (5, 1), (6, 2), (7, 3), (6, 4), (7, 5), (6, 6), (7, 7)] cost 11
Sidef
<lang ruby>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}"</lang>
- 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]]
zkl
<lang zkl> // we use strings as hash keys: (x,y)-->"x,y", keys are a single pair fcn toKey(xy){ xy.concat(",") }
fcn AStarSearch(start,end,graph){
G:=Dictionary(); # Actual movement cost to each position from the start position F:=Dictionary(); # Estimated movement cost of start to end going via this position #Initialize starting values kstart:=toKey(start); G[kstart]=0; F[kstart]=graph.heuristic(start,end); closedVertices,openVertices,cameFrom := List(),List(start),Dictionary(); while(openVertices){ # Get the vertex in the open list with the lowest F score current,currentFscore := Void, Void; foreach pos in (openVertices){ kpos:=toKey(pos); if(current==Void or F[kpos]<currentFscore)
currentFscore,current = F[kpos],pos;
# Check if we have reached the goal if(current==end){ # Yes! Retrace our route backward path,kcurrent := List(current),toKey(current); while(current = cameFrom.find(kcurrent)){ path.append(current); kcurrent=toKey(current); } return(path.reverse(),F[toKey(end)]) # Done! }
# Mark the current vertex as closed openVertices.remove(current); if(not closedVertices.holds(current)) closedVertices.append(current);
# Update scores for vertices near the current position foreach neighbor in (graph.get_vertex_neighbors(current)){ if(closedVertices.holds(neighbor)) continue; # We have already processed this node exhaustively kneighbor:=toKey(neighbor); candidateG:=G[toKey(current)] + graph.move_cost(current, neighbor);
if(not openVertices.holds(neighbor)) openVertices.append(neighbor); # Discovered a new vertex else if(candidateG>=G[kneighbor]) continue; # This G score is worse than previously found
# Adopt this G score cameFrom[kneighbor]=current; G[kneighbor]=candidateG; F[kneighbor]=G[kneighbor] + graph.heuristic(neighbor,end); }
} } // while throw(Exception.AssertionError("A* failed to find a solution"));
}</lang> <lang zkl>class [static] AStarGraph{ # Define a class board like grid with barriers
var [const] barriers = T( T(3,2),T(4,2),T(5,2), // T is RO List
T(5,3), T(2,4), T(5,4), T(2,5), T(5,5), T(2,6),T(3,6),T(4,6),T(5,6) );
fcn heuristic(start,goal){ // (x,y),(x,y) # Use Chebyshev distance heuristic if we can move one square either # adjacent or diagonal D,D2,dx,dy := 1,1, (start[0] - goal[0]).abs(), (start[1] - goal[1]).abs(); D*(dx + dy) + (D2 - 2*D)*dx.min(dy); } fcn get_vertex_neighbors([(x,y)]){ # Move like a chess king var moves=Walker.cproduct([-1..1],[-1..1]).walk(); // 8 moves + (0,0) moves.pump(List,'wrap([(dx,dy)]){
x2,y2 := x + dx, y + dy; if((dx==dy==0) or x2 < 0 or x2 > 7 or y2 < 0 or y2 > 7) Void.Skip; else T(x2,y2);
}) } fcn move_cost(a,b){ // ( (x,y),(x,y) ) if(barriers.holds(b))
return(100); # Extremely high cost to enter barrier squares
1 # Normal movement cost }
}</lang> <lang zkl>graph:=AStarGraph; route,cost := AStarSearch(T(0,0), T(7,7), graph); println("Route: ", route.apply(fcn(xy){ String("(",toKey(xy),")") }).concat(",")); println("Cost: ", cost);
// graph the solution:
grid:=(10).pump(List,List.createLong(10," ").copy); foreach x,y in (graph.barriers){ grid[x][y]="#" } foreach x,y in (route){ grid[x][y]="+" } grid[0][0] = "S"; grid[7][7] = "E"; foreach line in (grid){ println(line.concat()) }</lang>
- Output:
Route: (0,0),(1,1),(2,2),(3,1),(4,0),(5,1),(6,2),(7,3),(7,4),(7,5),(7,6),(7,7) Cost: 11 S + + ### +# # + # # +##### + ++++E