[u/mrichter/AliRoot.git] / HLT / comp / AliL3FitUtilities.c


#include "AliL3FitUtilities.h"
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <setjmp.h>


jmp_buf env;

DPOINT *plane;

#ifndef PB
int build_exp_table()
/******************************************************************************/
{
	exp_header_t tab_h;
	int i;
	FLOAT_SIZE *exp_val;
	FILE  *fp;
	
	if( !(fp = fopen( "exp_table", "wb" )))
	  printf("build_exp_table: I/O error\n");
		
	tab_h.float_size = sizeof( FLOAT_SIZE );
	tab_h.steps     = 100000;
	tab_h.dmin      = -5;
	tab_h.dmax      = 1;
	tab_h.step_size = (tab_h.dmax - tab_h.dmin)/tab_h.steps;
	if( !(exp_val = (FLOAT_SIZE *)malloc( tab_h.steps * sizeof( FLOAT_SIZE ) ) ) )
	  printf("build_exp_table: malloc error\n");
	fwrite((const void *)&tab_h, (size_t)sizeof(exp_header_t), (size_t)1, fp );
	for( i=0; i<tab_h.steps; ++i ) {
		exp_val[i] = exp(tab_h.dmin + i*tab_h.step_size);
	}
	fwrite((const void *)exp_val, (size_t)sizeof(FLOAT_SIZE), (size_t)tab_h.steps, fp );
	
	free( (void *)exp_val );
	return 0;
}

#ifdef TEST_TABLE
test_table()
/**********************************************************/
{
	double i, delta, dmin, dmax, exp_tab();
	FILE   *fp;
	
	fp = fopen( "exp_test", "w" );
	
	delta = 6.0/10000.;
	dmin  = -5.1;
	dmax  =  1.1;
	for( i=dmin; i<dmax; i+=delta ){
		fprintf( fp, "%lf\t%lf\t%lf\n", i, exp(i), exp_tab(i) );
	}
	return 0;
}
#endif
double exp_tab(double in)
{
	return exp( in );
}
#else
double exp_tab(in)
/**********************************************************/
double in;
{
	static exp_header_t tab_h;
	static FLOAT_SIZE   *exp_val=NULL, slope;
	FILE                *fp=NULL;
	
#ifdef HP
	if( isnan(in) ) {
		if( (num_nan /100)*100 == num_nan )
			fprintf( stderr, "exp_tab: NaN %d\n", num_nan );
		++num_nan;
		return 1;
	}
#endif
#ifdef MAC
		return exp( in );
#else
	if( !exp_val ) {
		if( !(fp = fopen( "exp_table", "rb" ))) {
			build_exp_table();
		}
		if( !(fp = fopen( "exp_table", "rb" )))
			printf("exp_tab: I/O error\n");
		fread(&tab_h, (size_t)sizeof(exp_header_t), (size_t)1, fp );
		if( tab_h.float_size != sizeof( FLOAT_SIZE ) )
			build_exp_table();

		if( !(exp_val = (FLOAT_SIZE *)malloc( tab_h.steps * sizeof( FLOAT_SIZE ) ) ) )
			printf("exp_tab: malloc error\n");
		fread(exp_val, (size_t)sizeof(FLOAT_SIZE), (size_t)tab_h.steps, fp );
		slope = tab_h.steps / (tab_h.dmax - tab_h.dmin);
	}
	if( in < tab_h.dmin || in > tab_h.dmax )
		return exp( in );
	else
		return exp_val[(int)((in-tab_h.dmin) * slope)];
#endif
}
#endif

void f2gauss5(double x,double a[],double *y,double dyda[],int na)
{
  int i,index;
  double fac,fac1,fac2,ex,ex1,ex2,arg1,arg2,u,v;
  
	
	index = nint(x);
	if( index < 0 || index >=FIT_PTS )
	{
		fprintf( stderr, "ff2gauss: wrong index %ld\n", index );
		return;
	}
	u     = plane[index].u;
	v     = plane[index].v;
	
	*y=0.0;
	for (i=1;i<=na-1;i+=5)
	{
		arg1      = (u-a[i+1])/a[i+2];
		arg2      = (v-a[i+3])/a[i+4];
		ex1       = exp_tab(-arg1*arg1);
		ex2       = exp_tab(-arg2*arg2);
		ex        = ex1*ex2;
		fac       = a[i]*ex*2.0;
		fac1      = fac * arg1;
		fac2      = fac * arg2;
		*y       += a[i] * ex;
		dyda[i]   = ex;
		dyda[i+1] = fac1/a[i+2];
		dyda[i+2] = fac1*arg1/a[i+2];
		dyda[i+3] = fac2/a[i+4];
		dyda[i+4] = fac2*arg2/a[i+4];
	}
}

