123 lines
2.8 KiB
C++
123 lines
2.8 KiB
C++
#include <cmath>
|
|
#include "nr.h"
|
|
using namespace std;
|
|
|
|
Vec_DP *x_p;
|
|
Mat_DP *d_p;
|
|
|
|
void NR::stifbs(Vec_IO_DP &y, Vec_IO_DP &dydx, DP &xx, const DP htry,
|
|
const DP eps, Vec_I_DP &yscal, DP &hdid, DP &hnext,
|
|
void derivs(const DP, Vec_I_DP &, Vec_O_DP &))
|
|
{
|
|
const int KMAXX=7,IMAXX=KMAXX+1;
|
|
const DP SAFE1=0.25,SAFE2=0.7,REDMAX=1.0e-5,REDMIN=0.7;
|
|
const DP TINY=1.0e-30,SCALMX=0.1;
|
|
bool exitflag=false;
|
|
int i,iq,k,kk,km,reduct;
|
|
static int first=1,kmax,kopt,nvold = -1;
|
|
DP eps1,errmax,fact,h,red,scale,work,wrkmin,xest;
|
|
static DP epsold = -1.0,xnew;
|
|
static Vec_DP a(IMAXX);
|
|
static Mat_DP alf(KMAXX,KMAXX);
|
|
static int nseq_d[IMAXX]={2,6,10,14,22,34,50,70};
|
|
Vec_INT nseq(nseq_d,IMAXX);
|
|
|
|
int nv=y.size();
|
|
d_p=new Mat_DP(nv,KMAXX);
|
|
x_p=new Vec_DP(KMAXX);
|
|
Vec_DP dfdx(nv),err(KMAXX),yerr(nv),ysav(nv),yseq(nv);
|
|
Mat_DP dfdy(nv,nv);
|
|
if (eps != epsold || nv != nvold) {
|
|
hnext = xnew = -1.0e29;
|
|
eps1=SAFE1*eps;
|
|
a[0]=nseq[0]+1;
|
|
for (k=0;k<KMAXX;k++) a[k+1]=a[k]+nseq[k+1];
|
|
for (iq=1;iq<KMAXX;iq++) {
|
|
for (k=0;k<iq;k++)
|
|
alf[k][iq]=pow(eps1,(a[k+1]-a[iq+1])/
|
|
((a[iq+1]-a[0]+1.0)*(2*k+3)));
|
|
}
|
|
epsold=eps;
|
|
nvold=nv;
|
|
a[0] += nv;
|
|
for (k=0;k<KMAXX;k++) a[k+1]=a[k]+nseq[k+1];
|
|
for (kopt=1;kopt<KMAXX-1;kopt++)
|
|
if (a[kopt+1] > a[kopt]*alf[kopt-1][kopt]) break;
|
|
kmax=kopt;
|
|
}
|
|
h=htry;
|
|
for (i=0;i<nv;i++) ysav[i]=y[i];
|
|
jacobn_s(xx,y,dfdx,dfdy);
|
|
if (xx != xnew || h != hnext) {
|
|
first=1;
|
|
kopt=kmax;
|
|
}
|
|
reduct=0;
|
|
for (;;) {
|
|
for (k=0;k<=kmax;k++) {
|
|
xnew=xx+h;
|
|
if (xnew == xx) nrerror("step size underflow in stifbs");
|
|
simpr(ysav,dydx,dfdx,dfdy,xx,h,nseq[k],yseq,derivs);
|
|
xest=SQR(h/nseq[k]);
|
|
pzextr(k,xest,yseq,y,yerr);
|
|
if (k != 0) {
|
|
errmax=TINY;
|
|
for (i=0;i<nv;i++) errmax=MAX(errmax,fabs(yerr[i]/yscal[i]));
|
|
errmax /= eps;
|
|
km=k-1;
|
|
err[km]=pow(errmax/SAFE1,1.0/(2*km+3));
|
|
}
|
|
if (k != 0 && (k >= kopt-1 || first)) {
|
|
if (errmax < 1.0) {
|
|
exitflag=true;
|
|
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=MIN(red,REDMIN);
|
|
red=MAX(red,REDMAX);
|
|
h *= red;
|
|
reduct=1;
|
|
}
|
|
xx=xnew;
|
|
hdid=h;
|
|
first=0;
|
|
wrkmin=1.0e35;
|
|
for (kk=0;kk<=km;kk++) {
|
|
fact=MAX(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=MAX(scale/alf[kopt-1][kopt],SCALMX);
|
|
if (a[kopt+1]*fact <= wrkmin) {
|
|
hnext=h/fact;
|
|
kopt++;
|
|
}
|
|
}
|
|
delete d_p;
|
|
delete x_p;
|
|
}
|