packages/intt/InttVertexv1.cc

0001 #include "InttVertexv1.h"
0002
0003 #include <cmath>
0004
0005 InttVertexv1::InttVertexv1()
0006   : _id(0xFFFFFFFF)
0007 {
0008   std::fill(std::begin(_pos), std::end(_pos), std::numeric_limits<float>::quiet_NaN());
0009   std::fill(std::begin(_err), std::end(_err), std::numeric_limits<float>::quiet_NaN());
0010 }
0011
0012 void InttVertexv1::identify(std::ostream& os) const
0013 {
0014   os << "---InttVertexv1--------------------------------" << std::endl;
0015   os << "vertexid: " << get_id() << std::endl;
0016
0017   os << " (x,y,z) =  (" << get_position(0);
0018   os << ", " << get_position(1) << ", ";
0019   os << get_position(2) << ") cm" << std::endl;
0020
0021   os << "         ( ";
0022   os << get_error(0, 0) << " , ";
0023   os << get_error(0, 1) << " , ";
0024   os << get_error(0, 2) << " )" << std::endl;
0025   os << "  err  = ( ";
0026   os << get_error(1, 0) << " , ";
0027   os << get_error(1, 1) << " , ";
0028   os << get_error(1, 2) << " )" << std::endl;
0029   os << "         ( ";
0030   os << get_error(2, 0) << " , ";
0031   os << get_error(2, 1) << " , ";
0032   os << get_error(2, 2) << " )" << std::endl;
0033   os << "-----------------------------------------------" << std::endl;
0034
0035   return;
0036 }
0037
0038 int InttVertexv1::isValid() const
0039 {
0040   if (_id == std::numeric_limits<unsigned int>::max())
0041   {
0042     return 0;
0043   }
0044   for (float _po : _pos)
0045   {
0046     if (!std::isfinite(_po))
0047     {
0048       return 0;
0049     }
0050   }
0051   for (int j = 0; j < 3; ++j)
0052   {
0053     for (int i = j; i < 3; ++i)
0054     {
0055       if (!std::isfinite(get_error(i, j)))
0056       {
0057         return 0;
0058       }
0059     }
0060   }
0061   return 1;
0062 }
0063
0064 void InttVertexv1::set_error(unsigned int i, unsigned int j, float value)
0065 {
0066   _err[covar_index(i, j)] = value;
0067   return;
0068 }
0069
0070 float InttVertexv1::get_error(unsigned int i, unsigned int j) const
0071 {
0072   return _err[covar_index(i, j)];
0073 }
0074
0075 unsigned int InttVertexv1::covar_index(unsigned int i, unsigned int j) const
0076 {
0077   if (i > j)
0078   {
0079     std::swap(i, j);
0080   }
0081   return i + 1 + (j + 1) * (j) / 2 - 1;
0082 }