10 #include <framework/dataobjects/Helix.h>
12 #include <framework/gearbox/Const.h>
14 #include <boost/math/special_functions/sign.hpp>
15 #include <boost/math/special_functions/sinc.hpp>
16 #include <boost/math/tools/precision.hpp>
23 using namespace HelixParameterIndex;
35 const TVector3& momentum,
36 const short int charge,
39 setCartesian(position,
momentum, charge, bZ);
46 const double& tanLambda)
51 m_tanLambda(tanLambda)
55 double Helix::getPerigeeX()
const
57 return getD0() * getSinPhi0();
60 double Helix::getPerigeeY()
const
62 return -getD0() * getCosPhi0();
65 double Helix::getPerigeeZ()
const
70 TVector3 Helix::getPerigee()
const
72 return TVector3(getPerigeeX(), getPerigeeY(), getPerigeeZ());
75 double Helix::getMomentumX(
const double bZ)
const
77 return getCosPhi0() * getTransverseMomentum(bZ);
80 double Helix::getMomentumY(
const double bZ)
const
82 return getSinPhi0() * getTransverseMomentum(bZ);
85 double Helix::getMomentumZ(
const double bZ)
const
87 return getTanLambda() * getTransverseMomentum(bZ);
90 TVector3 Helix:: getMomentum(
const double bZ)
const
92 return TVector3(getMomentumX(bZ), getMomentumY(bZ), getMomentumZ(bZ));
95 TVector3 Helix::getDirection()
const
97 return TVector3(getCosPhi0(), getSinPhi0(), getTanLambda());
100 double Helix::getTransverseMomentum(
const double bZ)
const
102 return 1 / std::fabs(getAlpha(bZ) * getOmega());
105 double Helix::getKappa(
const double bZ)
const
107 return getOmega() * getAlpha(bZ);
110 double Helix::getAlpha(
const double bZ)
115 short Helix::getChargeSign()
const
117 return boost::math::sign(getOmega());
120 void Helix::reverse()
124 m_phi0 = reversePhi(m_phi0);
126 m_tanLambda = -m_tanLambda;
129 double Helix::getArcLength2DAtCylindricalR(
const double& cylindricalR)
const
136 const double dr = getD0();
137 const double deltaCylindricalR = cylindricalR;
138 const double absArcLength2D = calcArcLength2DAtDeltaCylindricalRAndDr(deltaCylindricalR,
dr);
139 return absArcLength2D;
142 double Helix::getArcLength2DAtXY(
const double& x,
const double& y)
const
145 double arcLength2D = 0;
146 calcArcLength2DAndDrAtXY(
x, y, arcLength2D,
dr);
150 double Helix::getArcLength2DAtNormalPlane(
const double& byX,
const double& byY,
151 const double& nX,
const double& nY)
const
154 const double tX = nY;
155 const double tY = -nX;
158 const double omega = getOmega();
159 const double cosPhi0 = getCosPhi0();
160 const double sinPhi0 = getSinPhi0();
161 const double d0 = getD0();
166 const double deltaOrthogonal = byY *
cosPhi0 - byX *
sinPhi0 + d0;
167 const double deltaCylindricalR = hypot(deltaOrthogonal, deltaParallel);
168 const double deltaCylindricalRSquared = deltaCylindricalR * deltaCylindricalR;
170 const double UOrthogonal = 1 + omega * deltaOrthogonal;
171 const double UParallel = omega * deltaParallel;
174 const double A = 2 * deltaOrthogonal + omega * deltaCylindricalRSquared;
178 const double tCylindricalR = (tX * tX + tY * tY);
180 const double c = A / 2;
181 const double b = UParallel * tParallel + UOrthogonal * tOrthogonal;
182 const double a = omega / 2 * tCylindricalR;
184 const double discriminant = ((double)b) * b - 4 *
a * c;
185 const double root = sqrt(discriminant);
186 const double bigSum = (b > 0) ? -b - root : -b + root;
188 const double bigOffset = bigSum / 2 /
a;
189 const double smallOffset = 2 * c / bigSum;
191 const double distance1 = hypot(byX + bigOffset * tX, byY + bigOffset * tY);
192 const double distance2 = hypot(byX + smallOffset * tX, byY + smallOffset * tY);
194 if (distance1 < distance2) {
195 return getArcLength2DAtXY(byX + bigOffset * tX, byY + bigOffset * tY);
197 return getArcLength2DAtXY(byX + smallOffset * tX, byY + smallOffset * tY);
202 TVector3 Helix::getPositionAtArcLength2D(
const double& arcLength2D)
const
226 const double chi = -arcLength2D * getOmega();
227 const double chiHalf = chi / 2.0;
229 using boost::math::sinc_pi;
231 const double x = arcLength2D * sinc_pi(chi);
232 const double y = arcLength2D * sinc_pi(chiHalf) * sin(chiHalf) - getD0();
235 const double z = fma((
double)arcLength2D, getTanLambda(), getZ0());
238 TVector3 position(
x, y, z);
241 position.RotateZ(getPhi0());
246 TVector3 Helix::getTangentialAtArcLength2D(
const double& arcLength2D)
const
248 const double omega = getOmega();
249 const double phi0 = getPhi0();
250 const double tanLambda = getTanLambda();
252 const double chi = - omega * arcLength2D;
254 const double tx = cos(chi +
phi0);
255 const double ty = sin(chi +
phi0);
256 const double tz = tanLambda;
258 TVector3 tangential(tx, ty, tz);
262 TVector3 Helix::getUnitTangentialAtArcLength2D(
const double& arcLength2D)
const
264 TVector3 unitTangential = getTangentialAtArcLength2D(arcLength2D);
265 const double norm = hypot(1, getTanLambda());
266 const double invNorm = 1 / norm;
267 unitTangential *= invNorm;
268 return unitTangential;
271 TVector3 Helix::getMomentumAtArcLength2D(
const double& arcLength2D,
const double& bz)
const
273 TVector3
momentum = getTangentialAtArcLength2D(arcLength2D);
274 const double pr = getTransverseMomentum(bz);
280 double Helix::passiveMoveBy(
const double& byX,
286 double arcLength2D = 0;
287 calcArcLength2DAndDrAtXY(byX, byY, arcLength2D, new_d0);
290 double chi = -arcLength2D * getOmega();
291 double new_phi0 = m_phi0 + chi;
292 double new_z0 = getZ0() - byZ + getTanLambda() * arcLength2D;
302 TMatrixD Helix::calcPassiveMoveByJacobian(
const TVector3& by,
const double expandBelowChi)
const
304 TMatrixD jacobian(5, 5);
305 calcPassiveMoveByJacobian(by.X(), by.Y(), jacobian, expandBelowChi);
310 void Helix::calcPassiveMoveByJacobian(
const double& byX,
313 const double expandBelowChi)
const
317 jacobian.UnitMatrix();
318 assert(jacobian.GetNrows() == jacobian.GetNcols());
319 assert(jacobian.GetNrows() == 5 or jacobian.GetNrows() == 6);
322 const double omega = getOmega();
323 const double cosPhi0 = getCosPhi0();
324 const double sinPhi0 = getSinPhi0();
325 const double d0 = getD0();
326 const double tanLambda = getTanLambda();
331 const double deltaOrthogonal = byY *
cosPhi0 - byX *
sinPhi0 + d0;
332 const double deltaCylindricalR = hypot(deltaOrthogonal, deltaParallel);
333 const double deltaCylindricalRSquared = deltaCylindricalR * deltaCylindricalR;
336 const double A = 2 * deltaOrthogonal + omega * deltaCylindricalRSquared;
337 const double USquared = 1 + omega * A;
338 const double U = sqrt(USquared);
339 const double UOrthogonal = 1 + omega * deltaOrthogonal;
340 const double UParallel = omega * deltaParallel;
342 const double u = 1 + omega * d0;
353 const double new_d0 = A / (1 + U);
355 jacobian(iD0, iOmega) = (deltaCylindricalR + new_d0) * (deltaCylindricalR - new_d0) / 2 / U;
356 jacobian(iD0, iPhi0) = - u * deltaParallel / U;
357 jacobian(iD0, iD0) = UOrthogonal / U;
362 const double dArcLength2D_dD0 = - UParallel / USquared;
363 const double dArcLength2D_dPhi0 = (omega * deltaCylindricalRSquared + deltaOrthogonal - d0 * UOrthogonal) / USquared;
365 jacobian(iPhi0, iD0) = - dArcLength2D_dD0 * omega;
366 jacobian(iPhi0, iPhi0) = u * UOrthogonal / USquared;
367 jacobian(iPhi0, iOmega) = -deltaParallel / USquared;
376 const double chi = -atan2(UParallel, UOrthogonal);
377 double arcLength2D = 0;
378 double dArcLength2D_dOmega = 0;
380 if (fabs(chi) < std::min(expandBelowChi, M_PI / 2.0)) {
383 double principleArcLength2D = deltaParallel / UOrthogonal;
384 const double dPrincipleArcLength2D_dOmega = - principleArcLength2D * deltaOrthogonal / UOrthogonal;
386 const double x = principleArcLength2D * omega;
387 const double f = calcATanXDividedByX(
x);
388 const double df_dx = calcDerivativeOfATanXDividedByX(
x);
390 arcLength2D = principleArcLength2D * f;
391 dArcLength2D_dOmega = (f +
x * df_dx) * dPrincipleArcLength2D_dOmega + principleArcLength2D * df_dx * principleArcLength2D;
397 arcLength2D = - chi / omega;
398 dArcLength2D_dOmega = (-arcLength2D - jacobian(iPhi0, iOmega)) / omega;
405 jacobian(iZ0, iD0) = tanLambda * dArcLength2D_dD0;
406 jacobian(iZ0, iPhi0) = tanLambda * dArcLength2D_dPhi0;
407 jacobian(iZ0, iOmega) = tanLambda * dArcLength2D_dOmega;
408 jacobian(iZ0, iZ0) = 1.0;
409 jacobian(iZ0, iTanLambda) = arcLength2D;
411 if (jacobian.GetNrows() == 6) {
413 jacobian(iArcLength2D, iD0) = dArcLength2D_dD0;
414 jacobian(iArcLength2D, iPhi0) = dArcLength2D_dPhi0;
415 jacobian(iArcLength2D, iOmega) = dArcLength2D_dOmega;
416 jacobian(iArcLength2D, iZ0) = 0.0;
417 jacobian(iArcLength2D, iTanLambda) = 0;
418 jacobian(iArcLength2D, iArcLength2D) = 1.0;
422 double Helix::reversePhi(
const double& phi)
424 return std::remainder(phi + M_PI, 2 * M_PI);
427 double Helix::calcArcLength2DFromSecantLength(
const double& secantLength)
const
429 return secantLength * calcSecantLengthToArcLength2DFactor(secantLength);
433 double Helix::calcSecantLengthToArcLength2DFactor(
const double& secantLength)
const
435 double x = secantLength * getOmega() / 2.0;
436 return calcASinXDividedByX(
x);
439 double Helix::calcASinXDividedByX(
const double& x)
444 BOOST_MATH_STD_USING;
446 auto const taylor_n_bound = boost::math::tools::forth_root_epsilon<double>();
448 if (abs(
x) >= taylor_n_bound) {
461 auto const taylor_0_bound = boost::math::tools::epsilon<double>();
462 if (abs(
x) >= taylor_0_bound) {
467 auto const taylor_2_bound = boost::math::tools::root_epsilon<double>();
468 if (abs(
x) >= taylor_2_bound) {
470 result += x2 * x2 * (3.0 / 40.0);
478 double Helix::calcATanXDividedByX(
const double& x)
483 BOOST_MATH_STD_USING;
485 auto const taylor_n_bound = boost::math::tools::forth_root_epsilon<double>();
487 if (abs(
x) >= taylor_n_bound) {
494 auto const taylor_0_bound = boost::math::tools::epsilon<double>();
495 if (abs(
x) >= taylor_0_bound) {
500 auto const taylor_2_bound = boost::math::tools::root_epsilon<double>();
501 if (abs(
x) >= taylor_2_bound) {
503 result += x2 * x2 * (1.0 / 5.0);
510 double Helix::calcDerivativeOfATanXDividedByX(
const double& x)
515 BOOST_MATH_STD_USING;
517 auto const taylor_n_bound = boost::math::tools::forth_root_epsilon<double>();
519 const double x2 =
x *
x;
520 if (abs(
x) >= taylor_n_bound) {
521 const double chi = atan(
x);
522 return ((1 - chi /
x) /
x - chi) / (1 + x2);
528 auto const taylor_0_bound = boost::math::tools::epsilon<double>();
529 if (abs(
x) >= taylor_0_bound) {
531 result -= 2.0 *
x / 3.0;
533 auto const taylor_2_bound = boost::math::tools::root_epsilon<double>();
534 if (abs(
x) >= taylor_2_bound) {
536 result += x2 *
x * (4.0 / 5.0);
546 void Helix::calcArcLength2DAndDrAtXY(
const double& x,
const double& y,
double& arcLength2D,
double& dr)
const
549 const double omega = getOmega();
550 const double cosPhi0 = getCosPhi0();
551 const double sinPhi0 = getSinPhi0();
552 const double d0 = getD0();
556 const double deltaCylindricalR = hypot(deltaOrthogonal, deltaParallel);
557 const double deltaCylindricalRSquared = deltaCylindricalR * deltaCylindricalR;
559 const double A = 2 * deltaOrthogonal + omega * deltaCylindricalRSquared;
560 const double U = sqrt(1 + omega * A);
561 const double UOrthogonal = 1 + omega * deltaOrthogonal;
562 const double UParallel = omega * deltaParallel;
569 const double chi = -atan2(UParallel, UOrthogonal);
571 if (fabs(chi) < M_PI / 8) {
573 double principleArcLength2D = deltaParallel / UOrthogonal;
574 arcLength2D = principleArcLength2D * calcATanXDividedByX(principleArcLength2D * omega);
578 arcLength2D = -chi / omega;
582 double Helix::calcArcLength2DAtDeltaCylindricalRAndDr(
const double& deltaCylindricalR,
const double& dr)
const
584 const double omega = getOmega();
585 double secantLength = sqrt((deltaCylindricalR +
dr) * (deltaCylindricalR -
dr) / (1 +
dr * omega));
586 return calcArcLength2DFromSecantLength(secantLength);
589 void Helix::setCartesian(
const TVector3& position,
590 const TVector3& momentum,
591 const short int charge,
594 assert(abs(charge) <= 1);
595 const double alpha = getAlpha(bZ);
602 const double x = position.X();
603 const double y = position.Y();
604 const double z = position.Z();
610 const double ptinv = 1 / hypot(px, py);
611 const double omega = charge * ptinv / alpha;
612 const double tanLambda = ptinv * pz;
613 const double phi0 = atan2(py, px);
620 m_tanLambda = tanLambda;
623 passiveMoveBy(-
x, -y, 0);
636 <<
"d0=" << helix.getD0() <<
", "
637 <<
"phi0=" << helix.getPhi0() <<
", "
638 <<
"omega=" << helix.getOmega() <<
", "
639 <<
"z0=" << helix.getZ0() <<
", "
640 <<
"tanLambda=" << helix.getTanLambda() <<
")";