I'm trying to make path finder, but I can't fix it to work both at the same time:
not cutting corners
going diagonal when it's the only way to go or it's necessary
My current code is:
def heuristic(a, b):
""" Manhattan distance heuristic to encourage straight-line movement first """
return abs(a[0] - b[0]) + abs(a[1] - b[1])
def get_neighbors(grid, node):
x, y = node
neighbors = []
rows = len(grid)
cols = len(grid[0]) if rows > 0 else 0
# Straight moves (up, down, left, right) - Cost 1
for dx, dy in [(-1,0), (1,0), (0,-1), (0,1)]:
nx, ny = x + dx, y + dy
if 0 <= nx < cols and 0 <= ny < rows and grid[ny][nx] == 1:
neighbors.append(((nx, ny), 1))
# Diagonal moves - Cost 2.1 (should always be avoided unless necessary)
for dx, dy in [(-1,-1), (-1,1), (1,-1), (1,1)]:
nx, ny = x + dx, y + dy
if 0 <= nx < cols and 0 <= ny < rows and grid[ny][nx] == 1:
# Allow diagonal move unless both adjacent straight cells are blocked
if not (grid[y][nx] == 0 and grid[ny][x] == 0):
neighbors.append(((nx, ny), 2.1))
return neighbors
def astar(grid, start, goal):
""" A* Pathfinding Algorithm """
open_set = []
heapq.heappush(open_set, (0, start))
came_from = {}
g_score = {start: 0}
f_score = {start: heuristic(start, goal)}
while open_set:
_, current = heapq.heappop(open_set)
if current == goal:
return reconstruct_path(came_from, current)
for neighbor, move_cost in get_neighbors(grid, current):
tentative_g_score = g_score[current] + move_cost
if neighbor not in g_score or tentative_g_score < g_score[neighbor]:
came_from[neighbor] = current
g_score[neighbor] = tentative_g_score
f_score[neighbor] = g_score[neighbor] + heuristic(neighbor, goal)
heapq.heappush(open_set, (f_score[neighbor], neighbor))
return None
def reconstruct_path(came_from, current):
""" Reconstructs path from goal to start """
path = [current]
while current in came_from:
current = came_from[current]
path.append(current)
path.reverse()
return path
image showing the result and expected path
The code works fine when it comes to non-diagonal movements, but when there is only one way that requires diagonal it doesn't work (on screenshoot red square, if we start from there it doesn't find path to "G", green way is the supposed way to go).
All the time either it starts to go only diagonals - then it finds way from red square to G or it's avoiding cutting corners, but then diagonals doesn't work.
I would like to achieve it finds blue line path for example... (# on screen is a non-walkable sqm, . is a walkable sqm).
I'm trying to make path finder, but I can't fix it to work both at the same time:
not cutting corners
going diagonal when it's the only way to go or it's necessary
My current code is:
def heuristic(a, b):
""" Manhattan distance heuristic to encourage straight-line movement first """
return abs(a[0] - b[0]) + abs(a[1] - b[1])
def get_neighbors(grid, node):
x, y = node
neighbors = []
rows = len(grid)
cols = len(grid[0]) if rows > 0 else 0
# Straight moves (up, down, left, right) - Cost 1
for dx, dy in [(-1,0), (1,0), (0,-1), (0,1)]:
nx, ny = x + dx, y + dy
if 0 <= nx < cols and 0 <= ny < rows and grid[ny][nx] == 1:
neighbors.append(((nx, ny), 1))
# Diagonal moves - Cost 2.1 (should always be avoided unless necessary)
for dx, dy in [(-1,-1), (-1,1), (1,-1), (1,1)]:
nx, ny = x + dx, y + dy
if 0 <= nx < cols and 0 <= ny < rows and grid[ny][nx] == 1:
# Allow diagonal move unless both adjacent straight cells are blocked
if not (grid[y][nx] == 0 and grid[ny][x] == 0):
neighbors.append(((nx, ny), 2.1))
return neighbors
def astar(grid, start, goal):
""" A* Pathfinding Algorithm """
open_set = []
heapq.heappush(open_set, (0, start))
came_from = {}
g_score = {start: 0}
f_score = {start: heuristic(start, goal)}
while open_set:
_, current = heapq.heappop(open_set)
if current == goal:
return reconstruct_path(came_from, current)
for neighbor, move_cost in get_neighbors(grid, current):
tentative_g_score = g_score[current] + move_cost
if neighbor not in g_score or tentative_g_score < g_score[neighbor]:
came_from[neighbor] = current
g_score[neighbor] = tentative_g_score
f_score[neighbor] = g_score[neighbor] + heuristic(neighbor, goal)
heapq.heappush(open_set, (f_score[neighbor], neighbor))
return None
def reconstruct_path(came_from, current):
""" Reconstructs path from goal to start """
path = [current]
while current in came_from:
current = came_from[current]
path.append(current)
path.reverse()
return path
image showing the result and expected path
The code works fine when it comes to non-diagonal movements, but when there is only one way that requires diagonal it doesn't work (on screenshoot red square, if we start from there it doesn't find path to "G", green way is the supposed way to go).
All the time either it starts to go only diagonals - then it finds way from red square to G or it's avoiding cutting corners, but then diagonals doesn't work.
I would like to achieve it finds blue line path for example... (# on screen is a non-walkable sqm, . is a walkable sqm).
1 Answer
Reset to default 1First, let's add some code that you should really have included in the question, allowing people to reproduce the issue (also, see Why should I not upload images of code/data/errors?):
map_data = [
[1, 7, 1, 2, 4, 1, 4],
[10, 1, 4, 5],
[0, 11, 9],
[0, 9, 1, 1, 6, 2, 1],
[0, 10, 9, 1],
[0, 7, 1, 3, 8, 1],
[0, 9, 1, 3, 5, 2],
[4, 2, 6, 2, 5, 1],
[1, 2, 1, 2, 1, 4, 1, 2, 4, 2],
[1, 10, 1, 2, 6],
[1, 10, 2, 1, 5, 1],
[1, 10, 2, 1, 4, 2],
[1, 10, 2, 1, 4, 1, 1],
[1, 10, 1, 1, 6, 1],
[1, 10, 1, 2, 5, 1],
[5, 1, 4, 1, 1, 2, 4, 2],
[2, 1, 1, 2, 1, 1, 2, 1, 1, 2, 4, 2],
[2, 6, 2, 1, 1, 2, 5, 1]
]
def build_map(data: list[list[int]], start: tuple[int, int], goal: tuple[int, int]) -> list[list[int]]:
from itertools import cycle
pattern = cycle([0, 1])
# pad data with zeroes at the end of rows, to get all even length rows
data = [row+[0] if len(row) % 2 else row for row in data]
width = sum(data[0])
assert all(sum(data[i]) == width for i in range(len(data)))
assert 0 <= start[0] < width and 0 <= goal[0] < width
assert 0 <= start[1] < len(data) and 0 <= goal[1] < len(data)
return [[x for count in counts for x in [next(pattern)] * count] for counts in data]
def print_map(map: list[list[int]], start: tuple[int, int] = None, goal: tuple[int, int] = None,
path: list[tuple[int, int]] = None):
if path is None:
path = []
for y, row in enumerate(map):
for x, cell in enumerate(row):
if (x, y) == start:
print('S', end='')
elif (x, y) == goal:
print('G', end='')
elif (x, y) in path:
print('P', end='')
else:
print('.' if cell == 1 else '#', end='')
print(' ', end='')
print()
def run(data, start, goal):
map = build_map(data, start, goal)
path = astar(map, start, goal)
print(path)
print_map(map, start, goal, [] if path is None else path)
if __name__ == '__main__':
run(map_data, (10, 8), (12, 8)
This reproduces the problem and shows the outcome you were getting:
[(10, 8), (9, 8), (8, 8), (7, 8), (7, 9), (6, 9), (5, 9), (5, 8), (5, 7), (5, 6), (6, 6), (7, 6), (8, 6), (8, 5), (9, 5), (10, 5), (10, 6), (11, 6), (12, 6), (12, 7), (12, 8)]
# . . . . . . . # . . # # # # . # # # #
# # # # # # # # # # . # # # # . . . . .
. . . . . . . . . . . # # # # # # # # #
. . . . . . . . . # . # # # # # # . . #
. . . . . . . . . . # # # # # # # # # .
. . . . . . . # P P P # # # # # # # # .
. . . . . P P P P # P P P # # # # # . .
# # # # . P # # # # # # P . # # # # # .
# . . # . P # P P P S # G . # # # # . .
# . . . . P P P . . . # . . # # # # # #
# . . . . . . . . . . # # . # # # # # .
# . . . . . . . . . . # # . # # # # . .
# . . . . . . . . . . # # . # # # # . #
# . . . . . . . . . . # . # # # # # # .
# . . . . . . . . . . # . . # # # # # .
# # # # # . # # # # . # . . # # # # . .
# # . # . . # . # # . # . . # # # # . .
# # . . . . . . # # . # . . # # # # # .
It also reproduces the issue when the goal is (12, 14) instead:
None
# . . . . . . . # . . # # # # . # # # #
# # # # # # # # # # . # # # # . . . . .
. . . . . . . . . . . # # # # # # # # #
. . . . . . . . . # . # # # # # # . . #
. . . . . . . . . . # # # # # # # # # .
. . . . . . . # . . . # # # # # # # # .
. . . . . . . . . # . . . # # # # # . .
# # # # . . # # # # # # . . # # # # # .
# . . # . . # . . . S # . . # # # # . .
# . . . . . . . . . . # . . # # # # # #
# . . . . . . . . . . # # . # # # # # .
# . . . . . . . . . . # # . # # # # . .
# . . . . . . . . . . # # . # # # # . #
# . . . . . . . . . . # . # # # # # # .
# . . . . . . . . . . # G . # # # # # .
# # # # # . # # # # . # . . # # # # . .
# # . # . . # . # # . # . . # # # # . .
# # . . . . . . # # . # . . # # # # # .
The problem is in the line user @gthanop pointed out:
if not(grid[y][nx] == 0 and grid[ny][x] == 0):
neighbors.append(((nx, ny), 2.1))
You're allowing adding (nx, ny) to the path only if it is NOT true that both (nx, y) and (x, ny) are walls. But you want to allow adding this to the path only when both ARE walls. So, changing that line to:
if grid[y][nx] == 0 and grid[ny][x] == 0:
neighbors.append(((nx, ny), 2.1))
And running again:
[(10, 8), (10, 9), (9, 9), (8, 9), (7, 9), (6, 9), (5, 9), (5, 8), (5, 7), (5, 6), (6, 6), (7, 6), (8, 6), (8, 5), (9, 5), (10, 5), (10, 6), (11, 6), (12, 6), (12, 7), (12, 8), (12, 9), (13, 9), (13, 10), (13, 11), (13, 12), (12, 13), (12, 14)]
# . . . . . . . # . . # # # # . # # # #
# # # # # # # # # # . # # # # . . . . .
. . . . . . . . . . . # # # # # # # # #
. . . . . . . . . # . # # # # # # . . #
. . . . . . . . . . # # # # # # # # # .
. . . . . . . # P P P # # # # # # # # .
. . . . . P P P P # P P P # # # # # . .
# # # # . P # # # # # # P . # # # # # .
# . . # . P # . . . S # P . # # # # . .
# . . . . P P P P P P # P P # # # # # #
# . . . . . . . . . . # # P # # # # # .
# . . . . . . . . . . # # P # # # # . .
# . . . . . . . . . . # # P # # # # . #
# . . . . . . . . . . # P # # # # # # .
# . . . . . . . . . . # G . # # # # # .
# # # # # . # # # # . # . . # # # # . .
# # . # . . # . # # . # . . # # # # . .
# # . . . . . . # # . # . . # # # # # .
However, your algorithm still has an issue. Consider this:
if __name__ == '__main__':
run([[0, 3], [0, 1, 1, 1,], [0, 2, 1]], (1, 2), (2, 0))
run([[0, 1, 1, 1], [0, 1, 1, 1,], [0, 2, 1]], (1, 2), (2, 0))
Output:
[(1, 2), (2, 0)]
. . G
. # P
. S #
[(1, 2), (2, 0)]
. # G
. # P
. S #
Clearly, in the first case the result should have been:
[(1, 2), (0, 2), (0, 1), (0, 0), (1, 0), (2, 0)]
P P G
P # .
P S #
Which it will be if you disallow diagonals. This shows that your current algorithm does not correctly weigh diagonal moves.
Since you only ever want the diagonal to be taken if it is absolutely required, you can increase its weight to be the full size of the map - that's an overestimate, but certain to be sufficient to be avoided unless needed:
if grid[y][nx] == 0 and grid[ny][x] == 0:
neighbors.append(((nx, ny), len(grid) * len(grid[0])))
Now the output for my example becomes correctly:
[(1, 2), (0, 2), (0, 1), (0, 0), (1, 0), (2, 0)]
P P G
P # .
P S #
[(1, 2), (2, 1), (2, 0)]
. # G
. # P
. S #
Of course you should avoid computing the weight on every iteration, but you can figure out how to clean up the code from here, I'm sure. It's also possible to compute a tighter lower bound on a sufficient weight for diagonal moves, but it's not trivial and likely not needed.
open_setpriorities based onf_score, but for thestartlocation you push0as itsf_scoreand notheuristic(start, goal)as I think it should be (at least according to Wikipedia pseudocode). I am not entirely certain right now if this can play a role here, but just saying. That is, you should doheapq.heappush(open_set, (heuristic(start, goal), start))I think (instead of pushing0). – gthanop Commented Mar 9 at 22:06get_neighbors. In particular because of the conditionif not (grid[y][nx] == 0 and grid[ny][x] == 0):the top right cell of the diagonal is not considered a neighbor (it is not returned in the listneighborsthus never evaluated). Thus, any cell inside the red rectangle is unreachable from the goal (and vice versa). – gthanop Commented Mar 9 at 23:37