il_mat il_mat_invert(il_mat a)
{
il_mat res = il_mat_identity();
// for every column ..
for (int col = 0; col < 4; ++col) {
float diagonalfield = a.data[col*4+col];
// if we have a weird matrix with zero on the diagonal ..
float eps = 0.0001;
if (fabs(diagonalfield) < eps) {
// SIIIGH.
// Okay, let's go hunting for a row with a better diagonal to use
// keep in mind, row swapping is also a linear operation!
for (int row = 0; row < 4; ++row) if (row != col) if (fabs(a.data[row*4+col]) >= eps) {
swaprow(&a, row, col);
swaprow(&res,row, col);
}
diagonalfield = a.data[col*4+col]; // recompute
// if it still fails, we probably have a denatured matrix and are proper f****d.
assert(fabs(diagonalfield) >= eps);
}
// for every row ..
for (int row = 0; row < 4; ++row) {
// if it's not on the diagonal ..
if (row != col) {
float field = a.data[row*4+col];
// bring this field to 0 by subtracting the row that's on the diagonal for this column
addrow(&a, /* src */ col, /* dst */ row, /* factor */ -field/diagonalfield);
// do the same to identity
addrow(&res,/* src */ col, /* dst */ row, /* factor */ -field/diagonalfield);
}
}
// and finally, bring the diagonal to 1
mulrow(&a, col, 1/diagonalfield);
mulrow(&res,col, 1/diagonalfield);
}
// Success! Because a is now id, res is now a^-1. Math is fun!
return res;
}
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];
}
int peidefunct(int nrow, int ncol, real_t par[], real_t res[],
int n, int m, int nobs, int *nbp, int first, int *sec,
int *max, int *nis, real_t eps1, int weight, int bp[],
real_t save[], real_t ymax[], real_t y[], real_t **yp,
real_t **fy, real_t **fp, int cobs[], real_t tobs[],
real_t obs[], real_t in[], real_t aux[], int clean,
int (*deriv)(int,int,real_t [],real_t [],real_t,real_t []),
int (*jacdfdy)(int,int,real_t [],real_t [],real_t,real_t **),
int (*jacdfdp)(int,int,real_t [],real_t [],real_t,real_t **),
void (*callystart)(int,int,real_t [],real_t [],real_t[]),
void (*monitor)(int,int,int,real_t [],real_t [],int,int))
{
/* this function is internally used by PEIDE */
void peidereset(int, int, real_t, real_t, real_t, real_t, real_t [],
real_t [], real_t *, real_t *, real_t *, int *);
void peideorder(int, int, real_t, real_t [], real_t [],
real_t *, real_t *, real_t *, real_t *, real_t *, int *);
void peidestep(int, int, int, real_t, real_t, real_t, real_t,
real_t [], real_t [], real_t [], real_t [], int *, real_t *);
real_t peideinterpol(int, int, int, real_t, real_t []);
int l,k,knew,fails,same,kpold,n6,nnpar,j5n,cobsii,*p,evaluate,
evaluated,decompose,conv,extra,npar,i,j,jj,ii;
real_t xold,hold,a0,tolup,tol,toldwn,tolconv,h,ch,chnew,error,
dfi,tobsdif,a[6],*delta,*lastdelta,*df,*y0,**jacob,xend,
hmax,hmin,eps,s,aa,x,t,c;
p=allocate_integer_vector(1,n);
delta=allocate_real_vector(1,n);
lastdelta=allocate_real_vector(1,n);
df=allocate_real_vector(1,n);
y0=allocate_real_vector(1,n);
jacob=allocate_real_matrix(1,n,1,n);
if (*sec) {
*sec=0;
goto Finish;
}
xend=tobs[nobs];
eps=in[2];
npar=m;
extra=(*nis)=0;
ii=1;
jj = (*nbp == 0) ? 0 : 1;
n6=n*6;
inivec(-3,-1,save,0.0);
inivec(n6+1,(6+m)*n,y,0.0);
inimat(1,nobs+(*nbp),1,m+(*nbp),yp,0.0);
t=tobs[1];
x=tobs[0];
(*callystart)(n,m,par,y,ymax);
hmax=tobs[1]-tobs[0];
hmin=hmax*in[1];
/* evaluate jacobian */
evaluate=0;
decompose=evaluated=1;
if (!(*jacdfdy)(n,m,par,y,x,fy)) {
save[-3]=4.0;
goto Finish;
}
nnpar=n*npar;
Newstart:
k=1;
kpold=0;
same=2;
peideorder(n,k,eps,a,save,&tol,&tolup,&toldwn,&tolconv,
&a0,&decompose);
if (!(*deriv)(n,m,par,y,x,df)) {
save[-3]=3.0;
goto Finish;
}
s=FLT_MIN;
for (i=1; i<=n; i++) {
aa=matvec(1,n,i,fy,df)/ymax[i];
s += aa*aa;
}
h=sqrt(2.0*eps/sqrt(s));
if (h > hmax)
h=hmax;
else
if (h < hmin) h=hmin;
xold=x;
hold=h;
ch=1.0;
for (i=1; i<=n; i++) {
save[i]=y[i];
save[n+i]=y[n+i]=df[i]*h;
}
fails=0;
while (x < xend) {
if (x+h <= xend)
x += h;
else {
h=xend-x;
x=xend;
ch=h/hold;
c=1.0;
for (j=n; j<=k*n; j += n) {
c *= ch;
for (i=j+1; i<=j+n; i++) y[i] *= c;
}
same = (same < 3) ? 3 : same+1;
}
/* prediction */
for (l=1; l<=n; l++) {
for (i=l; i<=(k-1)*n+l; i += n)
for (j=(k-1)*n+l; j>=i; j -= n) y[j] += y[j+n];
delta[l]=0.0;
}
evaluated=0;
/* correction and estimation local error */
for (l=1; l<=3; l++) {
if (!(*deriv)(n,m,par,y,x,df)) {
save[-3]=3;
goto Finish;
}
for (i=1; i<=n; i++) df[i]=df[i]*h-y[n+i];
if (evaluate) {
/* evaluate jacobian */
evaluate=0;
decompose=evaluated=1;
if (!(*jacdfdy)(n,m,par,y,x,fy)) {
save[-3]=4.0;
goto Finish;
}
}
if (decompose) {
/* decompose jacobian */
decompose=0;
c = -a0*h;
for (j=1; j<=n; j++) {
for (i=1; i<=n; i++) jacob[i][j]=fy[i][j]*c;
jacob[j][j] += 1.0;
}
dec(jacob,n,aux,p);
}
sol(jacob,n,p,df);
conv=1;
for (i=1; i<=n; i++) {
dfi=df[i];
y[i] += a0*dfi;
y[n+i] += dfi;
delta[i] += dfi;
conv=(conv && (fabs(dfi) < tolconv*ymax[i]));
}
if (conv) {
s=FLT_MIN;
for (i=1; i<=n; i++) {
aa=delta[i]/ymax[i];
s += aa*aa;
}
error=s;
break;
}
}
/* acceptance or rejection */
if (!conv) {
if (!evaluated)
evaluate=1;
else {
ch /= 4.0;
if (h < 4.0*hmin) {
save[-1] += 10.0;
hmin /= 10.0;
if (save[-1] > 40.0) goto Finish;
}
}
peidereset(n,k,hmin,hmax,hold,xold,y,save,&ch,&x,
&h,&decompose);
} else if (error > tol) {
fails++;
if (h > 1.1*hmin) {
if (fails > 2) {
peidereset(n,k,hmin,hmax,hold,xold,y,save,&ch,&x,
&h,&decompose);
goto Newstart;
} else {
/* calculate step and order */
peidestep(n,k,fails,tolup,toldwn,tol,error,delta,
lastdelta,y,ymax,&knew,&chnew);
if (knew != k) {
k=knew;
peideorder(n,k,eps,a,save,&tol,&tolup,
&toldwn,&tolconv,&a0,&decompose);
}
ch *= chnew;
peidereset(n,k,hmin,hmax,hold,xold,y,save,&ch,&x,
&h,&decompose);
}
} else {
if (k == 1) {
/* violate eps criterion */
save[-2] += 1.0;
same=4;
goto Errortestok;
}
k=1;
peidereset(n,k,hmin,hmax,hold,xold,y,save,&ch,&x,
&h,&decompose);
peideorder(n,k,eps,a,save,&tol,&tolup,
&toldwn,&tolconv,&a0,&decompose);
same=2;
}
} else {
Errortestok:
fails=0;
for (i=1; i<=n; i++) {
c=delta[i];
for (l=2; l<=k; l++) y[l*n+i] += a[l]*c;
if (fabs(y[i]) > ymax[i]) ymax[i]=fabs(y[i]);
}
same--;
if (same == 1)
dupvec(1,n,0,lastdelta,delta);
else if (same == 0) {
/* calculate step and order */
peidestep(n,k,fails,tolup,toldwn,tol,error,delta,
lastdelta,y,ymax,&knew,&chnew);
if (chnew > 1.1) {
if (k != knew) {
if (knew > k)
mulvec(knew*n+1,knew*n+n,-knew*n,y,delta,
a[k]/knew);
k=knew;
peideorder(n,k,eps,a,save,&tol,&tolup,
&toldwn,&tolconv,&a0,&decompose);
}
same=k+1;
if (chnew*h > hmax) chnew=hmax/h;
h *= chnew;
c=1.0;
for (j=n; j<=k*n; j += n) {
c *= chnew;
mulvec(j+1,j+n,0,y,y,c);
}
decompose=1;
} else
same=10;
}
(*nis)++;
/* start of an integration step of yp */
if (clean) {
hold=h;
xold=x;
kpold=k;
ch=1.0;
dupvec(1,k*n+n,0,save,y);
} else {
if (h != hold) {
ch=h/hold;
c=1.0;
for (j=n6+nnpar; j<=kpold*nnpar+n6; j += nnpar) {
c *= ch;
for (i=j+1; i<=j+nnpar; i++) y[i] *= c;
}
hold=h;
}
if (k > kpold)
inivec(n6+k*nnpar+1,n6+k*nnpar+nnpar,y,0.0);
xold=x;
kpold=k;
ch=1.0;
dupvec(1,k*n+n,0,save,y);
/* evaluate jacobian */
evaluate=0;
decompose=evaluated=1;
if (!(*jacdfdy)(n,m,par,y,x,fy)) {
save[-3]=4.0;
goto Finish;
}
/* decompose jacobian */
decompose=0;
c = -a0*h;
for (j=1; j<=n; j++) {
for (i=1; i<=n; i++) jacob[i][j]=fy[i][j]*c;
jacob[j][j] += 1.0;
}
dec(jacob,n,aux,p);
if (!(*jacdfdp)(n,m,par,y,x,fp)) {
save[-3]=5.0;
goto Finish;
}
if (npar > m) inimat(1,n,m+1,npar,fp,0.0);
/* prediction */
for (l=0; l<=k-1; l++)
for (j=k-1; j>=l; j--)
elmvec(j*nnpar+n6+1,j*nnpar+n6+nnpar,nnpar,
y,y,1.0);
/* correction */
for (j=1; j<=npar; j++) {
j5n=(j+5)*n;
dupvec(1,n,j5n,y0,y);
for (i=1; i<=n; i++)
df[i]=h*(fp[i][j]+matvec(1,n,i,fy,y0))-
y[nnpar+j5n+i];
sol(jacob,n,p,df);
for (l=0; l<=k; l++) {
i=l*nnpar+j5n;
elmvec(i+1,i+n,-i,y,df,a[l]);
}
}
}
while (x >= t) {
/* calculate a row of the jacobian matrix and an
element of the residual vector */
tobsdif=(tobs[ii]-x)/h;
cobsii=cobs[ii];
res[ii]=peideinterpol(cobsii,n,k,tobsdif,y)-obs[ii];
if (!clean) {
for (i=1; i<=npar; i++)
yp[ii][i]=peideinterpol(cobsii+(i+5)*n,nnpar,k,
tobsdif,y);
/* introducing break-points */
if (bp[jj] != ii) {
} else if (first && fabs(res[ii]) < eps1) {
(*nbp)--;
for (i=jj; i<=(*nbp); i++) bp[i]=bp[i+1];
bp[*nbp+1]=0;
} else {
extra++;
if (first) par[m+jj]=obs[ii];
/* introducing a jacobian row and a residual
vector element for continuity requirements */
yp[nobs+jj][m+jj] = -weight;
mulrow(1,npar,nobs+jj,ii,yp,yp,weight);
res[nobs+jj]=weight*(res[ii]+obs[ii]-par[m+jj]);
}
}
if (ii == nobs)
goto Finish;
else {
t=tobs[ii+1];
if (bp[jj] == ii && jj < *nbp) jj++;
hmax=t-tobs[ii];
hmin=hmax*in[1];
ii++;
}
}
/* break-points introduce new initial values for y & yp */
if (extra > 0) {
for (i=1; i<=n; i++) {
y[i]=peideinterpol(i,n,k,tobsdif,y);
for (j=1; j<=npar; j++)
y[i+(j+5)*n]=peideinterpol(i+(j+5)*n,nnpar,
k,tobsdif,y);
}
for (l=1; l<=extra; l++) {
cobsii=cobs[bp[npar-m+l]];
y[cobsii]=par[npar+l];
for (i=1; i<=npar+extra; i++) y[cobsii+(5+i)*n]=0.0;
inivec(1+nnpar+(l+5)*n,nnpar+(l+6)*n,y,0.0);
y[cobsii+(5+npar+l)*n]=1.0;
}
npar += extra;
extra=0;
x=tobs[ii-1];
/* evaluate jacobian */
evaluate=0;
decompose=evaluated=1;
if (!(*jacdfdy)(n,m,par,y,x,fy)) {
save[-3]=4.0;
goto Finish;
}
nnpar=n*npar;
goto Newstart;
}
}
}
Finish:
if (save[-2] > *max) *max=save[-2];
if (!first) (*monitor)(1,ncol,nrow,par,res,weight,*nis);
free_integer_vector(p,1);
free_real_vector(delta,1);
free_real_vector(lastdelta,1);
free_real_vector(df,1);
free_real_vector(y0,1);
free_real_matrix(jacob,1,n,1);
return (save[-1] <= 40.0 && save[-3] == 0.0);
}
