Anonymous user
A* search algorithm: Difference between revisions
no edit summary
(→{{header|Haskell}}: added solution) |
No edit summary |
||
Line 1:
{{draft task|Routing algorithms}}
<!-- A* is pronounced as: A star !-->
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
next
'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:
{{out}}
<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.<br />To see how it works on a random map go [http://paulo-jorente.de/tests/astar/ here]
<lang javascript>
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
<lang javascript>
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,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
];
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 ⟶ 2,260:
if self[i]:F() < self[1]:F() then
s = self[1]
self[1] = self[i]
self[i] = s
end
Line 2,241 ⟶ 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,
1,0,0,0,0,0,0,0,0,1,
Line 2,254 ⟶ 2,331:
1,0,0,0,0,0,0,0,0,1,
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 ⟶ 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 ⟶ 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 ⟶ 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 ⟶ 2,415:
if node == nil then break end
closed:push( node )
if addToOpen( node ) == true then
makePath()
return true
end
end
Line 2,868 ⟶ 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 ⟶ 2,959:
::::::::
"""</span><span style="color: #0000FF;">,</span><span style="color: #008000;">'\n'</span><span style="color: #0000FF;">)</span>
<span style="color: #008080;">constant</span> <span style="color: #000000;">permitted</span> <span style="color: #0000FF;">=</span> <span style="color: #0000FF;">{{-</span><span style="color: #000000;">1</span><span style="color: #0000FF;">,-</span><span style="color: #000000;">1</span><span style="color: #0000FF;">},{</span><span style="color: #000000;">0</span><span style="color: #0000FF;">,-</span><span style="color: #000000;">1</span><span style="color: #0000FF;">},{</span><span style="color: #000000;">1</span><span style="color: #0000FF;">,-</span><span style="color: #000000;">1</span><span style="color: #0000FF;">},</span>
<span style="color: #0000FF;">{-</span><span style="color: #000000;">1</span><span style="color: #0000FF;">,</span> <span style="color: #000000;">0</span><span style="color: #0000FF;">},</span> <span style="color: #0000FF;">{</span><span style="color: #000000;">1</span><span style="color: #0000FF;">,</span> <span style="color: #000000;">0</span><span style="color: #0000FF;">},</span>
<span style="color: #0000FF;">{-</span><span style="color: #000000;">1</span><span style="color: #0000FF;">,</span> <span style="color: #000000;">1</span><span style="color: #0000FF;">},{</span><span style="color: #000000;">0</span><span style="color: #0000FF;">,+</span><span style="color: #000000;">1</span><span style="color: #0000FF;">},{</span><span style="color: #000000;">1</span><span style="color: #0000FF;">,+</span><span style="color: #000000;">1</span><span style="color: #0000FF;">}}</span>
<span style="color: #004080;">sequence</span> <span style="color: #000000;">key</span> <span style="color: #0000FF;">=</span> <span style="color: #0000FF;">{</span><span style="color: #000000;">7</span><span style="color: #0000FF;">,</span><span style="color: #000000;">0</span><span style="color: #0000FF;">},</span> <span style="color: #000080;font-style:italic;">-- chebyshev, cost</span>
<span style="color: #000000;">moves</span> <span style="color: #0000FF;">=</span> <span style="color: #0000FF;">{{</span><span style="color: #000000;">1</span><span style="color: #0000FF;">,</span><span style="color: #000000;">1</span><span style="color: #0000FF;">}},</span>
Line 2,986 ⟶ 3,063:
<span style="color: #0000FF;">{</span><span style="color: #000000;">3</span><span style="color: #0000FF;">,</span><span style="color: #000000;">3</span><span style="color: #0000FF;">,</span><span style="color: #000000;">3</span><span style="color: #0000FF;">,</span><span style="color: #000000;">6</span><span style="color: #0000FF;">,</span><span style="color: #000000;">6</span><span style="color: #0000FF;">,</span><span style="color: #000000;">6</span><span style="color: #0000FF;">,</span><span style="color: #000000;">9</span><span style="color: #0000FF;">,</span><span style="color: #000000;">9</span><span style="color: #0000FF;">,</span><span style="color: #000000;">9</span><span style="color: #0000FF;">},</span> <span style="color: #000080;font-style:italic;">-- left</span>
<span style="color: #0000FF;">{</span><span style="color: #000000;">1</span><span style="color: #0000FF;">,</span><span style="color: #000000;">1</span><span style="color: #0000FF;">,</span><span style="color: #000000;">1</span><span style="color: #0000FF;">,</span><span style="color: #000000;">4</span><span style="color: #0000FF;">,</span><span style="color: #000000;">4</span><span style="color: #0000FF;">,</span><span style="color: #000000;">4</span><span style="color: #0000FF;">,</span><span style="color: #000000;">7</span><span style="color: #0000FF;">,</span><span style="color: #000000;">7</span><span style="color: #0000FF;">,</span><span style="color: #000000;">7</span><span style="color: #0000FF;">}}</span> <span style="color: #000080;font-style:italic;">-- right</span>
<span style="color: #008080;">function</span> <span style="color: #000000;">get_moves</span><span style="color: #0000FF;">(</span><span style="color: #004080;">sequence</span> <span style="color: #000000;">grid</span><span style="color: #0000FF;">,</span> <span style="color: #004080;">bool</span> <span style="color: #000000;">mtm</span><span style="color: #0000FF;">)</span>
<span style="color: #004080;">sequence</span> <span style="color: #000000;">valid</span> <span style="color: #0000FF;">=</span> <span style="color: #0000FF;">{}</span>
Line 3,002 ⟶ 3,079:
<span style="color: #008080;">return</span> <span style="color: #000000;">valid</span>
<span style="color: #008080;">end</span> <span style="color: #008080;">function</span>
<span style="color: #008080;">function</span> <span style="color: #000000;">make_move</span><span style="color: #0000FF;">(</span><span style="color: #004080;">sequence</span> <span style="color: #000000;">grid</span><span style="color: #0000FF;">,</span> <span style="color: #000000;">move</span><span style="color: #0000FF;">)</span>
<span style="color: #004080;">integer</span> <span style="color: #000000;">p0</span> <span style="color: #0000FF;">=</span> <span style="color: #7060A8;">find</span><span style="color: #0000FF;">(</span><span style="color: #000000;">0</span><span style="color: #0000FF;">,</span><span style="color: #000000;">grid</span><span style="color: #0000FF;">),</span>
Line 3,013 ⟶ 3,090:
<span style="color: #008080;">return</span> <span style="color: #000000;">grid</span>
<span style="color: #008080;">end</span> <span style="color: #008080;">function</span>
<span style="color: #008080;">function</span> <span style="color: #000000;">manhattan</span><span style="color: #0000FF;">(</span><span style="color: #004080;">sequence</span> <span style="color: #000000;">grid</span><span style="color: #0000FF;">)</span>
<span style="color: #004080;">integer</span> <span style="color: #000000;">res</span> <span style="color: #0000FF;">=</span> <span style="color: #000000;">0</span>
Line 3,021 ⟶ 3,098:
<span style="color: #008080;">return</span> <span style="color: #000000;">res</span>
<span style="color: #008080;">end</span> <span style="color: #008080;">function</span>
<span style="color: #004080;">sequence</span> <span style="color: #000000;">problem</span><span style="color: #0000FF;">,</span> <span style="color: #000000;">grid</span><span style="color: #0000FF;">,</span> <span style="color: #000000;">new_grid</span><span style="color: #0000FF;">,</span>
<span style="color: #000000;">moves</span><span style="color: #0000FF;">,</span> <span style="color: #000000;">next_moves</span><span style="color: #0000FF;">,</span> <span style="color: #000000;">move</span>
<span style="color: #008080;">procedure</span> <span style="color: #000000;">show_grid</span><span style="color: #0000FF;">()</span>
<span style="color: #7060A8;">printf</span><span style="color: #0000FF;">(</span><span style="color: #000000;">1</span><span style="color: #0000FF;">,</span><span style="color: #008000;">"%s\n"</span><span style="color: #0000FF;">,</span><span style="color: #7060A8;">join_by</span><span style="color: #0000FF;">(</span><span style="color: #7060A8;">sq_add</span><span style="color: #0000FF;">(</span><span style="color: #000000;">grid</span><span style="color: #0000FF;">,</span><span style="color: #008000;">'0'</span><span style="color: #0000FF;">),</span><span style="color: #000000;">1</span><span style="color: #0000FF;">,</span><span style="color: #000000;">3</span><span style="color: #0000FF;">,</span><span style="color: #008000;">""</span><span style="color: #0000FF;">))</span>
<span style="color: #008080;">end</span> <span style="color: #008080;">procedure</span>
<span style="color: #000000;">grid</span> <span style="color: #0000FF;">=</span> <span style="color: #000000;">target</span>
<span style="color: #008080;">for</span> <span style="color: #000000;">i</span><span style="color: #0000FF;">=</span><span style="color: #000000;">1</span> <span style="color: #008080;">to</span> <span style="color: #000000;">1000</span> <span style="color: #008080;">do</span>
Line 3,040 ⟶ 3,117:
<span style="color: #7060A8;">printf</span><span style="color: #0000FF;">(</span><span style="color: #000000;">1</span><span style="color: #0000FF;">,</span><span style="color: #008000;">"problem (manhattan cost is %d):\n"</span><span style="color: #0000FF;">,</span><span style="color: #000000;">manhattan</span><span style="color: #0000FF;">(</span><span style="color: #000000;">grid</span><span style="color: #0000FF;">))</span>
<span style="color: #000000;">show_grid</span><span style="color: #0000FF;">()</span>
<span style="color: #004080;">integer</span> <span style="color: #000000;">todo</span> <span style="color: #0000FF;">=</span> <span style="color: #7060A8;">pq_new</span><span style="color: #0000FF;">(),</span>
<span style="color: #000000;">seen</span> <span style="color: #0000FF;">=</span> <span style="color: #7060A8;">new_dict</span><span style="color: #0000FF;">()</span>
Line 3,062 ⟶ 3,139:
<span style="color: #000000;">move</span> <span style="color: #0000FF;">=</span> <span style="color: #000000;">next_moves</span><span style="color: #0000FF;">[</span><span style="color: #000000;">i</span><span style="color: #0000FF;">]</span>
<span style="color: #000000;">new_grid</span> <span style="color: #0000FF;">=</span> <span style="color: #000000;">make_move</span><span style="color: #0000FF;">(</span><span style="color: #000000;">grid</span><span style="color: #0000FF;">,</span><span style="color: #000000;">move</span><span style="color: #0000FF;">)</span>
<span style="color: #000000;">mc</span> <span style="color: #0000FF;">=</span> <span style="color: #000000;">manhattan</span><span style="color: #0000FF;">(</span><span style="color: #000000;">new_grid</span><span style="color: #0000FF;">)</span>
<span style="color: #008080;">if</span> <span style="color: #000000;">mc</span><span style="color: #0000FF;">=</span><span style="color: #000000;">0</span> <span style="color: #008080;">then</span>
<span style="color: #008080;">if</span> <span style="color: #000000;">new_grid</span><span style="color: #0000FF;">!=</span><span style="color: #000000;">target</span> <span style="color: #008080;">then</span> <span style="color: #0000FF;">?</span><span style="color: #000000;">9</span><span style="color: #0000FF;">/</span><span style="color: #000000;">0</span> <span style="color: #008080;">end</span> <span style="color: #008080;">if</span>
Line 3,263 ⟶ 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 ⟶ 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 ⟶ 3,367:
n.append((x2, y2))
return n
def move_cost(self, a, b):
for barrier in self.barriers:
Line 3,296 ⟶ 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 ⟶ 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 ⟶ 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 ⟶ 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 ⟶ 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 ⟶ 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 ⟶ 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 ⟶ 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 ⟶ 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 ⟶ 3,666:
(cons dx dy)])
random-path))
<map-display>
<path-display-line>
Line 3,847 ⟶ 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 ⟶ 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 ⟶ 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 ⟶ 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 ⟶ 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 ⟶ 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 ⟶ 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 ⟶ 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>
{{out}}
Line 4,548 ⟶ 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>
| |||