Replace Python numerical kernels with native C execution
This commit is contained in:
1 parent
48da6be21c
commit
3b38f73fe0
227 files changed
+16801
-75499
No files matched your search
@@ -0,0 +1,158 @@
|
||||
#include "runtime.h"
|
||||
#include <math.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <stdio.h>
|
||||
#include <time.h>
|
||||
#ifdef _WIN32
|
||||
#include <windows.h>
|
||||
#endif
|
||||
|
||||
double native_wall_time(void) {
|
||||
#ifdef _WIN32
|
||||
LARGE_INTEGER value, frequency;
|
||||
QueryPerformanceCounter(&value); QueryPerformanceFrequency(&frequency);
|
||||
return (double)value.QuadPart / (double)frequency.QuadPart;
|
||||
#else
|
||||
struct timespec value; clock_gettime(CLOCK_MONOTONIC, &value);
|
||||
return value.tv_sec + value.tv_nsec*1e-9;
|
||||
#endif
|
||||
}
|
||||
double native_cpu_time(void) {
|
||||
#ifdef _WIN32
|
||||
FILETIME creation, exit_time, kernel, user;
|
||||
GetProcessTimes(GetCurrentProcess(), &creation, &exit_time, &kernel, &user);
|
||||
ULARGE_INTEGER k, u; k.LowPart=kernel.dwLowDateTime; k.HighPart=kernel.dwHighDateTime;
|
||||
u.LowPart=user.dwLowDateTime; u.HighPart=user.dwHighDateTime;
|
||||
return (double)(k.QuadPart+u.QuadPart)*1e-7;
|
||||
#else
|
||||
return (double)clock()/CLOCKS_PER_SEC;
|
||||
#endif
|
||||
}
|
||||
|
||||
int native_poll(NativeRun *r, double t) {
|
||||
double now=native_wall_time();
|
||||
if (r->status) return 0;
|
||||
if (r->options.timeout > 0 && now-r->wall_start > r->options.timeout) {
|
||||
r->status=2; r->message="Native solve exceeded its time limit."; return 0;
|
||||
}
|
||||
if (now-r->last_progress >= 0.1) {
|
||||
r->last_progress=now;
|
||||
if (r->options.cancel_path) {
|
||||
FILE *f=fopen(r->options.cancel_path,"rb");
|
||||
if (f) { fclose(f); r->status=1; r->message="Simulation cancelled."; return 0; }
|
||||
}
|
||||
fprintf(stderr,"{\"phase\":\"integrating\",\"time\":%.17g,\"nfev\":%lu,\"acceptedSteps\":%lu}\n",t,r->nfev,r->accepted);
|
||||
fflush(stderr);
|
||||
}
|
||||
return 1;
|
||||
}
|
||||
|
||||
int native_rhs(NativeRun *r, double t, const double *y, double *dy) {
|
||||
double w[NOUTPUTS];
|
||||
r->nfev++;
|
||||
return model_eval(t,y,dy,w);
|
||||
}
|
||||
|
||||
int native_append(NativeRun *r, double t, const double *y) {
|
||||
r->final_time=t; memcpy(r->final_state,y,NSTATES*sizeof(double));
|
||||
if (!r->options.record_samples) return 1;
|
||||
if (r->count && r->times[r->count-1] == t) r->count--;
|
||||
if (r->count == r->capacity) {
|
||||
size_t cap=r->capacity ? r->capacity*2 : 1024;
|
||||
if (cap > 2000000 || cap > 268435456u / NSTATES / sizeof(double)) return 0;
|
||||
double *times=realloc(r->times,cap*sizeof(double));
|
||||
if (!times) return 0;
|
||||
r->times=times;
|
||||
double *states=realloc(r->states,cap*NSTATES*sizeof(double));
|
||||
if (!states) return 0;
|
||||
r->states=states; r->capacity=cap;
|
||||
}
|
||||
r->times[r->count]=t;
|
||||
memcpy(r->states+r->count*NSTATES,y,NSTATES*sizeof(double));
|
||||
r->count++; return 1;
|
||||
}
|
||||
|
||||
static double locate(int idx, double bound, int lower, double left, double right,
|
||||
NativeDense dense, void *context) {
|
||||
double state[NSTATES];
|
||||
for (int i=0;i<60;i++) {
|
||||
double mid=.5*(left+right);
|
||||
if (!dense(context,mid,state)) return NAN;
|
||||
if (lower ? state[idx] <= bound : state[idx] >= bound) right=mid;
|
||||
else left=mid;
|
||||
}
|
||||
return right;
|
||||
}
|
||||
|
||||
int native_accept(NativeRun *r, double t, double next, const double *old,
|
||||
const double *trial, NativeDense dense, void *context,
|
||||
double *accepted_time, double *accepted_state) {
|
||||
double when[2*(NSTOPS+1)], bounds[2*(NSTOPS+1)];
|
||||
double restitution[2*(NSTOPS+1)], thresholds[2*(NSTOPS+1)];
|
||||
int indices[2*(NSTOPS+1)], count=0;
|
||||
for (int j=0;j<NSTOPS;j++) {
|
||||
NativeStop s=model_stops[j]; int v=s.velocity_index, x=v+1;
|
||||
double vt0=1e-12*fmax(fabs(old[v]),1), vt1=1e-12*fmax(fabs(trial[v]),1);
|
||||
for (int side=0;side<2;side++) {
|
||||
int lower=side==0; double bound=lower?s.lower:s.upper;
|
||||
double tol=1e-12*fmax(fabs(bound),1), at=INFINITY;
|
||||
if (lower ? (old[x]<=bound+tol && old[v]<-vt0) : (old[x]>=bound-tol && old[v]>vt0)) at=t;
|
||||
else if (lower ? (old[x]>bound+tol && trial[x]<=bound) : (old[x]<bound-tol && trial[x]>=bound))
|
||||
at=locate(x,bound,lower,t,next,dense,context);
|
||||
else if (lower ? (old[x]<=bound && old[v]>vt0 && trial[v]<-vt1 && trial[x]<=bound) :
|
||||
(old[x]>=bound && old[v]<-vt0 && trial[v]>vt1 && trial[x]>=bound)) {
|
||||
double turn=locate(v,0,lower,t,next,dense,context);
|
||||
at=locate(x,bound,lower,turn,next,dense,context);
|
||||
}
|
||||
if (isfinite(at)) {
|
||||
when[count]=at; bounds[count]=bound; indices[count]=v;
|
||||
restitution[count]=lower?s.lower_restitution:s.upper_restitution;
|
||||
thresholds[count]=lower?s.lower_threshold:s.upper_threshold;
|
||||
count++;
|
||||
}
|
||||
}
|
||||
}
|
||||
double stop=next;
|
||||
for (int i=0;i<count;i++) stop=fmin(stop,when[i]);
|
||||
if (r->options.record_samples) {
|
||||
double st=r->options.start+r->sample_index*r->options.sample_step;
|
||||
while (st<=r->options.stop && (count ? st<stop : st<=stop)) {
|
||||
double sy[NSTATES];
|
||||
if (!dense(context,st,sy) || !native_append(r,st,sy)) return -1;
|
||||
r->sample_index++; st=r->options.start+r->sample_index*r->options.sample_step;
|
||||
}
|
||||
}
|
||||
if (count) {
|
||||
if (!dense(context,stop,accepted_state)) return -1;
|
||||
for (int i=0;i<count;i++) if (fabs(when[i]-stop)<=1e-12*fmax(fabs(stop),1)) {
|
||||
double incoming=accepted_state[indices[i]];
|
||||
accepted_state[indices[i]]=fabs(incoming)<=thresholds[i]?0:-restitution[i]*incoming;
|
||||
accepted_state[indices[i]+1]=bounds[i];
|
||||
}
|
||||
if (!native_append(r,stop,accepted_state)) return -1;
|
||||
while (r->options.start+r->sample_index*r->options.sample_step<=stop) r->sample_index++;
|
||||
r->events++;
|
||||
} else memcpy(accepted_state,trial,NSTATES*sizeof(double));
|
||||
*accepted_time=stop;
|
||||
r->final_time=stop; memcpy(r->final_state,accepted_state,NSTATES*sizeof(double));
|
||||
return count ? 1 : 0;
|
||||
}
|
||||
|
||||
int native_solve(NativeRun *r) {
|
||||
double y[NSTATES];
|
||||
if (!model_init(y)) { r->status=2; r->message="Native model initialization failed."; return 0; }
|
||||
r->sample_index=1;
|
||||
if (!native_append(r,r->options.start,y)) { r->status=2; r->message="Cannot allocate samples."; return 0; }
|
||||
r->wall_start=native_wall_time(); r->cpu_start=native_cpu_time();
|
||||
r->last_progress=r->wall_start-1;
|
||||
int ok=r->options.bdf ? native_bdf(r) : native_rk45(r);
|
||||
r->solve_seconds=native_wall_time()-r->wall_start;
|
||||
r->solve_cpu_seconds=native_cpu_time()-r->cpu_start;
|
||||
if (!ok && !r->status) { r->status=2; r->message="Native integration failed to advance."; }
|
||||
if (!native_append(r,r->final_time,r->final_state)) { r->status=2; r->message="Cannot save final state."; }
|
||||
if (!r->message) r->message=r->status ? "Native integration failed." : "Simulation completed.";
|
||||
return !r->status;
|
||||
}
|
||||
|
||||
void native_run_free(NativeRun *r) { free(r->times); free(r->states); }
|
||||
@@ -0,0 +1,97 @@
|
||||
/* CVODE owns its default numerical Jacobian and dense linear solver.
|
||||
* No project Jacobian, sparsity or derivative policy is changed here.
|
||||
*/
|
||||
#include "runtime.h"
|
||||
#include <cvode/cvode.h>
|
||||
#include <cvode/cvode_ls.h>
|
||||
#include <nvector/nvector_serial.h>
|
||||
#include <sunmatrix/sunmatrix_dense.h>
|
||||
#include <sunlinsol/sunlinsol_dense.h>
|
||||
#include <math.h>
|
||||
#include <string.h>
|
||||
|
||||
typedef struct { void *solver; N_Vector scratch; } CvDense;
|
||||
static int cv_dense(void *context, double t, double *out) {
|
||||
CvDense *d=context;
|
||||
if (CVodeGetDky(d->solver,t,0,d->scratch) < 0) return 0;
|
||||
memcpy(out,N_VGetArrayPointer(d->scratch),NSTATES*sizeof(double));
|
||||
return 1;
|
||||
}
|
||||
static int cv_rhs(sunrealtype t, N_Vector y, N_Vector dy, void *context) {
|
||||
NativeRun *r=context;
|
||||
if (!native_poll(r,r->final_time)) return -1;
|
||||
return native_rhs(r,t,N_VGetArrayPointer(y),N_VGetArrayPointer(dy)) ? 0 : 1;
|
||||
}
|
||||
static void counters(NativeRun *r, void *solver) {
|
||||
long int value=0;
|
||||
CVodeGetNumErrTestFails(solver,&value); r->rejected+=(unsigned long)value;
|
||||
CVodeGetNumJacEvals(solver,&value); r->njev+=(unsigned long)value;
|
||||
CVodeGetNumLinSolvSetups(solver,&value); r->nlu+=(unsigned long)value;
|
||||
}
|
||||
|
||||
int native_bdf(NativeRun *r) {
|
||||
SUNContext ctx=NULL;
|
||||
if (SUNContext_Create(SUN_COMM_NULL,&ctx)) return 0;
|
||||
N_Vector y=N_VNew_Serial(NSTATES,ctx), atol=N_VNew_Serial(NSTATES,ctx), scratch=N_VNew_Serial(NSTATES,ctx);
|
||||
SUNMatrix matrix=NULL; SUNLinearSolver linear=NULL; void *solver=NULL;
|
||||
int success=0;
|
||||
if (!y || !atol || !scratch) goto cleanup;
|
||||
memcpy(N_VGetArrayPointer(y),r->final_state,NSTATES*sizeof(double));
|
||||
memcpy(N_VGetArrayPointer(atol),model_atol,NSTATES*sizeof(double));
|
||||
matrix=SUNDenseMatrix(NSTATES,NSTATES,ctx);
|
||||
if (!matrix) goto cleanup;
|
||||
linear=SUNLinSol_Dense(y,matrix,ctx);
|
||||
if (!linear) goto cleanup;
|
||||
solver=CVodeCreate(CV_BDF,ctx);
|
||||
if (!solver) goto cleanup;
|
||||
double t=r->options.start;
|
||||
if (CVodeInit(solver,cv_rhs,t,y)<0 || CVodeSetUserData(solver,r)<0 ||
|
||||
CVodeSVtolerances(solver,r->options.rtol,atol)<0 ||
|
||||
CVodeSetLinearSolver(solver,linear,matrix)<0 ||
|
||||
CVodeSetMaxStep(solver,r->options.max_step)<0) goto cleanup;
|
||||
r->starts++;
|
||||
CvDense dense={solver,scratch};
|
||||
while (t<r->options.stop) {
|
||||
double boundary=model_next_break(t,r->options.stop);
|
||||
double end=boundary<r->options.stop?nextafter(boundary,-INFINITY):boundary;
|
||||
if (CVodeSetStopTime(solver,end)<0) goto cleanup;
|
||||
while (t<end) {
|
||||
if (!native_poll(r,t) || r->accepted>10000000 || r->events>10000) goto cleanup;
|
||||
double old[NSTATES], accepted[NSTATES], next=t;
|
||||
memcpy(old,N_VGetArrayPointer(y),sizeof(old));
|
||||
int flag=CVode(solver,end,y,&next,CV_ONE_STEP);
|
||||
if (flag<0 || next<=t) goto cleanup;
|
||||
r->accepted++; r->max_accepted_step=fmax(r->max_accepted_step,next-t);
|
||||
int impact=native_accept(r,t,next,old,N_VGetArrayPointer(y),cv_dense,&dense,&t,accepted);
|
||||
if (impact<0) goto cleanup;
|
||||
memcpy(N_VGetArrayPointer(y),accepted,sizeof(accepted));
|
||||
if (impact) {
|
||||
counters(r,solver);
|
||||
if (CVodeReInit(solver,t,y)<0) goto cleanup;
|
||||
r->starts++;
|
||||
}
|
||||
}
|
||||
t=boundary; r->final_time=t;
|
||||
memcpy(r->final_state,N_VGetArrayPointer(y),NSTATES*sizeof(double));
|
||||
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,r->final_state)) goto cleanup;
|
||||
r->sample_index++;
|
||||
}
|
||||
if (t<r->options.stop) {
|
||||
counters(r,solver);
|
||||
if (CVodeReInit(solver,t,y)<0) goto cleanup;
|
||||
r->starts++;
|
||||
}
|
||||
}
|
||||
success=1;
|
||||
cleanup:
|
||||
if (solver) { counters(r,solver); CVodeFree(&solver); }
|
||||
if (linear) SUNLinSolFree(linear);
|
||||
if (matrix) SUNMatDestroy(matrix);
|
||||
if (y) N_VDestroy(y);
|
||||
if (atol) N_VDestroy(atol);
|
||||
if (scratch) N_VDestroy(scratch);
|
||||
SUNContext_Free(&ctx);
|
||||
return success;
|
||||
}
|
||||
@@ -0,0 +1,119 @@
|
||||
#include "runtime.h"
|
||||
#include <sundials/sundials_config.h>
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <math.h>
|
||||
|
||||
static void json_string(FILE *f, const char *value) {
|
||||
fputc('"',f);
|
||||
for (const unsigned char *p=(const unsigned char *)value; *p; p++) {
|
||||
if (*p=='"' || *p=='\\') { fputc('\\',f); fputc(*p,f); }
|
||||
else if (*p<32) fprintf(f,"\\u%04x",*p);
|
||||
else fputc(*p,f);
|
||||
}
|
||||
fputc('"',f);
|
||||
}
|
||||
static void vector(FILE *f, const double *value, int count) {
|
||||
fputc('[',f);
|
||||
for (int i=0;i<count;i++) fprintf(f,"%s%.17g",i?",":"",value[i]);
|
||||
fputc(']',f);
|
||||
}
|
||||
static int probe(void) {
|
||||
double t, y[NSTATES], dy[NSTATES], w[NOUTPUTS];
|
||||
while (scanf("%lf",&t)==1) {
|
||||
for (int i=0;i<NSTATES;i++) if (scanf("%lf",&y[i])!=1) return 64;
|
||||
int ok=model_eval(t,y,dy,w);
|
||||
printf("{\"success\":%s",ok?"true":"false");
|
||||
if (ok) { printf(",\"rhs\":"); vector(stdout,dy,NSTATES); printf(",\"outputs\":"); vector(stdout,w,NOUTPUTS); }
|
||||
printf("}\n");
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
static int write_result(NativeRun *r, const char *path) {
|
||||
double dy[NSTATES], final[NOUTPUTS];
|
||||
int final_ok=model_eval(r->final_time,r->final_state,dy,final);
|
||||
size_t length=r->count*NOUTPUTS;
|
||||
if (length>268435456u/sizeof(double)) return 0;
|
||||
double *values=length?malloc(length*sizeof(double)):NULL;
|
||||
if (length && !values) return 0;
|
||||
for (size_t i=0;i<r->count;i++) {
|
||||
if (!model_eval(r->times[i],r->states+i*NSTATES,dy,values+i*NOUTPUTS)) {
|
||||
free(values); return 0;
|
||||
}
|
||||
}
|
||||
FILE *f=fopen(path,"wb"); if (!f) { free(values); return 0; }
|
||||
fprintf(f,"{\"success\":%s,\"status\":",!r->status?"true":"false");
|
||||
json_string(f,r->status==0?"completed":r->status==1?"cancelled":"failed");
|
||||
fprintf(f,",\"message\":"); json_string(f,r->message);
|
||||
fprintf(f,",\"backend\":\"native-c\",\"method\":\"%s\",\"solver\":\"%s\",\"sundialsVersion\":\"%s\","
|
||||
"\"simulatedUntil\":%.17g,\"solveSeconds\":%.17g,\"solveCpuSeconds\":%.17g,"
|
||||
"\"nfev\":%lu,\"acceptedSteps\":%lu,\"rejectedSteps\":%lu,\"stateTransitions\":%lu,"
|
||||
"\"solverStarts\":%lu,\"njev\":%lu,\"nlu\":%lu,\"maxAcceptedStep\":%.17g,\"series\":{",
|
||||
r->options.bdf?"BDF":"RK45",r->options.bdf?"CVODE":"Dormand-Prince 5(4)",SUNDIALS_VERSION,
|
||||
r->final_time,r->solve_seconds,r->solve_cpu_seconds,r->nfev,r->accepted,r->rejected,
|
||||
r->events,r->starts,r->njev,r->nlu,r->max_accepted_step);
|
||||
if (r->count) {
|
||||
fprintf(f,"\"time\":[");
|
||||
for (size_t i=0;i<r->count;i++) fprintf(f,"%s%.17g",i?",":"",r->times[i]);
|
||||
fputc(']',f);
|
||||
for (int j=0;j<NOUTPUTS;j++) {
|
||||
fputc(',',f); json_string(f,model_output_keys[j]); fprintf(f,":[");
|
||||
for (size_t i=0;i<r->count;i++) fprintf(f,"%s%.17g",i?",":"",values[i*NOUTPUTS+j]);
|
||||
fputc(']',f);
|
||||
}
|
||||
}
|
||||
fprintf(f,"},\"final\":{");
|
||||
if (final_ok) for (int j=0;j<NOUTPUTS;j++) {
|
||||
if (j) fputc(',',f);
|
||||
json_string(f,model_output_keys[j]); fprintf(f,":%.17g",final[j]);
|
||||
}
|
||||
fprintf(f,"},\"finalState\":"); vector(f,r->final_state,NSTATES);
|
||||
fprintf(f,"}\n"); int ok=!ferror(f); if (fclose(f)) ok=0;
|
||||
free(values); return ok;
|
||||
}
|
||||
|
||||
int main(int argc, char **argv) {
|
||||
NativeRun r={0};
|
||||
r.options=(NativeOptions){0,10,.02,.001,1e-6,300,0,1,NULL};
|
||||
const char *output="result.json";
|
||||
for (int i=1;i<argc;i++) {
|
||||
const char *arg=argv[i];
|
||||
if (!strcmp(arg,"--probe")) return probe();
|
||||
if (!strcmp(arg,"--init")) {
|
||||
double y[NSTATES]; if (!model_init(y)) return 2;
|
||||
vector(stdout,y,NSTATES); fputc('\n',stdout); return 0;
|
||||
}
|
||||
if (!strcmp(arg,"--solve-only")) { r.options.record_samples=0; continue; }
|
||||
if (i+1==argc) return 64;
|
||||
const char *value=argv[++i];
|
||||
if (!strcmp(arg,"--output")) output=value;
|
||||
else if (!strcmp(arg,"--cancel-file")) r.options.cancel_path=value;
|
||||
else if (!strcmp(arg,"--method")) {
|
||||
if (strcmp(value,"RK45") && strcmp(value,"BDF")) return 64;
|
||||
r.options.bdf=!strcmp(value,"BDF");
|
||||
} else {
|
||||
char *end; double number=strtod(value,&end);
|
||||
if (*end || !isfinite(number)) return 64;
|
||||
if (!strcmp(arg,"--start")) r.options.start=number;
|
||||
else if (!strcmp(arg,"--stop")) r.options.stop=number;
|
||||
else if (!strcmp(arg,"--sample-step")) r.options.sample_step=number;
|
||||
else if (!strcmp(arg,"--max-step")) r.options.max_step=number;
|
||||
else if (!strcmp(arg,"--rtol")) r.options.rtol=number;
|
||||
else if (!strcmp(arg,"--timeout")) r.options.timeout=number;
|
||||
else return 64;
|
||||
}
|
||||
}
|
||||
if (!(r.options.stop>r.options.start && r.options.sample_step>0 && r.options.max_step>0 &&
|
||||
r.options.rtol>0 && r.options.timeout>0) ||
|
||||
r.options.start+r.options.sample_step==r.options.start ||
|
||||
r.options.start+r.options.max_step==r.options.start ||
|
||||
!isfinite(r.options.stop-r.options.start)) return 64;
|
||||
if (r.options.record_samples &&
|
||||
((r.options.stop-r.options.start)/r.options.sample_step>1000000 ||
|
||||
((r.options.stop-r.options.start)/r.options.sample_step+1024)*(NSTATES+NOUTPUTS)*sizeof(double)>268435456)) return 64;
|
||||
native_solve(&r);
|
||||
int saved=write_result(&r,output);
|
||||
int code=saved?(r.status==2?2:0):3;
|
||||
native_run_free(&r); return code;
|
||||
}
|
||||
@@ -0,0 +1,119 @@
|
||||
/* 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));
|
||||
}
|
||||
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;
|
||||
}
|
||||
Reference in new issue
Block a user