lcsim/src/org/lcsim/util/step
diff -u -r1.4 -r1.5
--- CamKalmanStepper2.java 15 Nov 2008 13:14:14 -0000 1.4
+++ CamKalmanStepper2.java 15 Nov 2008 22:01:29 -0000 1.5
@@ -31,10 +31,12 @@
* a main- and is the (Driver/MainProject))
*/
public class CamKalmanStepper2{
- private static boolean stopTkELow = false;
- private static boolean Debug = false;
- private static boolean ktransport = true;
- private static boolean materialByName = false;
+ private static boolean stopTkELow = false;
+ private static boolean _ProjectiveGeometry = false;
+ private static boolean Debug = false;
+ private static boolean ktransport = true;
+ private static boolean kupDate = false;
+ private static boolean materialByName = false;
private final double kAlmost1 = 0.999;
private final double kAlmost0 = 1E-33;
private final double kdistsmall = 0.000001;
@@ -51,7 +53,6 @@
private double kMass ;
private double _dedx = 1.E-20, _radLength=1.E-20, _density=1.E-19;
private static String _materialName;
- public boolean kupDate = false;
private double [] kRp = new double [6];
private double [][]measCov = new double[3][3];
private double [] nwRp = new double[8];
@@ -134,7 +135,7 @@
* to s new position in a magnetic field b
*@param r the position
*@param b the magnetic field
- *@param matInfo={dedx,radLength/density)
+ *@param matInfo={dedx,radLength/density)
* energy loss by ionization in material & number radiation Length.
*/
public int [] propagateInBarrelByDr(double rToGo, double b,double[] matInfo){
@@ -192,11 +193,11 @@
int nStepsDone = 0; double drStep = dr/nStepsToDo;
//setMatNam(materialName); materialByName=true; REMOVE
for (int i=1;i<=nStepsToDo;i++){
- pm = Math.sqrt(kRp[3]*kRp[3]+kRp[4]*kRp[4]+kRp[5]*kRp[5]);
- setBField1Dim(b); setDeDx(pm, kMass, materialName);setRadLength(materialName);
+ pm = Math.sqrt(kRp[3]*kRp[3]+kRp[4]*kRp[4]+kRp[5]*kRp[5]);
+ setBField1Dim(b); setDeDx(pm, kMass, materialName);setRadLength(materialName);
stepByDr(drStep);
nStepsDone=i;
- if(getStopTkELow()) break;
+ if(getStopTkELow()) break;
}
numSteps[0]=nStepsToDo;numSteps[1]=nStepsDone;
return (numSteps);
@@ -218,8 +219,8 @@
double dsSteps = ds/nStepsToDo; int nStepsDone = 0;
for (int i=1;i<=nStepsToDo;i++) {
// setMatNam(materialName); materialByName=true; REMOVE
- double pm = Math.sqrt(kRp[3]*kRp[3]+kRp[4]*kRp[4]+kRp[5]*kRp[5]);
- setBField1Dim(b); setDeDx(pm, kMass, materialName); setRadLength(materialName);
+ double pm = Math.sqrt(kRp[3]*kRp[3]+kRp[4]*kRp[4]+kRp[5]*kRp[5]);
+ setBField1Dim(b); setDeDx(pm, kMass, materialName); setRadLength(materialName);
stepByDs(dsSteps);
nStepsDone=i;
if(getStopTkELow()) break;
@@ -284,7 +285,7 @@
int[]numSteps = new int[2];
setBField1Dim(b); _dedx=materialInfo[0]; _radLength=materialInfo[1];
int nStepsDone = 0 ; double dsSteps = ds/nStepsToDo;
- for (int i=1;i<=nStepsToDo;i++) {
+ for (int i=1;i<=nStepsToDo;i++) {
stepByDs(dsSteps);
nStepsDone=i;
if(getStopTkELow()) break;
@@ -324,9 +325,9 @@
// ds is negative
double pm;
int nStepsToDo = klb[0], nStepsDone = klb[1]; double dsSteps = ds/nStepsToDo;
- for (int i=nStepsDone;i>0;i--) {
- pm = Math.sqrt(kRp[3]*kRp[3]+kRp[4]*kRp[4]+kRp[5]*kRp[5]);
- setDeDx(pm, kMass, materialName); setRadLength(materialName); setBField1Dim(b);
+ for (int i=nStepsDone;i>0;i--) {
+ pm = Math.sqrt(kRp[3]*kRp[3]+kRp[4]*kRp[4]+kRp[5]*kRp[5]);
+ setDeDx(pm, kMass, materialName); setRadLength(materialName); setBField1Dim(b);
stepByDs(dsSteps);
}
}
@@ -361,12 +362,12 @@
*@param materialName
*/
public void propagateInBarrelBackByDr(double r, double b,String materialName, int [] klb){
- // ds is negative
+ // ds is negative
double drToGo = r-Math.sqrt(kRp[0]*kRp[0]+kRp[1]*kRp[1]); double pm;
int nStepsToDo = klb[0], nStepsDone=klb[1]; double drSteps = drToGo/nStepsToDo;
for (int i=nStepsDone;i>0;i--) {
- pm = Math.sqrt(kRp[3]*kRp[3]+kRp[4]*kRp[4]+kRp[5]*kRp[5]);
- setBField1Dim(b); setDeDx(pm, kMass, materialName); setRadLength(materialName);
+ pm = Math.sqrt(kRp[3]*kRp[3]+kRp[4]*kRp[4]+kRp[5]*kRp[5]);
+ setBField1Dim(b); setDeDx(pm, kMass, materialName); setRadLength(materialName);
stepByDr(drSteps);
}
}
@@ -407,7 +408,7 @@
_dedx = materialInfo[0]; _radLength= materialInfo[1] ; setBField1Dim(b);
stepByDz(dz/nStepsToDo);
nStepsDone=i;
- if(getStopTkELow()) break;
+ if(getStopTkELow()) break;
}
numSteps[0] = nStepsToDo; numSteps[1] = nStepsDone;
return (numSteps);
@@ -443,8 +444,8 @@
double dz = z-kRp[2]; double pm;
double dzSteps = dz/nStepsToDo; int nStepsDone=nStepsToDo;
for (int i = 1;i<=nStepsToDo;i++){
- pm= Math.sqrt(kRp[3]*kRp[3]+kRp[4]*kRp[4]+kRp[5]*kRp[5]);
- setBField1Dim(b); setDeDx(pm, kMass, materialName); setRadLength(materialName);
+ pm= Math.sqrt(kRp[3]*kRp[3]+kRp[4]*kRp[4]+kRp[5]*kRp[5]);
+ setBField1Dim(b); setDeDx(pm, kMass, materialName); setRadLength(materialName);
stepByDz(dzSteps);
nStepsDone=i;
if(getStopTkELow()) break;
@@ -488,8 +489,8 @@
int nStepsToDo = klb[0], nStepsDone=klb[1]; double dzSteps = dzToGo/nStepsToDo;
for (int i = nStepsDone;i>0;i--) {
//setMatNam(materialName); materialByName=true; REMOVE
- pm = Math.sqrt(kRp[3]*kRp[3]+kRp[4]*kRp[4]+kRp[5]*kRp[5]);
- setBField1Dim(b); setDeDx(pm, kMass, materialName); setRadLength(materialName);
+ pm = Math.sqrt(kRp[3]*kRp[3]+kRp[4]*kRp[4]+kRp[5]*kRp[5]);
+ setBField1Dim(b); setDeDx(pm, kMass, materialName); setRadLength(materialName);
stepByDz(dzSteps);
}
}
@@ -502,11 +503,11 @@
* Multiple Scattering
*/
public boolean stepByDs(double ds){ // one step by ds
-
+
if(Debug&&ncount1<100)System.out.println("ds at entry of stepByDs"+ds);
double[]wk = new double[3];
double pIn = Math.sqrt(kRp[3]*kRp[3]+ kRp[4]*kRp[4]+kRp[5]*kRp[5]) ;
- double E = Math.sqrt(pIn*pIn+kMass*kMass) ;
+ double E = Math.sqrt(pIn*pIn+kMass*kMass) ;
kB = getBField1Dim();
double k = kB2C*kQ*kB/pIn ;
double dpdx=ds*getDeDx()*(E/pIn) ;
@@ -532,19 +533,19 @@
double npzz = pzz;
double mpxx = npxx - dpdx*npxx/pIn ;
double mpyy = npyy - dpdx*npyy/pIn ;
- double mpzz = npzz - dpdx*npzz/pIn ;
+ double mpzz = npzz - dpdx*npzz/pIn ;
double pOut = Math.sqrt(mpxx*mpxx+mpyy*mpyy+mpzz*mpzz);
double pMean = (pIn+pOut)/2;
if(dpdx > pMean){
//System.out.println("!!!! StepByDs !!!!! : dpdx> p -track stopped not enough energy dpdx="+dpdx+" pMean="+pMean);
return stopTkELow=true ;
- }
+ }
double twoPmean=(pMean*2);
kRp[3] = mpxx; kRp[4] = mpyy; kRp[5] = mpzz;
kRp[0] = xxx + (ds/twoPmean)*(kRp[3]+pxx);
kRp[1] = yyy + (ds/twoPmean)*(kRp[4]+pyy);
kRp[2] = zzz + (ds/twoPmean)*(kRp[5]+pzz ) ;
-// *** Equations of motions in a matriciel format = transport Matrix
+// *** Equations of motions in a matriciel format = transport Matrix
fifi[0][0] = -((0.5*k*k)/(1+0.25*k*k))*(1.-(dpdx/pIn)) ;
fifi[0][1] = k/(1.+0.25*k*k)*(1.-(dpdx/pIn)) ;
fifi[1][0] = (-k/(1.+0.25*k*k))*(1.-(dpdx/pIn)) ;
@@ -563,7 +564,7 @@
fi = (fi.plus(II)).plus(IIdpdx);
prb = fi.times(pra);
//*** Check:Mtx against equations of motion
- if(Debug&&ncount1%10==0){ //Inverted location of positions and momenta in vectors prb and in kRp
+ if(Debug&&ncount1%10==0){ //Inverted location of positions and momenta in vectors prb and in kRp
System.out.println("prb[3]="+prb[3]+"prb[4]="+prb[4]+"prb[5]="+prb[5]); // momentum prb[0,1,2], position prb[3,4,5]
System.out.println("kRp[0]="+kRp[0]+"kRp[1]="+kRp[1]+"kRp[2]="+kRp[2]); // momentum kRp[3,4,5], position kRp[0,1,2]
System.out.println("prb[0]="+prb[0]+"prb[1]="+prb[1]+"prb[2]="+prb[2]);
@@ -619,9 +620,15 @@
*@param mstate a 3dim array with the measure position
*@param mCov the measurement error Matrix
*/
- public boolean upDate(double []mState,double[][]mCov){
+ public boolean upDate(double []mStateIn,double[][]mCov){
//*** update vector with current extrapolation
if(getStopTkELow()){ return (kupDate = false);}
+ double [] mState = new double[3];
+ if (_ProjectiveGeometry)
+ mState = rPhiThetaToXyz(mStateIn[0], mStateIn[1], mState[2]);
+ else for(int i=0; i<3;i++){
+ double measState=mStateIn[i]; mState[i]=measState ;
+ }
double [] cState = new double[6];
for(int i=0;i<3;i++) {double pxyztmp= kRp[i+3]; cState[i] = pxyztmp;} // px,py,pz
for(int i=3;i<6;i++) {double xyztmp = kRp[i-3]; cState[i] = xyztmp;} // x,y,z
@@ -631,7 +638,7 @@
M3x6tmp = Hk.times(PkMinus);
M3x3tmp = M3x6tmp.times(Hk.transpose());
Matrix Meas = new Matrix(measCov,3,3);
- Matrix KkPart=M3x3tmp.plus(Meas);
+ Matrix KkPart = M3x3tmp.plus(Meas);
Matrix Kk = (PkMinus.times(Hk.transpose())).times(KkPart.inverse());
M6x6tmp = Matrix.identity(6,6).minus(Kk.times(Hk));
PkPlus = M6x6tmp.times(PkMinus);
@@ -691,7 +698,7 @@
}
/** set the radLength using the materialCalculator of Jeremy McCormick
*@param materialName
- */
+ */
public void setRadLength(String materialName){
MaterialManager manager = MaterialManager.instance();
double radLength=1.E-20;
@@ -709,7 +716,7 @@
*/
public void setRadLength(double xxx){_radLength=xxx;}
/** Brings the value
- */
+ */
public double getRadLength() {
if(_radLength<0.) return 1.e-20;
return(_radLength); // in cm
@@ -756,8 +763,8 @@
for(int i=0; i<6; i++){
for(int j = 0;j<6;j++){
if(i ==j)kCovariance[i][j]=covdiag;
- if(i !=j)kCovariance[i][j] = 0.;
- }
+ if(i !=j)kCovariance[i][j] = 0.;
+ }
}
double zero=0.;
PkPlus= PkPlus.times(zero).plus(II.times(covdiag));
@@ -820,6 +827,40 @@
wk[2]=2*pabs*thetak[2];
return wk;
}
+/** rPhiThetaToXyz transfer from projective to cartesian
+*
+*/
+ public double[] rPhiThetaToXyz(double r,double theta,double phi)
+ {
+ double [] xyz = new double [2];
+ xyz[0] = r*Math.cos(theta)*Math.cos(phi);
+ xyz[1] = r*Math.cos(theta)*Math.cos(phi);
+ xyz[2] = r*Math.sin(phi);
+ return xyz;
+ }
+/** Cartesian to projective
+* From previous stepper
+*/
+ public double xyzToPhi(double[] r)
+ {
+//*** Determine phi corresponding to this Cartesian coordinate
+ double phi;
+ double r1=r[1]; double r0=r[0];
+ phi = Math.atan2(r1, r0);
+ if (phi < 0.) phi = phi + 2*Math.PI;
+ return(phi);
+ }
+
+ public double xyzToTheta(double[] r)
+ {
+//*** Determine theta corresponding to this Cartesian coordinate
+ double rho, theta;
+ double r1=r[1]/10; double r0=r[0]/10; double r2=r[2]/10;
+ rho = Math.sqrt(r0*r0 + r1*r1);
+ theta = Math.atan2(rho, r2);
+ if (theta < 0.) theta = theta + Math.PI;
+ return(theta);
+ }
/** Set Boolean Indicator If Energy Left
*/
public void setStopTkELow(boolean b) {
@@ -832,7 +873,21 @@
*/
public boolean getStopTkELow() {
return stopTkELow;}
-
+ /** Set Boolean Indicator If projective geometry
+ */
+ public void setProjectiveGeom(boolean b) {
+ _ProjectiveGeometry=b;}
+ /** Reset Boolean Indicator Energy Left for the step
+ */
+ public void resetProjective() {
+ setProjectiveGeom(false);}
+ /** Return Boolean Indicator Energy Left for the step
+ */
+ public boolean getProjectiveGeom() {
+ return _ProjectiveGeometry;}
+ /**@ return double [][] kCovariance
+ * double vector with covariant Matrix ele,ments
+ */
public double[][] getKCovariance() {
return kCovariance;
}
