Kepler problem in C

kepler.c

struct kepler_elements {
    double semi_latus_rectum;
    double eccentricity;
    double mean_motion;
    double inclination;
    double longitude_of_ascending_node;
    double argument_of_periapsis;
    double periapsis_time;
};

#include <stdbool.h>
#include <math.h>
#include <float.h>

void cross(const double *a, const double *b, double *c) {
    c[0] = a[1]*b[2] - a[2]*b[1];
    c[1] = -(a[0]*b[2] - a[2]*b[0]);
    c[2] = a[0]*b[1] - a[1]*b[0];
}

double dot(const double *a, const double *b) {
    return a[0]*b[0] + a[1]*b[1] + a[2]*b[2];
}

double mag(const double *a) {
    return sqrt(dot(a, a));
}

double clamp(double min, double max, double x){
    return (x < min ? min : (x > max ? max : x));
}

bool zero(double x) {
    return x*x < DBL_EPSILON;
    //return fabs(x) < DBL_EPSILON;
}

double sign(double x) { return x < 0 ? -1.0 : 1.0; }
double square(double x) { return x*x; }
double cube(double x) { return x*x*x; }

static void transpose3x3(double *m)
{
    for(int i = 0; i < 2; ++i) {
        for(int j = i; j < 3; ++j) {
            double temp = m[i * 3 + j];
            m[i * 3 + j] = m[j * 3 + i];
            m[j * 3 + i] = temp;
        }
    }
}

static void matrix_vector_product(const double *m, const double *v, double *x) {
    for(int i = 0; i < 3; ++i) {
        x[i] = 0.0;

        for(int j = 0; j < 3; ++j) {
            x[i] += m[3 * i + j] * v[j];
            //x[i] += m[3 * j + i] * v[j];
        }
    }
}

static inline double kepler_iter1(double e, double M, double x) {
    return M + e * sin(x);
}

static inline double kepler_iter2(double e, double M, double x) {
    return x + (M + e * sin(x) - x) / (1.0 - e * cos(x));
}

static inline double kepler_iter3(double e, double M, double x) {
    double s = e * sin(x);
    double c = e * cos(x);
    double f0 = x - s - M;
    double f1 = 1.0 - c;
    double f2 = s;

    return x + (-5.0) * f0 / (f1 + sign(f1) * sqrt(fabs(16.0 * f1 * f1 - 20.0 * f0 * f2)));
}

static inline double kepler_iter4(double e, double M, double x) {
    double s = e * sinh(x);
    double c = e * cosh(x);
    double f0 = s - x - M;
    double f1 = c - 1.0;
    double f2 = s;

    return x + (-5.0) * f0 / (f1 + sign(f1) * sqrt(fabs(16.0 * f1 * f1 - 20.0 * f0 * f2)));
}

double kepler_anomaly_mean_to_eccentric(double e, double M) {
    if(zero(e - 1.0)) {
        // parabolic anomaly
        double x = pow(sqrt(9.0*M*M + 1.0) + 3.0*M, 1.0/3.0);
        return x - 1.0/x;
    }

    typedef double (*iter_func)(double, double, double);
    iter_func iter = 0;
    if(e < 0.3) iter = kepler_iter1;
    else if(e < 0.9) iter = kepler_iter2;
    else if(e < 1.0) iter = kepler_iter3;
    else if(e > 1.0) iter = kepler_iter4;

    int num_steps = 1;
    if(e > 0.0 && e < 0.3) num_steps = 10;
    else if(e < 0.9) num_steps = 20;
    else if(e < 1.0) num_steps = 20;
    else if(e > 1.0) num_steps = 30;

    double threshold = DBL_EPSILON;

    double x = M, x0 = x;
    do {
        x0 = x;
        x = iter(e, M, x);
    } while(--num_steps && (x0-x)*(x0-x) > threshold);

    return x;
}

double kepler_anomaly_eccentric_to_mean(double e, double E) {
    if(zero(e - 1.0))
        // parabolic anomaly
        return E*E*E/6.0 + E/2.0;
    if(e > 1.0)
        return e * sinh(E) - E;
    return E - e * sin(E);
}

