A* search algorithm: Difference between revisions

Line 1:

The A* search algorithm is an extension of [[Dijkstra's algorithm]] useful for finding the lowest cost path between two nodes (aka vertices) of a graph. The path may traverse any number of nodes connected by edges (aka arcs) with each edge having an associated cost. The algorithm uses a heuristic which associates an estimate of the lowest cost path from this node to the goal node, such that this estimate is never greater than the actual cost.

The algorithm should not assume that all edge costs are the same. It should be possible to start and finish on any node, including ones identified as a barrier in the 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).

A route with the lowest cost should be found using the A* search algorithm (there are multiple optimal solutions with the same total cost).

Line 339:

}

// Class Cell, with the cost to reach it, the values g and f, and the coordinates

// of the cell that precedes it in a possible path

public class Cell

Line 399:

if (newCell.row == finishCell.row && newCell.col == finishCell.col)

{

cells[newCell.row, newCell.col].g = cells[currentCell.row,

currentCell.col].g + cells[newCell.row, newCell.col].cost;

cells[newCell.row, newCell.col].parent.row = currentCell.row;

Line 410:

else if (!opened.Contains(newCell) && !closed.Contains(newCell))

{

cells[newCell.row, newCell.col].g = cells[currentCell.row,

currentCell.col].g + cells[newCell.row, newCell.col].cost;

cells[newCell.row, newCell.col].f =

cells[newCell.row, newCell.col].g + Heuristic(newCell);

cells[newCell.row, newCell.col].parent.row = currentCell.row;

Line 419:

}

// If the cost to reach the considered cell from the actual one is

// smaller than the previous one

else if (cells[newCell.row, newCell.col].g > cells[currentCell.row,

currentCell.col].g + cells[newCell.row, newCell.col].cost)

{

cells[newCell.row, newCell.col].g = cells[currentCell.row,

currentCell.col].g + cells[newCell.row, newCell.col].cost;

cells[newCell.row, newCell.col].f =

cells[newCell.row, newCell.col].g + Heuristic(newCell);

cells[newCell.row, newCell.col].parent.row = currentCell.row;

Line 483:

}

// It select the cell between those in the list opened that have the smaller

// value of f

public Coordinates ShorterExpectedPath()

Line 492:

for (int i = 1; i < opened.Count; i++)

{

if (cells[opened[i].row, opened[i].col].f < cells[opened[sep].row,

opened[sep].col].f)

{

Line 508:

for (int i = -1; i <= 1; i++)

for (int j = -1; j <= 1; j++)

if (c.row+i >= 0 && c.row+i < 8 && c.col+j >= 0 && c.col+j < 8 &&

(i != 0 || j != 0))

{

Line 519:

public bool IsAWall(int row, int col)

{

int[,] walls = new int[,] { { 2, 4 }, { 2, 5 }, { 2, 6 }, { 3, 6 }, { 4, 6 },

{ 5, 6 }, { 5, 5 }, { 5, 4 }, { 5, 3 }, { 5, 2 }, { 4, 2 }, { 3, 2 } };

bool found = false;

Line 584:

#include <algorithm>

#include <iostream>

class point {

public:

Line 592:

int x, y;

};

class map {

public:

Line 611:

int w, h;

};

class node {

public:

Line 620:

int dist, cost;

};

class aStar {

public:

Line 629:

neighbours[6] = point( 0, 1 ); neighbours[7] = point( 1, 0 );

}

int calcDist( point& p ){

// need a better heuristic

Line 635:

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;

Line 654:

return false;

}

bool fillOpen( node& n ) {

int stepCost, nc, dist;

Line 664:

neighbour = n.pos + neighbours[x];

if( neighbour == end ) return true;

if( isValid( neighbour ) && m( neighbour.x, neighbour.y ) != 1 ) {

nc = stepCost + n.cost;

Line 671:

node m;

m.cost = nc; m.dist = dist;

m.pos = neighbour;

m.parent = n.pos;

open.push_back( m );

Line 679:

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() ) {

Line 693:

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 ) ) {

Line 709:

return cost;

}

map m; point end, start;

point neighbours[8];

Line 715:

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;

Line 736:

std::cout << "\n";

}

std::cout << "\nPath cost " << c << ": ";

for( std::list<point>::iterator i = path.begin(); i != path.end(); i++ ) {

Line 898:

;; Output some information each counter or nothing if information

;; equals 0.

(when (and (not (zerop information))

(zerop (mod counter information)))

(format t "~Dth Node, heap size: ~D, current costs: ~D~%"

counter (heap-count queue)

(node-cost current-node)))

;; If the target is not reached continue

(until (equalp current-position goal))

Line 946:

import std.range;

import std.array;

struct Point {

int x;

Line 963:

];

}

struct Node {

Point pos;

Line 987:

return( x * x + y * y );

}

bool isValid(Point b) {

return ( b.x >-1 && b.y > -1 && b.x < m.w && b.y < m.h );

}

bool existPoint(Point b, int cost) {

auto i = closed.countUntil(b);

Line 1,005:

return false;

}

bool fillOpen( ref Node n ) {

int stepCost;

Line 1,011:

int dist;

Point neighbour;

for( int x = 0; x < 8; ++x ) {

// one can make diagonals have different cost

Line 1,017:

neighbour = n.pos + neighbours[x];

if( neighbour == end ) return true;

if( isValid( neighbour ) && m.m[neighbour.y][neighbour.x] != 1 ) {

nc = stepCost + n.cost;

Line 1,024:

Node m;

m.cost = nc; m.dist = dist;

m.pos = neighbour;

m.parent = n.pos;

open ~= m;

Line 1,032:

return false;

}

bool search( ref Point s, ref Point e, ref Map mp ) {

Node n; end = e; start = s; m = mp;

n.cost = 0;

n.pos = s;

n.parent = Point();

n.dist = calcDist( s );

open ~= n ;

while( !open.empty() ) {

Line 1,049:

return false;

}

int path( ref Point[] path ) {

path = end ~ path;

int cost = 1 + closed.back().cost;

path = closed.back().pos ~ path;

Point parent = closed.back().parent;

foreach(ref i ; closed.retro) {

if( i.pos == parent && !( i.pos == start ) ) {

Line 1,066:

}

};

int main(string[] argv) {

Map m;

Line 1,072:

Point e = Point( 7, 7 );

AStar as;

if( as.search( s, e, m ) ) {

Point[] path;

Line 1,088:

writeln();

}

write("\nPath cost ", c, ": ");

foreach( i; path ) {

Line 1,334:

aCell.row = nCell.row

loop

'Drawing the path

for i = 0 to 7

Line 1,348:

print

'Writing the cells sequence and the path length

print

Line 1,569:

instance Ord a => Semigroup (PQueue a) where

h1@(Node (w1, x1) l1 r1) <> h2@(Node (w2, x2) l2 r2)

| w1 < w2 = Node (w1, x1) (h2 <> r1) l1

| otherwise = Node (w2, x2) (h1 <> r2) l2

EmptyQueue <> h = h

Line 1,614:

Finally, the search algorythm, as given in Wikipedia.

<lang haskell>get :: (Ord k, Bounded a) => Map k a -> k -> a

get m x = Map.findWithDefault maxBound x m

set :: Ord k => Map k a -> k -> a -> Map k a

set m k x = Map.insert k x m

data AstarData n = SetData { cameFrom :: Map n n

Line 1,672:

| i <- [0..8] ]

| j <- [0..8] ]

in do

print path

mapM_ putStrLn picture

Line 1,678:

<pre>λ> main

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

*****

###*

#*

# #*

####*

*

Path score: 11</pre>

Line 1,707:

private int xend, yend;

private final boolean diag;

// Node class for convienience

static class Node implements Comparable {

Line 1,816:

Collections.sort(this.open);

}

public static void main(String[] args) {

// -1 = blocked

Line 1,860:

<pre>

[0, 0] [1, 0] [2, 0] [3, 0] [4, 0] [5, 1] [6, 2] [7, 3] [6, 4] [6, 5] [6, 6] [7, 7]

Total cost: 11,00

*****___

Line 1,875:

Animated. To see how it works on a random map go [http://paulo-jorente.de/tests/astar/ here]

var ctx, map, opn = [], clsd = [], start = {x:1, y:1, f:0, g:0},

goal = {x:8, y:8, f:0, g:0}, mw = 10, mh = 10, neighbours, path;

Line 1,970:

}

map[5][3] = map[6][3] = map[7][3] = map[3][4] = map[7][4] = map[3][5] =

map[7][5] = map[3][6] = map[4][6] = map[5][6] = map[6][6] = map[7][6] = 1;

//map[start.x][start.y] = 2; map[goal.x][goal.y] = 3;

Line 1,981:

document.body.appendChild( canvas );

neighbours = [

{x:1, y:0, c:1}, {x:-1, y:0, c:1}, {x:0, y:1, c:1}, {x:0, y:-1, c:1},

{x:1, y:1, c:1.4}, {x:1, y:-1, c:1.4}, {x:-1, y:1, c:1.4}, {x:-1, y:-1, c:1.4}

];

Line 1,991:

[(1, 1) (2, 2) (2, 3) (2, 4) (2, 5) (2, 6) (3, 7) (4, 8) (5, 8) (6, 8) (7, 8) (8, 8) ]

</pre>

Implementation using recursive strategy

function manhattan(x1, y1, x2, y2) {

return Math.abs(x1 - x2) + Math.abs(y1 - y2);

}

function aStar (board, startx, starty, goalx, goaly,

open = Array(8 * 8).fill(null),

closed = Array(8 * 8).fill(null),

current = {

"coord": [startx, starty],

"distance": 0,

"heuristic": manhattan(startx, starty, goalx, goaly),

"previous": null

}) {

const [x, y] = [...current.coord];

if (x === goalx && y === goaly) {

closed[x + y * 8] = current;

return (lambda = (closed, x, y, startx, starty) => {

if (x === startx && y === starty) {

return [[x, y]];

}

const [px, py] = closed.filter(e => e !== null)

.find(({coord: [nx, ny]}) => {

return nx === x && ny === y

}).previous;

return lambda(closed, px, py, startx, starty).concat([[x,y]]);

})(closed, x, y, startx, starty);

}

let newOpen = open.slice();

[

[x + 1, y + 1], [x - 1, y - 1], [x + 1, y], [x, y + 1],

[x - 1, y + 1], [x + 1, y - 1], [x - 1, y], [x, y - 1]

].filter(([x,y]) => x >= 0 && x < 8 &&

y >= 0 && y < 8 &&

closed[x + y * 8] === null

).forEach(([x,y]) => {

newOpen[x + y * 8] = {

"coord": [x,y],

"distance": current.distance + (board[x + y * 8] === 1 ? 100 : 1),

"heuristic": manhattan(x, y, goalx, goaly),

"previous": [...current.coord]

};

});

let newClosed = closed.slice();

newClosed[x + y * 8] = current;

const [newCurrent,] = newOpen.filter(e => e !== null)

.sort((a, b) => {

return (a.distance + a.heuristic) - (b.distance + b.heuristic);

});

const [newx, newy] = [...newCurrent.coord];

newOpen[newx + newy * 8] = null;

return aStar(board, startx, starty, goalx, goaly,

newOpen, newClosed, newCurrent);

}

const board = [

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,1,1,1,1,0,

0,0,0,0,0,0,0,0,

0,0,0,0,0,0,0,0

];

console.log(aStar(board, 0,0, 7,7));

</lang>

{{output}} <pre>

[ [ 0, 0 ], [ 1, 1 ], [ 2, 2 ], [ 3, 1 ], [ 4, 1 ], [ 5, 1 ], [ 6, 1 ], [ 7, 2 ], [ 7, 3 ], [ 7, 4 ], [ 7, 5 ], [ 7, 6 ], [ 7, 7 ] ]

</pre>

=={{header|Julia}}==

The graphic in this solution is displayed in the more standard orientation of origin at bottom left and goal at top right.

Line 2,183:

Line 2,260:

if self[i]:F() < self[1]:F() then

s = self[1]

self[1] = self[i]

self[i] = s

end

Line 2,241:

Line 2,318:

-- A-STAR ----------------------------------------------------------------------

local nbours = {

{ 1, 0, 1 }, { 0, 1, 1 }, { 1, 1, 1.4 }, { 1, -1, 1.4 },

{ -1, -1, 1.4 }, { -1, 1, 1.4 }, { 0, -1, 1 }, { -1, 0, 1 }

}

local map = {

1,1,1,1,1,1,1,1,1,1,

1,0,0,0,0,0,0,0,0,1,

Line 2,254:

Line 2,331:

1,0,0,0,0,0,0,0,0,1,

1,1,1,1,1,1,1,1,1,1

}

local open, closed, start, goal,

mapW, mapH = Queue:new(), List:new(), Point:new(), Point:new(), 10, 10

start:set( 2, 2 ); goal:set( 9, 9 )

Line 2,269:

Line 2,346:

end

function isValid( pos )

return pos.x > 0 and pos.x <= mapW

and pos.y > 0 and pos.y <= mapH

and map[pos.x + mapW * pos.y - mapW] == 0

end

Line 2,285:

Line 2,362:

nNode.cost = node.cost + nbours[n][3]

nNode.dist = calcDist( nNode.pos )

if isValid( nNode.pos ) then

if nNode.pos:equals( goal ) then

closed:push( nNode )

return true

end

nx = hasNode( closed, nNode.pos )

Line 2,295:

Line 2,372:

nx = hasNode( open, nNode.pos )

if( nx < 0 ) or ( nx > 0 and nNode:F() < open[nx]:F() ) then

if( nx > 0 ) then

table.remove( open, nx )

end

Line 2,338:

Line 2,415:

if node == nil then break end

closed:push( node )

if addToOpen( node ) == true then

makePath()

return true

end

Line 2,868:

Line 2,945:

=={{header|Phix}}==

rows and columns are numbered 1 to 8. start position is {1,1} and end position is {8,8}.

barriers are simply avoided, rather than costed at 100.

Note that the 23 visited nodes does not count walls, but with them this algorithm exactly matches the 35 of Racket.

Line 2,882:

Line 2,959:

::::::::

""",'\n')

constant permitted = {{-1,-1},{0,-1},{1,-1},

{-1, 0}, {1, 0},

{-1, 1},{0,+1},{1,+1}}

sequence key = {7,0}, -- chebyshev, cost

moves = {{1,1}},

Line 2,986:

Line 3,063:

{3,3,3,6,6,6,9,9,9}, -- left

{1,1,1,4,4,4,7,7,7}} -- right

function get_moves(sequence grid, bool mtm)

sequence valid = {}

Line 3,002:

Line 3,079:

return valid

end function

function make_move(sequence grid, move)

integer p0 = find(0,grid),

Line 3,013:

Line 3,090:

return grid

end function

function manhattan(sequence grid)

integer res = 0

Line 3,021:

Line 3,098:

return res

end function

sequence problem, grid, new_grid,

moves, next_moves, move

procedure show_grid()

printf(1,"%s\n",join_by(sq_add(grid,'0'),1,3,""))

end procedure

grid = target

for i=1 to 1000 do

Line 3,040:

Line 3,117:

printf(1,"problem (manhattan cost is %d):\n",manhattan(grid))

show_grid()

integer todo = pq_new(),

seen = new_dict()

Line 3,062:

Line 3,139:

move = next_moves[i]

new_grid = make_move(grid,move)

mc = manhattan(new_grid)

if mc=0 then

if new_grid!=target then ?9/0 end if

Line 3,263:

Line 3,340:

<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 = []

Line 3,279:

Line 3,356:

dy = abs(start[1] - goal[1])

return D * (dx + dy) + (D2 - 2 * D) * min(dx, dy)

def get_vertex_neighbours(self, pos):

n = []

Line 3,290:

Line 3,367:

n.append((x2, y2))

return n

def move_cost(self, a, b):

for barrier in self.barriers:

Line 3,296:

Line 3,373:

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

Line 3,328:

Line 3,405:

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

Line 3,349:

Line 3,426:

H = graph.heuristic(neighbour, end)

F[neighbour] = G[neighbour] + H

raise RuntimeError("A* failed to find a solution")

if __name__=="__main__":

graph = AStarGraph()

Line 3,432:

Line 3,509:

(define count 0)

<a-star-setup>

(begin0

(let/ec esc

<a-star-loop>

#f)

(printf "visited ~a nodes\n" count))))

Line 3,448:

Line 3,525:

<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))

Line 3,472:

Line 3,549:

(define h-x (node-cost x))

(define path-x (hash-ref node->best-path x))

(when (zero? h-x)

(esc (reverse path-x))))

Line 3,479:

Line 3,556:

<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))])

Line 3,532:

Line 3,609:

(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>))

Line 3,573:

Line 3,650:

2htdp/image

racket/runtime-path)

<graph-sig>

<map-generation>

<map-graph-rep>

<map-graph>

<a-star>

<map-node-cost>

<map-example>

Line 3,589:

Line 3,666:

(cons dx dy)])

random-path))

<map-display>

<path-display-line>

Line 3,847:

Line 3,924:

newActual[i,j] := 0 foreach i within 1...width, j within 1...height;

newCameFrom[i,j] := (x : 0, y : 0) foreach i within 1...width, j within 1...height;

searchResults := search((open : [start], closed : [], estimate : newEstimate, actual : newActual, cameFrom : newCameFrom), barriers, end);

shortestPath := path(searchResults.cameFrom, start, end) ++ [end];

Line 3,906:

Line 3,983:

y := n.y + p.y;

in

(x : x, y : y) when x >= 1 and x <= w and y >= 1 and y <= h;

calculateCost(barriers(1), start, end) := 100 when elementOf(end, barriers) else 1;

Line 4,439:

Line 4,516:

path: 0,0 → 1,0 → 2,0 → 3,0 → 4,0 → 5,1 → 6,2 → 7,3 → 7,4 → 7,5 → 7,6 → 7,7

01234567

0◉

1↓

2↓ ■■■

3↓ ■ ■

4↘ ■ ■

5 ↘■■■■■

6 ↘

7 →→→→✪

</pre>

Line 4,462:

Line 4,539:

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

Line 4,470:

Line 4,547:

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

Line 4,484:

Line 4,561:

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)){

Line 4,491:

Line 4,568:

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;

Line 4,542:

Line 4,619:

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>

Line 4,548:

Line 4,625:

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

</pre>