From db802f198339487773ddfe29b70da86a6705b212 Mon Sep 17 00:00:00 2001 From: cinnaboot Date: Mon, 9 Feb 2026 07:58:35 -0500 Subject: [PATCH] Refactor check_maneuver_trigger: extract helpers, fix near-circular orbit handling, add next-step detection --- src/maneuver.cpp | 184 +++++++++++++++++++++++++++++------------------ 1 file changed, 113 insertions(+), 71 deletions(-) diff --git a/src/maneuver.cpp b/src/maneuver.cpp index f2aafa1..22589af 100644 --- a/src/maneuver.cpp +++ b/src/maneuver.cpp @@ -110,6 +110,85 @@ OrbitalElements preview_burn_result(const Spacecraft* craft, BurnDirection direc return cartesian_to_orbital_elements(pos, new_vel, parent_mass); } +static double normalize_angle(double angle) { + while (angle < 0.0) angle += 2.0 * M_PI; + while (angle >= 2.0 * M_PI) angle -= 2.0 * M_PI; + return angle; +} + +static double angular_distance(double a, double b) { + double diff = fabs(normalize_angle(a) - normalize_angle(b)); + return (diff > M_PI) ? (2.0 * M_PI - diff) : diff; +} + +static bool angle_between(double current, double next, double target) { + double curr_norm = normalize_angle(current); + double next_norm = normalize_angle(next); + double target_norm = normalize_angle(target); + + if (curr_norm <= next_norm) { + return (target_norm >= curr_norm) && (target_norm <= next_norm); + } else { + return (target_norm >= curr_norm) || (target_norm <= next_norm); + } +} + +static double calculate_true_anomaly(Vec3 r, Vec3 v, Vec3 e_vec, double e_mag, double r_mag) { + // For near-circular orbits, eccentricity vector is near-zero + // Compute true anomaly as the angle in the orbital plane + if (e_mag < 1e-10) { + Vec3 h = vec3_cross(r, v); + double h_mag = vec3_magnitude(h); + if (h_mag < 1e-10) return 0.0; + + // Create a coordinate system in the orbital plane + Vec3 z_hat = vec3_scale(h, 1.0 / h_mag); + // Choose x-axis as cross product of Z (world up) and orbit normal + // This gives a consistent reference direction in the orbital plane + Vec3 world_z = {0.0, 0.0, 1.0}; + Vec3 x_hat = vec3_cross(world_z, z_hat); + double x_hat_mag = vec3_magnitude(x_hat); + if (x_hat_mag < 1e-10) { + // Orbit is equatorial, use world X as reference + x_hat = (Vec3){1.0, 0.0, 0.0}; + } else { + x_hat = vec3_scale(x_hat, 1.0 / x_hat_mag); + } + Vec3 y_hat = vec3_cross(z_hat, x_hat); + + // Project position onto this orbital plane coordinate system + double x_proj = vec3_dot(r, x_hat); + double y_proj = vec3_dot(r, y_hat); + + // True anomaly is the angle in the orbital plane + double nu = atan2(y_proj, x_proj); + if (nu < 0) nu += 2.0 * M_PI; + return nu; + } + + // Standard calculation using eccentricity vector + double cos_nu = vec3_dot(e_vec, r) / (e_mag * r_mag); + cos_nu = fmax(-1.0, fmin(1.0, cos_nu)); + double nu = acos(cos_nu); + + // Determine correct quadrant using cross product + Vec3 r_cross_v = vec3_cross(r, v); + double r_cross_v_dot_e = vec3_dot(r_cross_v, e_vec); + if (r_cross_v_dot_e < 0) { + nu = 2.0 * M_PI - nu; + } + + return nu; +} + +static Vec3 calculate_eccentricity_vector(Vec3 r, Vec3 v, Vec3 h, double mu) { + Vec3 v_cross_h = vec3_cross(v, h); + Vec3 v_cross_h_over_mu = vec3_scale(v_cross_h, 1.0 / mu); + double r_mag = vec3_magnitude(r); + Vec3 r_over_mag = vec3_scale(r, 1.0 / r_mag); + return vec3_sub(v_cross_h_over_mu, r_over_mag); +} + bool check_maneuver_trigger(Maneuver* maneuver, Spacecraft* craft, SimulationState* sim) { switch (maneuver->trigger_type) { case TRIGGER_TIME: @@ -118,87 +197,50 @@ bool check_maneuver_trigger(Maneuver* maneuver, Spacecraft* craft, SimulationSta case TRIGGER_TRUE_ANOMALY: { Vec3 r = craft->local_position; Vec3 v = craft->local_velocity; - double r_mag = vec3_magnitude(r); - double v_mag = vec3_magnitude(v); - - if (r_mag < 1.0) { - return false; - } - Vec3 r_unit = vec3_scale(r, 1.0 / r_mag); + // Validate position magnitude (avoid division by zero) + if (r_mag < 1.0) return false; + // Calculate angular momentum Vec3 h = vec3_cross(r, v); double h_mag = vec3_magnitude(h); + if (h_mag < 1e-10) return false; // Near-linear trajectory - if (h_mag < 1e-10) { - return false; - } - - Vec3 e_vec = {0.0, 0.0, 0.0}; - double mu = 0.0; - - if (craft->parent_index >= 0 && craft->parent_index < sim->body_count) { - CelestialBody* parent = &sim->bodies[craft->parent_index]; - mu = G * parent->mass; - - Vec3 v_cross_h = vec3_cross(v, h); - Vec3 v_cross_h_over_mu = vec3_scale(v_cross_h, 1.0 / mu); - Vec3 r_over_mag = vec3_scale(r, 1.0 / r_mag); - e_vec = vec3_sub(v_cross_h_over_mu, r_over_mag); - } else { + // Get parent body for gravitational parameter + if (craft->parent_index < 0 || craft->parent_index >= sim->body_count) { return false; } + CelestialBody* parent = &sim->bodies[craft->parent_index]; + double mu = G * parent->mass; + Vec3 e_vec = calculate_eccentricity_vector(r, v, h, mu); double e_mag = vec3_magnitude(e_vec); - - if (e_mag < 1e-10) { - Vec3 v_unit = vec3_scale(v, 1.0 / v_mag); - double cos_nu = vec3_dot(r_unit, v_unit); - cos_nu = fmax(-1.0, fmin(1.0, cos_nu)); - double nu = acos(cos_nu); - - double target_nu = maneuver->trigger_value; - while (target_nu < 0) target_nu += 2.0 * M_PI; - while (target_nu >= 2.0 * M_PI) target_nu -= 2.0 * M_PI; - - double nu_normalized = nu; - while (nu_normalized < 0) nu_normalized += 2.0 * M_PI; - while (nu_normalized >= 2.0 * M_PI) nu_normalized -= 2.0 * M_PI; - - double angle_diff = fabs(nu_normalized - target_nu); - if (angle_diff > M_PI) { - angle_diff = 2.0 * M_PI - angle_diff; - } - - return angle_diff < 0.01; - } - - double cos_nu = vec3_dot(e_vec, r) / (e_mag * r_mag); - cos_nu = fmax(-1.0, fmin(1.0, cos_nu)); - double nu = acos(cos_nu); - - Vec3 r_cross_v = vec3_cross(r, v); - double r_cross_v_dot_e = vec3_dot(r_cross_v, e_vec); - - if (r_cross_v_dot_e < 0) { - nu = 2.0 * M_PI - nu; - } - - double target_nu = maneuver->trigger_value; - while (target_nu < 0) target_nu += 2.0 * M_PI; - while (target_nu >= 2.0 * M_PI) target_nu -= 2.0 * M_PI; - - double nu_normalized = nu; - while (nu_normalized < 0) nu_normalized += 2.0 * M_PI; - while (nu_normalized >= 2.0 * M_PI) nu_normalized -= 2.0 * M_PI; - - double angle_diff = fabs(nu_normalized - target_nu); - if (angle_diff > M_PI) { - angle_diff = 2.0 * M_PI - angle_diff; - } - - return angle_diff < 0.01; + double target_nu = normalize_angle(maneuver->trigger_value); + double current_nu = calculate_true_anomaly(r, v, e_vec, e_mag, r_mag); + double current_nu_norm = normalize_angle(current_nu); + double current_diff = angular_distance(current_nu_norm, target_nu); + if (current_diff < 0.01) return true; + + // Propagate orbit forward by one time step to check if we'll cross trigger + OrbitalElements future_elements = propagate_orbital_elements(craft->orbit, sim->dt, parent->mass); + Vec3 future_r, future_v; + orbital_elements_to_cartesian(future_elements, parent->mass, &future_r, &future_v); + + double future_r_mag = vec3_magnitude(future_r); + if (future_r_mag < 1.0) return false; + + // Calculate future eccentricity vector for true anomaly calculation + Vec3 future_h = vec3_cross(future_r, future_v); + Vec3 future_e_vec = calculate_eccentricity_vector(future_r, future_v, future_h, mu); + double future_e_mag = vec3_magnitude(future_e_vec); + + // Calculate future true anomaly + double future_nu = calculate_true_anomaly(future_r, future_v, future_e_vec, future_e_mag, future_r_mag); + double future_nu_norm = normalize_angle(future_nu); + + // Check if target lies between current and future positions + return angle_between(current_nu_norm, future_nu_norm, target_nu); } default: