KfTrackParam.h

Go to the documentation of this file.

/* Copyright (C) 2007-2023 GSI Helmholtzzentrum fuer Schwerionenforschung, Darmstadt
   SPDX-License-Identifier: GPL-3.0-only
   Authors: Igor Kulakov [committer], Maksym Zyzak, Sergei Zharko */
 

 
#ifndef KF_CORE_KfTrackParam_h
#define KF_CORE_KfTrackParam_h 1
 
#include "KfDefs.h"
#include "KfMath.h"
#include "KfSimd.h"
#include "KfUtils.h"
 
#include <boost/serialization/access.hpp>
 
#include <string>
 
#if !defined(NO_ROOT) && !XPU_IS_HIP_CUDA
#include <Rtypes.h>  // for ClassDef
#endif
 
namespace cbm::algo::kf
{
  template<typename T>
  class alignas(VcMemAlign) TrackParamBase {
 
   public:
    friend class boost::serialization::access;
 
    static constexpr int kNtrackParam{7};  
    static constexpr int kNcovParam{(kNtrackParam) * (kNtrackParam + 1) / 2};  
 
    typedef std::array<T, kNcovParam> CovMatrix_t;  
 
    TrackParamBase() = default;
 
    template<typename T1>
    TrackParamBase(const TrackParamBase<T1>& tr)
    {
      Set(tr);
    }

    template<typename T1>
    void Set(const TrackParamBase<T1>& Tb)
    {
      CopyBase<T1, true, true>(0, Tb, 0);
    }

    void Set(const TrackParamBase<fvec>& Tb, const int ib) { CopyBase<fvec, true, false>(0, Tb, ib); }

    template<typename T1>
    void SetOneEntry(const int ia, const TrackParamBase<T1>& Tb)
    {
      CopyBase<T1, false, true>(ia, Tb, 0);
    }

    void SetOneEntry(const int ia, const TrackParamBase<fvec>& Tb, const int ib)
    {
      CopyBase<fvec, false, false>(ia, Tb, ib);
    }


    T Z() const { return fZ; }

    T X() const { return fX; }

    T Y() const { return fY; }

    T Tx() const { return fTx; }

    T Ty() const { return fTy; }

    T Qp() const { return fQp; }

    T Time() const { return fT; }

    T Vi() const { return fVi; }

    T ChiSq() const { return fChiSq; }

    T Ndf() const { return fNdf; }

    T ChiSqTime() const { return fChiSqTime; }

    T NdfTime() const { return fNdfTime; }

    T C(int i, int j) const
    {
      int ind = (j <= i) ? i * (1 + i) / 2 + j : j * (1 + j) / 2 + i;
      return fCovMatrix[ind];
    }

    template<int i, int j>
    T C() const
    {
      constexpr int ind = (j <= i) ? i * (1 + i) / 2 + j : j * (1 + j) / 2 + i;
      return fCovMatrix[ind];
    }

    T C00() const { return C<0, 0>(); }
    T C01() const { return C<0, 1>(); }
    T C02() const { return C<0, 2>(); }
    T C03() const { return C<0, 3>(); }
    T C04() const { return C<0, 4>(); }
    T C05() const { return C<0, 5>(); }
    T C06() const { return C<0, 6>(); }
 
    T C10() const { return C<1, 0>(); }
    T C11() const { return C<1, 1>(); }
    T C12() const { return C<1, 2>(); }
    T C13() const { return C<1, 3>(); }
    T C14() const { return C<1, 4>(); }
    T C15() const { return C<1, 5>(); }
    T C16() const { return C<1, 6>(); }
 
    T C20() const { return C<2, 0>(); }
    T C21() const { return C<2, 1>(); }
    T C22() const { return C<2, 2>(); }
    T C23() const { return C<2, 3>(); }
    T C24() const { return C<2, 4>(); }
    T C25() const { return C<2, 5>(); }
    T C26() const { return C<2, 6>(); }
 
    T C30() const { return C<3, 0>(); }
    T C31() const { return C<3, 1>(); }
    T C32() const { return C<3, 2>(); }
    T C33() const { return C<3, 3>(); }
    T C34() const { return C<3, 4>(); }
    T C35() const { return C<3, 5>(); }
    T C36() const { return C<3, 6>(); }
 
