a star
def heuristic(a, b):
return abs(a[0] - b[0]) + abs(a[1] - b[1]) # Manhattan distance
def get_neighbors(pos, grid):
neighbors = []
directions = [(0,1), (1,0), (0,-1), (-1,0)]
for dx, dy in directions:
x, y = pos[0] + dx, pos[1] + dy
if 0 <= x < len(grid) and 0 <= y < len(grid[0]):
if grid[x][y] == 0: # 0 means walkable
neighbors.append((x, y))
return neighbors
def a_star(grid, start, goal):
open_list = [start]
came_from = {}
g_score = {start: 0}
f_score = {start: heuristic(start, goal)}
while open_list:
current = min(open_list, key=lambda x: f_score.get(x, float('inf')))
if current == goal:
path = []
while current in came_from:
path.append(current)
current = came_from[current]
path.append(start)
return path[::-1]
open_list.remove(current)
for neighbor in get_neighbors(current, grid):
temp_g = g_score[current] + 1
if temp_g < g_score.get(neighbor, float('inf')):
came_from[neighbor] = current
g_score[neighbor] = temp_g
f_score[neighbor] = temp_g + heuristic(neighbor, goal)
if neighbor not in open_list:
open_list.append(neighbor)
return None
User input for grid size and content
n = int(input("Enter grid size (n x n): "))
grid = []
print("Enter the grid row by row (0 = walkable, 1 = obstacle):")
for i in range(n):
row = list(map(int, input(f"Row {i+1}: ").split()))
if len(row) != n:
print(f"Invalid input, please enter {n} values.")
exit()
grid.append(row)
start = tuple(map(int, input("Enter the starting point (x y): ").split()))
goal = tuple(map(int, input("Enter the goal point (x y): ").split()))
Output the path
path = a_star(grid, start, goal)
if path:
print("Path found:", path)
else:
print("No path found.")