202 lines
6.2 KiB
Python
202 lines
6.2 KiB
Python
# 导入NumPy库，用于处理多维数组
|
|
import numpy as np
|
|
|
|
|
|
# 定义一个表示点的类
|
|
class Point:
|
|
"""
|
|
表示二维平面上的一个点，包含坐标和一个评估函数值 f。
|
|
"""
|
|
|
|
def __init__(self, x, y):
|
|
# 初始化点的 x 坐标
|
|
self.x = x
|
|
# 初始化点的 y 坐标
|
|
self.y = y
|
|
# 初始化评估函数值 f 为 0
|
|
self.f = 0
|
|
|
|
# 设置点的评估函数值 f
|
|
def setF(self, f):
|
|
self.f = f
|
|
|
|
# 定义点的相等比较方法
|
|
def __eq__(self, other):
|
|
# 当两个点的 x 和 y 坐标都相等时，认为这两个点相等
|
|
return self.x == other.x and self.y == other.y
|
|
|
|
# 计算当前点到终点的曼哈顿距离加上固定值 1，作为评估函数值
|
|
def getFunctionValue(self, end):
|
|
# 计算曼哈顿距离
|
|
dist = abs(self.x - end.x) + abs(self.y - end.y)
|
|
# 返回评估函数值
|
|
return dist + 1
|
|
|
|
|
|
# 定义一个表示状态的类
|
|
class State:
|
|
"""
|
|
表示地图的状态，包含地图状态、当前点和终点。
|
|
"""
|
|
|
|
def __init__(self, state, current_point=Point(0, 0), end_point=Point(0, 0)):
|
|
# 初始化地图状态
|
|
self.state = state
|
|
# 初始化当前点
|
|
self.cP = current_point
|
|
# 初始化终点
|
|
self.eP = end_point
|
|
|
|
# 定义状态的相等比较方法
|
|
def __eq__(self, other):
|
|
# 当两个状态的当前点相等时，认为这两个状态相等
|
|
return self.cP == other.cP
|
|
|
|
# 设置状态的评估函数值 f
|
|
def setF(self, f):
|
|
self.f = f
|
|
|
|
# 设置当前点的坐标
|
|
def setCurrentPoint(self, x, y):
|
|
self.cP.x = x
|
|
self.cP.y = y
|
|
|
|
# 获取当前点的坐标
|
|
def getCurPoint(self):
|
|
return self.cP.x, self.cP.y
|
|
|
|
|
|
# 确定下一步的方法
|
|
def nextStep(map, openTable, closeTable, wrongTable):
|
|
# 存储下一步可能的点
|
|
subPoints = []
|
|
# 获取地图的边界
|
|
boarder = len(map.state) - 1
|
|
# 获取当前所在的点
|
|
x, y = map.getCurPoint()
|
|
# 往左走
|
|
if y > 0 and map.state[x][y - 1] == 0:
|
|
# 创建新的点
|
|
p = Point(x, y - 1)
|
|
# 检查点是否不在关闭列表和错误列表中
|
|
if p not in closeTable and p not in wrongTable:
|
|
# 添加到开放列表
|
|
openTable.append(p)
|
|
# 获取 F 函数值并设置
|
|
p.setF(p.getFunctionValue(map.eP))
|
|
# 添加到可能的下一步点列表
|
|
subPoints.append(p)
|
|
# 往上走
|
|
if x > 0 and map.state[x - 1][y] == 0:
|
|
# 创建新的点
|
|
p = Point(x - 1, y)
|
|
# 检查点是否不在关闭列表和错误列表中
|
|
if p not in closeTable and p not in wrongTable:
|
|
# 添加到开放列表
|
|
openTable.append(p)
|
|
# 获取 F 函数值并设置
|
|
p.setF(p.getFunctionValue(map.eP))
|
|
# 添加到可能的下一步点列表
|
|
subPoints.append(p)
|
|
# 往下走
|
|
if x < boarder and map.state[x + 1][y] == 0:
|
|
# 创建新的点
|
|
p = Point(x + 1, y)
|
|
# 检查点是否不在关闭列表和错误列表中
|
|
if p not in closeTable and p not in wrongTable:
|
|
# 添加到开放列表
|
|
openTable.append(p)
|
|
# 获取 F 函数值并设置
|
|
p.setF(p.getFunctionValue(map.eP))
|
|
# 添加到可能的下一步点列表
|
|
subPoints.append(p)
|
|
# 往右走
|
|
if y < boarder and map.state[x][y + 1] == 0:
|
|
# 创建新的点
|
|
p = Point(x, y + 1)
|
|
# 检查点是否不在关闭列表和错误列表中
|
|
if p not in closeTable and p not in wrongTable:
|
|
# 添加到开放列表
|
|
openTable.append(p)
|
|
# 获取 F 函数值并设置
|
|
p.setF(p.getFunctionValue(map.eP))
|
|
# 添加到可能的下一步点列表
|
|
subPoints.append(p)
|
|
# 根据 F 值排序，获取 F 值最小的点
|
|
subPoints.sort(key=compareF)
|
|
if len(subPoints) < 1:
|
|
# 防止走到死路无法回头情况
|
|
wrongTable.append(Point(map.cP.x, map.cP.y))
|
|
closeTable.remove(map.cP)
|
|
# 获取上一个点作为下一步
|
|
next_point = closeTable[-1]
|
|
map.cP.x, map.cP.y = next_point.x, next_point.y
|
|
else:
|
|
# 获取 F 值最小的点作为下一步
|
|
next_point = subPoints[0]
|
|
map.cP.x, map.cP.y = next_point.x, next_point.y
|
|
# 将下一步点添加到关闭列表
|
|
closeTable.append(next_point)
|
|
# 从开放列表中移除下一步点
|
|
openTable.remove(next_point)
|
|
# 展示最终路径信息
|
|
# 合并起点和关闭列表作为最终路径
|
|
# 调用 solve 函数开始寻找路径
|
|
# 创建地图状态对象
|
|
# 初始化最终路径，包含起点
|
|
# 定义终点
|
|
# 定义起点
|
|
# 定义地图状态
|
|
# 错误列表，存储走错的点
|
|
# 关闭列表，存储已探索的点
|
|
# 开放列表，存储待探索的点
|
|
|
|
|
|
# 用于排序的比较函数，返回点的 F 值
|
|
# 开始循环，直到当前点到达终点
|
|
|
|
|
|
# 迭代走下一步
|
|
def solve(map, openTable, closeTable, wrongTable):
|
|
# start the loop
|
|
while not map.cP == map.eP:
|
|
nextStep(map, openTable, closeTable, wrongTable)
|
|
|
|
|
|
def compareF(p):
|
|
return p.f
|
|
|
|
|
|
# 展示最后结果
|
|
def showInfo(map, path):
|
|
for i in range(len(map.state)):
|
|
for j in range(len(map.state)):
|
|
if Point(i, j) in path:
|
|
# 正确路径用‘*’表示
|
|
print('*', end=' ')
|
|
else:
|
|
print(map.state[i, j], end=' ')
|
|
print("\n")
|
|
return
|
|
|
|
|
|
if __name__ == '__main__':
|
|
# openList
|
|
openTable = []
|
|
# closeList
|
|
closeTable = []
|
|
# 走错路返回用的
|
|
wrongTable = []
|
|
state = np.array([[0, 0, 0, 0, 0], [1, 0, 1, 0, 1], [0, 0, 0, 0, 1], [0, 1, 0, 0, 0], [0, 0, 0, 1, 0]])
|
|
# 起点终点
|
|
start_point = Point(0, 0)
|
|
end_point = Point(4, 4)
|
|
# 最终路径
|
|
path = [start_point]
|
|
Map = State(state, Point(0, 0), end_point)
|
|
solve(Map, openTable, closeTable, wrongTable)
|
|
print('Best Way:')
|
|
path = path + closeTable
|
|
showInfo(Map, path)
|
|
print("Total steps is %d" % (len(path) - 1))
|