我正在尝试在我的程序中实现A*路径查找算法,但它返回以下输出。
在图像中,蓝色块是已访问的块,带圆圈的块是尚未访问的块,黄色块是我们返回的路径。
我无法找出代码中的问题,这是我使用的代码:
class Node:
def __init__(self, parent=None, position=None):
self.parent = parent
self.position = position
self.g = 0
self.h = 0
self.f = 0
def __eq__(self, other):
return self.position == other.position
def search(maze, cost, start, end): # Hear maze is a 2D list with 1 means a wall and 0 is a clear block
start_node = Node(None, tuple(start))
start_node.g = start_node.h = start_node.f = 0
end_node = Node(None, tuple(end))
end_node.g = end_node.h = end_node.f = 0
yet_to_visit_list = []
visited_list = []
yet_to_visit_list.append(start_node)
outer_iterations = 0
max_iterations = (len(maze) // 2) ** 10
move = [[-1, 0], [0, -1], [1, 0], [0, 1]]
no_rows, no_columns = np.shape(maze)
while len(yet_to_visit_list) > 0:
outer_iterations += 1
current_node = yet_to_visit_list[0]
current_index = 0
for index, item in enumerate(yet_to_visit_list):
if item.f < current_node.f:
current_node = item
current_index = index
if outer_iterations > max_iterations:
print("Cann't find the path... too many iterations")
return return_path(current_node, maze)
yet_to_visit_list.pop(current_index)
visited_list.append(current_node)
maze[current_node.position[0]][current_node.position[1]] = 2 # 1 wall 2 visited 3 yet to visit
if current_node == end_node:
return return_path(current_node, maze)
children = []
for new_position in move:
node_position = (current_node.position[0]+ new_position[0], current_node.position[1]+ new_position[1])
if (node_position[0] > (no_rows - 1) or node_position[0] < 0 or node_position[1]>(no_columns-1) or node_position[1] < 0):
continue
if maze[node_position[0]][node_position[1]] != 0:
continue
new_node = Node(current_node, node_position)
children.append(new_node)
for child in children:
if len([visited_child for visited_child in visited_list if visited_child == child]) > 0:
continue
child.g = current_node.g + cost
child.h = (((child.position[0] - end_node.position[0]) ** 2 ) +
((child.position[0] - end_node.position[0])) ** 2)
child.f = child.g + child.h
if len([i for i in yet_to_visit_list if child == i and child.g>i.g]) > 0:
continue
yet_to_visit_list.append(child)
以下函数返回路径:
def return_path(current_node, maze):
path = []
no_rows, no_columns = np.shape(maze)
result = maze
current = current_node
while current is not None:
path.append(current.position)
current = current.parent
path = path[::-1]
start_value = 0
for i in range(len(path)):
result[path[i][0]][path[i][1]] = start_value
start_value += 1
return result