void NR::rkqs(Vec_IO_DP &y, Vec_IO_DP &dydx, DP &x, 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 DP SAFETY=0.9, PGROW=-0.2, PSHRNK=-0.25, ERRCON=1.89e-4;
int i;
DP errmax,h,htemp,xnew;
int n=y.size();
h=htry;
Vec_DP yerr(n),ytemp(n);
for (;;) {
rkck(y,dydx,x,h,ytemp,yerr,derivs);
errmax=0.0;
for (i=0;i<n;i++) errmax=MAX(errmax,fabs(yerr[i]/yscal[i]));
errmax /= eps;
if (errmax <= 1.0) break;
htemp=SAFETY*h*pow(errmax,PSHRNK);
h=(h >= 0.0 ? MAX(htemp,0.1*h) : MIN(htemp,0.1*h));
xnew=x+h;
if (xnew == x) nrerror("stepsize underflow in rkqs");
}
if (errmax > ERRCON) hnext=SAFETY*h*pow(errmax,PGROW);
else hnext=5.0*h;
x += (hdid=h);
for (i=0;i<n;i++) y[i]=ytemp[i];
}
void NR::eulsum(DP &sum, const DP term, const int jterm, Vec_IO_DP &wksp)
{
int j;
static int nterm;
DP tmp,dum;
if (jterm == 0) {
nterm=1;
sum=0.5*(wksp[0]=term);
} else {
if (nterm+1 > wksp.size()) nrerror("wksp too small in euler");
tmp=wksp[0];
wksp[0]=term;
for (j=1;j<nterm;j++) {
dum=wksp[j];
wksp[j]=0.5*(wksp[j-1]+tmp);
tmp=dum;
}
wksp[nterm]=0.5*(wksp[nterm-1]+tmp);
if (fabs(wksp[nterm]) <= fabs(wksp[nterm-1]))
sum += (0.5*wksp[nterm++]);
else
sum += wksp[nterm];
}
}
void NR::cosft2(Vec_IO_DP &y, const int isign)
{
const DP PI=3.141592653589793238;
int i;
DP sum,sum1,y1,y2,ytemp,theta,wi=0.0,wi1,wpi,wpr,wr=1.0,wr1,wtemp;
int n=y.size();
theta=0.5*PI/n;
wr1=cos(theta);
wi1=sin(theta);
wpr = -2.0*wi1*wi1;
wpi=sin(2.0*theta);
if (isign == 1) {
for (i=0;i<n/2;i++) {
y1=0.5*(y[i]+y[n-1-i]);
y2=wi1*(y[i]-y[n-1-i]);
y[i]=y1+y2;
y[n-1-i]=y1-y2;
wr1=(wtemp=wr1)*wpr-wi1*wpi+wr1;
wi1=wi1*wpr+wtemp*wpi+wi1;
}
realft(y,1);
for (i=2;i<n;i+=2) {
wr=(wtemp=wr)*wpr-wi*wpi+wr;
wi=wi*wpr+wtemp*wpi+wi;
y1=y[i]*wr-y[i+1]*wi;
y2=y[i+1]*wr+y[i]*wi;
y[i]=y1;
y[i+1]=y2;
}
sum=0.5*y[1];
for (i=n-1;i>0;i-=2) {
sum1=sum;
sum += y[i];
y[i]=sum1;
}
} else if (isign == -1) {
ytemp=y[n-1];
for (i=n-1;i>2;i-=2)
y[i]=y[i-2]-y[i];
y[1]=2.0*ytemp;
for (i=2;i<n;i+=2) {
wr=(wtemp=wr)*wpr-wi*wpi+wr;
wi=wi*wpr+wtemp*wpi+wi;
y1=y[i]*wr+y[i+1]*wi;
y2=y[i+1]*wr-y[i]*wi;
y[i]=y1;
y[i+1]=y2;
}
realft(y,-1);
for (i=0;i<n/2;i++) {
y1=y[i]+y[n-1-i];
y2=(0.5/wi1)*(y[i]-y[n-1-i]);
y[i]=0.5*(y1+y2);
y[n-1-i]=0.5*(y1-y2);
wr1=(wtemp=wr1)*wpr-wi1*wpi+wr1;
wi1=wi1*wpr+wtemp*wpi+wi1;
}
}
}
void NR::fixrts(Vec_IO_DP &d)
{
bool polish=true;
int i,j;
int m=d.size();
Vec_CPLX_DP a(m+1),roots(m);
a[m]=1.0;
for (j=0;j<m;j++)
a[j]= -d[m-1-j];
zroots(a,roots,polish);
for (j=0;j<m;j++)
if (abs(roots[j]) > 1.0)
roots[j]=1.0/conj(roots[j]);
a[0]= -roots[0];
a[1]=1.0;
for (j=1;j<m;j++) {
a[j+1]=1.0;
for (i=j;i>=1;i--)
a[i]=a[i-1]-roots[j]*a[i];
a[0]= -roots[j]*a[0];
}
for (j=0;j<m;j++)
d[m-1-j] = -real(a[j]);
}
void NR::kstwo(Vec_IO_DP &data1, Vec_IO_DP &data2, DP &d, DP &prob)
{
int j1=0,j2=0;
DP d1,d2,dt,en1,en2,en,fn1=0.0,fn2=0.0;
int n1=data1.size();
int n2=data2.size();
sort(data1);
sort(data2);
en1=n1;
en2=n2;
d=0.0;
while (j1 < n1 && j2 < n2) {
if ((d1=data1[j1]) <= (d2=data2[j2])) fn1=j1++/en1;
if (d2 <= d1) fn2=j2++/en2;
if ((dt=fabs(fn2-fn1)) > d) d=dt;
}
en=sqrt(en1*en2/(en1+en2));
prob=probks((en+0.12+0.11/en)*d);
}
void NR::wt1(Vec_IO_DP &a, const int isign,
void wtstep(Vec_IO_DP &, const int, const int))
{
int nn;
int n=a.size();
if (n < 4) return;
if (isign >= 0) {
for (nn=n;nn>=4;nn>>=1) wtstep(a,nn,isign);
} else {
for (nn=4;nn<=n;nn<<=1) wtstep(a,nn,isign);
void NR::sort3(Vec_IO_DP &ra, Vec_IO_DP &rb, Vec_IO_DP &rc)
{
int j;
int n=ra.size();
Vec_INT iwksp(n);
Vec_DP wksp(n);
indexx(ra,iwksp);
for (j=0;j<n;j++) wksp[j]=ra[j];
for (j=0;j<n;j++) ra[j]=wksp[iwksp[j]];
for (j=0;j<n;j++) wksp[j]=rb[j];
for (j=0;j<n;j++) rb[j]=wksp[iwksp[j]];
for (j=0;j<n;j++) wksp[j]=rc[j];
for (j=0;j<n;j++) rc[j]=wksp[iwksp[j]];
}
void NR::piksrt(Vec_IO_DP &arr)
{
int i,j;
DP a;
int n=arr.size();
for (j=1;j<n;j++) {
a=arr[j];
i=j;
while (i > 0 && arr[i-1] > a) {
arr[i]=arr[i-1];
i--;
}
arr[i]=a;
}
}
void NR::pcshft(const DP a, const DP b, Vec_IO_DP &d)
{
int k,j;
DP fac,cnst;
int n=d.size();
cnst=2.0/(b-a);
fac=cnst;
for (j=1;j<n;j++) {
d[j] *= fac;
fac *= cnst;
}
cnst=0.5*(a+b);
for (j=0;j<=n-2;j++)
for (k=n-2;k>=j;k--)
d[k] -= cnst*d[k+1];
}
void NR::four1(Vec_IO_DP &data, const int isign)
{
int n,mmax,m,j,istep,i;
DP wtemp,wr,wpr,wpi,wi,theta,tempr,tempi;
int nn=data.size()/2;
n=nn << 1;
j=1;
for (i=1;i<n;i+=2) {
if (j > i) {
SWAP(data[j-1],data[i-1]);
SWAP(data[j],data[i]);
}
m=nn;
while (m >= 2 && j > m) {
j -= m;
m >>= 1;
}
j += m;
}
mmax=2;
while (n > mmax) {
istep=mmax << 1;
theta=isign*(6.28318530717959/mmax);
wtemp=sin(0.5*theta);
wpr = -2.0*wtemp*wtemp;
wpi=sin(theta);
wr=1.0;
wi=0.0;
for (m=1;m<mmax;m+=2) {
for (i=m;i<=n;i+=istep) {
j=i+mmax;
tempr=wr*data[j-1]-wi*data[j];
tempi=wr*data[j]+wi*data[j-1];
data[j-1]=data[i-1]-tempr;
data[j]=data[i]-tempi;
data[i-1] += tempr;
data[i] += tempi;
}
wr=(wtemp=wr)*wpr-wi*wpi+wr;
wi=wi*wpr+wtemp*wpi+wi;
}
mmax=istep;
}
}
void NR::ksone(Vec_IO_DP &data, DP func(const DP), DP &d, DP &prob)
{
int j;
DP dt,en,ff,fn,fo=0.0;
int n=data.size();
sort(data);
en=n;
d=0.0;
for (j=0;j<n;j++) {
fn=(j+1)/en;
ff=func(data[j]);
dt=MAX(fabs(fo-ff),fabs(fn-ff));
if (dt > d) d=dt;
fo=fn;
}
en=sqrt(en);
prob=probks((en+0.12+0.11/en)*d);
}
void NR::frprmn(Vec_IO_DP &p, const DP ftol, int &iter, DP &fret,
DP func(Vec_I_DP &), void dfunc(Vec_I_DP &, Vec_O_DP &))
{
const int ITMAX=200;
const DP EPS=1.0e-18;
int j,its;
DP gg,gam,fp,dgg;
int n=p.size();
Vec_DP g(n),h(n),xi(n);
fp=func(p);
dfunc(p,xi);
for (j=0;j<n;j++) {
g[j] = -xi[j];
xi[j]=h[j]=g[j];
}
for (its=0;its<ITMAX;its++) {
iter=its;
linmin(p,xi,fret,func);
if (2.0*fabs(fret-fp) <= ftol*(fabs(fret)+fabs(fp)+EPS))
return;
fp=fret;
dfunc(p,xi);
dgg=gg=0.0;
for (j=0;j<n;j++) {
gg += g[j]*g[j];
// dgg += xi[j]*xi[j];
dgg += (xi[j]+g[j])*xi[j];
}
if (gg == 0.0)
return;
gam=dgg/gg;
for (j=0;j<n;j++) {
g[j] = -xi[j];
xi[j]=h[j]=g[j]+gam*h[j];
}
}
nrerror("Too many iterations in frprmn");
}
void NR::pade(Vec_IO_DP &cof, DP &resid)
{
const DP BIG=1.0e30;
int j,k;
DP d,rr,rrold,sum;
int n=(cof.size()-1)/2;
Mat_DP q(n,n),qlu(n,n);
Vec_INT indx(n);
Vec_DP x(n),y(n),z(n);
for (j=0;j<n;j++) {
y[j]=x[j]=cof[n+j+1];
for (k=0;k<n;k++) {
q[j][k]=cof[j-k+n];
qlu[j][k]=q[j][k];
}
}
ludcmp(qlu,indx,d);
lubksb(qlu,indx,x);
rr=BIG;
do {
rrold=rr;
for (j=0;j<n;j++) z[j]=x[j];
mprove(q,qlu,indx,y,x);
for (rr=0.0,j=0;j<n;j++)
rr += SQR(z[j]-x[j]);
} while (rr < rrold);
resid=sqrt(rrold);
for (k=0;k<n;k++) {
for (sum=cof[k+1],j=0;j<=k;j++)
sum -= z[j]*cof[k-j];
y[k]=sum;
}
for (j=0;j<n;j++) {
cof[j+1]=y[j];
cof[j+1+n] = -z[j];
}
}
DP NR::select(const int k, Vec_IO_DP &arr)
{
int i,ir,j,l,mid;
DP a;
int n=arr.size();
l=0;
ir=n-1;
for (;;) {
if (ir <= l+1) {
if (ir == l+1 && arr[ir] < arr[l])
SWAP(arr[l],arr[ir]);
return arr[k];
} else {
mid=(l+ir) >> 1;
SWAP(arr[mid],arr[l+1]);
if (arr[l] > arr[ir])
SWAP(arr[l],arr[ir]);
if (arr[l+1] > arr[ir])
SWAP(arr[l+1],arr[ir]);
if (arr[l] > arr[l+1])
SWAP(arr[l],arr[l+1]);
i=l+1;
j=ir;
a=arr[l+1];
for (;;) {
do i++; while (arr[i] < a);
do j--; while (arr[j] > a);
if (j < i) break;
SWAP(arr[i],arr[j]);
}
arr[l+1]=arr[j];
arr[j]=a;
if (j >= k) ir=j-1;
if (j <= k) l=i;
}
}
}
void NR::crank(Vec_IO_DP &w, DP &s)
{
int j=1,ji,jt;
DP t,rank;
int n=w.size();
s=0.0;
while (j < n) {
if (w[j] != w[j-1]) {
w[j-1]=j;
++j;
} else {
for (jt=j+1;jt<=n && w[jt-1]==w[j-1];jt++);
rank=0.5*(j+jt-1);
for (ji=j;ji<=(jt-1);ji++)
w[ji-1]=rank;
t=jt-j;
s += (t*t*t-t);
j=jt;
}
}
if (j == n) w[n-1]=n;
}
void NR::dfpmin(Vec_IO_DP &p, const DP gtol, int &iter, DP &fret,
DP func(Vec_I_DP &), void dfunc(Vec_I_DP &, Vec_O_DP &))
{
const int ITMAX=200;
const DP EPS=numeric_limits<DP>::epsilon();
const DP TOLX=4*EPS,STPMX=100.0;
bool check;
int i,its,j;
DP den,fac,fad,fae,fp,stpmax,sum=0.0,sumdg,sumxi,temp,test;
int n=p.size();
Vec_DP dg(n),g(n),hdg(n),pnew(n),xi(n);
Mat_DP hessin(n,n);
fp=func(p);
dfunc(p,g);
for (i=0; i<n; i++) {
for (j=0; j<n; j++) hessin[i][j]=0.0;
hessin[i][i]=1.0;
xi[i] = -g[i];
sum += p[i]*p[i];
}
stpmax=STPMX*MAX(sqrt(sum),DP(n));
for (its=0; its<ITMAX; its++) {
iter=its;
lnsrch(p,fp,g,xi,pnew,fret,stpmax,check,func);
fp=fret;
for (i=0; i<n; i++) {
xi[i]=pnew[i]-p[i];
p[i]=pnew[i];
}
test=0.0;
for (i=0; i<n; i++) {
temp=fabs(xi[i])/MAX(fabs(p[i]),1.0);
if (temp > test) test=temp;
}
if (test < TOLX)
return;
for (i=0; i<n; i++) dg[i]=g[i];
dfunc(p,g);
test=0.0;
den=MAX(fret,1.0);
for (i=0; i<n; i++) {
temp=fabs(g[i])*MAX(fabs(p[i]),1.0)/den;
if (temp > test) test=temp;
}
if (test < gtol)
return;
for (i=0; i<n; i++) dg[i]=g[i]-dg[i];
for (i=0; i<n; i++) {
hdg[i]=0.0;
for (j=0; j<n; j++) hdg[i] += hessin[i][j]*dg[j];
}
fac=fae=sumdg=sumxi=0.0;
for (i=0; i<n; i++) {
fac += dg[i]*xi[i];
fae += dg[i]*hdg[i];
sumdg += SQR(dg[i]);
sumxi += SQR(xi[i]);
}
if (fac > sqrt(EPS*sumdg*sumxi)) {
fac=1.0/fac;
fad=1.0/fae;
for (i=0; i<n; i++) dg[i]=fac*xi[i]-fad*hdg[i];
for (i=0; i<n; i++) {
for (j=i; j<n; j++) {
hessin[i][j] += fac*xi[i]*xi[j]
-fad*hdg[i]*hdg[j]+fae*dg[i]*dg[j];
hessin[j][i]=hessin[i][j];
}
}
}
for (i=0; i<n; i++) {
xi[i]=0.0;
for (j=0; j<n; j++) xi[i] -= hessin[i][j]*g[j];
}
}
nrerror("too many iterations in dfpmin");
}
void NR::stiff(Vec_IO_DP &y, Vec_IO_DP &dydx, DP &x, 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 DP SAFETY=0.9,GROW=1.5,PGROW= -0.25,SHRNK=0.5;
const DP PSHRNK=(-1.0/3.0),ERRCON=0.1296;
const int MAXTRY=40;
const DP GAM=1.0/2.0,A21=2.0,A31=48.0/25.0,A32=6.0/25.0,C21= -8.0,
C31=372.0/25.0,C32=12.0/5.0,C41=(-112.0/125.0),
C42=(-54.0/125.0),C43=(-2.0/5.0),B1=19.0/9.0,B2=1.0/2.0,
B3=25.0/108.0,B4=125.0/108.0,E1=17.0/54.0,E2=7.0/36.0,E3=0.0,
E4=125.0/108.0,C1X=1.0/2.0,C2X=(-3.0/2.0),C3X=(121.0/50.0),
C4X=(29.0/250.0),A2X=1.0,A3X=3.0/5.0;
int i,j,jtry;
DP d,errmax,h,xsav;
int n=y.size();
Mat_DP a(n,n),dfdy(n,n);
Vec_INT indx(n);
Vec_DP dfdx(n),dysav(n),err(n),ysav(n),g1(n),g2(n),g3(n),g4(n);
xsav=x;
for (i=0;i<n;i++) {
ysav[i]=y[i];
dysav[i]=dydx[i];
}
jacobn_s(xsav,ysav,dfdx,dfdy);
h=htry;
for (jtry=0;jtry<MAXTRY;jtry++) {
for (i=0;i<n;i++) {
for (j=0;j<n;j++) a[i][j] = -dfdy[i][j];
a[i][i] += 1.0/(GAM*h);
}
ludcmp(a,indx,d);
for (i=0;i<n;i++)
g1[i]=dysav[i]+h*C1X*dfdx[i];
lubksb(a,indx,g1);
for (i=0;i<n;i++)
y[i]=ysav[i]+A21*g1[i];
x=xsav+A2X*h;
derivs(x,y,dydx);
for (i=0;i<n;i++)
g2[i]=dydx[i]+h*C2X*dfdx[i]+C21*g1[i]/h;
lubksb(a,indx,g2);
for (i=0;i<n;i++)
y[i]=ysav[i]+A31*g1[i]+A32*g2[i];
x=xsav+A3X*h;
derivs(x,y,dydx);
for (i=0;i<n;i++)
g3[i]=dydx[i]+h*C3X*dfdx[i]+(C31*g1[i]+C32*g2[i])/h;
lubksb(a,indx,g3);
for (i=0;i<n;i++)
g4[i]=dydx[i]+h*C4X*dfdx[i]+(C41*g1[i]+C42*g2[i]+C43*g3[i])/h;
lubksb(a,indx,g4);
for (i=0;i<n;i++) {
y[i]=ysav[i]+B1*g1[i]+B2*g2[i]+B3*g3[i]+B4*g4[i];
err[i]=E1*g1[i]+E2*g2[i]+E3*g3[i]+E4*g4[i];
}
x=xsav+h;
if (x == xsav) nrerror("stepsize not significant in stiff");
errmax=0.0;
for (i=0;i<n;i++) errmax=MAX(errmax,fabs(err[i]/yscal[i]));
errmax /= eps;
if (errmax <= 1.0) {
hdid=h;
hnext=(errmax > ERRCON ? SAFETY*h*pow(errmax,PGROW) : GROW*h);
return;
} else {
hnext=SAFETY*h*pow(errmax,PSHRNK);
h=(h >= 0.0 ? MAX(hnext,SHRNK*h) : MIN(hnext,SHRNK*h));
}
}
nrerror("exceeded MAXTRY in stiff");
}
void NR::fourn(Vec_IO_DP &data, Vec_I_INT &nn, const int isign)
{
int idim,i1,i2,i3,i2rev,i3rev,ip1,ip2,ip3,ifp1,ifp2;
int ibit,k1,k2,n,nprev,nrem,ntot;
DP tempi,tempr,theta,wi,wpi,wpr,wr,wtemp;
int ndim=nn.size();
ntot=data.size()/2;
nprev=1;
for (idim=ndim-1;idim>=0;idim--) {
n=nn[idim];
nrem=ntot/(n*nprev);
ip1=nprev << 1;
ip2=ip1*n;
ip3=ip2*nrem;
i2rev=0;
for (i2=0;i2<ip2;i2+=ip1) {
if (i2 < i2rev) {
for (i1=i2;i1<i2+ip1-1;i1+=2) {
for (i3=i1;i3<ip3;i3+=ip2) {
i3rev=i2rev+i3-i2;
SWAP(data[i3],data[i3rev]);
SWAP(data[i3+1],data[i3rev+1]);
}
}
}
ibit=ip2 >> 1;
while (ibit >= ip1 && i2rev+1 > ibit) {
i2rev -= ibit;
ibit >>= 1;
}
i2rev += ibit;
}
ifp1=ip1;
while (ifp1 < ip2) {
ifp2=ifp1 << 1;
theta=isign*6.28318530717959/(ifp2/ip1);
wtemp=sin(0.5*theta);
wpr= -2.0*wtemp*wtemp;
wpi=sin(theta);
wr=1.0;
wi=0.0;
for (i3=0;i3<ifp1;i3+=ip1) {
for (i1=i3;i1<i3+ip1-1;i1+=2) {
for (i2=i1;i2<ip3;i2+=ifp2) {
k1=i2;
k2=k1+ifp1;
tempr=wr*data[k2]-wi*data[k2+1];
tempi=wr*data[k2+1]+wi*data[k2];
data[k2]=data[k1]-tempr;
data[k2+1]=data[k1+1]-tempi;
data[k1] += tempr;
data[k1+1] += tempi;
}
}
wr=(wtemp=wr)*wpr-wi*wpi+wr;
wi=wi*wpr+wtemp*wpi+wi;
}
ifp1=ifp2;
}
nprev *= n;
}
}
void NR::sort2(Vec_IO_DP &arr, Vec_IO_DP &brr)
{
const int M=7,NSTACK=50;
int i,ir,j,k,jstack=-1,l=0;
DP a,b;
Vec_INT istack(NSTACK);
int n=arr.size();
ir=n-1;
for (;;) {
if (ir-l < M) {
for (j=l+1;j<=ir;j++) {
a=arr[j];
b=brr[j];
for (i=j-1;i>=l;i--) {
if (arr[i] <= a) break;
arr[i+1]=arr[i];
brr[i+1]=brr[i];
}
arr[i+1]=a;
brr[i+1]=b;
}
if (jstack < 0) break;
ir=istack[jstack--];
l=istack[jstack--];
} else {
k=(l+ir) >> 1;
SWAP(arr[k],arr[l+1]);
SWAP(brr[k],brr[l+1]);
if (arr[l] > arr[ir]) {
SWAP(arr[l],arr[ir]);
SWAP(brr[l],brr[ir]);
}
if (arr[l+1] > arr[ir]) {
SWAP(arr[l+1],arr[ir]);
SWAP(brr[l+1],brr[ir]);
}
if (arr[l] > arr[l+1]) {
SWAP(arr[l],arr[l+1]);
SWAP(brr[l],brr[l+1]);
}
i=l+1;
j=ir;
a=arr[l+1];
b=brr[l+1];
for (;;) {
do i++; while (arr[i] < a);
do j--; while (arr[j] > a);
if (j < i) break;
SWAP(arr[i],arr[j]);
SWAP(brr[i],brr[j]);
}
arr[l+1]=arr[j];
arr[j]=a;
brr[l+1]=brr[j];
brr[j]=b;
jstack += 2;
if (jstack >= NSTACK) nrerror("NSTACK too small in sort2.");
if (ir-i+1 >= j-l) {
istack[jstack]=ir;
istack[jstack-1]=i;
ir=j-1;
} else {
istack[jstack]=j-1;
istack[jstack-1]=l;
l=i;
}
}
}
}