double kepler_anomaly_eccentric_to_true(double e, double E) {
    if(zero(e - 1.0))
        return 2.0 * atan(E);
    if(e > 1.0)
        return 2.0 * atan(sqrt((e+1.0) / (e-1.0)) * tanh(E/2.0));

    return atan2(sqrt(1.0-e*e) * sin(E), cos(E) - e);
}

double kepler_anomaly_true_to_eccentric(double e, double f) {
    if(zero(e - 1.0))
        return tan(f / 2.0);
    if(e > 1.0)
        return sign(f) * acosh((e + cos(f)) / (1.0 + e * cos(f)));

    return atan2(sqrt(1.0-e*e) * sin(f), cos(f) + e);
}

double kepler_anomaly_true_to_mean(double e, double f) {
    return kepler_anomaly_eccentric_to_mean(e, kepler_anomaly_true_to_eccentric(e, f));
}

double kepler_anomaly_mean_to_true(double e, double M) {
    return kepler_anomaly_eccentric_to_true(e, kepler_anomaly_mean_to_eccentric(e, M));
}

double kepler_anomaly_dEdM(double e, double E) {
    if(zero(e - 1.0))
        return 2.0 / (E*E + 1.0);
    if(e > 1.0)
        return 1.0 / (e*cosh(E) - 1.0);
    return 1.0 / (1.0 - e * cos(E));
}

bool kepler_orbit_parabolic(const struct kepler_elements *elements) {
    return zero(elements->eccentricity - 1.0);
}

bool kepler_orbit_hyperbolic(const struct kepler_elements *elements) {
    return elements->eccentricity > 1.0 && !kepler_orbit_parabolic(elements);
}

bool kepler_orbit_closed(const struct kepler_elements *elements) {
    return elements->eccentricity < 1.0 && !kepler_orbit_parabolic(elements);
}

bool kepler_orbit_circular(const struct kepler_elements *elements) {
    return zero(elements->eccentricity);
}

double kepler_orbit_semi_major_axis(const struct kepler_elements *elements) {
    if(kepler_orbit_parabolic(elements))
        return INFINITY;
    return elements->semi_latus_rectum / (1.0 - square(elements->eccentricity));
}

double kepler_orbit_semi_minor_axis(const struct kepler_elements *elements) {
    if(kepler_orbit_parabolic(elements))
        return INFINITY;
    if(elements->eccentricity > 1.0)
        return -elements->semi_latus_rectum / sqrt(square(elements->eccentricity) - 1.0);
    return elements->semi_latus_rectum / sqrt(1.0 - square(elements->eccentricity));
}

double kepler_orbit_gravity_parameter(const struct kepler_elements *elements) {
    if(kepler_orbit_parabolic(elements))
        return square(elements->mean_motion) * cube(elements->semi_latus_rectum);
    return square(elements->mean_motion) *
        cube(fabs(kepler_orbit_semi_major_axis(elements)));
}

double kepler_orbit_specific_orbital_energy(const struct kepler_elements *elements) {
    if(kepler_orbit_parabolic(elements))
        return 0.0;
    return -kepler_orbit_gravity_parameter(elements) /
        (2.0 * kepler_orbit_semi_major_axis(elements));
}

double kepler_orbit_specific_angular_momentum(const struct kepler_elements *elements) {
    return sqrt(elements->semi_latus_rectum * kepler_orbit_gravity_parameter(elements));
}

double kepler_orbit_apoapsis(const struct kepler_elements *elements) {
    if(elements->eccentricity >= 1.0)
        return INFINITY;
    return elements->semi_latus_rectum / (1 - elements->eccentricity);
}

double kepler_orbit_periapsis(const struct kepler_elements *elements) {
    return elements->semi_latus_rectum / (1 + elements->eccentricity);
}

double kepler_orbit_apoapsis_vel(const struct kepler_elements *elements) {
    if(elements->eccentricity >= 1.0)
        return INFINITY;

    double mu = kepler_orbit_gravity_parameter(elements);
    return sqrt((mu / elements->semi_latus_rectum) * square(1.0 - elements->eccentricity));
}

