Commit in lcsim/sandbox on MAIN | |||
HelixUtils.java | +555 | added 1.1 |
stash Pelle's copy of this file in sandbox
diff -N HelixUtils.java --- /dev/null 1 Jan 1970 00:00:00 -0000 +++ HelixUtils.java 18 Oct 2012 18:47:30 -0000 1.1 @@ -0,0 +1,555 @@
+/* + * HelixUtils.java + * + * Created on February 1, 2008, 5:48 PM + * + */ + +package org.lcsim.fit.helicaltrack; + +import hep.physics.matrix.BasicMatrix; +import hep.physics.matrix.Matrix; +import hep.physics.matrix.SymmetricMatrix; +import hep.physics.vec.BasicHep3Vector; +import hep.physics.vec.Hep3Vector; +import java.util.ArrayList; +import java.util.List; +import org.lcsim.fit.circle.CircleFit; + +/** + * Assorted helix utilities that operate on HelicalTrackFits. + * + * Since these utilities are typically called many times for a + * given HelicalTrackFit (or in some cases for a given CircleFit), + * a number of derived quantities used by the methods in this class + * are cached to minimize re-computation. Since a HelicalTrackFit + * (or CircleFit) is immutable, these quantities only need to be + * calculated once. + * @author Richard Partridge + * @version 1.0 + */ +public class HelixUtils { + private static double _minslope = 1.e-6; + private static double _maxpath = 2400; // todo, add a more "dynamic" maxpath + private static double _epsilon = 0.01; // value below which a z vector Or an angle is considered as null + private static double _tol = 1.e-4; // Maximum aspect ratio of hit covariance terms for hit to measure x-y coord + + /** + * Creates a new instance of HelixUtils. + */ + public HelixUtils() { + } + + /** + * Return the x-y path length between two HelicalTrackHits. + * @param cfit CircleFit to be used in calculating the path length + * @param hit1 first hit + * @param hit2 second hit + * @return path length from hit1 to hit2 + */ + public static double PathLength(CircleFit cfit, HelicalTrackHit hit1, HelicalTrackHit hit2) { + // Find the position on the circle for the hits + Hep3Vector pos1 = getPositionOnCircle(cfit, hit1); + Hep3Vector pos2 = getPositionOnCircle(cfit, hit2); + // Return the path length between hit1 and hit2 + return PathCalc(xc(cfit), yc(cfit), RC(cfit), pos1.x(), pos1.y(), pos2.x(), pos2.y()); + } + + /** + * Return the x-y path length from the DCA. + * @param cfit CircleFit to be used in calculating the path length + * @param hit hit to be used for the path length calculation + * @return path length from the DCA to the hit + */ + public static double PathLength(CircleFit cfit, HelicalTrackHit hit) { + // Find the position on the circle for the hit + Hep3Vector pos = getPositionOnCircle(cfit, hit); + // Return the path length from the DCA + return PathCalc(xc(cfit), yc(cfit), RC(cfit), x0(cfit), y0(cfit), pos.x(), pos.y()); + } + + /** + * Return the x-y path length from the DCA to a HelicalTrackHit. + * @param helix HelicalTrackFit to be used in calculating the path length + * @param hit hit to be used for the path length calculation + * @return path length from the DCA to the hit + */ + public static double PathLength(HelicalTrackFit helix, HelicalTrackHit hit) { + + // Calculate the shortest path length from the DCA to the hit x-y coordinates + double path0 = PathCalc(helix.xc(), helix.yc(), helix.R(), helix.x0(), helix.y0(), hit.x(), hit.y()); + if (hit instanceof HelicalTrack2DHit) return path0; + + // Find the closest 3D hit in the z coordinate (if there is one) + HelicalTrackHit close = null; + double zhit = hit.z(); + for (HelicalTrackHit fithit : helix.PathMap().keySet()) { + if (!(fithit instanceof HelicalTrack2DHit)) { + + if (close == null) { + // If this is the first 3D hit, mark it as being closest + close = fithit; + } else { + // Check if the fithit is closer in z + if (Math.abs(zhit - fithit.z()) < Math.abs(zhit - close.z())) close = fithit; + } + } + } + + // If we didn't find another 3D hit, return the path from the DCA + if (close == null) return path0; + + // Find the path length for the closest hit assuming it hasn't looped + double path = helix.PathMap().get(close) + + PathCalc(helix.xc(), helix.yc(), helix.R(), close.x(), close.y(), hit.x(), hit.y()); + return path; + } + + /** + * Return the x-y path length to a z-plane. + * @param helix HelicalTrackFit to be used in calculating the path length + * @param z location of z-plane + * @return path length from the DCA to the z-plane + */ + public static double PathToZPlane(HelicalTrackFit helix, double z) { + // Find the z distace between the DCA and the z plane and calculate the x-y path length + double zdist = z - helix.z0(); + double safeslope = helix.slope(); + if (Math.abs(safeslope) < _minslope) safeslope = _minslope * Math.signum(safeslope); + return zdist / safeslope; + } + + /** + * Return the x-y path length to an x-plane. + * @param helix HelicalTrackFit to be used in calculating the path length + * @param x location of x-plane + * @return path length from the DCA to the x-plane + */ + public static List<Double> PathToXPlane(HelicalTrackFit helix, double x,double smax, int mxint) { + // Create a list to hold the path lengths + List<Double> pathlist = new ArrayList<Double>(); + // Retrieve helix dca and RC + double x0 = helix.x0(); + double y0 = helix.y0(); + + double xc=helix.xc(); + double yc=helix.yc(); + double RC = helix.R(); + double y=yc+Math.signum(RC)*Math.sqrt(RC*RC-Math.pow(x-xc,2)); + + double s=PathCalc(xc,yc,RC,x0,y0,x,y); + +// System.out.println("PathToXPlane : s = "+s+"; sFromClass = "+sFromClass); + /* + while (s < smax && pathlist.size() < mxint) { + // Add this odd-numbered crossing to the list + pathlist.add(s); + // Advance to the next even-numbered crossing + s += 2. * (Math.PI - dphi) * Math.abs(RC); + // Check to see if we should add it + if (s < smax && pathlist.size() < mxint) pathlist.add(s); + // Add this even-numbered crossing to the list + s += 2. * dphi * Math.abs(RC); + }*/ + pathlist.add(s); + + return pathlist; + } + + + /** + * Return a list of x-y path lengths to a cylinder centered on the z axis. + * @param helix HelicalTrackFit to be used in calculating the path length + * @param r desired radius + * @param smax maximum path length to be considered + * @param mxint Maximum number of intersections + * @return list of path lengths + */ + public static List<Double> PathToCylinder(HelicalTrackFit helix, double r, double smax, int mxint) { + // Create a list to hold the path lengths + List<Double> pathlist = new ArrayList<Double>(); + // Retrieve helix dca and RC + double dca = helix.dca(); + double RC = helix.R(); + // Find cos(dphi) for dphi from the point of closest approach to the first crossing + double cdphi = 1 - (r * r - dca * dca) / (2. * RC * (RC - dca)); + // Make sure we have a physical intersection + if (Math.abs(cdphi) < 1.) { + // Calculate dphi and the path length to the first crossing + double dphi = Math.acos(cdphi); + Double s = dphi * Math.abs(RC); + // Loop over crossings until we exceed one of the limits + while (s < smax && pathlist.size() < mxint) { + // Add this odd-numbered crossing to the list + pathlist.add(s); + // Advance to the next even-numbered crossing + s += 2. * (Math.PI - dphi) * Math.abs(RC); + // Check to see if we should add it + if (s < smax && pathlist.size() < mxint) pathlist.add(s); + // Add this even-numbered crossing to the list + s += 2. * dphi * Math.abs(RC); + } + } + return pathlist; + } + + /** + * Return a unit vector giving the track direction at a given point on + * the helix. + * @param helix HelicalTrackFit to be used in calculating direction + * @param s path length to the desired point on helix + * @return direction unit vector + */ + public static Hep3Vector Direction(HelicalTrackFit helix, double s) { + // Calculate the azimuthal direction + double phi = helix.phi0() - s / helix.R(); + double sth = helix.sth(); + // Calculate the components of the direction unit vector + double ux = Math.cos(phi) * helix.sth(); + double uy = Math.sin(phi) * helix.sth(); + double uz = helix.cth(); + // Return the direction unit vector + return new BasicHep3Vector(ux, uy, uz); + } + + /** + * Return the TrackDirection object for a given point on a helix. + * This might seem like an odd place to put this code, but putting + * it here allows the cached helix quantities to be used in constructin + * the TrackDirection objects. + * @param helix HelicalTrackFit to use in constructing the TrackDirection + * @param s path length specifying location on helix + * @return TrackDirection object for this point on the helix + */ + public static TrackDirection CalculateTrackDirection(HelicalTrackFit helix, double s) { + Hep3Vector dir = Direction(helix, s); + Matrix deriv = DirectionDerivates(helix, dir, s); + return new TrackDirection(dir, deriv); + } + + /** + * Return the location in space for a particular point on a helix. + * @param helix HelicalTrackFit to be used + * @param s path length + * @return point in space corresponding to the given path length + */ + public static Hep3Vector PointOnHelix(HelicalTrackFit helix, double s) { + // Find the azimuthal direction at this path length + double RC = helix.R(); + double phi = helix.phi0() - s / RC; + // Calculate the position on the helix at this path length + double x = helix.xc() - RC * Math.sin(phi); + double y = helix.yc() + RC * Math.cos(phi); + double z = helix.z0() + s * helix.slope(); + // Return the position as a Hep3Vector + return new BasicHep3Vector(x, y, z); + } + + private static Hep3Vector getPositionOnCircle(CircleFit cfit, HelicalTrackHit hit) { + + // Get hit position and errors + Hep3Vector pos = hit.getCorrectedPosition(); + SymmetricMatrix cov = hit.getCorrectedCovMatrix(); + double dxdx = cov.diagonal(0); + double dydy = cov.diagonal(1); + if ((dxdx < _tol * dydy) && (dydy < _tol * dxdx)) return pos; + + // Get circle parameters + double x0 = xc(cfit); + double y0 = yc(cfit); + double R = Math.abs(RC(cfit)); + + // Get hit coordinates + double x = pos.x(); + double y = pos.y(); + + + if (_tol * dxdx > dydy && Math.abs(y-y0) < R) { + double xnew1 = Math.sqrt(R*R - (y-y0)*(y-y0)) + x0; + double xnew2 = -xnew1 + 2. * x0; + if (Math.abs(xnew1 - x) < Math.abs(xnew2 - x)) { + x = xnew1; + } else { + x = xnew2; + } + } + if (_tol * dydy > dxdx && Math.abs(x-x0) < R) { + double ynew1 = Math.sqrt(R*R - (x-x0)*(x-x0)) + y0; + double ynew2 = -ynew1 + 2. * y0; + if (Math.abs(ynew1 - y) < Math.abs(ynew2 - y)) { + y = ynew1; + } else { + y = ynew2; + } + } + + return new BasicHep3Vector(x, y, pos.z()); + } + + private static double PathCalc(double xc, double yc, double RC, double x1, double y1, double x2, double y2) { + // Find the angle between these points measured wrt the circle center + double phi1 = Math.atan2(y1 - yc, x1 - xc); + double phi2 = Math.atan2(y2 - yc, x2 - xc); + double dphi = phi2 - phi1; + // Make sure dphi is in the valid range (-pi, pi) + if (dphi > Math.PI) dphi -= 2. * Math.PI; + if (dphi < -Math.PI) dphi += 2. * Math.PI; + // Return the arc length + return -RC * dphi; + } + + /** + * Return the derivatives of the momentum unit vector with respect to the + * helix parameters. The direction derivatives are returned in matrix + * form, with the row giving the cartesian component of the momentum + * vector and the column giving the helix parameter. + * @param helix HelicalTrackFit to be used in calculating derivatives + * @param s path length to the desired point on the helix + * @return direction derivatives + */ + private static Matrix DirectionDerivates(HelicalTrackFit helix, Hep3Vector u, double s) { + // Create the matrix that will hold the derivatives + BasicMatrix deriv = new BasicMatrix(3,5); + // Retrieve some helix info + double cphi0 = Math.cos(helix.phi0()); + double sphi0 = Math.sin(helix.phi0()); + double sth = helix.sth(); + double cth = helix.cth(); + double dca = helix.dca(); + double omega = helix.curvature(); + // Calculate the non-zero derivatives of the direction with respect to the helix parameters + deriv.setElement(0, HelicalTrackFit.curvatureIndex, (u.x() - cphi0 * sth) / omega); // du_x / domega + deriv.setElement(1, HelicalTrackFit.curvatureIndex, (u.y() - sphi0 * sth) / omega); // du_y / domega + deriv.setElement(0, HelicalTrackFit.dcaIndex, -omega * cphi0 * sth); // du_x / ddca + deriv.setElement(1, HelicalTrackFit.dcaIndex, -omega * sphi0 * sth); // du_y / ddca + deriv.setElement(0, HelicalTrackFit.phi0Index, -(1 - dca * omega) * sphi0 * sth); // du_x / dphi0 + deriv.setElement(1, HelicalTrackFit.phi0Index, (1 - dca * omega) * cphi0 * sth); // du_y / dphi0 + deriv.setElement(0, HelicalTrackFit.slopeIndex, -u.x() * sth * cth); // du_x / dslope + deriv.setElement(1, HelicalTrackFit.slopeIndex, -u.y() * sth * cth); // du_y / dslope + deriv.setElement(2, HelicalTrackFit.slopeIndex, sth*sth*sth); // du_z / dslope + // Return the derivative matrix + return deriv; + } + /** + * return true if the helix is intercepting the bounded cylinder + * + * Check if at least one of the first ten intersection of the helix with the cylinder + * is between zmin and zmax...there might be a better way to do this + * @param helix + * @param r rarius of the cylinder + * @param zmin lower bound of the cylinder + * @param zmax higher bound of the cylinder + * @return + */ + public static boolean isInterceptingBoundedCylinder(HelicalTrackFit helix,double r,double zmin,double zmax){ + double minpath = PathToZPlane(helix, zmin);//not sure it's very efficient to calculate the maximum path + double maxpath = PathToZPlane(helix, zmax); + double path = Math.max(minpath, maxpath); + List<Double> pathlist = PathToCylinder(helix,r,path,10); + for(double s:pathlist){ + double z = PointOnHelix(helix,s).z(); + if(z<zmax && z> zmin) + return true; + } + return false; + } + /** + * return true if the helix is intercepting the given disk + * @param helix + * @param rmin + * @param rmax + * @param z + * @return + */ + public static boolean isInterceptingZDisk(HelicalTrackFit helix,double rmin,double rmax, double z ){ + double s = PathToZPlane(helix,z); + Hep3Vector point = PointOnHelix(helix,s); + double x = point.x(); + double y = point.y(); + double r = Math.sqrt(x*x+y*y); + if(r < rmax && r > rmin ){return true;} + return false; + } + + /** + * return true if the point on the helix at coord z is intercepting a given ZPolygon, the + * method don't check if the polygone is parallel to a z plane + * + * this method check if the private methode insidePolygone return a value a less than .5 deg from 2PI + * + * @param helix + * @param z + * @param vertices + * @return true OR false wether the helix intercept or not the polygone + */ + public static boolean isInterceptingZpolygon(HelicalTrackFit helix,double z,List<Hep3Vector> vertices){ + //todo: check if all vertices are on same z + double epsilon = Math.PI/360.; + double s = PathToZPlane(helix,z); + if(s<0){return false;} + if(s>_maxpath){return false;} + Hep3Vector point = PointOnHelix(helix,s); + double angle = insidePolygon(point,vertices); + if(Math.abs(angle-2*Math.PI)<epsilon) + return true; + return false; + } + + + + /** + * Check if the given helix is intercepting XY plan + * note, the z coordinate of the XY plane should be 0, but due to numerical + * approximation, it accept all z value < _epsilon + * @param helix the Helix + * @param normal a unitary normal vector to the plan + * @param orig one point of the plane + * @return true if the helix intersect at least once with the plane + */ + public static boolean isInterceptingXYPlane(HelicalTrackFit helix, Hep3Vector normal,Hep3Vector orig){ + if(normal.z()>_epsilon) + throw new UnsupportedOperationException("Not a XY plan !"+normal); + double x = -helix.x0()+orig.x(); + double y = -helix.y0()+orig.y(); + double z = -helix.z0()+orig.z(); + double xn= normal.x(); + double yn= normal.y(); + double zn= normal.z(); + double Phip = Math.atan2(yn, xn); + double dist = (xn*x+yn*y+zn*z); + double verif = Math.sin(Phip-helix.phi0())+helix.curvature()*dist; + if(normal.magnitude() < 0.99) + throw new UnsupportedOperationException("normal vector not unitary :"+normal.magnitude()); + if(Math.abs(verif)>1) + return false; + else + return true ; + } + + /** + * Check if the given helix is intercepting an XY plane bouded by a + * list of nodes + * @param helix + * @param normal normal vector to the plane + * @param nodes + * @return true if th e helix intercept the bounded plane + **/ + public static boolean isInterceptingBoundedXYPlane(HelicalTrackFit helix,Hep3Vector normal,List<Hep3Vector> nodes){ + Hep3Vector orig = nodes.get(0); + if(!isInterceptingXYPlane(helix,normal,orig)){return false;} + double s = PathToXYPlan(helix, normal, orig); + if(s<0) {return false;} + if(s>_maxpath){return false;} + Hep3Vector point = PointOnHelix(helix, s); + if(Math.abs(insidePolygon(point, nodes)-2*Math.PI)<_epsilon) + return true; + return false; + } + /** + * return one path length (projection on the XY plane) to an XY plane + * the methode is for now only used to check IF an helix is intercepting an + * XY plane + * note, the z coordinate of the XY plane should be 0, but due to numerical + * approximation, it accept all z value < _epsilon + * @param helix + * @param normal a UNITARY vector NORMAL to the plan + * @param origin one point of the plane + * @return path length + */ + public static double PathToXYPlan(HelicalTrackFit helix,Hep3Vector normal,Hep3Vector origin){ + if(normal.z()>_epsilon) + throw new UnsupportedOperationException("Not a XY plan ! normal vector is "+normal); + double x = -helix.x0()+origin.x(); + double y = -helix.y0()+origin.y(); + double z = -helix.z0()+origin.z(); + double xn= normal.x(); + double yn= normal.y(); + double zn= normal.z(); + double phip = Math.atan2(yn, xn); + double dist = (xn*x+yn*y+zn*z); + double sinPlan = Math.sin(phip-helix.phi0())+helix.curvature()*dist; + double Cs = (Math.asin(sinPlan)-phip+helix.phi0()); + double s=dist/(Math.sqrt(xn*xn+yn*yn)*(2./Cs)*Math.sin(Cs/2.)*Math.cos(Cs/2+phip-helix.phi0())+zn*helix.slope()); + return s; + } + + + /** + * adapted from http://local.wasp.uwa.edu.au/~pbourke/geometry/insidepoly/ + * find if a point is inside a given polygon in 3Dspace + * if the point is in the polygone the returned value is 2*PI + * if the point is outside of the polygone the returned value is 0 + * if the point is not of the plan of the polygone the value is between 0 and 2PI + * The polygone HAVE to Be convex for the algorythme to work + * + * This fonction sum all the angle made by q and 2 consecutives point of the polygone + * + * as this method is generic, it might be usefull to make it public,and mabe in another + * file because it can be used for anything else than helix + * + * @param q the point to determine if it is or not in the polygon + * @param p List of edges of the polygone + * @return 2PI if inside, 0 if outside,intermediate values if not on the plane + */ + private static double insidePolygon(Hep3Vector q,List<Hep3Vector> p) + { + int i; + double epsilon = _epsilon; + double m1,m2; + double anglesum=0,costheta; + double x1,x2,y1,y2,z1,z2,m1m2; + //might be usefull to check if all the p vector are on the same plane + Hep3Vector p1; + Hep3Vector p2; + int n = p.size(); + for (i=0;i<n;i++) { + p1 = p.get(i); + p2 = p.get((i+1)%n); // the "last+1" vector is the first + x1 = (p1.x()-q.x()); // not optimal, operation can be made only once + y1 = (p1.y()-q.y()); // + z1 = (p1.z()-q.z()); + x2 = (p2.x()-q.x()); + y2 = (p2.y()-q.y()); + z2 = (p2.z()-q.z()); + m1m2 = Math.sqrt(x1*x1+y1*y1+z1*z1)*Math.sqrt(x2*x2+y2*y2+z2*z2); + + + if (m1m2 <= epsilon){ + return 2*Math.PI; /* We are on a node, consider this inside */ + } + else{ + costheta = (x1*x2 + y1*y2 + z1*z2)/(m1m2); + anglesum += Math.acos(costheta); + } + } + return anglesum; + } + + + private static double RC(CircleFit cfit) { + return 1.0 / cfit.curvature(); + } + + private static double xc(CircleFit cfit) { + //Note that DCA for circle fit has opposite sign w.r.t. the standard L3 definition + return cfit.xref() + (RC(cfit) - (-1*cfit.dca())) * Math.sin(cfit.phi()); + + } + + private static double yc(CircleFit cfit) { + //Note that DCA for circle fit has opposite sign w.r.t. the standard L3 definition + return cfit.yref() - (RC(cfit) - (-1*cfit.dca())) * Math.cos(cfit.phi()); + + } + + private static double x0(CircleFit cfit) { + //Note that DCA for circle fit has opposite sign w.r.t. the standard L3 definition + return cfit.xref() - (-1*cfit.dca()) * Math.sin(cfit.phi()); + } + + private static double y0(CircleFit cfit) { + //Note that DCA for circle fit has opposite sign w.r.t. the standard L3 definition + return cfit.yref() + (-1*cfit.dca()) * Math.cos(cfit.phi()); + } + +}
\ No newline at end of file
Use REPLY-ALL to reply to list
To unsubscribe from the LCD-CVS list, click the following link:
https://listserv.slac.stanford.edu/cgi-bin/wa?SUBED1=LCD-CVS&A=1