    T C40() const { return C<4, 0>(); }
    T C41() const { return C<4, 1>(); }
    T C42() const { return C<4, 2>(); }
    T C43() const { return C<4, 3>(); }
    T C44() const { return C<4, 4>(); }
    T C45() const { return C<4, 5>(); }
    T C46() const { return C<4, 6>(); }
 
    T C50() const { return C<5, 0>(); }
    T C51() const { return C<5, 1>(); }
    T C52() const { return C<5, 2>(); }
    T C53() const { return C<5, 3>(); }
    T C54() const { return C<5, 4>(); }
    T C55() const { return C<5, 5>(); }
    T C56() const { return C<5, 6>(); }
 
    T C60() const { return C<6, 0>(); }
    T C61() const { return C<6, 1>(); }
    T C62() const { return C<6, 2>(); }
    T C63() const { return C<6, 3>(); }
    T C64() const { return C<6, 4>(); }
    T C65() const { return C<6, 5>(); }
    T C66() const { return C<6, 6>(); }


    T GetZ() const { return fZ; }

    T GetX() const { return fX; }

    T GetXError() const { return sqrt(C00()); }

    T GetY() const { return fY; }

    T GetYError() const { return sqrt(C11()); }

    T GetTx() const { return fTx; }

    T GetTxError() const { return sqrt(C22()); }

    T GetTy() const { return fTy; }

    T GetTyError() const { return sqrt(C33()); }

    T GetQp() const { return fQp; }

    T GetQpError() const { return sqrt(C44()); }

    T GetTime() const { return fT; }

    T GetTimeError() const { return sqrt(C55()); }

    T GetVi() const { return fVi; }

    T GetViError() const { return sqrt(C66()); }

    const CovMatrix_t& GetCovMatrix() const { return fCovMatrix; }

    T GetCovariance(int i, int j) const { return C(i, j); }

    T GetChiSq() const { return fChiSq; }

    T GetNdf() const { return fNdf; }

    T GetChiSqTime() const { return fChiSqTime; }

    T GetNdfTime() const { return fNdfTime; }

    T GetCharge() const { return utils::iif(GetQp() > T(0.), T(1.), T(-1.)); }

    T GetPhi() const { return atan2(GetTy(), GetTx()); }

    T GetPhiError() const;

    T GetTheta() const { return atan(sqrt(GetTx() * GetTx() + GetTy() * GetTy())); }

    T GetThetaError() const;

    T GetP() const { return utils::iif(utils::fabs(GetQp()) > T(1.e-4), T(1.) / utils::fabs(GetQp()), T(1.e4)); }

    T GetPz() const { return GetP() / sqrt(T(1.) + GetTx() * GetTx() + GetTy() * GetTy()); }

    T GetPx() const { return GetPz() * GetTx(); }

    T GetPy() const { return GetPz() * GetTy(); }

    T GetPt() const
    {
      T t2 = GetTx() * GetTx() + GetTy() * GetTy();
      return GetP() * sqrt(t2 / (T(1.) + t2));
    }


    void SetZ(T v) { fZ = v; }

    void SetX(T v) { fX = v; }

    void SetY(T v) { fY = v; }

    void SetTx(T v) { fTx = v; }

    void SetTy(T v) { fTy = v; }

    void SetQp(T v) { fQp = v; }

    void SetTime(T v) { fT = v; }

    void SetVi(T v) { fVi = v; }

    void SetChiSq(T v) { fChiSq = v; }

    void SetNdf(T v) { fNdf = v; }

    void SetChiSqTime(T v) { fChiSqTime = v; }

    void SetNdfTime(T v) { fNdfTime = v; }

    void SetCovMatrix(const CovMatrix_t& val) { fCovMatrix = val; }

    void SetCovariance(int i, int j, T val)
    {
      int ind         = (j <= i) ? i * (1 + i) / 2 + j : j * (1 + j) / 2 + i;
      fCovMatrix[ind] = val;
    }

    template<int i, int j>
    void SetCovariance(T val)
    {
      constexpr int ind = (j <= i) ? i * (1 + i) / 2 + j : j * (1 + j) / 2 + i;
      fCovMatrix[ind]   = val;
    }

