claudes_game/tests/test_extreme_orientation_mixed.cpp

#include <catch2/catch_test_macros.hpp>
#include <catch2/matchers/catch_matchers_floating_point.hpp>
#include "../src/physics.h"
#include "../src/orbital_mechanics.h"
#include "../src/simulation.h"
#include "../src/config_loader.h"
#include <cmath>
#include <array>

using Catch::Matchers::WithinAbs;

SCENARIO("Extreme orientation conversion accuracy and rotation matrix properties",
         "[extreme][orientation][mixed]") {
    const double TIME_STEP = 60.0;
    const double parent_mass = 5.972e24;
    const double mu = G * parent_mass;

    SimulationState* sim = create_simulation(10, 5, 0, TIME_STEP);
    REQUIRE(load_system_config(sim, "tests/test_extreme_orientation_mixed.toml"));

    Spacecraft* sc0 = &sim->spacecraft[0];
    Spacecraft* sc1 = &sim->spacecraft[1];
    Spacecraft* sc2 = &sim->spacecraft[2];
    Spacecraft* sc3 = &sim->spacecraft[3];
    Spacecraft* sc4 = &sim->spacecraft[4];

    // Tolerances
    const double R_TOL = 1e-6;
    const double V_TOL = 1e-6;
    const double E_TOL = 1e-12;
    const double ANG_TOL = 1e-12;
    const double REL_TOL = 1e-8;
    const double VDOT_TOL = 1e-3;
    const double MAT_TOL = 1e-10;

    // Precomputed periapsis radii
    const double r_peri0 = sc0->orbit.semi_major_axis * (1.0 - sc0->orbit.eccentricity);   // 7.5e6
    const double r_peri1 = sc1->orbit.semi_major_axis * (1.0 - sc1->orbit.eccentricity);   // 1.0e7
    const double r_peri2 = sc2->orbit.semi_major_axis * (1.0 - sc2->orbit.eccentricity);   // 7.0e6
    const double r_peri3 = sc3->orbit.semi_major_axis * (1.0 - sc3->orbit.eccentricity);   // 8.0e6
    const double r_peri4 = sc4->orbit.semi_major_axis * (1.0 - sc4->orbit.eccentricity);   // 8.0e6

    // Precomputed apoapsis radii (elliptical only)
    const double r_apo0 = sc0->orbit.semi_major_axis * (1.0 + sc0->orbit.eccentricity);    // 9.25e7
    const double r_apo1 = sc1->orbit.semi_major_axis * (1.0 + sc1->orbit.eccentricity);    // 3.0e7
    const double r_apo2 = sc2->orbit.semi_major_axis * (1.0 + sc2->orbit.eccentricity);    // 1.393e9


    // Helper: convert elements to cartesian at given true anomaly
    auto convert_at_nu = [&](Spacecraft* craft, double nu) {
        craft->orbit.true_anomaly = nu;
        orbital_elements_to_cartesian(craft->orbit, parent_mass, &craft->local_position, &craft->local_velocity);
    };

    // Helper: vis-viva check
    auto check_visviva = [&](double r, double v, double a) {
        const double expected_v_sq = mu * (2.0 / r - 1.0 / a);
        // Safety: expected_v_sq must be positive for sqrt (guaranteed for all elliptical orbits)
        REQUIRE(expected_v_sq > 0.0);
        const double expected_v = sqrt(expected_v_sq);
        const double rel_err = fabs(v - expected_v) / expected_v;
        INFO("v=" << v << " m/s, v_exp=" << expected_v << " m/s, rel_err=" << rel_err);
        REQUIRE_THAT(rel_err, WithinAbs(0.0, REL_TOL));
    };

    // Helper: round-trip check
    auto roundtrip = [&](double a, double e, double nu, double inc, double O, double w) {
        OrbitalElements elements = {};
        elements.semi_major_axis = a;
        elements.eccentricity = e;
        elements.true_anomaly = nu;
        elements.inclination = inc;
        elements.longitude_of_ascending_node = O;
        elements.argument_of_periapsis = w;
        Vec3 pos, vel;
        orbital_elements_to_cartesian(elements, parent_mass, &pos, &vel);
        OrbitalElements recovered = cartesian_to_orbital_elements(pos, vel, parent_mass);
        return recovered;
    };

    SECTION("periapsis position for extreme orbits") {
        convert_at_nu(sc0, 0.0);
        const double r0 = vec3_magnitude(sc0->local_position);
        const double v0 = vec3_magnitude(sc0->local_velocity);
        INFO("sc0 (i=1.2, e=0.85): r=" << r0 << " m, v=" << v0 << " m/s");
        REQUIRE_THAT(r0, WithinAbs(r_peri0, R_TOL));
        check_visviva(r0, v0, sc0->orbit.semi_major_axis);

        convert_at_nu(sc1, 0.0);
        const double r1 = vec3_magnitude(sc1->local_position);
        const double v1 = vec3_magnitude(sc1->local_velocity);
        INFO("sc1 (i=1.4, e=0.5): r=" << r1 << " m, v=" << v1 << " m/s");
        REQUIRE_THAT(r1, WithinAbs(r_peri1, R_TOL));
        check_visviva(r1, v1, sc1->orbit.semi_major_axis);

        convert_at_nu(sc2, 0.0);
        const double r2 = vec3_magnitude(sc2->local_position);
        const double v2 = vec3_magnitude(sc2->local_velocity);
        INFO("sc2 (i=0.5, e=0.99): r=" << r2 << " m, v=" << v2 << " m/s");
        REQUIRE_THAT(r2, WithinAbs(r_peri2, R_TOL));
        check_visviva(r2, v2, sc2->orbit.semi_major_axis);
    }

    SECTION("velocity at apsides for extreme orbits") {
        // sc0 at periapsis
        convert_at_nu(sc0, 0.0);
        const double v0p = vec3_magnitude(sc0->local_velocity);
        convert_at_nu(sc0, M_PI);
        const double v0a = vec3_magnitude(sc0->local_velocity);
        INFO("sc0: v_peri=" << v0p << " m/s, v_apo=" << v0a << " m/s");
        REQUIRE_THAT(v0p, WithinAbs(9915.577056, V_TOL));
        REQUIRE_THAT(v0a, WithinAbs(803.965707, V_TOL));

        // sc1 at periapsis
        convert_at_nu(sc1, 0.0);
        const double v1p = vec3_magnitude(sc1->local_velocity);
        convert_at_nu(sc1, M_PI);
        const double v1a = vec3_magnitude(sc1->local_velocity);
        INFO("sc1: v_peri=" << v1p << " m/s, v_apo=" << v1a << " m/s");
        REQUIRE_THAT(v1p, WithinAbs(7732.294575, V_TOL));
        REQUIRE_THAT(v1a, WithinAbs(2577.431525, V_TOL));

        // sc2 at periapsis
        convert_at_nu(sc2, 0.0);
        const double v2p = vec3_magnitude(sc2->local_velocity);
        convert_at_nu(sc2, M_PI);
        const double v2a = vec3_magnitude(sc2->local_velocity);
        INFO("sc2: v_peri=" << v2p << " m/s, v_apo=" << v2a << " m/s");
        REQUIRE_THAT(v2p, WithinAbs(10644.867979, V_TOL));
        REQUIRE_THAT(v2a, WithinAbs(53.491799, V_TOL));
    }

    SECTION("apoapsis position for extreme orbits") {
        convert_at_nu(sc0, M_PI);
        const double r0 = vec3_magnitude(sc0->local_position);
        INFO("sc0: r_apo=" << r0 << " m, expected=" << r_apo0 << " m");
        REQUIRE_THAT(r0, WithinAbs(r_apo0, R_TOL));

        convert_at_nu(sc1, M_PI);
        const double r1 = vec3_magnitude(sc1->local_position);
        INFO("sc1: r_apo=" << r1 << " m, expected=" << r_apo1 << " m");
        REQUIRE_THAT(r1, WithinAbs(r_apo1, R_TOL));

        convert_at_nu(sc2, M_PI);
        const double r2 = vec3_magnitude(sc2->local_position);
        INFO("sc2: r_apo=" << r2 << " m, expected=" << r_apo2 << " m");
        REQUIRE_THAT(r2, WithinAbs(r_apo2, R_TOL));
    }

    SECTION("vis-viva accuracy at multiple true anomalies") {
        const std::array<double, 4> true_anomalies = {0.0, M_PI / 2.0, M_PI, 3.0 * M_PI / 2.0};

        for (int i = 0; i < 5; i++) {
            Spacecraft* craft = &sim->spacecraft[i];
            const double a = craft->orbit.semi_major_axis;
            const double e = craft->orbit.eccentricity;

            INFO("Spacecraft " << i << ": e=" << e << ", a=" << a);

            for (int j = 0; j < 4; j++) {
                double nu = true_anomalies[j];
                craft->orbit.true_anomaly = nu;
                Vec3 pos, vel;
                orbital_elements_to_cartesian(craft->orbit, parent_mass, &pos, &vel);

                const double r = vec3_magnitude(pos);
                const double v = vec3_magnitude(vel);

                const double expected_v_sq = mu * (2.0 / r - 1.0 / a);
                // All spacecraft have elliptical orbits (e < 1), so vis-viva always yields v² > 0
                const double expected_v = sqrt(expected_v_sq);
                const double rel_err = fabs(v - expected_v) / expected_v;
                INFO("  nu=" << nu << " rad: v=" << v << " m/s, rel_err=" << rel_err);
                REQUIRE_THAT(rel_err, WithinAbs(0.0, REL_TOL));
            }
        }
    }

    SECTION("apsidal velocity orthogonality") {
        const std::array<double, 2> apsides = {0.0, M_PI};

        for (int i = 0; i < 5; i++) {
            Spacecraft* craft = &sim->spacecraft[i];
            INFO("Spacecraft " << i << ": e=" << craft->orbit.eccentricity
                     << ", i=" << craft->orbit.inclination << " rad");

            for (int j = 0; j < 2; j++) {
                craft->orbit.true_anomaly = apsides[j];
                Vec3 pos, vel;
                orbital_elements_to_cartesian(craft->orbit, parent_mass, &pos, &vel);

                const double pos_dot_vel = vec3_dot(pos, vel);
                const double h = vec3_magnitude(vec3_cross(pos, vel));

                INFO("  nu=" << apsides[j] << " rad: pos·vel=" << pos_dot_vel << ", |h|=" << h);
                REQUIRE_THAT(pos_dot_vel, WithinAbs(0.0, VDOT_TOL));
                // Angular momentum must be non-zero for a valid orbit
                REQUIRE(h > 0.0);
            }
        }
    }

    SECTION("Omega=0 singularity handling") {
        convert_at_nu(sc3, 0.0);
        const double r = vec3_magnitude(sc3->local_position);
        INFO("sc3 (Omega=0): r=" << r << " m");
        INFO("  pos=(" << sc3->local_position.x << ", " << sc3->local_position.y << ", " << sc3->local_position.z << ")");

        Vec3 pos, vel;
        orbital_elements_to_cartesian(sc3->orbit, parent_mass, &pos, &vel);

        // Rotation matrix at Omega=0 must produce a valid position in the x-y plane
        REQUIRE(pos.x > 0.0);
        REQUIRE_THAT(r, WithinAbs(r_peri3, R_TOL));

        const double h = vec3_magnitude(vec3_cross(pos, vel));
        INFO("  |h|=" << h);
        // Angular momentum must be non-zero for a valid orbit
        REQUIRE(h > 0.0);
    }

    SECTION("Arg_peri=0 singularity handling") {
        convert_at_nu(sc4, 0.0);
        const double r = vec3_magnitude(sc4->local_position);
        INFO("sc4 (w=0): r=" << r << " m, expected=" << r_peri4 << " m");
        REQUIRE_THAT(r, WithinAbs(r_peri4, R_TOL));

        const std::array<double, 4> true_anomalies = {0.0, M_PI / 2.0, M_PI, 3.0 * M_PI / 2.0};
        const std::array<double, 4> expected_r = {8.0e6, 1.44e7, 7.2e7, 1.44e7};
        const std::array<double, 4> expected_v = {9470.088125, 6737.572312, 1052.232014, 6737.572312};
        for (int j = 0; j < 4; j++) {
            sc4->orbit.true_anomaly = true_anomalies[j];
            Vec3 pos, vel;
            orbital_elements_to_cartesian(sc4->orbit, parent_mass, &pos, &vel);
            const double r_j = vec3_magnitude(pos);
            const double v_j = vec3_magnitude(vel);
            INFO("  nu=" << true_anomalies[j] << " rad: r=" << r_j << " m, v=" << v_j << " m/s");
            REQUIRE_THAT(r_j, WithinAbs(expected_r[j], R_TOL));
            REQUIRE_THAT(v_j, WithinAbs(expected_v[j], V_TOL));
        }
    }

    SECTION("round-trip conversion accuracy") {
        const std::array<Spacecraft*, 5> crafts = {sc0, sc1, sc2, sc3, sc4};

        for (int i = 0; i < 5; i++) {
            const Spacecraft* craft = crafts[i];
            const double a = craft->orbit.semi_major_axis;
            const double e = craft->orbit.eccentricity;
            const double inc = craft->orbit.inclination;
            const double O = craft->orbit.longitude_of_ascending_node;
            const double w = craft->orbit.argument_of_periapsis;

            INFO("Spacecraft " << i << ": " << craft->name);

            const OrbitalElements recovered = roundtrip(a, e, 0.0, inc, O, w);

            INFO("  e: " << e << " -> " << recovered.eccentricity);
            INFO("  i: " << inc << " -> " << recovered.inclination);
            INFO("  a: " << a << " -> " << recovered.semi_major_axis);

            REQUIRE_THAT(recovered.semi_major_axis, WithinAbs(a, fabs(a) * 0.01));
            REQUIRE_THAT(recovered.eccentricity, WithinAbs(e, E_TOL));
            REQUIRE_THAT(recovered.inclination, WithinAbs(inc, ANG_TOL));

            if (O > 1e-6 || O < -1e-6) {
                REQUIRE_THAT(recovered.longitude_of_ascending_node, WithinAbs(O, ANG_TOL));
            }

            if (w > 1e-6 || w < -1e-6) {
                REQUIRE_THAT(recovered.argument_of_periapsis, WithinAbs(w, ANG_TOL));
            }

            // Round-trip preserves position and velocity
            Vec3 pos, vel;
            orbital_elements_to_cartesian(recovered, parent_mass, &pos, &vel);

            Vec3 pos2, vel2;
            orbital_elements_to_cartesian(recovered, parent_mass, &pos2, &vel2);

            const double pos_err = vec3_magnitude(vec3_sub(pos, pos2));
            const double vel_err = vec3_magnitude(vec3_sub(vel, vel2));
            INFO("  pos_err=" << pos_err << " m, vel_err=" << vel_err << " m/s");
            REQUIRE_THAT(pos_err, WithinAbs(0.0, R_TOL));
            REQUIRE_THAT(vel_err, WithinAbs(0.0, V_TOL));
        }
    }

    SECTION("rotation matrix orthogonality") {
        for (int i = 0; i < 5; i++) {
            Spacecraft* craft = &sim->spacecraft[i];
            const double omega = craft->orbit.argument_of_periapsis;
            const double inc = craft->orbit.inclination;
            const double Omega = craft->orbit.longitude_of_ascending_node;

            Mat3 R = mat3_rotation_orbital(omega, inc, Omega);

            const Vec3 unit_x = {1.0, 0.0, 0.0};
            const Vec3 unit_y = {0.0, 1.0, 0.0};
            const Vec3 unit_z = {0.0, 0.0, 1.0};

            Vec3 rot_x = mat3_multiply_vec3(R, unit_x);
            Vec3 rot_y = mat3_multiply_vec3(R, unit_y);
            Vec3 rot_z = mat3_multiply_vec3(R, unit_z);

            const double mag_x = vec3_magnitude(rot_x);
            const double mag_y = vec3_magnitude(rot_y);
            const double mag_z = vec3_magnitude(rot_z);

            const double xy_dot = vec3_dot(rot_x, rot_y);
            const double yz_dot = vec3_dot(rot_y, rot_z);
            const double xz_dot = vec3_dot(rot_x, rot_z);

            INFO("Spacecraft " << i << ": " << craft->name);
            INFO("  |R·x|=" << mag_x << ", |R·y|=" << mag_y << ", |R·z|=" << mag_z);
            INFO("  (R·x)·(R·y)=" << xy_dot << ", (R·y)·(R·z)=" << yz_dot << ", (R·x)·(R·z)=" << xz_dot);

            REQUIRE_THAT(mag_x, WithinAbs(1.0, MAT_TOL));
            REQUIRE_THAT(mag_y, WithinAbs(1.0, MAT_TOL));
            REQUIRE_THAT(mag_z, WithinAbs(1.0, MAT_TOL));
            REQUIRE_THAT(xy_dot, WithinAbs(0.0, MAT_TOL));
            REQUIRE_THAT(yz_dot, WithinAbs(0.0, MAT_TOL));
            REQUIRE_THAT(xz_dot, WithinAbs(0.0, MAT_TOL));
        }
    }

    destroy_simulation(sim);
}