PixelSeeding/plugins/ThirdHitPredictionFromInvParabola.cc

0001
0002 #include "ThirdHitPredictionFromInvParabola.h"
0003
0004 #include <cmath>
0005 #include "DataFormats/GeometryVector/interface/GlobalVector.h"
0006 #include "DataFormats/GeometryVector/interface/GlobalPoint.h"
0007
0008 #include "RecoTracker/TkHitPairs/interface/OrderedHitPair.h"
0009 #include "RecoTracker/TkTrackingRegions/interface/TrackingRegion.h"
0010 #include "RecoTracker/TkMSParametrization/interface/PixelRecoUtilities.h"
0011
0012 #include "RecoTracker/TkMSParametrization/interface/PixelRecoRange.h"
0013
0014 #include "DataFormats/Math/interface/approx_atan2.h"
0015 namespace {
0016   inline float f_atan2f(float y, float x) { return unsafe_atan2f<7>(y, x); }
0017   template <typename V>
0018   inline float f_phi(V v) {
0019     return f_atan2f(v.y(), v.x());
0020   }
0021 }  // namespace
0022
0023 namespace {
0024   template <class T>
0025   inline T sqr(T t) {
0026     return t * t;
0027   }
0028 }  // namespace
0029
0030 using namespace std;
0031
0032 ThirdHitPredictionFromInvParabola::ThirdHitPredictionFromInvParabola(
0033     const GlobalPoint& P1, const GlobalPoint& P2, Scalar ip, Scalar curv, Scalar tolerance)
0034     : theTolerance(tolerance) {
0035   init(P1, P2, ip, std::abs(curv));
0036 }
0037
0038 void ThirdHitPredictionFromInvParabola::init(Scalar x1, Scalar y1, Scalar x2, Scalar y2, Scalar ip, Scalar curv) {
0039   //  GlobalVector aX = GlobalVector( P2.x()-P1.x(), P2.y()-P1.y(), 0.).unit();
0040
0041   Point2D p1(x1, y1);
0042   Point2D p2(x2, y2);
0043   theRotation = Rotation(p1);
0044   p1 = transform(p1);  // (1./P1.xy().mag(),0);
0045   p2 = transform(p2);
0046
0047   u1u2 = p1.x() * p2.x();
0048   overDu = 1. / (p2.x() - p1.x());
0049   pv = p1.y() * p2.x() - p2.y() * p1.x();
0050   dv = p2.y() - p1.y();
0051   su = p2.x() + p1.x();
0052
0053   ip = std::abs(ip);
0054   RangeD ipRange(-ip, ip);
0055
0056   Scalar ipIntyPlus = ipFromCurvature(0., true);
0057   Scalar ipCurvPlus = ipFromCurvature(curv, true);
0058   Scalar ipCurvMinus = ipFromCurvature(curv, false);
0059
0060   RangeD ipRangePlus(std::min(ipIntyPlus, ipCurvPlus), std::max(ipIntyPlus, ipCurvPlus));
0061   RangeD ipRangeMinus(std::min(-ipIntyPlus, ipCurvMinus), std::max(-ipIntyPlus, ipCurvMinus));
0062
0063   theIpRangePlus = ipRangePlus.intersection(ipRange);
0064   theIpRangeMinus = ipRangeMinus.intersection(ipRange);
0065
0066   emptyP = theIpRangePlus.empty();
0067   emptyM = theIpRangeMinus.empty();
0068 }
0069
0070 ThirdHitPredictionFromInvParabola::Range ThirdHitPredictionFromInvParabola::rangeRPhi(Scalar radius,
0071                                                                                       int icharge) const {
0072   bool pos = icharge > 0;
0073
0074   RangeD ip = (pos) ? theIpRangePlus : theIpRangeMinus;
0075
0076   //  it will vectorize with gcc 4.7 (with -O3 -fno-math-errno)
0077   // change sign as intersect assume -ip for negative charge...
0078   Scalar ipv[2] = {(pos) ? ip.min() : -ip.min(), (pos) ? ip.max() : -ip.max()};
0079   Scalar u[2], v[2];
0080   for (int i = 0; i != 2; ++i)
0081     findPointAtCurve(radius, ipv[i], u[i], v[i]);
0082
0083   //
0084   Scalar phi1 = f_phi(theRotation.rotateBack(Point2D(u[0], v[0])));
0085   Scalar phi2 = phi1 + (v[1] - v[0]);
0086
0087   if (phi2 < phi1)
0088     std::swap(phi1, phi2);
0089
0090   if (ip.empty()) {
0091     Range r1(phi1 * radius - theTolerance, phi1 * radius + theTolerance);
0092     Range r2(phi2 * radius - theTolerance, phi2 * radius + theTolerance);
0093     return r1.intersection(r2);  // this range can be empty
0094   }
0095
0096   return Range(radius * phi1 - theTolerance, radius * phi2 + theTolerance);
0097 }
0098
0099 ThirdHitPredictionFromInvParabola::Range ThirdHitPredictionFromInvParabola::rangeRPhi(Scalar radius) const {
0100   auto getRange = [&](Scalar phi1, Scalar phi2, bool empty) -> RangeD {
0101     if (phi2 < phi1)
0102       std::swap(phi1, phi2);
0103     if (empty) {
0104       RangeD r1(phi1 * radius - theTolerance, phi1 * radius + theTolerance);
0105       RangeD r2(phi2 * radius - theTolerance, phi2 * radius + theTolerance);
0106       return r1.intersection(r2);
0107     }
0108
0109     return RangeD(radius * phi1 - theTolerance, radius * phi2 + theTolerance);
0110   };
0111
0112   //  it will vectorize with gcc 4.7 (with -O3 -fno-math-errno)
0113   // change sign as intersect assume -ip for negative charge...
0114   Scalar ipv[4] = {theIpRangePlus.min(), -theIpRangeMinus.min(), theIpRangePlus.max(), -theIpRangeMinus.max()};
0115   Scalar u[4], v[4];
0116   for (int i = 0; i < 4; ++i)
0117     findPointAtCurve(radius, ipv[i], u[i], v[i]);
0118
0119   //
0120   auto xr = theRotation.x();
0121   auto yr = theRotation.y();
0122
0123   Scalar phi1[2], phi2[2];
0124   for (int i = 0; i < 2; ++i) {
0125     auto x = xr[0] * u[i] + yr[0] * v[i];
0126     auto y = xr[1] * u[i] + yr[1] * v[i];
0127     phi1[i] = f_atan2f(y, x);
0128     phi2[i] = phi1[i] + (v[i + 2] - v[i]);
0129   }
0130
0131   return getRange(phi1[1], phi2[1], emptyM).sum(getRange(phi1[0], phi2[0], emptyP));
0132 }
0133
0134 /*
0135 ThirdHitPredictionFromInvParabola::Range ThirdHitPredictionFromInvParabola::rangeRPhiSlow(
0136     Scalar radius, int charge, int nIter) const
0137 {
0138   Range predRPhi(1.,-1.);
0139
0140   Scalar invr2 = 1/(radius*radius);
0141   Scalar u = sqrt(invr2);
0142   Scalar v = 0.;
0143
0144   Range ip = (charge > 0) ? theIpRangePlus : theIpRangeMinus;
0145
0146   for (int i=0; i < nIter; ++i) {
0147     v = predV(u, charge*ip.min());
0148     Scalar d2 = invr2-sqr(v);
0149     u = (d2 > 0) ? sqrt(d2) : 0.;
0150   }
0151   Point2D  pred_tmp1(u, v);
0152   Scalar phi1 = transformBack(pred_tmp1).phi();
0153   while ( phi1 >= M_PI) phi1 -= 2*M_PI;
0154   while ( phi1 < -M_PI) phi1 += 2*M_PI;
0155
0156
0157   u = sqrt(invr2);
0158   v=0;
0159   for (int i=0; i < nIter; ++i) {
0160     v = predV(u, ip.max(), charge);
0161     Scalar d2 = invr2-sqr(v);
0162     u = (d2 > 0) ? sqrt(d2) : 0.;
0163   }
0164   Point2D  pred_tmp2(u, v);
0165   Scalar phi2 = transformBack(pred_tmp2).phi();
0166   while ( phi2-phi1 >= M_PI) phi2 -= 2*M_PI;
0167   while ( phi2-phi1 < -M_PI) phi2 += 2*M_PI;
0168
0169 // check faster alternative, without while(..) it is enough to:
0170 //  phi2 = phi1+radius*(pred_tmp2.v()-pred_tmp1.v());
0171
0172   if (ip.empty()) {
0173     Range r1(phi1*radius-theTolerance, phi1*radius+theTolerance);
0174     Range r2(phi2*radius-theTolerance, phi2*radius+theTolerance);
0175     predRPhi = r1.intersection(r2);
0176   } else {
0177     Range r(phi1, phi2);
0178     r.sort();
0179     predRPhi= Range(radius*r.min()-theTolerance, radius*r.max()+theTolerance);
0180   }
0181   return predRPhi;
0182
0183 }
0184 */