245 lines
11 KiB
C
245 lines
11 KiB
C
#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);
|
|
}
|
|
|
|
/* Derivative probes use deterministic property evaluation: the generated
|
|
* Jacobian entry point omits cross-storage cache seeds, while the ordinary RHS
|
|
* retains all existing physical expressions and state-property reuse. */
|
|
int native_jacobian_rhs(NativeRun *r, double t, const double *y, double *dy) {
|
|
#if defined(MODEL_JACOBIAN_CANONICAL_RHS) && MODEL_JACOBIAN_CANONICAL_RHS
|
|
double w[NOUTPUTS];
|
|
r->nfev++;
|
|
return model_eval_jacobian(t,y,dy,w);
|
|
#else
|
|
return native_rhs(r,t,y,dy);
|
|
#endif
|
|
}
|
|
|
|
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;
|
|
}
|
|
|
|
#if NFRICTIONS
|
|
static int friction_drives(NativeRun *r,double t,const double *state,double *drives) {
|
|
r->nfev++;
|
|
return model_friction_drives(t,state,drives);
|
|
}
|
|
|
|
static double locate_breakaway(NativeRun *r,int index,double left,double right,
|
|
NativeDense dense,void *context) {
|
|
double state[NSTATES],drives[NFRICTIONS];
|
|
for(int i=0;i<60;i++) {
|
|
double mid=left+.5*(right-left);
|
|
if(mid<=left || mid>=right) break;
|
|
if(!dense(context,mid,state) || !friction_drives(r,mid,state,drives)) return NAN;
|
|
if(fabs(drives[index])>model_frictions[index].breakaway_force) right=mid;
|
|
else left=mid;
|
|
}
|
|
return right;
|
|
}
|
|
#endif
|
|
|
|
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+NFRICTIONS+1)], bounds[2*(NSTOPS+NFRICTIONS+1)];
|
|
double restitution[2*(NSTOPS+NFRICTIONS+1)], thresholds[2*(NSTOPS+NFRICTIONS+1)];
|
|
int indices[2*(NSTOPS+NFRICTIONS+1)], friction[2*(NSTOPS+NFRICTIONS+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;
|
|
friction[count]=-1;
|
|
restitution[count]=lower?s.lower_restitution:s.upper_restitution;
|
|
thresholds[count]=lower?s.lower_threshold:s.upper_threshold;
|
|
count++;
|
|
}
|
|
}
|
|
}
|
|
#if NFRICTIONS
|
|
double before[NFRICTIONS],after[NFRICTIONS];
|
|
if(!friction_drives(r,t,old,before) || !friction_drives(r,next,trial,after)) return -1;
|
|
for(int j=0;j<NFRICTIONS;j++) {
|
|
NativeFriction f=model_frictions[j];
|
|
double mode=old[f.mode_index],at=INFINITY;
|
|
if(fabs(mode)<.5) {
|
|
if(fabs(before[j])>f.breakaway_force) at=t;
|
|
else if(fabs(after[j])>f.breakaway_force)
|
|
at=locate_breakaway(r,j,t,next,dense,context);
|
|
} else {
|
|
double direction=mode>0?1:-1;
|
|
if(direction*old[f.velocity_index]>f.velocity_threshold &&
|
|
direction*trial[f.velocity_index]<=f.velocity_threshold)
|
|
at=locate(f.velocity_index,direction*f.velocity_threshold,mode>0,t,next,dense,context);
|
|
else if(direction*old[f.velocity_index]>=0 && direction*trial[f.velocity_index]<0)
|
|
at=locate(f.velocity_index,0,mode>0,t,next,dense,context);
|
|
}
|
|
if(isnan(at)) return -1;
|
|
if(isfinite(at)) {
|
|
when[count]=at;indices[count]=f.velocity_index;friction[count]=j;
|
|
bounds[count]=restitution[count]=thresholds[count]=0;count++;
|
|
}
|
|
}
|
|
#endif
|
|
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)) {
|
|
if(friction[i]>=0) continue;
|
|
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 NFRICTIONS
|
|
/* Stop/reverse only at an accepted event. The discrete mode travels
|
|
* with saved states so replayed results and Jacobian trials are pure. */
|
|
double drives[NFRICTIONS];
|
|
for(int i=0;i<count;i++) if(friction[i]<0 && fabs(when[i]-stop)<=1e-12*fmax(fabs(stop),1)) {
|
|
for(int j=0;j<NFRICTIONS;j++) if(model_frictions[j].velocity_index==indices[i]) {
|
|
double velocity=accepted_state[indices[i]];
|
|
accepted_state[model_frictions[j].mode_index]=velocity>0?1:(velocity<0?-1:0);
|
|
}
|
|
}
|
|
if(!friction_drives(r,stop,accepted_state,drives)) return -1;
|
|
for(int i=0;i<count;i++) if(friction[i]>=0 && fabs(when[i]-stop)<=1e-12*fmax(fabs(stop),1)) {
|
|
int j=friction[i];NativeFriction f=model_frictions[j];
|
|
accepted_state[f.velocity_index]=0;
|
|
accepted_state[f.mode_index]=fabs(drives[j])>f.breakaway_force ? (drives[j]>0?1:-1) : 0;
|
|
}
|
|
#endif
|
|
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; }
|
|
#if NFRICTIONS
|
|
double drives[NFRICTIONS];
|
|
if(!friction_drives(r,r->options.start,y,drives)) {r->status=2;r->message="Cannot initialize friction.";return 0;}
|
|
for(int j=0;j<NFRICTIONS;j++) {
|
|
NativeFriction f=model_frictions[j];
|
|
if(fabs(y[f.mode_index])<.5 && fabs(drives[j])>f.breakaway_force)
|
|
y[f.mode_index]=drives[j]>0?1:-1;
|
|
}
|
|
#endif
|
|
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); }
|