def collide_fixed_cars_with_pose(self, pose):
agent_center = pose[:2]
verts = self.get_rect_verts(pose[:2], self.car_length, self.car_width,
pose[2])
agent_center_to_car1_center = np.linalg.norm(agent_center -
self.car1_center)
agent_center_to_car2_center = np.linalg.norm(agent_center -
self.car2_center)
car1_collision = False
car2_collision = False
if agent_center_to_car1_center > self.car_diagonal_length:
# in this case, agent is not possible to collide with car1
car1_collision = False
else:
car1_collision = tools.two_rects_intersect(verts, self.car1_verts)
if not car1_collision:
if agent_center_to_car2_center > self.car_diagonal_length:
car2_collision = False
else:
car2_collision = tools.two_rects_intersect(
verts, self.car2_verts)
if car1_collision or car2_collision:
return True
else:
return False
def collide_fixed_cars(self):
self.lock.acquire()
agent_center_to_car1_center = np.linalg.norm(self.agent_center -
self.car1_center)
agent_center_to_car2_center = np.linalg.norm(self.agent_center -
self.car2_center)
car1_collision = False
car2_collision = False
if agent_center_to_car1_center > self.car_diagonal_length:
# in this case, agent is not possible to collide with car1
car1_collision = False
else:
car1_collision = tools.two_rects_intersect(self.agent_verts,
self.car1_verts)
if not car1_collision:
if agent_center_to_car2_center > self.car_diagonal_length:
car2_collision = False
else:
car2_collision = tools.two_rects_intersect(
self.agent_verts, self.car2_verts)
self.lock.release()
if car1_collision or car2_collision:
return True
else:
return False
def collide_walls(self):
self.lock.acquire()
# agent_center_to_wall_center = np.linalg.norm(self.agent_center - self.wall_center)
# if agent_center_to_wall_center < self.wall_edge_length / 2.0 - self.car_length / 2:
# self.lock.release()
# return False
# else:
# wall_collision = tools.two_rects_intersect(self.agent_verts, self.wall_verts)
# out_of_wall = False
# if self.agent_center[0] > self.wall_verts[0,0] and self.agent_center[0] < self.wall_verts[1,0] \
# and self.agent_center[1] > self.wall_verts[2,1] and self.agent_center[1] < self.wall_verts[1,1]:
# out_of_wall = False
# else:
# out_of_wall = True
# self.lock.release()
# return wall_collision or out_of_wall
wall_collision = tools.two_rects_intersect(self.agent_verts,
self.wall_verts)
out_of_wall = False
if self.agent_center[0] > self.wall_verts[0, 0] and self.agent_center[0] < self.wall_verts[1, 0] \
and self.agent_center[1] > self.wall_verts[2, 1] and self.agent_center[1] < self.wall_verts[1, 1]:
out_of_wall = False
else:
out_of_wall = True
self.lock.release()
return wall_collision or out_of_wall
def collide_walls_with_pose(self, pose):
verts = self.get_rect_verts(pose[:2], self.car_length, self.car_width, pose[2])
wall_collision = tools.two_rects_intersect(verts, self.wall_verts)
out_of_wall = False
if pose[0] > self.wall_verts[0, 0] and pose[0] < self.wall_verts[1, 0] \
and pose[1] > self.wall_verts[2, 1] and pose[1] < self.wall_verts[1, 1]:
out_of_wall = False
else:
out_of_wall = True
return wall_collision or out_of_wall
