DP NR::amotsa(Mat_IO_DP &p, Vec_O_DP &y, Vec_IO_DP &psum, Vec_O_DP &pb, DP &yb,
DP funk(Vec_I_DP &), const int ihi, DP &yhi, const DP fac)
{
int j;
DP fac1,fac2,yflu,ytry;
int ndim=p.ncols();
Vec_DP ptry(ndim);
fac1=(1.0-fac)/ndim;
fac2=fac1-fac;
for (j=0;j<ndim;j++)
ptry[j]=psum[j]*fac1-p[ihi][j]*fac2;
ytry=funk(ptry);
if (ytry <= yb) {
for (j=0;j<ndim;j++) pb[j]=ptry[j];
yb=ytry;
}
yflu=ytry-tt*log(ran1(idum));
if (yflu < yhi) {
y[ihi]=ytry;
yhi=yflu;
for (j=0;j<ndim;j++) {
psum[j] += ptry[j]-p[ihi][j];
p[ihi][j]=ptry[j];
}
}
return yflu;
}
void NR::gaussj(Mat_IO_DP &a, Mat_IO_DP &b)
{
int i,icol,irow,j,k,l,ll;
DP big,dum,pivinv;
int n=a.nrows();
int m=b.ncols();
Vec_INT indxc(n),indxr(n),ipiv(n);
for (j=0;j<n;j++) ipiv[j]=0;
for (i=0;i<n;i++) {
big=0.0;
for (j=0;j<n;j++)
if (ipiv[j] != 1)
for (k=0;k<n;k++) {
if (ipiv[k] == 0) {
if (fabs(a[j][k]) >= big) {
big=fabs(a[j][k]);
irow=j;
icol=k;
}
}
}
++(ipiv[icol]);
if (irow != icol) {
for (l=0;l<n;l++) SWAP(a[irow][l],a[icol][l]);
for (l=0;l<m;l++) SWAP(b[irow][l],b[icol][l]);
}
indxr[i]=irow;
indxc[i]=icol;
if (a[icol][icol] == 0.0) nrerror("gaussj: Singular Matrix");
pivinv=1.0/a[icol][icol];
a[icol][icol]=1.0;
for (l=0;l<n;l++) a[icol][l] *= pivinv;
for (l=0;l<m;l++) b[icol][l] *= pivinv;
for (ll=0;ll<n;ll++)
if (ll != icol) {
dum=a[ll][icol];
a[ll][icol]=0.0;
for (l=0;l<n;l++) a[ll][l] -= a[icol][l]*dum;
for (l=0;l<m;l++) b[ll][l] -= b[icol][l]*dum;
}
}
for (l=n-1;l>=0;l--) {
if (indxr[l] != indxc[l])
for (k=0;k<n;k++)
SWAP(a[k][indxr[l]],a[k][indxc[l]]);
}
}
void NR::svdcmp(Mat_IO_DP &a, Vec_O_DP &w, Mat_O_DP &v)
{
bool flag;
int i,its,j,jj,k,l,nm;
DP anorm,c,f,g,h,s,scale,x,y,z;
int m=a.nrows();
int n=a.ncols();
Vec_DP rv1(n);
g=scale=anorm=0.0;
for (i=0;i<n;i++) {
l=i+2;
rv1[i]=scale*g;
g=s=scale=0.0;
if (i < m) {
for (k=i;k<m;k++) scale += fabs(a[k][i]);
if (scale != 0.0) {
for (k=i;k<m;k++) {
a[k][i] /= scale;
s += a[k][i]*a[k][i];
}
f=a[i][i];
g = -SIGN(sqrt(s),f);
h=f*g-s;
a[i][i]=f-g;
for (j=l-1;j<n;j++) {
for (s=0.0,k=i;k<m;k++) s += a[k][i]*a[k][j];
f=s/h;
for (k=i;k<m;k++) a[k][j] += f*a[k][i];
}
for (k=i;k<m;k++) a[k][i] *= scale;
}
}
w[i]=scale *g;
g=s=scale=0.0;
if (i+1 <= m && i+1 != n) {
for (k=l-1;k<n;k++) scale += fabs(a[i][k]);
if (scale != 0.0) {
for (k=l-1;k<n;k++) {
a[i][k] /= scale;
s += a[i][k]*a[i][k];
}
f=a[i][l-1];
g = -SIGN(sqrt(s),f);
h=f*g-s;
a[i][l-1]=f-g;
for (k=l-1;k<n;k++) rv1[k]=a[i][k]/h;
for (j=l-1;j<m;j++) {
for (s=0.0,k=l-1;k<n;k++) s += a[j][k]*a[i][k];
for (k=l-1;k<n;k++) a[j][k] += s*rv1[k];
}
for (k=l-1;k<n;k++) a[i][k] *= scale;
}
}
anorm=MAX(anorm,(fabs(w[i])+fabs(rv1[i])));
}
for (i=n-1;i>=0;i--) {
if (i < n-1) {
if (g != 0.0) {
for (j=l;j<n;j++)
v[j][i]=(a[i][j]/a[i][l])/g;
for (j=l;j<n;j++) {
for (s=0.0,k=l;k<n;k++) s += a[i][k]*v[k][j];
for (k=l;k<n;k++) v[k][j] += s*v[k][i];
}
}
for (j=l;j<n;j++) v[i][j]=v[j][i]=0.0;
}
v[i][i]=1.0;
g=rv1[i];
l=i;
}
for (i=MIN(m,n)-1;i>=0;i--) {
l=i+1;
g=w[i];
for (j=l;j<n;j++) a[i][j]=0.0;
if (g != 0.0) {
g=1.0/g;
for (j=l;j<n;j++) {
for (s=0.0,k=l;k<m;k++) s += a[k][i]*a[k][j];
f=(s/a[i][i])*g;
for (k=i;k<m;k++) a[k][j] += f*a[k][i];
}
for (j=i;j<m;j++) a[j][i] *= g;
} else for (j=i;j<m;j++) a[j][i]=0.0;
++a[i][i];
}
for (k=n-1;k>=0;k--) {
for (its=0;its<30;its++) {
flag=true;
for (l=k;l>=0;l--) {
nm=l-1;
if (fabs(rv1[l])+anorm == anorm) {
flag=false;
break;
}
if (fabs(w[nm])+anorm == anorm) break;
}
if (flag) {
c=0.0;
s=1.0;
for (i=l;i<k+1;i++) {
f=s*rv1[i];
rv1[i]=c*rv1[i];
if (fabs(f)+anorm == anorm) break;
g=w[i];
h=pythag(f,g);
w[i]=h;
h=1.0/h;
c=g*h;
s = -f*h;
for (j=0;j<m;j++) {
y=a[j][nm];
z=a[j][i];
a[j][nm]=y*c+z*s;
a[j][i]=z*c-y*s;
}
}
}
z=w[k];
if (l == k) {
if (z < 0.0) {
w[k] = -z;
for (j=0;j<n;j++) v[j][k] = -v[j][k];
}
break;
}
if (its == 29) { nrerror("no convergence in 30 svdcmp iterations"); return; }
x=w[l];
nm=k-1;
y=w[nm];
g=rv1[nm];
h=rv1[k];
f=((y-z)*(y+z)+(g-h)*(g+h))/(2.0*h*y);
g=pythag(f,1.0);
f=((x-z)*(x+z)+h*((y/(f+SIGN(g,f)))-h))/x;
c=s=1.0;
for (j=l;j<=nm;j++) {
i=j+1;
g=rv1[i];
y=w[i];
h=s*g;
g=c*g;
z=pythag(f,h);
rv1[j]=z;
c=f/z;
s=h/z;
f=x*c+g*s;
g=g*c-x*s;
h=y*s;
y *= c;
for (jj=0;jj<n;jj++) {
x=v[jj][j];
z=v[jj][i];
v[jj][j]=x*c+z*s;
v[jj][i]=z*c-x*s;
}
z=pythag(f,h);
w[j]=z;
if (z) {
z=1.0/z;
c=f*z;
s=h*z;
}
f=c*g+s*y;
x=c*y-s*g;
for (jj=0;jj<m;jj++) {
y=a[jj][j];
z=a[jj][i];
a[jj][j]=y*c+z*s;
a[jj][i]=z*c-y*s;
}
}
rv1[l]=0.0;
rv1[k]=f;
w[k]=x;
}
}
}
void NR::amebsa(Mat_IO_DP &p, Vec_IO_DP &y, Vec_O_DP &pb, DP &yb, const DP ftol,
DP funk(Vec_I_DP &), int &iter, const DP temptr)
{
int i,ihi,ilo,j,n;
DP rtol,yhi,ylo,ynhi,ysave,yt,ytry;
int mpts=p.nrows();
int ndim=p.ncols();
Vec_DP psum(ndim);
tt = -temptr;
get_psum(p,psum);
for (;;) {
ilo=0;
ihi=1;
ynhi=ylo=y[0]+tt*log(ran1(idum));
yhi=y[1]+tt*log(ran1(idum));
if (ylo > yhi) {
ihi=0;
ilo=1;
ynhi=yhi;
yhi=ylo;
ylo=ynhi;
}
for (i=3;i<=mpts;i++) {
yt=y[i-1]+tt*log(ran1(idum));
if (yt <= ylo) {
ilo=i-1;
ylo=yt;
}
if (yt > yhi) {
ynhi=yhi;
ihi=i-1;
yhi=yt;
} else if (yt > ynhi) {
ynhi=yt;
}
}
rtol=2.0*fabs(yhi-ylo)/(fabs(yhi)+fabs(ylo));
if (rtol < ftol || iter < 0) {
SWAP(y[0],y[ilo]);
for (n=0;n<ndim;n++)
SWAP(p[0][n],p[ilo][n]);
break;
}
iter -= 2;
ytry=amotsa(p,y,psum,pb,yb,funk,ihi,yhi,-1.0);
if (ytry <= ylo) {
ytry=amotsa(p,y,psum,pb,yb,funk,ihi,yhi,2.0);
} else if (ytry >= ynhi) {
ysave=yhi;
ytry=amotsa(p,y,psum,pb,yb,funk,ihi,yhi,0.5);
if (ytry >= ysave) {
for (i=0;i<mpts;i++) {
if (i != ilo) {
for (j=0;j<ndim;j++) {
psum[j]=0.5*(p[i][j]+p[ilo][j]);
p[i][j]=psum[j];
}
y[i]=funk(psum);
}
}
iter -= ndim;
get_psum(p,psum);
}
} else ++iter;
}
}
