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