-
Notifications
You must be signed in to change notification settings - Fork 3
/
shortpath.py
80 lines (68 loc) · 2.23 KB
/
shortpath.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
import math
from geometry import x,y,euclidian_distance
def astar(graph, start, goal, cost = euclidian_distance, heuristic = euclidian_distance):
opened = set()
closed = set()
m_heur = {}
m_parent = {}
m_cost = {} # absolute path costs
def path_from(node):
def parents(node):
while node:
yield node
node = m_parent.get(node, None)
path = [p for p in parents(node)]
cost = m_cost[goal]
path.reverse()
return path,cost
opened.add(start)
m_cost[start] = 0
while opened:
# sort opened nodes based on the heuristic and consider the first one
current = sorted(opened, key=lambda n : m_heur.get( n, heuristic(n,goal) ) )[0]
if current == goal:
# FIXME add backtrack validation
return path_from(current)
closed.add(current)
opened.remove(current)
for neighbor in graph[current]:
if neighbor in closed:
continue
elif neighbor in opened:
next_cost = m_cost[current] + cost(current,neighbor)
if next_cost < m_cost[neighbor]:
m_cost[neighbor] = next_cost
m_parent[neighbor] = current
else:
m_cost[neighbor] = m_cost[current] + cost(current,neighbor)
m_heur[neighbor] = heuristic( neighbor, goal )
m_parent[neighbor] = current
opened.add(neighbor)
return []
if __name__ == "__main__":
print """Graph:
-1 0 2 : x
1 o o-----o
| | |
0 o--o-----o
| |
| |
-2 o--o-----o
:
y
"""
G = {
( 0, 0) : [(-1, 0),( 0, 1),( 2, 0),( 0,-2)],
( 0, 1) : [( 0, 0),( 2, 1)],
( 0,-2) : [( 0, 0),( 2,-2),(-1,-2)],
(-1, 0) : [(-1, 1),( 0, 0)],
(-1, 1) : [(-1, 0)],
(-1,-2) : [( 0,-2)],
( 2, 0) : [( 2, 1),( 2,-2),( 0, 0)],
( 2, 1) : [( 0, 1),( 2, 0)],
( 2,-2) : [( 2, 0),( 0,-2)],
}
print "Path from (-1,1) to (-1,-2):"
path,cost = astar( G, (-1,1), (-1,-2) )
print "Cost:",cost
print"Path:",path