00001 #include "VertexFit/HTrackParameter.h"
00002 #include "VertexFit/WTrackParameter.h"
00003 #include "VertexFit/BField.h"
00004 #include "CLHEP/Units/PhysicalConstants.h"
00005 using namespace CLHEP;
00006 const double alpha = -0.00299792458;
00007
00008
00009 HTrackParameter::HTrackParameter() {
00010 m_trackID = -1;
00011 m_partID = -1;
00012 m_hel = HepVector(5, 0);
00013 m_eHel = HepSymMatrix(5, 0);
00014 }
00015
00016 HTrackParameter::HTrackParameter(const HTrackParameter &htrk) {
00017 m_trackID = htrk.m_trackID;
00018 m_partID = htrk.m_partID;
00019 m_hel = htrk.m_hel;
00020 m_eHel = htrk.m_eHel;
00021 }
00022
00023 HTrackParameter &HTrackParameter :: operator = (const HTrackParameter &htrk) {
00024 m_trackID = htrk.m_trackID;
00025 m_partID = htrk.m_partID;
00026 m_hel = htrk.m_hel;
00027 m_eHel = htrk.m_eHel;
00028 return (*this);
00029 }
00030
00031
00032
00033
00034
00035
00036 HTrackParameter::HTrackParameter(const HepVector h, const HepSymMatrix eH,
00037 const int trackid, const int partid) {
00038
00039 m_hel = HepVector(5, 0);
00040 m_eHel = HepSymMatrix(5, 0);
00041
00042 m_trackID = trackid;
00043 m_partID = partid;
00044 m_hel = h;
00045 m_eHel = eH;
00046
00047 }
00048
00049
00050
00051
00052
00053
00054 HTrackParameter::HTrackParameter(const HepVector h, const double eH[],
00055 const int trackid, const int partid) {
00056
00057 m_hel = HepVector(5, 0);
00058 m_eHel = HepSymMatrix(5, 0);
00059
00060 m_trackID = trackid;
00061 m_partID = partid;
00062 m_hel = h;
00063 int k = 0;
00064 for(int i = 0; i < 5; i++){
00065 for(int j = 0; j < 5; j++) {
00066 m_eHel[i][j] = eH[k];
00067 m_eHel[j][i] = eH[k];
00068 k++;
00069 }
00070 }
00071 }
00072
00073
00074
00075
00076
00077 HTrackParameter::HTrackParameter(const WTrackParameter wtrk) {
00078
00079 HepVector p(3, 0);
00080 HepVector x(3, 0);
00081 for(int i = 0; i < 3; i++) {
00082 p[i] = wtrk.w()[i];
00083 x[i] = wtrk.w()[i+4];
00084 }
00085
00086 HTrackParameter htrk(wtrk.charge(), p, x);
00087 HepMatrix A(5, 3, 0);
00088 HepMatrix B(5, 3, 0);
00089
00090 A = htrk.dHdx(p, x);
00091 B = htrk.dHdp(p, x);
00092 HepMatrix T(5, 7, 0);
00093 for(int i = 0; i < 5; i++) {
00094 for(int j = 0; j < 3; j++){
00095 T[i][j] = B[i][j];
00096 T[i][j+4] = A[i][j];
00097 }
00098 }
00099
00100 HepSymMatrix eH(5, 0);
00101 eH = (wtrk.Ew()).similarity(T);
00102 int m_trackID = -1;
00103 double mass = (wtrk.p()).m();
00104 int m_partID = -1;
00105 for(int i = 0; i < 5; i++) {
00106 if(fabs(mass - xmass(i)) < 0.01) {
00107 m_partID = i;
00108 break;
00109 }
00110 }
00111
00112
00113 htrk.setEHel(eH);
00114 m_hel = htrk.hel();
00115 m_eHel = htrk.eHel();
00116 }
00117
00118
00119
00120
00121
00122 double HTrackParameter::radius() const {
00123 double bField = VertexFitBField::instance()->getBFieldZ(x3());
00124 int charge = m_hel[2] > 0 ? +1: -1;
00125 double a = alpha * bField * charge;
00126 double pxy = charge/m_hel[2];
00127 return (pxy/a);
00128 }
00129
00130 HepPoint3D HTrackParameter::center() const {
00131 double bField = VertexFitBField::instance()->getBFieldZ(x3());
00132 int charge = m_hel[2] > 0 ? +1: -1;
00133 double a = alpha * bField * charge;
00134 double pxy = charge/m_hel[2];
00135 double rad = pxy/a;
00136 double x = (m_hel[0] + rad) * cos(m_hel[1]);
00137 double y = (m_hel[0] + rad) * sin(m_hel[1]);
00138 double z = 0.0;
00139 return HepPoint3D(x, y, z);
00140 }
00141
00142
00143
00144
00145
00146 HTrackParameter::HTrackParameter(const int charge, const HepVector p, const HepVector vx) {
00147 m_trackID = -1;
00148 m_partID = -1;
00149 m_hel = HepVector(5, 0);
00150 m_eHel = HepSymMatrix(5, 0);
00151
00152 HepPoint3D xyz(vx[0], vx[1], vx[2]);
00153 double bField = VertexFitBField::instance()->getBFieldZ(xyz);
00154
00155
00156 double a = alpha * bField * charge;
00157 double px = p[0];
00158 double py = p[1];
00159 double pz = p[2];
00160
00161 double x = vx[0];
00162 double y = vx[1];
00163 double z = vx[2];
00164
00165 double pxy = sqrt(px*px+py*py);
00166 double T = sqrt((px+a*y)*(px+a*y)+(py-a*x)*(py-a*x));
00167 double J= (x*px+y*py)/T*a/pxy;
00168
00169 double drho = (pxy/a)-(T/a);
00170 double phi0 = fmod((4*CLHEP::pi)+atan2(0-px-a*y, py-a*x), (2*CLHEP::pi));
00171 double kappa = charge/pxy;
00172 double dz = z - (pz/a) *asin(J);
00173 double lambda = pz/pxy;
00174
00175 m_hel[0] = drho; m_hel[1] = phi0; m_hel[2] = kappa;
00176 m_hel[3] = dz; m_hel[4] = lambda;
00177 }
00178
00179
00180
00181
00182
00183 HepMatrix HTrackParameter::dHdx(const HepVector p, const HepVector vx){
00184 HepPoint3D xyz(vx[0], vx[1], vx[2]);
00185 double bField = VertexFitBField::instance()->getBFieldZ(xyz);
00186
00187
00188 double a = alpha * bField * charge();
00189 double px = p[0];
00190 double py = p[1];
00191 double pz = p[2];
00192
00193 double x = vx[0];
00194 double y = vx[1];
00195
00196
00197
00198 double T = sqrt((px+a*y)*(px+a*y)+(py-a*x)*(py-a*x));
00199
00200
00201 HepMatrix m_A(5, 3, 0);
00202
00203 m_A[0][0] = (py-a*x)/T;
00204 m_A[0][1] = 0 -(px + a*y)/T;
00205 m_A[1][0] = 0- a*(px + a*y)/T/T;
00206 m_A[1][1] = 0 - a * (py - a*x)/T/T;
00207 m_A[3][0] = 0 - (pz/T)*(px + a*y)/T;
00208 m_A[3][1] = 0 - (pz/T)*(py - a*x)/T;
00209 m_A[3][2] = 1;
00210
00211 return m_A;
00212 }
00213
00214
00215
00216
00217
00218 HepMatrix HTrackParameter::dHdp(const HepVector p, const HepVector vx){
00219 HepPoint3D xyz(vx[0], vx[1], vx[2]);
00220 double bField = VertexFitBField::instance()->getBFieldZ(xyz);
00221
00222 double a = alpha * bField * charge();
00223 double px = p[0];
00224 double py = p[1];
00225 double pz = p[2];
00226
00227 double x = vx[0];
00228 double y = vx[1];
00229
00230
00231 double pxy = sqrt(px*px+py*py);
00232 double T = sqrt((px+a*y)*(px+a*y)+(py-a*x)*(py-a*x));
00233 double J= (x*px+y*py)/T*a/pxy;
00234
00235 HepMatrix m_B(5, 3, 0);
00236
00237 m_B[0][0] = (px/pxy - (px+a*y)/T)/a;
00238 m_B[0][1] = (py/pxy - (py-a*x)/T)/a;
00239 m_B[1][0] = 0 -(py-a*x)/T/T;
00240 m_B[1][1] = (px + a*y)/T/T;
00241 m_B[2][0] = 0-charge() *px/pxy/pxy/pxy;
00242 m_B[2][1] = 0-charge() *py/pxy/pxy/pxy;
00243 m_B[3][0] = (pz/a)*(py/pxy/pxy-(py-a*x)/T/T);
00244 m_B[3][1] = 0-(pz/a)*(px/pxy/pxy-(px+a*y)/T/T);
00245 m_B[3][2] = 0 - asin(J)/a;
00246 m_B[4][0] = 0 - (px/pxy)*(pz/pxy)/pxy;
00247 m_B[4][1] = 0 - (py/pxy)*(pz/pxy)/pxy;
00248 m_B[4][2] = 1/pxy;
00249
00250 return m_B;
00251 }
00252
00253
00254
00255
00256
00257 WTrackParameter HTrackParameter::wTrack(const double mass) const {
00258
00259 WTrackParameter wtrk;
00260 wtrk.setCharge(charge());
00261 HepVector w(7,0);
00262 HepMatrix dWdh(7, 5, 0);
00263
00264 double ptrk = p3().rho(); double E = sqrt(ptrk*ptrk + mass * mass);
00265 double px = p3().x();
00266 double py = p3().y();
00267 double pz = p3().z();
00268 double x0 = x3().x();
00269 double y0 = x3().y();
00270 double z0 = x3().z();
00271 w[0] = px; w[1] = py; w[2] = pz; w[3] = E;
00272 w[4] = x0; w[5] = y0; w[6] = z0;
00273 wtrk.setW(w);
00274 dWdh[0][1] = -py; dWdh[0][2] = 0 - px/kappa();
00275 dWdh[1][1] = px; dWdh[1][2] = 0 - py/kappa();
00276 dWdh[2][2] = 0 - pz/kappa(); dWdh[2][4] = charge()/kappa();
00277 dWdh[3][2] = 0 - (1 + lambda() * lambda())/ E / kappa() / kappa() / kappa();
00278 dWdh[3][4] = lambda() / E / kappa() / kappa();
00279 dWdh[4][0] = cos(phi0()); dWdh[4][1] = 0 - y0;
00280 dWdh[5][0] = sin(phi0()); dWdh[5][1] = x0;
00281 dWdh[6][3] = 1;
00282 HepSymMatrix Ew(7, 0);
00283 Ew = m_eHel.similarity(dWdh);
00284 wtrk.setEw(Ew);
00285 return wtrk;
00286 }
00287
00288 WTrackParameter HTrackParameter::wTrack() const {
00289 WTrackParameter wtrk;
00290 if(m_partID > -1 && m_partID < 5) {
00291 double mass = xmass(m_partID);
00292 wtrk = wTrack(mass);
00293 }
00294 return wtrk;
00295 }
00296
00297
00298
00299
00300
00301 double HTrackParameter::xmass(int n) const {
00302 double mass[5] = {0.000511, 0.105658, 0.139570,0.493677, 0.938272};
00303 if(n < 0 || n >=5) return 0.0;
00304 return mass[n];
00305 }
00306
00307
00308
00309
00310
00311 HepPoint3D HTrackParameter::positionTwoHelix(const HTrackParameter htrk) const{
00312 double bField1 = VertexFitBField::instance()->getBFieldZ(x3());
00313 double bField2 = VertexFitBField::instance()->getBFieldZ(htrk.x3());
00314
00315 HepPoint3D pos1 = x3();
00316 HepPoint3D pos2 = htrk.x3();
00317 double rad1 = radius();
00318 double rad2 = htrk.radius();
00319 HepPoint3D xc1 = center();
00320 HepPoint3D xc2 = htrk.center();
00321
00322
00323
00324
00325
00326
00327
00328
00329
00330
00331
00332
00333
00334
00335 double xt = xc2.x() - xc1.x();
00336 double yt = xc2.y() - xc1.y();
00337 if(xt == 0 && yt == 0) return HepPoint3D(999,999,999);
00338 double rt = rad1*rad1-rad2*rad2-xc1.perp2()+xc2.perp2();
00339 double x1, y1, x2, y2;
00340 if( xt != 0) {
00341 double a = 1 + yt*yt/(xt*xt);
00342 if(a == 0) return HepPoint3D(999,999,999);
00343 double b = 2*xc1.x()*yt/xt-yt*rt/(xt*xt)-2*xc1.y();
00344 double c = rt*rt/(4*xt*xt)-xc1.x()*rt/xt-rad1*rad1+xc1.perp2();
00345 double d = b*b - 4 * a * c;
00346 if( d < 0) return HepPoint3D(999,999,999);
00347 d = sqrt(d);
00348
00349
00350 y1 = (-b + d)/(2*a);
00351 x1 = (rt - 2 * yt * y1)/(2*xt);
00352
00353 y2 = (-b - d)/(2*a);
00354 x2 = (rt - 2 * yt * y2)/(2*xt);
00355 } else {
00356 double a = 1 + xt*xt/(yt*yt);
00357 if(a == 0) return HepPoint3D(999,999,999);
00358 double b = 2*xc1.y()*xt/yt-xt*rt/(yt*yt)-2*xc1.x();
00359 double c = rt*rt/(4*yt*yt)-xc1.y()*rt/yt-rad1*rad1+xc1.perp2();
00360 double d = b*b - 4 * a * c;
00361 if( d < 0) return HepPoint3D(999,999,999);
00362 d = sqrt(d);
00363
00364
00365 x1 = (-b + d)/(2*a);
00366 y1 = (rt - 2 * xt * x1)/(2*yt);
00367
00368 x2 = (-b - d)/(2*a);
00369 y2 = (rt - 2 * xt * x2)/(2*yt);
00370 }
00371
00372 double z1[2], z2[2], J1[2], J2[2];
00373
00374
00375 double a1 = alpha * bField1 * charge();
00376 double a2 = alpha * bField2 * htrk.charge();
00377
00378 J1[0] = a1*((x1-x3().x())*p3().x()+(y1-x3().y())*p3().y())/p3().perp2();
00379 J1[1] = a2*((x1-htrk.x3().x())*htrk.p3().x()+(y1-htrk.x3().y())*htrk.p3().y())/htrk.p3().perp2();
00380 z1[0] = x3().z()+p3().z()/a1*asin(J1[0]);
00381 z1[1] = htrk.x3().z()+htrk.p3().z()/a2*asin(J1[1]);
00382
00383 J2[0] = a1*((x2-x3().x())*p3().x()+(y2-x3().y())*p3().y())/p3().perp2();
00384 J2[1] = a2*((x2-htrk.x3().x())*htrk.p3().x()+(y2-htrk.x3().y())*htrk.p3().y())/htrk.p3().perp2();
00385 z2[0] = x3().z()+p3().z()/a2*asin(J2[0]);
00386 z2[1] = htrk.x3().z()+htrk.p3().z()/a2*asin(J2[1]);
00387
00388
00389
00390 if(fabs(z1[0]-z1[1]) < fabs(z2[0]-z2[1])) {
00391 return HepPoint3D(x1, y1, 0.5*(z1[0]+z1[1]));
00392 } else {
00393 return HepPoint3D(x2, y2, 0.5*(z2[0]+z2[1]));
00394 }
00395 }
00396
00397
00398
00399
00400
00401
00402
00403 HepPoint3D HTrackParameter::positionPlane(const double zp) const {
00404 return HepPoint3D(999,999,999);
00405 }
00406
00407
00408
00409
00410
00411 HepPoint3D HTrackParameter::positionCylinder(const double R) const {
00412 return HepPoint3D(999,999,999);
00413 }
00414
00415
00416
00417
00418
00419 HepPoint3D HTrackParameter::positionCone() const {
00420 return HepPoint3D(999,999,999);
00421 }
00422
00423
00424
00425
00426
00427 double HTrackParameter::minDistanceTwoHelix(const HTrackParameter G, HepPoint3D &mpos) {
00428 int ifail;
00429 double h = radius();
00430 double g = G.radius();
00431 double phiH0 = fmod((4*CLHEP::pi)+atan2(p3().y(), p3().x()), (2*CLHEP::pi));
00432 double phiG0 = fmod((4*CLHEP::pi)+atan2(G.p3().y(), G.p3().x()), (2*CLHEP::pi));
00433 double lamH = lambda();
00434 double lamG = G.lambda();
00435 double a = x3().x() - G.x3().x() + g*sin(phiG0) - h*sin(phiH0);
00436 double b = x3().y() - G.x3().y() - g*cos(phiG0) + h*cos(phiH0);
00437 double c1 = h*lamH*lamH;
00438 double c2 = 0 - g*lamG*lamG;
00439 double d1 = 0 - g*lamG*lamH;
00440 double d2 = h*lamG*lamH;
00441 double e1 = lamH*(x3().z() - G.x3().z() - h*phiH0*lamH + g*phiG0*lamG);
00442 double e2 = lamG*(x3().z() - G.x3().z() - h*phiH0*lamH + g*phiG0*lamG);
00443
00444 HepVector phiE(2, 0);
00445 phiE[0] = phiH0;
00446 phiE[1] = phiG0;
00447 for(int iter = 0; iter < 100; iter++) {
00448 HepVector z(2, 0);
00449 HepSymMatrix Omega(2, 0);
00450 double phiH = phiE[0];
00451 double phiG = phiE[1];
00452 z[0] = cos(phiH)*(a-g*sin(phiG))+sin(phiH)*(b+g*cos(phiG))+c1*phiH+d1*phiG+e1;
00453 z[1] = cos(phiG)*(a+h*sin(phiH))+sin(phiG)*(b-h*cos(phiH))+c2*phiG+d2*phiH+e2;
00454 Omega[0][0] = 0-sin(phiH)*(a-g*sin(phiG))+cos(phiH)*(b+g*cos(phiG))+c1;
00455 Omega[0][1] = -g*cos(phiH)*cos(phiG)-g*sin(phiH)*sin(phiG)+d1;
00456 Omega[1][0] =h*cos(phiH)*cos(phiG)+h*sin(phiG)*sin(phiH)+d2;
00457 Omega[1][1] = -sin(phiG)*(a+h*sin(phiH))+cos(phiG)*(b-h*cos(phiH))+c2;
00458 HepVector phi(2, 0);
00459 phi = phiE - Omega.inverse(ifail) * z;
00460 if((fabs(phi[0]-phiE[0])<1E-3) && (fabs(phi[1]-phiE[1])<1E-3)) {
00461 phiE = phi;
00462
00463 break;
00464 }
00465 phiE = phi;
00466 }
00467
00468 double phiH = phiE[0];
00469 double phiG = phiE[1];
00470 HepPoint3D posH = HepPoint3D(x3().x()+h*(sin(phiH)-sin(phiH0)), x3().y()+h*(cos(phiH0)-cos(phiH)), x3().z()+lamH*(phiH-phiH0));
00471 HepPoint3D posG = HepPoint3D(G.x3().x()+g*(sin(phiG)-sin(phiG0)), G.x3().y()+g*(cos(phiG0)-cos(phiG)), G.x3().z()+lamG*(phiG-phiG0));
00472 double dis = (posH-posG).rho();
00473 mpos = 0.5*(posH+posG);
00474 return dis;
00475 }
00476
00477
00478