def move_the_rocket(self):
timestep = self.parameters.time.timestep
rocket_hist = self.history.rocket
rocket_params = self.parameters.rocket
vel = self.physics.calc_rocket_velocity()
acc_relative = self.parameters.rocket.calc_acc_relative2rocket(
rocket_hist.engine_forces
)
acc = acc_relative.rotate_by(rocket_hist.angle - math.pi / 2)
updated_vel = vel + acc * timestep
rocket_hist.locations.append(rocket_hist.location + updated_vel * timestep)
angular_vel = self.physics.calc_rocket_angular_velocity()
angular_acc = rocket_params.calc_angular_acc(rocket_hist.thruster_forces)
updated_angular_vel = angular_vel + angular_acc * timestep
updated_angle = normalise_angle(
rocket_hist.angle + updated_angular_vel * timestep
)
rocket_hist.angles.append(updated_angle)
def _calc_single_obstacle_shadow_attraction(
self, obstacle: Coordinate) -> Coordinate:
# TODO: Make this more efficient by storing it between cycles
# TODO: Deal with perfectly vertical gradients
angle_turret2obstacle = (obstacle.location -
self.parameters.turret.location).angle
edge_gradients = self.calc_shadow_edge_gradients(obstacle)
shadow_attractions = []
rocket_location = self.history.rocket.location
for gradient in edge_gradients:
y_intercept = Line.calculate_y_intercept(
gradient, self.parameters.turret.location)
shadow_edge_line = Line(gradient, y_intercept)
closest_point = shadow_edge_line.closest_point_on_line(
rocket_location)
strength = 1 / rocket_location.distance2(closest_point)
angle_turret2closest_point = (
closest_point - self.parameters.turret.location).angle
if normalise_angle(angle_turret2closest_point -
angle_turret2obstacle) > 0:
rotation = -1
else:
rotation = 1
direction_angle = angle_turret2closest_point + rotation * math.pi / 2
shadow_attractions.append(
PolarCoordinate(strength, direction_angle).pol2cart())
return sum(shadow_attractions)
def _calc_projectile_path_avoidance(self):
rocket_location = self.history.rocket.location
turret_location = self.parameters.turret.location
projectile_path_avoidance = []
for projectile in self.history.active_projectiles:
gradient = math.tan(projectile.firing_angle)
y_intercept = turret_location.y - gradient * turret_location.x
y_value = gradient * rocket_location.x + y_intercept
avoidance_angle = normalise_angle(
projectile.firing_angle +
(1 if rocket_location.y > y_value else -1) * math.pi / 2)
min_dist_rocket2path = self.calc_minimum_distance_from_location2line(
rocket_location, gradient, y_intercept)
projectile_location = self.helpers.calc_projectile_location(
projectile)
dist_rockt2projectile = rocket_location.distance2(
projectile_location)
try:
avoidance_strength = 1 / (
min_dist_rocket2path *
(dist_rockt2projectile -
self.parameters.rocket.target_radius))
except ZeroDivisionError:
avoidance_strength = float("inf")
projectile_path_avoidance.append(
PolarCoordinate(avoidance_strength,
avoidance_angle).pol2cart())
return (average(projectile_path_avoidance)
if projectile_path_avoidance else Coordinate(0.0, 0.0))
def firing_angle2hit_rocket(self) -> Optional[float]:
"""
Calculates the firing angle to hit the rocket.
If the turret was currently at said angle and fired a projectile, and the rocket continued at its current velocity,
then the projectile would hit the target.
https://math.stackexchange.com/questions/213545/solving-trigonometric-equations-of-the-form-a-sin-x-b-cos-x-c
return:
firing angle (rad): The angle as described above
Will return None if no firing angle is possible due to high rocket velocity
"""
projectile_speed = self.parameters.turret.projectile_speed
turret_location = self.parameters.turret.location
rocket_velocity = self.physics.calc_rocket_velocity()
rocket_location = self.history.rocket.location
try:
k = (turret_location.y - rocket_location.y) / (turret_location.x -
rocket_location.x)
except ZeroDivisionError: # k = inf
try:
m = math.asin(rocket_velocity.x / projectile_speed)
except ValueError: # Intercept is not possible due to rocket velocity
return None
beta = math.pi / 2
else:
try:
a = -projectile_speed
b = k * projectile_speed
c = k * rocket_velocity.x - rocket_velocity.y
m = math.asin(c / math.sqrt(a**2 + b**2))
except ValueError: # Intercept is not possible due to rocket velocity
return None
A = a / math.sqrt(a**2 + b**2)
B = b / math.sqrt(a**2 + b**2)
beta = math.atan2(B, A)
firing_angle = normalise_angle(m - beta)
if self.will_firing_angle_hit(firing_angle):
return firing_angle
return normalise_angle(math.pi - m - beta)
def rotate_the_turret(self):
rotation_velocity = self.history.turret.rotation_velocity
timestep = self.parameters.time.timestep
d_theta = rotation_velocity * timestep
angles = self.history.turret.angles
angles.append(normalise_angle(angles[-1] + d_theta))
def _plot_engine_thrust(
self, force, projection_location, relative_engine_angle, engine_plot
):
rocket_angle = self.history.rocket.angle
projection_angle = normalise_angle(rocket_angle + relative_engine_angle)
max_main_engine_force = self.parameters.rocket.max_main_engine_force
rocket_length = self.parameters.rocket.length
thrust_ratio = force / max_main_engine_force
edge_length = (
self.visual.rocket_length_max_thrust_length_ratio
* rocket_length
* thrust_ratio
)
left_angle = normalise_angle(projection_angle + self.visual.thrust_cone_angle)
relative_thrust_left_location = PolarCoordinate(
edge_length, left_angle
).pol2cart()
thrust_left_location = projection_location + relative_thrust_left_location
right_angle = normalise_angle(projection_angle - self.visual.thrust_cone_angle)
relative_thrust_right_location = PolarCoordinate(
edge_length, right_angle
).pol2cart()
thrust_right_location = projection_location + relative_thrust_right_location
engine_plot.set_data(
[
projection_location.x,
thrust_left_location.x,
thrust_right_location.x,
projection_location.x,
],
[
projection_location.y,
thrust_left_location.y,
thrust_right_location.y,
projection_location.y,
],
)
def _calc_rotation_factor(self):
"""1 if aligned with intercept angle, 0 if pointing in the opposite direction"""
intercept_angle = self.controller_helpers.firing_angle2hit_rocket()
if intercept_angle is None: # Moving too fast to hit
return 0.0
angle_turret_barrel2rocket = normalise_angle(intercept_angle -
self.history.turret.angle)
return 1 - abs(angle_turret_barrel2rocket) / math.pi
def calc_rotation_velocity(self):
firing_angle = self.controller_helpers.firing_angle2hit_rocket()
angle2rocket = self.calc_angle2rocket()
if firing_angle is None or abs(firing_angle -
angle2rocket) > math.pi / 2:
firing_angle = (
angle2rocket # If firing angle is at > 90 deg, aim for the rocket
)
delta_angle = normalise_angle(firing_angle - self.history.turret.angle)
abs_rotation_speed = min(
self.parameters.turret.max_rotation_speed,
abs(delta_angle) / self.parameters.time.timestep,
) # Take a smaller step if a full move would carry past the rocket
return (1 if delta_angle >= 0 else -1) * abs_rotation_speed
def get_engine_bridge_ends(self):
_, rocket_rear_location = self.get_rocket_ends()
rocket_length = self.parameters.rocket.length
engine_bridge_width = (
self.visual.rocket_length_engine_bridge_width_ratio * rocket_length
)
rocket_angle = self.history.rocket.angle
perpendicular_angle = normalise_angle(rocket_angle + math.pi / 2)
relative_engine_end = PolarCoordinate(
engine_bridge_width / 2, perpendicular_angle
).pol2cart()
engine_bridge_left_location = rocket_rear_location + relative_engine_end
engine_bridge_right_location = rocket_rear_location - relative_engine_end
return engine_bridge_left_location, engine_bridge_right_location
def _calc_firing_path_avoidance(self):
rocket_location = self.history.rocket.location
turret_location = self.parameters.turret.location
recharge_factor = self._calc_recharge_factor()
rotation_factor = self._calc_rotation_factor()
turret_angle = self.history.turret.angle
gradient = math.tan(turret_angle)
y_intercept = turret_location.y - gradient * turret_location.x
y_value = gradient * rocket_location.x + y_intercept
avoidance_angle = normalise_angle(
turret_angle +
(1 if rocket_location.y > y_value else -1) * math.pi / 2)
min_dist_rocket2path = self.calc_minimum_distance_from_location2line(
rocket_location, gradient, y_intercept)
dist_rockt2projectile = (
self.controller_helpers.calc_dist_between_rocket_and_turret())
return PolarCoordinate(
rotation_factor * recharge_factor /
(min_dist_rocket2path * dist_rockt2projectile),
avoidance_angle,
).pol2cart()
def calc_inputs(self):
safety_buffer = 2.0
direction = self._calc_direction(safety_buffer)
# Current velocity
rocket_velocity = self.physics.calc_rocket_velocity()
direction_velocity_ratio = 250.0
thrust_direction = direction_velocity_ratio * direction - rocket_velocity
thrust_angle = thrust_direction.angle
rocket_angle = self.history.rocket.angle
relative_thrust_angle = normalise_angle(thrust_angle - rocket_angle)
# If facing away from the turret, spin the rocket using the thrusters
# Use PD controller
p_c = 0.75
d_c = -0.35
# Derivative
angular_vel = self.physics.calc_rocket_angular_velocity()
control_signal = p_c * relative_thrust_angle + d_c * angular_vel
max_thruster_force = self.parameters.rocket.max_thruster_force
thruster_force = min(
abs(control_signal) * max_thruster_force, max_thruster_force)
clockwise_thrust = thruster_force if control_signal < 0 else 0.0
anticlockwise_thrust = thruster_force if control_signal >= 0 else 0.0
rotational_outputs = [
0.0,
clockwise_thrust,
anticlockwise_thrust,
anticlockwise_thrust,
clockwise_thrust,
]
max_outputs = [self.parameters.rocket.max_main_engine_force
] + [max_thruster_force] * 4
remaining_outputs = [
max_thrust - rotational_thrust for max_thrust, rotational_thrust in
zip(max_outputs, rotational_outputs)
]
if self.is_within_buffer(safety_buffer):
if not math.isclose(relative_thrust_angle, 0) and not math.isclose(
relative_thrust_angle, math.pi):
idx1, idx2 = (3, 4) if relative_thrust_angle > 0 else (1, 2)
min_remaining_thruster_output = min(remaining_outputs[idx1],
remaining_outputs[idx2])
for idx in (idx1, idx2):
rotational_outputs[idx] += min_remaining_thruster_output
if abs(relative_thrust_angle) < 3 * math.pi / 4:
rotational_outputs[
0] = self.parameters.rocket.max_main_engine_force
return rotational_outputs
if abs(relative_thrust_angle) > math.pi / 2:
return rotational_outputs
left_thrusters_active = relative_thrust_angle < 0
right_thrusters_active = relative_thrust_angle >= 0
thrusters_active = [left_thrusters_active
] * 2 + [right_thrusters_active] * 2
engine_translation_force_ratio = [math.cos(relative_thrust_angle)] + [
active * abs(math.sin(relative_thrust_angle)) / 2
for active in thrusters_active
]
try:
min_max_ratio = min(
max_output / force_ratio for max_output, force_ratio in zip(
max_outputs, engine_translation_force_ratio))
except ZeroDivisionError:
min_max_ratio = min(
max_output / force_ratio for max_output, force_ratio in zip(
max_outputs, engine_translation_force_ratio)
if not math.isclose(force_ratio, 0.0))
translation_engine_forces = [
min_max_ratio * force_ratio
for force_ratio in engine_translation_force_ratio
]
try:
max_output_ratio = max(
output / remaining for output, remaining in zip(
translation_engine_forces, remaining_outputs))
except ZeroDivisionError: # Remaining thrust is 0; cannot manouver directionally
return rotational_outputs
directional_outputs = [
output / max_output_ratio for output in translation_engine_forces
]
return [
rotation + direction for rotation, direction in zip(
rotational_outputs, directional_outputs)
]