 |
 |
| Commit in hps-java/src/main/java/org/lcsim/hps/users/phansson on MAIN | |||
| AlignmentUtils.java | +320 | -11 | 1.8 -> 1.9 |
Fixed formula for phi. Fixed product rule error for dfx_dR and dfy_dR. Fixed dphi range to be valid in path length calc. Added Numerical ders (five-point, Newton, finite difference) for local ders.
diff -u -r1.8 -r1.9 --- AlignmentUtils.java 23 Oct 2012 01:52:23 -0000 1.8 +++ AlignmentUtils.java 24 Oct 2012 06:45:52 -0000 1.9 @@ -5,8 +5,11 @@
package org.lcsim.hps.users.phansson; 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 org.lcsim.fit.helicaltrack.HelicalTrackFit;
-import org.lcsim.fit.helicaltrack.HelicalTrackStrip;
import org.lcsim.fit.helicaltrack.HelixUtils; /**
@@ -15,6 +18,8 @@
*/
public class AlignmentUtils {
+ + private boolean _debug;
public AlignmentUtils() {
_debug = false;
@@ -25,6 +30,8 @@
public void setDebug(boolean debug) {
_debug = debug;
}
+ +
public BasicMatrix calculateLocalHelixDerivatives(HelicalTrackFit trk, double xint) {
// Calculate the derivative w.r.t. to the track parameters (in order/index):
@@ -76,7 +83,16 @@
}
-
+ private BasicMatrix FillMatrix(double[][] array, int nrow, int ncol) {
+ BasicMatrix retMat = new BasicMatrix(nrow, ncol);
+ for (int i = 0; i < nrow; i++) {
+ for (int j = 0; j < ncol; j++) {
+ retMat.setElement(i, j, array[i][j]);
+ }
+ }
+ return retMat;
+ }
+
@@ -97,7 +113,7 @@
return (R-d0)*Math.cos(phi0) - R*Math.cos(phi)*dphi_dphi0(x,d0,phi0,R);
}
public double dx_dR(double x, double xr, double yr, double d0, double phi0, double R, double phi) {
- return Math.sin(phi0) - R*Math.cos(phi)*dphi_dR(x,d0,phi0,R);
+ return Math.sin(phi0) - R*Math.cos(phi)*dphi_dR(x,d0,phi0,R) - Math.sin(phi);
}
//-----------------------------------
@@ -119,7 +135,7 @@
return (R-d0)*Math.sin(phi0) - R*Math.sin(phi)*this.dphi_dphi0(x, d0, phi0, R);
}
public double dy_dR(double x, double d0, double phi0, double R, double phi) {
- return -Math.cos(phi0) - R*Math.sin(phi)*this.dphi_dR(x, d0, phi0, R);
+ return -Math.cos(phi0) - R*Math.sin(phi)*this.dphi_dR(x, d0, phi0, R) + Math.cos(phi);
}
@@ -152,8 +168,8 @@
// Derivatives of phi w.r.t. track parameters
public double dphi_dd0(double x,double d0, double phi0, double R) {
- double num = Math.sin(phi0); - double den = R*Math.sqrt(Math.pow(x+(d0-R)*Math.sin(phi0), 2)/Math.pow(R,2));
+ double num = -1*Math.sin(phi0); + double den = R*Math.sqrt( 1 - Math.pow(x+(d0-R)*Math.sin(phi0), 2)/Math.pow(R,2));
return num/den;
}
@@ -164,14 +180,14 @@
return 0;
}
public double dphi_dphi0(double x,double d0, double phi0, double R) {
- double num = (d0-R)*Math.sin(phi0); - double den = R*Math.sqrt(1-Math.pow(x+(d0-R)*Math.sin(phi0),2)/Math.pow(R, 2));
+ double num = -1*(d0-R)*Math.cos(phi0); + double den = R*Math.sqrt( 1 - Math.pow(x+(d0-R)*Math.sin(phi0),2)/Math.pow(R, 2));
return num/den;
}
public double dphi_dR(double x,double d0, double phi0, double R) {
- double num = -x-d0*Math.sin(phi0); - double den = Math.pow(R, 2)*Math.sqrt(1-Math.pow(x+(d0-R)*Math.sin(phi0),2)/Math.pow(R, 2));
+ double num = x + d0*Math.sin(phi0); + double den = Math.pow(R, 2)*Math.sqrt( 1 - Math.pow(x+(d0-R)*Math.sin(phi0),2)/Math.pow(R, 2));
return num/den;
}
@@ -539,9 +555,302 @@
return Math.signum(val);
}
+
+
+ /**
+ *
+ * Nested class that contains the numerical derivatives
+ * Nested class to increase encapsulation and save this for future cross-checks
+ */
+
+
+
+ public class NumDerivatives {
+
+ private double _eps = 1e-9;
+ private int _type = 0; //0=five point stencil, 1=finite difference, 2=Newton difference quotient
+ public NumDerivatives() {}
+ public NumDerivatives(double eps, int type) {
+ _eps = eps;
+ _type = type;
+ }
+ public void setEpsilon(double eps) {
+ _eps = eps;
+ }
+ public void setType(int type) {
+ _type = type;
+ }
+ public BasicMatrix calculateLocalHelixDerivatives(HelicalTrackFit trk, double xint) {
+
+ double d0 = trk.dca();
+ double z0 = trk.z0();
+ 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
+
+ Hep3Vector df_dd0 = df_dd0(trk, xint, d0, z0, phi0, R, slope);
+ Hep3Vector df_dz0 = df_dz0(trk, xint, d0, z0, phi0, R, slope);
+ Hep3Vector df_dslope = df_dslope(trk, xint, d0, z0, phi0, R, slope);
+ Hep3Vector df_dphi0 = df_dphi0(trk, xint, d0, z0, phi0, R, slope);
+ Hep3Vector df_dR = df_dR(trk, xint, d0, z0, phi0, R, slope);
+
+ dfdq.setElement(0, 0, df_dd0.x());
+ dfdq.setElement(0, 1, df_dz0.x());
+ dfdq.setElement(0, 2, df_dslope.x());
+ dfdq.setElement(0, 3, df_dphi0.x());
+ dfdq.setElement(0, 4, df_dR.x());
+
+ dfdq.setElement(1, 0, df_dd0.y());
+ dfdq.setElement(1, 1, df_dz0.y());
+ dfdq.setElement(1, 2, df_dslope.y());
+ dfdq.setElement(1, 3, df_dphi0.y());
+ dfdq.setElement(1, 4, df_dR.y());
+
+ dfdq.setElement(2, 0, df_dd0.z());
+ dfdq.setElement(2, 1, df_dz0.z());
+ dfdq.setElement(2, 2, df_dslope.z());
+ dfdq.setElement(2, 3, df_dphi0.z());
+ dfdq.setElement(2, 4, df_dR.z());
+
+ return dfdq;
+
+ }
+ 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) {
+ ders[i] = getNumDer(f2h.v()[i], fh.v()[i], f.v()[i], fmh.v()[i], fm2h.v()[i]);
+ }
+ return new BasicHep3Vector(ders);
+ }
+ public double getNumDer(double f2h, double fh, double f, double fmh, double fm2h) {
+ double d = 0;
+ try {
+ switch (_type) {
+ case 0:
+ d = getFivePointStencilValue(f2h, fh, fmh, fm2h);
+ break;
+ case 1:
+ d = this.getFiniteDifference(fh, fmh);
+ break;
+ case 2:
+ d = this.getFiniteDifferenceNewton(fh, f);
+ break;
+ default:
+ throw new RuntimeException(this.getClass().getSimpleName() + ": error wrong numder type " + _type);
+ }
+ } catch (RuntimeException e) {
+ System.out.println("Error " + e);
+ }
+ return d;
+ }
+ public double getFivePointStencilValue(double f2h, double fh, double fmh, double fm2h) {
+ return 1/(12*_eps)*(-f2h + 8*fh - 8*fmh + fm2h);
+ }
+ public double getFiniteDifference(double fh, double fmh) {
+ return 1/(2*_eps)*(fh - fmh);
+ }
+ public double getFiniteDifferenceNewton(double fh, double f) {
+ return 1/(_eps)*(fh - f);
+ }
+ public double phiHelix(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 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;
+ //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;
+
+ double xc = (R-d0)*Math.sin(phi0);
+ double yc = -1*(R-d0)*Math.cos(phi0);
+ double x = xc - R*Math.sin(phi);
+ double y = yc + R*Math.cos(phi);
+ 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(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());
+ }
+ 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);
+ Hep3Vector fh = pointOnHelix(xint, d0+_eps, z0, phi0, R, slope);
+ Hep3Vector fmh = pointOnHelix(xint, d0-_eps, z0, phi0, R, slope);
+ Hep3Vector f2h = pointOnHelix(xint, d0+2*_eps, z0, phi0, R, slope);
+ Hep3Vector fm2h = pointOnHelix(xint, d0-2*_eps, z0, phi0, R, slope);
+ Hep3Vector df = this.getNumDer(f2h, fh, f, fmh, fm2h);
+ return df;
+ }
+ public Hep3Vector df_dz0(HelicalTrackFit trk, double xint, double d0, double z0, double phi0, double R, double slope) {
+ Hep3Vector f = pointOnHelix( trk, xint, d0, z0, phi0, R, slope);
+ Hep3Vector fh = pointOnHelix(xint, d0, z0+_eps, phi0, R, slope);
+ Hep3Vector fmh = pointOnHelix(xint, d0, z0-_eps, phi0, R, slope);
+ Hep3Vector f2h = pointOnHelix(xint, d0, z0+2*_eps, phi0, R, slope);
+ Hep3Vector fm2h = pointOnHelix(xint, d0, z0-2*_eps, phi0, R, slope);
+ Hep3Vector df = this.getNumDer(f2h, fh, f, fmh, fm2h);
+ return df;
+ }
+ public Hep3Vector df_dslope(HelicalTrackFit trk, double xint, double d0, double z0, double phi0, double R, double slope) {
+ Hep3Vector f = pointOnHelix( trk, xint, d0, z0, phi0, R, slope);
+ Hep3Vector fh = pointOnHelix(xint, d0, z0, phi0, R, slope+_eps);
+ Hep3Vector fmh = pointOnHelix(xint, d0, z0, phi0, R, slope-_eps);
+ Hep3Vector f2h = pointOnHelix(xint, d0, z0, phi0, R, slope+2*_eps);
+ Hep3Vector fm2h = pointOnHelix(xint, d0, z0, phi0, R, slope-2*_eps);
+ Hep3Vector df = this.getNumDer(f2h, fh, f, fmh, fm2h);
+ return df;
+ }
+ public Hep3Vector df_dphi0(HelicalTrackFit trk, double xint, double d0, double z0, double phi0, double R, double slope) {
+ Hep3Vector f = pointOnHelix( trk, xint, d0, z0, phi0, R, slope);
+ Hep3Vector fh = pointOnHelix(xint, d0, z0, phi0+_eps, R, slope);
+ Hep3Vector fmh = pointOnHelix(xint, d0, z0, phi0-_eps, R, slope);
+ Hep3Vector f2h = pointOnHelix(xint, d0, z0, phi0+2*_eps, R, slope);
+ Hep3Vector fm2h = pointOnHelix(xint, d0, z0, phi0-2*_eps, R, slope);
+ Hep3Vector df = this.getNumDer(f2h, fh, f, fmh, fm2h);
+ return df;
+ }
+ public Hep3Vector df_dR(HelicalTrackFit trk, double xint, double d0, double z0, double phi0, double R, double slope) {
+ Hep3Vector f = pointOnHelix( trk, xint, d0, z0, phi0, R, slope);
+ Hep3Vector fh = pointOnHelix(xint, d0, z0, phi0, R+_eps, slope);
+ Hep3Vector fmh = pointOnHelix(xint, d0, z0, phi0, R-_eps, slope);
+ Hep3Vector f2h = pointOnHelix(xint, d0, z0, phi0, R+2*_eps, slope);
+ Hep3Vector fm2h = pointOnHelix(xint, d0, z0, phi0, R-2*_eps, slope);
+ Hep3Vector df = this.getNumDer(f2h, fh, f, fmh, fm2h);
+ return df;
+ }
+
+
+
+
+ }
+
+
+
+
+
+
+
+
+
+
//-------------------------------------------
- private boolean _debug;
+
+
+
+ /**
+ *
+ * Nested class to increase encapsulation and save this for future cross-checks
+ */
+ public class OldAlignmentUtils {
+ public OldAlignmentUtils() {}
+ public BasicMatrix calculateLocalHelixDerivatives(HelicalTrackFit trk,double xint) {
+ //get track parameters.
+ double d0 = trk.dca();
+ double z0 = trk.z0();
+ double slope = trk.slope();
+ double phi0 = trk.phi0();
+ double R = trk.R();
+ //strip origin is defined in the tracking coordinate system (x=beamline)
+ double s = HelixUtils.PathToXPlane(trk, xint, 0, 0).get(0);
+ double phi = -s/R + phi0;
+ double[][] dfdq = new double[3][5];
+ //dx/dq
+ //these are wrong for X, but for now it doesn't matter
+ dfdq[0][0] = Math.sin(phi0);
+ dfdq[0][1] = 0;
+ dfdq[0][2] = 0;
+ dfdq[0][3] = d0 * Math.cos(phi0) + R * Math.sin(phi0) - s * Math.cos(phi0);
+ dfdq[0][4] = (phi - phi0) * Math.cos(phi0);
+ double[] mydydq = dydq(R, d0, phi0, xint, s);
+ double[] mydzdq = dzdq(R, d0, phi0, xint, slope, s);
+ for (int i = 0; i < 5; i++) {
+ dfdq[1][i] = mydydq[i];
+ dfdq[2][i] = mydzdq[i];
+ }
+
+ BasicMatrix dfdqGlobal = FillMatrix(dfdq, 3, 5);
+ return dfdqGlobal;
+ }
+ private double[] dydq(double R, double d0, double phi0, double xint, double s) {
+ double[] dy = new double[5];
+ // dy[0] = Math.cos(phi0) + Cot(phi0 - s / R) * Csc(phi0 - s / R) * dsdd0(R, d0, phi0, xint);
+ dy[0] = Math.cos(phi0) - Sec(phi0 - s / R) * Math.tan(phi0 - s / R) * dsdd0(R, d0, phi0, xint);
+ dy[1] = 0;
+ dy[2] = 0;
+ // dy[3] = (-(d0 - R)) * Math.sin(phi0) - R * Cot(phi0 - s / R) * Csc(phi0 - s / R) * (1 - dsdphi(R, d0, phi0, xint) / R);
+ dy[3] = (-(d0 - R)) * Math.sin(phi0) + Sec(phi0 - s / R) * Math.tan(phi0 - s / R) * (R - dsdphi(R, d0, phi0, xint));
+ // dy[4] = -Math.cos(phi0) + Csc(phi0 - s / R) - R * Cot(phi0 - s / R) * Csc(phi0 - s / R) * (s / (R * R) - dsdR(R, d0, phi0, xint) / R);
+ dy[4] = -Math.cos(phi0) + Sec(phi0 - s / R) + (1 / R) * Sec(phi0 - s / R) * Math.tan(phi0 - s / R) * (s - R * dsdR(R, d0, phi0, xint));
+ return dy;
+ }
+ private double[] dzdq(double R, double d0, double phi0, double xint, double slope, double s) {
+ double[] dz = new double[5];
+ dz[0] = slope * dsdd0(R, d0, phi0, xint);
+ dz[1] = 1;
+ dz[2] = s;
+ dz[3] = slope * dsdphi(R, d0, phi0, xint);
+ dz[4] = slope * dsdR(R, d0, phi0, xint);
+ return dz;
+ }
+
+ private double dsdphi(double R, double d0, double phi0, double xint) {
+ double sqrtTerm = Math.sqrt(R * R - Math.pow(((d0 - R) * Math.sin(phi0) + xint), 2));
+ double rsign = Math.signum(R);
+ double dsdphi = R * (sqrtTerm + rsign * d0 * Math.cos(phi0) - rsign * R * Math.cos(phi0)) / sqrtTerm;
+ // if (_DEBUG)
+ // System.out.println("xint = " + xint + "; dsdphi = " + dsdphi);
+ return dsdphi;
+ }
+
+ private double dsdd0(double R, double d0, double phi0, double xint) {
+ double sqrtTerm = Math.sqrt(R * R - Math.pow(((d0 - R) * Math.sin(phi0) + xint), 2));
+ double rsign = Math.signum(R);
+ double dsdd0 = rsign * (R * Math.sin(phi0)) / sqrtTerm;
+ // if (_DEBUG)
+ // System.out.println("xint = " + xint + "; dsdd0 = " + dsdd0);
+ return dsdd0;
+ }
+
+ private double dsdR(double R, double d0, double phi0, double xint) {
+ double sqrtTerm = Math.sqrt(R * R - Math.pow(((d0 - R) * Math.sin(phi0) + xint), 2));
+
+ double rsign = Math.signum(R);
+ double dsdr = (1 / sqrtTerm) * ((-rsign * xint) + (-rsign) * d0 * Math.sin(phi0)
+ + Math.atan2(R * Math.cos(phi0), (-R) * Math.sin(phi0))
+ * sqrtTerm
+ - Math.atan2(rsign * sqrtTerm, xint + (d0 - R) * Math.sin(phi0))
+ * sqrtTerm);
+
+
+ // if (_DEBUG)
+ // System.out.println("xint = " + xint + "; dsdr = " + dsdr);
+ return dsdr;
+ }
+
+ private double Sec(double val) {
+ return 1 / Math.cos(val);
+ }
+
+
+
+ }
+
+
+
}
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