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