35 int charge,
double Bz,
38 Eigen::Matrix<double, 5, 6>& jacobian)
41 helix =
Belle2::Helix(ROOT::Math::XYZVector(positionAndMomentum(0), positionAndMomentum(1), positionAndMomentum(2)),
42 ROOT::Math::XYZVector(positionAndMomentum(3), positionAndMomentum(4), positionAndMomentum(5)),
45 L = helix.getArcLength2DAtXY(positionAndMomentum(0),
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;
134 int charge,
double Bz,
136 Eigen::Matrix<double, 5, 6>& jacobian)
139 helix =
Belle2::Helix(ROOT::Math::XYZVector(positionAndMom(0), positionAndMom(1), positionAndMom(2)),
140 ROOT::Math::XYZVector(positionAndMom(3), positionAndMom(4), positionAndMom(5)),
148 ROOT::Math::XYZVector postmp;
149 ROOT::Math::XYZVector momtmp;
151 for (
int jin = 0; jin < 6; ++jin) {
152 postmp.SetCoordinates(positionAndMom(0), positionAndMom(1), positionAndMom(2));
153 momtmp.SetCoordinates(positionAndMom(3), positionAndMom(4), positionAndMom(5));
154 if (jin == 0) postmp.SetX(postmp.X() + delta);
155 if (jin == 1) postmp.SetY(postmp.Y() + delta);
156 if (jin == 2) postmp.SetZ(postmp.Z() + delta);
157 if (jin == 3) momtmp.SetX(momtmp.X() + delta);
158 if (jin == 4) momtmp.SetY(momtmp.Y() + delta);
159 if (jin == 5) momtmp.SetZ(momtmp.Z() + delta);
162 jacobian(iD0, jin) = (helixPlusDelta.getD0() - helix.getD0()) / delta ;
163 jacobian(iPhi0, jin) = (helixPlusDelta.getPhi0() - helix.getPhi0()) / delta ;
164 jacobian(iOmega, jin) = (helixPlusDelta.getOmega() - helix.getOmega()) / delta ;
165 jacobian(iZ0, jin) = (helixPlusDelta.getZ0() - helix.getZ0()) / delta ;
166 jacobian(iTanLambda, jin) = (helixPlusDelta.getTanLambda() - helix.getTanLambda()) / delta ;
173 const Eigen::Matrix<double, 1, 6>& positionAndMom,
174 int charge,
double Bz,
176 Eigen::Matrix<double, 5, 6>& jacobian,
183 ROOT::Math::XYZVector postmp;
184 ROOT::Math::XYZVector momtmp;
186 for (
int jin = 0; jin < 6; ++jin) {
187 postmp.SetCoordinates(positionAndMom(0), positionAndMom(1), positionAndMom(2));
188 momtmp.SetCoordinates(positionAndMom(3), positionAndMom(4), positionAndMom(5));
189 if (jin == 0) postmp.SetX(postmp.X() + delta);
190 if (jin == 1) postmp.SetY(postmp.Y() + delta);
191 if (jin == 2) postmp.SetZ(postmp.Z() + delta);
192 if (jin == 3) momtmp.SetX(momtmp.X() + delta);
193 if (jin == 4) momtmp.SetY(momtmp.Y() + delta);
194 if (jin == 5) momtmp.SetZ(momtmp.Z() + delta);
197 jacobian(iD0, jin) = (helixPlusDelta.getD0() - helix.getD0()) / delta ;
198 jacobian(iPhi0, jin) = (helixPlusDelta.getPhi0() - helix.getPhi0()) / delta ;
199 jacobian(iOmega, jin) = (helixPlusDelta.getOmega() - helix.getOmega()) / delta ;
200 jacobian(iZ0, jin) = (helixPlusDelta.getZ0() - helix.getZ0()) / delta ;
201 jacobian(iTanLambda, jin) = (helixPlusDelta.getTanLambda() - helix.getTanLambda()) / delta ;
219 double& flt1,
double& flt2,
220 Eigen::Vector3d& vertex,
bool parallel)
223 const double d0_1 = helix1.getD0();
224 const double phi0_1 = helix1.getPhi0();
225 const double omega_1 = helix1.getOmega();
227 const double d0_2 = helix2.getD0();
228 const double phi0_2 = helix2.getPhi0();
229 const double omega_2 = helix2.getOmega();
232 const double r_1 = 1 / omega_1 ;
233 const double r_2 = 1 / omega_2 ;
237 const double x0_1 = (r_1 + d0_1) * sin(phi0_1) ;
238 const double y0_1 = -(r_1 + d0_1) * cos(phi0_1) ;
240 const double x0_2 = (r_2 + d0_2) * sin(phi0_2) ;
241 const double y0_2 = -(r_2 + d0_2) * cos(phi0_2) ;
244 const double deltax = x0_2 - x0_1 ;
245 const double deltay = y0_2 - y0_1 ;
253 const double phi = - atan2(deltax, deltay) ;
254 const double phinot = phi > 0 ? phi - TMath::Pi() : phi + TMath::Pi() ;
255 phi1[0] = r_1 < 0 ? phi : phinot ;
256 phi2[0] = r_2 > 0 ? phi : phinot ;
259 const double R1 = fabs(r_1) ;
260 const double R2 = fabs(r_2) ;
261 const double Rmin = R1 < R2 ? R1 : R2 ;
262 const double Rmax = R1 > R2 ? R1 : R2 ;
263 const double dX = hypot(deltax, deltay) ;
265 if (!parallel && dX + Rmin > Rmax && dX < R1 + R2) {
270 const double ddphi1 = acos((dX * dX - R2 * R2 + R1 * R1) / (2.*dX * R1)) ;
274 const double ddphi2 = acos((dX * dX - R1 * R1 + R2 * R2) / (2.*dX * R2)) ;
278 }
else if (dX < Rmax) {
280 if (R1 > R2) phi2[0] = r_2 < 0 ? phi : phinot ;
281 else phi1[0] = r_1 < 0 ? phi : phinot ;
287 double x1[2], y1[2], x2[2], y2[2];
288 for (
int i = 0; i < nsolutions; i++) {
289 x1[i] = r_1 * sin(phi1[i]) + x0_1 ;
290 y1[i] = -r_1 * cos(phi1[i]) + y0_1 ;
291 x2[i] = r_2 * sin(phi2[i]) + x0_2 ;
292 y2[i] = -r_2 * cos(phi2[i]) + y0_2 ;
299 const int nturnsmax = 10;
302 for (
int i = 0; i < nsolutions; ++i) {
303 const double l1 = helix1.getArcLength2DAtXY(x1[i], y1[i]);
304 const double l2 = helix2.getArcLength2DAtXY(x2[i], y2[i]);
307 std::vector<double> z1s;
308 for (
int n1 = 0; n1 <= nturnsmax; ++n1) {
311 for (
int sn1 : {n1, -n1}) {
312 const double tmpz1 = helix1.getPositionAtArcLength2D(l1 + sn1 * TMath::TwoPi() / omega_1).Z();
313 if (sn1 == 0 || (-82 <= tmpz1 && tmpz1 <= 158)) {
315 z1s.push_back(tmpz1);
327 for (
int n2 = 0; n2 <= nturnsmax; ++n2) {
330 for (
int sn2 : {n2, -n2}) {
331 const double tmpz2 = helix2.getPositionAtArcLength2D(l2 + sn2 * TMath::TwoPi() / omega_2).Z();
332 if (sn2 == 0 || (-82 <= tmpz2 && tmpz2 <= 158)) {
336 const auto i1best = std::min_element(
337 z1s.cbegin(), z1s.cend(), [&tmpz2](
const double & z1a,
const double & z1b) {
338 return fabs(z1a - tmpz2) < fabs(z1b - tmpz2);
340 const double tmpz1 = *i1best;
342 if (first || fabs(tmpz1 - tmpz2) < fabs(z1 - z2)) {
360 vertex.x() = 0.5 * (x1[ibest] + x2[ibest]);
361 vertex.y() = 0.5 * (y1[ibest] + y2[ibest]);
362 vertex.z() = 0.5 * (z1 + z2);
364 return std::hypot(x2[ibest] - x1[ibest], y2[ibest] - y1[ibest], z2 - z1);
369 const ROOT::Math::XYZVector& point,
372 const double d0 = helix.getD0();
373 const double phi0 = helix.getPhi0();
374 const double omega = helix.getOmega();
375 const double z0 = helix.getZ0();
376 const double tandip = helix.getTanLambda();
377 const double cosdip = cos(atan(tandip)) ;
379 const double r = 1 / omega ;
381 const double x0 = - (r + d0) * sin(phi0) ;
382 const double y0 = (r + d0) * cos(phi0) ;
384 const double deltax = x0 - point.X() ;
385 const double deltay = y0 - point.Y() ;
387 const double pi = TMath::Pi();
388 double phi = - atan2(deltax, deltay) ;
389 if (r < 0) phi = phi > 0 ? phi - pi : phi + pi ;
392 const double x = r * sin(phi) + x0 ;
393 const double y = -r * cos(phi) + y0 ;
397 const double dphi =
phidomain(phi - phi0) ;
398 for (
int n = 1 - ncirc; n <= 1 + ncirc ; ++n) {
399 const double l = (dphi + n * TMath::TwoPi()) / omega ;
400 const double tmpz = (z0 + l * tandip) ;
401 if (first || fabs(tmpz - point.Z()) < fabs(z - point.Z())) {
407 return sqrt(sqr(x - point.X()) + sqr(y - point.Y()) + sqr(z - point.Z())) ;
413 const double z __attribute__((unused)),
423 const double aq = charge / alpha;
425 const double pt = std::hypot(px, py);
426 const double pt2 = pt * pt;
427 const double pt3 = pt2 * pt;
428 const double aq2 = aq * aq;
430 const double x2 = x * x;
431 const double y2 = y * y;
432 const double r = x2 + y2;
434 const double px2 = px * px;
435 const double py2 = py * py;
437 const double px0 = px - aq * y;
438 const double py0 = py + aq * x;
440 const double pt02 = px0 * px0 + py0 * py0;
441 const double pt0 = std::sqrt(pt02);
442 double sqrt13 = pt0 / pt;
445 jacobian(0, 0) = py0 / pt0;
446 jacobian(0, 1) = -px0 / pt0;
448 jacobian(0, 3) = (-(y * (aq2 * r + 2 * aq * py * x + 2 * py2 * (1 + sqrt13))) - px * (2 * py * x * (1 + sqrt13) + aq * (y2 *
449 (-1 + sqrt13) + x2 * (1 + sqrt13)))) /
450 (pt2 * pt0 * (1 + sqrt13) * (1 + sqrt13));
452 jacobian(0, 4) = (2 * px2 * x * (1 + sqrt13) + 2 * px * y * (py - aq * x + py * sqrt13) + aq * (aq * r * x - py * (x2 *
453 (-1 + sqrt13) + y2 * (1 + sqrt13)))) /
454 (pt2 * pt0 * (1 + sqrt13) * (1 + sqrt13));
458 jacobian(1, 0) = aq * px0 / pt02;
459 jacobian(1, 1) = aq * py0 / pt02;
461 jacobian(1, 3) = -py0 / pt02;
462 jacobian(1, 4) = px0 / pt02;
469 jacobian(2, 3) = - aq * px / pt3;
470 jacobian(2, 4) = - aq * py / pt3;
474 jacobian(3, 0) = -pz * px0 / pt02;
475 jacobian(3, 1) = -pz * py0 / pt02;
477 jacobian(3, 3) = (pz * (px2 * x - py * (aq * r + py * x) + 2 * px * py * y)) / (pt2 * pt02);
478 jacobian(3, 4) = (pz * (px * (aq * r + 2 * py * x) - px2 * y + py2 * y)) / (pt2 * pt02);
479 jacobian(3, 5) = std::atan2(-(aq * (px * x + py * y)), (px2 + py * py0 - aq * px * y)) / aq;
485 jacobian(4, 3) = -pz * px / pt3;
486 jacobian(4, 4) = -pz * py / pt3;
487 jacobian(4, 5) = 1. / pt;
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 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 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.