3 added + 4 modified, total 7 files
lcsim/src/org/lcsim/recon/vertexing/zvtop4/fitter
diff -N BilloirFitter.java
--- /dev/null 1 Jan 1970 00:00:00 -0000
+++ BilloirFitter.java 27 Mar 2006 00:00:21 -0000 1.1
@@ -0,0 +1,985 @@
+package org.lcsim.recon.vertexing.zvtop4.fitter;
+
+/**
+ * @version $Id: BilloirFitter.java,v 1.1 2006/03/27 00:00:21 jstrube Exp $
+ */
+
+// Performs a Kalman fit to a list of tracks and returns
+// a Vertex object
+import static java.lang.Math.sin;
+import static java.lang.Math.cos;
+import static java.lang.Math.tan;
+import static java.lang.Math.atan;
+import static java.lang.Math.atan2;
+import static java.lang.Math.sqrt;
+import static java.lang.Math.PI;
+import java.util.List;
+import org.lcsim.event.Track;
+import static org.lcsim.recon.tracking.trfutil.TRFMath.fmod1;
+
+import Jama.Matrix;
+import org.lcsim.spacegeom.SpacePoint;
+
+
+public class BilloirFitter implements VertexFitter {
+ // the value of the magnetic field in the vicinity of the vertex
+ // default is a constant field along the z axis
+ private double _bField;
+
+ // constructor
+ public BilloirFitter(double bField) {
+ _bField = bField;
+ }
+
+ // fitter
+ private Vertex fit(int ntrk, boolean fitwb, boolean[] invtx, double[][] par, double[][] wgt, double[] xyz) {
+ return pxfvtx(ntrk, fitwb, invtx, par, wgt, xyz);
+ }
+
+
+ public Vertex fit(List<Track> tracks, SpacePoint initialPosition, boolean withBeamConstraint) {
+ int ntrk = tracks.size();
+ boolean[] isInVtx = new boolean[ntrk];
+ for (int i=0; i<ntrk; i++) {
+ isInVtx[i] = true;
+ }
+ double[][] parameters = new double[5][ntrk];
+ double[][] errors = new double[15][ntrk];
+ for (Track iTrack : tracks) {
+ double[] iOldParams = iTrack.getTrackParameters();
+ Matrix jacobi = new Matrix(getJacobi(iOldParams));
+ Matrix olderrors = new Matrix(iTrack.getErrorMatrix());
+
+ double theta = PI/2 - atan(iOldParams[4]);
+ double[] newparams = new double[]{iOldParams[0], iOldParams[3], theta, iOldParams[1], iOldParams[2]};
+ double[] iErrors = flattenMatrix(jacobi.times(olderrors).times(jacobi.transpose()).getArray());
+ int iTrackIndex = tracks.indexOf(iTrack);
+ for (int i=0; i<iErrors.length; ++i) {
+ errors[i][iTrackIndex] = iErrors[i];
+ }
+ for (int i=0; i<newparams.length; ++i) {
+ parameters[i][iTrackIndex] = newparams[i];
+ }
+ }
+ return pxfvtx(ntrk, withBeamConstraint, isInVtx, parameters, errors, initialPosition.getCartesianArray());
+ }
+
+ private double[][] getJacobi(double[] old) {
+ double[][] jacobi = new double[][]{
+ {old[0], 0, 0, 0, 0}
+ , {0, 0, 0, old[1], 0}
+ , {0, 0, 0, 0, old[2]}
+ , {0, old[3], 0, 0, 0}
+ , {0, 0, 1+Math.tan(old[4])*Math.tan(old[4]), 0, 0}};
+ return jacobi;
+ }
+
+ private double[] flattenMatrix(double[][] matrix) {
+ int length = matrix.length;
+ double[] result = new double[15];
+ int count = 0;
+ for (int i=0; i<5; ++i)
+ for (int j=i; j<5; ++j)
+ result[count++] = matrix[i][j];
+ return result;
+ }
+
+ private double[] pxmi5(double[] wgt) {
+
+ // ***********************************************************************
+ // * *
+ // * *
+ // * inversion of a (5x5) symmetric positive matrix (cholesky method) *
+ // * internal computation in double precision *
+ // * check on positivity (ierr=2 if wgt not positive) *
+ // * *
+ // ***********************************************************************
+ // *
+ double[] cov = new double[15];
+ double t11, t12, t13, t14, t15;
+ double t22, t23, t24, t25;
+ double t33, t34, t35;
+ double t44, t45;
+ double t55;
+ double s12, s13, s14, s15;
+ double s23, s24, s25;
+ double s34, s35;
+ double s45;
+ if (wgt[0] < 0.)
+ throw new IllegalArgumentException("Bad weight matrix!");
+
+ t11 = 1. / sqrt(wgt[0]);
+ s12 = wgt[1] * t11;
+ s13 = wgt[3] * t11;
+ s14 = wgt[6] * t11;
+ s15 = wgt[10] * t11;
+ //
+ t22 = wgt[2] - s12 * s12;
+ if (t22 < 0.)
+ throw new IllegalArgumentException("Bad weight matrix!");
+
+ t22 = 1. / sqrt(t22);
+ s23 = (wgt[4] - s12 * s13) * t22;
+ s24 = (wgt[7] - s12 * s14) * t22;
+ s25 = (wgt[11] - s12 * s15) * t22;
+ //
+ t33 = wgt[5] - s13 * s13 - s23 * s23;
+ if (t33 < 0.)
+ throw new IllegalArgumentException("Bad weight matrix!");
+
+ t33 = 1. / sqrt(t33);
+ s34 = (wgt[8] - s13 * s14 - s23 * s24) * t33;
+ s35 = (wgt[12] - s13 * s15 - s23 * s25) * t33;
+ //
+ t44 = wgt[9] - s14 * s14 - s24 * s24 - s34 * s34;
+ if (t44 < 0.)
+ throw new IllegalArgumentException("Bad weight matrix!");
+
+ t44 = 1. / sqrt(t44);
+ s45 = (wgt[13] - s14 * s15 - s24 * s25 - s34 * s35) * t44;
+ //
+ t55 = wgt[14] - s15 * s15 - s25 * s25 - s35 * s35 - s45 * s45;
+ if (t55 < 0.)
+ throw new IllegalArgumentException("Bad weight matrix!");
+
+ t55 = 1. / sqrt(t55);
+ //
+ t45 = -t44 * (s45 * t55);
+ t34 = -t33 * (s34 * t44);
+ t35 = -t33 * (s34 * t45 + s35 * t55);
+ t23 = -t22 * (s23 * t33);
+ t24 = -t22 * (s23 * t34 + s24 * t44);
+ t25 = -t22 * (s23 * t35 + s24 * t45 + s25 * t55);
+ t12 = -t11 * (s12 * t22);
+ t13 = -t11 * (s12 * t23 + s13 * t33);
+ t14 = -t11 * (s12 * t24 + s13 * t34 + s14 * t44);
+ t15 = -t11 * (s12 * t25 + s13 * t35 + s14 * t45 + s15 * t55);
+ //
+ cov[0] = t11 * t11 + t12 * t12 + t13 * t13 + t14 * t14 + t15 * t15;
+ cov[1] = t12 * t22 + t13 * t23 + t14 * t24 + t15 * t25;
+ cov[2] = t22 * t22 + t23 * t23 + t24 * t24 + t25 * t25;
+ cov[3] = t13 * t33 + t14 * t34 + t15 * t35;
+ cov[4] = t23 * t33 + t24 * t34 + t25 * t35;
+ cov[5] = t33 * t33 + t34 * t34 + t35 * t35;
+ cov[6] = t14 * t44 + t15 * t45;
+ cov[7] = t24 * t44 + t25 * t45;
+ cov[8] = t34 * t44 + t35 * t45;
+ cov[9] = t44 * t44 + t45 * t45;
+ cov[10] = t15 * t55;
+ cov[11] = t25 * t55;
+ cov[12] = t35 * t55;
+ cov[13] = t45 * t55;
+ cov[14] = t55 * t55;
+ return cov;
+ }
+
+ private double fwdch2(double[] wgt, double d1, double d2, double d3, double d4, double d5) {
+ // ***********************************************************************
+ // * *
+ // * computation of dt*wgt*d *
+ // * wgt is a (5x5) symmetric matrix ; d is a 5-vector *
+ // * *
+ // ***********************************************************************
+ // *
+ return wgt[0]
+ * d1
+ * d1
+ + wgt[2]
+ * d2
+ * d2
+ + wgt[5]
+ * d3
+ * d3
+ + wgt[9]
+ * d4
+ * d4
+ + wgt[14]
+ * d5
+ * d5
+ + 2
+ * (d2 * d1 * wgt[1] + d3 * (d1 * wgt[3] + d2 * wgt[4]) + d4 * (d1 * wgt[6] + d2 * wgt[7] + d3 * wgt[8]) + d5
+ * (d1 * wgt[10] + d2 * wgt[11] + d3 * wgt[12] + d4 * wgt[13]));
+ }
+
+ // ************************************************************************
+ // * *
+ public Perigee perigee(double[] rawdat, double xb, double yb) {
+ // * *
+ // ************************************************************************
+ // name : perigee
+ //
+ // created : 30-jun-1998 author : norman a. graf
+ // based on pxfxpe 7-jun-1989 author : p. billoir (cdf)
+ //
+ // function : compute from input data (with cylindrical or cartesian
+ // coordinates)
+ // the "perigee" parameters (point of closest approach to the
+ // reference axis) and the covariance matrix on them
+ // for error propagation, an approximation is made, assuming
+ // that the r coordinate of reference point is small w.r.t.
+ // the radius of curvature
+ // no account is taken for multiple scattering, so that the
+ // error matrix is underestimated if the data reference point
+ // is outside the beam pipe (except at high momentum)
+ //
+ // references : none
+ //
+ // arguments : input : dat(1:21) : tix array
+ // dat(1:3) : coord. of a point on the track
+ // if "cylindrical" coord. : r,r*phi,z
+ // if "cartesian" coord. : x,y,z
+ // dat(4:5) : theta, phi of the tangent at this point
+ // dat[5] : 1/p with geometrical sign
+ // dat(7:21) : covariance matrix on 5 "free" param. :
+ // rphi,z,theta,phi,1/p if "cylind." coord. (r fixed)
+ // x,y,theta,phi,1/p if "cartes." coord. (z fixed)
+ //
+ // xb,yb : coordinates of the reference axis
+ //
+ // output : par(1:5) : "perigee" parameters
+ // 1 : epsilon (impact par. in xy projection, with sign)
+ // 2 : z coordinate
+ // 3 : theta angle
+ // 4 : phi angle
+ // 5 : 1/r (r = radius of curvature, with sign)
+ // cov(1:15) : covariance matrix of par
+ // wgt(1:15) : weight matrix of par (inverse of cov)
+ //
+ // errors : ierr = 1 : incorrect data in tkr array
+ // (for example not cylindrical coordinates)
+ // 2 : cov not positive (wgt not computed)
+ //
+ // ************************************************************************
+ //
+ //
+ // conversion from d0 cylindrical coordinates to mine...
+ //
+ double sthet, sthet2, cthet, gamma;
+ double d0der11, d0der43, d0der45, d0der55;
+ // double[] rawdat = new double [21];
+ //
+ double cosf, sinf, r0, rcosb, d11, d12, d21, d22, cov1, cov3, cov4, cov7, cov11;
+ // to define the magnetic field - values to be provided :
+ // FIXME this needs to be replaced by the official org.lcsim constant
+ double consa = 0.0003;
+ double bmag = _bField;
+ double consb = consa * bmag;
+
+ // consa = velocity of light (here in GeV/(mm.T) ; depends on units)
+ // bmag = magnetic field along z axis
+ //
+ //
+ double[] par = new double[5];
+ double[] cov = new double[15];
+ double[] wgt = new double[15];
+ double sgn;
+ double[] dat = new double[21];
+ //
+ // local variables
+ // __________________
+ //
+ int ierr, ii;
+ double capphi, cotth, dphi, rdphi, rtrk, xc, x0, yc, y0;
+ double der1, der2, der11, der14, der15, der23, der45;
+ //
+ //
+ // executable statements
+ // ________________________
+ //
+ double twopi = 2. * PI;
+ double halfpi = PI / 2.;
+
+ int icypl = 1;
+ // icypl = 1 if "cylindrical" coordinates, 0 if "plane" coordinates
+ //
+ // convert here...
+ //
+ //
+ // ::: parameters r, rphi, z, theta, phi, 1/p
+ //
+ dat[0] = rawdat[0]; // r
+ dat[1] = rawdat[0] * rawdat[1]; // r*phi
+ dat[2] = rawdat[2]; // z
+ dat[3] = atan(1. / rawdat[4]); // theta
+ if (rawdat[4] < 0)
+ dat[3] = dat[3] + PI;
+ dat[4] = rawdat[1] + rawdat[3]; // phi (recall beta= phi(dir)-phi(pos) )
+ dat[5] = -rawdat[5] * sin(dat[3]); // 1/p (recall 1/pt = 1/(p*sin(theta))
+ //
+ // ::: covariance matrix
+ // ::: there are seven terms in the conversion from
+ //
+ // phi rphi
+ // z z
+ // alfa to theta
+ // cot(theta) phi
+ // q/pt 1/p
+ //
+ // d0der14 = 1
+ // d0der22 = 1
+ // d0der34 = -1
+ //
+ sthet = sin(dat[3]);
+ sthet2 = sthet * sthet;
+ cthet = cos(dat[3]);
+ gamma = -rawdat[5] * cthet * sthet2;
+
+ d0der11 = rawdat[0];
+ d0der43 = -sthet2;
+ d0der45 = -rawdat[5] * cthet * sthet2;
+ d0der55 = sthet;
+ //
+ dat[6] = rawdat[6] * rawdat[0] * rawdat[0];
+ dat[7] = rawdat[7] * rawdat[0];
+ dat[8] = rawdat[8];
+ dat[9] = -rawdat[12] * rawdat[0] * sthet2;
+ dat[10] = -rawdat[13] * sthet2;
+ dat[11] = rawdat[15] * sthet2 * sthet2;
+ dat[12] = (rawdat[6] - rawdat[9]) * rawdat[0];
+ dat[13] = rawdat[7] - rawdat[10];
+ dat[14] = (rawdat[14] - rawdat[12]) * sthet2;
+ dat[15] = rawdat[6] - 2 * rawdat[9] + rawdat[11];
+ dat[16] = - (rawdat[16] * sthet + rawdat[12] * gamma) * rawdat[0];
+ dat[17] = -rawdat[17] * sthet - rawdat[13] * gamma;
+ dat[18] = (rawdat[19] * sthet + rawdat[15] * gamma) * sthet2;
+ dat[19] = (rawdat[14] - rawdat[12]) * gamma + (rawdat[18] - rawdat[16]) * sthet;
+ dat[20] = (rawdat[15] * gamma + 2 * rawdat[19] * sthet) * gamma + rawdat[20] * sthet2;
+
+ //
+ // icypl = 1 if "cylindrical" coordinates, 0 if "plane" coordinates
+ //
+ if ( (icypl == 1 && dat[0] == 0.) || dat[3] == 0. || dat[5] == 0.) {
+ System.out.println("******error in perigee*******");
+ throw new IllegalArgumentException("Choke!");
+ }
+
+ //
+ // now computation of "perigee" parameters
+ //
+ capphi = dat[1] / dat[0];
+ if (icypl == 1) {
+ x0 = dat[0] * cos(capphi) - xb;
+ y0 = dat[0] * sin(capphi) - yb;
+ } else {
+ x0 = dat[0] - xb;
+ y0 = dat[1] - yb;
+ }
+ rtrk = sin(dat[3]) / (consb * dat[5]);
+ cosf = cos(dat[4]);
+ sinf = sin(dat[4]);
+ xc = x0 - rtrk * sinf;
+ yc = y0 + rtrk * cosf;
+ // epsilon (impact parameter in xy projection, with geometrical sign)
+ sgn = 1.;
+ if (rtrk < 0.)
+ sgn = -1.;
+ par[0] = rtrk - sgn * sqrt(xc * xc + yc * yc);
+ // phi at perigee (range 0,2*PI)
+ par[3] = atan2(yc, xc) + PI + sgn * halfpi;
+ if (par[3] < 0.)
+ par[3] = par[3] + twopi;
+ else if (par[3] > twopi)
+ par[3] = par[3] - twopi;
+ // variation of phi from reference point of tkr to perigee (range -pi,+pi)
+ dphi = fmod1(par[3] - dat[4] + twopi + PI, twopi) - PI;
+ rdphi = rtrk * dphi;
+ cotth = 1. / tan(dat[3]);
+ // z of perigee
+ par[1] = dat[2] + cotth * rdphi;
+ // theta and 1/r at perigee (unchanged)
+ par[2] = dat[3];
+ par[4] = 1. / rtrk;
+ //
+ // computation of covariance matrix
+ //
+ for (ii = 0; ii < 15; ++ii) {
+ cov[ii] = dat[6 + ii];
+ }
+ //
+ // transformation from 1/p to 1/r = consb * (1/p) / sin(theta)
+ //
+ der1 = -cotth * par[4];
+ der2 = par[4] / dat[5];
+ cov[14] = der1 * der1 * cov[5] + 2 * der1 * der2 * cov[12] + der2 * der2 * cov[14];
+ cov[10] = der1 * cov[3] + der2 * cov[10];
+ cov[11] = der1 * cov[4] + der2 * cov[11];
+ cov[12] = der1 * cov[5] + der2 * cov[12];
+ cov[13] = der1 * cov[8] + der2 * cov[13];
+ //
+ // if cartesian coordinates transformation from (x,y) to (r*phi,z)
+ // (neglecting curvature effects )
+ //
+ if (icypl == 0) {
+ r0 = sqrt(dat[0] * dat[0] + dat[1] * dat[1]);
+ rcosb = dat[0] * cosf + dat[1] * sinf;
+ d11 = -r0 * sinf / rcosb;
+ d12 = r0 * cosf / rcosb;
+ d21 = -dat[0] * cotth / rcosb;
+ d22 = -dat[1] * cotth / rcosb;
+ cov1 = d11 * d11 * cov[0] + 2 * d11 * d12 * cov[1] + d12 * d12 * cov[2];
+ cov3 = d21 * d21 * cov[0] + 2 * d21 * d22 * cov[1] + d22 * d22 * cov[2];
+ cov[1] = d11 * d21 * cov[0] + (d12 * d21 + d11 * d22) * cov[1] + d12 * d22 * cov[2];
+ cov[0] = cov1;
+ cov[2] = cov3;
+ cov4 = d11 * cov[3] + d12 * cov[4];
+ cov[4] = d21 * cov[3] + d22 * cov[4];
+ cov[3] = cov4;
+ cov7 = d11 * cov[6] + d12 * cov[7];
+ cov[7] = d21 * cov[6] + d22 * cov[7];
+ cov[6] = cov7;
+ cov11 = d11 * cov[10] + d12 * cov[11];
+ cov[11] = d21 * cov[10] + d22 * cov[11];
+ cov[10] = cov11;
+ }
+ //
+ // transformation from (r*phi,z0,theta,phi0,1/r)
+ // to (epsilon,zp,theta,phip,1/r)
+ //
+ // approximation for derivatives d(epsilon)/d(r*phi), d(epsilon)/d(phi0),
+ // d(epsilon)/d(1/r), d(zp)/d(z0) and d(phip)/d(1/r)
+ // the other ones are exactly or approximately 1 (diagonal terms)
+ // or 0 (non-diagonal terms)
+ //
+ der11 = rdphi / sqrt(x0 * x0 + y0 * y0);
+ der14 = -rdphi;
+ der15 = -rdphi * rdphi / 2.;
+ der23 = - (1. + cotth * cotth) * rdphi;
+ der45 = rdphi;
+ //
+ cov[0] = der11
+ * der11
+ * cov[0]
+ + 2
+ * der11
+ * (der14 * cov[6] + der15 * cov[10])
+ + der14
+ * (der14 * cov[9] + 2 * der15 * cov[13])
+ + der15
+ * der15
+ * cov[14];
+ cov[1] = der11
+ * (cov[1] + der23 * cov[3])
+ + der14
+ * (cov[7] + der23 * cov[8])
+ + der15
+ * (cov[11] + der23 * cov[12]);
+ cov[2] = cov[2] + 2 * der23 * cov[4] + der23 * der23 * cov[5];
+ cov[3] = der11 * cov[3] + der14 * cov[8] + der15 * cov[12];
+ cov[4] = cov[4] + der23 * cov[5];
+ cov[6] = der11
+ * (cov[6] + der45 * cov[10])
+ + der14
+ * (cov[9] + der45 * cov[13])
+ + der15
+ * (cov[13] + der45 * cov[14]);
+ cov[7] = cov[7] + der23 * cov[8] + der45 * (cov[11] + der23 * cov[12]);
+ cov[8] = cov[8] + der45 * cov[12];
+ cov[9] = cov[9] + 2 * der45 * cov[13] + der45 * der45 * cov[14];
+ cov[10] = der11 * cov[10] + der14 * cov[13] + der15 * cov[14];
+ cov[11] = cov[11] + der23 * cov[12];
+ cov[13] = cov[13] + der45 * cov[14];
+ //
+ // invert matrix cov to get wgt
+ //
+ wgt = pxmi5(cov);
+ //
+ return new Perigee(par, cov, wgt);
+
+ }
+
+ private double[] pxmi3(double[] wgt) {
+ double[][] x = new double[3][3];
+ x[0][0] = wgt[0];
+ x[0][1] = wgt[1];
+ x[1][0] = wgt[1];
+ x[0][2] = wgt[2];
+ x[2][0] = wgt[2];
+ x[1][1] = wgt[3];
+ x[1][2] = wgt[4];
+ x[2][1] = wgt[4];
+ x[2][2] = wgt[5];
+ Matrix xMat = new Matrix(x);
+ double[][] y = xMat.inverse().getArray();
+ return new double[] {y[0][0], y[0][1], y[0][2], y[1][1], y[1][2], y[2][2]};
+ }
+
+ private double[] pxmi3_old(double[] wgt) {
+ // ***********************************************************************
+ // * *
+ // * *
+ // * inversion of a (3x3) symmetric positive matrix (cholesky method) *
+ // * internal computation in double precision *
+ // * check on positivity (ierr=2 if wgt not positive) *
+ // * cov (output) may overwrite wgt (input) *
+ // * *
+ // ***********************************************************************
+ // *
+
+ double[] cov = new double[6];
+ double t11, t12, t13;
+ double t21, t22, t23;
+ double t33;
+ double s12, s13;
+ double s23;
+
+ if (wgt[0] < 0.)
+ throw new IllegalArgumentException("Bad weight matrix!");
+
+ t11 = 1. / sqrt(wgt[0]);
+ s12 = wgt[1] * t11;
+ s13 = wgt[3] * t11;
+ //
+ t22 = wgt[2] - s12 * s12;
+ if (t22 < 0.)
+ throw new IllegalArgumentException("Bad weight matrix!");
+
+ t22 = 1. / sqrt(t22);
+ s23 = (wgt[4] - s12 * s13) * t22;
+ //
+ t33 = wgt[5] - s13 * s13 - s23 * s23;
+ if (t33 < 0.)
+ throw new IllegalArgumentException("Bad weight matrix!");
+
+ t33 = 1. / sqrt(t33);
+ //
+ t23 = -t22 * (s23 * t33);
+ t12 = -t11 * (s12 * t22);
+ t13 = -t11 * (s12 * t23 + s13 * t33);
+ //
+ cov[0] = t11 * t11 + t12 * t12 + t13 * t13;
+ cov[1] = t12 * t22 + t13 * t23;
+ cov[2] = t22 * t22 + t23 * t23;
+ cov[3] = t13 * t33;
+ cov[4] = t23 * t33;
+ cov[5] = t33 * t33;
+ return cov;
+ }
+
+ // ************************************************************************
+ // * *
+ public Vertex pxfvtx(int ntrk, boolean fitwb, boolean[] invtx, double[][] par, double[][] wgt, double[] xyz) {
+ // * *
+ // ************************************************************************
+ // * name : pxfvtx
+ // *
+ // * created : 5-jan-1989 author : p. billoir (cdf)
+ // *
+ // * function : perform a full vertex vertex fit with weighted mean
+ // * of tracks considered as ellipsoids in 5d space of
+ // * "perigee" parameters (epsilon,zp,theta,phi,1/r)
+ // *
+ // ******** 1-12-90 changes in pxfvtx routine : p. billoir
+ // * - to allow the use of the beam spot constraint (logical var. fitwb)
+ // * - to know the contribution of individual tracks to the total chi2.
+ // *
+ // * references : none
+ // *
+ // * arguments : input : ntrk : number of tracks
+ // * fitwb = .true. if we want the beam spot
+ // * constraint
+ // * invtx(i)=.true. to use the track i in the
+ // * vertex fit
+ // * par(1:5,1:ntrk) : "perigee" parameters
+ // * 1 : epsilon (impact par. in xy projection, with sign)
+ // * 2 : z coordinate
+ // * 3 : theta angle
+ // * 4 : phi angle
+ // * 5 : 1/r (r = radius of curvature, with sign)
+ // * wgt(1:15,1:ntrk) : weight matrix on par
+ // * xyz(1:3) : approximate coordinates of the vertex
+ // *
+ // * output : xyzf(1:3) : fitted coordinates of the vertex
+ // * parf(1:3,1:ntrk) : fitted parameters at the vertex
+ // * 1 : theta
+ // * 2 : phi
+ // * 3 : 1/r
+ // * vcov(1:6) : covariance matrix on xyz(vertex)
+ // * tcov(1:6,1:ntrk) : covariance matrix on parf
+ // * chi2 : chi2 of the fit
+ // * chi2tr(i)=contribution of track i to the
+ // * total chi2
+ // *
+ // * errors : ierr = 1 : too many tracks (ntrk > ntrmax)
+ // * ierr = 2 : covariance or weight matrix not positive
+ // *
+ // *************************************************************************
+ // *
+ // *
+ // * "beam spot" description (common to be provided by the user)
+ // * beamoy(1-3) are the mean beam spot positions in x,y and z.
+ // * sigxbe,sigybe,sigzbe are the beam spread in x,y,z
+ // common/pxcpro/beamoy[2],sigxbe,sigybe,sigzbe
+ // double beamoy,sigxbe,sigybe,sigzbe
+ // *
+ // formal parameters
+ // *____________________
+ // *
+ // int i,j,ierr,ntrk,ntrmax
+ // *
+ int ntrmax = 1000;
+
+ double[] xyzf = new double[3];
+ double[][] parf = new double[3][ntrk];
+ double[] vcov = new double[6];
+ double[][] tcov = new double[6][ntrk];
+ double chi2;
+
+ // *
+ // * local variables
+ // *__________________
+ // *
+ double[] wa = new double[6];
+ double[][] wb = new double[9][ntrk];
+ double[][] wc = new double[6][ntrk];
+ double[][] wci = new double[6][ntrk];
+ double[][] wbci = new double[9][ntrk];
+ double[] tv = new double[3];
+ double[][] tt = new double[3][ntrk];
+ double[] dxyz = new double[3];
+ double[] phiv = new double[ntrk];
+ double[] eps = new double[ntrk];
+ double[] zp = new double[ntrk];
+ double[] deps = new double[ntrk];
+ double[] dzp = new double[ntrk];
+ 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;
+
+ double fwdch2;
+ double[] chi2tr = new double[ntrk];
+
+ // needs to be set
+ double sigxbe = .1;
+ double sigybe = .1;
+ double sigzbe = .1;
+ double beamoy[] = { 0., 0., 0. };
+ //
+ // *
+ // * executable statements
+ // *________________________
+ // *
+
+ // *
+ // * loop over the tracks
+ // *
+ chi2i = 0.;
+ for (int i = 0; i < ntrk; ++i) {
+ if (invtx[i]) {
+ // *
+ // * starting conditions :
+ // * "perigee" parameters eps and zp if the trajectory goes through xyz
+ // * and its theta,phi,1/r at perigee are equal to the values at input
+ // *
+ cotth = 1. / tan(par[2][i]);
+ uu = xyz[0] * cos(par[3][i]) + xyz[1] * sin(par[3][i]);
+ vv = xyz[1] * cos(par[3][i]) - xyz[0] * sin(par[3][i]);
+ eps[i] = -vv + .5 * uu * uu * par[4][i];
+ zp[i] = xyz[2] - uu * cotth;
+ // * phi at vertex with these parameters
+ phiv[i] = par[3][i] + uu * par[4][i];
+ cosf = cos(phiv[i]);
+ sinf = sin(phiv[i]);
+ // *
+ // * contribution of this track to chi2 with initial values
+ // *
+ deps[i] = par[0][i] - eps[i];
+ dzp[i] = par[1][i] - zp[i];
+ chi2i = chi2i
+ + wgt[0][i]
+ * deps[i]
+ * deps[i]
+ + 2
+ * wgt[1][i]
+ * deps[i]
+ * dzp[i]
+ + wgt[2][i]
+ * dzp[i]
+ * dzp[i];
+ // *
+ // * derivatives (deriv1) of perigee param. w.r.t. x,y,z (vertex)
+ d11 = sinf;
+ d12 = -cosf;
+ d21 = -cosf * cotth;
+ d22 = -sinf * cotth;
+ d41 = -cosf * par[4][i];
+ d42 = -sinf * par[4][i];
+ // *
+ // * matrix dw = (deriv1)t * weight
+ dw11 = d11 * wgt[0][i] + d21 * wgt[1][i] + d41 * wgt[6][i];
+ dw12 = d11 * wgt[1][i] + d21 * wgt[2][i] + d41 * wgt[7][i];
+ dw13 = d11 * wgt[3][i] + d21 * wgt[4][i] + d41 * wgt[8][i];
+ dw14 = d11 * wgt[6][i] + d21 * wgt[7][i] + d41 * wgt[9][i];
+ dw15 = d11 * wgt[10][i] + d21 * wgt[11][i] + d41 * wgt[13][i];
+ dw21 = d12 * wgt[0][i] + d22 * wgt[1][i] + d42 * wgt[6][i];
+ dw22 = d12 * wgt[1][i] + d22 * wgt[2][i] + d42 * wgt[7][i];
+ dw23 = d12 * wgt[3][i] + d22 * wgt[4][i] + d42 * wgt[8][i];
+ dw24 = d12 * wgt[6][i] + d22 * wgt[7][i] + d42 * wgt[9][i];
+ dw25 = d12 * wgt[10][i] + d22 * wgt[11][i] + d42 * wgt[13][i];
+ dw31 = wgt[1][i];
+ dw32 = wgt[2][i];
+ dw33 = wgt[4][i];
+ dw34 = wgt[7][i];
+ dw35 = wgt[11][i];
+ // *
+ // * summation of dw * dpar to vector tv
+ tv[0] = tv[0] + dw11 * deps[i] + dw12 * dzp[i];
+ tv[1] = tv[1] + dw21 * deps[i] + dw22 * dzp[i];
+ tv[2] = tv[2] + dw31 * deps[i] + dw32 * dzp[i];
+ // *
+ // * derivatives (deriv2) of perigee param. w.r.t. theta,phi,1/r (vertex)
+ e12 = uu;
+ e13 = -.5 * uu * uu;
+ e21 = -uu * (1. + cotth * cotth);
+ e22 = -vv * cotth;
+ e23 = uu * vv * cotth;
+ e43 = -uu;
+ // *
+ // * matrix ew = (deriv2)t * weight
+ ew11 = e21 * wgt[1][i] + wgt[3][i];
+ ew12 = e21 * wgt[2][i] + wgt[4][i];
+ ew13 = e21 * wgt[4][i] + wgt[5][i];
+ ew14 = e21 * wgt[7][i] + wgt[8][i];
+ ew15 = e21 * wgt[11][i] + wgt[12][i];
+ ew21 = e12 * wgt[0][i] + e22 * wgt[1][i] + wgt[6][i];
+ ew22 = e12 * wgt[1][i] + e22 * wgt[2][i] + wgt[7][i];
+ ew23 = e12 * wgt[3][i] + e22 * wgt[4][i] + wgt[8][i];
+ ew24 = e12 * wgt[6][i] + e22 * wgt[7][i] + wgt[9][i];
+ ew25 = e12 * wgt[10][i] + e22 * wgt[11][i] + wgt[13][i];
+ ew31 = e13 * wgt[0][i] + e23 * wgt[1][i] + e43 * wgt[6][i] + wgt[10][i];
+ ew32 = e13 * wgt[1][i] + e23 * wgt[2][i] + e43 * wgt[7][i] + wgt[11][i];
+ ew33 = e13 * wgt[3][i] + e23 * wgt[4][i] + e43 * wgt[8][i] + wgt[12][i];
+ ew34 = e13 * wgt[6][i] + e23 * wgt[7][i] + e43 * wgt[9][i] + wgt[13][i];
+ ew35 = e13 * wgt[10][i] + e23 * wgt[11][i] + e43 * wgt[13][i] + wgt[14][i];
+ // *
+ // * computation of vector tt = ew * dpar
+ tt[0][i] = ew11 * deps[i] + ew12 * dzp[i];
+ tt[1][i] = ew21 * deps[i] + ew22 * dzp[i];
+ tt[2][i] = ew31 * deps[i] + ew32 * dzp[i];
+ // *
+ // * summation of (deriv1)t * weight * (deriv1) to matrix wa
+ wa[0] = wa[0] + dw11 * d11 + dw12 * d21 + dw14 * d41;
+ wa[1] = wa[1] + dw11 * d12 + dw12 * d22 + dw14 * d42;
+ wa[2] = wa[2] + dw21 * d12 + dw22 * d22 + dw24 * d42;
+ wa[3] = wa[3] + dw12;
+ wa[4] = wa[4] + dw22;
+ wa[5] = wa[5] + dw32;
+ // *
+ // * computation of matrix wb = (deriv1)t * weight * (deriv2)
+ wb[0][i] = dw12 * e21 + dw13;
+ wb[1][i] = dw22 * e21 + dw23;
+ wb[2][i] = dw32 * e21 + dw33;
+ wb[3][i] = dw11 * e12 + dw12 * e22 + dw14;
+ wb[4][i] = dw21 * e12 + dw22 * e22 + dw24;
+ wb[5][i] = dw31 * e12 + dw32 * e22 + dw34;
+ wb[6][i] = dw11 * e13 + dw12 * e23 + dw14 * e43 + dw15;
+ wb[7][i] = dw21 * e13 + dw22 * e23 + dw24 * e43 + dw25;
+ wb[8][i] = dw31 * e13 + dw32 * e23 + dw34 * e43 + dw35;
+ // *
+ // * computation of matrix wc = (deriv2)t * weight * (deriv2)
+ wc[0][i] = ew12 * e21 + ew13;
+ wc[1][i] = ew22 * e21 + ew23;
+ wc[3][i] = ew32 * e21 + ew33;
+ wc[2][i] = ew21 * e12 + ew22 * e22 + ew24;
+ wc[4][i] = ew31 * e12 + ew32 * e22 + ew34;
+ wc[5][i] = ew31 * e13 + ew32 * e23 + ew34 * e43 + ew35;
+ // *
+ // * computation of matrices wci = (wc)**(-1) and wbci = wb * wci
+
+ // Not very efficient here...
+ double[] tmp = new double[6];
+ for (int j = 0; j < 6; ++j) {
+ tmp[j] = wc[j][i];
+ System.out.println("tmp[" + j + "]= " + tmp[j]);
+ }
+ double[] tmpinv = new double[6];
+ tmpinv = pxmi3(tmp);
+ for (int j = 0; j < 6; ++j) {
+ wci[j][i] = tmpinv[j];
+ }
+ // wci[0][i] = pxmi3(wc[0][i]);
+
+ wbci[0][i] = wb[0][i] * wci[0][i] + wb[3][i] * wci[1][i] + wb[6][i] * wci[3][i];
+ wbci[1][i] = wb[1][i] * wci[0][i] + wb[4][i] * wci[1][i] + wb[7][i] * wci[3][i];
+ wbci[2][i] = wb[2][i] * wci[0][i] + wb[5][i] * wci[1][i] + wb[8][i] * wci[3][i];
+ wbci[3][i] = wb[0][i] * wci[1][i] + wb[3][i] * wci[2][i] + wb[6][i] * wci[4][i];
+ wbci[4][i] = wb[1][i] * wci[1][i] + wb[4][i] * wci[2][i] + wb[7][i] * wci[4][i];
+ wbci[5][i] = wb[2][i] * wci[1][i] + wb[5][i] * wci[2][i] + wb[8][i] * wci[4][i];
+ wbci[6][i] = wb[0][i] * wci[3][i] + wb[3][i] * wci[4][i] + wb[6][i] * wci[5][i];
+ wbci[7][i] = wb[1][i] * wci[3][i] + wb[4][i] * wci[4][i] + wb[7][i] * wci[5][i];
+ wbci[8][i] = wb[2][i] * wci[3][i] + wb[5][i] * wci[4][i] + wb[8][i] * wci[5][i];
+ // *
+ // * subtraction of wbci * (wb)t from matrix wa
+ wa[0] = wa[0] - wbci[0][i] * wb[0][i] - wbci[3][i] * wb[3][i] - wbci[6][i] * wb[6][i];
+ wa[1] = wa[1] - wbci[0][i] * wb[1][i] - wbci[3][i] * wb[4][i] - wbci[6][i] * wb[7][i];
+ wa[2] = wa[2] - wbci[1][i] * wb[1][i] - wbci[4][i] * wb[4][i] - wbci[7][i] * wb[7][i];
+ wa[3] = wa[3] - wbci[0][i] * wb[2][i] - wbci[3][i] * wb[5][i] - wbci[6][i] * wb[8][i];
+ wa[4] = wa[4] - wbci[1][i] * wb[2][i] - wbci[4][i] * wb[5][i] - wbci[7][i] * wb[8][i];
+ wa[5] = wa[5] - wbci[2][i] * wb[2][i] - wbci[5][i] * wb[5][i] - wbci[8][i] * wb[8][i];
+ // *
+ // * subtraction of wbci * tt from vector tv
+ tv[0] = tv[0] - wbci[0][i] * tt[0][i] - wbci[3][i] * tt[1][i] - wbci[6][i] * tt[2][i];
+ tv[1] = tv[1] - wbci[1][i] * tt[0][i] - wbci[4][i] * tt[1][i] - wbci[7][i] * tt[2][i];
+ tv[2] = tv[2] - wbci[2][i] * tt[0][i] - wbci[5][i] * tt[1][i] - wbci[8][i] * tt[2][i];
+ // *
+ }// check on whether to use track
+ }// loop over tracks
+
+ // beam constraint
+ if (fitwb) {
+ wa[0] += 1. / (sigxbe * sigxbe);
+ wa[2] += 1. / (sigxbe * sigybe);
+ wa[5] += 1. / (sigzbe * sigzbe);
+ tv[0] += (beamoy[0] - xyz[0]) / (sigxbe * sigxbe);
+ tv[1] += (beamoy[1] - xyz[1]) / (sigybe * sigybe);
+ tv[2] += (beamoy[2] - xyz[2]) / (sigzbe * sigzbe);
+ }
+ // *
+ // * solution of the linear system
+ // *
+ // * covariance matrix on vertex
+ vcov = pxmi3(wa);
+ for (int ii = 0; ii < vcov.length; ++ii) {
+ System.out.println("vcov[" + ii + "]= " + vcov[ii]);
+ }
+ //
+ // *
+ // * corrections to vertex coordinates
+ dxyz[0] = vcov[0] * tv[0] + vcov[1] * tv[1] + vcov[3] * tv[2];
+ dxyz[1] = vcov[1] * tv[0] + vcov[2] * tv[1] + vcov[4] * tv[2];
+ dxyz[2] = vcov[3] * tv[0] + vcov[4] * tv[1] + vcov[5] * tv[2];
+ xyzf[0] = xyz[0] + dxyz[0];
+ xyzf[1] = xyz[1] + dxyz[1];
+ xyzf[2] = xyz[2] + dxyz[2];
+ // *
+ // * corrections to track parameters and covariance matrices
+ // *
+ for (int i = 0; i < ntrk; ++i) {
+ if (invtx[i]) {
+ // *
+ // * variation on par is wci * tt - (wbci)t * dxyz
+ parf[0][i] = par[2][i]
+ + wci[0][i]
+ * tt[0][i]
+ + wci[1][i]
+ * tt[1][i]
+ + wci[3][i]
+ * tt[2][i]
+ - wbci[0][i]
+ * dxyz[0]
+ - wbci[1][i]
+ * dxyz[1]
+ - wbci[2][i]
+ * dxyz[2];
+ parf[1][i] = phiv[i]
+ + wci[1][i]
+ * tt[0][i]
+ + wci[2][i]
+ * tt[1][i]
+ + wci[4][i]
+ * tt[2][i]
+ - wbci[3][i]
+ * dxyz[0]
+ - wbci[4][i]
+ * dxyz[1]
+ - wbci[5][i]
+ * dxyz[2];
+ parf[2][i] = par[4][i]
+ + wci[3][i]
+ * tt[0][i]
+ + wci[4][i]
+ * tt[1][i]
+ + wci[5][i]
+ * tt[2][i]
+ - wbci[6][i]
+ * dxyz[0]
+ - wbci[7][i]
+ * dxyz[1]
+ - wbci[8][i]
+ * dxyz[2];
+ // *
+ // * covariance matrix of par is wci + (wbci)t * vcov * wbci
+ tcov[0][i] = wci[0][i]
+ + wbci[0][i]
+ * (vcov[0] * wbci[0][i] + vcov[1] * wbci[1][i] + vcov[3] * wbci[2][i])
+ + wbci[1][i]
+ * (vcov[1] * wbci[0][i] + vcov[2] * wbci[1][i] + vcov[4] * wbci[2][i])
+ + wbci[2][i]
+ * (vcov[3] * wbci[0][i] + vcov[4] * wbci[1][i] + vcov[5] * wbci[2][i]);
+
+ tcov[1][i] = wci[1][i]
+ + wbci[0][i]
+ * (vcov[0] * wbci[3][i] + vcov[1] * wbci[4][i] + vcov[3] * wbci[5][i])
+ + wbci[1][i]
+ * (vcov[1] * wbci[3][i] + vcov[2] * wbci[4][i] + vcov[4] * wbci[5][i])
+ + wbci[2][i]
+ * (vcov[3] * wbci[3][i] + vcov[4] * wbci[4][i] + vcov[5] * wbci[5][i]);
+
+ tcov[2][i] = wci[2][i]
+ + wbci[3][i]
+ * (vcov[0] * wbci[3][i] + vcov[1] * wbci[4][i] + vcov[3] * wbci[5][i])
+ + wbci[4][i]
+ * (vcov[1] * wbci[3][i] + vcov[2] * wbci[4][i] + vcov[4] * wbci[5][i])
+ + wbci[5][i]
+ * (vcov[3] * wbci[3][i] + vcov[4] * wbci[4][i] + vcov[5] * wbci[5][i]);
+
+ tcov[3][i] = wci[3][i]
+ + wbci[0][i]
+ * (vcov[0] * wbci[6][i] + vcov[1] * wbci[7][i] + vcov[3] * wbci[8][i])
+ + wbci[1][i]
+ * (vcov[1] * wbci[6][i] + vcov[2] * wbci[7][i] + vcov[4] * wbci[8][i])
+ + wbci[2][i]
+ * (vcov[3] * wbci[6][i] + vcov[4] * wbci[7][i] + vcov[5] * wbci[8][i]);
+
+ tcov[4][i] = wci[4][i]
+ + wbci[3][i]
+ * (vcov[0] * wbci[6][i] + vcov[1] * wbci[7][i] + vcov[3] * wbci[8][i])
+ + wbci[4][i]
+ * (vcov[1] * wbci[6][i] + vcov[2] * wbci[7][i] + vcov[4] * wbci[8][i])
+ + wbci[5][i]
+ * (vcov[3] * wbci[6][i] + vcov[4] * wbci[7][i] + vcov[5] * wbci[8][i]);
+
+ tcov[5][i] = wci[5][i]
+ + wbci[6][i]
+ * (vcov[0] * wbci[6][i] + vcov[1] * wbci[7][i] + vcov[3] * wbci[8][i])
+ + wbci[7][i]
+ * (vcov[1] * wbci[6][i] + vcov[2] * wbci[7][i] + vcov[4] * wbci[8][i])
+ + wbci[8][i]
+ * (vcov[3] * wbci[6][i] + vcov[4] * wbci[7][i] + vcov[5] * wbci[8][i]);
+
+ // *
+ } // check on whether to use track
+ } // loop over tracks
+
+ // *
+ // * chi2 with fitted values
+ // *
+ chi2 = 0.;
+ if (fitwb)
+ chi2 += ( (xyzf[0] - beamoy[0]) / sigxbe)
+ * ( (xyzf[0] - beamoy[0]) / sigxbe)
+ + ( (xyzf[1] - beamoy[1]) / sigybe)
+ * ( (xyzf[1] - beamoy[1]) / sigybe)
+ + ( (xyzf[2] - beamoy[2]) / sigzbe)
+ * ( (xyzf[2] - beamoy[2]) / sigzbe);
+
+ for (int i = 0; i < ntrk; ++i) {
+ if (invtx[i]) {
+ uu = xyzf[0] * cos(parf[1][i]) + xyzf[1] * sin(parf[1][i]);
+ vv = xyzf[1] * cos(parf[1][i]) - xyzf[0] * sin(parf[1][i]);
+ epsf = -vv - .5 * uu * uu * parf[2][i];
+ zpf = xyzf[2] - uu / tan(parf[0][i]);
+ phif = parf[1][i] - uu * parf[2][i];
+ // not very efficient here...
+ double[] tmp = new double[15];
+ for (int j = 0; j < 15; ++j) {
+ tmp[j] = wgt[j][i];
+ }
+ chi2tr[i] = fwdch2(tmp, epsf - par[0][i], zpf - par[1][i], parf[0][i] - par[2][i], phif - par[3][i],
+ parf[2][i] - par[4][i]);
+ chi2 += chi2tr[i];
+ }
+ }
+ System.out.println("chi2= " + chi2);
+ return new Vertex(ntrk, xyzf, parf, vcov, tcov, chi2, chi2tr);
+
+ }
+
+}
lcsim/src/org/lcsim/recon/vertexing/zvtop4/fitter
diff -u -r1.2 -r1.3
--- VertexFitter.java 22 Mar 2006 09:36:27 -0000 1.2
+++ VertexFitter.java 27 Mar 2006 00:00:21 -0000 1.3
@@ -1,934 +1,21 @@
-package org.lcsim.recon.vertexing.zvtop4.fitter;
-
-/**
- * @version $Id: VertexFitter.java,v 1.2 2006/03/22 09:36:27 jstrube Exp $
+/*
+ * VertexFitter.java
+ *
+ * Created on March 25, 2006, 4:54 PM
+ *
+ * @version $Id: VertexFitter.java,v 1.3 2006/03/27 00:00:21 jstrube Exp $
*/
-// Performs a Kalman fit to a list of tracks and returns
-// a Vertex object
-import static java.lang.Math.sin;
-import static java.lang.Math.cos;
-import static java.lang.Math.tan;
-import static java.lang.Math.atan;
-import static java.lang.Math.atan2;
-import static java.lang.Math.sqrt;
-import static java.lang.Math.PI;
-import static org.lcsim.recon.tracking.trfutil.TRFMath.fmod1;
-
-import Jama.Matrix;
-
-
-public class VertexFitter {
- // the value of the magnetic field in the vicinity of the vertex
- // default is a constant field along the z axis
- private double _bField;
-
- // constructor
- public VertexFitter(double bField) {
- _bField = bField;
- }
-
- // fitter
- public Vertex fit(int ntrk, boolean fitwb, boolean[] invtx, double[][] par, double[][] wgt, double[] xyz) {
- return pxfvtx(ntrk, fitwb, invtx, par, wgt, xyz);
- }
-
- private double[] pxmi5(double[] wgt) {
-
- // ***********************************************************************
- // * *
- // * *
- // * inversion of a (5x5) symmetric positive matrix (cholesky method) *
- // * internal computation in double precision *
- // * check on positivity (ierr=2 if wgt not positive) *
- // * *
- // ***********************************************************************
- // *
- double[] cov = new double[15];
- double t11, t12, t13, t14, t15;
- double t22, t23, t24, t25;
- double t33, t34, t35;
- double t44, t45;
- double t55;
- double s12, s13, s14, s15;
- double s23, s24, s25;
- double s34, s35;
- double s45;
- if (wgt[0] < 0.)
- throw new IllegalArgumentException("Bad weight matrix!");
-
- t11 = 1. / sqrt(wgt[0]);
- s12 = wgt[1] * t11;
- s13 = wgt[3] * t11;
- s14 = wgt[6] * t11;
- s15 = wgt[10] * t11;
- //
- t22 = wgt[2] - s12 * s12;
- if (t22 < 0.)
- throw new IllegalArgumentException("Bad weight matrix!");
-
- t22 = 1. / sqrt(t22);
- s23 = (wgt[4] - s12 * s13) * t22;
- s24 = (wgt[7] - s12 * s14) * t22;
- s25 = (wgt[11] - s12 * s15) * t22;
- //
- t33 = wgt[5] - s13 * s13 - s23 * s23;
- if (t33 < 0.)
- throw new IllegalArgumentException("Bad weight matrix!");
-
- t33 = 1. / sqrt(t33);
- s34 = (wgt[8] - s13 * s14 - s23 * s24) * t33;
- s35 = (wgt[12] - s13 * s15 - s23 * s25) * t33;
- //
- t44 = wgt[9] - s14 * s14 - s24 * s24 - s34 * s34;
- if (t44 < 0.)
- throw new IllegalArgumentException("Bad weight matrix!");
-
- t44 = 1. / sqrt(t44);
- s45 = (wgt[13] - s14 * s15 - s24 * s25 - s34 * s35) * t44;
- //
- t55 = wgt[14] - s15 * s15 - s25 * s25 - s35 * s35 - s45 * s45;
- if (t55 < 0.)
- throw new IllegalArgumentException("Bad weight matrix!");
-
- t55 = 1. / sqrt(t55);
- //
- t45 = -t44 * (s45 * t55);
- t34 = -t33 * (s34 * t44);
- t35 = -t33 * (s34 * t45 + s35 * t55);
- t23 = -t22 * (s23 * t33);
- t24 = -t22 * (s23 * t34 + s24 * t44);
- t25 = -t22 * (s23 * t35 + s24 * t45 + s25 * t55);
- t12 = -t11 * (s12 * t22);
- t13 = -t11 * (s12 * t23 + s13 * t33);
- t14 = -t11 * (s12 * t24 + s13 * t34 + s14 * t44);
- t15 = -t11 * (s12 * t25 + s13 * t35 + s14 * t45 + s15 * t55);
- //
- cov[0] = t11 * t11 + t12 * t12 + t13 * t13 + t14 * t14 + t15 * t15;
- cov[1] = t12 * t22 + t13 * t23 + t14 * t24 + t15 * t25;
- cov[2] = t22 * t22 + t23 * t23 + t24 * t24 + t25 * t25;
- cov[3] = t13 * t33 + t14 * t34 + t15 * t35;
- cov[4] = t23 * t33 + t24 * t34 + t25 * t35;
- cov[5] = t33 * t33 + t34 * t34 + t35 * t35;
- cov[6] = t14 * t44 + t15 * t45;
- cov[7] = t24 * t44 + t25 * t45;
- cov[8] = t34 * t44 + t35 * t45;
- cov[9] = t44 * t44 + t45 * t45;
- cov[10] = t15 * t55;
- cov[11] = t25 * t55;
- cov[12] = t35 * t55;
- cov[13] = t45 * t55;
- cov[14] = t55 * t55;
- return cov;
- }
-
- private double fwdch2(double[] wgt, double d1, double d2, double d3, double d4, double d5) {
- // ***********************************************************************
- // * *
- // * computation of dt*wgt*d *
- // * wgt is a (5x5) symmetric matrix ; d is a 5-vector *
- // * *
- // ***********************************************************************
- // *
- return wgt[0]
- * d1
- * d1
- + wgt[2]
- * d2
- * d2
- + wgt[5]
- * d3
- * d3
- + wgt[9]
- * d4
- * d4
- + wgt[14]
- * d5
- * d5
- + 2
- * (d2 * d1 * wgt[1] + d3 * (d1 * wgt[3] + d2 * wgt[4]) + d4 * (d1 * wgt[6] + d2 * wgt[7] + d3 * wgt[8]) + d5
- * (d1 * wgt[10] + d2 * wgt[11] + d3 * wgt[12] + d4 * wgt[13]));
- }
-
- // ************************************************************************
- // * *
- public Perigee perigee(double[] rawdat, double xb, double yb) {
- // * *
- // ************************************************************************
- // name : perigee
- //
- // created : 30-jun-1998 author : norman a. graf
- // based on pxfxpe 7-jun-1989 author : p. billoir (cdf)
- //
- // function : compute from input data (with cylindrical or cartesian
- // coordinates)
- // the "perigee" parameters (point of closest approach to the
- // reference axis) and the covariance matrix on them
- // for error propagation, an approximation is made, assuming
- // that the r coordinate of reference point is small w.r.t.
- // the radius of curvature
- // no account is taken for multiple scattering, so that the
- // error matrix is underestimated if the data reference point
- // is outside the beam pipe (except at high momentum)
- //
- // references : none
- //
- // arguments : input : dat(1:21) : tix array
- // dat(1:3) : coord. of a point on the track
- // if "cylindrical" coord. : r,r*phi,z
- // if "cartesian" coord. : x,y,z
- // dat(4:5) : theta, phi of the tangent at this point
- // dat[5] : 1/p with geometrical sign
- // dat(7:21) : covariance matrix on 5 "free" param. :
- // rphi,z,theta,phi,1/p if "cylind." coord. (r fixed)
- // x,y,theta,phi,1/p if "cartes." coord. (z fixed)
- //
- // xb,yb : coordinates of the reference axis
- //
- // output : par(1:5) : "perigee" parameters
- // 1 : epsilon (impact par. in xy projection, with sign)
- // 2 : z coordinate
- // 3 : theta angle
- // 4 : phi angle
- // 5 : 1/r (r = radius of curvature, with sign)
- // cov(1:15) : covariance matrix of par
- // wgt(1:15) : weight matrix of par (inverse of cov)
- //
- // errors : ierr = 1 : incorrect data in tkr array
- // (for example not cylindrical coordinates)
- // 2 : cov not positive (wgt not computed)
- //
- // ************************************************************************
- //
- //
- // conversion from d0 cylindrical coordinates to mine...
- //
- double sthet, sthet2, cthet, gamma;
- double d0der11, d0der43, d0der45, d0der55;
- // double[] rawdat = new double [21];
- //
- double cosf, sinf, r0, rcosb, d11, d12, d21, d22, cov1, cov3, cov4, cov7, cov11;
- // to define the magnetic field - values to be provided :
- // FIXME this needs to be replaced by the official org.lcsim constant
- double consa = 0.0003;
- double bmag = _bField;
- double consb = consa * bmag;
-
- // consa = velocity of light (here in GeV/(mm.T) ; depends on units)
- // bmag = magnetic field along z axis
- //
- //
- double[] par = new double[5];
- double[] cov = new double[15];
- double[] wgt = new double[15];
- double sgn;
- double[] dat = new double[21];
- //
- // local variables
- // __________________
- //
- int ierr, ii;
- double capphi, cotth, dphi, rdphi, rtrk, xc, x0, yc, y0;
- double der1, der2, der11, der14, der15, der23, der45;
- //
- //
- // executable statements
- // ________________________
- //
- double twopi = 2. * PI;
- double halfpi = PI / 2.;
-
- int icypl = 1;
- // icypl = 1 if "cylindrical" coordinates, 0 if "plane" coordinates
- //
- // convert here...
- //
- //
- // ::: parameters r, rphi, z, theta, phi, 1/p
- //
- dat[0] = rawdat[0]; // r
- dat[1] = rawdat[0] * rawdat[1]; // r*phi
- dat[2] = rawdat[2]; // z
- dat[3] = atan(1. / rawdat[4]); // theta
- if (rawdat[4] < 0)
- dat[3] = dat[3] + PI;
- dat[4] = rawdat[1] + rawdat[3]; // phi (recall beta= phi(dir)-phi(pos) )
- dat[5] = -rawdat[5] * sin(dat[3]); // 1/p (recall 1/pt = 1/(p*sin(theta))
- //
- // ::: covariance matrix
- // ::: there are seven terms in the conversion from
- //
- // phi rphi
- // z z
- // alfa to theta
- // cot(theta) phi
- // q/pt 1/p
- //
- // d0der14 = 1
- // d0der22 = 1
- // d0der34 = -1
- //
- sthet = sin(dat[3]);
- sthet2 = sthet * sthet;
- cthet = cos(dat[3]);
- gamma = -rawdat[5] * cthet * sthet2;
-
- d0der11 = rawdat[0];
- d0der43 = -sthet2;
- d0der45 = -rawdat[5] * cthet * sthet2;
- d0der55 = sthet;
- //
- dat[6] = rawdat[6] * rawdat[0] * rawdat[0];
- dat[7] = rawdat[7] * rawdat[0];
- dat[8] = rawdat[8];
- dat[9] = -rawdat[12] * rawdat[0] * sthet2;
- dat[10] = -rawdat[13] * sthet2;
- dat[11] = rawdat[15] * sthet2 * sthet2;
- dat[12] = (rawdat[6] - rawdat[9]) * rawdat[0];
- dat[13] = rawdat[7] - rawdat[10];
- dat[14] = (rawdat[14] - rawdat[12]) * sthet2;
- dat[15] = rawdat[6] - 2 * rawdat[9] + rawdat[11];
- dat[16] = - (rawdat[16] * sthet + rawdat[12] * gamma) * rawdat[0];
- dat[17] = -rawdat[17] * sthet - rawdat[13] * gamma;
- dat[18] = (rawdat[19] * sthet + rawdat[15] * gamma) * sthet2;
- dat[19] = (rawdat[14] - rawdat[12]) * gamma + (rawdat[18] - rawdat[16]) * sthet;
- dat[20] = (rawdat[15] * gamma + 2 * rawdat[19] * sthet) * gamma + rawdat[20] * sthet2;
-
- //
- // icypl = 1 if "cylindrical" coordinates, 0 if "plane" coordinates
- //
- if ( (icypl == 1 && dat[0] == 0.) || dat[3] == 0. || dat[5] == 0.) {
- System.out.println("******error in perigee*******");
- throw new IllegalArgumentException("Choke!");
- }
-
- //
- // now computation of "perigee" parameters
- //
- capphi = dat[1] / dat[0];
- if (icypl == 1) {
- x0 = dat[0] * cos(capphi) - xb;
- y0 = dat[0] * sin(capphi) - yb;
- } else {
- x0 = dat[0] - xb;
- y0 = dat[1] - yb;
- }
- rtrk = sin(dat[3]) / (consb * dat[5]);
- cosf = cos(dat[4]);
- sinf = sin(dat[4]);
- xc = x0 - rtrk * sinf;
- yc = y0 + rtrk * cosf;
- // epsilon (impact parameter in xy projection, with geometrical sign)
- sgn = 1.;
- if (rtrk < 0.)
- sgn = -1.;
- par[0] = rtrk - sgn * sqrt(xc * xc + yc * yc);
- // phi at perigee (range 0,2*PI)
- par[3] = atan2(yc, xc) + PI + sgn * halfpi;
- if (par[3] < 0.)
- par[3] = par[3] + twopi;
- else if (par[3] > twopi)
- par[3] = par[3] - twopi;
- // variation of phi from reference point of tkr to perigee (range -pi,+pi)
- dphi = fmod1(par[3] - dat[4] + twopi + PI, twopi) - PI;
- rdphi = rtrk * dphi;
- cotth = 1. / tan(dat[3]);
- // z of perigee
- par[1] = dat[2] + cotth * rdphi;
- // theta and 1/r at perigee (unchanged)
- par[2] = dat[3];
- par[4] = 1. / rtrk;
- //
- // computation of covariance matrix
- //
- for (ii = 0; ii < 15; ++ii) {
- cov[ii] = dat[6 + ii];
- }
- //
- // transformation from 1/p to 1/r = consb * (1/p) / sin(theta)
- //
- der1 = -cotth * par[4];
- der2 = par[4] / dat[5];
- cov[14] = der1 * der1 * cov[5] + 2 * der1 * der2 * cov[12] + der2 * der2 * cov[14];
- cov[10] = der1 * cov[3] + der2 * cov[10];
- cov[11] = der1 * cov[4] + der2 * cov[11];
- cov[12] = der1 * cov[5] + der2 * cov[12];
- cov[13] = der1 * cov[8] + der2 * cov[13];
- //
- // if cartesian coordinates transformation from (x,y) to (r*phi,z)
- // (neglecting curvature effects )
- //
- if (icypl == 0) {
- r0 = sqrt(dat[0] * dat[0] + dat[1] * dat[1]);
- rcosb = dat[0] * cosf + dat[1] * sinf;
- d11 = -r0 * sinf / rcosb;
- d12 = r0 * cosf / rcosb;
- d21 = -dat[0] * cotth / rcosb;
- d22 = -dat[1] * cotth / rcosb;
- cov1 = d11 * d11 * cov[0] + 2 * d11 * d12 * cov[1] + d12 * d12 * cov[2];
- cov3 = d21 * d21 * cov[0] + 2 * d21 * d22 * cov[1] + d22 * d22 * cov[2];
- cov[1] = d11 * d21 * cov[0] + (d12 * d21 + d11 * d22) * cov[1] + d12 * d22 * cov[2];
- cov[0] = cov1;
- cov[2] = cov3;
- cov4 = d11 * cov[3] + d12 * cov[4];
- cov[4] = d21 * cov[3] + d22 * cov[4];
- cov[3] = cov4;
- cov7 = d11 * cov[6] + d12 * cov[7];
- cov[7] = d21 * cov[6] + d22 * cov[7];
- cov[6] = cov7;
- cov11 = d11 * cov[10] + d12 * cov[11];
- cov[11] = d21 * cov[10] + d22 * cov[11];
- cov[10] = cov11;
- }
- //
- // transformation from (r*phi,z0,theta,phi0,1/r)
- // to (epsilon,zp,theta,phip,1/r)
- //
- // approximation for derivatives d(epsilon)/d(r*phi), d(epsilon)/d(phi0),
- // d(epsilon)/d(1/r), d(zp)/d(z0) and d(phip)/d(1/r)
- // the other ones are exactly or approximately 1 (diagonal terms)
- // or 0 (non-diagonal terms)
- //
- der11 = rdphi / sqrt(x0 * x0 + y0 * y0);
- der14 = -rdphi;
- der15 = -rdphi * rdphi / 2.;
- der23 = - (1. + cotth * cotth) * rdphi;
- der45 = rdphi;
- //
- cov[0] = der11
- * der11
- * cov[0]
- + 2
- * der11
- * (der14 * cov[6] + der15 * cov[10])
- + der14
- * (der14 * cov[9] + 2 * der15 * cov[13])
- + der15
- * der15
- * cov[14];
- cov[1] = der11
- * (cov[1] + der23 * cov[3])
- + der14
- * (cov[7] + der23 * cov[8])
- + der15
- * (cov[11] + der23 * cov[12]);
- cov[2] = cov[2] + 2 * der23 * cov[4] + der23 * der23 * cov[5];
- cov[3] = der11 * cov[3] + der14 * cov[8] + der15 * cov[12];
- cov[4] = cov[4] + der23 * cov[5];
- cov[6] = der11
- * (cov[6] + der45 * cov[10])
- + der14
- * (cov[9] + der45 * cov[13])
- + der15
- * (cov[13] + der45 * cov[14]);
- cov[7] = cov[7] + der23 * cov[8] + der45 * (cov[11] + der23 * cov[12]);
- cov[8] = cov[8] + der45 * cov[12];
- cov[9] = cov[9] + 2 * der45 * cov[13] + der45 * der45 * cov[14];
- cov[10] = der11 * cov[10] + der14 * cov[13] + der15 * cov[14];
- cov[11] = cov[11] + der23 * cov[12];
- cov[13] = cov[13] + der45 * cov[14];
- //
- // invert matrix cov to get wgt
- //
- wgt = pxmi5(cov);
- //
- return new Perigee(par, cov, wgt);
-
- }
-
- private double[] pxmi3(double[] wgt) {
- double[][] x = new double[3][3];
- x[0][0] = wgt[0];
- x[0][1] = wgt[1];
- x[1][0] = wgt[1];
- x[0][2] = wgt[2];
- x[2][0] = wgt[2];
- x[1][1] = wgt[3];
- x[1][2] = wgt[4];
- x[2][1] = wgt[4];
- x[2][2] = wgt[5];
- Matrix xMat = new Matrix(x);
- double[][] y = xMat.inverse().getArray();
- return new double[] {y[0][0], y[0][1], y[0][2], y[1][1], y[1][2], y[2][2]};
- }
-
- private double[] pxmi3_old(double[] wgt) {
- // ***********************************************************************
- // * *
- // * *
- // * inversion of a (3x3) symmetric positive matrix (cholesky method) *
- // * internal computation in double precision *
- // * check on positivity (ierr=2 if wgt not positive) *
- // * cov (output) may overwrite wgt (input) *
- // * *
- // ***********************************************************************
- // *
-
- double[] cov = new double[6];
- double t11, t12, t13;
- double t21, t22, t23;
- double t33;
- double s12, s13;
- double s23;
-
- if (wgt[0] < 0.)
- throw new IllegalArgumentException("Bad weight matrix!");
-
- t11 = 1. / sqrt(wgt[0]);
- s12 = wgt[1] * t11;
- s13 = wgt[3] * t11;
- //
- t22 = wgt[2] - s12 * s12;
- if (t22 < 0.)
- throw new IllegalArgumentException("Bad weight matrix!");
-
- t22 = 1. / sqrt(t22);
- s23 = (wgt[4] - s12 * s13) * t22;
- //
- t33 = wgt[5] - s13 * s13 - s23 * s23;
- if (t33 < 0.)
- throw new IllegalArgumentException("Bad weight matrix!");
-
- t33 = 1. / sqrt(t33);
- //
- t23 = -t22 * (s23 * t33);
- t12 = -t11 * (s12 * t22);
- t13 = -t11 * (s12 * t23 + s13 * t33);
- //
- cov[0] = t11 * t11 + t12 * t12 + t13 * t13;
- cov[1] = t12 * t22 + t13 * t23;
- cov[2] = t22 * t22 + t23 * t23;
- cov[3] = t13 * t33;
- cov[4] = t23 * t33;
- cov[5] = t33 * t33;
- return cov;
- }
-
- // ************************************************************************
- // * *
- public Vertex pxfvtx(int ntrk, boolean fitwb, boolean[] invtx, double[][] par, double[][] wgt, double[] xyz) {
- // * *
- // ************************************************************************
- // * name : pxfvtx
- // *
- // * created : 5-jan-1989 author : p. billoir (cdf)
- // *
- // * function : perform a full vertex vertex fit with weighted mean
- // * of tracks considered as ellipsoids in 5d space of
- // * "perigee" parameters (epsilon,zp,theta,phi,1/r)
- // *
- // ******** 1-12-90 changes in pxfvtx routine : p. billoir
- // * - to allow the use of the beam spot constraint (logical var. fitwb)
- // * - to know the contribution of individual tracks to the total chi2.
- // *
- // * references : none
- // *
- // * arguments : input : ntrk : number of tracks
- // * fitwb = .true. if we want the beam spot
- // * constraint
- // * invtx(i)=.true. to use the track i in the
- // * vertex fit
- // * par(1:5,1:ntrk) : "perigee" parameters
- // * 1 : epsilon (impact par. in xy projection, with sign)
- // * 2 : z coordinate
- // * 3 : theta angle
- // * 4 : phi angle
- // * 5 : 1/r (r = radius of curvature, with sign)
- // * wgt(1:15,1:ntrk) : weight matrix on par
- // * xyz(1:3) : approximate coordinates of the vertex
- // *
- // * output : xyzf(1:3) : fitted coordinates of the vertex
- // * parf(1:3,1:ntrk) : fitted parameters at the vertex
- // * 1 : theta
- // * 2 : phi
- // * 3 : 1/r
- // * vcov(1:6) : covariance matrix on xyz(vertex)
- // * tcov(1:6,1:ntrk) : covariance matrix on parf
- // * chi2 : chi2 of the fit
- // * chi2tr(i)=contribution of track i to the
- // * total chi2
- // *
- // * errors : ierr = 1 : too many tracks (ntrk > ntrmax)
- // * ierr = 2 : covariance or weight matrix not positive
- // *
- // *************************************************************************
- // *
- // *
- // * "beam spot" description (common to be provided by the user)
- // * beamoy(1-3) are the mean beam spot positions in x,y and z.
- // * sigxbe,sigybe,sigzbe are the beam spread in x,y,z
- // common/pxcpro/beamoy[2],sigxbe,sigybe,sigzbe
- // double beamoy,sigxbe,sigybe,sigzbe
- // *
- // formal parameters
- // *____________________
- // *
- // int i,j,ierr,ntrk,ntrmax
- // *
- int ntrmax = 1000;
-
- double[] xyzf = new double[3];
- double[][] parf = new double[3][ntrk];
- double[] vcov = new double[6];
- double[][] tcov = new double[6][ntrk];
- double chi2;
-
- // *
- // * local variables
- // *__________________
- // *
- double[] wa = new double[6];
- double[][] wb = new double[9][ntrk];
- double[][] wc = new double[6][ntrk];
- double[][] wci = new double[6][ntrk];
- double[][] wbci = new double[9][ntrk];
- double[] tv = new double[3];
- double[][] tt = new double[3][ntrk];
- double[] dxyz = new double[3];
- double[] phiv = new double[ntrk];
- double[] eps = new double[ntrk];
- double[] zp = new double[ntrk];
- double[] deps = new double[ntrk];
- double[] dzp = new double[ntrk];
- 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;
-
- double fwdch2;
- double[] chi2tr = new double[ntrk];
-
- // needs to be set
- double sigxbe = .1;
- double sigybe = .1;
- double sigzbe = .1;
- double beamoy[] = { 0., 0., 0. };
- //
- // *
- // * executable statements
- // *________________________
- // *
-
- // *
- // * loop over the tracks
- // *
- chi2i = 0.;
- for (int i = 0; i < ntrk; ++i) {
- if (invtx[i]) {
- // *
- // * starting conditions :
- // * "perigee" parameters eps and zp if the trajectory goes through xyz
- // * and its theta,phi,1/r at perigee are equal to the values at input
- // *
- cotth = 1. / tan(par[2][i]);
- uu = xyz[0] * cos(par[3][i]) + xyz[1] * sin(par[3][i]);
- vv = xyz[1] * cos(par[3][i]) - xyz[0] * sin(par[3][i]);
- eps[i] = -vv + .5 * uu * uu * par[4][i];
- zp[i] = xyz[2] - uu * cotth;
- // * phi at vertex with these parameters
- phiv[i] = par[3][i] + uu * par[4][i];
- cosf = cos(phiv[i]);
- sinf = sin(phiv[i]);
- // *
- // * contribution of this track to chi2 with initial values
- // *
- deps[i] = par[0][i] - eps[i];
- dzp[i] = par[1][i] - zp[i];
- chi2i = chi2i
- + wgt[0][i]
- * deps[i]
- * deps[i]
- + 2
- * wgt[1][i]
- * deps[i]
- * dzp[i]
- + wgt[2][i]
- * dzp[i]
- * dzp[i];
- // *
- // * derivatives (deriv1) of perigee param. w.r.t. x,y,z (vertex)
- d11 = sinf;
- d12 = -cosf;
- d21 = -cosf * cotth;
- d22 = -sinf * cotth;
- d41 = -cosf * par[4][i];
- d42 = -sinf * par[4][i];
- // *
- // * matrix dw = (deriv1)t * weight
- dw11 = d11 * wgt[0][i] + d21 * wgt[1][i] + d41 * wgt[6][i];
- dw12 = d11 * wgt[1][i] + d21 * wgt[2][i] + d41 * wgt[7][i];
- dw13 = d11 * wgt[3][i] + d21 * wgt[4][i] + d41 * wgt[8][i];
- dw14 = d11 * wgt[6][i] + d21 * wgt[7][i] + d41 * wgt[9][i];
- dw15 = d11 * wgt[10][i] + d21 * wgt[11][i] + d41 * wgt[13][i];
- dw21 = d12 * wgt[0][i] + d22 * wgt[1][i] + d42 * wgt[6][i];
- dw22 = d12 * wgt[1][i] + d22 * wgt[2][i] + d42 * wgt[7][i];
- dw23 = d12 * wgt[3][i] + d22 * wgt[4][i] + d42 * wgt[8][i];
- dw24 = d12 * wgt[6][i] + d22 * wgt[7][i] + d42 * wgt[9][i];
- dw25 = d12 * wgt[10][i] + d22 * wgt[11][i] + d42 * wgt[13][i];
- dw31 = wgt[1][i];
- dw32 = wgt[2][i];
- dw33 = wgt[4][i];
- dw34 = wgt[7][i];
- dw35 = wgt[11][i];
- // *
- // * summation of dw * dpar to vector tv
- tv[0] = tv[0] + dw11 * deps[i] + dw12 * dzp[i];
- tv[1] = tv[1] + dw21 * deps[i] + dw22 * dzp[i];
- tv[2] = tv[2] + dw31 * deps[i] + dw32 * dzp[i];
- // *
- // * derivatives (deriv2) of perigee param. w.r.t. theta,phi,1/r (vertex)
- e12 = uu;
- e13 = -.5 * uu * uu;
- e21 = -uu * (1. + cotth * cotth);
- e22 = -vv * cotth;
- e23 = uu * vv * cotth;
- e43 = -uu;
- // *
- // * matrix ew = (deriv2)t * weight
- ew11 = e21 * wgt[1][i] + wgt[3][i];
- ew12 = e21 * wgt[2][i] + wgt[4][i];
- ew13 = e21 * wgt[4][i] + wgt[5][i];
- ew14 = e21 * wgt[7][i] + wgt[8][i];
- ew15 = e21 * wgt[11][i] + wgt[12][i];
- ew21 = e12 * wgt[0][i] + e22 * wgt[1][i] + wgt[6][i];
- ew22 = e12 * wgt[1][i] + e22 * wgt[2][i] + wgt[7][i];
- ew23 = e12 * wgt[3][i] + e22 * wgt[4][i] + wgt[8][i];
- ew24 = e12 * wgt[6][i] + e22 * wgt[7][i] + wgt[9][i];
- ew25 = e12 * wgt[10][i] + e22 * wgt[11][i] + wgt[13][i];
- ew31 = e13 * wgt[0][i] + e23 * wgt[1][i] + e43 * wgt[6][i] + wgt[10][i];
- ew32 = e13 * wgt[1][i] + e23 * wgt[2][i] + e43 * wgt[7][i] + wgt[11][i];
- ew33 = e13 * wgt[3][i] + e23 * wgt[4][i] + e43 * wgt[8][i] + wgt[12][i];
- ew34 = e13 * wgt[6][i] + e23 * wgt[7][i] + e43 * wgt[9][i] + wgt[13][i];
- ew35 = e13 * wgt[10][i] + e23 * wgt[11][i] + e43 * wgt[13][i] + wgt[14][i];
- // *
- // * computation of vector tt = ew * dpar
- tt[0][i] = ew11 * deps[i] + ew12 * dzp[i];
- tt[1][i] = ew21 * deps[i] + ew22 * dzp[i];
- tt[2][i] = ew31 * deps[i] + ew32 * dzp[i];
- // *
- // * summation of (deriv1)t * weight * (deriv1) to matrix wa
- wa[0] = wa[0] + dw11 * d11 + dw12 * d21 + dw14 * d41;
- wa[1] = wa[1] + dw11 * d12 + dw12 * d22 + dw14 * d42;
- wa[2] = wa[2] + dw21 * d12 + dw22 * d22 + dw24 * d42;
- wa[3] = wa[3] + dw12;
- wa[4] = wa[4] + dw22;
- wa[5] = wa[5] + dw32;
- // *
- // * computation of matrix wb = (deriv1)t * weight * (deriv2)
- wb[0][i] = dw12 * e21 + dw13;
- wb[1][i] = dw22 * e21 + dw23;
- wb[2][i] = dw32 * e21 + dw33;
- wb[3][i] = dw11 * e12 + dw12 * e22 + dw14;
- wb[4][i] = dw21 * e12 + dw22 * e22 + dw24;
- wb[5][i] = dw31 * e12 + dw32 * e22 + dw34;
- wb[6][i] = dw11 * e13 + dw12 * e23 + dw14 * e43 + dw15;
- wb[7][i] = dw21 * e13 + dw22 * e23 + dw24 * e43 + dw25;
- wb[8][i] = dw31 * e13 + dw32 * e23 + dw34 * e43 + dw35;
- // *
- // * computation of matrix wc = (deriv2)t * weight * (deriv2)
- wc[0][i] = ew12 * e21 + ew13;
- wc[1][i] = ew22 * e21 + ew23;
- wc[3][i] = ew32 * e21 + ew33;
- wc[2][i] = ew21 * e12 + ew22 * e22 + ew24;
- wc[4][i] = ew31 * e12 + ew32 * e22 + ew34;
- wc[5][i] = ew31 * e13 + ew32 * e23 + ew34 * e43 + ew35;
- // *
- // * computation of matrices wci = (wc)**(-1) and wbci = wb * wci
-
- // Not very efficient here...
- double[] tmp = new double[6];
- for (int j = 0; j < 6; ++j) {
- tmp[j] = wc[j][i];
- System.out.println("tmp[" + j + "]= " + tmp[j]);
- }
- double[] tmpinv = new double[6];
- tmpinv = pxmi3(tmp);
- for (int j = 0; j < 6; ++j) {
- wci[j][i] = tmpinv[j];
- }
- // wci[0][i] = pxmi3(wc[0][i]);
-
- wbci[0][i] = wb[0][i] * wci[0][i] + wb[3][i] * wci[1][i] + wb[6][i] * wci[3][i];
- wbci[1][i] = wb[1][i] * wci[0][i] + wb[4][i] * wci[1][i] + wb[7][i] * wci[3][i];
- wbci[2][i] = wb[2][i] * wci[0][i] + wb[5][i] * wci[1][i] + wb[8][i] * wci[3][i];
- wbci[3][i] = wb[0][i] * wci[1][i] + wb[3][i] * wci[2][i] + wb[6][i] * wci[4][i];
- wbci[4][i] = wb[1][i] * wci[1][i] + wb[4][i] * wci[2][i] + wb[7][i] * wci[4][i];
- wbci[5][i] = wb[2][i] * wci[1][i] + wb[5][i] * wci[2][i] + wb[8][i] * wci[4][i];
- wbci[6][i] = wb[0][i] * wci[3][i] + wb[3][i] * wci[4][i] + wb[6][i] * wci[5][i];
- wbci[7][i] = wb[1][i] * wci[3][i] + wb[4][i] * wci[4][i] + wb[7][i] * wci[5][i];
- wbci[8][i] = wb[2][i] * wci[3][i] + wb[5][i] * wci[4][i] + wb[8][i] * wci[5][i];
- // *
- // * subtraction of wbci * (wb)t from matrix wa
- wa[0] = wa[0] - wbci[0][i] * wb[0][i] - wbci[3][i] * wb[3][i] - wbci[6][i] * wb[6][i];
- wa[1] = wa[1] - wbci[0][i] * wb[1][i] - wbci[3][i] * wb[4][i] - wbci[6][i] * wb[7][i];
- wa[2] = wa[2] - wbci[1][i] * wb[1][i] - wbci[4][i] * wb[4][i] - wbci[7][i] * wb[7][i];
- wa[3] = wa[3] - wbci[0][i] * wb[2][i] - wbci[3][i] * wb[5][i] - wbci[6][i] * wb[8][i];
- wa[4] = wa[4] - wbci[1][i] * wb[2][i] - wbci[4][i] * wb[5][i] - wbci[7][i] * wb[8][i];
- wa[5] = wa[5] - wbci[2][i] * wb[2][i] - wbci[5][i] * wb[5][i] - wbci[8][i] * wb[8][i];
- // *
- // * subtraction of wbci * tt from vector tv
- tv[0] = tv[0] - wbci[0][i] * tt[0][i] - wbci[3][i] * tt[1][i] - wbci[6][i] * tt[2][i];
- tv[1] = tv[1] - wbci[1][i] * tt[0][i] - wbci[4][i] * tt[1][i] - wbci[7][i] * tt[2][i];
- tv[2] = tv[2] - wbci[2][i] * tt[0][i] - wbci[5][i] * tt[1][i] - wbci[8][i] * tt[2][i];
- // *
- }// check on whether to use track
- }// loop over tracks
-
- // beam constraint
- if (fitwb) {
- wa[0] += 1. / (sigxbe * sigxbe);
- wa[2] += 1. / (sigxbe * sigybe);
- wa[5] += 1. / (sigzbe * sigzbe);
- tv[0] += (beamoy[0] - xyz[0]) / (sigxbe * sigxbe);
- tv[1] += (beamoy[1] - xyz[1]) / (sigybe * sigybe);
- tv[2] += (beamoy[2] - xyz[2]) / (sigzbe * sigzbe);
- }
- // *
- // * solution of the linear system
- // *
- // * covariance matrix on vertex
- vcov = pxmi3(wa);
- for (int ii = 0; ii < vcov.length; ++ii) {
- System.out.println("vcov[" + ii + "]= " + vcov[ii]);
- }
- //
- // *
- // * corrections to vertex coordinates
- dxyz[0] = vcov[0] * tv[0] + vcov[1] * tv[1] + vcov[3] * tv[2];
- dxyz[1] = vcov[1] * tv[0] + vcov[2] * tv[1] + vcov[4] * tv[2];
- dxyz[2] = vcov[3] * tv[0] + vcov[4] * tv[1] + vcov[5] * tv[2];
- xyzf[0] = xyz[0] + dxyz[0];
- xyzf[1] = xyz[1] + dxyz[1];
- xyzf[2] = xyz[2] + dxyz[2];
- // *
- // * corrections to track parameters and covariance matrices
- // *
- for (int i = 0; i < ntrk; ++i) {
- if (invtx[i]) {
- // *
- // * variation on par is wci * tt - (wbci)t * dxyz
- parf[0][i] = par[2][i]
- + wci[0][i]
- * tt[0][i]
- + wci[1][i]
- * tt[1][i]
- + wci[3][i]
- * tt[2][i]
- - wbci[0][i]
- * dxyz[0]
- - wbci[1][i]
- * dxyz[1]
- - wbci[2][i]
- * dxyz[2];
- parf[1][i] = phiv[i]
- + wci[1][i]
- * tt[0][i]
- + wci[2][i]
- * tt[1][i]
- + wci[4][i]
- * tt[2][i]
- - wbci[3][i]
- * dxyz[0]
- - wbci[4][i]
- * dxyz[1]
- - wbci[5][i]
- * dxyz[2];
- parf[2][i] = par[4][i]
- + wci[3][i]
- * tt[0][i]
- + wci[4][i]
- * tt[1][i]
- + wci[5][i]
- * tt[2][i]
- - wbci[6][i]
- * dxyz[0]
- - wbci[7][i]
- * dxyz[1]
- - wbci[8][i]
- * dxyz[2];
- // *
- // * covariance matrix of par is wci + (wbci)t * vcov * wbci
- tcov[0][i] = wci[0][i]
- + wbci[0][i]
- * (vcov[0] * wbci[0][i] + vcov[1] * wbci[1][i] + vcov[3] * wbci[2][i])
- + wbci[1][i]
- * (vcov[1] * wbci[0][i] + vcov[2] * wbci[1][i] + vcov[4] * wbci[2][i])
- + wbci[2][i]
- * (vcov[3] * wbci[0][i] + vcov[4] * wbci[1][i] + vcov[5] * wbci[2][i]);
-
- tcov[1][i] = wci[1][i]
- + wbci[0][i]
- * (vcov[0] * wbci[3][i] + vcov[1] * wbci[4][i] + vcov[3] * wbci[5][i])
- + wbci[1][i]
- * (vcov[1] * wbci[3][i] + vcov[2] * wbci[4][i] + vcov[4] * wbci[5][i])
- + wbci[2][i]
- * (vcov[3] * wbci[3][i] + vcov[4] * wbci[4][i] + vcov[5] * wbci[5][i]);
-
- tcov[2][i] = wci[2][i]
- + wbci[3][i]
- * (vcov[0] * wbci[3][i] + vcov[1] * wbci[4][i] + vcov[3] * wbci[5][i])
- + wbci[4][i]
- * (vcov[1] * wbci[3][i] + vcov[2] * wbci[4][i] + vcov[4] * wbci[5][i])
- + wbci[5][i]
- * (vcov[3] * wbci[3][i] + vcov[4] * wbci[4][i] + vcov[5] * wbci[5][i]);
-
- tcov[3][i] = wci[3][i]
- + wbci[0][i]
- * (vcov[0] * wbci[6][i] + vcov[1] * wbci[7][i] + vcov[3] * wbci[8][i])
- + wbci[1][i]
- * (vcov[1] * wbci[6][i] + vcov[2] * wbci[7][i] + vcov[4] * wbci[8][i])
- + wbci[2][i]
- * (vcov[3] * wbci[6][i] + vcov[4] * wbci[7][i] + vcov[5] * wbci[8][i]);
-
- tcov[4][i] = wci[4][i]
- + wbci[3][i]
- * (vcov[0] * wbci[6][i] + vcov[1] * wbci[7][i] + vcov[3] * wbci[8][i])
- + wbci[4][i]
- * (vcov[1] * wbci[6][i] + vcov[2] * wbci[7][i] + vcov[4] * wbci[8][i])
- + wbci[5][i]
- * (vcov[3] * wbci[6][i] + vcov[4] * wbci[7][i] + vcov[5] * wbci[8][i]);
-
- tcov[5][i] = wci[5][i]
- + wbci[6][i]
- * (vcov[0] * wbci[6][i] + vcov[1] * wbci[7][i] + vcov[3] * wbci[8][i])
- + wbci[7][i]
- * (vcov[1] * wbci[6][i] + vcov[2] * wbci[7][i] + vcov[4] * wbci[8][i])
- + wbci[8][i]
- * (vcov[3] * wbci[6][i] + vcov[4] * wbci[7][i] + vcov[5] * wbci[8][i]);
-
- // *
- } // check on whether to use track
- } // loop over tracks
-
- // *
- // * chi2 with fitted values
- // *
- chi2 = 0.;
- if (fitwb)
- chi2 += ( (xyzf[0] - beamoy[0]) / sigxbe)
- * ( (xyzf[0] - beamoy[0]) / sigxbe)
- + ( (xyzf[1] - beamoy[1]) / sigybe)
- * ( (xyzf[1] - beamoy[1]) / sigybe)
- + ( (xyzf[2] - beamoy[2]) / sigzbe)
- * ( (xyzf[2] - beamoy[2]) / sigzbe);
-
- for (int i = 0; i < ntrk; ++i) {
- if (invtx[i]) {
- uu = xyzf[0] * cos(parf[1][i]) + xyzf[1] * sin(parf[1][i]);
- vv = xyzf[1] * cos(parf[1][i]) - xyzf[0] * sin(parf[1][i]);
- epsf = -vv - .5 * uu * uu * parf[2][i];
- zpf = xyzf[2] - uu / tan(parf[0][i]);
- phif = parf[1][i] - uu * parf[2][i];
- // not very efficient here...
- double[] tmp = new double[15];
- for (int j = 0; j < 15; ++j) {
- tmp[j] = wgt[j][i];
- }
- chi2tr[i] = fwdch2(tmp, epsf - par[0][i], zpf - par[1][i], parf[0][i] - par[2][i], phif - par[3][i],
- parf[2][i] - par[4][i]);
- chi2 += chi2tr[i];
- }
- }
- System.out.println("chi2= " + chi2);
- return new Vertex(ntrk, xyzf, parf, vcov, tcov, chi2, chi2tr);
+package org.lcsim.recon.vertexing.zvtop4.fitter;
- }
+import java.util.List;
+import org.lcsim.event.Track;
+import org.lcsim.spacegeom.SpacePoint;
+/**
+ *
+ * @author jstrube
+ */
+public interface VertexFitter {
+ public Vertex fit(List<Track> tracks, SpacePoint initialPosition, boolean withBeamConstraint);
}
lcsim/src/org/lcsim/recon/vertexing/zvtop4/fitter
diff -u -r1.1 -r1.2
--- Perigee.java 20 Mar 2006 07:42:04 -0000 1.1
+++ Perigee.java 27 Mar 2006 00:00:21 -0000 1.2
@@ -1,4 +1,7 @@
package org.lcsim.recon.vertexing.zvtop4.fitter;
+/**
+ * @version $Id: Perigee.java,v 1.2 2006/03/27 00:00:21 jstrube Exp $
+ */
public class Perigee
{
//attributes
lcsim/src/org/lcsim/recon/vertexing/zvtop4/fitter
diff -u -r1.1 -r1.2
--- Vertex.java 20 Mar 2006 07:42:04 -0000 1.1
+++ Vertex.java 27 Mar 2006 00:00:21 -0000 1.2
@@ -1,5 +1,7 @@
package org.lcsim.recon.vertexing.zvtop4.fitter;
-
+/**
+ * @version $Id: Vertex.java,v 1.2 2006/03/27 00:00:21 jstrube Exp $
+ */
//Testing...
// A class encapsulating the behavior of a vertex constructed
// from tracks. It is written to be independent of any other
@@ -8,18 +10,18 @@
// But it should work.
//
public class Vertex {
- int _ntrk; // the number of tracks used in this fit
- double[] _xyzf; // the vertex position in Cartesian coordinates
- double[] _vcov; // packed covariance matrix on _xyzf
+ public int _ntrk; // the number of tracks used in this fit
+ public double[] _xyzf; // the vertex position in Cartesian coordinates
+ public double[] _vcov; // packed covariance matrix on _xyzf
- double[][] _parf; // the fitted parameters at the vertex
+ public double[][] _parf; // the fitted parameters at the vertex
// 0: theta
// 1: phi
// 2: 1/R
- double[][] _tcov; // covariance matrix on _parf
+ public double[][] _tcov; // covariance matrix on _parf
- double _chi2; // chisquared of vertex fit;
- double[] _chi2tr; // chisquared contribution of each track
+ public double _chi2; // chisquared of vertex fit;
+ public double[] _chi2tr; // chisquared contribution of each track
// default constructor
public Vertex() {
lcsim/src/org/lcsim/contrib/JanStrube
diff -N VertexFitterDriver.java
--- /dev/null 1 Jan 1970 00:00:00 -0000
+++ VertexFitterDriver.java 27 Mar 2006 00:00:22 -0000 1.1
@@ -0,0 +1,67 @@
+/*
+ * VertexFitterDriver.java
+ *
+ * Created on March 25, 2006, 3:15 PM
+ *
+ *@version $Id: VertexFitterDriver.java,v 1.1 2006/03/27 00:00:22 jstrube Exp $
+ */
+
+package org.lcsim.contrib.JanStrube;
+
+import hep.aida.IAnalysisFactory;
+import hep.aida.ITuple;
+import hep.aida.ITupleFactory;
+import java.util.List;
+import org.lcsim.event.EventHeader;
+import org.lcsim.event.MCParticle;
+import org.lcsim.event.Track;
+import org.lcsim.recon.vertexing.zvtop4.fitter.BilloirFitter;
+import org.lcsim.recon.vertexing.zvtop4.fitter.Vertex;
+import org.lcsim.recon.vertexing.zvtop4.fitter.VertexFitter;
+import org.lcsim.spacegeom.CartesianPoint;
+import org.lcsim.spacegeom.SpacePoint;
+import org.lcsim.util.Driver;
+import org.lcsim.util.aida.AIDA;
+
+/**
+ *
+ * @author jstrube
+ */
+public class VertexFitterDriver extends Driver {
+ private AIDA aida = AIDA.defaultInstance();
+ private IAnalysisFactory af = aida.analysisFactory();
+ private ITupleFactory tf = af.createTupleFactory(aida.tree());
+ private ITuple tuple;
+ private VertexFitter fitter;
+ private int primaryParticle = 13;
+ private SpacePoint primaryVertex;
+ public VertexFitterDriver() {
+ String[] names = new String[]{"chi2", "fittedX", "fittedY", "fittedZ", "xError", "yError", "zError", "primaryX", "primaryY", "primaryZ"};
+ Class[] classes = new Class[]{Float.TYPE, Float.TYPE, Float.TYPE, Float.TYPE, Float.TYPE, Float.TYPE, Float.TYPE, Float.TYPE, Float.TYPE, Float.TYPE};
+ tuple = tf.create("vertexFitterResults", "", names, classes);
+ fitter = new BilloirFitter(5);
+ }
+
+ public void process(EventHeader event) {
+ List<MCParticle> particles = event.getMCParticles();
+ List<Track> tracks = event.getTracks();
+ if (tracks.size() != 2)
+ return;
+ for (MCParticle iPart : particles) {
+ if (iPart.getGeneratorStatus() == MCParticle.INTERMEDIATE)
+ if (iPart.getPDGID() == primaryParticle)
+ primaryVertex = new SpacePoint(iPart.getEndPoint());
+ }
+ Vertex vertex = fitter.fit(tracks, new CartesianPoint(0, 0, 0), true);
+ tuple.fill(0, vertex._chi2);
+ tuple.fill(1, vertex._xyzf[0]);
+ tuple.fill(2, vertex._xyzf[1]);
+ tuple.fill(3, vertex._xyzf[2]);
+ tuple.fill(4, vertex._vcov[0]);
+ tuple.fill(5, vertex._vcov[3]);
+ tuple.fill(6, vertex._vcov[5]);
+ tuple.fill(7, primaryVertex.x());
+ tuple.fill(8, primaryVertex.y());
+ tuple.fill(9, primaryVertex.z());
+ }
+}
lcsim/test/org/lcsim/recon/vertexing/zvtop4/fitter
diff -u -r1.4 -r1.5
--- VertexFitterTest.java 22 Mar 2006 09:43:01 -0000 1.4
+++ VertexFitterTest.java 27 Mar 2006 00:00:23 -0000 1.5
@@ -1,7 +1,9 @@
+package org.lcsim.recon.vertexing.zvtop4.fitter;
+
/**
- *
+ * @version $Id: VertexFitterTest.java,v 1.5 2006/03/27 00:00:23 jstrube Exp $
*/
-package org.lcsim.recon.vertexing.zvtop4.fitter;
+
import Jama.Matrix;
import java.io.File;
@@ -12,6 +14,7 @@
import org.lcsim.event.EventHeader;
import org.lcsim.event.Track;
import org.lcsim.mc.fast.MCFast;
+import org.lcsim.spacegeom.CartesianPoint;
import org.lcsim.util.Driver;
import org.lcsim.util.aida.AIDA;
import org.lcsim.util.cache.FileCache;
@@ -33,7 +36,7 @@
private URL location;
private FileCache cache;
private File trackFile;
- private VertexFitter fitter;
+ private BilloirFitter fitter;
private AIDA aida = AIDA.defaultInstance();
protected void setUp() throws Exception {
super.setUp();
@@ -44,7 +47,7 @@
cache = new FileCache();
trackFile = cache.getCachedFile(location);
loop.setStdhepRecordSource(trackFile, "sidaug05");
- fitter = new VertexFitter(5);
+ fitter = new BilloirFitter(5);
}
public void testFit() throws org.freehep.record.loop.LoopException, java.io.IOException {
@@ -75,7 +78,7 @@
Matrix nw = new Matrix(weightArray);
nw.print(31, 30);
System.err.printf("Old Det: %g\nNew Det: %g\nJacobi Det: %g\n", olderrors.det(), nw.det(), jacobi.det());
- weights.add(flattenWeights(weightArray));
+ weights.add(flattenMatrix(weightArray));
}
System.err.printf("%d tracks\n", trackParams.size());
@@ -110,7 +113,7 @@
newTrackParams.print(20, 21);
Matrix newWeights = new Matrix(weights.toArray(new double[][]{}));
newWeights.print(21, 20);
- Vertex v = fitter.fit(tracks.size(), false, inVtx, newTrackParams.transpose().getArray(), newWeights.transpose().getArray(), initialPosition);
+ Vertex v = fitter.fit(tracks, new CartesianPoint(initialPosition), true);
aida.cloud1D("vtx_chi2").fill(v._chi2);
}
@@ -124,12 +127,13 @@
return jacobi;
}
- private double[] flattenWeights(double[][] weightMatrix) {
+ private double[] flattenMatrix(double[][] matrix) {
+ int length = matrix.length;
double[] result = new double[15];
int count = 0;
for (int i=0; i<5; ++i)
for (int j=i; j<5; ++j)
- result[count++] = weightMatrix[i][j];
+ result[count++] = matrix[i][j];
return result;
}
}
CVSspam 0.2.8