Acts/Utilities/VectorHelpers.hpp

0001 // This file is part of the ACTS project.
0002 //
0003 // Copyright (C) 2016 CERN for the benefit of the ACTS project
0004 //
0005 // This Source Code Form is subject to the terms of the Mozilla Public
0006 // License, v. 2.0. If a copy of the MPL was not distributed with this
0007 // file, You can obtain one at https://mozilla.org/MPL/2.0/.
0008
0009 #pragma once
0010
0011 #include "Acts/Definitions/Algebra.hpp"
0012 #include "Acts/Definitions/Common.hpp"
0013 #include "Acts/Definitions/TrackParametrization.hpp"
0014 #include "Acts/Utilities/AngleHelpers.hpp"
0015 #include "Acts/Utilities/AxisDefinitions.hpp"
0016 #include "Acts/Utilities/MathHelpers.hpp"
0017 #include "Acts/Utilities/detail/periodic.hpp"
0018
0019 #include <array>
0020 #include <cassert>
0021 #include <limits>
0022 #include <numbers>
0023
0024 #include "Eigen/Dense"
0025 namespace Acts::VectorHelpers {
0026
0027 /// Calculate phi (transverse plane angle) from compatible Eigen types with
0028 /// static size
0029 /// @tparam Derived Eigen derived concrete type with compile-time known size >= 2
0030 /// @param v Any vector like Eigen type with static size
0031 /// @return The value of the angle in the transverse plane.
0032 template <typename Derived>
0033 double phi(const Eigen::MatrixBase<Derived>& v) noexcept
0034   requires(Eigen::MatrixBase<Derived>::RowsAtCompileTime >= 2)
0035 {
0036   return std::atan2(v[1], v[0]);
0037 }
0038
0039 /// Calculate phi (transverse plane angle) from compatible Eigen types with
0040 /// dynamic size
0041 /// @tparam Derived Eigen derived concrete type with dynamic size
0042 /// @param v Any vector like Eigen type with dynamic size
0043 /// @note Will abort execution if the number of rows of @p v is less than 2.
0044 /// @return The value of the angle in the transverse plane.
0045 template <typename Derived>
0046 double phi(const Eigen::MatrixBase<Derived>& v) noexcept
0047   requires(Eigen::MatrixBase<Derived>::RowsAtCompileTime == -1)
0048 {
0049   assert(v.rows() >= 2 && "Phi function not valid for vectors not at least 2D");
0050   return std::atan2(v[1], v[0]);
0051 }
0052
0053 /// Calculate phi (transverse plane angle) from anything implementing a method
0054 /// like `phi()` returning anything convertible to `double`.
0055 /// @tparam T anything that has a phi method
0056 /// @param v Any type that implements a phi method
0057 /// @return The phi value
0058 template <typename T>
0059 double phi(const T& v) noexcept
0060   requires requires {
0061     { v.phi() } -> std::floating_point;
0062   }
0063 {
0064   return v.phi();
0065 }
0066
0067 /// Calculate radius in the transverse (xy) plane of a vector
0068 /// @tparam Derived Eigen derived concrete type
0069 /// @param v Any vector like Eigen type, static or dynamic
0070 /// @note Will static assert that the number of rows of @p v is at least 2, or
0071 /// in case of dynamic size, will abort execution if that is not the case.
0072 /// @return The transverse radius value.
0073 template <typename Derived>
0074 double perp(const Eigen::MatrixBase<Derived>& v) noexcept {
0075   constexpr int rows = Eigen::MatrixBase<Derived>::RowsAtCompileTime;
0076   if constexpr (rows != -1) {
0077     // static size, do compile time check
0078     static_assert(rows >= 2,
0079                   "Perp function not valid for vectors not at least 2D");
0080   } else {
0081     // dynamic size
0082     assert(v.rows() >= 2 &&
0083            "Perp function not valid for vectors not at least 2D");
0084   }
0085   return v.template head<2>().norm();
0086 }
0087
0088 /// Calculate the theta angle (longitudinal w.r.t. z axis) of a vector with
0089 /// static size
0090 /// @tparam Derived Eigen derived concrete type with compile-time size == 3
0091 /// @param v Any 3D vector like Eigen type with static size
0092 /// @return The theta value
0093 template <typename Derived>
0094 double theta(const Eigen::MatrixBase<Derived>& v) noexcept
0095   requires(Eigen::MatrixBase<Derived>::RowsAtCompileTime == 3)
0096 {
0097   return std::atan2(perp(v), v[2]);
0098 }
0099
0100 /// Calculate the theta angle (longitudinal w.r.t. z axis) of a vector with
0101 /// dynamic size
0102 /// @tparam Derived Eigen derived concrete type with dynamic size
0103 /// @param v Any vector like Eigen type with dynamic size
0104 /// @note Will abort execution if the number of rows of @p v is not exactly 3.
0105 /// @return The theta value
0106 template <typename Derived>
0107 double theta(const Eigen::MatrixBase<Derived>& v) noexcept
0108   requires(Eigen::MatrixBase<Derived>::RowsAtCompileTime == -1)
0109 {
0110   assert(v.rows() == 3 && "Theta function not valid for non-3D vectors.");
0111   return std::atan2(perp(v), v[2]);
0112 }
0113
0114 /// Calculate the pseudorapidity for a vector with static size.
0115 /// @tparam Derived Eigen derived concrete type with compile-time size == 3
0116 /// @param v Any 3D vector like Eigen type with static size
0117 /// @return The pseudorapidity value
0118 template <typename Derived>
0119 double eta(const Eigen::MatrixBase<Derived>& v) noexcept
0120   requires(Eigen::MatrixBase<Derived>::RowsAtCompileTime == 3)
0121 {
0122   if (v[0] == 0. && v[1] == 0.) {
0123     return std::copysign(std::numeric_limits<double>::infinity(), v[2]);
0124   } else {
0125     return std::asinh(v[2] / perp(v));
0126   }
0127 }
0128
0129 /// Calculate the pseudorapidity for a vector with dynamic size.
0130 /// @tparam Derived Eigen derived concrete type with dynamic size
0131 /// @param v Any vector like Eigen type with dynamic size
0132 /// @note Will abort execution if the number of rows of @p v is not exactly 3.
0133 /// @return The pseudorapidity value
0134 template <typename Derived>
0135 double eta(const Eigen::MatrixBase<Derived>& v) noexcept
0136   requires(Eigen::MatrixBase<Derived>::RowsAtCompileTime == -1)
0137 {
0138   assert(v.rows() == 3 && "Eta function not valid for non-3D vectors.");
0139   if (v[0] == 0. && v[1] == 0.) {
0140     return std::copysign(std::numeric_limits<double>::infinity(), v[2]);
0141   } else {
0142     return std::asinh(v[2] / perp(v));
0143   }
0144 }
0145
0146 /// Calculate the pseudo rapdity from anything implementing a method
0147 /// like `theta()` returning anything convertible to `double`.
0148 /// @tparam T anything that has a theta method
0149 /// @param v Any type that implements a theta method
0150 /// @return The pseudo rapidity value
0151 template <typename T>
0152 double eta(const T& v) noexcept
0153   requires requires {
0154     { v.theta() } -> std::floating_point;
0155   }
0156 {
0157   return Acts::AngleHelpers::etaFromTheta(v.theta());
0158 }
0159
0160 /// @brief Fast evaluation of trigonomic functions.
0161 ///
0162 /// @param direction for this evaluatoin
0163 ///
0164 /// @return cos(phi), sin(phi), cos(theta), sin(theta), 1/sin(theta)
0165 inline std::array<double, 4> evaluateTrigonomics(const Vector3& direction) {
0166   const double x = direction(0);  // == cos(phi) * sin(theta)
0167   const double y = direction(1);  // == sin(phi) * sin(theta)
0168   const double z = direction(2);  // == cos(theta)
0169   // can be turned into cosine/sine
0170   const double cosTheta = z;
0171   const double sinTheta = fastCathetus(1, z);
0172   assert(sinTheta != 0 &&
0173          "VectorHelpers: Vector is parallel to the z-axis "
0174          "which leads to division by zero");
0175   const double invSinTheta = 1. / sinTheta;
0176   const double cosPhi = x * invSinTheta;
0177   const double sinPhi = y * invSinTheta;
0178
0179   return {cosPhi, sinPhi, cosTheta, sinTheta};
0180 }
0181
0182 /// Helper method to extract the binning value from a 3D vector.
0183 ///
0184 /// For this method a 3D vector is required to guarantee all potential
0185 /// axis directions to be casted from
0186 ///
0187 /// @param position is the position in global
0188 /// @param aDir is the axis direction to be extracted
0189 ///
0190 /// @return the value of the binning direction
0191 inline double cast(const Vector3& position, AxisDirection aDir) {
0192   using enum AxisDirection;
0193   switch (aDir) {
0194     case AxisX:
0195       return position[0];
0196     case AxisY:
0197       return position[1];
0198     case AxisZ:
0199       return position[2];
0200     case AxisR:
0201       return perp(position);
0202     case AxisPhi:
0203       return phi(position);
0204     case AxisRPhi:
0205       return perp(position) * phi(position);
0206     case AxisTheta:
0207       return theta(position);
0208     case AxisEta:
0209       return eta(position);
0210     case AxisMag:
0211       return position.norm();
0212     default:
0213       assert(false && "Invalid AxisDirection enum value");
0214       return std::numeric_limits<double>::quiet_NaN();
0215   }
0216 }
0217
0218 /// @brief Calculates column-wise cross products of a matrix and a vector and
0219 /// stores the result column-wise in a matrix.
0220 ///
0221 /// @param [in] m Matrix that will be used for cross products
0222 /// @param [in] v Vector for cross products
0223 /// @return Constructed matrix
0224 inline SquareMatrix3 cross(const SquareMatrix3& m, const Vector3& v) {
0225   SquareMatrix3 r;
0226   r.col(0) = m.col(0).cross(v);
0227   r.col(1) = m.col(1).cross(v);
0228   r.col(2) = m.col(2).cross(v);
0229
0230   return r;
0231 }
0232
0233 /// Access the three-position components in a four-position vector.
0234 inline auto position(const Vector4& pos4) {
0235   return pos4.segment<3>(ePos0);
0236 }
0237
0238 /// Access the three-position components in a free parameters vector.
0239 inline auto position(const FreeVector& params) {
0240   return params.segment<3>(eFreePos0);
0241 }
0242
0243 /// Construct a four-vector from a three-vector and scalar fourth component.
0244 template <typename vector3_t>
0245 inline auto makeVector4(const Eigen::MatrixBase<vector3_t>& vec3,
0246                         typename vector3_t::Scalar w)
0247     -> Eigen::Matrix<typename vector3_t::Scalar, 4, 1> {
0248   EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(vector3_t, 3);
0249
0250   Eigen::Matrix<typename vector3_t::Scalar, 4, 1> vec4;
0251   vec4[ePos0] = vec3[ePos0];
0252   vec4[ePos1] = vec3[ePos1];
0253   vec4[ePos2] = vec3[ePos2];
0254   vec4[eTime] = w;
0255   return vec4;
0256 }
0257
0258 /// Calculate the incident angles of a vector with in a given reference frame
0259 /// @tparam Derived Eigen derived concrete type
0260 /// @param direction The crossing direction in the global frame
0261 /// @param globalToLocal Rotation from global to local frame
0262 /// @return The angles of incidence in the two normal planes
0263 inline std::pair<double, double> incidentAngles(
0264     const Acts::Vector3& direction,
0265     const Acts::RotationMatrix3& globalToLocal) {
0266   Acts::Vector3 trfDir = globalToLocal * direction;
0267   // The angles are defined with respect to the measurement axis
0268   // i.e. "head-on" == pi/2, parallel = 0
0269   double phi = std::atan2(trfDir[2], trfDir[0]);
0270   double theta = std::atan2(trfDir[2], trfDir[1]);
0271   return {phi, theta};
0272 }
0273
0274 /// Calculate the deltaR between two vectors.
0275 /// @note DeltaR is defined as sqrt(deltaPhi^2 + deltaEta^2)
0276 /// @tparam Derived Eigen derived concrete type
0277 /// @param v1 First vector
0278 /// @param v2 Second vector
0279 /// @return The deltaR value
0280 template <typename Derived>
0281 double deltaR(const Eigen::MatrixBase<Derived>& v1,
0282               const Eigen::MatrixBase<Derived>& v2)
0283   requires(Eigen::MatrixBase<Derived>::RowsAtCompileTime == 3)
0284 {
0285   const double dphi =
0286       detail::difference_periodic(phi(v1), phi(v2), 2 * std::numbers::pi);
0287   const double deta = eta(v1) - eta(v2);
0288   return fastHypot(dphi, deta);
0289 }
0290
0291 }  // namespace Acts::VectorHelpers