Belle II Software  light-2205-abys
HelixUtils.cc
1 /**************************************************************************
2  * basf2 (Belle II Analysis Software Framework) *
3  * Author: The Belle II Collaboration *
4  * External Contributor: Wouter Hulsbergen *
5  * *
6  * See git log for contributors and copyright holders. *
7  * This file is licensed under LGPL-3.0, see LICENSE.md. *
8  **************************************************************************/
9 
10 #include <TMath.h>
11 
12 #include <framework/gearbox/Const.h>
13 #include <framework/logging/Logger.h>
14 
15 #include <analysis/VertexFitting/TreeFitter/HelixUtils.h>
16 
17 namespace TreeFitter {
18 
20  double L, double Bz,
21  Belle2::B2Vector3D& position,
22  Belle2::B2Vector3D& momentum, int& charge)
23  {
24  position = helix.getPositionAtArcLength2D(L);
25  momentum = helix.getMomentumAtArcLength2D(L, Bz);
26  charge = helix.getChargeSign();
27  }
28 
29  void HelixUtils::helixFromVertex(const Eigen::Matrix<double, 1, 6>& positionAndMomentum,
30  int charge, double Bz,
31  Belle2::Helix& helix,
32  double& L,
33  Eigen::Matrix<double, 5, 6>& jacobian)
34  {
35 
36 
37  Belle2::B2Vector3D position(positionAndMomentum(0),
38  positionAndMomentum(1),
39  positionAndMomentum(2));
40  Belle2::B2Vector3D momentum(positionAndMomentum(3),
41  positionAndMomentum(4),
42  positionAndMomentum(5));
43 
44  helix = Belle2::Helix(position, momentum, charge, Bz);
45  L = helix.getArcLength2DAtXY(positionAndMomentum(0),
46  positionAndMomentum(1));
47 
48  const double alpha = helix.getAlpha(Bz);
49 
50  //Copied from Belle2::UncertainHelix
51  // COMPLETELY WRONG SINCE IT ASSUMES IT'S IN THE.operator() PERIGEE,
52  // ONLY A PLACEHOLDER FOR NOW
53  // 1. Rotate to a system where phi0 = 0
54  Eigen::Matrix<double, 6, 6> jacobianRot = Eigen::Matrix<double, 6, 6>::Zero(6, 6);
55 
56  const double px = positionAndMomentum(3);
57  const double py = positionAndMomentum(4);
58  const double pt = hypot(px, py);
59  const double cosPhi0 = px / pt;
60  const double sinPhi0 = py / pt;
61 
62  // Passive rotation matrix by phi0:
63  jacobianRot(iX, iX) = cosPhi0;
64  jacobianRot(iX, iY) = sinPhi0;
65  jacobianRot(iY, iX) = -sinPhi0;
66  jacobianRot(iY, iY) = cosPhi0;
67  jacobianRot(iZ, iZ) = 1.0;
68 
69  jacobianRot(iPx, iPx) = cosPhi0;
70  jacobianRot(iPx, iPy) = sinPhi0;
71  jacobianRot(iPy, iPx) = -sinPhi0;
72  jacobianRot(iPy, iPy) = cosPhi0;
73  jacobianRot(iPz, iPz) = 1.0;
74 
75  // 2. Translate to perigee parameters on the position
76  const double pz = positionAndMomentum(5);
77  const double invPt = 1 / pt;
78  const double invPtSquared = invPt * invPt;
79  Eigen::Matrix<double, 5, 6> jacobianToHelixParameters = Eigen::Matrix<double, 5, 6>::Zero(5, 6);
80  jacobianToHelixParameters(iD0, iY) = -1;
81  jacobianToHelixParameters(iPhi0, iX) = charge * invPt / alpha;
82  jacobianToHelixParameters(iPhi0, iPy) = invPt;
83  jacobianToHelixParameters(iOmega, iPx) = -charge * invPtSquared / alpha;
84  jacobianToHelixParameters(iTanLambda, iPx) = - pz * invPtSquared;
85  jacobianToHelixParameters(iTanLambda, iPz) = invPt;
86  jacobianToHelixParameters(iZ0, iX) = - pz * invPt;
87  jacobianToHelixParameters(iZ0, iZ) = 1;
88  //
89  jacobian = jacobianToHelixParameters * jacobianRot;
90 
91  }
92 
93  std::string HelixUtils::helixParName(int i)
94  {
95  std::string rc ;
96  switch (i) {
97  case 1 : rc = "d0 : " ; break ;
98  case 2 : rc = "phi0 : " ; break ;
99  case 3 : rc = "omega : " ; break ;
100  case 4 : rc = "z0 : " ; break ;
101  case 5 : rc = "tandip: " ; break ;
102  case 6 : rc = "L : " ; break ;
103  }
104  return rc ;
105  }
106 
107  std::string HelixUtils::vertexParName(int i)
108  {
109  std::string rc ;
110  switch (i) {
111  case 1 : rc = "x : " ; break ;
112  case 2 : rc = "y : " ; break ;
113  case 3 : rc = "z : " ; break ;
114  case 4 : rc = "px : " ; break ;
115  case 5 : rc = "py : " ; break ;
116  case 6 : rc = "pz : " ; break ;
117  }
118  return rc ;
119  }
120 
121  void HelixUtils::printVertexPar(const Belle2::B2Vector3D& position, const Belle2::B2Vector3D& momentum, int charge)
122  {
123  for (int i = 0; i < 3; ++i)
124  B2INFO(vertexParName(i + 1).c_str() << position[i]);
125  for (int i = 0; i < 3; ++i)
126  B2INFO(vertexParName(i + 4).c_str() << momentum[i]);
127  B2INFO("charge: " << charge);
128 
129  }
130 
131  void HelixUtils::getHelixAndJacobianFromVertexNumerical(const Eigen::Matrix<double, 1, 6>& positionAndMom,
132  int charge, double Bz,
133  Belle2::Helix& helix,
134  Eigen::Matrix<double, 5, 6>& jacobian)
135  {
136 
137  Belle2::B2Vector3D position(positionAndMom(0),
138  positionAndMom(1),
139  positionAndMom(2));
140 
141  Belle2::B2Vector3D momentum(positionAndMom(3),
142  positionAndMom(4),
143  positionAndMom(5));
144 
145  helix = Belle2::Helix(position, momentum, charge, Bz);
146 
147  // numeric calculation of the jacobian
148  Belle2::Helix helixPlusDelta;
149 
150  double delta = 1e-5;// this is arbitrary, only needs to be small
151 
152  Belle2::B2Vector3D postmp;
153  Belle2::B2Vector3D momtmp;
154 
155  for (int jin = 0; jin < 6; ++jin) {
156  for (int i = 0; i < 3; ++i) {
157  postmp[i] = positionAndMom(i);
158  momtmp[i] = positionAndMom(i + 3);
159  }
160  if (jin < 3) {
161  postmp[jin] += delta;
162  } else {
163  momtmp[jin - 3] += delta;
164  }
165 
166  helixPlusDelta = Belle2::Helix(postmp, momtmp, charge, Bz);
167  jacobian(iD0, jin) = (helixPlusDelta.getD0() - helix.getD0()) / delta ;
168  jacobian(iPhi0, jin) = (helixPlusDelta.getPhi0() - helix.getPhi0()) / delta ;
169  jacobian(iOmega, jin) = (helixPlusDelta.getOmega() - helix.getOmega()) / delta ;
170  jacobian(iZ0, jin) = (helixPlusDelta.getZ0() - helix.getZ0()) / delta ;
171  jacobian(iTanLambda, jin) = (helixPlusDelta.getTanLambda() - helix.getTanLambda()) / delta ;
172 
173  // jacobian[iArcLength2D][jin] = (LPlusDelta - L) / delta ;
174  }
175  }
176 
178  const Eigen::Matrix<double, 1, 6>& positionAndMom,
179  int charge, double Bz,
180  const Belle2::Helix& helix,
181  Eigen::Matrix<double, 5, 6>& jacobian,
182  double delta
183  )
184  {
185  // numeric calculation of the jacobian
186  Belle2::Helix helixPlusDelta;
187 
188  Belle2::B2Vector3D postmp;
189  Belle2::B2Vector3D momtmp;
190 
191  for (int jin = 0; jin < 6; ++jin) {
192  for (int i = 0; i < 3; ++i) {
193  postmp[i] = positionAndMom(i);
194  momtmp[i] = positionAndMom(i + 3);
195  }
196 
197  if (jin < 3) {
198  postmp[jin] += delta;
199  } else {
200  momtmp[jin - 3] += delta;
201  }
202 
203  helixPlusDelta = Belle2::Helix(postmp, momtmp, charge, Bz);
204  jacobian(iD0, jin) = (helixPlusDelta.getD0() - helix.getD0()) / delta ;
205  jacobian(iPhi0, jin) = (helixPlusDelta.getPhi0() - helix.getPhi0()) / delta ;
206  jacobian(iOmega, jin) = (helixPlusDelta.getOmega() - helix.getOmega()) / delta ;
207  jacobian(iZ0, jin) = (helixPlusDelta.getZ0() - helix.getZ0()) / delta ;
208  jacobian(iTanLambda, jin) = (helixPlusDelta.getTanLambda() - helix.getTanLambda()) / delta ;
209  }
210 
211  }
212 
213  inline double sqr(double x) { return x * x ; }
214 
215  double HelixUtils::phidomain(const double phi)
216  {
217  double rc = phi ;
218  if (phi < -TMath::Pi()) rc += TMath::TwoPi();
219  else if (phi > TMath::Pi()) rc -= TMath::TwoPi();
220  return rc ;
221  }
222 
223  //POCA between two tracks
224  double HelixUtils::helixPoca(const Belle2::Helix& helix1,
225  const Belle2::Helix& helix2,
226  double& flt1, double& flt2,
227  Belle2::B2Vector3D& vertex, bool parallel)
228  {
229 
230  double d0_1 = helix1.getD0();
231  double phi0_1 = helix1.getPhi0();
232  double omega_1 = helix1.getOmega();
233  double z0_1 = helix1.getZ0();
234  double tandip_1 = helix1.getTanLambda();
235  double cosdip_1 = cos(atan(tandip_1)) ; // can do that faster
236 
237  double d0_2 = helix2.getD0();
238  double phi0_2 = helix2.getPhi0();
239  double omega_2 = helix2.getOmega();
240  double z0_2 = helix2.getZ0();
241  double tandip_2 = helix2.getTanLambda();
242  double cosdip_2 = cos(atan(tandip_2)) ; // can do that faster
243 
244  double r_1 = 1 / omega_1 ;
245  double r_2 = 1 / omega_2 ;
246 
247  double x0_1 = - (r_1 + d0_1) * sin(phi0_1) ;
248  double y0_1 = (r_1 + d0_1) * cos(phi0_1) ;
249 
250  double x0_2 = - (r_2 + d0_2) * sin(phi0_2) ;
251  double y0_2 = (r_2 + d0_2) * cos(phi0_2) ;
252 
253  double deltax = x0_2 - x0_1 ;
254  double deltay = y0_2 - y0_1 ;
255 
256  double phi1[2] ;
257  double phi2[2] ;
258  int nsolutions = 1;
259 
260  // the phi of the 'intersection'.
261  const double pi = TMath::Pi();
262  double phi = - atan2(deltax, deltay) ;
263  double phinot = phi > 0 ? phi - pi : phi + pi ;
264  phi1[0] = r_1 < 0 ? phi : phinot ;
265  phi2[0] = r_2 > 0 ? phi : phinot ;
266 
267  double R1 = fabs(r_1) ;
268  double R2 = fabs(r_2) ;
269  double Rmin = R1 < R2 ? R1 : R2 ;
270  double Rmax = R1 > R2 ? R1 : R2 ;
271  double dX = sqrt(deltax * deltax + deltay * deltay) ;
272 
273  if (!parallel && dX + Rmin > Rmax && dX < R1 + R2) {
274  // there are two solutions
275  nsolutions = 2 ;
276  double ddphi1 = acos((dX * dX - R2 * R2 + R1 * R1) / (2.*dX * R1)) ;
277  phi1[1] = phidomain(phi1[0] + ddphi1) ;
278  phi1[0] = phidomain(phi1[0] - ddphi1) ;
279 
280  double ddphi2 = acos((dX * dX - R1 * R1 + R2 * R2) / (2.*dX * R2)) ;
281  phi2[1] = phidomain(phi2[0] - ddphi2) ;
282  phi2[0] = phidomain(phi2[0] + ddphi2) ;
283 
284  } else if (dX < Rmax) {
285  if (R1 > R2) phi2[0] = r_2 < 0 ? phi : phinot ;
286  else phi1[0] = r_1 < 0 ? phi : phinot ;
287  }
288 
289  // find the best solution for z by running multiples of 2_pi
290  double z1(0), z2(0) ;
291  bool first(true) ;
292  int ibest = 0;
293  const int ncirc(2) ;
294  for (int i = 0; i < nsolutions; ++i) {
295  double dphi1 = phidomain(phi1[i] - phi0_1) ;
296  double dphi2 = phidomain(phi2[i] - phi0_2) ;
297  for (int n1 = 1 - ncirc; n1 <= 1 + ncirc ; ++n1) {
298  double l1 = (dphi1 + n1 * TMath::TwoPi()) / omega_1 ;
299  double tmpz1 = (z0_1 + l1 * tandip_1) ;
300  if (n1 == 0 || fabs(tmpz1) < 100) {
301  for (int n2 = 1 - ncirc ; n2 <= 1 + ncirc; ++n2) {
302  double l2 = (dphi2 + n2 * TMath::TwoPi()) / omega_2 ;
303  double tmpz2 = (z0_2 + l2 * tandip_2) ;
304  if (n2 == 0 || fabs(tmpz2) < 100) {
305  if (first || fabs(tmpz1 - tmpz2) < fabs(z1 - z2)) {
306  ibest = i ;
307  first = false ;
308  z1 = tmpz1 ;
309  z2 = tmpz2 ;
310  flt1 = l1 / cosdip_1 ;
311  flt2 = l2 / cosdip_2 ;
312  }
313  }
314  }
315  }
316  }
317  }
318 
319  double x1 = r_1 * sin(phi1[ibest]) + x0_1 ;
320  double y1 = -r_1 * cos(phi1[ibest]) + y0_1 ;
321 
322  double x2 = r_2 * sin(phi2[ibest]) + x0_2 ;
323  double y2 = -r_2 * cos(phi2[ibest]) + y0_2 ;
324 
325  vertex.SetX(0.5 * (x1 + x2));
326  vertex.SetY(0.5 * (y1 + y2));
327  vertex.SetZ(0.5 * (z1 + z2));
328 
329  return sqrt(sqr(x2 - x1) + sqr(y2 - y1) + sqr(z2 - z1)) ;
330  }
331 
332  //POCA between a track and a point
334  const Belle2::B2Vector3D& point,
335  double& flt)
336  {
337  double d0 = helix.getD0();
338  double phi0 = helix.getPhi0();
339  double omega = helix.getOmega();
340  double z0 = helix.getZ0();
341  double tandip = helix.getTanLambda();
342  double cosdip = cos(atan(tandip)) ; // can do that faster
343 
344  double r = 1 / omega ;
345 
346  double x0 = - (r + d0) * sin(phi0) ;
347  double y0 = (r + d0) * cos(phi0) ;
348 
349  double deltax = x0 - point.X() ;
350  double deltay = y0 - point.Y() ;
351 
352  const double pi = TMath::Pi();
353  double phi = - atan2(deltax, deltay) ;
354  if (r < 0) phi = phi > 0 ? phi - pi : phi + pi ;
355 
356  // find the best solution for z by running multiples of 2_pi
357  double x = r * sin(phi) + x0 ;
358  double y = -r * cos(phi) + y0 ;
359  double z(0) ;
360  bool first(true) ;
361  const int ncirc(2) ;
362  double dphi = phidomain(phi - phi0) ;
363  for (int n = 1 - ncirc; n <= 1 + ncirc ; ++n) {
364  double l = (dphi + n * TMath::TwoPi()) / omega ;
365  double tmpz = (z0 + l * tandip) ;
366  if (first || fabs(tmpz - point.z()) < fabs(z - point.z())) {
367  first = false ;
368  z = tmpz ;
369  flt = l / cosdip ;
370  }
371  }
372  return sqrt(sqr(x - point.x()) + sqr(y - point.y()) + sqr(z - point.z())) ;
373  }
374 
375  void HelixUtils::getJacobianToCartesianFrameworkHelix(Eigen::Matrix<double, 5, 6>& jacobian,
376  const double x,
377  const double y,
378  const double z __attribute__((unused)),
379  const double px,
380  const double py,
381  const double pz,
382  const double bfield,
383  const double charge
384  )
385 
386  {
387  const double alpha = 1.0 / (bfield * Belle2::Const::speedOfLight) * 1E4;
388  const double aq = charge / alpha;
389 
390  const double pt = std::hypot(px, py);
391  const double pt2 = pt * pt;
392  const double pt3 = pt2 * pt;
393  const double aq2 = aq * aq;
394 
395  const double x2 = x * x;
396  const double y2 = y * y;
397  const double r = x2 + y2;
398 
399  const double px2 = px * px;
400  const double py2 = py * py;
401 
402  const double px0 = px - aq * y;
403  const double py0 = py + aq * x;
404 
405  const double pt02 = px0 * px0 + py0 * py0;
406  const double pt0 = std::sqrt(pt02);
407  double sqrt13 = pt0 / pt;
408 
409  // D d0 / Dx_i
410  jacobian(0, 0) = py0 / pt0;
411  jacobian(0, 1) = -px0 / pt0;
412  jacobian(0, 2) = 0;
413  jacobian(0, 3) = (-(y * (aq2 * r + 2 * aq * py * x + 2 * py2 * (1 + sqrt13))) - px * (2 * py * x * (1 + sqrt13) + aq * (y2 *
414  (-1 + sqrt13) + x2 * (1 + sqrt13)))) /
415  (pt2 * pt0 * (1 + sqrt13) * (1 + sqrt13));
416 
417  jacobian(0, 4) = (2 * px2 * x * (1 + sqrt13) + 2 * px * y * (py - aq * x + py * sqrt13) + aq * (aq * r * x - py * (x2 *
418  (-1 + sqrt13) + y2 * (1 + sqrt13)))) /
419  (pt2 * pt0 * (1 + sqrt13) * (1 + sqrt13));
420  jacobian(0, 5) = 0;
421 
422  // D phi0 / Dx_i0;
423  jacobian(1, 0) = aq * px0 / pt02;
424  jacobian(1, 1) = aq * py0 / pt02;
425  jacobian(1, 2) = 0;
426  jacobian(1, 3) = -py0 / pt02;
427  jacobian(1, 4) = px0 / pt02;
428  jacobian(1, 5) = 0;
429 
430  // D omega / Dx_i
431  jacobian(2, 0) = 0;
432  jacobian(2, 1) = 0;
433  jacobian(2, 2) = 0;
434  jacobian(2, 3) = - aq * px / pt3;
435  jacobian(2, 4) = - aq * py / pt3;
436  jacobian(2, 5) = 0;
437 
438  // D z0 / Dx_i
439  jacobian(3, 0) = -pz * px0 / pt02;
440  jacobian(3, 1) = -pz * py0 / pt02;
441  jacobian(3, 2) = 1;
442  jacobian(3, 3) = (pz * (px2 * x - py * (aq * r + py * x) + 2 * px * py * y)) / (pt2 * pt02);
443  jacobian(3, 4) = (pz * (px * (aq * r + 2 * py * x) - px2 * y + py2 * y)) / (pt2 * pt02);
444  jacobian(3, 5) = std::atan2(-(aq * (px * x + py * y)), (px2 + py * py0 - aq * px * y)) / aq; //pt on num. and denom cancels.
445 
446  // D tan lambda / Dx_i
447  jacobian(4, 0) = 0;
448  jacobian(4, 1) = 0;
449  jacobian(4, 2) = 0;
450  jacobian(4, 3) = -pz * px / pt3;
451  jacobian(4, 4) = -pz * py / pt3;
452  jacobian(4, 5) = 1. / pt;
453  }
454 
455 }
DataType y() const
access variable Y (= .at(1) without boundary check)
Definition: B2Vector3.h:421
DataType z() const
access variable Z (= .at(2) without boundary check)
Definition: B2Vector3.h:423
DataType X() const
access variable X (= .at(0) without boundary check)
Definition: B2Vector3.h:425
DataType Y() const
access variable Y (= .at(1) without boundary check)
Definition: B2Vector3.h:427
DataType x() const
access variable X (= .at(0) without boundary check)
Definition: B2Vector3.h:419
static const double speedOfLight
[cm/ns]
Definition: Const.h:575
This class represents an ideal helix in perigee parameterization.
Definition: Helix.h:59
short getChargeSign() const
Return track charge sign (1, 0 or -1).
Definition: Helix.cc:113
double getArcLength2DAtXY(const double &x, const double &y) const
Calculates the two dimensional arc length at which the circle in the xy projection is closest to the ...
Definition: Helix.cc:140
TVector3 getMomentumAtArcLength2D(const double &arcLength2D, const double &bz) const
Calculates the momentum vector at the given two dimensional arc length.
Definition: Helix.cc:269
static double getAlpha(const double bZ)
Calculates the alpha value for a given magnetic field in Tesla.
Definition: Helix.cc:108
double getOmega() const
Getter for omega, which is a signed curvature measure of the track.
Definition: Helix.h:387
double getD0() const
Getter for d0, which is the signed distance to the perigee in the r-phi plane.
Definition: Helix.h:372
double getTanLambda() const
Getter for tan lambda, which is the z over two dimensional arc length slope of the track.
Definition: Helix.h:393
TVector3 getPositionAtArcLength2D(const double &arcLength2D) const
Calculates the position on the helix at the given two dimensional arc length.
Definition: Helix.cc:200
double getZ0() const
Getter for z0, which is the z coordinate of the perigee.
Definition: Helix.h:390
double getPhi0() const
Getter for phi0, which is the azimuth angle of the transverse momentum at the perigee.
Definition: Helix.h:378
static std::string vertexParName(int i)
map of the vertex parameters by list index
Definition: HelixUtils.cc:107
static void helixFromVertex(const Eigen::Matrix< double, 1, 6 > &positionAndMomentum, int charge, double Bz, Belle2::Helix &helix, double &L, Eigen::Matrix< double, 5, 6 > &jacobian)
vertex --> helix
Definition: HelixUtils.cc:29
static void getHelixAndJacobianFromVertexNumerical(const Eigen::Matrix< double, 1, 6 > &positionAndMom, int charge, double Bz, Belle2::Helix &helix, Eigen::Matrix< double, 5, 6 > &jacobian)
get helix and jacobian from a vertex
Definition: HelixUtils.cc:131
static void printVertexPar(const Belle2::B2Vector3D &position, const Belle2::B2Vector3D &momentum, int charge)
Print the vertex parameters.
Definition: HelixUtils.cc:121
static double helixPoca(const Belle2::Helix &helix1, const Belle2::Helix &helix2, double &flt1, double &flt2, Belle2::B2Vector3D &vertex, bool parallel=false)
POCA between two tracks.
Definition: HelixUtils.cc:224
static void vertexFromHelix(const Belle2::Helix &helix, double L, double Bz, Belle2::B2Vector3D &position, Belle2::B2Vector3D &momentum, int &charge)
helix --> vertex
Definition: HelixUtils.cc:19
static void getJacobianFromVertexNumerical(const Eigen::Matrix< double, 1, 6 > &positionAndMom, int charge, double Bz, const Belle2::Helix &helix, Eigen::Matrix< double, 5, 6 > &jacobian, double delta=1e-5)
get jacobian from a vertex
Definition: HelixUtils.cc:177
static std::string helixParName(int i)
map of the helix parameters by list index
Definition: HelixUtils.cc:93
static double phidomain(const double phi)
the domain of phi
Definition: HelixUtils.cc:215
static void getJacobianToCartesianFrameworkHelix(Eigen::Matrix< double, 5, 6 > &jacobian, const double x, const double y, const double z, const double px, const double py, const double pz, const double bfield, const double charge)
get the jacobian dh={helix pars}/dx={x,y,z,px,py,pz} for the implementation of the framework helix.
Definition: HelixUtils.cc:375