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