You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
104 lines
3.0 KiB
104 lines
3.0 KiB
#include "physics.h" |
|
#include <cmath> |
|
|
|
// Vector addition |
|
Vec3 vec3_add(Vec3 a, Vec3 b) { |
|
return {a.x + b.x, a.y + b.y, a.z + b.z}; |
|
} |
|
|
|
// Vector subtraction |
|
Vec3 vec3_sub(Vec3 a, Vec3 b) { |
|
return {a.x - b.x, a.y - b.y, a.z - b.z}; |
|
} |
|
|
|
// Cross product |
|
Vec3 vec3_cross(Vec3 a, Vec3 b) { |
|
return { |
|
a.y * b.z - a.z * b.y, |
|
a.z * b.x - a.x * b.z, |
|
a.x * b.y - a.y * b.x |
|
}; |
|
} |
|
|
|
// Scalar multiplication |
|
Vec3 vec3_scale(Vec3 v, double s) { |
|
return {v.x * s, v.y * s, v.z * s}; |
|
} |
|
|
|
// Vector magnitude |
|
double vec3_magnitude(Vec3 v) { |
|
return sqrt(v.x * v.x + v.y * v.y + v.z * v.z); |
|
} |
|
|
|
// Distance between two points |
|
double vec3_distance(Vec3 a, Vec3 b) { |
|
Vec3 diff = vec3_sub(a, b); |
|
return vec3_magnitude(diff); |
|
} |
|
|
|
// Normalize vector to unit length |
|
Vec3 vec3_normalize(Vec3 v) { |
|
double mag = vec3_magnitude(v); |
|
if (mag > 0.0) { |
|
return vec3_scale(v, 1.0 / mag); |
|
} |
|
return {0.0, 0.0, 0.0}; |
|
} |
|
|
|
// Calculate acceleration from force: a = F / m |
|
Vec3 calculate_acceleration(Vec3 force, double mass) { |
|
if (mass > 0.0) { |
|
return vec3_scale(force, 1.0 / 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); |
|
}
|
|
|