-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathdijkstra.py
133 lines (104 loc) · 5.04 KB
/
dijkstra.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
124
125
126
127
128
129
130
131
132
133
from FibonacciHeap import FibHeap
from priority_queue import FibPQ, HeapPQ, QueuePQ
def solve(maze):
# Width is used for indexing, total is used for array sizes
width = maze.width
total = maze.width * maze.height
# Start node, end node
start = maze.start
startpos = start.Position
end = maze.end
endpos = end.Position
# Visited holds true/false on whether a node has been seen already. Used to stop us returning to nodes multiple times
visited = [False] * total
# Previous holds a link to the previous node in the path. Used at the end for reconstructing the route
prev = [None] * total
# Distances holds the distance to any node taking the best known path so far. Better paths replace worse ones as we find them.
# We start with all distances at infinity
infinity = float("inf")
distances = [infinity] * total
# The priority queue. There are multiple implementations in priority_queue.py
# unvisited = FibHeap()
unvisited = HeapPQ()
# unvisited = FibPQ()
# unvisited = QueuePQ()
# This index holds all priority queue nodes as they are created. We use this to decrease the key of a specific node when a shorter path is found.
# This isn't hugely memory efficient, but likely to be faster than a dictionary or similar.
nodeindex = [None] * total
# To begin, we set the distance to the start to zero (we're there) and add it into the unvisited queue
distances[start.Position[0] * width + start.Position[1]] = 0
startnode = FibHeap.Node(0, start)
nodeindex[start.Position[0] * width + start.Position[1]] = startnode
unvisited.insert(startnode)
# Zero nodes visited, and not completed yet.
count = 0
completed = False
# Begin Dijkstra - continue while there are unvisited nodes in the queue
while len(unvisited) > 0:
count += 1
# Find current shortest path point to explore
n = unvisited.removeminimum()
# Current node u, all neighbours will be v
u = n.value
upos = u.Position
uposindex = upos[0] * width + upos[1]
if distances[uposindex] == infinity:
break
# If upos == endpos, we're done!
if upos == endpos:
completed = True
break
for v in u.Neighbours:
if v != None:
vpos = v.Position
vposindex = vpos[0] * width + vpos[1]
if visited[vposindex] == False:
# The extra distance from where we are (upos) to the neighbour (vpos) - this is manhattan distance
# https://en.wikipedia.org/wiki/Taxicab_geometry
d = abs(vpos[0] - upos[0]) + abs(vpos[1] - upos[1])
# New path cost to v is distance to u + extra
newdistance = distances[uposindex] + d
# If this new distance is the new shortest path to v
if newdistance < distances[vposindex]:
vnode = nodeindex[vposindex]
# v isn't already in the priority queue - add it
if vnode == None:
vnode = FibHeap.Node(newdistance, v)
unvisited.insert(vnode)
nodeindex[vposindex] = vnode
distances[vposindex] = newdistance
prev[vposindex] = u
# v is already in the queue - decrease its key to re-prioritise it
else:
unvisited.decreasekey(vnode, newdistance)
distances[vposindex] = newdistance
prev[vposindex] = u
visited[uposindex] = True
# We want to reconstruct the path. We start at end, and then go prev[end] and follow all the prev[] links until we're back at the start
from collections import deque
path = deque()
current = end
while (current != None):
path.appendleft(current)
current = prev[current.Position[0] * width + current.Position[1]]
return [path, [count, len(path), completed]]
from FibonacciHeap import FibHeap
from priority_queue import FibPQ, HeapPQ, QueuePQ
def solve(maze):
# Width is used for indexing, total is used for array sizes
width = maze.width
total = maze.width * maze.height
# Start node, end node
start = maze.start
startpos = start.Position
end = maze.end
endpos = end.Position
# Visited holds true/false on whether a node has been seen already. Used to stop us returning to nodes multiple times
visited = [False] * total
# Previous holds a link to the previous node in the path. Used at the end for reconstructing the route
prev = [None] * total
# Distances holds the distance to any node taking the best known path so far. Better paths replace worse ones as we find them.
# We start with all distances at infinity
infinity = float("inf")
distances = [infinity] * total
# The priority queue. There are multiple implementations in priority_queue.py