1 /*
2 * HelixUtils.java
3 *
4 * Created on February 1, 2008, 5:48 PM
5 *
6 */
7
8 package org.lcsim.fit.helicaltrack;
9
10 import hep.physics.matrix.BasicMatrix;
11 import hep.physics.matrix.Matrix;
12 import hep.physics.matrix.SymmetricMatrix;
13 import hep.physics.vec.BasicHep3Vector;
14 import hep.physics.vec.Hep3Vector;
15 import java.util.ArrayList;
16 import java.util.List;
17 import org.lcsim.fit.circle.CircleFit;
18
19 /**
20 * Assorted helix utilities that operate on HelicalTrackFits.
21 *
22 * Since these utilities are typically called many times for a
23 * given HelicalTrackFit (or in some cases for a given CircleFit),
24 * a number of derived quantities used by the methods in this class
25 * are cached to minimize re-computation. Since a HelicalTrackFit
26 * (or CircleFit) is immutable, these quantities only need to be
27 * calculated once.
28 * @author Richard Partridge
29 * @version 1.0
30 */
31 public class HelixUtils {
32 private static double _minslope = 1.e-6;
33 private static double _maxpath = 2400; // todo, add a more "dynamic" maxpath
34 private static double _epsilon = 0.01; // value below which a z vector Or an angle is considered as null
35 private static double _tol = 1.e-4; // Maximum aspect ratio of hit covariance terms for hit to measure x-y coord
36
37 /**
38 * Creates a new instance of HelixUtils.
39 */
40 public HelixUtils() {
41 }
42
43 /**
44 * Return the x-y path length between two HelicalTrackHits.
45 * @param cfit CircleFit to be used in calculating the path length
46 * @param hit1 first hit
47 * @param hit2 second hit
48 * @return path length from hit1 to hit2
49 */
50 public static double PathLength(CircleFit cfit, HelicalTrackHit hit1, HelicalTrackHit hit2) {
51 // Find the position on the circle for the hits
52 Hep3Vector pos1 = getPositionOnCircle(cfit, hit1);
53 Hep3Vector pos2 = getPositionOnCircle(cfit, hit2);
54 // Return the path length between hit1 and hit2
55 return PathCalc(xc(cfit), yc(cfit), RC(cfit), pos1.x(), pos1.y(), pos2.x(), pos2.y());
56 }
57
58 /**
59 * Return the x-y path length from the DCA.
60 * @param cfit CircleFit to be used in calculating the path length
61 * @param hit hit to be used for the path length calculation
62 * @return path length from the DCA to the hit
63 */
64 public static double PathLength(CircleFit cfit, HelicalTrackHit hit) {
65 // Find the position on the circle for the hit
66 Hep3Vector pos = getPositionOnCircle(cfit, hit);
67 // Return the path length from the DCA
68 return PathCalc(xc(cfit), yc(cfit), RC(cfit), x0(cfit), y0(cfit), pos.x(), pos.y());
69 }
70
71 /**
72 * Return the x-y path length from the DCA to a HelicalTrackHit.
73 * @param helix HelicalTrackFit to be used in calculating the path length
74 * @param hit hit to be used for the path length calculation
75 * @return path length from the DCA to the hit
76 */
77 public static double PathLength(HelicalTrackFit helix, HelicalTrackHit hit) {
78
79 // Calculate the shortest path length from the DCA to the hit x-y coordinates
80 double path0 = PathCalc(helix.xc(), helix.yc(), helix.R(), helix.x0(), helix.y0(), hit.x(), hit.y());
81 if (hit instanceof HelicalTrack2DHit) return path0;
82
83 // Find the closest 3D hit in the z coordinate (if there is one)
84 HelicalTrackHit close = null;
85 double zhit = hit.z();
86 for (HelicalTrackHit fithit : helix.PathMap().keySet()) {
87 if (!(fithit instanceof HelicalTrack2DHit)) {
88
89 if (close == null) {
90 // If this is the first 3D hit, mark it as being closest
91 close = fithit;
92 } else {
93 // Check if the fithit is closer in z
94 if (Math.abs(zhit - fithit.z()) < Math.abs(zhit - close.z())) close = fithit;
95 }
96 }
97 }
98
99 // If we didn't find another 3D hit, return the path from the DCA
100 if (close == null) return path0;
101
102 // Find the path length for the closest hit assuming it hasn't looped
103 double path = helix.PathMap().get(close) +
104 PathCalc(helix.xc(), helix.yc(), helix.R(), close.x(), close.y(), hit.x(), hit.y());
105 return path;
106 }
107
108 /**
109 * Return the x-y path length to a z-plane.
110 * @param helix HelicalTrackFit to be used in calculating the path length
111 * @param z location of z-plane
112 * @return path length from the DCA to the z-plane
113 */
114 public static double PathToZPlane(HelicalTrackFit helix, double z) {
115 // Find the z distace between the DCA and the z plane and calculate the x-y path length
116 double zdist = z - helix.z0();
117 double safeslope = helix.slope();
118 if (Math.abs(safeslope) < _minslope) safeslope = _minslope * Math.signum(safeslope);
119 return zdist / safeslope;
120 }
121
122 /**
123 * Return the x-y path length to an x-plane.
124 * @param helix HelicalTrackFit to be used in calculating the path length
125 * @param x location of x-plane
126 * @return path length from the DCA to the x-plane
127 */
128 public static List<Double> PathToXPlane(HelicalTrackFit helix, double x,double smax, int mxint) {
129 // Create a list to hold the path lengths
130 List<Double> pathlist = new ArrayList<Double>();
131 // Retrieve helix dca and RC
132 double x0 = helix.x0();
133 double y0 = helix.y0();
134
135 double xc=helix.xc();
136 double yc=helix.yc();
137 double RC = helix.R();
138 double y=yc+Math.signum(RC)*Math.sqrt(RC*RC-Math.pow(x-xc,2));
139
140 double s=PathCalc(xc,yc,RC,x0,y0,x,y);
141
142 // System.out.println("PathToXPlane : s = "+s+"; sFromClass = "+sFromClass);
143 /*
144 while (s < smax && pathlist.size() < mxint) {
145 // Add this odd-numbered crossing to the list
146 pathlist.add(s);
147 // Advance to the next even-numbered crossing
148 s += 2. * (Math.PI - dphi) * Math.abs(RC);
149 // Check to see if we should add it
150 if (s < smax && pathlist.size() < mxint) pathlist.add(s);
151 // Add this even-numbered crossing to the list
152 s += 2. * dphi * Math.abs(RC);
153 }*/
154 pathlist.add(s);
155
156 return pathlist;
157 }
158
159
160 /**
161 * Return a list of x-y path lengths to a cylinder centered on the z axis.
162 * @param helix HelicalTrackFit to be used in calculating the path length
163 * @param r desired radius
164 * @param smax maximum path length to be considered
165 * @param mxint Maximum number of intersections
166 * @return list of path lengths
167 */
168 public static List<Double> PathToCylinder(HelicalTrackFit helix, double r, double smax, int mxint) {
169 // Create a list to hold the path lengths
170 List<Double> pathlist = new ArrayList<Double>();
171 // Retrieve helix dca and RC
172 double dca = helix.dca();
173 double RC = helix.R();
174 // Find cos(dphi) for dphi from the point of closest approach to the first crossing
175 double cdphi = 1 - (r * r - dca * dca) / (2. * RC * (RC - dca));
176 // Make sure we have a physical intersection
177 if (Math.abs(cdphi) < 1.) {
178 // Calculate dphi and the path length to the first crossing
179 double dphi = Math.acos(cdphi);
180 Double s = dphi * Math.abs(RC);
181 // Loop over crossings until we exceed one of the limits
182 while (s < smax && pathlist.size() < mxint) {
183 // Add this odd-numbered crossing to the list
184 pathlist.add(s);
185 // Advance to the next even-numbered crossing
186 s += 2. * (Math.PI - dphi) * Math.abs(RC);
187 // Check to see if we should add it
188 if (s < smax && pathlist.size() < mxint) pathlist.add(s);
189 // Add this even-numbered crossing to the list
190 s += 2. * dphi * Math.abs(RC);
191 }
192 }
193 return pathlist;
194 }
195
196 /**
197 * Return a unit vector giving the track direction at a given point on
198 * the helix.
199 * @param helix HelicalTrackFit to be used in calculating direction
200 * @param s path length to the desired point on helix
201 * @return direction unit vector
202 */
203 public static Hep3Vector Direction(HelicalTrackFit helix, double s) {
204 // Calculate the azimuthal direction
205 double phi = helix.phi0() - s / helix.R();
206 double sth = helix.sth();
207 // Calculate the components of the direction unit vector
208 double ux = Math.cos(phi) * helix.sth();
209 double uy = Math.sin(phi) * helix.sth();
210 double uz = helix.cth();
211 // Return the direction unit vector
212 return new BasicHep3Vector(ux, uy, uz);
213 }
214
215 /**
216 * Return the TrackDirection object for a given point on a helix.
217 * This might seem like an odd place to put this code, but putting
218 * it here allows the cached helix quantities to be used in constructin
219 * the TrackDirection objects.
220 * @param helix HelicalTrackFit to use in constructing the TrackDirection
221 * @param s path length specifying location on helix
222 * @return TrackDirection object for this point on the helix
223 */
224 public static TrackDirection CalculateTrackDirection(HelicalTrackFit helix, double s) {
225 Hep3Vector dir = Direction(helix, s);
226 Matrix deriv = DirectionDerivates(helix, dir, s);
227 return new TrackDirection(dir, deriv);
228 }
229
230 /**
231 * Return the location in space for a particular point on a helix.
232 * @param helix HelicalTrackFit to be used
233 * @param s path length
234 * @return point in space corresponding to the given path length
235 */
236 public static Hep3Vector PointOnHelix(HelicalTrackFit helix, double s) {
237 // Find the azimuthal direction at this path length
238 double RC = helix.R();
239 double phi = helix.phi0() - s / RC;
240 // Calculate the position on the helix at this path length
241 double x = helix.xc() - RC * Math.sin(phi);
242 double y = helix.yc() + RC * Math.cos(phi);
243 double z = helix.z0() + s * helix.slope();
244 // Return the position as a Hep3Vector
245 return new BasicHep3Vector(x, y, z);
246 }
247
248 private static Hep3Vector getPositionOnCircle(CircleFit cfit, HelicalTrackHit hit) {
249
250 // Get hit position and errors
251 Hep3Vector pos = hit.getCorrectedPosition();
252 SymmetricMatrix cov = hit.getCorrectedCovMatrix();
253 double dxdx = cov.diagonal(0);
254 double dydy = cov.diagonal(1);
255 if ((dxdx < _tol * dydy) && (dydy < _tol * dxdx)) return pos;
256
257 // Get circle parameters
258 double x0 = xc(cfit);
259 double y0 = yc(cfit);
260 double R = Math.abs(RC(cfit));
261
262 // Get hit coordinates
263 double x = pos.x();
264 double y = pos.y();
265
266
267 if (_tol * dxdx > dydy && Math.abs(y-y0) < R) {
268 double xnew1 = Math.sqrt(R*R - (y-y0)*(y-y0)) + x0;
269 double xnew2 = -xnew1 + 2. * x0;
270 if (Math.abs(xnew1 - x) < Math.abs(xnew2 - x)) {
271 x = xnew1;
272 } else {
273 x = xnew2;
274 }
275 }
276 if (_tol * dydy > dxdx && Math.abs(x-x0) < R) {
277 double ynew1 = Math.sqrt(R*R - (x-x0)*(x-x0)) + y0;
278 double ynew2 = -ynew1 + 2. * y0;
279 if (Math.abs(ynew1 - y) < Math.abs(ynew2 - y)) {
280 y = ynew1;
281 } else {
282 y = ynew2;
283 }
284 }
285
286 return new BasicHep3Vector(x, y, pos.z());
287 }
288
289 private static double PathCalc(double xc, double yc, double RC, double x1, double y1, double x2, double y2) {
290 // Find the angle between these points measured wrt the circle center
291 double phi1 = Math.atan2(y1 - yc, x1 - xc);
292 double phi2 = Math.atan2(y2 - yc, x2 - xc);
293 double dphi = phi2 - phi1;
294 // Make sure dphi is in the valid range (-pi, pi)
295 if (dphi > Math.PI) dphi -= 2. * Math.PI;
296 if (dphi < -Math.PI) dphi += 2. * Math.PI;
297 // Return the arc length
298 return -RC * dphi;
299 }
300
301 /**
302 * Return the derivatives of the momentum unit vector with respect to the
303 * helix parameters. The direction derivatives are returned in matrix
304 * form, with the row giving the cartesian component of the momentum
305 * vector and the column giving the helix parameter.
306 * @param helix HelicalTrackFit to be used in calculating derivatives
307 * @param s path length to the desired point on the helix
308 * @return direction derivatives
309 */
310 private static Matrix DirectionDerivates(HelicalTrackFit helix, Hep3Vector u, double s) {
311 // Create the matrix that will hold the derivatives
312 BasicMatrix deriv = new BasicMatrix(3,5);
313 // Retrieve some helix info
314 double cphi0 = Math.cos(helix.phi0());
315 double sphi0 = Math.sin(helix.phi0());
316 double sth = helix.sth();
317 double cth = helix.cth();
318 double dca = helix.dca();
319 double omega = helix.curvature();
320 // Calculate the non-zero derivatives of the direction with respect to the helix parameters
321 deriv.setElement(0, HelicalTrackFit.curvatureIndex, (u.x() - cphi0 * sth) / omega); // du_x / domega
322 deriv.setElement(1, HelicalTrackFit.curvatureIndex, (u.y() - sphi0 * sth) / omega); // du_y / domega
323 deriv.setElement(0, HelicalTrackFit.dcaIndex, -omega * cphi0 * sth); // du_x / ddca
324 deriv.setElement(1, HelicalTrackFit.dcaIndex, -omega * sphi0 * sth); // du_y / ddca
325 deriv.setElement(0, HelicalTrackFit.phi0Index, -(1 - dca * omega) * sphi0 * sth); // du_x / dphi0
326 deriv.setElement(1, HelicalTrackFit.phi0Index, (1 - dca * omega) * cphi0 * sth); // du_y / dphi0
327 deriv.setElement(0, HelicalTrackFit.slopeIndex, -u.x() * sth * cth); // du_x / dslope
328 deriv.setElement(1, HelicalTrackFit.slopeIndex, -u.y() * sth * cth); // du_y / dslope
329 deriv.setElement(2, HelicalTrackFit.slopeIndex, sth*sth*sth); // du_z / dslope
330 // Return the derivative matrix
331 return deriv;
332 }
333 /**
334 * return true if the helix is intercepting the bounded cylinder
335 *
336 * Check if at least one of the first ten intersection of the helix with the cylinder
337 * is between zmin and zmax...there might be a better way to do this
338 * @param helix
339 * @param r rarius of the cylinder
340 * @param zmin lower bound of the cylinder
341 * @param zmax higher bound of the cylinder
342 * @return
343 */
344 public static boolean isInterceptingBoundedCylinder(HelicalTrackFit helix,double r,double zmin,double zmax){
345 double minpath = PathToZPlane(helix, zmin);//not sure it's very efficient to calculate the maximum path
346 double maxpath = PathToZPlane(helix, zmax);
347 double path = Math.max(minpath, maxpath);
348 List<Double> pathlist = PathToCylinder(helix,r,path,10);
349 for(double s:pathlist){
350 double z = PointOnHelix(helix,s).z();
351 if(z<zmax && z> zmin)
352 return true;
353 }
354 return false;
355 }
356 /**
357 * return true if the helix is intercepting the given disk
358 * @param helix
359 * @param rmin
360 * @param rmax
361 * @param z
362 * @return
363 */
364 public static boolean isInterceptingZDisk(HelicalTrackFit helix,double rmin,double rmax, double z ){
365 double s = PathToZPlane(helix,z);
366 Hep3Vector point = PointOnHelix(helix,s);
367 double x = point.x();
368 double y = point.y();
369 double r = Math.sqrt(x*x+y*y);
370 if(r < rmax && r > rmin ){return true;}
371 return false;
372 }
373
374 /**
375 * return true if the point on the helix at coord z is intercepting a given ZPolygon, the
376 * method don't check if the polygone is parallel to a z plane
377 *
378 * this method check if the private methode insidePolygone return a value a less than .5 deg from 2PI
379 *
380 * @param helix
381 * @param z
382 * @param vertices
383 * @return true OR false wether the helix intercept or not the polygone
384 */
385 public static boolean isInterceptingZpolygon(HelicalTrackFit helix,double z,List<Hep3Vector> vertices){
386 //todo: check if all vertices are on same z
387 double epsilon = Math.PI/360.;
388 double s = PathToZPlane(helix,z);
389 if(s<0){return false;}
390 if(s>_maxpath){return false;}
391 Hep3Vector point = PointOnHelix(helix,s);
392 double angle = insidePolygon(point,vertices);
393 if(Math.abs(angle-2*Math.PI)<epsilon)
394 return true;
395 return false;
396 }
397
398
399
400 /**
401 * Check if the given helix is intercepting XY plan
402 * note, the z coordinate of the XY plane should be 0, but due to numerical
403 * approximation, it accept all z value < _epsilon
404 * @param helix the Helix
405 * @param normal a unitary normal vector to the plan
406 * @param orig one point of the plane
407 * @return true if the helix intersect at least once with the plane
408 */
409 public static boolean isInterceptingXYPlane(HelicalTrackFit helix, Hep3Vector normal,Hep3Vector orig){
410 if(normal.z()>_epsilon)
411 throw new UnsupportedOperationException("Not a XY plan !"+normal);
412 double x = -helix.x0()+orig.x();
413 double y = -helix.y0()+orig.y();
414 double z = -helix.z0()+orig.z();
415 double xn= normal.x();
416 double yn= normal.y();
417 double zn= normal.z();
418 double Phip = Math.atan2(yn, xn);
419 double dist = (xn*x+yn*y+zn*z);
420 double verif = Math.sin(Phip-helix.phi0())+helix.curvature()*dist;
421 if(normal.magnitude() < 0.99)
422 throw new UnsupportedOperationException("normal vector not unitary :"+normal.magnitude());
423 if(Math.abs(verif)>1)
424 return false;
425 else
426 return true ;
427 }
428
429 /**
430 * Check if the given helix is intercepting an XY plane bouded by a
431 * list of nodes
432 * @param helix
433 * @param normal normal vector to the plane
434 * @param nodes
435 * @return true if th e helix intercept the bounded plane
436 **/
437 public static boolean isInterceptingBoundedXYPlane(HelicalTrackFit helix,Hep3Vector normal,List<Hep3Vector> nodes){
438 Hep3Vector orig = nodes.get(0);
439 if(!isInterceptingXYPlane(helix,normal,orig)){return false;}
440 double s = PathToXYPlan(helix, normal, orig);
441 if(s<0) {return false;}
442 if(s>_maxpath){return false;}
443 Hep3Vector point = PointOnHelix(helix, s);
444 if(Math.abs(insidePolygon(point, nodes)-2*Math.PI)<_epsilon)
445 return true;
446 return false;
447 }
448 /**
449 * return one path length (projection on the XY plane) to an XY plane
450 * the methode is for now only used to check IF an helix is intercepting an
451 * XY plane
452 * note, the z coordinate of the XY plane should be 0, but due to numerical
453 * approximation, it accept all z value < _epsilon
454 * @param helix
455 * @param normal a UNITARY vector NORMAL to the plan
456 * @param origin one point of the plane
457 * @return path length
458 */
459 public static double PathToXYPlan(HelicalTrackFit helix,Hep3Vector normal,Hep3Vector origin){
460 if(normal.z()>_epsilon)
461 throw new UnsupportedOperationException("Not a XY plan ! normal vector is "+normal);
462 double x = -helix.x0()+origin.x();
463 double y = -helix.y0()+origin.y();
464 double z = -helix.z0()+origin.z();
465 double xn= normal.x();
466 double yn= normal.y();
467 double zn= normal.z();
468 double phip = Math.atan2(yn, xn);
469 double dist = (xn*x+yn*y+zn*z);
470 double sinPlan = Math.sin(phip-helix.phi0())+helix.curvature()*dist;
471 double Cs = (Math.asin(sinPlan)-phip+helix.phi0());
472 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());
473 return s;
474 }
475
476
477 /**
478 * adapted from http://local.wasp.uwa.edu.au/~pbourke/geometry/insidepoly/
479 * find if a point is inside a given polygon in 3Dspace
480 * if the point is in the polygone the returned value is 2*PI
481 * if the point is outside of the polygone the returned value is 0
482 * if the point is not of the plan of the polygone the value is between 0 and 2PI
483 * The polygone HAVE to Be convex for the algorythme to work
484 *
485 * This fonction sum all the angle made by q and 2 consecutives point of the polygone
486 *
487 * as this method is generic, it might be usefull to make it public,and mabe in another
488 * file because it can be used for anything else than helix
489 *
490 * @param q the point to determine if it is or not in the polygon
491 * @param p List of edges of the polygone
492 * @return 2PI if inside, 0 if outside,intermediate values if not on the plane
493 */
494 private static double insidePolygon(Hep3Vector q,List<Hep3Vector> p)
495 {
496 int i;
497 double epsilon = _epsilon;
498 double m1,m2;
499 double anglesum=0,costheta;
500 double x1,x2,y1,y2,z1,z2,m1m2;
501 //might be usefull to check if all the p vector are on the same plane
502 Hep3Vector p1;
503 Hep3Vector p2;
504 int n = p.size();
505 for (i=0;i<n;i++) {
506 p1 = p.get(i);
507 p2 = p.get((i+1)%n); // the "last+1" vector is the first
508 x1 = (p1.x()-q.x()); // not optimal, operation can be made only once
509 y1 = (p1.y()-q.y()); //
510 z1 = (p1.z()-q.z());
511 x2 = (p2.x()-q.x());
512 y2 = (p2.y()-q.y());
513 z2 = (p2.z()-q.z());
514 m1m2 = Math.sqrt(x1*x1+y1*y1+z1*z1)*Math.sqrt(x2*x2+y2*y2+z2*z2);
515
516
517 if (m1m2 <= epsilon){
518 return 2*Math.PI; /* We are on a node, consider this inside */
519 }
520 else{
521 costheta = (x1*x2 + y1*y2 + z1*z2)/(m1m2);
522 anglesum += Math.acos(costheta);
523 }
524 }
525 return anglesum;
526 }
527
528
529 private static double RC(CircleFit cfit) {
530 return 1.0 / cfit.curvature();
531 }
532
533 private static double xc(CircleFit cfit) {
534 //Note that DCA for circle fit has opposite sign w.r.t. the standard L3 definition
535 return cfit.xref() + (RC(cfit) - (-1*cfit.dca())) * Math.sin(cfit.phi());
536
537 }
538
539 private static double yc(CircleFit cfit) {
540 //Note that DCA for circle fit has opposite sign w.r.t. the standard L3 definition
541 return cfit.yref() - (RC(cfit) - (-1*cfit.dca())) * Math.cos(cfit.phi());
542
543 }
544
545 private static double x0(CircleFit cfit) {
546 //Note that DCA for circle fit has opposite sign w.r.t. the standard L3 definition
547 return cfit.xref() - (-1*cfit.dca()) * Math.sin(cfit.phi());
548 }
549
550 private static double y0(CircleFit cfit) {
551 //Note that DCA for circle fit has opposite sign w.r.t. the standard L3 definition
552 return cfit.yref() + (-1*cfit.dca()) * Math.cos(cfit.phi());
553 }
554
555 }