Compiler Explorer

Source code

#include <math.h>
#include <stdbool.h>
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <time.h>

/*
    Const
*/

const double G = 6.67408 * 1e-11;

const double EARTH_MASS = 5.97237 * 1e24;
const double EARTH_RADIUS = 6.378 * 1e6;
const double MOON_MASS = 7.342 * 1e22;
// const double MOON_RADIUS = 1.737 * 1e6;
const double EARTH_MOON_D = 3.84402 * 1e8;

const double ANGULAR_SPEED = 2.0*M_PI / (27.3*86400.0);  // rad/sec

const double SIM_DURATION = 10.0*86400;  // sec
const double INJECTION_ALTITUDE = 185*1000;  // m // 100 nm (Apollo 16-17: 90 nm)

// 50 sec => 600 Ko / 13 k points => 500 km / 5 deg between 2 points around the Earth
const uint64_t EXPORT_DT = 50;  // sec

/*
    Helpers
*/

double deg2rad(double angle) {
    return angle*M_PI/180.0;
}

void print_time(char *s) {
    static struct timespec last;
    static bool once = false;

if (once == false)
    {
        clock_gettime(CLOCK_MONOTONIC_RAW, &last);
        once = true;
    }

struct timespec cur;
    clock_gettime(CLOCK_MONOTONIC_RAW, &cur);

double delta_ms = (cur.tv_sec - last.tv_sec) * 1000 + 1.0 * (cur.tv_nsec - last.tv_nsec) / (1000*1000);

printf("print_time(): %s: %.3f ms\n", s, delta_ms);

memcpy(&last, &cur, sizeof(struct timespec));
}

/*
    Integrators
*/

typedef enum _Integrators {
    IntegratorsEulerNaive,
    IntegratorsEulerOptim,
    IntegratorsRk4Optim,
} Integrators;

char *Integrators_to_str(Integrators i)
{
    switch (i)
    {
        case IntegratorsEulerNaive:
            return "IntegratorsEulerNaive";
            break;

case IntegratorsEulerOptim:
            return "IntegratorsEulerOptim";
            break;

case IntegratorsRk4Optim:
            return "IntegratorsRk4Optim";
            break;
    }
    fprintf(stderr, "Error: Integrators_to_str(%d)\n", i);
    exit(1);
}

Integrators Integrators_from_str(char *s)
{
    if (strcmp("euler-naive", s) == 0)
        return IntegratorsEulerNaive;
    if (strcmp("euler-optim", s) == 0)
        return IntegratorsEulerOptim;
    if (strcmp("rk4-optim", s) == 0)
        return IntegratorsRk4Optim;
    fprintf(stderr, "Error: Integrators_from_str(%s)\n", s);
    exit(1);
}

/*
    Vector
*/

typedef struct _Vector {
    double x, y;
} Vector;

Vector Vector_from_polar_rad(double magnitude, double angle) {
    return (Vector){
        .x = magnitude*cos(angle),
        .y = magnitude*sin(angle),
    };
}

double Vector_magnitude(Vector v) {
    return sqrt(v.x*v.x + v.y*v.y);
}

Vector Vector_rotate(Vector v, double rotation_matrix[2][2]) {
    return (Vector){
        .x = rotation_matrix[0][0]*v.x + rotation_matrix[0][1]*v.y,
        .y = rotation_matrix[1][0]*v.x + rotation_matrix[1][1]*v.y,
    };
}

Vector Vector_add(Vector v1, Vector v2) {
    return (Vector){
        .x = v1.x + v2.x,
        .y = v1.y + v2.y,
    };
}

void Vector_addAssign(Vector *v1, Vector v2) {
    v1->x += v2.x;
    v1->y += v2.y;
}

Vector Vector_sub(Vector v1, Vector v2) {
    return (Vector){
        .x = v1.x - v2.x,
        .y = v1.y - v2.y,
    };
}

Vector Vector_mul(Vector v1, double factor) {
    return (Vector){
        .x = v1.x * factor,
        .y = v1.y * factor,
    };
}

/*
    Body
*/

typedef struct _Body {
    double mass;
    double mu;  // mass*G
    Vector pos;
    Vector vel;
    Vector acc;
} Body;