void nrerror(char error_text[])
/* Numerical Recipes standard error handler */
{
  printf("%s\n",error_text);
  exit(1);
}

void free_vector(double *v, long nl, long nh)
/* free a double vector allocated with vector() */
{
	free((FREE_ARG) (v+nl-NR_END));
}

void free_ivector(int *v, long nl, long nh)
/* free an int vector allocated with ivector() */
{
	free((FREE_ARG) (v+nl-NR_END));
}

void free_matrix(double **m, long nrl, long nrh, long ncl, long nch)
/* free a double matrix allocated by matrix() */
{
	free((FREE_ARG) (m[nrl]+ncl-NR_END));
	free((FREE_ARG) (m+nrl-NR_END));
}

int *ivector(long nl, long nh)
/* allocate an int vector with subscript range v[nl..nh] */
{
	int *v;

	v=(int *)malloc((size_t) ((nh-nl+1+NR_END)*sizeof(int)));
	if (!v) nrerror("allocation failure in ivector()");
	return v-nl+NR_END;
}

double *vector(long nl,long nh)
/* allocate a double vector with subscript range v[nl..nh] */
{
	double *v;

	v=(double *)malloc((unsigned int) ((nh-nl+1+NR_END)*sizeof(double)));
	if (!v) nrerror("allocation failure in vector()");
	return v-nl+NR_END;
}

double **matrix(long nrl,long nrh,long ncl,long nch)
/* allocate a double matrix with subscript range m[nrl..nrh][ncl..nch] */
{
	long i, nrow=nrh-nrl+1,ncol=nch-ncl+1;
	double **m;

	/* allocate pointers to rows */
	m=(double **) malloc((unsigned int)((nrow+NR_END)*sizeof(double*)));
	if (!m) nrerror("allocation failure 1 in matrix()");
	m += NR_END;
	m -= nrl;

	/* allocate rows and set pointers to them */
	m[nrl]=(double *) malloc((unsigned int)((nrow*ncol+NR_END)*sizeof(double)));
	if (!m[nrl]) nrerror("allocation failure 2 in matrix()");
	m[nrl] += NR_END;
	m[nrl] -= ncl;

	for(i=nrl+1;i<=nrh;i++) m[i]=m[i-1]+ncol;

	/* return pointer to array of pointers to rows */
	return m;
}