    void SetC00(T val) { SetCovariance<0, 0>(val); }
    void SetC10(T val) { SetCovariance<1, 0>(val); }
    void SetC11(T val) { SetCovariance<1, 1>(val); }
    void SetC20(T val) { SetCovariance<2, 0>(val); }
    void SetC21(T val) { SetCovariance<2, 1>(val); }
    void SetC22(T val) { SetCovariance<2, 2>(val); }
    void SetC30(T val) { SetCovariance<3, 0>(val); }
    void SetC31(T val) { SetCovariance<3, 1>(val); }
    void SetC32(T val) { SetCovariance<3, 2>(val); }
    void SetC33(T val) { SetCovariance<3, 3>(val); }
    void SetC40(T val) { SetCovariance<4, 0>(val); }
    void SetC41(T val) { SetCovariance<4, 1>(val); }
    void SetC42(T val) { SetCovariance<4, 2>(val); }
    void SetC43(T val) { SetCovariance<4, 3>(val); }
    void SetC44(T val) { SetCovariance<4, 4>(val); }
    void SetC50(T val) { SetCovariance<5, 0>(val); }
    void SetC51(T val) { SetCovariance<5, 1>(val); }
    void SetC52(T val) { SetCovariance<5, 2>(val); }
    void SetC53(T val) { SetCovariance<5, 3>(val); }
    void SetC54(T val) { SetCovariance<5, 4>(val); }
    void SetC55(T val) { SetCovariance<5, 5>(val); }
    void SetC60(T val) { SetCovariance<6, 0>(val); }
    void SetC61(T val) { SetCovariance<6, 1>(val); }
    void SetC62(T val) { SetCovariance<6, 2>(val); }
    void SetC63(T val) { SetCovariance<6, 3>(val); }
    void SetC64(T val) { SetCovariance<6, 4>(val); }
    void SetC65(T val) { SetCovariance<6, 5>(val); }
    void SetC66(T val) { SetCovariance<6, 6>(val); }


    T& Z() { return fZ; }

    T& X() { return fX; }

    T& Y() { return fY; }

    T& Tx() { return fTx; }

    T& Ty() { return fTy; }

    T& Qp() { return fQp; }

    T& Time() { return fT; }

    T& Vi() { return fVi; }

    CovMatrix_t& CovMatrix() { return fCovMatrix; }

    T& ChiSq() { return fChiSq; }

    T& Ndf() { return fNdf; }

    T& ChiSqTime() { return fChiSqTime; }

    T& NdfTime() { return fNdfTime; }

    T& C(int i, int j) { return fCovMatrix[math::SymIndex(i, j)]; }

    template<int i, int j>
    T& C()
    {
      return fCovMatrix[math::SymIndex(i, j)];
    }

    T& C00() { return C<0, 0>(); }
    T& C01() { return C<0, 1>(); }
    T& C02() { return C<0, 2>(); }
    T& C03() { return C<0, 3>(); }
    T& C04() { return C<0, 4>(); }
    T& C05() { return C<0, 5>(); }
    T& C06() { return C<0, 6>(); }
 
    T& C10() { return C<1, 0>(); }
    T& C11() { return C<1, 1>(); }
    T& C12() { return C<1, 2>(); }
    T& C13() { return C<1, 3>(); }
    T& C14() { return C<1, 4>(); }
    T& C15() { return C<1, 5>(); }
    T& C16() { return C<1, 6>(); }
 
    T& C20() { return C<2, 0>(); }
    T& C21() { return C<2, 1>(); }
    T& C22() { return C<2, 2>(); }
    T& C23() { return C<2, 3>(); }
    T& C24() { return C<2, 4>(); }
    T& C25() { return C<2, 5>(); }
    T& C26() { return C<2, 6>(); }
 
    T& C30() { return C<3, 0>(); }
    T& C31() { return C<3, 1>(); }
    T& C32() { return C<3, 2>(); }
    T& C33() { return C<3, 3>(); }
    T& C34() { return C<3, 4>(); }
    T& C35() { return C<3, 5>(); }
    T& C36() { return C<3, 6>(); }
 
    T& C40() { return C<4, 0>(); }
    T& C41() { return C<4, 1>(); }
    T& C42() { return C<4, 2>(); }
    T& C43() { return C<4, 3>(); }
    T& C44() { return C<4, 4>(); }
    T& C45() { return C<4, 5>(); }
    T& C46() { return C<4, 6>(); }
 
