00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014 #include <cmath>
00015 #include <cfloat>
00016 #include "VertexFit/Helix.h"
00017 #include "VertexFit/BField.h"
00018
00019
00020
00021
00022
00023
00024
00025 const double
00026 M_PI2 = 2. * M_PI;
00027
00028 const double
00029 M_PI4 = 4. * M_PI;
00030
00031 const double
00032 M_PI8 = 8. * M_PI;
00033
00034 const double
00035 VFHelix::ConstantAlpha = 333.564095;
00036
00037 VFHelix::VFHelix(const HepPoint3D & pivot,
00038 const HepVector & a,
00039 const HepSymMatrix & Ea)
00040 : m_bField(10.0),
00041 m_alpha(333.564095),
00042 m_pivot(pivot),
00043 m_a(a),
00044 m_Ea(Ea),
00045 m_matrixValid(true)
00046 {
00047
00048
00049 m_bField = 10 * VertexFitBField::instance()->getBFieldZRef();
00050 m_alpha = 10000. / 2.99792458 / m_bField;
00051
00052 updateCache();
00053 }
00054
00055 VFHelix::VFHelix(const HepPoint3D & pivot,
00056 const HepVector & a)
00057 : m_bField(10.0),
00058 m_alpha(333.564095),
00059 m_pivot(pivot),
00060 m_a(a),
00061 m_Ea(HepSymMatrix(5,0)),
00062 m_matrixValid(false)
00063 {
00064
00065 m_bField = 10 * VertexFitBField::instance()->getBFieldZRef();
00066 m_alpha = 10000. / 2.99792458 / m_bField;
00067
00068 updateCache();
00069 }
00070
00071 VFHelix::VFHelix(const HepPoint3D & position,
00072 const Hep3Vector & momentum,
00073 double charge)
00074 : m_bField(10.0),
00075 m_alpha(333.564095),
00076 m_pivot(position),
00077 m_a(HepVector(5,0)),
00078 m_Ea(HepSymMatrix(5,0)),
00079 m_matrixValid(false)
00080 {
00081
00082 m_bField = 10 * VertexFitBField::instance()->getBFieldZRef();
00083 m_alpha = 10000. / 2.99792458 / m_bField;
00084
00085 m_a[0] = 0.;
00086 m_a[1] = fmod(atan2(- momentum.x(), momentum.y())
00087 + M_PI4, M_PI2);
00088 m_a[3] = 0.;
00089 double perp(momentum.perp());
00090 if (perp != 0.0) {
00091 m_a[2] = charge / perp;
00092 m_a[4] = momentum.z() / perp;
00093 }
00094 else {
00095 m_a[2] = charge * (DBL_MAX);
00096 if (momentum.z() >= 0) {
00097 m_a[4] = (DBL_MAX);
00098 } else {
00099 m_a[4] = -(DBL_MAX);
00100 }
00101 }
00102
00103 updateCache();
00104 }
00105
00106 VFHelix::~VFHelix() {
00107 }
00108
00109 HepPoint3D
00110 VFHelix::x(double phi) const {
00111
00112
00113
00114
00115
00116
00117
00118
00119 double x = m_pivot.x() + m_ac[0] * m_cp + m_r * (m_cp - cos(m_ac[1] +phi));
00120 double y = m_pivot.y() + m_ac[0] * m_sp + m_r * (m_sp - sin(m_ac[1] +phi));
00121 double z = m_pivot.z() + m_ac[3] - m_r * m_ac[4] * phi;
00122
00123 return HepPoint3D(x, y, z);
00124 }
00125
00126 double *
00127 VFHelix::x(double phi, double p[3]) const {
00128
00129
00130
00131
00132
00133
00134
00135
00136 p[0] = m_pivot.x() + m_ac[0] * m_cp + m_r * (m_cp - cos(m_ac[1] + phi));
00137 p[1] = m_pivot.y() + m_ac[0] * m_sp + m_r * (m_sp - sin(m_ac[1] + phi));
00138 p[2] = m_pivot.z() + m_ac[3] - m_r * m_ac[4] * phi;
00139
00140 return p;
00141 }
00142
00143 HepPoint3D
00144 VFHelix::x(double phi, HepSymMatrix & Ex) const {
00145 double x = m_pivot.x() + m_ac[0] * m_cp + m_r * (m_cp - cos(m_ac[1] +phi));
00146 double y = m_pivot.y() + m_ac[0] * m_sp + m_r * (m_sp - sin(m_ac[1] +phi));
00147 double z = m_pivot.z() + m_ac[3] - m_r * m_ac[4] * phi;
00148
00149
00150
00151
00152
00153
00154
00155
00156
00157
00158 if (m_matrixValid) Ex = m_Ea.similarity(delXDelA(phi));
00159 else Ex = m_Ea;
00160
00161 return HepPoint3D(x, y, z);
00162 }
00163
00164 Hep3Vector
00165 VFHelix::momentum(double phi) const {
00166
00167
00168
00169
00170
00171
00172
00173
00174
00175
00176 double pt = fabs(m_pt);
00177 double px = - pt * sin(m_ac[1] + phi);
00178 double py = pt * cos(m_ac[1] + phi);
00179 double pz = pt * m_ac[4];
00180
00181 return Hep3Vector(px, py, pz);
00182 }
00183
00184 Hep3Vector
00185 VFHelix::momentum(double phi, HepSymMatrix & Em) const {
00186
00187
00188
00189
00190
00191
00192
00193
00194
00195
00196 double pt = fabs(m_pt);
00197 double px = - pt * sin(m_ac[1] + phi);
00198 double py = pt * cos(m_ac[1] + phi);
00199 double pz = pt * m_ac[4];
00200
00201 if (m_matrixValid) Em = m_Ea.similarity(delMDelA(phi));
00202 else Em = m_Ea;
00203
00204 return Hep3Vector(px, py, pz);
00205 }
00206
00207 HepLorentzVector
00208 VFHelix::momentum(double phi, double mass) const {
00209
00210
00211
00212
00213
00214
00215
00216
00217
00218
00219
00220 double pt = fabs(m_pt);
00221 double px = - pt * sin(m_ac[1] + phi);
00222 double py = pt * cos(m_ac[1] + phi);
00223 double pz = pt * m_ac[4];
00224 double E = sqrt(pt*pt*(1.+m_ac[4]*m_ac[4])+mass*mass);
00225
00226 return HepLorentzVector(px, py, pz, E);
00227 }
00228
00229
00230 HepLorentzVector
00231 VFHelix::momentum(double phi, double mass, HepSymMatrix & Em) const {
00232
00233
00234
00235
00236
00237
00238
00239
00240
00241
00242
00243 double pt = fabs(m_pt);
00244 double px = - pt * sin(m_ac[1] + phi);
00245 double py = pt * cos(m_ac[1] + phi);
00246 double pz = pt * m_ac[4];
00247 double E = sqrt(pt*pt*(1.+m_ac[4]*m_ac[4])+mass*mass);
00248
00249 if (m_matrixValid) Em = m_Ea.similarity(del4MDelA(phi,mass));
00250 else Em = m_Ea;
00251
00252 return HepLorentzVector(px, py, pz, E);
00253 }
00254
00255 HepLorentzVector
00256 VFHelix::momentum(double phi,
00257 double mass,
00258 HepPoint3D & x,
00259 HepSymMatrix & Emx) const {
00260
00261
00262
00263
00264
00265
00266
00267
00268
00269
00270
00271 double pt = fabs(m_pt);
00272 double px = - pt * sin(m_ac[1] + phi);
00273 double py = pt * cos(m_ac[1] + phi);
00274 double pz = pt * m_ac[4];
00275 double E = sqrt(pt * pt * (1. + m_ac[4] * m_ac[4]) + mass * mass);
00276
00277 x.setX(m_pivot.x() + m_ac[0] * m_cp + m_r * (m_cp - cos(m_ac[1] + phi)));
00278 x.setY(m_pivot.y() + m_ac[0] * m_sp + m_r * (m_sp - sin(m_ac[1] + phi)));
00279 x.setZ(m_pivot.z() + m_ac[3] - m_r * m_ac[4] * phi);
00280
00281 if (m_matrixValid) Emx = m_Ea.similarity(del4MXDelA(phi,mass));
00282 else Emx = m_Ea;
00283
00284 return HepLorentzVector(px, py, pz, E);
00285 }
00286
00287
00288 const HepPoint3D &
00289 VFHelix::pivot(const HepPoint3D & newPivot) {
00290 const double & dr = m_ac[0];
00291 const double & phi0 = m_ac[1];
00292 const double & kappa = m_ac[2];
00293 const double & dz = m_ac[3];
00294 const double & tanl = m_ac[4];
00295
00296 double rdr = dr + m_r;
00297 double phi = fmod(phi0 + M_PI4, M_PI2);
00298 double csf0 = cos(phi);
00299 double snf0 = (1. - csf0) * (1. + csf0);
00300 snf0 = sqrt((snf0 > 0.) ? snf0 : 0.);
00301 if(phi > M_PI) snf0 = - snf0;
00302
00303 double xc = m_pivot.x() + rdr * csf0;
00304 double yc = m_pivot.y() + rdr * snf0;
00305 double csf, snf;
00306 if(m_r != 0.0) {
00307 csf = (xc - newPivot.x()) / m_r;
00308 snf = (yc - newPivot.y()) / m_r;
00309 double anrm = sqrt(csf * csf + snf * snf);
00310 if(anrm != 0.0) {
00311 csf /= anrm;
00312 snf /= anrm;
00313 phi = atan2(snf, csf);
00314 } else {
00315 csf = 1.0;
00316 snf = 0.0;
00317 phi = 0.0;
00318 }
00319 } else {
00320 csf = 1.0;
00321 snf = 0.0;
00322 phi = 0.0;
00323 }
00324 double phid = fmod(phi - phi0 + M_PI8, M_PI2);
00325 if(phid > M_PI) phid = phid - M_PI2;
00326 double drp = (m_pivot.x() + dr * csf0 + m_r * (csf0 - csf) - newPivot.x())
00327 * csf
00328 + (m_pivot.y() + dr * snf0 + m_r * (snf0 - snf) - newPivot.y()) * snf;
00329 double dzp = m_pivot.z() + dz - m_r * tanl * phid - newPivot.z();
00330
00331 HepVector ap(5);
00332 ap[0] = drp;
00333 ap[1] = fmod(phi + M_PI4, M_PI2);
00334 ap[2] = kappa;
00335 ap[3] = dzp;
00336 ap[4] = tanl;
00337
00338
00339 if (m_matrixValid) m_Ea = m_Ea.similarity(delApDelA(ap));
00340
00341 m_a = ap;
00342 m_pivot = newPivot;
00343
00344
00345 updateCache();
00346 return m_pivot;
00347 }
00348
00349 void
00350 VFHelix::set(const HepPoint3D & pivot,
00351 const HepVector & a,
00352 const HepSymMatrix & Ea) {
00353 m_pivot = pivot;
00354 m_a = a;
00355 m_Ea = Ea;
00356 m_matrixValid = true;
00357 updateCache();
00358 }
00359
00360 VFHelix &
00361 VFHelix::operator = (const VFHelix & i) {
00362 if (this == & i) return * this;
00363
00364 m_bField = i.m_bField;
00365 m_alpha = i.m_alpha;
00366 m_pivot = i.m_pivot;
00367 m_a = i.m_a;
00368 m_Ea = i.m_Ea;
00369 m_matrixValid = i.m_matrixValid;
00370
00371 m_center = i.m_center;
00372 m_cp = i.m_cp;
00373 m_sp = i.m_sp;
00374 m_pt = i.m_pt;
00375 m_r = i.m_r;
00376 m_ac[0] = i.m_ac[0];
00377 m_ac[1] = i.m_ac[1];
00378 m_ac[2] = i.m_ac[2];
00379 m_ac[3] = i.m_ac[3];
00380 m_ac[4] = i.m_ac[4];
00381
00382 return * this;
00383 }
00384
00385 void
00386 VFHelix::updateCache(void) {
00387
00388
00389
00390
00391
00392
00393
00394 m_ac[0] = m_a[0];
00395 m_ac[1] = m_a[1];
00396 m_ac[2] = m_a[2];
00397 m_ac[3] = m_a[3];
00398 m_ac[4] = m_a[4];
00399
00400 m_cp = cos(m_ac[1]);
00401 m_sp = sin(m_ac[1]);
00402 if (m_ac[2] != 0.0) {
00403 m_pt = 1. / m_ac[2];
00404 m_r = m_alpha / m_ac[2];
00405 }
00406 else {
00407 m_pt = (DBL_MAX);
00408 m_r = (DBL_MAX);
00409 }
00410
00411 double x = m_pivot.x() + (m_ac[0] + m_r) * m_cp;
00412 double y = m_pivot.y() + (m_ac[0] + m_r) * m_sp;
00413 m_center.setX(x);
00414 m_center.setY(y);
00415 m_center.setZ(0.);
00416 }
00417
00418 HepMatrix
00419 VFHelix::delApDelA(const HepVector & ap) const {
00420
00421
00422
00423
00424
00425 HepMatrix dApDA(5,5,0);
00426
00427 const double & dr = m_ac[0];
00428 const double & phi0 = m_ac[1];
00429 const double & cpa = m_ac[2];
00430 const double & dz = m_ac[3];
00431 const double & tnl = m_ac[4];
00432
00433 double drp = ap[0];
00434 double phi0p = ap[1];
00435 double cpap = ap[2];
00436 double dzp = ap[3];
00437 double tnlp = ap[4];
00438
00439 double rdr = m_r + dr;
00440 double rdrpr;
00441 if ((m_r + drp) != 0.0) {
00442 rdrpr = 1. / (m_r + drp);
00443 } else {
00444 rdrpr = (DBL_MAX);
00445 }
00446
00447
00448 double csfd = cos(phi0p - phi0);
00449 double snfd = sin(phi0p - phi0);
00450 double phid = fmod(phi0p - phi0 + M_PI8, M_PI2);
00451 if (phid > M_PI) phid = phid - M_PI2;
00452
00453 dApDA[0][0] = csfd;
00454 dApDA[0][1] = rdr*snfd;
00455 if(cpa!=0.0) {
00456 dApDA[0][2] = (m_r/cpa)*( 1.0 - csfd );
00457 } else {
00458 dApDA[0][2] = (DBL_MAX);
00459 }
00460
00461 dApDA[1][0] = - rdrpr*snfd;
00462 dApDA[1][1] = rdr*rdrpr*csfd;
00463 if(cpa!=0.0) {
00464 dApDA[1][2] = (m_r/cpa)*rdrpr*snfd;
00465 } else {
00466 dApDA[1][2] = (DBL_MAX);
00467 }
00468
00469 dApDA[2][2] = 1.0;
00470
00471 dApDA[3][0] = m_r*rdrpr*tnl*snfd;
00472 dApDA[3][1] = m_r*tnl*(1.0 - rdr*rdrpr*csfd);
00473 if(cpa!=0.0) {
00474 dApDA[3][2] = (m_r/cpa)*tnl*(phid - m_r*rdrpr*snfd);
00475 } else {
00476 dApDA[3][2] = (DBL_MAX);
00477 }
00478 dApDA[3][3] = 1.0;
00479 dApDA[3][4] = - m_r*phid;
00480
00481 dApDA[4][4] = 1.0;
00482
00483 return dApDA;
00484 }
00485
00486 HepMatrix
00487 VFHelix::delXDelA(double phi) const {
00488
00489
00490
00491
00492
00493
00494 HepMatrix dXDA(3,5,0);
00495
00496 const double & dr = m_ac[0];
00497 const double & phi0 = m_ac[1];
00498 const double & cpa = m_ac[2];
00499 const double & dz = m_ac[3];
00500 const double & tnl = m_ac[4];
00501
00502 double cosf0phi = cos(phi0 + phi);
00503 double sinf0phi = sin(phi0 + phi);
00504
00505 dXDA[0][0] = m_cp;
00506 dXDA[0][1] = - dr * m_sp + m_r * (- m_sp + sinf0phi);
00507 if(cpa!=0.0) {
00508 dXDA[0][2] = - (m_r / cpa) * (m_cp - cosf0phi);
00509 } else {
00510 dXDA[0][2] = (DBL_MAX);
00511 }
00512
00513
00514
00515 dXDA[1][0] = m_sp;
00516 dXDA[1][1] = dr * m_cp + m_r * (m_cp - cosf0phi);
00517 if(cpa!=0.0) {
00518 dXDA[1][2] = - (m_r / cpa) * (m_sp - sinf0phi);
00519 } else {
00520 dXDA[1][2] = (DBL_MAX);
00521 }
00522
00523
00524
00525
00526
00527 if(cpa!=0.0) {
00528 dXDA[2][2] = (m_r / cpa) * tnl * phi;
00529 } else {
00530 dXDA[2][2] = (DBL_MAX);
00531 }
00532 dXDA[2][3] = 1.0;
00533 dXDA[2][4] = - m_r * phi;
00534
00535 return dXDA;
00536 }
00537
00538
00539
00540 HepMatrix
00541 VFHelix::delMDelA(double phi) const {
00542
00543
00544
00545
00546
00547
00548 HepMatrix dMDA(3,5,0);
00549
00550 const double & phi0 = m_ac[1];
00551 const double & cpa = m_ac[2];
00552 const double & tnl = m_ac[4];
00553
00554 double cosf0phi = cos(phi0+phi);
00555 double sinf0phi = sin(phi0+phi);
00556
00557 double rho;
00558 if(cpa != 0.)rho = 1./cpa;
00559 else rho = (DBL_MAX);
00560
00561 double charge = 1.;
00562 if(cpa < 0.)charge = -1.;
00563
00564 dMDA[0][1] = -fabs(rho)*cosf0phi;
00565 dMDA[0][2] = charge*rho*rho*sinf0phi;
00566
00567 dMDA[1][1] = -fabs(rho)*sinf0phi;
00568 dMDA[1][2] = -charge*rho*rho*cosf0phi;
00569
00570 dMDA[2][2] = -charge*rho*rho*tnl;
00571 dMDA[2][4] = fabs(rho);
00572
00573 return dMDA;
00574 }
00575
00576
00577 HepMatrix
00578 VFHelix::del4MDelA(double phi, double mass) const {
00579
00580
00581
00582
00583
00584
00585 HepMatrix d4MDA(4,5,0);
00586
00587 double phi0 = m_ac[1];
00588 double cpa = m_ac[2];
00589 double tnl = m_ac[4];
00590
00591 double cosf0phi = cos(phi0+phi);
00592 double sinf0phi = sin(phi0+phi);
00593
00594 double rho;
00595 if(cpa != 0.)rho = 1./cpa;
00596 else rho = (DBL_MAX);
00597
00598 double charge = 1.;
00599 if(cpa < 0.)charge = -1.;
00600
00601 double E = sqrt(rho*rho*(1.+tnl*tnl)+mass*mass);
00602
00603 d4MDA[0][1] = -fabs(rho)*cosf0phi;
00604 d4MDA[0][2] = charge*rho*rho*sinf0phi;
00605
00606 d4MDA[1][1] = -fabs(rho)*sinf0phi;
00607 d4MDA[1][2] = -charge*rho*rho*cosf0phi;
00608
00609 d4MDA[2][2] = -charge*rho*rho*tnl;
00610 d4MDA[2][4] = fabs(rho);
00611
00612 if (cpa != 0.0 && E != 0.0) {
00613 d4MDA[3][2] = (-1.-tnl*tnl)/(cpa*cpa*cpa*E);
00614 d4MDA[3][4] = tnl/(cpa*cpa*E);
00615 } else {
00616 d4MDA[3][2] = (DBL_MAX);
00617 d4MDA[3][4] = (DBL_MAX);
00618 }
00619 return d4MDA;
00620 }
00621
00622
00623 HepMatrix
00624 VFHelix::del4MXDelA(double phi, double mass) const {
00625
00626
00627
00628
00629
00630
00631 HepMatrix d4MXDA(7,5,0);
00632
00633 const double & dr = m_ac[0];
00634 const double & phi0 = m_ac[1];
00635 const double & cpa = m_ac[2];
00636 const double & dz = m_ac[3];
00637 const double & tnl = m_ac[4];
00638
00639 double cosf0phi = cos(phi0+phi);
00640 double sinf0phi = sin(phi0+phi);
00641
00642 double rho;
00643 if(cpa != 0.)rho = 1./cpa;
00644 else rho = (DBL_MAX);
00645
00646 double charge = 1.;
00647 if(cpa < 0.)charge = -1.;
00648
00649 double E = sqrt(rho * rho * (1. + tnl * tnl) + mass * mass);
00650
00651 d4MXDA[0][1] = - fabs(rho) * cosf0phi;
00652 d4MXDA[0][2] = charge * rho * rho * sinf0phi;
00653
00654 d4MXDA[1][1] = - fabs(rho) * sinf0phi;
00655 d4MXDA[1][2] = - charge * rho * rho * cosf0phi;
00656
00657 d4MXDA[2][2] = - charge * rho * rho * tnl;
00658 d4MXDA[2][4] = fabs(rho);
00659
00660 if (cpa != 0.0 && E != 0.0) {
00661 d4MXDA[3][2] = (- 1. - tnl * tnl) / (cpa * cpa * cpa * E);
00662 d4MXDA[3][4] = tnl / (cpa * cpa * E);
00663 } else {
00664 d4MXDA[3][2] = (DBL_MAX);
00665 d4MXDA[3][4] = (DBL_MAX);
00666 }
00667
00668 d4MXDA[4][0] = m_cp;
00669 d4MXDA[4][1] = - dr * m_sp + m_r * (- m_sp + sinf0phi);
00670 if (cpa != 0.0) {
00671 d4MXDA[4][2] = - (m_r / cpa) * (m_cp - cosf0phi);
00672 } else {
00673 d4MXDA[4][2] = (DBL_MAX);
00674 }
00675
00676 d4MXDA[5][0] = m_sp;
00677 d4MXDA[5][1] = dr * m_cp + m_r * (m_cp - cosf0phi);
00678 if (cpa != 0.0) {
00679 d4MXDA[5][2] = - (m_r / cpa) * (m_sp - sinf0phi);
00680
00681 d4MXDA[6][2] = (m_r / cpa) * tnl * phi;
00682 } else {
00683 d4MXDA[5][2] = (DBL_MAX);
00684
00685 d4MXDA[6][2] = (DBL_MAX);
00686 }
00687
00688 d4MXDA[6][3] = 1.;
00689 d4MXDA[6][4] = - m_r * phi;
00690
00691 return d4MXDA;
00692 }
00693
00694 void
00695 VFHelix::ignoreErrorMatrix(void) {
00696 m_matrixValid = false;
00697 m_Ea *= 0.;
00698 }