A* (A-Star) in Python

Download source

 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
# A-star algorithm, A*

# pqueue.py, available in ruiaf.org
from pqueue import heap,container
from math import sqrt

#('city',[('city1','distancebytrain'),.....],(coordx,coordy))
# don't mind city positioning too much :)
edges = [('Lisbon',[('Madrid',4),('Paris',12)],(0,0)),
         ('Madrid',[('Athens',12),('Berlin',15)],(2,2)),
	 ('Athens',[('Moscow',2)],(8,10)),
         ('Paris',[('Berlin',5)],(5,5)),
	 ('Berlin',[('Budapest',8),('Warsaw',6)],(6,7)),
         ('Budapest',[('Moscow',8)],(5,10)),
	 ('Warsaw',[('Moscow',4)],(7,11)),
         ('Moscow',[],(8,11))]
node_list = {}
node_coord = {}
graph = {}

for (node,con,coord) in edges:
	node_list[node]=None
	node_coord[node]=coord
	graph[node]=dict()

for (node,connections,coord) in edges:
	for (con,weight) in connections:
		graph[node][con]=weight
		graph[con][node]=weight

# has to be a lower bownder for the distance
# we are using euclidean because the graph was made
# taking that into account
# if this heuristic returns 0, it's dykstra algorithm!
def heuristic(node1,node2):
	#return 0
	difx = node_coord[node1][0]-node_coord[node2][0]
	dify = node_coord[node1][0]-node_coord[node2][0]
	return sqrt(difx*difx+dify*dify)


def astar(startpoint,endpoint):
	pq = heap()
	node = container()
	node.value = 0
	node.distance = 0
	node.key = startpoint
	node.previous_node = None
	pq.append(node)

	while not pq.isempty():
		node = pq.pop()
		if node_list[node.key]!=None:
			#we have already checked this node, move along!
			#it could be possible to avoid this by cross-referencing
			#the nodes and the pqueue elements, updating the values there
			#then only a percolateup would be needed instead of
			#adding several times the same element to the pq
			continue
		node_list[node.key]=(node.previous_node,node.distance)

		if node.key==endpoint:
			break

		for child_node in graph[node.key].keys():
			child = container()
			child.key = child_node
			child.distance = graph[node.key][child_node]+node.distance
			child.value = child.distance+heuristic(child.key,endpoint)
			child.previous_node = node.key
			pq.append(child)

astar('Lisbon','Moscow')
for node in node_list.keys():
	print node, node_list[node]