-
Notifications
You must be signed in to change notification settings - Fork 8
/
Copy pathAbortedDijkstra.py
173 lines (143 loc) · 6.47 KB
/
AbortedDijkstra.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
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
# -*- coding: utf-8 -*-
"""
Created on Sat Oct 25 16:14:48 2014
@author: Brian Donovan ([email protected])
"""
from Queue import PriorityQueue
# A Dijkstra search that aborts when all of the boundary_nodes have been
# expanded
# params:
# origin_node - the node that the search originates
# origin_nodeId - see DijkstrasAlgorithm.init_dict()
# boundary_nodes - nodes at the boundary of the region. The search stops
# when they are all expanded
# this_region_only - if True, ignore nodes from other regions (WARNING:
# shortest path between two boundary nodes may go outside
# of region.)
# on_forward_graph - if True, use the backward_links to expand
# returns:
# nodes_to_search - the "frontier" of nodes which have been touche
def aborted_dijkstra(origin_node, boundary_nodes, this_region_only=False,
on_forward_graph=True):
# maintain set of boundary nodes that have been visited by this search
visited_boundary_nodes = set()
visited_nodes = set()
i = origin_node.boundary_node_id
# Initialize Dijkstra queue with the origin node
nodes_to_search = PriorityQueue()
nodes_to_search.put((0, origin_node))
expanded_count = 0
max_pq_size = 0
while(not nodes_to_search.empty()):
# Get the nearest node from the priority queue
max_pq_size = max(nodes_to_search.qsize(), max_pq_size)
(_, node) = nodes_to_search.get()
expanded_count += 1
if boundary_nodes is not None:
visited_nodes.add(node)
# If this is a boundary node for this region, mark it as visited
if(boundary_nodes is not None and node.is_boundary_node and
node.region_id == origin_node.region_id):
visited_boundary_nodes.add(node)
# If we have now visited all boundary nodes, stop early
if len(visited_boundary_nodes) == len(boundary_nodes):
break
connecting_links = None
if on_forward_graph:
connecting_links = node.backward_links
else:
connecting_links = node.forward_links
# Propagate to neighbors on the forward graph using the backward links
for connecting_link in connecting_links:
neighbor = None
if on_forward_graph:
neighbor = connecting_link.origin_node
else:
neighbor = connecting_link.connecting_node
# if this_region_only is set, then skip nodes from other regions
if(this_region_only and
neighbor.region_id != origin_node.region_id):
continue
time_from_boundary_node = None
neighbor_time = None
if on_forward_graph:
time_from_boundary_node = node.forward_boundary_time
neighbor_time = neighbor.forward_boundary_time
else:
time_from_boundary_node = node.backward_boundary_time
neighbor_time = neighbor.backward_boundary_time
# Compute the distance if we were to travel to the neighbor from
# the current node
proposed_distance = (time_from_boundary_node[i] +
connecting_link.time)
# If this is better than the current best path to the neighbor,
# update it (relaxation)
if(proposed_distance < neighbor_time[i]):
neighbor_time[i] = proposed_distance
if on_forward_graph:
neighbor.forward_predecessors[i] = node
else:
neighbor.backward_predecessors[i] = node
# since the distance was updated, this node needs to be
# re-added to the PQ
nodes_to_search.put((proposed_distance, neighbor))
# Now, all origin nodes (and some other nodes) all know their distance from
# the given origin_node
return visited_nodes, expanded_count, max_pq_size
def reset_all_node_costs(road_map):
for node in road_map.nodes:
node.cost = float('inf')
# A Dijkstra search that aborts when all of the boundary_nodes have been
# expanded
# params:
# origin_node - the node that the search originates
# origin_nodeId - see DijkstrasAlgorithm.init_dict()
# boundary_nodes - nodes at the boundary of the region. The search stops
# when they are all expanded
# this_region_only - if True, ignore nodes from other regions (WARNING:
# shortest path between two boundary nodes may go outside
# of region.)
# on_forward_graph - if True, use the backward_links to expand
# returns:
# nodes_to_search - the "frontier" of nodes which have been touche
def find_nearest_neighbors(origin_node, num_neighbors, on_forward_graph=True):
# maintain set of boundary nodes that have been visited by this search
visited_nodes_cost = {}
origin_node.cost = 0
# Initialize Dijkstra queue with the origin node
nodes_to_search = PriorityQueue()
nodes_to_search.put((0, origin_node))
expanded_count = 0
max_pq_size = 0
while(not nodes_to_search.empty()):
# Get the nearest node from the priority queue
max_pq_size = max(nodes_to_search.qsize(), max_pq_size)
(cost, node) = nodes_to_search.get()
expanded_count += 1
visited_nodes_cost[node] = cost
if(len(visited_nodes_cost) >= num_neighbors):
break
connecting_links = None
if on_forward_graph:
connecting_links = node.backward_links
else:
connecting_links = node.forward_links
# Propagate to neighbors on the forward graph using the backward links
for connecting_link in connecting_links:
neighbor = None
if on_forward_graph:
neighbor = connecting_link.origin_node
else:
neighbor = connecting_link.connecting_node
proposed_cost = node.cost + connecting_link.time
if(proposed_cost < neighbor.cost):
neighbor.cost = proposed_cost
nodes_to_search.put((proposed_cost, neighbor))
#cleanup
for node in visited_nodes_cost:
node.cost = float('inf')
for (cost,node) in nodes_to_search.queue:
node.cost = float('inf')
# Now, all origin nodes (and some other nodes) all know their distance from
# the given origin_node
return visited_nodes_cost