void gssinv(real_t **a, int n, real_t aux[])
{
int *allocate_integer_vector(int, int);
void free_integer_vector(int *, int);
void gsselm(real_t **, int, real_t [], int [], int []);
real_t inv1(real_t **, int, int [], int [], int);
int *ri,*ci;
ri=allocate_integer_vector(1,n);
ci=allocate_integer_vector(1,n);
gsselm(a,n,aux,ri,ci);
if (aux[3] == n) aux[9]=inv1(a,n,ri,ci,1);
free_integer_vector(ri,1);
free_integer_vector(ci,1);
}
void vecperm(int perm[], int low, int upp, real_t vector[])
{
int *allocate_integer_vector(int, int);
void free_integer_vector(int *, int);
int t,j,k,*todo;
real_t a;
todo=allocate_integer_vector(low,upp);
for (t=low; t<=upp; t++) todo[t]=1;
for (t=low; t<=upp; t++)
if (todo[t]) {
k=t;
a=vector[k];
j=perm[k];
while (j != t) {
vector[k]=vector[j];
todo[k]=0;
k=j;
j=perm[k];
}
vector[k]=a;
todo[k]=0;
}
free_integer_vector(todo,low);
}
void decsol(real_t **a, int n, real_t aux[], real_t b[])
{
int *allocate_integer_vector(int, int);
void free_integer_vector(int *, int);
void sol(real_t **, int, int [], real_t []);
void dec(real_t **, int, real_t [], int []);
int *p;
p=allocate_integer_vector(1,n);
dec(a,n,aux,p);
if (aux[3] == n) sol(a,n,p,b);
free_integer_vector(p,1);
}
void gssnewton(int m, int n, real_t par[], real_t rv[], real_t **jjinv,
int (*funct)(int, int, real_t[], real_t[]),
void (*jacobian)(int, int, real_t[], real_t[], real_t **),
real_t in[], real_t out[])
{
int *allocate_integer_vector(int, int);
real_t *allocate_real_vector(int, int);
real_t **allocate_real_matrix(int, int, int, int);
void free_integer_vector(int *, int);
void free_real_vector(real_t *, int);
void free_real_matrix(real_t **, int, int, int);
real_t vecvec(int, int, int, real_t [], real_t []);
void dupvec(int, int, int, real_t [], real_t []);
void elmvec(int, int, int, real_t [], real_t [], real_t);
void lsqortdec(real_t **, int, int, real_t [], real_t [], int []);
void lsqsol(real_t **, int, int, real_t [], int [], real_t []);
void lsqinv(real_t **, int, real_t [], int []);
int i,j,inr,mit,text,it,itmax,inrmax,tim,feval,fevalmax,conv,
testthf,dampingon,*ci,fail;
real_t rho,res1,res2,rn,reltolpar,abstolpar,abstolres,stap,normx,
**jac,*pr,*aid,*sol,*fu2,aux[6];
ci=allocate_integer_vector(1,n);
pr=allocate_real_vector(1,n);
aid=allocate_real_vector(1,n);
sol=allocate_real_vector(1,n);
fu2=allocate_real_vector(1,m);
jac=allocate_real_matrix(1,m+1,1,n);
itmax=fevalmax=in[5];
aux[2]=n*in[0];
tim=in[7];
reltolpar=in[1]*in[1];
abstolpar=in[2]*in[2];
abstolres=in[4]*in[4];
inrmax=in[6];
dupvec(1,n,0,pr,par);
if (m < n)
for (i=1; i<=n; i++) jac[m+1][i]=0.0;
text=4;
mit=0;
testthf=1;
res2=stap=out[5]=out[6]=out[7]=0.0;
(*funct)(m,n,par,fu2);
rn=vecvec(1,m,0,fu2,fu2);
out[3]=sqrt(rn);
feval=1;
dampingon=0;
fail=0;
it=1;
do {
out[5]=it;
(*jacobian)(m,n,par,fu2,jac);
if (!testthf) {
text=7;
fail=1;
break;
}
lsqortdec(jac,m,n,aux,aid,ci);
if (aux[3] != n) {
text=5;
fail=1;
break;
}
lsqsol(jac,m,n,aid,ci,fu2);
dupvec(1,n,0,sol,fu2);
stap=vecvec(1,n,0,sol,sol);
rho=2.0;
normx=vecvec(1,n,0,par,par);
if (stap > reltolpar*normx+abstolpar || it == 1 && stap > 0.0) {
inr=0;
do {
rho /= 2.0;
if (inr > 0) {
res1=res2;
dupvec(1,m,0,rv,fu2);
dampingon = inr > 1;
}
for (i=1; i<=n; i++) pr[i]=par[i]-sol[i]*rho;
feval++;
if (!(*funct)(m,n,pr,fu2)) {
text=6;
fail=1;
break;
}
res2=vecvec(1,m,0,fu2,fu2);
conv = inr >= inrmax;
inr++;
} while ((inr == 1) ? (dampingon || res2 >= rn) :
(!conv && (rn <= res1 || res2 < res1)));
if (fail) break;
if (conv) {
mit++;
if (mit < tim) conv=0;
} else
mit=0;
if (inr > 1) {
rho *= 2.0;
elmvec(1,n,0,par,sol,-rho);
rn=res1;
if (inr > 2) out[7]=it;
} else {
dupvec(1,n,0,par,pr);
rn=res2;
dupvec(1,m,0,rv,fu2);
}
if (rn <= abstolres) {
text=1;
itmax=it;
} else
if (conv && inrmax > 0) {
text=3;
itmax=it;
} else
dupvec(1,m,0,fu2,rv);
} else {
text=2;
rho=1.0;
itmax=it;
}
it++;
} while (it <= itmax && feval < fevalmax);
if (!fail) {
lsqinv(jac,n,aid,ci);
for (i=1; i<=n; i++) {
jjinv[i][i]=jac[i][i];
for (j=i+1; j<=n; j++) jjinv[i][j]=jjinv[j][i]=jac[i][j];
}
}
out[6]=sqrt(stap)*rho;
out[2]=sqrt(rn);
out[4]=feval;
out[1]=text;
out[8]=aux[3];
out[9]=aux[5];
free_integer_vector(ci,1);
free_real_vector(pr,1);
free_real_vector(aid,1);
free_real_vector(sol,1);
free_real_vector(fu2,1);
free_real_matrix(jac,1,m+1,1);
}
void write_vtk(char *basename, double time, int n_variables, char **variable_name, int n_unknowns, int *unknown_to_id, double *x, int n_nodes, struct NODE *node, int n_faces, struct FACE *face, int n_cells, struct CELL *cell, int n_zones, struct ZONE *zone)
{
char *filename;
exit_if_false(allocate_character_vector(&filename, MAX_STRING_LENGTH),"allocating filename");
FILE *file;
int *point_used, *point_index;
exit_if_false(allocate_integer_vector(&point_used,n_nodes + n_cells),"allocating point usage array");
exit_if_false(allocate_integer_vector(&point_index,n_nodes + n_cells),"allocating point index array");
int n_points, n_elements;
int i, j, u, v, z, offset;
for(v = 0; v < n_variables; v ++)
{
generate_timed_named_filename(filename, basename, time, variable_name[v]);
file = fopen(filename,"w");
exit_if_false(file != NULL,"opening file");
fprintf(file,"<VTKFile type=\"UnstructuredGrid\" version=\"0.1\" byte_order=\"LittleEndian\">\n<UnstructuredGrid>\n");
for(i = 0; i < n_nodes + n_cells; i ++) point_used[i] = 0;
n_elements = 0;
for(u = 0; u < n_unknowns; u ++)
{
i = ID_TO_INDEX(unknown_to_id[u]);
z = ID_TO_ZONE(unknown_to_id[u]);
if(zone[z].variable == v)
{
n_elements ++;
if(zone[z].location == 'f')
{
point_used[(int)(face[i].node[0] - &node[0])] = 1;
point_used[(int)(face[i].node[face[i].n_nodes - 1] - &node[0])] = 1;
for(j = 0; j < face[i].n_borders; j ++) point_used[n_nodes + (int)(face[i].border[j] - &cell[0])] = 1;
}
else if(zone[z].location == 'c')
{
for(j = 0; j < cell[i].n_faces; j ++)
{
point_used[(int)(cell[i].face[j]->node[0] - &node[0])] = 1;
point_used[(int)(face[i].node[cell[i].face[j]->n_nodes - 1] - &node[0])] = 1;
}
}
else exit_if_false(0,"recognising the location");
}
}
n_points = 0;
for(i = 0; i < n_nodes + n_cells; i ++) if(point_used[i]) point_index[n_points ++] = i;
fprintf(file,"<Piece NumberOfPoints=\"%i\" NumberOfCells=\"%i\">\n", n_points, n_elements);
fprintf(file,"<CellData>\n");
fprintf(file,"<DataArray Name=\"%s\" type=\"Float64\" format=\"ascii\">\n",variable_name[v]);
for(u = 0; u < n_unknowns; u ++) if(zone[ID_TO_ZONE(unknown_to_id[u])].variable == v) fprintf(file," %+.10e",x[u]);
fprintf(file,"\n</DataArray>\n");
fprintf(file,"</CellData>\n");
fprintf(file,"<Points>\n");
fprintf(file,"<DataArray type=\"Float64\" NumberOfComponents=\"3\" format=\"ascii\">\n");
for(i = 0; i < n_nodes; i ++) if(point_used[i]) fprintf(file," %+.10e %+.10e %+.10e",node[i].x[0],node[i].x[1],0.0);
for(i = 0; i < n_cells; i ++) if(point_used[n_nodes + i]) fprintf(file," %+.10e %+.10e %+.10e",cell[i].centroid[0],cell[i].centroid[1],0.0);
fprintf(file,"\n</DataArray>\n");
fprintf(file,"</Points>\n");
fprintf(file,"<Cells>\n");
fprintf(file,"<DataArray type=\"Int32\" Name=\"connectivity\" format=\"ascii\">\n");
for(u = 0; u < n_unknowns; u ++)
{
i = ID_TO_INDEX(unknown_to_id[u]);
z = ID_TO_ZONE(unknown_to_id[u]);
if(zone[z].variable == v)
{
if(zone[z].location == 'f')
{
fprintf(file," %i",point_index[(int)((face[i].oriented[0] ? face[i].node[1] : face[i].node[0]) - &node[0])]);
fprintf(file," %i",point_index[n_nodes + (int)(face[i].border[0] - &cell[0])]);
fprintf(file," %i",point_index[(int)((face[i].oriented[0] ? face[i].node[0] : face[i].node[1]) - &node[0])]);
if(face[i].n_borders == 2) fprintf(file," %i",point_index[n_nodes + (int)(face[i].border[1] - &cell[0])]);
}
else if(zone[z].location == 'c')
{
for(j = 0; j < cell[i].n_faces; j ++)
{
fprintf(file," %i",point_index[(int)(cell[i].face[j]->node[!cell[i].oriented[j]] - &node[0])]);
}
}
else exit_if_false(0,"recognising the location");
}
}
fprintf(file,"\n</DataArray>\n");
fprintf(file,"<DataArray type=\"Int32\" Name=\"offsets\" format=\"ascii\">\n");
offset = 0;
for(u = 0; u < n_unknowns; u ++)
{
i = ID_TO_INDEX(unknown_to_id[u]);
z = ID_TO_ZONE(unknown_to_id[u]);
if(zone[z].variable == v)
{
if(zone[z].location == 'f')
{
offset += 2 + face[i].n_borders;
}
else if(zone[z].location == 'c')
{
offset += cell[i].n_faces;
}
else exit_if_false(0,"recognising the location");
fprintf(file," %i",offset);
}
}
fprintf(file,"\n</DataArray>\n");
fprintf(file,"<DataArray type=\"Int32\" Name=\"types\" format=\"ascii\">\n");
for(i = 0; i < n_elements; i ++) fprintf(file," %i",7);
fprintf(file,"\n</DataArray>\n");
fprintf(file,"</Cells>");
fprintf(file,"\n</Piece>\n</UnstructuredGrid>\n</VTKFile>");
fclose(file);
}
free_vector(filename);
free_vector(point_used);
free_vector(point_index);
}
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);
}
void peide(int n, int m, int nobs, int *nbp, real_t par[],
real_t res[], int bp[], real_t **jtjinv,
real_t in[], real_t out[],
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 (*data)(int,real_t [],real_t [],int[]),
void (*monitor)(int,int,int,real_t [],real_t [],int,int))
{
int i,j,weight,ncol,nrow,away,max,nfe,nis,*cobs,
first,sec,clean,nbpold,maxfe,fe,it,err,emergency;
real_t eps1,res1,in3,in4,fac3,fac4,aux[4],*obs,*save,*tobs,
**yp,*ymax,*y,**fy,**fp,w,**aid,temp,
vv,ww,w2,mu,res2,fpar,fparpres,lambda,lambdamin,p,pw,
reltolres,abstolres,em[8],*val,*b,*bb,*parpres,**jaco;
static real_t save1[35]={1.0, 1.0, 9.0, 4.0, 0.0, 2.0/3.0, 1.0,
1.0/3.0, 36.0, 20.25, 1.0, 6.0/11.0, 1.0, 6.0/11.0,
1.0/11.0, 84.028, 53.778, 0.25, 0.48, 1.0, 0.7, 0.2,
0.02, 156.25, 108.51, 0.027778, 120.0/274.0, 1.0,
225.0/274.0, 85.0/274.0, 15.0/274.0, 1.0/274.0, 0.0,
187.69, 0.0047361};
nbpold=(*nbp);
cobs=allocate_integer_vector(1,nobs);
obs=allocate_real_vector(1,nobs);
save=allocate_real_vector(-38,6*n);
tobs=allocate_real_vector(0,nobs);
ymax=allocate_real_vector(1,n);
y=allocate_real_vector(1,6*n*(nbpold+m+1));
yp=allocate_real_matrix(1,nbpold+nobs,1,nbpold+m);
fy=allocate_real_matrix(1,n,1,n);
fp=allocate_real_matrix(1,n,1,m+nbpold);
aid=allocate_real_matrix(1,m+nbpold,1,m+nbpold);
for (i=0; i<=34; i++) save[-38+i]=save1[i];
(*data)(nobs,tobs,obs,cobs);
weight=1;
first=sec=0;
clean=(*nbp > 0);
aux[2]=FLT_EPSILON;
eps1=1.0e10;
out[1]=0.0;
bp[0]=max=0;
/* smooth integration without break-points */
if (!peidefunct(nobs,m,par,res,
n,m,nobs,nbp,first,&sec,&max,&nis,eps1,weight,bp,
save,ymax,y,yp,fy,fp,cobs,tobs,obs,in,aux,clean,deriv,
jacdfdy,jacdfdp,callystart,monitor)) goto Escape;
res1=sqrt(vecvec(1,nobs,0,res,res));
nfe=1;
if (in[5] == 1.0) {
out[1]=1.0;
goto Escape;
}
if (clean) {
first=1;
clean=0;
fac3=sqrt(sqrt(in[3]/res1));
fac4=sqrt(sqrt(in[4]/res1));
eps1=res1*fac4;
if (!peidefunct(nobs,m,par,res,
n,m,nobs,nbp,first,&sec,&max,&nis,eps1,weight,bp,
save,ymax,y,yp,fy,fp,cobs,tobs,obs,in,aux,clean,deriv,
jacdfdy,jacdfdp,callystart,monitor)) goto Escape;
first=0;
} else
nfe=0;
ncol=m+(*nbp);
nrow=nobs+(*nbp);
sec=1;
in3=in[3];
in4=in[4];
in[3]=res1;
weight=away=0;
out[4]=out[5]=w=0.0;
temp=sqrt(weight)+1.0;
weight=temp*temp;
while (weight != 16 && *nbp > 0) {
if (away == 0 && w != 0.0) {
/* if no break-points were omitted then one function
function evaluation is saved */
w=weight/w;
for (i=nobs+1; i<=nrow; i++) {
for (j=1; j<=ncol; j++) yp[i][j] *= w;
res[i] *= w;
}
sec=1;
nfe--;
}
in[3] *= fac3*weight;
in[4]=eps1;
(*monitor)(2,ncol,nrow,par,res,weight,nis);
/* marquardt's method */
val=allocate_real_vector(1,ncol);
b=allocate_real_vector(1,ncol);
bb=allocate_real_vector(1,ncol);
parpres=allocate_real_vector(1,ncol);
jaco=allocate_real_matrix(1,nrow,1,ncol);
vv=10.0;
w2=0.5;
mu=0.01;
ww = (in[6] < 1.0e-7) ? 1.0e-8 : 1.0e-1*in[6];
em[0]=em[2]=em[6]=in[0];
em[4]=10*ncol;
reltolres=in[3];
abstolres=in[4]*in[4];
maxfe=in[5];
err=0;
fe=it=1;
p=fpar=res2=0.0;
pw = -log(ww*in[0])/2.30;
if (!peidefunct(nrow,ncol,par,res,
n,m,nobs,nbp,first,&sec,&max,&nis,eps1,
weight,bp,save,ymax,y,yp,fy,fp,cobs,tobs,obs,
in,aux,clean,deriv,jacdfdy,jacdfdp,
callystart,monitor))
err=3;
else {
fpar=vecvec(1,nrow,0,res,res);
out[3]=sqrt(fpar);
emergency=0;
it=1;
do {
dupmat(1,nrow,1,ncol,jaco,yp);
i=qrisngvaldec(jaco,nrow,ncol,val,aid,em);
if (it == 1)
lambda=in[6]*vecvec(1,ncol,0,val,val);
else
if (p == 0.0) lambda *= w2;
for (i=1; i<=ncol; i++)
b[i]=val[i]*tamvec(1,nrow,i,jaco,res);
while (1) {
for (i=1; i<=ncol; i++)
bb[i]=b[i]/(val[i]*val[i]+lambda);
for (i=1; i<=ncol; i++)
parpres[i]=par[i]-matvec(1,ncol,i,aid,bb);
fe++;
if (fe >= maxfe)
err=1;
else
if (!peidefunct(nrow,ncol,parpres,res,
n,m,nobs,nbp,first,&sec,&max,&nis,
eps1,weight,bp,save,ymax,y,yp,fy,fp,
cobs,tobs,obs,in,aux,clean,deriv,
jacdfdy,jacdfdp,callystart,monitor))
err=2;
if (err != 0) {
emergency=1;
break;
}
fparpres=vecvec(1,nrow,0,res,res);
res2=fpar-fparpres;
if (res2 < mu*vecvec(1,ncol,0,b,bb)) {
p += 1.0;
lambda *= vv;
if (p == 1.0) {
lambdamin=ww*vecvec(1,ncol,0,val,val);
if (lambda < lambdamin) lambda=lambdamin;
}
if (p >= pw) {
err=4;
emergency=1;
break;
}
} else {
dupvec(1,ncol,0,par,parpres);
fpar=fparpres;
break;
}
}
if (emergency) break;
it++;
} while (fpar>abstolres &&
res2>reltolres*fpar+abstolres);
for (i=1; i<=ncol; i++)
mulcol(1,ncol,i,i,jaco,aid,1.0/(val[i]+in[0]));
for (i=1; i<=ncol; i++)
for (j=1; j<=i; j++)
aid[i][j]=aid[j][i]=mattam(1,ncol,i,j,jaco,jaco);
lambda=lambdamin=val[1];
for (i=2; i<=ncol; i++)
if (val[i] > lambda)
lambda=val[i];
else
if (val[i] < lambdamin) lambdamin=val[i];
temp=lambda/(lambdamin+in[0]);
out[7]=temp*temp;
out[2]=sqrt(fpar);
out[6]=sqrt(res2+fpar)-out[2];
}
out[4]=fe;
out[5]=it-1;
out[1]=err;
free_real_vector(val,1);
free_real_vector(b,1);
free_real_vector(bb,1);
free_real_vector(parpres,1);
free_real_matrix(jaco,1,nrow,1);
if (out[1] > 0.0) goto Escape;
/* the relative starting value of lambda is adjusted
to the last value of lambda used */
away=out[4]-out[5]-1.0;
in[6] *= pow(5.0,away)*pow(2.0,away-out[5]);
nfe += out[4];
w=weight;
temp=sqrt(weight)+1.0;
eps1=temp*temp*in[4]*fac4;
away=0;
/* omit useless break-points */
for (j=1; j<=(*nbp); j++)
if (fabs(obs[bp[j]]+res[bp[j]]-par[j+m]) < eps1) {
(*nbp)--;
for (i=j; i<=(*nbp); i++) bp[i]=bp[i+1];
dupvec(j+m,(*nbp)+m,1,par,par);
j--;
away++;
bp[*nbp+1]=0;
}
ncol -= away;
nrow -= away;
temp=sqrt(weight)+1.0;
weight=temp*temp;
}
in[3]=in3;
in[4]=in4;
*nbp=0;
weight=1;
(*monitor)(2,m,nobs,par,res,weight,nis);
/* marquardt's method */
val=allocate_real_vector(1,m);
b=allocate_real_vector(1,m);
bb=allocate_real_vector(1,m);
parpres=allocate_real_vector(1,m);
jaco=allocate_real_matrix(1,nobs,1,m);
vv=10.0;
w2=0.5;
mu=0.01;
ww = (in[6] < 1.0e-7) ? 1.0e-8 : 1.0e-1*in[6];
em[0]=em[2]=em[6]=in[0];
em[4]=10*m;
reltolres=in[3];
abstolres=in[4]*in[4];
maxfe=in[5];
err=0;
fe=it=1;
p=fpar=res2=0.0;
pw = -log(ww*in[0])/2.30;
if (!peidefunct(nobs,m,par,res,
n,m,nobs,nbp,first,&sec,&max,&nis,eps1,weight,bp,
save,ymax,y,yp,fy,fp,cobs,tobs,obs,in,aux,clean,
deriv,jacdfdy,jacdfdp,callystart,monitor))
err=3;
else {
fpar=vecvec(1,nobs,0,res,res);
out[3]=sqrt(fpar);
emergency=0;
it=1;
do {
dupmat(1,nobs,1,m,jaco,yp);
i=qrisngvaldec(jaco,nobs,m,val,jtjinv,em);
if (it == 1)
lambda=in[6]*vecvec(1,m,0,val,val);
else
if (p == 0.0) lambda *= w2;
for (i=1; i<=m; i++)
b[i]=val[i]*tamvec(1,nobs,i,jaco,res);
while (1) {
for (i=1; i<=m; i++)
bb[i]=b[i]/(val[i]*val[i]+lambda);
for (i=1; i<=m; i++)
parpres[i]=par[i]-matvec(1,m,i,jtjinv,bb);
fe++;
if (fe >= maxfe)
err=1;
else
if (!peidefunct(nobs,m,parpres,res,
n,m,nobs,nbp,first,&sec,&max,&nis,eps1,
weight,bp,save,ymax,y,yp,fy,fp,cobs,tobs,
obs,in,aux,clean,deriv,jacdfdy,jacdfdp,
callystart,monitor))
err=2;
if (err != 0) {
emergency=1;
break;
}
fparpres=vecvec(1,nobs,0,res,res);
res2=fpar-fparpres;
if (res2 < mu*vecvec(1,m,0,b,bb)) {
p += 1.0;
lambda *= vv;
if (p == 1.0) {
lambdamin=ww*vecvec(1,m,0,val,val);
if (lambda < lambdamin) lambda=lambdamin;
}
if (p >= pw) {
err=4;
emergency=1;
break;
}
} else {
dupvec(1,m,0,par,parpres);
fpar=fparpres;
break;
}
}
if (emergency) break;
it++;
} while (fpar>abstolres && res2>reltolres*fpar+abstolres);
for (i=1; i<=m; i++)
mulcol(1,m,i,i,jaco,jtjinv,1.0/(val[i]+in[0]));
for (i=1; i<=m; i++)
for (j=1; j<=i; j++)
jtjinv[i][j]=jtjinv[j][i]=mattam(1,m,i,j,jaco,jaco);
lambda=lambdamin=val[1];
for (i=2; i<=m; i++)
if (val[i] > lambda)
lambda=val[i];
else
if (val[i] < lambdamin) lambdamin=val[i];
temp=lambda/(lambdamin+in[0]);
out[7]=temp*temp;
out[2]=sqrt(fpar);
out[6]=sqrt(res2+fpar)-out[2];
}
out[4]=fe;
out[5]=it-1;
out[1]=err;
free_real_vector(val,1);
free_real_vector(b,1);
free_real_vector(bb,1);
free_real_vector(parpres,1);
free_real_matrix(jaco,1,nobs,1);
nfe += out[4];
Escape:
if (out[1] == 3.0)
out[1]=2.0;
else
if (out[1] == 4.0) out[1]=6.0;
if (save[-3] != 0.0) out[1]=save[-3];
out[3]=res1;
out[4]=nfe;
out[5]=max;
free_integer_vector(cobs,1);
free_real_vector(obs,1);
free_real_vector(save,-38);
free_real_vector(tobs,0);
free_real_vector(ymax,1);
free_real_vector(y,1);
free_real_matrix(yp,1,nbpold+nobs,1);
free_real_matrix(fy,1,n,1);
free_real_matrix(fp,1,n,1);
free_real_matrix(aid,1,m+nbpold,1);
}
void efsirk(real_t *x, real_t xe, int m, real_t y[],
real_t *delta, void (*derivative)(int, real_t[], real_t *),
void (*jacobian)(int, real_t **, real_t [], real_t *),
real_t **j, int *n, real_t aeta, real_t reta, real_t hmin,
real_t hmax, int linear,
void (*output)(real_t, real_t, int, real_t [],
real_t, real_t **, int))
{
int *allocate_integer_vector(int, int);
real_t *allocate_real_vector(int, int);
real_t **allocate_real_matrix(int, int, int, int);
void free_integer_vector(int *, int);
void free_real_vector(real_t *, int);
void free_real_matrix(real_t **, int, int, int);
real_t vecvec(int, int, int, real_t [], real_t []);
real_t matmat(int, int, int, int, real_t **, real_t **);
real_t matvec(int, int, int, real_t **, real_t []);
void gsselm(real_t **, int, real_t [], int [], int []);
void solelm(real_t **, int, int [], int [], real_t []);
int k,l,lin,*ri,*ci;
real_t step,h,mu0,mu1,mu2,theta0,theta1,nu1,nu2,nu3,yk,fk,c1,c2,
d,*f,*k0,*labda,**j1,aux[8],discr,eta,s,z1,z2,e,alpha1,a,b;
ri=allocate_integer_vector(1,m);
ci=allocate_integer_vector(1,m);
f=allocate_real_vector(1,m);
k0=allocate_real_vector(1,m);
labda=allocate_real_vector(1,m);
j1=allocate_real_matrix(1,m,1,m);
aux[2]=FLT_EPSILON;
aux[4]=8.0;
for (k=1; k<=m; k++) f[k]=y[k];
*n = 0;
(*output)(*x,xe,m,y,*delta,j,*n);
step=0.0;
do {
(*n)++;
/* difference scheme */
(*derivative)(m,f,delta);
/* step size */
if (linear)
s=h=hmax;
else
if (*n == 1 || hmin == hmax)
s=h=hmin;
else {
eta=aeta+reta*sqrt(vecvec(1,m,0,y,y));
c1=nu3*step;
for (k=1; k<=m; k++) labda[k] += c1*f[k]-y[k];
discr=sqrt(vecvec(1,m,0,labda,labda));
s=h=(eta/(0.75*(eta+discr))+0.33)*h;
if (h < hmin)
s=h=hmin;
else
if (h > hmax) s=h=hmax;
}
if ((*x)+s > xe) s=xe-(*x);
lin=((step == s) && linear);
step=s;
if (!linear || *n == 1) (*jacobian)(m,j,y,delta);
if (!lin) {
/* coefficient */
z1=step*(*delta);
if (*n == 1) z2=z1+z1;
if (fabs(z2-z1) > 1.0e-6*fabs(z1) || z2 > -1.0) {
a=z1*z1+12.0;
b=6.0*z1;
if (fabs(z1) < 0.1)
alpha1=(z1*z1/140.0-1.0)*z1/30.0;
else if (z1 < 1.0e-14)
alpha1=1.0/3.0;
else if (z1 < -33.0)
alpha1=(a+b)/(3.0*z1*(2.0+z1));
else {
e=((z1 < 230.0) ? exp(z1) : FLT_MAX);
alpha1=((a-b)*e-a-b)/(((2.0-z1)*e-2.0-z1)*3.0*z1);
}
mu2=(1.0/3.0+alpha1)*0.25;
mu1 = -(1.0+alpha1)*0.5;
mu0=(6.0*mu1+2.0)/9.0;
theta0=0.25;
theta1=0.75;
a=3.0*alpha1;
nu3=(1.0+a)/(5.0-a)*0.5;
a=nu3+nu3;
nu1=0.5-a;
nu2=(1.0+a)*0.75;
z2=z1;
}
c1=step*mu1;
d=step*step*mu2;
for (k=1; k<=m; k++) {
for (l=1; l<=m; l++)
j1[k][l]=d*matmat(1,m,k,l,j,j)+c1*j[k][l];
j1[k][k] += 1.0;
}
gsselm(j1,m,aux,ri,ci);
}
c1=step*step*mu0;
d=step*2.0/3.0;
for (k=1; k<=m; k++) {
k0[k]=fk=f[k];
labda[k]=d*fk+c1*matvec(1,m,k,j,f);
}
solelm(j1,m,ri,ci,labda);
for (k=1; k<=m; k++) f[k]=y[k]+labda[k];
(*derivative)(m,f,delta);
c1=theta0*step;
c2=theta1*step;
d=nu1*step;
for (k=1; k<=m; k++) {
yk=y[k];
fk=f[k];
labda[k]=yk+d*fk+nu2*labda[k];
y[k]=f[k]=yk+c1*k0[k]+c2*fk;
}
(*x) += step;
(*output)(*x,xe,m,y,*delta,j,*n);
} while (*x < xe);
free_integer_vector(ri,1);
free_integer_vector(ci,1);
free_real_vector(f,1);
free_real_vector(k0,1);
free_real_vector(labda,1);
free_real_matrix(j1,1,m,1);
}
int constrained_least_squares(int m, int n, double **matrix, int c, int *constrained)
{
//check problem dimensions
if(m < 1 || n < 1 || n > m || c > n) return LS_DIMENSION_ERROR;
//counters
int i, j;
//extra problem dimensions
int f = m - c, u = n - c;
//lapack and blas inputs
char transa, transb;
double alpha, beta;
//lapack output
int info;
//lapack workspace
int lwork = m*m;
double *work; if(!allocate_double_vector(&work, lwork)) { return LS_MEMORY_ERROR; }
//lapack LU pivot indices
int *ipiv; if(!allocate_integer_vector(&ipiv,c)) { return LS_MEMORY_ERROR; }
//lapack coefficients of QR elementary reflectors
double *tau; if(!allocate_double_vector(&tau,c)) { return LS_MEMORY_ERROR; }
//matrices used
double **t_matrix; if(!allocate_double_matrix(&t_matrix, m, m)) { return LS_MEMORY_ERROR; }
double **c_matrix; if(!allocate_double_matrix(&c_matrix, n, n)) { return LS_MEMORY_ERROR; }
double **r_matrix; if(!allocate_double_matrix(&r_matrix, c, c)) { return LS_MEMORY_ERROR; }
double **a_matrix; if(!allocate_double_matrix(&a_matrix, n, f)) { return LS_MEMORY_ERROR; }
double **d_matrix; if(!allocate_double_matrix(&d_matrix, f, f)) { return LS_MEMORY_ERROR; }
//indices of unconstrained equations
int *temp, *unconstrained;
if(!allocate_integer_vector(&temp,m)) { return LS_MEMORY_ERROR; }
if(!allocate_integer_vector(&unconstrained,f)) { return LS_MEMORY_ERROR; }
//create vector of unconstrained indices
for(i = 0; i < m; i ++) temp[i] = 0;
for(i = 0; i < c; i ++) temp[constrained[i]] = 1;
j = 0;
for(i = 0; i < m; i ++) if(!temp[i]) unconstrained[j++] = i;
//copy unconstrained equations from input matrix -> t_matrix
for(i = 0; i < f; i ++) for(j = 0; j < n; j ++) t_matrix[i][j] = matrix[j][unconstrained[i]];
//copy constrained equations from input matrix -> c_matrix
for(i = 0; i < c; i ++) for(j = 0; j < n; j ++) c_matrix[i][j] = matrix[j][constrained[i]];
//QR decomposition of the transposed constrained equations -> c_matrix
dgeqrf_(&n, &c, c_matrix[0], &n, tau, work, &lwork, &info);
//copy R out of the above QR decomposition -> r_matrix
for(i = 0; i < c; i ++) for(j = 0; j < c; j ++) r_matrix[i][j] = ((j >= i) ? c_matrix[j][i] : 0);
//form the square matrix Q from the above QR decomposition -> c_matrix'
dorgqr_(&n, &n, &c, c_matrix[0], &n, tau, work, &lwork, &info);
//multiply unconstrained eqations by Q -> a_matrix'
transa = 'T'; transb = 'N'; alpha = 1.0; beta = 0.0;
dgemm_(&transa, &transb, &f, &n, &n, &alpha, t_matrix[0], &m, c_matrix[0], &n, &beta, a_matrix[0], &f);
//invert R' of the above QR decomposition -> r_matrix
dgetrf_(&c, &c, r_matrix[0], &c, ipiv, &info);
dgetri_(&c, r_matrix[0], &c, ipiv, work, &lwork, &info);
//LS inversion of the non-square parts from unconstrained * Q -> d_matrix'
for(i = 0; i < f; i ++) for(j = 0; j < u; j ++) t_matrix[j][i] = a_matrix[j+c][i];
for(i = 0; i < f; i ++) for(j = 0; j < f; j ++) d_matrix[i][j] = (i == j);
transa = 'N';
dgels_(&transa, &f, &u, &f, t_matrix[0], &m, d_matrix[0], &f, work, &lwork, &info);
//multiply matrices together to form the CLS solution -> t_matrix'
transa = transb = 'N'; alpha = 1.0; beta = 0.0;
dgemm_(&transa, &transb, &n, &f, &u, &alpha, c_matrix[c], &n, d_matrix[0], &f, &beta, t_matrix[0], &m);
alpha = -1.0; beta = 1.0;
dgemm_(&transa, &transb, &n, &c, &f, &alpha, t_matrix[0], &m, a_matrix[0], &f, &beta, c_matrix[0], &n);
alpha = 1.0; beta = 0.0;
dgemm_(&transa, &transb, &n, &c, &c, &alpha, c_matrix[0], &n, r_matrix[0], &c, &beta, t_matrix[f], &m);
//copy the result out of the temporary matrix -> matrix
for(i = 0; i < n; i ++) for(j = 0; j < f; j ++) matrix[i][unconstrained[j]] = t_matrix[j][i];
for(i = 0; i < n; i ++) for(j = 0; j < c; j ++) matrix[i][constrained[j]] = t_matrix[j+f][i];
//clean up and return successful
free_vector(work);
free_vector(ipiv);
free_vector(tau);
free_vector(temp);
free_vector(unconstrained);
free_matrix((void **)t_matrix);
free_matrix((void **)c_matrix);
free_matrix((void **)r_matrix);
free_matrix((void **)a_matrix);
free_matrix((void **)d_matrix);
return LS_SUCCESS;
}
void calculate_cell_reconstruction_matrices(int n_variables, double *weight_exponent, int *maximum_order, struct FACE *face, int n_cells, struct CELL *cell, struct ZONE *zone)
{
int c, u, i, j, k, l;
int order, n_powers, n_stencil;
//find the overall maximum order
int maximum_maximum_order = 0;
for(u = 0; u < n_variables; u ++) if(maximum_order[u] > maximum_maximum_order) maximum_maximum_order = maximum_order[u];
//cell structure allocation
for(c = 0; c < n_cells; c ++) exit_if_false(cell_matrix_new(n_variables, &cell[c]),"allocating cell matrices");
//numerics values
double **matrix, *weight;
int n_constraints, *constraint;
exit_if_false(allocate_double_matrix(&matrix,ORDER_TO_POWERS(maximum_maximum_order),MAX_STENCIL),"allocating matrix");
exit_if_false(allocate_integer_vector(&constraint,MAX_STENCIL),"allocating constraints");
exit_if_false(allocate_double_vector(&weight,MAX_STENCIL),"allocating weights");
//stencil element properties
int s_id, s_index;
struct ZONE *s_zone;
char s_location, *s_condition;
double s_area, *s_centroid, s_weight;
//integration
double x[2];
int differential[2], d;
//CV polygon
int n_polygon;
double ***polygon;
exit_if_false(allocate_double_pointer_matrix(&polygon,MAX(MAX_CELL_FACES,4),2),"allocating polygon memory");
for(c = 0; c < n_cells; c ++)
{
for(u = 0; u < n_variables; u ++)
{
//problem size
order = cell[c].order[u];
n_powers = ORDER_TO_POWERS(order);
n_stencil = cell[c].n_stencil[u];
n_constraints = 0;
for(i = 0; i < n_stencil; i ++)
{
//stencil element properties
s_id = cell[c].stencil[u][i];
s_index = ID_TO_INDEX(s_id);
s_zone = &zone[ID_TO_ZONE(s_id)];
s_location = s_zone->location;
s_condition = s_zone->condition;
if(s_location == 'f') {
s_centroid = face[s_index].centroid;
s_area = face[s_index].area;
} else if(s_location == 'c') {
s_centroid = cell[s_index].centroid;
s_area = cell[s_index].area;
} else exit_if_false(0,"recognising zone location");
s_weight = (s_centroid[0] - cell[c].centroid[0])*(s_centroid[0] - cell[c].centroid[0]);
s_weight += (s_centroid[1] - cell[c].centroid[1])*(s_centroid[1] - cell[c].centroid[1]);
s_weight = 1.0/pow(s_weight,0.5*weight_exponent[u]);
if(s_location == 'c' && s_index == c) s_weight = 1.0;
weight[i] = s_weight;
//unknown and dirichlet conditions have zero differentiation
differential[0] = differential[1] = 0;
//other conditions have differential determined from numbers of x and y-s in the condition string
if(s_condition[0] != 'u' && s_condition[0] != 'd')
{
j = 0;
while(s_condition[j] != '\0')
{
differential[0] += (s_condition[j] == 'x');
differential[1] += (s_condition[j] == 'y');
j ++;
}
}
//index for the determined differential
d = differential_index[differential[0]][differential[1]];
//unknowns
if(s_condition[0] == 'u')
{
/*//fit unknowns to centroid points
x[0] = s_centroid[0] - cell[c].centroid[0];
x[1] = s_centroid[1] - cell[c].centroid[1];
for(j = 0; j < n_powers; j ++)
{
matrix[j][i] = polynomial_coefficient[d][j]*
integer_power(x[0],polynomial_power_x[d][j])*
integer_power(x[1],polynomial_power_y[d][j])*
s_weight;
}*/
//fit unknowns to CV average
n_polygon = generate_control_volume_polygon(polygon, s_index, s_location, face, cell);
for(j = 0; j < n_powers; j ++) matrix[j][i] = 0.0;
for(j = 0; j <= order; j ++)
{
for(k = 0; k < n_polygon; k ++)
{
x[0] = 0.5*polygon[k][0][0]*(1.0 - gauss_x[order][j]) +
0.5*polygon[k][1][0]*(1.0 + gauss_x[order][j]) -
cell[c].centroid[0];
x[1] = 0.5*polygon[k][0][1]*(1.0 - gauss_x[order][j]) +
0.5*polygon[k][1][1]*(1.0 + gauss_x[order][j]) -
cell[c].centroid[1];
for(l = 0; l < n_powers; l ++)
{
//[face integral of polynomial integrated wrt x] * [x normal] / [CV area]
matrix[l][i] += polynomial_coefficient[d][l] *
(1.0 / (polynomial_power_x[d][l] + 1.0)) *
integer_power(x[0],polynomial_power_x[d][l]+1) *
integer_power(x[1],polynomial_power_y[d][l]) *
s_weight * gauss_w[order][j] * 0.5 *
(polygon[k][1][1] - polygon[k][0][1]) / s_area;
}
}
}
}
//knowns
else
{
//known faces fit to face average
if(s_location == 'f')
{
for(j = 0; j < n_powers; j ++) matrix[j][i] = 0.0;
for(j = 0; j < order; j ++)
{
x[0] = 0.5*face[s_index].node[0]->x[0]*(1.0 - gauss_x[order-1][j]) +
0.5*face[s_index].node[1]->x[0]*(1.0 + gauss_x[order-1][j]) -
cell[c].centroid[0];
x[1] = 0.5*face[s_index].node[0]->x[1]*(1.0 - gauss_x[order-1][j]) +
0.5*face[s_index].node[1]->x[1]*(1.0 + gauss_x[order-1][j]) -
cell[c].centroid[1];
for(k = 0; k < n_powers; k ++)
{
matrix[k][i] += polynomial_coefficient[d][k] *
integer_power(x[0],polynomial_power_x[d][k]) *
integer_power(x[1],polynomial_power_y[d][k]) *
s_weight*gauss_w[order-1][j]*0.5;
}
}
}
//cells need implementing
//if(s_location == 'c')
//{
//}
}
//constraints are the centre cell and any dirichlet boundaries
if((s_location == 'c' && s_index == c) || s_condition[0] == 'd') constraint[n_constraints++] = i;
}
//solve
if(n_constraints > 0)
exit_if_false(constrained_least_squares(n_stencil,n_powers,matrix,n_constraints,constraint) == LS_SUCCESS, "doing CLS calculation");
else
exit_if_false(least_squares(n_stencil,n_powers,matrix) == LS_SUCCESS,"doing LS calculation");
//multiply by the weights
for(i = 0; i < n_powers; i ++) for(j = 0; j < n_stencil; j ++) matrix[i][j] *= weight[j];
//store in the cell structure
for(i = 0; i < n_powers; i ++) for(j = 0; j < n_stencil; j ++) cell[c].matrix[u][i][j] = matrix[i][j];
}
}
//clean up
free_matrix((void**)matrix);
free_vector(constraint);
free_vector(weight);
free_matrix((void**)polygon);
}
