8#include <framework/dataobjects/Helix.h>
10#include <framework/gearbox/Const.h>
12#include <boost/math/special_functions/sign.hpp>
13#include <boost/math/special_functions/sinc.hpp>
14#include <boost/math/tools/precision.hpp>
16#include <Math/VectorUtil.h>
22using namespace HelixParameterIndex;
34 const ROOT::Math::XYZVector& momentum,
35 const short int charge,
38 setCartesian(position,
momentum, charge, bZ);
45 const double& tanLambda)
50 m_tanLambda(tanLambda)
56 return getD0() * getSinPhi0();
61 return -getD0() * getCosPhi0();
71 return ROOT::Math::XYZVector(getPerigeeX(), getPerigeeY(), getPerigeeZ());
76 return getCosPhi0() * getTransverseMomentum(bZ);
81 return getSinPhi0() * getTransverseMomentum(bZ);
86 return getTanLambda() * getTransverseMomentum(bZ);
91 return ROOT::Math::XYZVector(getMomentumX(bZ), getMomentumY(bZ), getMomentumZ(bZ));
96 return ROOT::Math::XYZVector(getCosPhi0(), getSinPhi0(), getTanLambda());
101 return 1 / std::fabs(getAlpha(bZ) * getOmega());
106 return getOmega() * getAlpha(bZ);
116 return boost::math::sign(getOmega());
123 m_phi0 = reversePhi(m_phi0);
125 m_tanLambda = -m_tanLambda;
135 const double dr = getD0();
136 const double deltaCylindricalR = cylindricalR;
137 const double absArcLength2D = calcArcLength2DAtDeltaCylindricalRAndDr(deltaCylindricalR,
dr);
138 return absArcLength2D;
144 double arcLength2D = 0;
145 calcArcLength2DAndDrAtXY(
x, y, arcLength2D,
dr);
150 const double& nX,
const double& nY)
const
153 const double tX = nY;
154 const double tY = -nX;
157 const double omega = getOmega();
158 const double cosPhi0 = getCosPhi0();
159 const double sinPhi0 = getSinPhi0();
160 const double d0 = getD0();
165 const double deltaOrthogonal = byY *
cosPhi0 - byX *
sinPhi0 + d0;
166 const double deltaCylindricalR = hypot(deltaOrthogonal, deltaParallel);
167 const double deltaCylindricalRSquared = deltaCylindricalR * deltaCylindricalR;
169 const double UOrthogonal = 1 + omega * deltaOrthogonal;
170 const double UParallel = omega * deltaParallel;
173 const double A = 2 * deltaOrthogonal + omega * deltaCylindricalRSquared;
177 const double tCylindricalR = (tX * tX + tY * tY);
179 const double c = A / 2;
180 const double b = UParallel * tParallel + UOrthogonal * tOrthogonal;
181 const double a = omega / 2 * tCylindricalR;
183 const double discriminant = ((double)b) * b - 4 *
a * c;
184 const double root =
sqrt(discriminant);
185 const double bigSum = (b > 0) ? -b - root : -b + root;
187 const double bigOffset = bigSum / 2 /
a;
188 const double smallOffset = 2 * c / bigSum;
190 const double distance1 = hypot(byX + bigOffset * tX, byY + bigOffset * tY);
191 const double distance2 = hypot(byX + smallOffset * tX, byY + smallOffset * tY);
193 if (distance1 < distance2) {
194 return getArcLength2DAtXY(byX + bigOffset * tX, byY + bigOffset * tY);
196 return getArcLength2DAtXY(byX + smallOffset * tX, byY + smallOffset * tY);
225 const double chi = -arcLength2D * getOmega();
226 const double chiHalf = chi / 2.0;
228 using boost::math::sinc_pi;
230 const double x = arcLength2D * sinc_pi(chi);
231 const double y = arcLength2D * sinc_pi(chiHalf) * sin(chiHalf) - getD0();
234 const double z = fma((
double)arcLength2D, getTanLambda(), getZ0());
237 ROOT::Math::XYZVector position(
x, y, z);
240 position = ROOT::Math::VectorUtil::RotateZ(position, getPhi0());
247 const double omega = getOmega();
248 const double phi0 = getPhi0();
249 const double tanLambda = getTanLambda();
251 const double chi = - omega * arcLength2D;
253 const double tx = cos(chi +
phi0);
254 const double ty = sin(chi +
phi0);
255 const double tz = tanLambda;
257 ROOT::Math::XYZVector tangential(tx, ty, tz);
263 ROOT::Math::XYZVector unitTangential = getTangentialAtArcLength2D(arcLength2D);
264 const double norm = hypot(1, getTanLambda());
265 const double invNorm = 1 / norm;
266 unitTangential *= invNorm;
267 return unitTangential;
272 ROOT::Math::XYZVector
momentum = getTangentialAtArcLength2D(arcLength2D);
273 const double pr = getTransverseMomentum(bz);
285 double arcLength2D = 0;
286 calcArcLength2DAndDrAtXY(byX, byY, arcLength2D, new_d0);
289 double chi = -arcLength2D * getOmega();
290 double new_phi0 = m_phi0 + chi;
291 double new_z0 = getZ0() - byZ + getTanLambda() * arcLength2D;
303 TMatrixD jacobian(5, 5);
304 calcPassiveMoveByJacobian(by.X(), by.Y(), jacobian, expandBelowChi);
312 const double expandBelowChi)
const
316 jacobian.UnitMatrix();
317 assert(jacobian.GetNrows() == jacobian.GetNcols());
318 assert(jacobian.GetNrows() == 5 or jacobian.GetNrows() == 6);
321 const double omega = getOmega();
322 const double cosPhi0 = getCosPhi0();
323 const double sinPhi0 = getSinPhi0();
324 const double d0 = getD0();
325 const double tanLambda = getTanLambda();
330 const double deltaOrthogonal = byY *
cosPhi0 - byX *
sinPhi0 + d0;
331 const double deltaCylindricalR = hypot(deltaOrthogonal, deltaParallel);
332 const double deltaCylindricalRSquared = deltaCylindricalR * deltaCylindricalR;
335 const double A = 2 * deltaOrthogonal + omega * deltaCylindricalRSquared;
336 const double USquared = 1 + omega * A;
337 const double U =
sqrt(USquared);
338 const double UOrthogonal = 1 + omega * deltaOrthogonal;
339 const double UParallel = omega * deltaParallel;
341 const double u = 1 + omega * d0;
352 const double new_d0 = A / (1 + U);
354 jacobian(iD0, iOmega) = (deltaCylindricalR + new_d0) * (deltaCylindricalR - new_d0) / 2 / U;
355 jacobian(iD0, iPhi0) = - u * deltaParallel / U;
356 jacobian(iD0, iD0) = UOrthogonal / U;
361 const double dArcLength2D_dD0 = - UParallel / USquared;
362 const double dArcLength2D_dPhi0 = (omega * deltaCylindricalRSquared + deltaOrthogonal - d0 * UOrthogonal) / USquared;
364 jacobian(iPhi0, iD0) = - dArcLength2D_dD0 * omega;
365 jacobian(iPhi0, iPhi0) = u * UOrthogonal / USquared;
366 jacobian(iPhi0, iOmega) = -deltaParallel / USquared;
375 const double chi = -atan2(UParallel, UOrthogonal);
376 double arcLength2D = 0;
377 double dArcLength2D_dOmega = 0;
379 if (fabs(chi) < std::min(expandBelowChi, M_PI / 2.0)) {
382 double principleArcLength2D = deltaParallel / UOrthogonal;
383 const double dPrincipleArcLength2D_dOmega = - principleArcLength2D * deltaOrthogonal / UOrthogonal;
385 const double x = principleArcLength2D * omega;
386 const double f = calcATanXDividedByX(
x);
387 const double df_dx = calcDerivativeOfATanXDividedByX(
x);
389 arcLength2D = principleArcLength2D * f;
390 dArcLength2D_dOmega = (f +
x * df_dx) * dPrincipleArcLength2D_dOmega + principleArcLength2D * df_dx * principleArcLength2D;
396 arcLength2D = - chi / omega;
397 dArcLength2D_dOmega = (-arcLength2D - jacobian(iPhi0, iOmega)) / omega;
404 jacobian(iZ0, iD0) = tanLambda * dArcLength2D_dD0;
405 jacobian(iZ0, iPhi0) = tanLambda * dArcLength2D_dPhi0;
406 jacobian(iZ0, iOmega) = tanLambda * dArcLength2D_dOmega;
407 jacobian(iZ0, iZ0) = 1.0;
408 jacobian(iZ0, iTanLambda) = arcLength2D;
410 if (jacobian.GetNrows() == 6) {
412 jacobian(iArcLength2D, iD0) = dArcLength2D_dD0;
413 jacobian(iArcLength2D, iPhi0) = dArcLength2D_dPhi0;
414 jacobian(iArcLength2D, iOmega) = dArcLength2D_dOmega;
415 jacobian(iArcLength2D, iZ0) = 0.0;
416 jacobian(iArcLength2D, iTanLambda) = 0;
417 jacobian(iArcLength2D, iArcLength2D) = 1.0;
423 return std::remainder(phi + M_PI, 2 * M_PI);
428 return secantLength * calcSecantLengthToArcLength2DFactor(secantLength);
434 double x = secantLength * getOmega() / 2.0;
435 return calcASinXDividedByX(
x);
443 BOOST_MATH_STD_USING;
445 auto const taylor_n_bound = boost::math::tools::forth_root_epsilon<double>();
447 if (abs(
x) >= taylor_n_bound) {
460 auto const taylor_0_bound = boost::math::tools::epsilon<double>();
461 if (abs(
x) >= taylor_0_bound) {
466 auto const taylor_2_bound = boost::math::tools::root_epsilon<double>();
467 if (abs(
x) >= taylor_2_bound) {
469 result += x2 * x2 * (3.0 / 40.0);
482 BOOST_MATH_STD_USING;
484 auto const taylor_n_bound = boost::math::tools::forth_root_epsilon<double>();
486 if (abs(
x) >= taylor_n_bound) {
493 auto const taylor_0_bound = boost::math::tools::epsilon<double>();
494 if (abs(
x) >= taylor_0_bound) {
499 auto const taylor_2_bound = boost::math::tools::root_epsilon<double>();
500 if (abs(
x) >= taylor_2_bound) {
502 result += x2 * x2 * (1.0 / 5.0);
514 BOOST_MATH_STD_USING;
516 auto const taylor_n_bound = boost::math::tools::forth_root_epsilon<double>();
518 const double x2 =
x *
x;
519 if (abs(
x) >= taylor_n_bound) {
520 const double chi =
atan(
x);
521 return ((1 - chi /
x) /
x - chi) / (1 + x2);
527 auto const taylor_0_bound = boost::math::tools::epsilon<double>();
528 if (abs(
x) >= taylor_0_bound) {
530 result -= 2.0 *
x / 3.0;
532 auto const taylor_2_bound = boost::math::tools::root_epsilon<double>();
533 if (abs(
x) >= taylor_2_bound) {
535 result += x2 *
x * (4.0 / 5.0);
548 const double omega = getOmega();
549 const double cosPhi0 = getCosPhi0();
550 const double sinPhi0 = getSinPhi0();
551 const double d0 = getD0();
555 const double deltaCylindricalR = hypot(deltaOrthogonal, deltaParallel);
556 const double deltaCylindricalRSquared = deltaCylindricalR * deltaCylindricalR;
558 const double A = 2 * deltaOrthogonal + omega * deltaCylindricalRSquared;
559 const double U =
sqrt(1 + omega * A);
560 const double UOrthogonal = 1 + omega * deltaOrthogonal;
561 const double UParallel = omega * deltaParallel;
568 const double chi = -atan2(UParallel, UOrthogonal);
570 if (fabs(chi) < M_PI / 8) {
572 double principleArcLength2D = deltaParallel / UOrthogonal;
573 arcLength2D = principleArcLength2D * calcATanXDividedByX(principleArcLength2D * omega);
577 arcLength2D = -chi / omega;
583 const double omega = getOmega();
584 double secantLength =
sqrt((deltaCylindricalR +
dr) * (deltaCylindricalR -
dr) / (1 +
dr * omega));
585 return calcArcLength2DFromSecantLength(secantLength);
589 const ROOT::Math::XYZVector& momentum,
590 const short int charge,
593 assert(abs(charge) <= 1);
594 const double alpha = getAlpha(bZ);
601 const double x = position.X();
602 const double y = position.Y();
603 const double z = position.Z();
609 const double ptinv = 1 / hypot(px, py);
610 const double omega = charge * ptinv / alpha;
611 const double tanLambda = ptinv * pz;
612 const double phi0 = atan2(py, px);
619 m_tanLambda = tanLambda;
622 passiveMoveBy(-
x, -y, 0);
631 std::ostream& operator<<(std::ostream& output,
const Helix& helix)
635 <<
"d0=" << helix.getD0() <<
", "
636 <<
"phi0=" << helix.getPhi0() <<
", "
637 <<
"omega=" << helix.getOmega() <<
", "
638 <<
"z0=" << helix.getZ0() <<
", "
639 <<
"tanLambda=" << helix.getTanLambda() <<
")";
double phi0(void) const
Return helix parameter phi0.
const HepVector & a(void) const
Returns helix parameters.
double dr(void) const
Return helix parameter dr.
Hep3Vector momentum(double dPhi=0.) const
returns momentum vector after rotating angle dPhi in phi direction.
HepPoint3D x(double dPhi=0.) const
returns position after rotating angle dPhi in phi direction.
double cosPhi0(void) const
Return cos phi0.
double sinPhi0(void) const
Return sin phi0.
static const double speedOfLight
[cm/ns]
double getPerigeeX() const
Calculates the x coordinate of the perigee point.
double getMomentumZ(const double bZ) const
Calculates the z momentum of the particle at the perigee point.
double getMomentumY(const double bZ) const
Calculates the y momentum of the particle at the perigee point.
void reverse()
Reverses the direction of travel of the helix in place.
double getArcLength2DAtCylindricalR(const double &cylindricalR) const
Calculates the transverse travel distance at the point the helix first reaches the given cylindrical ...
static double calcASinXDividedByX(const double &x)
Implementation of the function asin(x) / x which handles small x values smoothly.
static double reversePhi(const double &phi)
Reverses an azimuthal angle to the opposite direction.
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 ...
static double getAlpha(const double bZ)
Calculates the alpha value for a given magnetic field in Tesla.
ROOT::Math::XYZVector getDirection() const
Getter for unit vector of momentum at the perigee position.
ROOT::Math::XYZVector getTangentialAtArcLength2D(const double &arcLength2D) const
Calculates the tangential vector to the helix curve at the given two dimensional arc length.
ROOT::Math::XYZVector getPositionAtArcLength2D(const double &arcLength2D) const
Calculates the position on the helix at the given two dimensional arc length.
ROOT::Math::XYZVector getUnitTangentialAtArcLength2D(const double &arcLength2D) const
Calculates the unit tangential vector to the helix curve at the given two dimensional arc length.
double getPerigeeZ() const
Calculates the z coordinate of the perigee point.
void setCartesian(const ROOT::Math::XYZVector &position, const ROOT::Math::XYZVector &momentum, const short int charge, const double bZ)
Cartesian to Perigee conversion.
ROOT::Math::XYZVector getMomentum(const double bZ) const
Getter for vector of momentum at the perigee position.
void calcArcLength2DAndDrAtXY(const double &x, const double &y, double &arcLength2D, double &dr) const
Helper method to calculate the signed two dimensional arc length and the signed distance to the circl...
double passiveMoveBy(const ROOT::Math::XYZVector &by)
Moves origin of the coordinate system (passive transformation) by the given vector.
double getArcLength2DAtNormalPlane(const double &x, const double &y, const double &nX, const double &nY) const
Calculates the arc length to reach a plane parallel to the z axes.
double getMomentumX(const double bZ) const
Calculates the x momentum of the particle at the perigee point.
TMatrixD calcPassiveMoveByJacobian(const ROOT::Math::XYZVector &by, const double expandBelowChi=M_PI/8) const
Calculate the 5x5 jacobian matrix for the transport of the helix parameters, when moving the origin o...
double getPerigeeY() const
Calculates the y coordinate of the perigee point.
double calcSecantLengthToArcLength2DFactor(const double &secantLength2D) const
Helper function to calculate the factor between the dimensional secant length and the two dimensional...
double calcArcLength2DAtDeltaCylindricalRAndDr(const double &deltaCylindricalR, const double &dr) const
Helper method to calculate the two dimensional arc length from the perigee to a point at cylindrical ...
double calcArcLength2DFromSecantLength(const double &secantLength2D) const
Helper function to calculate the two dimensional arc length from the length of a secant.
double getKappa(const double bZ) const
Getter for kappa, which is charge / transverse momentum or equivalently omega * alpha.
static double calcDerivativeOfATanXDividedByX(const double &x)
Implementation of the function d / dx (atan(x) / x) which handles small x values smoothly.
Helix()
Constructor initializing all perigee parameters to zero.
ROOT::Math::XYZVector getMomentumAtArcLength2D(const double &arcLength2D, const double &bz) const
Calculates the momentum vector at the given two dimensional arc length.
ROOT::Math::XYZVector getPerigee() const
Getter for the perigee position.
double getTransverseMomentum(const double bZ) const
Getter for the absolute value of the transverse momentum at the perigee.
static double calcATanXDividedByX(const double &x)
Implementation of the function atan(x) / x which handles small x values smoothly.
double atan(double a)
atan for double
double sqrt(double a)
sqrt for double
Abstract base class for different kinds of events.