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