SystemSimulationApp/native/runtime/rk45.c

/* Dormand-Prince 5(4), initial step and quartic dense output.
 * Adapted from SciPy's BSD-3-Clause implementation; see THIRD_PARTY_NOTICES.txt.
 */
#include "runtime.h"
#include <math.h>
#include <string.h>
static const double C[6]={0,1./5,3./10,4./5,8./9,1};
static const double A[6][6]={
    {0},{1./5},{3./40,9./40},{44./45,-56./15,32./9},
    {19372./6561,-25360./2187,64448./6561,-212./729},
    {9017./3168,-355./33,46732./5247,49./176,-5103./18656}};
static const double B[6]={35./384,0,500./1113,125./192,-2187./6784,11./84};
static const double E[7]={-71./57600,0,71./16695,-71./1920,17253./339200,-22./525,1./40};
static const double P[7][4]={
    {1,-8048581381./2820520608,8663915743./2820520608,-12715105075./11282082432},
    {0,0,0,0},
    {0,131558114200./32700410799,-68118460800./10900136933,87487479700./32700410799},
    {0,-1754552775./470086768,14199869525./1410260304,-10690763975./1880347072},
    {0,127303824393./49829197408,-318862633887./49829197408,701980252875./199316789632},
    {0,-282668133./205662961,2019193451./616988883,-1453857185./822651844},
    {0,40617522./29380423,-110615467./29380423,69997945./29380423}};
static double initial_step(NativeRun *s,double t,double end,const double *y,const double *f) {
    double scale[NSTATES],trial[NSTATES],f1[NSTATES],d0=0,d1=0,d2=0;
    for(int i=0;i<NSTATES;i++) { scale[i]=model_atol[i]+fabs(y[i])*s->options.rtol;
        d0+=pow(y[i]/scale[i],2);d1+=pow(f[i]/scale[i],2); }
    d0=sqrt(d0/NSTATES);d1=sqrt(d1/NSTATES);
    double h0=d0<1e-5||d1<1e-5?1e-6:.01*d0/d1;h0=fmin(h0,end-t);
    for(int i=0;i<NSTATES;i++) trial[i]=y[i]+h0*f[i];
    if(!native_rhs(s,t+h0,trial,f1)) return NAN;
    for(int i=0;i<NSTATES;i++) d2+=pow((f1[i]-f[i])/scale[i],2);
    d2=sqrt(d2/NSTATES)/h0;
    double h1=d1<=1e-15&&d2<=1e-15?fmax(1e-6,h0*1e-3):pow(.01/fmax(d1,d2),.2);
    return fmin(fmin(100*h0,h1),fmin(end-t,s->options.max_step));
}

typedef struct { double t, h, y[NSTATES], q[NSTATES][4]; } RkDense;
static int rk_dense(void *context, double t, double *out) {
    RkDense *d=context; double x=(t-d->t)/d->h;
    double powers[4]={x,x*x,x*x*x,x*x*x*x};
    for (int i=0;i<NSTATES;i++) {
        double sum=0; for (int j=0;j<4;j++) sum+=d->q[i][j]*powers[j];
        out[i]=d->y[i]+d->h*sum;
    }
    return 1;
}

int native_rk45(NativeRun *r) {
    double y[NSTATES], f[NSTATES], t=r->options.start;
    memcpy(y,r->final_state,sizeof(y));
    while (t < r->options.stop) {
        double boundary=model_next_break(t,r->options.stop);
        double end=boundary<r->options.stop ? nextafter(boundary,-INFINITY) : boundary;
        int restart=1; double h_abs=0;
        while (t<end) {
            if (!native_poll(r,t) || r->accepted>10000000 || r->events>10000) return 0;
            if (restart) {
                r->starts++;
                if (!native_rhs(r,t,y,f)) return 0;
                h_abs=initial_step(r,t,end,y,f);
                if (!isfinite(h_abs) || h_abs<=0) return 0;
                restart=0;
            }
            double minimum=10*fabs(nextafter(t,INFINITY)-t);
            h_abs=fmax(fmin(h_abs,r->options.max_step),minimum);
            double K[7][NSTATES], yn[NSTATES], next, h;
            int rejected=0;
            for (;;) {
                if (h_abs<minimum || !native_poll(r,t)) return 0;
                next=fmin(t+h_abs,end); h=next-t; h_abs=fabs(h);
                memcpy(K[0],f,sizeof(f)); int valid=1;
                for (int stage=1;stage<6;stage++) {
                    double temp[NSTATES];
                    for (int i=0;i<NSTATES;i++) {
                        double sum=0; for (int j=0;j<stage;j++) sum+=A[stage][j]*K[j][i];
                        temp[i]=y[i]+h*sum;
                    }
                    if (!native_rhs(r,t+C[stage]*h,temp,K[stage])) { valid=0; break; }
                }
                if (valid) {
                    for (int i=0;i<NSTATES;i++) {
                        double sum=0; for (int j=0;j<6;j++) sum+=B[j]*K[j][i];
                        yn[i]=y[i]+h*sum;
                    }
                    valid=native_rhs(r,t+h,yn,K[6]);
                }
                double error=0;
                if (valid) {
                    for (int i=0;i<NSTATES;i++) {
                        double sum=0; for (int j=0;j<7;j++) sum+=E[j]*K[j][i];
                        double scale=model_atol[i]+fmax(fabs(y[i]),fabs(yn[i]))*r->options.rtol;
                        error+=pow(h*sum/scale,2);
                    }
                    error=sqrt(error/NSTATES);
                } else error=INFINITY;
                if (error<1) {
                    double factor=error==0?10:fmin(10,.9*pow(error,-.2));
                    if (rejected) factor=fmin(factor,1);
                    h_abs*=factor; break;
                }
                h_abs*=fmax(.2,.9*pow(error,-.2)); rejected=1; r->rejected++;
            }
            RkDense dense={0}; dense.t=t; dense.h=h; memcpy(dense.y,y,sizeof(y));
            for (int i=0;i<NSTATES;i++) for (int j=0;j<4;j++)
                for (int k=0;k<7;k++) dense.q[i][j]+=K[k][i]*P[k][j];
            r->accepted++; r->max_accepted_step=fmax(r->max_accepted_step,h);
            int impact=native_accept(r,t,next,y,yn,rk_dense,&dense,&t,y);
            if (impact<0) return 0;
            if (impact) restart=1;
            else memcpy(f,K[6],sizeof(f));
        }
        if (!native_time_boundary_samples(r,boundary,y)) return 0;
        t=boundary; r->final_time=t; memcpy(r->final_state,y,sizeof(y));
        double sample=r->options.start+r->sample_index*r->options.sample_step;
        if (r->options.record_samples && sample<=t && sample<=r->options.stop) {
            if (!native_append(r,sample,y)) return 0;
            r->sample_index++;
        }
    }
    return t==r->options.stop;
}