double kepler_orbit_periapsis_vel(const struct kepler_elements *elements) {
    double mu = kepler_orbit_gravity_parameter(elements);
    return sqrt((mu / elements->semi_latus_rectum) * square(1.0 + elements->eccentricity));
}

double kepler_orbit_period(const struct kepler_elements *elements) {
    if(!kepler_orbit_closed(elements))
        return INFINITY;
    return 2.0 * M_PI / elements->mean_motion;
}

double kepler_orbit_mean_anomaly_at_time(const struct kepler_elements *elements, double t) {
    double M = elements->mean_motion * (t - elements->periapsis_time);
    if(kepler_orbit_closed(elements) && fabs(M) >= M_PI) {
        double x = (M+M_PI)/(2.0*M_PI);
        M = -M_PI + 2.0*M_PI * (x - floor(x));
    }
    return M;
}

void kepler_orientation_normal(double i, double an, double arg, double *dir) {
    (void)arg;
    dir[0] = sin(an) * sin(i);
    dir[1] = -cos(an) * sin(i);
    dir[2] = cos(i);
}

void kepler_orientation_tangent(double i, double an, double arg, double *dir) {
    dir[0] = (cos(arg) * cos(an)) - (sin(arg) * sin(an) * cos(i));
    dir[1] = (sin(arg) * cos(an) * cos(i)) + (cos(arg) * sin(an));
    dir[2] = sin(arg) * sin(i);
}

void kepler_orientation_bitangent(double i, double an, double arg, double *dir) {
    dir[0] = -(cos(arg) * sin(an) * cos(i)) - (sin(arg) * cos(an));
    dir[1] = (cos(arg) * cos(an) * cos(i)) - (sin(arg) * sin(an));
    dir[2] = cos(arg) * sin(i);
}

void kepler_orientation_matrix(double i, double an, double arg, double *mat) {
    kepler_orientation_tangent(i, an, arg, mat+0);
    kepler_orientation_bitangent(i, an, arg, mat+3);
    kepler_orientation_normal(i, an, arg, mat+6);
    transpose3x3(mat);
}

void kepler_orbit_normal(const struct kepler_elements *elements, double *dir) {
    kepler_orientation_normal(
        elements->inclination,
        elements->longitude_of_ascending_node,
        elements->argument_of_periapsis,
        dir);
}

void kepler_orbit_tangent(const struct kepler_elements *elements, double *dir) {
    kepler_orientation_tangent(
        elements->inclination,
        elements->longitude_of_ascending_node,
        elements->argument_of_periapsis,
        dir);
}

void kepler_orbit_bitangent(const struct kepler_elements *elements, double *dir) {
    kepler_orientation_bitangent(
        elements->inclination,
        elements->longitude_of_ascending_node,
        elements->argument_of_periapsis,
        dir);
}

void kepler_orbit_matrix(const struct kepler_elements *elements, double *mat) {
    kepler_orientation_matrix(
        elements->inclination,
        elements->longitude_of_ascending_node,
        elements->argument_of_periapsis,
        mat);
}

