def A_Search_Re_Backward(map_length, blocklist, start, goal, show=False): # 0 search, 1 for g, 2 for blocked, 3 for closed num_expend = 0 current_cell = start goal_cell = goal magic_map = np.zeros((3, map_length, map_length)) magic_map[2][current_cell] = 1 current_steps = 0 move_trace = [start] UpdataBlocks(magic_map, map_length, current_cell, blocklist) while current_cell != goal_cell: current_steps += 1 magic_map[1][current_cell] = np.inf magic_map[0][current_cell] = current_steps magic_map[1][goal_cell] = 0 magic_map[0][goal_cell] = current_steps open_list = [] open_dict = dict() from_dict = dict() dh.insert(open_list, open_dict, Manhatton_value(current_cell, goal_cell), goal_cell) while open_list and magic_map[1][current_cell] > open_list[0]: num_expend += 1 c_cell = dh.pop(open_list, open_dict) for n_cell in vaild_neighbor(c_cell, map_length): if magic_map[2][n_cell] != 2: if magic_map[0][n_cell] < current_steps: magic_map[1][n_cell] = np.inf magic_map[0][n_cell] = current_steps if magic_map[1][n_cell] > magic_map[1][c_cell] + 1: magic_map[1][n_cell] = magic_map[1][c_cell] + 1 from_dict[n_cell] = c_cell dh.insert( open_list, open_dict, magic_map[1][n_cell] + Manhatton_value(n_cell, current_cell), n_cell) if not open_list: #print("no rout") return None, None if show: current_path = getBackPath(from_dict, current_cell, goal_cell) if current_cell == start: plt.ion() im, fig, oldline, pathline = showmode_launch( map_length, m_map=magic_map, pathlist=current_path, old_path=move_trace) plt.show() else: showmode_update(im, fig, oldline, pathline, magic_map[2], move_trace, current_path) while current_cell != goal_cell: cell = from_dict[current_cell] if magic_map[2][cell] != 2: move_trace.append(cell) current_cell = cell UpdataBlocks(magic_map, map_length, current_cell, blocklist) else: break if show: showmode_update(im, fig, oldline, pathline, magic_map[2], move_trace, current_path) return move_trace, num_expend
def A_Search_adp(map_length, blocklist, start, goal, show=False): # 0 search, 1 for g, 2 for blocked, 3 for pre_closed, 4 for current_closed, max_g = map_length * map_length current_cell = start goal_cell = goal magic_map = np.zeros((5, map_length, map_length)) magic_map[3] = np.zeros((map_length, map_length), dtype=bool) magic_map[4] = np.zeros((map_length, map_length), dtype=bool) magic_map[2][current_cell] = 1 p_max_g = 0 current_steps = 0 num_expend = 0 move_trace = [start] UpdataBlocks(magic_map, map_length, current_cell, blocklist) while current_cell != goal_cell: current_steps += 1 magic_map[1][current_cell] = 0 magic_map[0][current_cell] = current_steps magic_map[1][goal_cell] = np.inf magic_map[0][goal_cell] = current_steps open_list = [] open_dict = dict() from_dict = dict() magic_map[4] = np.zeros((map_length, map_length), dtype=bool) dh.insert(open_list, open_dict, Manhatton_value(current_cell, goal_cell), current_cell) while open_list and magic_map[1][goal_cell] > open_list[0]: c_cell = dh.pop(open_list, open_dict) magic_map[4][c_cell] = True num_expend += 1 for n_cell in vaild_neighbor(c_cell, map_length): if magic_map[2][n_cell] != 2: if magic_map[3][n_cell]: h_new = p_max_g - magic_map[1][n_cell] else: h_new = Manhatton_value(n_cell, goal_cell) if magic_map[0][n_cell] < current_steps: magic_map[1][n_cell] = np.inf magic_map[0][n_cell] = current_steps if magic_map[1][n_cell] > magic_map[1][c_cell] + 1: from_dict[n_cell] = c_cell new_g = magic_map[1][c_cell] + 1 dh.insert(open_list, open_dict, (max_g * (new_g + h_new) - new_g), n_cell) magic_map[1][n_cell] = new_g magic_map[3] = magic_map[4] p_max_g = magic_map[1][goal_cell] if not open_list: return None, None current_path = getPath(from_dict, current_cell, goal_cell) if show: if current_cell == start: plt.ion() im, fig, oldline, pathline = showmode_launch( map_length, m_map=magic_map, pathlist=current_path, old_path=move_trace) plt.show() else: showmode_update(im, fig, oldline, pathline, magic_map[2], move_trace, current_path) for cell in current_path: if cell == current_cell: continue else: if magic_map[2][cell] != 2: move_trace.append(cell) current_cell = cell UpdataBlocks(magic_map, map_length, current_cell, blocklist) else: break if show: showmode_update(im, fig, oldline, pathline, magic_map[2], move_trace, current_path) return move_trace, num_expend