append/code/heuristicallySearch.py

116 lines
4.0 KiB
Python
Raw Blame History

This file contains ambiguous Unicode characters

This file contains Unicode characters that might be confused with other characters. If you think that this is intentional, you can safely ignore this warning. Use the Escape button to reveal them.

# 判断当前节点是否可以拓展并加入开放列表
def validNode(open_list, open_parent_node, g, x, y, x_expand, y_expand, min_idx):
if maze[x_expand][y_expand] == '0' or maze[x_expand][y_expand] == 'E':
if [x_expand, y_expand] in open_list:
idx = open_list.index([x_expand, y_expand])
if g[idx] < g[min_idx] + 1:
g[idx] = g[min_idx] + 1
open_parent_node[idx] = [x,y]
else:
open_list.append([x_expand,y_expand])
open_parent_node.append([x,y])
g.append(g[min_idx]+1)
# 拓展开放列表中的节点启发函数为Lp距离
def expandNode(open_list, close_list, open_parent_node, close_parent_node, g):
min_cost = 100000 # 当前最小开销
min_idx = 0 # 开销最小的节点的下标
p = 1 # Lp距离的p
# 找到可扩展的开销最小的节点
for i in range(len(open_list)):
h = (abs(open_list[i][0] - end_node[0])**p + abs(open_list[i][1] - end_node[1])**p ) ** (1/p)
# h = max(abs(open_list[i][0] - end_node[0]), abs(open_list[i][1] - end_node[1]))
# h = 0
h *= 4
f = h + g[i]
if f < min_cost:
min_cost = f
min_idx = i
# 将该节点的可扩展节点加入开放列表
x = open_list[min_idx][0]
y = open_list[min_idx][1]
maze[x][y] = '1'
validNode(open_list, open_parent_node, g, x, y, x+1, y, min_idx)
validNode(open_list, open_parent_node, g, x, y, x-1, y, min_idx)
validNode(open_list, open_parent_node, g, x, y, x, y+1, min_idx)
validNode(open_list, open_parent_node, g, x, y, x, y-1, min_idx)
print(len(open_list))
# 将该节点从开放列表中删除并加入关闭列表
close_list.append([x,y])
close_parent_node.append(open_parent_node[min_idx])
expanded.add((x,y))
del open_list[min_idx]
del open_parent_node[min_idx]
del g[min_idx]
# 读入迷宫数据
maze = []
with open('MazeData.txt', 'r') as f:
for eachLine in f:
line = []
for eachPos in eachLine:
if eachPos == '\n':
break
line.append(eachPos)
maze.append(line)
# 找到起点和终点坐标,并加入开启列表
row = len(maze)
col = len(maze[0])
open_list = [] # 开启列表
open_parent_node = [None] # 开启列表中每个节点对应的父节点
g = [0] # 开启列表中每个节点对应的 g(x) 值
close_list = [] # 关闭列表
close_parent_node = [] # 关闭列表中每个节点对应的父节点
end_node = []
expanded = set()
for i in range(row):
for j in range(col):
if maze[i][j] == 'S':
open_list.append([i,j])
if maze[i][j] == 'E':
end_node = [i,j]
# 开始搜索
find_ans = False
while 1:
# 若开启列表为空,则迷宫无解
if open_list == []:
find_ans = False
break
# 对节点进行拓展
expandNode(open_list, close_list, open_parent_node, close_parent_node, g)
# 若开放节点中存在终点节点则找到解
if end_node in open_list:
find_ans = True
break
if find_ans == False:
print('no answer for this maze')
else:
idx = open_list.index(end_node)
last_node = open_parent_node[idx]
path = [end_node]
while last_node is not None:
path.append(last_node)
last_node = close_parent_node[close_list.index(last_node)]
path = path[::-1]
print(len(expanded) + 1)
print(len(path))
print(path)
# maze = []
# with open('MazeData.txt', 'r') as f:
# for eachLine in f:
# maze.append(eachLine)
# for i in range(row):
# for j in range(col):
# if [i,j] in path:
# if (i,j) in expanded:
# maze[i] = maze[i][:j] + 'X' + maze[i][j+1:]
# with open('ans.txt', 'w') as f:
# for eachLine in maze:
# f.write(eachLine)