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