""" https://en.wikipedia.org/wiki/Bidirectional_search """ from __future__ import annotations import time from math import sqrt # 1 for manhattan, 0 for euclidean HEURISTIC = 0 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 TPosition = tuple[int, int] class Node: """ >>> k = Node(0, 0, 4, 3, 0, None) >>> k.calculate_heuristic() 5.0 >>> n = Node(1, 4, 3, 4, 2, None) >>> n.calculate_heuristic() 2.0 >>> l = [k, n] >>> n == l[0] False >>> l.sort() >>> n == l[0] True """ def __init__( self, pos_x: int, pos_y: int, goal_x: int, goal_y: int, g_cost: int, parent: Node | None, ) -> 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.g_cost = g_cost self.parent = parent self.h_cost = self.calculate_heuristic() self.f_cost = self.g_cost + self.h_cost def calculate_heuristic(self) -> float: """ Heuristic for the A* """ dy = self.pos_x - self.goal_x dx = self.pos_y - self.goal_y if HEURISTIC == 1: return abs(dx) + abs(dy) else: return sqrt(dy ** 2 + dx ** 2) def __lt__(self, other: Node) -> bool: return self.f_cost < other.f_cost class AStar: """ >>> astar = AStar((0, 0), (len(grid) - 1, len(grid[0]) - 1)) >>> (astar.start.pos_y + delta[3][0], astar.start.pos_x + delta[3][1]) (0, 1) >>> [x.pos for x in astar.get_successors(astar.start)] [(1, 0), (0, 1)] >>> (astar.start.pos_y + delta[2][0], astar.start.pos_x + delta[2][1]) (1, 0) >>> astar.retrace_path(astar.start) [(0, 0)] >>> astar.search() # doctest: +NORMALIZE_WHITESPACE [(0, 0), (1, 0), (2, 0), (2, 1), (2, 2), (2, 3), (3, 3), (4, 3), (4, 4), (5, 4), (5, 5), (6, 5), (6, 6)] """ def __init__(self, start: TPosition, goal: TPosition): self.start = Node(start[1], start[0], goal[1], goal[0], 0, None) self.target = Node(goal[1], goal[0], goal[1], goal[0], 99999, None) self.open_nodes = [self.start] self.closed_nodes: list[Node] = [] self.reached = False def search(self) -> list[TPosition]: while self.open_nodes: # Open Nodes are sorted using __lt__ self.open_nodes.sort() current_node = self.open_nodes.pop(0) if current_node.pos == self.target.pos: return self.retrace_path(current_node) self.closed_nodes.append(current_node) successors = self.get_successors(current_node) for child_node in successors: if child_node in self.closed_nodes: continue if child_node not in self.open_nodes: self.open_nodes.append(child_node) else: # retrieve the best current path better_node = self.open_nodes.pop(self.open_nodes.index(child_node)) if child_node.g_cost < better_node.g_cost: self.open_nodes.append(child_node) else: self.open_nodes.append(better_node) return [self.start.pos] def get_successors(self, parent: Node) -> list[Node]: """ Returns a list of successors (both in the grid and free spaces) """ successors = [] for action in delta: pos_x = parent.pos_x + action[1] pos_y = parent.pos_y + action[0] if not (0 <= pos_x <= len(grid[0]) - 1 and 0 <= pos_y <= len(grid) - 1): continue if grid[pos_y][pos_x] != 0: continue successors.append( Node( pos_x, pos_y, self.target.pos_y, self.target.pos_x, parent.g_cost + 1, parent, ) ) return successors def retrace_path(self, node: Node | None) -> list[TPosition]: """ Retrace the path from parents to parents until start node """ current_node = node path = [] while current_node is not None: path.append((current_node.pos_y, current_node.pos_x)) current_node = current_node.parent path.reverse() return path class BidirectionalAStar: """ >>> bd_astar = BidirectionalAStar((0, 0), (len(grid) - 1, len(grid[0]) - 1)) >>> bd_astar.fwd_astar.start.pos == bd_astar.bwd_astar.target.pos True >>> bd_astar.retrace_bidirectional_path(bd_astar.fwd_astar.start, ... bd_astar.bwd_astar.start) [(0, 0)] >>> bd_astar.search() # doctest: +NORMALIZE_WHITESPACE [(0, 0), (0, 1), (0, 2), (1, 2), (1, 3), (2, 3), (2, 4), (2, 5), (3, 5), (4, 5), (5, 5), (5, 6), (6, 6)] """ def __init__(self, start: TPosition, goal: TPosition) -> None: self.fwd_astar = AStar(start, goal) self.bwd_astar = AStar(goal, start) self.reached = False def search(self) -> list[TPosition]: while self.fwd_astar.open_nodes or self.bwd_astar.open_nodes: self.fwd_astar.open_nodes.sort() self.bwd_astar.open_nodes.sort() current_fwd_node = self.fwd_astar.open_nodes.pop(0) current_bwd_node = self.bwd_astar.open_nodes.pop(0) if current_bwd_node.pos == current_fwd_node.pos: return self.retrace_bidirectional_path( current_fwd_node, current_bwd_node ) self.fwd_astar.closed_nodes.append(current_fwd_node) self.bwd_astar.closed_nodes.append(current_bwd_node) self.fwd_astar.target = current_bwd_node self.bwd_astar.target = current_fwd_node successors = { self.fwd_astar: self.fwd_astar.get_successors(current_fwd_node), self.bwd_astar: self.bwd_astar.get_successors(current_bwd_node), } for astar in [self.fwd_astar, self.bwd_astar]: for child_node in successors[astar]: if child_node in astar.closed_nodes: continue if child_node not in astar.open_nodes: astar.open_nodes.append(child_node) else: # retrieve the best current path better_node = astar.open_nodes.pop( astar.open_nodes.index(child_node) ) if child_node.g_cost < better_node.g_cost: astar.open_nodes.append(child_node) else: astar.open_nodes.append(better_node) return [self.fwd_astar.start.pos] def retrace_bidirectional_path( self, fwd_node: Node, bwd_node: Node ) -> list[TPosition]: fwd_path = self.fwd_astar.retrace_path(fwd_node) bwd_path = self.bwd_astar.retrace_path(bwd_node) bwd_path.pop() bwd_path.reverse() path = fwd_path + bwd_path return path if __name__ == "__main__": # all coordinates are given in format [y,x] init = (0, 0) goal = (len(grid) - 1, len(grid[0]) - 1) for elem in grid: print(elem) start_time = time.time() a_star = AStar(init, goal) path = a_star.search() end_time = time.time() - start_time print(f"AStar execution time = {end_time:f} seconds") bd_start_time = time.time() bidir_astar = BidirectionalAStar(init, goal) bd_end_time = time.time() - bd_start_time print(f"BidirectionalAStar execution time = {bd_end_time:f} seconds")