void gaussj(double **a, int n, double **b, int m)
{
	int *indxc,*indxr,*ipiv;
	int i,icol,irow,j,k,l,ll;
	double big,dum,pivinv,swap;

	indxc=ivector(1,n);
	indxr=ivector(1,n);
	ipiv=ivector(1,n);
	for (j=1;j<=n;j++) ipiv[j]=0;
	for (i=1;i<=n;i++) {
		big=0.0;
		for (j=1;j<=n;j++)
			if (ipiv[j] != 1)
				for (k=1;k<=n;k++) {
					if (ipiv[k] == 0) {
						if (fabs(a[j][k]) >= big) {
							big=fabs(a[j][k]);
							irow=j;
							icol=k;
						}
					} else if (ipiv[k] > 1) {
						free_ivector(ipiv,1,n);
						free_ivector(indxr,1,n);
						free_ivector(indxc,1,n);
						nrerror("gaussj: Singular Matrix-1");
					}
				}
		++(ipiv[icol]);
		if (irow != icol) {
			for (l=1;l<=n;l++) SWAP(a[irow][l],a[icol][l])
			for (l=1;l<=m;l++) SWAP(b[irow][l],b[icol][l])
		}
		indxr[i]=irow;
		indxc[i]=icol;
		if (a[icol][icol] == 0.0){
			free_ivector(ipiv,1,n);
			free_ivector(indxr,1,n);
			free_ivector(indxc,1,n);
			nrerror("gaussj: Singular Matrix-2");
		}
		if (mabs(a[icol][icol]) < EPSILON ) 
			nrerror("gaussj: a[icol][icol] == 0");
		pivinv=1.0/a[icol][icol];

		pivinv=1.0/a[icol][icol];
		a[icol][icol]=1.0;
		for (l=1;l<=n;l++) a[icol][l] *= pivinv;
		for (l=1;l<=m;l++) b[icol][l] *= pivinv;
		for (ll=1;ll<=n;ll++)
			if (ll != icol) {
				dum=a[ll][icol];
				a[ll][icol]=0.0;
				for (l=1;l<=n;l++) a[ll][l] -= a[icol][l]*dum;
				for (l=1;l<=m;l++) b[ll][l] -= b[icol][l]*dum;
			}
	}
	for (l=n;l>=1;l--) {
		if (indxr[l] != indxc[l])
			for (k=1;k<=n;k++)
				SWAP(a[k][indxr[l]],a[k][indxc[l]]);
	}
	free_ivector(ipiv,1,n);
	free_ivector(indxr,1,n);
	free_ivector(indxc,1,n);
}

void covsrt(double **covar, int ma, int ia[], int mfit)
{
	int i,j,k;
	double swap;

	for (i=mfit+1;i<=ma;i++)
		for (j=1;j<=i;j++) covar[i][j]=covar[j][i]=0.0;
	k=mfit;
	for (j=ma;j>=1;j--) {
		if (ia[j]) {
			for (i=1;i<=ma;i++) SWAP(covar[i][k],covar[i][j])
			for (i=1;i<=ma;i++) SWAP(covar[k][i],covar[j][i])
			k--;
		}
	}
}

void mrqcof(double x[], double y[], double sig[], int ndata, double a[], int ia[],
	int ma, double **alpha, double beta[], double *chisq,
	void (*funcs)(double, double [], double *, double [], int))
{
	int i,j,k,l,m,mfit=0;
	double ymod,wt,sig2i,dy,*dyda;

	dyda=vector(1,ma);

	for (j=1;j<=ma;j++)
		if (ia[j]) mfit++;
	for (j=1;j<=mfit;j++) {
		for (k=1;k<=j;k++) alpha[j][k]=0.0;
		beta[j]=0.0;
	}
	*chisq=0.0;
	for (i=1;i<=ndata;i++) {
		(*funcs)(x[i],a,&ymod,dyda,ma);
		sig2i=1.0/(sig[i]*sig[i]);
		dy=y[i]-ymod;
		for (j=0,l=1;l<=ma;l++) {
			if (ia[l]) {
				wt=dyda[l]*sig2i;
				for (j++,k=0,m=1;m<=l;m++)
					if (ia[m]) alpha[j][++k] += wt*dyda[m];
				beta[j] += dy*wt;
			}
		}
		*chisq += dy*dy*sig2i;
	}
	for (j=2;j<=mfit;j++)
		for (k=1;k<j;k++) alpha[k][j]=alpha[j][k];
	free_vector(dyda,1,ma);
}

