hps-java/src/main/java/org/lcsim/hps/users/phansson
diff -u -r1.9 -r1.10
--- AlignmentUtils.java 24 Oct 2012 06:45:52 -0000 1.9
+++ AlignmentUtils.java 1 Nov 2012 15:42:26 -0000 1.10
@@ -6,9 +6,7 @@
import hep.physics.matrix.BasicMatrix;
import hep.physics.matrix.MatrixOp;
-import hep.physics.vec.BasicHep3Vector;
-import hep.physics.vec.Hep3Vector;
-import hep.physics.vec.VecOp;
+import hep.physics.vec.*;
import org.lcsim.fit.helicaltrack.HelicalTrackFit;
import org.lcsim.fit.helicaltrack.HelixUtils;
@@ -19,18 +17,37 @@
public class AlignmentUtils {
- private boolean _debug;
+ private int _debug;
+
public AlignmentUtils() {
- _debug = false;
+ _debug = 0;
}
public AlignmentUtils(boolean debug) {
- _debug = debug;
+ _debug = debug ? 1 : 0;
}
public void setDebug(boolean debug) {
+ _debug = debug ? 1 : 0;
+ }
+ public void setDebug(int debug) {
_debug = debug;
}
+ public Hep3Matrix getRotationMatrix(String axis, double angle) {
+ Hep3Matrix m;
+ if(axis.equalsIgnoreCase("x")) m = new BasicHep3Matrix(1,0,0,0,Math.cos(angle),Math.sin(angle),0,-Math.sin(angle),Math.cos(angle));
+ else if(axis.equalsIgnoreCase("y")) m = new BasicHep3Matrix(Math.cos(angle),0,-Math.sin(angle),0,1,0,Math.sin(angle),0,Math.cos(angle));
+ else if (axis.equalsIgnoreCase("z")) m = new BasicHep3Matrix(Math.cos(angle),Math.sin(angle),0,-Math.sin(angle),Math.cos(angle),0,0,0,1);
+ else throw new RuntimeException(this.getClass().getSimpleName()+": axis " + axis + " is not valid!!");
+ return m;
+ }
+
+
+ public Hep3Matrix getRotationMatrixAroundX(double angle) {
+ return new BasicHep3Matrix(1,0,0,0,Math.cos(angle),-Math.sin(angle),0,Math.sin(angle),Math.cos(angle));
+ }
+
+
public BasicMatrix calculateLocalHelixDerivatives(HelicalTrackFit trk, double xint) {
@@ -199,21 +216,29 @@
- public double dphi_dx(double xint, double d0, double phi0, double R, double phi) {
- double num = -Math.pow(R, 2)*sign(R);
- double den1 = Math.sqrt( Math.pow(R, 2) - Math.pow(xint+(d0-R)*Math.sin(phi0), 2) );
- double den2 = -Math.pow(xint+(d0-R)*Math.sin(phi0),2);
- double den3 = sign(R)*sign(R)*(-Math.pow(R, 2)+Math.pow(xint, 2)+2*(d0-R)*xint*Math.sin(phi0)+Math.pow(d0-R,2)*Math.pow(Math.sin(phi0),2) );
- return num/(den1*(den2+den3));
- }
-
- public double dphi_dy(double y, double d0, double phi0, double R, double phi) {
- double c0 = Math.cos(phi0);
- double num = Math.pow(R,2)*sign(R)*sign(R);
- double den1 = Math.sqrt(Math.pow(R, 2)-Math.pow(y+(d0-R)*c0, 2));
- double den2 = -Math.pow(y+(d0-R)*c0,2);
- double den3 = (-Math.pow(R,2) + Math.pow(y, 2) + 2*(d0-R)*y*c0 + Math.pow(d0-R, 2)*Math.pow(c0, 2))*sign(R)*sign(R);
- return num/(den1*(den2+den3));
+ public double dphi_dx(double xint, double d0, double phi0, double R) {
+ double num = -1.0;
+ double A_x = -xint + (-d0+R)*Math.sin(phi0);
+ double den = R*Math.sqrt(1-(A_x*A_x)/(R*R));
+ return num/den;
+// double num = -Math.pow(R, 2)*sign(R);
+// double den1 = Math.sqrt( Math.pow(R, 2) - Math.pow(xint+(d0-R)*Math.sin(phi0), 2) );
+// double den2 = -Math.pow(xint+(d0-R)*Math.sin(phi0),2);
+// double den3 = sign(R)*sign(R)*(-Math.pow(R, 2)+Math.pow(xint, 2)+2*(d0-R)*xint*Math.sin(phi0)+Math.pow(d0-R,2)*Math.pow(Math.sin(phi0),2) );
+// return num/(den1*(den2+den3));
+ }
+
+ public double dphi_dy(double y, double d0, double phi0, double R, double phi) {
+ double A_y = y + (-d0+R)*Math.cos(phi0);
+ double num = A_y * Math.signum(R);
+ double den = R*Math.sqrt(R*R-A_y*A_y)*Math.sqrt( 1 - (R*R-A_y*A_y)/(R*R));
+ return num/den;
+// double c0 = Math.cos(phi0);
+// double num = Math.pow(R,2)*sign(R)*sign(R);
+// double den1 = Math.sqrt(Math.pow(R, 2)-Math.pow(y+(d0-R)*c0, 2));
+// double den2 = -Math.pow(y+(d0-R)*c0,2);
+// double den3 = (-Math.pow(R,2) + Math.pow(y, 2) + 2*(d0-R)*y*c0 + Math.pow(d0-R, 2)*Math.pow(c0, 2))*sign(R)*sign(R);
+// return num/(den1*(den2+den3));
}
public double dphi_dz(double slope, double R) {
@@ -233,15 +258,15 @@
}
public double dy_dx(double xint, double d0, double phi0, double R, double phi) {
- return -R*Math.sin(phi)*this.dphi_dx(xint, d0, phi0, R, phi);
+ return -R*Math.sin(phi)*this.dphi_dx(xint, d0, phi0, R);
}
- public double dz_dx(double xint, double xr,double yr,double d0, double phi0, double slope, double R, double phi) {
- return -R*slope*this.dphi_dx(xint, d0, phi0, R, phi);
+ public double dz_dx(double xint, double xr,double yr,double d0, double phi0, double slope, double R) {
+ return -R*slope*this.dphi_dx(xint, d0, phi0, R);
}
public double dx_dy(double y, double d0, double phi0, double R, double phi) {
- return R*Math.cos(phi)*this.dphi_dy(y, d0, phi0, R, phi);
+ return -R*Math.cos(phi)*this.dphi_dy(y, d0, phi0, R, phi);
}
public double dy_dy() {
@@ -254,11 +279,13 @@
public double dx_dz(double slope, double R, double phi) {
- return sign(R)*R*Math.cos(phi)*this.dphi_dz(slope, R);
+ return -R*Math.cos(phi)*this.dphi_dz(slope, R);
+ //return sign(R)*R*Math.cos(phi)*this.dphi_dz(slope, R);
}
public double dy_dz(double slope, double R, double phi) {
- return -sign(R)*R*Math.sin(phi)*this.dphi_dz(slope, R);
+ return -R*Math.sin(phi)*this.dphi_dz(slope, R);
+ //return -sign(R)*R*Math.sin(phi)*this.dphi_dz(slope, R);
}
public double dz_dz() {
@@ -266,42 +293,513 @@
}
- //-----------------------------------
- // Derivatives for rotation
- // alpha - rotation around x (beamline direction)
- // beta - rotation around y (bend direction)
- // gammaa - rotation around z (non-bend direction)
+
- public double dx_dalpha() {
- return 0;
+ public Hep3Matrix rotationDer_da() {
+ /*
+ * Derivative w.r.t. a of a rotation matrix with small Euler angles a,b,c around axis x,y,z
+ * Small angle limit
+ */
+ return new BasicHep3Matrix(0,0,0,0,0,1,0,-1,0);
}
- public double dy_dalpha() {
- return -1;
+ public Hep3Matrix rotationDer_db() {
+ /*
+ * Derivative w.r.t. b of a rotation matrix with Euler angles a,b,c around axis x,y,z
+ * Small angle limit
+ */
+ return new BasicHep3Matrix(0,0,-1,0,0,0,1,0,0);
}
- public double dz_dalpha() {
- return 1;
+ public Hep3Matrix rotationDer_dc() {
+ /*
+ * Derivative w.r.t. c of a rotation matrix with Euler angles a,b,c around axis x,y,z
+ * Small angle limit
+ */
+ return new BasicHep3Matrix(0,1,0,-1,0,0,0,0,0);
}
- public double dx_dbeta() {
- return 1;
+
+
+ public BasicMatrix calculateJacobian(Hep3Vector x_vec, Hep3Matrix T) {
+ /*
+ * Calculate jacobian da/db
+ * Arguments:
+ * Hep3Matrix T is the 3x3 rotation matrix from the global/tracking to local frame
+ * Hep3Vector x_vec is the position of the track on the sensor
+ */
+
+
+ //Derivatives of the rotation matrix deltaR' w.r.t. rotations a,b,c around axis x,y,z
+ Hep3Matrix ddeltaRprime_da = rotationDer_da();
+ Hep3Matrix ddeltaRprime_db = rotationDer_db();
+ Hep3Matrix ddeltaRprime_dc = rotationDer_dc();
+
+ if(_debug>1) {
+ System.out.printf("%s: Rotation matrx from tracking/global to local frame T:\n %s\n",this.getClass().getSimpleName(),T.toString());
+ System.out.printf("%s: Derivatives of the rotation matrix deltaR' w.r.t. rotation a,b,c around x,y,z axis:\n",this.getClass().getSimpleName());
+ System.out.printf("%s: ddeltaRprime_da:\n %s\n",this.getClass().getSimpleName(),ddeltaRprime_da.toString());
+ System.out.printf("%s: ddeltaRprime_db:\n %s\n",this.getClass().getSimpleName(),ddeltaRprime_db.toString());
+ System.out.printf("%s: ddeltaRprime_dc:\n %s\n",this.getClass().getSimpleName(),ddeltaRprime_dc.toString());
+ }
+
+
+
+
+ // Upper left 3x3
+ Hep3Vector deltaX_gl = new BasicHep3Vector(1,0,0);
+ Hep3Vector deltaY_gl = new BasicHep3Vector(0,1,0);
+ Hep3Vector deltaZ_gl = new BasicHep3Vector(0,0,1);
+ Hep3Vector dq_dx = VecOp.mult(T, deltaX_gl);
+ Hep3Vector dq_dy = VecOp.mult(T, deltaY_gl);
+ Hep3Vector dq_dz = VecOp.mult(T, deltaZ_gl);
+
+ if(_debug>1) {
+ System.out.printf("%s: - Upper left 3x3 of Jacobian da/db: dq_dx,dq_dy,dq_dz\n",this.getClass().getSimpleName());
+ System.out.printf("%s: dq_dx: %s\n",this.getClass().getSimpleName(),dq_dx.toString());
+ System.out.printf("%s: dq_dy: %s\n",this.getClass().getSimpleName(),dq_dy.toString());
+ System.out.printf("%s: dq_dz: %s\n",this.getClass().getSimpleName(),dq_dz.toString());
+ }
+
+
+
+ if(_debug>1) {
+ System.out.printf("%s: - Upper right 3x3 of Jacobian da/db: dq_dx,dq_dy,dq_dz\n",this.getClass().getSimpleName());
+ }
+
+
+ // Upper right 3x3
+
+ Hep3Vector x_vec_tmp = VecOp.mult(ddeltaRprime_da, x_vec); // derivative of the position
+ Hep3Vector x_vec_tmp2 = VecOp.sub(x_vec_tmp, x_vec); // subtract position
+ Hep3Vector dq_da = VecOp.mult(T,x_vec_tmp2); //rotated into local frame
+
+ if(_debug>1) {
+ System.out.printf("%s: position %s rotation derivative w.r.t. a ddeltaR'/da %s\n",this.getClass().getSimpleName(),x_vec.toString(),x_vec_tmp.toString());
+ System.out.printf("%s: subtracted %s and rotated to local %s \n",this.getClass().getSimpleName(),x_vec_tmp2.toString(),dq_da.toString());
+ }
+
+
+ x_vec_tmp = VecOp.mult(ddeltaRprime_db, x_vec);
+ x_vec_tmp2 = VecOp.sub(x_vec_tmp, x_vec);
+ Hep3Vector dq_db = VecOp.mult(T,x_vec_tmp2);
+
+ if(_debug>1) {
+ System.out.printf("%s: position %s rotation derivative w.r.t. a ddeltaR'/db %s\n",this.getClass().getSimpleName(),x_vec.toString(),x_vec_tmp.toString());
+ System.out.printf("%s: subtracted %s and rotated to local %s \n",this.getClass().getSimpleName(),x_vec_tmp2.toString(),dq_db.toString());
+ }
+
+ x_vec_tmp = VecOp.mult(ddeltaRprime_dc, x_vec);
+ x_vec_tmp2 = VecOp.sub(x_vec_tmp, x_vec);
+ Hep3Vector dq_dc = VecOp.mult(T,x_vec_tmp2);
+
+ if(_debug>1) {
+ System.out.printf("%s: position %s rotation derivative w.r.t. a ddeltaR'/dc %s\n",this.getClass().getSimpleName(),x_vec.toString(),x_vec_tmp.toString());
+ System.out.printf("%s: subtracted %s and rotated to local %s \n",this.getClass().getSimpleName(),x_vec_tmp2.toString(),dq_dc.toString());
+ }
+
+
+ if(_debug>1) {
+ System.out.printf("%s: Summary:\n",this.getClass().getSimpleName());
+ System.out.printf("%s: dq_da: %s\n",this.getClass().getSimpleName(),dq_da.toString());
+ System.out.printf("%s: dq_db: %s\n",this.getClass().getSimpleName(),dq_db.toString());
+ System.out.printf("%s: dq_dc: %s\n",this.getClass().getSimpleName(),dq_dc.toString());
+ }
+
+
+
+
+ // Lower left 3x3
+ //deltaR = (alpha,beta,gamma) the rotation alignment parameters in the local frame
+ Hep3Vector ddeltaR_dx = new BasicHep3Vector(0,0,0);
+ Hep3Vector ddeltaR_dy = new BasicHep3Vector(0,0,0);
+ Hep3Vector ddeltaR_dz = new BasicHep3Vector(0,0,0);
+
+
+ if(_debug>1) {
+ System.out.printf("%s: - Lower left 3x3 of Jacobian ddeltaR/d(x,y,z): dDeltaR_dx,dDeltaR_dy,dDeltaR_dz\n",this.getClass().getSimpleName());
+ System.out.printf("%s: ddeltaR_dx: %s\n",this.getClass().getSimpleName(),ddeltaR_dx.toString());
+ System.out.printf("%s: ddeltaR_dy: %s\n",this.getClass().getSimpleName(),ddeltaR_dy.toString());
+ System.out.printf("%s: ddeltaR_dz: %s\n",this.getClass().getSimpleName(),ddeltaR_dz.toString());
+ }
+
+
+
+
+ // Lower right 3x3
+/*
+ //deltaR = (alpha,beta,gamma) the rotation alignment parameters in the local frame
+ //Expressing T in Euler angles i,j,k
+ double i = 0;
+ double j = 0;
+ double k = 0;
+ double dalpha_da = Math.cos(k)*Math.sin(i)*Math.sin(j) + Math.cos(i)*Math.sin(k);
+ double dbeta_da = Math.cos(i)*Math.cos(k) - Math.sin(i)*Math.sin(j)*Math.sin(k);
+ double dgamma_da = -Math.cos(j)*Math.sin(i);
+ double dalpha_db = -Math.cos(i)*Math.cos(k)*Math.sin(j) + Math.sin(i)*Math.sin(k);
+ double dbeta_db = Math.cos(k)*Math.sin(i) + Math.cos(i)*Math.sin(j)*Math.sin(k);
+ double dgamma_db = Math.cos(i)*Math.cos(j);
+ double dalpha_dc = -Math.cos(i)*Math.cos(k)*Math.sin(j) + Math.sin(i)*Math.sin(k);
+ double dbeta_dc = Math.cos(k)*Math.sin(i) + Math.cos(i)*Math.sin(j)*Math.sin(k);
+ double dgamma_dc = Math.sin(i)*Math.sin(j);
+
+ Hep3Vector ddeltaR_da = new BasicHep3Vector(dalpha_da,dbeta_da,dgamma_da);
+ Hep3Vector ddeltaR_db = new BasicHep3Vector(dalpha_db,dbeta_db,dgamma_db);
+ Hep3Vector ddeltaR_dc = new BasicHep3Vector(dalpha_dc,dbeta_dc,dgamma_dc);
+*/
+
+ if(_debug>1) {
+ System.out.printf("%s: - Lower right 3x3 of Jacobian ddeltaR/d(a,b,c): \n",this.getClass().getSimpleName());
+ System.out.printf("%s: T: %s\n",this.getClass().getSimpleName(),T.toString());
+ }
+
+
+
+ //Now fill the Jacobian 6x6 matrix
+ BasicMatrix da_db = new BasicMatrix(6,6);
+ //upper left 3x3
+ da_db.setElement(0,0,dq_dx.x());
+ da_db.setElement(1,0,dq_dx.y());
+ da_db.setElement(2,0,dq_dx.z());
+ da_db.setElement(0,1,dq_dy.x());
+ da_db.setElement(1,1,dq_dy.y());
+ da_db.setElement(2,1,dq_dy.z());
+ da_db.setElement(0,2,dq_dz.x());
+ da_db.setElement(1,2,dq_dz.y());
+ da_db.setElement(2,2,dq_dz.z());
+ //upper right 3x3
+ da_db.setElement(0,3,dq_da.x());
+ da_db.setElement(1,3,dq_da.y());
+ da_db.setElement(2,3,dq_da.z());
+ da_db.setElement(0,4,dq_db.x());
+ da_db.setElement(1,4,dq_db.y());
+ da_db.setElement(2,4,dq_db.z());
+ da_db.setElement(0,5,dq_dc.x());
+ da_db.setElement(1,5,dq_dc.y());
+ da_db.setElement(2,5,dq_dc.z());
+ //lower right 3x3
+ da_db.setElement(3,3,T.e(0, 0));
+ da_db.setElement(4,3,T.e(1, 0));
+ da_db.setElement(5,3,T.e(2, 0));
+ da_db.setElement(3,4,T.e(0, 1));
+ da_db.setElement(4,4,T.e(1, 1));
+ da_db.setElement(5,4,T.e(2, 1));
+ da_db.setElement(3,5,T.e(0, 2));
+ da_db.setElement(4,5,T.e(1, 2));
+ da_db.setElement(5,5,T.e(2, 2));
+// da_db.setElement(4,3,ddeltaR_da.y());
+// da_db.setElement(5,3,ddeltaR_da.z());
+// da_db.setElement(3,4,ddeltaR_db.x());
+// da_db.setElement(4,4,ddeltaR_db.y());
+// da_db.setElement(5,4,ddeltaR_db.z());
+// da_db.setElement(3,5,ddeltaR_dc.x());
+// da_db.setElement(4,5,ddeltaR_dc.y());
+// da_db.setElement(5,5,ddeltaR_dc.z());
+ //lower left 3x3
+ da_db.setElement(3,0,ddeltaR_dx.x());
+ da_db.setElement(4,0,ddeltaR_dx.y());
+ da_db.setElement(5,0,ddeltaR_dx.z());
+ da_db.setElement(3,1,ddeltaR_dy.x());
+ da_db.setElement(4,1,ddeltaR_dy.y());
+ da_db.setElement(5,1,ddeltaR_dy.z());
+ da_db.setElement(3,2,ddeltaR_dz.x());
+ da_db.setElement(4,2,ddeltaR_dz.y());
+ da_db.setElement(5,2,ddeltaR_dz.z());
+
+ if(_debug>1) {
+ System.out.printf("%s: da_db:\n%s \n",this.getClass().getSimpleName(),da_db.toString());
+ System.out.printf("%s: det(da_db) = %.3f \n",this.getClass().getSimpleName(),da_db.det());
+ }
+
+ return da_db;
+
}
- public double dy_dbeta() {
- return 0;
+
+
+
+ public void printJacobianInfo(BasicMatrix da_db) {
+
+ System.out.printf("%s: Jacobian info ---\nda_db:\n%s \n",this.getClass().getSimpleName(),da_db.toString());
+ System.out.printf("du_dx = %8.3f du_dy = %8.3f du_dz = %8.3f\n",da_db.e(0,0),da_db.e(0,1),da_db.e(0,2));
+ System.out.printf("dv_dx = %8.3f dv_dy = %8.3f dv_dz = %8.3f\n",da_db.e(1,0),da_db.e(1,1),da_db.e(1,2));
+ System.out.printf("dw_dx = %8.3f dw_dy = %8.3f dw_dz = %8.3f\n",da_db.e(2,0),da_db.e(2,1),da_db.e(2,2));
+ System.out.printf("du_da = %8.3f du_db = %8.3f du_dc = %8.3f\n",da_db.e(0,3),da_db.e(0,4),da_db.e(0,5));
+ System.out.printf("dv_da = %8.3f dv_db = %8.3f dv_dc = %8.3f\n",da_db.e(1,3),da_db.e(1,4),da_db.e(1,5));
+ System.out.printf("dw_da = %8.3f dw_db = %8.3f dw_dc = %8.3f\n",da_db.e(2,3),da_db.e(2,4),da_db.e(2,5));
+ System.out.printf("dalpha_dx = %8.3f dalpha_dy = %8.3f dalpha_dz = %8.3f\n",da_db.e(3,0),da_db.e(3,1),da_db.e(3,2));
+ System.out.printf("dbeta_dx = %8.3f dbeta_dy = %8.3f dbeta_dz = %8.3f\n",da_db.e(4,0),da_db.e(4,1),da_db.e(4,2));
+ System.out.printf("dgamma_dx = %8.3f dgamma_dy = %8.3f dgamma_dz = %8.3f\n",da_db.e(5,0),da_db.e(5,1),da_db.e(5,2));
+ System.out.printf("dalpha_da = %8.3f dalpha_db = %8.3f dalpha_dc = %8.3f\n",da_db.e(3,3),da_db.e(3,4),da_db.e(3,5));
+ System.out.printf("dbeta_da = %8.3f dbeta_db = %8.3f dbeta_dc = %8.3f\n",da_db.e(4,3),da_db.e(4,4),da_db.e(4,5));
+ System.out.printf("dgamma_da = %8.3f dgamma_db = %8.3f dgamma_dc = %8.3f\n",da_db.e(5,3),da_db.e(5,4),da_db.e(5,5));
+
}
- public double dz_dbeta() {
- return -1;
+
+
+
+
+ public BasicMatrix calculateGlobalHitPositionDers(Hep3Vector x_vec) {
+
+
+ //****************************************************************************
+ // Calculate the global derivatives in the global/tracking frame dq_a^gl/db
+ // q_a^gl is the alignment corrected hit position in the global/tracking frame
+ // b = (dx,dy,dz,a,b,c)
+
+
+ //Derivatives of the rotation matrix deltaR' w.r.t. rotations a,b,c around axis x,y,z
+ Hep3Matrix ddeltaRprime_da = rotationDer_da();
+ Hep3Matrix ddeltaRprime_db = rotationDer_db();
+ Hep3Matrix ddeltaRprime_dc = rotationDer_dc();
+
+ if(_debug>1) {
+ System.out.printf("%s: Derivatives of the rotation matrix deltaR' w.r.t. rotation a,b,c around x,y,z axis:\n",this.getClass().getSimpleName());
+ System.out.printf("%s: ddeltaRprime_da:\n %s\n",this.getClass().getSimpleName(),ddeltaRprime_da.toString());
+ System.out.printf("%s: ddeltaRprime_db:\n %s\n",this.getClass().getSimpleName(),ddeltaRprime_db.toString());
+ System.out.printf("%s: ddeltaRprime_dc:\n %s\n",this.getClass().getSimpleName(),ddeltaRprime_dc.toString());
+ }
+
+
+
+ BasicMatrix dq_agl_db = new BasicMatrix(3,6);
+ //Translations
+ Hep3Vector dq_agl_dx = new BasicHep3Vector(1,0,0);
+ Hep3Vector dq_agl_dy = new BasicHep3Vector(0,1,0);
+ Hep3Vector dq_agl_dz = new BasicHep3Vector(0,0,1);
+ //Rotations
+ // Derivative of the rotations w.r.t. rotations a,b,c was already calculated above:
+ // but they need to be avaluated at the hit positon
+ Hep3Vector dq_agl_dalpha = VecOp.mult(ddeltaRprime_da,x_vec);
+ Hep3Vector dq_agl_dbeta = VecOp.mult(ddeltaRprime_db,x_vec);
+ Hep3Vector dq_agl_dgamma = VecOp.mult(ddeltaRprime_dc,x_vec);
+
+ //put them all in one big matrix
+
+ dq_agl_db.setElement(0, 0, dq_agl_dx.x());
+ dq_agl_db.setElement(1, 0, dq_agl_dx.y());
+ dq_agl_db.setElement(2, 0, dq_agl_dx.z());
+
+ dq_agl_db.setElement(0, 1, dq_agl_dy.x());
+ dq_agl_db.setElement(1, 1, dq_agl_dy.y());
+ dq_agl_db.setElement(2, 1, dq_agl_dy.z());
+
+ dq_agl_db.setElement(0, 2, dq_agl_dz.x());
+ dq_agl_db.setElement(1, 2, dq_agl_dz.y());
+ dq_agl_db.setElement(2, 2, dq_agl_dz.z());
+
+ dq_agl_db.setElement(0, 3, dq_agl_dalpha.x());
+ dq_agl_db.setElement(1, 3, dq_agl_dalpha.y());
+ dq_agl_db.setElement(2, 3, dq_agl_dalpha.z());
+
+ dq_agl_db.setElement(0, 4, dq_agl_dbeta.x());
+ dq_agl_db.setElement(1, 4, dq_agl_dbeta.y());
+ dq_agl_db.setElement(2, 4, dq_agl_dbeta.z());
+
+ dq_agl_db.setElement(0, 5, dq_agl_dgamma.x());
+ dq_agl_db.setElement(1, 5, dq_agl_dgamma.y());
+ dq_agl_db.setElement(2, 5, dq_agl_dgamma.z());
+
+
+
+ if(_debug>1) {
+ System.out.printf("%s: Translation derivative of the alignment corrected hit:\n",this.getClass().getSimpleName());
+ System.out.printf("dq_agl_dx=%s \ndq_agl_dy=%s \ndq_agl_dz=%s\n",dq_agl_dx.toString(),dq_agl_dy.toString(),dq_agl_dz.toString());
+ System.out.printf("%s: Rotation derivative of the alignment corrected hit evaluated at trkpos(or x_vec)=%s:\n",this.getClass().getSimpleName(),x_vec.toString());
+ System.out.printf("dq_agl_dalpha=%s \ndq_agl_dbeta=%s \ndq_agl_dgamma=%s\n",dq_agl_dalpha.toString(),dq_agl_dbeta.toString(),dq_agl_dgamma.toString());
+ System.out.printf("%s: Putting it together\ndq_agl_db:\n%s\n",this.getClass().getSimpleName(),dq_agl_db.toString());
+
+ //cross-check using manual calculation from note
+ BasicMatrix dq_agl_db_tmp = new BasicMatrix(3,6);
+ dq_agl_db_tmp.setElement(0, 0, 1);
+ dq_agl_db_tmp.setElement(1, 0, 0);
+ dq_agl_db_tmp.setElement(2, 0, 0);
+
+ dq_agl_db_tmp.setElement(0, 1, 0);
+ dq_agl_db_tmp.setElement(1, 1, 1);
+ dq_agl_db_tmp.setElement(2, 1, 0);
+
+ dq_agl_db_tmp.setElement(0, 2, 0);
+ dq_agl_db_tmp.setElement(1, 2, 0);
+ dq_agl_db_tmp.setElement(2, 2, 1);
+
+ dq_agl_db_tmp.setElement(0, 3, 0);
+ dq_agl_db_tmp.setElement(1, 3, x_vec.z());
+ dq_agl_db_tmp.setElement(2, 3, -x_vec.y());
+
+ dq_agl_db_tmp.setElement(0, 4, -x_vec.z());
+ dq_agl_db_tmp.setElement(1, 4, 0);
+ dq_agl_db_tmp.setElement(2, 4, x_vec.x());
+
+ dq_agl_db_tmp.setElement(0, 5, x_vec.y());
+ dq_agl_db_tmp.setElement(1, 5, -x_vec.x());
+ dq_agl_db_tmp.setElement(2, 5, 0);
+
+ System.out.printf("%s: Cross-check with this manual calculated matrix\ndq_agl_db_tmp:\n%s\n",this.getClass().getSimpleName(),dq_agl_db_tmp.toString());
+
+ }
+
+ return dq_agl_db;
+
}
- public double dx_dgamma() {
- return -1;
+
+ public void printGlobalHitPositionDers(BasicMatrix dq_db) {
+
+ System.out.printf("%s: Derivatives of the hit position w.r.t. to the global alignment parameters b: dq_agl_db---\n%s \n",this.getClass().getSimpleName(),dq_db.toString());
+ System.out.printf("dx_dx = %8.3f dx_dy = %8.3f dx_dz = %8.3f\n",dq_db.e(0,0),dq_db.e(0,1),dq_db.e(0,2));
+ System.out.printf("dy_dx = %8.3f dy_dy = %8.3f dy_dz = %8.3f\n",dq_db.e(1,0),dq_db.e(1,1),dq_db.e(1,2));
+ System.out.printf("dz_dx = %8.3f dz_dy = %8.3f dz_dz = %8.3f\n",dq_db.e(2,0),dq_db.e(2,1),dq_db.e(2,2));
+ System.out.printf("dx_da = %8.3f dx_db = %8.3f dx_dc = %8.3f\n",dq_db.e(0,3),dq_db.e(0,4),dq_db.e(0,5));
+ System.out.printf("dy_dx = %8.3f dy_dy = %8.3f dy_dz = %8.3f\n",dq_db.e(1,3),dq_db.e(1,4),dq_db.e(1,5));
+ System.out.printf("dz_dx = %8.3f dz_dy = %8.3f dz_dz = %8.3f\n",dq_db.e(2,3),dq_db.e(2,4),dq_db.e(2,5));
+
+
}
- public double dy_dgamma() {
- return 1;
+
+ public void printGlobalPredictedHitPositionDers(BasicMatrix dq_db) {
+
+ System.out.printf("%s: Derivatives of the predicted hit position w.r.t. to the global alignment parameters b: dq_pgl_db---\n%s \n",this.getClass().getSimpleName(),dq_db.toString());
+ System.out.printf("dx_dx = %8.3f dx_dy = %8.3f dx_dz = %8.3f\n",dq_db.e(0,0),dq_db.e(0,1),dq_db.e(0,2));
+ System.out.printf("dy_dx = %8.3f dy_dy = %8.3f dy_dz = %8.3f\n",dq_db.e(1,0),dq_db.e(1,1),dq_db.e(1,2));
+ System.out.printf("dz_dx = %8.3f dz_dy = %8.3f dz_dz = %8.3f\n",dq_db.e(2,0),dq_db.e(2,1),dq_db.e(2,2));
+ System.out.printf("dx_da = %8.3f dx_db = %8.3f dx_dc = %8.3f\n",dq_db.e(0,3),dq_db.e(0,4),dq_db.e(0,5));
+ System.out.printf("dy_dx = %8.3f dy_dy = %8.3f dy_dz = %8.3f\n",dq_db.e(1,3),dq_db.e(1,4),dq_db.e(1,5));
+ System.out.printf("dz_dx = %8.3f dz_dy = %8.3f dz_dz = %8.3f\n",dq_db.e(2,3),dq_db.e(2,4),dq_db.e(2,5));
+
+
}
- public double dz_dgamma() {
- return 0;
+
+
+ public void printGlobalResidualDers(BasicMatrix dz_db) {
+
+ System.out.printf("%s: Derivatives of the residual z=q_a - q_p w.r.t. to the global alignment parameters b: dz_db---\n%s \n",this.getClass().getSimpleName(),dz_db.toString());
+ System.out.printf("dx_dx = %8.3f dx_dy = %8.3f dx_dz = %8.3f\n",dz_db.e(0,0),dz_db.e(0,1),dz_db.e(0,2));
+ System.out.printf("dy_dx = %8.3f dy_dy = %8.3f dy_dz = %8.3f\n",dz_db.e(1,0),dz_db.e(1,1),dz_db.e(1,2));
+ System.out.printf("dz_dx = %8.3f dz_dy = %8.3f dz_dz = %8.3f\n",dz_db.e(2,0),dz_db.e(2,1),dz_db.e(2,2));
+ System.out.printf("dx_da = %8.3f dx_db = %8.3f dx_dc = %8.3f\n",dz_db.e(0,3),dz_db.e(0,4),dz_db.e(0,5));
+ System.out.printf("dy_da = %8.3f dy_db = %8.3f dy_dc = %8.3f\n",dz_db.e(1,3),dz_db.e(1,4),dz_db.e(1,5));
+ System.out.printf("dz_da = %8.3f dz_db = %8.3f dz_dc = %8.3f\n",dz_db.e(2,3),dz_db.e(2,4),dz_db.e(2,5));
+
+
}
+ public void printLocalResidualDers(BasicMatrix dz_da) {
+
+ System.out.printf("%s: Derivatives of the residual z=q_a - q_p w.r.t. to the local alignment parameters a: dz_da---\n%s \n",this.getClass().getSimpleName(),dz_da.toString());
+ System.out.printf("du_du = %8.3f du_dv = %8.3f du_dw = %8.3f\n",dz_da.e(0,0),dz_da.e(0,1),dz_da.e(0,2));
+ System.out.printf("dv_du = %8.3f dv_dv = %8.3f dv_dw = %8.3f\n",dz_da.e(1,0),dz_da.e(1,1),dz_da.e(1,2));
+ System.out.printf("dw_du = %8.3f dw_dv = %8.3f dw_dw = %8.3f\n",dz_da.e(2,0),dz_da.e(2,1),dz_da.e(2,2));
+ System.out.printf("du_dalpha = %8.3f du_dbeta = %8.3f du_dgamma = %8.3f\n",dz_da.e(0,3),dz_da.e(0,4),dz_da.e(0,5));
+ System.out.printf("dv_dalpha = %8.3f dv_dbeta = %8.3f dv_dgamma = %8.3f\n",dz_da.e(1,3),dz_da.e(1,4),dz_da.e(1,5));
+ System.out.printf("dw_dalpha = %8.3f dw_dbeta = %8.3f dw_dgamma = %8.3f\n",dz_da.e(2,3),dz_da.e(2,4),dz_da.e(2,5));
+
+
+ }
+
+
+
+ public BasicMatrix calculateGlobalPredictedHitPositionDers(HelicalTrackFit trk, Hep3Vector x_vec) {
+
+ double d0 = trk.dca();
+ double phi0 = trk.phi0();
+ double R = trk.R();
+ double s = HelixUtils.PathToXPlane(trk, x_vec.x(), 0, 0).get(0);
+ double phi = -s/R + phi0;
+ double slope = trk.slope();
+ double xr = 0.0;
+ double yr = 0.0;
+
+
+ //Derivatives of the predicted hit position qp for a translation of in x,y,z
+
+ BasicMatrix dq_pgl_dx = new BasicMatrix(3,1);
+ dq_pgl_dx.setElement(0, 0, dx_dx());
+ dq_pgl_dx.setElement(1, 0, dy_dx(x_vec.x(), d0, phi0, R, phi));
+ dq_pgl_dx.setElement(2, 0, dz_dx(x_vec.x(), xr, yr, d0, phi0, slope, R));
+
+ BasicMatrix dq_pgl_dy = new BasicMatrix(3,1);
+ dq_pgl_dy.setElement(0, 0, dx_dy(x_vec.y(), d0, phi0, R, phi));
+ dq_pgl_dy.setElement(1, 0, dy_dy());
+ dq_pgl_dy.setElement(2, 0, dz_dy(x_vec.y(), d0, phi0, slope, R, phi));
+
+ BasicMatrix dq_pgl_dz = new BasicMatrix(3,1);
+ dq_pgl_dz.setElement(0, 0, dx_dz(slope, R, phi));
+ dq_pgl_dz.setElement(1, 0, dy_dz(slope, R, phi));
+ dq_pgl_dz.setElement(2, 0, dz_dz());
+
+
+
+ /*
+ * Derivatives of the predicted hit position qp for a rotation of a,b,c around x,y,z
+ *
+ * Note that since these rotations are not around the center of the sensor
+ * a rotation also leads to a translation, this is nto taken into account here
+ * Since the lever arm might be substantial this might be non-negligible?
+ *
+ */
+
+ // THIS MIGHT NOT SINCE THE ROTATION IS NOT AROUND THE CENTER OF THE SENSOR!?
+
+ Hep3Matrix ddeltaRprime_da = rotationDer_da();
+ Hep3Matrix ddeltaRprime_db = rotationDer_db();
+ Hep3Matrix ddeltaRprime_dc = rotationDer_dc();
+
+ Hep3Vector dq_pgl_drota = VecOp.mult(ddeltaRprime_da, x_vec);
+ Hep3Vector dq_pgl_drotb = VecOp.mult(ddeltaRprime_db, x_vec);
+ Hep3Vector dq_pgl_drotc = VecOp.mult(ddeltaRprime_dc, x_vec);
+
+
+ //put them all in one big matrix
+
+ BasicMatrix dq_pgl_db = new BasicMatrix(3,6);
+
+ dq_pgl_db.setElement(0, 0, dq_pgl_dx.e(0,0));
+ dq_pgl_db.setElement(1, 0, dq_pgl_dx.e(1,0));
+ dq_pgl_db.setElement(2, 0, dq_pgl_dx.e(2,0));
+
+ dq_pgl_db.setElement(0, 1, dq_pgl_dy.e(0,0));
+ dq_pgl_db.setElement(1, 1, dq_pgl_dy.e(1,0));
+ dq_pgl_db.setElement(2, 1, dq_pgl_dy.e(2,0));
+
+ dq_pgl_db.setElement(0, 2, dq_pgl_dz.e(0,0));
+ dq_pgl_db.setElement(1, 2, dq_pgl_dz.e(1,0));
+ dq_pgl_db.setElement(2, 2, dq_pgl_dz.e(2,0));
+
+ dq_pgl_db.setElement(0, 3, dq_pgl_drota.x());
+ dq_pgl_db.setElement(1, 3, dq_pgl_drota.y());
+ dq_pgl_db.setElement(2, 3, dq_pgl_drota.z());
+
+ dq_pgl_db.setElement(0, 4, dq_pgl_drotb.x());
+ dq_pgl_db.setElement(1, 4, dq_pgl_drotb.y());
+ dq_pgl_db.setElement(2, 4, dq_pgl_drotb.z());
+
+ dq_pgl_db.setElement(0, 5, dq_pgl_drotc.x());
+ dq_pgl_db.setElement(1, 5, dq_pgl_drotc.y());
+ dq_pgl_db.setElement(2, 5, dq_pgl_drotc.z());
+
+
+
+ if(_debug>1) {
+ System.out.printf("%s: Translation derivative of the predicted hit position:\n",this.getClass().getSimpleName());
+ System.out.printf("dq_pgl_dx=\n%s \ndq_pgl_dy=\n%s \ndq_pgl_dz=\n%s\n",dq_pgl_dx.toString(),dq_pgl_dy.toString(),dq_pgl_dz.toString());
+ System.out.printf("%s: Rotation derivative of the predicted hit position:\n",this.getClass().getSimpleName());
+ System.out.printf("dq_pgl_dalpha=\n%s \ndq_pgl_dbeta=\n%s \ndq_pgl_dgamma=\n%s\n",dq_pgl_drota.toString(),dq_pgl_drotb.toString(),dq_pgl_drotc.toString());
+ System.out.printf("%s: Putting it together\ndq_pgl_db:\n%s\n",this.getClass().getSimpleName(),dq_pgl_db.toString());
+ }
+
+
+ return dq_pgl_db;
+ }
+
+
+
+
+
+
+
+
+
+
+
+
@@ -569,17 +1067,42 @@
private double _eps = 1e-9;
private int _type = 0; //0=five point stencil, 1=finite difference, 2=Newton difference quotient
- public NumDerivatives() {}
+ private Hep3Matrix[][] _rot_eps = new BasicHep3Matrix[3][2];
+ private Hep3Matrix[][] _rot_twoeps = new BasicHep3Matrix[3][2];
+ public NumDerivatives() {
+ constructRotationMatrices();
+ }
public NumDerivatives(double eps, int type) {
_eps = eps;
_type = type;
+ constructRotationMatrices();
}
public void setEpsilon(double eps) {
_eps = eps;
}
+ public void setDebug(boolean debug) {
+ _debug = debug ? 1 : 0;
+ }
+ public void setDebug(int debug) {
+ _debug = debug;
+ }
public void setType(int type) {
_type = type;
}
+
+ public final void constructRotationMatrices() {
+ String[] axis = {"x","y","z"};
+ for(int i=0;i<3;++i) {
+ _rot_eps[i][0] = getRotationMatrix(axis[i],_eps);
+ _rot_twoeps[i][0] = getRotationMatrix(axis[i],2*_eps);
+ _rot_eps[i][1] = getRotationMatrix(axis[i],-_eps);
+ _rot_twoeps[i][1] = getRotationMatrix(axis[i],-2*_eps);
+ }
+ }
+
+
+
+
public BasicMatrix calculateLocalHelixDerivatives(HelicalTrackFit trk, double xint) {
double d0 = trk.dca();
@@ -587,8 +1110,6 @@
double R = trk.R();
double phi0 = trk.phi0();
double slope = trk.slope();
- double s = HelixUtils.PathToXPlane(trk, xint, 0, 0).get(0);
- double phi = -s/R + phi0;
BasicMatrix dfdq = new BasicMatrix(3,5); //3-dim,ntrackparams
@@ -619,6 +1140,114 @@
return dfdq;
}
+
+
+ public BasicMatrix calculateGlobalPredictedHitPositionDers(HelicalTrackFit trk, Hep3Vector x_vec) {
+
+ double d0 = trk.dca();
+ double phi0 = trk.phi0();
+ double R = trk.R();
+ double s = HelixUtils.PathToXPlane(trk, x_vec.x(), 0, 0).get(0);
+ double phi = -s/R + phi0;
+ double slope = trk.slope();
+ double xr = 0.0;
+ double yr = 0.0;
+
+
+ //Derivatives of the predicted hit position qp for a translation of in x,y,z
+ Hep3Vector dfdx = this.df_dx(trk, x_vec);
+ Hep3Vector dfdy = this.df_dy(trk, x_vec);
+ Hep3Vector dfdz = this.df_dz(trk, x_vec);
+
+ //Use the same structure as the analytic derivatives
+
+ BasicMatrix dq_pgl_dx = new BasicMatrix(3,1);
+ dq_pgl_dx.setElement(0, 0, dfdx.x()); //dx_dx());
+ dq_pgl_dx.setElement(1, 0, dfdx.y()); //dy_dx(x_vec.x(), d0, phi0, R, phi));
+ dq_pgl_dx.setElement(2, 0, dfdx.z()); //dz_dx(x_vec.x(), xr, yr, d0, phi0, slope, R));
+
+ BasicMatrix dq_pgl_dy = new BasicMatrix(3,1);
+ dq_pgl_dy.setElement(0, 0, dfdy.x()); //dx_dy(x_vec.y(), d0, phi0, R, phi));
+ dq_pgl_dy.setElement(1, 0, dfdy.y()); //dy_dy());
+ dq_pgl_dy.setElement(2, 0, dfdy.z()); //dz_dy(x_vec.y(), d0, phi0, slope, R, phi));
+
+ BasicMatrix dq_pgl_dz = new BasicMatrix(3,1);
+ dq_pgl_dz.setElement(0, 0, dfdz.x()); //dx_dz(slope, R, phi));
+ dq_pgl_dz.setElement(1, 0, dfdz.y()); //dy_dz(slope, R, phi));
+ dq_pgl_dz.setElement(2, 0, dfdz.z()); //dz_dz());
+
+
+ /*
+ * Numerical derivative w.r.t. the rotation around x,y,z axes
+ */
+
+ Hep3Vector dfda = this.df_drot(trk, x_vec, "x");
+ Hep3Vector dfdb = this.df_drot(trk, x_vec, "y");
+ Hep3Vector dfdc = this.df_drot(trk, x_vec, "z");
+
+ BasicMatrix dq_pgl_dalpha = new BasicMatrix(3,1);
+ dq_pgl_dalpha.setElement(0, 0, dfda.x());
+ dq_pgl_dalpha.setElement(1, 0, dfda.y());
+ dq_pgl_dalpha.setElement(2, 0, dfda.z());
+
+ BasicMatrix dq_pgl_dbeta = new BasicMatrix(3,1);
+ dq_pgl_dbeta.setElement(0, 0, dfdb.x());
+ dq_pgl_dbeta.setElement(1, 0, dfdb.y());
+ dq_pgl_dbeta.setElement(2, 0, dfdb.z());
+
+ BasicMatrix dq_pgl_dgamma = new BasicMatrix(3,1);
+ dq_pgl_dgamma.setElement(0, 0, dfdc.x());
+ dq_pgl_dgamma.setElement(1, 0, dfdc.y());
+ dq_pgl_dgamma.setElement(2, 0, dfdc.z());
+
+
+ //put them all in one big matrix
+
+ BasicMatrix dq_pgl_db = new BasicMatrix(3,6);
+
+
+ dq_pgl_db.setElement(0, 0, dq_pgl_dx.e(0,0));
+ dq_pgl_db.setElement(1, 0, dq_pgl_dx.e(1,0));
+ dq_pgl_db.setElement(2, 0, dq_pgl_dx.e(2,0));
+
+ dq_pgl_db.setElement(0, 1, dq_pgl_dy.e(0,0));
+ dq_pgl_db.setElement(1, 1, dq_pgl_dy.e(1,0));
+ dq_pgl_db.setElement(2, 1, dq_pgl_dy.e(2,0));
+
+ dq_pgl_db.setElement(0, 2, dq_pgl_dz.e(0,0));
+ dq_pgl_db.setElement(1, 2, dq_pgl_dz.e(1,0));
+ dq_pgl_db.setElement(2, 2, dq_pgl_dz.e(2,0));
+
+ dq_pgl_db.setElement(0, 3, dq_pgl_dalpha.e(0,0));
+ dq_pgl_db.setElement(1, 3, dq_pgl_dalpha.e(1,0));
+ dq_pgl_db.setElement(2, 3, dq_pgl_dalpha.e(2,0));
+
+ dq_pgl_db.setElement(0, 4, dq_pgl_dbeta.e(0,0));
+ dq_pgl_db.setElement(1, 4, dq_pgl_dbeta.e(1,0));
+ dq_pgl_db.setElement(2, 4, dq_pgl_dbeta.e(2,0));
+
+ dq_pgl_db.setElement(0, 5, dq_pgl_dgamma.e(0,0));
+ dq_pgl_db.setElement(1, 5, dq_pgl_dgamma.e(1,0));
+ dq_pgl_db.setElement(2, 5, dq_pgl_dgamma.e(2,0));
+
+
+ /*
+ if(_debug) {
+ System.out.printf("%s: Translation derivative of the predicted hit position:\n",this.getClass().getSimpleName());
+ System.out.printf("dq_pgl_dx=\n%s \ndq_pgl_dy=\n%s \ndq_pgl_dz=\n%s\n",dq_pgl_dx.toString(),dq_pgl_dy.toString(),dq_pgl_dz.toString());
+ System.out.printf("%s: Rotation derivative of the predicted hit position:\n",this.getClass().getSimpleName());
+ System.out.printf("dq_pgl_dalpha=\n%s \ndq_pgl_dbeta=\n%s \ndq_pgl_dgamma=\n%s\n",dq_pgl_dalpha.toString(),dq_pgl_dbeta.toString(),dq_pgl_dgamma.toString());
+ System.out.printf("%s: Putting it together\ndq_pgl_db:\n%s\n",this.getClass().getSimpleName(),dq_pgl_db.toString());
+ }
+ */
+
+ return dq_pgl_db;
+
+ }
+
+
+
+
public Hep3Vector getNumDer(Hep3Vector f2h, Hep3Vector fh, Hep3Vector f, Hep3Vector fmh, Hep3Vector fm2h) {
double[] ders = new double[3];
for(int i=0;i<3;++i) {
@@ -656,15 +1285,18 @@
public double getFiniteDifferenceNewton(double fh, double f) {
return 1/(_eps)*(fh - f);
}
- public double phiHelix(double xint, double d0, double phi0, double R) {
+ public double phiHelixFromX(double xint, double d0, double phi0, double R) {
return Math.asin(1/R*((R-d0)*Math.sin(phi0) - xint));
}
- public Hep3Vector pointOnHelix(double xint, double d0, double z0, double phi0, double R, double slope) {
- return this.pointOnHelix(null, xint, d0, z0, phi0, R, slope);
+ public double phiHelixFromY(double y, double d0, double phi0, double R) {
+ double A = R*R - Math.pow(y+(R-d0)*Math.cos(phi0),2);
+ return Math.asin( 1/R*(-Math.signum(R))*Math.sqrt(A) );
}
- public Hep3Vector pointOnHelix(HelicalTrackFit trk, double xint, double d0, double z0, double phi0, double R, double slope) {
- double phi = phiHelix(xint,d0,phi0,R);
- double dphi = phi-phi0;
+ public double phiHelixFromZ(double z, double z0, double slope, double R, double phi0) {
+ return -1*(z-z0)/(R*slope)+phi0;
+ }
+ public Hep3Vector pointOnHelixFromPhi(double phi, double d0, double z0, double phi0, double R, double slope) {
+ double dphi = phi-phi0;
//Make sure dphi is in the valid range -pi,+pi
if(dphi>Math.PI) dphi -= 2.0*Math.PI;
if(dphi<-Math.PI) dphi += 2.0*Math.PI;
@@ -676,15 +1308,59 @@
double s = -R*dphi;
double z = z0 + s*slope;
Hep3Vector p = new BasicHep3Vector(x,y,z);
- System.out.printf("PointOnHelix s=%.3f dphi=%.3f d0=%.3f R=%.2f phi0=%.3f phi=%.3f (xc=%.3f,yc=%.3f)\n",s,dphi,d0,R,phi0,phi,xc,yc);
+ if(_debug>2) System.out.printf("PointOnHelix s=%.3f dphi=%.3f d0=%.3f R=%.2f phi0=%.3f phi=%.3f (xc=%.3f,yc=%.3f)\n",s,dphi,d0,R,phi0,phi,xc,yc);
+ return p;
+ }
+ public Hep3Vector pointOnHelix(double xint, double d0, double z0, double phi0, double R, double slope) {
+ return this.pointOnHelix(null, xint, d0, z0, phi0, R, slope);
+ }
+ public Hep3Vector pointOnHelix(HelicalTrackFit trk, double xint, double d0, double z0, double phi0, double R, double slope) {
+ double phi = phiHelixFromX(xint,d0,phi0,R);
+ Hep3Vector p = this.pointOnHelixFromPhi(phi, d0, z0, phi0, R, slope);
if(trk!=null) {
double s_tmp = HelixUtils.PathToXPlane(trk, xint, 0, 0).get(0);
Hep3Vector p_tmp = HelixUtils.PointOnHelix(trk, s_tmp);
Hep3Vector diff = VecOp.sub(p, p_tmp);
- System.out.printf(this.getClass().getSimpleName()+": diff=%s p=%s p_tmp=%s\n",diff.toString(),p.toString(),p_tmp.toString());
+ if(_debug>2) System.out.printf(this.getClass().getSimpleName()+": diff=%s p=%s p_tmp=%s\n",diff.toString(),p.toString(),p_tmp.toString());
+ }
+ return p;
+ }
+ public Hep3Vector pointOnHelixY(double yint, double d0, double z0, double phi0, double R, double slope) {
+ return this.pointOnHelixY(null, yint, d0, z0, phi0, R, slope);
+ }
+ public Hep3Vector pointOnHelixY(HelicalTrackFit trk, double yint, double d0, double z0, double phi0, double R, double slope) {
+ if(_debug>2) System.out.printf(this.getClass().getSimpleName()+": pointOnHelixY\n");
+ double phi = phiHelixFromY(yint,d0,phi0,R);
+ Hep3Vector p = this.pointOnHelixFromPhi(phi, d0, z0, phi0, R, slope);
+ if(_debug>2) System.out.printf(this.getClass().getSimpleName()+": point = %s , phiHelixFromY=%.3f\n",p.toString(),phi);
+ if(trk!=null) {
+ //not sure this works as I use x here for the extrapolation
+ double s_tmp = HelixUtils.PathToXPlane(trk, p.x(), 0, 0).get(0);
+ Hep3Vector p_tmp = HelixUtils.PointOnHelix(trk, s_tmp);
+ double RC_from_utils = trk.R();
+ double phi_from_utils = trk.phi0() - s_tmp / RC_from_utils;
+ Hep3Vector diff = VecOp.sub(p, p_tmp);
+ if(_debug>2) System.out.printf(this.getClass().getSimpleName()+": phi_from_utils=%.3f s_tmp=%s\n",phi_from_utils,s_tmp);
+ if(_debug>2) System.out.printf(this.getClass().getSimpleName()+": diff=%s p=%s p_tmp=%s\n",diff.toString(),p.toString(),p_tmp.toString());
+ }
+ return p;
+ }
+ public Hep3Vector pointOnHelixZ(double zint, double d0, double z0, double phi0, double R, double slope) {
+ return this.pointOnHelixZ(null, zint, d0, z0, phi0, R, slope);
+ }
+ public Hep3Vector pointOnHelixZ(HelicalTrackFit trk, double zint, double d0, double z0, double phi0, double R, double slope) {
+ double phi = this.phiHelixFromZ(zint, z0, slope, R, phi0);
+ Hep3Vector p = this.pointOnHelixFromPhi(phi, d0, z0, phi0, R, slope);
+ if(trk!=null) {
+ //not sure this works as I use x here for the extrapolation
+ double s_tmp = HelixUtils.PathToXPlane(trk, p.x(), 0, 0).get(0);
+ Hep3Vector p_tmp = HelixUtils.PointOnHelix(trk, s_tmp);
+ Hep3Vector diff = VecOp.sub(p, p_tmp);
+ if(_debug>2) System.out.printf(this.getClass().getSimpleName()+": diff=%s p=%s p_tmp=%s\n",diff.toString(),p.toString(),p_tmp.toString());
}
return p;
}
+
public Hep3Vector df_dd0(HelicalTrackFit trk, double xint, double d0, double z0, double phi0, double R, double slope) {
Hep3Vector f = pointOnHelix(trk, xint, d0, z0, phi0, R, slope);
@@ -734,6 +1410,122 @@
+ public Hep3Vector df_dx(HelicalTrackFit trk, Hep3Vector x_vec) {
+ double d0 = trk.dca();
+ double z0 = trk.z0();
+ double R = trk.R();
+ double phi0 = trk.phi0();
+ double slope = trk.slope();
+ Hep3Vector f = this.pointOnHelix(x_vec.x(), d0, z0, phi0, R, slope);
+ Hep3Vector fh = this.pointOnHelix(x_vec.x()+_eps, d0, z0, phi0, R, slope);
+ Hep3Vector fmh = this.pointOnHelix(x_vec.x()-_eps, d0, z0, phi0, R, slope);
+ Hep3Vector f2h = this.pointOnHelix(x_vec.x()+2*_eps, d0, z0, phi0, R, slope);
+ Hep3Vector fm2h = this.pointOnHelix(x_vec.x()-2*_eps, d0, z0, phi0, R, slope);
+ Hep3Vector df = this.getNumDer(f2h, fh, f, fmh, fm2h);
+ return df;
+ }
+
+ public Hep3Vector df_dy(HelicalTrackFit trk, Hep3Vector x_vec) {
+ double d0 = trk.dca();
+ double z0 = trk.z0();
+ double R = trk.R();
+ double phi0 = trk.phi0();
+ double slope = trk.slope();
+ Hep3Vector f = this.pointOnHelixY(trk,x_vec.y(), d0, z0, phi0, R, slope);
+ Hep3Vector fh = this.pointOnHelixY(x_vec.y()+_eps, d0, z0, phi0, R, slope);
+ Hep3Vector fmh = this.pointOnHelixY(x_vec.y()-_eps, d0, z0, phi0, R, slope);
+ Hep3Vector f2h = this.pointOnHelixY(x_vec.y()+2*_eps, d0, z0, phi0, R, slope);
+ Hep3Vector fm2h = this.pointOnHelixY(x_vec.y()-2*_eps, d0, z0, phi0, R, slope);
+ Hep3Vector df = this.getNumDer(f2h, fh, f, fmh, fm2h);
+ return df;
+ }
+ public Hep3Vector df_dz(HelicalTrackFit trk, Hep3Vector x_vec) {
+ double d0 = trk.dca();
+ double z0 = trk.z0();
+ double R = trk.R();
+ double phi0 = trk.phi0();
+ double slope = trk.slope();
+ Hep3Vector f = this.pointOnHelixZ(trk,x_vec.z(), d0, z0, phi0, R, slope);
+ Hep3Vector fh = this.pointOnHelixZ(x_vec.z()+_eps, d0, z0, phi0, R, slope);
+ Hep3Vector fmh = this.pointOnHelixZ(x_vec.z()-_eps, d0, z0, phi0, R, slope);
+ Hep3Vector f2h = this.pointOnHelixZ(x_vec.z()+2*_eps, d0, z0, phi0, R, slope);
+ Hep3Vector fm2h = this.pointOnHelixZ(x_vec.z()-2*_eps, d0, z0, phi0, R, slope);
+ Hep3Vector df = this.getNumDer(f2h, fh, f, fmh, fm2h);
+ return df;
+ }
+
+
+ public int getAxisId(String axis) {
+ if(!axis.matches("x|y|z")) {
+ throw new RuntimeException(this.getClass().getSimpleName()+": this axis " + axis + " is nto defined");
+ }
+ return axis.equalsIgnoreCase("x") ? 0 : axis.equalsIgnoreCase("y") ? 1 : 2;
+ }
+ public Hep3Vector rotateEpsPlus(Hep3Vector x_vec,String axis) {
+ return VecOp.mult(_rot_eps[getAxisId(axis)][0], x_vec);
+ }
+ public Hep3Vector rotateEpsMinus(Hep3Vector x_vec,String axis) {
+ return VecOp.mult(_rot_eps[getAxisId(axis)][1], x_vec);
+ }
+ public Hep3Vector rotateTwoEpsPlus(Hep3Vector x_vec,String axis) {
+ return VecOp.mult(_rot_twoeps[getAxisId(axis)][0], x_vec);
+ }
+ public Hep3Vector rotateTwoEpsMinus(Hep3Vector x_vec,String axis) {
+ return VecOp.mult(_rot_twoeps[getAxisId(axis)][1], x_vec);
+ }
+
+ public Hep3Vector df_drot(HelicalTrackFit trk, Hep3Vector x_vec, String axis) {
+ //NUmerical derivative w.r.t. the rotation around given axis (x,y,z)
+ double d0 = trk.dca();
+ double z0 = trk.z0();
[truncated at 1000 lines; 60 more skipped]