def myFindPath(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 not line_intersects_grid(start, end, grid, tilesize): return ([end], point_dist(start, end), True) direct = False mesh = copy.deepcopy(mesh) mesh[start] = dict([(n, point_dist(start,n)) for n in mesh if not line_intersects_grid(start,n,grid,tilesize)]) 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) if len(nodes) == 1: direct = True return (nodes, length, direct)
def bangBangStar(start, startAngle, end, mesh, lineMesh, grid, tilesize, max_turn, max_speed): if not line_intersects_grid(start, end, grid, tilesize): return ([end], point_dist(start, end), True) direct = False lineMesh = copy.deepcopy(lineMesh) lineMesh[(start,start)] = {} for n in mesh: if not line_intersects_grid(start, n, grid, tilesize): if start != n: lineMesh[(start,start)][(start,n)] = max_speed * \ (math.fabs(angle_fix(getAngle(start,n) - startAngle))//max_turn) + \ point_dist(start,n) if start not in mesh: lineMesh[(start,n)] = {} for n2 in mesh[n]: lineMesh[(start,n)][(n,n2)] = max_speed * \ (math.fabs(angle_fix(getAngle(n,n2)-getAngle(start,n)))//max_turn) + \ point_dist(n, n2) if end != start: if end not in mesh: for n2 in mesh.keys() + [start]: if not line_intersects_grid(end, n2, grid, tilesize): lineMesh[(n2,end)] = dict([((end,end),0)]) for n in mesh.keys() + [start]: if (n, n2) in lineMesh: lineMesh[(n,n2)][(n2,end)] = max_speed * \ (math.fabs(angle_fix(getAngle(n2,end)-getAngle(n,n2)))//max_turn) + \ point_dist(n2, end) else: for n in mesh.keys() + [start]: if (n, end) in lineMesh: lineMesh[(n,end)][(end,end)] = 0 neighbours = lambda n: lineMesh[n].keys() cost = lambda n1, n2: lineMesh[n1][n2] goal = lambda n: n == (end,end) heuristic = lambda n: point_dist(n[1], end) nodes, length = astar((start,start), neighbours, goal, 0, cost, heuristic) if len(nodes) == 1: direct = True path = [] for (n,n2) in nodes: path += [n2] return (path, length, direct)