穿透障碍，到达对面的点
import math
import sys
import time
import numpy as np
map_be_search = np.array([
[1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1],
[1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 1],
[1, 0, 0, 0, 1, 1, 1, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 1],
[1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 1, 1, 1, 0, 0, 0, 0, 0, 1],
[1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 1],
[1, 0, 0, 0, 1, 1, 1, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 1],
[1, 0, 0, 0, 1, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0],
[1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 1],
[1, 0, 0, 0, 1, 1, 1, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 1],
[1, 0, 0, 0, 1, 1, 1, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 1],
[1, 0, 0, 1, 1, 1, 1, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 1],
[1, 0, 0, 1, 1, 1, 1, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 1],
[1, 0, 0, 0, 1, 1, 1, 0, 0, 0, 1, 1, 1, 1, 1, 1, 0, 0, 1],
[1, 0, 0, 0, 1, 1, 1, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 1],
[1, 0, 0, 0, 1, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 1],
[1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 1, 0, 0, 0, 0, 0, 0, 0, 1],
[1, 0, 0, 1, 1, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 1, 1, 0, 1],
[1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 1],
[1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1],
]
)
print(map_be_search.shape)
print(map_be_search[18][18])
# 边界
border_x_min = 0
border_x_max = 18
border_y_min = 0
border_y_max = 18
class BStarSearch():
def __init__(self):
# 始点和终点
self.start = {"point": (9, 1), "F": 0, "direct": None,"maker":1}
self.end = {"point": (6, 17),"maker":1}
# 当前点
self.cur = self.start
# 开启的表
self.openSet = []
# 关闭的表
self.closeSet = []
# 边界
self.border_x_min = 0
self.border_x_max = 18
self.border_y_min = 0
self.border_y_max = 18
# 启发式方法 欧里几德距离,计算F的值
def get_F(self, cur):
F = math.sqrt((cur[0] - self.end["point"][0]) ** 2 + (cur[1] - self.end["point"][1]) ** 2)
F = round(F, 2)
return F
# 获取周围八个点的坐标
def get_neighbors(self, x, y):
up = x, y + 1
down = x, y - 1
left = x - 1, y
right = x + 1, y
left_up = x - 1, y + 1
right_up = x + 1, y + 1
left_down = x - 1, y - 1
right_down = x + 1, y - 1
result = [up, down, left, right, left_up, right_up, left_down, right_down]
return result
# 当前点指向下一个点的向量。 这里没有存储为父节点，用父节点指向该点的向量表示，通过与向量的加减也可以得到父节点
def get_two_point_direct(self,cur, next_):
direct = (next_[0] - cur[0]), (next_[1] - cur[1])
return direct
# 当前点指向终点的向量。 四个方向 通过斜率相近 得到方向向量（1,0），（-1,0）（0，-1）（0,1）
def get_direct(self, cur):
x_sub, y_sub = (self.end["point"][0] - cur[0]), (self.end["point"][1] - cur[1])
if x_sub==0 :
# 可能是 （1,0）或者（-1,0） 所以除以绝对值
return x_sub,int(y_sub/abs(y_sub))
k = y_sub / x_sub
if x_sub > 0:
if k > 1/2:
return (0, 1)
elif -1/2<= k <=1/2:
return (1, 0)
else:
return (0, -1)
else:
if k > 1/2:
return (0, -1)
elif -1/2<= k <=1/2:
return (-1, 0)
else:
return (0, 1)
# 撞墙后 方向 变为相对的 左右方向
def direct_split(self, direct):
if direct[0] == 0:
return (-1, 0), (1, 0)
else:
return (0, -1), (0, -1)
# 返回爬墙路径 和 直线穿透障碍的第一个点,, 传入的是 当前点 {}字典信息 和 前进一步的障碍点坐标元祖 （）
def obstacle_path(self, cur, obstacle_point: tuple):
# 攀爬的终点信息
temp_point=cur["point"]
while True:
# 传进来的点,沿着终点方向，穿透障碍，得到可以探索的第一个点 ：地图内的任意两点连线都不可能穿过地图边界
end_point = temp_point[0] + cur["direct"][0], temp_point[1] + cur["direct"][1]
if map_be_search[end_point[0]][end_point[1]] == 0:
break
temp_point = end_point
end_info = {}
end_info["point"] = end_point
# end_info 的direct 需要攀爬过后才能知道
# 开始爬墙，先 把障碍周围所有可走的点加入 self.openSet
self.obstacle_around_openSet(obstacle_point)
#----------------第一轮 攀爬-----------
l1=len(self.closeSet)
# 开启的表,
start = cur
openSet = [start]
while openSet != []:
cur=openSet.pop()
self.closeSet.append(cur)
map_be_search[cur["point"][0]][cur["point"][1]] = 5
print(map_be_search)
time.sleep(0.5)
# 当前点已经是 穿透后的点了，
if cur["point"] == end_point:
break
# 对当前格相邻的8格中的每一个
neighbors = self.get_neighbors(cur["point"][0], cur["point"][1])
next_point_info = {}
for neighbor in neighbors:
# 该邻居 在障碍周围的可探索点中。 并且没有关闭（走过），则下一个点就是它
if neighbor in self.openSet and neighbor not in [p["point"] for p in self.closeSet]:
next_point_info["point"] = neighbor
next_point_info["direct"] = self.get_two_point_direct(cur["point"], neighbor)
# 但是对于最初的一个点来说，它的邻居 左右有两个点是符合的，这里打断一下只取一个。所以初始开放列表是 两个一样的，
break
if next_point_info:
openSet.append(next_point_info)
# ------------第二轮攀爬--------------
l2=len(self.closeSet)
openSet = [start]
while openSet != []:
cur=openSet.pop()
self.closeSet.append(cur)
map_be_search[cur["point"][0]][cur["point"][1]] = 5
print(map_be_search)
time.sleep(0.5)
# 当前点已经是 穿透后的点了，
if cur["point"] == end_point:
break
# 对当前格相邻的8格中的每一个
neighbors = self.get_neighbors(cur["point"][0], cur["point"][1])
next_point_info = {}
for neighbor in neighbors:
# 在障碍周围的可探索点中。 并且没有关闭，则下一个点就是它
if neighbor in self.openSet and neighbor not in [p["point"] for p in self.closeSet]:
next_point_info["point"] = neighbor
next_point_info["direct"] = self.get_two_point_direct(cur["point"], neighbor)
# 但是对于最初的一个点来说，它的邻居 左右有两个点是符合的，这里打断一下只取一个。所以初始开放列表是 两个一样的，
break
if next_point_info:
openSet.append(next_point_info)
# ，，如果只有一条路径到达，那么探索过 end_point点只有一个，否则有两个相同的 end_point，但是他们的父节点不一样，回溯路径是不好处理，
l3=len(self.closeSet)
last_index=[self.closeSet.index(p) for p in self.closeSet if p["point"]==end_point]
if last_index==[]:
return 0
else:
if len(last_index)==1:
return self.closeSet[last_index[0]]
# ----通过两轮攀爬的 路径长度，， 舍去其中一个 end_point,留下一个即可
if l2-l1
