Bnenne
7289543f6a
All checks were successful
Build Simulation and Test / Run All Tests (push) Successful in 2m56s
104 lines
7.5 KiB
Python
104 lines
7.5 KiB
Python
import math
|
|
|
|
from config.constants import MAX_VELOCITY, MAX_ROTATIONAL_VELOCITY
|
|
|
|
|
|
class Physics:
|
|
"""
|
|
Physics
|
|
⠀⠀⢀⣀⢀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⣀⡠⣄⠀⠀⠀⠀⠀
|
|
⠀⣴⠊⠛⠦⣅⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢸⠃⠈⠈⠳⡄⠀⠀⠀
|
|
⠸⣿⠀⠀⠀⠉⢹⣆⠀⠀⠀⠠⡶⠖⢦⡄⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠈⢻⡄⠀⠀⠈⡆⠀⠀
|
|
⠀⠈⣧⠀⠀⠀⠀⠉⠓⠂⠐⠋⠀⠀⢸⣷⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⣴⡆⢀⢰⣶⣄⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⡒⠀⣤⢀⠀⠈⡇⠀⠀⠀⢹⡂⠀
|
|
⠀⠀⢹⣇⠀⠀⠀⠀⠀⠀⠀⠀⠀⠰⠞⠄⠀⠀⠀⠀⠀⠀⠀⣀⣤⣾⠿⠁⠈⠁⠉⠻⢷⠶⠴⢭⡄⠤⡀⠀⠀⠀⠀⠀⢿⡇⠀⠈⢢⠀⣸⡇⠀⠀⠀⠐⠁⠀
|
|
⠀⠀⠀⢻⠀⠀⠀⠀⠀⠀⠀⠀⢐⡏⡀⠀⠀⠀⠀⠀⠀⣠⡚⠛⠋⠁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠈⠑⢮⡀⡀⠀⠀⠀⠉⢳⡄⠀⡘⠛⠋⠀⠀⠀⠀⠀⡄⠀
|
|
⠀⠀⠀⢀⣇⠀⠀⠀⠀⠀⠀⠀⢸⠀⠀⠀⠀⠀⢀⣴⠏⠁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠘⣌⢂⠀⠀⠀⢸⡇⠀⠀⠀⠀⠀⠀⠀⠀⢰⣧⡅
|
|
⠀⠀⠀⠀⢻⣄⠀⠀⠀⠀⠀⠀⢸⠀⠀⠀⠀⣠⡾⠁⠀⠀⠀⠀⠀⣠⡴⠖⠤⠴⠤⠶⠀⢄⠀⠀⠀⠀⠀⠀⠈⠓⠀⠀⠀⢸⡇⠀⠀⠀⠀⠀⠀⢠⣤⠞⠵⠁
|
|
⠀⠀⠀⠀⠀⠻⣦⡀⠀⠀⠀⢰⡺⠀⠀⠀⢀⡾⠇⠀⠀⠀⠀⣠⡾⠋⠀⠀⠀⠀⠀⠀⠀⠀⢹⣀⡀⠀⠀⠀⠀⠀⢱⠀⠀⠘⠛⢧⡀⠀⠀⣀⣴⠟⠁⠀⠀⠀
|
|
⠀⠀⠀⠀⠀⠀⠈⠓⠖⠶⠶⠵⠃⠀⠀⠀⢺⢈⠀⠀⠀⢀⣰⠏⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠠⠙⢆⠀⠀⠀⠀⠀⠸⣄⠁⠀⠀⠈⠻⠒⠚⠏⠁⠀⠀⠀⠀⠀
|
|
⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢘⡀⠀⠀⠀⢸⠏⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠸⣡⡀⠀⠀⠀⠀⢸⠂⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
|
|
⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢸⠃⠀⠀⣠⡎⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⣸⠀⠀⠀⠀⢻⡁⡀⠀⠀⢀⢸⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
|
|
⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢸⡔⠀⠀⡾⠀⠀⠀⠀⢀⡤⠞⠁⠀⠀⠀⠹⠄⠀⠀⠀⠈⢯⠀⠀⠀⣠⣼⠅⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
|
|
⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠿⣧⠀⢸⡇⠀⠀⢠⡼⠋⠀⠀⠀⠀⠀⠀⠀⠀⢰⠀⠀⠀⠸⡄⠀⢀⣿⣏⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
|
|
⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢿⣄⣸⡁⠀⢀⠾⡆⠇⠀⠀⠀⠀⠀⠀⠀⠀⠀⠐⠀⠀⢠⡣⠀⣸⡿⠂⠁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
|
|
⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠿⣬⠛⠁⡆⠉⡆⠃⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠂⢧⠀⣿⡀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
|
|
⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠙⣿⣆⣀⣁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⣠⠀⢀⣸⣤⣿⠯⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
|
|
⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠈⠉⠀⠘⠳⣄⡀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⣠⣴⠿⠋⠛⠛⠃⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
|
|
⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠉⠙⠛⠲⠖⠒⠒⠂⠒⠒⠒⠛⠉⠁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
|
|
|
|
⣿⣿⣿⣿⣿⣿⣿⣿⠿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿
|
|
⣿⣿⣿⣿⣿⣿⣹⡏⠀⠈⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿
|
|
⣿⣿⣿⣿⣿⣿⣿⣿⣧⠀⢸⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿
|
|
⣿⣿⣿⣿⣿⣿⣿⣿⣿⡇⠀⢻⢿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿
|
|
⣿⣿⣿⣿⣿⣿⣿⠿⣋⡁⢶⣦⣤⣍⠙⢿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿
|
|
⣿⣿⣿⣿⣿⣿⠉⡴⣿⡟⢸⣿⣿⡿⣷⡄⢻⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿
|
|
⣿⣿⣿⣿⣿⡿⢸⣿⣿⡗⣼⣿⣿⣯⣽⢻⡆⢿⣿⣿⣿⣿⣿⣿⣿⣿⣿
|
|
⣿⣿⣿⣿⣿⡇⣾⣿⣿⣾⣿⣿⣾⣿⣯⣿⣧⠘⣿⣿⣿⣿⣿⣿⣿⣿⣿
|
|
⣿⣿⣿⣿⡟⣱⣿⣿⣿⣿⣿⣽⣾⣿⣽⢻⣏⡄⣿⢿⣿⣿⣿⣿⣿⣿⣿
|
|
⣿⣿⣿⡿⢡⣿⣯⣿⣿⡿⣟⣻⣿⣿⢿⣿⣏⢻⠘⣿⣿⣿⣿⣿⣿⣿⣿
|
|
⣿⣿⠗⠒⣛⢻⣿⣿⣿⡟⢻⣿⣏⣿⣷⣠⣿⡘⡧⡘⢿⣿⣿⣿⣿⣿⣿
|
|
⠛⠁⢴⠺⢿⡀⠻⣿⢿⡇⣼⠏⠯⠈⢿⡿⢘⣿⡘⣦⡘⣿⣿⣿⣿⣿⣿
|
|
⠀⠀⠀⠀⠈⢻⡆⠈⠈⢠⡇⠐⠀⠀⠈⠃⢸⣯⣧⠉⣧⠸⣿⣿⣿⣿⣿
|
|
⡄⣀⢃⠀⠀⣸⣿⣧⡀⣼⣿⡀⠀⠀⢠⡄⠸⣿⣿⣷⢉⠃⣿⣿⣿⣿⣿
|
|
⡇⢹⣦⡻⣿⣿⣿⢩⣷⣿⣿⣿⣷⣾⣾⠇⣶⣞⡛⣷⣌⡀⣿⣿⣿⣿⣿
|
|
⡶⢄⠻⣿⣾⣿⠓⠬⠽⠟⢛⣿⣿⣿⣟⢐⢻⣿⣿⣿⠿⢡⣿⣿⣿⣿⣿
|
|
⣿⣇⢱⠙⢿⣿⣶⣖⣲⣶⣿⣿⡿⢋⣡⣿⣿⣿⣿⡟⢠⣿⣿⣿⣿⣿⣿
|
|
⣿⣿⣷⣅⢲⣌⠛⠿⠿⠿⢟⠁⣴⣷⣾⣿⣾⡿⠟⣰⣿⣷⣮⣭⣭⣟⣻
|
|
⣿⣿⣿⣿⣷⣌⡐⠰⣴⣶⠾⠿⢿⠿⠷⠟⠈⣠⣾⣿⣿⣿⣿⣿⣿⣿⣿
|
|
⣿⣿⣿⣿⣿⣿⣧⠀⠀⠀⠉⠁⠀⢀⣀⣠⣴⣾⣿⣿⣿⣿⣿⣿⣿⣿⣿
|
|
⣿⣿⣿⣿⣿⣿⣿⣷⣶⣬⣘⣒⠷⢦⣶⣶⣶⣗⣿⣿⣿⣿⣿⣿⣿⣿⣿
|
|
⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣶⣶⣶⣾⣿⣿⣟⣛⣿⣿⣿⣿⣿⣿
|
|
"""
|
|
def __init__(self, drag_coefficient: float, rotational_drag: float):
|
|
self.drag_coefficient: float = drag_coefficient
|
|
self.rotational_drag: float = rotational_drag
|
|
|
|
self.velocity: tuple[int, int] = (0, 0)
|
|
self.acceleration: tuple[int, int] = (0, 0)
|
|
|
|
self.rotational_velocity: int = 0
|
|
self.angular_acceleration: int = 0
|
|
|
|
|
|
def move(self, linear_acceleration: float, angular_acceleration: int, rotational_position):
|
|
# Apply drag force
|
|
drag_coefficient = self.drag_coefficient
|
|
drag_x = -self.velocity[0] * drag_coefficient
|
|
drag_y = -self.velocity[1] * drag_coefficient
|
|
|
|
# Combine all forces
|
|
total_linear_accel = linear_acceleration
|
|
total_linear_accel = max(-0.1, min(0.1, total_linear_accel))
|
|
|
|
# Convert to world coordinates
|
|
x_component = total_linear_accel * math.cos(math.radians(rotational_position))
|
|
y_component = total_linear_accel * math.sin(math.radians(rotational_position))
|
|
|
|
# Add drag to total acceleration
|
|
total_accel_x = x_component + drag_x
|
|
total_accel_y = y_component + drag_y
|
|
|
|
self.acceleration = (total_accel_x, total_accel_y)
|
|
|
|
# Apply drag force to angular acceleration
|
|
rotational_drag = self.rotational_drag
|
|
self.angular_acceleration = angular_acceleration - self.rotational_velocity * rotational_drag
|
|
|
|
# tick acceleration
|
|
velocity_x = self.velocity[0] + self.acceleration[0]
|
|
velocity_y = self.velocity[1] + self.acceleration[1]
|
|
self.velocity = (velocity_x, velocity_y)
|
|
|
|
# # clamp velocity
|
|
speed = math.sqrt(self.velocity[0] ** 2 + self.velocity[1] ** 2)
|
|
if speed > MAX_VELOCITY:
|
|
scale = MAX_VELOCITY / speed
|
|
self.velocity = (self.velocity[0] * scale, self.velocity[1] * scale)
|
|
|
|
self.angular_acceleration = angular_acceleration
|
|
self.rotational_velocity += self.angular_acceleration
|
|
|
|
# clamp rotational velocity
|
|
self.rotational_velocity = max(-MAX_ROTATIONAL_VELOCITY, min(MAX_ROTATIONAL_VELOCITY, self.rotational_velocity))
|
|
|
|
return self.velocity, self.acceleration, self.rotational_velocity, self.angular_acceleration |