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)