    T& C50() { return C<5, 0>(); }
    T& C51() { return C<5, 1>(); }
    T& C52() { return C<5, 2>(); }
    T& C53() { return C<5, 3>(); }
    T& C54() { return C<5, 4>(); }
    T& C55() { return C<5, 5>(); }
    T& C56() { return C<5, 6>(); }
 
    T& C60() { return C<6, 0>(); }
    T& C61() { return C<6, 1>(); }
    T& C62() { return C<6, 2>(); }
    T& C63() { return C<6, 3>(); }
    T& C64() { return C<6, 4>(); }
    T& C65() { return C<6, 5>(); }
    T& C66() { return C<6, 6>(); }
 


    void ResetErrors(T c00, T c11, T c22, T c33, T c44, T c55, T c66);

    std::string ToString(int i = -1) const;

    std::string ToStringCorrelations(int i = -1) const;

    bool IsFinite(bool printWhenWrong) const;

    bool IsEntryConsistent(bool printWhenWrong, int i) const;

    bool IsConsistent(bool printWhenWrong, int nFilled) const;

    void InitVelocityRange(fscal minP);
 

    template<class Archive>
    void serialize(Archive& ar, const unsigned int /*version*/)
    {
      ar& fX;
      ar& fY;
      ar& fTx;
      ar& fTy;
      ar& fQp;
      ar& fZ;
      ar& fT;
      ar& fVi;
      ar& fCovMatrix;
      ar& fChiSq;
      ar& fNdf;
      ar& fChiSqTime;
      ar& fNdfTime;
    }
 
   private:
    template<typename T1, bool TDoAllA, bool TDoAllB>
    void CopyBase(const int ia, const TrackParamBase<T1>& Tb, const int ib);
 
 
   private:
 
    // Initializing parameters with NANs spoils the track fit where
    // the masked-out SIMD entries are suppressed by a multication by 0.
    // Therefore, we initialize the data members here with finite numbers.
    // For the numerical safety, with some reasonable numbers.
 
    // clang-format off
    CovMatrix_t fCovMatrix {1.,
                            0., 1.,
                            0., 0., 1.,
                            0., 0., 0., 1.,
                            0., 0., 0., 0., 1.,
                            0., 0., 0., 0., 0., 1.,
                            0., 0., 0., 0., 0., 0., 1.};  
    // clang-format on
 
    T fX{0.};   
    T fY{0.};   
    T fZ{0.};   
    T fTx{0.};  
    T fTy{0.};  
    T fQp{0.};  
    T fT{0.};   
    T fVi{0.};  
 
    T fChiSq{0.};      
    T fNdf{0.};        
    T fChiSqTime{0.};  
    T fNdfTime{0.};    
 
#if !defined(NO_ROOT) && !XPU_IS_HIP_CUDA
    ClassDefNV(TrackParamBase, 1);
#endif
 
  };  // class TrackParamBase
 

  template<typename T>
  class TrackParamBaseScalar : public TrackParamBase<T> {
   public:
    using TrackParamBase<T>::TrackParamBase;

    T GetEta() const { return -log(tan(this->GetTheta() * T(0.5))); }
 
#if !defined(NO_ROOT) && !XPU_IS_HIP_CUDA
    ClassDefNV(TrackParamBaseScalar, 1);
#endif
  };
 
 
  // ---------------------------------------------------------------------------------------------------------------------

  template<typename T>
  class TrackParam : public TrackParamBaseScalar<T> {
   public:
    using TrackParamBaseScalar<T>::TrackParamBaseScalar;
 
#if !defined(NO_ROOT) && !XPU_IS_HIP_CUDA
    ClassDefNV(TrackParam, 1);
#endif
  };
 
  template<>
  class TrackParam<fvec> : public TrackParamBase<fvec> {
   public:
    using TrackParamBase<fvec>::TrackParamBase;
 
#if !defined(NO_ROOT) && !XPU_IS_HIP_CUDA
    ClassDefNV(TrackParam<fvec>, 1);
#endif
  };
 
 
  typedef TrackParam<fvec> TrackParamV;
  typedef TrackParam<fscal> TrackParamS;
  typedef TrackParam<double> TrackParamD;
 

 

