Checking in the seeds of new cluster fitting code.
[u/mrichter/AliRoot.git] / HLT / comp / AliL3FitUtilities.c
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]*ex*2.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)((nrow*ncol+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 += dy*dy*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