def test_find_mean_angle_wrap_around_2(self):
angles = [354, 5]
mean_angle = geometry.find_mean_angle(angles, 11)
expected_mean = 359.5
self.assertAlmostEqual(mean_angle, expected_mean, 1)
def test_find_mean_angle_big_difference(self):
angles = [161.59438184601854, 246.7735850288153]
mean_angle = geometry.find_mean_angle(angles, 179)
self.assertAlmostEqual(mean_angle, 204.2, 1)
def test_find_mean_angle(self):
angles = [299.19, 305.42]
mean_angle = geometry.find_mean_angle(angles, 8)
expected_mean = (299.19 + 305.42) / 2
self.assertAlmostEqual(mean_angle, expected_mean, 1)
def approximate_position_direction_speed(
self, minimum_data_points_used) -> (Coordinate, int, int):
if self.projection is not None:
return self.projection
if len(self.position_history.list) <= 1:
return None, None, None # No information can be deduced about movement of ball
history = self.position_history.list
time_1 = history[0][1]
time_2 = history[1][1]
c1: Coordinate = history[0][0]
c2: Coordinate = history[1][0]
first_coord = c1
last_coord = c2
if time_1 == time_2 or c1.euclidean_distance_from(
c2) < 0.1 or c1.euclidean_distance_from(c2) > 4.2:
return c1, 0, 0
final_speed = (c1.euclidean_distance_from(c2) /
(time_1 - time_2)) * BALL_DECAY
final_direction = degrees(calculate_full_origin_angle_radians(c1, c2))
angles = [final_direction] # Used for calculating 'average' angle
max_deviation = 50 # angle deviation
max_speed_deviation = 0.8
age = time_1 - time_2
max_age = 20
def allowed_angle_deviation(index):
return 90 if index == 0 else max_deviation
previous_dist = final_speed
data_points_used = 2
for i, pos_and_time in enumerate(islice(history, 2, len(history))):
c1 = c2
c2 = pos_and_time[0]
time_1 = time_2
time_2 = pos_and_time[1]
age += time_1 - time_2
dist = c1.euclidean_distance_from(c2)
if time_1 == time_2 or dist <= 0.05 or (
dist < 0.3 and previous_dist < 0.3) or dist > 4.2:
break
previous_dist = dist
# calculate angle from point observed 2 ticks prior
direction = degrees(
calculate_full_origin_angle_radians(history[i][0], c2))
direction_similar = is_angle_in_range(
direction,
(final_direction - allowed_angle_deviation(i)) % 360,
(final_direction + allowed_angle_deviation(i)) % 360)
speed = (dist / (time_1 - time_2)) * pow(BALL_DECAY, age)
speed_similar = (final_speed - max_speed_deviation) <= speed <= (
final_speed + max_speed_deviation)
if direction_similar and speed_similar and age < max_age:
data_points_used += 1
last_coord = c2
angles.append(
degrees(
calculate_full_origin_angle_radians(first_coord, c2)))
final_speed = (final_speed * age + speed) / (
age + 1) # calculate average with new value
final_direction = find_mean_angle(angles, 179)
else:
debug_msg(
"Previous points did not match. Speed : " + str(speed) +
"vs." + str(final_speed) + "| Direction :" +
str(direction) + "vs." + str(final_direction) + "| age: " +
str(age) + str(c1) + str(c2), "POSITIONAL")
break # This vector did not fit projection, so no more history is used in the projection
if data_points_used < minimum_data_points_used:
return None, None, None
debug_msg(
"Prediction based on {0} of these data points: {1}".format(
data_points_used, self.position_history), "INTERCEPTION")
direction = degrees(
calculate_full_origin_angle_radians(first_coord, last_coord))
self.projection = self.position_history.list[0][
0], direction, final_speed
return self.position_history.list[0][0], direction, final_speed