-
Notifications
You must be signed in to change notification settings - Fork 0
/
p1.py
123 lines (95 loc) · 3 KB
/
p1.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
110
111
112
113
114
115
116
117
118
119
120
121
122
123
from p1_support import load_level, show_level
from math import sqrt
from heapq import heappush, heappop
import operator
VERBOSE = False
def debug(*args):
if (VERBOSE):
print ''.join([str(arg) for arg in args])
def dijkstras_shortest_path(src, dst, graph, adj):
dist = {}
prev = {}
dist[src] = 0
prev[src] = None # parent of the source node
queue = []
# Python heapq (heap, item) : item can be a tuple or single value
# If tuple is used, the first element will be used as key (key, data)
heappush(queue, (dist[src], src))
while queue:
pathCost, node = heappop(queue)
if node == dst:
break
adjacent = adj(graph, node)
# Extract (position, cost) from list of adjacent states
for neighbor, cost in adjacent:
totalCost = pathCost + cost
#print totalCost
if neighbor not in dist or totalCost < dist[neighbor]:
dist[neighbor] = totalCost
prev[neighbor] = node # parent of [ neighbor ] is node
heappush(queue, ( totalCost, neighbor))
path = []
# Path found build it, else return empty path
if node == dst:
# Traverse up the parent tree
while node: # while there is a parent (prev[src] = None)
path.append(node)
node = prev[node] # update to the parent
# Path is from dst to src, reverse it
path.reverse()
if path:
debug("Path: ", path)
debug("Path cost: ", pathCost)
return path
def navigation_edges(level, cell):
# Valid movement deltas
deltas = {
'LEFT_DOWN': (-1, -1),
'LEFT': (-1, 0),
'LEFT_UP': (-1, 1),
'DOWN': (0, -1),
'UP': (0, 1),
'RIGHT_DOWN': (1, -1),
'RIGHT': (1, 0),
'RIGHT_UP': (1, 1)
};
validMoves = []
for delta in deltas.values():
# Calculate new position
position = (cell[0]+delta[0], cell[1]+delta[1])
if position in level['spaces']:
# Calculate edge cost
cost = sqrt(delta[0] ** 2 + delta[1] ** 2)
# Valid move is a tuple (nextState, edgeCost)
validMoves.append((position, cost))
return validMoves
def test_route(filename, src_waypoint, dst_waypoint):
level = load_level(filename)
if VERBOSE:
print("Level layout:")
show_level(level)
src = level['waypoints'][src_waypoint]
dst = level['waypoints'][dst_waypoint]
path = dijkstras_shortest_path(src, dst, level, navigation_edges)
if path:
show_level(level, path)
else:
print "No path possible!"
# Show the level if the user hasn't already seen it
if not VERBOSE:
show_level(level, [])
if __name__ == '__main__':
import sys
# Use command line options
from optparse import OptionParser
parser = OptionParser(usage="usage: %prog [options] level_file src_waypoint dst_waypoint")
parser.add_option("-v", "--verbose", dest="verbose", help="use verbose logging", action="store_true", default=False)
(options, args) = parser.parse_args()
# Make sure the appropriate number of arguments was supplied
if (len(args) != 3):
print "Unexpected argument count."
parser.print_help()
else:
VERBOSE = options.verbose
filename, src_waypoint, dst_waypoint = args
test_route(filename, src_waypoint, dst_waypoint)