Commit in hps-java/src/main/java/org/lcsim/hps/users/phansson on MAIN | |||
AlignmentUtils.java | +854 | -62 | 1.9 -> 1.10 |
Added better debug. Improved modularization.
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]
Use REPLY-ALL to reply to list
To unsubscribe from the LCD-CVS list, click the following link:
https://listserv.slac.stanford.edu/cgi-bin/wa?SUBED1=LCD-CVS&A=1