lcsim/sandbox/Partridge
diff -N FastCheck.java
--- /dev/null 1 Jan 1970 00:00:00 -0000
+++ FastCheck.java 14 Nov 2010 00:36:31 -0000 1.1
@@ -0,0 +1,580 @@
+package org.lcsim.recon.tracking.seedtracker;
+
+import hep.physics.vec.Hep3Vector;
+import hep.physics.vec.VecOp;
+import org.lcsim.constants.Constants;
+import org.lcsim.event.MCParticle;
+import org.lcsim.fit.threepointcircle.CircleFit;
+import org.lcsim.fit.helicaltrack.HelicalTrack2DHit;
+import org.lcsim.fit.helicaltrack.HelicalTrack3DHit;
+import org.lcsim.fit.helicaltrack.HelicalTrackCross;
+import org.lcsim.fit.helicaltrack.HelicalTrackFit;
+import org.lcsim.fit.helicaltrack.HelicalTrackHit;
+import org.lcsim.fit.helicaltrack.HelicalTrackStrip;
+import org.lcsim.fit.helicaltrack.HelixParamCalculator;
+import org.lcsim.fit.helicaltrack.HelixUtils;
+import org.lcsim.fit.helicaltrack.TrackDirection;
+import org.lcsim.fit.threepointcircle.ThreePointCircleFitter;
+import org.lcsim.fit.twopointcircle.TwoPointCircleFit;
+import org.lcsim.fit.twopointcircle.TwoPointCircleFitter;
+import org.lcsim.fit.twopointcircle.TwoPointLineFit;
+import org.lcsim.geometry.subdetector.BarrelEndcapFlag;
+import org.lcsim.recon.tracking.seedtracker.diagnostic.ISeedTrackerDiagnostics;
+
+/**
+ *
+ * @author Richard Partridge
+ */
+public class FastCheck {
+
+ private SeedStrategy _strategy;
+ private double _bfield;
+ private double _RMin;
+ private double _dMax;
+ private double _z0Max;
+ private double _nsig;
+ private TwoPointCircleFitter _cfit2;
+ private ThreePointCircleFitter _cfit3;
+ private static double twopi = 2. * Math.PI;
+ private double _eps = 1.0e-6;
+ private ISeedTrackerDiagnostics _diag;
+
+ public FastCheck(SeedStrategy strategy, double bfield, ISeedTrackerDiagnostics diag) {
+ _strategy = strategy;
+ _bfield = bfield;
+ _diag = diag;
+
+ // Calculate the minimum radius of curvature, maximum DCA and Maximum z0
+ _RMin = strategy.getMinPT() / (Constants.fieldConversion * bfield);
+ _dMax = strategy.getMaxDCA();
+ _z0Max = strategy.getMaxZ0();
+ _nsig = Math.sqrt(strategy.getMaxChisq());
+
+ // Instantiate the two point circle fitter for this minimum radius, maximum DCA
+ _cfit2 = new TwoPointCircleFitter(_RMin);
+
+ // Instantiate the three point circle fitter
+ _cfit3 = new ThreePointCircleFitter();
+ }
+
+ public boolean CheckHitSeed(HelicalTrackHit hit, SeedCandidate seed) {
+
+ // Check the hit against each hit in the seed
+ for (HelicalTrackHit hit2 : seed.getHits()) {
+ if (!TwoPointCircleCheck(hit, hit2, seed)) return false;
+ }
+ return true;
+ }
+
+ public boolean CheckSector(SeedCandidate seed, Sector sector) {
+
+ boolean match = false;
+ if (_diag != null && seed.isTrueSeed()) {
+ for (HelicalTrackHit hit : sector.Hits()) {
+ for (MCParticle mcp : hit.getMCParticles()) {
+ if (seed.getMCParticles().contains(mcp)) match = true;
+ }
+ }
+ }
+
+ // Get limits on r, phi, and z for hits in this sector
+ double rmin = sector.rmin();
+ double rmax = sector.rmax();
+ double phimin = sector.phimin();
+ double phimax = sector.phimax();
+ double zmin = sector.zmin();
+ double zmax = sector.zmax();
+
+ // Calculate the midpoint and half the span in phi for this layer
+ double midphisec = (phimin + phimax) / 2.;
+ double dphisec = 0.5 * (phimax - phimin);
+
+ // Check each hit for compatibility with this sector
+ for (HelicalTrackHit hit : seed.getHits()) {
+
+ // Adjust the hit position for stereo hits
+ CorrectHitPosition(hit, seed);
+
+ // Calculate the max track angle change between the hit and sector layer
+ double dphitrk1 = dphimax(hit.r(), rmin);
+ double dphitrk2 = dphimax(hit.r(), rmax);
+ double dphitrk = Math.max(dphitrk1, dphitrk2);
+
+ // Calculate the phi dev between the hit and midpoint of the sector
+ double dphi = phidif(hit.phi(), midphisec);
+
+ // The maximum dphi is the sum of the track bend and half the sector span
+ double dphimx = dphitrk + dphisec;
+ if (dphi > dphimx) {
+ if (match) {
+ for (MCParticle mcp : seed.getMCParticles()) {
+ System.out.println("phi sector error - p: "+mcp.getMomentum().toString());
+ }
+ }
+ return false;
+ }
+
+ double smin1 = smin(rmin);
+ double smax1 = smax(rmax);
+ double r = hit.r();
+ double smin2 = smin(r);
+ double smax2 = smax(r);
+
+ // Get the z limits for the hit
+ double zlen = 0.;
+ if (hit instanceof HelicalTrack2DHit) {
+ zlen = ((HelicalTrack2DHit) hit).zlen();
+ }
+ double zmin2 = hit.z() - 0.5 * zlen;
+ double zmax2 = zmin2 + zlen;
+
+ // Check the z0 limits
+ boolean zOK = checkz0(smin1, smax1, zmin, zmax, smin2, smax2, zmin2, zmax2);
+ if (!zOK) {
+ if (match) {
+ for (MCParticle mcp : seed.getMCParticles()) {
+ System.out.println("z sector error - p: "+mcp.getMomentum().toString());
+ }
+ }
+ return false;
+ }
+ }
+ return true;
+ }
+
+ public boolean CheckSectorPair(Sector s1, Sector s2) {
+
+ // Calculate the maximum change in azimuth
+ double dphi1 = dphimax(s1.rmin(), s2.rmax());
+ double dphi2 = dphimax(s1.rmax(), s2.rmin());
+
+ // Calculate the angular difference between the midpoints of the 2 sectors
+ double mid1 = (s1.phimax() + s1.phimin()) / 2.0;
+ double mid2 = (s2.phimax() + s2.phimin()) / 2.0;
+ double dmid = phidif(mid1, mid2);
+
+ // Calculate the half widths of the 2 sectors
+ double wid1 = s1.phimax() - mid1;
+ double wid2 = s2.phimax() - mid2;
+
+ // Check that the sectors are compatible in the bend coordinate
+ boolean phiOK;
+
+ phiOK = dmid < dphi1 + wid1 + wid2;
+ if (!phiOK) phiOK = dmid < dphi2 + wid1 + wid2;
+ if (!phiOK) return false;
+
+ // Get the minimum and maximum path lengths
+ double s1min = smin(s1.rmin());
+ double s2min = smin(s2.rmin());
+ double s1max = smax(s1.rmax());
+ double s2max = smax(s2.rmax());
+
+ // Get the minimum and maximum z's
+ double z1min = s1.zmin();
+ double z2min = s2.zmin();
+ double z1max = s1.zmax();
+ double z2max = s2.zmax();
+
+ // Check that the sectors are compatible in the non-bend coordinate
+ boolean zOK = checkz0(s1min, s1max, z1min, z1max, s2min, s2max, z2min, z2max);
+
+ return zOK;
+ }
+
+ public boolean TwoPointCircleCheck(HelicalTrackHit hit1, HelicalTrackHit hit2, SeedCandidate seed) {
+
+ boolean match = false;
+ MCParticle mcp = null;
+ for (MCParticle mcptrial : hit1.getMCParticles()) {
+ if (hit2.getMCParticles().contains(mcptrial)) {
+// match = true;
+ mcp = mcptrial;
+ break;
+ }
+ }
+
+ // Initialize the hit coordinates for an unknown track direction
+ CorrectHitPosition(hit1, seed);
+ CorrectHitPosition(hit2, seed);
+
+ // Check that hits are outside the maximum DCA
+ if (hit1.r() < _dMax || hit2.r() < _dMax) return false;
+
+ // Try to find a circle passing through the 2 hits and the maximum DCA
+ boolean success = false;
+ try
+ {
+ success = _cfit2.FitCircle(hit1, hit2, _dMax);
+ }
+ catch(Exception x){}
+
+
+ // Check for success
+ if (!success) return false;
+
+ // Initialize the minimum/maximum arc lengths
+ double s1min = 1.0e99;
+ double s1max = -1.0e99;
+ double s2min = 1.0e99;
+ double s2max = -1.0e99;
+
+ // Loop over the circle fits and find the min/max arc lengths
+ for (TwoPointCircleFit fit : _cfit2.getCircleFits()) {
+ double s1 = fit.s1();
+ double s2 = fit.s2();
+ if (s1 < s1min) s1min = s1;
+ if (s1 > s1max) s1max = s1;
+ if (s2 < s2min) s2min = s2;
+ if (s2 > s2max) s2max = s2;
+ }
+
+ // If we are consistent with a straight-line fit, update the minimum s1, s2
+ TwoPointLineFit lfit = _cfit2.getLineFit();
+ if (lfit != null) {
+
+ // Find the distance from the DCA to the maximum DCA circle
+ double x0 = lfit.x0();
+ double y0 = lfit.y0();
+ double s0 = 0.;
+ double s0sq = _dMax*_dMax - (x0*x0 + y0*y0);
+ if (s0sq > _eps*_eps) s0 = Math.sqrt(s0sq);
+
+ // Update the minimum arc length to the distance from the DCA to the hit
+ s1min = lfit.s1() - s0;
+ s2min = lfit.s2() - s0;
+ }
+
+ // Now check for consistent hits in the s-z plane
+ // First get the z limits for the hits
+ double z1len = 0.;
+ double z2len = 0.;
+ if (hit1 instanceof HelicalTrack2DHit) {
+ z1len = ((HelicalTrack2DHit) hit1).zlen();
+ }
+ if (hit2 instanceof HelicalTrack2DHit) {
+ z2len = ((HelicalTrack2DHit) hit2).zlen();
+ }
+ double z1min = hit1.z() - 0.5 * z1len;
+ double z1max = z1min + z1len;
+ double z2min = hit2.z() - 0.5 * z2len;
+ double z2max = z2min + z2len;
+
+ if (match) System.out.println(" z1min: "+z1min+" z1max: "+z1max+" z2min: "+z2min+" z2max: "+z2max);
+ if (match) System.out.println("Hit 1 position: "+hit1.getCorrectedPosition().toString());
+ if (match) System.out.println("Hit 2 position: "+hit2.getCorrectedPosition().toString());
+ if (match) {
+ HelixParamCalculator hmc = new HelixParamCalculator(mcp, _bfield);
+ double[] pars = new double[5];
+ pars[HelicalTrackFit.curvatureIndex] = hmc.getMCOmega();
+ pars[HelicalTrackFit.dcaIndex] = hmc.getDCA();
+ pars[HelicalTrackFit.phi0Index] = hmc.getPhi0();
+ pars[HelicalTrackFit.slopeIndex] = hmc.getSlopeSZPlane();
+ pars[HelicalTrackFit.z0Index] = hmc.getZ0();
+ HelicalTrackFit helixmc = new HelicalTrackFit(pars, null, null, null, null, null);
+ double s1mc = HelixUtils.PathToZPlane(helixmc, hit1.z());
+ Hep3Vector pos1mc = HelixUtils.PointOnHelix(helixmc, s1mc);
+ double s2mc = HelixUtils.PathToZPlane(helixmc, hit2.z());
+ Hep3Vector pos2mc = HelixUtils.PointOnHelix(helixmc, s2mc);
+ System.out.println("s1 mc: "+s1mc+" pos 1 mc: "+pos1mc.toString());
+ System.out.println("s2 mc: "+s2mc+" pos 2 mc: "+pos2mc.toString());
+ if (hit1 instanceof HelicalTrackCross) {
+ HelicalTrackCross cross = (HelicalTrackCross) hit1;
+ for (HelicalTrackStrip strip : cross.getStrips()) {
+ Hep3Vector dist = VecOp.sub(VecOp.add(strip.origin(), VecOp.mult(strip.umeas(), strip.u())), pos1mc);
+ System.out.println("Strip 1 error: "+(VecOp.dot(dist, strip.u())));
+ }
+ }
+ if (hit2 instanceof HelicalTrackCross) {
+ HelicalTrackCross cross = (HelicalTrackCross) hit2;
+ for (HelicalTrackStrip strip : cross.getStrips()) {
+ Hep3Vector dist = VecOp.sub(VecOp.add(strip.origin(), VecOp.mult(strip.umeas(), strip.u())), pos2mc);
+ System.out.println("Strip 2 error: "+(VecOp.dot(dist, strip.u())));
+ }
+ }
+ }
+
+ // Check the z0 limits using the min/max path lengths
+ boolean zOK = checkz0(s1min, s1max, z1min, z1max, s2min, s2max, z2min, z2max);
+
+ // Done!
+ return zOK;
+ }
+
+ public boolean ThreePointHelixCheck(HelicalTrackHit hit1, HelicalTrackHit hit2, HelicalTrackHit hit3) {
+
+ // Setup for a 3 point circle fit
+ double p[][] = new double[3][2];
+ double[] pos;
+ double z[] = new double[3];
+ double dztot = 0.;
+ int indx;
+ HelicalTrackHit hit;
+ boolean zfirst = true;
+
+ // While not terribly elegant, code for speed
+ // Use calls that give uncorrected position and error
+ // Get the relevant variables for hit 1
+ indx = 0;
+ hit = hit1;
+ pos = hit.getPosition();
+ p[indx][0] = pos[0];
+ p[indx][1] = pos[1];
+ z[indx] = pos[2];
+ if (hit.BarrelEndcapFlag() == BarrelEndcapFlag.BARREL) {
+ if (hit instanceof HelicalTrack3DHit) dztot += _nsig * ((HelicalTrack3DHit) hit).dz();
+ else {
+ zfirst = false;
+ if (hit instanceof HelicalTrack2DHit) dztot += ((HelicalTrack2DHit) hit).zlen() / 2.;
+ else dztot += _nsig * Math.sqrt(hit.getCovMatrix()[5]);
+ }
+ } else {
+ dztot += hit.dr() * Math.abs(pos[2]) / Math.sqrt(pos[0]*pos[0] + pos[1]*pos[1]);
+ }
+
+ // Get the relevant variables for hit 2
+ indx = 1;
+ hit = hit2;
+ pos = hit.getPosition();
+ p[indx][0] = pos[0];
+ p[indx][1] = pos[1];
+ z[indx] = pos[2];
+ if (hit.BarrelEndcapFlag() == BarrelEndcapFlag.BARREL) {
+ if (hit instanceof HelicalTrack3DHit) dztot += _nsig * ((HelicalTrack3DHit) hit).dz();
+ else {
+ zfirst = false;
+ if (hit instanceof HelicalTrack2DHit) dztot += ((HelicalTrack2DHit) hit).zlen() / 2.;
+ else dztot += _nsig * Math.sqrt(hit.getCovMatrix()[5]);
+ }
+ } else {
+ dztot += hit.dr() * Math.abs(pos[2]) / Math.sqrt(pos[0]*pos[0] + pos[1]*pos[1]);
+ }
+
+ // Get the relevant variables for hit 3
+ indx = 2;
+ hit = hit3;
+ pos = hit.getPosition();
+ p[indx][0] = pos[0];
+ p[indx][1] = pos[1];
+ z[indx] = pos[2];
+ if (hit.BarrelEndcapFlag() == BarrelEndcapFlag.BARREL) {
+ if (hit instanceof HelicalTrack3DHit) dztot += _nsig * ((HelicalTrack3DHit) hit).dz();
+ else {
+ zfirst = false;
+ if (hit instanceof HelicalTrack2DHit) dztot += ((HelicalTrack2DHit) hit).zlen() / 2.;
+ else dztot += _nsig * Math.sqrt(hit.getCovMatrix()[5]);
+ }
+ } else {
+ dztot += hit.dr() * Math.abs(pos[2]) / Math.sqrt(pos[0]*pos[0] + pos[1]*pos[1]);
+ }
+
+ // Add multiple scattering error here - for now, just set it to 1 mm
+ dztot += 1.;
+
+ // Unless the three hits are all pixel hits, do the circle checks first
+ if (!zfirst) {
+ if (!TwoPointCircleCheck(hit1, hit3, null)) return false;
+ if (!TwoPointCircleCheck(hit2, hit3, null)) return false;
+ }
+
+ // Do the 3 point circle fit and check for success
+ boolean success = _cfit3.fit(p[0], p[1], p[2]);
+ if (!success) return false;
+
+ // Retrieve the circle parameters
+ CircleFit circle = _cfit3.getFit();
+ double xc = circle.x0();
+ double yc = circle.y0();
+ double rc = Math.sqrt(xc*xc + yc*yc);
+ double rcurv = circle.radius();
+
+ // Find the point of closest approach
+ double x0 = xc * (1. - rcurv / rc);
+ double y0 = yc * (1. - rcurv / rc);
+
+ // Find the x-y arc lengths to the hits and the smallest arc length
+ double phi0 = Math.atan2(y0-yc, x0-xc);
+ double[] dphi = new double[3];
+ double dphimin = 999.;
+
+ for (int i=0; i<3; i++) {
+ // Find the angle between the hits and the DCA under the assumption that |dphi| < pi
+ dphi[i] = Math.atan2(p[i][1]-yc, p[i][0]-xc) - phi0;
+ if (dphi[i] > Math.PI) dphi[i] -= twopi;
+ if (dphi[i] < -Math.PI) dphi[i] += twopi;
+// System.out.println("dphi = "+dphi[i]+" for hit "+i);
+ if (Math.abs(dphi[i]) < Math.abs(dphimin)) dphimin = dphi[i];
+ }
+
+ // Use the hit closest to the DCA to determine the circle "direction"
+ boolean cw = dphimin < 0.;
+
+ // Find the arc lengths to the hits
+ double[] s = new double[3];
+ for (int i=0; i<3; i++) {
+
+ // Arc set to be positive if they have the same sign as dphimin
+ if (cw) s[i] = -dphi[i] * rcurv;
+ else s[i] = dphi[i] * rcurv;
+
+ // Treat the case where a point has dphi opposite in sign to dphimin as an incoming looper hit
+ if (s[i] < 0.) s[i] += twopi * rcurv;
+ }
+
+ // Order the arc lengths and z info by increasing arc length
+ for (int i=0; i<2; i++) {
+ for (int j=i+1; j<3; j++) {
+ if (s[j] < s[i]) {
+ double temp = s[i];
+ s[i] = s[j];
+ s[j] = temp;
+ temp = z[i];
+ z[i] = z[j];
+ z[j] = temp;
+ }
+ }
+ }
+
+ // Predict the middle z and see if it is consistent with the measurements
+ double slope = (z[2] - z[0]) / (s[2] - s[0]);
+ double z0 = z[0] - s[0] * slope;
+ double zpred = z0 + s[1] * slope;
+ if (Math.abs(zpred - z[1]) > dztot) return false;
+
+ // If we haven't already done the circle checks, do them now
+ if (zfirst) {
+ if (!TwoPointCircleCheck(hit1, hit3, null)) return false;
+ if (!TwoPointCircleCheck(hit2, hit3, null)) return false;
+ }
+
+ // Passed all checks - success!
+ return true;
+ }
+
+
+ private double dphimax(double r1, double r2) {
+
+ // Order the two radii
+ double rmin = r1;
+ double rmax = r2;
+ if (rmin > rmax) {
+ rmin = r2;
+ rmax = r1;
+ }
+
+ // Don't let the maximum DCA exceed rmin
+ double d = _dMax;
+ if (d > rmin) d = rmin = _eps;
+
+ // First try for a circle that fits between the DCA and rmax...
+ double R = (rmax + _dMax) / 2.0 + _eps;
+ // ...but don't let the circle have a radius smaller than the minimum allowed
+ if (R < _RMin) R = _RMin;
+
+ // Calculate the maximum deviations from a straight-line track
+ double cth1 = (d * d + rmin * rmin - 2 * R * d) / (2 * rmin * (R - d));
+ double cth2 = (d * d + rmax * rmax - 2 * R * d) / (2 * rmax * (R - d));
+ if (Math.abs(cth1) > 1 || Math.abs(cth2) > 1) return 0.;
+ return phidif(Math.acos(cth1), Math.acos(cth2));
+ }
+
+ private double smin(double r) {
+ return r - _dMax;
+ }
+
+ private double smax(double r) {
+ double R = _RMin;
+ if (r > _RMin) R = r;
+ return 2.0 * R * Math.asin((r + _dMax) / (2.0 * R));
+ }
+
+ private boolean checkz0(double s1min, double s1max, double zmin1, double zmax1,
+ double s2min, double s2max, double zmin2, double zmax2) {
+
+ double z0[] = new double[2];
+ double z1[] = new double[4];
+ double z2[] = new double[2];
+ double s1[] = new double[4];
+ double s2[] = new double[2];
+
+ // Set limits on z0
+ z0[0] = -_z0Max;
+ z0[1] = _z0Max;
+
+ // Set corners of allowed region for s1, z1
+ z1[0] = zmin1;
+ z1[1] = zmin1;
+ z1[2] = zmax1;
+ z1[3] = zmax1;
+ s1[0] = s1min;
+ s1[1] = s1max;
+ s1[2] = s1min;
+ s1[3] = s1max;
+
+ // Set limits on z2, s2
+ z2[0] = zmin2;
+ z2[1] = zmax2;
+ s2[0] = s2min;
+ s2[1] = s2max;
+
+ // Initialize min/max of s, z at point 2
+ double zmax = -1.0e10;
+ double zmin = 1.0e10;
+ double smax = -1.0e10;
+ double smin = 1.0e10;
+
+ // Loop over z0 limits
+ for (int i=0; i<2; i++) {
+
+ // Loop over corners of s1, z1
+ for (int j=0; j<4; j++) {
+
+ // Calculate slope of line in s-z space from z0 limit to point 1 corner
+ double slope = (z1[j] - z0[i]) / s1[j];
+
+ // Loop over limits on z2, s2
+ for (int k=0; k<2; k++) {
+
+ // Calculate extrapolation of s-z line to the point 2 limit
+ double z = z0[i] + s2[k] * slope;
+ double s = (z2[k] - z0[i]) / slope;
+
+ // Find the min/max values of the extrapolated s, z at point 2
+ if (z > zmax) zmax = z;
+ if (z < zmin) zmin = z;
+ if (s > smax) smax = s;
+ if (s < smin) smin = s;
+ }
+ }
+ }
+
+ // Check to see if the extrapolated points are consistent with measurements
+ boolean checkz0 = (zmin2 <= zmax && zmax2 >= zmin) || (s2min <= smax && s2max >= smin);
+
+ return checkz0;
+ }
+
+ private double phidif(double phi1, double phi2) {
+
+ // Find the magnitude of the phi difference
+ double phidif = Math.abs(phi2 - phi1);
+
+ // Properly wrap around two pi to always give the smaller phi dif
+ if (phidif > Math.PI) phidif = 2. * Math.PI - phidif;
+ return phidif;
+ }
+
+ private void CorrectHitPosition(HelicalTrackHit hit, SeedCandidate seed) {
+ if (hit instanceof HelicalTrackCross) {
+ HelicalTrackCross cross = (HelicalTrackCross) hit;
+ HelicalTrackFit helix = null;
+ if (seed != null) helix = seed.getHelix();
+ if (helix != null) {
+ TrackDirection trkdir = HelixUtils.CalculateTrackDirection(helix, helix.PathMap().get(hit));
+ cross.setTrackDirection(trkdir, helix.covariance());
+ } else {
+ cross.resetTrackDirection();
+ }
+ }
+ }
+}
\ No newline at end of file
