-
Notifications
You must be signed in to change notification settings - Fork 15
/
m_astar.py
109 lines (90 loc) · 3.3 KB
/
m_astar.py
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
101
102
103
104
105
106
107
108
109
import pqueue
def manhattan_dist(a, b):
"""
Returns the Manhattan distance between two points.
>>> manhattan_dist((0, 0), (5, 5))
10
>>> manhattan_dist((0, 5), (10, 7))
12
>>> manhattan_dist((12, 9), (2, 3))
16
>>> manhattan_dist((0, 5), (5, 0))
10
"""
return abs(a[0] - b[0]) + abs(a[1] - b[1])
def extract_fn(a):
# print 'a :', a, 'Extract :', a[:-1]
return a
# return a[1:]
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])