12#include <framework/gearbox/Const.h>
13#include <framework/logging/Logger.h>
15#include <analysis/VertexFitting/TreeFitter/HelixUtils.h>
18#include <initializer_list>
25 ROOT::Math::XYZVector& position,
26 ROOT::Math::XYZVector& momentum,
int& charge)
28 position = helix.getPositionAtArcLength2D(L);
29 momentum = helix.getMomentumAtArcLength2D(L, Bz);
30 charge = helix.getChargeSign();
34 int charge,
double Bz,
37 Eigen::Matrix<double, 5, 6>& jacobian)
40 helix =
Belle2::Helix(ROOT::Math::XYZVector(positionAndMomentum(0), positionAndMomentum(1), positionAndMomentum(2)),
41 ROOT::Math::XYZVector(positionAndMomentum(3), positionAndMomentum(4), positionAndMomentum(5)),
44 L = helix.getArcLength2DAtXY(positionAndMomentum(0),
45 positionAndMomentum(1));
47 const double alpha = helix.getAlpha(Bz);
53 Eigen::Matrix<double, 6, 6> jacobianRot = Eigen::Matrix<double, 6, 6>::Zero(6, 6);
55 const double px = positionAndMomentum(3);
56 const double py = positionAndMomentum(4);
57 const double pt = hypot(px, py);
58 const double cosPhi0 = px / pt;
59 const double sinPhi0 = py / pt;
62 jacobianRot(iX, iX) = cosPhi0;
63 jacobianRot(iX, iY) = sinPhi0;
64 jacobianRot(iY, iX) = -sinPhi0;
65 jacobianRot(iY, iY) = cosPhi0;
66 jacobianRot(iZ, iZ) = 1.0;
68 jacobianRot(iPx, iPx) = cosPhi0;
69 jacobianRot(iPx, iPy) = sinPhi0;
70 jacobianRot(iPy, iPx) = -sinPhi0;
71 jacobianRot(iPy, iPy) = cosPhi0;
72 jacobianRot(iPz, iPz) = 1.0;
75 const double pz = positionAndMomentum(5);
76 const double invPt = 1 / pt;
77 const double invPtSquared = invPt * invPt;
78 Eigen::Matrix<double, 5, 6> jacobianToHelixParameters = Eigen::Matrix<double, 5, 6>::Zero(5, 6);
79 jacobianToHelixParameters(iD0, iY) = -1;
80 jacobianToHelixParameters(iPhi0, iX) = charge * invPt / alpha;
81 jacobianToHelixParameters(iPhi0, iPy) = invPt;
82 jacobianToHelixParameters(iOmega, iPx) = -charge * invPtSquared / alpha;
83 jacobianToHelixParameters(iTanLambda, iPx) = - pz * invPtSquared;
84 jacobianToHelixParameters(iTanLambda, iPz) = invPt;
85 jacobianToHelixParameters(iZ0, iX) = - pz * invPt;
86 jacobianToHelixParameters(iZ0, iZ) = 1;
88 jacobian = jacobianToHelixParameters * jacobianRot;
96 case 1 : rc =
"d0 : " ; break ;
97 case 2 : rc =
"phi0 : " ; break ;
98 case 3 : rc =
"omega : " ; break ;
99 case 4 : rc =
"z0 : " ; break ;
100 case 5 : rc =
"tandip: " ; break ;
101 case 6 : rc =
"L : " ; break ;
110 case 1 : rc =
"x : " ; break ;
111 case 2 : rc =
"y : " ; break ;
112 case 3 : rc =
"z : " ; break ;
113 case 4 : rc =
"px : " ; break ;
114 case 5 : rc =
"py : " ; break ;
115 case 6 : rc =
"pz : " ; break ;
122 for (
int i = 0; i < 3; ++i)
124 for (
int i = 0; i < 3; ++i)
126 B2INFO(
"charge: " << charge);
131 int charge,
double Bz,
133 Eigen::Matrix<double, 5, 6>& jacobian)
136 helix =
Belle2::Helix(ROOT::Math::XYZVector(positionAndMom(0), positionAndMom(1), positionAndMom(2)),
137 ROOT::Math::XYZVector(positionAndMom(3), positionAndMom(4), positionAndMom(5)),
145 ROOT::Math::XYZVector postmp;
146 ROOT::Math::XYZVector momtmp;
148 for (
int jin = 0; jin < 6; ++jin) {
149 postmp.SetCoordinates(positionAndMom(0), positionAndMom(1), positionAndMom(2));
150 momtmp.SetCoordinates(positionAndMom(3), positionAndMom(4), positionAndMom(5));
151 if (jin == 0) postmp.SetX(postmp.X() + delta);
152 if (jin == 1) postmp.SetY(postmp.Y() + delta);
153 if (jin == 2) postmp.SetZ(postmp.Z() + delta);
154 if (jin == 3) momtmp.SetX(momtmp.X() + delta);
155 if (jin == 4) momtmp.SetY(momtmp.Y() + delta);
156 if (jin == 5) momtmp.SetZ(momtmp.Z() + delta);
159 jacobian(iD0, jin) = (helixPlusDelta.getD0() - helix.getD0()) / delta ;
160 jacobian(iPhi0, jin) = (helixPlusDelta.getPhi0() - helix.getPhi0()) / delta ;
161 jacobian(iOmega, jin) = (helixPlusDelta.getOmega() - helix.getOmega()) / delta ;
162 jacobian(iZ0, jin) = (helixPlusDelta.getZ0() - helix.getZ0()) / delta ;
163 jacobian(iTanLambda, jin) = (helixPlusDelta.getTanLambda() - helix.getTanLambda()) / delta ;
170 const Eigen::Matrix<double, 1, 6>& positionAndMom,
171 int charge,
double Bz,
173 Eigen::Matrix<double, 5, 6>& jacobian,
180 ROOT::Math::XYZVector postmp;
181 ROOT::Math::XYZVector momtmp;
183 for (
int jin = 0; jin < 6; ++jin) {
184 postmp.SetCoordinates(positionAndMom(0), positionAndMom(1), positionAndMom(2));
185 momtmp.SetCoordinates(positionAndMom(3), positionAndMom(4), positionAndMom(5));
186 if (jin == 0) postmp.SetX(postmp.X() + delta);
187 if (jin == 1) postmp.SetY(postmp.Y() + delta);
188 if (jin == 2) postmp.SetZ(postmp.Z() + delta);
189 if (jin == 3) momtmp.SetX(momtmp.X() + delta);
190 if (jin == 4) momtmp.SetY(momtmp.Y() + delta);
191 if (jin == 5) momtmp.SetZ(momtmp.Z() + delta);
194 jacobian(iD0, jin) = (helixPlusDelta.getD0() - helix.getD0()) / delta ;
195 jacobian(iPhi0, jin) = (helixPlusDelta.getPhi0() - helix.getPhi0()) / delta ;
196 jacobian(iOmega, jin) = (helixPlusDelta.getOmega() - helix.getOmega()) / delta ;
197 jacobian(iZ0, jin) = (helixPlusDelta.getZ0() - helix.getZ0()) / delta ;
198 jacobian(iTanLambda, jin) = (helixPlusDelta.getTanLambda() - helix.getTanLambda()) / delta ;
203 inline double sqr(
double x) {
return x * x ; }
208 if (phi < -TMath::Pi()) rc += TMath::TwoPi();
209 else if (phi > TMath::Pi()) rc -= TMath::TwoPi();
216 double& flt1,
double& flt2,
220 const double d0_1 = helix1.getD0();
221 const double phi0_1 = helix1.getPhi0();
222 const double omega_1 = helix1.getOmega();
224 const double d0_2 = helix2.getD0();
225 const double phi0_2 = helix2.getPhi0();
226 const double omega_2 = helix2.getOmega();
229 const double r_1 = 1 / omega_1 ;
230 const double r_2 = 1 / omega_2 ;
234 const double x0_1 = (r_1 + d0_1) * sin(phi0_1) ;
235 const double y0_1 = -(r_1 + d0_1) * cos(phi0_1) ;
237 const double x0_2 = (r_2 + d0_2) * sin(phi0_2) ;
238 const double y0_2 = -(r_2 + d0_2) * cos(phi0_2) ;
241 const double deltax = x0_2 - x0_1 ;
242 const double deltay = y0_2 - y0_1 ;
250 const double phi = - atan2(deltax, deltay) ;
251 const double phinot = phi > 0 ? phi - TMath::Pi() : phi + TMath::Pi() ;
252 phi1[0] = r_1 < 0 ? phi : phinot ;
253 phi2[0] = r_2 > 0 ? phi : phinot ;
256 const double R1 = fabs(r_1) ;
257 const double R2 = fabs(r_2) ;
258 const double Rmin = R1 < R2 ? R1 : R2 ;
259 const double Rmax = R1 > R2 ? R1 : R2 ;
260 const double dX = hypot(deltax, deltay) ;
262 if (!parallel && dX + Rmin > Rmax && dX < R1 + R2) {
267 const double ddphi1 = acos((dX * dX - R2 * R2 + R1 * R1) / (2.*dX * R1)) ;
271 const double ddphi2 = acos((dX * dX - R1 * R1 + R2 * R2) / (2.*dX * R2)) ;
275 }
else if (dX < Rmax) {
277 if (R1 > R2) phi2[0] = r_2 < 0 ? phi : phinot ;
278 else phi1[0] = r_1 < 0 ? phi : phinot ;
284 double x1[2], y1[2], x2[2], y2[2];
285 for (
int i = 0; i < nsolutions; i++) {
286 x1[i] = r_1 * sin(phi1[i]) + x0_1 ;
287 y1[i] = -r_1 * cos(phi1[i]) + y0_1 ;
288 x2[i] = r_2 * sin(phi2[i]) + x0_2 ;
289 y2[i] = -r_2 * cos(phi2[i]) + y0_2 ;
296 const int nturnsmax = 10;
299 for (
int i = 0; i < nsolutions; ++i) {
300 const double l1 = helix1.getArcLength2DAtXY(x1[i], y1[i]);
301 const double l2 = helix2.getArcLength2DAtXY(x2[i], y2[i]);
304 std::vector<double> z1s;
305 for (
int n1 = 0; n1 <= nturnsmax; ++n1) {
308 for (
int sn1 : {n1, -n1}) {
309 const double tmpz1 = helix1.getPositionAtArcLength2D(l1 + sn1 * TMath::TwoPi() / omega_1).Z();
310 if (sn1 == 0 || (-82 <= tmpz1 && tmpz1 <= 158)) {
312 z1s.push_back(tmpz1);
324 for (
int n2 = 0; n2 <= nturnsmax; ++n2) {
327 for (
int sn2 : {n2, -n2}) {
328 const double tmpz2 = helix2.getPositionAtArcLength2D(l2 + sn2 * TMath::TwoPi() / omega_2).Z();
329 if (sn2 == 0 || (-82 <= tmpz2 && tmpz2 <= 158)) {
333 const auto i1best = std::min_element(
334 z1s.cbegin(), z1s.cend(), [&tmpz2](
const double & z1a,
const double & z1b) {
335 return fabs(z1a - tmpz2) < fabs(z1b - tmpz2);
337 const double tmpz1 = *i1best;
339 if (first || fabs(tmpz1 - tmpz2) < fabs(z1 - z2)) {
357 vertex.SetX(0.5 * (x1[ibest] + x2[ibest]));
358 vertex.SetY(0.5 * (y1[ibest] + y2[ibest]));
359 vertex.SetZ(0.5 * (z1 + z2));
361 return hypot(x2[ibest] - x1[ibest], y2[ibest] - y1[ibest], z2 - z1);
369 const double d0 = helix.getD0();
370 const double phi0 = helix.getPhi0();
371 const double omega = helix.getOmega();
372 const double z0 = helix.getZ0();
373 const double tandip = helix.getTanLambda();
374 const double cosdip = cos(atan(tandip)) ;
376 const double r = 1 / omega ;
378 const double x0 = - (r + d0) * sin(phi0) ;
379 const double y0 = (r + d0) * cos(phi0) ;
381 const double deltax = x0 - point.
X() ;
382 const double deltay = y0 - point.
Y() ;
384 const double pi = TMath::Pi();
385 double phi = - atan2(deltax, deltay) ;
386 if (r < 0) phi = phi > 0 ? phi - pi : phi + pi ;
389 const double x = r * sin(phi) + x0 ;
390 const double y = -r * cos(phi) + y0 ;
394 const double dphi =
phidomain(phi - phi0) ;
395 for (
int n = 1 - ncirc; n <= 1 + ncirc ; ++n) {
396 const double l = (dphi + n * TMath::TwoPi()) / omega ;
397 const double tmpz = (z0 + l * tandip) ;
398 if (first || fabs(tmpz - point.
Z()) < fabs(z - point.
Z())) {
404 return sqrt(sqr(x - point.
X()) + sqr(y - point.
Y()) + sqr(z - point.
Z())) ;
410 const double z __attribute__((unused)),
420 const double aq = charge / alpha;
422 const double pt = std::hypot(px, py);
423 const double pt2 = pt * pt;
424 const double pt3 = pt2 * pt;
425 const double aq2 = aq * aq;
427 const double x2 = x * x;
428 const double y2 = y * y;
429 const double r = x2 + y2;
431 const double px2 = px * px;
432 const double py2 = py * py;
434 const double px0 = px - aq * y;
435 const double py0 = py + aq * x;
437 const double pt02 = px0 * px0 + py0 * py0;
438 const double pt0 = std::sqrt(pt02);
439 double sqrt13 = pt0 / pt;
442 jacobian(0, 0) = py0 / pt0;
443 jacobian(0, 1) = -px0 / pt0;
445 jacobian(0, 3) = (-(y * (aq2 * r + 2 * aq * py * x + 2 * py2 * (1 + sqrt13))) - px * (2 * py * x * (1 + sqrt13) + aq * (y2 *
446 (-1 + sqrt13) + x2 * (1 + sqrt13)))) /
447 (pt2 * pt0 * (1 + sqrt13) * (1 + sqrt13));
449 jacobian(0, 4) = (2 * px2 * x * (1 + sqrt13) + 2 * px * y * (py - aq * x + py * sqrt13) + aq * (aq * r * x - py * (x2 *
450 (-1 + sqrt13) + y2 * (1 + sqrt13)))) /
451 (pt2 * pt0 * (1 + sqrt13) * (1 + sqrt13));
455 jacobian(1, 0) = aq * px0 / pt02;
456 jacobian(1, 1) = aq * py0 / pt02;
458 jacobian(1, 3) = -py0 / pt02;
459 jacobian(1, 4) = px0 / pt02;
466 jacobian(2, 3) = - aq * px / pt3;
467 jacobian(2, 4) = - aq * py / pt3;
471 jacobian(3, 0) = -pz * px0 / pt02;
472 jacobian(3, 1) = -pz * py0 / pt02;
474 jacobian(3, 3) = (pz * (px2 * x - py * (aq * r + py * x) + 2 * px * py * y)) / (pt2 * pt02);
475 jacobian(3, 4) = (pz * (px * (aq * r + 2 * py * x) - px2 * y + py2 * y)) / (pt2 * pt02);
476 jacobian(3, 5) = std::atan2(-(aq * (px * x + py * y)), (px2 + py * py0 - aq * px * y)) / aq;
482 jacobian(4, 3) = -pz * px / pt3;
483 jacobian(4, 4) = -pz * py / pt3;
484 jacobian(4, 5) = 1. / pt;
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)
static const double speedOfLight
[cm/ns]
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 vertexFromHelix(const Belle2::Helix &helix, double L, double Bz, ROOT::Math::XYZVector &position, ROOT::Math::XYZVector &momentum, int &charge)
helix --> 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 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.