hps-java/src/main/java/org/lcsim/hps/recon/tracking/gbl
diff -N GBLFileIO.java
--- /dev/null 1 Jan 1970 00:00:00 -0000
+++ GBLFileIO.java 14 Jul 2013 16:58:13 -0000 1.1
@@ -0,0 +1,199 @@
+/*
+ * To change this template, choose Tools | Templates
+ * and open the template in the editor.
+ */
+package org.lcsim.hps.recon.tracking.gbl;
+
+import hep.physics.matrix.BasicMatrix;
+import hep.physics.matrix.SymmetricMatrix;
+import hep.physics.vec.Hep3Matrix;
+import hep.physics.vec.Hep3Vector;
+import java.io.FileWriter;
+import java.io.IOException;
+import java.io.PrintWriter;
+import java.util.logging.Level;
+import java.util.logging.Logger;
+import org.lcsim.fit.helicaltrack.HelicalTrackFit;
+import org.lcsim.hps.recon.tracking.gbl.GBLOutput.ClParams;
+import org.lcsim.hps.recon.tracking.gbl.GBLOutput.PerigeeParams;
+import org.lcsim.hps.users.mgraham.alignment.RunAlignment;
+
+/**
+ *
+ * @author phansson
+ */
+public class GBLFileIO {
+
+ PrintWriter _pWriter;
+ FileWriter _fWriter;
+ private String _outputFileName;
+
+ GBLFileIO(String fileName) {
+ this._outputFileName = fileName;
+ openFile();
+ }
+
+ public void printEventNr(int evtnr) {
+ addLine(String.format("New Event %d", evtnr));
+ }
+
+ protected void addLine(String line) {
+ this._pWriter.println(line);
+ }
+
+ public void closeFile() {
+ try {
+ _pWriter.close();
+ _fWriter.close();
+ } catch(IOException ex) {
+ Logger.getLogger(RunAlignment.class.getName()).log(Level.SEVERE, null, ex);
+ }
+ }
+ private void openFile() {
+ try {
+ _fWriter = new FileWriter(_outputFileName);
+ _pWriter = new PrintWriter(_fWriter);
+ } catch (IOException ex) {
+ Logger.getLogger(RunAlignment.class.getName()).log(Level.SEVERE, null, ex);
+ }
+ }
+
+ void printTrackID(int iTrack) {
+ addLine(String.format("New Track %d", iTrack));
+ }
+
+ void printOldPerTrackParam(HelicalTrackFit htf) {
+ addLine(String.format("Track perPar (R phi0 slope d0 z0) %f %f %f %f %f",htf.R(),htf.phi0(),htf.slope(),htf.dca(),htf.z0()));
+ }
+
+ String getPerTrackParamStr(PerigeeParams perPar) {
+ return String.format("Track perPar (kappa theta phi d0 z0) %f %f %f %f %f",perPar.getKappa(),perPar.getTheta(),perPar.getPhi(),perPar.getD0(),perPar.getZ0());
+ }
+
+ void printPerTrackParam(PerigeeParams perPar) {
+ addLine(this.getPerTrackParamStr(perPar));
+ }
+
+ String getClTrackParamStr(ClParams perPar) {
+ return String.format("Track clPar (q/p lambda phi xT yT) %f %f %f %f %f",perPar.getQoverP(),perPar.getLambda(),perPar.getPhi(),perPar.getXt(),perPar.getYt());
+ }
+ void printClTrackParam(ClParams perPar) {
+ addLine(String.format("%s",this.getClTrackParamStr(perPar)));
+ }
+
+ void printNrHits(int n) {
+ addLine(String.format("Track nr hits <%d>",n));
+ }
+
+// void printStrip(int id, int layer, double s, Hep3Vector trkpos, Hep3Vector trkposlocal, Hep3Vector m, Hep3Vector res_local,Hep3Vector res_local_error, Hep3Vector u, Hep3Vector origin) {
+// addLine(String.format("Strip id <%d> layer <%d> org <%f %f %f> s <%f> tp <%f %f %f> tplocal <%f %f %f> m <%f %f %f> res <%f %f %f> reserr <%f %f %f> u <%f %f %f> ",id,layer,
+// origin.x(),origin.y(),origin.z(),
+// s,
+// trkpos.x(),trkpos.y(),trkpos.z(),
+// trkposlocal.x(),trkposlocal.y(),trkposlocal.z(),
+// m.x(),m.y(),m.z(),
+// res_local.x(),res_local.y(),res_local.z(),
+// res_local_error.x(),res_local_error.y(),res_local_error.z(),
+// u.x(),u.y(),u.z()));
+// }
+
+ void printStripJacPointToPoint(int id, int layer, double s, BasicMatrix jac) {
+ String str = String.format("Strip id <%d> layer <%d> s <%f> jac <",id,layer,s);
+ for(int r=0;r<jac.getNRows();++r) {
+ for(int c=0;c<jac.getNColumns();++c) {
+ str += String.format("%f ", jac.e(r, c));
+ }
+ }
+ str += ">";
+ addLine(str);
+ }
+
+
+ void printStripScatJacPointToPoint(int id, int layer, double s, BasicMatrix jac) {
+ String str = String.format("Strip scat id <%d> layer <%d> s <%f> jac <",id,layer,s);
+ for(int r=0;r<jac.getNRows();++r) {
+ for(int c=0;c<jac.getNColumns();++c) {
+ str += String.format("%f ", jac.e(r, c));
+ }
+ }
+ str += ">";
+ addLine(str);
+ }
+
+ void printStripL2m(int id, int layer, double s, BasicMatrix jac) {
+ String str = String.format("Strip LocalToMeas id <%d> layer <%d> s <%f> L2m <",id,layer,s);
+ for(int r=0;r<jac.getNRows();++r) {
+ for(int c=0;c<jac.getNColumns();++c) {
+ str += String.format("%f ", jac.e(r, c));
+ }
+ }
+ str += ">";
+ addLine(str);
+ }
+
+ void printPerTrackCov(SymmetricMatrix cov) {
+ String str = "Track perCov ";
+ for(int irow=0;irow<cov.getNRows();++irow) {
+ for(int icol=0;icol<cov.getNColumns();++icol) {
+ str += String.format("%f ", cov.e(irow, icol));
+ }
+ }
+ addLine(str);
+ }
+
+ void printCLTrackCov(BasicMatrix cov) {
+ String str = "Track clCov ";
+ for(int irow=0;irow<cov.getNRows();++irow) {
+ for(int icol=0;icol<cov.getNColumns();++icol) {
+ str += String.format("%f ", cov.e(irow, icol));
+ }
+ }
+ addLine(str);
+ }
+
+
+ void printStripTrackDir(double sinPhi, double sinLambda) {
+ String str = String.format("Strip sinPhi sinLambda %f %f",sinPhi,sinLambda);
+ addLine(str);
+ }
+
+ void printStrip(int id, int layer) {
+ addLine(String.format("New Strip id layer %d %d", id,layer));
+ }
+
+ void printStripPathLen(double s) {
+ addLine(String.format("Strip pathLen %f", s));
+ }
+
+ void printStereoAngle(double stereoAngle) {
+ addLine(String.format("Strip stereo angle %f", stereoAngle));
+ }
+
+ void printStripMeas(double ures, double uresErr) {
+ addLine(String.format("Strip ures %f %f", ures, uresErr));
+ }
+
+ void printStripScat(double scatAngle) {
+ addLine(String.format("Strip scatangle %f",scatAngle));
+ }
+
+ void printMeasDir(Hep3Vector u) {
+ addLine(String.format("Strip meas dir %f %f %f", u.x(), u.y(), u.z()));
+ }
+
+ void printMomentum(double p, double p_truth) {
+ addLine(String.format("Track mom %f %f", p, p_truth));
+ }
+
+ void printPerToClPrj(Hep3Matrix perToClPrj) {
+ String str = "Track perToClPrj ";
+ for(int irow=0;irow<perToClPrj.getNRows();++irow) {
+ for(int icol=0;icol<perToClPrj.getNColumns();++icol) {
+ str += String.format("%f ", perToClPrj.e(irow, icol));
+ }
+ }
+ addLine(str);
+ }
+
+
+}
hps-java/src/main/java/org/lcsim/hps/recon/tracking/gbl
diff -N GBLOutput.java
--- /dev/null 1 Jan 1970 00:00:00 -0000
+++ GBLOutput.java 14 Jul 2013 16:58:14 -0000 1.1
@@ -0,0 +1,544 @@
+/*
+ * To change this template, choose Tools | Templates
+ * and open the template in the editor.
+ */
+package org.lcsim.hps.recon.tracking.gbl;
+
+import hep.physics.matrix.BasicMatrix;
+import hep.physics.matrix.MatrixOp;
+import hep.physics.matrix.SymmetricMatrix;
+import hep.physics.vec.*;
+import java.io.FileWriter;
+import java.io.PrintWriter;
+import java.util.ArrayList;
+import java.util.HashMap;
+import java.util.List;
+import java.util.Map;
+import org.lcsim.constants.Constants;
+import org.lcsim.event.MCParticle;
+import org.lcsim.event.RawTrackerHit;
+import org.lcsim.event.Track;
+import org.lcsim.fit.helicaltrack.*;
+import org.lcsim.geometry.Detector;
+import org.lcsim.hps.event.HPSTransformations;
+import org.lcsim.hps.recon.tracking.*;
+import org.lcsim.hps.recon.tracking.MaterialSupervisor.DetectorPlane;
+import org.lcsim.hps.recon.tracking.MaterialSupervisor.ScatteringDetectorVolume;
+import org.lcsim.hps.recon.tracking.MultipleScattering.ScatterPoint;
+import org.lcsim.hps.recon.tracking.MultipleScattering.ScatterPoints;
+import org.lcsim.recon.tracking.seedtracker.ScatterAngle;
+import org.lcsim.recon.tracking.seedtracker.SeedCandidate;
+import org.lcsim.recon.tracking.seedtracker.SeedTrack;
+
+/**
+ *
+ * @author phansson
+ */
+public class GBLOutput {
+
+ private FileWriter _fWriter;
+ private PrintWriter _pWriter;
+ private int _debug;
+ private boolean _hideFrame = false;
+ private GBLFileIO file;
+ private Hep3Vector _B;
+ private TrackerHitUtils _trackerHitUtils = new TrackerHitUtils();
+ private MaterialSupervisor _materialmanager;
+ private MultipleScattering _scattering;
+ private Detector detector;
+ private HPSTransformations _hpstrans = new HPSTransformations();
+
+
+
+ /*
+ * file name
+ * Bz in Tesla
+ */
+ GBLOutput(String outputFileName,Hep3Vector bfield) {
+ file = new GBLFileIO(outputFileName);
+ //B = VecOp.mult(0.3, new BasicHep3Vector(0.,0.,Bz));
+ _materialmanager = new MaterialSupervisor();
+ _scattering = new MultipleScattering(_materialmanager);
+ _B = _hpstrans.transformVectorToTracking(bfield);
+ System.out.printf("%s: B field %s\n",this.getClass().getSimpleName(),_B.toString());
+ _scattering.setBField(_B.z());
+ }
+ public void setDebug(int debug) {
+ _debug = debug;
+ }
+ public void setHideFrame(boolean hide) {
+ _hideFrame = hide;
+ }
+ public void buildModel(Detector detector) {
+ _materialmanager.buildModel(detector);
+ }
+ void printNewEvent(int eventNumber) {
+ file.printEventNr(eventNumber);
+ }
+ void printTrackID(int iTrack) {
+ file.printTrackID(iTrack);
+ }
+ void close() {
+ file.closeFile();
+ }
+
+
+ void printGBL(Track trk, List<MCParticle> mcParticles) {
+
+ SeedTrack st = (SeedTrack)trk;
+ SeedCandidate seed = st.getSeedCandidate();
+ HelicalTrackFit htf = seed.getHelix();
+ List<HelicalTrackHit> hits = seed.getHits();
+
+ if (this._debug>1) {
+ System.out.printf("%s: find scatters\n",this.getClass().getSimpleName());
+ }
+ //find the scattering angle from material manager
+ ScatterPoints scatters = _scattering.FindHPSScatterPoints(htf);
+
+
+ // Convert from perigee (L3 parameters) to curvilinear frame
+ PerigeeParams perPar = new PerigeeParams(htf);
+ ClParams clPar = new ClParams(htf);
+ //BasicMatrix jacPerToCl = getJacPerToCl(htf);
+ //BasicMatrix clPar = (BasicMatrix) MatrixOp.mult(jacPerToCl, MatrixOp.transposed(perPar));
+ //clPar = (BasicMatrix) MatrixOp.transposed(clPar);
+
+ // print track parameters for easier debugging later
+ file.printOldPerTrackParam(htf);
+ if(_debug>0) {
+ System.out.printf("%s\n",file.getClTrackParamStr(clPar));
+ System.out.printf("%s\n",file.getPerTrackParamStr(perPar));
+ }
+ file.printPerTrackParam(perPar);
+ file.printClTrackParam(clPar);
+
+ List<MCParticle> truthParticles = getTruthParticle(trk.getCharge()>0?11:-11,mcParticles);
+ double p_truth = 0.;
+ double dp_min = 99999999.9;
+ for(MCParticle p : truthParticles) {
+ double dp_loop = Math.abs(p.getMomentum().magnitude() - getMomentum(trk).magnitude());
+ if(dp_loop < dp_min) {
+ dp_min = dp_loop;
+ p_truth = p.getMomentum().magnitude();
+ }
+ }
+ file.printMomentum(htf.p(this._B.magnitude()),p_truth);
+
+ // find the projection from the I,J,K to U,V,T coordinates
+ Hep3Matrix perToClPrj = this.getPerToClPrj(htf);
+
+ file.printPerToClPrj(perToClPrj);
+
+ // covariance matrix
+ SymmetricMatrix perCov = htf.covariance();
+ file.printPerTrackCov(perCov);
+ BasicMatrix clCov = new BasicMatrix(5,5);
+ this.initUnit(clCov);
+ clCov = (BasicMatrix) MatrixOp.mult(0.1*0.1,clCov);
+ file.printCLTrackCov(clCov);
+
+ if(_debug>0) System.out.printf("%d hits\n",hits.size());
+
+
+ int istrip = 0;
+ for(int ihit=0;ihit!=hits.size();++ihit) {
+
+ HelicalTrackHit hit = hits.get(ihit);
+ HelicalTrackCross htc = (HelicalTrackCross) hit;
+ List<HelicalTrackStrip> strips = htc.getStrips();
+
+ for(HelicalTrackStrip strip : strips) {
+
+ if(_debug>0) System.out.printf("%s: layer %d\n",this.getClass().getSimpleName(),strip.layer());
+
+ file.printStrip(istrip,strip.layer());
+
+ //Center of the sensor
+ Hep3Vector origin = strip.origin();
+
+ //Find intercept point with sensor in tracking frame
+ Hep3Vector trkpos = TrackUtils.getHelixPlaneIntercept(htf, strip, Math.abs(_B.z()));
+
+ //path length to intercept
+ double s = HelixUtils.PathToXPlane(htf,trkpos.x(),0,0).get(0);
+ double s_hit = HelixUtils.PathLength(htf, hit); //cross-check
+
+ //print the track direction
+ file.printStripPathLen(s);
+
+ //print stereo angle in YZ plane
+ if(_debug>0) System.out.printf("u = %s v = %s \n",strip.u().toString(),strip.v().toString());
+ double stereoAngle = Math.atan(strip.v().z());
+ file.printMeasDir(strip.u());
+
+ //Print track direction at intercept
+ Hep3Vector tDir = HelixUtils.Direction(htf, s);
+ double phi = htf.phi0() - s/htf.R();
+ double lambda = Math.atan(htf.slope());
+ file.printStripTrackDir(Math.sin(phi),Math.sin(lambda));
+
+ //Print residual in measurment system
+
+ // start by find the distance vector between the center and the track position
+ Hep3Vector vdiffTrk = VecOp.sub(trkpos, origin);
+ // then find the rotation from tracking to measurement frame
+ Hep3Matrix trkToStripRot = _trackerHitUtils.getTrackToStripRotation(strip);
+ // then rotate that vector into the measurement frame to get the predicted measurement position
+ Hep3Vector trkpos_meas = VecOp.mult(trkToStripRot, vdiffTrk);
+
+ // hit uncertainty in measurement frame
+ double umeas = strip.umeas();
+ double uError = strip.du();
+ double vmeas = 0;
+ double vError = (strip.vmax() - strip.vmin()) / Math.sqrt(12);
+ double wmeas = 0;
+ double wError = 10.0/Math.sqrt(12); //0.001;
+
+ // measurement in measurement frame
+ Hep3Vector m_meas = new BasicHep3Vector(umeas,vmeas,wmeas);
+
+ // residual in measurement frame
+ Hep3Vector res_meas = VecOp.sub(m_meas, trkpos_meas);
+
+ // residual uncertainty in measurement frame
+ Hep3Vector res_err_meas = new BasicHep3Vector(uError,vError,wError);
+
+ // print measurement to file
+ file.printStripMeas(res_meas.x(),res_err_meas.x());
+
+
+ // print scattering angle
+ ScatterPoint scatter = scatters.getScatterPoint(((RawTrackerHit)strip.rawhits().get(0)).getDetectorElement());
+ double scatAngle;
+
+ if(scatter != null) {
+
+ scatAngle = scatter.getScatterAngle().Angle();
+
+ }
+ else {
+
+ System.out.printf("%s: WARNING cannot find scatter for detector %s with strip cluster at %s\n",this.getClass(),((RawTrackerHit)strip.rawhits().get(0)).getDetectorElement().getName(),strip.origin().toString());
+
+ //can be edge case where helix is outside, but close to sensor, so use hack with the sensor origin closest to hit -> FIX THIS!
+ MaterialManager matMan = ((MaterialManager)_scattering.getMaterialManager());
+ DetectorPlane closest = null;
+ double dx = 999999.9;
+ if(MaterialSupervisor.class.isInstance(_scattering.getMaterialManager())) {
+ MaterialSupervisor matSup = (MaterialSupervisor)_scattering.getMaterialManager();
+ for(ScatteringDetectorVolume vol : matSup.getMaterialVolumes()) {
+ DetectorPlane plane = (DetectorPlane)vol;
+ double x_origin = plane.origin().x();
+ double dx_loop = strip.origin().x();
+ if(dx_loop<dx) {
+ dx = dx_loop;
+ closest = plane;
+ }
+ }
+ if(closest==null) {
+ throw new RuntimeException("cannot find any plane that is close to strip!");
+ } else {
+ // find scatterlength
+ double s_closest = HelixUtils.PathToXPlane(htf,closest.origin().x(), 0., 0).get(0);
+ double X0 = closest.getMaterialTraversedInRL(HelixUtils.Direction(htf, s_closest));
+ ScatterAngle scatterAngle = new ScatterAngle(s_closest, _scattering.msangle(htf.p(this._B.magnitude()),X0));
+ scatAngle = scatterAngle.Angle();
+ }
+ }
+ else {
+ throw new UnsupportedOperationException("Should not happen. This problem is only solved with the MaterialSupervisor.");
+ }
+ }
+
+
+ //print scatterer to file
+ file.printStripScat(scatAngle);
+
+
+ ++istrip;
+
+
+
+ }
+
+ }
+
+
+
+ }
+
+
+ List<MCParticle> getTruthParticle(int pdgId, List<MCParticle> mcParticles) {
+
+ if(mcParticles==null) return null;
+
+ List<MCParticle> fsParticles = new ArrayList<MCParticle>();
+ for (MCParticle mcparticle : mcParticles) {
+ if (mcparticle.getGeneratorStatus() == MCParticle.FINAL_STATE && pdgId==mcparticle.getPDGID()) {
+ fsParticles.add(mcparticle);
+ }
+ }
+ return fsParticles;
+ }
+
+
+
+ private BasicMatrix getPerParVector(HelicalTrackFit htf) {
+ BasicMatrix perPar = new BasicMatrix(1,5);
+ Hep3Vector T = HelixUtils.Direction(htf, 0.);
+ Hep3Vector p = VecOp.mult(htf.p(Math.abs(_B.z())), T);
+ double kappa = -1.0*Math.signum(htf.R())*Constants.fieldConversion*this._B.z()/htf.pT(Math.abs(_B.z()));
+ double theta = Math.PI/2.0 - Math.atan(htf.slope());
+ perPar.setElement(0,0,kappa);
+ perPar.setElement(0,1,theta);
+ perPar.setElement(0,2,htf.phi0());
+ perPar.setElement(0,3,htf.dca());
+ perPar.setElement(0,4,htf.z0());
+ return perPar;
+
+ }
+
+
+
+ private Hep3Vector getMomentum(Track trk) {
+ double pvec[] = trk.getTrackStates().get(0).getMomentum();
+ //Hep3Vector p = VecOp.mult(1.0e3,new BasicHep3Vector(pvec[0],pvec[1],pvec[2]));
+ Hep3Vector p = new BasicHep3Vector(pvec[0],pvec[1],pvec[2]);
+
+
+ return p;
+ }
+
+ private double getLambda(Track trk) {
+ return Math.atan(trk.getTrackStates().get(0).getTanLambda());
+ }
+
+
+
+ private BasicMatrix getJacPerToCl(HelicalTrackFit htf) {
+ System.out.printf("%s: getJacPerToCl\n",this.getClass().getSimpleName());
+ //use propoerly normalized B-field
+ Hep3Vector Bnorm = VecOp.mult(Constants.fieldConversion, _B);
+ //init jacobian to zero
+ BasicMatrix j = new BasicMatrix(5,5);
+ initZero(j);
+ double lambda = Math.atan(htf.slope());
+ double q = Math.signum(htf.R());
+ double theta = Math.PI/2.0 - lambda;
+ Hep3Vector T = HelixUtils.Direction(htf, 0.);
+ Hep3Vector p = VecOp.mult(htf.p(Math.abs(_B.z())), T);
+ double pT = htf.pT(Math.abs(_B.z()));
+ Hep3Vector H = VecOp.mult(1./(Bnorm.magnitude()), Bnorm);
+ Hep3Vector Z = new BasicHep3Vector(0,0,1);
+ Hep3Vector J = VecOp.mult(1./VecOp.cross(T,Z).magnitude(), VecOp.cross(T, Z));
+ Hep3Vector U = VecOp.mult(-1, J);
+ Hep3Vector V = VecOp.cross(T, U);
+ double alpha = VecOp.cross(H,T).magnitude();
+ Hep3Vector N = VecOp.mult(1./alpha,VecOp.cross(H, T));
+ Hep3Vector K = Z;
+ double gamma = VecOp.dot(H, T);
+ double Q = -Bnorm.magnitude()*q/p.magnitude();
+ double kappa = -1.0*q*Bnorm.z()/pT;
+
+ if (this._debug!=0) {
+ System.out.printf("%s: Bnorm=%s mag(Bnorm)=%f\n",this.getClass().getSimpleName(),Bnorm.toString(),Bnorm.magnitude());
+ System.out.printf("%s: p=%s |p|=%f pT=%f\n",this.getClass().getSimpleName(),p.toString(),p.magnitude(),pT);
+ System.out.printf("%s: q=%f\n",this.getClass().getSimpleName(),q);
+ System.out.printf("%s: q/p=%f\n",this.getClass().getSimpleName(),q/p.magnitude());
+ System.out.printf("%s: T=%s\n",this.getClass().getSimpleName(),T.toString());
+ System.out.printf("%s: H=%s\n",this.getClass().getSimpleName(),H.toString());
+ System.out.printf("%s: kappa=%f\n",this.getClass().getSimpleName(),kappa);
+ System.out.printf("%s: alpha=%f Q=%f \n",this.getClass().getSimpleName(),alpha,Q);
+ System.out.printf("%s: J=%s \n",this.getClass().getSimpleName(),J.toString());
+ System.out.printf("%s: V=%s \n",this.getClass().getSimpleName(),V.toString());
+ System.out.printf("%s: N=%s \n",this.getClass().getSimpleName(),N.toString());
+ System.out.printf("%s: TdotJ=%f \n",this.getClass().getSimpleName(),VecOp.dot(T, J));
+ System.out.printf("%s: VdotN=%f \n",this.getClass().getSimpleName(),VecOp.dot(V, N));
+ System.out.printf("%s: TdotK=%f \n",this.getClass().getSimpleName(),VecOp.dot(T, K));
+ System.out.printf("%s: UdotN=%f \n",this.getClass().getSimpleName(),VecOp.dot(U, N));
+ }
+
+
+
+
+
+ j.setElement(0,0,-1.0*Math.sin(theta)/Bnorm.z());
+
+ j.setElement(0,1,q/(p.magnitude()*Math.tan(theta)));
+
+ j.setElement(1,1,-1);
+
+ j.setElement(1,3,-alpha*Q*VecOp.dot(T, J)*VecOp.dot(V, N));
+
+ j.setElement(1,4,-alpha*Q*VecOp.dot(T, K)*VecOp.dot(V, N));
+
+ j.setElement(2,2,1);
+
+ j.setElement(2,3,-alpha*Q*VecOp.dot(T,J)*VecOp.dot(U, N)/Math.cos(lambda));
+
+ j.setElement(2,4,-alpha*Q*VecOp.dot(T,K)*VecOp.dot(U, N)/Math.cos(lambda));
+
+ j.setElement(3,3,-1);
+
+ j.setElement(4,4,VecOp.dot(V, K));
+
+
+ if(_debug>0) {
+ System.out.printf("%s: lambda= J(1,1)=%f * theta + J(1,3)=%f * eps + J(1,4)=%f * z0 \n",
+ this.getClass().getSimpleName(),
+ j.e(1, 1),j.e(1,3),j.e(1,4));
+
+ }
+
+
+ return j;
+
+ }
+
+
+ BasicMatrix gblSimpleJacobian(double ds,double cosl,double bfac) {
+ BasicMatrix mat = new BasicMatrix(5,5);
+ this.initUnit(mat);
+ mat.setElement(1, 0, -bfac * ds * cosl);
+ mat.setElement(3, 0, -0.5 * bfac * ds * ds * cosl);
+ mat.setElement(3, 1, ds);
+ mat.setElement(4, 2, ds);
+ return mat;
+ }
+
+
+
+ private void initUnit(BasicMatrix mat) {
+ for(int row=0;row!=mat.getNRows();row++) {
+ for(int col=0;col!=mat.getNColumns();col++) {
+ if(row!=col) mat.setElement(row, col, 0);
+ else mat.setElement(row, col, 1);
+ }
+ }
+ }
+
+ private void initZero(BasicMatrix mat) {
+ for(int row=0;row!=mat.getNRows();row++) {
+ for(int col=0;col!=mat.getNColumns();col++) {
+ mat.setElement(row, col, 0);
+ }
+ }
+ }
+
+
+
+
+ public class PerigeeParams {
+ private BasicMatrix _params;
+
+ private PerigeeParams(HelicalTrackFit htf) {
+ _params = GBLOutput.this.getPerParVector(htf);
+ }
+ public BasicMatrix getParams() {
+ return _params;
+ }
+ public double getKappa() {
+ return _params.e(0, 0);
+ }
+ public double getTheta() {
+ return _params.e(0, 1);
+ }
+ public double getPhi() {
+ return _params.e(0, 2);
+ }
+ public double getD0() {
+ return _params.e(0, 3);
+ }
+ public double getZ0() {
+ return _params.e(0, 4);
+ }
+ }
+
+ /**
+ * Computes the projection matrix from the perigee XY plane variables
+ * dca and z0 into the curvilinear xT,yT,zT frame (U,V,T)
+ * @param htf input helix to find the track direction
+ * @return 3x3 projection matrix
+ */
+ Hep3Matrix getPerToClPrj(HelicalTrackFit htf) {
+ Hep3Vector Z = new BasicHep3Vector(0,0,1);
+ Hep3Vector T = HelixUtils.Direction(htf, 0.);
+ Hep3Vector J = VecOp.mult(1./VecOp.cross(T,Z).magnitude(), VecOp.cross(T, Z));
+ Hep3Vector K = Z;
+ Hep3Vector U = VecOp.mult(-1, J);
+ Hep3Vector V = VecOp.cross(T, U);
+ Hep3Vector I = VecOp.cross(J, K);
+
+ BasicHep3Matrix trans = new BasicHep3Matrix();
+ trans.setElement(0, 0, VecOp.dot(I, U));
+ trans.setElement(0, 1, VecOp.dot(J, U));
+ trans.setElement(0, 2, VecOp.dot(K, U));
+ trans.setElement(1, 0, VecOp.dot(I, V));
+ trans.setElement(1, 1, VecOp.dot(J, V));
+ trans.setElement(1, 2, VecOp.dot(K, V));
+ trans.setElement(2, 0, VecOp.dot(I, T));
+ trans.setElement(2, 1, VecOp.dot(J, T));
+ trans.setElement(2, 2, VecOp.dot(K, T));
+ return trans;
+
+ }
+
+
+ public class ClParams {
+ private BasicMatrix _params = new BasicMatrix(1,5);
+ private ClParams(HelicalTrackFit htf) {
+
+ Hep3Matrix perToClPrj = GBLOutput.this.getPerToClPrj(htf);
+
+ double d0 = htf.dca();
+ double z0 = htf.dca();
+ Hep3Vector vecPer = new BasicHep3Vector(0.,d0,z0);
+ //System.out.printf("%s: vecPer=%s\n",this.getClass().getSimpleName(),vecPer.toString());
+
+ Hep3Vector vecCl = VecOp.mult(perToClPrj, vecPer);
+ //System.out.printf("%s: vecCl=%s\n",this.getClass().getSimpleName(),vecCl.toString());
+ double xT = vecCl.x();
+ double yT = vecCl.y();
+ double zT = vecCl.z();
+
+ Hep3Vector T = HelixUtils.Direction(htf, 0.);
+ Hep3Vector p = VecOp.mult(htf.p(Math.abs(_B.z())), T);
+ double lambda = Math.atan(htf.slope());
+ double q = Math.signum(htf.R());
+ double qOverP = q/p.magnitude();
+ double phi = htf.phi0();
+
+ _params.setElement(0, 0, qOverP);
+ _params.setElement(0, 1, lambda);
+ _params.setElement(0, 2, phi);
+ _params.setElement(0, 3, xT);
+ _params.setElement(0, 4, yT);
+
+ }
+
+ public BasicMatrix getParams() {
+ return _params;
+ }
+
+ double getLambda() {
+ return _params.e(0,1);
+ }
+ double getQoverP() {
+ return _params.e(0,0);
+ }
+ double getPhi() {
+ return _params.e(0,2);
+ }
+ double getXt() {
+ return _params.e(0,3);
+ }
+ double getYt() {
+ return _params.e(0,4);
+ }
+
+ }
+
+
+
+}