void kepler_elements_from_state(
    double mu,
    const double *pos,
    const double *vel,
    double epoch,
    struct kepler_elements *elements) {
    double r = mag(pos);
    double v2 = dot(vel, vel);

    // specific angular momentum
    double h[3];
    cross(pos, vel, h);
    // TODO: check for radial trajectory

    // eccentricity vector, direction: to periapsis, magnitude: eccentricity
    // e = 1/mu * (v^2 - mu/r) * r - dot(r, v) * v;
    double ecc[3];
    for(int i = 0; i < 3; ++i)
        ecc[i] = (1.0 / mu) * (pos[i]*(v2 - mu/r) - vel[i]*dot(pos, vel));

    double e = mag(ecc);
    bool circular = zero(e);
    bool parabolic = zero(e - 1.0);

    // line of nodes, pointing to ascending node, equatorial -> zero
    double nodes[3] = { -h[1], h[0], 0.0 };
    double N = mag(nodes);
    bool equatorial = zero(dot(nodes, nodes));

    // semi-latus rectum
    double p = dot(h, h) / mu;

    // inclination
    double i = acos(clamp(-1.0, 1.0, h[2] / mag(h)));

    // longitude of ascending node
    double an = equatorial ?  0.0 : atan2(nodes[1], nodes[0]);

    // argument of periapsis
    double arg = 0.0 / 0.0; // NaN
    if(circular)
        // circular, zero
        arg = 0.0;
    else if(equatorial)
        // equatorial, measure from X-axis, negative for retrograde
        arg = sign(h[2]) * atan2(ecc[1], ecc[0]);
    else
        // angle between eccentricity vector and line of nodes (ascending node)
        arg = sign(ecc[2]) * acos(clamp(-1.0, 1.0, dot(nodes, ecc) / (N * e)));

    // true anomaly
    double f = 0.0 / 0.0; // NaN

    if(circular && equatorial)
        // circular, equatorial -> measure from X-axis, negative for retrograde
        f = sign(h[2]) * atan2(pos[1], pos[0]);
    else if(circular)
        // circular orbit -> measure from ascending node
        f = -sign(dot(vel, nodes)) *
            acos(clamp(-1.0, 1.0, dot(nodes, pos) / (N * r)));
    else
        // measure true anomaly from periapsis (eccentricity vector)
        f = -sign(dot(vel, ecc)) *
            acos(clamp(-1.0, 1.0, dot(ecc, pos) / (e * r)));

    // mean anomaly at epoch
    double M0 = kepler_anomaly_true_to_mean(e, f);

    // mean motion
    double a = p / (1.0 - e*e);
    double n = parabolic ?
        sqrt(mu / (p*p*p)) :
        sqrt(mu / fabs(a*a*a));

    // time at periapsis
    double periapsis_time = epoch - M0 / n;

    elements->semi_latus_rectum = p;
    elements->eccentricity = e;
    elements->mean_motion = n;
    elements->inclination = i;
    elements->longitude_of_ascending_node = an;
    elements->argument_of_periapsis = arg;
    elements->periapsis_time = periapsis_time;
}

void kepler_elements_to_state_f(
    const struct kepler_elements *elements,
    double f,
    double *pos,
    double *vel) {
    // generic conic trajectory with true anomaly and vis-viva equation
    double p = elements->semi_latus_rectum;
    double e = elements->eccentricity;
    double a = kepler_orbit_semi_major_axis(elements);
    double mu = kepler_orbit_gravity_parameter(elements);

    double r = p / (1.0 + e*cos(f));
    double v = kepler_orbit_parabolic(elements) ?
        sqrt(mu * (2.0 / r)) :
        sqrt(mu * (2.0 / r - 1.0 / a));

    double x = r * cos(f);
    double y = r * sin(f);

    double dx = -p * sin(f) / square(1.0 + e*cos(f));
    double dy = p * (e + cos(f)) / square(1.0 + e*cos(f));
    double d = sqrt(dx*dx + dy*dy);

    double vx = v * dx / d;
    double vy = v * dy / d;

    pos[0] = x; pos[1] = y; pos[2] = 0.0;
    vel[0] = vx; vel[1] = vy; vel[2] = 0.0;
}

void kepler_elements_to_state_E(
    const struct kepler_elements *elements,
    double E,
    double *pos,
    double *vel) {
    double e = elements->eccentricity;
    double p = elements->semi_latus_rectum;
    double a = kepler_orbit_semi_major_axis(elements);
    double b = kepler_orbit_semi_minor_axis(elements);
    double n = elements->mean_motion;

    double Edot = n * kepler_anomaly_dEdM(e, E);

    double x, y, vx, vy;
    if(kepler_orbit_parabolic(elements)) {
        // parabolic trajectory
        x = p/2.0 * (1.0 - E*E);
        y = p * E;

        vx = -p * E * Edot;
        vy = p * Edot;

    } else if(kepler_orbit_closed(elements)) {
        // elliptic trajectory
        x = a * (cos(E) - e);
        y = b * sin(E);

        vx = -a * sin(E) * Edot;
        vy = b * cos(E) * Edot;
    } else {
        // hyperbolic trajectory
        x = a * (cosh(E) - e);
        y = -b * sinh(E);

        vx = a * sinh(E) * Edot;
        vy = -b * cosh(E) * Edot;
    }

    pos[0] = x; pos[1] = y; pos[2] = 0.0;
    vel[0] = vx; vel[1] = vy; vel[2] = 0.0;
}

