def test_rotated_rectangles_intersect(): assert rotated_rectangles_intersect( ([12.86076812, 28.60182391], 5.0, 2.0, -0.4675779906495494), ([9.67753944, 28.90585412], 5.0, 2.0, -0.3417019364473201)) assert rotated_rectangles_intersect(([0, 0], 2, 1, 0), ([0, 1], 2, 1, 0)) assert not rotated_rectangles_intersect(([0, 0], 2, 1, 0), ([0, 2.1], 2, 1, 0)) assert not rotated_rectangles_intersect(([0, 0], 2, 1, 0), ([1, 1.1], 2, 1, 0)) assert rotated_rectangles_intersect(([0, 0], 2, 1, np.pi / 4), ([1, 1.1], 2, 1, 0))
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().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.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.velocity = other.velocity = min(self.velocity, other.velocity) self.crashed = other.crashed = True
def _is_colliding(self, other): # Fast spherical pre-check if np.linalg.norm(other.position - self.position) > self.LENGTH: return False # Accurate rectangular check return utils.rotated_rectangles_intersect((self.position, 0.9*self.LENGTH, 0.9*self.WIDTH, self.heading), (other.position, 0.9*other.LENGTH, 0.9*other.WIDTH, other.heading))
def single_check_collision(self, ego_pos, ego_heading, other_pos, other_heading): LENGTH = 5.0/self.range WIDTH = 2.0/self.range ego_crashed = 0 if np.linalg.norm(other_pos - ego_pos) <= LENGTH: if rotated_rectangles_intersect((ego_pos, LENGTH, WIDTH, ego_heading), (other_pos, LENGTH, WIDTH, other_heading)): ego_crashed = 1 return ego_crashed
def is_conflict_possible(v1, v2, horizon=3, step=0.25): times = np.arange(step, horizon, step) positions_1, headings_1 = v1.predict_trajectory_constant_velocity(times) positions_2, headings_2 = v2.predict_trajectory_constant_velocity(times) for position_1, heading_1, position_2, heading_2 in zip(positions_1, headings_1, positions_2, headings_2): # Fast spherical pre-check if np.linalg.norm(position_2 - position_1) > v1.LENGTH: continue # Accurate rectangular check if utils.rotated_rectangles_intersect((position_1, 1.5*v1.LENGTH, 0.9*v1.WIDTH, heading_1), (position_2, 1.5*v2.LENGTH, 0.9*v2.WIDTH, heading_2)): return True
def check_collision_flag(self, other): # Delete the self.crashed to enable double check if not self.COLLISIONS_ENABLED or not other.COLLISIONS_ENABLED or other is self: return False # Fast spherical pre-check if np.linalg.norm(other.position - self.position) > self.LENGTH: return False # Accurate rectangular check if utils.rotated_rectangles_intersect((self.position, 0.9*self.LENGTH, 0.9*self.WIDTH, self.heading), (other.position, 0.9*other.LENGTH, 0.9*other.WIDTH, other.heading)): #self.velocity = other.velocity = min(self.velocity, other.velocity) #self.crashed = other.crashed = True return True else: return False
def check_collision(self, other): """ Check for collision with another vehicle. :param other: the other vehicle """ if not self.COLLISIONS_ENABLED or not other.COLLISIONS_ENABLED or self.crashed or other is self: return # Fast spherical pre-check if np.linalg.norm(other.position - self.position) > self.LENGTH: return # Accurate rectangular check if utils.rotated_rectangles_intersect((self.position, 0.9*self.LENGTH, 0.9*self.WIDTH, self.heading), (other.position, 0.9*other.LENGTH, 0.9*other.WIDTH, other.heading)): self.velocity = other.velocity = min([self.velocity, other.velocity], key=abs) self.crashed = other.crashed = True
def find_initial_impact(v1: "Vehicle" = None, v2: "Vehicle" = None) -> np.array: """ Uses shooter method to find point of initial contact backwards in time """ # first find time in past where cars don't intersect c = utils.rotated_rectangles_intersect( (v1.position, v1.LENGTH, v1.WIDTH, v1.heading), (v2.position, v2.LENGTH, v2.WIDTH, v2.heading)) v1pos = v1.position.copy() v2pos = v2.position.copy() t = .25 while c and t < 1.0: t += .25 v1pos[0] = v1.position[0] - v1.velocity[0] * t v1pos[1] = v1.position[1] - v1.velocity[1] * t v2pos[0] = v2.position[0] - v2.velocity[0] * t v2pos[1] = v2.position[1] - v2.velocity[1] * t c = utils.rotated_rectangles_intersect( (v1pos, v1.LENGTH, v1.WIDTH, v1.heading), (v2pos, v2.LENGTH, v2.WIDTH, v2.heading)) t1 = 0 t2 = t if utils.rotated_rectangles_intersect( (v1.position, v1.LENGTH, v1.WIDTH, v1.heading), (v2.position, v2.LENGTH, v2.WIDTH, v2.heading)): # Shooter method to find exactly when the cars hit while t2 - t1 > 10E-8: t = (t2 + t1) / 2 v1pos[0] = v1.position[0] - v1.velocity[0] * t v1pos[1] = v1.position[1] - v1.velocity[1] * t v2pos[0] = v2.position[0] - v2.velocity[0] * t v2pos[1] = v2.position[1] - v2.velocity[1] * t if utils.rotated_rectangles_intersect( (v1pos, v1.LENGTH, v1.WIDTH, v1.heading), (v2pos, v2.LENGTH, v2.WIDTH, v2.heading)): t1 = t else: t2 = t v1pos[0] = v1.position[0] - v1.velocity[0] * t1 v1pos[1] = v1.position[1] - v1.velocity[1] * t1 v2pos[0] = v2.position[0] - v2.velocity[0] * t1 v2pos[1] = v2.position[1] - v2.velocity[1] * t1 ncorner = 0. corner_avg = np.array([0., 0.]) corners1Flag = False corners2Flag = False vpos1tmp = v1.position.copy() v1.position = v1pos.copy() vpos2tmp = v2.position.copy() v2.position = v2pos.copy() # Determine which corner if utils.has_corner_inside((v1pos, v1.LENGTH, v1.WIDTH, v1.heading), (v2pos, v2.LENGTH, v2.WIDTH, v2.heading)): corners1Flag = True #V1 insind V2 corners1 = corner_positions(v1) for i in range(4): if utils.point_in_rotated_rectangle(corners1[i, :], v2pos, v2.LENGTH * 1.1, v2.WIDTH * 1.1, v2.heading): corner_avg[0] += corners1[i, 0] corner_avg[1] += corners1[i, 1] ncorner += 1. if utils.has_corner_inside((v2pos, v2.LENGTH, v2.WIDTH, v2.heading), (v1pos, v1.LENGTH, v1.WIDTH, v1.heading)): corners2Flag = True #V2 insind V1 corners2 = corner_positions(v2) for i in range(4): if utils.point_in_rotated_rectangle(corners2[i, :], v1pos, v1.LENGTH * 1.1, v1.WIDTH * 1.1, v1.heading): corner_avg[0] += corners2[i, 0] corner_avg[1] += corners2[i, 1] ncorner += 1. v1.position = vpos1tmp.copy() v2.position = vpos2tmp.copy() if ncorner < 1.: if corners1Flag: indx1 = np.argmin( np.linalg.norm(corners1 - np.array(v2pos), axis=1)) cVal1 = np.linalg.norm(corners1[indx1, :] - np.array(v2pos)) else: cVal1 = 10000. if corners2Flag: indx2 = np.argmin( np.linalg.norm(corners2 - np.array(v1pos), axis=1)) cVal2 = np.linalg.norm(corners2[indx2, :] - np.array(v1pos)) else: cVal2 = 10000. if cVal1 < cVal2: return corners1[indx1, :] elif cVal2 < cVal1: return corners2[indx2, :] else: return np.array([0., 0.]) corner_avg[0] /= ncorner corner_avg[1] /= ncorner return corner_avg else: return np.array([0.0, 0.0])
def _is_colliding(self, other): # Fast spherical pre-check if np.linalg.norm(other.position - self.position) > self.LENGTH: return False # Accurate point-inside checks c = 0 self.got_crashed = 0 self.did_crash = 0 other.got_crashed = 0 other.did_crash = 0 #if utils.point_in_rotated_rectangle(self.position, other.position, 0.9*other.LENGTH, 0.9*other.WIDTH, other.heading) or utils.point_in_rotated_rectangle(other.position, self.position, 0.9*self.LENGTH, 0.9*self.WIDTH, self.heading): if utils.rotated_rectangles_intersect( (self.position, self.LENGTH * .9, self.WIDTH * .9, self.heading), (other.position, other.LENGTH * .9, other.WIDTH * .9, other.heading)): # Determine who hit who (striker is the one with the smallest angle between the line connecting the two centers and their heading) POI = find_initial_impact(self, other) pos_self = np.array(self.position) pos_other = np.array(other.position) CenterVectorSelf = (POI - pos_self) / np.linalg.norm(POI - pos_self) CenterVectorOther = (POI - pos_other) / np.linalg.norm(POI - pos_other) if self.speed != 0.0: SelfHVec = np.array(self.velocity, dtype=np.float32) / np.fabs( 1.0 * self.speed) else: SelfHVec = np.array([0, 0], dtype=np.float32) SelfHVec[0] += CenterVectorSelf[1] SelfHVec[1] += -1.0 * CenterVectorSelf[0] if other.speed != 0.0: OtherHVec = np.array(other.velocity, dtype=np.float32) / np.fabs( 1.0 * other.speed) else: OtherHVec = np.array([0, 0], dtype=np.float32) OtherHVec[0] += CenterVectorOther[1] OtherHVec[1] += -1.0 * CenterVectorOther[0] SelfCosAlpha = np.fabs(SelfHVec[0] * CenterVectorSelf[0] + SelfHVec[1] * CenterVectorSelf[1]) OtherCosAlpha = np.fabs( OtherHVec[0] * CenterVectorOther[0] + OtherHVec[1] * CenterVectorOther[1] ) #minus because the vector connecting two cars is pointed towards the "other" car if SelfCosAlpha > OtherCosAlpha: print(self, " hit ", other, "at ", POI) self.did_crash = 1 other.got_crashed = 1 self.crash_angle = (self.heading - other.heading) other.crash_angle = self.crash_angle self.crash_speed2 = np.sum( np.multiply(self.velocity - other.velocity, self.velocity - other.velocity)) other.crash_speed2 = self.crash_speed2 c = 1 elif OtherCosAlpha > SelfCosAlpha: print(self, " was hit by ", other, "at ", POI) self.got_crashed = 1 other.did_crash = 1 self.crash_angle = (self.heading - other.heading) other.crash_angle = self.crash_angle self.crash_speed2 = np.sum( np.multiply(self.velocity - other.velocity, self.velocity - other.velocity)) other.crash_speed2 = self.crash_speed2 c = 1 else: print("Double Collision, both lose") self.got_crashed = 1 other.got_crashed = 1 self.crash_angle = (self.heading - other.heading) other.crash_angle = self.crash_angle self.crash_speed2 = np.sum( np.multiply(self.velocity - other.velocity, self.velocity - other.velocity)) other.crash_speed2 = self.crash_speed2 c = 1 return c