hps-java/sandbox

diff -N WTrackUtils.java
--- /dev/null	1 Jan 1970 00:00:00 -0000
+++ WTrackUtils.java	27 Nov 2012 02:33:14 -0000	1.1
@@ -0,0 +1,258 @@

+/*
+ * To change this template, choose Tools | Templates
+ * and open the template in the editor.
+ */
+package org.lcsim.hps.users.phansson;
+
+import hep.physics.vec.BasicHep3Vector;
+import hep.physics.vec.Hep3Vector;
+import hep.physics.vec.VecOp;
+import java.util.ArrayList;
+import java.util.List;
+
+/**
+ *
+ * @author phansson
+ */
+public class WTrackUtils {
+    private int _debug = 0;
+    List<Double> _delta_point =  new ArrayList<Double>();
+    
+    public WTrackUtils() {
+    }
+    
+    public WTrackUtils(boolean debug) {
+        _debug = debug?1:0;
+    }
+
+    public WTrackUtils(int debug) {
+        _debug = debug;
+    }
+
+    
+    public void setDebug(boolean debug) {
+        _debug = debug?1:0;
+    }
+     
+    public boolean getDebug() {
+        return _debug>0?true:false;
+    }
+    
+    public List<Double> getDeltas() {
+        return _delta_point;
+    }
+    
+    private static Hep3Vector getMomentumOnHelix(WTrack track, double s) {
+        double a = track.getBfieldConstant();
+        Hep3Vector p0 = track.getP0();
+        double rho = a / p0.magnitude();
+        double px = p0.x()*Math.cos(rho*s) - p0.y()*Math.sin(rho*s);
+        double py = p0.y()*Math.cos(rho*s) + p0.x()*Math.sin(rho*s);
+        double pz = p0.z(); 
+        return (new BasicHep3Vector(px,py,pz));
+    }
+    
+    private static Hep3Vector getPointOnHelix(WTrack track, double s) {
+        double a = track.getBfieldConstant();
+        Hep3Vector p0 = track.getP0();
+        Hep3Vector x0 = track.getX0();
+        double rho = a / p0.magnitude();
+        double x = x0.x() + p0.x()/a*Math.sin(rho*s) - p0.y()/a*(1-Math.cos(rho*s));
+        double y = x0.y() + p0.y()/a*Math.sin(rho*s) + p0.x()/a*(1-Math.cos(rho*s));
+        double z = x0.z() + p0.z()/p0.magnitude()*s;
+        return (new BasicHep3Vector(x,y,z));
+    }
+    
+    private static double getPathLengthToPlaneApprox(WTrack track, Hep3Vector xp, Hep3Vector eta, Hep3Vector h) {
+        /*
+         * Find the approximate path length to the point xp 
+         * in arbitrary oriented, constant magnetic field with unit vector h
+         */
+        boolean debug = false;
+        double a = track.getBfieldConstant();
+        Hep3Vector p0 = track.getP0();
+        Hep3Vector x0 = track.getX0();
+        double p = p0.magnitude();
+        double rho = a / p;
+        double A = VecOp.dot(eta,VecOp.cross(p0, h))/p*0.5*rho;
+        double B = VecOp.dot(p0,eta)/p;
+        double C = VecOp.dot(VecOp.sub(x0,xp),eta);
+        double t = B*B-4*A*C;
+        if(t<0) {
+            System.out.println(" getPathLengthToPlaneApprox ERROR t is negative (" + t + ")!" );
+            System.out.println(" p " + p + " rho " + rho + " A " + A + " B " + B + " C " + C);
+            System.out.println(" track params\n" + track.toString());
+            System.out.println(" xp " + xp.toString());
+            System.out.println(" eta " + eta.toString());
+            System.out.println(" h " + h.toString());
+            System.exit(1);
+        }
+        double root1 = (-B + Math.sqrt(t)) /(2*A);
+        double root2 = (-B - Math.sqrt(t)) /(2*A);
+
+        // choose the smallest positive solution
+        // if both negative choose the smallest negative ???
+        //if(root1==0 || root2==0) root=0;
+        double root = Math.abs(root1) <= Math.abs(root2) ? root1 : root2;
+//        else if(Math.signum(root1)>0 && Math.signum(root2)<0) root = root1;
+//        else if(Math.signum(root2)>0 && Math.signum(root1)<0) root = root2;
+//        else if(Math.signum(root1)>0 && Math.signum(root2)>0) root =  root1 > root2 ? root2 : root1;
+//        else if(Math.signum(root1)<0 && Math.signum(root2)<0) root =  root1 < root2 ? root2 : root1;
+//        else {
+//            System.out.println(" I should never get here! (root1=" + root1 + " root2=" + root2+")");
+//            System.exit(1);
+//        }
+        if((debug)) {
+                System.out.println(" getPathLengthToPlaneApprox ");
+                System.out.println(" " + track.toString());
+                System.out.println(" xp " + xp.toString());
+                System.out.println(" eta " + eta.toString());
+                System.out.println(" h " + h.toString());
+                System.out.println(" p " + p + " rho " + rho + " t " + t + " A " + A + " B " + B + " C " + C);
+                System.out.println(" root1 " + root1 + " root2 " + root2 + " -> root " + root);
+        }
+        return root;
+    
+    }
+    
+    
+    private static Hep3Vector getPointOnHelix(WTrack track, double s, Hep3Vector h) {
+        /*
+         * Get point on helix at path lenght s 
+         * in arbitrary oriented, constant magnetic field with unit vector h
+         */
+        
+        double a = track.getBfieldConstant();
+        Hep3Vector p0 = track.getP0();
+        double p = p0.magnitude();
+        Hep3Vector x0 = track.getX0();
+        double rho = a / p0.magnitude();
+        double srho = s*rho;
+        Hep3Vector a1 = VecOp.mult(1/a*Math.sin(srho), p0);
+        Hep3Vector a2 = VecOp.mult(1/a*(1-Math.cos(srho)),VecOp.cross(p0,h));
+        Hep3Vector a3 = VecOp.mult(VecOp.dot(p0, h)/p,h);
+        Hep3Vector a4 = VecOp.mult(s-Math.sin(srho)/rho, a3);
+        Hep3Vector x = VecOp.add(x0,a1);
+        x = VecOp.sub(x,a2);
+        x = VecOp.add(x,a4);
+        return x;
+    }
+    
+    private static Hep3Vector getMomentumOnHelix(WTrack track, double s, Hep3Vector h) {
+        /*
+         * Get point on helix at path lenght s 
+         * in arbitrary oriented, constant magnetic field with unit vector h
+         */
+        
+        double a = track.getBfieldConstant();
+        Hep3Vector p0 = track.getP0();
+        double rho = a / p0.magnitude();
+        double srho = s*rho;
+        Hep3Vector a1 = VecOp.mult(Math.cos(srho), p0);
+        Hep3Vector a2 = VecOp.cross(p0, VecOp.mult(Math.sin(srho),h));
+        Hep3Vector a3 = VecOp.mult(VecOp.dot(p0,h),VecOp.mult(1-Math.cos(srho),h));
+        Hep3Vector p  = VecOp.sub(a1,a2);
+        p = VecOp.add(p,a3);
+        return p;
+    }
+    
+    
+    private static double[] getHelixParametersAtPathLength(WTrack track, double s, Hep3Vector h) {
+        /*
+         * Calculate the exact position of the new helix parameters at path length s
+         * in an arbitrarily oriented, constant magnetic field
+         * 
+         * point xp is the point 
+         * h is a unit vector in the direction of the magnetic field
+         */
+        boolean debug = false;
+        // Find track parameters at that path length
+        Hep3Vector p = getMomentumOnHelix(track, s, h);
+        Hep3Vector x = getPointOnHelix(track, s, h);
+        
+        Hep3Vector p_tmp = getMomentumOnHelix(track, s);
+        Hep3Vector x_tmp = getPointOnHelix(track, s);
+        
+        if((debug)) {
+            System.out.println(" point on helix at s");
+            System.out.println(" p  " + p.toString() + "   p_tmp " + p_tmp.toString());
+            System.out.println(" x  " + x.toString() + "   x_tmp " + x_tmp.toString());
+        }
+        
+        
+        //Create the new parameter array
+        double [] pars = new double[7];
+        pars[0] = p.x();
+        pars[1] = p.y();
+        pars[2] = p.z();
+        pars[3] = track.getParameters()[3]; //E is unchanged
+        pars[4] = x.x();
+        pars[5] = x.y();
+        pars[6] = x.z();
+        return pars;
+    }
+    
+    
+    
+    public static Hep3Vector getHelixAndPlaneIntercept(WTrack track, Hep3Vector xp, Hep3Vector eta, Hep3Vector h) {
+    
+        /*
+         * Find the interception point between the helix and plane
+         * track: the helix in W representation
+         * xp: point on the plane
+         * eta: unit vector of the plane
+         * h: unit vector of magnetic field
+         */
+        boolean debug = false;
+        
+        int max_it = 10;
+        double epsilon = 1e-4;
+        int iteration = 1;
+        double delta_s = 9999;
+        double s_prev = 999999;
+        List<WTrack> tracks = new ArrayList<WTrack>();
+        WTrack trk = new WTrack(track);
+        while(iteration<=max_it && Math.abs(s_prev)>epsilon) {
+            if((debug)) {
+                System.out.println("iteration " + iteration + " --- ");
+                System.out.println(" s_prev " + s_prev + " delta_s " + delta_s);
+                System.out.println(trk.toString());
+            }
+            // Start by approximating the path length to the point
+            double s = getPathLengthToPlaneApprox(trk, xp, eta, h);
+            
+            if((debug)) {
+                System.out.println(": Approx. path s " + s);
+            }
+            
+            // Find the track parameters at this point
+            double[] params = getHelixParametersAtPathLength(trk, s, h);
+            // update the track parameters           
+            trk.setTrackParameters(params);
+            
+            if((debug)) {
+                System.out.println(" updated params " + trk.toString());
+            }
+            
+            tracks.add(trk);
+            iteration++;
+            delta_s = Math.abs(s-s_prev);
+            s_prev = s;
+            
+            //Save distance between point and estimate
+            Hep3Vector dpoint = VecOp.sub(xp, trk.getX0());
+            
+            
+        }
+        
+        if((debug)) {
+            System.out.println(" Final s " + s_prev + " delta_s " + delta_s);
+            System.out.println(" Final track params " + trk.toString());
+        }
+        
+        Hep3Vector point = trk.getX0();
+        return point;
+    
+    }
+    
+}