CGEM BOSS 6.6.5.h
BESIII Offline Software System
Loading...
Searching...
No Matches
Reconstruction/KalFitAlg/KalFitAlg-00-15-18/src/helix/Helix.cxx
Go to the documentation of this file.
1//
2// Class Helix
3//
4// Author Date comments
5// Y.Ohnishi 03/01/1997 original version
6// Y.Ohnishi 06/03/1997 updated
7// Y.Iwasaki 17/02/1998 BFILED removed, func. name changed, func. added
8// J.Tanaka 06/12/1998 add some utilities.
9// Y.Iwasaki 07/07/1998 cache added to speed up
10//
11#include <iostream>
12#include <math.h>
13#include <float.h>
14#include "KalFitAlg/helix/Helix.h"
15#include "CLHEP/Matrix/Matrix.h"
16#include "GaudiKernel/StatusCode.h"
17#include "GaudiKernel/IInterface.h"
18#include "GaudiKernel/Kernel.h"
19#include "GaudiKernel/Service.h"
20#include "GaudiKernel/ISvcLocator.h"
21#include "GaudiKernel/SvcFactory.h"
22#include "GaudiKernel/IDataProviderSvc.h"
23#include "GaudiKernel/Bootstrap.h"
24#include "GaudiKernel/MsgStream.h"
25#include "GaudiKernel/SmartDataPtr.h"
26#include "GaudiKernel/IMessageSvc.h"
27
28// const double
29// Helix::m_BFIELD = 10.0; // KG
30// const double
31// Helix::m_ALPHA = 222.376063; // = 10000. / 2.99792458 / BFIELD
32// now Helix::m_ALPHA = 333.564095;
33
34const double
35M_PI2 = 2. * M_PI;
36
37const double
38M_PI4 = 4. * M_PI;
39
40const double
41M_PI8 = 8. * M_PI;
42
43
44namespace KalmanFit{
45
46const double Helix::ConstantAlpha = 333.564095;
47
48Helix::Helix()
49: m_matrixValid(true)
50{
51 StatusCode scmgn = Gaudi::svcLocator()->service("MagneticFieldSvc",m_pmgnIMF);
52 if(scmgn!=StatusCode::SUCCESS) {
53 std::cout<< "Unable to open Magnetic field service"<<std::endl;
54 }
55 m_bField = 10000*(m_pmgnIMF->getReferField());//zhangr 2012-09-06 for MagnField
56 //m_bField = 10000*(m_pmgnIMF->getReferField());
57 m_alpha = 10000. / 2.99792458 / m_bField;
58
59 HepPoint3D pivot(0,0,0);
60 HepVector a(5,0);
61 HepSymMatrix Ea(5,0);
62 m_pivot = pivot;
63 m_a = a;
64 m_Ea = Ea;
65 updateCache();
66}
67
68
69Helix::Helix(const HepPoint3D & pivot,
70 const HepVector & a,
71 const HepSymMatrix & Ea)
72 : //m_bField(-10.0),
73 //m_alpha(-333.564095),
74 m_pivot(pivot),
75 m_a(a),
76 m_matrixValid(true),
77 m_Ea(Ea) {
78 StatusCode scmgn = Gaudi::svcLocator()->service("MagneticFieldSvc",m_pmgnIMF);
79 if(scmgn!=StatusCode::SUCCESS) {
80 std::cout<< "Unable to open Magnetic field service"<<std::endl;
81 }
82 m_bField = 10000*(m_pmgnIMF->getReferField());
83 m_alpha = 10000. / 2.99792458 / m_bField;
84 // m_alpha = 10000. / 2.99792458 / m_bField;
85 // m_alpha = 333.564095;
86 updateCache();
87}
88
89Helix::Helix(const HepPoint3D & pivot,
90 const HepVector & a)
91 : //m_bField(-10.0),
92 //m_alpha(-333.564095),
93 m_pivot(pivot),
94 m_a(a),
95 m_matrixValid(false),
96 m_Ea(HepSymMatrix(5,0)) {
97 StatusCode scmgn = Gaudi::svcLocator()->service("MagneticFieldSvc",m_pmgnIMF);
98 if(scmgn!=StatusCode::SUCCESS) {
99 // log << MSG::ERROR << "Unable to open Magnetic field service"<<endreq;
100 std::cout<< "Unable to open Magnetic field service"<<std::endl;
101 }
102 m_bField = 10000*(m_pmgnIMF->getReferField());
103 m_alpha = 10000. / 2.99792458 / m_bField;
104 // m_alpha = 333.564095;
105 //cout<<"MdcFastTrakAlg:: bField,alpha: "<<m_bField<<" , "<<m_alpha<<endl;
106 updateCache();
107}
108
109Helix::Helix(const HepPoint3D & position,
110 const Hep3Vector & momentum,
111 double charge)
112 : //m_bField(-10.0),
113 //m_alpha(-333.564095),
114 m_pivot(position),
115 m_a(HepVector(5,0)),
116 m_matrixValid(false),
117 m_Ea(HepSymMatrix(5,0)) {
118 StatusCode scmgn = Gaudi::svcLocator()->service("MagneticFieldSvc",m_pmgnIMF);
119 if(scmgn!=StatusCode::SUCCESS) {
120 // log << MSG::ERROR << "Unable to open Magnetic field service"<<endreq;
121 std::cout<< "Unable to open Magnetic field service"<<std::endl;
122 }
123 m_bField = 10000*(m_pmgnIMF->getReferField());
124 m_alpha = 10000. / 2.99792458 / m_bField;
125
126 m_a[0] = 0.;
127 m_a[1] = fmod(atan2(- momentum.x(), momentum.y())
128 + M_PI4, M_PI2);
129 m_a[3] = 0.;
130 double perp(momentum.perp());
131 if (perp != 0.0) {
132 m_a[2] = charge / perp;
133 m_a[4] = momentum.z() / perp;
134 }
135 else {
136 m_a[2] = charge * (DBL_MAX);
137 if (momentum.z() >= 0) {
138 m_a[4] = (DBL_MAX);
139 } else {
140 m_a[4] = -(DBL_MAX);
141 }
142 }
143 // m_alpha = 333.564095;
144 updateCache();
145}
146
149
150HepPoint3D
151Helix::x(double phi) const {
152 //
153 // Calculate position (x,y,z) along helix.
154 //
155 // x = x0 + dr * cos(phi0) + (alpha / kappa) * (cos(phi0) - cos(phi0+phi))
156 // y = y0 + dr * sin(phi0) + (alpha / kappa) * (sin(phi0) - sin(phi0+phi))
157 // z = z0 + dz - (alpha / kappa) * tan(lambda) * phi
158 //
159
160 double x = m_pivot.x() + m_ac[0] * m_cp + m_r * (m_cp - cos(m_ac[1] +phi));
161 double y = m_pivot.y() + m_ac[0] * m_sp + m_r * (m_sp - sin(m_ac[1] +phi));
162 double z = m_pivot.z() + m_ac[3] - m_r * m_ac[4] * phi;
163
164 return HepPoint3D(x, y, z);
165}
166
167double *
168Helix::x(double phi, double p[3]) const {
169 //
170 // Calculate position (x,y,z) along helix.
171 //
172 // x = x0 + dr * cos(phi0) + (alpha / kappa) * (cos(phi0) - cos(phi0+phi))
173 // y = y0 + dr * sin(phi0) + (alpha / kappa) * (sin(phi0) - sin(phi0+phi))
174 // z = z0 + dz - (alpha / kappa) * tan(lambda) * phi
175 //
176
177 p[0] = m_pivot.x() + m_ac[0] * m_cp + m_r * (m_cp - cos(m_ac[1] + phi));
178 p[1] = m_pivot.y() + m_ac[0] * m_sp + m_r * (m_sp - sin(m_ac[1] + phi));
179 p[2] = m_pivot.z() + m_ac[3] - m_r * m_ac[4] * phi;
180
181 return p;
182}
183
184HepPoint3D
185Helix::x(double phi, HepSymMatrix & Ex) const {
186 double x = m_pivot.x() + m_ac[0] * m_cp + m_r * (m_cp - cos(m_ac[1] +phi));
187 double y = m_pivot.y() + m_ac[0] * m_sp + m_r * (m_sp - sin(m_ac[1] +phi));
188 double z = m_pivot.z() + m_ac[3] - m_r * m_ac[4] * phi;
189
190 //
191 // Calculate position error matrix.
192 // Ex(phi) = (@x/@a)(Ea)(@x/@a)^T, phi is deflection angle to specify the
193 // point to be calcualted.
194 //
195 // HepMatrix dXDA(3, 5, 0);
196 // dXDA = delXDelA(phi);
197 // Ex.assign(dXDA * m_Ea * dXDA.T());
198
199 if (m_matrixValid) Ex = m_Ea.similarity(delXDelA(phi));
200 else Ex = m_Ea;
201
202 return HepPoint3D(x, y, z);
203}
204
205Hep3Vector
206Helix::momentum(double phi) const {
207 //
208 // Calculate momentum.
209 //
210 // Pt = | 1/kappa | (GeV/c)
211 //
212 // Px = -Pt * sin(phi0 + phi)
213 // Py = Pt * cos(phi0 + phi)
214 // Pz = Pt * tan(lambda)
215 //
216
217 double pt = fabs(m_pt);
218 double px = - pt * sin(m_ac[1] + phi);
219 double py = pt * cos(m_ac[1] + phi);
220 double pz = pt * m_ac[4];
221
222 return Hep3Vector(px, py, pz);
223}
224
225Hep3Vector
226Helix::momentum(double phi, HepSymMatrix & Em) const {
227 //
228 // Calculate momentum.
229 //
230 // Pt = | 1/kappa | (GeV/c)
231 //
232 // Px = -Pt * sin(phi0 + phi)
233 // Py = Pt * cos(phi0 + phi)
234 // Pz = Pt * tan(lambda)
235 //
236
237 double pt = fabs(m_pt);
238 double px = - pt * sin(m_ac[1] + phi);
239 double py = pt * cos(m_ac[1] + phi);
240 double pz = pt * m_ac[4];
241
242 if (m_matrixValid) Em = m_Ea.similarity(delMDelA(phi));
243 else Em = m_Ea;
244
245 return Hep3Vector(px, py, pz);
246}
247
248HepLorentzVector
249Helix::momentum(double phi, double mass) const {
250 //
251 // Calculate momentum.
252 //
253 // Pt = | 1/kappa | (GeV/c)
254 //
255 // Px = -Pt * sin(phi0 + phi)
256 // Py = Pt * cos(phi0 + phi)
257 // Pz = Pt * tan(lambda)
258 //
259 // E = sqrt( 1/kappa/kappa * (1+tan(lambda)*tan(lambda)) + mass*mass )
260
261 double pt = fabs(m_pt);
262 double px = - pt * sin(m_ac[1] + phi);
263 double py = pt * cos(m_ac[1] + phi);
264 double pz = pt * m_ac[4];
265 double E = sqrt(pt*pt*(1.+m_ac[4]*m_ac[4])+mass*mass);
266
267 return HepLorentzVector(px, py, pz, E);
268}
269
270
271HepLorentzVector
272Helix::momentum(double phi, double mass, HepSymMatrix & Em) const {
273 //
274 // Calculate momentum.
275 //
276 // Pt = | 1/kappa | (GeV/c)
277 //
278 // Px = -Pt * sin(phi0 + phi)
279 // Py = Pt * cos(phi0 + phi)
280 // Pz = Pt * tan(lambda)
281 //
282 // E = sqrt( 1/kappa/kappa * (1+tan(lambda)*tan(lambda)) + mass*mass )
283
284 double pt = fabs(m_pt);
285 double px = - pt * sin(m_ac[1] + phi);
286 double py = pt * cos(m_ac[1] + phi);
287 double pz = pt * m_ac[4];
288 double E = sqrt(pt*pt*(1.+m_ac[4]*m_ac[4])+mass*mass);
289
290 if (m_matrixValid) Em = m_Ea.similarity(del4MDelA(phi,mass));
291 else Em = m_Ea;
292
293 return HepLorentzVector(px, py, pz, E);
294}
295
296HepLorentzVector
297Helix::momentum(double phi,
298 double mass,
299 HepPoint3D & x,
300 HepSymMatrix & Emx) const {
301 //
302 // Calculate momentum.
303 //
304 // Pt = | 1/kappa | (GeV/c)
305 //
306 // Px = -Pt * sin(phi0 + phi)
307 // Py = Pt * cos(phi0 + phi)
308 // Pz = Pt * tan(lambda)
309 //
310 // E = sqrt( 1/kappa/kappa * (1+tan(lambda)*tan(lambda)) + mass*mass )
311
312 double pt = fabs(m_pt);
313 double px = - pt * sin(m_ac[1] + phi);
314 double py = pt * cos(m_ac[1] + phi);
315 double pz = pt * m_ac[4];
316 double E = sqrt(pt * pt * (1. + m_ac[4] * m_ac[4]) + mass * mass);
317
318 x.setX(m_pivot.x() + m_ac[0] * m_cp + m_r * (m_cp - cos(m_ac[1] + phi)));
319 x.setY(m_pivot.y() + m_ac[0] * m_sp + m_r * (m_sp - sin(m_ac[1] + phi)));
320 x.setZ(m_pivot.z() + m_ac[3] - m_r * m_ac[4] * phi);
321
322 if (m_matrixValid) Emx = m_Ea.similarity(del4MXDelA(phi,mass));
323 else Emx = m_Ea;
324
325 return HepLorentzVector(px, py, pz, E);
326}
327
328
329const HepPoint3D &
330Helix::pivot(const HepPoint3D & newPivot) {
331
332 //std::cout<<" in Helix::pivot:"<<std::endl;
333 //std::cout<<" m_alpha: "<<m_alpha<<std::endl;
334
335 const double & dr = m_ac[0];
336 const double & phi0 = m_ac[1];
337 const double & kappa = m_ac[2];
338 const double & dz = m_ac[3];
339 const double & tanl = m_ac[4];
340
341 double rdr = dr + m_r;
342 double phi = fmod(phi0 + M_PI4, M_PI2);
343 double csf0 = cos(phi);
344 double snf0 = (1. - csf0) * (1. + csf0);
345 snf0 = sqrt((snf0 > 0.) ? snf0 : 0.);
346 if(phi > M_PI) snf0 = - snf0;
347
348 double xc = m_pivot.x() + rdr * csf0;
349 double yc = m_pivot.y() + rdr * snf0;
350 double csf, snf;
351 if(m_r != 0.0) {
352 csf = (xc - newPivot.x()) / m_r;
353 snf = (yc - newPivot.y()) / m_r;
354 double anrm = sqrt(csf * csf + snf * snf);
355 if(anrm != 0.0) {
356 csf /= anrm;
357 snf /= anrm;
358 phi = atan2(snf, csf);
359 } else {
360 csf = 1.0;
361 snf = 0.0;
362 phi = 0.0;
363 }
364 } else {
365 csf = 1.0;
366 snf = 0.0;
367 phi = 0.0;
368 }
369 double phid = fmod(phi - phi0 + M_PI8, M_PI2);
370 if(phid > M_PI) phid = phid - M_PI2;
371 double drp = (m_pivot.x() + dr * csf0 + m_r * (csf0 - csf) - newPivot.x())
372 * csf
373 + (m_pivot.y() + dr * snf0 + m_r * (snf0 - snf) - newPivot.y()) * snf;
374 double dzp = m_pivot.z() + dz - m_r * tanl * phid - newPivot.z();
375
376 HepVector ap(5);
377 ap[0] = drp;
378 ap[1] = fmod(phi + M_PI4, M_PI2);
379 ap[2] = kappa;
380 ap[3] = dzp;
381 ap[4] = tanl;
382
383 // if (m_matrixValid) m_Ea.assign(delApDelA(ap) * m_Ea * delApDelA(ap).T());
384 if (m_matrixValid) m_Ea = m_Ea.similarity(delApDelA(ap));
385
386 m_a = ap;
387 m_pivot = newPivot;
388
389 //...Are these needed?...iw...
390 updateCache();
391 return m_pivot;
392}
393
394void
395Helix::set(const HepPoint3D & pivot,
396 const HepVector & a,
397 const HepSymMatrix & Ea) {
398 m_pivot = pivot;
399 m_a = a;
400 m_Ea = Ea;
401 m_matrixValid = true;
402 updateCache();
403}
404
405Helix &
406Helix::operator = (const Helix & i) {
407 if (this == & i) return * this;
408
409 m_bField = i.m_bField;
410 m_alpha = i.m_alpha;
411 m_pivot = i.m_pivot;
412 m_a = i.m_a;
413 m_Ea = i.m_Ea;
414 m_matrixValid = i.m_matrixValid;
415
416 m_center = i.m_center;
417 m_cp = i.m_cp;
418 m_sp = i.m_sp;
419 m_pt = i.m_pt;
420 m_r = i.m_r;
421 m_ac[0] = i.m_ac[0];
422 m_ac[1] = i.m_ac[1];
423 m_ac[2] = i.m_ac[2];
424 m_ac[3] = i.m_ac[3];
425 m_ac[4] = i.m_ac[4];
426
427 return * this;
428}
429
430Helix::Helix(const Helix& i)
431{
432 m_bField = i.m_bField;
433 m_alpha = i.m_alpha;
434 m_pivot = i.m_pivot;
435 m_a = i.m_a;
436 m_Ea = i.m_Ea;
437 m_matrixValid = i.m_matrixValid;
438
439 m_center = i.m_center;
440 m_cp = i.m_cp;
441 m_sp = i.m_sp;
442 m_pt = i.m_pt;
443 m_r = i.m_r;
444 m_ac[0] = i.m_ac[0];
445 m_ac[1] = i.m_ac[1];
446 m_ac[2] = i.m_ac[2];
447 m_ac[3] = i.m_ac[3];
448 m_ac[4] = i.m_ac[4];
449}
450
451
452void
453Helix::updateCache(void) {
454 //
455 // Calculate Helix center( xc, yc ).
456 //
457 // xc = x0 + (dr + (alpha / kappa)) * cos(phi0) (cm)
458 // yc = y0 + (dr + (alpha / kappa)) * sin(phi0) (cm)
459 //
460
461 //std::cout<<" in updateCache, m_alpha: "<<m_alpha<<std::endl;
462
463 m_ac[0] = m_a[0];
464 m_ac[1] = m_a[1];
465 m_ac[2] = m_a[2];
466 m_ac[3] = m_a[3];
467 m_ac[4] = m_a[4];
468
469 m_cp = cos(m_ac[1]);
470 m_sp = sin(m_ac[1]);
471 if (m_ac[2] != 0.0) {
472 m_pt = 1. / m_ac[2];
473 m_r = m_alpha / m_ac[2];
474 }
475 else {
476 m_pt = (DBL_MAX);
477 m_r = (DBL_MAX);
478 }
479
480 double x = m_pivot.x() + (m_ac[0] + m_r) * m_cp;
481 double y = m_pivot.y() + (m_ac[0] + m_r) * m_sp;
482 m_center.setX(x);
483 m_center.setY(y);
484 m_center.setZ(0.);
485}
486
487HepMatrix
488Helix::delApDelA(const HepVector & ap) const {
489 //
490 // Calculate Jacobian (@ap/@a)
491 // Vector ap is new helix parameters and a is old helix parameters.
492 //
493
494 HepMatrix dApDA(5,5,0);
495
496 const double & dr = m_ac[0];
497 const double & phi0 = m_ac[1];
498 const double & cpa = m_ac[2];
499 const double & dz = m_ac[3];
500 const double & tnl = m_ac[4];
501
502 double drp = ap[0];
503 double phi0p = ap[1];
504 double cpap = ap[2];
505 double dzp = ap[3];
506 double tnlp = ap[4];
507
508 double rdr = m_r + dr;
509 double rdrpr;
510 if ((m_r + drp) != 0.0) {
511 rdrpr = 1. / (m_r + drp);
512 } else {
513 rdrpr = (DBL_MAX);
514 }
515 // double csfd = cos(phi0)*cos(phi0p) + sin(phi0)*sin(phi0p);
516 // double snfd = cos(phi0)*sin(phi0p) - sin(phi0)*cos(phi0p);
517 double csfd = cos(phi0p - phi0);
518 double snfd = sin(phi0p - phi0);
519 double phid = fmod(phi0p - phi0 + M_PI8, M_PI2);
520 if (phid > M_PI) phid = phid - M_PI2;
521
522 dApDA[0][0] = csfd;
523 dApDA[0][1] = rdr*snfd;
524 if(cpa!=0.0) {
525 dApDA[0][2] = (m_r/cpa)*( 1.0 - csfd );
526 } else {
527 dApDA[0][2] = (DBL_MAX);
528 }
529
530 dApDA[1][0] = - rdrpr*snfd;
531 dApDA[1][1] = rdr*rdrpr*csfd;
532 if(cpa!=0.0) {
533 dApDA[1][2] = (m_r/cpa)*rdrpr*snfd;
534 } else {
535 dApDA[1][2] = (DBL_MAX);
536 }
537
538 dApDA[2][2] = 1.0;
539
540 dApDA[3][0] = m_r*rdrpr*tnl*snfd;
541 dApDA[3][1] = m_r*tnl*(1.0 - rdr*rdrpr*csfd);
542 if(cpa!=0.0) {
543 dApDA[3][2] = (m_r/cpa)*tnl*(phid - m_r*rdrpr*snfd);
544 } else {
545 dApDA[3][2] = (DBL_MAX);
546 }
547 dApDA[3][3] = 1.0;
548 dApDA[3][4] = - m_r*phid;
549
550 dApDA[4][4] = 1.0;
551
552 return dApDA;
553}
554
555HepMatrix
556Helix::delXDelA(double phi) const {
557 //
558 // Calculate Jacobian (@x/@a)
559 // Vector a is helix parameters and phi is internal parameter
560 // which specifys the point to be calculated for Ex(phi).
561 //
562
563 HepMatrix dXDA(3,5,0);
564
565 const double & dr = m_ac[0];
566 const double & phi0 = m_ac[1];
567 const double & cpa = m_ac[2];
568 const double & dz = m_ac[3];
569 const double & tnl = m_ac[4];
570
571 double cosf0phi = cos(phi0 + phi);
572 double sinf0phi = sin(phi0 + phi);
573
574 dXDA[0][0] = m_cp;
575 dXDA[0][1] = - dr * m_sp + m_r * (- m_sp + sinf0phi);
576 if(cpa!=0.0) {
577 dXDA[0][2] = - (m_r / cpa) * (m_cp - cosf0phi);
578 } else {
579 dXDA[0][2] = (DBL_MAX);
580 }
581 // dXDA[0][3] = 0.0;
582 // dXDA[0][4] = 0.0;
583
584 dXDA[1][0] = m_sp;
585 dXDA[1][1] = dr * m_cp + m_r * (m_cp - cosf0phi);
586 if(cpa!=0.0) {
587 dXDA[1][2] = - (m_r / cpa) * (m_sp - sinf0phi);
588 } else {
589 dXDA[1][2] = (DBL_MAX);
590 }
591 // dXDA[1][3] = 0.0;
592 // dXDA[1][4] = 0.0;
593
594 // dXDA[2][0] = 0.0;
595 // dXDA[2][1] = 0.0;
596 if(cpa!=0.0) {
597 dXDA[2][2] = (m_r / cpa) * tnl * phi;
598 } else {
599 dXDA[2][2] = (DBL_MAX);
600 }
601 dXDA[2][3] = 1.0;
602 dXDA[2][4] = - m_r * phi;
603
604 return dXDA;
605}
606
607
608
609HepMatrix
610Helix::delMDelA(double phi) const {
611 //
612 // Calculate Jacobian (@m/@a)
613 // Vector a is helix parameters and phi is internal parameter.
614 // Vector m is momentum.
615 //
616
617 HepMatrix dMDA(3,5,0);
618
619 const double & phi0 = m_ac[1];
620 const double & cpa = m_ac[2];
621 const double & tnl = m_ac[4];
622
623 double cosf0phi = cos(phi0+phi);
624 double sinf0phi = sin(phi0+phi);
625
626 double rho;
627 if(cpa != 0.)rho = 1./cpa;
628 else rho = (DBL_MAX);
629
630 double charge = 1.;
631 if(cpa < 0.)charge = -1.;
632
633 dMDA[0][1] = -fabs(rho)*cosf0phi;
634 dMDA[0][2] = charge*rho*rho*sinf0phi;
635
636 dMDA[1][1] = -fabs(rho)*sinf0phi;
637 dMDA[1][2] = -charge*rho*rho*cosf0phi;
638
639 dMDA[2][2] = -charge*rho*rho*tnl;
640 dMDA[2][4] = fabs(rho);
641
642 return dMDA;
643}
644
645
646HepMatrix
647Helix::del4MDelA(double phi, double mass) const {
648 //
649 // Calculate Jacobian (@4m/@a)
650 // Vector a is helix parameters and phi is internal parameter.
651 // Vector 4m is 4 momentum.
652 //
653
654 HepMatrix d4MDA(4,5,0);
655
656 double phi0 = m_ac[1];
657 double cpa = m_ac[2];
658 double tnl = m_ac[4];
659
660 double cosf0phi = cos(phi0+phi);
661 double sinf0phi = sin(phi0+phi);
662
663 double rho;
664 if(cpa != 0.)rho = 1./cpa;
665 else rho = (DBL_MAX);
666
667 double charge = 1.;
668 if(cpa < 0.)charge = -1.;
669
670 double E = sqrt(rho*rho*(1.+tnl*tnl)+mass*mass);
671
672 d4MDA[0][1] = -fabs(rho)*cosf0phi;
673 d4MDA[0][2] = charge*rho*rho*sinf0phi;
674
675 d4MDA[1][1] = -fabs(rho)*sinf0phi;
676 d4MDA[1][2] = -charge*rho*rho*cosf0phi;
677
678 d4MDA[2][2] = -charge*rho*rho*tnl;
679 d4MDA[2][4] = fabs(rho);
680
681 if (cpa != 0.0 && E != 0.0) {
682 d4MDA[3][2] = (-1.-tnl*tnl)/(cpa*cpa*cpa*E);
683 d4MDA[3][4] = tnl/(cpa*cpa*E);
684 } else {
685 d4MDA[3][2] = (DBL_MAX);
686 d4MDA[3][4] = (DBL_MAX);
687 }
688 return d4MDA;
689}
690
691
692HepMatrix
693Helix::del4MXDelA(double phi, double mass) const {
694 //
695 // Calculate Jacobian (@4mx/@a)
696 // Vector a is helix parameters and phi is internal parameter.
697 // Vector 4xm is 4 momentum and position.
698 //
699
700 HepMatrix d4MXDA(7,5,0);
701
702 const double & dr = m_ac[0];
703 const double & phi0 = m_ac[1];
704 const double & cpa = m_ac[2];
705 const double & dz = m_ac[3];
706 const double & tnl = m_ac[4];
707
708 double cosf0phi = cos(phi0+phi);
709 double sinf0phi = sin(phi0+phi);
710
711 double rho;
712 if(cpa != 0.)rho = 1./cpa;
713 else rho = (DBL_MAX);
714
715 double charge = 1.;
716 if(cpa < 0.)charge = -1.;
717
718 double E = sqrt(rho * rho * (1. + tnl * tnl) + mass * mass);
719
720 d4MXDA[0][1] = - fabs(rho) * cosf0phi;
721 d4MXDA[0][2] = charge * rho * rho * sinf0phi;
722
723 d4MXDA[1][1] = - fabs(rho) * sinf0phi;
724 d4MXDA[1][2] = - charge * rho * rho * cosf0phi;
725
726 d4MXDA[2][2] = - charge * rho * rho * tnl;
727 d4MXDA[2][4] = fabs(rho);
728
729 if (cpa != 0.0 && E != 0.0) {
730 d4MXDA[3][2] = (- 1. - tnl * tnl) / (cpa * cpa * cpa * E);
731 d4MXDA[3][4] = tnl / (cpa * cpa * E);
732 } else {
733 d4MXDA[3][2] = (DBL_MAX);
734 d4MXDA[3][4] = (DBL_MAX);
735 }
736
737 d4MXDA[4][0] = m_cp;
738 d4MXDA[4][1] = - dr * m_sp + m_r * (- m_sp + sinf0phi);
739 if (cpa != 0.0) {
740 d4MXDA[4][2] = - (m_r / cpa) * (m_cp - cosf0phi);
741 } else {
742 d4MXDA[4][2] = (DBL_MAX);
743 }
744
745 d4MXDA[5][0] = m_sp;
746 d4MXDA[5][1] = dr * m_cp + m_r * (m_cp - cosf0phi);
747 if (cpa != 0.0) {
748 d4MXDA[5][2] = - (m_r / cpa) * (m_sp - sinf0phi);
749
750 d4MXDA[6][2] = (m_r / cpa) * tnl * phi;
751 } else {
752 d4MXDA[5][2] = (DBL_MAX);
753
754 d4MXDA[6][2] = (DBL_MAX);
755 }
756
757 d4MXDA[6][3] = 1.;
758 d4MXDA[6][4] = - m_r * phi;
759
760 return d4MXDA;
761}
762
763void
764Helix::ignoreErrorMatrix(void) {
765 m_matrixValid = false;
766 m_Ea *= 0.;
767}
768
769double Helix::IntersectCylinder(double r) const
770{
771 double m_rad = radius();
772 double l = center().perp();
773
774 double cosPhi = (m_rad * m_rad + l * l - r * r) / (2 * m_rad * l);
775
776 if(cosPhi < -1 || cosPhi > 1) return 0;
777
778 double dPhi = center().phi() - acos(cosPhi) - phi0();
779
780 if(dPhi < -M_PI) dPhi += 2 * M_PI;
781
782 return dPhi;
783}
784
785double Helix::flightArc(HepPoint3D& hit) const
786{
787 /*
788 double rc = center().perp();
789 //double l = center().perp(hit);
790 double l = sqrt( (center().x()-hit.x()) * (center().x()-hit.x()) + (center().y()-hit.y()) * (center().y()-hit.y()) );
791 double rHit = hit.perp();
792 double cosPhi = (rc*rc + l * l - rHit * rHit) / (2 * rc * l);
793 //double cosPhi = (m_rad * m_rad + l * l - r * r) / (2 * m_rad * l);
794 if(cosPhi < -1 || cosPhi > 1) return 0;
795 double dPhi = acos(cosPhi);// [0, pi)
796 //double phiTrkFlt = IntersectCylinder(rHit);
797 //double arcLength = rc*dPhi;
798 double xHit = hit.getHitPoint().x();
799 double yHit = hit.getHitPoint().y();
800 double xCenter = center().x();
801 double yCenter = center().y();
802 double leftOrRight = xHit*yCenter - xCenter*yHit;
803 */
804 double dphi = dPhi(hit);
805 double arcLength = fabs(m_r*dphi);
806 return arcLength;
807}
808
809double Helix::flightLength(HepPoint3D& hit) const
810{
811 /*
812 double rc = center().perp();
813 //double l = center().perp(hit);
814 double l = sqrt( (center().x()-hit.x()) * (center().x()-hit.x()) + (center().y()-hit.y()) * (center().y()-hit.y()) );
815 double rHit = hit.perp();
816 double cosPhi = (rc*rc + l * l - rHit * rHit) / (2 * rc * l);
817 //double cosPhi = (m_rad * m_rad + l * l - r * r) / (2 * m_rad * l);
818 if(cosPhi < -1 || cosPhi > 1) return 0;
819 double dPhi = acos(cosPhi);
820 //double dPhi = center().phi() - acos(cosPhi) - phi0();
821 //double phiTrkFlt = IntersectCylinder(rHit);
822 double arcLength = rc*dPhi;
823 */
824
825 double arcLength = flightArc(hit);
826
827 double cosLambda = 1/sqrt(1+m_ac[4]*m_ac[4]);
828 double flightLength = arcLength/cosLambda;
829 return flightLength;
830}
831
832double Helix::dPhi(HepPoint3D& hit) const
833{
834 double isClockWise = m_r/fabs(m_r);
835 double x_cw = center().x()-hit.x();
836 double y_cw = center().y()-hit.y();
837 x_cw = isClockWise*x_cw;
838 y_cw = isClockWise*y_cw;
839 double phi_cw = atan2(y_cw,x_cw);
840 double dphi = phi_cw-phi0();
841 //while(dphi> M_PI) dphi-=2*M_PI;
842 //while(dphi<-M_PI) dphi+=2*M_PI;
843 if(isClockWise>0)
844 {
845 while(dphi>0) dphi-=2*M_PI;
846 while(dphi<-2*M_PI) dphi+=2*M_PI;
847 }
848 else {
849 while(dphi<0) dphi+=2*M_PI;
850 while(dphi>2*M_PI) dphi-=2*M_PI;
851 }
852 double delZ_2pi=2*M_PI*fabs(m_r)*m_ac[4];
853 double delZ_current=hit.z()-x(dphi).z();
854 double n2pi=0;
855 if(delZ_2pi!=0&&fabs(delZ_current)>0.5*fabs(delZ_2pi))
856 {
857 double sign=delZ_current/delZ_2pi;
858 sign/=fabs(sign);
859 n2pi=floor(fabs(delZ_current/delZ_2pi)+0.5);
860 n2pi=-isClockWise*sign*n2pi;
861 }
862 dphi+=n2pi*2*M_PI;
863 return dphi;
864}
865
866double Helix::flightArc(double r) const
867{
868 double phi_turn = IntersectCylinder(r);
869 double arcLength = m_r*phi_turn;
870 arcLength = fabs(arcLength);
871 return arcLength;
872}
873
874double Helix::flightLength(double r) const
875{
876 double arcLength = flightArc(r);
877 double cosLambda = 1/sqrt(1+m_ac[4]*m_ac[4]);
878 double flightLength = arcLength/cosLambda;
879 return flightLength;
880}
881
882} // end of namespace KalmanFit
sin
double sin(const BesAngle a)
Definition BesAngle.h:210
cos
double cos(const BesAngle a)
Definition BesAngle.h:213
momentum
**********INTEGER nmxhep !maximum number of particles DOUBLE PRECISION vhep INTEGER jdahep COMMON hepevt $ !serial number $ !number of particles $ !status code $ !particle ident KF $ !parent particles $ !childreen particles $ !four momentum
Definition BesBdkRc/BesBdkRc-00-00-02/src/fortran/HepEvt.h:23
mass
double mass
Definition CosmicGenerator.cxx:138
x
Double_t x[10]
Definition DataBase/tau_mode.c:57
HepPoint3D
HepGeom::Point3D< double > HepPoint3D
Definition Gam4pikp.cxx:37
DBL_MAX
#define DBL_MAX
Definition KalFitAlg.h:13
M_PI
#define M_PI
Definition TConstant.h:4
IMagneticFieldSvc::getReferField
virtual double getReferField()=0
KalmanFit::Helix::center
const HepPoint3D & center(void) const
returns position of helix center(z = 0.);
Definition Reconstruction/KalFitAlg/KalFitAlg-00-15-18/KalFitAlg/helix/Helix.h:205
KalmanFit::Helix::set
void set(const HepPoint3D &pivot, const HepVector &a, const HepSymMatrix &Ea)
sets helix pivot position, parameters, and error matrix.
Definition Reconstruction/KalFitAlg/KalFitAlg-00-15-18/src/helix/Helix.cxx:395
KalmanFit::Helix::ignoreErrorMatrix
void ignoreErrorMatrix(void)
unsets error matrix. Error calculations will be ignored after this function call until an error matri...
Definition Reconstruction/KalFitAlg/KalFitAlg-00-15-18/src/helix/Helix.cxx:764
KalmanFit::Helix::momentum
Hep3Vector momentum(double dPhi=0.) const
returns momentum vector after rotating angle dPhi in phi direction.
Definition Reconstruction/KalFitAlg/KalFitAlg-00-15-18/src/helix/Helix.cxx:206
KalmanFit::Helix::ConstantAlpha
static const double ConstantAlpha
Constant alpha for uniform field.
Definition Reconstruction/KalFitAlg/KalFitAlg-00-15-18/KalFitAlg/helix/Helix.h:173
KalmanFit::Helix::Ea
const HepSymMatrix & Ea(void) const
returns error matrix.
Definition Reconstruction/KalFitAlg/KalFitAlg-00-15-18/KalFitAlg/helix/Helix.h:271
KalmanFit::Helix::x
HepPoint3D x(double dPhi=0.) const
returns position after rotating angle dPhi in phi direction.
Definition Reconstruction/KalFitAlg/KalFitAlg-00-15-18/src/helix/Helix.cxx:151
KalmanFit::Helix::dr
double dr(void) const
returns an element of parameters.
Definition Reconstruction/KalFitAlg/KalFitAlg-00-15-18/KalFitAlg/helix/Helix.h:229
KalmanFit::Helix::a
const HepVector & a(void) const
returns helix parameters.
Definition Reconstruction/KalFitAlg/KalFitAlg-00-15-18/KalFitAlg/helix/Helix.h:265
KalmanFit::Helix::pivot
const HepPoint3D & pivot(void) const
returns pivot position.
Definition Reconstruction/KalFitAlg/KalFitAlg-00-15-18/KalFitAlg/helix/Helix.h:211