MkFitCore/src/MiniPropagators.cc

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