def is_intersect_circle(self, o, d, a, r):
d2 = np.dot(d, d)
delta = self.delta
if d2 == 0:
return False
t = np.dot([a[0] - o[0], a[1] - o[1]], d) / d2
if 0 <= t <= 1:
shot = Node((o[0] + t * d[0], o[1] + t * d[1]))
if self.get_dist(shot, Node(a)) <= r + delta:
return True
return False
def is_intersect_rec(self, start, end, o, d, a, b):
# o d 为偏差向量的起点与偏差向量
# a b 为障碍物线段的两端点
v1 = [o[0] - a[0], o[1] - a[1]]
v2 = [b[0] - a[0], b[1] - a[1]]
v3 = [-d[1], d[0]]
div = np.dot(v2, v3)
# 平行
if div == 0:
return False
#
t1 = np.linalg.norm(np.cross(v2, v1)) / div
t2 = np.dot(v1, v3) / div
if t1 >= 0 and 0 <= t2 <= 1:
shot = Node((o[0] + t1 * d[0], o[1] + t1 * d[1]))
dist_obs = self.get_dist(start, shot)
dist_seg = self.get_dist(start, end)
if dist_obs <= dist_seg:
return True
return False