diff -N AlignmentUtils.java
--- /dev/null	1 Jan 1970 00:00:00 -0000
+++ AlignmentUtils.java	25 Aug 2012 01:29:18 -0000	1.1
@@ -0,0 +1,277 @@

+/*
+ * To change this template, choose Tools | Templates
+ * and open the template in the editor.
+ */
+package org.lcsim.hps.users.phansson;
+
+import hep.physics.matrix.BasicMatrix;
+import org.lcsim.fit.helicaltrack.HelicalTrackStrip;
+
+/**
+ *
+ * @author phansson
+ */
+public class AlignmentUtils {
+    
+
+    public AlignmentUtils() {
+        
+    }
+
+    public BasicMatrix calculateLocalHelixDerivatives(HelicalTrackStrip strip) {
+        
+        
+        
+        BasicMatrix dfdq = new BasicMatrix(3,5); //3-dim,ntrackparams
+        
+        
+        
+        
+        return dfdq;
+        
+    }
+    
+    
+    
+    
+    //d0,z0,slope,phi0,R
+    
+    
+    //-----------------------------------
+    // Derivatives of x 
+    
+    
+    public double dx_dd0(double x, double xr,double d0, double phi0, double R) {
+        return -Math.sin(phi0) + sign(R)*ddeltayc_dd0(x, xr, d0,phi0,R);    
+    }
+    public double dx_dz0() {
+        return 0.0;
+    }
+    public double dx_dslope() {
+        return 0.0;
+    }
+    public double dx_dphi0(double x, double xr, double yr, double d0, double phi0, double R) {
+        return (-d0+R)*Math.cos(phi0) + sign(R)*ddeltayc_dphi0(x,xr,yr,d0,phi0,R); 
+    
+    }
+    public double dx_dR(double x, double xr, double yr, double d0, double phi0, double R) {
+        return Math.sin(phi0) +  deltayc(x,xr,yr,d0,phi0,R)*sign(R) + sign(R)*ddeltayc_dR(x,xr,d0,phi0,R);
+    }
+    
+    //-----------------------------------
+    
+    //-----------------------------------
+    // Derivatives of y 
+    
+    public double dy_dz0() {
+        return 0.0;
+    }
+    public double dy_dslope() {
+        return 0.0;
+    }
+    public double dy_dd0(double x, double xr,double d0, double phi0, double R) {
+        //double num = sign(R)*Math.sin(phi0)*( x - xr + (d0-R)*Math.sin(phi0)  );
+        //double den =  Math.sqrt( R*R - Math.pow(x-xr+(d0-R)*Math.sin(phi0),2)   );   
+        //simplify using xc
+        double num = sign(R)*Math.sin(phi0)*( x - xc(xr,d0,phi0,R) );
+        double den =  Math.sqrt( R*R - Math.pow(x-xc(xr,d0,phi0,R),2)   );   
+        return Math.cos(phi0) - num/den;
+    }
+    public double dy_dphi0(double x, double xr,double d0, double phi0, double R) {
+        //simplify using xc
+        double num = Math.cos(phi0)*sign(R)*( x - xc(xr,d0,phi0,R) );
+        double den = Math.sqrt( R*R - Math.pow( x - xc(xr,d0,phi0,R) ,2)  );
+        return (-d0+R)*( Math.sin(phi0) + num/den  );
+    }
+    public double dy_dR(double x, double xr,double d0, double phi0, double R) {
+        //Simplify with xc
+        double num = sign(R) * ( d0 + R + (-d0+R)*Math.cos(2*phi0) + 2*(x-xr)*Math.sin(phi0) ); 
+        double den = 2*Math.sqrt( R*R - Math.pow( x - xc(xr,d0,phi0,R), 2) );
+        double C = Math.sqrt( R*R + Math.pow( x - xc(xr,d0,phi0,R), 2) )*sign(R);
+        return -Math.cos(phi0) + num/den + C;
+    }
+    
+    
+     //-----------------------------------
+    // Derivatives of z 
+    
+    
+    public double dz_dd0(double x, double xr, double yr, double d0, double phi0, double slope, double R) {                
+        return -R*slope*dtmpdphi_dd0(x,  xr,  yr,  d0,  phi0,  R);
+    }
+        
+    public double dz_dz0() {
+        return 1.0;
+    }
+
+    public double dz_dslope(double x, double xr, double yr, double d0, double phi0, double slope, double R) {        
+        double dphi = get_dphi(x,xr,yr,d0,phi0,R);
+        return -R*dphi;
+    }
+
+    public double dz_ddphi(double x, double xr, double yr, double d0, double phi0, double slope, double R) {                
+        return -R*slope*dtmpdphi_ddphi(x,  xr,  yr,  d0,  phi0,  R);
+    }
+    
+    public double dz_dR(double x, double xr, double yr, double d0, double phi0, double slope, double R) {
+        double dphi = get_dphi(x,xr,yr,d0,phi0,R);
+        double derdphi_dR = dtmpdphi_dR(x,xr,yr,d0,phi0,R);
+        return -slope*( dphi + R*derdphi_dR); 
+    }
+
+    
+    
+    
+    
+    //-------------------------------------------
+    //Helper functions
+
+    
+
+    
+    public double get_dphi(double x, double xr, double yr, double d0, double phi0, double R) {
+        //Takes into account that phi=[-pi,pi] by checking where the points (x0,y0) and (x,y) are        
+        double dphi = tmpdphi(x,xr,yr,d0,phi0,R);        
+        dphi = dphi > Math.PI ? dphi - 2*Math.PI :  dphi < -Math.PI ? dphi + 2*Math.PI : dphi;
+        return dphi;
+    }
+
+    
+
+    
+    
+    //Generic point y on circle/helix
+    public double y(double x, double xr, double yr, double d0, double phi0, double R) {
+        return yc(yr,d0,phi0,R) + sign(R)*deltaxc(x,xr,d0,phi0,R);
+    }
+    
+    //x-coordiante of center of circle/helix
+    double xc(double xr,double d0, double phi0, double R) {
+        return xr + (R-d0)*Math.sin(phi0);
+    }
+
+    //y-coordiante of center of circle/helix
+    double yc(double yr,double d0, double phi0, double R) {
+        return yr - (R-d0)*Math.cos(phi0);
+    }
+    
+    // x-coordinate of point of closest approach
+    double x0(double xr, double d0, double phi0) {
+        return xr - d0*Math.sin(phi0);
+    }
+    
+    // y-coordinate of point of closest approach
+    double y0(double yr, double d0, double phi0) {
+        return yr + d0*Math.cos(phi0);
+    }
+    
+    
+    // distance between generic point y and center of circle/helix => y-yc 
+    // FIX THE NAME SO THAT THIS IS deltayc ->MAKES MORE SENSE
+    public double deltaxc(double x, double xr, double d0,double phi0, double R) {
+        return Math.sqrt( R*R - Math.pow(x - xc(xr,d0,phi0,R),2) );
+    }
+ 
+    // distance between generic point x and center of circle/helix => x-xc
+    // FIX THE NAME SO THAT THIS IS deltaxc ->MAKES MORE SENSE
+    public double deltayc(double x, double xr, double yr, double d0,double phi0, double R) {
+        return Math.sqrt( R*R - Math.pow(y(x,xr,yr,d0,phi0,R) - yc(yr,d0,phi0,R),2) );
+    }
+
+    // derivate of deltayc
+    public double ddeltayc_dd0(double x, double xr, double d0,double phi0, double R) {
+        //double num = sign(R)*sign(R) * Math.sin(phi0) * (x-xr-(-d0+R)*Math.sin(phi0));
+        //double den = Math.sqrt( R*R - sign(R)*sign(R) * (R*R - Math.pow( x-xr-(-d0+R)*Math.sin(phi0), 2)));
+        
+        //Use xc to simplify
+        double num = sign(R)*sign(R) * Math.sin(phi0) * (x-xc(xr,d0,phi0,R));
+        double den = Math.sqrt( R*R - sign(R)*sign(R) * (R*R - Math.pow( x-xc(xr,d0,phi0,R), 2)));
+        
+        return num/den;
+    }
+    
+    // derivate of deltayc
+    public double ddeltayc_dphi0(double x, double xr, double yr, double d0, double phi0, double R) {
+        //double num = (-d0+R)*Math.cos(phi0)*sign(R)*sign(R)*(x-xr-(-d0+R)*Math.sin(phi0));
+        //double den = Math.sqrt( R*R - sign(R)*sign(R)*( R*R - Math.pow(x-xr-(-d0+R)*Math.sin(phi0), 2)  )     );
+        //return -1.0*num/den;
+        
+        //Use yc and xc to simplify
+        double num = ( yc(yr,d0,phi0,R)-yr)*sign(R)*sign(R)*(x-xc(xr,d0,phi0,R));
+        double den = Math.sqrt( R*R - sign(R)*sign(R)*( R*R - Math.pow(x-xc(xr,d0,phi0,R), 2)  )   );
+        return num/den;
+        
+    }
+    
+    // derivate of deltayc
+    public double ddeltayc_dR(double x, double xr, double d0, double phi0, double R) {
+        //simplifying using xc
+        double num = ( 2*R - sign(R)*sign(R)*( 2*R + 2*Math.sin(phi0)*( x - xc(xr,d0,phi0,R)) )  -  2*sign(R)*( R*R - Math.pow(x-xc(xr,d0,phi0,R), 2))*sign(R)  );
+        double den = 2*Math.sqrt( R*R - sign(R)*sign(R)*( R*R - Math.pow(x-xc(xr,d0,phi0,R), 2) )   );        
+        return num/den;
+    }
+    
+    // Azimuthal angle for PCA - used in dphi calculation
+    public double phi1(double xr, double yr, double d0, double phi0, double R) {
+        return Math.atan2( x0(xr,d0,phi0) - xc(xr,d0,phi0,R)  , y0(yr,d0,phi0) - yc(yr,d0,phi0,R) );
+    }
+
+    // Azimuthal angle for generic point - used in dphi calculation
+    public double phi2(double x, double xr, double yr, double d0, double phi0, double R) {
+        return Math.atan2( x - xc(xr,d0,phi0,R)  , y(x, xr, yr, d0, phi0, R) - yc(yr,d0,phi0,R) );
+    }
+
+    
+    // delta phi between point on helix (x,y) and point of closest approach (x0,y0)
+    public double tmpdphi(double x, double xr, double yr, double d0, double phi0, double R) {
+        return phi1(xr,yr,d0,phi0,R)- phi2(x,xr,yr,d0,phi0,R);
+    }
+    
+    // derivative of delta phi between point on helix (x,y) and point of closest approach (x0,y0)
+    public double dtmpdphi_dR(double x, double xr, double yr, double d0, double phi0, double R) {
+        // this calculation has been large "simplified" from the original by using x, x0, xc, y, y0, yc
+        double term1 = ((yc(yr,d0,phi0,R) - y0(yr,d0,phi0))*Math.sin(phi0) - (x0(xr,d0,phi0) - xc(xr,d0,phi0,R))*Math.cos(phi0) ) / (R*R);
+        
+        double num = Math.sin(phi0)*Math.pow(y(x, xr, yr, d0, phi0, R) - y0(yr,d0,phi0),2) + ( x - xc(xr,d0,phi0,R))*( R + Math.sin(phi0)*(x-xc(xr,d0,phi0,R)) + Math.pow(y(x, xr, yr, d0, phi0, R)-y0(yr,d0,phi0),2) );
+        
+        double den = (y(x, xr, yr, d0, phi0, R) - yc(yr,d0,phi0,R))*Math.pow(x-xc(xr,d0,phi0,R),2) + sign(R)*sign(R)*Math.pow(y(x, xr, yr, d0, phi0, R) - yc(yr,d0,phi0,R),3);
+        
+        double term2 = sign(R) * num/den;
+        
+        return term1 + term2;
+    }
+    
+    
+    // derivative of delta phi between point on helix (x,y) and point of closest approach (x0,y0)
+    public double dtmpdphi_dd0(double x, double xr, double yr, double d0, double phi0, double R) {
+    
+        double num = sign(R)*Math.sin(phi0)*Math.pow(x - xc(xr,d0,phi0,R),2) + sign(R)*Math.sin(phi0)*Math.pow(y(x, xr, yr, d0, phi0, R) - yc(yr,d0,phi0,R),2);
+        
+        double den = Math.pow(x-xc(xr,d0,phi0,R),2) + sign(R)*sign(R)*Math.pow( y(x, xr, yr, d0, phi0, R) - yc(yr,d0,phi0,R),2);
+        
+        return -1/(y(x, xr, yr, d0, phi0, R) - yc(yr,d0,phi0,R))*num/den; 
+        
+    }
+
+    
+    // derivative of delta phi between point on helix (x,y) and point of closest approach (x0,y0)
+    public double dtmpdphi_ddphi(double x, double xr, double yr, double d0, double phi0, double R) {
+        
+        double term1 = -(Math.pow(yc(yr,d0,phi0,R)-y0(yr,d0,phi0),2)+Math.pow(x0(xr,d0,phi0)-xc(xr,d0,phi0,R),2))/(R*R);
+        
+        double term2 = sign(R)*(yr-yc(yr,d0,phi0,R))/(y(x, xr, yr, d0, phi0, R) - yc(yr,d0,phi0,R))*(R*R)/( Math.pow(x-xc(xr,d0,phi0,R),2) + sign(R)*sign(R)*Math.pow(y(x, xr, yr, d0, phi0, R) - yc(yr,d0,phi0,R),2) ); 
+        
+        return term1 + term2;
+        
+    }    
+    
+    double sign(double val) {
+        return Math.signum(val);
+    }
+    
+    //-------------------------------------------
+    
+
+
+
+}