code/core/lst_math.h

0001 #ifndef lst_math_h
0002 #define lst_math_h
0003 #include <tuple>
0004 #include <vector>
0005 #include <cmath>
0006
0007 namespace lst_math {
0008   inline float Phi_mpi_pi(float x) {
0009     if (std::isnan(x)) {
0010       std::cout << "MathUtil::Phi_mpi_pi() function called with NaN" << std::endl;
0011       return x;
0012     }
0013
0014     while (x >= M_PI)
0015       x -= 2. * M_PI;
0016
0017     while (x < -M_PI)
0018       x += 2. * M_PI;
0019
0020     return x;
0021   };
0022
0023   inline float ATan2(float y, float x) {
0024     if (x != 0)
0025       return atan2(y, x);
0026     if (y == 0)
0027       return 0;
0028     if (y > 0)
0029       return M_PI / 2;
0030     else
0031       return -M_PI / 2;
0032   };
0033
0034   inline float ptEstimateFromRadius(float radius) { return 2.99792458e-3 * 3.8 * radius; };
0035
0036   class Helix {
0037   public:
0038     std::vector<float> center_;
0039     float radius_;
0040     float phi_;
0041     float lam_;
0042     float charge_;
0043
0044     Helix(std::vector<float> c, float r, float p, float l, float q) {
0045       center_ = c;
0046       radius_ = r;
0047       phi_ = Phi_mpi_pi(p);
0048       lam_ = l;
0049       charge_ = q;
0050     }
0051
0052     Helix(float pt, float eta, float phi, float vx, float vy, float vz, float charge) {
0053       // Radius based on pt
0054       radius_ = pt / (2.99792458e-3 * 3.8);
0055       phi_ = phi;
0056       charge_ = charge;
0057
0058       // reference point vector which for sim track is the vertex point
0059       float ref_vec_x = vx;
0060       float ref_vec_y = vy;
0061       float ref_vec_z = vz;
0062
0063       // The reference to center vector
0064       float inward_radial_vec_x = charge_ * radius_ * sin(phi_);
0065       float inward_radial_vec_y = charge_ * radius_ * -cos(phi_);
0066       float inward_radial_vec_z = 0;
0067
0068       // Center point
0069       float center_vec_x = ref_vec_x + inward_radial_vec_x;
0070       float center_vec_y = ref_vec_y + inward_radial_vec_y;
0071       float center_vec_z = ref_vec_z + inward_radial_vec_z;
0072       center_.push_back(center_vec_x);
0073       center_.push_back(center_vec_y);
0074       center_.push_back(center_vec_z);
0075
0076       // Lambda
0077       lam_ = copysign(M_PI / 2. - 2. * atan(exp(-abs(eta))), eta);
0078     }
0079
0080     const std::vector<float> center() { return center_; }
0081     const float radius() { return radius_; }
0082     const float phi() { return phi_; }
0083     const float lam() { return lam_; }
0084     const float charge() { return charge_; }
0085
0086     std::tuple<float, float, float, float> get_helix_point(float t) {
0087       float x = center()[0] - charge() * radius() * sin(phi() - (charge()) * t);
0088       float y = center()[1] + charge() * radius() * cos(phi() - (charge()) * t);
0089       float z = center()[2] + radius() * tan(lam()) * t;
0090       float r = sqrt(x * x + y * y);
0091       return std::make_tuple(x, y, z, r);
0092     }
0093
0094     float infer_t(const std::vector<float> point) {
0095       // Solve for t based on z position
0096       float t = (point[2] - center()[2]) / (radius() * tan(lam()));
0097       return t;
0098     }
0099
0100     float compare_radius(const std::vector<float> point) {
0101       float t = infer_t(point);
0102       auto [x, y, z, r] = get_helix_point(t);
0103       float point_r = sqrt(point[0] * point[0] + point[1] * point[1]);
0104       return (point_r - r);
0105     }
0106
0107     float compare_xy(const std::vector<float> point) {
0108       float t = infer_t(point);
0109       auto [x, y, z, r] = get_helix_point(t);
0110       float xy_dist = sqrt(pow(point[0] - x, 2) + pow(point[1] - y, 2));
0111       return xy_dist;
0112     }
0113   };
0114
0115   class Hit {
0116   public:
0117     float x_, y_, z_;
0118     // Derived quantities
0119     float r3_, rt_, phi_, eta_;
0120     int idx_;
0121
0122     // Default constructor
0123     Hit() : x_(0), y_(0), z_(0), idx_(-1) { setDerivedQuantities(); }
0124
0125     // Parameterized constructor
0126     Hit(float x, float y, float z, int idx = -1) : x_(x), y_(y), z_(z), idx_(idx) { setDerivedQuantities(); }
0127
0128     // Copy constructor
0129     Hit(const Hit& hit) : x_(hit.x_), y_(hit.y_), z_(hit.z_), idx_(hit.idx_) { setDerivedQuantities(); }
0130
0131     // Getters for derived quantities
0132     float phi() const { return phi_; }
0133     float eta() const { return eta_; }
0134     float rt() const { return rt_; }
0135     float x() const { return x_; }
0136     float y() const { return y_; }
0137
0138   private:
0139     void setDerivedQuantities() {
0140       // Setting r3
0141       r3_ = sqrt(x_ * x_ + y_ * y_ + z_ * z_);
0142
0143       // Setting rt
0144       rt_ = sqrt(x_ * x_ + y_ * y_);
0145
0146       // Setting phi
0147       phi_ = Phi_mpi_pi(M_PI + ATan2(-y_, -x_));
0148
0149       // Setting eta
0150       eta_ = ((z_ > 0) - (z_ < 0)) * std::acosh(r3_ / rt_);
0151     }
0152   };
0153
0154   inline Hit getCenterFromThreePoints(Hit& hitA, Hit& hitB, Hit& hitC) {
0155     //       C
0156     //
0157     //
0158     //
0159     //    B           d <-- find this point that makes the arc that goes throw a b c
0160     //
0161     //
0162     //     A
0163
0164     // Steps:
0165     // 1. Calculate mid-points of lines AB and BC
0166     // 2. Find slopes of line AB and BC
0167     // 3. construct a perpendicular line between AB and BC
0168     // 4. set the two equations equal to each other and solve to find intersection
0169
0170     float xA = hitA.x();
0171     float yA = hitA.y();
0172     float xB = hitB.x();
0173     float yB = hitB.y();
0174     float xC = hitC.x();
0175     float yC = hitC.y();
0176
0177     float x_mid_AB = (xA + xB) / 2.;
0178     float y_mid_AB = (yA + yB) / 2.;
0179
0180     //float x_mid_BC = (xB + xC) / 2.;
0181     //float y_mid_BC = (yB + yC) / 2.;
0182
0183     bool slope_AB_inf = (xB - xA) == 0;
0184     bool slope_BC_inf = (xC - xB) == 0;
0185
0186     bool slope_AB_zero = (yB - yA) == 0;
0187     bool slope_BC_zero = (yC - yB) == 0;
0188
0189     float slope_AB = slope_AB_inf ? 0 : (yB - yA) / (xB - xA);
0190     float slope_BC = slope_BC_inf ? 0 : (yC - yB) / (xC - xB);
0191
0192     float slope_perp_AB = slope_AB_inf or slope_AB_zero ? 0. : -1. / (slope_AB);
0193     //float slope_perp_BC = slope_BC_inf or slope_BC_zero ? 0. : -1. / (slope_BC);
0194
0195     if ((slope_AB - slope_BC) == 0) {
0196       std::cout << " slope_AB_zero: " << slope_AB_zero << std::endl;
0197       std::cout << " slope_BC_zero: " << slope_BC_zero << std::endl;
0198       std::cout << " slope_AB_inf: " << slope_AB_inf << std::endl;
0199       std::cout << " slope_BC_inf: " << slope_BC_inf << std::endl;
0200       std::cout << " slope_AB: " << slope_AB << std::endl;
0201       std::cout << " slope_BC: " << slope_BC << std::endl;
0202       std::cout << "MathUtil::getCenterFromThreePoints() function the three points are in straight line!" << std::endl;
0203       return Hit();
0204     }
0205
0206     float x =
0207         (slope_AB * slope_BC * (yA - yC) + slope_BC * (xA + xB) - slope_AB * (xB + xC)) / (2. * (slope_BC - slope_AB));
0208     float y = slope_perp_AB * (x - x_mid_AB) + y_mid_AB;
0209
0210     return Hit(x, y, 0);
0211   };
0212 }  // namespace lst_math
0213 #endif