116 lines
4.0 KiB
Python
116 lines
4.0 KiB
Python
# 判断当前节点是否可以拓展并加入开放列表
|
||
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)
|