diff --git a/src/maneuver.cpp b/src/maneuver.cpp index a0277ac..a0679b4 100644 --- a/src/maneuver.cpp +++ b/src/maneuver.cpp @@ -2,6 +2,7 @@ #include "physics.h" #include "spacecraft.h" #include "simulation.h" +#include "orbital_mechanics.h" #include #include @@ -162,8 +163,14 @@ bool check_maneuver_trigger(Maneuver* maneuver, Spacecraft* craft, SimulationSta } } -void execute_maneuver(Maneuver* maneuver, Spacecraft* craft, double current_time) { +void execute_maneuver(Maneuver* maneuver, Spacecraft* craft, SimulationState* sim, double current_time) { apply_impulsive_burn(craft, maneuver->direction, maneuver->delta_v); + + if (craft->parent_index >= 0 && craft->parent_index < sim->body_count) { + CelestialBody* parent = &sim->bodies[craft->parent_index]; + craft->orbit = cartesian_to_orbital_elements(craft->local_position, craft->local_velocity, parent->mass); + } + maneuver->executed = true; maneuver->executed_time = current_time; } diff --git a/src/maneuver.h b/src/maneuver.h index 3394dcb..768a789 100644 --- a/src/maneuver.h +++ b/src/maneuver.h @@ -47,6 +47,6 @@ void apply_custom_burn(Spacecraft* craft, Vec3 delta_v_local); // Maneuver execution functions bool check_maneuver_trigger(Maneuver* maneuver, Spacecraft* craft, SimulationState* sim); -void execute_maneuver(Maneuver* maneuver, Spacecraft* craft, double current_time); +void execute_maneuver(Maneuver* maneuver, Spacecraft* craft, SimulationState* sim, double current_time); #endif diff --git a/src/orbital_mechanics.cpp b/src/orbital_mechanics.cpp index 94be15b..41280ed 100644 --- a/src/orbital_mechanics.cpp +++ b/src/orbital_mechanics.cpp @@ -81,7 +81,25 @@ double solve_kepler_hyperbolic(double mean_anomaly, double eccentricity) { H_prev = H; double sinh_H = sinh(H); double cosh_H = cosh(H); - H = H - (H - eccentricity * sinh_H - mean_anomaly) / (1.0 - eccentricity * cosh_H); + H = H - (H - eccentricity * sinh_H - mean_anomaly) / (1.0 - eccentricity * cosh(H)); + iterations++; + } + + return H; +} + +double solve_kepler_hyperbolic_with_prev(double mean_anomaly, double eccentricity, double H_prev_guess) { + // Solve hyperbolic Kepler equation with a hint from previous H value + double H = H_prev_guess; + + double H_iter = H + 2.0 * KEPLER_TOLERANCE; + int iterations = 0; + + while (fabs(H - H_iter) > KEPLER_TOLERANCE && iterations < KEPLER_MAX_ITERATIONS) { + H_iter = H; + double sinh_H = sinh(H); + double cosh_H = cosh(H); + H = H - (H - eccentricity * sinh_H - mean_anomaly) / (1.0 - eccentricity * cosh(H)); iterations++; } @@ -113,10 +131,48 @@ double eccentric_to_true_anomaly(double eccentric_anomaly, double eccentricity) } double hyperbolic_to_true_anomaly(double hyperbolic_anomaly, double eccentricity) { - // Hyperbolic E to true anomaly: tanh(ν/2) = √((e-1)/(e+1)) · tanh(H/2) + // Hyperbolic H to true anomaly: tan(ν/2) = √((e+1)/(e-1)) · tanh(H/2) double tanh_half_H = tanh(hyperbolic_anomaly / 2.0); - double tanh_half_nu = sqrt((eccentricity - 1.0) / (eccentricity + 1.0)) * tanh_half_H; - return 2.0 * atanh(tanh_half_nu); + double factor = sqrt((eccentricity + 1.0) / (eccentricity - 1.0)); // Inverted + double tan_half_nu = factor * tanh_half_H; + + // Clamp for numeric stability + if (tan_half_nu >= 1e10) { + tan_half_nu = 1e10; + } else if (tan_half_nu <= -1e10) { + tan_half_nu = -1e10; + } + + return 2.0 * atan(tan_half_nu); // Use atan, not atanh +} + +int is_near_hyperbolic_asymptote(double true_anomaly, double eccentricity) { + // Check if true anomaly is close to asymptote + // For hyperbolic orbit, asymptotes are at ν = ± acos(-1/e) + double asymptote = acos(-1.0 / eccentricity); + double distance_from_asymptote = fabs(fabs(true_anomaly) - asymptote); + return distance_from_asymptote < 0.01; +} + +double true_anomaly_to_hyperbolic(double true_anomaly, double eccentricity) { + // True anomaly to hyperbolic anomaly: tanh(H/2) = √((e-1)/(e+1)) · tan(ν/2) + // Solving for H: H = 2 · atanh(√((e-1)/(e+1)) · tan(ν/2)) + + if (is_near_hyperbolic_asymptote(true_anomaly, eccentricity)) { + return -1e10; + } + + double tan_half_nu = tan(true_anomaly / 2.0); + double factor = sqrt((eccentricity - 1.0) / (eccentricity + 1.0)); + double tanh_half_H = tan_half_nu * factor; // Multiply, not divide + + if (tanh_half_H >= 1.0) { + tanh_half_H = 0.999999999999999; + } else if (tanh_half_H <= -1.0) { + tanh_half_H = -0.999999999999999; + } + + return 2.0 * atanh(tanh_half_H); } // Conversion chain: M → E/H → ν @@ -277,9 +333,8 @@ OrbitalElements propagate_orbital_elements(const OrbitalElements& elements, doub OrbitalElements result = elements; result.true_anomaly = nu_new; return result; - } else { - double p = a * (1.0 - e * e); - double n = sqrt(mu / pow(fabs(a), 3.0)); + } else if (e < 1.0) { + double n = sqrt(mu / pow(a, 3.0)); double E = 2.0 * atan(sqrt((1.0 - e) / (1.0 + e)) * tan(nu / 2.0)); @@ -304,6 +359,35 @@ OrbitalElements propagate_orbital_elements(const OrbitalElements& elements, doub OrbitalElements result = elements; result.true_anomaly = 2.0 * atan(sqrt((1.0 + e) / (1.0 - e)) * tan(E_new / 2.0)); + return result; + } else { // e >= 1.0 (hyperbolic) + double n = sqrt(mu / pow(-a, 3.0)); + + // Convert true anomaly to hyperbolic anomaly + double H = true_anomaly_to_hyperbolic(nu, e); + + // Compute mean anomaly from hyperbolic anomaly + double M = e * sinh(H) - H; + double M_new = M + n * dt; + + // Newton-Raphson iteration for convergence + const double HYPERBOLIC_TOLERANCE = 1.0e-10; + const int MAX_HYPERBOLIC_ITERATIONS = 50; + int iterations = 0; + double H_new = H; + double H_prev = H_new + 2.0 * HYPERBOLIC_TOLERANCE; + + while (fabs(H_new - H_prev) > HYPERBOLIC_TOLERANCE && iterations < MAX_HYPERBOLIC_ITERATIONS) { + H_prev = H_new; + double sinh_H = sinh(H_new); + double cosh_H = cosh(H_new); + H_new = H_new - (e * sinh_H - H_new - M_new) / (e * cosh_H - 1.0); + iterations++; + } + + OrbitalElements result = elements; + result.true_anomaly = hyperbolic_to_true_anomaly(H_new, e); + return result; } } diff --git a/src/orbital_mechanics.h b/src/orbital_mechanics.h index b17c1ab..4405c63 100644 --- a/src/orbital_mechanics.h +++ b/src/orbital_mechanics.h @@ -30,10 +30,15 @@ double solve_kepler_elliptical(double mean_anomaly, double eccentricity); // Hyperbolic Kepler equation solver: H - e·sinh(H) = M double solve_kepler_hyperbolic(double mean_anomaly, double eccentricity); +double solve_kepler_hyperbolic_with_prev(double mean_anomaly, double eccentricity, double H_prev_guess); // Conversions between anomaly types double eccentric_to_true_anomaly(double eccentric_anomaly, double eccentricity); double hyperbolic_to_true_anomaly(double hyperbolic_anomaly, double eccentricity); +double true_anomaly_to_hyperbolic(double true_anomaly, double eccentricity); + +// Check if true anomaly is near asymptote for hyperbolic orbit +int is_near_hyperbolic_asymptote(double true_anomaly, double eccentricity); // Unified mean anomaly to true anomaly conversion // Automatically dispatches to elliptical or hyperbolic based on eccentricity diff --git a/src/simulation.cpp b/src/simulation.cpp index b5234ea..32454bd 100644 --- a/src/simulation.cpp +++ b/src/simulation.cpp @@ -250,6 +250,8 @@ void update_bodies_physics(SimulationState* sim) { CelestialBody* new_parent_body = &sim->bodies[body->parent_index]; body->local_position = vec3_sub(body->global_position, new_parent_body->global_position); body->local_velocity = vec3_sub(body->global_velocity, new_parent_body->global_velocity); + + body->orbit = cartesian_to_orbital_elements(body->local_position, body->local_velocity, new_parent_body->mass); } else { body->local_position = body->global_position; body->local_velocity = body->global_velocity; @@ -259,8 +261,16 @@ void update_bodies_physics(SimulationState* sim) { if (body->parent_index >= 0 && body->parent_index < sim->body_count) { CelestialBody* parent = &sim->bodies[body->parent_index]; - rk4_step(&body->local_position, &body->local_velocity, - sim->dt, body->mass, parent->mass); + Vec3 expected_pos, expected_vel; + orbital_elements_to_cartesian(body->orbit, parent->mass, &expected_pos, &expected_vel); + + double vel_diff = vec3_magnitude(vec3_sub(body->local_velocity, expected_vel)); + if (vel_diff > 1e-6) { + body->orbit = cartesian_to_orbital_elements(body->local_position, body->local_velocity, parent->mass); + } + + body->orbit = propagate_orbital_elements(body->orbit, sim->dt, parent->mass); + orbital_elements_to_cartesian(body->orbit, parent->mass, &body->local_position, &body->local_velocity); } } } @@ -275,8 +285,16 @@ void update_spacecraft_physics(SimulationState* sim) { CelestialBody* parent = &sim->bodies[craft->parent_index]; - rk4_step(&craft->local_position, &craft->local_velocity, - sim->dt, craft->mass, parent->mass); + Vec3 expected_pos, expected_vel; + orbital_elements_to_cartesian(craft->orbit, parent->mass, &expected_pos, &expected_vel); + + double vel_diff = vec3_magnitude(vec3_sub(craft->local_velocity, expected_vel)); + if (vel_diff > 1e-6) { + craft->orbit = cartesian_to_orbital_elements(craft->local_position, craft->local_velocity, parent->mass); + } + + craft->orbit = propagate_orbital_elements(craft->orbit, sim->dt, parent->mass); + orbital_elements_to_cartesian(craft->orbit, parent->mass, &craft->local_position, &craft->local_velocity); } } @@ -295,7 +313,7 @@ void execute_pending_maneuvers(SimulationState* sim) { Spacecraft* craft = &sim->spacecraft[maneuver->craft_index]; if (check_maneuver_trigger(maneuver, craft, sim)) { - execute_maneuver(maneuver, craft, sim->time); + execute_maneuver(maneuver, craft, sim, sim->time); } } } diff --git a/tests/test_hybrid_burns.cpp b/tests/test_hybrid_burns.cpp index 99a4644..eff0d9b 100644 --- a/tests/test_hybrid_burns.cpp +++ b/tests/test_hybrid_burns.cpp @@ -37,7 +37,7 @@ void execute_maneuver_by_name(SimulationState* sim, const char* maneuver_name, S sim->time = maneuver->trigger_value; } - execute_maneuver(maneuver, craft, sim->time); + execute_maneuver(maneuver, craft, sim, sim->time); REQUIRE(maneuver->executed); REQUIRE(maneuver->executed_time == sim->time);