|
| 1 | +""" |
| 2 | +A* Pathfinding Algorithm (Python Implementation) |
| 3 | +
|
| 4 | +This algorithm finds the shortest path between two points on a grid |
| 5 | +using a heuristic (Manhattan distance). It combines Dijkstra’s optimal |
| 6 | +pathfinding with heuristic-based search for efficiency. |
| 7 | +
|
| 8 | +Time Complexity: O(E log V) |
| 9 | +Space Complexity: O(V) |
| 10 | +""" |
| 11 | + |
| 12 | +from heapq import heappush, heappop |
| 13 | + |
| 14 | +def heuristic(a, b): |
| 15 | + """Heuristic function: Manhattan distance.""" |
| 16 | + return abs(a[0] - b[0]) + abs(a[1] - b[1]) |
| 17 | + |
| 18 | +def a_star_search(grid, start, goal): |
| 19 | + """ |
| 20 | + Performs A* search on a 2D grid. |
| 21 | + |
| 22 | + Args: |
| 23 | + grid (list[list[int]]): 0 = free cell, 1 = obstacle |
| 24 | + start (tuple): (row, col) start position |
| 25 | + goal (tuple): (row, col) goal position |
| 26 | + |
| 27 | + Returns: |
| 28 | + path (list[tuple]): Shortest path as list of coordinates. |
| 29 | + """ |
| 30 | + rows, cols = len(grid), len(grid[0]) |
| 31 | + open_set = [] |
| 32 | + heappush(open_set, (0, start)) |
| 33 | + |
| 34 | + came_from = {} |
| 35 | + g_score = {start: 0} |
| 36 | + |
| 37 | + while open_set: |
| 38 | + _, current = heappop(open_set) |
| 39 | + |
| 40 | + if current == goal: |
| 41 | + # Reconstruct path |
| 42 | + path = [] |
| 43 | + while current in came_from: |
| 44 | + path.append(current) |
| 45 | + current = came_from[current] |
| 46 | + path.append(start) |
| 47 | + return path[::-1] |
| 48 | + |
| 49 | + neighbors = [ |
| 50 | + (current[0] + dx, current[1] + dy) |
| 51 | + for dx, dy in [(1,0), (-1,0), (0,1), (0,-1)] |
| 52 | + ] |
| 53 | + |
| 54 | + for n in neighbors: |
| 55 | + if 0 <= n[0] < rows and 0 <= n[1] < cols and grid[n[0]][n[1]] == 0: |
| 56 | + tentative_g = g_score[current] + 1 |
| 57 | + if tentative_g < g_score.get(n, float('inf')): |
| 58 | + came_from[n] = current |
| 59 | + g_score[n] = tentative_g |
| 60 | + f_score = tentative_g + heuristic(n, goal) |
| 61 | + heappush(open_set, (f_score, n)) |
| 62 | + |
| 63 | + return None # No path found |
| 64 | + |
| 65 | +# Example usage: |
| 66 | +if __name__ == "__main__": |
| 67 | + grid = [ |
| 68 | + [0, 1, 0, 0, 0], |
| 69 | + [0, 1, 0, 1, 0], |
| 70 | + [0, 0, 0, 1, 0], |
| 71 | + [0, 1, 1, 1, 0], |
| 72 | + [0, 0, 0, 0, 0] |
| 73 | + ] |
| 74 | + start = (0, 0) |
| 75 | + goal = (4, 4) |
| 76 | + path = a_star_search(grid, start, goal) |
| 77 | + print("Shortest Path:", path) |
0 commit comments