void kepler_elements_to_state_t(
    const struct kepler_elements *elements,
    double t,
    double *pos,
    double *vel) {
    // mean anomaly
    double M = kepler_orbit_mean_anomaly_at_time(elements, t);

    // eccentric anomaly
    double E = kepler_anomaly_mean_to_eccentric(elements->eccentricity, M);

    kepler_elements_to_state_E(elements, E, pos, vel);
}

#include "../numtest.c"

static bool eqv3(double *a, double *b) {
    double diff[3] = { a[0]-b[0], a[1]-b[1], a[2]-b[2] };
    return (ZEROF(dot(a,a)) && ZEROF(dot(b,b))) ||
        ZEROF(dot(diff, diff)/(dot(a, a) + dot(b, b)));
}

static void rotation_matrix_euler(double rx, double ry, double rz, double *m)
{
    double sn[3] = { sin(rx), sin(ry), sin(rz) };
    double cs[3] = { cos(rx), cos(ry), cos(rz) };

    m[0] = cs[1]*cs[2];
    m[1] = cs[1]*sn[2];
    m[2] = -sn[1];

    m[3] = sn[0]*sn[1]*cs[2] - cs[0]*sn[2];
    m[4] = sn[0]*sn[1]*sn[2] + cs[0]*cs[2];
    m[5] = sn[0]*cs[1];

    m[6] = cs[0]*sn[1]*cs[2] + sn[0]*sn[2];
    m[7] = cs[0]*sn[1]*sn[2] - sn[0]*cs[2];
    m[8] = cs[0]*cs[1];
}

static void anomaly_test(double *params, int num_params, void *extra_args, struct numtest_ctx *test_ctx) {
    (void)extra_args;

    ASSERT(num_params == 2, "num_params");

    double e = params[0] * 5.0;
    double t = -1.0 + params[1] * 2.0;

    double maxf = e > 1.0 ?
        acos(1.0/e) :       // hyperbolic
        (zero(e-1.0) ?
            4*M_PI/5 :      // parabolic
            M_PI);          // closed orbit
    double maxE = kepler_anomaly_true_to_eccentric(e, maxf);
    double maxM = kepler_anomaly_eccentric_to_mean(e, maxE);

    double M = t * maxM;
    double E = t * maxE;
    double f = t * maxf;

    double f2 = kepler_anomaly_eccentric_to_true(e, E);
    ASSERT_RANGEF(f2, -maxf, maxf, "True anomaly within range");
    ASSERT_EQF(E, kepler_anomaly_true_to_eccentric(e, f2), "Eccentric -> True");

    double E2 = kepler_anomaly_true_to_eccentric(e, f);
    ASSERT_RANGEF(E2, -maxE, maxE, "Eccentric anomaly within range");
    ASSERT_EQF(f, kepler_anomaly_eccentric_to_true(e, E2), "True -> Eccentric");

    double E3 = kepler_anomaly_mean_to_eccentric(e, M);
    ASSERT_RANGEF(E3, -maxE, maxE, "Eccentric anomaly within range");
    ASSERT_EQF(M, kepler_anomaly_eccentric_to_mean(e, E3), "True -> Eccentric");

    double M2 = kepler_anomaly_eccentric_to_mean(e, E);
    ASSERT_RANGEF(M2, -maxM, maxM, "Mean anomaly within range");
    ASSERT_EQF(E, kepler_anomaly_mean_to_eccentric(e, M2), "Eccentric -> Mean");

    double M3 = kepler_anomaly_true_to_mean(e, f);
    ASSERT_RANGEF(M3, -maxM, maxM, "Mean anomaly within range");
    ASSERT_EQF(f, kepler_anomaly_mean_to_true(e, M3), "True -> Mean");

    double f3 = kepler_anomaly_mean_to_true(e, M);
    ASSERT_RANGEF(f3, -maxf, maxf, "True anomaly within range");
    ASSERT_EQF(M, kepler_anomaly_true_to_mean(e, f3), "Mean -> True");

    double dEdM = kepler_anomaly_dEdM(e, E);
    double MM = kepler_anomaly_eccentric_to_mean(e, E);
    double dM = 1.0e-10 * maxM;
    double Eplus = kepler_anomaly_mean_to_eccentric(e, MM+dM);
    double Eminus = kepler_anomaly_mean_to_eccentric(e, MM-dM);
    ASSERT_EQF(dEdM * 2.0 * dM, (Eplus-Eminus), "dE/dM");
}

