View Javadoc

1   package org.lcsim.recon.vertexing.billoir;
2   
3   /**
4    * @version $Id: BilloirFitter.java,v 1.3 2010/04/08 20:59:18 jeremy Exp $
5    */
6   
7   // Performs a Kalman fit to a list of tracks and returns
8   // a Vertex object
9   import Jama.util.Maths;
10  import static java.lang.Math.sin;
11  import static java.lang.Math.cos;
12  import static java.lang.Math.tan;
13  import static java.lang.Math.atan;
14  import static java.lang.Math.atan2;
15  import static java.lang.Math.sqrt;
16  import static java.lang.Math.PI;
17  import java.util.List;
18  import org.lcsim.event.Track;
19  
20  import Jama.Matrix;
21  import org.lcsim.spacegeom.SpacePoint;
22  
23  
24  public class BilloirFitter implements VertexFitter {
25      // the value of the magnetic field in the vicinity of the vertex
26      // default is a constant field along the z axis
27      private double _bField;
28  
29      // constructor
30      public BilloirFitter(double bField) {
31          _bField = bField;
32      }
33  
34      // Function copied from trf to avoid unnecessary dependency on that package.  --JM
35      public static double fmod1( double value, double range )
36      {
37          double tmp = value%range;
38          if ( tmp < 0.0 ) return tmp + Math.abs(range);
39          return tmp;
40      }
41  
42  
43      // fitter
44      private Vertex fit(int ntrk, boolean fitwb, boolean[] invtx, double[][] par, double[][] wgt, double[] xyz) {
45          return pxfvtx(ntrk, fitwb, invtx, par, wgt, xyz);
46      }
47  
48      
49      public Vertex fit(List<Track> tracks, SpacePoint initialPosition, boolean withBeamConstraint) {
50          int ntrk = tracks.size();
51          boolean[] isInVtx = new boolean[ntrk];
52          for (int i=0; i<ntrk; i++) {
53              isInVtx[i] = true;
54          }
55          double[][] parameters = new double[5][ntrk];
56          double[][] errors = new double[15][ntrk];
57          for (Track iTrack : tracks) {
58              double[] iOldParams = iTrack.getTrackParameters();
59              Matrix jacobi = new Matrix(getJacobi(iOldParams));
60              Matrix olderrors = Maths.toJamaMatrix(iTrack.getErrorMatrix());
61  
62              double theta = PI/2 - atan(iOldParams[4]);
63              double[] newparams = new double[]{iOldParams[0], iOldParams[3], theta, iOldParams[1], iOldParams[2]};
64              double[] iErrors = flattenMatrix(jacobi.times(olderrors).times(jacobi.transpose()).getArray());
65              int iTrackIndex = tracks.indexOf(iTrack);
66              for (int i=0; i<iErrors.length; ++i) {
67                  errors[i][iTrackIndex] = iErrors[i];
68              }
69              for (int i=0; i<newparams.length; ++i) {
70                  parameters[i][iTrackIndex] = newparams[i];
71              }
72          }
73          return pxfvtx(ntrk, withBeamConstraint, isInVtx, parameters, errors, initialPosition.getCartesianArray());
74      }
75      /**
76       * Conversion matrix from org.lcsim track parameters (old) to internal parameters
77       */
78      private double[][] getJacobi(double[] old) {
79          double[][] jacobi = new double[][]{
80                   {old[0], 0, 0, 0, 0}
81                 , {0, 0, 0, old[1], 0}
82                 , {0, 0, 0, 0, old[2]}
83                 , {0, old[3], 0, 0, 0}
84                 , {0, 0, -1-old[4]*old[4], 0, 0}};
85          return jacobi;
86      }
87  
88      private double[] flattenMatrix(double[][] matrix) {
89          int length = matrix.length;
90          double[] result = new double[length*(length+1)/2];
91          int count = 0;
92          for (int i=0; i<length; ++i)
93              for (int j=0; j<=i; ++j)
94                  result[count++] = matrix[i][j];
95          return result;
96      }
97  
98      private double[] pxmi5(double[] wgt) {
99  
100         // ***********************************************************************
101         // * *
102         // * *
103         // * inversion of a (5x5) symmetric positive matrix (cholesky method) *
104         // * internal computation in double precision *
105         // * check on positivity (ierr=2 if wgt not positive) *
106         // * *
107         // ***********************************************************************
108         // *
109         double[] cov = new double[15];
110         double t11, t12, t13, t14, t15;
111         double t22, t23, t24, t25;
112         double t33, t34, t35;
113         double t44, t45;
114         double t55;
115         double s12, s13, s14, s15;
116         double s23, s24, s25;
117         double s34, s35;
118         double s45;
119         if (wgt[0] < 0.)
120             throw new IllegalArgumentException("Bad weight matrix!");
121 
122         t11 = 1. / sqrt(wgt[0]);
123         s12 = wgt[1] * t11;
124         s13 = wgt[3] * t11;
125         s14 = wgt[6] * t11;
126         s15 = wgt[10] * t11;
127         //
128         t22 = wgt[2] - s12 * s12;
129         if (t22 < 0.)
130             throw new IllegalArgumentException("Bad weight matrix!");
131 
132         t22 = 1. / sqrt(t22);
133         s23 = (wgt[4] - s12 * s13) * t22;
134         s24 = (wgt[7] - s12 * s14) * t22;
135         s25 = (wgt[11] - s12 * s15) * t22;
136         //
137         t33 = wgt[5] - s13 * s13 - s23 * s23;
138         if (t33 < 0.)
139             throw new IllegalArgumentException("Bad weight matrix!");
140 
141         t33 = 1. / sqrt(t33);
142         s34 = (wgt[8] - s13 * s14 - s23 * s24) * t33;
143         s35 = (wgt[12] - s13 * s15 - s23 * s25) * t33;
144         //
145         t44 = wgt[9] - s14 * s14 - s24 * s24 - s34 * s34;
146         if (t44 < 0.)
147             throw new IllegalArgumentException("Bad weight matrix!");
148 
149         t44 = 1. / sqrt(t44);
150         s45 = (wgt[13] - s14 * s15 - s24 * s25 - s34 * s35) * t44;
151         //
152         t55 = wgt[14] - s15 * s15 - s25 * s25 - s35 * s35 - s45 * s45;
153         if (t55 < 0.)
154             throw new IllegalArgumentException("Bad weight matrix!");
155 
156         t55 = 1. / sqrt(t55);
157         //
158         t45 = -t44 * (s45 * t55);
159         t34 = -t33 * (s34 * t44);
160         t35 = -t33 * (s34 * t45 + s35 * t55);
161         t23 = -t22 * (s23 * t33);
162         t24 = -t22 * (s23 * t34 + s24 * t44);
163         t25 = -t22 * (s23 * t35 + s24 * t45 + s25 * t55);
164         t12 = -t11 * (s12 * t22);
165         t13 = -t11 * (s12 * t23 + s13 * t33);
166         t14 = -t11 * (s12 * t24 + s13 * t34 + s14 * t44);
167         t15 = -t11 * (s12 * t25 + s13 * t35 + s14 * t45 + s15 * t55);
168         //
169         cov[0] = t11 * t11 + t12 * t12 + t13 * t13 + t14 * t14 + t15 * t15;
170         cov[1] = t12 * t22 + t13 * t23 + t14 * t24 + t15 * t25;
171         cov[2] = t22 * t22 + t23 * t23 + t24 * t24 + t25 * t25;
172         cov[3] = t13 * t33 + t14 * t34 + t15 * t35;
173         cov[4] = t23 * t33 + t24 * t34 + t25 * t35;
174         cov[5] = t33 * t33 + t34 * t34 + t35 * t35;
175         cov[6] = t14 * t44 + t15 * t45;
176         cov[7] = t24 * t44 + t25 * t45;
177         cov[8] = t34 * t44 + t35 * t45;
178         cov[9] = t44 * t44 + t45 * t45;
179         cov[10] = t15 * t55;
180         cov[11] = t25 * t55;
181         cov[12] = t35 * t55;
182         cov[13] = t45 * t55;
183         cov[14] = t55 * t55;
184         return cov;
185     }
186 
187     private double fwdch2(double[] wgt, double d1, double d2, double d3, double d4, double d5) {
188         // ***********************************************************************
189         // * *
190         // * computation of dt*wgt*d *
191         // * wgt is a (5x5) symmetric matrix ; d is a 5-vector *
192         // * *
193         // ***********************************************************************
194         // *
195         return wgt[0]
196                 * d1
197                 * d1
198                 + wgt[2]
199                 * d2
200                 * d2
201                 + wgt[5]
202                 * d3
203                 * d3
204                 + wgt[9]
205                 * d4
206                 * d4
207                 + wgt[14]
208                 * d5
209                 * d5
210                 + 2
211                 * (d2 * d1 * wgt[1] + d3 * (d1 * wgt[3] + d2 * wgt[4]) + d4 * (d1 * wgt[6] + d2 * wgt[7] + d3 * wgt[8]) + d5
212                         * (d1 * wgt[10] + d2 * wgt[11] + d3 * wgt[12] + d4 * wgt[13]));
213     }
214 
215     // ************************************************************************
216     // * *
217     public Perigee perigee(double[] rawdat, double xb, double yb) {
218         // * *
219         // ************************************************************************
220         // name : perigee
221         //
222         // created : 30-jun-1998 author : norman a. graf
223         // based on pxfxpe 7-jun-1989 author : p. billoir (cdf)
224         //
225         // function : compute from input data (with cylindrical or cartesian
226         // coordinates)
227         // the "perigee" parameters (point of closest approach to the
228         // reference axis) and the covariance matrix on them
229         // for error propagation, an approximation is made, assuming
230         // that the r coordinate of reference point is small w.r.t.
231         // the radius of curvature
232         // no account is taken for multiple scattering, so that the
233         // error matrix is underestimated if the data reference point
234         // is outside the beam pipe (except at high momentum)
235         //
236         // references : none
237         //
238         // arguments : input : dat(1:21) : tix array
239         // dat(1:3) : coord. of a point on the track
240         // if "cylindrical" coord. : r,r*phi,z
241         // if "cartesian" coord. : x,y,z
242         // dat(4:5) : theta, phi of the tangent at this point
243         // dat[5] : 1/p with geometrical sign
244         // dat(7:21) : covariance matrix on 5 "free" param. :
245         // rphi,z,theta,phi,1/p if "cylind." coord. (r fixed)
246         // x,y,theta,phi,1/p if "cartes." coord. (z fixed)
247         //
248         // xb,yb : coordinates of the reference axis
249         //
250         // output : par(1:5) : "perigee" parameters
251         // 1 : epsilon (impact par. in xy projection, with sign)
252         // 2 : z coordinate
253         // 3 : theta angle
254         // 4 : phi angle
255         // 5 : 1/r (r = radius of curvature, with sign)
256         // cov(1:15) : covariance matrix of par
257         // wgt(1:15) : weight matrix of par (inverse of cov)
258         //
259         // errors : ierr = 1 : incorrect data in tkr array
260         // (for example not cylindrical coordinates)
261         // 2 : cov not positive (wgt not computed)
262         //
263         // ************************************************************************
264         //
265         //
266         // conversion from d0 cylindrical coordinates to mine...
267         //
268         double sthet, sthet2, cthet, gamma;
269         double d0der11, d0der43, d0der45, d0der55;
270         // double[] rawdat = new double [21];
271         //
272         double cosf, sinf, r0, rcosb, d11, d12, d21, d22, cov1, cov3, cov4, cov7, cov11;
273         // to define the magnetic field - values to be provided :
274         // FIXME this needs to be replaced by the official org.lcsim constant
275         double consa = 0.0003;
276         double bmag = _bField;
277         double consb = consa * bmag;
278 
279         // consa = velocity of light (here in GeV/(mm.T) ; depends on units)
280         // bmag = magnetic field along z axis
281         //
282         //
283         double[] par = new double[5];
284         double[] cov = new double[15];
285         double[] wgt = new double[15];
286         double sgn;
287         double[] dat = new double[21];
288         //
289         // local variables
290         // __________________
291         //
292         int ierr, ii;
293         double capphi, cotth, dphi, rdphi, rtrk, xc, x0, yc, y0;
294         double der1, der2, der11, der14, der15, der23, der45;
295         //
296         //
297         // executable statements
298         // ________________________
299         //
300         double twopi = 2. * PI;
301         double halfpi = PI / 2.;
302 
303         int icypl = 1;
304         // icypl = 1 if "cylindrical" coordinates, 0 if "plane" coordinates
305         //
306         // convert here...
307         //
308         //
309         // ::: parameters r, rphi, z, theta, phi, 1/p
310         //
311         dat[0] = rawdat[0]; // r
312         dat[1] = rawdat[0] * rawdat[1]; // r*phi
313         dat[2] = rawdat[2]; // z
314         dat[3] = atan(1. / rawdat[4]); // theta
315         if (rawdat[4] < 0)
316             dat[3] = dat[3] + PI;
317         dat[4] = rawdat[1] + rawdat[3]; // phi (recall beta= phi(dir)-phi(pos) )
318         dat[5] = -rawdat[5] * sin(dat[3]); // 1/p (recall 1/pt = 1/(p*sin(theta))
319         //
320         // ::: covariance matrix
321         // ::: there are seven terms in the conversion from
322         //
323         // phi rphi
324         // z z
325         // alfa to theta
326         // cot(theta) phi
327         // q/pt 1/p
328         //
329         // d0der14 = 1
330         // d0der22 = 1
331         // d0der34 = -1
332         //
333         sthet = sin(dat[3]);
334         sthet2 = sthet * sthet;
335         cthet = cos(dat[3]);
336         gamma = -rawdat[5] * cthet * sthet2;
337 
338         d0der11 = rawdat[0];
339         d0der43 = -sthet2;
340         d0der45 = -rawdat[5] * cthet * sthet2;
341         d0der55 = sthet;
342         //
343         dat[6] = rawdat[6] * rawdat[0] * rawdat[0];
344         dat[7] = rawdat[7] * rawdat[0];
345         dat[8] = rawdat[8];
346         dat[9] = -rawdat[12] * rawdat[0] * sthet2;
347         dat[10] = -rawdat[13] * sthet2;
348         dat[11] = rawdat[15] * sthet2 * sthet2;
349         dat[12] = (rawdat[6] - rawdat[9]) * rawdat[0];
350         dat[13] = rawdat[7] - rawdat[10];
351         dat[14] = (rawdat[14] - rawdat[12]) * sthet2;
352         dat[15] = rawdat[6] - 2 * rawdat[9] + rawdat[11];
353         dat[16] = - (rawdat[16] * sthet + rawdat[12] * gamma) * rawdat[0];
354         dat[17] = -rawdat[17] * sthet - rawdat[13] * gamma;
355         dat[18] = (rawdat[19] * sthet + rawdat[15] * gamma) * sthet2;
356         dat[19] = (rawdat[14] - rawdat[12]) * gamma + (rawdat[18] - rawdat[16]) * sthet;
357         dat[20] = (rawdat[15] * gamma + 2 * rawdat[19] * sthet) * gamma + rawdat[20] * sthet2;
358 
359         //
360         // icypl = 1 if "cylindrical" coordinates, 0 if "plane" coordinates
361         //
362         if ( (icypl == 1 && dat[0] == 0.) || dat[3] == 0. || dat[5] == 0.) {
363             System.err.println("******error in perigee*******");
364             throw new IllegalArgumentException("Choke!");
365         }
366 
367         //
368         // now computation of "perigee" parameters
369         //
370         capphi = dat[1] / dat[0];
371         if (icypl == 1) {
372             x0 = dat[0] * cos(capphi) - xb;
373             y0 = dat[0] * sin(capphi) - yb;
374         } else {
375             x0 = dat[0] - xb;
376             y0 = dat[1] - yb;
377         }
378         rtrk = sin(dat[3]) / (consb * dat[5]);
379         cosf = cos(dat[4]);
380         sinf = sin(dat[4]);
381         xc = x0 - rtrk * sinf;
382         yc = y0 + rtrk * cosf;
383         // epsilon (impact parameter in xy projection, with geometrical sign)
384         sgn = 1.;
385         if (rtrk < 0.)
386             sgn = -1.;
387         par[0] = rtrk - sgn * sqrt(xc * xc + yc * yc);
388         // phi at perigee (range 0,2*PI)
389         par[3] = atan2(yc, xc) + PI + sgn * halfpi;
390         if (par[3] < 0.)
391             par[3] = par[3] + twopi;
392         else if (par[3] > twopi)
393             par[3] = par[3] - twopi;
394         // variation of phi from reference point of tkr to perigee (range -pi,+pi)
395         dphi = fmod1(par[3] - dat[4] + twopi + PI, twopi) - PI;
396         rdphi = rtrk * dphi;
397         cotth = 1. / tan(dat[3]);
398         // z of perigee
399         par[1] = dat[2] + cotth * rdphi;
400         // theta and 1/r at perigee (unchanged)
401         par[2] = dat[3];
402         par[4] = 1. / rtrk;
403         //
404         // computation of covariance matrix
405         //
406         for (ii = 0; ii < 15; ++ii) {
407             cov[ii] = dat[6 + ii];
408         }
409         //
410         // transformation from 1/p to 1/r = consb * (1/p) / sin(theta)
411         //
412         der1 = -cotth * par[4];
413         der2 = par[4] / dat[5];
414         cov[14] = der1 * der1 * cov[5] + 2 * der1 * der2 * cov[12] + der2 * der2 * cov[14];
415         cov[10] = der1 * cov[3] + der2 * cov[10];
416         cov[11] = der1 * cov[4] + der2 * cov[11];
417         cov[12] = der1 * cov[5] + der2 * cov[12];
418         cov[13] = der1 * cov[8] + der2 * cov[13];
419         //
420         // if cartesian coordinates transformation from (x,y) to (r*phi,z)
421         // (neglecting curvature effects )
422         //
423         if (icypl == 0) {
424             r0 = sqrt(dat[0] * dat[0] + dat[1] * dat[1]);
425             rcosb = dat[0] * cosf + dat[1] * sinf;
426             d11 = -r0 * sinf / rcosb;
427             d12 = r0 * cosf / rcosb;
428             d21 = -dat[0] * cotth / rcosb;
429             d22 = -dat[1] * cotth / rcosb;
430             cov1 = d11 * d11 * cov[0] + 2 * d11 * d12 * cov[1] + d12 * d12 * cov[2];
431             cov3 = d21 * d21 * cov[0] + 2 * d21 * d22 * cov[1] + d22 * d22 * cov[2];
432             cov[1] = d11 * d21 * cov[0] + (d12 * d21 + d11 * d22) * cov[1] + d12 * d22 * cov[2];
433             cov[0] = cov1;
434             cov[2] = cov3;
435             cov4 = d11 * cov[3] + d12 * cov[4];
436             cov[4] = d21 * cov[3] + d22 * cov[4];
437             cov[3] = cov4;
438             cov7 = d11 * cov[6] + d12 * cov[7];
439             cov[7] = d21 * cov[6] + d22 * cov[7];
440             cov[6] = cov7;
441             cov11 = d11 * cov[10] + d12 * cov[11];
442             cov[11] = d21 * cov[10] + d22 * cov[11];
443             cov[10] = cov11;
444         }
445         //
446         // transformation from (r*phi,z0,theta,phi0,1/r)
447         // to (epsilon,zp,theta,phip,1/r)
448         //
449         // approximation for derivatives d(epsilon)/d(r*phi), d(epsilon)/d(phi0),
450         // d(epsilon)/d(1/r), d(zp)/d(z0) and d(phip)/d(1/r)
451         // the other ones are exactly or approximately 1 (diagonal terms)
452         // or 0 (non-diagonal terms)
453         //
454         der11 = rdphi / sqrt(x0 * x0 + y0 * y0);
455         der14 = -rdphi;
456         der15 = -rdphi * rdphi / 2.;
457         der23 = - (1. + cotth * cotth) * rdphi;
458         der45 = rdphi;
459         //
460         cov[0] = der11
461                 * der11
462                 * cov[0]
463                 + 2
464                 * der11
465                 * (der14 * cov[6] + der15 * cov[10])
466                 + der14
467                 * (der14 * cov[9] + 2 * der15 * cov[13])
468                 + der15
469                 * der15
470                 * cov[14];
471         cov[1] = der11
472                 * (cov[1] + der23 * cov[3])
473                 + der14
474                 * (cov[7] + der23 * cov[8])
475                 + der15
476                 * (cov[11] + der23 * cov[12]);
477         cov[2] = cov[2] + 2 * der23 * cov[4] + der23 * der23 * cov[5];
478         cov[3] = der11 * cov[3] + der14 * cov[8] + der15 * cov[12];
479         cov[4] = cov[4] + der23 * cov[5];
480         cov[6] = der11
481                 * (cov[6] + der45 * cov[10])
482                 + der14
483                 * (cov[9] + der45 * cov[13])
484                 + der15
485                 * (cov[13] + der45 * cov[14]);
486         cov[7] = cov[7] + der23 * cov[8] + der45 * (cov[11] + der23 * cov[12]);
487         cov[8] = cov[8] + der45 * cov[12];
488         cov[9] = cov[9] + 2 * der45 * cov[13] + der45 * der45 * cov[14];
489         cov[10] = der11 * cov[10] + der14 * cov[13] + der15 * cov[14];
490         cov[11] = cov[11] + der23 * cov[12];
491         cov[13] = cov[13] + der45 * cov[14];
492         //
493         // invert matrix cov to get wgt
494         //
495         wgt = pxmi5(cov);
496         //
497         return new Perigee(par, cov, wgt);
498 
499     }
500 
501     private double[] pxmi3(double[] wgt) {
502         double[][] x = new double[3][3];
503         x[0][0] = wgt[0];
504         x[0][1] = wgt[1];
505         x[1][0] = wgt[1];
506         x[0][2] = wgt[2];
507         x[2][0] = wgt[2];
508         x[1][1] = wgt[3];
509         x[1][2] = wgt[4];
510         x[2][1] = wgt[4];
511         x[2][2] = wgt[5];
512         Matrix xMat = new Matrix(x);
513         double[][] y = xMat.inverse().getArray();
514         return new double[] {y[0][0], y[0][1], y[0][2], y[1][1], y[1][2], y[2][2]};
515     }
516     
517     private double[] pxmi3_old(double[] wgt) {
518         // ***********************************************************************
519         // * *
520         // * *
521         // * inversion of a (3x3) symmetric positive matrix (cholesky method) *
522         // * internal computation in double precision *
523         // * check on positivity (ierr=2 if wgt not positive) *
524         // * cov (output) may overwrite wgt (input) *
525         // * *
526         // ***********************************************************************
527         // *
528 
529         double[] cov = new double[6];
530         double t11, t12, t13;
531         double t21, t22, t23;
532         double t33;
533         double s12, s13;
534         double s23;
535 
536         if (wgt[0] < 0.)
537             throw new IllegalArgumentException("Bad weight matrix!");
538 
539         t11 = 1. / sqrt(wgt[0]);
540         s12 = wgt[1] * t11;
541         s13 = wgt[3] * t11;
542         //
543         t22 = wgt[2] - s12 * s12;
544         if (t22 < 0.)
545             throw new IllegalArgumentException("Bad weight matrix!");
546 
547         t22 = 1. / sqrt(t22);
548         s23 = (wgt[4] - s12 * s13) * t22;
549         //
550         t33 = wgt[5] - s13 * s13 - s23 * s23;
551         if (t33 < 0.)
552             throw new IllegalArgumentException("Bad weight matrix!");
553 
554         t33 = 1. / sqrt(t33);
555         //
556         t23 = -t22 * (s23 * t33);
557         t12 = -t11 * (s12 * t22);
558         t13 = -t11 * (s12 * t23 + s13 * t33);
559         //
560         cov[0] = t11 * t11 + t12 * t12 + t13 * t13;
561         cov[1] = t12 * t22 + t13 * t23;
562         cov[2] = t22 * t22 + t23 * t23;
563         cov[3] = t13 * t33;
564         cov[4] = t23 * t33;
565         cov[5] = t33 * t33;
566         return cov;
567     }
568 
569     // ************************************************************************
570     // * *
571     public Vertex pxfvtx(int ntrk, boolean fitwb, boolean[] invtx, double[][] par, double[][] wgt, double[] xyz) {
572         // * *
573         // ************************************************************************
574         // * name : pxfvtx
575         // *
576         // * created : 5-jan-1989 author : p. billoir (cdf)
577         // *
578         // * function : perform a full vertex vertex fit with weighted mean
579         // * of tracks considered as ellipsoids in 5d space of
580         // * "perigee" parameters (epsilon,zp,theta,phi,1/r)
581         // *
582         // ******** 1-12-90 changes in pxfvtx routine : p. billoir
583         // * - to allow the use of the beam spot constraint (logical var. fitwb)
584         // * - to know the contribution of individual tracks to the total chi2.
585         // *
586         // * references : none
587         // *
588         // * arguments : input : ntrk : number of tracks
589         // * fitwb = .true. if we want the beam spot
590         // * constraint
591         // * invtx(i)=.true. to use the track i in the
592         // * vertex fit
593         // * par(1:5,1:ntrk) : "perigee" parameters
594         // * 1 : epsilon (impact par. in xy projection, with sign)
595         // * 2 : z coordinate
596         // * 3 : theta angle
597         // * 4 : phi angle
598         // * 5 : 1/r (r = radius of curvature, with sign)
599         // * wgt(1:15,1:ntrk) : weight matrix on par
600         // * xyz(1:3) : approximate coordinates of the vertex
601         // *
602         // * output : xyzf(1:3) : fitted coordinates of the vertex
603         // * parf(1:3,1:ntrk) : fitted parameters at the vertex
604         // * 1 : theta
605         // * 2 : phi
606         // * 3 : 1/r
607         // * vcov(1:6) : covariance matrix on xyz(vertex)
608         // * tcov(1:6,1:ntrk) : covariance matrix on parf
609         // * chi2 : chi2 of the fit
610         // * chi2tr(i)=contribution of track i to the
611         // * total chi2
612         // *
613         // * errors : ierr = 1 : too many tracks (ntrk > ntrmax)
614         // * ierr = 2 : covariance or weight matrix not positive
615         // *
616         // *************************************************************************
617         // *
618         // *
619         // * "beam spot" description (common to be provided by the user)
620         // * beamoy(1-3) are the mean beam spot positions in x,y and z.
621         // * sigxbe,sigybe,sigzbe are the beam spread in x,y,z
622         // common/pxcpro/beamoy[2],sigxbe,sigybe,sigzbe
623         // double beamoy,sigxbe,sigybe,sigzbe
624         // *
625         // formal parameters
626         // *____________________
627         // *
628         // int i,j,ierr,ntrk,ntrmax
629         // *
630         int ntrmax = 1000;
631 
632         double[] xyzf = new double[3];
633         double[][] parf = new double[3][ntrk];
634         double[] vcov = new double[6];
635         double[][] tcov = new double[6][ntrk];
636         double chi2;
637 
638         // *
639         // * local variables
640         // *__________________
641         // *
642         double[] wa = new double[6];
643         double[][] wb = new double[9][ntrk];
644         double[][] wc = new double[6][ntrk];
645         double[][] wci = new double[6][ntrk];
646         double[][] wbci = new double[9][ntrk];
647         double[] tv = new double[3];
648         double[][] tt = new double[3][ntrk];
649         double[] dxyz = new double[3];
650         double[] phiv = new double[ntrk];
651         double[] eps = new double[ntrk];
652         double[] zp = new double[ntrk];
653         double[] deps = new double[ntrk];
654         double[] dzp = new double[ntrk];
655         double cotth, cosf, sinf, uu, vv, d11, d12, d21, d22, d41, d42, e12, e13, e21, e22, e23, e43, dw11, dw12, dw13, dw14, dw15, dw21, dw22, dw23, dw24, dw25, dw31, dw32, dw33, dw34, dw35, ew11, ew12, ew13, ew14, ew15, ew21, ew22, ew23, ew24, ew25, ew31, ew32, ew33, ew34, ew35, chi2i, epsf, zpf, phif;
656 
657         double fwdch2;
658         double[] chi2tr = new double[ntrk];
659 
660         // needs to be set
661         double sigxbe = .1;
662         double sigybe = .1;
663         double sigzbe = .1;
664         double beamoy[] = { 0., 0., 0. };
665         //
666         // *
667         // * executable statements
668         // *________________________
669         // *
670 
671         // *
672         // * loop over the tracks
673         // *
674         chi2i = 0.;
675         for (int i = 0; i < ntrk; ++i) {
676             if (invtx[i]) {
677                 // *
678                 // * starting conditions :
679                 // * "perigee" parameters eps and zp if the trajectory goes through xyz
680                 // * and its theta,phi,1/r at perigee are equal to the values at input
681                 // *
682                 cotth = 1. / tan(par[2][i]);
683                 uu = xyz[0] * cos(par[3][i]) + xyz[1] * sin(par[3][i]);
684                 vv = xyz[1] * cos(par[3][i]) - xyz[0] * sin(par[3][i]);
685                 eps[i] = -vv + .5 * uu * uu * par[4][i];
686                 zp[i] = xyz[2] - uu * cotth;
687                 // * phi at vertex with these parameters
688                 phiv[i] = par[3][i] + uu * par[4][i];
689                 cosf = cos(phiv[i]);
690                 sinf = sin(phiv[i]);
691                 // *
692                 // * contribution of this track to chi2 with initial values
693                 // *
694                 deps[i] = par[0][i] - eps[i];
695                 dzp[i] = par[1][i] - zp[i];
696                 chi2i = chi2i
697                         + wgt[0][i]
698                         * deps[i]
699                         * deps[i]
700                         + 2
701                         * wgt[1][i]
702                         * deps[i]
703                         * dzp[i]
704                         + wgt[2][i]
705                         * dzp[i]
706                         * dzp[i];
707                 // *
708                 // * derivatives (deriv1) of perigee param. w.r.t. x,y,z (vertex)
709                 d11 = sinf;
710                 d12 = -cosf;
711                 d21 = -cosf * cotth;
712                 d22 = -sinf * cotth;
713                 d41 = -cosf * par[4][i];
714                 d42 = -sinf * par[4][i];
715                 // *
716                 // * matrix dw = (deriv1)t * weight
717                 dw11 = d11 * wgt[0][i] + d21 * wgt[1][i] + d41 * wgt[6][i];
718                 dw12 = d11 * wgt[1][i] + d21 * wgt[2][i] + d41 * wgt[7][i];
719                 dw13 = d11 * wgt[3][i] + d21 * wgt[4][i] + d41 * wgt[8][i];
720                 dw14 = d11 * wgt[6][i] + d21 * wgt[7][i] + d41 * wgt[9][i];
721                 dw15 = d11 * wgt[10][i] + d21 * wgt[11][i] + d41 * wgt[13][i];
722                 dw21 = d12 * wgt[0][i] + d22 * wgt[1][i] + d42 * wgt[6][i];
723                 dw22 = d12 * wgt[1][i] + d22 * wgt[2][i] + d42 * wgt[7][i];
724                 dw23 = d12 * wgt[3][i] + d22 * wgt[4][i] + d42 * wgt[8][i];
725                 dw24 = d12 * wgt[6][i] + d22 * wgt[7][i] + d42 * wgt[9][i];
726                 dw25 = d12 * wgt[10][i] + d22 * wgt[11][i] + d42 * wgt[13][i];
727                 dw31 = wgt[1][i];
728                 dw32 = wgt[2][i];
729                 dw33 = wgt[4][i];
730                 dw34 = wgt[7][i];
731                 dw35 = wgt[11][i];
732                 // *
733                 // * summation of dw * dpar to vector tv
734                 tv[0] = tv[0] + dw11 * deps[i] + dw12 * dzp[i];
735                 tv[1] = tv[1] + dw21 * deps[i] + dw22 * dzp[i];
736                 tv[2] = tv[2] + dw31 * deps[i] + dw32 * dzp[i];
737                 // *
738                 // * derivatives (deriv2) of perigee param. w.r.t. theta,phi,1/r (vertex)
739                 e12 = uu;
740                 e13 = -.5 * uu * uu;
741                 e21 = -uu * (1. + cotth * cotth);
742                 e22 = -vv * cotth;
743                 e23 = uu * vv * cotth;
744                 e43 = -uu;
745                 // *
746                 // * matrix ew = (deriv2)t * weight
747                 ew11 = e21 * wgt[1][i] + wgt[3][i];
748                 ew12 = e21 * wgt[2][i] + wgt[4][i];
749                 ew13 = e21 * wgt[4][i] + wgt[5][i];
750                 ew14 = e21 * wgt[7][i] + wgt[8][i];
751                 ew15 = e21 * wgt[11][i] + wgt[12][i];
752                 ew21 = e12 * wgt[0][i] + e22 * wgt[1][i] + wgt[6][i];
753                 ew22 = e12 * wgt[1][i] + e22 * wgt[2][i] + wgt[7][i];
754                 ew23 = e12 * wgt[3][i] + e22 * wgt[4][i] + wgt[8][i];
755                 ew24 = e12 * wgt[6][i] + e22 * wgt[7][i] + wgt[9][i];
756                 ew25 = e12 * wgt[10][i] + e22 * wgt[11][i] + wgt[13][i];
757                 ew31 = e13 * wgt[0][i] + e23 * wgt[1][i] + e43 * wgt[6][i] + wgt[10][i];
758                 ew32 = e13 * wgt[1][i] + e23 * wgt[2][i] + e43 * wgt[7][i] + wgt[11][i];
759                 ew33 = e13 * wgt[3][i] + e23 * wgt[4][i] + e43 * wgt[8][i] + wgt[12][i];
760                 ew34 = e13 * wgt[6][i] + e23 * wgt[7][i] + e43 * wgt[9][i] + wgt[13][i];
761                 ew35 = e13 * wgt[10][i] + e23 * wgt[11][i] + e43 * wgt[13][i] + wgt[14][i];
762                 // *
763                 // * computation of vector tt = ew * dpar
764                 tt[0][i] = ew11 * deps[i] + ew12 * dzp[i];
765                 tt[1][i] = ew21 * deps[i] + ew22 * dzp[i];
766                 tt[2][i] = ew31 * deps[i] + ew32 * dzp[i];
767                 // *
768                 // * summation of (deriv1)t * weight * (deriv1) to matrix wa
769                 wa[0] = wa[0] + dw11 * d11 + dw12 * d21 + dw14 * d41;
770                 wa[1] = wa[1] + dw11 * d12 + dw12 * d22 + dw14 * d42;
771                 wa[2] = wa[2] + dw21 * d12 + dw22 * d22 + dw24 * d42;
772                 wa[3] = wa[3] + dw12;
773                 wa[4] = wa[4] + dw22;
774                 wa[5] = wa[5] + dw32;
775                 // *
776                 // * computation of matrix wb = (deriv1)t * weight * (deriv2)
777                 wb[0][i] = dw12 * e21 + dw13;
778                 wb[1][i] = dw22 * e21 + dw23;
779                 wb[2][i] = dw32 * e21 + dw33;
780                 wb[3][i] = dw11 * e12 + dw12 * e22 + dw14;
781                 wb[4][i] = dw21 * e12 + dw22 * e22 + dw24;
782                 wb[5][i] = dw31 * e12 + dw32 * e22 + dw34;
783                 wb[6][i] = dw11 * e13 + dw12 * e23 + dw14 * e43 + dw15;
784                 wb[7][i] = dw21 * e13 + dw22 * e23 + dw24 * e43 + dw25;
785                 wb[8][i] = dw31 * e13 + dw32 * e23 + dw34 * e43 + dw35;
786                 // *
787                 // * computation of matrix wc = (deriv2)t * weight * (deriv2)
788                 wc[0][i] = ew12 * e21 + ew13;
789                 wc[1][i] = ew22 * e21 + ew23;
790                 wc[3][i] = ew32 * e21 + ew33;
791                 wc[2][i] = ew21 * e12 + ew22 * e22 + ew24;
792                 wc[4][i] = ew31 * e12 + ew32 * e22 + ew34;
793                 wc[5][i] = ew31 * e13 + ew32 * e23 + ew34 * e43 + ew35;
794                 // *
795                 // * computation of matrices wci = (wc)**(-1) and wbci = wb * wci
796 
797                 // Not very efficient here...
798                 double[] tmp = new double[6];
799                 for (int j = 0; j < 6; ++j) {
800                     tmp[j] = wc[j][i];
801 //                    System.out.println("tmp[" + j + "]= " + tmp[j]);
802                 }
803                 double[] tmpinv = new double[6];
804                 tmpinv = pxmi3(tmp);
805                 for (int j = 0; j < 6; ++j) {
806                     wci[j][i] = tmpinv[j];
807                 }
808                 // wci[0][i] = pxmi3(wc[0][i]);
809 
810                 wbci[0][i] = wb[0][i] * wci[0][i] + wb[3][i] * wci[1][i] + wb[6][i] * wci[3][i];
811                 wbci[1][i] = wb[1][i] * wci[0][i] + wb[4][i] * wci[1][i] + wb[7][i] * wci[3][i];
812                 wbci[2][i] = wb[2][i] * wci[0][i] + wb[5][i] * wci[1][i] + wb[8][i] * wci[3][i];
813                 wbci[3][i] = wb[0][i] * wci[1][i] + wb[3][i] * wci[2][i] + wb[6][i] * wci[4][i];
814                 wbci[4][i] = wb[1][i] * wci[1][i] + wb[4][i] * wci[2][i] + wb[7][i] * wci[4][i];
815                 wbci[5][i] = wb[2][i] * wci[1][i] + wb[5][i] * wci[2][i] + wb[8][i] * wci[4][i];
816                 wbci[6][i] = wb[0][i] * wci[3][i] + wb[3][i] * wci[4][i] + wb[6][i] * wci[5][i];
817                 wbci[7][i] = wb[1][i] * wci[3][i] + wb[4][i] * wci[4][i] + wb[7][i] * wci[5][i];
818                 wbci[8][i] = wb[2][i] * wci[3][i] + wb[5][i] * wci[4][i] + wb[8][i] * wci[5][i];
819                 // *
820                 // * subtraction of wbci * (wb)t from matrix wa
821                 wa[0] = wa[0] - wbci[0][i] * wb[0][i] - wbci[3][i] * wb[3][i] - wbci[6][i] * wb[6][i];
822                 wa[1] = wa[1] - wbci[0][i] * wb[1][i] - wbci[3][i] * wb[4][i] - wbci[6][i] * wb[7][i];
823                 wa[2] = wa[2] - wbci[1][i] * wb[1][i] - wbci[4][i] * wb[4][i] - wbci[7][i] * wb[7][i];
824                 wa[3] = wa[3] - wbci[0][i] * wb[2][i] - wbci[3][i] * wb[5][i] - wbci[6][i] * wb[8][i];
825                 wa[4] = wa[4] - wbci[1][i] * wb[2][i] - wbci[4][i] * wb[5][i] - wbci[7][i] * wb[8][i];
826                 wa[5] = wa[5] - wbci[2][i] * wb[2][i] - wbci[5][i] * wb[5][i] - wbci[8][i] * wb[8][i];
827                 // *
828                 // * subtraction of wbci * tt from vector tv
829                 tv[0] = tv[0] - wbci[0][i] * tt[0][i] - wbci[3][i] * tt[1][i] - wbci[6][i] * tt[2][i];
830                 tv[1] = tv[1] - wbci[1][i] * tt[0][i] - wbci[4][i] * tt[1][i] - wbci[7][i] * tt[2][i];
831                 tv[2] = tv[2] - wbci[2][i] * tt[0][i] - wbci[5][i] * tt[1][i] - wbci[8][i] * tt[2][i];
832                 // *
833             }// check on whether to use track
834         }// loop over tracks
835 
836         // beam constraint
837         if (fitwb) {
838             wa[0] += 1. / (sigxbe * sigxbe);
839             wa[2] += 1. / (sigxbe * sigybe);
840             wa[5] += 1. / (sigzbe * sigzbe);
841             tv[0] += (beamoy[0] - xyz[0]) / (sigxbe * sigxbe);
842             tv[1] += (beamoy[1] - xyz[1]) / (sigybe * sigybe);
843             tv[2] += (beamoy[2] - xyz[2]) / (sigzbe * sigzbe);
844         }
845         // *
846         // * solution of the linear system
847         // *
848         // * covariance matrix on vertex
849         vcov = pxmi3(wa);
850         for (int ii = 0; ii < vcov.length; ++ii) {
851 //            System.out.println("vcov[" + ii + "]= " + vcov[ii]);
852         }
853         //
854         // *
855         // * corrections to vertex coordinates
856         dxyz[0] = vcov[0] * tv[0] + vcov[1] * tv[1] + vcov[3] * tv[2];
857         dxyz[1] = vcov[1] * tv[0] + vcov[2] * tv[1] + vcov[4] * tv[2];
858         dxyz[2] = vcov[3] * tv[0] + vcov[4] * tv[1] + vcov[5] * tv[2];
859         xyzf[0] = xyz[0] + dxyz[0];
860         xyzf[1] = xyz[1] + dxyz[1];
861         xyzf[2] = xyz[2] + dxyz[2];
862         // *
863         // * corrections to track parameters and covariance matrices
864         // *
865         for (int i = 0; i < ntrk; ++i) {
866             if (invtx[i]) {
867                 // *
868                 // * variation on par is wci * tt - (wbci)t * dxyz
869                 parf[0][i] = par[2][i]
870                         + wci[0][i]
871                         * tt[0][i]
872                         + wci[1][i]
873                         * tt[1][i]
874                         + wci[3][i]
875                         * tt[2][i]
876                         - wbci[0][i]
877                         * dxyz[0]
878                         - wbci[1][i]
879                         * dxyz[1]
880                         - wbci[2][i]
881                         * dxyz[2];
882                 parf[1][i] = phiv[i]
883                         + wci[1][i]
884                         * tt[0][i]
885                         + wci[2][i]
886                         * tt[1][i]
887                         + wci[4][i]
888                         * tt[2][i]
889                         - wbci[3][i]
890                         * dxyz[0]
891                         - wbci[4][i]
892                         * dxyz[1]
893                         - wbci[5][i]
894                         * dxyz[2];
895                 parf[2][i] = par[4][i]
896                         + wci[3][i]
897                         * tt[0][i]
898                         + wci[4][i]
899                         * tt[1][i]
900                         + wci[5][i]
901                         * tt[2][i]
902                         - wbci[6][i]
903                         * dxyz[0]
904                         - wbci[7][i]
905                         * dxyz[1]
906                         - wbci[8][i]
907                         * dxyz[2];
908                 // *
909                 // * covariance matrix of par is wci + (wbci)t * vcov * wbci
910                 tcov[0][i] = wci[0][i]
911                         + wbci[0][i]
912                         * (vcov[0] * wbci[0][i] + vcov[1] * wbci[1][i] + vcov[3] * wbci[2][i])
913                         + wbci[1][i]
914                         * (vcov[1] * wbci[0][i] + vcov[2] * wbci[1][i] + vcov[4] * wbci[2][i])
915                         + wbci[2][i]
916                         * (vcov[3] * wbci[0][i] + vcov[4] * wbci[1][i] + vcov[5] * wbci[2][i]);
917 
918                 tcov[1][i] = wci[1][i]
919                         + wbci[0][i]
920                         * (vcov[0] * wbci[3][i] + vcov[1] * wbci[4][i] + vcov[3] * wbci[5][i])
921                         + wbci[1][i]
922                         * (vcov[1] * wbci[3][i] + vcov[2] * wbci[4][i] + vcov[4] * wbci[5][i])
923                         + wbci[2][i]
924                         * (vcov[3] * wbci[3][i] + vcov[4] * wbci[4][i] + vcov[5] * wbci[5][i]);
925 
926                 tcov[2][i] = wci[2][i]
927                         + wbci[3][i]
928                         * (vcov[0] * wbci[3][i] + vcov[1] * wbci[4][i] + vcov[3] * wbci[5][i])
929                         + wbci[4][i]
930                         * (vcov[1] * wbci[3][i] + vcov[2] * wbci[4][i] + vcov[4] * wbci[5][i])
931                         + wbci[5][i]
932                         * (vcov[3] * wbci[3][i] + vcov[4] * wbci[4][i] + vcov[5] * wbci[5][i]);
933 
934                 tcov[3][i] = wci[3][i]
935                         + wbci[0][i]
936                         * (vcov[0] * wbci[6][i] + vcov[1] * wbci[7][i] + vcov[3] * wbci[8][i])
937                         + wbci[1][i]
938                         * (vcov[1] * wbci[6][i] + vcov[2] * wbci[7][i] + vcov[4] * wbci[8][i])
939                         + wbci[2][i]
940                         * (vcov[3] * wbci[6][i] + vcov[4] * wbci[7][i] + vcov[5] * wbci[8][i]);
941 
942                 tcov[4][i] = wci[4][i]
943                         + wbci[3][i]
944                         * (vcov[0] * wbci[6][i] + vcov[1] * wbci[7][i] + vcov[3] * wbci[8][i])
945                         + wbci[4][i]
946                         * (vcov[1] * wbci[6][i] + vcov[2] * wbci[7][i] + vcov[4] * wbci[8][i])
947                         + wbci[5][i]
948                         * (vcov[3] * wbci[6][i] + vcov[4] * wbci[7][i] + vcov[5] * wbci[8][i]);
949 
950                 tcov[5][i] = wci[5][i]
951                         + wbci[6][i]
952                         * (vcov[0] * wbci[6][i] + vcov[1] * wbci[7][i] + vcov[3] * wbci[8][i])
953                         + wbci[7][i]
954                         * (vcov[1] * wbci[6][i] + vcov[2] * wbci[7][i] + vcov[4] * wbci[8][i])
955                         + wbci[8][i]
956                         * (vcov[3] * wbci[6][i] + vcov[4] * wbci[7][i] + vcov[5] * wbci[8][i]);
957 
958                 // *
959             } // check on whether to use track
960         } // loop over tracks
961 
962         // *
963         // * chi2 with fitted values
964         // *
965         chi2 = 0.;
966         if (fitwb)
967             chi2 += ( (xyzf[0] - beamoy[0]) / sigxbe)
968                     * ( (xyzf[0] - beamoy[0]) / sigxbe)
969                     + ( (xyzf[1] - beamoy[1]) / sigybe)
970                     * ( (xyzf[1] - beamoy[1]) / sigybe)
971                     + ( (xyzf[2] - beamoy[2]) / sigzbe)
972                     * ( (xyzf[2] - beamoy[2]) / sigzbe);
973 
974         for (int i = 0; i < ntrk; ++i) {
975             if (invtx[i]) {
976                 uu = xyzf[0] * cos(parf[1][i]) + xyzf[1] * sin(parf[1][i]);
977                 vv = xyzf[1] * cos(parf[1][i]) - xyzf[0] * sin(parf[1][i]);
978                 epsf = -vv - .5 * uu * uu * parf[2][i];
979                 zpf = xyzf[2] - uu / tan(parf[0][i]);
980                 phif = parf[1][i] - uu * parf[2][i];
981                 // not very efficient here...
982                 double[] tmp = new double[15];
983                 for (int j = 0; j < 15; ++j) {
984                     tmp[j] = wgt[j][i];
985                 }
986                 chi2tr[i] = fwdch2(tmp, epsf - par[0][i], zpf - par[1][i], parf[0][i] - par[2][i], phif - par[3][i],
987                         parf[2][i] - par[4][i]);
988                 chi2 += chi2tr[i];
989             }
990         }
991 //        System.out.println("chi2= " + chi2);
992         return new Vertex(ntrk, xyzf, parf, vcov, tcov, chi2, chi2tr);
993 
994     }
995 
996 }