PixelSeeding/src/ThirdHitPredictionFromCircle.cc

0001 #include <cmath>
0002 #include "DataFormats/GeometryVector/interface/GlobalVector.h"
0003 #include "DataFormats/GeometryVector/interface/GlobalPoint.h"
0004
0005 #include "RecoTracker/TkMSParametrization/interface/PixelRecoUtilities.h"
0006 #include "RecoTracker/TkMSParametrization/interface/PixelRecoRange.h"
0007
0008 #include "RecoTracker/PixelSeeding/interface/ThirdHitPredictionFromCircle.h"
0009 #include "FWCore/Utilities/interface/Likely.h"
0010
0011 #include <iostream>
0012 #include <algorithm>
0013
0014 #include "DataFormats/Math/interface/normalizedPhi.h"
0015 #include "DataFormats/Math/interface/approx_asin.h"
0016
0017 // there are tons of safety checks.
0018 // Try to move all of the out the regular control flow using gcc magic
0019 #include "DataFormats/Math/interface/approx_atan2.h"
0020 namespace {
0021   inline float f_atan2f(float y, float x) { return unsafe_atan2f<7>(y, x); }
0022   template <typename V>
0023   inline float f_phi(V v) {
0024     return f_atan2f(v.y(), v.x());
0025   }
0026 }  // namespace
0027
0028 namespace {
0029   template <class T>
0030   inline T sqr(T t) {
0031     return t * t;
0032   }
0033   template <class T>
0034   inline T sgn(T t) {
0035     return std::signbit(t) ? -T(1.) : T(1.);
0036   }
0037   template <class T>
0038   inline T clamped_acos(T x) {
0039     x = std::min(T(1.), std::max(-T(1.), x));
0040     return unsafe_acos<5>(x);
0041   }
0042
0043   template <class T>
0044   inline T clamped_sqrt(T t) {
0045     return std::sqrt(std::max(T(0), t));
0046   }
0047 }  // namespace
0048
0049 ThirdHitPredictionFromCircle::ThirdHitPredictionFromCircle(const GlobalPoint &P1,
0050                                                            const GlobalPoint &P2,
0051                                                            float tolerance)
0052     : p1(P1.x(), P1.y()), theTolerance(tolerance) {
0053   Vector2D p2(P2.x(), P2.y());
0054   Vector2D diff = 0.5 * (p2 - p1);
0055   delta2 = diff.mag2();
0056   delta = std::sqrt(delta2);
0057   axis = Vector2D(-diff.y(), diff.x()) / delta;
0058   center = p1 + diff;
0059 }
0060
0061 float ThirdHitPredictionFromCircle::phi(float curvature, float radius) const {
0062   float phi;
0063   if (UNLIKELY(std::abs(curvature) < float(1.0e-5))) {
0064     float cos = (center * axis) / radius;
0065     phi = axis.phi() - clamped_acos(cos);
0066   } else {
0067     float sign = sgn(curvature);
0068     float radius2 = sqr(1.0f / curvature);
0069     float orthog = clamped_sqrt(radius2 - delta2);
0070     Basic2DVector<float> lcenter = Basic2DVector<float>(center) - sign * orthog * Basic2DVector<float>(axis);
0071     float rc2 = lcenter.mag2();
0072     float r2 = sqr(radius);
0073     float cos = (rc2 + r2 - radius2) / std::sqrt(4.f * rc2 * r2);
0074     phi = f_phi(lcenter) + sign * clamped_acos(cos);
0075   }
0076   return phi;  // not normalized
0077 }
0078
0079 float ThirdHitPredictionFromCircle::angle(float curvature, float radius) const {
0080   if (UNLIKELY(std::abs(curvature) < float(1.0e-5))) {
0081     float sin = (center * axis) / radius;
0082     return sin / clamped_sqrt(1 - sqr(sin));
0083   } else {
0084     float radius2 = sqr(1.0f / curvature);
0085     float orthog = clamped_sqrt(radius2 - delta2);
0086     Basic2DVector<float> lcenter = Basic2DVector<float>(center) - sgn(curvature) * orthog * Basic2DVector<float>(axis);
0087
0088     float cos = (radius2 + sqr(radius) - lcenter.mag2()) * curvature / (2 * radius);
0089     return -cos / clamped_sqrt(1.f - sqr(cos));
0090   }
0091 }
0092
0093 ThirdHitPredictionFromCircle::Range ThirdHitPredictionFromCircle::operator()(Range curvature, float radius) const {
0094   float phi1 = normalizedPhi(phi(curvature.second, radius));
0095   float phi2 = proxim(phi(curvature.first, radius), phi1);
0096   if (phi2 < phi1)
0097     std::swap(phi2, phi1);
0098
0099   return Range(phi1 * radius - theTolerance, phi2 * radius + theTolerance);
0100 }
0101
0102 ThirdHitPredictionFromCircle::Range ThirdHitPredictionFromCircle::curvature(double transverseIP) const {
0103   // this is a mess.  Use a CAS and lots of drawings to verify...
0104
0105   transverseIP = std::abs(transverseIP);
0106   double transverseIP2 = sqr(transverseIP);
0107   double tip = axis * center;
0108   double tip2 = sqr(tip);
0109   double lip = axis.x() * center.y() - axis.y() * center.x();
0110   double lip2 = sqr(lip);
0111
0112   double origin = std::sqrt(tip2 + lip2);
0113   double tmp1 = lip2 + tip2 - transverseIP2;
0114   double tmp2 = 2. * (tip - transverseIP) * (tip + transverseIP);
0115   double tmp3 = 2. * delta * origin;
0116   double tmp4 = tmp1 + delta2;
0117   double tmp5 = 2. * delta * lip;
0118
0119   // I am probably being overly careful here with border cases
0120   // but you never know what crap you might get fed
0121   // VI fixed for finiteMath
0122
0123   double u1 = 0, u2 = 0;
0124   constexpr double SMALL = 1.0e-23;
0125   constexpr double LARGE = 1.0e23;
0126
0127   if (UNLIKELY(tmp4 - tmp5 < 1.0e-15)) {
0128     u1 = -SMALL;
0129     u2 = +SMALL;
0130   } else {
0131     if (UNLIKELY(std::abs(tmp2) < 1.0e-15)) {
0132       // the denominator is zero
0133       // this means that one of the tracks will be straight
0134       // and the other can be computed from the limit of the equation
0135       double tmp = lip2 - delta2;
0136       u1 = LARGE;
0137       u2 = (sqr(0.5 * tmp) - delta2 * tip2) / (tmp * tip);
0138       if (tip < 0)
0139         std::swap(u1, u2);
0140     } else {
0141       double tmp6 = (tmp4 - tmp5) * (tmp4 + tmp5);
0142       if (UNLIKELY(tmp6 < 1.0e-15)) {
0143         u1 = -SMALL;
0144         u2 = +SMALL;
0145       } else {
0146         double tmp7 = tmp6 > 0 ? (transverseIP * std::sqrt(tmp6) / tmp2) : 0.;
0147         double tmp8 = tip * (tmp1 - delta2) / tmp2;
0148         // the two quadratic solutions
0149         u1 = tmp8 + tmp7;
0150         u2 = tmp8 - tmp7;
0151       }
0152     }
0153
0154     if (tmp4 <= std::abs(tmp3)) {
0155       if ((tmp3 < 0) == (tip < 0))
0156         u2 = +SMALL;
0157       else
0158         u1 = -SMALL;
0159     }
0160   }
0161
0162   return Range(sgn(u1) / std::sqrt(sqr(u1) + delta2), sgn(u2) / std::sqrt(sqr(u2) + delta2));
0163 }
0164
0165 ThirdHitPredictionFromCircle::Scalar ThirdHitPredictionFromCircle::invCenterOnAxis(const Vector2D &p2) const {
0166   Vector2D del = p2 - p1;
0167   Vector2D axis2 = Vector2D(-del.y(), del.x()) / del.mag();
0168   Vector2D diff = p1 + 0.5f * del - center;
0169   Scalar a = diff.y() * axis2.x() - diff.x() * axis2.y();
0170   Scalar b = axis.y() * axis2.x() - axis.x() * axis2.y();
0171   return b / a;
0172 }
0173
0174 double ThirdHitPredictionFromCircle::curvature(const Vector2D &p2) const {
0175   double invDist = invCenterOnAxis(p2);
0176   double invDist2 = sqr(invDist);
0177   double curv = std::sqrt(invDist2 / (1. + invDist2 * delta2));
0178   return sgn(invDist) * curv;
0179 }
0180
0181 double ThirdHitPredictionFromCircle::transverseIP(const Vector2D &p2) const {
0182   double invDist = invCenterOnAxis(p2);
0183   if (UNLIKELY(std::abs(invDist) < 1.0e-5))
0184     return std::abs(p2 * axis);
0185   else {
0186     double dist = 1.0 / invDist;
0187     double radius = std::sqrt(sqr(dist) + delta2);
0188     return std::abs((center + axis * dist).mag() - radius);
0189   }
0190 }
0191
0192 //------------------------------------------------------------------------------
0193
0194 namespace {
0195   // 2asin(cd)/c
0196   inline float arc(float c, float d) {
0197     float z = c * d;
0198     z *= z;
0199     float x = 2.f * d;
0200
0201     if (z > 0.5f)
0202       return std::min(1.f, 2.f * unsafe_asin<5>(c * d) / c);
0203
0204     return x * approx_asin_P<7>(z);
0205   }
0206 }  // namespace
0207
0208 ThirdHitPredictionFromCircle::HelixRZ::HelixRZ(const ThirdHitPredictionFromCircle *icircle,
0209                                                double iz1,
0210                                                double z2,
0211                                                double curv)
0212     : circle(icircle), curvature(curv), radius(1. / curv), z1(iz1) {
0213   Scalar orthog = sgn(curv) * clamped_sqrt(radius * radius - circle->delta2);
0214   center = circle->center + orthog * circle->axis;
0215
0216   Scalar absCurv = std::abs(curv);
0217   seg = circle->delta;
0218   seg = arc(absCurv, seg);
0219   dzdu = (z2 - z1) / seg;
0220 }
0221
0222 double ThirdHitPredictionFromCircle::HelixRZ::maxCurvature(const ThirdHitPredictionFromCircle *circle,
0223                                                            double z1,
0224                                                            double z2,
0225                                                            double z3) {
0226   constexpr double maxAngle = M_PI;
0227   double halfAngle = (0.5 * maxAngle) * (z2 - z1) / (z3 - z1);
0228   if (UNLIKELY(halfAngle <= 0.0))
0229     return 0.0;
0230
0231   return std::sin(halfAngle) / circle->delta;
0232 }
0233
0234 ThirdHitPredictionFromCircle::HelixRZ::Scalar ThirdHitPredictionFromCircle::HelixRZ::zAtR(Scalar r) const {
0235   if (UNLIKELY(std::abs(curvature) < 1.0e-5)) {
0236     Scalar tip = circle->axis * circle->p1;
0237     Scalar lip = circle->axis.y() * circle->p1.x() - circle->axis.x() * circle->p1.y();
0238     return z1 + (std::sqrt(sqr(r) - sqr(tip)) - lip) * dzdu;
0239   }
0240
0241   Scalar radius2 = sqr(radius);
0242
0243   Scalar b2 = center.mag2();
0244   Scalar b = std::sqrt(b2);
0245
0246   Scalar cos1 = 0.5 * curvature * (radius2 + b2 - sqr(r)) / b;
0247   Scalar cos2 = 0.5 * curvature * (radius2 + b2 - circle->p1.mag2()) / b;
0248
0249   Scalar phi1 = clamped_acos(cos1);
0250   Scalar phi2 = clamped_acos(cos2);
0251
0252   // more plausbility checks needed...
0253   // the two circles can have two possible intersections
0254   Scalar u1 = std::abs((phi1 - phi2) * radius);
0255   Scalar u2 = std::abs((phi1 + phi2) * radius);
0256
0257   return z1 + ((u1 >= seg && u1 < u2) ? u1 : u2) * dzdu;
0258 }
0259
0260 #include <vdt/sincos.h>
0261
0262 ThirdHitPredictionFromCircle::HelixRZ::Scalar ThirdHitPredictionFromCircle::HelixRZ::rAtZ(Scalar z) const {
0263   if (UNLIKELY(std::abs(dzdu) < 1.0e-5))
0264     return 99999.0;
0265
0266   if (UNLIKELY(std::abs(curvature) < 1.0e-5)) {
0267     Scalar tip = circle->axis * circle->p1;
0268     Scalar lip = circle->axis.y() * circle->p1.x() - circle->axis.x() * circle->p1.y();
0269     return std::sqrt(sqr(tip) + sqr(lip + (z - z1) / dzdu));
0270   }
0271
0272   // we won't go below that (see comment below)
0273   Scalar minR2 = (2. * circle->center - circle->p1).mag2();
0274
0275   float phi = curvature * (z - z1) / dzdu;
0276
0277   if (UNLIKELY(std::abs(phi) > 2. * M_PI)) {
0278     // with a helix we can get problems here - this is used to get the limits
0279     // however, if phi gets large, we get into the regions where we loop back
0280     // to smaller r's.  The question is - do we care about these tracks?
0281     // The answer is probably no:  Too much pain, and the rest of the
0282     // tracking won't handle looping tracks anyway.
0283     // So, what we do here is to return nothing smaller than the radius
0284     // than any of the two hits, i.e. the second hit, which is presumably
0285     // outside of the 1st hit.
0286
0287     return std::sqrt(minR2);
0288   }
0289
0290   Vector2D rel = circle->p1 - center;
0291
0292   float c;
0293   float s;
0294   vdt::fast_sincosf(phi, s, c);
0295
0296   Vector2D p(center.x() + c * rel.x() - s * rel.y(), center.y() + s * rel.x() + c * rel.y());
0297
0298   return std::sqrt(std::max(minR2, p.mag2()));
0299 }