def least_cost_path(G, start, dest, cost):
"""
path = least_cost_path(G, start, dest, cost)
least_cost_path returns a least cost path in the digraph G from vertex
start to vertex dest, where costs are defined by the cost function.
cost should be a function that takes a single edge argument and returns
a real-valued cost.
if there is no path, then returns None
the path from start to start is [start]
>>> g = digraph.Digraph( [(1, 2), (4, 2), (4, 6), (6, 7), (1, 7), (2, 4)] )
>>> least_cost_path(g, 1, 7, lambda e: abs(2*e[0]-e[1]))
[1, 7]
>>> least_cost_path(g, 7, 2, lambda e: 1) is None
True
"""
# Create a priority queue
todo = pqueue.PQueue()
todo.update(start, 0)
# v in visited when the vertex v's least cost from start has been determined
visited = set()
# parent[v] is the vertex that just precedes v in the path from start to v
parent = {}
while todo and (dest not in visited):
# priority queue operation
# remove smallest estimated cost vertex from todo list
(cur, c) = todo.pop_smallest()
# it is now visited, and will never have a smaller cost
visited.add(cur)
for n in G.adj_to(cur):
if n in visited: continue
if todo.update(n, c + cost((cur, n))):
parent[n] = cur
# now, if there is a path, extract it. The graph may be disconnected
# so in that case return None
if dest not in visited:
return None
path = [dest]
cur = dest
while start not in path:
cur = parent[cur]
path.append(cur)
path.reverse()
return path
def find_path(neighbour_fn, start, end, passable):
"""
Returns a list of points between 2 points using A*.
If no path can be found, an empty list is returned.
The cost function shows how much it cost to take a step. F(x) = g(x) + h(x)
should always be greater than 1 or else the shortest path is not guaranteed.
The passable function returns whether or not the agent can pass through the node.
The heuristic function uses the manhattan distance for a quick and guarantee
esitamte of the optimal path cost.
"""
cost = 1
costs = 0
# tiles to check (tuples of (x, y), cost)
frontier = pqueue.PQueue()
frontier.update(start, 0)
# tiles we've been to
visited = set()
# associated G and H costs for each tile (tuples of G, H)
if (not costs):
costs = {start: (0, manhattan_dist(start, end))}
# parents for each tile
parents = {}
while (frontier and (end not in visited)):
cur, c = frontier.pop_smallest()
visited.add(cur)
# check neighbours
for n in neighbour_fn(cur):
# skip it if we've already checked it, or if it isn't passable
if ((n in visited) or (not passable(n, None))):
continue
if not (n in frontier):
# we haven't looked at this tile yet, so calculate its costs
g = costs[cur][0] + cost
h = manhattan_dist(n, end)
costs[n] = (g, h)
parents[n] = cur
frontier.update(n, g + h)
else:
# if we've found a better path, update it
g, h = costs[n]
new_g = costs[cur][0] + cost
if new_g < g:
g = new_g
frontier.update(n, g + h)
costs[n] = (g, h)
parents[n] = cur
# we didn't find a path
if end not in visited:
return []
# build the path backward
path = []
while end != start:
path.append(end)
end = parents[end]
path.append(start)
path.reverse()
return path, (len(path) - 1)
def find_path(neighbour_fn,
start,
end,
cost=lambda pos: 1,
passable=lambda pos: True,
heuristic=manhattan_dist,
stopCondOr=lambda x=0: False,
stopCondAnd=lambda x=0: True,
costs=0):
"""
Returns the path between two nodes as a list of nodes using the A*
algorithm.
If no path could be found, an empty list is returned.
The cost function is how much it costs to leave the given node. This should
always be greater than or equal to 1, or shortest path is not guaranteed.
The passable function returns whether the given node is passable.
The heuristic function takes two nodes and computes the distance between the
two. Underestimates are guaranteed to provide an optimal path, but it may
take longer to compute the path. Overestimates lead to faster path
computations, but may not give an optimal path.
"""
# tiles to check (tuples of (x, y), cost)
todo = pqueue.PQueue()
todo.update(start, 0)
# tiles we've been to
visited = set()
# associated G and H costs for each tile (tuples of G, H)
if (not costs):
costs = {start: (0, heuristic(start, end))}
# parents for each tile
parents = {}
while (((todo and (end not in visited)) and stopCondAnd())
or stopCondOr()):
cur, c = todo.pop_smallest()
visited.add(cur)
# check neighbours
for n in neighbour_fn(cur):
# skip it if we've already checked it, or if it isn't passable
if ((n in visited) or (not passable(n))):
continue
if not (n in todo):
# we haven't looked at this tile yet, so calculate its costs
g = costs[cur][0] + cost(cur)
h = heuristic(n, end)
costs[n] = (g, h)
parents[n] = cur
todo.update(n, g + h)
else:
# if we've found a better path, update it
g, h = costs[n]
new_g = costs[cur][0] + cost(cur)
if new_g < g:
g = new_g
todo.update(n, g + h)
costs[n] = (g, h)
parents[n] = cur
# we didn't find a path
if end not in visited:
return []
# build the path backward
path = []
while end != start:
path.append(end)
end = parents[end]
path.append(start)
path.reverse()
return path, (len(path) - 1)
def reachable_tiles(graph,
start,
max_cost,
cost=lambda pos: 1,
passable=lambda pos: True):
"""
Returns a set of nodes which can be reached with a total cost of max_cost.
The cost function is how much it costs to leave the given node. This should
always be greater than or equal to 1, or shortest path is not guaranteed.
The passable function returns whether the given node.
Example use:
>>> t = TileMap("assets/tiles.png", 20, 20)
>>> t.load_from_file("maps/test-2.gif")
>>> reachable_tiles(t, (2, 2), 2) == set([(2, 0), (1, 1), (2, 1), (3, 1),
... (0, 2), (1, 2), (2, 2), (3, 2), (4, 2), (1, 3), (2, 3), (3, 3),
... (2, 4)])
True
>>> t = TileMap("assets/tiles.png", 20, 20)
>>> t.load_from_file("maps/test-3.gif")
>>> cost = lambda c: 1
>>> passable = lambda c: t.tile_data(c).passable
>>> reachable_tiles(t, (2, 0), 6, cost, passable) == set([(3, 0), (2, 0),
... (1, 0), (0, 0), (0, 1), (0, 2), (0, 3), (0, 4), (1, 2), (2, 2)])
True
"""
# tiles to check (tuples of x, y)
todo = pqueue.PQueue()
todo.update(start, 0)
# tiles we've been to
visited = set()
# tiles which we can get to within max_cost
reachable = set()
reachable.add(start)
while todo:
cur, c = todo.pop_smallest()
visited.add(cur)
# it's too expensive to get here, so don't bother checking
if c > max_cost:
continue
# check neighbours
for n in graph.neighbours(cur):
# skip it if it doesn't exist, if we've already checked it, or
# if it isn't passable
if ((n in visited) or (not passable(n))):
continue
# try updating the tile's cost
new_cost = c + cost(cur)
if todo.update(n, new_cost) and new_cost <= max_cost:
reachable.add(n)
return reachable
def find_path(graph,
start,
end,
cost=lambda pos: 1,
passable=lambda pos: True,
heuristic=helper.manhattan_dist):
"""
Returns the path between two nodes as a list of nodes using the A*
algorithm.
If no path could be found, an empty list is returned.
The cost function is how much it costs to leave the given node. This should
always be greater than or equal to 1, or shortest path is not guaranteed.
The passable function returns whether the given node is passable.
The heuristic function takes two nodes and computes the distance between the
two. Underestimates are guaranteed to provide an optimal path, but it may
take longer to compute the path. Overestimates lead to faster path
computations, but may not give an optimal path.
Code based on algorithm described in:
http://www.policyalmanac.org/games/aStarTutorial.htm
Example use:
>>> t = TileMap("assets/tiles.png", 20, 20)
>>> t.load_from_file("maps/test-2.gif")
>>> find_path(t, (0, 0), (4, 4))
[(0, 0), (1, 0), (1, 1), (2, 1), (2, 2), (3, 2), (3, 3), (4, 3), (4, 4)]
>>> find_path(t, (0, 0), (5, 5))
[]
>>> t = TileMap("assets/tiles.png", 20, 20)
>>> t.load_from_file("maps/test-3.gif")
>>> cost = lambda c: 1
>>> passable = lambda c: t.tile_data(c).passable
>>> find_path(t, (2, 0), (4, 1), cost, passable) == [(2, 0), (1, 0), (0, 0),
... (0, 1), (0, 2), (1, 2), (2, 2), (3, 2), (3, 3), (3, 4), (4, 4), (5, 4),
... (5, 3), (5, 2), (5, 1), (4, 1)]
True
"""
# tiles to check (tuples of (x, y), cost)
todo = pqueue.PQueue()
todo.update(start, 0)
# tiles we've been to
visited = set()
# associated G and H costs for each tile (tuples of G, H)
costs = {start: (0, heuristic(start, end))}
# parents for each tile
parents = {}
while todo and (end not in visited):
todo.tie_breaker = lambda a, b: better_tile(a, b, start, end)
cur, c = todo.pop_smallest()
visited.add(cur)
# check neighbours
for n in graph.neighbours(cur):
# skip it if we've already checked it, or if it isn't passable
if ((n in visited) or (not passable(n))):
continue
if not (n in todo):
# we haven't looked at this tile yet, so calculate its costs
g = costs[cur][0] + cost(cur)
h = heuristic(n, end)
costs[n] = (g, h)
parents[n] = cur
todo.update(n, g + h)
else:
# if we've found a better path, update it
g, h = costs[n]
new_g = costs[cur][0] + cost(cur)
if new_g < g:
g = new_g
todo.update(n, g + h)
costs[n] = (g, h)
parents[n] = cur
# we didn't find a path
if end not in visited:
return []
# build the path backward
path = []
while end != start:
path.append(end)
end = parents[end]
path.append(start)
path.reverse()
return path
def get_costmat(neighbour_fn,
start,
end,
cost=lambda pos: 1,
passable=lambda pos: True,
costs=0,
heuristic=least_dist_to_b,
stopCondOr=lambda x=0: False,
stopCondAnd=lambda x=0: True):
# tiles to check (tuples of (x, y), cost)
todo = pqueue.PQueue()
for start_pos in start:
todo.update(start_pos, 0)
# for end_pos in end:
# todo.update(end_pos, 1000)
# tiles we've been to
visited = set()
# associated G and H costs for each tile (tuples of G, H)
if (not costs):
costs = dict()
for start_pos in start:
costs[start_pos] = (0, least_dist_to_b(start_pos, end))
# parents for each tile
parents = {}
# while ( ( (todo and ( all_b_in_a(visited, end) ) ) and stopCondAnd()) or stopCondOr()):
while ((todo and stopCondAnd()) or stopCondOr()):
cur, c = todo.pop_smallest()
visited.add(cur)
# check neighbours
nbors = neighbour_fn(cur)
for n in nbors:
# skip it if we've already checked it, or if it isn't passable
if ((n in visited) or (not passable(n))):
continue
if not (n in todo):
# we haven't looked at this tile yet, so calculate its costs
g = costs[cur][0] + cost(cur)
h = heuristic(n, end)
costs[n] = (g, h)
parents[n] = cur
todo.update(n, g + h)
else:
# if we've found a better path, update it
g, h = costs[n]
new_g = costs[cur][0] + cost(cur)
if new_g < g:
g = new_g
todo.update(n, g + h)
costs[n] = (g, h)
parents[n] = cur
return costs
def find_path(neighbour_fn,
start,
end,
cost=lambda pos: 1,
passable=lambda pos, constraints=None: True,
heuristic=manhattan_dist,
constraints=None,
extract=extract_fn):
"""
Returns the path between two nodes as a list of nodes using the A*
algorithm.
If no path could be found, an empty list is returned.
The cost function is how much it costs to leave the given node. This should
always be greater than or equal to 1, or shortest path is not guaranteed.
The passable function returns whether the given node is passable.
The heuristic function takes two nodes and computes the distance between the
two. Underestimates are guaranteed to provide an optimal path, but it may
take longer to compute the path. Overestimates lead to faster path
computations, but may not give an optimal path.
"""
# tiles to check (tuples of (x, y), cost)
todo = pqueue.PQueue()
todo.update(start, 0)
# tiles we've been to
visited = set()
# associated G and H costs for each tile (tuples of G, H)
costs = {start: (0, heuristic(start, end))}
# parents for each tile
parents = {}
if (heuristic(start, end) == 0):
return [start]
while todo and (extract(end) not in visited):
cur, c = todo.pop_smallest()
# print 'Current: ', cur, 'cost: ', sum(costs[cur])
# something = input('Press some key to continue...')
visited.add(extract(cur))
# check neighbours
for n in neighbour_fn(cur):
# skip it if we've already checked it, or if it isn't passable
if ((extract(n) in visited) or (not passable(n, constraints))):
# print 'Nbor: ', n, (not passable(n, constraints)), (extract(n) in visited)
continue
if not (n in todo):
# we haven't looked at this tile yet, so calculate its costs
g = costs[cur][0] + cost(cur)
h = heuristic(n, end)
costs[n] = (g, h)
parents[n] = cur
todo.update(n, g + h)
else:
# if we've found a better path, update it
g, h = costs[n]
new_g = costs[cur][0] + cost(cur)
if new_g < g:
g = new_g
todo.update(n, g + h)
costs[n] = (g, h)
parents[n] = cur
# print '\nVisited: ', visited
# print '\nParents: ', parents
# we didn't find a path
if extract(end) not in visited:
return [], 32767
# build the path backward
path = []
while extract(end) != extract(start):
path.append(end)
end = parents[end]
path.append(start)
path.reverse()
return path, sum(costs[start])
