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). Continue reading...
