/* Ouput: y=updated_conc, xx=end_time, hdid=achieved_stepsize , hnext= estimated_next_ss */
bool stifbs(double y[], double dydx[], int nv, double *xx, double htry, double eps,
double yscal[], double *hdid, double *hnext,
void (*derivs)(double, double [], double [], int, long, double), long node)
{
int i,iq,k,kk,km;
static int first=1,kmax,kopt,nvold = -1;
static double epsold = -1.0,xnew;
double eps1,errmax,fact,h,red,scale,work,wrkmin,xest;
double *dfdx,**dfdy,*err,*yerr,*ysav,*yseq;
static double a[IMAXX+1];
static double alf[KMAXX+1][KMAXX+1];
static int nseq[IMAXX+1]={0,2,6,10,14,22,34,50,70};
// static int nseq[IMAXX+1]={0,2,6,10,14,22,34,50,70,98,138,194,274,386};
// Num Recip S. 744
// Differenz zwischen zwei Werten muss ein Vielfaches von 4 sein
// So wählen, dass Verhältnis der Werte <= 5/7 ist
// z.B. 10, 14 -> 10/14 <= 5/7
// nächster wäre 18, aber 14/18 > 5/7, deshalb 22 mit 14/22 <= 5/7
int reduct,exitflag=0;
km=0; //SB avoid warnings
red = 0.0; //SB avoid warning
errmax = 0.0; // SB avoid warning
scale = 0.0; // SB avoid warning
bool success = true;
d=dmatrix(1,nv,1,KMAXX);
dfdx=dvector(1,nv);
dfdy=dmatrix(1,nv,1,nv);
err=dvector(1,KMAXX);
x=dvector(1,KMAXX);
yerr=dvector(1,nv);
ysav=dvector(1,nv);
yseq=dvector(1,nv);
// reinitialize as eps is a new tolerance
// or if nv have changed
if(eps != epsold || nv != nvold)
{
*hnext = xnew = -1.0e29; // impossible values
eps1=SAFE1*eps;
// compute work coefficients Ak
a[1]=nseq[1]+1;
for (k=1;k<=KMAXX;k++) a[k+1]=a[k]+nseq[k+1];
// compute alpha(k,q)
for (iq=2;iq<=KMAXX;iq++)
{
for (k=1;k<iq;k++)
alf[k][iq]=pow(eps1,((a[k+1]-a[iq+1])/
((a[iq+1]-a[1]+1.0)*(2*k+1))));
}
epsold=eps;
// save nv
nvold=nv;
// add cost of Jacobian evals to work coeffs a[]
a[1] += nv;
for (k=1;k<=KMAXX;k++) a[k+1]=a[k]+nseq[k+1];
// Determine opt. row no. for convergence
for (kopt=2;kopt<KMAXX;kopt++)
if (a[kopt+1] > a[kopt]*alf[kopt-1][kopt]) break;
kmax=kopt;
}
h=htry;
// save starting values
for (i=1;i<=nv;i++) ysav[i]=y[i];
// evaluate jacobian matrix, update dfdx, dfdy
jacobn(*xx,y,dfdx,dfdy,nv,node);
// new stepsize or new integration --> re-establish order window
if (*xx != xnew || h != (*hnext))
{
first=1;
kopt=kmax;
}
reduct=0;
// start stepping
for (;;)
{
// evaluate the sequence of modified midpoint integrations
for (k=1;k<=kmax;k++)
{
xnew=(*xx)+h;
if (xnew == (*xx)) //CB
{
std::cout << "step size underflow in stifbs" << "\n";
//nrerror("step size underflow in stifbs");
success = false;
free_dvector(yseq,1,nv);
free_dvector(ysav,1,nv);
free_dvector(yerr,1,nv);
free_dvector(x,1,KMAXX);
free_dvector(err,1,KMAXX);
free_dmatrix(dfdy,1,nv,1,nv);
free_dvector(dfdx,1,nv);
free_dmatrix(d,1,KMAXX,1,KMAXX);
return success;
}
// semi-implicite midpoint algorithm
success = simpr(ysav,dydx,dfdx,dfdy,nv,*xx,h,nseq[k],yseq,derivs,node);
if(success==0){
free_dvector(yseq,1,nv);
free_dvector(ysav,1,nv);
free_dvector(yerr,1,nv);
free_dvector(x,1,KMAXX);
free_dvector(err,1,KMAXX);
free_dmatrix(dfdy,1,nv,1,nv);
free_dvector(dfdx,1,nv);
free_dmatrix(d,1,KMAXX,1,KMAXX);
return success;
}
// squared since error is even
xest=DSQR(h/nseq[k]);
pzextr(k,xest,yseq,y,yerr,nv);
// compute normalized error estimate eps(k)
if (k != 1)
{
errmax=TINY;
for (i=1;i<=nv;i++) errmax=DMAX(errmax,fabs(yerr[i]/yscal[i]));
// scale error relative to tolerance
errmax /= eps;
km=k-1;
err[km]=pow(errmax/SAFE1,1.0/(double)(2*km+1));
}
if (k != 1 && (k >= kopt-1 || first))
{
if (errmax < 1.0)
{
exitflag=1;
break;
}
if (k == kmax || k == kopt+1)
{
red=SAFE2/err[km];
break;
}
else if (k == kopt && alf[kopt-1][kopt] < err[km])
{
red=1.0/err[km];
break;
}
else if (kopt == kmax && alf[km][kmax-1] < err[km])
{
red=alf[km][kmax-1]*SAFE2/err[km];
break;
}
else if (alf[km][kopt] < err[km])
{
red=alf[km][kopt-1]/err[km];
break;
}
}
}
// if (exitflag) std::cout << " Exitflag > 0 in stifbs of biodegradation" << "\n";
if (exitflag) break;
// reduce stepsize by at least REDMIN and at most by REDMAX
red=DMIN(red,REDMIN);
red=DMAX(red,REDMAX);
h *= red;
reduct=1;
} // try again
// successfull step was taken
*xx=xnew;
*hdid=h;
first=0;
wrkmin=1.0e35;
// compute optimal row for convergence and corresponding stepsize
for (kk=1;kk<=km;kk++)
{
fact=DMAX(err[kk],SCALMX);
work=fact*a[kk+1];
if (work < wrkmin)
{
scale=fact;
wrkmin=work;
kopt=kk+1;
}
}
*hnext=h/scale;
if (kopt >= k && kopt != kmax && !reduct)
{
// check for possible order increse but not if stepsize was just reduced
fact=DMAX(scale/alf[kopt-1][kopt],SCALMX);
if (a[kopt+1]*fact <= wrkmin)
{
*hnext=h/fact;
kopt++;
}
}
free_dvector(yseq,1,nv);
free_dvector(ysav,1,nv);
free_dvector(yerr,1,nv);
free_dvector(x,1,KMAXX);
free_dvector(err,1,KMAXX);
free_dmatrix(dfdy,1,nv,1,nv);
free_dvector(dfdx,1,nv);
free_dmatrix(d,1,KMAXX,1,KMAXX);
return success;
}
void bsstep(double y[], double dydx[], int nv, double *xx, double htry, double eps,
double yscal[], double *hdid, double *hnext,
void (*derivs)(double, double [], double []))
{
void mmid(double y[], double dydx[], int nvar, double xs, double htot,
int nstep, double yout[], void (*derivs)(double, double[], double[]));
void pzextr(int iest, double xest, double yest[], double yz[], double dy[],
int nv);
int i,iq,k,kk,km;
static int first=1,kmax,kopt;
static double epsold = -1.0,xnew;
double eps1,errmax,fact,h,red,scale,work,wrkmin,xest;
double *err,*yerr,*ysav,*yseq;
static double a[IMAXX+1];
static double alf[KMAXX+1][KMAXX+1];
static int nseq[IMAXX+1]={0,2,4,6,8,10,12,14,16,18};
int reduct,exitflag=0;
d=dmatrix(1,nv,1,KMAXX);
err=dvector(1,KMAXX);
x=dvector(1,KMAXX);
yerr=dvector(1,nv);
ysav=dvector(1,nv);
yseq=dvector(1,nv);
if (eps != epsold) {
*hnext = xnew = -1.0e29;
eps1=SAFE1*eps;
a[1]=nseq[1]+1;
for (k=1;k<=KMAXX;k++) a[k+1]=a[k]+nseq[k+1];
for (iq=2;iq<=KMAXX;iq++) {
for (k=1;k<iq;k++)
alf[k][iq]=pow(eps1,(a[k+1]-a[iq+1])/
((a[iq+1]-a[1]+1.0)*(2*k+1)));
}
epsold=eps;
for (kopt=2;kopt<KMAXX;kopt++)
if (a[kopt+1] > a[kopt]*alf[kopt-1][kopt]) break;
kmax=kopt;
}
h=htry;
for (i=1;i<=nv;i++) ysav[i]=y[i];
if (*xx != xnew || h != (*hnext)) {
first=1;
kopt=kmax;
}
reduct=0;
for (;;) {
for (k=1;k<=kmax;k++) {
xnew=(*xx)+h;
if (xnew == (*xx)) nrerror("step size underflow in bsstep");
mmid(ysav,dydx,nv,*xx,h,nseq[k],yseq,derivs);
xest=SQR(h/nseq[k]);
pzextr(k,xest,yseq,y,yerr,nv);
if (k != 1) {
errmax=TINY;
for (i=1;i<=nv;i++) errmax=FMAX(errmax,fabs(yerr[i]/yscal[i]));
errmax /= eps;
km=k-1;
err[km]=pow(errmax/SAFE1,1.0/(2*km+1));
}
if (k != 1 && (k >= kopt-1 || first)) {
if (errmax < 1.0) {
exitflag=1;
break;
}
if (k == kmax || k == kopt+1) {
red=SAFE2/err[km];
break;
}
else if (k == kopt && alf[kopt-1][kopt] < err[km]) {
red=1.0/err[km];
break;
}
else if (kopt == kmax && alf[km][kmax-1] < err[km]) {
red=alf[km][kmax-1]*SAFE2/err[km];
break;
}
else if (alf[km][kopt] < err[km]) {
red=alf[km][kopt-1]/err[km];
break;
}
}
}
if (exitflag) break;
red=FMIN(red,REDMIN);
red=FMAX(red,REDMAX);
h *= red;
reduct=1;
}
*xx=xnew;
*hdid=h;
first=0;
wrkmin=1.0e35;
for (kk=1;kk<=km;kk++) {
fact=FMAX(err[kk],SCALMX);
work=fact*a[kk+1];
if (work < wrkmin) {
scale=fact;
wrkmin=work;
kopt=kk+1;
}
}
*hnext=h/scale;
if (kopt >= k && kopt != kmax && !reduct) {
fact=FMAX(scale/alf[kopt-1][kopt],SCALMX);
if (a[kopt+1]*fact <= wrkmin) {
*hnext=h/fact;
kopt++;
}
}
free_dvector(yseq,1,nv);
free_dvector(ysav,1,nv);
free_dvector(yerr,1,nv);
free_dvector(x,1,KMAXX);
free_dvector(err,1,KMAXX);
free_dmatrix(d,1,nv,1,KMAXX);
}
void stifbs(float y[], float dydx[], int nv, float *xx, float htry, float eps,
float yscal[], float *hdid, float *hnext,
void (*derivs)(float, float [], float []))
{
void jacobn(float x, float y[], float dfdx[], float **dfdy, int n);
void simpr(float y[], float dydx[], float dfdx[], float **dfdy,
int n, float xs, float htot, int nstep, float yout[],
void (*derivs)(float, float [], float []));
void pzextr(int iest, float xest, float yest[], float yz[], float dy[],
int nv);
int i,iq,k,kk,km;
static int first=1,kmax,kopt,nvold = -1;
static float epsold = -1.0,xnew;
float eps1,errmax,fact,h,red,scale,work,wrkmin,xest;
float *dfdx,**dfdy,*err,*yerr,*ysav,*yseq;
static float a[IMAXX+1];
static float alf[KMAXX+1][KMAXX+1];
static int nseq[IMAXX+1]={0,2,6,10,14,22,34,50,70};
int reduct,exitflag=0;
d=matrix(1,nv,1,KMAXX);
dfdx=vector(1,nv);
dfdy=matrix(1,nv,1,nv);
err=vector(1,KMAXX);
x=vector(1,KMAXX);
yerr=vector(1,nv);
ysav=vector(1,nv);
yseq=vector(1,nv);
if(eps != epsold || nv != nvold) {
*hnext = xnew = -1.0e29;
eps1=SAFE1*eps;
a[1]=nseq[1]+1;
for (k=1;k<=KMAXX;k++) a[k+1]=a[k]+nseq[k+1];
for (iq=2;iq<=KMAXX;iq++) {
for (k=1;k<iq;k++)
alf[k][iq]=pow(eps1,((a[k+1]-a[iq+1])/
((a[iq+1]-a[1]+1.0)*(2*k+1))));
}
epsold=eps;
nvold=nv;
a[1] += nv;
for (k=1;k<=KMAXX;k++) a[k+1]=a[k]+nseq[k+1];
for (kopt=2;kopt<KMAXX;kopt++)
if (a[kopt+1] > a[kopt]*alf[kopt-1][kopt]) break;
kmax=kopt;
}
h=htry;
for (i=1;i<=nv;i++) ysav[i]=y[i];
jacobn(*xx,y,dfdx,dfdy,nv);
if (*xx != xnew || h != (*hnext)) {
first=1;
kopt=kmax;
}
reduct=0;
for (;;) {
for (k=1;k<=kmax;k++) {
xnew=(*xx)+h;
if (xnew == (*xx)) nrerror("step size underflow in stifbs");
simpr(ysav,dydx,dfdx,dfdy,nv,*xx,h,nseq[k],yseq,derivs);
xest=SQR(h/nseq[k]);
pzextr(k,xest,yseq,y,yerr,nv);
if (k != 1) {
errmax=TINY;
for (i=1;i<=nv;i++) errmax=FMAX(errmax,fabs(yerr[i]/yscal[i]));
errmax /= eps;
km=k-1;
err[km]=pow(errmax/SAFE1,1.0/(2*km+1));
}
if (k != 1 && (k >= kopt-1 || first)) {
if (errmax < 1.0) {
exitflag=1;
break;
}
if (k == kmax || k == kopt+1) {
red=SAFE2/err[km];
break;
}
else if (k == kopt && alf[kopt-1][kopt] < err[km]) {
red=1.0/err[km];
break;
}
else if (kopt == kmax && alf[km][kmax-1] < err[km]) {
red=alf[km][kmax-1]*SAFE2/err[km];
break;
}
else if (alf[km][kopt] < err[km]) {
red=alf[km][kopt-1]/err[km];
break;
}
}
}
if (exitflag) break;
red=FMIN(red,REDMIN);
red=FMAX(red,REDMAX);
h *= red;
reduct=1;
}
*xx=xnew;
*hdid=h;
first=0;
wrkmin=1.0e35;
for (kk=1;kk<=km;kk++) {
fact=FMAX(err[kk],SCALMX);
work=fact*a[kk+1];
if (work < wrkmin) {
scale=fact;
wrkmin=work;
kopt=kk+1;
}
}
*hnext=h/scale;
if (kopt >= k && kopt != kmax && !reduct) {
fact=FMAX(scale/alf[kopt-1][kopt],SCALMX);
if (a[kopt+1]*fact <= wrkmin) {
*hnext=h/fact;
kopt++;
}
}
free_vector(yseq,1,nv);
free_vector(ysav,1,nv);
free_vector(yerr,1,nv);
free_vector(x,1,KMAXX);
free_vector(err,1,KMAXX);
free_matrix(dfdy,1,nv,1,nv);
free_vector(dfdx,1,nv);
free_matrix(d,1,nv,1,KMAXX);
}