void mrqmin(double x[], double y[], double sig[], int ndata, double a[], int ia[],
	int ma, double **covar, double **alpha, double *chisq,
	void (*funcs)(double, double [], double *, double [], int), double *alamda)
{
	void covsrt(double **covar, int ma, int ia[], int mfit);
	void gaussj(double **a, int n, double **b, int m);
	void mrqcof(double x[], double y[], double sig[], int ndata, double a[],
		int ia[], int ma, double **alpha, double beta[], double *chisq,
		void (*funcs)(double, double [], double *, double [], int));
	int j,k,l,m;
	static int mfit;
	static double ochisq,*atry,*beta,*da,**oneda;

	if (*alamda < 0.0) {
		atry=vector(1,ma);
		beta=vector(1,ma);
		da=vector(1,ma);
		for (mfit=0,j=1;j<=ma;j++)
			if (ia[j]) mfit++;
		oneda=matrix(1,mfit,1,1);
		*alamda=0.001;
		mrqcof(x,y,sig,ndata,a,ia,ma,alpha,beta,chisq,funcs);
		ochisq=(*chisq);
		for (j=1;j<=ma;j++) atry[j]=a[j];
	}
	for (j=0,l=1;l<=ma;l++) {
		if (ia[l]) {
			for (j++,k=0,m=1;m<=ma;m++) {
				if (ia[m]) {
					k++;
					covar[j][k]=alpha[j][k];
				}
			}
			covar[j][j]=alpha[j][j]*(1.0+(*alamda));
			oneda[j][1]=beta[j];
		}
	}
	gaussj(covar,mfit,oneda,1);
	for (j=1;j<=mfit;j++) da[j]=oneda[j][1];
	if (*alamda == 0.0) {
		covsrt(covar,ma,ia,mfit);
		free_matrix(oneda,1,mfit,1,1);
		free_vector(da,1,ma);
		free_vector(beta,1,ma);
		free_vector(atry,1,ma);
		return;
	}
	for (j=0,l=1;l<=ma;l++)
		if (ia[l]) atry[l]=a[l]+da[++j];
	mrqcof(x,y,sig,ndata,atry,ia,ma,covar,da,chisq,funcs);
	if (*chisq < ochisq) {
		*alamda *= 0.1;
		ochisq=(*chisq);
		for (j=0,l=1;l<=ma;l++) {
			if (ia[l]) {
				for (j++,k=0,m=1;m<=ma;m++) {
					if (ia[m]) {
						k++;
						alpha[j][k]=covar[j][k];
					}
				}
				beta[j]=da[j];
				a[l]=atry[l];
			}
		}
	} else {
		*alamda *= 10.0;
		*chisq=ochisq;
	}
}


int lev_marq_fit( double x[], double y[], double sig[], int NPT, double a[], int ia[], double dev[], int MA,
                  double *chisq_p, void (*funcs)(double, double [], double *, double [], int) )
{
	int     i,itst,k;
	double   alamda,chisq,ochisq,**covar,**alpha;
	
	covar=matrix(1,MA,1,MA);
	alpha=matrix(1,MA,1,MA);

	if( setjmp(env) == 0 ) {	
		alamda = -1;
		mrqmin(x,y,sig,NPT,a,ia,MA,covar,alpha,&chisq,funcs,&alamda);
		k=1;
		itst=0;
		for (;;) {
			k++;
			ochisq=chisq;
			mrqmin(x,y,sig,NPT,a,ia,MA,covar,alpha,&chisq,funcs,&alamda);
			if (chisq > ochisq)
				itst=0;
			else if (fabs(ochisq-chisq) < 0.1)
				itst++;
			if (itst < 4) continue;
			alamda=0.0;
			mrqmin(x,y,sig,NPT,a,ia,MA,covar,alpha,&chisq,funcs,&alamda);
			*chisq_p = chisq;
			for (i=1;i<=MA;i++) 
				dev[i] = sqrt(covar[i][i]);
			break;
		}
		free_matrix(alpha,1,MA,1,MA);
		free_matrix(covar,1,MA,1,MA);
		return 0;
	}
	else {
	  //if( control_g.print_fit_errors==2 )
	  fprintf( stderr, " runtime error\n" );

		free_matrix(alpha,1,MA,1,MA);
		free_matrix(covar,1,MA,1,MA);
		return -1;
	}
}
Commit	Line	Data
6b8f6f6e	1
	2	#include "AliL3FitUtilities.h"
	3	#include <stdio.h>
	4	#include <stdlib.h>
	5	#include <math.h>
	6	#include <setjmp.h>
	7
	8
	9	jmp_buf env;
	10
	11	DPOINT *plane;
	12
	13	#ifndef PB
	14	int build_exp_table()
	15	/******************************************************************************/
	16	{
	17	exp_header_t tab_h;
	18	int i;
	19	FLOAT_SIZE *exp_val;
	20	FILE *fp;
	21
	22	if( !(fp = fopen( "exp_table", "wb" )))
	23	printf("build_exp_table: I/O error\n");
	24
	25	tab_h.float_size = sizeof( FLOAT_SIZE );
	26	tab_h.steps = 100000;
	27	tab_h.dmin = -5;
	28	tab_h.dmax = 1;
	29	tab_h.step_size = (tab_h.dmax - tab_h.dmin)/tab_h.steps;
	30	if( !(exp_val = (FLOAT_SIZE )malloc( tab_h.steps sizeof( FLOAT_SIZE ) ) ) )
	31	printf("build_exp_table: malloc error\n");
	32	fwrite((const void *)&tab_h, (size_t)sizeof(exp_header_t), (size_t)1, fp );
	33	for( i=0; i<tab_h.steps; ++i ) {
	34	exp_val[i] = exp(tab_h.dmin + i*tab_h.step_size);
	35	}
	36	fwrite((const void *)exp_val, (size_t)sizeof(FLOAT_SIZE), (size_t)tab_h.steps, fp );
	37
	38	free( (void *)exp_val );
	39	return 0;
	40	}
	41
	42	#ifdef TEST_TABLE
	43	test_table()
	44	/**********************************************************/
	45	{
	46	double i, delta, dmin, dmax, exp_tab();
	47	FILE *fp;
	48
	49	fp = fopen( "exp_test", "w" );
	50
	51	delta = 6.0/10000.;
	52	dmin = -5.1;
	53	dmax = 1.1;
	54	for( i=dmin; i<dmax; i+=delta ){
	55	fprintf( fp, "%lf\t%lf\t%lf\n", i, exp(i), exp_tab(i) );
	56	}
	57	return 0;
	58	}
	59	#endif
	60	double exp_tab(double in)
	61	{
	62	return exp( in );
	63	}
	64	#else