  template<typename T>
  inline T TrackParamBase<T>::GetPhiError() const
  {
    // phi = atan( tx / ty ); }
 
    T phiDdenom = GetTx() * GetTx() + GetTy() * GetTy();
    T phiDTx    = -GetTy() / phiDdenom;  // partial derivative of phi over Tx
    T phiDTy    = +GetTx() / phiDdenom;  // partial derivative of phi over Ty
 
    T varTx   = C22();  // variance of Tx
    T varTy   = C33();  // variance of Ty
    T covTxTy = C32();  // covariance of Tx and Ty
 
    T varPhi = phiDTx * phiDTx * varTx + phiDTy * phiDTy * varTy + T(2.) * phiDTx * phiDTy * covTxTy;
    return sqrt(varPhi);
  }
 

  template<typename T>
  inline T TrackParamBase<T>::GetThetaError() const
  {
    // theta = atan(sqrt( tx * tx + ty * ty) )
 
    T sumSqSlopes = GetTx() * GetTx() + GetTy() * GetTy();
    T thetaDdenom = sqrt(sumSqSlopes) * (T(1.) + sumSqSlopes);
    T thetaDTx    = GetTx() / thetaDdenom;
    T thetaDTy    = GetTy() / thetaDdenom;
 
    T varTx   = C22();  // variance of Tx
    T varTy   = C33();  // variance of Ty
    T covTxTy = C32();  // covariance of Tx and Ty
 
    T varTheta = thetaDTx * thetaDTx * varTx + thetaDTy * thetaDTy * varTy + T(2.) * thetaDTx * thetaDTy * covTxTy;
 
    return sqrt(varTheta);
  }
 
 
  // ---------------------------------------------------------------------------------------------------------------------
  //
  template<typename TdataA>
  template<typename TdataB, bool TDoAllA, bool TDoAllB>
  inline void TrackParamBase<TdataA>::CopyBase(const int ia, const TrackParamBase<TdataB>& Tb, const int ib)
  {
    auto copy = [&](TdataA& a, const TdataB& b) {
      utils::VecCopy<TdataA, TdataB, TDoAllA, TDoAllB>::CopyEntries(a, ia, b, ib);
    };
 
    copy(fX, Tb.GetX());
    copy(fY, Tb.GetY());
    copy(fTx, Tb.GetTx());
    copy(fTy, Tb.GetTy());
    copy(fQp, Tb.GetQp());
    copy(fZ, Tb.GetZ());
    copy(fT, Tb.GetTime());
    copy(fVi, Tb.GetVi());
 
    for (int i = 0; i < kNcovParam; ++i) {
      copy(fCovMatrix[i], Tb.GetCovMatrix()[i]);
    }
 
    copy(fChiSq, Tb.GetChiSq());
    copy(fNdf, Tb.GetNdf());
    copy(fChiSqTime, Tb.GetChiSqTime());
    copy(fNdfTime, Tb.GetNdfTime());
 
  }  // CopyBase
 
 
  // ---------------------------------------------------------------------------------------------------------------------
  //
  template<typename T>
  inline void TrackParamBase<T>::ResetErrors(T c00, T c11, T c22, T c33, T c44, T c55, T c66)
  {
    fCovMatrix.fill(0.);
 
    SetC00(c00);
    SetC11(c11);
    SetC22(c22);
    SetC33(c33);
    SetC44(c44);
    SetC55(c55);
    SetC66(c66);
 
    SetChiSq(0.);
    SetNdf(-5.);
    SetChiSqTime(0.);
    SetNdfTime(-2.);
  }
 
  // ---------------------------------------------------------------------------------------------------------------------
  //
  template<typename T>
  inline void TrackParamBase<T>::InitVelocityRange(fscal minP)
  {
    // initialise the velocity range with respect to the minimal momentum minP {GeV/c}
    using defs::ProtonMass;
    using defs::SpeedOfLightInv;
    fscal maxVi = sqrt(1. + (ProtonMass<double> / minP) * (ProtonMass<double> / minP)) * SpeedOfLightInv<double>;
    fscal minVi = SpeedOfLightInv<fscal>;
    fscal vmean = minVi + 0.4 * (maxVi - minVi);
    fscal dvi   = (maxVi - vmean) / 3.;
    SetVi(vmean);
    SetC66(dvi * dvi);
  }
 
}  // namespace cbm::algo::kf
 
#endif  // KF_CORE_KfTrackParam_h