packages/trackbase_historic/SvtxTrackState_v3.cc

0001 #include "SvtxTrackState_v3.h"
0002
0003 #include <iostream>
0004 #include <utility>  // for swap
0005
0006 using namespace std;
0007
0008 namespace
0009 {
0010
0011   // square convenience function
0012   template <class T>
0013   inline constexpr T square(const T& x)
0014   {
0015     return x * x;
0016   }
0017
0018   // get unique index in cov. matrix array from i and j
0019   inline unsigned int covar_index(unsigned int i, unsigned int j)
0020   {
0021     if (i > j)
0022     {
0023       std::swap(i, j);
0024     }
0025     return i + 1 + (j + 1) * (j) / 2 - 1;
0026   }
0027
0028 }  // namespace
0029
0030 SvtxTrackState_v3::SvtxTrackState_v3(float pathlength)
0031   : _pathlength(pathlength)
0032 {
0033   for (float& _locpo : _locpos)
0034    {
0035       _locpo = 0.0;
0036    }
0037   for (float& _po : _pos)
0038    {
0039       _po = 0.0;
0040    }
0041   for (float& i : _mom)
0042   {
0043     i = NAN;
0044   }
0045   for (int i = 0; i < 6; ++i)
0046   {
0047     for (int j = i; j < 6; ++j)
0048     {
0049       set_error(i, j, 0.0);
0050     }
0051   }
0052   state_name = "UNKNOWN";
0053 }
0054
0055 void SvtxTrackState_v3::identify(std::ostream& os) const
0056 {
0057   os << "---SvtxTrackState_v3-------------" << endl;
0058   os << "pathlength: " << get_pathlength() << endl;
0059   os << "(px,py,pz) = ("
0060      << get_px() << ","
0061      << get_py() << ","
0062      << get_pz() << ")" << endl;
0063
0064   os << "(locX,locY) = (" << get_localX() << "," << get_localY() << ")" << endl;
0065   os << "(x,y,z) = (" << get_x() << "," << get_y() << "," << get_z() << ")" << endl;
0066   os << "---------------------------------" << endl;
0067 }
0068
0069 float SvtxTrackState_v3::get_error(unsigned int i, unsigned int j) const
0070 {
0071   return _covar[covar_index(i, j)];
0072 }
0073
0074 void SvtxTrackState_v3::set_error(unsigned int i, unsigned int j, float value)
0075 {
0076   _covar[covar_index(i, j)] = value;
0077   return;
0078 }
0079
0080 float SvtxTrackState_v3::get_phi_error() const
0081 {
0082   const float r = std::sqrt(square(_pos[0]) + square(_pos[1]));
0083   if (r > 0)
0084   {
0085     return get_rphi_error() / r;
0086   }
0087   return 0;
0088 }
0089
0090 float SvtxTrackState_v3::get_rphi_error() const
0091 {
0092   const auto phi = -std::atan2(_pos[1], _pos[0]);
0093   const auto cosphi = std::cos(phi);
0094   const auto sinphi = std::sin(phi);
0095   return std::sqrt(
0096       square(sinphi) * get_error(0, 0) +
0097       square(cosphi) * get_error(1, 1) +
0098       2. * cosphi * sinphi * get_error(0, 1));
0099 }
0100
0101 float SvtxTrackState_v3::get_z_error() const
0102 {
0103   return std::sqrt(get_error(2, 2));
0104 }