def get_intersection(first_point, first_direction, second_point, second_direction): a1 = first_direction.y / first_direction.x b1 = first_point.y - a1 * first_point.x a2 = second_direction.y / second_direction.x b2 = second_point.y - a2 * second_point.x x = (b2 - b1) / (a1 - a2) return Point(x, a1 * x + b1)
def join_walls(walls: list) -> list: joined_walls = [] cur_wall = walls.pop(0) while len(walls): wall = walls.pop(0) diff = get_angles_diff(get_angle(cur_wall.direction), get_angle(wall.direction)) cur_wall_angle_variance = get_angle(Point(cur_wall.dist, 0.01)) wall_angle_variance = get_angle(Point(wall.dist, 0.01)) if cur_wall.begin_point == cur_wall.end_point or wall.begin_point == wall.end_point or diff < ( cur_wall_angle_variance + wall_angle_variance) * 2: cur_wall = Wall(cur_wall.begin_point, wall.end_point) else: joined_walls.append(cur_wall) cur_wall = wall joined_walls.append(cur_wall) return joined_walls
def interpret_points_sequence(points): if len(points) == 0: return [] if len(points) < 3: return [Wall(points[0], points[-1])] middle_index = len(points) // 2 begin_point = points[0] middle_point = points[middle_index] end_point = points[-1] first_part_vector = middle_point - begin_point second_part_vector = end_point - middle_point first_part_variance = get_angle(Point(first_part_vector.dist, 0.01)) second_part_variance = get_angle(Point(second_part_vector.dist, 0.01)) diff = get_angles_diff(get_angle(first_part_vector), get_angle(second_part_vector)) if abs(diff) > first_part_variance + second_part_variance: first_part_walls = interpret_points_sequence(points[:middle_index]) second_part_walls = interpret_points_sequence(points[middle_index:]) return first_part_walls + second_part_walls return [Wall(begin_point, end_point)]
def __verify_intersections(self, next_point: Point, best_wall: Wall, walls: list): for wall in walls: intersection = get_intersection(Point.zero(), best_wall.direction.normalized, wall.begin_point, wall.direction) angles_diff = get_angles_diff(get_angle(intersection), get_angle(next_point)) if self.__is_safe_spot( next_point, intersection ) and angles_diff < 45 and wall.intersect(next_point): return self.__find_next_point( wall, [w for w in walls if w != best_wall]) return next_point
def closer_point_from_origin(self) -> Point: closer_point_on_line = get_intersection( Point.zero(), rotate_vector(self.direction, 90), self.begin_point, self.direction) closer_point_diff = closer_point_on_line - self.begin_point angles_diff = round( abs( abs(get_angle(self.direction)) - abs(get_angle(closer_point_diff.normalized))), 4) if closer_point_diff.dist < 0 or closer_point_diff.dist > self.dist or angles_diff != 0: if self.begin_point.dist < self.end_point.dist: return self.begin_point else: return self.end_point return closer_point_on_line
def get_points_sequences(ranges: DataBuffer) -> list: sequences = [] cur_suit = [] for index, r in enumerate(ranges): if r > 12: if len(cur_suit): sequences.append(cur_suit) cur_suit = [] continue position = get_position(index, r) cur_suit.append((index, Point(position.x, position.y))) if len(cur_suit): if len(sequences) and sequences[0][0][0] == 0: sequences[0] = cur_suit + sequences[0] else: sequences.append(cur_suit) return [[t[1] for t in s] for s in sequences]
def __reset_location_and_rotation(self): self._location = Point.zero() self._rotation = 0.0
def __state_test_moving(self): self.__reset_location_and_rotation() self._target_location = Point.up() * 4 self._target_rotation = math.radians(get_angle(self._target_location)) self._cur_state = self.STATE_MOVE_TO_WALL
def rotation_vector_to_quaternion(direction_vector: Point) -> Point: angle = (math.atan2(direction_vector.x, direction_vector.y) + ((3 * math.pi) / 2)) return Point(math.sin(angle / 2), -math.cos(angle / 2))
def get_position(angle, dist): x = math.cos(math.radians(angle)) * dist y = math.sin(math.radians(angle)) * dist return Point(x, y)