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)