65	double exp_tab(in)
66	/**********************************************************/
67	double in;
68	{
69	static exp_header_t tab_h;
70	static FLOAT_SIZE *exp_val=NULL, slope;
71	FILE *fp=NULL;
72
73	#ifdef HP
74	if( isnan(in) ) {
75	if( (num_nan /100)*100 == num_nan )
76	fprintf( stderr, "exp_tab: NaN %d\n", num_nan );
77	++num_nan;
78	return 1;
79	}
80	#endif
81	#ifdef MAC
82	return exp( in );
83	#else
84	if( !exp_val ) {
85	if( !(fp = fopen( "exp_table", "rb" ))) {
86	build_exp_table();
87	}
88	if( !(fp = fopen( "exp_table", "rb" )))
89	printf("exp_tab: I/O error\n");
90	fread(&tab_h, (size_t)sizeof(exp_header_t), (size_t)1, fp );
91	if( tab_h.float_size != sizeof( FLOAT_SIZE ) )
92	build_exp_table();
93
94	if( !(exp_val = (FLOAT_SIZE )malloc( tab_h.steps sizeof( FLOAT_SIZE ) ) ) )
95	printf("exp_tab: malloc error\n");
96	fread(exp_val, (size_t)sizeof(FLOAT_SIZE), (size_t)tab_h.steps, fp );
97	slope = tab_h.steps / (tab_h.dmax - tab_h.dmin);
98	}
99	if( in < tab_h.dmin \|\| in > tab_h.dmax )
100	return exp( in );
101	else
102	return exp_val[(int)((in-tab_h.dmin) * slope)];
103	#endif
104	}
105	#endif
106
107	void f2gauss5(double x,double a[],double *y,double dyda[],int na)
108	{
109	int i,index;
110	double fac,fac1,fac2,ex,ex1,ex2,arg1,arg2,u,v;
111
112
113	index = nint(x);
114	if( index < 0 \|\| index >=FIT_PTS )
115	{
116	fprintf( stderr, "ff2gauss: wrong index %ld\n", index );
117	return;
118	}
119	u = plane[index].u;
120	v = plane[index].v;
121
122	*y=0.0;
123	for (i=1;i<=na-1;i+=5)
124	{
125	arg1 = (u-a[i+1])/a[i+2];
126	arg2 = (v-a[i+3])/a[i+4];
127	ex1 = exp_tab(-arg1*arg1);
128	ex2 = exp_tab(-arg2*arg2);
129	ex = ex1*ex2;
130	fac = a[i]ex2.0;
131	fac1 = fac * arg1;
132	fac2 = fac * arg2;
133	y += a[i] ex;
134	dyda[i] = ex;
135	dyda[i+1] = fac1/a[i+2];
136	dyda[i+2] = fac1*arg1/a[i+2];
137	dyda[i+3] = fac2/a[i+4];
138	dyda[i+4] = fac2*arg2/a[i+4];
139	}
140	}
141
142	void nrerror(char error_text[])
143	/* Numerical Recipes standard error handler */
144	{
145	printf("%s\n",error_text);
146	exit(1);
147	}
148
149	void free_vector(double *v, long nl, long nh)
150	/* free a double vector allocated with vector() */
151	{
152	free((FREE_ARG) (v+nl-NR_END));
153	}
154
155	void free_ivector(int *v, long nl, long nh)
156	/* free an int vector allocated with ivector() */
157	{
158	free((FREE_ARG) (v+nl-NR_END));
159	}
160
161	void free_matrix(double **m, long nrl, long nrh, long ncl, long nch)
162	/* free a double matrix allocated by matrix() */
163	{
164	free((FREE_ARG) (m[nrl]+ncl-NR_END));
165	free((FREE_ARG) (m+nrl-NR_END));
166	}
167
168	int *ivector(long nl, long nh)
169	/* allocate an int vector with subscript range v[nl..nh] */
170	{
171	int *v;
172
173	v=(int )malloc((size_t) ((nh-nl+1+NR_END)sizeof(int)));
174	if (!v) nrerror("allocation failure in ivector()");
175	return v-nl+NR_END;
176	}
177
178	double *vector(long nl,long nh)
179	/* allocate a double vector with subscript range v[nl..nh] */
180	{
181	double *v;
182
183	v=(double )malloc((unsigned int) ((nh-nl+1+NR_END)sizeof(double)));
184	if (!v) nrerror("allocation failure in vector()");
185	return v-nl+NR_END;
186	}
187
188	double **matrix(long nrl,long nrh,long ncl,long nch)
189	/* allocate a double matrix with subscript range m[nrl..nrh][ncl..nch] */
190	{
191	long i, nrow=nrh-nrl+1,ncol=nch-ncl+1;
192	double **m;
193
194	/* allocate pointers to rows */
195	m=(double *) malloc((unsigned int)((nrow+NR_END)sizeof(double*)));
196	if (!m) nrerror("allocation failure 1 in matrix()");
197	m += NR_END;
198	m -= nrl;
199
200	/* allocate rows and set pointers to them */
201	m[nrl]=(double ) malloc((unsigned int)((nrowncol+NR_END)*sizeof(double)));
202	if (!m[nrl]) nrerror("allocation failure 2 in matrix()");
203	m[nrl] += NR_END;
204	m[nrl] -= ncl;
205
206	for(i=nrl+1;i<=nrh;i++) m[i]=m[i-1]+ncol;
207
208	/* return pointer to array of pointers to rows */
209	return m;
210	}
211
212	void gaussj(double a, int n, double b, int m)
213	{
214	int indxc,indxr,*ipiv;
215	int i,icol,irow,j,k,l,ll;
216	double big,dum,pivinv,swap;
217
218	indxc=ivector(1,n);
219	indxr=ivector(1,n);
220	ipiv=ivector(1,n);
221	for (j=1;j<=n;j++) ipiv[j]=0;
222	for (i=1;i<=n;i++) {
223	big=0.0;
224	for (j=1;j<=n;j++)
225	if (ipiv[j] != 1)
226	for (k=1;k<=n;k++) {
227	if (ipiv[k] == 0) {
228	if (fabs(a[j][k]) >= big) {
229	big=fabs(a[j][k]);
230	irow=j;
231	icol=k;
232	}
233	} else if (ipiv[k] > 1) {
234	free_ivector(ipiv,1,n);
235	free_ivector(indxr,1,n);
236	free_ivector(indxc,1,n);
237	nrerror("gaussj: Singular Matrix-1");
238	}
239	}
240	++(ipiv[icol]);
241	if (irow != icol) {
242	for (l=1;l<=n;l++) SWAP(a[irow][l],a[icol][l])
243	for (l=1;l<=m;l++) SWAP(b[irow][l],b[icol][l])
244	}
245	indxr[i]=irow;
246	indxc[i]=icol;
247	if (a[icol][icol] == 0.0){
248	free_ivector(ipiv,1,n);
249	free_ivector(indxr,1,n);
250	free_ivector(indxc,1,n);
251	nrerror("gaussj: Singular Matrix-2");
252	}
253	if (mabs(a[icol][icol]) < EPSILON )
254	nrerror("gaussj: a[icol][icol] == 0");
255	pivinv=1.0/a[icol][icol];
256
257	pivinv=1.0/a[icol][icol];
258	a[icol][icol]=1.0;
259	for (l=1;l<=n;l++) a[icol][l] *= pivinv;
260	for (l=1;l<=m;l++) b[icol][l] *= pivinv;
261	for (ll=1;ll<=n;ll++)
262	if (ll != icol) {
263	dum=a[ll][icol];
264	a[ll][icol]=0.0;
265	for (l=1;l<=n;l++) a[ll][l] -= a[icol][l]*dum;
266	for (l=1;l<=m;l++) b[ll][l] -= b[icol][l]*dum;
267	}
268	}
269	for (l=n;l>=1;l--) {
270	if (indxr[l] != indxc[l])
271	for (k=1;k<=n;k++)
272	SWAP(a[k][indxr[l]],a[k][indxc[l]]);
273	}
274	free_ivector(ipiv,1,n);
275	free_ivector(indxr,1,n);
276	free_ivector(indxc,1,n);
277	}
278
279	void covsrt(double **covar, int ma, int ia[], int mfit)
280	{
281	int i,j,k;
282	double swap;
283
284	for (i=mfit+1;i<=ma;i++)
285	for (j=1;j<=i;j++) covar[i][j]=covar[j][i]=0.0;
286	k=mfit;
287	for (j=ma;j>=1;j--) {
288	if (ia[j]) {
289	for (i=1;i<=ma;i++) SWAP(covar[i][k],covar[i][j])
290	for (i=1;i<=ma;i++) SWAP(covar[k][i],covar[j][i])
291	k--;
292	}
293	}
294	}
295
296	void mrqcof(double x[], double y[], double sig[], int ndata, double a[], int ia[],
297	int ma, double *alpha, double beta[], double chisq,
298	void (funcs)(double, double [], double , double [], int))
299	{
300	int i,j,k,l,m,mfit=0;
301	double ymod,wt,sig2i,dy,*dyda;
302
303	dyda=vector(1,ma);
304
305	for (j=1;j<=ma;j++)
306	if (ia[j]) mfit++;
307	for (j=1;j<=mfit;j++) {
308	for (k=1;k<=j;k++) alpha[j][k]=0.0;
309	beta[j]=0.0;
310	}
311	*chisq=0.0;
312	for (i=1;i<=ndata;i++) {
313	(*funcs)(x[i],a,&ymod,dyda,ma);
314	sig2i=1.0/(sig[i]*sig[i]);
315	dy=y[i]-ymod;
316	for (j=0,l=1;l<=ma;l++) {
317	if (ia[l]) {
318	wt=dyda[l]*sig2i;
319	for (j++,k=0,m=1;m<=l;m++)
320	if (ia[m]) alpha[j][++k] += wt*dyda[m];
321	beta[j] += dy*wt;
322	}
323	}
324	chisq += dydy*sig2i;
325	}
326	for (j=2;j<=mfit;j++)
327	for (k=1;k<j;k++) alpha[k][j]=alpha[j][k];
328	free_vector(dyda,1,ma);
329	}
330
331	void mrqmin(double x[], double y[], double sig[], int ndata, double a[], int ia[],
332	int ma, double covar, double alpha, double *chisq,
333	void (funcs)(double, double [], double , double [], int), double *alamda)
334	{
335	void covsrt(double **covar, int ma, int ia[], int mfit);
336	void gaussj(double a, int n, double b, int m);
337	void mrqcof(double x[], double y[], double sig[], int ndata, double a[],
338	int ia[], int ma, double *alpha, double beta[], double chisq,
339	void (funcs)(double, double [], double , double [], int));
340	int j,k,l,m;
341	static int mfit;
342	static double ochisq,atry,beta,da,*oneda;
343
344	if (*alamda < 0.0) {
345	atry=vector(1,ma);
346	beta=vector(1,ma);
347	da=vector(1,ma);
348	for (mfit=0,j=1;j<=ma;j++)
349	if (ia[j]) mfit++;
350	oneda=matrix(1,mfit,1,1);
351	*alamda=0.001;
352	mrqcof(x,y,sig,ndata,a,ia,ma,alpha,beta,chisq,funcs);
353	ochisq=(*chisq);
354	for (j=1;j<=ma;j++) atry[j]=a[j];
355	}
356	for (j=0,l=1;l<=ma;l++) {
357	if (ia[l]) {
358	for (j++,k=0,m=1;m<=ma;m++) {
359	if (ia[m]) {
360	k++;
361	covar[j][k]=alpha[j][k];
362	}
363	}
364	covar[j][j]=alpha[j][j](1.0+(alamda));
365	oneda[j][1]=beta[j];
366	}
367	}
368	gaussj(covar,mfit,oneda,1);
369	for (j=1;j<=mfit;j++) da[j]=oneda[j][1];
370	if (*alamda == 0.0) {
371	covsrt(covar,ma,ia,mfit);
372	free_matrix(oneda,1,mfit,1,1);
373	free_vector(da,1,ma);
374	free_vector(beta,1,ma);
375	free_vector(atry,1,ma);
376	return;
377	}
378	for (j=0,l=1;l<=ma;l++)
379	if (ia[l]) atry[l]=a[l]+da[++j];
380	mrqcof(x,y,sig,ndata,atry,ia,ma,covar,da,chisq,funcs);
381	if (*chisq < ochisq) {
382	alamda = 0.1;
383	ochisq=(*chisq);
384	for (j=0,l=1;l<=ma;l++) {
385	if (ia[l]) {
386	for (j++,k=0,m=1;m<=ma;m++) {
387	if (ia[m]) {
388	k++;
389	alpha[j][k]=covar[j][k];
390	}
391	}
392	beta[j]=da[j];
393	a[l]=atry[l];
394	}
395	}
396	} else {
397	alamda = 10.0;
398	*chisq=ochisq;
399	}
400	}
401
402
403	int lev_marq_fit( double x[], double y[], double sig[], int NPT, double a[], int ia[], double dev[], int MA,
404	double chisq_p, void (funcs)(double, double [], double *, double [], int) )
405	{
406	int i,itst,k;
407	double alamda,chisq,ochisq,covar,alpha;
408
409	covar=matrix(1,MA,1,MA);
410	alpha=matrix(1,MA,1,MA);
411
412	if( setjmp(env) == 0 ) {
413	alamda = -1;
414	mrqmin(x,y,sig,NPT,a,ia,MA,covar,alpha,&chisq,funcs,&alamda);
415	k=1;
416	itst=0;
417	for (;;) {
418	k++;
419	ochisq=chisq;
420	mrqmin(x,y,sig,NPT,a,ia,MA,covar,alpha,&chisq,funcs,&alamda);
421	if (chisq > ochisq)
422	itst=0;
423	else if (fabs(ochisq-chisq) < 0.1)
424	itst++;
425	if (itst < 4) continue;
426	alamda=0.0;
427	mrqmin(x,y,sig,NPT,a,ia,MA,covar,alpha,&chisq,funcs,&alamda);
428	*chisq_p = chisq;
429	for (i=1;i<=MA;i++)
430	dev[i] = sqrt(covar[i][i]);
431	break;
432	}
433	free_matrix(alpha,1,MA,1,MA);
434	free_matrix(covar,1,MA,1,MA);
435	return 0;
436	}
437	else {
438	//if( control_g.print_fit_errors==2 )
439	fprintf( stderr, " runtime error\n" );
440
441	free_matrix(alpha,1,MA,1,MA);
442	free_matrix(covar,1,MA,1,MA);
443	return -1;
444	}
445	}
446