def find_path(start, end, mesh, grid, tilesize=16): """ Uses astar to find a path from start to end, using the given mesh and tile grid. >>> grid = [[0,0,0,0,0],[0,0,0,0,0],[0,0,1,0,0],[0,0,0,0,0],[0,0,0,0,0]] >>> mesh = make_nav_mesh([(2,2,1,1)],(0,0,4,4),1) >>> find_path((0,0),(4,4),mesh,grid,1) [(4, 1), (4, 4)] """ # If there is a straight line, just return the end point if not line_intersects_grid(start, end, grid, tilesize): return [end] # Copy mesh so we can add temp nodes mesh = copy.deepcopy(mesh) # Add temp notes for start mesh[start] = dict([(n, point_dist(start,n)) for n in mesh if not line_intersects_grid(start,n,grid,tilesize)]) # Add temp nodes for end: if end not in mesh: endconns = [(n, point_dist(end,n)) for n in mesh if not line_intersects_grid(end,n,grid,tilesize)] for n, dst in endconns: mesh[n][end] = dst neighbours = lambda n: mesh[n].keys() cost = lambda n1, n2: mesh[n1][n2] goal = lambda n: n == end heuristic = lambda n: ((n[0]-end[0]) ** 2 + (n[1]-end[1]) ** 2) ** 0.5 nodes, length = astar(start, neighbours, goal, 0, cost, heuristic) return nodes
def find_path(start, end, mesh, grid, tilesize=16): """ Uses astar to find a path from start to end, using the given mesh and tile grid. >>> grid = [[0,0,0,0,0],[0,0,0,0,0],[0,0,1,0,0],[0,0,0,0,0],[0,0,0,0,0]] >>> mesh = make_nav_mesh([(2,2,1,1)],(0,0,4,4),1) >>> find_path((0,0),(4,4),mesh,grid,1) [(4, 1), (4, 4)] """ # If there is a straight line, just return the end point if not line_intersects_grid(start, end, grid, tilesize): return [end] # Copy mesh so we can add temp nodes mesh = copy.deepcopy(mesh) # Add temp notes for start mesh[start] = dict([(n, point_dist(start, n)) for n in mesh if not line_intersects_grid(start, n, grid, tilesize)]) # Add temp nodes for end: if end not in mesh: endconns = [(n, point_dist(end, n)) for n in mesh if not line_intersects_grid(end, n, grid, tilesize)] for n, dst in endconns: mesh[n][end] = dst neighbours = lambda n: mesh[n].keys() cost = lambda n1, n2: mesh[n1][n2] goal = lambda n: n == end heuristic = lambda n: ((n[0] - end[0])**2 + (n[1] - end[1])**2)**0.5 nodes, length = astar(start, neighbours, goal, 0, cost, heuristic) return nodes
def astar_path_length(m, start, end): """ Length of a path from start to end """ neighbours = lambda n: m[n].keys() cost = lambda n1, n2: m[n1][n2] goal = lambda n: n == end heuristic = lambda n: point_dist(end, n) nodes, length = astar(start, neighbours, goal, 0, cost, heuristic) return length