void Ti_Optimization::hshreabid(double **a, int m, int n, double d[], double b[],
double em[])
{
int i,j,i1;
double norm,machtol,w,s,f,g,h;
norm=0.0;
for (i=1; i<=m; i++)
{
w=0.0;
for (j=1; j<=n; j++)
w += fabs(a[i][j]);
if (w > norm)
norm=w;
}
machtol=em[0]*norm;
em[1]=norm;
for (i=1; i<=n; i++)
{
i1=i+1;
s=tammat(i1,m,i,i,a,a);
if (s < machtol)
d[i]=a[i][i];
else
{
f=a[i][i];
s += f*f;
d[i] = g = (f < 0.0) ? sqrt(s) : -sqrt(s);
h=f*g-s;
a[i][i]=f-g;
for (j=i1; j<=n; j++)
elmcol(i,m,j,i,a,a,tammat(i,m,i,j,a,a)/h);
}
if (i < n)
{
s=mattam(i1+1,n,i,i,a,a);
if (s < machtol)
b[i]=a[i][i1];
else
{
f=a[i][i1];
s += f*f;
b[i] = g = (f < 0.0) ? sqrt(s) : -sqrt(s);
h=f*g-s;
a[i][i1]=f-g;
for (j=i1; j<=m; j++)
elmrow(i1,n,j,i,a,a,mattam(i1,n,i,j,a,a)/h);
}
}
}
}
void orthog(int n, int lc, int uc, real_t **x)
{
int *allocate_integer_vector(int, int);
void free_integer_vector(int *, int);
real_t tammat(int, int, int, int, real_t **, real_t **);
void elmcol(int, int, int, int, real_t **, real_t **, real_t);
int i,j,k;
real_t normx;
for (j=lc; j<=uc; j++) {
normx=sqrt(tammat(1,n,j,j,x,x));
for (i=1; i<=n; i++) x[i][j] /=normx;
for (k=j+1; k<=uc; k++) elmcol(1,n,k,j,x,x,-tammat(1,n,k,j,x,x));
}
}
void hshdecmul(int n, real_t **a, real_t **b, real_t dwarf)
{
real_t *allocate_real_vector(int, int);
void free_real_vector(real_t *, int);
real_t tammat(int, int, int, int, real_t **, real_t **);
void hshvecmat(int, int, int, int, real_t, real_t [], real_t **);
int j,k,k1,n1;
real_t r,t,c,*v;
v=allocate_real_vector(1,n);
k=1;
n1=n+1;
for (k1=2; k1<=n1; k1++) {
r=tammat(k1,n,k,k,b,b);
if (r > dwarf) {
r = (b[k][k] < 0.0) ? -sqrt(r+b[k][k]*b[k][k]) :
sqrt(r+b[k][k]*b[k][k]);
t=b[k][k]+r;
c = -t/r;
b[k][k] = -r;
v[k]=1.0;
for (j=k1; j<=n; j++) v[j]=b[j][k]/t;
hshvecmat(k,n,k1,n,c,v,b);
hshvecmat(k,n,1,n,c,v,a);
}
k=k1;
}
free_real_vector(v,1);
}
void Ti_Optimization::pretfmmat(double **a, int m, int n, double d[])
{
/*double tammat(int, int, int, int, double **, double **);
void elmcol(int, int, int, int, double **, double **, double);*/
int i,i1,j;
double g,h;
for (i=n; i>=1; i--) {
i1=i+1;
g=d[i];
h=g*a[i][i];
for (j=i1; j<=n; j++) a[i][j]=0.0;
if (h < 0.0) {
for (j=i1; j<=n; j++)
elmcol(i,m,j,i,a,a,tammat(i1,m,i,j,a,a)/h);
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] += 1.0;
}
}
void praxis( int n, double *x, int *data, double (*funct)(double *, void *data), double *in, double *out) {
int illc,i,j,k,k2,nl,maxf,nf,kl,kt,ktm,emergency;
double s,sl,dn,dmin,fx,f1,lds,ldt,sf,df,qf1,qd0,qd1,qa,qb,qc,m2,m4,
small,vsmall,large,vlarge,scbd,ldfac,t2,macheps,reltol,
abstol,h,**v,*d,*y,*z,*q0,*q1,**a,em[8],l;
/*
* Seed random number generator
*/
#ifdef MSWIN
srand(34084320);
#else
srand48(34084320);
#endif
// for (i=0; i<8; ++i) x[i+1] = (double)data->x[i];
d=allocate_real_vector(1,n);
y=allocate_real_vector(1,n);
z=allocate_real_vector(1,n);
q0=allocate_real_vector(1,n);
q1=allocate_real_vector(1,n);
v=allocate_real_matrix(1,n,1,n);
a=allocate_real_matrix(1,n,1,n);
// heuristic numbers:
//
// If the axes may be badly scaled (which is to be avoided if
// possible), then set scbd = 10. otherwise set scbd=1.
//
// If the problem is known to be ill-conditioned, set ILLC = true.
//
// KTM is the number of iterations without improvement before the
// algorithm terminates. KTM = 4 is very cautious; usually KTM = 1
// is satisfactory.
//
macheps=in[0];
reltol=in[1];
abstol=in[2];
maxf=in[5];
h=in[6];
scbd=in[7];
ktm=in[8];
illc = in[9] < 0.0;
small=macheps*macheps;
vsmall=small*small;
large=1.0/small;
vlarge=1.0/vsmall;
m2=reltol;
m4=sqrt(m2);
srand(1);
ldfac = (illc ? 0.1 : 0.01);
kt=nl=0;
nf=1;
out[3]=qf1=fx=(*funct)(x, data);
abstol=t2=small+fabs(abstol);
dmin=small;
if (h < abstol*100.0) h=abstol*100;
ldt=h;
inimat(1,n,1,n,v,0.0);
for (i=1; i<=n; i++) v[i][i]=1.0;
d[1]=qd0=qd1=0.0;
dupvec(1,n,0,q1,x);
inivec(1,n,q0,0.0);
emergency=0;
while (1) {
sf=d[1];
d[1]=s=0.0;
praxismin(1,2,&(d[1]),&s,&fx,0,
n,x,v,&qa,&qb,&qc,qd0,qd1,q0,q1,&nf,
&nl,&fx,m2,m4,dmin,ldt,reltol,abstol,small,h,funct, data);
if (s <= 0.0) mulcol(1,n,1,1,v,v,-1.0);
if (sf <= 0.9*d[1] || 0.9*sf >= d[1]) inivec(2,n,d,0.0);
for (k=2; k<=n; k++) {
dupvec(1,n,0,y,x);
sf=fx;
illc = (illc || kt > 0);
while (1) {
kl=k;
df=0.0;
if (illc) {
/* random stop to get off resulting valley */
for (i=1; i<=n; i++) {
s=z[i]=(0.1*ldt+t2*pow(10.0,kt))*
#ifdef MSWIN
((double)(rand())/RAND_MAX-0.5);
#else
(drand48()-0.5);
#endif
elmveccol(1,n,i,x,v,s);
}
fx=(*funct)(x, data);
nf++;
}
for (k2=k; k2<=n; k2++) {
sl=fx;
s=0.0;
praxismin(k2,2,&(d[k2]),&s,&fx,0,
n,x,v,&qa,&qb,&qc,qd0,qd1,q0,q1,&nf,
&nl,&fx,m2,m4,dmin,ldt,reltol,abstol,small,h,funct, data);
s = illc ? d[k2]*(s+z[k2])*(s+z[k2]) : sl-fx;
if (df < s) {
df=s;
kl=k2;
}
}
if (!illc && df < fabs(100.0*macheps*fx))
illc=1;
else
break;
}
for (k2=1; k2<=k-1; k2++) {
s=0.0;
praxismin(k2,2,&(d[k2]),&s,&fx,0,
n,x,v,&qa,&qb,&qc,qd0,qd1,q0,q1,&nf,
&nl,&fx,m2,m4,dmin,ldt,reltol,abstol,small,h,funct, data);
}
f1=fx;
fx=sf;
lds=0.0;
for (i=1; i<=n; i++) {
sl=x[i];
x[i]=y[i];
y[i] = sl -= y[i];
lds += sl*sl;
}
lds=sqrt(lds);
if (lds > small) {
for (i=kl-1; i>=k; i--) {
for (j=1; j<=n; j++) v[j][i+1]=v[j][i];
d[i+1]=d[i];
}
d[k]=0.0;
dupcolvec(1,n,k,v,y);
mulcol(1,n,k,k,v,v,1.0/lds);
praxismin(k,4,&(d[k]),&lds,&f1,1,
n,x,v,&qa,&qb,&qc,qd0,qd1,q0,q1,&nf,
&nl,&fx,m2,m4,dmin,ldt,reltol,abstol,small,h,funct, data);
if (lds <= 0.0) {
lds = -lds;
mulcol(1,n,k,k,v,v,-1.0);
}
}
ldt *= ldfac;
if (ldt < lds) ldt=lds;
t2=m2*sqrt(vecvec(1,n,0,x,x))+abstol;
kt = (ldt > 0.5*t2) ? 0 : kt+1;
if (kt > ktm) {
out[1]=0.0;
emergency=1;
}
}
if (emergency) break;
/* quad */
s=fx;
fx=qf1;
qf1=s;
qd1=0.0;
for (i=1; i<=n; i++) {
s=x[i];
x[i]=l=q1[i];
q1[i]=s;
qd1 += (s-l)*(s-l);
}
l=qd1=sqrt(qd1);
s=0.0;
if ((qd0*qd1 > DBL_MIN) && (nl >=3*n*n)) {
praxismin(0,2,&s,&l,&qf1,1,
n,x,v,&qa,&qb,&qc,qd0,qd1,q0,q1,&nf,
&nl,&fx,m2,m4,dmin,ldt,reltol,abstol,small,h,funct, data);
qa=l*(l-qd1)/(qd0*(qd0+qd1));
qb=(l+qd0)*(qd1-l)/(qd0*qd1);
qc=l*(l+qd0)/(qd1*(qd0+qd1));
} else {
fx=qf1;
qa=qb=0.0;
qc=1.0;
}
qd0=qd1;
for (i=1; i<=n; i++) {
s=q0[i];
q0[i]=x[i];
x[i]=qa*s+qb*x[i]+qc*q1[i];
}
/* end of quad */
dn=0.0;
for (i=1; i<=n; i++) {
d[i]=1.0/sqrt(d[i]);
if (dn < d[i]) dn=d[i];
}
for (j=1; j<=n; j++) {
s=d[j]/dn;
mulcol(1,n,j,j,v,v,s);
}
if (scbd > 1.0) {
s=vlarge;
for (i=1; i<=n; i++) {
sl=z[i]=sqrt(mattam(1,n,i,i,v,v));
if (sl < m4) z[i]=m4;
if (s > sl) s=sl;
}
for (i=1; i<=n; i++) {
sl=s/z[i];
z[i]=1.0/sl;
if (z[i] > scbd) {
sl=1.0/scbd;
z[i]=scbd;
}
mulrow(1,n,i,i,v,v,sl);
}
}
for (i=1; i<=n; i++) ichrowcol(i+1,n,i,i,v);
em[0]=em[2]=macheps;
em[4]=10*n;
em[6]=vsmall;
dupmat(1,n,1,n,a,v);
if (qrisngvaldec(a,n,n,d,v,em) != 0) {
out[1]=2.0;
emergency=1;
}
if (emergency) break;
if (scbd > 1.0) {
for (i=1; i<=n; i++) mulrow(1,n,i,i,v,v,z[i]);
for (i=1; i<=n; i++) {
s=sqrt(tammat(1,n,i,i,v,v));
d[i] *= s;
s=1.0/s;
mulcol(1,n,i,i,v,v,s);
}
}
for (i=1; i<=n; i++) {
s=dn*d[i];
d[i] = (s > large) ? vsmall :
((s < small) ? vlarge : 1.0/(s*s));
}
/* sort */
for (i=1; 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=1; j<=n; j++) {
s=v[j][i];
v[j][i]=v[j][k];
v[j][k]=s;
}
}
}
/* end of sort */
dmin=d[n];
if (dmin < small) dmin=small;
illc = (m2*d[1]) > dmin;
if (nf >= maxf) {
out[1]=1.0;
break;
}
}
out[2]=fx;
out[4]=nf;
out[5]=nl;
out[6]=ldt;
free_real_vector(d,1);
free_real_vector(y,1);
free_real_vector(z,1);
free_real_vector(q0,1);
free_real_vector(q1,1);
free_real_matrix(v,1,n,1);
free_real_matrix(a,1,n,1);
// for (i=0; i<40; ++i) data->x[i] = (double)x[i+1];
}
void lsqdecomp(real_t **a, int n, int m, int n1, real_t aux[],
real_t aid[], int ci[])
{
real_t *allocate_real_vector(int, int);
void free_real_vector(real_t *, int);
real_t matmat(int, int, int, int, real_t **, real_t **);
real_t tammat(int, int, int, int, real_t **, real_t **);
void elmcol(int, int, int, int, real_t **, real_t **, real_t);
void ichcol(int, int, int, int, real_t **);
int j,k,kpiv,nr,s,fsum;
real_t beta,sigma,norm,aidk,akk,w,eps,temp,*sum;
sum=allocate_real_vector(1,m);
norm=0.0;
aux[3]=m;
nr=n1;
fsum=1;
for (k=1; k<=m; k++) {
if (k == n1+1) {
fsum=1;
nr=n;
}
if (fsum)
for (j=k; j<=m; j++) {
w=sum[j]=tammat(k,nr,j,j,a,a);
if (w > norm) norm=w;
}
fsum=0;
eps=aux[2]*sqrt(norm);
sigma=sum[k];
kpiv=k;
for (j=k+1; j<=m; j++)
if (sum[j] > sigma) {
sigma=sum[j];
kpiv=j;
}
if (kpiv != k) {
sum[kpiv]=sum[k];
ichcol(1,n,k,kpiv,a);
}
ci[k]=kpiv;
akk=a[k][k];
sigma=tammat(k,nr,k,k,a,a);
w=sqrt(sigma);
aidk=aid[k]=((akk < 0.0) ? w : -w);
if (w < eps) {
aux[3]=k-1;
break;
}
beta=1.0/(sigma-akk*aidk);
a[k][k]=akk-aidk;
for (j=k+1; j<=m; j++) {
elmcol(k,nr,j,k,a,a,-beta*tammat(k,nr,k,j,a,a));
temp=a[k][j];
sum[j] -= temp*temp;
}
if (k == n1)
for (j=n1+1; j<=n; j++)
for (s=1; s<=m; s++) {
nr = (s > n1) ? n1 : s-1;
w=a[j][s]-matmat(1,nr,j,s,a,a);
a[j][s] = (s > n1) ? w : w/aid[s];
}
}
free_real_vector(sum,1);
}