Magic from the 20th century: Runge-Kutta-Nyström numerical integration in structure of array style.

rkn-steps.c

#include <stdio.h>

static float mad(float a, float b, float c)
{
    return a * b + c;
}

static void step1(const float *vel0, const float *a, float *d, float *v, float *delta, float *vel, float dt, size_t n)
{
    for(size_t i = 0; i < n; ++i)
    {
        float k1 = a[i] * 0.5 * dt;
        v[i] = vel0[i] + k1;
        d[i] = mad(k1, 0.5, vel0[i]) * 0.5 * dt; // d = K

        delta[i] = mad(k1, 1.0/3.0, vel0[i]); // delta = vel0 + (1/3)*k1
        vel[i] = mad(k1, 1.0/3.0, vel0[i]); // vel = vel0 + (1/3)*k1
    }
}

static void step2(const float *vel0, const float *a, float *v, float *delta, float *vel, float dt, size_t n)
{
    for(size_t i = 0; i < n; ++i)
    {
        float k2 = a[i] * 0.5 * dt;

        v[i] = vel0[i] + k2; // v = vel0 + k2

        delta[i] = mad(k2, 1.0/3.0, delta[i]); // delta = vel0 + (1/3)(k1+k2)
        vel[i] = mad(k2, 2.0/3.0, vel[i]); // vel = (1/3)*k1 + (2/3)*k2
    }
}

static void step3(const float *vel0, const float *a, float *d, float *v, float *delta, float *vel, float dt, size_t n)
{
    for(size_t i = 0; i < n; ++i)
    {
        float k3 = a[i] * 0.5 * dt;

        d[i] = (vel[i] + k3) * dt; // d = L
        v[i] = mad(k3, 2.0, vel0[i]);

        delta[i] = mad(k3, 1.0/3.0, delta[i]); // delta = vel0 + (1/3)*(k1+k2+k3)
        vel[i] = mad(k3, 2.0/3.0, vel[i]); // vel = (1/3)*k1+(2/3)*k2+(2/3)*k3
    }
}

static void step4(const float *a, float *delta, float *vel, float dt, size_t n)
{
    for(size_t i = 0; i < n; ++i)
    {
        float k4 = a[i] * 0.5 * dt;

        vel[i] = mad(k4, 1.0/3.0, vel[i]); // vel = vel0 + (1/3)*k1+(2/3)*k2+(2/3)*k3+(1/3)*k4
        delta[i] = delta[i] * dt; // delta = dt * (vel0 + (1/3)*(k1+k2+k3))
    }
}

static void accelerate(const float *s, const float *vel, float *a, float t, size_t n)
{
    for(size_t i = 0; i < n; ++i)
        a[i] = -s[i];
}

static void update(const float *s0, const float *delta, float *s, size_t n)
{
    for(size_t i = 0; i < n; ++i)
        s[i] = s0[i] + delta[i];
}

static void sim_step(const float *s0, const float *vel0, float *s, float *vel, float t0, float dt, size_t n)
{
    float d_temp[n], v_temp[n];
    float a[n];
    float delta[n];

    accelerate(s0, vel0, a, t0, n);
    step1(vel0, a, d_temp, v_temp, delta, vel, dt, n);
    update(s0, d_temp, s, n);

    accelerate(s, v_temp, a, t0 + 0.5 * dt, n);
    step2(vel0, a, v_temp, delta, vel, dt, n);
    accelerate(s, v_temp, a, t0 + 0.5 * dt, n);
    step3(vel0, a, d_temp, v_temp, delta, vel, dt, n);
    update(s0, d_temp, s, n);

    accelerate(s, v_temp, a, t0 + dt, n);
    step4(a, delta, vel, dt, n);
    update(s0, delta, s, n);

    accelerate(s, vel, a, t0+dt, n);
    printf("%f\t%f\t%f\t%f\n", t0+dt, s[0], vel[0], a[0]);
}


int main(int argc, char *argv)
{
    const size_t n = 1;

    float t0 = 0.0, dt = 0.1;
    float s0[n], s[n];
    float vel0[n], vel[n];

    for(size_t i = 0; i < n; ++i)
    {
        vel0[i] = 0;
        s0[i] = 1;
    }

    for(int i = 0; i < 100; ++i)
    {
        float *s_in = i&1 ? s : s0, *s_out = i&1 ? s0 : s;
        float *vel_in = i&1 ? vel : vel0, *vel_out = i&1 ? vel0 : vel;

        float t = t0 + i * dt;
        sim_step(s_in, vel_in, s_out, vel_out, t, dt, n);

    }

    return 0;
}