PropagationMPlex.icc

///////////////////////////////////////////////////////////////////////////////
/// helixAtRFromIterativeCCS_impl
///////////////////////////////////////////////////////////////////////////////

template <typename Tf, typename Ti, typename TfLL1, typename Tf11, typename TfLLL>
static inline void helixAtRFromIterativeCCS_impl(const Tf& __restrict__ inPar,
                                                 const Ti& __restrict__ inChg,
                                                 const Tf11& __restrict__ msRad,
                                                 TfLL1& __restrict__ outPar,
                                                 TfLLL& __restrict__ errorProp,
                                                 Ti& __restrict__ outFailFlag,  // expected to be initialized to 0
                                                 const int nmin,
                                                 const int nmax,
                                                 const int N_proc,
                                                 const PropagationFlags pf) {
  // bool debug = true;

#pragma omp simd
  for (int n = nmin; n < nmax; ++n) {
    //initialize erroProp to identity matrix
    errorProp(n, 0, 0) = 1.f;
    errorProp(n, 1, 1) = 1.f;
    errorProp(n, 2, 2) = 1.f;
    errorProp(n, 3, 3) = 1.f;
    errorProp(n, 4, 4) = 1.f;
    errorProp(n, 5, 5) = 1.f;
  }
  float r0[nmax - nmin];
#pragma omp simd
  for (int n = nmin; n < nmax; ++n) {
    r0[n - nmin] = hipo(inPar(n, 0, 0), inPar(n, 1, 0));
  }
  float k[nmax - nmin];
  if (pf.use_param_b_field) {
#pragma omp simd
    for (int n = nmin; n < nmax; ++n) {
      k[n - nmin] = inChg(n, 0, 0) * 100.f / (-Const::sol * Config::bFieldFromZR(inPar(n, 2, 0), r0[n - nmin]));
    }
  } else {
#pragma omp simd
    for (int n = nmin; n < nmax; ++n) {
      k[n - nmin] = inChg(n, 0, 0) * 100.f / (-Const::sol * Config::Bfield);
    }
  }
  float r[nmax - nmin];
#pragma omp simd
  for (int n = nmin; n < nmax; ++n) {
    r[n - nmin] = msRad(n, 0, 0);
  }

  float xin[nmax - nmin];
  float yin[nmax - nmin];
  float ipt[nmax - nmin];
  float phiin[nmax - nmin];
  float theta[nmax - nmin];
#pragma omp simd
  for (int n = nmin; n < nmax; ++n) {
    // if (std::abs(r-r0)<0.0001f) {
    // 	dprint("distance less than 1mum, skip");
    // 	continue;
    // }

    xin[n - nmin] = inPar(n, 0, 0);
    yin[n - nmin] = inPar(n, 1, 0);
    ipt[n - nmin] = inPar(n, 3, 0);
    phiin[n - nmin] = inPar(n, 4, 0);
    theta[n - nmin] = inPar(n, 5, 0);

    //dprint(std::endl);
  }

  for (int n = nmin; n < nmax; ++n) {
    dprint_np(n,
              "input parameters"
                  << " inPar(n, 0, 0)=" << std::setprecision(9) << inPar(n, 0, 0) << " inPar(n, 1, 0)="
                  << std::setprecision(9) << inPar(n, 1, 0) << " inPar(n, 2, 0)=" << std::setprecision(9)
                  << inPar(n, 2, 0) << " inPar(n, 3, 0)=" << std::setprecision(9) << inPar(n, 3, 0)
                  << " inPar(n, 4, 0)=" << std::setprecision(9) << inPar(n, 4, 0)
                  << " inPar(n, 5, 0)=" << std::setprecision(9) << inPar(n, 5, 0));
  }

  float kinv[nmax - nmin];
  float pt[nmax - nmin];
#pragma omp simd
  for (int n = nmin; n < nmax; ++n) {
    kinv[n - nmin] = 1.f / k[n - nmin];
    pt[n - nmin] = 1.f / ipt[n - nmin];
  }

  //no trig approx here, phi can be large
  float cosPorT[nmax - nmin];
  float sinPorT[nmax - nmin];
#pragma omp simd
  for (int n = nmin; n < nmax; ++n) {
    cosPorT[n - nmin] = std::cos(phiin[n - nmin]);
  }
#pragma omp simd
  for (int n = nmin; n < nmax; ++n) {
    sinPorT[n - nmin] = std::sin(phiin[n - nmin]);
  }

  for (int n = nmin; n < nmax; ++n) {
    dprint_np(n,
              "k=" << std::setprecision(9) << k[n - nmin] << " pxin=" << std::setprecision(9) << cosPorT[n - nmin] * pt[n - nmin]
                   << " pyin=" << std::setprecision(9) << sinPorT[n - nmin] * pt[n - nmin] << " cosPorT=" << std::setprecision(9)
                   << cosPorT[n - nmin] << " sinPorT=" << std::setprecision(9) << sinPorT[n - nmin]
                   << " pt=" << std::setprecision(9) << pt[n - nmin]);
  }

  // Use one over dot product of transverse momentum and radial
  // direction to scale the step. Propagation is prevented from reaching
  // too close to the apex (dotp > 0.2).
  // - Can / should we come up with a better approximation?
  // - Can / should take +/- curvature into account?

  float D[nmax - nmin];
  float oodotp[nmax - nmin];
  float oor0[nmax - nmin];
  float dr[nmax - nmin];
#pragma omp simd
  for (int n = nmin; n < nmax; ++n) {
    oodotp[n - nmin] = r0[n - nmin] / ( xin[n - nmin] * cosPorT[n - nmin] + yin[n - nmin] * sinPorT[n - nmin] );
  }
#pragma omp simd
  for (int n = nmin; n < nmax; ++n) {
    oor0[n - nmin] = 1.f / r0[n - nmin];
  }
#pragma omp simd
  for (int n = nmin; n < nmax; ++n) {
    dr[n - nmin] = r[n - nmin] - r0[n - nmin];
  }
#pragma omp simd
  for (int n = nmin; n < nmax; ++n) {
    // Can we come up with a better approximation?
    // Should take +/- curvature into account.
    D[n - nmin] = dr[n - nmin] * oodotp[n - nmin];
  }

  float dDdx[nmax - nmin];
  float dDdy[nmax - nmin];
  float dDdipt[nmax - nmin];
  float dDdphi[nmax - nmin];

#pragma omp simd
  for (int n = nmin; n < nmax; ++n) {
    dDdipt[n - nmin] = 0.;
  }
#pragma omp simd
  for (int n = nmin; n < nmax; ++n) {
    dDdphi[n - nmin] = D[n - nmin] * ( xin[n - nmin] * sinPorT[n - nmin] - yin[n - nmin] * cosPorT[n - nmin] ) * oor0[n - nmin] * oodotp[n - nmin];
  }
#pragma omp simd
  for (int n = nmin; n < nmax; ++n) {
    dDdx[n - nmin] = r0[n - nmin] > 0.f ? oor0[n - nmin] * ( D[n - nmin] * xin[n - nmin] * oor0[n - nmin] - D[n - nmin] * cosPorT[n - nmin] * oodotp[n - nmin] - xin[n - nmin] * oodotp[n - nmin] ) : 0.f;
  }
#pragma omp simd
  for (int n = nmin; n < nmax; ++n) {
    dDdy[n - nmin] = r0[n - nmin] > 0.f ? oor0[n - nmin] * ( D[n - nmin] * yin[n - nmin] * oor0[n - nmin] - D[n - nmin] * sinPorT[n - nmin] * oodotp[n - nmin] - yin[n - nmin] * oodotp[n - nmin] ) : 0.f;
  }

#pragma omp simd
  for (int n = nmin; n < nmax; ++n) {
    if (oodotp[n - nmin] > 5.0f || oodotp[n - nmin] < 0)  // 0.2 is 78.5 deg
    {
      outFailFlag(n, 0, 0) = 1;
      oodotp[n - nmin] = 0.0f;
      D[n - nmin] = 0.0f;
    } else if (dr[n - nmin] < 0.0f && pt[n - nmin] < 1.0f) {
      // Recompute the distance with scale down the correction for low-pT ingoing tracks.
      D[n - nmin] = dr[n - nmin] * (1.0f + (oodotp[n - nmin] - 1.0f) * pt[n - nmin]);
      // And update derivatives
      dDdipt[n - nmin] = - dr[n - nmin] * ( oodotp[n - nmin] - 1.f ) * pt[n - nmin] * pt[n - nmin];
      dDdphi[n - nmin] = pt[n - nmin] * dr[n - nmin] * ( xin[n - nmin] * sinPorT[n - nmin] - yin[n - nmin] * cosPorT[n - nmin] ) * oor0[n - nmin] * oodotp[n - nmin] * oodotp[n - nmin];
      dDdx[n - nmin] = pt[n - nmin] * dr[n - nmin] * oor0[n - nmin] * oodotp[n - nmin] * ( xin[n - nmin] * oor0[n - nmin] - cosPorT[n - nmin] * oodotp[n - nmin] ) - xin[n - nmin] * oor0[n - nmin] * (1.0f + (oodotp[n - nmin] - 1.0f) * pt[n - nmin]);
      dDdx[n - nmin] = pt[n - nmin] * dr[n - nmin] * oor0[n - nmin] * oodotp[n - nmin] * ( yin[n - nmin] * oor0[n - nmin] - sinPorT[n - nmin] * oodotp[n - nmin] ) - yin[n - nmin] * oor0[n - nmin] * (1.0f + (oodotp[n - nmin] - 1.0f) * pt[n - nmin]);
    }
  }

  float alpha[nmax - nmin];
  float dadx[nmax - nmin];
  float dady[nmax - nmin];
  float dadipt[nmax - nmin];
  float dadphi[nmax - nmin];
  float cosa[nmax - nmin];
  float sina[nmax - nmin];
  float cosah[nmax - nmin];
  float sinah[nmax - nmin];

#pragma omp simd
  for (int n = nmin; n < nmax; ++n) {
    alpha[n - nmin] = D[n - nmin] * ipt[n - nmin] * kinv[n - nmin];
    dadx[n - nmin] = dDdx[n - nmin] * ipt[n - nmin] * kinv[n - nmin];
    dady[n - nmin] = dDdy[n - nmin] * ipt[n - nmin] * kinv[n - nmin];
    dadipt[n - nmin] = (ipt[n - nmin] * dDdipt[n - nmin] + D[n - nmin]) * kinv[n - nmin];
    dadphi[n - nmin] = dDdphi[n - nmin] * ipt[n - nmin] * kinv[n - nmin];
  }

  if constexpr (Config::useTrigApprox) {
#if !defined(__INTEL_COMPILER)
#pragma omp simd
#endif
    for (int n = nmin; n < nmax; ++n) {
      sincos4(alpha[n - nmin] * 0.5f, sinah[n - nmin], cosah[n - nmin]);
    }
  } else {
#if !defined(__INTEL_COMPILER)
#pragma omp simd
#endif
    for (int n = nmin; n < nmax; ++n) {
      cosah[n - nmin] = std::cos(alpha[n - nmin] * 0.5f);
      sinah[n - nmin] = std::sin(alpha[n - nmin] * 0.5f);
    }
  }

#pragma omp simd
  for (int n = nmin; n < nmax; ++n) {
    cosa[n - nmin] = 1.f - 2.f * sinah[n - nmin] * sinah[n - nmin];
    sina[n - nmin] = 2.f * sinah[n - nmin] * cosah[n - nmin];
  }

  for (int n = nmin; n < nmax; ++n) {
    dprint_np(n,
              "Attempt propagation from r="
                  << r0[n - nmin] << " to r=" << r[n - nmin] << std::endl
                  << "   x=" << xin[n - nmin] << " y=" << yin[n - nmin] << " z=" << inPar(n, 2, 0)
                  << " px=" << cosPorT[n - nmin] * pt[n - nmin] << " py=" << sinPorT[n - nmin] * pt[n - nmin]
                  << " pz=" << pt[n - nmin] * std::tan(theta[n - nmin]) << " q=" << inChg(n, 0, 0) << std::endl
                  << "   r=" << std::setprecision(9) << r[n - nmin] << " r0=" << std::setprecision(9) << r0[n - nmin]
                  << " D=" << std::setprecision(9) << D[n - nmin] << " dr=" << std::setprecision(9)
                  << dr[n - nmin] << " cosa=" << cosa[n - nmin] << " sina=" << sina[n - nmin]
                  << " dir_cos(rad,pT)=" << 1.0f / oodotp[n]);
  }

#pragma omp simd
  for (int n = nmin; n < nmax; ++n) {
    outPar(n, 0, 0) = inPar(n, 0, 0) + 2.f * k[n - nmin] * sinah[n - nmin] * pt[n - nmin] *
                                            (cosPorT[n - nmin] * cosah[n - nmin] - sinPorT[n - nmin] * sinah[n - nmin]);
    errorProp(n, 0, 0) = 1.f + k[n - nmin] * dadx[n - nmin] *
                                   (cosPorT[n - nmin] * cosa[n - nmin] - sinPorT[n - nmin] * sina[n - nmin]) *
                                   pt[n - nmin];
    errorProp(n, 0, 1) = k[n - nmin] * dady[n - nmin] *
                         (cosPorT[n - nmin] * cosa[n - nmin] - sinPorT[n - nmin] * sina[n - nmin]) * pt[n - nmin];
    errorProp(n, 0, 2) = 0.f;
    errorProp(n, 0, 3) =
        k[n - nmin] *
        (cosPorT[n - nmin] * (ipt[n - nmin] * dadipt[n - nmin] * cosa[n - nmin] - sina[n - nmin]) +
         sinPorT[n - nmin] * ((1.f - cosa[n - nmin]) - ipt[n - nmin] * dadipt[n - nmin] * sina[n - nmin])) *
        pt[n - nmin] * pt[n - nmin];
    errorProp(n, 0, 4) =
        k[n - nmin] *
        (cosPorT[n - nmin] * dadphi[n - nmin] * cosa[n - nmin] - sinPorT[n - nmin] * dadphi[n - nmin] * sina[n - nmin] -
         sinPorT[n - nmin] * sina[n - nmin] + cosPorT[n - nmin] * cosa[n - nmin] - cosPorT[n - nmin]) *
        pt[n - nmin];
    errorProp(n, 0, 5) = 0.f;
  }

#pragma omp simd
  for (int n = nmin; n < nmax; ++n) {
    outPar(n, 1, 0) = inPar(n, 1, 0) + 2.f * k[n - nmin] * sinah[n - nmin] * pt[n - nmin] *
                                            (sinPorT[n - nmin] * cosah[n - nmin] + cosPorT[n - nmin] * sinah[n - nmin]);
    errorProp(n, 1, 0) = k[n - nmin] * dadx[n - nmin] *
                         (sinPorT[n - nmin] * cosa[n - nmin] + cosPorT[n - nmin] * sina[n - nmin]) * pt[n - nmin];
    errorProp(n, 1, 1) = 1.f + k[n - nmin] * dady[n - nmin] *
                                   (sinPorT[n - nmin] * cosa[n - nmin] + cosPorT[n - nmin] * sina[n - nmin]) *
                                   pt[n - nmin];
    errorProp(n, 1, 2) = 0.f;
    errorProp(n, 1, 3) =
        k[n - nmin] *
        (sinPorT[n - nmin] * (ipt[n - nmin] * dadipt[n - nmin] * cosa[n - nmin] - sina[n - nmin]) +
         cosPorT[n - nmin] * (ipt[n - nmin] * dadipt[n - nmin] * sina[n - nmin] - (1.f - cosa[n - nmin]))) *
        pt[n - nmin] * pt[n - nmin];
    errorProp(n, 1, 4) =
        k[n - nmin] *
        (sinPorT[n - nmin] * dadphi[n - nmin] * cosa[n - nmin] + cosPorT[n - nmin] * dadphi[n - nmin] * sina[n - nmin] +
         sinPorT[n - nmin] * cosa[n - nmin] + cosPorT[n - nmin] * sina[n - nmin] - sinPorT[n - nmin]) *
        pt[n - nmin];
    errorProp(n, 1, 5) = 0.f;
  }

#pragma omp simd
  for (int n = nmin; n < nmax; ++n) {
    //no trig approx here, theta can be large
    cosPorT[n - nmin] = std::cos(theta[n - nmin]);
  }

#pragma omp simd
  for (int n = nmin; n < nmax; ++n) {
    sinPorT[n - nmin] = std::sin(theta[n - nmin]);
  }

#pragma omp simd
  for (int n = nmin; n < nmax; ++n) {
    //redefine sinPorT as 1./sinPorT to reduce the number of temporaries
    sinPorT[n - nmin] = 1.f / sinPorT[n - nmin];
  }

#pragma omp simd
  for (int n = nmin; n < nmax; ++n) {
    outPar(n, 2, 0) =
        inPar(n, 2, 0) + k[n - nmin] * alpha[n - nmin] * cosPorT[n - nmin] * pt[n - nmin] * sinPorT[n - nmin];
    errorProp(n, 2, 0) = k[n - nmin] * cosPorT[n - nmin] * dadx[n - nmin] * pt[n - nmin] * sinPorT[n - nmin];
    errorProp(n, 2, 1) = k[n - nmin] * cosPorT[n - nmin] * dady[n - nmin] * pt[n - nmin] * sinPorT[n - nmin];
    errorProp(n, 2, 2) = 1.f;
    errorProp(n, 2, 3) = k[n - nmin] * cosPorT[n - nmin] * (ipt[n - nmin] * dadipt[n - nmin] - alpha[n - nmin]) *
                         pt[n - nmin] * pt[n - nmin] * sinPorT[n - nmin];
    errorProp(n, 2, 4) = k[n - nmin] * dadphi[n - nmin] * cosPorT[n - nmin] * pt[n - nmin] * sinPorT[n - nmin];
    errorProp(n, 2, 5) = -k[n - nmin] * alpha[n - nmin] * pt[n - nmin] * sinPorT[n - nmin] * sinPorT[n - nmin];
  }

#pragma omp simd
  for (int n = nmin; n < nmax; ++n) {
    outPar(n, 3, 0) = ipt[n - nmin];
    errorProp(n, 3, 0) = 0.f;
    errorProp(n, 3, 1) = 0.f;
    errorProp(n, 3, 2) = 0.f;
    errorProp(n, 3, 3) = 1.f;
    errorProp(n, 3, 4) = 0.f;
    errorProp(n, 3, 5) = 0.f;
  }

#pragma omp simd
  for (int n = nmin; n < nmax; ++n) {
    outPar(n, 4, 0) = inPar(n, 4, 0) + alpha[n - nmin];
    errorProp(n, 4, 0) = dadx[n - nmin];
    errorProp(n, 4, 1) = dady[n - nmin];
    errorProp(n, 4, 2) = 0.f;
    errorProp(n, 4, 3) = dadipt[n - nmin];
    errorProp(n, 4, 4) = 1.f + dadphi[n - nmin];
    errorProp(n, 4, 5) = 0.f;
  }

#pragma omp simd
  for (int n = nmin; n < nmax; ++n) {
    outPar(n, 5, 0) = theta[n - nmin];
    errorProp(n, 5, 0) = 0.f;
    errorProp(n, 5, 1) = 0.f;
    errorProp(n, 5, 2) = 0.f;
    errorProp(n, 5, 3) = 0.f;
    errorProp(n, 5, 4) = 0.f;
    errorProp(n, 5, 5) = 1.f;
  }

  for (int n = nmin; n < nmax; ++n) {
    dprint_np(n,
              "propagation end, dump parameters\n"
                  << "   pos = " << outPar(n, 0, 0) << " " << outPar(n, 1, 0) << " " << outPar(n, 2, 0) << "\t\t r="
                  << std::sqrt(outPar(n, 0, 0) * outPar(n, 0, 0) + outPar(n, 1, 0) * outPar(n, 1, 0)) << std::endl
                  << "   mom = " << std::cos(outPar(n, 4, 0)) / outPar(n, 3, 0) << " "
                  << std::sin(outPar(n, 4, 0)) / outPar(n, 3, 0) << " " << 1. / (outPar(n, 3, 0) * tan(outPar(n, 5, 0)))
                  << "\t\tpT=" << 1. / std::abs(outPar(n, 3, 0)) << std::endl);
  }

#ifdef DEBUG
  for (int n = nmin; n < nmax; ++n) {
    if (debug && g_debug && n < N_proc) {
      dmutex_guard;
      std::cout << n << ": jacobian" << std::endl;
      printf("%5f %5f %5f %5f %5f %5f\n",
             errorProp(n, 0, 0),
             errorProp(n, 0, 1),
             errorProp(n, 0, 2),
             errorProp(n, 0, 3),
             errorProp(n, 0, 4),
             errorProp(n, 0, 5));
      printf("%5f %5f %5f %5f %5f %5f\n",
             errorProp(n, 1, 0),
             errorProp(n, 1, 1),
             errorProp(n, 1, 2),
             errorProp(n, 1, 3),
             errorProp(n, 1, 4),
             errorProp(n, 1, 5));
      printf("%5f %5f %5f %5f %5f %5f\n",
             errorProp(n, 2, 0),
             errorProp(n, 2, 1),
             errorProp(n, 2, 2),
             errorProp(n, 2, 3),
             errorProp(n, 2, 4),
             errorProp(n, 2, 5));
      printf("%5f %5f %5f %5f %5f %5f\n",
             errorProp(n, 3, 0),
             errorProp(n, 3, 1),
             errorProp(n, 3, 2),
             errorProp(n, 3, 3),
             errorProp(n, 3, 4),
             errorProp(n, 3, 5));
      printf("%5f %5f %5f %5f %5f %5f\n",
             errorProp(n, 4, 0),
             errorProp(n, 4, 1),
             errorProp(n, 4, 2),
             errorProp(n, 4, 3),
             errorProp(n, 4, 4),
             errorProp(n, 4, 5));
      printf("%5f %5f %5f %5f %5f %5f\n",
             errorProp(n, 5, 0),
             errorProp(n, 5, 1),
             errorProp(n, 5, 2),
             errorProp(n, 5, 3),
             errorProp(n, 5, 4),
             errorProp(n, 5, 5));
      printf("\n");
    }
  }
#endif
}