实现人物在二维地图中寻路的算法代码,并给出测试
时间: 2024-05-02 21:21:20 浏览: 12
以下是一个基于A*算法的人物寻路代码实现,使用Python语言编写:
```python
import heapq
class Node:
def __init__(self, x, y, parent=None):
self.x = x
self.y = y
self.parent = parent
self.g = 0
self.h = 0
def __lt__(self, other):
return self.g + self.h < other.g + other.h
class AStar:
def __init__(self, walls, start, end):
self.walls = walls
self.start = start
self.end = end
self.open_list = []
self.closed_list = []
def heuristic(self, a, b):
return abs(a.x - b.x) + abs(a.y - b.y)
def get_neighbors(self, node):
neighbors = []
for dx, dy in [(0, 1), (0, -1), (1, 0), (-1, 0)]:
x = node.x + dx
y = node.y + dy
if x < 0 or x >= len(self.walls) or y < 0 or y >= len(self.walls[0]):
continue
if self.walls[x][y]:
continue
neighbors.append(Node(x, y, node))
return neighbors
def get_path(self):
path = []
current = self.closed_list[-1]
while current is not None:
path.append((current.x, current.y))
current = current.parent
return path[::-1]
def search(self):
heapq.heappush(self.open_list, Node(self.start[0], self.start[1]))
while self.open_list:
current = heapq.heappop(self.open_list)
if current.x == self.end[0] and current.y == self.end[1]:
self.closed_list.append(current)
return self.get_path()
self.closed_list.append(current)
for neighbor in self.get_neighbors(current):
if neighbor in self.closed_list:
continue
if neighbor in self.open_list:
new_g = current.g + 1
if new_g < neighbor.g:
neighbor.g = new_g
neighbor.parent = current
else:
neighbor.g = current.g + 1
neighbor.h = self.heuristic(neighbor, Node(self.end[0], self.end[1]))
neighbor.parent = current
heapq.heappush(self.open_list, neighbor)
return None
# 测试代码
walls = [[0, 0, 0, 0, 0],
[0, 1, 0, 1, 0],
[0, 1, 0, 1, 0],
[0, 1, 0, 1, 0],
[0, 0, 0, 0, 0]]
start = (0, 0)
end = (4, 4)
astar = AStar(walls, start, end)
path = astar.search()
print(path)
```
在上面的代码中,我们使用了一个Node类来表示一个节点,包含了节点的坐标、父节点、g值和h值。g值表示从起点到该节点的距离,h值表示从该节点到终点的估价距离,使用曼哈顿距离作为估价函数。
在AStar类中,我们使用一个二维数组walls来表示地图,其中0表示可通过的格子,1表示障碍物。start和end表示起点和终点的坐标。open_list和closed_list分别表示开放列表和关闭列表,使用堆来实现open_list,以便快速找到f值最小的节点。heuristic方法和get_neighbors方法分别用来计算两个节点之间的估价距离和获取一个节点的所有邻居节点。get_path方法用来获取最终的路径,search方法是主要的寻路算法,实现了A*算法的核心逻辑。
在测试代码中,我们使用一个5x5的地图进行测试,其中从左上角到右下角有一条可行路径。运行代码后,输出的是从起点到终点的路径坐标。