packages/trackbase_historic/SvtxTrackState_v2.cc

0001 #include "SvtxTrackState_v2.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_v2::SvtxTrackState_v2(float pathlength)
0031   : _pathlength(pathlength)
0032 {
0033   for (float& _po : _pos)
0034   {
0035     _po = 0.0;
0036   }
0037   for (float& i : _mom)
0038   {
0039     i = NAN;
0040   }
0041   for (int i = 0; i < 6; ++i)
0042   {
0043     for (int j = i; j < 6; ++j)
0044     {
0045       set_error(i, j, 0.0);
0046     }
0047   }
0048   state_name = "UNKNOWN";
0049 }
0050
0051 void SvtxTrackState_v2::identify(std::ostream& os) const
0052 {
0053   os << "---SvtxTrackState_v2-------------" << endl;
0054   os << "pathlength: " << get_pathlength() << endl;
0055   os << "(px,py,pz) = ("
0056      << get_px() << ","
0057      << get_py() << ","
0058      << get_pz() << ")" << endl;
0059
0060   os << "(x,y,z) = (" << get_x() << "," << get_y() << "," << get_z() << ")" << endl;
0061   os << "---------------------------------" << endl;
0062 }
0063
0064 float SvtxTrackState_v2::get_error(unsigned int i, unsigned int j) const
0065 {
0066   return _covar[covar_index(i, j)];
0067 }
0068
0069 void SvtxTrackState_v2::set_error(unsigned int i, unsigned int j, float value)
0070 {
0071   _covar[covar_index(i, j)] = value;
0072   return;
0073 }
0074
0075 float SvtxTrackState_v2::get_phi_error() const
0076 {
0077   const float r = std::sqrt(square(_pos[0]) + square(_pos[1]));
0078   if (r > 0)
0079   {
0080     return get_rphi_error() / r;
0081   }
0082   return 0;
0083 }
0084
0085 float SvtxTrackState_v2::get_rphi_error() const
0086 {
0087   const auto phi = -std::atan2(_pos[1], _pos[0]);
0088   const auto cosphi = std::cos(phi);
0089   const auto sinphi = std::sin(phi);
0090   return std::sqrt(
0091       square(sinphi) * get_error(0, 0) +
0092       square(cosphi) * get_error(1, 1) +
0093       2. * cosphi * sinphi * get_error(0, 1));
0094 }
0095
0096 float SvtxTrackState_v2::get_z_error() const
0097 {
0098   return std::sqrt(get_error(2, 2));
0099 }