def handle_collisions(self, other: 'RoadObject', dt: float) -> None:
"""
Worst-case collision check.
For robust planning, we assume that MDPVehicles collide with the uncertainty set of an IntervalVehicle,
which corresponds to worst-case outcome.
:param other: the other vehicle
:param dt: a timestep
"""
if not isinstance(other, MDPVehicle):
super().handle_collisions(other)
return
if not self.collidable or self.crashed or other is self:
return
# Fast rectangular pre-check
if not utils.point_in_rectangle(other.position,
self.interval.position[0] - self.LENGTH,
self.interval.position[1] + self.LENGTH):
return
# Projection of other vehicle to uncertainty rectangle. This is the possible position of this vehicle which is
# the most likely to collide with other vehicle
projection = np.minimum(np.maximum(other.position, self.interval.position[0]),
self.interval.position[1])
# Accurate rectangular check
if utils.rotated_rectangles_intersect((projection, self.LENGTH, self.WIDTH, self.heading),
(other.position, 0.9*other.LENGTH, 0.9*other.WIDTH, other.heading)):
self.speed = other.speed = min(self.speed, other.speed)
self.crashed = other.crashed = True
def check_collision(self, other):
"""
For robust planning, we assume that MDPVehicles collide with the uncertainty set of an IntervalVehicle,
which corresponds to worst-case outcome.
:param other: the other vehicle
"""
if not isinstance(other, MDPVehicle):
return super(IntervalVehicle, self).check_collision(other)
if not self.COLLISIONS_ENABLED or self.crashed or other is self:
return
# Fast rectangular pre-check
if not utils.point_in_rectangle(other.position,
self.interval_observer.position[0]-self.LENGTH,
self.interval_observer.position[1]+self.LENGTH):
return
# Projection of other vehicle to uncertainty rectangle. This is the possible position of this vehicle which is
# the most likely to collide with other vehicle
projection = np.minimum(np.maximum(other.position, self.interval_observer.position[0]),
self.interval_observer.position[1])
# Accurate elliptic check
if utils.point_in_ellipse(other.position, projection, self.heading, self.LENGTH, self.WIDTH):
self.velocity = other.velocity = min(self.velocity, other.velocity)
self.crashed = other.crashed = True