Back to home page

Project CMSSW displayed by LXR

 
 

    


File indexing completed on 2025-01-18 03:42:18

0001 #include "RecoTracker/MkFitCore/src/MiniPropagators.h"
0002 #include "RecoTracker/MkFitCore/interface/TrackerInfo.h"
0003 #include "vdt/atan2.h"
0004 #include "vdt/tan.h"
0005 #include "vdt/sincos.h"
0006 #include "vdt/sqrt.h"
0007 
0008 namespace mkfit::mini_propagators {
0009 
0010   State::State(const MPlexLV& par, int ti) {
0011     x = par.constAt(ti, 0, 0);
0012     y = par.constAt(ti, 1, 0);
0013     z = par.constAt(ti, 2, 0);
0014     const float pt = 1.0f / par.constAt(ti, 3, 0);
0015     px = pt * std::cos(par.constAt(ti, 4, 0));
0016     py = pt * std::sin(par.constAt(ti, 4, 0));
0017     pz = pt / std::tan(par.constAt(ti, 5, 0));
0018   }
0019 
0020   bool InitialState::propagate_to_r(PropAlgo_e algo, float R, State& c, bool update_momentum) const {
0021     switch (algo) {
0022       case PA_Line: {
0023       }
0024       case PA_Quadratic: {
0025       }
0026 
0027       case PA_Exact: {
0028         // Momentum is always updated -- used as temporary for stepping.
0029         const float k = 1.0f / inv_k;
0030 
0031         const float curv = 0.5f * inv_k * inv_pt;
0032         const float oo_curv = 1.0f / curv;  // 2 * radius of curvature
0033         const float lambda = pz * inv_pt;
0034 
0035         float D = 0;
0036 
0037         c = *this;
0038         c.dalpha = 0;
0039         for (int i = 0; i < Config::Niter; ++i) {
0040           // compute tangental and ideal distance for the current iteration.
0041           // 3-rd order asin for symmetric incidence (shortest arc lenght).
0042           float r0 = hipo(c.x, c.y);
0043           float td = (R - r0) * curv;
0044           float id = oo_curv * td * (1.0f + 0.16666666f * td * td);
0045           // This would be for line approximation:
0046           // float id = R - r0;
0047           D += id;
0048 
0049           //printf("%-3d r0=%f R-r0=%f td=%f id=%f id_line=%f delta_id=%g\n",
0050           //       i, r0, R-r0, td, id, R - r0, id - (R-r0));
0051 
0052           float alpha = id * inv_pt * inv_k;
0053           float sina, cosa;
0054           vdt::fast_sincosf(alpha, sina, cosa);
0055 
0056           // update parameters
0057           c.dalpha += alpha;
0058           c.x += k * (c.px * sina - c.py * (1.0f - cosa));
0059           c.y += k * (c.py * sina + c.px * (1.0f - cosa));
0060 
0061           const float o_px = c.px;  // copy before overwriting
0062           c.px = c.px * cosa - c.py * sina;
0063           c.py = c.py * cosa + o_px * sina;
0064         }
0065 
0066         c.z += lambda * D;
0067       }
0068     }
0069     // should have some epsilon constant / member? relative vs. abs?
0070     c.fail_flag = std::abs(hipo(c.x, c.y) - R) < 0.1f ? 0 : 1;
0071     return c.fail_flag;
0072   }
0073 
0074   bool InitialState::propagate_to_z(PropAlgo_e algo, float Z, State& c, bool update_momentum) const {
0075     switch (algo) {
0076       case PA_Line: {
0077       }
0078       case PA_Quadratic: {
0079       }
0080 
0081       case PA_Exact: {
0082         const float k = 1.0f / inv_k;
0083 
0084         const float dz = Z - z;
0085         const float alpha = dz * inv_k / pz;
0086 
0087         float sina, cosa;
0088         vdt::fast_sincosf(alpha, sina, cosa);
0089 
0090         c.dalpha = alpha;
0091         c.x = x + k * (px * sina - py * (1.0f - cosa));
0092         c.y = y + k * (py * sina + px * (1.0f - cosa));
0093         c.z = Z;
0094 
0095         if (update_momentum) {
0096           c.px = px * cosa - py * sina;
0097           c.py = py * cosa + px * sina;
0098           c.pz = pz;
0099         }
0100       } break;
0101     }
0102     c.fail_flag = 0;
0103     return c.fail_flag;
0104   }
0105 
0106   bool InitialState::propagate_to_plane(PropAlgo_e algo, const ModuleInfo& mi, State& c, bool update_momentum) const {
0107     switch (algo) {
0108       case PA_Line: {
0109         // Momentum is never changed ... we simply step along its direction
0110         // to hit the plane.
0111         // const float k = 1.0f / inv_k;
0112 
0113         // const float curv = 0.5f * inv_k * inv_pt;
0114         // const float oo_curv = 1.0f / curv;  // 2 * radius of curvature
0115         // const float lambda = pz * inv_pt;
0116 
0117         //X float dist = (x - mi.pos(0)) * mi.zdir(0) +
0118         //X              (y - mi.pos(1)) * mi.zdir(1) +
0119         //X              (z - mi.pos(2)) * mi.zdir(2);
0120 
0121         // t * p_vec intersects the plane:
0122         float t = (mi.pos(0) * mi.zdir(0) + mi.pos(1) * mi.zdir(1) + mi.pos(2) * mi.zdir(2) - x * mi.zdir(0) -
0123                    y * mi.zdir(1) - z * mi.zdir(2)) /
0124                   (px * mi.zdir(0) + py * mi.zdir(1) + pz * mi.zdir(2));
0125 
0126         //X printf("  module-center: %.2f,%.2f,%.2f  pos: %.2f,%.2f,%.2f  normal: %.2f,%.2f,%.2f\n",
0127         //X        mi.pos(0),mi.pos(1),mi.pos(2), x, y, z, mi.zdir(0),mi.zdir(1),mi.zdir(2));
0128 
0129         c = *this;
0130         c.x += t * c.px;
0131         c.y += t * c.py;
0132         c.z += t * c.pz;
0133 
0134         //X re-check ditance to plane
0135         //X float dist2 = (c.x - mi.pos(0)) * mi.zdir(0) +
0136         //X               (c.y - mi.pos(1)) * mi.zdir(1) +
0137         //X               (c.z - mi.pos(2)) * mi.zdir(2);
0138         //X printf("  dist = %.3f, t = %.4f ..... dist2 = %.4f\n", dist, t, dist2);
0139         break;
0140       }
0141 
0142       case PA_Quadratic: {
0143         throw std::runtime_error("Quadratic prop_to_plane not implemented");
0144       }
0145 
0146       case PA_Exact: {
0147         throw std::runtime_error("Exact prop_to_plane not implemented");
0148       }
0149     }
0150     return false;
0151   }
0152 
0153   //===========================================================================
0154   // Vectorized version
0155   //===========================================================================
0156 
0157   MPF fast_atan2(const MPF& y, const MPF& x) {
0158     MPF t;
0159     for (int i = 0; i < y.kTotSize; ++i) {
0160       t[i] = vdt::fast_atan2f(y[i], x[i]);
0161     }
0162     return t;
0163   }
0164 
0165   MPF fast_tan(const MPF& a) {
0166     MPF t;
0167     for (int i = 0; i < a.kTotSize; ++i) {
0168       t[i] = vdt::fast_tanf(a[i]);
0169     }
0170     return t;
0171   }
0172 
0173   void fast_sincos(const MPF& a, MPF& s, MPF& c) {
0174     for (int i = 0; i < a.kTotSize; ++i) {
0175       vdt::fast_sincosf(a[i], s[i], c[i]);
0176     }
0177   }
0178 
0179   StatePlex::StatePlex(const MPlexLV& par) {
0180     x = par.ReduceFixedIJ(0, 0);
0181     y = par.ReduceFixedIJ(1, 0);
0182     z = par.ReduceFixedIJ(2, 0);
0183     const MPF pt = 1.0f / par.ReduceFixedIJ(3, 0);
0184     fast_sincos(par.ReduceFixedIJ(4, 0), py, px);
0185     px *= pt;
0186     py *= pt;
0187     pz = pt / fast_tan(par.ReduceFixedIJ(5, 0));
0188   }
0189 
0190   // propagate to radius; returns number of failed propagations
0191   int InitialStatePlex::propagate_to_r(
0192       PropAlgo_e algo, const MPF& R, StatePlex& c, bool update_momentum, int N_proc) const {
0193     switch (algo) {
0194       case PA_Line: {
0195       }
0196       case PA_Quadratic: {
0197       }
0198 
0199       case PA_Exact: {
0200         // Momentum is always updated -- used as temporary for stepping.
0201         const MPF k = 1.0f / inv_k;
0202 
0203         const MPF curv = 0.5f * inv_k * inv_pt;
0204         const MPF oo_curv = 1.0f / curv;  // 2 * radius of curvature
0205         const MPF lambda = pz * inv_pt;
0206 
0207         MPF D = 0;
0208 
0209         c = *this;
0210         c.dalpha = 0;
0211         for (int i = 0; i < Config::Niter; ++i) {
0212           // compute tangental and ideal distance for the current iteration.
0213           // 3-rd order asin for symmetric incidence (shortest arc lenght).
0214           MPF r0 = Matriplex::hypot(c.x, c.y);
0215           MPF td = (R - r0) * curv;
0216           MPF id = oo_curv * td * (1.0f + 0.16666666f * td * td);
0217           // This would be for line approximation:
0218           // float id = R - r0;
0219           D += id;
0220 
0221           //printf("%-3d r0=%f R-r0=%f td=%f id=%f id_line=%f delta_id=%g\n",
0222           //       i, r0, R-r0, td, id, R - r0, id - (R-r0));
0223 
0224           MPF alpha = id * inv_pt * inv_k;
0225 
0226           MPF sina, cosa;
0227           fast_sincos(alpha, sina, cosa);
0228 
0229           // update parameters
0230           c.dalpha += alpha;
0231           c.x += k * (c.px * sina - c.py * (1.0f - cosa));
0232           c.y += k * (c.py * sina + c.px * (1.0f - cosa));
0233 
0234           MPF o_px = c.px;  // copy before overwriting
0235           c.px = c.px * cosa - c.py * sina;
0236           c.py = c.py * cosa + o_px * sina;
0237         }
0238 
0239         c.z += lambda * D;
0240       }
0241     }
0242 
0243     // should have some epsilon constant / member? relative vs. abs?
0244     MPF r = Matriplex::hypot(c.x, c.y);
0245     c.fail_flag = 0;
0246     int n_fail = 0;
0247     for (int i = 0; i < N_proc; ++i) {
0248       if (std::abs(R[i] - r[i]) > 0.1f) {
0249         c.fail_flag[i] = 1;
0250         ++n_fail;
0251       }
0252     }
0253     return n_fail;
0254   }
0255 
0256   int InitialStatePlex::propagate_to_z(
0257       PropAlgo_e algo, const MPF& Z, StatePlex& c, bool update_momentum, int N_proc) const {
0258     switch (algo) {
0259       case PA_Line: {
0260       }
0261       case PA_Quadratic: {
0262       }
0263 
0264       case PA_Exact: {
0265         MPF k = 1.0f / inv_k;
0266 
0267         MPF dz = Z - z;
0268         MPF alpha = dz * inv_k / pz;
0269 
0270         MPF sina, cosa;
0271         fast_sincos(alpha, sina, cosa);
0272 
0273         c.dalpha = alpha;
0274         c.x = x + k * (px * sina - py * (1.0f - cosa));
0275         c.y = y + k * (py * sina + px * (1.0f - cosa));
0276         c.z = Z;
0277 
0278         if (update_momentum) {
0279           c.px = px * cosa - py * sina;
0280           c.py = py * cosa + px * sina;
0281           c.pz = pz;
0282         }
0283       } break;
0284     }
0285     c.fail_flag = 0;
0286     return 0;
0287   }
0288 
0289 }  // namespace mkfit::mini_propagators