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