A* search algorithm
A* search algorithm is a draft programming task. It is not yet considered ready to be promoted as a complete task, for reasons that should be found in its talk page.
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).
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*!
- Extra Credit
Use this algorithm to solve an 8 puzzle. Each node of the input graph will represent an arrangement of the tiles. The nodes will be connected by 4 edges representing swapping the blank tile up, down, left, or right. The cost of each edge is 1. The heuristic will be the sum of the manhatten distance of each numbered tile from its goal position. An 8 puzzle graph will have 9!/2 (181,440) nodes. The 15 puzzle has over 10 trillion nodes. This algorithm may solve simple 15 puzzles (but there are not many of those).
- See also
- Wikipedia webpage: A* search algorithm.
- An introduction to: Breadth First Search |> Dijkstra’s Algorithm |> A*
- Related tasks
F AStarSearch(start, end, barriers) F heuristic(start, goal) V D = 1 V D2 = 1 V dx = abs(start[0] - goal[0]) V dy = abs(start[1] - goal[1]) R D * (dx + dy) + (D2 - 2 * D) * min(dx, dy) F get_vertex_neighbours(pos) [(Int, Int)] n L(dx, dy) [(1, 0), (-1, 0), (0, 1), (0, -1), (1, 1), (-1, 1), (1, -1), (-1, -1)] V x2 = pos[0] + dx V y2 = pos[1] + dy I x2 < 0 | x2 > 7 | y2 < 0 | y2 > 7 L.continue n.append((x2, y2)) R n F move_cost(a, b) L(barrier) @barriers I b C barrier R 100 R 1 [(Int, Int) = Int] G [(Int, Int) = Int] f G[start] = 0 f[start] = heuristic(start, end) Set[(Int, Int)] closedVertices V openVertices = Set([start]) [(Int, Int) = (Int, Int)] cameFrom L openVertices.len > 0 (Int, Int)? current V currentFscore = 0 L(pos) openVertices I current == N | f[pos] < currentFscore currentFscore = f[pos] current = pos I current == end V path = [current] L current C cameFrom current = cameFrom[current] path.append(current) path.reverse() R (path, f[end]) openVertices.remove(current) closedVertices.add(current) L(neighbour) get_vertex_neighbours(current) I neighbour C closedVertices L.continue V candidateG = G[current] + move_cost(current, neighbour) I neighbour !C openVertices openVertices.add(neighbour) E I candidateG >= G[neighbour] L.continue cameFrom[neighbour] = current G[neighbour] = candidateG V H = heuristic(neighbour, end) f[neighbour] = G[neighbour] + H X.throw RuntimeError(‘A* failed to find a solution’) V (result, cost) = AStarSearch((0, 0), (7, 7), [[(2, 4), (2, 5), (2, 6), (3, 6), (4, 6), (5, 6), (5, 5), (5, 4), (5, 3), (5, 2), (4, 2), (3, 2)]]) print(‘route ’result) print(‘cost ’cost)- Output:
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)] cost 11
#include <stdlib.h> #include <stdio.h> #include <string.h> #include <float.h> /* and not not_eq */ #include <iso646.h> /* add -lm to command line to compile with this header */ #include <math.h> #define map_size_rows 10 #define map_size_cols 10 char map[map_size_rows][map_size_cols] = { {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}, {1, 0, 0, 0, 0, 1, 1, 1, 0, 1}, {1, 0, 0, 1, 0, 0, 0, 1, 0, 1}, {1, 0, 0, 1, 0, 0, 0, 1, 0, 1}, {1, 0, 0, 1, 1, 1, 1, 1, 0, 1}, {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} }; /* description of graph node */ struct stop { double col, row; /* array of indexes of routes from this stop to neighbours in array of all routes */ int * n; int n_len; double f, g, h; int from; }; int ind[map_size_rows][map_size_cols] = { {-1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1, -1, -1, -1, -1} }; /* description of route between two nodes */ struct route { /* route has only one direction! */ int x; /* index of stop in array of all stops of src of this route */ int y; /* intex of stop in array of all stops od dst of this route */ double d; }; int main() { int i, j, k, l, b, found; int p_len = 0; int * path = NULL; int c_len = 0; int * closed = NULL; int o_len = 1; int * open = (int*)calloc(o_len, sizeof(int)); double min, tempg; int s; int e; int current; int s_len = 0; struct stop * stops = NULL; int r_len = 0; struct route * routes = NULL; for (i = 1; i < map_size_rows - 1; i++) { for (j = 1; j < map_size_cols - 1; j++) { if (!map[i][j]) { ++s_len; stops = (struct stop *)realloc(stops, s_len * sizeof(struct stop)); int t = s_len - 1; stops[t].col = j; stops[t].row = i; stops[t].from = -1; stops[t].g = DBL_MAX; stops[t].n_len = 0; stops[t].n = NULL; ind[i][j] = t; } } } /* index of start stop */ s = 0; /* index of finish stop */ e = s_len - 1; for (i = 0; i < s_len; i++) { stops[i].h = sqrt(pow(stops[e].row - stops[i].row, 2) + pow(stops[e].col - stops[i].col, 2)); } for (i = 1; i < map_size_rows - 1; i++) { for (j = 1; j < map_size_cols - 1; j++) { if (ind[i][j] >= 0) { for (k = i - 1; k <= i + 1; k++) { for (l = j - 1; l <= j + 1; l++) { if ((k == i) and (l == j)) { continue; } if (ind[k][l] >= 0) { ++r_len; routes = (struct route *)realloc(routes, r_len * sizeof(struct route)); int t = r_len - 1; routes[t].x = ind[i][j]; routes[t].y = ind[k][l]; routes[t].d = sqrt(pow(stops[routes[t].y].row - stops[routes[t].x].row, 2) + pow(stops[routes[t].y].col - stops[routes[t].x].col, 2)); ++stops[routes[t].x].n_len; stops[routes[t].x].n = (int*)realloc(stops[routes[t].x].n, stops[routes[t].x].n_len * sizeof(int)); stops[routes[t].x].n[stops[routes[t].x].n_len - 1] = t; } } } } } } open[0] = s; stops[s].g = 0; stops[s].f = stops[s].g + stops[s].h; found = 0; while (o_len and not found) { min = DBL_MAX; for (i = 0; i < o_len; i++) { if (stops[open[i]].f < min) { current = open[i]; min = stops[open[i]].f; } } if (current == e) { found = 1; ++p_len; path = (int*)realloc(path, p_len * sizeof(int)); path[p_len - 1] = current; while (stops[current].from >= 0) { current = stops[current].from; ++p_len; path = (int*)realloc(path, p_len * sizeof(int)); path[p_len - 1] = current; } } for (i = 0; i < o_len; i++) { if (open[i] == current) { if (i not_eq (o_len - 1)) { for (j = i; j < (o_len - 1); j++) { open[j] = open[j + 1]; } } --o_len; open = (int*)realloc(open, o_len * sizeof(int)); break; } } ++c_len; closed = (int*)realloc(closed, c_len * sizeof(int)); closed[c_len - 1] = current; for (i = 0; i < stops[current].n_len; i++) { b = 0; for (j = 0; j < c_len; j++) { if (routes[stops[current].n[i]].y == closed[j]) { b = 1; } } if (b) { continue; } tempg = stops[current].g + routes[stops[current].n[i]].d; b = 1; if (o_len > 0) { for (j = 0; j < o_len; j++) { if (routes[stops[current].n[i]].y == open[j]) { b = 0; } } } if (b or (tempg < stops[routes[stops[current].n[i]].y].g)) { stops[routes[stops[current].n[i]].y].from = current; stops[routes[stops[current].n[i]].y].g = tempg; stops[routes[stops[current].n[i]].y].f = stops[routes[stops[current].n[i]].y].g + stops[routes[stops[current].n[i]].y].h; if (b) { ++o_len; open = (int*)realloc(open, o_len * sizeof(int)); open[o_len - 1] = routes[stops[current].n[i]].y; } } } } for (i = 0; i < map_size_rows; i++) { for (j = 0; j < map_size_cols; j++) { if (map[i][j]) { putchar(0xdb); } else { b = 0; for (k = 0; k < p_len; k++) { if (ind[i][j] == path[k]) { ++b; } } if (b) { putchar('x'); } else { putchar('.'); } } } putchar('\n'); } if (not found) { puts("IMPOSSIBLE"); } else { printf("path cost is %d:\n", p_len); for (i = p_len - 1; i >= 0; i--) { printf("(%1.0f, %1.0f)\n", stops[path[i]].col, stops[path[i]].row); } } for (i = 0; i < s_len; ++i) { free(stops[i].n); } free(stops); free(routes); free(path); free(open); free(closed); return 0; } - Output:
▒▒▒▒▒▒▒▒▒▒ ▒x.......▒ ▒.x......▒ ▒.x..▒▒▒.▒ ▒.x▒...▒.▒ ▒.x▒...▒.▒ ▒.x▒▒▒▒▒.▒ ▒..xxxxx.▒ ▒.......x▒ ▒▒▒▒▒▒▒▒▒▒ path cost is 12: (1, 1) (2, 2) (2, 3) (2, 4) (2, 5) (2, 6) (3, 7) (4, 7) (5, 7) (6, 7) (7, 7) (8, 8)
using System; using System.Collections.Generic; namespace A_star { class A_star { // Coordinates of a cell - implements the method Equals public class Coordinates : IEquatable<Coordinates> { public int row; public int col; public Coordinates() { this.row = -1; this.col = -1; } public Coordinates(int row, int col) { this.row = row; this.col = col; } public Boolean Equals(Coordinates c) { if (this.row == c.row && this.col == c.col) return true; else return false; } } // 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 { public int cost; public int g; public int f; public Coordinates parent; } // Class Astar, which finds the shortest path public class Astar { // The array of the cells public Cell[,] cells = new Cell[8, 8]; // The possible path found public List<Coordinates> path = new List<Coordinates>(); // The list of the opened cells public List<Coordinates> opened = new List<Coordinates>(); // The list of the closed cells public List<Coordinates> closed = new List<Coordinates>(); // The start of the searched path public Coordinates startCell = new Coordinates(0, 0); // The end of the searched path public Coordinates finishCell = new Coordinates(7, 7); // The constructor public Astar() { // Initialization of the cells values for (int i = 0; i < 8; i++) for (int j = 0; j < 8; j++) { cells[i, j] = new Cell(); cells[i, j].parent = new Coordinates(); if (IsAWall(i, j)) cells[i, j].cost = 100; else cells[i, j].cost = 1; } // Adding the start cell on the list opened opened.Add(startCell); // Boolean value which indicates if a path is found Boolean pathFound = false; // Loop until the list opened is empty or a path is found do { List<Coordinates> neighbors = new List<Coordinates>(); // The next cell analyzed Coordinates currentCell = ShorterExpectedPath(); // The list of cells reachable from the actual one neighbors = neighborsCells(currentCell); foreach (Coordinates newCell in neighbors) { // If the cell considered is the final one 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; cells[newCell.row, newCell.col].parent.col = currentCell.col; pathFound = true; break; } // If the cell considered is not between the open and closed ones 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; cells[newCell.row, newCell.col].parent.col = currentCell.col; SetCell(newCell, opened); } // 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; cells[newCell.row, newCell.col].parent.col = currentCell.col; SetCell(newCell, opened); ResetCell(newCell, closed); } } SetCell(currentCell, closed); ResetCell(currentCell, opened); } while (opened.Count > 0 && pathFound == false); if (pathFound) { path.Add(finishCell); Coordinates currentCell = new Coordinates(finishCell.row, finishCell.col); // It reconstructs the path starting from the end while (cells[currentCell.row, currentCell.col].parent.row >= 0) { path.Add(cells[currentCell.row, currentCell.col].parent); int tmp_row = cells[currentCell.row, currentCell.col].parent.row; currentCell.col = cells[currentCell.row, currentCell.col].parent.col; currentCell.row = tmp_row; } // Printing on the screen the 'chessboard' and the path found for (int i = 0; i < 8; i++) { for (int j = 0; j < 8; j++) { // Symbol for a cell that doesn't belong to the path and isn't // a wall char gr = '.'; // Symbol for a cell that belongs to the path if (path.Contains(new Coordinates(i, j))) { gr = 'X'; } // Symbol for a cell that is a wall else if (cells[i, j].cost > 1) { gr = '\u2588'; } System.Console.Write(gr); } System.Console.WriteLine(); } // Printing the coordinates of the cells of the path System.Console.Write("\nPath: "); for (int i = path.Count - 1; i >= 0; i--) { System.Console.Write("({0},{1})", path[i].row, path[i].col); } // Printing the cost of the path System.Console.WriteLine("\nPath cost: {0}", path.Count - 1); // Waiting to the key Enter to be pressed to end the program String wt = System.Console.ReadLine(); } } // It select the cell between those in the list opened that have the smaller // value of f public Coordinates ShorterExpectedPath() { int sep = 0; if (opened.Count > 1) { 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) { sep = i; } } } return opened[sep]; } // It finds che cells that could be reached from c public List<Coordinates> neighborsCells(Coordinates c) { List<Coordinates> lc = new List<Coordinates>(); 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)) { lc.Add(new Coordinates(c.row + i, c.col + j)); } return lc; } // It determines if the cell with coordinates (row, col) is a wall 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; for (int i = 0; i < walls.GetLength(0); i++) if (walls[i,0] == row && walls[i,1] == col) found = true; return found; } // The function Heuristic, which determines the shortest path that a 'king' can do // This is the maximum value between the orizzontal distance and the vertical one public int Heuristic(Coordinates cell) { int dRow = Math.Abs(finishCell.row - cell.row); int dCol = Math.Abs(finishCell.col - cell.col); return Math.Max(dRow, dCol); } // It inserts the coordinates of cell in a list, if it's not already present public void SetCell(Coordinates cell, List<Coordinates> coordinatesList) { if (coordinatesList.Contains(cell) == false) { coordinatesList.Add(cell); } } // It removes the coordinates of cell from a list, if it's already present public void ResetCell(Coordinates cell, List<Coordinates> coordinatesList) { if (coordinatesList.Contains(cell)) { coordinatesList.Remove(cell); } } } // The main method static void Main(string[] args) { Astar astar = new Astar(); } } } - Output:
X....... .X...... ..X.███. .X█...█. .X█...█. .X█████. ..XXXXX. .......X Path: (0,0)(1,1)(2,2)(3,1)(4,1)(5,1)(6,2)(6,3)(6,4)(6,5)(6,6)(7,7) Path cost: 11
#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; } - 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)
;; * Using external libraries with quicklisp (eval-when (:load-toplevel :compile-toplevel :execute) (ql:quickload '("pileup" "iterate"))) ;; * The package definition (defpackage :a*-search (:use :common-lisp :pileup :iterate)) (in-package :a*-search) ;; * The data (defvar *size* 8 "The size of the area.") ;; I will use simple conses for the positions and directions. (defvar *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)) "The position of the barriers in (X Y) pairs, starting with (0 0) at the lower left corner.") (defvar *barrier-cost* 100 "The costs of a barrier field.") (defvar *directions* '((0 . -1) (0 . 1) (1 . 0) (-1 . 0) (-1 . -1) (1 . 1)) "The possible directions left, right, up, down and diagonally.") ;; * Tha data structure for the node in the search graph (defstruct (node (:constructor node)) (pos (cons 0 0) :type cons) (path nil) (cost 0 :type fixnum) ; The costs so far (f-value 0 :type fixnum) ; The value for the heuristics ) ;; * The functions ;; ** Printing the final path (defun print-path (path start end &optional (barriers *barriers*) &aux (size (+ 2 *size*))) "Prints the area with the BARRIERS." ;; The upper boarder (format t "~v@{~A~:*~}~%" size "█") ;; The actual area ;; The lines (iter (for y from (1- *size*) downto 0) (format t "█") ;; The columns (iter (for x from 0 below *size*) (format t "~A" (cond ((member (cons y x) barriers :test #'equal) "█") ((equal (cons y x) start) "●") ((equal (cons y x) end) "◆") ((Member (cons y x) path :test #'equal) "▪") (t " ")))) ;; The last column and jump to the next line (format t "█~%")) ;; The lower boarder (format t "~v@{~A~:*~}~%" size "█") (iter (for position in path) (format t "(~D,~D)" (car position) (cdr position)) (finally (terpri)))) ;; ** Generating the next positions ;; *** Check if a position is possible (defun valid-position-p (position) "Returns T if POSITION is a valid position." (let ((x (car position)) (y (cdr position)) (max (1- *size*))) (and (<= 0 x max) (<= 0 y max)))) ;; *** Move from the current position in direction (defun move (position direction) "Returns a new position after moving from POSITION in DIRECTION assuming only valid positions." (let ((x (car position)) (y (cdr position)) (dx (car direction)) (dy (cdr direction))) (cons (+ x dx) (+ y dy)))) ;; *** Generate the possible next positions (defun next-positions (current-position) "Returns a list of conses with possible next positions." (remove-if-not #'valid-position-p (mapcar (lambda (d) (move current-position d)) *directions*))) ;; ** The heuristics (defun distance (current-position goal) "Returns the Manhattan distance from CURRENT-POSITION to GOAL." (+ (abs (- (car goal) (car current-position))) (abs (- (cdr goal) (cdr current-position))))) ;; ** The A+ search (defun a* (start goal heuristics next &optional (information 0)) "Returns the shortest path from START to GOAL using HEURISTICS, generating the next nodes using NEXT." (let ((visited (make-hash-table :test #'equalp))) (flet ((pick-next-node (queue) ;; Get the next node from the queue (heap-pop queue)) (expand-node (node queue) ;; Expand the next possible nodes from node and add them to the ;; queue if not already visited. (iter (with costs = (node-cost node)) (for position in (funcall next (node-pos node))) (for cost = (1+ costs)) (for f-value = (+ cost (funcall heuristics position goal) (if (member position *barriers* :test #'equal) 100 0))) ;; Check if this state was already looked at (unless (gethash position visited) ;; Insert the next node into the queue (heap-insert (node :pos position :path (cons position (node-path node)) :cost cost :f-value f-value) queue))))) ;; The actual A* search (iter ;; The priority queue (with queue = (make-heap #'<= :name "queue" :size 1000 :key #'node-f-value)) (with initial-cost = (funcall heuristics start goal)) (initially (heap-insert (node :pos start :path (list start) :cost 0 :f-value initial-cost) queue)) (for counter from 1) (for current-node = (pick-next-node queue)) (for current-position = (node-pos current-node)) ;; 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)) ;; Add the current state to the hash of visited states (setf (gethash current-node visited) t) ;; Expand the current node and continue (expand-node current-node queue) (finally (return (values (nreverse (node-path current-node)) (node-cost current-node) counter))))))) ;; ** The main function (defun search-path (&key (start '(0 . 0)) (goal '(7 . 7)) (heuristics #'distance)) "Searches the shortest path from START to GOAL using HEURISTICS." (multiple-value-bind (path cost steps) (a* start goal heuristics #'next-positions 0) (format t "Found the shortest path from Start (●) to Goal (◆) in ~D steps with cost: ~D~%" steps cost) (print-path path start goal))) - Output:
A*-SEARCH> (search-path) Found the shortest path from Start (●) to Goal (◆) in 323 steps with cost: 11 ██████████ █ ▪▪▪▪◆█ █ ▪ █ █ ▪█████ █ █ ▪█ █ █ █ ▪█ █ █ █ ▪ ███ █ █ ▪ █ █● █ ██████████ (0,0)(1,1)(2,1)(3,1)(4,1)(5,1)(6,2)(7,3)(7,4)(7,5)(7,6)(7,7)
import std.stdio; import std.algorithm; import std.range; import std.array; struct Point { int x; int y; Point opBinary(string op = "+")(Point o) { return Point( o.x + x, o.y + y ); } } struct Map { int w = 8; int h = 8; bool[][] m = [ [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] ]; } struct Node { Point pos; Point parent; int dist; int cost; bool opEquals(const Node n) { return pos == n.pos; } bool opEquals(const Point p) { return pos == p; } int opCmp(ref const Node n) const { return (n.dist + n.cost) - (dist + cost); } }; struct AStar { Map m; Point end; Point start; Point[8] neighbours = [Point(-1,-1), Point(1,-1), Point(-1,1), Point(1,1), Point(0,-1), Point(-1,0), Point(0,1), Point(1,0)]; Node[] open; Node[] closed; int calcDist(Point b) { // need a better heuristic int x = end.x - b.x, y = end.y - b.y; 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); if( i != -1 ) { if( closed[i].cost + closed[i].dist < cost ) return true; else { closed = closed.remove!(SwapStrategy.stable)(i); return false; } } i = open.countUntil(b); if( i != -1 ) { if( open[i].cost + open[i].dist < cost ) return true; else { open = open.remove!(SwapStrategy.stable)(i); return false; } } return false; } bool fillOpen( ref Node n ) { int stepCost; int nc; int 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.m[neighbour.y][neighbour.x] != 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 ~= m; } } } 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() ) { //open.sort(); Node nx = open.front(); open = open.drop(1).array; closed ~= nx ; if( fillOpen( nx ) ) return true; } 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 ) ) { path = i.pos ~ path; parent = i.parent; } } path = start ~ path; return cost; } }; int main(string[] argv) { Map m; Point s; Point e = Point( 7, 7 ); AStar as; if( as.search( s, e, m ) ) { 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.m[y][x] == 1 ) write(cast(char)0xdb); else { if( path.canFind(Point(x,y))) write("x"); else write("."); } } writeln(); } write("\nPath cost ", c, ": "); foreach( i; path ) { write("(", i.x, ", ", i.y, ") "); } } write("\n\n"); return 0; } - 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)
import 'dart:collection'; import 'dart:math'; // In Dart, classes should be defined at the top level, not nested class Node implements Comparable<Node> { Node? parent; int x, y; double g; double h; Node(this.parent, this.x, this.y, this.g, this.h); // Compare by f value (g + h) @override int compareTo(Node other) { return ((this.g + this.h) - (other.g + other.h)).toInt(); } } class MazeCoord { MazeCoord? father; int _r = 0; int _c = 0; int costToGoal = 0; int pathCost = 0; int aStartCost = 0; int getR() => _r; int getC() => _c; MazeCoord expandDirection() => MazeCoord(); int calculateCost(MazeCoord goal) { int rState = getR(); int rGoal = goal.getR(); int diffR = rState - rGoal; int diffC = getC() - goal.getC(); if (diffR * diffC > 0) { // same sign return diffR.abs() + diffC.abs(); } else { return max(diffR.abs(), diffC.abs()); } } MazeCoord getFather() { return father!; } void setFather(MazeCoord node) { this.father = node; } int getAStartCost() { return aStartCost; } void setAStartCost(int aStartCost) { this.aStartCost = aStartCost; } int getCostToGoal() { return costToGoal; } void setCostToGoal(int costToGoal) { this.costToGoal = costToGoal; } int getPathCost() { return pathCost; } void setPathCost(int pathCost) { this.pathCost = pathCost; } } class AStar { final List<Node> open; final List<Node> closed; final List<Node> path; final List<List<int>> maze; late Node now; final int xstart; final int ystart; late int xend, yend; final bool diag; // Additional members to support the other methods late MazeCoord start; late MazeCoord goal; Queue<MazeCoord> frontier = Queue<MazeCoord>(); List<MazeCoord> explored = []; AStar(this.maze, this.xstart, this.ystart, this.diag) : open = [], closed = [], path = [] { this.now = Node(null, xstart, ystart, 0, 0); this.start = MazeCoord(); this.goal = MazeCoord(); } /* ** Finds path to xend/yend or returns null ** ** @param (int) xend coordinates of the target position ** @param (int) yend ** @return (List<Node> | null) the path */ List<Node>? findPathTo(int xend, int yend) { this.xend = xend; this.yend = yend; this.closed.add(this.now); addNeighborsToOpenList(); while (this.now.x != this.xend || this.now.y != this.yend) { if (this.open.isEmpty) { // Nothing to examine return null; } this.now = this.open[0]; // get first node (lowest f score) this.open.removeAt(0); // remove it this.closed.add(this.now); // and add to the closed addNeighborsToOpenList(); } this.path.insert(0, this.now); while (this.now.x != this.xstart || this.now.y != this.ystart) { this.now = this.now.parent!; this.path.insert(0, this.now); } return this.path; } /* ** This function is the step of expanding nodes ** */ void expandAStar(List<List<int>> maze, int xstart, int ystart, bool diag) { Queue<MazeCoord> exploreNodes = Queue<MazeCoord>(); MazeCoord stateNode = MazeCoord(); if (maze[stateNode.getR()][stateNode.getC()] == 2) { if (isNodeILegal(stateNode, stateNode.expandDirection())) { exploreNodes.add(stateNode.expandDirection()); } } } bool isNodeILegal(MazeCoord stateNode, MazeCoord direction) { // This is a placeholder implementation return true; } /* ** Calculate distance for goal three methods shown. ** */ void aStarSearch() { this.start.setCostToGoal(this.start.calculateCost(this.goal)); this.start.setPathCost(0); this.start.setAStartCost(this.start.getPathCost() + this.start.getCostToGoal()); MazeCoord initialNode = this.start; MazeCoord stateNode = initialNode; frontier.add(initialNode); //explored<Queue> is empty while (true) { if (frontier.isEmpty) { print("fail"); print(explored.length); // In Dart, we don't use System.exit, instead we can return or throw return; } // Rest of the algorithm would go here break; // Added to prevent infinite loop } } /* ** Third method for distance calculation. */ double distance(int dx, int dy) { if (this.diag) { // if diagonal movement is allowed return sqrt(pow(this.now.x + dx - this.xend, 2) + pow(this.now.y + dy - this.yend, 2)); // return hypotenuse } else { return ((this.now.x + dx - this.xend).abs() + (this.now.y + dy - this.yend).abs()).toDouble(); // else return "Manhattan distance" } } bool findNeighborInList(List<Node> list, Node node) { return list.any((n) => n.x == node.x && n.y == node.y); } void addNeighborsToOpenList() { for (int x = -1; x <= 1; x++) { for (int y = -1; y <= 1; y++) { if (!this.diag && x != 0 && y != 0) { continue; // skip if diagonal movement is not allowed } Node node = Node(this.now, this.now.x + x, this.now.y + y, this.now.g, this.distance(x, y)); if ((x != 0 || y != 0) && // not this.now this.now.x + x >= 0 && this.now.x + x < this.maze[0].length && // check maze boundaries this.now.y + y >= 0 && this.now.y + y < this.maze.length && this.maze[this.now.y + y][this.now.x + x] != -1 && // check if square is walkable !findNeighborInList(this.open, node) && !findNeighborInList(this.closed, node)) { // if not already done node.g = node.parent!.g + 1.0; // Horizontal/vertical cost = 1.0 node.g += maze[this.now.y + y][this.now.x + x]; // add movement cost for this square // diagonal cost = sqrt(hor_cost² + vert_cost²) // in this example the cost would be 12.2 instead of 11 /* if (diag && x != 0 && y != 0) { node.g += 0.4; // Diagonal movement cost = 1.4 } */ this.open.add(node); } } } this.open.sort(); } } void main() { // -1 = blocked // 0+ = additional movement cost List<List<int>> maze = [ [0, 0, 0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0, 0, 0], [0, 0, 0, 100, 100, 100, 0, 0], [0, 0, 0, 0, 0, 100, 0, 0], [0, 0, 100, 0, 0, 100, 0, 0], [0, 0, 100, 0, 0, 100, 0, 0], [0, 0, 100, 100, 100, 100, 0, 0], [0, 0, 0, 0, 0, 0, 0, 0], ]; AStar as = AStar(maze, 0, 0, true); List<Node>? path = as.findPathTo(7, 7); if (path != null) { for (Node n in path) { print("[${n.x}, ${n.y}] "); maze[n.y][n.x] = -1; } print("\nTotal cost: ${path[path.length - 1].g.toStringAsFixed(2)}"); for (List<int> mazeRow in maze) { String rowStr = ""; for (int mazeEntry in mazeRow) { switch (mazeEntry) { case 0: rowStr += "_"; break; case -1: rowStr += "*"; break; default: rowStr += "#"; } } print(rowStr); } } } - Output:
[0, 0] [1, 0] [2, 0] [3, 0] [4, 0] [5, 1] [6, 2] [6, 3] [6, 4] [6, 5] [6, 6] [7, 7] Total cost: 11.00 *****___ _____*__ ___###*_ _____#*_ __#__#*_ __#__#*_ __####*_ _______*
'############################### '### A* search algorithm ### '############################### 'A number big enough to be greater than any possible path cost #define MAX_DIST 100000 type coordinates 'coordinates of a cell row as integer col as integer end type type listCoordinates 'list of coordinates length as integer coord(1 to 64) as coordinates end type type cell 'properties of a cell cost as integer g as integer f as integer parent as coordinates end type sub AddCoordinates(list as listCoordinates, c as coordinates) 'Adds coordinates c to the listCoordinates, checking if it's already present dim i as integer, inList as integer = 0 if (list.length > 0) then for i = 1 to list.length if (list.coord(i).row = c.row and list.coord(i).col = c.col) then inList = i exit for end if next if (inList > 0) then exit sub end if end if if (list.length < 64) then list.length = list.length + 1 list.coord(list.length).row = c.row list.coord(list.length).col = c.col end if end sub sub RemoveCoordinates(list as listCoordinates, c as coordinates) 'Removes coordinates c from listCoordinates dim i as integer, inList as integer = 0 if (list.length > 0) then for i = 1 to list.length if (list.coord(i).row = c.row and list.coord(i).col = c.col) then inList = i exit for end if next if (inList > 0) then list.coord(inList).row = list.coord(list.length).row list.coord(inList).col = list.coord(list.length).col list.length = list.length - 1 end if end if end sub function GetOpened(list as listCoordinates, cells() as cell) as coordinates 'Gets the cell between the open ones with the shortest expected cost dim i as integer, minf as integer dim rv as coordinates minf = 1 if (list.length > 1) then for i = 2 to list.length if (cells(list.coord(i).row, list.coord(i).col).f < cells(list.coord(minf).row, list.coord(minf).col).f) then minf = i end if next end if rv.row = list.coord(minf).row rv.col = list.coord(minf).col return rv end function function Heuristic(byval a as coordinates, byval b as coordinates) as integer 'In a chessboard, the shortest path of a king between two cells is the maximum value 'between the orizzontal distance and the vertical one. This could be used as 'heuristic value in the A* algorithm. dim dr as integer, dc as integer dr = abs(a.row - b.row) dc = abs(a.col - b.col) if (dr > dc) then return dr else return dc end if end function function IsACell(r as integer, c as integer) as integer 'It determines if a couple of indeces are inside the chessboard (returns 1) or outside (returns 0) dim isCell as integer if (r < 0 or r > 7 or c < 0 or c > 7) then isCell = 0 else isCell = 1 end if return isCell end function sub AppendCell(p as listCoordinates, c as coordinates) 'It appends che coordinates c at the end of the list p p.length = p.length + 1 p.coord(p.length).row = c.row p.coord(p.length).col = c.col end sub function InList(r as integer, c as integer, p as listCoordinates) as integer 'It determines if the cell with coordinates (r,c) is in the list p dim isInPath as integer = 0 dim i as integer for i = 1 to Ubound(p.coord) if (p.coord(i).row = r and p.coord(i).col = c) then isInPath = 1 exit for end if next return isInPath end function 'Variables declaration 'Cost to go to the cell of coordinates (row, column) dim costs(0 to 7, 0 to 7) as integer => { _ {1, 1, 1, 1, 1, 1, 1, 1}, {1, 1, 1, 1, 1, 1, 1, 1}, _ {1, 1, 1, 1, 100, 100, 100, 1}, {1, 1, 100, 1, 1, 1, 100, 1}, _ {1, 1, 100, 1, 1, 1, 100, 1}, {1, 1, 100, 100, 100, 100, 100, 1}, _ {1, 1, 1, 1, 1, 1, 1, 1}, {1, 1, 1, 1, 1, 1, 1, 1}} dim start as coordinates, finish as coordinates 'the first and the last cell dim opened as listCoordinates, closed as listCoordinates dim aCell as coordinates, nCell as coordinates 'the cell evaluates and the next one dim cells(0 to 7, 0 to 7) as cell 'the cells of the chessboard dim path as listCoordinates 'list used to the path found dim i as integer, j as integer 'MAIN PROCEDURE 'Fixing the starting cell and the finishing one start.row = 0 start.col = 0 finish.row = 7 finish.col = 7 opened.length = 0 closed.length = 0 'Initializing the chessboard for i=0 to 7 for j=0 to 7 cells(i, j).cost = costs(i, j) cells(i, j).g = MAX_DIST cells(i, j).f = MAX_DIST cells(i, j).parent.row = -1 cells(i, j).parent.col = -1 next next cells(start.row, start.col).g = 0 cells(start.row, start.col).f = Heuristic(start, finish) AddCoordinates(opened, start) do while (opened.length > 0) aCell = GetOpened(opened, cells()) for i = -1 to 1 for j = -1 to 1 if ((i <> 0 or j <> 0) and IsACell(aCell.row + i, aCell.col + j)) then nCell.row = aCell.row + i nCell.col = aCell.col + j if (nCell.row = finish.row and nCell.col = finish.col) then 'The final cell is reached cells(finish.row, finish.col).g = cells(aCell.row, aCell.col).g + cells(finish.row, finish.col).cost cells(finish.row, finish.col).parent.row = aCell.row cells(finish.row, finish.col).parent.col = aCell.col exit do end if if (InList(nCell.row, nCell.col, opened) = 0 and InList(nCell.row, nCell.col, closed) = 0) then 'This cell was never visited before cells(nCell.row, nCell.col).g = cells(aCell.row, aCell.col).g + cells(nCell.row, nCell.col).cost cells(nCell.row, nCell.col).f = cells(nCell.row, nCell.col).g + Heuristic(nCell, finish) AddCoordinates(opened, nCell) cells(nCell.row, nCell.col).parent.row = aCell.row cells(nCell.row, nCell.col).parent.col = aCell.col else 'This cell was visited before, it's reopened only if the actual path is shortest of the previous valutation if (cells(aCell.row, aCell.col).g + cells(nCell.row, nCell.col).cost < cells(nCell.row, nCell.col).g) then cells(nCell.row, nCell.col).g = cells(aCell.row, aCell.col).g + cells(nCell.row, nCell.col).cost cells(nCell.row, nCell.col).f = cells(nCell.row, nCell.col).g + Heuristic(nCell, finish) AddCoordinates(opened, nCell) RemoveCoordinates(closed, nCell) cells(nCell.row, nCell.col).parent.row = aCell.row cells(nCell.row, nCell.col).parent.col = aCell.col end if end if end if next next 'The current cell is closed AddCoordinates(closed, aCell) RemoveCoordinates(opened, aCell) loop if (cells(finish.row, finish.col).parent.row >= 0) then 'A possible path was found 'Add the cells of the shortest path to the list 'path', proceding backward path.length = 0 aCell.row = finish.row aCell.col = finish.col do while (cells(aCell.row, aCell.col).parent.row >= 0) AppendCell(path, aCell) nCell.row = cells(aCell.row, aCell.col).parent.row aCell.col = cells(aCell.row, aCell.col).parent.col aCell.row = nCell.row loop 'Drawing the path for i = 0 to 7 for j = 0 to 7 if (costs(i,j) > 1) then print chr(219); elseif (InList(i, j, path)) then print "X"; else print "."; end if next print next 'Writing the cells sequence and the path length print print "Path: " for i = path.length to 1 step -1 print "("; path.coord(i).row; ","; path.coord(i).col; ")"; next print print print "Path cost: "; cells(finish.row, finish.col).g print else print "Path not found" end if end - Output:
X....... .X...... ..X.███. .X█...█. .X█...█. .X█████. ..X..... ...XXXXX Path: ( 1, 1)( 2, 2)( 3, 1)( 4, 1)( 5, 1)( 6, 2)( 7, 3)( 7, 4)( 7, 5)( 7, 6)( 7, 7) Path cost: 11
// Package astar implements the A* search algorithm with minimal constraints // on the graph representation. package astar import "container/heap" // Exported node type. type Node interface { To() []Arc // return list of arcs from this node to another Heuristic(from Node) int // heuristic cost from another node to this one } // An Arc, actually a "half arc", leads to another node with integer cost. type Arc struct { To Node Cost int } // rNode holds data for a "reached" node type rNode struct { n Node from Node l int // route len g int // route cost f int // "g+h", route cost + heuristic estimate fx int // heap.Fix index } type openHeap []*rNode // priority queue // Route computes a route from start to end nodes using the A* algorithm. // // The algorithm is general A*, where the heuristic is not required to be // monotonic. If a route exists, the function will find a route regardless // of the quality of the Heuristic. For an admissiable heuristic, the route // will be optimal. func Route(start, end Node) (route []Node, cost int) { // start node initialized with heuristic cr := &rNode{n: start, l: 1, f: end.Heuristic(start)} // maintain a set of reached nodes. start is reached initially r := map[Node]*rNode{start: cr} // oh is a heap of nodes "open" for exploration. nodes go on the heap // when they get an initial or new "g" route distance, and therefore a // new "f" which serves as priority for exploration. oh := openHeap{cr} for len(oh) > 0 { bestRoute := heap.Pop(&oh).(*rNode) bestNode := bestRoute.n if bestNode == end { // done. prepare return values cost = bestRoute.g route = make([]Node, bestRoute.l) for i := len(route) - 1; i >= 0; i-- { route[i] = bestRoute.n bestRoute = r[bestRoute.from] } return } l := bestRoute.l + 1 for _, to := range bestNode.To() { // "g" route distance from start g := bestRoute.g + to.Cost if alt, ok := r[to.To]; !ok { // alt being reached for the first time alt = &rNode{n: to.To, from: bestNode, l: l, g: g, f: g + end.Heuristic(to.To)} r[to.To] = alt heap.Push(&oh, alt) } else { if g >= alt.g { continue // candidate route no better than existing route } // it's a better route // update data and make sure it's on the heap alt.from = bestNode alt.l = l alt.g = g alt.f = end.Heuristic(alt.n) if alt.fx < 0 { heap.Push(&oh, alt) } else { heap.Fix(&oh, alt.fx) } } } } return nil, 0 } // implement container/heap func (h openHeap) Len() int { return len(h) } func (h openHeap) Less(i, j int) bool { return h[i].f < h[j].f } func (h openHeap) Swap(i, j int) { h[i], h[j] = h[j], h[i] h[i].fx = i h[j].fx = j } func (p *openHeap) Push(x interface{}) { h := *p fx := len(h) h = append(h, x.(*rNode)) h[fx].fx = fx *p = h } func (p *openHeap) Pop() interface{} { h := *p last := len(h) - 1 *p = h[:last] h[last].fx = -1 return h[last] } package main import ( "fmt" "astar" ) // rcNode implements the astar.Node interface type rcNode struct{ r, c int } var barrier = map[rcNode]bool{{2, 4}: true, {2, 5}: true, {2, 6}: true, {3, 6}: true, {4, 6}: true, {5, 6}: true, {5, 5}: true, {5, 4}: true, {5, 3}: true, {5, 2}: true, {4, 2}: true, {3, 2}: true} // graph representation is virtual. Arcs from a node are generated when // requested, but there is no static graph representation. func (fr rcNode) To() (a []astar.Arc) { for r := fr.r - 1; r <= fr.r+1; r++ { for c := fr.c - 1; c <= fr.c+1; c++ { if (r == fr.r && c == fr.c) || r < 0 || r > 7 || c < 0 || c > 7 { continue } n := rcNode{r, c} cost := 1 if barrier[n] { cost = 100 } a = append(a, astar.Arc{n, cost}) } } return a } // The heuristic computed is max of row distance and column distance. // This is effectively the cost if there were no barriers. func (n rcNode) Heuristic(fr astar.Node) int { dr := n.r - fr.(rcNode).r if dr < 0 { dr = -dr } dc := n.c - fr.(rcNode).c if dc < 0 { dc = -dc } if dr > dc { return dr } return dc } func main() { route, cost := astar.Route(rcNode{0, 0}, rcNode{7, 7}) fmt.Println("Route:", route) fmt.Println("Cost:", cost) } - Output:
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}] Cost: 11 The simplest standalone FIFO priority queue is implemented after Sleator and Tarjan in Louis Wasserman's "Playing with Priority Queues"[1].
{-# language DeriveFoldable #-} module PQueue where data PQueue a = EmptyQueue | Node (Int, a) (PQueue a) (PQueue a) deriving (Show, Foldable) 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 h <> EmptyQueue = h entry :: Ord a => a -> Int -> PQueue a entry x w = Node (w, x) EmptyQueue EmptyQueue enque :: Ord a => PQueue a -> a -> Int -> PQueue a enque q x w = if x `notElem` q then entry x w <> q else q deque :: Ord a => PQueue a -> Maybe (a, PQueue a) deque q = case q of EmptyQueue -> Nothing Node (_, x) l r -> Just (x, l <> r) The simple graph combinators:
import PQueue import Data.Map (Map(..)) import qualified Data.Map as Map import Data.List (unfoldr) newtype Graph n = Graph { links :: n -> Map n Int } grid :: Int -> Int -> Graph (Int,Int) grid a b = Graph $ \(x,y) -> let links = [((x+dx,y+dy), dx*dx+dy*dy) | dx <- [-1..1], dy <- [-1..1] , not (dx == 0 && dy == 0) , 0 <= x+dx && x+dx <= a , 0 <= y+dy && y+dy <= b] in Map.fromList links withHole :: (Foldable t, Ord n) => Graph n -> t n -> Graph n withHole (Graph g) ns = Graph $ \x -> if x `elem` ns then Map.empty else foldr Map.delete (g x) ns Finally, the search algorithm, as given in Wikipedia.
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 , gScore :: Map n Int , openSet :: PQueue n } findPath :: Ord n => Graph n -> (n -> n -> Int) -> n -> n -> [n] findPath (Graph links) metric start goal = loop a0 where a0 = SetData { cameFrom = mempty , gScore = Map.singleton start 0 , openSet = entry start (h start) } h = metric goal dist = get . links loop a = case deque (openSet a) of Nothing -> [] Just (current, q') -> if current == goal then getPath (cameFrom a) else loop a' where a' = Map.foldlWithKey go a { openSet = q' } neighbours neighbours = links current go a n _ = let g = get $ gScore a trial_Score = g current + dist current n in if trial_Score >= g n then a else SetData ( set (cameFrom a) n current ) ( set (gScore a) n trial_Score ) ( openSet a `enque` n $ trial_Score + h n ) getPath m = reverse $ goal : unfoldr go goal where go n = (\x -> (x,x)) <$> Map.lookup n m Example
distL1 (x,y) (a,b) = max (abs $ x-a) (abs $ y-b) main = let g = grid 9 9 `withHole` wall wall = [ (2,4),(2,5),(2,6),(3,6) , (4,6),(5,6),(5,5),(5,4) , (5,3),(5,2),(3,2),(4,2) ] path = shortestPath g distL1 (1,1) (7,7) picture = [ [ case (i,j) of p | p `elem` path -> '*' | p `elem` wall -> '#' | otherwise -> ' ' | i <- [0..8] ] | j <- [0..8] ] in do print path mapM_ putStrLn picture putStrLn $ "Path score: " <> show (length path) λ> 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
Implementation:
barrier=: 2 4,2 5,2 6,3 6,4 6,5 6,5 5,5 4,5 3,5 2,4 2,:3 2 price=: _,.~_,~100 barrier} 8 8$1 dirs=: 0 0-.~>,{,~<i:1 start=: 0 0 end=: 7 7 next=: {{ frontier=. ~.locs=. ,/dests=. ($price)|"1 ({:"2 y)+"1/dirs paths=. ,/y,"2 1/"2 dests costs=. ,x+(<"1 dests){price deals=. (1+locs <.//. costs) <. (<"1 frontier) { values keep=. costs < (frontier i. locs) { deals (keep#costs);keep#paths }} Asrch=: {{ values=: ($price)$_ best=: ($price)$a: paths=: ,:,:start costs=: ,0 while. #paths do. dests=. <"1 {:"2 paths values=: costs dests} values best=: (<"2 paths) dests} best 'costs paths'=.costs next paths end. ((<end){values) ; (<end){best }} Task example:
Asrch'' ┌──┬───┐ │11│0 0│ │ │1 1│ │ │2 2│ │ │3 1│ │ │4 1│ │ │5 1│ │ │6 2│ │ │7 3│ │ │7 4│ │ │7 5│ │ │7 6│ │ │7 7│ └──┴───┘ 'A B'=: Asrch'' 'x' (<"1 B)} '. #'{~(i.~~.@,) price x....... .x...... ..x.###. .x#...#. .x#...#. .x#####. ..x..... ...xxxxx Note that this is based on a literal reading of the task, where we are paying a cost to move into a new square -- here, we are not paying for the cost of the start square, because we never move into that square. If we paid to move into the start square, the final cost would have to include that price.
Replaced the bad program with a working copy (that I got from my friends)
import java.util.List; import java.util.ArrayList; import java.util.Collections; public class AStar { private final List<Node> open; private final List<Node> closed; private final List<Node> path; private final int[][] maze; private Node now; private final int xstart; private final int ystart; private int xend, yend; private final boolean diag; /** Node class */ static class Node implements Comparable<Node> { public Node parent; public int x, y; public double g; public double h; Node(Node parent, int xpos, int ypos, double g, double h) { this.parent = parent; this.x = xpos; this.y = ypos; this.g = g; this.h = h; } @Override public int compareTo(Node that) { return Double.compare(this.g + this.h, that.g + that.h); } } public AStar(int[][] maze, int xstart, int ystart, boolean diag) { this.open = new ArrayList<>(); this.closed = new ArrayList<>(); this.path = new ArrayList<>(); this.maze = maze; this.now = new Node(null, xstart, ystart, 0, 0); this.xstart = xstart; this.ystart = ystart; this.diag = diag; } /** Finds path to xend/yend or returns null */ public List<Node> findPathTo(int xend, int yend) { this.xend = xend; this.yend = yend; this.closed.add(this.now); addNeighborsToOpenList(); while (this.now.x != this.xend || this.now.y != this.yend) { if (this.open.isEmpty()) { return null; } this.now = this.open.remove(0); this.closed.add(this.now); addNeighborsToOpenList(); } this.path.add(0, this.now); while (this.now.parent != null) { this.now = this.now.parent; this.path.add(0, this.now); } return this.path; } private static boolean findNeighborInList(List<Node> list, Node node) { return list.stream().anyMatch(n -> n.x == node.x && n.y == node.y); } private double distance(int dx, int dy) { int x = this.now.x + dx; int y = this.now.y + dy; if (this.diag) { return Math.hypot(x - this.xend, y - this.yend); } else { return Math.abs(x - this.xend) + Math.abs(y - this.yend); } } private void addNeighborsToOpenList() { for (int x = -1; x <= 1; x++) { for (int y = -1; y <= 1; y++) { if (x == 0 && y == 0) continue; if (!this.diag && x != 0 && y != 0) continue; int nx = this.now.x + x; int ny = this.now.y + y; if (nx < 0 || ny < 0 || ny >= maze.length || nx >= maze[0].length) continue; if (maze[ny][nx] == -1) continue; Node node = new Node(this.now, nx, ny, this.now.g + 1.0 + maze[ny][nx], distance(x, y)); if (!findNeighborInList(this.open, node) && !findNeighborInList(this.closed, node)) { this.open.add(node); } } } Collections.sort(this.open); } public static void main(String[] args) { int[][] maze = { {0, 0, 0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0, 0, 0}, {0, 0, 0,100,100,100, 0, 0}, {0, 0, 0, 0, 0,100, 0, 0}, {0, 0,100, 0, 0,100, 0, 0}, {0, 0,100, 0, 0,100, 0, 0}, {0, 0,100,100,100,100, 0, 0}, {0, 0, 0, 0, 0, 0, 0, 0}, }; AStar as = new AStar(maze, 0, 0, true); List<Node> path = as.findPathTo(7, 7); if (path != null) { for (Node n : path) { System.out.print("[" + n.x + ", " + n.y + "] "); maze[n.y][n.x] = -1; } System.out.printf("\nTotal cost: %.02f\n", path.get(path.size() - 1).g); for (int[] maze_row : maze) { for (int maze_entry : maze_row) { switch (maze_entry) { case 0: System.out.print("_"); break; case -1: System.out.print("*"); break; default: System.out.print("#"); break; } } System.out.println(); } } else { System.out.println("No path found."); } } } - Output:
[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 *****___ _____*__ ___###*_ _____#_* __#__#*_ __#__#*_ __####*_ _______*
Animated.
To see how it works on a random map go 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; function findNeighbour( arr, n ) { var a; for( var i = 0; i < arr.length; i++ ) { a = arr[i]; if( n.x === a.x && n.y === a.y ) return i; } return -1; } function addNeighbours( cur ) { var p; for( var i = 0; i < neighbours.length; i++ ) { var n = {x: cur.x + neighbours[i].x, y: cur.y + neighbours[i].y, g: 0, h: 0, prt: {x:cur.x, y:cur.y}}; if( map[n.x][n.y] == 1 || findNeighbour( clsd, n ) > -1 ) continue; n.g = cur.g + neighbours[i].c; n.h = Math.abs( goal.x - n.x ) + Math.abs( goal.y - n.y ); p = findNeighbour( opn, n ); if( p > -1 && opn[p].g + opn[p].h <= n.g + n.h ) continue; opn.push( n ); } opn.sort( function( a, b ) { return ( a.g + a.h ) - ( b.g + b.h ); } ); } function createPath() { path = []; var a, b; a = clsd.pop(); path.push( a ); while( clsd.length ) { b = clsd.pop(); if( b.x != a.prt.x || b.y != a.prt.y ) continue; a = b; path.push( a ); } } function solveMap() { drawMap(); if( opn.length < 1 ) { document.body.appendChild( document.createElement( "p" ) ).innerHTML = "Impossible!"; return; } var cur = opn.splice( 0, 1 )[0]; clsd.push( cur ); if( cur.x == goal.x && cur.y == goal.y ) { createPath(); drawMap(); return; } addNeighbours( cur ); requestAnimationFrame( solveMap ); } function drawMap() { ctx.fillStyle = "#ee6"; ctx.fillRect( 0, 0, 200, 200 ); for( var j = 0; j < mh; j++ ) { for( var i = 0; i < mw; i++ ) { switch( map[i][j] ) { case 0: continue; case 1: ctx.fillStyle = "#990"; break; case 2: ctx.fillStyle = "#090"; break; case 3: ctx.fillStyle = "#900"; break; } ctx.fillRect( i, j, 1, 1 ); } } var a; if( path.length ) { var txt = "Path: " + ( path.length - 1 ) + "<br />["; for( var i = path.length - 1; i > -1; i-- ) { a = path[i]; ctx.fillStyle = "#999"; ctx.fillRect( a.x, a.y, 1, 1 ); txt += "(" + a.x + ", " + a.y + ") "; } document.body.appendChild( document.createElement( "p" ) ).innerHTML = txt + "]"; return; } for( var i = 0; i < opn.length; i++ ) { a = opn[i]; ctx.fillStyle = "#909"; ctx.fillRect( a.x, a.y, 1, 1 ); } for( var i = 0; i < clsd.length; i++ ) { a = clsd[i]; ctx.fillStyle = "#009"; ctx.fillRect( a.x, a.y, 1, 1 ); } } function createMap() { map = new Array( mw ); for( var i = 0; i < mw; i++ ) { map[i] = new Array( mh ); for( var j = 0; j < mh; j++ ) { if( !i || !j || i == mw - 1 || j == mh - 1 ) map[i][j] = 1; else map[i][j] = 0; } } 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; } function init() { var canvas = document.createElement( "canvas" ); canvas.width = canvas.height = 200; ctx = canvas.getContext( "2d" ); ctx.scale( 20, 20 ); 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} ]; path = []; createMap(); opn.push( start ); solveMap(); } - Output:
Path: 11 [(1, 1) (2, 2) (2, 3) (2, 4) (2, 5) (2, 6) (3, 7) (4, 8) (5, 8) (6, 8) (7, 8) (8, 8) ]
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,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)); - Output:
[ [ 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 ] ]
The graphic in this solution is displayed in the more standard orientation of origin at bottom left and goal at top right.
using LightGraphs, SimpleWeightedGraphs const chessboardsize = 8 const givenobstacles = [(2,4), (2,5), (2,6), (3,6), (4,6), (5,6), (5,5), (5,4), (5,3), (5,2), (4,2), (3,2)] vfromcart(p, n) = (p[1] - 1) * n + p[2] const obstacles = [vfromcart(o .+ 1, chessboardsize) for o in givenobstacles] zbasedpath(path, n) = [(div(v - 1, n), (v - 1) % n) for v in path] pathcost(path) = sum(map(x -> x in obstacles ? 100 : 1, path[2:end])) function surround(x, y, n) bottomx = x > 1 ? x -1 : x topx = x < n ? x + 1 : x bottomy = y > 1 ? y - 1 : y topy = y < n ? y + 1 : y [CartesianIndex(x,y) for x in bottomx:topx for y in bottomy:topy] end function kinggraph(N) graph = SimpleWeightedGraph(N*N) for row in 1:N, col in 1:N, p in surround(row, col, N) origin = vfromcart(CartesianIndex(row, col), N) targ = vfromcart(p, N) hcost = (targ in obstacles || origin in obstacles) ? 100 : 1 add_edge!(graph, origin, targ, hcost) end graph end kgraph = kinggraph(chessboardsize) path = enumerate_paths(dijkstra_shortest_paths(kgraph, 1), 64) println("Solution has cost $(pathcost(path)):\n", zbasedpath(path, chessboardsize)) path2graphic(x, path) = (x in obstacles ? '█' : x in path ? 'x' : '.') for row in 8:-1:1, col in 7:-1:0 print(path2graphic(row*8 - col, path)) if col == 0 println() end end - Output:
Solution has cost 11: Tuple{Int64,Int64}[(0, 0), (1, 1), (2, 2), (3, 1), (4, 1), (5, 1), (6, 2), (7, 3), (7, 4), (6, 5), (6, 6), (7, 7)] ...xx..x ..x..xx. .x█████. .x█...█. .x█...█. ..x.███. .x...... x.......
import java.lang.Math.abs typealias GridPosition = Pair<Int, Int> typealias Barrier = Set<GridPosition> const val MAX_SCORE = 99999999 abstract class Grid(private val barriers: List<Barrier>) { open fun heuristicDistance(start: GridPosition, finish: GridPosition): Int { val dx = abs(start.first - finish.first) val dy = abs(start.second - finish.second) return (dx + dy) + (-2) * minOf(dx, dy) } fun inBarrier(position: GridPosition) = barriers.any { it.contains(position) } abstract fun getNeighbours(position: GridPosition): List<GridPosition> open fun moveCost(from: GridPosition, to: GridPosition) = if (inBarrier(to)) MAX_SCORE else 1 } class SquareGrid(width: Int, height: Int, barriers: List<Barrier>) : Grid(barriers) { private val heightRange: IntRange = (0 until height) private val widthRange: IntRange = (0 until width) private val validMoves = listOf(Pair(1, 0), Pair(-1, 0), Pair(0, 1), Pair(0, -1), Pair(1, 1), Pair(-1, 1), Pair(1, -1), Pair(-1, -1)) override fun getNeighbours(position: GridPosition): List<GridPosition> = validMoves .map { GridPosition(position.first + it.first, position.second + it.second) } .filter { inGrid(it) } private fun inGrid(it: GridPosition) = (it.first in widthRange) && (it.second in heightRange) } /** * Implementation of the A* Search Algorithm to find the optimum path between 2 points on a grid. * * The Grid contains the details of the barriers and methods which supply the neighboring vertices and the * cost of movement between 2 cells. Examples use a standard Grid which allows movement in 8 directions * (i.e. includes diagonals) but alternative implementation of Grid can be supplied. * */ fun aStarSearch(start: GridPosition, finish: GridPosition, grid: Grid): Pair<List<GridPosition>, Int> { /** * Use the cameFrom values to Backtrack to the start position to generate the path */ fun generatePath(currentPos: GridPosition, cameFrom: Map<GridPosition, GridPosition>): List<GridPosition> { val path = mutableListOf(currentPos) var current = currentPos while (cameFrom.containsKey(current)) { current = cameFrom.getValue(current) path.add(0, current) } return path.toList() } val openVertices = mutableSetOf(start) val closedVertices = mutableSetOf<GridPosition>() val costFromStart = mutableMapOf(start to 0) val estimatedTotalCost = mutableMapOf(start to grid.heuristicDistance(start, finish)) val cameFrom = mutableMapOf<GridPosition, GridPosition>() // Used to generate path by back tracking while (openVertices.size > 0) { val currentPos = openVertices.minBy { estimatedTotalCost.getValue(it) }!! // Check if we have reached the finish if (currentPos == finish) { // Backtrack to generate the most efficient path val path = generatePath(currentPos, cameFrom) return Pair(path, estimatedTotalCost.getValue(finish)) // First Route to finish will be optimum route } // Mark the current vertex as closed openVertices.remove(currentPos) closedVertices.add(currentPos) grid.getNeighbours(currentPos) .filterNot { closedVertices.contains(it) } // Exclude previous visited vertices .forEach { neighbour -> val score = costFromStart.getValue(currentPos) + grid.moveCost(currentPos, neighbour) if (score < costFromStart.getOrDefault(neighbour, MAX_SCORE)) { if (!openVertices.contains(neighbour)) { openVertices.add(neighbour) } cameFrom.put(neighbour, currentPos) costFromStart.put(neighbour, score) estimatedTotalCost.put(neighbour, score + grid.heuristicDistance(neighbour, finish)) } } } throw IllegalArgumentException("No Path from Start $start to Finish $finish") } fun main(args: Array<String>) { val barriers = listOf(setOf( Pair(2,4), Pair(2,5), Pair(2,6), Pair(3,6), Pair(4,6), Pair(5,6), Pair(5,5), Pair(5,4), Pair(5,3), Pair(5,2), Pair(4,2), Pair(3,2))) val (path, cost) = aStarSearch(GridPosition(0,0), GridPosition(7,7), SquareGrid(8,8, barriers)) println("Cost: $cost Path: $path") } - Output:
Cost: 11 Path: [(0, 0), (1, 1), (2, 2), (3, 1), (4, 1), (5, 1), (6, 2), (6, 3), (6, 4), (6, 5), (6, 6), (7, 7)]
-- QUEUE ----------------------------------------------------------------------- Queue = {} function Queue:new() local q = {} self.__index = self return setmetatable( q, self ) end function Queue:push( v ) table.insert( self, v ) end function Queue:pop() return table.remove( self, 1 ) end function Queue:getSmallestF() local s, i = nil, 2 while( self[i] ~= nil and self[1] ~= nil ) do if self[i]:F() < self[1]:F() then s = self[1] self[1] = self[i] self[i] = s end i = i + 1 end return self:pop() end -- LIST ------------------------------------------------------------------------ List = {} function List:new() local l = {} self.__index = self return setmetatable( l, self ) end function List:push( v ) table.insert( self, v ) end function List:pop() return table.remove( self ) end -- POINT ----------------------------------------------------------------------- Point = {} function Point:new() local p = { y = 0, x = 0 } self.__index = self return setmetatable( p, self ) end function Point:set( x, y ) self.x, self.y = x, y end function Point:equals( o ) return (o.x == self.x and o.y == self.y) end function Point:print() print( self.x, self.y ) end -- NODE ------------------------------------------------------------------------ Node = {} function Node:new() local n = { pos = Point:new(), parent = Point:new(), dist = 0, cost = 0 } self.__index = self return setmetatable( n, self ) end function Node:set( pt, parent, dist, cost ) self.pos = pt self.parent = parent self.dist = dist self.cost = cost end function Node:F() return ( self.dist + self.cost ) end -- 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, 1,0,0,0,0,1,1,1,0,1, 1,0,0,1,0,0,0,1,0,1, 1,0,0,1,0,0,0,1,0,1, 1,0,0,1,1,1,1,1,0,1, 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 ) function hasNode( arr, pos ) for nx, val in ipairs( arr ) do if val.pos:equals( pos ) then return nx end end return -1 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 function calcDist( p1 ) local x, y = goal.x - p1.x, goal.y - p1.y return math.abs( x ) + math.abs( y ) end function addToOpen( node ) local nx for n = 1, 8 do nNode = Node:new() nNode.parent:set( node.pos.x, node.pos.y ) nNode.pos:set( node.pos.x + nbours[n][1], node.pos.y + nbours[n][2] ) 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 ) if nx < 0 then 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 open:push( nNode ) else nNode = nil end end end end return false end function makePath() local i, l = #closed, List:new() local node, parent = closed[i], nil l:push( node.pos ) parent = node.parent while( i > 0 ) do i = i - 1 node = closed[i] if node ~= nil and node.pos:equals( parent ) then l:push( node.pos ) parent = node.parent end end print( string.format( "Cost: %d", #l - 1 ) ) io.write( "Path: " ) for i = #l, 1, -1 do map[l[i].x + mapW * l[i].y - mapW] = 2 io.write( string.format( "(%d, %d) ", l[i].x, l[i].y ) ) end print( "" ) end function aStar() local n = Node:new() n.dist = calcDist( start ) n.pos:set( start.x, start.y ) open:push( n ) while( true ) do local node = open:getSmallestF() if node == nil then break end closed:push( node ) if addToOpen( node ) == true then makePath() return true end end return false end -- ENTRY POINT ----------------------------------------------------------------- if true == aStar() then local m for j = 1, mapH do for i = 1, mapW do m = map[i + mapW * j - mapW] if m == 0 then io.write( "." ) elseif m == 1 then io.write( string.char(0xdb) ) else io.write( "x" ) end end io.write( "\n" ) end else print( "can not find a path!" ) end - Output:
Cost: 11 Path: (2, 2) (3, 3) (3, 4) (3, 5) (3, 6) (3, 7) (4, 8) (5, 9) (6, 9) (7, 9) (8, 9) (9, 9) ██████████ █x.......█ █.x......█ █.x..███.█ █.x█...█.█ █.x█...█.█ █.x█████.█ █..x.....█ █...xxxxx█ ██████████
Implementation of the Wikipedia pseudocode.
# A* search algorithm. from algorithm import reverse import sets import strformat import tables const Infinity = 1_000_000_000 type Cell = tuple[row, col: int] const 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)].toHashSet Moves = [(-1, -1), (-1, 0), (-1, 1), (0, 1), (1, 1), (1, 0), (1, -1), (0, -1)] #--------------------------------------------------------------------------------------------------- iterator neighbors(cell: Cell): Cell = ## Yield the neighbors of "cell". for move in Moves: let next = (row: cell.row + move[0], col: cell.col + move[1]) if next.row in 0..7 and next.col in 0..7: yield next #--------------------------------------------------------------------------------------------------- func d(current, neighbor: Cell): int = ## Return the cost for the move from "current" to "neighbor". if neighbor in Barriers: 100 else: 1 #--------------------------------------------------------------------------------------------------- func h(cell, goal: Cell): int = ## Compute the heuristic cost for a move form the cell to the goal. ## We use the Chebychev distance as appropriate for this kind of move. max(abs(goal.row - cell.row), abs(goal.col - cell.col)) #--------------------------------------------------------------------------------------------------- func reconstructedPath(cameFrom: Table[Cell, Cell]; current: Cell): seq[Cell] = ## Return the shortest path from the start to the goal. var cell = current result = @[cell] while cell in cameFrom: cell = cameFrom[cell] result.add(cell) result.reverse() #--------------------------------------------------------------------------------------------------- func search(start, goal: Cell): tuple[path: seq[Cell], cost: int] = ## Search the shortest path from "start" to "goal" using A* algorithm. ## Return the path and the cost. var openSet = [start].toHashSet() var visited: HashSet[Cell] var cameFrom: Table[Cell, Cell] var gScore, fScore: Table[Cell, int] gscore[start] = 0 fScore[start] = h(start, goal) while openSet.len != 0: # Find cell in "openset" with minimal "fScore". var current: Cell var minScore = Infinity for cell in openSet: let score = fScore.getOrDefault(cell, Infinity) if score < minScore: current = cell minScore = score if current == goal: # Return the path and cost. return (reconstructedPath(cameFrom, current), fScore[goal]) openSet.excl(current) visited.incl(current) # Update scores for neighbors. for neighbor in current.neighbors(): if neighbor in visited: # Already processed. continue let tentative = gScore[current] + d(current, neighbor) if tentative < gScore.getOrDefault(neighbor, Infinity): cameFrom[neighbor]= current gScore[neighbor] = tentative fScore[neighbor] = tentative + h(neighbor, goal) openSet.incl(neighbor) #--------------------------------------------------------------------------------------------------- proc drawBoard(path: seq[Cell]) = ## Draw the board and the path. func `$`(cell: Cell): string = ## Return the Unicode string to use for a cell. if cell in Barriers: "██" else: (if cell in path: " #" else: " ·") echo "████████████████████" for row in 0..7: stdout.write("██") for col in 0..7: stdout.write((row, col)) stdout.write("██\n") echo "████████████████████" echo '\n' #--------------------------------------------------------------------------------------------------- proc printSolution(path: seq[Cell]; cost: int) = ## Print the solution. var pathLine = "Path: " let start = pathLine.len for cell in path: pathLine.addSep(" → ", start) pathLine.add(&"({cell.row}, {cell.col})") echo pathLine echo(&"Cost: {cost}\n\n") drawBoard(path) #--------------------------------------------------------------------------------------------------- let (path, cost) = search((0, 0), (7, 7)) if cost == 0: echo "No solution found" else: printSolution(path, cost) - Output:
Path: (0, 0) → (1, 1) → (2, 2) → (3, 1) → (4, 1) → (5, 1) → (6, 2) → (7, 3) → (7, 4) → (6, 5) → (7, 6) → (7, 7) Cost: 11 ████████████████████ ██ # · · · · · · ·██ ██ · # · · · · · ·██ ██ · · # ·██████ ·██ ██ · #██ · · ·██ ·██ ██ · #██ · · ·██ ·██ ██ · #██████████ ·██ ██ · · # · · # · ·██ ██ · · · # # · # #██ ████████████████████
A very close/straightforward implementation of the Wikipedia pseudocode.
module IntPairs = struct type t = int * int let compare (x0,y0) (x1,y1) = match Stdlib.compare x0 x1 with | 0 -> Stdlib.compare y0 y1 | c -> c end module PairsMap = Map.Make(IntPairs) module PairsSet = Set.Make(IntPairs) let find_path start goal board = let max_y = Array.length board in let max_x = Array.length board.(0) in let get_neighbors (x, y) = let moves = [(0, 1); (0, -1); (1, 0); (-1, 0); (1, 1); (1, -1); (-1, 1); (-1, -1)] in let ms = List.map (fun (_x, _y) -> x+_x, y+_y) moves in let ms = List.filter (fun (x, y) -> x >= 0 && x < max_x && y >= 0 && y < max_y && board.(y).(x) <> 0 ) ms in (ms) in let h (x0, y0) (x1, y1) = abs (x0 - x1) + abs (y0 - y1) in let openSet = PairsSet.add start PairsSet.empty in let closedSet = PairsSet.empty in let fScore = PairsMap.add start (h goal start) PairsMap.empty in let gScore = PairsMap.add start 0 PairsMap.empty in let cameFrom = PairsMap.empty in let reconstruct_path cameFrom current = let rec aux acc current = let from = PairsMap.find current cameFrom in if from = start then (from::acc) else aux (from::acc) from in aux [current] current in let d current neighbor = let x, y = neighbor in board.(y).(x) in let g gScore cell = match PairsMap.find_opt cell gScore with | Some v -> v | None -> max_int in let rec _find_path (openSet, closedSet, fScore, gScore, cameFrom) = if PairsSet.is_empty openSet then None else let current = PairsSet.fold (fun p1 p2 -> if p2 = (-1, -1) then p1 else let s1 = PairsMap.find p1 fScore and s2 = PairsMap.find p2 fScore in if s1 < s2 then p1 else p2 ) openSet (-1, -1) in if current = goal then Some (reconstruct_path cameFrom current) else let openSet = PairsSet.remove current openSet in let closedSet = PairsSet.add current closedSet in let neighbors = get_neighbors current in neighbors |> List.fold_left (fun ((openSet, closedSet, fScore, gScore, cameFrom) as v) neighbor -> if PairsSet.mem neighbor closedSet then (v) else let tentative_gScore = (g gScore current) + (d current neighbor) in if tentative_gScore < (g gScore neighbor) then let cameFrom = PairsMap.add neighbor current cameFrom in let gScore = PairsMap.add neighbor tentative_gScore gScore in let f = (g gScore neighbor) + (h neighbor goal) in let fScore = PairsMap.add neighbor f fScore in let openSet = if not (PairsSet.mem neighbor openSet) then PairsSet.add neighbor openSet else openSet in (openSet, closedSet, fScore, gScore, cameFrom) else (v) ) (openSet, closedSet, fScore, gScore, cameFrom) |> _find_path in _find_path (openSet, closedSet, fScore, gScore, cameFrom) let () = let board = [| [| 1; 1; 1; 1; 1; 1; 1; 1; |]; [| 1; 1; 1; 1; 1; 1; 1; 1; |]; [| 1; 1; 1; 0; 0; 0; 1; 1; |]; [| 1; 1; 1; 1; 1; 0; 1; 1; |]; [| 1; 1; 0; 1; 1; 0; 1; 1; |]; [| 1; 1; 0; 1; 1; 0; 1; 1; |]; [| 1; 1; 0; 0; 0; 0; 1; 1; |]; [| 1; 1; 1; 1; 1; 1; 1; 1; |]; |] in let start = (0, 0) in let goal = (7, 7) in let dim_x = Array.length board.(0) in let dim_y = Array.length board in let r = find_path start goal board in match r with | None -> failwith "path not found" | Some p -> List.iter (fun (x, y) -> Printf.printf " (%d, %d)\n" x y) p; print_newline (); let _board = Array.init dim_y (fun y -> Array.init dim_x (fun x -> if board.(y).(x) = 0 then '#' else '.')) in List.iter (fun (x, y) -> _board.(y).(x) <- '*') p; Array.iter (fun line -> Array.iter (fun c -> Printf.printf " %c" c; ) line; print_newline () ) _board; print_newline () - Output:
(0, 0) (1, 1) (2, 2) (2, 3) (1, 4) (1, 5) (1, 6) (2, 7) (3, 7) (4, 7) (5, 7) (6, 7) (7, 7) * . . . . . . . . * . . . . . . . . * # # # . . . . * . . # . . . * # . . # . . . * # . . # . . . * # # # # . . . . * * * * * *
; level: list of lists, any except 1 means the cell is empty ; from: start cell in (x . y) mean ; to: destination cell in (x . y) mean (define (A* level from to) (define (hash xy) ; internal hash (+ (<< (car xy) 16) (cdr xy))) ; "is the cell is empty?" (define (floor? x y) (let ((line (list-ref level y))) (if line (not (eq? (list-ref line x) 1))))) (unless (equal? from to) ; search not finished yet (let step1 ((n 999) ; maximal count of search steps (c-list-set #empty) (o-list-set (put #empty (hash from) [from #f 0 0 0]))) (unless (empty? o-list-set) ; do we have a space to move? ; no. let's find cell with minimal const (let*((f (ff-fold (lambda (s key value) (if (< (ref value 5) (car s)) (cons (ref value 5) value) s)) (cons 9999 #f) o-list-set)) (xy (ref (cdr f) 1)) ; move the cell from "open" to "closed" list (o-list-set (del o-list-set (hash xy))) (c-list-set (put c-list-set (hash xy) (cdr f)))) ; (if (or (eq? n 0) (equal? xy to)) (let rev ((xy xy)) ; let's unroll the math and return only first step (let*((parent (ref (get c-list-set (hash xy) #f) 2)) (parent-of-parent (ref (get c-list-set (hash parent) #f) 2))) (if parent-of-parent (rev parent) (cons (- (car xy) (car parent)) (- (cdr xy) (cdr parent)))))) (let*((x (car xy)) (y (cdr xy)) (o-list-set (fold (lambda (n v) (if (and (floor? (car v) (cdr v)) (eq? #f (get c-list-set (hash v) #f))) (let ((G (+ (ref (get c-list-set (hash xy) #f) 3) 1)); G of parent + 1 ; H calculated by "Manhattan method" (H (* (+ (abs (- (car v) (car to))) (abs (- (cdr v) (cdr to)))) 2)) (got (get o-list-set (hash v) #f))) (if got (if (< G (ref got 3)) (put n (hash v) [v xy G H (+ G H)]) n) (put n (hash v) [v xy G H (+ G H)]))) n)) o-list-set (list (cons x (- y 1)) (cons x (+ y 1)) (cons (- x 1) y) (cons (+ x 1) y))))) (step1 (- n 1) c-list-set o-list-set)))))))) - Output:
(define level '( (1 1 1 1 1 1 1 1 1 1) (1 A 0 0 0 0 0 0 0 1) (1 0 0 0 0 0 0 0 0 1) (1 0 0 0 0 1 1 1 0 1) (1 1 0 0 0 0 0 1 0 1) (1 0 0 1 0 0 0 1 0 1) (1 0 0 1 1 1 1 1 0 1) (1 0 0 0 0 0 0 0 0 1) (1 0 0 0 1 0 0 0 B 1) (1 1 1 1 1 1 1 1 1 1) )) (for-each print level) ; let's check that we can't move to (into wall) (print (A* level '(1 . 1) '(9 . 9))) (define to '(8 . 8)) (define (plus a b) (cons (+ (car a) (car b)) (+ (cdr a) (cdr b)))) ; helper (define path (let loop ((me '(1 . 1)) (path '())) (if (equal? me to) (begin (print "here I am!") (cons to path)) (let ((move (A* level me to))) (unless move (begin (print "no way, sorry :(") #false) (let ((step (plus me move))) (print me " + " move " -> " step) (loop step (cons me path)))))))) ; let's draw the path? (define (has? lst x) ; helper (cond ((null? lst) #false) ((equal? (car lst) x) lst) (else (has? (cdr lst) x)))) (define solved (map (lambda (row y) (map (lambda (cell x) (cond ((equal? (cons x y) '(1 . 1)) "A") ((equal? (cons x y) '(8 . 8)) "B") ((has? path (cons x y)) "*") (else cell))) row (iota 10))) level (iota 10))) (for-each print solved) the map: (1 1 1 1 1 1 1 1 1 1) (1 A 0 0 0 0 0 0 0 1) (1 0 0 0 0 0 0 0 0 1) (1 0 0 0 0 1 1 1 0 1) (1 1 0 0 0 0 0 1 0 1) (1 0 0 1 0 0 0 1 0 1) (1 0 0 1 1 1 1 1 0 1) (1 0 0 0 0 0 0 0 0 1) (1 0 0 0 1 0 0 0 B 1) (1 1 1 1 1 1 1 1 1 1) we should not reach the '(9 . 9) cell: #false ok, we got #false, so really can't. now try to reach cell '(8 . 8) - the 'B' point: (1 . 1) + (0 . 1) -> (1 . 2) (1 . 2) + (0 . 1) -> (1 . 3) (1 . 3) + (1 . 0) -> (2 . 3) (2 . 3) + (0 . 1) -> (2 . 4) (2 . 4) + (0 . 1) -> (2 . 5) (2 . 5) + (0 . 1) -> (2 . 6) (2 . 6) + (0 . 1) -> (2 . 7) (2 . 7) + (1 . 0) -> (3 . 7) (3 . 7) + (1 . 0) -> (4 . 7) (4 . 7) + (1 . 0) -> (5 . 7) (5 . 7) + (0 . 1) -> (5 . 8) (5 . 8) + (1 . 0) -> (6 . 8) (6 . 8) + (1 . 0) -> (7 . 8) (7 . 8) + (1 . 0) -> (8 . 8) here I am! (1 1 1 1 1 1 1 1 1 1) (1 A 0 0 0 0 0 0 0 1) (1 * 0 0 0 0 0 0 0 1) (1 * * 0 0 1 1 1 0 1) (1 1 * 0 0 0 0 1 0 1) (1 0 * 1 0 0 0 1 0 1) (1 0 * 1 1 1 1 1 0 1) (1 0 * * * * 0 0 0 1) (1 0 0 0 1 * * * B 1) (1 1 1 1 1 1 1 1 1 1)
/* REXX */ Parse Arg which If which='' Then /* original input */ Call init0 Else /* extended bars */ Call init Do row=0 To 7 maze=translate(row.row,' ','{,}') Do col=0 to 7 Parse var maze maze.row.col maze maz.row.col=maze.row.col End End maze_rows=8 maze_cols=8 open=.queue~new closed=.queue~new path=.queue~new parse Value '0 0' with xstart ystart parse Value '7 7' with xend yend now=.node~new(.nil,xstart,ystart,0,0) diag=1 oi=0 path=findPathTo(xend,yend) if path<>.nil Then Do Say "A minimum cost path:" i=0 l='' Do pp over path -- Say i pp~y pp~x '->' pp~g l=l '['pp~y',' pp~x']' cost.i=pp~g end Say strip(l) Say "Total cost:" cost.i l.1='+---+---+---+---+---+---+---+---+' Do r=1 To 8 lia=2*r lib=2*r+1 l.lia='| | | | | | | | |' l.lib='+---+---+---+---+---+---+---+---+' End Do r=0 To 7 Do c=0 To 7 If maz.r.c<>0 Then Do ll=(r+1)*2 cc=(c+1)*4 l.ll=overlay('X',l.ll,cc-1) End End End i=0 Do pp over path i+=1 r=pp~y c=pp~x ll=(r+1)*2 cc=(c+1)*4-2 Select When i=1 Then l.ll=overlay('-0-',l.ll,cc) When i=path~items Then l.ll=overlay('end',l.ll,cc) Otherwise l.ll=overlay(right(i-1,2),l.ll,cc) End end Do li=1 to 17 Say l.li End Do row=0 To 7 l='' Do col=0 To 7 Select When maze.row.col=0 Then l=l||'_' When maze.row.col=-1 Then l=l||'*' Otherwise l=l||'#' End End Say l End End Else Say "No path found." Exit findPathTo: closed~append(now) Call addNeighborsToOpenList Do while now~x<>xend | now~y<>yend If open~items=0 Then return .nil now = open~remove(open~last) closed~append(now) Call addNeighborsToOpenList End path~push(now) xp=now~x yp=now~y maze.yp.xp=-1 Do While now.parent<>.nil now = now~parent If now<>.nil Then do path~push(now) xp=now~x yp=now~y maze.yp.xp=-1 End Else Leave End Call showp return path addNeighborsToOpenList: Do x=-1 To 1 Do y=-1 To 1 If x=0 & y=0 Then Iterate If \diag & x<>0 & y<>0 Then Iterate nx=now~x + x; ny=now~y + y; if (nx<0 | ny<0 | ny>=maze_rows | nx>=maze_cols) Then Iterate If maze.ny.nx=-1 Then Iterate node=.node~new(now,nx,ny,now~g+1+maze.ny.nx,distance(x,y)) fo=findNeighborInList(node,open) fc=findNeighborInList(node,closed) If \fo & \fc Then Do open~append(node) End End End oi+=1 -- Call Showo 'B'oi Call sort_open -- Call Showo 'A'oi Return findNeighborInList: Use Arg n,nl Do no Over nl If n~x=no~x & n~y=no~y Then Return 1 End Return 0 distance: Parse Arg dx,dy xx=now~x+dx yy=now~y+dy If diag Then Return hypot(xx-xend,yy-yend) Else Return abs(xx-xend)+abs(yy-yend) hypot: Parse Arg k1,k2 Return rxCalcSqrt(k1**2+k2**2) init0: row.0='{0, 0, 0, 0, 0, 0, 0, 0},' row.1='{0, 0, 0, 0, 0, 0, 0, 0},' row.2='{0, 0, 0,100,100,100,0,0},' row.3='{0, 0, 0, 0, 0,100, 0, 0},' row.4='{0, 0,100, 0, 0,100, 0, 0},' row.5='{0, 0,100, 0, 0,100, 0, 0},' row.6='{0, 0,100,100,100,100, 0, 0},' row.7='{0, 0, 0, 0, 0, 0,0, 0},' Return init: row.0='{0, 0, 0, 0, 0, 0, 0, 0},' row.1='{0, 0, 0, 0, 0, 0, 0, 0},' row.2='{0, 0, 0,100,100,100,0,0},' row.3='{0, 0, 0, 0, 0,100, 0, 0},' row.4='{0, 0,100, 0, 0,100, 0, 0},' row.5='{0, 0,100, 0, 0,100, 0, 0},' row.6='{0,10,100,100,100,100,10,10},' row.7='{0, 0, 0, 0, 0, 0,0, 0},' Return showo: Parse Arg tag l='' Do e over open Say '['e~x',' e~y'] ->'e~gh l=l '['e~x',' e~y']' End Say " "tag "open"l Return showc: Call dbga arg(1) l='' Do e over close l=l '['e~x',' e~y']' End Call dbga " clos"l Return showp: Call dbga arg(1) l='' Do e over path If e<>.nil Then l=l '['e~x',' e~y']' End Call dbga " path"l Return sort_open: /* Sort the open queue ascending by gh */ i=0 a=.array~new as=.array~new Do e over open i+=1 a[i]=e End n=i as[1]=a[1] m=1 Do i=2 To n Do j=1 To m If a[i]~gh<=as[j]~gh Then Leave End Do k=m To j By -1 as[k+1]=as[k] End as[j]=a[i] m+=1 End Do i=1 To m Call dbg i as[i]~gh End open=.queue~new Do i=m To 1 By -1 open~append(as[i]) End Return ::Class node ::Attribute parent ::Attribute x ::Attribute y ::Attribute g ::Attribute h ::Attribute gh ::Method init Expose parent x y g h gh Use Arg p,xx,yy,gg,hh parent=p x=xx y=yy g=gg h=hh gh=g+h Call dbg "x="xx "y="yy "g="gg "h="hh "gh="gh ::method compareto ---- this didn't work. use strict arg left, right Select When left~gh<right~gh Then return -1 When left~gh=right~gh Then return 0 When left~gh>right~gh Then return 1 End ::Routine dbg public ::Routine dbga public Return ::Requires rxMath Library - Output:
G:\astar>rexx ast1 A minimum cost path: [0, 0] [1, 1] [2, 2] [1, 3] [1, 4] [1, 5] [2, 6] [3, 7] [4, 7] [5, 7] [6, 7] [7, 7] Total cost: 11 +---+---+---+---+---+---+---+---+ |-0-| | | | | | | | +---+---+---+---+---+---+---+---+ | | 1 | | 3 | 4 | 5 | | | +---+---+---+---+---+---+---+---+ | | | 2 | X | X | X | 6 | | +---+---+---+---+---+---+---+---+ | | | | | | X | | 7 | +---+---+---+---+---+---+---+---+ | | | X | | | X | | 8 | +---+---+---+---+---+---+---+---+ | | | X | | | X | | 9 | +---+---+---+---+---+---+---+---+ | | | X | X | X | X | |10 | +---+---+---+---+---+---+---+---+ | | | | | | | |end| +---+---+---+---+---+---+---+---+ *_______ _*_***__ __*###*_ _____#_* __#__#_* __#__#_* __####_* _______* G:\astar>rexx ast1 1 A minimum cost path: [0, 0] [1, 1] [2, 2] [3, 2] [4, 1] [5, 1] [6, 0] [7, 1] [7, 2] [7, 3] [7, 4] [7, 5] [7, 6] [7, 7] Total cost: 13 +---+---+---+---+---+---+---+---+ |-0-| | | | | | | | +---+---+---+---+---+---+---+---+ | | 1 | | | | | | | +---+---+---+---+---+---+---+---+ | | | 2 | X | X | X | | | +---+---+---+---+---+---+---+---+ | | | 3 | | | X | | | +---+---+---+---+---+---+---+---+ | | 4 | X | | | X | | | +---+---+---+---+---+---+---+---+ | | 5 | X | | | X | | | +---+---+---+---+---+---+---+---+ | 6 | X | X | X | X | X | X | X | +---+---+---+---+---+---+---+---+ | | 7 | 8 | 9 |10 |11 |12 |end| +---+---+---+---+---+---+---+---+ *_______ _*______ __*###__ __*__#__ _*#__#__ _*#__#__ *####### _*******
#!/usr/bin/perl use strict; # https://rosettacode.org/wiki/A*_search_algorithm use warnings; use List::AllUtils qw( nsort_by ); sub distance { my ($r1, $c1, $r2, $c2) = split /[, ]/, "@_"; sqrt( ($r1-$r2)**2 + ($c1-$c2)**2 ); } my $start = '0,0'; my $finish = '7,7'; my %barrier = map {$_, 100} split ' ', '2,4 2,5 2,6 3,6 4,6 5,6 5,5 5,4 5,3 5,2 4,2 3,2'; my %values = ( $start, 0 ); my @new = [ $start, 0 ]; my %from; my $mid; while( ! exists $values{$finish} and @new ) { my $pick = (shift @new)->[0]; for my $n ( nsort_by { distance($_, $finish) } # heuristic grep !/-|8/ && ! exists $values{$_}, glob $pick =~ s/\d+/{@{[$&-1]},$&,@{[$&+1]}}/gr ) { $from{$n} = $pick; $values{$n} = $values{$pick} + ( $barrier{$n} // 1 ); my $new = [ $n, my $dist = $values{$n} ]; my $low = 0; # binary insertion into @new (the priority queue) my $high = @new; $new[$mid = $low + $high >> 1][1] <= $dist ? ($low = $mid + 1) : ($high = $mid) while $low < $high; splice @new, $low, 0, $new; # insert in order } } my $grid = "s.......\n" . ('.' x 8 . "\n") x 7; substr $grid, /,/ * $` * 9 + $', 1, 'b' for keys %barrier; my @path = my $pos = $finish; # the walkback to get path while( $pos ne $start ) { substr $grid, $pos =~ /,/ ? $` * 9 + $' : die, 1, 'x'; unshift @path, $pos = $from{$pos}; } print "$grid\nvalue $values{$finish} path @path\n"; - Output:
s....... .x...... ..x.bbb. .xb...b. .xb...b. .xbbbbb. ..x..... ...xxxxx value 11 path 0,0 1,1 2,2 3,1 4,1 5,1 6,2 7,3 7,4 7,5 7,6 7,7
Extra Credit
#!/usr/bin/perl use strict; # https://rosettacode.org/wiki/A*_search_algorithm use warnings; # extra credit use List::AllUtils qw( sum ); my $start = <<END; 087 654 321 END my $finish = <<END; 123 456 780 END my @tiles = $finish =~ /[1-9a-z]/g; my $width = index $start, "\n"; my $gap = qr/.{$width}/s; my $mod = $width + 1; my %rc = map {$_, int($_ / $mod) . ',' . ($_ % $mod)} 0 .. length($start) - 2; my %finishrc = map { $_, [ split /,/, $rc{index $finish, $_} ] } @tiles; my %found = ( $start, 1 ); my @new = [ $start, heuristic($start) ]; # a priority queue my %from; my $mid; while( ! exists $found{$finish} and @new ) { my $pick = (shift @new)->[0]; for my $n ( grep ! exists $found{$_}, $pick =~ s/0(\w)/${1}0/r, # up $pick =~ s/(\w)0/0$1/r, # down $pick =~ s/0($gap)(\w)/$2${1}0/r, # left $pick =~ s/(\w)($gap)0/0$2$1/r, # right ) { $found{$n} = $from{$n} = $pick; my $new = [ $n, my $dist = heuristic( $n ) ]; my $low = 0; # binary insertion into @new (the priority queue) my $high = @new; $new[$mid = $low + $high >> 1][1] <= $dist ? ($low = $mid + 1) : ($high = $mid) while $low < $high; splice @new, $low, 0, $new; # insert in order } } #use Data::Dump 'dd'; dd \%found; my $count = keys %found; exists $found{$finish} or die "no solution found with $count\n"; my @path = my $pos = $finish; # the walkback to get path unshift @path, $pos = $from{$pos} while $pos ne $start; my $steps = @path - 1; my $states = keys %found; #print "$_\n" for @path; my (undef, $w) = split ' ', qx(stty size); while( @path ) { my @section = splice @path, 0, int( $w / ($mod + 1) ); while( $section[0] ) { s/(.*)\n/ print "$1 "; ''/e for @section; print "\n"; } print "\n"; } print "steps: $steps states: $states\n"; sub heuristic { my $from = shift; sum map { my ($r1, $c1) = split /,/, $rc{index $from, $_}; my ($r2, $c2) = @{ $finishrc{$_} }; abs($r1 - $r2) + abs($c1 - $c2) } @tiles; } - Output:
087 807 870 874 874 874 874 874 074 704 740 741 741 741 741 741 041 654 654 654 650 651 651 651 051 851 851 851 850 852 852 852 052 752 321 321 321 321 320 302 032 632 632 632 632 632 630 603 063 863 863 401 410 412 412 412 412 412 012 102 120 123 123 752 752 750 753 753 753 053 453 453 453 450 456 863 863 863 860 806 086 786 786 786 786 786 780 steps: 28 states: 53
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.
sequence grid = split(""" x::::::: :::::::: ::::###: ::#:::#: ::#:::#: ::#####: :::::::: :::::::: """,'\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}}, data = {moves}, acta = {} -- actually analysed set setd(key,data) bool found = false integer count = 0 while not found do if dict_size()=0 then ?"impossible" exit end if key = getd_partial_key(0) data = getd(key) moves = data[$] if length(data)=1 then deld(key) else data = data[1..$-1] putd(key,data) end if count += 1 acta = append(acta,moves[$]) for i=1 to length(permitted) do sequence newpos = sq_add(moves[$],permitted[i]) integer {nx,ny} = newpos if nx>=1 and nx<=8 and ny>=1 and ny<=8 and grid[nx,ny] = ':' then -- (unvisited) grid[nx,ny] = '.' sequence newkey = {max(8-nx,8-ny),key[2]+1}, newmoves = deep_copy(moves) newmoves = append(newmoves,newpos) if newpos = {8,8} then moves = newmoves found = true exit end if integer k = getd_index(newkey) if k=0 then data = {} else data = deep_copy(getd_by_index(k)) end if data = append(data,newmoves) putd(newkey,data) end if end for end while if found then printf(1,"visited %d nodes\ncost:%d\npath:%v\n",{count,length(moves)-1,moves}) for i=1 to length(acta) do integer {x,y} = acta[i] grid[x,y] = '_' end for for i=1 to length(moves) do integer {x,y} = moves[i] grid[x,y] = 'x' end for puts(1,join(grid,'\n')) end if
- Output:
visited 23 nodes cost:11 path:{{1,1},{2,2},{3,3},{4,2},{5,2},{6,2},{7,3},{8,4},{8,5},{8,6},{8,7},{8,8}} x......: .x____.: ._x_###: .x#___#: .x#___#: .x#####: ..x..... :..xxxxx The : represent nodes it did not even look at, the . those added but never gone back to, obviously x represent the path, and together _ and x all nodes actually analysed.
Extra credit
Well, why not. Note this does not reuse/share any code with the above, although I presume the task author assumed it would, instead the main loop uses a priority queue to obtain the next lowest cost and a simple dictionary to avoid re-examination/inifinte recursion.
--set_rand(3) -- (for consistent output) constant optimal = false, mtm = true, -- mutli-tile metrics target = {1,2,3,4,5,6,7,8,0}, -- <-tile found 0..8-> mcost = {{0,0,1,2,1,2,3,2,3}, -- position 1 {0,1,0,1,2,1,2,3,2}, {0,2,1,0,3,2,1,4,3}, {0,1,2,3,0,1,2,1,2}, {0,2,1,2,1,0,1,2,1}, -- ... {0,3,2,1,2,1,0,3,2}, {0,2,3,4,1,2,3,0,1}, {0,3,2,3,2,1,2,1,0}, {0,4,3,2,3,2,1,2,1}}, -- position 9 udlr = "udlr", dirs = {+3,-3,+1,-1}, -- udlr lims = {{9,9,9,9,9,9,9,9,9}, -- up {1,1,1,1,1,1,1,1,1}, -- down {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 = {} integer p0 = find(0,grid) for dx=1 to length(dirs) do integer step = dirs[dx], lim = lims[dx][p0], count = 1 integer i = p0+step while true do if step<0 then if i<lim then exit end if else if i>lim then exit end if end if valid = append(valid,{step,i,udlr[dx],count}) if not mtm then exit end if count += 1 i += step end while end for return valid end function function make_move(sequence grid, move) integer p0 = find(0,grid), {step,lim} = move grid = deep_copy(grid) integer i = p0+step while true do if step<0 then if i<lim then exit end if else if i>lim then exit end if end if grid[p0] = grid[i] grid[i] = 0 p0 = i i += step end while return grid end function function manhattan(sequence grid) integer res = 0 for i=1 to 9 do res += mcost[i][grid[i]+1] end for 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 -- (initially shuffle as if mtm==true, otherwise -- output compares answers to different puzzles) moves = get_moves(grid,true) move = moves[rand(length(moves))] grid = make_move(grid,move) end for problem = grid printf(1,"problem (manhattan cost is %d):\n",manhattan(grid)) show_grid() integer todo = pq_new(), seen = new_dict() pq_add({{grid,{}},iff(optimal?0:manhattan(grid))},todo) setd(grid,true,seen) atom t1 = time()+1 bool found = false integer count = 0, mc while not found do if pq_size(todo)=0 then ?"impossible" exit end if {{grid,moves},mc} = pq_pop(todo) if time()>t1 then string m = iff(optimal?"moves":"manhattan") printf(1,"searching (count=%d, %s=%d)\r",{count,m,mc}) t1 = time()+1 end if next_moves = get_moves(grid,mtm) count += length(next_moves) integer l = length(moves) for i=1 to length(next_moves) do 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 moves = append(moves,move) found = true exit end if if getd_index(new_grid,seen)=NULL then if optimal then mc = l+1 end if pq_add({{new_grid,append(deep_copy(moves),move)},mc},todo) setd(new_grid,true,seen) end if end for end while if found then string s = iff(length(moves)=1?"":"s") if optimal then s &= sprintf(" (max shd be %d)",iff(mtm?24:31)) end if grid = problem string soln = "" for i=1 to length(moves) do move = moves[i] grid = make_move(grid,move) integer {{},{},ch,c} = move soln &= ch if c>1 then soln&='0'+c end if -- show_grid() -- (set the initial shuffle to eg 5 first!) end for -- show_grid() -- (not very educational!) if grid!=target then ?9/0 end if printf(1,"solved in %d move%s:%s\n",{length(moves),s,soln}) end if printf(1,"count:%d, seen:%d, queue:%d\n",{count,dict_size(seen),pq_size(todo)})
- Output:
Note: The solutions are non-optimal (far from it, in fact), since it searches lowest manhattan() first.
In fact that set_rand(3), used for all the results below, is somewhat worse than 0, 1, and 2, and the first to breach optimal limits, ie 31/24, but obviously only when the optimal flag is set to false, as well as being the first to hint at the potential thousand-fold-or-more performance gains on offer.
An optimal solution can instead be found by searching fewest moves first, albeit significantly slower! Note this approach is not really suitable for solving 15-puzzles (or larger).
with optimal false and mtm false:
problem (manhattan cost is 20): 546 807 321 solved in 88 moves:ulddruurdluldrdluurrddlurulldrrdlulurrddlurulldrdlururdllurrdlulddrurdlurdlulurrddlurull count:592, seen:371, queue:155
with optimal false and mtm true:
solved in 45 moves:uld2r2u2l2d2r2u2ld2rul2dru2rdl2urdrdlu2rd2luruld2ru2l2dr2uldlu count:328, seen:164, queue:82
with optimal true and mtm false:
solved in 26 moves (max shd be 31):rulldrdruulddruullddrruull count:399996, seen:163976, queue:13728
with optimal true and mtm true:
solved in 17 moves (max shd be 24):rul2drdru2ld2ru2l2d2r2u2l2 count:298400, seen:106034, queue:31434
function CreateGrid($h, $w, $fill) { $grid = 0..($h - 1) | ForEach-Object { , (, $fill * $w) } return $grid } function EstimateCost($a, $b) { $xd = [Math]::Abs($a.Item1 - $b.Item1) $yd = [Math]::Abs($a.Item2 - $b.Item2) return [Math]::Max($xd, $yd) } function AStar($costs, $start, $goal) { # ValueTuples can be used to index a Hashtable: $start = [ValueTuple]::Create($start[0], $start[1]) $goal = [ValueTuple]::Create($goal[0], $goal[1]) $rows = $costs.Length $cols = $costs[0].Length $cameFrom = CreateGrid $rows $cols $null $openSet = @{$start = (EstimateCost $start $goal), 0} $closedSet = @{} while ($openSet.Count -gt 0) { # find the value in openSet with the lowest fScore $curFScore = [int]::MaxValue foreach ($p in $openSet.Keys) { $fScore, $gScore = $openSet[$p] if ($fScore -lt $curFScore) { $curFScore = $fScore $curGScore = $gScore $cur = $p } } if ($cur -eq $goal) { $totalCost = $curGScore break } $openSet.Remove($cur) $closedSet.Add($cur, 0) $r, $c = $cur.Item1, $cur.Item2 # iterate over each cell in the 3x3 neighborhood foreach ($i in [Math]::Max($r - 1, 0)..[Math]::Min($r + 1, $rows - 1)) { foreach ($j in [Math]::Max($c - 1, 0)..[Math]::Min($c + 1, $cols - 1)) { $neighbor = [ValueTuple]::Create($i, $j) if ($closedSet.ContainsKey($neighbor)) { continue } $newGScore = $curGScore + $costs[$i][$j] $newFScore = $newGScore + (EstimateCost $neighbor $goal) if (-not $openSet.ContainsKey($neighbor)) { $openSet[$neighbor] = $newFScore, $newGScore } else { $fs, $gs = $openSet[$neighbor] if ($newGScore -ge $gs) { continue } } $cameFrom[$i][$j] = $cur } } } # Walk back from the goal $route = @(, ($goal.Item1, $goal.Item2)) $cur = $goal while ($cur -ne $start) { $cur = $cameFrom[$cur.Item1][$cur.Item2] $route += , ($cur.Item1, $cur.Item2) } [array]::Reverse($route) return $route, $totalCost } $grid = CreateGrid 8 8 1 $grid[2][4] = 100 $grid[2][5] = 100 $grid[2][6] = 100 $grid[3][6] = 100 $grid[4][6] = 100 $grid[5][6] = 100 $grid[5][5] = 100 $grid[5][4] = 100 $grid[5][3] = 100 $grid[5][2] = 100 $grid[4][2] = 100 $grid[3][2] = 100 $route, $cost = AStar $grid (0, 0) (7, 7) $displayGrid = CreateGrid 8 8 '.' foreach ($i in 0..7) { foreach ($j in 0..7) { if ($grid[$i][$j] -gt 1) { $displayGrid[$i][$j] = '#' } } } foreach ($step in $route) { $displayGrid[$step[0]][$step[1]] = 'x' } Write-Output ($displayGrid | ForEach-Object { $_ -join '' }) Write-Output "Cost: $cost" $routeString = ($route | ForEach-Object { "($($_[0]), $($_[1]))" }) -join ', ' Write-Output "Route: $routeString" - Output:
x....... .x...... ..x.###. .x#...#. .x#...#. .x#####. ..x.x.x. ...x.x.x Cost: 11 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)
% Picat's tabling system uses an algorithm like Dijkstra's to find an optimal solution. % Picat's planner supports A* search with heuristics. % See the program for the 15-puzzle at https://rosettacode.org/wiki/15_puzzle_solver#Picat % main => Maze = new_array(8,8), Obs = [(2,4), (2,5), (2,6), (3,6), (4,6), (5,6), (5,5), (5,4), (5,3), (5,2), (4,2), (3,2)], foreach ((R0,C0) in Obs) Maze[R0+1,C0+1] = 100 end, foreach (R in 1..8, C in 1..8) (var(Maze[R,C]) -> Maze[R,C] = 1; true) end, search((1,1),(8,8),Maze,Cost,Path), writeln(cost=Cost), println([(R0,C0) : (R1,C1) in Path, R0 = R1-1, C0 = C1-1]). table (+,+,+,min,-) search(G,G,_Maze,Cost,Path) => Cost = 0, Path = [G]. search(S@(R,C),G,Maze,Cost,Path) => neibs(R,C,Neibs), member(S1,Neibs), S1 = (R1,C1), search(S1,G,Maze,Cost1,Path1), Cost = Cost1+Maze[R1,C1], Path = [S|Path1]. neibs(R,C,Neibs) => Neibs = [(R1,C1) : Dr in [-1,0,1], Dc in [-1,0,1], R1 = R+Dr, C1 = C+Dc, R1 >= 1, R1 <= 8, C1 >= 1, C1 <= 8, (R,C) != (R1,C1)].- Output:
cost = 11 [(0,0),(1,0),(2,0),(3,0),(4,0),(5,1),(6,2),(6,3),(6,4),(6,5),(6,6),(7,7)]
:- use_module(library(assoc)). :- use_module(library(heaps)). moore_neighbourhood(yx(Y0, X0), yx(Y, X)) :- between(-1, 1, XOff), X is X0 + XOff, between(-1, 1, YOff), Y is Y0 + YOff, once( XOff \= 0 ; YOff \= 0 ). barriers([yx(2, 3), yx(2, 4), yx(2, 5), yx(3, 5), yx(4, 2), yx(4, 5), yx(5, 2), yx(5, 5), yx(6, 2), yx(6, 3), yx(6, 4), yx(6, 5)]). astar_search(Start) :- barriers(Barriers), findall(Coord - MovementCost, ( between(0, 7, Y), between(0, 7, X), Coord = yx(Y, X), ( ord_memberchk(Coord, Barriers) -> MovementCost = 100 ; MovementCost = 1 ) ), Pairs), ord_list_to_assoc(Pairs, Assoc), singleton_heap(Heap, 7, [Start] - 0), astar_search(Assoc, Heap, Result), display(Assoc, Result). astar_search(Assoc0, Heap0, Result) :- get_from_heap(Heap0, _, [Here | Path] - Length0, Heap1), ( Here = yx(7, 7) -> Result = [Here | Path] - Length0 ; del_assoc(Here, Assoc0, _, Assoc) -> findall([There, Here | Path] - Length, ( moore_neighbourhood(Here, There), get_assoc(There, Assoc, Length1), Length is Length0 + Length1 ), NextSteps), foldl([S, H0, H] >> ( S = [yx(Y, X) | _] - L, Heuristic is max(abs(7 - Y), abs(7 - X)) + L, add_to_heap(H0, Heuristic, S, H) ), NextSteps, Heap1, Heap), astar_search(Assoc, Heap, Result) ; astar_search(Assoc0, Heap1, Result) ). display(Assoc, Path - Length) :- format("Optimal path length: ~d~n", [Length]), writeln(Path), nl, foreach(( between(0, 7, Y), between(0, 7, X), Here = yx(Y, X) ), ( ( memberchk(Here, Path) -> write('O') ; get_assoc(Here, Assoc, 100) -> write('#') ; write('.') ), ( X = 7 -> nl ; true ) )). - Output:
?- astar_search(yx(0, 0)). Optimal path length: 11 [yx(7,7),yx(6,7),yx(5,7),yx(4,7),yx(3,7),yx(2,6),yx(1,5),yx(0,4),yx(0,3),yx(1,2),yx(1,1),yx(0,0)] O..OO... .OO..O.. ...###O. .....#.O ..#..#.O ..#..#.O ..####.O .......O true.
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() - 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
library(igraph) # Constants chessboard_size <- 8 given_obstacles <- list(c(2,4), c(2,5), c(2,6), c(3,6), c(4,6), c(5,6), c(5,5), c(5,4), c(5,3), c(5,2), c(4,2), c(3,2)) # Helper functions vfromcart <- function(p, n) { (p[1] - 1) * n + p[2] } obstacles <- sapply(given_obstacles, function(o) vfromcart(o + 1, chessboard_size)) zbasedpath <- function(path, n) { lapply(path, function(v) c(floor((v - 1) / n), (v - 1) %% n)) } pathcost <- function(path) { sum(sapply(path[-1], function(x) ifelse(x %in% obstacles, 100, 1))) } surround <- function(x, y, n) { bottomx <- ifelse(x > 1, x - 1, x) topx <- ifelse(x < n, x + 1, x) bottomy <- ifelse(y > 1, y - 1, y) topy <- ifelse(y < n, y + 1, y) coords <- expand.grid(x = bottomx:topx, y = bottomy:topy) lapply(1:nrow(coords), function(i) c(coords$x[i], coords$y[i])) } kinggraph <- function(N) { edges <- list() weights <- c() for (row in 1:N) { for (col in 1:N) { neighbors <- surround(row, col, N) origin <- vfromcart(c(row, col), N) for (p in neighbors) { targ <- vfromcart(p, N) hcost <- ifelse(targ %in% obstacles | origin %in% obstacles, 100, 1) edges[[length(edges) + 1]] <- c(origin, targ) weights <- c(weights, hcost) } } } edge_matrix <- do.call(rbind, edges) graph <- graph_from_edgelist(edge_matrix, directed = FALSE) E(graph)$weight <- weights graph } # Create graph and find shortest path kgraph <- kinggraph(chessboard_size) path <- shortest_paths(kgraph, from = 1, to = 64, weights = E(kgraph)$weight)$vpath[[1]] path <- as.numeric(path) cat("Solution has cost", pathcost(path), ":\n") print(zbasedpath(path, chessboard_size)) # Visualize path path2graphic <- function(x, path) { if (x %in% obstacles) return('█') if (x %in% path) return('x') return('.') } for (row in 8:1) { for (col in 7:0) { cat(path2graphic(row * 8 - col, path)) } cat("\n") } - Output:
Attaching package: 'igraph' The following objects are masked from 'package:stats': decompose, spectrum The following object is masked from 'package:base': union Solution has cost 11 : [[1]] [1] 0 0 [[2]] [1] 1 1 [[3]] [1] 2 0 [[4]] [1] 3 1 [[5]] [1] 4 0 [[6]] [1] 5 1 [[7]] [1] 6 2 [[8]] [1] 7 3 [[9]] [1] 7 4 [[10]] [1] 7 5 [[11]] [1] 7 6 [[12]] [1] 7 7 ...xxxxx ..x..... .x█████. x.█...█. .x█...█. x...███. .x...... x.......
This code is lifted from: this blog post. Read it, it's very good.
#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)) - 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.
# 20200427 Raku programming solution 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) { my (\D1,\D2) = 1, 1; my (\dx,\dy) = ( start.list »-« goal.list )».abs; return (D1 * (dx + dy)) + (D2 - 2*D1) * min dx, dy } method get_vertex_neighbours(\pos) { gather { for <1 0>,<-1 0>,<0 1>,<0 -1>,<1 1>,<-1 1>,<1 -1>,<-1 -1> -> \d { my (\x2,\y2) = pos.list »+« d.list; (x2 < 0 || x2 > 7 || y2 < 0 || y2 > 7) && next; take x2, y2; } } } method move_cost(\a,\b) { (b ~~ any self.barriers) ?? 100 !! 1 } } sub AStarSearch(\start, \end, \graph) { my (%G,%F); %G{start.Str} = 0; %F{start.Str} = graph.heuristic(start, end); my @closedVertices = []; my @openVertices = [].push(start); my %cameFrom; while (@openVertices.Bool) { my $current = Nil; my $currentFscore = Inf; for @openVertices -> \pos { if (%F{pos.Str} < $currentFscore) { $currentFscore = %F{pos.Str}; $current = pos } } if $current ~~ end { my @path = [].push($current); while %cameFrom{$current.Str}:exists { $current = %cameFrom{$current.Str}; @path.push($current) } return @path.reverse, %F{end.Str} } @openVertices .= grep: * !eqv $current; @closedVertices.push($current); for (graph.get_vertex_neighbours($current)) -> \neighbour { next if neighbour ~~ any @closedVertices; my \candidateG = %G{$current.Str}+graph.move_cost($current,neighbour); if !(neighbour ~~ any @openVertices) { @openVertices.push(neighbour) } elsif (candidateG ≥ %G{neighbour.Str}) { next } %cameFrom{neighbour.Str} = $current; %G{neighbour.Str} = candidateG; my \H = graph.heuristic(neighbour, end); %F{neighbour.Str} = %G{neighbour.Str} + H; } } die "A* failed to find a solution" } my \graph = AStarGraph.new; my (\route, \cost) = AStarSearch(<0 0>, <7 7>, graph); my \w = my \h = 10; my @grid = [ ['.' xx w ] xx h ]; for ^h -> \y { @grid[y;0] = "█"; @grid[y;*-1] = "█" } for ^w -> \x { @grid[0;x] = "█"; @grid[*-1;x] = "█" } for (graph.barriers) -> \d { @grid[d[0]+1][d[1]+1] = "█" } for @(route) -> \d { @grid[d[0]+1][d[1]+1] = "x" } .join.say for @grid ; say "Path cost : ", cost, " and route : ", route; - Output:
███████████x.......█ █.x......█ █..x.███.█ █.x█...█.█ █.x█...█.█ █.x█████.█ █..xxxxx.█ █.......x█ ██████████
Path cost : 11 and 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))
/*REXX program solves the A* search problem for a (general) NxN grid. */ parse arg N sCol sRow . /*obtain optional arguments from the CL*/ if N=='' | N=="," then N=8 /*No grid size specified? Use default.*/ if sCol=='' | sCol=="," then sCol=1 /*No starting column given? " " */ if sRow=='' | sRow=="," then sRow=1 /* " " row " " " */ beg= '─0─' /*mark the start of the journey in grid*/ o.=.; p.=0 /*list of optimum start journey starts.*/ times=0 /*cntr/pos for number of optimizations.*/ Pc = ' 1 1 0 0 1 -1 -1 -1 ' /*the possible column moves for a path.*/ Pr = ' 1 0 1 -1 -1 0 1 -1 ' /* " " row " " " " */ Pcm=words(Pc) /* [↑] optimized for moving right&down*/ $.=1e6; OK=0; min$=$. /*# possible directions; cost; solution*/ @Aa= " A* search algorithm on" /*a handy─dandy literal for the SAYs. */ flasher= '@. $. min$ N o. p. Pc. Pcm Pr. sCol sRow times' /*a literal list for EXPOSE.*/ call path 0 /*find a possible solution for the grid*/ @NxN= 'a ' N"x"N ' grid' /*a literal used for a SAY statement.*/ if OK then say 'A solution for the' @Aa @NxN "with a score of " @.N.N':' else say 'No' @Aa "solution for" @NxN'.' call show 1 /*invoke subroutine to display the grid*/ exit /*stick a fork in it, we're all done. */ /*──────────────────────────────────────────────────────────────────────────────────────*/ @: parse arg x,y,aChar; if arg()==3 then @.x.y=aChar; return @.x.y @p: parse arg x,y; if datatype(@.x.y, 'W') then return @.x.y<m-1; return 0 /*──────────────────────────────────────────────────────────────────────────────────────*/ barr: $=2.4 2.5 2.6 3.6 4.6 5.6 5.5 5.4 5.3 5.2 4.2 3.2 /*locations of barriers on grid*/ do b=1 for words($); _=word($, b); parse var _ c '.' r; call @ c+1,r+1,"█" end /*b*/; return /*──────────────────────────────────────────────────────────────────────────────────────*/ move: procedure expose (flasher); parse arg m,col,row /*obtain move,col,row.*/ do t=1 for Pcm; nc=col + Pc.t; nr=row + Pr.t /*a new path position. */ if @.nc.nr==. then do; if opti() then iterate /*Costlier path? Next.*/ @.nc.nr=m; p.1.m=nc nr /*Empty? A legal path.*/ p.pcm.m=nr nc-1 /*used for a fast path.*/ if nc==N then if nr==N then return 1 /*last move? */ if move(m + 1, nc, nr) then return 1 /* " " */ @.nc.nr=. /*undo the above move. */ end /*try a different move.*/ end /*t*/ /* [↑] all moves tried*/ return 0 /*path isn't possible. */ /*──────────────────────────────────────────────────────────────────────────────────────*/ opti: ncm=nc-1; nrm=nr-1; if @p(ncm, nrm) then return 1 if @p(ncm, nr ) then return 1 if @p(nc, nrm) then return 1 ncp=nc+1; nrp=nr+1; if @p(ncp, nr ) then return 1 if @p(ncp, nrm) then return 1 if @p(nc, nrp) then return 1 if @p(ncm, nrp) then return 1 if @p(ncp, nrp) then return 1; return 0 /*──────────────────────────────────────────────────────────────────────────────────────*/ path: parse arg z; t=times /*initial move can only be one of eight*/ do #=1 for Pcm; @.= /*optimize for each degree of movement.*/ if z\==0 then if #\==z then iterate /*This a particular low─cost request ? */ do c=1 for N; do r=1 for N; @.c.r=.; end /*r*/ end /*c*/ iCol=sCol; iRow=sRow; @.sCol.sRow= beg /*all path's initial starting position*/ call barr /*place the barriers on the grid. */ Pco=subword(Pc Pc, #, Pcm); Pro=subword(Pr Pr, #, Pcm) parse var Pco Pc.1 Pc.2 Pc.3 Pc.4 Pc.5 Pc.6 Pc.7 Pc.8 /*possible directions.*/ parse var Pro Pr.1 Pr.2 Pr.3 Pr.4 Pr.5 Pr.6 Pr.7 Pr.8 /* " " */ do o=1 for times; parse var o.o c r; @.c.r=o; iRow=r; iCol=c end /*o*/ fp=move(1+times, iCol, iRow); sol=@N.N\==. & fp if sol then do; $.#=@.N.N /*Found a solution? Remember the cost.*/ OK=1; min$=min(min$, $.#) end end /*#*/ wp=1e7; wg=0; do g=1 for Pcm; if $.g<wp & $.g>0 & t\=2 then do; wg=g; wp=$.g; end end /*g*/ /* [↑] find minimum non-zero path cost*/ if wg==0 then wg=8 /*Not found? Then use last cost found.*/ times=times + 1 /*bump # times a marker has been placed*/ o.times= p.wg.times /*remember this move location for PATH.*/ if times<4 then call path 0 /*only do memoization for first 3 moves*/ return /*──────────────────────────────────────────────────────────────────────────────────────*/ show: ind=left('', 9 * (n<18) ); say /*the indentation of the displayed grid*/ _=substr(copies("┼───", N),2); say ind translate('┌'_"┐", '┬', "┼") /*grid top.*/ /* [↓] build a display for the grid. */ do c=1 for N; if c\==1 & arg(1) then say ind '├'_"┤"; L=@. do r=1 for N; ?=@.c.r; if c ==N & r==N & ?\==. then ?='end'; L=L"│"center(?, 3) end /*r*/ /*done with rank of the grid. */ say ind translate(L'│', , .) /*display a " " " " */ end /*c*/ /*a 19x19 grid can be shown 80 columns.*/ say ind translate('└'_"┘",'┴',"┼"); return /*display the very bottom of the grid. */ - output when using the default input:
A solution for the A* search algorithm on a 8x8 grid with a score of 11: ┌───┬───┬───┬───┬───┬───┬───┬───┐ │─0─│ │ │ │ │ │ │ │ ├───┼───┼───┼───┼───┼───┼───┼───┤ │ │ 1 │ │ │ │ │ │ │ ├───┼───┼───┼───┼───┼───┼───┼───┤ │ │ │ 2 │ │ █ │ █ │ █ │ │ ├───┼───┼───┼───┼───┼───┼───┼───┤ │ │ 3 │ █ │ │ │ │ █ │ │ ├───┼───┼───┼───┼───┼───┼───┼───┤ │ │ 4 │ █ │ │ │ │ █ │ │ ├───┼───┼───┼───┼───┼───┼───┼───┤ │ │ 5 │ █ │ █ │ █ │ █ │ █ │ │ ├───┼───┼───┼───┼───┼───┼───┼───┤ │ │ │ 6 │ │ │ │ │ │ ├───┼───┼───┼───┼───┼───┼───┼───┤ │ │ │ │ 7 │ 8 │ 9 │10 │end│ └───┴───┴───┴───┴───┴───┴───┴───┘
use std::collections::VecDeque; #[derive(Clone, Copy, PartialEq)] struct Point { x: i32, y: i32, } impl Point { fn new(x: i32, y: i32) -> Self { Point { x, y } } fn add(&self, other: &Point) -> Point { Point { x: self.x + other.x, y: self.y + other.y, } } } #[derive(Clone)] // Added Clone trait struct Map { m: [[u8; 8]; 8], w: i32, h: i32, } impl Map { fn new() -> Self { let t = [ [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], ]; Map { m: t, w: 8, h: 8, } } fn get(&self, x: i32, y: i32) -> u8 { self.m[y as usize][x as usize] } } #[derive(Clone)] struct Node { pos: Point, parent: Point, dist: i32, cost: i32, } impl PartialEq for Node { fn eq(&self, other: &Self) -> bool { self.pos == other.pos } } struct AStar { m: Map, end: Point, start: Point, neighbours: [Point; 8], open: Vec<Node>, closed: Vec<Node>, } impl AStar { fn new() -> Self { AStar { m: Map::new(), end: Point::new(0, 0), start: Point::new(0, 0), neighbours: [ Point::new(-1, -1), Point::new(1, -1), Point::new(-1, 1), Point::new(1, 1), Point::new(0, -1), Point::new(-1, 0), Point::new(0, 1), Point::new(1, 0), ], open: Vec::new(), closed: Vec::new(), } } fn calc_dist(&self, p: &Point) -> i32 { let x = self.end.x - p.x; let y = self.end.y - p.y; x * x + y * y } fn is_valid(&self, p: &Point) -> bool { p.x >= 0 && p.y >= 0 && p.x < self.m.w && p.y < self.m.h } fn exist_point(&self, p: &Point, cost: i32) -> bool { // Changed to immutable borrow if let Some(i) = self.closed.iter().position(|n| n.pos == *p) { if self.closed[i].cost + self.closed[i].dist < cost { return true; } } if let Some(i) = self.open.iter().position(|n| n.pos == *p) { if self.open[i].cost + self.open[i].dist < cost { return true; } } false } fn fill_open(&mut self, n: &Node) -> bool { for (x, &neighbour) in self.neighbours.iter().enumerate() { let step_cost = if x < 4 { 1 } else { 1 }; let neighbour_pos = n.pos.add(&neighbour); if neighbour_pos == self.end { return true; } if self.is_valid(&neighbour_pos) && self.m.get(neighbour_pos.x, neighbour_pos.y) != 1 { let nc = step_cost + n.cost; let dist = self.calc_dist(&neighbour_pos); if !self.exist_point(&neighbour_pos, nc + dist) { self.open.push(Node { cost: nc, dist, pos: neighbour_pos, parent: n.pos, }); } } } false } fn search(&mut self, s: Point, e: Point, m: Map) -> bool { self.end = e; self.start = s; self.m = m; let dist = self.calc_dist(&s); self.open.push(Node { cost: 0, pos: s, parent: Point::new(0, 0), dist, }); while !self.open.is_empty() { self.open.sort_by(|a, b| (a.dist + a.cost).cmp(&(b.dist + b.cost))); let n = self.open.remove(0); self.closed.push(n.clone()); if self.fill_open(&n) { return true; } } false } fn path(&self) -> (Vec<Point>, i32) { let mut path = VecDeque::new(); path.push_front(self.end); let cost = 1 + self.closed.last().unwrap().cost; path.push_front(self.closed.last().unwrap().pos); let mut parent = self.closed.last().unwrap().parent; for node in self.closed.iter().rev() { if node.pos == parent && node.pos != self.start { path.push_front(node.pos); parent = node.parent; } } path.push_front(self.start); (path.into_iter().collect(), cost) } } fn main() { let m = Map::new(); let s = Point::new(0, 0); let e = Point::new(7, 7); let mut astar = AStar::new(); if astar.search(s, e, m.clone()) { // Clone the map here let (path, c) = astar.path(); for y in -1..9 { for x in -1..9 { if x < 0 || y < 0 || x > 7 || y > 7 || m.get(x, y) == 1 { print!("#"); } else if path.contains(&Point::new(x, y)) { print!("x"); } else { print!("."); } } println!(); } println!("\nPath cost {}:", c); for p in path { print!("({}, {}) ", p.x, p.y); } println!(); } println!(); } - Output:
########## #x.......# #.x..xxx.# #..xx###x# #..#...#x# #..#...#x# #..#####x# #.......x# #.......x# ########## Path cost 12: (0, 0) (1, 1) (2, 2) (3, 2) (4, 1) (5, 1) (6, 1) (7, 2) (7, 3) (7, 4) (7, 5) (7, 6) (7, 7)
import <Utilities/Set.sl>; import <Utilities/Math.sl>; import <Utilities/Sequence.sl>; Point ::= (x : int, y : int); State ::= (open : Point(1), closed : Point(1), cameFrom : Point(2), estimate : int(2), actual : int(2)); allNeighbors := [(x : -1, y : -1), (x : 1, y : -1), (x : -1, y : 1), (x : 1, y : 1), (x : 0, y : -1), (x : -1, y : 0), (x : 0, y : 1), (x : 1, y : 0)]; defaultBarriers := [(x : 3, y : 5),(x : 3, y : 6),(x : 3, y : 7),(x : 4, y : 7), (x : 5, y : 7),(x : 6, y : 7),(x : 6, y : 6),(x : 6, y : 5),(x : 6, y : 4), (x : 6, y : 3),(x : 5, y : 3),(x : 4, y : 3)]; defaultWidth := 8; defaultHeight := 8; main(args(2)) := aStar(defaultWidth, defaultHeight, defaultBarriers, (x : 1, y : 1), (x : defaultWidth, y : defaultHeight)); aStar(width, height, barriers(1), start, end) := let newEstimate[i,j] := heuristic(start, end) when i = start.x and j = start.y else 0 foreach i within 1...width, j within 1 ... height; 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]; in "No Path Found" when size(searchResults.open) = 0 else "Path: " ++ toString(shortestPath) ++ "\nCost:" ++ toString(searchResults.actual[end.x, end.y]) ++ "\nMap:\n" ++ join(appendNT(drawMap(barriers,shortestPath,width, height),"\n")); path(cameFrom(2), start, current) := let next := cameFrom[current.x, current.y]; in [] when current = start else path(cameFrom, start, next) ++ [next]; drawMap(barriers(1), path(1), width, height)[i,j] := '#' when elementOf((x:i, y:j), barriers) else 'X' when elementOf((x:i, y:j), path) else '.' foreach i within 1 ... width, j within 1 ... height; search(state, barriers(1), end) := let nLocation := smallestEstimate(state.open, state.estimate, 2, 1, state.estimate[state.open[1].x, state.open[1].y]); n := state.open[nLocation]; neighbors := createNeighbors(n, allNeighbors, size(state.actual), size(state.actual[1])); startState := (open : state.open[1...nLocation-1] ++ state.open[nLocation+1 ... size(state.open)], closed : state.closed ++ [n], cameFrom : state.cameFrom, estimate : state.estimate, actual : state.actual); newState := findOpenNeighbors(n, startState, barriers, end, neighbors); in state when size(state.open) = 0 else state when n = end else search(newState, barriers, end); smallestEstimate(open(1), estimate(2), index, minIndex, minEstimate) := let newEstimate := estimate[open[index].x, open[index].y]; in minIndex when index > size(open) else smallestEstimate(open, estimate, index + 1, minIndex, minEstimate) when newEstimate > minEstimate else smallestEstimate(open, estimate, index + 1, index, newEstimate); findOpenNeighbors(n, state, barriers(1), end, neighbors(1)) := let neighbor := head(neighbors); cost := 1 + n.cost; candidate := state.actual[n.x, n.y] + calculateCost(barriers, n, neighbor); in state when size(neighbors) = 0 else findOpenNeighbors(n, state, barriers, end, tail(neighbors)) when elementOf(neighbor, state.closed) else findOpenNeighbors(n, state, barriers, end, tail(neighbors)) when elementOf(neighbor, state.open) and candidate >= state.actual[neighbor.x, neighbor.y] else findOpenNeighbors(n, (open : state.open ++ [neighbor], closed : state.closed, cameFrom : setMap(state.cameFrom, neighbor, n), estimate : setMap(state.estimate, neighbor, candidate + heuristic(neighbor, end)), actual : setMap(state.actual, neighbor, candidate)), barriers, end, tail(neighbors)); createNeighbors(n, p, w, h) := let x := n.x + p.x; 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; heuristic(start, end) := let dx := abs(start.x - end.x); dy := abs(start.y - end.y); in (dx + dy) - min(dx, dy); setMap(map(2), point, value)[i,j] := value when point.x = i and point.y = j else map[i,j] foreach i within 1 ... size(map), j within 1 ... size(map[1]);- Output
Path: [(x:1,y:1),(x:2,y:2),(x:3,y:3),(x:4,y:2),(x:5,y:2),(x:6,y:2),(x:7,y:3),(x:7,y:4),(x:7,y:5),(x:7,y:6),(x:7,y:7),(x:8,y:8)] Cost:11 Map: X....... .X...... ..X.###. .X#...#. .X#...#. .X#####. ..XXXXX. .......X
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]]
<-- using Cerebras, GPT-OSS-120B -->
import Foundation // ------------------------------------------------------------ // MARK: - Point (equivalent to the C++ class `point`) // ------------------------------------------------------------ struct Point: Equatable, Hashable { var x: Int var y: Int init(_ x: Int = 0, _ y: Int = 0) { self.x = x self.y = y } // overload the + operator static func + (lhs: Point, rhs: Point) -> Point { return Point(lhs.x + rhs.x, lhs.y + rhs.y) } } // ------------------------------------------------------------ // MARK: - GridMap (the 8×8 board) // ------------------------------------------------------------ struct GridMap { private let data: [[Int]] // 0 = free, 1 = wall let width = 8 let height = 8 init() { // raw layout taken directly from the C++ constructor let raw: [[Int]] = [ [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] ] // transpose so we can use data[x][y] like the C++ version var transposed = Array(repeating: Array(repeating: 0, count: height), count: width) for y in 0..<height { for x in 0..<width { transposed[x][y] = raw[y][x] } } self.data = transposed } // subscript works like the C++ operator() subscript(x: Int, y: Int) -> Int { return data[x][y] } } // ------------------------------------------------------------ // MARK: - Node (stores a cell for the A* algorithm) // ------------------------------------------------------------ struct Node: Comparable { var pos: Point // cell coordinates var parent: Point? // predecessor (nil for the start node) var g: Int // cost from start (called “cost” in C++) var h: Int // heuristic distance to goal (called “dist”) var f: Int { // total estimated cost = g + h return g + h } // Comparable – smallest f comes first static func < (lhs: Node, rhs: Node) -> Bool { return lhs.f < rhs.f } static func == (lhs: Node, rhs: Node) -> Bool { return lhs.pos == rhs.pos } } // ------------------------------------------------------------ // MARK: - A* implementation // ------------------------------------------------------------ final class AStar { // eight neighbour offsets (same order as the C++ array) private let neighbours: [Point] = [ Point(-1, -1), Point( 1, -1), Point(-1, 1), Point( 1, 1), Point( 0, -1), Point(-1, 0), Point( 0, 1), Point( 1, 0) ] private var map: GridMap! private var start: Point! private var goal: Point! private var open: [Node] = [] // frontier private var closed: [Node] = [] // already processed // ---------------------------------------------------------------- // Heuristic – squared Euclidean distance (identical to the C++ version) // ---------------------------------------------------------------- private func heuristic(_ p: Point) -> Int { let dx = goal.x - p.x let dy = goal.y - p.y return dx * dx + dy * dy } // ---------------------------------------------------------------- // Returns true if a node with a *better* (smaller) f‑value already // exists for the same point in either the open or closed list. // ---------------------------------------------------------------- private func isBetterNodeAlready(at point: Point, withF f: Int) -> Bool { if let n = closed.first(where: { $0.pos == point }) { return n.f < f } if let n = open.first(where: { $0.pos == point }) { return n.f < f } return false } // ---------------------------------------------------------------- // Expand a node: generate its neighbours and push the admissible ones // onto the open list. // ---------------------------------------------------------------- private func expand(_ node: Node) -> Bool { for offset in neighbours { // stepCost is 1 for every direction (the C++ code kept the variable) let stepCost = 1 let neighbourPos = node.pos + offset // Goal reached – put the final node into the closed list if neighbourPos == goal { let finalNode = Node(pos: neighbourPos, parent: node.pos, g: node.g + stepCost, h: 0) closed.append(finalNode) return true } // Bounds & obstacle check guard neighbourPos.x >= 0, neighbourPos.y >= 0, neighbourPos.x < map.width, neighbourPos.y < map.height, map[neighbourPos.x, neighbourPos.y] == 0 else { continue } let tentativeG = node.g + stepCost let h = heuristic(neighbourPos) let f = tentativeG + h // Skip if we already have a better node for this cell if isBetterNodeAlready(at: neighbourPos, withF: f) { continue } // Otherwise add the new node to the open list let newNode = Node(pos: neighbourPos, parent: node.pos, g: tentativeG, h: h) open.append(newNode) } return false } // ---------------------------------------------------------------- // Public entry point – exactly the same semantics as the C++ `search` // ---------------------------------------------------------------- func search(start: Point, goal: Point, on map: GridMap) -> Bool { // reset everything self.map = map self.start = start self.goal = goal open.removeAll() closed.removeAll() // seed the algorithm let startNode = Node(pos: start, parent: nil, g: 0, h: heuristic(start)) open.append(startNode) // main loop (no priority queue – sorting a tiny array is fine) while !open.isEmpty { open.sort() // smallest f first let current = open.removeFirst() closed.append(current) if expand(current) { // true → goal found return true } } return false // no path exists } // ---------------------------------------------------------------- // Rebuild the path (start → goal) and compute its total cost. // ---------------------------------------------------------------- func reconstructPath() -> (path: [Point], cost: Int) { guard let last = closed.last else { return ([], 0) } var path: [Point] = [goal] // goal was already added var parent = last.parent // walk backwards through the closed list following parent pointers for node in closed.reversed() { if node.pos == parent { path.append(node.pos) parent = node.parent if node.pos == start { break } } } path.append(start) // reverse to obtain start → goal order let ordered = path.reversed() // the original C++ code adds 1 to the cost when reporting it let totalCost = last.g + 1 return (Array(ordered), totalCost) } } // ------------------------------------------------------------ // MARK: - Demo (mirrors the original `main()`) // ------------------------------------------------------------ func demo() { let map = GridMap() let start = Point(0, 0) // default C++ constructor = (0,0) let goal = Point(7, 7) let astar = AStar() guard astar.search(start: start, goal: goal, on: map) else { print("No path found") return } let (path, cost) = astar.reconstructPath() // ---- pretty‑print the board (identical visual output) ---- for y in -1...9 { var line = "" for x in -1...9 { if x < 0 || y < 0 || x > 7 || y > 7 || map[x, y] == 1 { // full‑block character – same visual as C++ char(0xdb) line.append("\u{2588}") } else { if path.contains(Point(x, y)) { line.append("x") } else { line.append(".") } } } print(line) } // ---- report the cost and the coordinate list ---- print("\nPath cost \(cost): ", terminator: "") for p in path { print("(\(p.x),\(p.y)) ", terminator: "") } print("\n") } // Run the demo demo() - Output:
███████████ █x.......██ █.x..xxx.██ █..xx███x██ █..█...█x██ █..█...█x██ █..█████x██ █.......x██ █.......x██ ███████████ ███████████ Path cost 13: (0,0) (0,0) (1,1) (2,2) (3,2) (4,1) (5,1) (6,1) (7,2) (7,3) (7,4) (7,5) (7,6) (7,7)
#!/bin/bash # This option will make the script exit when there is an error set -o errexit # This option will make the script exit when it tries to use an unset variable set -o nounset declare -A grid declare -A cell_type=( ["empty"]=0 ["barrier"]=1 ["start"]=2 ["end"]=3 ["path"]=4 ["right"]=5 ["left"]=6 ["up"]=7 ["down"]=8 ["left_up"]=9 ["left_down"]=10 ["right_up"]=11 ["right_down"]=12 ) grid_size=(10 10) generate_rosetta_grid(){ grid_size=(8 8) start=(0 0) end=(7 7) for (( i = 0; i < grid_size[0]; i++ )); do for (( j = 0; j < grid_size[1]; j++ )); do grid[$i,$j]=${cell_type[empty]} done done 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") for barrier in ${barriers[*]};do grid["$barrier"]=${cell_type[barrier]} done grid[${start[0]},${start[1]}]=${cell_type[start]} grid[${end[0]},${end[1]}]=${cell_type[end]} } abs(){ # Number absolute value. # Params: # ------ # $1 -> number # Return: # number abs if [[ $1 -gt 0 ]]; then echo "$1" else echo "$((-$1))" fi } print_table(){ # Print table using unicode symbols. # Symbols: # " " -> empty cell # ◼ -> barrier # ◉ -> start position # ✪ -> goal # arrows -> path from start to goal printf ' ' # Print letters at top. for ((i=0;i< grid_size[1];i++)) do printf "%s" $i done echo for ((i=0;i < grid_size[0];i++)) do # Print numbers. printf "%s" $i for ((j=0;j < grid_size[1];j++)) do cell=${grid[$i,$j]} if [[ $cell -eq ${cell_type[empty]} ]]; then # If cell is empty prints space printf " " elif [[ $cell -eq ${cell_type[barrier]} ]]; then # If cell is a barrier printf "■" elif [[ $cell -eq ${cell_type[start]} ]]; then # Print start and end position printf "◉" elif [[ $cell -eq ${cell_type[end]} ]]; then # Print end position printf "✪" elif [[ $cell -eq ${cell_type[path]} ]]; then # Print path printf "*" elif [[ $cell -eq ${cell_type[up]} ]]; then # Print path printf "↑" elif [[ $cell -eq ${cell_type[down]} ]]; then # Print path printf "↓" elif [[ $cell -eq ${cell_type[right]} ]]; then # Print path printf "→" elif [[ $cell -eq ${cell_type[left]} ]]; then # Print path printf "←" elif [[ $cell -eq ${cell_type[right_up]} ]]; then # Print path printf "↗" elif [[ $cell -eq ${cell_type[right_down]} ]]; then # Print path printf "↙" elif [[ $cell -eq ${cell_type[left_up]} ]]; then # Print path printf "↖" elif [[ $cell -eq ${cell_type[left_down]} ]]; then # Print path printf "↘" fi done echo done } get_neighbours(){ # Calculates all point's neighbours # Params: # ------ # $1 -> "x,y" formatted point position # Return: # ------ # array of available positions # Skips nonexistent indices. neighbours=() for i in {-1..1},{-1..1}; do if [[ ( ${i%,*} -eq 0 ) && ( ${i#*,} -eq 0 ) ]]; then continue fi dx=${i%,*} dy=${i#*,} x=$((${1%,*}+dx)) y=$((${1#*,}+dy)) if [[ $x -lt 0 ]] || [[ $x -ge ${grid_size[0]} ]]; then continue fi if [[ $y -lt 0 || $y -ge ${grid_size[1]} ]]; then continue fi neighbours+=("$x,$y") done echo "${neighbours[*]}" } move_cost(){ # Calculates how much will it cost # to travel to point b. # return 100 if b is barrier # # Params: # ------ # $1 -> a # $2 -> b # Return: # ------ # movement cost. barrier=${cell_type[barrier]} if [[ ${grid[${2%,*},${2#*,}]} -eq barrier ]]; then echo 100 else echo 1 fi } print_raw(){ # Print raw grid values. for ((i=0;i < grid_size[0];i++)) do for ((j=0;j < grid_size[1];j++)) do printf "%s" "${grid[$i,$j]}" done echo done } minimum(){ # Minimum between two numbers # Params: # ------ # $1 -> a # $2 -> b # Return: # ------ # less value if [[ $1 -lt $2 ]]; then echo "$1" else echo "$2" fi } heuristic_cost(){ # Chebyshev distance heuristic score # if we can move one square either # adjacent or diagonal d=1 d2=1 dx=$(abs $((${1#*,} - ${2#*,}))) dy=$(abs $((${1%,*} - ${2%,*}))) echo "$(((d*(dx + dy))+(d2 - 2 * d)*$(minimum dx dy)))" } contains(){ for el in "${2[@]}"; do echo "$el" done } contains_value() { # Check if element exists in array # Params: # ------ # $1 -> array # $2 -> element to find. # Returns: # 1 if element exists in array # 0 otherwise. local array="$1[@]" arr=("${!array}") local seeking=$2 local in=0 for element in ${arr[*]}; do if [ "$element" = "$seeking" ]; then in=1 break fi done echo "$in" } reverse_array(){ # Reverse given array. # Params: # ------ # $1 -> array # Return: # ------ # reversed array. local array="$1[@]" arr=("${!array}") result=() for (( idx=${#arr[@]}-1 ; idx>=0 ; idx-- )) ; do result+=("${arr[$idx]}") done echo "${result[@]}" } find_path(){ declare -A fScore declare -A gScore declare -A cameFrom declare -a openVertices declare -a closedVertices for (( i = 0; i < grid_size[0]; i++ )); do for (( j = 0; j < grid_size[1]; j++ )); do gScore[$i,$j]=$((1<<62)) fScore[$i,$j]=$((1<<62)) done done gScore["${start[0]},${start[1]}"]=0 fScore["${start[0]},${start[1]}"]=$(heuristic_cost "${start[0]},${start[1]}" "${end[0]},${end[1]}") openVertices+=("${start[0]},${start[1]}") while [[ -n "${openVertices[*]}" ]]; do current=-1 currentFscore=0 for pos in ${openVertices[*]}; do if [[ $current -eq -1 ]] || [[ ${fScore["$pos"]} -lt $currentFscore ]]; then currentFscore=${fScore["$pos"]} current=$pos fi done if [[ "$current" = "${end[0]},${end[1]}" ]]; then path=( "$current" ) while [ ${cameFrom["$current"]+_} ]; do current=${cameFrom["$current"]} path+=("$current") done reverse_array path return 0 fi openVertices=( "$( echo "${openVertices[@]/$current}" | xargs )" ) closedVertices+=( "$current" ) neighbours=( "$(get_neighbours "$current")" ) for neighbour in ${neighbours[*]}; do if [[ $(contains_value closedVertices "$neighbour") -eq 1 ]]; then continue fi mCost="$(move_cost "$current" "$neighbour")" candidateG=$(( ${gScore["$current"]}+mCost )) if [[ $candidateG -gt 100 ]]; then continue fi if [[ $(contains_value openVertices "$neighbour") -eq 0 ]]; then openVertices+=("$neighbour") elif [[ $candidateG -gt ${gScore[$neighbour]} ]]; then continue fi cameFrom["$neighbour"]="$current" gScore["$neighbour"]=$candidateG heuristic_score=$(heuristic_cost "$neighbour" "${end[0]},${end[1]}") fScore["$neighbour"]=$(( candidateG+heuristic_score )) done done } map_to_arrows(){ local array="$1[@]" arr=("${!array}") last="${start[0]},${start[1]}" for el in ${arr[*]}; do if [[ $((${el#*,}-${last#*,})) -eq -1 ]] && [[ $((${el%,*}-${last%,*})) -eq -1 ]]; then grid["$last"]=${cell_type[left_up]} elif [[ $((${el#*,}-${last#*,})) -eq -1 ]] && [[ $((${el%,*}-${last%,*})) -eq 1 ]]; then grid["$last"]=${cell_type[right_down]} elif [[ $((${el#*,}-${last#*,})) -eq 1 ]] && [[ $((${el%,*}-${last%,*})) -eq -1 ]]; then grid["$last"]=${cell_type[right_up]} elif [[ $((${el#*,}-${last#*,})) -eq 1 ]] && [[ $((${el%,*}-${last%,*})) -eq 1 ]]; then grid["$last"]=${cell_type[left_down]} elif [[ $((${el#*,}-${last#*,})) -eq -1 ]];then grid["$last"]=${cell_type[left]} elif [[ $((${el%,*}-${last%,*})) -eq -1 ]];then grid["$last"]=${cell_type[up]} elif [[ $((${el#*,}-${last#*,})) -eq 1 ]];then grid["$last"]=${cell_type[right]} elif [[ $((${el%,*}-${last%,*})) -eq 1 ]];then grid["$last"]=${cell_type[down]} else grid["$last"]=${cell_type[path]} fi last=$el done grid[${start[0]},${start[1]}]=${cell_type[start]} grid[${end[0]},${end[1]}]=${cell_type[end]} } main(){ generate_rosetta_grid path=( "$(find_path)" ) pstr="$(echo "${path[*]}" | xargs | sed "s/[[:space:]]/ → /g")" echo path: "$pstr" if [[ -z $pstr ]]; then echo "No path found." else map_to_arrows path print_table fi } main "$@" - Output:
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 →→→→✪
var Equals = Fn.new { |p1, p2| p1[0] == p2[0] && p1[1] == p2[1] } var Contains = Fn.new { |pairs, p| for (pair in pairs) { if (Equals.call(p, pair)) return true } return false } var Remove = Fn.new { |pairs, p| for (pair in pairs) { if (Equals.call(p, pair)) { pairs.remove(pair) return } } } class AStarGraph { construct new() { _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]] } barriers { _barriers } heuristic(start, goal) { var D1 = 1 var D2 = 1 var dx = (start[0] - goal[0]).abs var dy = (start[1] - goal[1]).abs return D1 * (dx + dy) + (D2 - 2*D1) * dx.min(dy) } getVertexNeighbors(pos) { var n = [] for (d in [[1,0], [-1,0], [0,1], [0,-1], [1,1], [-1,1], [1,-1], [-1,-1]]) { var x2 = pos[0] + d[0] var y2 = pos[1] + d[1] if (x2 < 0 || x2 > 7 || y2 < 0 || y2 > 7) continue n.add([x2, y2]) } return n } moveCost(b) { Contains.call(_barriers, b) ? 100 : 1 } } var AStarSearch = Fn.new { |start, end, graph| var G = {start.toString: 0} var F = {start.toString: graph.heuristic(start, end)} var closedVertices = [] var openVertices = [start] var cameFrom = {} while (openVertices.count > 0) { var current = null var currentFscore = 1 / 0 for (pos in openVertices) { var v if ((v = F[pos.toString]) && v && v < currentFscore) { currentFscore = v current = pos } } if (Equals.call(current, end)) { var path = [current] while (cameFrom.containsKey(current.toString)) { current = cameFrom[current.toString] path.add(current) } path = path[-1..0] return [path, F[end.toString]] } Remove.call(openVertices, current) closedVertices.add(current) for (neighbor in graph.getVertexNeighbors(current)) { if (Contains.call(closedVertices, neighbor)) continue var candidateG = G[current.toString] + graph.moveCost(neighbor) if (!Contains.call(openVertices, neighbor)) { openVertices.add(neighbor) } else if (candidateG >= G[neighbor.toString]) continue cameFrom[neighbor.toString] = current G[neighbor.toString] = candidateG var H = graph.heuristic(neighbor, end) F[neighbor.toString] = G[neighbor.toString] + H } } Fiber.abort("A* failed to find a solution") } var graph = AStarGraph.new() var rc = AStarSearch.call([0,0], [7,7], graph) var route = rc[0] var cost = rc[1] var w = 10 var h = 10 var grid = List.filled(h, null) for (i in 0...h) grid[i] = List.filled(w, ".") for (y in 0...h) { grid[y][0] = "█" grid[y][-1] = "█" } for (x in 0...w) { grid[0][x] = "█" grid[-1][x] = "█" } for (p in graph.barriers) { var x = p[0] var y = p[1] grid[x+1][y+1] = "█" } for (p in route) { var x = p[0] var y = p[1] grid[x+1][y+1] = "x" } for (line in grid) System.print(line.join()) System.print("\npath 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]]
// 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")); }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 } }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()) }- 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
const std = @import("std"); const Point = struct { x: i32, y: i32, pub fn init(a: i32, b: i32) Point { return Point{ .x = a, .y = b }; } pub fn equals(self: Point, other: Point) bool { return self.x == other.x and self.y == other.y; } pub fn add(self: Point, other: Point) Point { return Point.init(self.x + other.x, self.y + other.y); } }; const Map = struct { m: [8][8]u8, w: i32, h: i32, pub fn init() Map { var map = Map{ .m = undefined, .w = 8, .h = 8, }; const t = [_][8]u8{ [_]u8{0, 0, 0, 0, 0, 0, 0, 0}, [_]u8{0, 0, 0, 0, 0, 0, 0, 0}, [_]u8{0, 0, 0, 0, 1, 1, 1, 0}, [_]u8{0, 0, 1, 0, 0, 0, 1, 0}, [_]u8{0, 0, 1, 0, 0, 0, 1, 0}, [_]u8{0, 0, 1, 1, 1, 1, 1, 0}, [_]u8{0, 0, 0, 0, 0, 0, 0, 0}, [_]u8{0, 0, 0, 0, 0, 0, 0, 0}, }; for (0..@as(usize, @intCast(map.h))) |r| { for (0..@as(usize, @intCast(map.w))) |s| { map.m[s][r] = t[r][s]; } } return map; } pub fn get(self: Map, x: i32, y: i32) u8 { return self.m[@as(usize, @intCast(x))][@as(usize, @intCast(y))]; } }; const Node = struct { pos: Point, parent: Point, dist: i32, cost: i32, pub fn equals(self: Node, other: Node) bool { return self.pos.equals(other.pos); } pub fn equalsPoint(self: Node, p: Point) bool { return self.pos.equals(p); } pub fn lessThan(self: Node, other: Node) bool { return (self.dist + self.cost) < (other.dist + other.cost); } }; const AStar = struct { m: Map, end: Point, start: Point, neighbours: [8]Point, open: std.ArrayList(Node), closed: std.ArrayList(Node), allocator: std.mem.Allocator, pub fn init(allocator: std.mem.Allocator) AStar { return AStar{ .m = undefined, .end = undefined, .start = undefined, .neighbours = [_]Point{ Point.init(-1, -1), Point.init(1, -1), Point.init(-1, 1), Point.init(1, 1), Point.init(0, -1), Point.init(-1, 0), Point.init(0, 1), Point.init(1, 0), }, .open = std.ArrayList(Node).init(allocator), .closed = std.ArrayList(Node).init(allocator), .allocator = allocator, }; } pub fn deinit(self: *AStar) void { self.open.deinit(); self.closed.deinit(); } pub fn calcDist(self: AStar, p: Point) i32 { // need a better heuristic const x = self.end.x - p.x; const y = self.end.y - p.y; return (x * x + y * y); } pub fn isValid(self: AStar, p: Point) bool { return (p.x > -1 and p.y > -1 and p.x < self.m.w and p.y < self.m.h); } pub fn existPoint(self: *AStar, p: Point, cost: i32) !bool { // Check in closed list for (self.closed.items, 0..) |node, i| { if (node.equalsPoint(p)) { if (node.cost + node.dist < cost) { return true; } else { _ = self.closed.orderedRemove(i); return false; } } } // Check in open list for (self.open.items, 0..) |node, i| { if (node.equalsPoint(p)) { if (node.cost + node.dist < cost) { return true; } else { _ = self.open.orderedRemove(i); return false; } } } return false; } pub fn fillOpen(self: *AStar, n: Node) !bool { for (0..8) |x| { // one can make diagonals have different cost const stepCost: i32 = if (x < 4) 1 else 1; const neighbour = n.pos.add(self.neighbours[x]); if (neighbour.equals(self.end)) { return true; } if (self.isValid(neighbour) and self.m.get(neighbour.x, neighbour.y) != 1) { const nc = stepCost + n.cost; const dist = self.calcDist(neighbour); if (!(try self.existPoint(neighbour, nc + dist))) { const m = Node{ .cost = nc, .dist = dist, .pos = neighbour, .parent = n.pos, }; try self.open.append(m); } } } return false; } fn nodeLessThan(_: void, a: Node, b: Node) bool { return a.lessThan(b); } pub fn search(self: *AStar, s: Point, e: Point, mp: Map) !bool { self.end = e; self.start = s; self.m = mp; // Clear lists self.open.clearRetainingCapacity(); self.closed.clearRetainingCapacity(); const n = Node{ .cost = 0, .pos = s, .parent = Point.init(0, 0), .dist = self.calcDist(s), }; try self.open.append(n); while (self.open.items.len > 0) { // Sort open list by cost + dist std.sort.pdq(Node, self.open.items, {}, AStar.nodeLessThan); const node = self.open.orderedRemove(0); try self.closed.append(node); if (try self.fillOpen(node)) { return true; } } return false; } pub fn path(self: *AStar, pathList: *std.ArrayList(Point)) !i32 { try pathList.insert(0, self.end); const cost = 1 + self.closed.items[self.closed.items.len - 1].cost; try pathList.insert(0, self.closed.items[self.closed.items.len - 1].pos); var parent = self.closed.items[self.closed.items.len - 1].parent; var i = self.closed.items.len; while (i > 0) { i -= 1; if (self.closed.items[i].pos.equals(parent) and !self.closed.items[i].pos.equals(self.start)) { try pathList.insert(0, self.closed.items[i].pos); parent = self.closed.items[i].parent; } } try pathList.insert(0, self.start); return cost; } }; fn inPath(path: std.ArrayList(Point), p: Point) bool { for (path.items) |point| { if (point.equals(p)) { return true; } } return false; } pub fn main() !void { var gpa = std.heap.GeneralPurposeAllocator(.{}){}; const allocator = gpa.allocator(); defer _ = gpa.deinit(); var m = Map.init(); const s = Point.init(0, 0); const e = Point.init(7, 7); var as = AStar.init(allocator); defer as.deinit(); if (try as.search(s, e, m)) { var path = std.ArrayList(Point).init(allocator); defer path.deinit(); const c = try as.path(&path); const stdout = std.io.getStdOut().writer(); for (0..9) |y_usize| { const y = @as(i32, @intCast(y_usize)) - 1; for (0..9) |x_usize| { const x = @as(i32, @intCast(x_usize)) - 1; if (x < 0 or y < 0 or x > 7 or y > 7 or m.get(x, y) == 1) { try stdout.print("█", .{}); } else { if (inPath(path, Point.init(x, y))) { try stdout.print("x", .{}); } else { try stdout.print(".", .{}); } } } try stdout.print("\n", .{}); } try stdout.print("\nPath cost {d}: ", .{c}); for (path.items) |p| { try stdout.print("({d}, {d}) ", .{p.x, p.y}); } } try std.io.getStdOut().writer().print("\n\n", .{}); } - Output:
█████████ █x....... █.x..xxx. █..xx███x █..█...█x █..█...█x █..█████x █.......x █.......x Path cost 12: (0, 0) (1, 1) (2, 2) (3, 2) (4, 1) (5, 1) (6, 1) (7, 2) (7, 3) (7, 4) (7, 5) (7, 6) (7, 7)