├── README ├── LICENSE └── src ├── astar.py └── astar_demo.py /README: -------------------------------------------------------------------------------- 1 | A* search algorithm implemented in Python. 2 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Copyright (c) 2008 Mikael Lind 2 | 3 | Permission is hereby granted, free of charge, to any person 4 | obtaining a copy of this software and associated documentation 5 | files (the "Software"), to deal in the Software without 6 | restriction, including without limitation the rights to use, 7 | copy, modify, merge, publish, distribute, sublicense, and/or sell 8 | copies of the Software, and to permit persons to whom the 9 | Software is furnished to do so, subject to the following 10 | conditions: 11 | 12 | The above copyright notice and this permission notice shall be 13 | included in all copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, 16 | EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES 17 | OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND 18 | NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT 19 | HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, 20 | WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 21 | FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR 22 | OTHER DEALINGS IN THE SOFTWARE. 23 | -------------------------------------------------------------------------------- /src/astar.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2008 Mikael Lind 2 | # 3 | # Permission is hereby granted, free of charge, to any person obtaining a copy 4 | # of this software and associated documentation files (the "Software"), to deal 5 | # in the Software without restriction, including without limitation the rights 6 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 7 | # copies of the Software, and to permit persons to whom the Software is 8 | # furnished to do so, subject to the following conditions: 9 | # 10 | # The above copyright notice and this permission notice shall be included in 11 | # all copies or substantial portions of the Software. 12 | # 13 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 14 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 15 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 16 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 17 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 18 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 19 | # SOFTWARE. 20 | 21 | 22 | from heapq import heappush, heappop 23 | from sys import maxint 24 | 25 | 26 | # Represent each node as a list, ordering the elements so that a heap of nodes 27 | # is ordered by f = g + h, with h as a first, greedy tie-breaker and num as a 28 | # second, definite tie-breaker. Store the redundant g for fast and accurate 29 | # calculations. 30 | 31 | F, H, NUM, G, POS, OPEN, VALID, PARENT = xrange(8) 32 | 33 | 34 | def astar(start_pos, neighbors, goal, start_g, cost, heuristic, limit=maxint, 35 | debug=None): 36 | 37 | """Find the shortest path from start to goal. 38 | 39 | Arguments: 40 | 41 | start_pos - The starting position. 42 | neighbors(pos) - A function returning all neighbor positions of the given 43 | position. 44 | goal(pos) - A function returning true given a goal position, false 45 | otherwise. 46 | start_g - The starting cost. 47 | cost(a, b) - A function returning the cost for moving from one 48 | position to another. 49 | heuristic(pos) - A function returning an estimate of the total cost 50 | remaining for reaching goal from the given position. 51 | Overestimates can yield suboptimal paths. 52 | limit - The maximum number of positions to search. 53 | debug(nodes) - This function will be called with a dictionary of all 54 | nodes. 55 | 56 | The function returns the best path found. The returned path excludes the 57 | starting position. 58 | """ 59 | 60 | # Create the start node. 61 | nums = iter(xrange(maxint)) 62 | start_h = heuristic(start_pos) 63 | start = [start_g + start_h, start_h, nums.next(), start_g, start_pos, True, 64 | True, None] 65 | 66 | # Track all nodes seen so far. 67 | nodes = {start_pos: start} 68 | 69 | # Maintain a heap of nodes. 70 | heap = [start] 71 | 72 | # Track the best path found so far. 73 | best = start 74 | 75 | while heap: 76 | 77 | # Pop the next node from the heap. 78 | current = heappop(heap) 79 | current[OPEN] = False 80 | 81 | # Have we reached the goal? 82 | if goal(current[POS]): 83 | best = current 84 | break 85 | 86 | # Visit the neighbors of the current node. 87 | for neighbor_pos in neighbors(current[POS]): 88 | neighbor_g = current[G] + cost(current[POS], neighbor_pos) 89 | neighbor = nodes.get(neighbor_pos) 90 | if neighbor is None: 91 | 92 | # Limit the search. 93 | if len(nodes) >= limit: 94 | continue 95 | 96 | # We have found a new node. 97 | neighbor_h = heuristic(neighbor_pos) 98 | neighbor = [neighbor_g + neighbor_h, neighbor_h, nums.next(), 99 | neighbor_g, neighbor_pos, True, True, current[POS]] 100 | nodes[neighbor_pos] = neighbor 101 | heappush(heap, neighbor) 102 | if neighbor_h < best[H]: 103 | 104 | # We are approaching the goal. 105 | best = neighbor 106 | 107 | elif neighbor_g < neighbor[G]: 108 | 109 | # We have found a better path to the neighbor. 110 | if neighbor[OPEN]: 111 | 112 | # The neighbor is already open. Finding and updating it 113 | # in the heap would be a linear complexity operation. 114 | # Instead we mark the neighbor as invalid and make an 115 | # updated copy of it. 116 | 117 | neighbor[VALID] = False 118 | nodes[neighbor_pos] = neighbor = neighbor[:] 119 | neighbor[F] = neighbor_g + neighbor[H] 120 | neighbor[NUM] = nums.next() 121 | neighbor[G] = neighbor_g 122 | neighbor[VALID] = True 123 | neighbor[PARENT] = current[POS] 124 | heappush(heap, neighbor) 125 | 126 | else: 127 | 128 | # Reopen the neighbor. 129 | neighbor[F] = neighbor_g + neighbor[H] 130 | neighbor[G] = neighbor_g 131 | neighbor[PARENT] = current[POS] 132 | neighbor[OPEN] = True 133 | heappush(heap, neighbor) 134 | 135 | # Discard leading invalid nodes from the heap. 136 | while heap and not heap[0][VALID]: 137 | heappop(heap) 138 | 139 | if debug is not None: 140 | # Pass the dictionary of nodes to the caller. 141 | debug(nodes) 142 | 143 | # Return the best path as a list. 144 | path = [] 145 | current = best 146 | while current[PARENT] is not None: 147 | path.append(current[POS]) 148 | current = nodes[current[PARENT]] 149 | path.reverse() 150 | return path 151 | -------------------------------------------------------------------------------- /src/astar_demo.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2008 Mikael Lind 2 | # 3 | # Permission is hereby granted, free of charge, to any person obtaining a copy 4 | # of this software and associated documentation files (the "Software"), to deal 5 | # in the Software without restriction, including without limitation the rights 6 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 7 | # copies of the Software, and to permit persons to whom the Software is 8 | # furnished to do so, subject to the following conditions: 9 | # 10 | # The above copyright notice and this permission notice shall be included in 11 | # all copies or substantial portions of the Software. 12 | # 13 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 14 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 15 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 16 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 17 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 18 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 19 | # SOFTWARE. 20 | 21 | 22 | from astar import astar 23 | import curses, random 24 | 25 | 26 | DUNGEON = """ 27 | ################# 28 | # 29 | # ########### 30 | # # 31 | ############# # # 32 | # # # 33 | # # # 34 | # ################### # 35 | # # # 36 | # # # 37 | # # # # 38 | # ############# # # 39 | # # 40 | ############### # # 41 | # # 42 | # # 43 | # # 44 | ###################### 45 | """ 46 | 47 | HEIGHT, WIDTH = 22, 79 48 | MAX_LIMIT = HEIGHT * WIDTH 49 | LIMIT = MAX_LIMIT // 2 50 | DEBUG = False 51 | COLOR = True 52 | 53 | 54 | class Cell(object): 55 | def __init__(self, char): 56 | self.char = char 57 | self.tag = 0 58 | self.index = 0 59 | self.neighbors = None 60 | 61 | 62 | class Grid(object): 63 | 64 | def __init__(self, cells): 65 | self.height, self.width = len(cells), len(cells[0]) 66 | self.cells = cells 67 | 68 | def __contains__(self, pos): 69 | y, x = pos 70 | return 0 <= y < self.height and 0 <= x < self.width 71 | 72 | def __getitem__(self, pos): 73 | y, x = pos 74 | return self.cells[y][x] 75 | 76 | def neighbors(self, y, x): 77 | for dy, dx in ((-1, -1), (-1, 0), (-1, 1), (0, -1), (0, 1), (1, -1), 78 | (1, 0), (1, 1)): 79 | if (y + dy, x + dx) in self: 80 | yield y + dy, x + dx 81 | 82 | 83 | def parse_grid(grid_str, width, height): 84 | 85 | # Split the grid string into lines. 86 | lines = [line.rstrip() for line in grid_str.splitlines()[1:]] 87 | 88 | # Pad the top and bottom. 89 | top = (height - len(lines)) // 2 90 | bottom = (height - len(lines) + 1) // 2 91 | lines = ([''] * top + lines + [''] * bottom)[:height] 92 | 93 | # Pad the left and right sides. 94 | max_len = max(len(line) for line in lines) 95 | left = (width - max_len) // 2 96 | lines = [' ' * left + line.ljust(width - left)[:width - left] 97 | for line in lines] 98 | 99 | # Create the grid. 100 | cells = [[Cell(char) for char in line] for line in lines] 101 | return Grid(cells) 102 | 103 | 104 | class Engine(object): 105 | 106 | def __init__(self, grid): 107 | self.grid = grid 108 | self.y = random.randrange(self.grid.height) 109 | self.x = random.randrange(self.grid.width) 110 | self.goal = (random.randrange(self.grid.height), 111 | random.randrange(self.grid.width)) 112 | self.limit = LIMIT 113 | self.tag = 1 114 | self.nodes = {} 115 | self.path = [] 116 | self.dirty = True 117 | self.debug = DEBUG 118 | self.color = COLOR 119 | 120 | def move_cursor(self, dy, dx): 121 | y, x = self.y + dy, self.x + dx 122 | if (y, x) in self.grid: 123 | self.y, self.x = y, x 124 | self.dirty = True 125 | 126 | def update_path(self): 127 | if not self.dirty: 128 | return 129 | self.dirty = False 130 | self.tag += 1 131 | def neighbors(pos): 132 | cell = self.grid[pos] 133 | if cell.neighbors is None: 134 | y, x = pos 135 | cell.neighbors = [] 136 | for neighbor_y, neighbor_x in self.grid.neighbors(y, x): 137 | if self.grid[neighbor_y, neighbor_x].char != '#': 138 | cell.neighbors.append((neighbor_y, neighbor_x)) 139 | return cell.neighbors 140 | def goal(pos): 141 | return pos == self.goal 142 | def cost(from_pos, to_pos): 143 | from_y, from_x = from_pos 144 | to_y, to_x = to_pos 145 | return 14 if to_y - from_y and to_x - from_x else 10 146 | def estimate(pos): 147 | y, x = pos 148 | goal_y, goal_x = self.goal 149 | dy, dx = abs(goal_y - y), abs(goal_x - x) 150 | return min(dy, dx) * 14 + abs(dy - dx) * 10 151 | def debug(nodes): 152 | self.nodes = nodes 153 | self.path = astar((self.y, self.x), neighbors, goal, 0, cost, 154 | estimate, self.limit, debug) 155 | 156 | 157 | def update_view(stdscr, engine): 158 | 159 | # Update the grid view. 160 | success = ((engine.y, engine.x) == engine.goal 161 | or engine.path and engine.goal == engine.path[-1]) 162 | for y, line in enumerate(engine.grid.cells): 163 | for x, cell in enumerate(line): 164 | char = cell.char 165 | color = curses.COLOR_BLUE if char == '#' else curses.COLOR_BLACK 166 | if engine.debug: 167 | node = engine.nodes.get((y, x)) 168 | if node is not None: 169 | char = '.' 170 | color = curses.COLOR_YELLOW 171 | stdscr.addch(y, x, char, curses.color_pair(color) if engine.color 172 | else 0) 173 | 174 | # Update the status lines. 175 | blocked = (engine.grid[engine.y, engine.x].char == '#') 176 | status_1 = ['[+-] Limit = %d' % engine.limit] 177 | if (engine.y, engine.x) != engine.goal: 178 | status_1.append('[ENTER] Goal') 179 | status_1.append('[SPACE] %s' % ('Unblock' if blocked else 'Block')) 180 | status_1.append('[Q]uit') 181 | status_2 = 'Searched %d nodes.' % len(engine.nodes) 182 | stdscr.addstr(HEIGHT, 0, (' '.join(status_1)).ljust(WIDTH)[:WIDTH], 183 | curses.A_STANDOUT) 184 | stdscr.addstr(HEIGHT + 1, 0, status_2.ljust(WIDTH)[:WIDTH]) 185 | 186 | # Update the path and goal. 187 | path_color = curses.COLOR_GREEN if success else curses.COLOR_RED 188 | path_attr = curses.color_pair(path_color) if engine.color else 0 189 | if engine.debug: 190 | path_attr |= curses.A_STANDOUT 191 | for i, pos in enumerate(engine.path): 192 | y, x = pos 193 | stdscr.addch(y, x, ':', path_attr) 194 | goal_y, goal_x = engine.goal 195 | stdscr.addch(goal_y, goal_x, '%', path_attr) 196 | 197 | # Update the start. 198 | if (engine.y, engine.x) == engine.goal: 199 | char = '%' 200 | elif engine.grid[engine.y, engine.x].char == '#': 201 | char = '#' 202 | else: 203 | char = '@' 204 | stdscr.addch(engine.y, engine.x, char) 205 | stdscr.move(engine.y, engine.x) 206 | 207 | 208 | def read_command(stdscr): 209 | key = stdscr.getch() 210 | stdscr.nodelay(True) 211 | while True: 212 | if stdscr.getch() == -1: 213 | break 214 | stdscr.nodelay(False) 215 | return key 216 | 217 | 218 | def handle_command(key, engine): 219 | 220 | # Move the cursor. 221 | if key == ord('7'): engine.move_cursor(-1, -1) 222 | if key in (ord('8'), curses.KEY_UP): engine.move_cursor(-1, 0) 223 | if key == ord('9'): engine.move_cursor(-1, 1) 224 | if key in (ord('4'), curses.KEY_LEFT): engine.move_cursor( 0, -1) 225 | if key in (ord('6'), curses.KEY_RIGHT): engine.move_cursor( 0, 1) 226 | if key == ord('1'): engine.move_cursor( 1, -1) 227 | if key in (ord('2'), curses.KEY_DOWN): engine.move_cursor( 1, 0) 228 | if key == ord('3'): engine.move_cursor( 1, 1) 229 | 230 | # Change the search limit. 231 | if key == ord('+'): 232 | if engine.limit < MAX_LIMIT: 233 | engine.limit += 1 234 | engine.dirty = True 235 | if key == ord('-'): 236 | if engine.limit > 0: 237 | engine.limit -= 1 238 | engine.dirty = True 239 | 240 | # Insert or delete a block at the cursor. 241 | if key == ord(' '): 242 | cell = engine.grid[engine.y, engine.x] 243 | cell.char = ' ' if cell.char == '#' else '#' 244 | for y, x in engine.grid.neighbors(engine.y, engine.x): 245 | engine.grid[y, x].neighbors = None 246 | engine.dirty = True 247 | 248 | if key in (ord('\n'), curses.KEY_ENTER): 249 | if (engine.y, engine.x) != engine.goal: 250 | engine.goal = engine.y, engine.x 251 | engine.dirty = True 252 | 253 | if key in (ord('d'), ord('D')): 254 | engine.debug = not engine.debug 255 | if key in (ord('c'), ord('C')) and COLOR: 256 | engine.color = not engine.color 257 | 258 | 259 | def main(stdscr): 260 | if COLOR: 261 | curses.use_default_colors() 262 | for i in xrange(curses.COLOR_RED, curses.COLOR_WHITE + 1): 263 | curses.init_pair(i, i, -1) 264 | grid = parse_grid(DUNGEON, WIDTH, HEIGHT) 265 | engine = Engine(grid) 266 | while True: 267 | engine.update_path() 268 | update_view(stdscr, engine) 269 | key = read_command(stdscr) 270 | if key in (ord('q'), ord('Q')): 271 | break 272 | handle_command(key, engine) 273 | 274 | 275 | if __name__ == '__main__': 276 | curses.wrapper(main) 277 | --------------------------------------------------------------------------------