From 7d419f8d30841472aba03766f0584d98b47d29ca Mon Sep 17 00:00:00 2001 From: cinnaboot Date: Wed, 22 Apr 2026 12:24:24 -0400 Subject: [PATCH] Revert "update docs/technical_reference.md new maneuver logic" This reverts commit 601e564fa78a41d747a072b66c55f1974a110791. --- continue.md | 39 --- docs/technical_reference.md | 46 ++-- src/maneuver.cpp | 30 +-- src/simulation.cpp | 3 +- src/test_utilities.cpp | 48 ++-- src/test_utilities.h | 7 +- tests/compute_rendezvous_params.py | 204 -------------- tests/simulate_rendezvous.py | 413 ----------------------------- tests/test_rendezvous.cpp | 37 +-- 9 files changed, 42 insertions(+), 785 deletions(-) delete mode 100644 continue.md delete mode 100644 tests/compute_rendezvous_params.py delete mode 100644 tests/simulate_rendezvous.py diff --git a/continue.md b/continue.md deleted file mode 100644 index c23b52f..0000000 --- a/continue.md +++ /dev/null @@ -1,39 +0,0 @@ -# Hohmann Transfer Rendezvous — Debug Report - -## Root Cause - -The test uses **two different time variables** that are off by a factor of 10: - -- `sim->time` — simulation's internal clock, increments by `sim->dt = 0.1` each `update_simulation()` call -- `sim_time` — test loop counter, increments by `DT = 1.0` each iteration - -The dump milestones use `sim_time`: -```cpp -if (i == int(wait_time / DT)) // wait_time=60062.65, DT=1.0 → i=60062 -``` - -But the maneuvers trigger on `sim->time`. At `i=60062`, `sim->time = 6006.2` — **10x too early**. The departure maneuver actually fires at `i ≈ 600627` (when `sim->time ≥ 60062.65`). - -## What This Means - -The "AFTER DEPARTURE BURN" dump at `i=60064` shows `exec=0` — the burn hasn't happened yet. The dumps are capturing state at completely wrong simulation times. - -## Why the 11,579 km Separation - -With `TIME_STEP = 0.1`, the Hohmann transfer parameters (`wait_time = 60062.65`, `arrival_time = 62804.47`) were computed assuming an **instantaneous burn at exactly `wait_time`**. - -The sub-step interpolation now propagates `burn_dt = 0.05` (from `sim->time = 60062.6` to `trigger = 60062.65`), then burns, then propagates `remaining = 0.05`. This puts the chaser at a **different orbital position at burn time** than the old code, which propagated the full `0.1` step before burning. - -The chaser's true anomaly at `t = 60062.65` differs from its true anomaly at `t = 60062.7` by ~0.006 rad. The Hohmann phasing was calculated for one starting position but the burn now happens at the other. - -## The Two Options - -1. **Update test expectations** — The sub-step interpolation is *more physically accurate*. The test expectations were tuned to the old quantized (step-boundary) behavior. The 11,579 km error is the old test asserting old behavior. - -2. **Change trigger logic** — If you want the burn to fire at the step boundary (old behavior), revert `scheduled_dt` to `sim->dt` for time triggers. But then you lose sub-step accuracy. - -## Files to Look At in New Session - -- `tests/test_rendezvous.cpp` line ~571 — the rendezvous assertion (expects <100m separation) -- `src/maneuver.cpp` `check_maneuver_trigger()` — the TRIGGER_TIME sub-step logic -- `tests/test_rendezvous.toml` — initial conditions for the rendezvous scenario diff --git a/docs/technical_reference.md b/docs/technical_reference.md index b7b49c2..e3af733 100644 --- a/docs/technical_reference.md +++ b/docs/technical_reference.md @@ -2,7 +2,7 @@ ## Overview -2-body orbital mechanics simulator using **analytical propagation** for precise Keplerian trajectories. Supports elliptical, parabolic, and hyperbolic orbits with dynamic Sphere of Influence (SOI) transitions, impulsive burns, and 3D visualization via Raylib. +N-body orbital mechanics simulator using **analytical propagation** for precise Keplerian trajectories. Supports elliptical, parabolic, and hyperbolic orbits with dynamic Sphere of Influence (SOI) transitions, impulsive burns, and 3D visualization via Raylib. ## Architecture @@ -131,18 +131,15 @@ Sequence: argument_of_periapsis (ω) → inclination (i) → longitude_of_ascend - BURN_CUSTOM: user-specified vector ### Exact Position Execution -True anomaly triggers use analytical mean anomaly delta to compute exact time to target, eliminating per-frame propagation probes: +True anomaly triggers must execute at precise orbital position: +1. `check_maneuver_trigger()` calculates scheduled_dt to target anomaly (triggers when angular distance < 0.01 rad) +2. If scheduled_dt < sim->dt, trigger fires +3. Propagate spacecraft by scheduled_dt to exact position +4. Execute burn (apply delta-v, reconstruct elements) +5. Propagate remaining time (sim->dt - scheduled_dt) +6. Mark spacecraft as handled to skip in update_spacecraft_physics() -1. `check_maneuver_trigger()` converts current and target true anomaly to mean anomaly, computes delta-M, divides by mean motion to get `dt_needed` -2. If `0 < dt_needed <= sim->dt`, trigger fires and `scheduled_dt` is set -3. In `update_spacecraft_physics()`, for each spacecraft: check all pending maneuvers for that craft -4. If a maneuver fires: propagate by `burn_dt` (scheduled_dt), execute burn, propagate remaining (`sim->dt - burn_dt`) -5. No separate maneuver execution step — all inline in the spacecraft propagation loop - -**TRIGGER_TIME**: `scheduled_dt` is always 0 (burn at step boundary, quantization error in [0, DT)). -**TRIGGER_TRUE_ANOMALY**: Sub-step timing supported via analytical mean anomaly calculation. - -**Future TODO**: Parabolic (Barker's equation) and hyperbolic branches for `check_maneuver_trigger()`. +**Wraparound handling**: When current_nu > 5.0 and future_nu < 1.0, detect 2π→0 crossing at periapsis. ### Hohmann Transfer `calculate_hohmann_transfer()` computes optimal two-burn transfer between two circular orbits using the vis-viva equation. Transfer time equals half the period of the transfer ellipse. @@ -176,22 +173,15 @@ Handles orbital rendezvous planning and execution via Hohmann transfers and phas 5. Main loop begins **Main Loop Order**: -1. update_bodies_physics() - SOI checks, drift detection, propagation -2. compute_global_coordinates() -3. update_spacecraft_physics() - maneuver checking, propagation, burns -4. compute_spacecraft_globals() -5. time += dt - -**Spacecraft Physics Per-Frame** (`update_spacecraft_physics`): -- For each spacecraft: - 1. Validate local velocity against expected Keplerian velocity; if vel_diff > 1e-6, recalculate orbital elements - 2. Check all pending maneuvers for this craft — if a trigger fires: - a. Propagate by `burn_dt` (scheduled sub-step offset) - b. Execute the maneuver (apply delta-v) - c. Propagate remaining (`sim->dt - burn_dt`) - 3. If no maneuver: propagate full `sim->dt` - -**Body Physics Per-Frame** (`update_bodies_physics`): +1. reset_spacecraft_tracking() - reset spacecraft handled flags +2. update_bodies_physics() - SOI checks, drift detection, propagation +3. compute_global_coordinates() +4. execute_pending_maneuvers() +5. update_spacecraft_physics() +6. compute_spacecraft_globals() +7. time += dt + +**Body Physics Per-Frame**: - Check SOI via find_dominant_body() - Handle transitions (compute global, update parent, compute local, reconstruct elements) - Check velocity drift (> 1e-6 m/s) and reconstruct if needed diff --git a/src/maneuver.cpp b/src/maneuver.cpp index 9ec426e..20a66ff 100644 --- a/src/maneuver.cpp +++ b/src/maneuver.cpp @@ -115,34 +115,8 @@ OrbitalElements preview_burn_result(const Spacecraft* craft, BurnDirection direc // TODO: add parabolic (Barker's equation) and hyperbolic branches. bool check_maneuver_trigger(Maneuver* maneuver, Spacecraft* craft, SimulationState* sim) { switch (maneuver->trigger_type) { - case TRIGGER_TIME: { - // Fire at the step that contains the trigger time. - // The orbit state is at sim->time (start of current step). - // We propagate forward to trigger_value, burn, then propagate - // the remaining time to reach sim->time + sim->dt. - if (sim->time > maneuver->trigger_value) { - // Trigger is before the start of this step — clamp to 0 - // (should have fired in an earlier step; fire immediately) - maneuver->scheduled_dt = 0.0; - return true; - } - if (sim->time + sim->dt <= maneuver->trigger_value) { - return false; - } - - double dt_to_burn = maneuver->trigger_value - sim->time; - - // Clamp to valid range [0, sim->dt] - if (dt_to_burn < 0.0) { - dt_to_burn = 0.0; - } - if (dt_to_burn > sim->dt) { - dt_to_burn = sim->dt; - } - - maneuver->scheduled_dt = dt_to_burn; - return true; - } + case TRIGGER_TIME: + return sim->time >= maneuver->trigger_value; case TRIGGER_TRUE_ANOMALY: { if (craft->parent_index < 0 || craft->parent_index >= sim->body_count) { diff --git a/src/simulation.cpp b/src/simulation.cpp index 146cb96..96046b8 100644 --- a/src/simulation.cpp +++ b/src/simulation.cpp @@ -329,8 +329,7 @@ void update_spacecraft_physics(SimulationState* sim) { craft->orbit = propagate_orbital_elements(craft->orbit, burn_dt, parent->mass); orbital_elements_to_cartesian(craft->orbit, parent->mass, &craft->local_position, &craft->local_velocity); - double burn_time = sim->time + burn_dt; - execute_maneuver(fired_maneuver, craft, sim, burn_time); + execute_maneuver(fired_maneuver, craft, sim, sim->time + burn_dt); double remaining_dt = sim->dt - burn_dt; craft->orbit = propagate_orbital_elements(craft->orbit, remaining_dt, parent->mass); diff --git a/src/test_utilities.cpp b/src/test_utilities.cpp index edd73ad..2df9181 100644 --- a/src/test_utilities.cpp +++ b/src/test_utilities.cpp @@ -166,23 +166,16 @@ bool compare_vec3(Vec3 a, Vec3 b, double tolerance) { fabs(a.z - b.z) <= tolerance; } -int dump_simulation_state(SimulationState* sim, const char* label, - char* buffer, int buffer_size) { - int offset = 0; +void dump_simulation_state(SimulationState* sim, const char* label) { + printf("\n=== %s (t=%.0f s) ===\n", label, sim->time); - offset += snprintf(buffer + offset, buffer_size - offset, - "\n=== %s (t=%.0f s) ===\n", label, sim->time); - - offset += snprintf(buffer + offset, buffer_size - offset, - "Bodies (%d):\n", sim->body_count); + printf("Bodies (%d):\n", sim->body_count); for (int i = 0; i < sim->body_count; i++) { - offset += snprintf(buffer + offset, buffer_size - offset, - " [%d] %s: mass=%.2e kg\n", - i, sim->bodies[i].name, sim->bodies[i].mass); + printf(" [%d] %s: mass=%.2e kg\n", + i, sim->bodies[i].name, sim->bodies[i].mass); } - offset += snprintf(buffer + offset, buffer_size - offset, - "Spacecraft (%d):\n", sim->craft_count); + printf("Spacecraft (%d):\n", sim->craft_count); for (int i = 0; i < sim->craft_count; i++) { Spacecraft* s = &sim->spacecraft[i]; double r = sqrt(s->local_position.x*s->local_position.x + @@ -191,28 +184,19 @@ int dump_simulation_state(SimulationState* sim, const char* label, double v = sqrt(s->local_velocity.x*s->local_velocity.x + s->local_velocity.y*s->local_velocity.y + s->local_velocity.z*s->local_velocity.z); - offset += snprintf(buffer + offset, buffer_size - offset, - " [%d] %s: r=%.1f v=%.1f nu=%.5f a=%.1f e=%.6f, omega=%.6f\n", - i, s->name, r, v, - s->orbit.true_anomaly, - s->orbit.semi_major_axis, - s->orbit.eccentricity, - s->orbit.argument_of_periapsis); - offset += snprintf(buffer + offset, buffer_size - offset, - " pos=(%.1f, %.1f, %.1f) vel=(%.1f, %.1f, %.1f)\n", - s->local_position.x, s->local_position.y, s->local_position.z, - s->local_velocity.x, s->local_velocity.y, s->local_velocity.z); + printf(" [%d] %s: r=%.1f v=%.1f nu=%.5f a=%.1f e=%.6f\n", + i, s->name, r, v, + s->orbit.true_anomaly, s->orbit.semi_major_axis, s->orbit.eccentricity); + printf(" pos=(%.1f, %.1f, %.1f) vel=(%.1f, %.1f, %.1f)\n", + s->local_position.x, s->local_position.y, s->local_position.z, + s->local_velocity.x, s->local_velocity.y, s->local_velocity.z); } - offset += snprintf(buffer + offset, buffer_size - offset, - "Maneuvers (%d):\n", sim->maneuver_count); + printf("Maneuvers (%d):\n", sim->maneuver_count); for (int i = 0; i < sim->maneuver_count; i++) { Maneuver* m = &sim->maneuvers[i]; - offset += snprintf(buffer + offset, buffer_size - offset, - " [%d] %s: craft=%d dir=%d dv=%.4f trigger=%d val=%.2f exec=%d\n", - i, m->name, m->craft_index, m->direction, m->delta_v, - m->trigger_type, m->trigger_value, m->executed); + printf(" [%d] %s: craft=%d dir=%d dv=%.4f trigger=%d val=%.2f exec=%d\n", + i, m->name, m->craft_index, m->direction, m->delta_v, + m->trigger_type, m->trigger_value, m->executed); } - - return offset; } diff --git a/src/test_utilities.h b/src/test_utilities.h index d71087a..71bfcf0 100644 --- a/src/test_utilities.h +++ b/src/test_utilities.h @@ -46,10 +46,7 @@ void destroy_orbit_tracker(OrbitTracker* tracker); bool compare_double(double a, double b, double tolerance); bool compare_vec3(Vec3 a, Vec3 b, double tolerance); -// Write simulation state to a caller-allocated buffer. -// Returns number of characters written (excluding null terminator). -// Caller must ensure buffer is large enough. -int dump_simulation_state(SimulationState* sim, const char* label, - char* buffer, int buffer_size); +// Debug helper: dump simulation state to console +void dump_simulation_state(SimulationState* sim, const char* label); #endif diff --git a/tests/compute_rendezvous_params.py b/tests/compute_rendezvous_params.py deleted file mode 100644 index 72f3c55..0000000 --- a/tests/compute_rendezvous_params.py +++ /dev/null @@ -1,204 +0,0 @@ -#!/usr/bin/env python3 -""" -Pre-compute Hohmann transfer rendezvous parameters for test validation. -Replicates the exact rendezvous module phasing logic from src/rendezvous.cpp. -Hardcoded from tests/test_rendezvous.toml — no TOML parser needed. - -Usage: python3 tests/compute_rendezvous_params.py -""" - -import math -import sys - -G = 6.67430e-11 - -# Central body -EARTH_MASS = 5.972e24 - -# Spacecraft orbits (from test_rendezvous.toml) -TARGET_R = 6.771e6 # 400 km altitude -TARGET_NU = 0.0 - -CHASER_R = 6.671e6 # 300 km altitude -CHASER_NU = 4.71238898038469 # 270 degrees - -MU = G * EARTH_MASS - - -def calc_mean_motion(radius, mass): - """n = sqrt(mu / a^3)""" - return math.sqrt(MU / (radius ** 3)) - - -def hohmann_transfer_time(r1, r2, mass): - """Half orbit of transfer ellipse.""" - a_transfer = (r1 + r2) / 2.0 - T_transfer = 2.0 * math.pi * math.sqrt(a_transfer ** 3 / MU) - return T_transfer / 2.0 - - -def required_separation(r1, r2, mass): - """ - Required angular separation at first burn. - chaser_pos - target_pos = target_angle - pi - """ - transfer_time = hohmann_transfer_time(r1, r2, mass) - n2 = calc_mean_motion(r2, mass) - target_angle = n2 * transfer_time - return target_angle - math.pi - - -def normalize_angle_2pi(angle): - """Normalize to [0, 2*pi).""" - while angle < 0.0: - angle += 2.0 * math.pi - while angle >= 2.0 * math.pi: - angle -= 2.0 * math.pi - return angle - - -def normalize_angle_pi(angle): - """Normalize to [-pi, pi].""" - angle = normalize_angle_2pi(angle) - while angle > math.pi: - angle -= 2.0 * math.pi - while angle < -math.pi: - angle += 2.0 * math.pi - return angle - - -def calculate_wait_time_for_hohmann(r1, r2, angular_separation, mass): - """ - Wait time before Hohmann transfer. - Positive = wait, negative = transfer already late. - """ - required_sep = required_separation(r1, r2, mass) - n1 = calc_mean_motion(r1, mass) - n2 = calc_mean_motion(r2, mass) - rel_angular_vel = n1 - n2 - - current_sep = normalize_angle_pi(angular_separation) - required_sep = normalize_angle_pi(required_sep) - - angle_to_close = required_sep - current_sep - - return angle_to_close / rel_angular_vel - - -def calculate_relative_orbit_period(r1, r2, mass): - """Time between consecutive phasing opportunities.""" - n1 = calc_mean_motion(r1, mass) - n2 = calc_mean_motion(r2, mass) - rel_angular_vel = abs(n1 - n2) - return 2.0 * math.pi / rel_angular_vel - - -def calculate_next_hohmann_wait_time(r1, r2, angular_separation, mass, min_wait_time): - """ - Like calculate_wait_time_for_hohmann, but advances to next phasing - opportunity if wait_time < min_wait_time. Always returns non-negative. - """ - wait_time = calculate_wait_time_for_hohmann(r1, r2, angular_separation, mass) - rel_period = calculate_relative_orbit_period(r1, r2, mass) - - while wait_time < min_wait_time: - wait_time += rel_period - - return wait_time - - -def main(): - print(f"Central body: Earth, mass = {EARTH_MASS:.6e} kg") - print(f"mu = {MU:.6e} m^3/s^2") - - print(f"\n=== INITIAL ORBITAL ELEMENTS ===") - print(f"Chaser_Lower: r = {CHASER_R:.6e} m, nu = {CHASER_NU:.6f} rad ({math.degrees(CHASER_NU):.2f} deg)") - print(f"Target: r = {TARGET_R:.6e} m, nu = {TARGET_NU:.6f} rad ({math.degrees(TARGET_NU):.2f} deg)") - - # Angular separation: chaser - target - angular_sep = CHASER_NU - TARGET_NU - angular_sep = normalize_angle_pi(angular_sep) - print(f"\nAngular separation (chaser - target): {angular_sep:.6f} rad ({math.degrees(angular_sep):.2f} deg)") - - # Mean motions - n1 = calc_mean_motion(CHASER_R, EARTH_MASS) - n2 = calc_mean_motion(TARGET_R, EARTH_MASS) - print(f"\nMean motions:") - print(f" n1 (chaser): {n1:.10f} rad/s") - print(f" n2 (target): {n2:.10f} rad/s") - print(f" n1 - n2: {n1 - n2:.10f} rad/s") - - # Orbital periods - p_chaser = 2.0 * math.pi / n1 - p_target = 2.0 * math.pi / n2 - print(f"\nOrbital periods:") - print(f" Chaser: {p_chaser:.2f} s ({p_chaser/3600:.2f} h)") - print(f" Target: {p_target:.2f} s ({p_target/3600:.2f} h)") - - # Hohmann transfer - tt = hohmann_transfer_time(CHASER_R, TARGET_R, EARTH_MASS) - a_t = (CHASER_R + TARGET_R) / 2.0 - print(f"\n=== HOHMANN TRANSFER ===") - print(f" Transfer semi-major axis: {a_t:.6e} m") - print(f" Transfer time: {tt:.6f} s ({tt/60:.2f} min)") - - # Required separation - req_sep = required_separation(CHASER_R, TARGET_R, EARTH_MASS) - req_sep_norm = normalize_angle_pi(req_sep) - print(f"\n=== REQUIRED SEPARATION ===") - print(f" Raw: {req_sep:.6f} rad ({math.degrees(req_sep):.2f} deg)") - print(f" Norm: {req_sep_norm:.6f} rad ({math.degrees(req_sep_norm):.2f} deg)") - - # Relative orbit period - rel_period = calculate_relative_orbit_period(CHASER_R, TARGET_R, EARTH_MASS) - print(f"\nRelative orbit period: {rel_period:.6f} s ({rel_period/3600:.2f} h)") - - # Detailed phasing calculation - print(f"\n=== PHASING CALCULATION ===") - current_sep = normalize_angle_pi(angular_sep) - print(f" Current separation (normalized): {current_sep:.6f} rad ({math.degrees(current_sep):.2f} deg)") - print(f" Required separation (normalized): {req_sep_norm:.6f} rad ({math.degrees(req_sep_norm):.2f} deg)") - - angle_to_close = req_sep_norm - current_sep - print(f" Angle to close: {angle_to_close:.6f} rad ({math.degrees(angle_to_close):.2f} deg)") - - wait_time = calculate_wait_time_for_hohmann(CHASER_R, TARGET_R, angular_sep, EARTH_MASS) - print(f" Raw wait_time: {wait_time:.6f} s ({wait_time/3600:.2f} h)") - - # Wait times for various DT values - dt_values = [0.1, 0.5, 1.0, 2.0, 5.0, 10.0] - print(f"\n=== WAIT TIME vs DT (via calculate_next_hohmann_wait_time) ===") - for dt in dt_values: - wt = calculate_next_hohmann_wait_time(CHASER_R, TARGET_R, angular_sep, EARTH_MASS, dt) - arrival = wt + tt - steps = int(arrival / dt) + 1 - print(f" DT={dt:6.1f} s: wait={wt:12.2f} s arrival={arrival:12.2f} s steps~{steps}") - - # Recommended values for TIME_STEP = 0.1 - dt = 0.1 - wt = calculate_next_hohmann_wait_time(CHASER_R, TARGET_R, angular_sep, EARTH_MASS, dt) - arrival = wt + tt - max_steps = int(arrival / dt) + 1000 - - print(f"\n=== RECOMMENDED FOR TEST (DT=0.1) ===") - print(f" wait_time: {wt:.2f} s") - print(f" arrival_time: {arrival:.2f} s") - print(f" expected_steps: {int(arrival / dt)}") - print(f" max_steps (with margin): {max_steps}") - print(f" safety_limit (1 yr): {3600.0 * 24.0 * 365.0:.2f} s") - print(f"\n Milestone step indices:") - print(f" just_before_departure: {int(wt / dt)}") - print(f" after_departure: {int(wt / dt) + 1}") - print(f" just_before_arrival: {int(arrival / dt)}") - - # Verify against C++ test output - print(f"\n=== COMPARISON WITH C++ TEST OUTPUT ===") - print(f" Python wait_time: {wt:.2f} s") - print(f" C++ test wait_time: 60062.7 s") - print(f" Python arrival: {arrival:.2f} s") - print(f" C++ test arrival: 62804.5 s") - print(f" Match: {abs(wt - 60062.7) < 0.1 and abs(arrival - 62804.5) < 0.1}") - - -if __name__ == '__main__': - main() diff --git a/tests/simulate_rendezvous.py b/tests/simulate_rendezvous.py deleted file mode 100644 index 5b84bae..0000000 --- a/tests/simulate_rendezvous.py +++ /dev/null @@ -1,413 +0,0 @@ -#!/usr/bin/env python3 -""" -Full analytical propagation simulation of the Hohmann rendezvous scenario. -Replicates the exact physics from src/orbital_mechanics.cpp and src/maneuver.cpp. - -Step-by-step trace to find where the 11,578 km separation comes from. - -Usage: python3 tests/simulate_rendezvous.py -""" - -import math -import sys - -G = 6.67430e-11 -MU = G * 5.972e24 # Earth - -# ---- Vector operations ---- -def vadd(a, b): return (a[0]+b[0], a[1]+b[1], a[2]+b[2]) -def vsub(a, b): return (a[0]-b[0], a[1]-b[1], a[2]-b[2]) -def vscale(v, s): return (v[0]*s, v[1]*s, v[2]*s) -def vmag(v): return math.sqrt(v[0]**2 + v[1]**2 + v[2]**2) -def vdot(a, b): return a[0]*b[0] + a[1]*b[1] + a[2]*b[2] -def vcross(a, b): return ( - a[1]*b[2] - a[2]*b[1], - a[2]*b[0] - a[0]*b[2], - a[0]*b[1] - a[1]*b[0] -) -def vnorm(v): - m = vmag(v) - if m < 1e-15: return (0, 0, 0) - return (v[0]/m, v[1]/m, v[2]/m) - -def normalize_angle(angle): - while angle < 0.0: angle += 2*math.pi - while angle >= 2*math.pi: angle -= 2*math.pi - return angle - -def normalize_angle_2pi(angle): - while angle < 0.0: angle += 2*math.pi - while angle >= 2*math.pi: angle -= 2*math.pi - return angle - -def normalize_angle_pi(angle): - angle = normalize_angle_2pi(angle) - while angle > math.pi: angle -= 2*math.pi - while angle < -math.pi: angle += 2*math.pi - return angle - -# ---- Kepler equation solvers (exact C++ logic) ---- -def get_initial_trial_value(mean_anomaly, eccentricity): - return (mean_anomaly + eccentricity * math.sin(mean_anomaly) - + ((eccentricity**2 / 2.0) * math.sin(2.0 * mean_anomaly))) - -def solve_kepler_elliptical(mean_anomaly, eccentricity): - E = get_initial_trial_value(mean_anomaly, eccentricity) - E_prev = E + 2.0e-10 - for _ in range(50): - if abs(E - E_prev) < 1e-10: - break - E_prev = E - sin_E = math.sin(E) - E = E - (E - eccentricity * sin_E - mean_anomaly) / (1.0 - eccentricity * math.cos(E)) - return E - -def eccentric_to_true_anomaly(eccentric_anomaly, eccentricity): - if abs(1.0 - eccentricity) < 0.01: - E = eccentric_anomaly - e = eccentricity - cos_E = math.cos(E) - sin_E = math.sin(E) - denom = 1.0 - e * cos_E - cos_nu = max(-1.0, min(1.0, (cos_E - e) / denom)) - sin_nu = max(-1.0, min(1.0, sin_E * math.sqrt(1.0 - e*e) / denom)) - return math.atan2(sin_nu, cos_nu) - tan_half_E = math.tan(eccentric_anomaly / 2.0) - tan_half_nu = math.sqrt((1.0 + eccentricity) / (1.0 - eccentricity)) * tan_half_E - return 2.0 * math.atan(tan_half_nu) - -# ---- Propagation (exact C++ propagate_orbital_elements) ---- -def propagate(elements, dt, parent_mass): - a = elements['a'] - e = elements['e'] - nu = elements['nu'] - mu = MU # fixed for this sim - - if e < 1.0: - n = math.sqrt(mu / a**3) - E = 2.0 * math.atan(math.sqrt((1.0 - e) / (1.0 + e)) * math.tan(nu / 2.0)) - M = E - e * math.sin(E) - M = M + n * dt - E_new = get_initial_trial_value(M, e) - E_prev = E_new + 2.0e-10 - for _ in range(50): - if abs(E_new - E_prev) < 1e-10: - break - E_prev = E_new - sin_E = math.sin(E_new) - E_new = E_new - (E_new - e * sin_E - M) / (1.0 - e * math.cos(E_new)) - nu_new = 2.0 * math.atan(math.sqrt((1.0 + e) / (1.0 - e)) * math.tan(E_new / 2.0)) - result = dict(elements) - result['nu'] = nu_new - return result - else: - # Hyperbolic (not needed for this test) - raise NotImplementedError("hyperbolic propagation not needed") - -# ---- Cartesian from orbital elements ---- -def orbital_to_cartesian(elements, parent_mass): - a = elements['a'] - e = elements['e'] - nu = elements['nu'] - inc = elements['inc'] - Omega = elements['Omega'] - omega = elements['omega'] - mu = MU - - p = a * (1.0 - e*e) - r = p / (1.0 + e * math.cos(nu)) - - # Orbital plane position/velocity - x_orb = r * math.cos(nu) - y_orb = r * math.sin(nu) - - vx_orb = -math.sqrt(mu / p) * math.sin(nu) - vy_orb = math.sqrt(mu / p) * (e + math.cos(nu)) - - # z-x-z rotation: Rz(Omega) * Rx(inc) * Rz(omega) - # Apply Rz(omega) first - cos_w = math.cos(omega) - sin_w = math.sin(omega) - x1 = x_orb * cos_w - y_orb * sin_w - y1 = x_orb * sin_w + y_orb * cos_w - - # Then Rx(inc) - cos_i = math.cos(inc) - sin_i = math.sin(inc) - x2 = x1 - y2 = y1 * cos_i - z2 = y1 * sin_i - - # Then Rz(Omega) - cos_O = math.cos(Omega) - sin_O = math.sin(Omega) - pos = (x2 * cos_O - y2 * sin_O, - x2 * sin_O + y2 * cos_O, - z2) - - # Same rotation for velocity - vx1 = vx_orb * cos_w - vy_orb * sin_w - vy1 = vx_orb * sin_w + vy_orb * cos_w - vx2 = vx1 - vy2 = vy1 * cos_i - vz2 = vy1 * sin_i - vel = (vx2 * cos_O - vy2 * sin_O, - vx2 * sin_O + vy2 * cos_O, - vz2) - - return pos, vel - -# ---- Cartesian to orbital elements ---- -def cartesian_to_elements(pos, vel, parent_mass): - mu = MU - r = vmag(pos) - v = vmag(vel) - - # Specific orbital energy - specific_energy = -mu / r + v**2 / 2.0 - - # Semi-major axis - if abs(specific_energy) < 1e-10: - a = 1e10 - else: - a = -mu / (2.0 * specific_energy) - - # Angular momentum - h_vec = vcross(pos, vel) - h = vmag(h_vec) - - # Eccentricity vector - r_dot_v = vdot(pos, vel) - e_vec = ((v**2 - mu/r) * pos[0] - r_dot_v * vel[0]) / mu, \ - ((v**2 - mu/r) * pos[1] - r_dot_v * vel[1]) / mu, \ - ((v**2 - mu/r) * pos[2] - r_dot_v * vel[2]) / mu - e = vmag(e_vec) - - # True anomaly - if e < 1e-10: - nu = 0.0 - else: - cos_nu = vdot(pos, e_vec) / (r * e) - cos_nu = max(-1.0, min(1.0, cos_nu)) - if abs(cos_nu) > 1.0 - 1e-10: - h_cross_e = vcross(h_vec, e_vec) - denom = r * e * h - sin_nu = vdot(pos, h_cross_e) / denom if denom > 1e-10 else 0.0 - else: - r_cross_h = vcross(pos, h_vec) - denom = r * e * h - sin_nu = vdot(r_cross_h, e_vec) / denom if denom > 1e-10 else 0.0 - nu = math.atan2(sin_nu, cos_nu) - if nu == -math.pi: - nu = math.pi - nu = normalize_angle(nu) - - # Inclination - if h > 1e-10: - i = math.acos(h_vec[2] / h) - else: - i = 0.0 - - # RAAN - n_vec = (0, 0, 1) - n = vcross(n_vec, h_vec) - n_mag = vmag(n) - if n_mag > 1e-10: - Omega = math.acos(n[0] / n_mag) - if n[1] < 0.0: - Omega = 2*math.pi - Omega - else: - Omega = 0.0 - - # Argument of periapsis - if e > 1e-10 and n_mag > 1e-10 and i > 0.01: - cos_omega = vdot(e_vec, n) / (e * n_mag) - n_cross_e = vcross(n, e_vec) - sin_omega = vdot(n_cross_e, h_vec) / (e * n_mag * h) - omega = math.atan2(sin_omega, cos_omega) - if omega < 0: omega += 2*math.pi - elif e > 1e-10: - omega = math.atan2(e_vec[1], e_vec[0]) - if omega < 0: omega += 2*math.pi - else: - omega = 0.0 - - return {'a': a, 'e': e, 'nu': nu, 'inc': i, 'Omega': Omega, 'omega': omega} - -# ---- Hohmann transfer calculations ---- -def hohmann_transfer_time(r1, r2): - a_t = (r1 + r2) / 2.0 - T = 2*math.pi * math.sqrt(a_t**3 / MU) - return T / 2.0 - -def required_separation(r1, r2): - tt = hohmann_transfer_time(r1, r2) - n2 = math.sqrt(MU / r2**3) - target_angle = n2 * tt - return target_angle - math.pi - -def calc_mean_motion(radius): - return math.sqrt(MU / radius**3) - -def calculate_wait_time_for_hohmann(r1, r2, angular_separation): - required_sep = required_separation(r1, r2) - n1 = calc_mean_motion(r1) - n2 = calc_mean_motion(r2) - rel_angular_vel = n1 - n2 - - current_sep = normalize_angle_pi(angular_separation) - required_sep = normalize_angle_pi(required_sep) - - angle_to_close = required_sep - current_sep - return angle_to_close / rel_angular_vel - -def relative_orbit_period(r1, r2): - n1 = calc_mean_motion(r1) - n2 = calc_mean_motion(r2) - return 2*math.pi / abs(n1 - n2) - -def calculate_next_hohmann_wait_time(r1, r2, angular_sep, dt): - wait_time = calculate_wait_time_for_hohmann(r1, r2, angular_sep) - rel_period = relative_orbit_period(r1, r2) - while wait_time < dt: - wait_time += rel_period - return wait_time - -# ---- Burn application ---- -def apply_burn(pos, vel, direction, delta_v, parent_mass): - """Apply impulsive burn in local orbital frame.""" - # direction: 'prograde', 'retrograde', 'normal' - if direction == 'prograde': - d = vnorm(vel) - elif direction == 'retrograde': - d = vscale(vnorm(vel), -1) - elif direction == 'normal': - h = vcross(pos, vel) - d = vnorm(h) - else: - raise ValueError(f"Unknown direction: {direction}") - - new_vel = vadd(vel, vscale(d, delta_v)) - return pos, new_vel - -# ---- Full rendezvous scenario ---- -def main(): - # Initial conditions from test_rendezvous.toml - TARGET_R = 6.771e6 - TARGET_NU = 0.0 - CHASER_R = 6.671e6 - CHASER_NU = 4.71238898038469 # 270 degrees - - print("=== INITIAL STATE ===") - print(f"Chaser: r={CHASER_R:.1f} m, nu={math.degrees(CHASER_NU):.1f} deg") - print(f"Target: r={TARGET_R:.1f} m, nu={math.degrees(TARGET_NU):.1f} deg") - - # Create orbital elements (coplanar, circular) - chaser = {'a': CHASER_R, 'e': 0.0, 'nu': CHASER_NU, - 'inc': 0.0, 'Omega': 0.0, 'omega': 0.0} - target = {'a': TARGET_R, 'e': 0.0, 'nu': TARGET_NU, - 'inc': 0.0, 'Omega': 0.0, 'omega': 0.0} - - chaser_pos, chaser_vel = orbital_to_cartesian(chaser, 5.972e24) - target_pos, target_vel = orbital_to_cartesian(target, 5.972e24) - - print(f"Chaser pos: {chaser_pos}, vel: {vmag(chaser_vel):.1f} m/s") - print(f"Target pos: {target_pos}, vel: {vmag(target_vel):.1f} m/s") - - # Angular separation - angular_sep = chaser['nu'] - target['nu'] - angular_sep = normalize_angle_pi(angular_sep) - print(f"\nAngular separation (chaser - target): {math.degrees(angular_sep):.1f} deg") - - # Hohmann parameters - hohmann_tt = hohmann_transfer_time(CHASER_R, TARGET_R) - dv1 = math.sqrt(MU * (2/CHASER_R - 2/(CHASER_R + TARGET_R))) - math.sqrt(MU/CHASER_R) - dv2 = math.sqrt(MU/TARGET_R) - math.sqrt(MU * (2/TARGET_R - 2/(CHASER_R + TARGET_R))) - print(f"\nHohmann transfer: tt={hohmann_tt:.1f} s, dv1={dv1:.2f} m/s, dv2={dv2:.2f} m/s") - - # Phasing - dt = 0.1 - wait_time = calculate_next_hohmann_wait_time(CHASER_R, TARGET_R, angular_sep, dt) - arrival_time = wait_time + hohmann_tt - print(f"Wait time: {wait_time:.2f} s") - print(f"Arrival time: {arrival_time:.2f} s") - print(f"Steps: {int(arrival_time/dt)}") - - # ---- Run simulation ---- - print(f"\n=== SIMULATION (dt={dt}) ===") - sim_time = 0.0 - steps = 0 - chaser_executed = False - arrival_executed = False - - while steps < int(arrival_time / dt) + 1000: - chaser, target, chaser_pos, chaser_vel, target_pos, target_vel = \ - update_simulation(chaser, target, sim_time, dt, dv1, dv2, wait_time, arrival_time, - chaser_pos, chaser_vel, target_pos, target_vel, - chaser_executed, arrival_executed) - sim_time += dt - steps += 1 - - if steps % 100000 == 0: - c_sep = vmag(vsub(chaser_pos, target_pos)) - c_r = vmag(chaser_pos) - t_r = vmag(target_pos) - c_nu = chaser['nu'] - t_nu = target['nu'] - print(f" step={steps:7d} t={sim_time:10.1f}s chaser_r={c_r:.0f} nu={math.degrees(c_nu):7.1f}° " - f"target_r={t_r:.0f} nu={math.degrees(t_nu):7.1f}° sep={c_sep:.0f}m") - - if not chaser_executed and sim_time >= wait_time: - # Execute departure burn - print(f"\n *** DEPARTURE BURN at t={sim_time:.1f}s ***") - print(f" Before: pos={chaser_pos}, vel={chaser_vel}") - chaser_pos, chaser_vel = apply_burn(chaser_pos, chaser_vel, 'prograde', dv1, 5.972e24) - print(f" After: pos={chaser_pos}, vel={chaser_vel}") - chaser = cartesian_to_elements(chaser_pos, chaser_vel, 5.972e24) - chaser_executed = True - print(f" Chaser: r={vmag(chaser_pos):.0f} nu={math.degrees(chaser['nu']):.1f}° " - f"a={chaser['a']:.0f} e={chaser['e']:.6f}") - - if not arrival_executed and sim_time >= arrival_time: - # Execute arrival burn - chaser_pos, chaser_vel = apply_burn(chaser_pos, chaser_vel, 'prograde', dv2, 5.972e24) - chaser = cartesian_to_elements(chaser_pos, chaser_vel, 5.972e24) - arrival_executed = True - print(f"\n *** ARRIVAL BURN at t={sim_time:.1f}s ***") - print(f" Chaser: r={vmag(chaser_pos):.0f} nu={math.degrees(chaser['nu']):.1f}° " - f"a={chaser['a']:.0f} e={chaser['e']:.6f}") - - # Final comparison - c_sep = vmag(vsub(chaser_pos, target_pos)) - c_r = vmag(chaser_pos) - t_r = vmag(target_pos) - c_vel = vmag(chaser_vel) - t_vel = vmag(target_vel) - print(f"\n=== FINAL STATE ===") - print(f"Chaser: r={c_r:.0f} m, nu={chaser['nu']:.6f} rad ({math.degrees(chaser['nu']):.1f}°)") - print(f" pos={chaser_pos}, vel={chaser_vel}") - print(f"Target: r={t_r:.0f} m, nu={target['nu']:.6f} rad ({math.degrees(target['nu']):.1f}°)") - print(f" pos={target_pos}, vel={target_vel}") - print(f"Separation: {c_sep:.0f} m") - print(f"Speed: chaser={c_vel:.2f} target={t_vel:.2f} m/s") - print(f"Radius error: {abs(c_r - t_r):.6f} m") - print(f"Chaser eccentricity: {chaser['e']:.15f}") - print(f"Target eccentricity: {target['e']:.15f}") - break - - -def update_simulation(chaser, target, sim_time, dt, dv1, dv2, wait_time, arrival_time, - chaser_pos, chaser_vel, target_pos, target_vel, - chaser_executed, arrival_executed): - """Propagate one timestep for both spacecraft.""" - chaser = propagate(chaser, dt, 5.972e24) - target = propagate(target, dt, 5.972e24) - - chaser_pos, chaser_vel = orbital_to_cartesian(chaser, 5.972e24) - target_pos, target_vel = orbital_to_cartesian(target, 5.972e24) - - return chaser, target, chaser_pos, chaser_vel, target_pos, target_vel - - -if __name__ == '__main__': - main() diff --git a/tests/test_rendezvous.cpp b/tests/test_rendezvous.cpp index 149c1ab..91e31f7 100644 --- a/tests/test_rendezvous.cpp +++ b/tests/test_rendezvous.cpp @@ -27,9 +27,7 @@ static int find_spacecraft_by_name(SimulationState* sim, const char* name) { return -1; } -// ============================================================================ -// Test-only output helper -// ============================================================================ +// ── Test-only output helper ────────────────────────────────────────────────── struct TestOutput { char buf[32768]; @@ -38,34 +36,6 @@ struct TestOutput { void dump_state(SimulationState* sim, const char* label) { int n = dump_simulation_state(sim, label, buf + offset, sizeof(buf) - offset); if (n > 0) offset += n; - - int target_idx = -1, chaser_idx = -1; - for (int i = 0; i < sim->craft_count; i++) { - if (strcmp(sim->spacecraft[i].name, "Target_Satellite") == 0) - target_idx = i; - if (strcmp(sim->spacecraft[i].name, "Chaser_Lower") == 0) - chaser_idx = i; - } - - if (target_idx >= 0 && chaser_idx >= 0) { - Vec3 target_pos = sim->spacecraft[target_idx].local_position; - Vec3 chaser_pos = sim->spacecraft[chaser_idx].local_position; - - double target_angle = atan2(target_pos.y, target_pos.x); - double chaser_angle = atan2(chaser_pos.y, chaser_pos.x); - double angular_sep = chaser_angle - target_angle; - while (angular_sep > M_PI) angular_sep -= 2.0 * M_PI; - while (angular_sep < -M_PI) angular_sep += 2.0 * M_PI; - - Vec3 diff = vec3_sub(chaser_pos, target_pos); - double sep_mag = vec3_magnitude(diff); - - n = snprintf(buf + offset, sizeof(buf) - offset, - " Angular separation (Chaser-Target): %.6f rad (%.4f deg)\n" - " Separation magnitude: %.2f m\n", - angular_sep, angular_sep * 180.0 / M_PI, sep_mag); - if (n > 0) offset += n; - } } }; @@ -565,7 +535,7 @@ SCENARIO("Hohmann transfer rendezvous with validation", "[rendezvous_hohmann][in if (i == 0) out.dump_state(sim, "T=0 (initial)"); if (i == static_cast(wait_time / sim->dt)) out.dump_state(sim, "JUST BEFORE DEPARTURE"); if (i == static_cast(wait_time / sim->dt) + 1) out.dump_state(sim, "AFTER DEPARTURE BURN"); - if (i == static_cast(arrival_time / sim->dt) - 1) out.dump_state(sim, "JUST BEFORE ARRIVAL BURN"); + if (i == static_cast(arrival_time / sim->dt)) out.dump_state(sim, "JUST BEFORE ARRIVAL"); if (sim->maneuvers[arr_idx].executed && !transfer_complete) { out.dump_state(sim, "AFTER ARRIVAL BURN"); transfer_complete = true; @@ -573,8 +543,6 @@ SCENARIO("Hohmann transfer rendezvous with validation", "[rendezvous_hohmann][in } } - INFO(out.buf); - // Verify rendezvous quality double final_radius = vec3_magnitude(chaser->local_position); double radius_error = fabs(final_radius - r2); @@ -598,6 +566,7 @@ SCENARIO("Hohmann transfer rendezvous with validation", "[rendezvous_hohmann][in INFO(" Target speed: " << target_speed << " m/s"); INFO(" Separation: " << separation_distance << " m"); INFO(" Relative velocity: " << relative_velocity << " m/s"); + INFO(out.buf); // Verify maneuvers executed REQUIRE(sim->maneuvers[dep_idx].executed);