I am implementing an A* pathfinding algorithm in a simulation model. The costs in the map that agents navigate over changes over time. In other words, there is a different 2D grid at each time step. In addition, I need agents to be able to decide to "wait" at a given location on the grid, if waiting allows them to take an overall lower cost path.
My code seems to do this, but it is very slow, and seems to become substantially slower with longer paths and larger maps. I would eventually like to have agents navigate anywhere (including diagonally) on a 50x50 grid, so I need a way to speed it up.
Any suggestions on how to speed up the search?
For reference, here is my current code, with this map and start end, I would expect the following path to be returned:
[(0, 0), (1, 0), (2, 0), (2, 0), (2, 1), (1, 1), (0, 1), (0, 2), (0, 3), (0, 4), (0, 5), (0, 6), (1, 6), (2, 6), (3, 6), (4, 6), (5, 6), (6, 6)]
# %%
import heapq
class Node:
def __init__(self, x, y, cost, parent=None):
self.x = x
self.y = y
self.cost = cost
self.parent = parent
def __lt__(self, other):
return self.cost < other.cost
def heuristic(node, goal):
# Calculate the Manhattan distance as the heuristic
return abs(node.x - goal.x) + abs(node.y - goal.y)
def astar_with_variable_costs(fullgrid, start, goal):
open_set = []
closed_set = set()
start_node = Node(start[0], start[1], 0)
goal_node = Node(goal[0], goal[1], 0)
heapq.heappush(open_set, start_node)
while open_set:
current = heapq.heappop(open_set)
if current.x == goal_node.x and current.y == goal_node.y:
path = []
while current:
path.append((current.x, current.y))
current = current.parent
path.reverse()
return path
# Construct the path from the goal node to the start node
path = []
current2 = current
while current2:
path.append((current2.x, current2.y))
current2 = current2.parent
path.reverse()
grid = fullgrid[min(len(path), len(fullgrid)-1)]
closed_set.add((current.x, current.y))
for dx, dy in [(1, 0), (-1, 0), (0, 1), (0, -1), (0, 0)]:
x, y = current.x + dx, current.y + dy
if 0 <= x < len(grid) and 0 <= y < len(grid[0]) and grid[x][y] != 0 and ((x, y) not in closed_set or (x,y)==(current.x,current.y)):
new_cost = current.cost + grid[x][y]
new_node = Node(x, y, new_cost, current)
if new_node not in open_set:
heapq.heappush(open_set, new_node)
return None
# Example usage:
matrix_3d = [
[
# Another 10x10 matrix with different values
[101, 1102, 1013, 104, 105, 106, 107, 108, 109, 110],
[100, 1000, 1113, 114, 115, 116, 117, 118, 119, 120],
[1, 1212, 1123, 124, 125, 126, 127, 128, 129, 130],
[131, 132, 133, 134, 135, 136, 137, 138, 139, 140],
[141, 142, 143, 144, 145, 146, 147, 148, 149, 150],
[151, 152, 153, 154, 155, 156, 157, 158, 159, 160],
[161, 162, 163, 164, 165, 166, 167, 168, 169, 170],
[171, 172, 173, 174, 175, 176, 177, 178, 179, 180],
[181, 182, 183, 184, 185, 186, 187, 188, 189, 190],
[191, 192, 193, 194, 195, 196, 197, 198, 199, 200]
],
[
[101, 1000, 1013, 104, 105, 106, 107, 108, 109, 110],
[100, 1000, 1113, 114, 115, 116, 117, 118, 119, 120],
[1, 1122, 1123, 124, 125, 126, 127, 128, 129, 130],
[131, 132, 133, 134, 135, 136, 137, 138, 139, 140],
[141, 142, 143, 144, 145, 146, 147, 148, 149, 150],
[151, 152, 153, 154, 155, 156, 157, 158, 159, 160],
[161, 162, 163, 164, 165, 166, 167, 168, 169, 170],
[171, 172, 173, 174, 175, 176, 177, 178, 179, 180],
[181, 182, 183, 184, 185, 186, 187, 188, 189, 190],
[191, 192, 193, 194, 195, 196, 197, 198, 199, 200]
],
[
[101, 102, 1103, 104, 105, 106, 107, 108, 109, 110],
[100, 1000, 1000, 114, 115, 116, 117, 118, 119, 120],
[1, 1212, 1000, 124, 125, 126, 127, 128, 129, 130],
[131, 132, 133, 134, 135, 136, 137, 138, 139, 140],
[141, 142, 143, 144, 145, 146, 147, 148, 149, 150],
[151, 152, 153, 154, 155, 156, 157, 158, 159, 160],
[161, 162, 163, 164, 165, 166, 167, 168, 169, 170],
[171, 172, 173, 174, 175, 176, 177, 178, 179, 180],
[181, 182, 183, 184, 185, 186, 187, 188, 189, 190],
[191, 192, 193, 194, 195, 196, 197, 198, 199, 200]
],
[
[101, 102, 103, 104, 105, 106, 107, 108, 109, 110],
[100, 1000, 113, 114, 115, 116, 117, 118, 119, 120],
[1, 122, 123, 124, 125, 126, 127, 128, 129, 130],
[131, 132, 133, 134, 135, 136, 137, 138, 139, 140],
[141, 142, 143, 144, 145, 146, 147, 148, 149, 150],
[151, 152, 153, 154, 155, 156, 157, 158, 159, 160],
[161, 162, 163, 164, 165, 166, 167, 168, 169, 170],
[171, 172, 173, 174, 175, 176, 177, 178, 179, 180],
[181, 182, 183, 184, 185, 186, 187, 188, 189, 190],
[191, 192, 193, 194, 195, 196, 197, 198, 199, 200]
],
[
[1, 2, 3, 4, 5, 6, 7, 8, 9, 10],
[11, 12, 13, 14, 15, 16, 17, 18, 19, 20],
[21, 22, 23, 24, 25, 26, 27, 28, 29, 30],
[31, 32, 33, 34, 35, 36, 37, 38, 39, 40],
[41, 42, 43, 44, 45, 46, 47, 48, 49, 50],
[51, 52, 53, 54, 55, 56, 57, 58, 59, 60],
[61, 62, 63, 64, 65, 66, 67, 68, 69, 70],
[71, 72, 73, 74, 75, 76, 77, 78, 79, 80],
[81, 82, 83, 84, 85, 86, 87, 88, 89, 90],
[91, 92, 93, 94, 95, 96, 97, 98, 99, 100]
],
[
[1, 2, 3, 4, 5, 6, 7, 8, 9, 10],
[11, 12, 13, 14, 15, 16, 17, 18, 19, 20],
[21, 22, 23, 24, 25, 26, 27, 28, 29, 30],
[31, 32, 33, 34, 35, 36, 37, 38, 39, 40],
[41, 42, 43, 44, 45, 46, 47, 48, 49, 50],
[51, 52, 53, 54, 55, 56, 57, 58, 59, 60],
[61, 62, 63, 64, 65, 66, 67, 68, 69, 70],
[71, 72, 73, 74, 75, 76, 77, 78, 79, 80],
[81, 82, 83, 84, 85, 86, 87, 88, 89, 90],
[91, 92, 93, 94, 95, 96, 97, 98, 99, 100]
]
]
start = (0, 0)
goal = (6, 6)
path = astar_with_variable_costs(matrix_3d, start, goal)
print(path)