static void orbit_from_state_test(double *params, int num_params, void *extra_args, struct numtest_ctx *test_ctx) {
    (void)extra_args;

    ASSERT(num_params == 6, "");

    struct kepler_elements elements = { 0, 0, 0, 0, 0, 0, 0};

    double mu = 1.0 + params[0] * 1.0e10;
    double r0 = 1.0 + params[1] * 1.0e10;
    double v_circ = sqrt(mu / r0), v_esc = sqrt(2.0 * mu / r0);
    double v0 = v_circ + (v_esc - v_circ) * 2.0 * params[2];

    double r = r0, v = v0;

    double mat[9];
    double rot[3];
    for(int i = 0; i < 3; ++i)
        rot[i] = -M_PI + 2.0*M_PI * params[3+i];
    rotation_matrix_euler(rot[0], rot[1], rot[2], mat);

    // TODO: start above periapsis
    double pos[3] = { r * mat[0], r * mat[1], r * mat[2] };
    double vel[3] = { v * mat[3], v * mat[4], v * mat[5] };
    double t0 = 0.0;

    kepler_elements_from_state(mu, pos, vel, t0, &elements);

    ASSERT(
        isfinite(elements.semi_latus_rectum) &&
        isfinite(elements.eccentricity) &&
        isfinite(elements.mean_motion) &&
        isfinite(elements.inclination) &&
        isfinite(elements.longitude_of_ascending_node) &&
        isfinite(elements.argument_of_periapsis) &&
        isfinite(elements.periapsis_time),
        "All elements not NaN or Inf");

    ASSERT_EQF(kepler_orbit_gravity_parameter(&elements), mu,
        "Gravity parameter matches");

    ASSERT_RANGEF(elements.inclination, 0.0, M_PI,
        "Inclination within range 0..pi");
    ASSERT_RANGEF(elements.longitude_of_ascending_node, -M_PI, M_PI,
        "Longitude of ascending node with range -pi..pi");
    ASSERT_RANGEF(elements.argument_of_periapsis, -M_PI, M_PI,
        "Argument of periapsis with range -pi..pi");

    ASSERT(elements.eccentricity >= 0.0,
        "Eccentricity is greater than zero");

    ASSERT(v >= v_esc || kepler_orbit_closed(&elements),
        "Closed orbit if less than escape velocity");

    ASSERT_LTF(kepler_orbit_periapsis(&elements), r,
        "Distance is greater than periapsis");
    ASSERT_LTF(v, kepler_orbit_periapsis_vel(&elements),
        "Velocity is smaller than periapsis velocity");

    ASSERT(kepler_orbit_closed(&elements) ==
        !(kepler_orbit_parabolic(&elements) || kepler_orbit_hyperbolic(&elements)),
        "Closed orbits are not parabolic or hyperbolic");
    ASSERT(kepler_orbit_closed(&elements) ||
        kepler_orbit_parabolic(&elements) != kepler_orbit_hyperbolic(&elements),
        "Parabolic orbits are not hyperbolic");

    if(!kepler_orbit_parabolic(&elements)) {
        double visviva = v*v/2.0 - mu/r;
        ASSERT_EQF(visviva, kepler_orbit_specific_orbital_energy(&elements),
            "Specific orbital energy (vis-viva)");
    } else {
        ASSERT(ZEROF(kepler_orbit_specific_orbital_energy(&elements)),
            "Parabolic orbit specific orbital energy is zero (vis-viva)");
    }

    double h[3];
    cross(pos, vel, h);
    ASSERT_EQF(mag(h), kepler_orbit_specific_angular_momentum(&elements),
        "Specific relative angular momentum");

    if(kepler_orbit_closed(&elements)) {
        ASSERT_LTF(kepler_orbit_periapsis(&elements), kepler_orbit_apoapsis(&elements),
            "Apoapsis is greater than periapsis");
        ASSERT_LTF(kepler_orbit_apoapsis_vel(&elements), kepler_orbit_periapsis_vel(&elements),
            "Periapsis velocity is greater than apoapsis velocity");

        ASSERT(kepler_orbit_semi_major_axis(&elements) > 0 && kepler_orbit_semi_minor_axis(&elements) > 0,
            "Closed orbit semi-major and semi-minor axes are positive");
        ASSERT_LTF(kepler_orbit_semi_minor_axis(&elements), kepler_orbit_semi_major_axis(&elements),
            "Semi-minor axis is less than semi_major axis");

        ASSERT(isfinite(kepler_orbit_period(&elements)),
            "Closed orbits have finite period");

        ASSERT_RANGEF(r, kepler_orbit_periapsis(&elements), kepler_orbit_apoapsis(&elements),
            "Distance is between apoapsis and periapsis");
        ASSERT_RANGEF(v, kepler_orbit_apoapsis_vel(&elements), kepler_orbit_periapsis_vel(&elements),
            "Velocity is between apoapsis and periapsis velocity");

        ASSERT(0.0 > kepler_orbit_specific_orbital_energy(&elements),
            "Closed orbits have negative specific orbital energy");
    }

    if(kepler_orbit_circular(&elements)) {
        ASSERT_EQF(kepler_orbit_apoapsis(&elements), kepler_orbit_periapsis(&elements),
            "Circular orbit periapsis == apoapsis");
        ASSERT_EQF(kepler_orbit_apoapsis_vel(&elements), kepler_orbit_periapsis_vel(&elements),
            "Circular orbit periapsis velocity == apoapsis velocity");
    }

    if(kepler_orbit_parabolic(&elements)) {
        // TODO: test parabolic orbits
    }

    if(kepler_orbit_hyperbolic(&elements)) {
        ASSERT(kepler_orbit_semi_major_axis(&elements) < 0 && kepler_orbit_semi_minor_axis(&elements) < 0,
            "Hyperbolic orbit semi-major and semi-minor axes are positive");

        ASSERT(0.0 < kepler_orbit_specific_orbital_energy(&elements),
            "Hyperbolic orbits have positive specific orbital energy");
    }

    double t[3], n[3], b[3];
    kepler_orbit_tangent(&elements, t);
    kepler_orbit_normal(&elements, n);
    kepler_orbit_bitangent(&elements, b);

    ASSERT(zero(dot(t, n)) && zero(dot(t, b)) && zero(dot(n, b)),
        "Orbit normal, tangent, binormal are orthogonal");

    ASSERT(ZEROF(dot(pos, n)/r) && ZEROF(dot(vel, n)/v),
        "Orbit motion is planar");

    ASSERT_EQF(dot(n, h), mag(h),
        "Orbit normal and angular momentum");

    double matrix[9];
    kepler_orbit_matrix(&elements, matrix);

    for(int pass = 0; pass < 2; ++pass)
    {
        double pos2[3], vel2[3];
        double M = kepler_orbit_mean_anomaly_at_time(&elements, t0);
        double E = kepler_anomaly_mean_to_eccentric(elements.eccentricity, M);
        double f = kepler_anomaly_eccentric_to_true(elements.eccentricity, E);

        if(pass == 0)
            kepler_elements_to_state_E(&elements, E, pos2, vel2);
        else
            kepler_elements_to_state_f(&elements, f, pos2, vel2);

        double pos3[3], vel3[3];
        matrix_vector_product(matrix, pos2, pos3);
        matrix_vector_product(matrix, vel2, vel3);

        ASSERT(eqv3(pos, pos3), "Position identity");
        ASSERT(eqv3(vel, vel3), "Velocity identity");
    }
}