Vector Body_compute_acc_naive(Vector *pos, double mass, int others_len, Body *others) {
    Vector a = (Vector){.x = 0, .y = 0};
    int i = 0;

for (i = 0; i < others_len; ++i) {
        Vector dpos = Vector_sub(others[i].pos, *pos);

// acc = force / mass = G*m1*m2/d**2 / m1
        double grav_acc = (G * mass * others[i].mass) / pow(Vector_magnitude(dpos), 2) / mass;
        double angle = atan2(dpos.y, dpos.x);
        Vector_addAssign(&a, Vector_from_polar_rad(grav_acc, angle));
    }

return a;
}

Vector Body_compute_acc_optim(Vector *pos, double mass, int others_len, Body *others) {
    (void)mass;

Vector a = (Vector){.x = 0, .y = 0};
    int i = 0;

for (i = 0; i < others_len; ++i) {
        Vector dpos = Vector_sub(others[i].pos, *pos);

// x = grav_acc             * dpos.x / mag(dpos)
        //   = (mu / mag(dpos)**2)  * dpos.x / mag(dpos)
        //   = mu/mag(dpos)**3 * dpos.x
        double mag = Vector_magnitude(dpos);
        double mudist3 = others[i].mu / (mag*mag*mag);
        Vector_addAssign(&a, (Vector){
            .x = mudist3*dpos.x,
            .y = mudist3*dpos.y,
        });
    }

return a;
}

void Body_update_acc_naive(Body *self, int others_len, Body *others) {
    self->acc = Body_compute_acc_naive(&self->pos, self->mass, others_len, others);
}

void Body_update_acc_optim(Body *self, int others_len, Body *others) {
    self->acc = Body_compute_acc_optim(&self->pos, self->mass, others_len, others);
}

void Body_update_state_euler(Body *self, int others_len, Body *others, double sim_dt) {
    (void)others_len;
    (void)others;

Vector_addAssign(&self->vel, Vector_mul(self->acc, sim_dt));
    Vector_addAssign(&self->pos, Vector_mul(self->vel, sim_dt));
}

void Body_update_state_rk4(Body *self, int others_len, Body *others, double sim_dt) {
    Vector a1 = Body_compute_acc_optim(&self->pos, self->mass, others_len, others);

Vector p2 = Vector_add(self->pos, Vector_mul(self->vel, sim_dt/2.0));
    Vector a2 = Body_compute_acc_optim(&p2, self->mass, others_len, others);
    Vector v2 = Vector_add(self->vel, Vector_mul(a1, sim_dt/2.0));

Vector p3 = Vector_add(self->pos, Vector_mul(v2, sim_dt/2.0));
    Vector a3 = Body_compute_acc_optim(&p3, self->mass, others_len, others);
    Vector v3 = Vector_add(self->vel, Vector_mul(a2, sim_dt/2.0));

Vector p4 = Vector_add(self->pos, Vector_mul(v3, sim_dt));
    Vector a4 = Body_compute_acc_optim(&p4, self->mass, others_len, others);
    Vector v4 = Vector_add(self->vel, Vector_mul(a3, sim_dt));

Vector a =
        Vector_add(
            Vector_add(
                Vector_add(
                    Vector_mul(a1, 1/6.0),
                    Vector_mul(a2, 1/3.0)
                ),
                Vector_mul(a3, 1/3.0)),
            Vector_mul(a4, 1/6.0)
        );
    Vector v =
        Vector_add(
            Vector_add(
                Vector_add(
                    Vector_mul(self->vel, 1/6.0),
                    Vector_mul(v2, 1/3.0)
                ),
                Vector_mul(v3, 1/3.0)),
            Vector_mul(v4, 1/6.0)
        );

Vector_addAssign(&self->vel, Vector_mul(a, sim_dt));
    Vector_addAssign(&self->pos, Vector_mul(v, sim_dt));
}

/*
    Funcs
*/

double barycenter_distance(double d, double m1, double m2) {
    return d * m2 / (m1 + m2);
}

double orbital_speed(double mass, double distance) {
    return sqrt(G*mass/abs(distance));
}

/*
    Main
*/

