1+ import heapq
2+
3+ class OneDirectionalAStar (object ):
4+ """AStar object
5+ Finds the optimal path between two nodes on
6+ a graph while taking into account weights.
7+ """
8+
9+ # Some miscellaneous notes:
10+
11+ # River example neighbors
12+ # Imagine you had a graph that was constructed by the time it
13+ # would take to get to different strategic locations on a map.
14+ # Suppose there is a river that cuts the map in half vertically,
15+ # and two bridges that allow crossing at the top and bottom of
16+ # the map, but swimming is an option but very slow.
17+ # For simplicity, on each side there is 1 base that acts as a
18+ # strategic location, both sides of the each bridge, and both
19+ # sides of the river directly in the vertical center, for a total
20+ # graph of 8 nodes (see imgs/onedirectionalastar_riverexample.png)
21+ #
22+ # Now suppose the heuristic naively used euclidean distance while
23+ # the actual weights were based on precalculated paths.
24+ #
25+ # Looking at the picture, if you were going from one base to the other
26+ # middle side of the river, you would first expand the base and find
27+ # 3 nodes: top (15 + 10), your side of center (5 + 3), bottom (15 + 10).
28+ #
29+ # You would expand your side of center and find the top and bottom at
30+ # (5 + 12) - WORSE than just going to them. This is the case where we
31+ # would NOT add the path base->center->top to the open list because
32+ # (for these weights) it will never be better than base->top.
33+ #
34+ # You would also add the new node (55 + 0) or the destination.
35+ #
36+ # Then you expand the top node (or bottom) on the other side of
37+ # river with a cost of (18 + 12).
38+ #
39+ # You expand the top node on the other side of the river next and find
40+ # one of the neighbors is already on the open list (the destination)
41+ # at a score of (55 + 0), but your cost to get there is (30 + 0). This
42+ # is where you would REPLACE the old path with yourself.
43+
44+ def __init__ (self ):
45+ pass
46+
47+ def reverse_path (self , node ):
48+ """
49+ Walks backward from an end node to the start
50+ node and reconstructs a path. Meant for internal
51+ use.
52+ :param node: dict containing { 'vertex': any hashable, 'parent': dict or None }
53+ :return: a list of vertices ending on the node
54+ """
55+ result = []
56+ while node is not None :
57+ result .insert (0 , node ['vertex' ])
58+ node = node ['parent' ]
59+ return result
60+
61+ def find_path (self , graph , start , end , heuristic_fn ):
62+ """
63+ Calculates the optimal path from start to end
64+ on the graph. Weights are taken into account.
65+ This implementation is one-directional expanding
66+ from the start to the end. This implementation is
67+ faster than dijkstra based on how much better the
68+ heuristic is than flooding.
69+
70+ The heuristic must never overestimate the distance
71+ between two nodes (in other words, the heuristic
72+ must be "admissible"). Note however that, in practice,
73+ it is often acceptable to relax this requirement and
74+ get very slightly incorrect paths if:
75+ - The distance between nodes are small
76+ - There are too many nodes for an exhaustive search
77+ to ever be feasible.
78+ - The world is mostly open (ie there are many paths
79+ from the start to the end that are acceptable)
80+ - Execution speed is more important than accuracy.
81+ The best way to do this is to make the heuristic slightly
82+ pessimistic (typically by multiplying by small value such
83+ as 1.1). This will have the algorithm favor finishing its
84+ path rather than finding a better one. This optimization
85+ needs to be tested based on the map.
86+
87+ :param graph: object contains `graphs` as per pygorithm.data_structures.WeightedUndirectedGraph
88+ and `get_edge_weight` in the same manner.
89+ :param start: the start vertex (which is the same type of the verticies in the graph)
90+ :param end: the end vertex (which is the same type of the vertices in the graph)
91+ :param heuristic_fn: function(graph, start, end) that when given two vertices returns an expected cost to get
92+ to get between the two vertices.
93+ :return: a list starting with `start` and ending with `end`, or None if no path is possible.
94+ """
95+
96+ # It starts off very similiar to Dijkstra. However, we will need to lookup
97+ # nodes in the open list before. There can be thousands of nodes in the open
98+ # list and any unordered search is too expensive, so we trade some memory usage for
99+ # more consistent performance by maintaining a dictionary (O(1) lookup) between
100+ # vertices and their nodes.
101+ open_lookup = {}
102+ open = []
103+ closed = set ()
104+
105+ # We require a bit more information on each node than Dijkstra
106+ # and we do slightly more calculation, so the heuristic must
107+ # prune enough nodes to offset those costs. In practice this
108+ # is almost always the case if their are any large open areas
109+ # (nodes with many connected nodes).
110+
111+ # Rather than simply expanding nodes that are on the open list
112+ # based on how close they are to the start, we will expand based
113+ # on how much distance we predict is between the start and end
114+ # node IF we go through that parent. That is a combination of
115+ # the distance from the start to the node (which is certain) and
116+ # the distance from the node to the end (which is guessed).
117+
118+ # We use the counter to enforce consistent ordering between nodes
119+ # with the same total predicted distance.
120+
121+ counter = 0
122+ heur = heuristic_fn (graph , start , end )
123+ open_lookup [start ] = { 'vertex' : start , 'dist_start_to_here' : 0 , 'pred_dist_here_to_end' : heur , 'pred_total_dist' : heur , 'parent' : None }
124+ heapq .heappush (open , (heur , counter , start ))
125+ counter += 1
126+
127+ while len (open ) > 0 :
128+ current = heapq .heappop (open )
129+ current_vertex = current [2 ]
130+ current_dict = open_lookup [current_vertex ]
131+ del open_lookup [current_vertex ]
132+ closed .update (current_vertex )
133+
134+ if current_vertex == end :
135+ return self .reverse_path (current_dict )
136+
137+ neighbors = graph .graph [current_vertex ]
138+ for neighbor in neighbors :
139+ if neighbor in closed :
140+ # If we already expanded it it's definitely not better
141+ # to go through this node, or we would have expanded this
142+ # node first.
143+ continue
144+
145+ cost_start_to_neighbor = current_dict ['dist_start_to_here' ] + graph .get_edge_weight (current_vertex , neighbor )
146+ neighbor_from_lookup = open_lookup .get (neighbor , None ) # avoid searching twice
147+ if neighbor_from_lookup is not None :
148+ # If our heuristic is NOT consistent or the grid is NOT uniform,
149+ # it is possible that there is a better path to a neighbor of a
150+ # previously expanded node. See above, ctrl+f "river example neighbors"
151+
152+ # Note that the heuristic distance from here to end will be the same for
153+ # both, so the only difference will be in start->here through neighbor
154+ # and through the old neighbor.
155+
156+ old_dist_start_to_neighbor = neighbor_from_lookup ['dist_start_to_here' ]
157+
158+ if cost_start_to_neighbor < old_dist_start_to_neighbor :
159+ pred_dist_neighbor_to_end = neighbor_from_lookup ['pred_dist_here_to_end' ]
160+ pred_total_dist_through_neighbor_to_end = cost_start_to_neighbor + pred_dist_neighbor_to_end
161+ # Note, we've already shown that neighbor (the vector) is already in the open list,
162+ # but unfortunately we don't know where and we have to do a slow lookup to fix the
163+ # key its sorting by to the new predicted total distance.
164+
165+ # In case we're using a fancy debugger we want to search in user-code so when
166+ # this lookup freezes we can see how much longer its going to take.
167+ found = None
168+ for i in range (0 , len (open )):
169+ if open [i ][2 ] == neighbor :
170+ found = i
171+ break
172+ if found is None :
173+ raise Exception ('A vertex is in the open lookup but not in open. This is impossible, please submit an issue + include the graph!' )
174+ # todo I'm not certain about the performance characteristics of doing this with heapq, nor if
175+ # it would be better to delete heapify and push or rather than replace
176+ open [i ] = (pred_total_dist_through_neighbor_to_end , counter , neighbor )
177+ counter += 1
178+ heapq .heapify (open )
179+ open_lookup [neighbor ] = { 'vertex' : neighbor ,
180+ 'dist_start_to_here' : cost_start_to_neighbor ,
181+ 'pred_dist_here_to_end' : pred_dist_neighbor_to_end ,
182+ 'pred_total_dist' : pred_total_dist_through_neighbor_to_end ,
183+ 'parent' : current_dict }
184+ continue
185+
186+
187+ # We've found the first possible way to the path!
188+ pred_dist_neighbor_to_end = heuristic_fn (graph , neighbor , end )
189+ pred_total_dist_through_neighbor_to_end = cost_start_to_neighbor + pred_dist_neighbor_to_end
190+ heapq .heappush (open , (pred_total_dist_through_neighbor_to_end , counter , neighbor ))
191+ open_lookup [neighbor ] = { 'vertex' : neighbor ,
192+ 'dist_start_to_here' : cost_start_to_neighbor ,
193+ 'pred_dist_here_to_end' : pred_dist_neighbor_to_end ,
194+ 'pred_total_dist' : pred_total_dist_through_neighbor_to_end ,
195+ 'parent' : current_dict }
196+
197+ return None
198+
199+ @staticmethod
200+ def get_code (self ):
201+ """
202+ returns the code for the current class
203+ """
204+ return inspect .getsource (OneDirectionalAStar )
205+
206+
207+
0 commit comments