static void orbit_from_elements_test(double *params, int num_params, void *extra_args, struct numtest_ctx *test_ctx) {
    (void)extra_args;

    ASSERT(num_params == 4, "");

    double p = 1.0 + params[0] * 1.0e10;
    double e = params[1] * 5.0;
    double mu = 1.0 + params[2] * 1.0e10;
    double a = p / (1.0 - square(e));
    double n = zero(e - 1.0) ?
        sqrt(mu / cube(p)) :    // parabolic
        sqrt(mu / cube(fabs(a)));
    double t0 = 0.0;

    struct kepler_elements elements = { p, e, n, 0.0, 0.0, 0.0, t0 };

    ASSERT_EQF(mu, kepler_orbit_gravity_parameter(&elements),
        "Gravitational constant is correct");

    double maxf = e > 1.0 ?
        acos(1.0/e) :       // hyperbolic
        (zero(e-1.0) ?
            4*M_PI/5 :      // parabolic
            M_PI);          // closed orbit

    double x1 = -1.0 + params[3] * 2.0;
    double f1 = maxf * x1;
    double E1 = kepler_anomaly_true_to_eccentric(e, f1);

    const int states_size = 2 * 2 * 3; // 2 x position,velocity x vec3
    double state_data[states_size];

    kepler_elements_to_state_f(&elements, f1, &state_data[6*0 + 0], &state_data[6*0 + 3]);
    kepler_elements_to_state_E(&elements, E1, &state_data[6*1 + 0], &state_data[6*1 + 3]);

    for(double *ptr = state_data+0; ptr != state_data + states_size; ++ptr)
        ASSERT(isfinite(*ptr), "Position and velocity not NaN");

    ASSERT(eqv3(state_data + 6*0 + 0, state_data + 6*1 + 0),
        "Conic and eccentric positions match");

    ASSERT(eqv3(state_data + 6*0 + 3, state_data + 6*1 + 3),
        "Conic and eccentric velocities match");

    for(int pass = 0; pass < 2; ++pass) {
        double *ptr = state_data + pass*2*3;
        double *pos = ptr + 0, *vel = ptr + 3;
        double r = mag(pos), v2 = dot(vel, vel); //, v = sqrt(v2);

        ASSERT(dot(pos, pos) > 0 && dot(vel, vel) > 0,
            "Position and Velocity non zero");

        double h[3];
        cross(pos, vel, h);

        ASSERT_EQF(mag(h), kepler_orbit_specific_angular_momentum(&elements),
            "Specific relative angular momentum");

        if(!kepler_orbit_parabolic(&elements)) {
            double visviva = v2/2 - mu/r;
            ASSERT_EQF(visviva, kepler_orbit_specific_orbital_energy(&elements),
                "Specific orbital energy (vis-viva)");
        } else {
            ASSERT_EQF(v2/2, mu/r,
                "Specific orbital energy (vis-viva)");
        }

        if(kepler_orbit_parabolic(&elements)) {
            ASSERT_EQF(
                pos[0] - p/2.0,
                -1.0/(2.0 * p) * square(pos[1]),
                "Orbit is a parabola");
        } else if(kepler_orbit_hyperbolic(&elements)) {
            ASSERT_EQF(1.0,
                square(pos[0] + a*e)/square(kepler_orbit_semi_major_axis(&elements)) -
                square(pos[1])/square(kepler_orbit_semi_minor_axis(&elements)),
                "Orbit is a hyperbola");
        } else {
            ASSERT_EQF(1.0,
                square(pos[0] + a*e)/square(kepler_orbit_semi_major_axis(&elements)) +
                square(pos[1])/square(kepler_orbit_semi_minor_axis(&elements)),
                "Orbit is an ellipse");
        }
    }
}

const struct numtest_case numtest_cases[] = {
    { "anomaly_test", anomaly_test, 2, NULL },
    { "orbit_from_state_test", orbit_from_state_test, 6, NULL },
    { "orbit_from_elements_test", orbit_from_elements_test, 4, NULL },
    { 0, 0, 0, 0 }
};