12 #include <framework/gearbox/Const.h>
13 #include <framework/logging/Logger.h>
15 #include <analysis/VertexFitting/TreeFitter/HelixUtils.h>
17 namespace TreeFitter {
30 int charge,
double Bz,
33 Eigen::Matrix<double, 5, 6>& jacobian)
38 positionAndMomentum(1),
39 positionAndMomentum(2));
41 positionAndMomentum(4),
42 positionAndMomentum(5));
46 positionAndMomentum(1));
48 const double alpha = helix.
getAlpha(Bz);
54 Eigen::Matrix<double, 6, 6> jacobianRot = Eigen::Matrix<double, 6, 6>::Zero(6, 6);
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;
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;
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;
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;
89 jacobian = jacobianToHelixParameters * jacobianRot;
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 ;
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 ;
123 for (
int i = 0; i < 3; ++i)
125 for (
int i = 0; i < 3; ++i)
127 B2INFO(
"charge: " << charge);
132 int charge,
double Bz,
134 Eigen::Matrix<double, 5, 6>& jacobian)
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);
161 postmp[jin] += delta;
163 momtmp[jin - 3] += delta;
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 ;
178 const Eigen::Matrix<double, 1, 6>& positionAndMom,
179 int charge,
double Bz,
181 Eigen::Matrix<double, 5, 6>& jacobian,
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);
198 postmp[jin] += delta;
200 momtmp[jin - 3] += delta;
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 ;
213 inline double sqr(
double x) {
return x * x ; }
218 if (phi < -TMath::Pi()) rc += TMath::TwoPi();
219 else if (phi > TMath::Pi()) rc -= TMath::TwoPi();
226 double& flt1,
double& flt2,
230 double d0_1 = helix1.
getD0();
231 double phi0_1 = helix1.
getPhi0();
233 double z0_1 = helix1.
getZ0();
235 double cosdip_1 = cos(atan(tandip_1)) ;
237 double d0_2 = helix2.
getD0();
238 double phi0_2 = helix2.
getPhi0();
240 double z0_2 = helix2.
getZ0();
242 double cosdip_2 = cos(atan(tandip_2)) ;
244 double r_1 = 1 / omega_1 ;
245 double r_2 = 1 / omega_2 ;
247 double x0_1 = - (r_1 + d0_1) * sin(phi0_1) ;
248 double y0_1 = (r_1 + d0_1) * cos(phi0_1) ;
250 double x0_2 = - (r_2 + d0_2) * sin(phi0_2) ;
251 double y0_2 = (r_2 + d0_2) * cos(phi0_2) ;
253 double deltax = x0_2 - x0_1 ;
254 double deltay = y0_2 - y0_1 ;
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 ;
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) ;
273 if (!parallel && dX + Rmin > Rmax && dX < R1 + R2) {
276 double ddphi1 = acos((dX * dX - R2 * R2 + R1 * R1) / (2.*dX * R1)) ;
280 double ddphi2 = acos((dX * dX - R1 * R1 + R2 * R2) / (2.*dX * R2)) ;
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 ;
290 double z1(0), z2(0) ;
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)) {
310 flt1 = l1 / cosdip_1 ;
311 flt2 = l2 / cosdip_2 ;
319 double x1 = r_1 * sin(phi1[ibest]) + x0_1 ;
320 double y1 = -r_1 * cos(phi1[ibest]) + y0_1 ;
322 double x2 = r_2 * sin(phi2[ibest]) + x0_2 ;
323 double y2 = -r_2 * cos(phi2[ibest]) + y0_2 ;
325 vertex.SetX(0.5 * (x1 + x2));
326 vertex.SetY(0.5 * (y1 + y2));
327 vertex.SetZ(0.5 * (z1 + z2));
329 return sqrt(sqr(x2 - x1) + sqr(y2 - y1) + sqr(z2 - z1)) ;
337 double d0 = helix.
getD0();
340 double z0 = helix.
getZ0();
342 double cosdip = cos(atan(tandip)) ;
344 double r = 1 / omega ;
346 double x0 = - (r + d0) * sin(phi0) ;
347 double y0 = (r + d0) * cos(phi0) ;
349 double deltax = x0 - point.
X() ;
350 double deltay = y0 - point.
Y() ;
352 const double pi = TMath::Pi();
353 double phi = - atan2(deltax, deltay) ;
354 if (r < 0) phi = phi > 0 ? phi - pi : phi + pi ;
357 double x = r * sin(phi) + x0 ;
358 double y = -r * cos(phi) + y0 ;
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())) {
372 return sqrt(sqr(x - point.
x()) + sqr(y - point.
y()) + sqr(z - point.
z())) ;
378 const double z __attribute__((unused)),
388 const double aq = charge / alpha;
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;
395 const double x2 = x * x;
396 const double y2 = y * y;
397 const double r = x2 + y2;
399 const double px2 = px * px;
400 const double py2 = py * py;
402 const double px0 = px - aq * y;
403 const double py0 = py + aq * x;
405 const double pt02 = px0 * px0 + py0 * py0;
406 const double pt0 = std::sqrt(pt02);
407 double sqrt13 = pt0 / pt;
410 jacobian(0, 0) = py0 / pt0;
411 jacobian(0, 1) = -px0 / pt0;
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));
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));
423 jacobian(1, 0) = aq * px0 / pt02;
424 jacobian(1, 1) = aq * py0 / pt02;
426 jacobian(1, 3) = -py0 / pt02;
427 jacobian(1, 4) = px0 / pt02;
434 jacobian(2, 3) = - aq * px / pt3;
435 jacobian(2, 4) = - aq * py / pt3;
439 jacobian(3, 0) = -pz * px0 / pt02;
440 jacobian(3, 1) = -pz * py0 / pt02;
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;
450 jacobian(4, 3) = -pz * px / pt3;
451 jacobian(4, 4) = -pz * py / pt3;
452 jacobian(4, 5) = 1. / pt;
DataType y() const
access variable Y (= .at(1) without boundary check)
DataType z() const
access variable Z (= .at(2) without boundary check)
DataType X() const
access variable X (= .at(0) without boundary check)
DataType Y() const
access variable Y (= .at(1) without boundary check)
DataType x() const
access variable X (= .at(0) without boundary check)
static const double speedOfLight
[cm/ns]
This class represents an ideal helix in perigee parameterization.
short getChargeSign() const
Return track charge sign (1, 0 or -1).
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 ...
TVector3 getMomentumAtArcLength2D(const double &arcLength2D, const double &bz) const
Calculates the momentum vector at the given two dimensional arc length.
static double getAlpha(const double bZ)
Calculates the alpha value for a given magnetic field in Tesla.
double getOmega() const
Getter for omega, which is a signed curvature measure of the track.
double getD0() const
Getter for d0, which is the signed distance to the perigee in the r-phi plane.
double getTanLambda() const
Getter for tan lambda, which is the z over two dimensional arc length slope of the track.
TVector3 getPositionAtArcLength2D(const double &arcLength2D) const
Calculates the position on the helix at the given two dimensional arc length.
double getZ0() const
Getter for z0, which is the z coordinate of the perigee.
double getPhi0() const
Getter for phi0, which is the azimuth angle of the transverse momentum at the perigee.
static std::string vertexParName(int i)
map of the vertex parameters by list index
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
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
static void printVertexPar(const Belle2::B2Vector3D &position, const Belle2::B2Vector3D &momentum, int charge)
Print the vertex parameters.
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.
static void vertexFromHelix(const Belle2::Helix &helix, double L, double Bz, Belle2::B2Vector3D &position, Belle2::B2Vector3D &momentum, int &charge)
helix --> vertex
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
static std::string helixParName(int i)
map of the helix parameters by list index
static double phidomain(const double phi)
the domain of phi
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.