示例#1
0
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);
}
示例#2
0
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);
}
示例#3
0
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);
}
示例#4
0
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);
}
示例#5
0
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);
}
示例#6
0
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);
}
示例#7
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);
}
示例#8
0
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);
}
示例#9
0
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;
}
示例#10
0
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);
}