有代码如下:start_cor = (19, 0) end_cor = (1, 20) # 定义路径上的路标点 waypoints = [(5, 15), (5, 1), (9, 3), (11, 17), (7, 19), (15, 19), (13, 1), (15, 5)] # 定义计算两个点之间距离的函数 def distance(_from, _to): x1, y1 = _from x2, y2 = _to # 使用 A* 算法寻路,计算出两个点之间的距离 dist = get_dist(x1, y1, x2, y2) return dist 其中distance函数返回的是两个坐标之间的距离将以上代码改进以实现从坐标start_cor开始经过waypoint中的所有坐标最后到达终点坐标end_co,要使整个路径最短并打印出经过的每一个坐标
时间: 2023-09-19 16:10:04 浏览: 41
可以使用 A* 算法来实现从起点到终点的最短路径,并经过所有的路标点。
首先,需要实现 A* 算法中的节点类,其中包括节点坐标、父节点、g 值、h 值和 f 值:
```python
class Node:
def __init__(self, x, y, parent=None):
self.x = x
self.y = y
self.parent = parent
self.g = 0
self.h = 0
self.f = 0
```
接下来,实现 A* 算法的主函数:
```python
def astar(start, end, waypoints):
open_list = []
closed_list = []
path = []
# 创建起点和终点节点
start_node = Node(start[0], start[1])
end_node = Node(end[0], end[1])
# 加入起点节点到 open_list 中
open_list.append(start_node)
# 当 open_list 不为空时循环
while open_list:
# 找到 f 值最小的节点
current_node = open_list[0]
current_index = 0
for index, node in enumerate(open_list):
if node.f < current_node.f:
current_node = node
current_index = index
# 将当前节点从 open_list 中移除,并加入到 closed_list 中
open_list.pop(current_index)
closed_list.append(current_node)
# 如果当前节点是目标节点,返回路径
if current_node.x == end_node.x and current_node.y == end_node.y:
path = []
current = current_node
while current is not None:
path.append((current.x, current.y))
current = current.parent
return path[::-1]
# 找出当前节点相邻的节点
adjacent_nodes = []
for new_x, new_y in ((0, -1), (0, 1), (-1, 0), (1, 0)):
node_x = current_node.x + new_x
node_y = current_node.y + new_y
if node_x < 0 or node_x >= 20 or node_y < 0 or node_y >= 20:
continue
if (node_x, node_y) in waypoints:
continue
new_node = Node(node_x, node_y, current_node)
adjacent_nodes.append(new_node)
# 对相邻节点进行遍历
for adjacent_node in adjacent_nodes:
# 如果相邻节点已经在 closed_list 中,则跳过
if adjacent_node in closed_list:
continue
# 计算相邻节点的 g 值、h 值和 f 值
adjacent_node.g = current_node.g + distance((current_node.x, current_node.y), (adjacent_node.x, adjacent_node.y))
adjacent_node.h = distance((adjacent_node.x, adjacent_node.y), (end_node.x, end_node.y))
adjacent_node.f = adjacent_node.g + adjacent_node.h
# 如果相邻节点已经在 open_list 中,则比较两者的 g 值大小
if adjacent_node in open_list:
index = open_list.index(adjacent_node)
if open_list[index].g > adjacent_node.g:
open_list[index].g = adjacent_node.g
open_list[index].parent = current_node
open_list[index].f = open_list[index].g + open_list[index].h
else:
# 如果相邻节点不在 open_list 中,则加入 open_list
open_list.append(adjacent_node)
return path
```
最后,可以使用如下代码来实现从起点到终点经过所有路标点的最短路径,并打印出每一个坐标:
```python
start_cor = (19, 0)
end_cor = (1, 20)
waypoints = [(5, 15), (5, 1), (9, 3), (11, 17), (7, 19), (15, 19), (13, 1), (15, 5)]
# 定义计算两个点之间距离的函数
def distance(_from, _to):
x1, y1 = _from
x2, y2 = _to
dist = abs(x1 - x2) + abs(y1 - y2)
return dist
# 计算从起点到终点经过所有路标点的最短路径
path = [start_cor]
for i in range(len(waypoints)):
start = path[-1]
end = waypoints[i]
subpath = astar(start, end, [])
path += subpath[1:]
subpath = astar(path[-1], end_cor, [])
path += subpath[1:]
# 打印出每一个坐标
for i in range(len(path)):
print(path[i])
```
相关推荐
![rar](https://img-home.csdnimg.cn/images/20210720083606.png)
![zip](https://img-home.csdnimg.cn/images/20210720083736.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![rar](https://img-home.csdnimg.cn/images/20210720083606.png)