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