bidirectional_breadth_first_search.py

"""
https://en.wikipedia.org/wiki/Bidirectional_search
"""

from __future__ importannotations

importtime

Path=list[tuple[int, int]]

grid= [
 [0, 0, 0, 0, 0, 0, 0],
 [0, 1, 0, 0, 0, 0, 0], # 0 are free path whereas 1's are obstacles
 [0, 0, 0, 0, 0, 0, 0],
 [0, 0, 1, 0, 0, 0, 0],
 [1, 0, 1, 0, 0, 0, 0],
 [0, 0, 0, 0, 0, 0, 0],
 [0, 0, 0, 0, 1, 0, 0],
]

delta= [[-1, 0], [0, -1], [1, 0], [0, 1]] # up, left, down, right


classNode:
def__init__(
self, pos_x: int, pos_y: int, goal_x: int, goal_y: int, parent: Node|None
 ):
self.pos_x=pos_x
self.pos_y=pos_y
self.pos= (pos_y, pos_x)
self.goal_x=goal_x
self.goal_y=goal_y
self.parent=parent


classBreadthFirstSearch:
"""
 # Comment out slow pytests...
 # 9.15s call graphs/bidirectional_breadth_first_search.py:: \
 # graphs.bidirectional_breadth_first_search.BreadthFirstSearch
 # >>> bfs = BreadthFirstSearch((0, 0), (len(grid) - 1, len(grid[0]) - 1))
 # >>> (bfs.start.pos_y + delta[3][0], bfs.start.pos_x + delta[3][1])
 (0, 1)
 # >>> [x.pos for x in bfs.get_successors(bfs.start)]
 [(1, 0), (0, 1)]
 # >>> (bfs.start.pos_y + delta[2][0], bfs.start.pos_x + delta[2][1])
 (1, 0)
 # >>> bfs.retrace_path(bfs.start)
 [(0, 0)]
 # >>> bfs.search() # doctest: +NORMALIZE_WHITESPACE
 [(0, 0), (1, 0), (2, 0), (3, 0), (3, 1), (4, 1),
 (5, 1), (5, 2), (5, 3), (5, 4), (5, 5), (6, 5), (6, 6)]
 """

def__init__(self, start: tuple[int, int], goal: tuple[int, int]):
self.start=Node(start[1], start[0], goal[1], goal[0], None)
self.target=Node(goal[1], goal[0], goal[1], goal[0], None)

self.node_queue= [self.start]
self.reached=False

defsearch(self) ->Path|None:
whileself.node_queue:
current_node=self.node_queue.pop(0)

ifcurrent_node.pos==self.target.pos:
self.reached=True
returnself.retrace_path(current_node)

successors=self.get_successors(current_node)

fornodeinsuccessors:
self.node_queue.append(node)

ifnotself.reached:
return [self.start.pos]
returnNone

defget_successors(self, parent: Node) ->list[Node]:
"""
 Returns a list of successors (both in the grid and free spaces)
 """
successors= []
foractionindelta:
pos_x=parent.pos_x+action[1]
pos_y=parent.pos_y+action[0]
ifnot (0<=pos_x<=len(grid[0]) -1and0<=pos_y<=len(grid) -1):
continue

ifgrid[pos_y][pos_x] !=0:
continue

successors.append(
Node(pos_x, pos_y, self.target.pos_y, self.target.pos_x, parent)
 )
returnsuccessors

defretrace_path(self, node: Node|None) ->Path:
"""
 Retrace the path from parents to parents until start node
 """
current_node=node
path= []
whilecurrent_nodeisnotNone:
path.append((current_node.pos_y, current_node.pos_x))
current_node=current_node.parent
path.reverse()
returnpath


classBidirectionalBreadthFirstSearch:
"""
 >>> bd_bfs = BidirectionalBreadthFirstSearch((0, 0), (len(grid) - 1,
 ... len(grid[0]) - 1))
 >>> bd_bfs.fwd_bfs.start.pos == bd_bfs.bwd_bfs.target.pos
 True
 >>> bd_bfs.retrace_bidirectional_path(bd_bfs.fwd_bfs.start,
 ... bd_bfs.bwd_bfs.start)
 [(0, 0)]
 >>> bd_bfs.search() # doctest: +NORMALIZE_WHITESPACE
 [(0, 0), (0, 1), (0, 2), (1, 2), (2, 2), (2, 3),
 (2, 4), (3, 4), (3, 5), (3, 6), (4, 6), (5, 6), (6, 6)]
 """

def__init__(self, start, goal):
self.fwd_bfs=BreadthFirstSearch(start, goal)
self.bwd_bfs=BreadthFirstSearch(goal, start)
self.reached=False

defsearch(self) ->Path|None:
whileself.fwd_bfs.node_queueorself.bwd_bfs.node_queue:
current_fwd_node=self.fwd_bfs.node_queue.pop(0)
current_bwd_node=self.bwd_bfs.node_queue.pop(0)

ifcurrent_bwd_node.pos==current_fwd_node.pos:
self.reached=True
returnself.retrace_bidirectional_path(
current_fwd_node, current_bwd_node
 )

self.fwd_bfs.target=current_bwd_node
self.bwd_bfs.target=current_fwd_node

successors= {
self.fwd_bfs: self.fwd_bfs.get_successors(current_fwd_node),
self.bwd_bfs: self.bwd_bfs.get_successors(current_bwd_node),
 }

forbfsin [self.fwd_bfs, self.bwd_bfs]:
fornodeinsuccessors[bfs]:
bfs.node_queue.append(node)

ifnotself.reached:
return [self.fwd_bfs.start.pos]
returnNone

defretrace_bidirectional_path(self, fwd_node: Node, bwd_node: Node) ->Path:
fwd_path=self.fwd_bfs.retrace_path(fwd_node)
bwd_path=self.bwd_bfs.retrace_path(bwd_node)
bwd_path.pop()
bwd_path.reverse()
path=fwd_path+bwd_path
returnpath


if__name__=="__main__":
# all coordinates are given in format [y,x]
importdoctest

doctest.testmod()
init= (0, 0)
goal= (len(grid) -1, len(grid[0]) -1)
forelemingrid:
print(elem)

start_bfs_time=time.time()
bfs=BreadthFirstSearch(init, goal)
path=bfs.search()
bfs_time=time.time() -start_bfs_time

print("Unidirectional BFS computation time : ", bfs_time)

start_bd_bfs_time=time.time()
bd_bfs=BidirectionalBreadthFirstSearch(init, goal)
bd_path=bd_bfs.search()
bd_bfs_time=time.time() -start_bd_bfs_time

print("Bidirectional BFS computation time : ", bd_bfs_time)