hps-java/src/main/java/org/lcsim/hps/users/phansson
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);
+ }
+
+ //-------------------------------------------
+
+
+
+
+}