int main(int ac, char **av) {
    const double EXPORT_DT_MS = EXPORT_DT*1000; // unit: ms

print_time("pre (init)");

/*
        Args
    */

if (ac != 1+4)
    {
        printf("Usage: %s <injection_angle_deg:float> <injection_dv:float> <sim_step_per_sec:float> <integrator:euler-naive|euler-optim|rk4-optim>\n", av[0]);
        printf("Example: %s -123.7 3150 10 euler-naive\n", av[0]);
        exit(1);
    }

double injection_angle_deg = atof(av[1]);
    double injection_dv = atof(av[2]);
    double sim_step_per_sec = atof(av[3]);
    Integrators flag_integrator = Integrators_from_str(av[4]);

printf("Args\n");
    printf("\tinjection_angle_deg: %.3f\n", injection_angle_deg);
    printf("\tinjection_dv: %.3f\n", injection_dv);
    printf("\tsim_step_per_sec: %.3f\n", sim_step_per_sec);
    printf("\tintegrator: %s\n", Integrators_to_str(flag_integrator));
    printf("\n");

/*
        Vars
    */

double injection_angle = deg2rad(injection_angle_deg);

Vector earth_pos = (Vector){.x=-barycenter_distance(EARTH_MOON_D, EARTH_MASS, MOON_MASS), .y=0.0};
    Vector moon_pos = (Vector){.x=barycenter_distance(EARTH_MOON_D, MOON_MASS, EARTH_MASS), .y=0.0};
    Vector spacecraft_pos = Vector_add(earth_pos, Vector_from_polar_rad(EARTH_RADIUS+INJECTION_ALTITUDE, injection_angle));

Vector earth_vel = Vector_from_polar_rad(Vector_magnitude(earth_pos)*ANGULAR_SPEED, -M_PI/2.0);
    Vector moon_vel = Vector_from_polar_rad(Vector_magnitude(moon_pos)*ANGULAR_SPEED, M_PI/2.0);
    double sc_s = orbital_speed(EARTH_MASS, Vector_magnitude(Vector_sub(spacecraft_pos, earth_pos))) + injection_dv;
    Vector spacecraft_vel = Vector_from_polar_rad(sc_s, injection_angle+M_PI/2.0);

Body earthmoon[2] = {
        (Body){.mass=EARTH_MASS, .mu=G*EARTH_MASS, .pos=earth_pos, .vel=earth_vel, .acc=(Vector){.x=0, .y=0}},
        (Body){.mass=MOON_MASS, .mu=G*MOON_MASS, .pos=moon_pos, .vel=moon_vel, .acc=(Vector){.x=0, .y=0}}
    };
    Body *earth = &earthmoon[0];
    Body *moon = &earthmoon[1];
    Body *spacecraft = &(Body){
        .mass=1.0,
        .mu=G*1.0,
        .pos=spacecraft_pos,
        .vel=spacecraft_vel,
        .acc=(Vector){.x=0, .y=0}
    };

double sim_dt = 1.0 / sim_step_per_sec;
    double sim_dt_ms = 1000.0 / sim_step_per_sec;
    double t_max_ms = SIM_DURATION * 1000.0;

uint64_t export_ts = t_max_ms / EXPORT_DT_MS;

Vector **poss = malloc(sizeof(Vector*)*export_ts);
    if (poss == NULL) {
        exit(43);
    }
    uint64_t i = 0;
    for (i = 0; i < export_ts; ++i) {
        poss[i] = malloc(sizeof(Vector)*3);
        if (poss[i] == NULL) {
            exit(44);
        }
    }

if ( ! (EXPORT_DT_MS >= sim_dt_ms))
    {
        fprintf(stderr, "Error: sim_dt_ms must be lower than EXPORT_DT_MS\n");
        exit(1);
    }

/*
        Prints
    */

printf("Pos\n");
    printf("\tEarth:      Vector(%f, %f)\n", earth->pos.x, earth->pos.y);
    printf("\tMoon:       Vector(%f, %f)\n", moon->pos.x, moon->pos.y);
    printf("\tSpacecraft: Vector(%f, %f)\n", spacecraft->pos.x, spacecraft->pos.y);
    printf("Vel\n");
    printf("\tEarth:      Vector(%f, %f)\n", earth->vel.x, earth->vel.y);
    printf("\tMoon:       Vector(%f, %f)\n", moon->vel.x, moon->vel.y);
    printf("\tSpacecraft: Vector(%f, %f)\n", spacecraft->vel.x, spacecraft->vel.y);
    // printf("Grav\n");
    // printf("\tMoon <- Earth:       Vector(%f, %f)\n", Body_grav_acc_from(&earth, &moon).x, Body_grav_acc_from(&earth, &moon).y);
    // printf("\tEarth <- Moon:       Vector(%f, %f)\n", Body_grav_acc_from(&moon, &earth).x, Body_grav_acc_from(&moon, &earth).y);
    // printf("\tEarth <- Spacecraft: Vector(%f, %f)\n", Body_grav_acc_from(&spacecraft, &earth).x, Body_grav_acc_from(&spacecraft, &earth).y);
    // printf("\tMoon <- Spacecraft:  Vector(%f, %f)\n", Body_grav_acc_from(&spacecraft, &moon).x, Body_grav_acc_from(&spacecraft, &moon).y);
    printf("\n");

printf("Miscs\n");
    printf("\tsim_dt: %.3f\n", sim_dt);
    printf("\tsim_dt_ms: %.3f\n", sim_dt_ms);
    printf("\tt_max_ms: %.3f\n", t_max_ms);
    printf("\texport_ts: %lu\n", export_ts);
    printf("\n");

print_time("pre (var + prints)");

/*
        Sim
    */

double last_sim_ms = 0;
    double last_export_ms = 0;

while (last_sim_ms < t_max_ms)
    {
        switch (flag_integrator)
        {
            case IntegratorsEulerNaive:
                Body_update_acc_naive(earth, 1, moon);
                Body_update_acc_naive(moon, 1, earth);
                Body_update_acc_naive(spacecraft, 2, earthmoon);
            break;
            case IntegratorsEulerOptim:
            case IntegratorsRk4Optim:
                Body_update_acc_optim(earth, 1, moon);
                Body_update_acc_optim(moon, 1, earth);
                Body_update_acc_optim(spacecraft, 2, earthmoon);
            break;
        }
        switch (flag_integrator)
        {
            case IntegratorsEulerNaive:
            case IntegratorsEulerOptim:
                Body_update_state_euler(earth, 1, moon, sim_dt);
                Body_update_state_euler(moon, 1, earth, sim_dt);
                Body_update_state_euler(spacecraft, 2, earthmoon, sim_dt);
            break;
            case IntegratorsRk4Optim:
                Body_update_state_rk4(earth, 1, moon, sim_dt);
                Body_update_state_rk4(moon, 1, earth, sim_dt);
                Body_update_state_rk4(spacecraft, 2, earthmoon, sim_dt);
            break;
        }

if (last_sim_ms >= last_export_ms)
        {
            double alpha = -ANGULAR_SPEED * last_sim_ms/1000.0;
            double r[2][2] = {
                {cos(alpha), -sin(alpha)},
                {sin(alpha), cos(alpha)},
            };

uint64_t index = last_export_ms/EXPORT_DT_MS;

poss[index][0] = Vector_rotate(earth->pos, r);
            poss[index][1] = Vector_rotate(moon->pos, r);
            poss[index][2] = Vector_rotate(spacecraft->pos, r);

last_export_ms += EXPORT_DT_MS;
        }

last_sim_ms += sim_dt_ms;
    }

print_time("sim");

printf("poss size: %lu\n", export_ts);
    Vector scp = poss[export_ts-1][2];
    printf("spacecraft pos %.0f %.0f\n", scp.x, scp.y);

/*
        Export
    */

FILE *f = NULL;

f = fopen("out.txt", "w");
    if (f == NULL) {
        exit(45);
    }

fprintf(f, "t earth_x earth_y moon_x moon_y spacecraft_x spacecraft_y\n");

for (i = 0; i < export_ts; ++i) {
        Vector ep = poss[i][0];
        Vector mp = poss[i][1];
        Vector scp = poss[i][2];

fprintf(f,
            "%lu %.0f %.0f %.0f %.0f %.0f %.0f\n",
            i * EXPORT_DT,
            ep.x, ep.y,
            mp.x, mp.y,
            scp.x, scp.y
        );

// format: 1 sec // writeln : 2 secs // total: 4 secs
    }

fclose(f);

print_time("post (export)");

return 0;
}