def freeprob_turn_line(self, p1, a1, p2, view, starttime):
"""Free probability for turning and then walking on a straight line"""
a2 = (p2 - p1).angle()
dt = abs(angle_diff(a1, a2)) / self.turningspeed
free = self.freeprob_turn(p1, a1, a2, view, starttime)
free *= self.freeprob_line(p1, p2, view, starttime + dt)
return free
def freeprob_turn(self, position, a1, a2, view, starttime):
""" Probability that a turn started at starttime at position between angles a1 and a2 will be collision free """
dur = abs(angle_diff(a1, a2)) / self.turningspeed
for p in view.pedestrians:
p1 = p.position
p2 = self.future_position(p, dur) # extrapolate
if linesegdist2(p1, p2, position) < (self.radius + p.radius + self.FREEMARGIN) ** 2:
self.freeprob_fail_pedestrian = p
return 0
return 1
def _traversal_time(self, candidate_position, existing_node):
distance = candidate_position.distance_to(existing_node.position)
if existing_node.parent is None:
# existing_node is goal node
#i.e. once it's reached, we don't have to turn
turndist = 0
else:
movement_vector = (existing_node.position - candidate_position)
turndist = abs(angle_diff(movement_vector.angle(), existing_node.angle))
traversal_time = (distance / self.speed) + (turndist / self.turningspeed)
return traversal_time
def test_move(self, p1, a1, p2, view, starttime):
"""Get from a state (a1, p1) to ((p2-p1).angle(), p2)
Returns (traversal_time, safeness)
"""
diff = p2 - p1
a2 = diff.angle()
turningtime = abs(angle_diff(a1, a2)) / self.turningspeed
movetime = diff.length() / self.speed
turn_safeness = self.turn_safeness(p1, a1, a2, view, starttime)
line_safeness = self.line_safeness(
p1, p2, view,
starttime + turningtime
)
return ((turningtime + movetime), turn_safeness * line_safeness)
def turn_safeness(self, position, a1, a2, view, starttime):
""" Probability that a turn is collision free
Started at starttime at position between
angles a1 and a2.
"""
dur = abs(angle_diff(a1, a2)) / self.turningspeed
for p in view.pedestrians:
# extrapolate to start of turn
p1 = self.future_position(p, starttime)
# extrapolate to end of turn
p2 = self.future_position(p, starttime + dur)
safedist2 = (self.radius + p.radius + self.FREEMARGIN) ** 2
if linesegdist2(p1, p2, position) < safedist2:
self.safeness_fail_pedestrian = p
return 0
return 1
def segment_time(self, a1, p1, p2):
"""Returns time to get from a state (a1, p1) to ((p2-p1).angle(), p2)"""
diff = p2 - p1
turningtime = abs(angle_diff(a1, diff.angle())) / self.turningspeed
movetime = diff.length() / self.speed
return turningtime + movetime
