[Common] add cpaRZ in RecoDecay.h (#16227)

Author: dsekihat

Common/Core/RecoDecay.h

@@ -279,6 +279,27 @@ struct RecoDecay {
     return cos;
   }
 
+  /// Calculates cosine of pointing angle in the {r, z} plane.
+  /// \param posPV {x, y, z} position of the primary vertex
+  /// \param posSV {x, y, z} position of the secondary vertex
+  /// \param mom {x, y, z} momentum array
+  /// \return cosine of pointing angle in {r, z}
+  template <std::size_t N, typename T, typename U, typename V>
+  static double cpaRZ(const T& posPV, const U& posSV, const std::array<V, 3>& mom)
+  {
+    // CPARZ = (r . pz)/(|r| |pz|)
+    auto lineDecay = std::array{sqrtSumOfSquares(posSV[0] - posPV[0], posSV[1] - posPV[1]), posSV[2] - posPV[2]};
+    auto momRZ = std::array{sqrtSumOfSquares(mom[0], mom[1]), mom[2]};
+    auto cos = dotProd(lineDecay, momRZ) / std::sqrt(mag2(lineDecay) * mag2(momRZ));
+    if (cos < -1.) {
+      return -1.;
+    }
+    if (cos > 1.) {
+      return 1.;
+    }
+    return cos;
+  }
+
   /// Calculates proper lifetime times c.
   /// \note Promotes numbers to double before squaring to avoid precision loss in float multiplication.
   /// \param mom  3-momentum array