Select Git revision
Code owners
Assign users and groups as approvers for specific file changes. Learn more.
praxis.c 25.86 KiB
/// @file praxis.c
/// @brief Powell-Brent (Praxis) nonlinear optimization.
/// @note Not yet for production use.
/// @todo Test with practical applications.
/*****************************************************************************/
#include "tpcclibConfig.h"
/*****************************************************************************/
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <time.h>
#include <string.h>
/*****************************************************************************/
#include "tpcextensions.h"
#include "tpcrand.h"
/*****************************************************************************/
#include "tpcnlopt.h"
/*****************************************************************************/
/*****************************************************************************/
/// @cond
/* Local functions */
int praxisMin(
int j, unsigned int nits, double *d2, double *x1, double f1,
unsigned int fk, double *fx, double **v, unsigned int n, double h, double t,
double macheps, double m2, double m4, double small,
double dmin, double ldt, unsigned int *nl,
unsigned int *nf, double qd0, double qd1, double *q0, double *q1,
double *p, double (*_fun)(int, double*, void*), void *_fundata
);
double praxisFlin(
double l, int j, double **v, unsigned int n,
unsigned int *nf, double qd0, double qd1, double *q0, double *q1,
double *p, double (*_fun)(int, double*, void*), void *_fundata
);
int praxisMinfit(
int n, double eps, double tol, double **ab, double *q
);
void praxisSort(double *d, double **v, unsigned int n);
/// @endcond
/*****************************************************************************/
/*****************************************************************************/
/** @brief Powell-Brent (Praxis) non-linear unconstrained optimization.
*
* Praxis is a general purpose routine for the minimization of a function in several variables.
* The algorithm used is a modification of conjugate gradient search method by Powell, with
* changes by Brent, who gave an algol-w program, which served as a basis for this function.
*
* References:
* -# Powell, MJD (1964) An efficient method for finding the minimum of a function in several
* variables without calculating derivatives. Computer Journal 7:155-162.
* -# Brent, RP (1973) Algorithms for minimization without derivatives.
* Prentice Hall, Englewood Cliffs. Dover 2013 republication.
*
* @return enum tpcerror (TPCERROR_OK when successful).
* @author Vesa Oikonen
*/
int nloptPowellBrent(
/** Pointer to NLOPT struct.
* Initial guess must be given in x[].
* Initial step sizes must be given in xdelta[].
* Note that constraints are not applied here.
* Parameter tolerances are used as stopping criteria. */
NLOPT *nlo,
/** Parameter tolerances (xtol[]) are given in the previous struct.
* Praxis stops if the criterion
* 2 * ||x[k]-x[k-1]|| <= sqrt(macheps) * ||x[k]|| + xtol
* is fulfilled more than ktm times for every parameter x.
* You should first try with ktm=1, and usually not higher than 4. */ unsigned int ktm,
/** Pointer to status data; enter NULL if not needed. */
TPCSTATUS *status
) {
int verbose=0; if(status!=NULL) verbose=status->verbose;
if(verbose>0) printf("%s()\n", __func__);
if(nlo==NULL) {
statusSet(status, __func__, __FILE__, __LINE__, TPCERROR_FAIL);
return TPCERROR_FAIL;
}
if(nlo->totalNr<1 || nlo->xfull==NULL || nlo->_fun==NULL) {
statusSet(status, __func__, __FILE__, __LINE__, TPCERROR_NO_DATA);
return TPCERROR_NO_DATA;
}
/* Check deltas */
unsigned int fixedNr=0;
for(unsigned int i=0; i<nlo->totalNr; i++) if(nlo->xdelta[i]<=0.0) fixedNr++;
if(fixedNr==nlo->totalNr) {
statusSet(status, __func__, __FILE__, __LINE__, TPCERROR_INVALID_PARNR);
return TPCERROR_INVALID_PARNR;
}
/* Set a safe machine epsilon and related values */
double macheps=doubleMachEps()*100.0;
if(verbose>1) printf("macheps := %g\n", macheps);
double m2=sqrt(macheps);
double m4=sqrt(m2);
if(verbose>1) {
printf("m2 := %e\n", m2);
printf("m4 := %e\n", m4);
}
double small=macheps*macheps;
double vsmall=small*small;
double large=1.0/small;
double vlarge=1.0/vsmall;
if(verbose>1) {
printf("small := %e\n", small);
printf("vsmall := %e\n", vsmall);
printf("large := %e\n", large);
printf("vlarge := %e\n", vlarge);
}
/* Set common initial step size and scaling parameter */
double h=0.0, scbd=1.0, t=0.0, t2;
{
unsigned int i;
double min=nan(""), max=nan("");
for(i=0; i<nlo->totalNr; i++) if(nlo->xdelta[i]>0.0) {
h+=nlo->xdelta[i];
t+=nlo->xtol[i];
if(isnan(min) || nlo->xdelta[i]<min) min=nlo->xdelta[i];
if(isnan(max) || nlo->xdelta[i]>max) max=nlo->xdelta[i];
}
h/=(double)(nlo->totalNr-fixedNr);
t/=(double)(nlo->totalNr-fixedNr);
if(verbose>2) {printf("xdelta_min := %g\nxdelta_max := %g\n", min, max); fflush(stdout);}
scbd+=log10(max/min); // >1 if param scales are different
t2=small+fabs(t); t=t2;
if(h<100.0*t) h=100.0*t;
}
if(verbose>1) {
printf("step := %g\n", h);
printf("scbd := %g\n", scbd);
printf("t(2) := %g\n", t);
fflush(stdout);
}
if(isnan(scbd)) { // checking that user provided xdeltas statusSet(status, __func__, __FILE__, __LINE__, TPCERROR_INVALID_VALUE);
return TPCERROR_INVALID_VALUE;
}
/* Is the problem ill-conditioned (1) or not?
This variable is automatically set, when Praxis finds the problem to be
ill-conditioned during iterations. */
int illc=0;
/* Initialize */
if(verbose>2) {printf("initializing\n"); fflush(stdout);}
unsigned int i, j, dim=nlo->totalNr;
double ldfac; if(illc) ldfac=0.1; else ldfac=0.01;
unsigned int nl=0, kt=0;
unsigned int nf=0; // nr of objective function calls
double dmin=small;
double ldt=h;
double d[dim], y[dim], z[dim];
// double v[dim][dim];
double **v=malloc(dim*sizeof(double *)); if(v==NULL) {
statusSet(status, __func__, __FILE__, __LINE__, TPCERROR_OUT_OF_MEMORY);
return TPCERROR_OUT_OF_MEMORY;
}
for(i=0; i<dim; i++) {
v[i]=malloc(dim*sizeof(double));
if(v[i]==NULL) {
statusSet(status, __func__, __FILE__, __LINE__, TPCERROR_OUT_OF_MEMORY);
return TPCERROR_OUT_OF_MEMORY;
}
}
/* Make a copy of parameter list */
if(verbose>2) {printf("copying parameters\n"); fflush(stdout);}
double p[dim];
for(i=0; i<dim; i++) p[i]=nlo->xfull[i];
for(i=0; i<dim; i++) {
d[i]=y[i]=z[i]=0.0;
for(j=0; j<dim; j++) v[i][j]=(i==j ? 1.0 : 0.0);
}
double qd0=0.0, q0[dim], q1[dim];
for(i=0; i<dim; i++) q1[i]=q0[i]=nlo->xfull[i];
/* Calculate function value with the initial guess */
if(verbose>2) {printf("evaluating initial guess\n"); fflush(stdout);}
double fx, qf1;
fx=qf1=(*nlo->_fun)(nlo->totalNr, nlo->xfull, nlo->fundata);
nf++;
if(!isfinite(fx)) {
for(i=0; i<dim; i++) free(v[i]);
free(v);
statusSet(status, __func__, __FILE__, __LINE__, TPCERROR_INVALID_VALUE);
return TPCERROR_INVALID_VALUE;
}
if(verbose>1) {printf("initial_objfunc := %g\n", fx); fflush(stdout);}
/* The main loop */
double sf, s, dn, qd1=0.0;
int done=0, nLoop=0, ret=0;
while(!done) {
nLoop++;
if(verbose>2) {
printf("\n-------------------------------\nloop %d, with %d fcalls\n", nLoop, nf);
fflush(stdout);
}
sf=d[0]; s=d[0]=0.0;
/* Minimize along the first direction */ ret=praxisMin(0, 2, &d[0], &s, fx, 0, &fx, v, dim, h, t,
macheps, m2, m4, small, dmin, ldt, &nl,
&nf, qd0, qd1, q0, q1,
p, nlo->_fun, nlo->fundata);
if(ret) {
if(verbose>0) printf("Error %d in praxisMin()\n", ret);
for(i=0; i<dim; i++) free(v[i]);
free(v);
statusSet(status, __func__, __FILE__, __LINE__, TPCERROR_FAIL);
return TPCERROR_FAIL;
}
if((sf<=(0.9*d[0])) || ((0.9*sf)>=d[0])) for(i=1; i<dim; i++) d[i]=0.0;
/* Other directions */
unsigned int k, kl=0, k2;
double sl, df, f1, lds;
for(k=1; k<dim; k++) {
if(verbose>5) printf("direction %d\n", k);
for(i=0; i<dim; i++) y[i]=p[i];
sf=fx; if(kt>0) illc=1;
do { // Usually only once, but twice if noticed here that ill-conditioned
kl=k; df=0.0;
/* If ill-conditioned, take random step to get off resolution valley */
if(illc) {
for(i=0; i<dim; i++) {
z[i]=(0.1*ldt+t2*pow(10.0,(double)kt)) * (drand()-0.5);
s=z[i]; for(j=0; j<dim; j++) p[j]+=s*v[j][i];
}
/* Calculate function value */
fx=(*nlo->_fun)(dim, p, nlo->fundata);
if(!isfinite(fx)) {
statusSet(status, __func__, __FILE__, __LINE__, TPCERROR_INVALID_VALUE);
return TPCERROR_INVALID_VALUE;
}
nf++;
}
/* Minimize along non-conjugate directions */
for(k2=k; k2<dim; k2++) {
if(verbose>6) printf(" non-conjugate direction %d\n", k2);
sl=fx; s=0.0;
ret=praxisMin(k2, 2, &d[k2], &s, fx, 0, &fx, v, dim, h, t,
macheps, m2, m4, small, dmin, ldt, &nl,
&nf, qd0, qd1, q0, q1,
p, nlo->_fun, nlo->fundata);
if(ret) {
if(verbose>0) printf("Error %d in praxisMin()\n", ret);
for(i=0; i<dim; i++) free(v[i]);
free(v);
statusSet(status, __func__, __FILE__, __LINE__, TPCERROR_FAIL);
return TPCERROR_FAIL;
}
if(illc) s=d[k2]*(s+z[k2])*(s+z[k2]); else s=sl-fx;
if(df<s) {df=s; kl=k2;}
}
if(illc) break;
if(df>=fabs(100.0*macheps*fx)) break;
illc=1;
} while(1);
/* Minimize along conjugate directions */
for(k2=0; k2<k; k2++) { // WHY NOT UNTIL k2==k ??????
// for(k2=0; k2<=k; k2++) {
if(verbose>6) printf(" conjugate direction %d\n", k2);
s=0.0;
ret=praxisMin(k2, 2, &d[k2], &s, fx, 0, &fx, v, dim, h, t,
macheps, m2, m4, small, dmin, ldt, &nl,
&nf, qd0, qd1, q0, q1,
p, nlo->_fun, nlo->fundata);
if(ret) { if(verbose>0) printf("Error %d in praxisMin()\n", ret);
for(i=0; i<dim; i++) free(v[i]);
free(v);
statusSet(status, __func__, __FILE__, __LINE__, TPCERROR_FAIL);
return TPCERROR_FAIL;
}
}
f1=fx; fx=sf; lds=0.0;
for(i=0; i<dim; i++) {
sl=p[i]; p[i]=y[i]; y[i]=sl-y[i]; sl=y[i]; lds+=sl*sl;
}
lds=sqrt(lds);
if(lds>small) {
for(i=kl-1; i>=k; i--) {
for(j=0; j<dim; j++) v[j][i+1]=v[j][i];
d[i+1]=d[i];
}
d[k]=0.0; for(i=0; i<dim; i++) v[i][k]=y[i]/lds;
ret=praxisMin(k, 4, &d[k], &lds, f1, 1, &fx, v, dim, h, t,
macheps, m2, m4, small, dmin, ldt, &nl,
&nf, qd0, qd1, q0, q1,
p, nlo->_fun, nlo->fundata);
if(ret) {
if(verbose>0) printf("Error %d in praxisMin()\n", ret);
for(i=0; i<dim; i++) free(v[i]);
free(v);
statusSet(status, __func__, __FILE__, __LINE__, TPCERROR_FAIL);
return TPCERROR_FAIL;
}
if(lds<=0.0) {lds=-lds; for(i=0; i<dim; i++) v[i][k]=-v[i][k];}
}
ldt*=ldfac; if(ldt<lds) ldt=lds;
for(i=0, t2=0.0; i<dim; i++) t2+=p[i]*p[i];
t2=sqrt(t2); t2*=m2; t2+=t;
if(verbose>1) printf(" objfunc := %g\n", fx);
/* Stopping criterion */
if(ldt>(0.5*t2)) kt=0; else kt++;
if(verbose>3 && kt>0) printf(" kt := %d\n", kt);
if(kt>ktm) {done=1; break;}
} // next direction k
if(done) break;
/*
* We are probably in a curved valley, try quadratic extrapolation
*/
/* but only if we have more than one parameter to fit */
if(dim<2) {
if(nf>1000) done=1;
continue;
}
/* Praxis quad:
looks for the minimum along a curve defined by q0, q1, and x (q2). */
{
double qa, qb, qc, qs, ql;
qs=fx; fx=qf1; qf1=qs;
for(i=0, qd1=0.0; i<dim; i++) {
qs=p[i]; ql=q1[i]; p[i]=ql; q1[i]=qs;
qd1+=(qs-ql)*(qs-ql);
}
qd1=sqrt(qd1); ql=qd1; qs=0.0;
if(qd0>0.0 && qd1>0.0 && nl>=3*dim*dim) {
ret=praxisMin(-1, 2, &qs, &ql, qf1, 1, &fx, v, dim, h, t,
macheps, m2, m4, small, dmin, ldt, &nl,
&nf, qd0, qd1, q0, q1,
p, nlo->_fun, nlo->fundata);
if(ret) { if(verbose>0) printf("Error %d in praxisMin()\n", ret);
for(i=0; i<dim; i++) free(v[i]);
free(v);
statusSet(status, __func__, __FILE__, __LINE__, TPCERROR_FAIL);
return TPCERROR_FAIL;
}
qa = ql*(ql-qd1)/(qd0*(qd0+qd1));
qb = (ql+qd0)*(qd1-ql)/(qd0*qd1);
qc = ql*(ql+qd0)/(qd1*(qd0+qd1));
} else {
fx=qf1; qa=qb=0.0; qc=1.0;
}
if(!isfinite(qa)) qa=0.0;
if(!isfinite(qb)) qb=0.0;
if(!isfinite(qc)) qc=1.0;
qd0=qd1;
for(i=0; i<dim; i++) {
qs=q0[i]; q0[i]=p[i];
p[i]=qa*s + qb*p[i] + qc*q1[i];
}
}
dn=0.0;
for(i=0; i<dim; i++) {d[i]=1.0/sqrt(d[i]); if(dn<d[i]) dn=d[i];}
for(j=0; j<dim; j++) {
s=d[j]/dn;
if(isfinite(s)) for(i=0; i<dim; i++) v[i][j]*=s;
}
/* Scale axis to reduce condition number */
if(scbd>1.0) {
s=vlarge;
for(i=0; i<dim; i++) {
sl=0.0;
for(j=0; j<dim; j++) sl+=v[i][j]*v[i][j];
z[i]=sqrt(sl); if(!isfinite(z[i]) || z[i]<m4) z[i]=m4;
if(s>z[i]) s=z[i];
}
for(i=0; i<dim; i++) {
sl=s/z[i]; z[i]=1.0/sl;
if(z[i]>scbd) {sl=1.0/scbd; z[i]=scbd;}
for(j=0; j<dim; j++) v[i][j]*=sl;
}
}
/* Calculate new set of orthogonal directions before repeating
the main loop; first, transpose v[][] for the Praxis minfit */
for(i=1; i<dim; i++) for(j=0; j<i; j++) {
s=v[i][j]; v[i][j]=v[j][i]; v[j][i]=s;
}
/* Call Praxis minfit to find the SVD of v[][].
This gives the principal values and principal directions of approximating
quadratic form without squaring the condition number */
ret=praxisMinfit((int)dim, macheps, vsmall, v, d);
if(ret) {
if(verbose>0) printf("Error %d in praxisMinfit()\n", ret);
for(i=0; i<dim; i++) free(v[i]);
free(v);
statusSet(status, __func__, __FILE__, __LINE__, TPCERROR_FAIL);
return TPCERROR_FAIL;
}
if(scbd>1.0) {
for(i=0; i<dim; i++) {s=z[i]; for(j=0; j<dim; j++) v[i][j]*=s;}
for(i=0; i<dim; i++) {
s=0.0; for(j=0; j<dim; j++) s+=v[j][i]*v[j][i];
s=sqrt(s); d[i]*=s; s=1.0/s; for(j=0; j<dim; j++) v[j][i]*=s;
}
}
for(i=0; i<dim; i++) {
if((dn*d[i]) > large) d[i]=vsmall; // dn was calculated above
else if((dn*d[i]) < small) d[i]=vlarge;
else d[i]=pow(dn*d[i], -2.0);
} /* the new eigenvalues and eigenvectors */
praxisSort(d, v, dim);
dmin=d[dim-1]; if(dmin<small) dmin=small;
illc=(m2*d[0]) > dmin;
if(nf>1000) done=1;
}
if(verbose>1) printf("func_evals := %d\n", nf);
/* Free allocated memory */
for(i=0; i<dim; i++) free(v[i]);
free(v);
/* If ok, then copy parameters to the provided struct */
for(i=0; i<dim; i++) nlo->xfull[i]=p[i];
// Just to prevent warning for now
if(sf>0.0) sf=0.0;
if(kt>0) kt=0;
if(nl>0) nl=0;
if(ldfac>0.0) ldfac=0.0;
if(ktm>0) ktm=0;
return TPCERROR_OK;
}
/*****************************************************************************/
/*****************************************************************************/
/// @cond
/** Praxis min function
* @return 0 if no errors.
* */
int praxisMin(
/** Linear (>=0) or nonlinear (-1) search */
int j,
/** Nr of times an attempt is made to halve the interval */
unsigned int nits,
double *d2,
double *x1,
double f1,
unsigned int fk,
double *fx,
/** v[0..dim-1][0..dim-1] */
double **v,
/** Dimension of the problem, nr of parameters */
unsigned int dim,
double h,
double t,
/** Machine epsilon */
double macheps,
/** sqrt(epsilon) */
double m2,
/** sqrt(sqrt(epsilon)) */
double m4,
/** small */
double small,
/** dmin */
double dmin,
/** ldt */
double ldt,
/** nl */
unsigned int *nl,
/** Number of obj function calls */
unsigned int *nf, /** qd0 */
double qd0,
/** qd1 */
double qd1,
/** q0[] */
double *q0,
/** q1[] */
double *q1,
/** Parameter list */
double *p,
/** Pointer to objective function */
double (*_fun)(int, double*, void*),
/** Pointer to extra data to objective function */
void *_fundata
) {
/* Check input */
if(dim<1 || d2==NULL || x1==NULL || fx==NULL || v==NULL) return(1);
/* Initialize */
double sf1=f1;
double sx1=*x1;
double xm=0.0;
double fm=*fx;
double f0=*fx;
int dz; if((*d2)<macheps) dz=1; else dz=0; // prevent div by (almost) zero
// find step size
unsigned int i;
double s=0.0; for(i=0; i<dim; i++) s+=p[i]*p[i]; s=sqrt(s);
double t2;
if(dz) t2=m4*sqrt(fabs(*fx)/dmin + s*ldt) + m2*ldt;
else t2=m4*sqrt(fabs(*fx)/(*d2) + s*ldt) + m2*ldt;
if(!isfinite(t2)) return(2);
s*=m4; s+=t; if(dz && t2>s) t2=s;
if(t2<small) t2=small;
if(t2>0.01*h) t2=0.01*h;
if(fk && f1<=fm) {xm=*x1; fm=f1;}
if(!fk || fabs(*x1)<t2) {
*x1=(*x1>0.0 ? t2:-t2);
f1 = praxisFlin(*x1, j, v, dim,
nf, qd0, qd1, q0, q1,
p, _fun, _fundata);
if(!isfinite(f1)) return(2);
}
if(f1<=fm) {xm=*x1; fm=f1;}
int done=1;
unsigned int k=0;
double x2, f2, d1;
do {
if(dz) {
x2=(f0<f1 ? -(*x1) : 2.0*(*x1));
f2=praxisFlin(x2, j, v, dim,
nf, qd0, qd1, q0, q1,
p, _fun, _fundata);
if(!isfinite(f2)) return(3);
if(f2<=fm) {xm=x2; fm=f2;}
*d2=(x2*(f1-f0)-(*x1)*(f2-f0))/((*x1)*x2*((*x1)-x2));
}
d1=(f1-f0)/(*x1) - (*x1)*(*d2); dz=1;
if(*d2<=small) x2=(d1<0 ? h:-h); else x2=-0.5*d1/(*d2);
if(fabs(x2)>h) x2=(x2>0 ? h:-h);
f2=praxisFlin(x2, j, v, dim,
nf, qd0, qd1, q0, q1,
p, _fun, _fundata);
if(!isfinite(f2)) return(4);
done=1;
while((k<nits) && (f2>f0)) {
k++;
if((f0<f1) && ((*x1)*x2>0.0)) {done=0; break;}
x2*=0.5; f2=praxisFlin(x2, j, v, dim,
nf, qd0, qd1, q0, q1,
p, _fun, _fundata);
if(!isfinite(f2)) return(5);
}
} while(!done);
*nl=(*nl)+1;
if(f2>fm) x2=xm; else fm=f2;
if(fabs(x2*(x2-(*x1))) > small) {
*d2=(x2*(f1-f0) - (*x1)*(fm-f0))/((*x1)*x2*((*x1)-x2));
} else {
if(k>0) *d2=0.0;
}
if(*d2<small) *d2=small;
*x1=x2; *fx=fm; if(sf1<*fx) {*fx=sf1; *x1=sx1;}
if(j!=-1) for(i=0; i<dim; i++) p[i]+=(*x1)*v[i][j];
return(0);
}
/*****************************************************************************/
/*****************************************************************************/
/** Praxis flin().
* @returns function value, or NaN in case of an error.
*/
double praxisFlin(
double la,
/** Linear (>=0) or nonlinear (-1) search */
int j,
/** v[0..dim-1][0..dim-1] */
double **v,
/** Problem dimension, number of parameters */
unsigned int dim,
/** Number of obj function calls */
unsigned int *nf,
/** qd0 */
double qd0,
/** qd1 */
double qd1,
/** q0[] */
double *q0,
/** q1[] */
double *q1,
/** Parameter list */
double *p,
/** Pointer to objective function */
double (*_fun)(int, double*, void*),
/** Pointer to extra data to objective function */
void *_fundata
) {
unsigned int i;
double tflin[dim];
if(j<-1 || j>=(int)dim) return(nan(""));
if(j>=0) { // linear search
for(i=0; i<dim; i++) tflin[i]=p[i]+la*v[i][j];
} else { // search along parabolic space curve
double qa, qb, qc, f=qd0+qd1;
if(qd0*f!=0.0) qa=la*(la-qd1)/(qd0*f); else qa=0.0;
if(!isfinite(qa)) qa=0.0;
if(f!=0.0) qb=(la+qd0)*(qd1-la)/f; else qb=0.;
if(!isfinite(qb)) qb=0.0;
if(qd1*f!=0.0) qc=la*(la+qd0)/(qd1*f); else qc=0.;
if(!isfinite(qc)) qc=0.0;
for(i=0; i<dim; i++) tflin[i]= qa*q0[i] + qb*p[i] + qc*q1[i];
}
*nf=(*nf)+1;
return(*_fun)(dim, tflin, _fundata);}
/*****************************************************************************/
/*****************************************************************************/
/** Singular value decomposition for Praxis.
* @return 0 if no errors.
*/
int praxisMinfit(
/** Size of data matrix; n can be 1, although then this is not much of use */
int n,
/** Machine epsilon */
double eps,
/** Allowed tolerance */
double tol,
/** Data matrix ab[0..n-1][0..n-1] */
double **ab,
/** Pointer array of dimension n where singular values are returned */
double *q
) {
int l, kt, l2, i, j, k, done, fdone;
double c, f, g, h, s, x, y, z;
double e[n];
/* Check the input */
if(n<1) return(1);
if(eps<=0.0 || ab==NULL || q==NULL) return(2);
if(tol<eps) tol=eps;
if(n==1) {
q[0]=ab[0][0]; ab[0][0]=1.0;
return(0);
}
/* Householder's reduction to bidiagonal form */
for(i=0, x=g=0.0; i<n; i++) {
e[i]=g; s=0.0; l=i+1; for(j=i; j<n; j++) s+=ab[j][i]*ab[j][i];
if(s<tol) {
g=0.0;
} else {
g=sqrt(s); f=ab[i][i]; if(f>=0.0) g=-g;
h=f*g-s; ab[i][i]=f-g;
for(j=l; j<n; j++) {
f=0.0; for(k=i; k<n; k++) f+=ab[k][i]*ab[k][j];
f/=h; for(k=i; k<n; k++) ab[k][j]+=f*ab[k][i];
}
}
q[i]=g; s=0.0;
if(i<n) for(j=l; j<n; j++) s+=ab[i][j]*ab[i][j];
if(s<tol || i>n-2) {
g=0.0;
} else {
f=ab[i][i+1];
g=sqrt(s); if(f>=0.0) g=-g;
h=f*g-s;
ab[i][i+1]=f-g;
for(j=l; j<n; j++) e[j]=ab[i][j]/h;
for(j=l; j<n; j++) {
s=0.0;
for(k=l; k<n; k++) s+=ab[j][k]*ab[i][k];
for(k=l; k<n; k++) ab[j][k]+=s*e[k];
}
}
y=fabs(q[i])+fabs(e[i]);
if(y>x) x=y;
}
/* Accumulation of right-hand transformations */
l=n-1; ab[l][l]=1.0; g=e[l];
for(i=l-1; i>=0; i--) {
if(g!=0.0) {
h=ab[i][i+1]*g; for(j=l; j<n; j++) ab[j][i]=ab[i][j]/h;
for(j=l; j<n; j++) {
for(k=l, s=0.0; k<n; k++) s+=ab[i][k]*ab[k][j];
for(k=l; k<n; k++) ab[k][j]+=s*ab[k][i];
}
}
for(j=l; j<n; j++) ab[i][j]=ab[j][i]=0.0;
ab[i][i]=1.0; g=e[i]; l=i;
}
/* Diagonalization to bi-diagonal form */
eps*=x;
for(k=n-1; k>=0; k--) {
kt=0; done=0;
while(!done) {
if(++kt>30) e[k]=0.0; // QR failed
fdone=0;
for(l2=k; l2>=0; l2--) {
l=l2;
if(fabs(e[l])<=eps) {fdone=1; break;} // F convergence
if(l>0 && fabs(q[l-1])<=eps) break;
}
if(!fdone) for(i=l, c=0.0, s=1.0; i<=k; i++) {
f=s*e[i]; e[i]*=c; if(fabs(f)<=eps) break; // F convergence
g=q[i];
if(fabs(f)<fabs(g)) {
double fg=f/g; h=fabs(g)*sqrt(1.0+fg*fg);
} else {
double gf=g/f; h=(f!=0.0 ? fabs(f)*sqrt(1.0+gf*gf) : 0.0);
}
q[i]=h; if(h==0.0) {h=1.0; g=1.0;}
c=g/h; s=-f/h;
}
/* test convergence */
z=q[k]; if(l==k) {done=1; break;} // convergence
/* Also stop if k==0 because below there is index k-1 */
if(k==0) {done=1; break;}
// Shift from bottom 2x2 minor
x=q[l];
y=q[k-1]; // In many source codes index is k-L but in Brents book k-1
g=e[k-1];
h=e[k];
if(h!=0.0 && y!=0.0) f=((y-z)*(y+z)+(g-h)*(g+h))/(2.0*h*y); else f=0.0;
g=sqrt(f*f+1.0);
if(f<=0.0) f=((x-z)*(x+z)+h*(y/(f-g)-h))/x;
else f=((x-z)*(x+z)+h*(y/(f+g)-h))/x;
// Next QR transformation
for(i=l+1, s=c=1.0; i<=k; i++) {
g=e[i]; y=q[i]; h=s*g; g*=c;
if(fabs(f)<fabs(h)) {
double fh=f/h; z=fabs(h)*sqrt(1.0+fh*fh);
} else {
double hf=h/f; z=(f!=0.0 ? fabs(f)*sqrt(1.0+hf*hf) : 0.0);
}
e[i-1]=z; if(z==0.0) f=z=1.0;
c=f/z; s=h/z; f=x*c+g*s; g=-x*s+g*c; h=y*s; y*=c;
for(j=0; j<n; j++) {
x=ab[j][i-1]; z=ab[j][i]; ab[j][i-1]=x*c+z*s; ab[j][i]=-x*s+z*c; }
if(fabs(f)<fabs(h)) {
double fh=f/h; z=fabs(h)*sqrt(1.0+fh*fh);
} else {
double hf=h/f; z=(f!=0.0 ? fabs(f)*sqrt(1.0+hf*hf) : 0.0);
}
q[i-1]=z; if(z==0.0) z=f=1.0; c=f/z; s=h/z; f=c*g+s*y; x=-s*g+c*y;
}
e[l]=0.0; e[k]=f; q[k]=x;
} // while !done
if(z<0.0) {q[k]=-z; for(j=0; j<n; j++) ab[j][k]=-ab[j][k]; }
}
return(0);}
/*****************************************************************************/
/*****************************************************************************/
/** Praxis sort:
Sorts d and v in ascending order.
*/
void praxisSort(
/** d[0..n-1] */
double *d,
/** v[0..n-1][0..n-1] */
double **v,
/** Matrix size */
unsigned int n
) {
unsigned int k, i, j;
double s;
for(i=0; i<n-1; i++) {
k=i; s=d[i];
for(j=i+1; j<n; j++) {if(d[j]>s) {k=j; s=d[j];}}
if(k>i) {
d[k]=d[i]; d[i]=s;
for(j=0; j<n; j++) {s=v[j][i]; v[j][i]=v[j][k]; v[j][k]=s;}
}
}
}
/*****************************************************************************/
/*****************************************************************************/
/// @endcond