8
8
from math import sqrt
9
9
10
10
# 1 for manhattan, 0 for euclidean
11
+ from typing import Optional
12
+
11
13
HEURISTIC = 0
12
14
13
15
grid = [
22
24
23
25
delta = [[- 1 , 0 ], [0 , - 1 ], [1 , 0 ], [0 , 1 ]] # up, left, down, right
24
26
27
+ TPosition = tuple [int , int ]
28
+
25
29
26
30
class Node :
27
31
"""
@@ -39,7 +43,15 @@ class Node:
39
43
True
40
44
"""
41
45
42
- def __init__ (self , pos_x , pos_y , goal_x , goal_y , g_cost , parent ):
46
+ def __init__ (
47
+ self ,
48
+ pos_x : int ,
49
+ pos_y : int ,
50
+ goal_x : int ,
51
+ goal_y : int ,
52
+ g_cost : int ,
53
+ parent : Optional [Node ],
54
+ ) -> None :
43
55
self .pos_x = pos_x
44
56
self .pos_y = pos_y
45
57
self .pos = (pos_y , pos_x )
@@ -61,7 +73,7 @@ def calculate_heuristic(self) -> float:
61
73
else :
62
74
return sqrt (dy ** 2 + dx ** 2 )
63
75
64
- def __lt__ (self , other ) -> bool :
76
+ def __lt__ (self , other : Node ) -> bool :
65
77
return self .f_cost < other .f_cost
66
78
67
79
@@ -81,23 +93,22 @@ class AStar:
81
93
(4, 3), (4, 4), (5, 4), (5, 5), (6, 5), (6, 6)]
82
94
"""
83
95
84
- def __init__ (self , start , goal ):
96
+ def __init__ (self , start : TPosition , goal : TPosition ):
85
97
self .start = Node (start [1 ], start [0 ], goal [1 ], goal [0 ], 0 , None )
86
98
self .target = Node (goal [1 ], goal [0 ], goal [1 ], goal [0 ], 99999 , None )
87
99
88
100
self .open_nodes = [self .start ]
89
- self .closed_nodes = []
101
+ self .closed_nodes : list [ Node ] = []
90
102
91
103
self .reached = False
92
104
93
- def search (self ) -> list [tuple [ int ] ]:
105
+ def search (self ) -> list [TPosition ]:
94
106
while self .open_nodes :
95
107
# Open Nodes are sorted using __lt__
96
108
self .open_nodes .sort ()
97
109
current_node = self .open_nodes .pop (0 )
98
110
99
111
if current_node .pos == self .target .pos :
100
- self .reached = True
101
112
return self .retrace_path (current_node )
102
113
103
114
self .closed_nodes .append (current_node )
@@ -118,8 +129,7 @@ def search(self) -> list[tuple[int]]:
118
129
else :
119
130
self .open_nodes .append (better_node )
120
131
121
- if not (self .reached ):
122
- return [(self .start .pos )]
132
+ return [self .start .pos ]
123
133
124
134
def get_successors (self , parent : Node ) -> list [Node ]:
125
135
"""
@@ -147,7 +157,7 @@ def get_successors(self, parent: Node) -> list[Node]:
147
157
)
148
158
return successors
149
159
150
- def retrace_path (self , node : Node ) -> list [tuple [ int ] ]:
160
+ def retrace_path (self , node : Optional [ Node ] ) -> list [TPosition ]:
151
161
"""
152
162
Retrace the path from parents to parents until start node
153
163
"""
@@ -173,20 +183,19 @@ class BidirectionalAStar:
173
183
(2, 5), (3, 5), (4, 5), (5, 5), (5, 6), (6, 6)]
174
184
"""
175
185
176
- def __init__ (self , start , goal ) :
186
+ def __init__ (self , start : TPosition , goal : TPosition ) -> None :
177
187
self .fwd_astar = AStar (start , goal )
178
188
self .bwd_astar = AStar (goal , start )
179
189
self .reached = False
180
190
181
- def search (self ) -> list [tuple [ int ] ]:
191
+ def search (self ) -> list [TPosition ]:
182
192
while self .fwd_astar .open_nodes or self .bwd_astar .open_nodes :
183
193
self .fwd_astar .open_nodes .sort ()
184
194
self .bwd_astar .open_nodes .sort ()
185
195
current_fwd_node = self .fwd_astar .open_nodes .pop (0 )
186
196
current_bwd_node = self .bwd_astar .open_nodes .pop (0 )
187
197
188
198
if current_bwd_node .pos == current_fwd_node .pos :
189
- self .reached = True
190
199
return self .retrace_bidirectional_path (
191
200
current_fwd_node , current_bwd_node
192
201
)
@@ -220,12 +229,11 @@ def search(self) -> list[tuple[int]]:
220
229
else :
221
230
astar .open_nodes .append (better_node )
222
231
223
- if not self .reached :
224
- return [self .fwd_astar .start .pos ]
232
+ return [self .fwd_astar .start .pos ]
225
233
226
234
def retrace_bidirectional_path (
227
235
self , fwd_node : Node , bwd_node : Node
228
- ) -> list [tuple [ int ] ]:
236
+ ) -> list [TPosition ]:
229
237
fwd_path = self .fwd_astar .retrace_path (fwd_node )
230
238
bwd_path = self .bwd_astar .retrace_path (bwd_node )
231
239
bwd_path .pop ()
@@ -236,9 +244,6 @@ def retrace_bidirectional_path(
236
244
237
245
if __name__ == "__main__" :
238
246
# all coordinates are given in format [y,x]
239
- import doctest
240
-
241
- doctest .testmod ()
242
247
init = (0 , 0 )
243
248
goal = (len (grid ) - 1 , len (grid [0 ]) - 1 )
244
249
for elem in grid :
@@ -252,6 +257,5 @@ def retrace_bidirectional_path(
252
257
253
258
bd_start_time = time .time ()
254
259
bidir_astar = BidirectionalAStar (init , goal )
255
- path = bidir_astar .search ()
256
260
bd_end_time = time .time () - bd_start_time
257
261
print (f"BidirectionalAStar execution time = { bd_end_time :f} seconds" )
0 commit comments