|
|
|
|
@ -58,56 +58,6 @@ Vec3 calculate_acceleration(Vec3 force, double mass) {
|
|
|
|
|
return {0.0, 0.0, 0.0}; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void rk4_step(Vec3* position, Vec3* velocity, double dt, |
|
|
|
|
double body_mass, double parent_mass) { |
|
|
|
|
Vec3 k1_vel, k2_vel, k3_vel, k4_vel; |
|
|
|
|
Vec3 k1_pos, k2_pos, k3_pos, k4_pos; |
|
|
|
|
|
|
|
|
|
Vec3 pos0 = *position; |
|
|
|
|
Vec3 vel0 = *velocity; |
|
|
|
|
|
|
|
|
|
k1_vel = evaluate_acceleration(pos0, body_mass, parent_mass); |
|
|
|
|
k1_pos = vel0; |
|
|
|
|
|
|
|
|
|
Vec3 pos1 = vec3_add(pos0, vec3_scale(k1_pos, dt * 0.5)); |
|
|
|
|
Vec3 vel1 = vec3_add(vel0, vec3_scale(k1_vel, dt * 0.5)); |
|
|
|
|
k2_vel = evaluate_acceleration(pos1, body_mass, parent_mass); |
|
|
|
|
k2_pos = vel1; |
|
|
|
|
|
|
|
|
|
Vec3 pos2 = vec3_add(pos0, vec3_scale(k2_pos, dt * 0.5)); |
|
|
|
|
Vec3 vel2 = vec3_add(vel0, vec3_scale(k2_vel, dt * 0.5)); |
|
|
|
|
k3_vel = evaluate_acceleration(pos2, body_mass, parent_mass); |
|
|
|
|
k3_pos = vel2; |
|
|
|
|
|
|
|
|
|
Vec3 pos3 = vec3_add(pos0, vec3_scale(k3_pos, dt)); |
|
|
|
|
Vec3 vel3 = vec3_add(vel0, vec3_scale(k3_vel, dt)); |
|
|
|
|
k4_vel = evaluate_acceleration(pos3, body_mass, parent_mass); |
|
|
|
|
k4_pos = vel3; |
|
|
|
|
|
|
|
|
|
Vec3 k_vel_sum = vec3_add(vec3_add(k1_vel, vec3_scale(k2_vel, 2.0)), |
|
|
|
|
vec3_add(vec3_scale(k3_vel, 2.0), k4_vel)); |
|
|
|
|
Vec3 k_pos_sum = vec3_add(vec3_add(k1_pos, vec3_scale(k2_pos, 2.0)), |
|
|
|
|
vec3_add(vec3_scale(k3_pos, 2.0), k4_pos)); |
|
|
|
|
|
|
|
|
|
*velocity = vec3_add(vel0, vec3_scale(k_vel_sum, dt / 6.0)); |
|
|
|
|
*position = vec3_add(pos0, vec3_scale(k_pos_sum, dt / 6.0)); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
Vec3 evaluate_acceleration(Vec3 relative_pos, double body_mass, double parent_mass) { |
|
|
|
|
Vec3 total_force = {0.0, 0.0, 0.0}; |
|
|
|
|
|
|
|
|
|
double distance = vec3_magnitude(relative_pos); |
|
|
|
|
if (distance < 1.0) { |
|
|
|
|
distance = 1.0; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
double force_magnitude = G * body_mass * parent_mass / (distance * distance); |
|
|
|
|
Vec3 direction = vec3_normalize(vec3_scale(relative_pos, -1.0)); |
|
|
|
|
total_force = vec3_scale(direction, force_magnitude); |
|
|
|
|
|
|
|
|
|
return calculate_acceleration(total_force, body_mass); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
Mat3 mat3_identity() { |
|
|
|
|
return {1.0, 0.0, 0.0, |
|
|
|
|
0.0, 1.0, 0.0, |
|
|
|
|
@ -172,3 +122,9 @@ Mat3 mat3_rotation_orbital(double omega, double i, double Omega) {
|
|
|
|
|
Mat3 temp = mat3_multiply(Rx_i, Rz_omega); |
|
|
|
|
return mat3_multiply(Rz_Omega, temp); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
bool compare_vec3(Vec3 a, Vec3 b, double tolerance) { |
|
|
|
|
return fabs(a.x - b.x) <= tolerance && |
|
|
|
|
fabs(a.y - b.y) <= tolerance && |
|
|
|
|
fabs(a.z - b.z) <= tolerance; |
|
|
|
|
} |
|
|
|
|
|