def free_path(p1, p2, view, safe_distance=0):
"""Returns if a path is free to traverse assuming that the view is complete and static"""
for p in view.pedestrians:
safedistsquare = (safe_distance + p.radius) ** 2
if linesegdist2(p1, p2, p.position) <= safedistsquare:
return False
safedistsquare = safe_distance ** 2
for o in view.obstacles:
if line_distance2(p1, p2, o.p1, o.p2) <= safedistsquare:
return False
return True
def goal_occupied(self, view):
"""Returns if goal is currently occupied by obstacle/unit that isn't moving
Convenience method that can be used to fallback to a nearby goal or similar
"""
for line in view.obstacles:
if linesegdist2(line.p1, line.p2, self.goal) < self.radius ** 2:
return True
for p in view.pedestrians:
if p.velocity.length2() == 0.0:
if p.position.distance_to2(self.goal) < p.radius:
return True
return False
