UncertainHelix.h

/**************************************************************************
 * BASF2 (Belle Analysis Framework 2)                                     *
 * Copyright(C) 2014 - Belle II Collaboration                             *
 *                                                                        *
 * Author: The Belle II Collaboration                                     *
 * Contributors: Oliver Frost                                             *
 *                                                                        *
 * This software is provided "as is" without any warranty.                *
 **************************************************************************/
#pragma once
 
#include <tracking/trackFindingCDC/geometry/Helix.h>
 
#include <tracking/trackFindingCDC/geometry/UncertainPerigeeCircle.h>
#include <tracking/trackFindingCDC/geometry/UncertainSZLine.h>
#include <tracking/trackFindingCDC/geometry/SZLine.h>
 
#include <tracking/trackFindingCDC/geometry/HelixParameters.h>
#include <tracking/trackFindingCDC/geometry/PerigeeParameters.h>
#include <tracking/trackFindingCDC/geometry/SZParameters.h>
#include <tracking/trackFindingCDC/geometry/Vector2D.h>
 
#include <cstddef>
#include <iosfwd>
 
namespace Belle2 {
  namespace TrackFindingCDC {
    class Vector3D;
 
    class UncertainHelix {
 
    public:
      UncertainHelix()
      {
      }
 
      UncertainHelix(const double curvature,
                     const double phi0,
                     const double impact,
                     const double tanLambda,
                     const double z0,
                     const HelixCovariance& helixCovariance = HelixUtil::identity(),
                     const double chi2 = 0.0,
                     std::size_t ndf = 0)
        : m_helix(curvature, phi0, impact, tanLambda, z0)
        , m_helixCovariance(helixCovariance)
        , m_chi2(chi2)
        , m_ndf(ndf)
      {
      }
 
      explicit UncertainHelix(const HelixParameters& parameters,
                              const HelixCovariance& helixCovariance = HelixUtil::identity(),
                              const double chi2 = 0.0,
                              std::size_t ndf = 0)
        : m_helix(parameters)
        , m_helixCovariance(helixCovariance)
        , m_chi2(chi2)
        , m_ndf(ndf)
      {
      }
 
      UncertainHelix(const double curvature,
                     const Vector2D& phi0Vec,
                     const double impact,
                     const double tanLambda,
                     const double z0,
                     const HelixCovariance& helixCovariance = HelixUtil::identity(),
                     const double chi2 = 0.0,
                     std::size_t ndf = 0)
        : m_helix(curvature, phi0Vec, impact, tanLambda, z0)
        , m_helixCovariance(helixCovariance)
        , m_chi2(chi2)
        , m_ndf(ndf)
      {
      }
 
      explicit UncertainHelix(const Helix& helix,
                              const HelixCovariance& helixCovariance = HelixUtil::identity(),
                              const double chi2 = 0.0,
                              std::size_t ndf = 0)
        : m_helix(helix)
        , m_helixCovariance(helixCovariance)
        , m_chi2(chi2)
        , m_ndf(ndf)
      {
      }
 
      UncertainHelix(const UncertainPerigeeCircle& uncertainPerigeeCircle,
                     const UncertainSZLine& uncertainSZLine)
        : m_helix(uncertainPerigeeCircle, uncertainSZLine)
        , m_helixCovariance(HelixUtil::stackBlocks(uncertainPerigeeCircle.perigeeCovariance(),
                                                   uncertainSZLine.szCovariance()))
        , m_chi2(uncertainPerigeeCircle.chi2() + uncertainSZLine.chi2())
        , m_ndf(uncertainPerigeeCircle.ndf() + uncertainSZLine.ndf())
      {
      }
 
      static UncertainHelix average(const UncertainHelix& fromHelix, const UncertainHelix& toHelix);
 
      static UncertainHelix average(const UncertainPerigeeCircle& fromPerigeeCircle,
                                    const PerigeeHelixAmbiguity& fromAmbiguity,
                                    const UncertainPerigeeCircle& toPerigeeCircle,
                                    const PerigeeHelixAmbiguity& toAmbiguity,
                                    const SZParameters& szParameters);
 
      static UncertainHelix average(const UncertainHelix& helix,
                                    const UncertainPerigeeCircle& perigeeCircle,
                                    const PerigeeHelixAmbiguity& ambiguityMatrix)
      {
        return average(perigeeCircle, ambiguityMatrix, helix);
      }
 
      static UncertainHelix average(const UncertainPerigeeCircle& fromPerigeeCircle,
                                    const PerigeeHelixAmbiguity& fromAmbiguity,
                                    const UncertainHelix& toHelix);
 
    public:
      const Helix* operator->() const
      {
        return &m_helix;
      }
 
      operator const Helix& () const
      {
        return m_helix;
      }
 
      const Helix& helix() const
      {
        return m_helix;
      }
 
      HelixParameters helixParameters() const
      {
        return m_helix.helixParameters();
      }
 
    public:
      UncertainPerigeeCircle uncertainCircleXY() const
      {
        return UncertainPerigeeCircle(helix().circleXY(),
                                      HelixUtil::getPerigeeCovariance(helixCovariance()));
      }
 
      UncertainSZLine uncertainSZLine() const
      {
        return UncertainSZLine(helix().szLine(), HelixUtil::getSZCovariance(helixCovariance()));
      }
 
      void setHelixCovariance(const HelixCovariance& helixCovariance)
      {
        m_helixCovariance = helixCovariance;
      }
 
      const HelixCovariance& helixCovariance() const
      {
        return m_helixCovariance;
      }
 
      double covariance(const EHelixParameter& iRow, const EHelixParameter& iCol) const
      {
        return helixCovariance()(iRow, iCol);
      }
 
      double variance(const EHelixParameter& i) const
      {
        return helixCovariance()(i, i);
      }
 
      double chi2() const
      {
        return m_chi2;
      }
 
      void setChi2(const double chi2)
      {
        m_chi2 = chi2;
      }
 
      std::size_t ndf() const
      {
        return m_ndf;
      }
 
      void setNDF(std::size_t ndf)
      {
        m_ndf = ndf;
      }
 
      void invalidate()
      {
        m_helix.invalidate();
        m_helixCovariance = HelixUtil::identity();
        m_chi2 = 0.0;
        m_ndf = 0;
      }
 
    public:
      void reverse()
      {
        m_helix.reverse();
        HelixUtil::reverse(m_helixCovariance);
      }
 
      UncertainHelix reversed() const
      {
        return UncertainHelix(m_helix.reversed(),
                              HelixUtil::reversed(m_helixCovariance),
                              chi2(),
                              ndf());
      }
 
    public:
      void passiveMoveBy(const Vector3D& by)
      {
        // Move the covariance matrix first to have access to the original parameters
        HelixJacobian jacobian = m_helix.passiveMoveByJacobian(by);
        HelixUtil::transport(jacobian, m_helixCovariance);
        m_helix.passiveMoveBy(by);
      }
 
      double shiftPeriod(int nPeriods)
      {
        double arcLength2D = m_helix.shiftPeriod(nPeriods);
        SZJacobian szJacobian = m_helix.szLine().passiveMoveByJacobian(Vector2D(arcLength2D, 0));
        PerigeeJacobian perigeeJacobian = PerigeeUtil::identity();
        HelixJacobian jacobian = HelixUtil::stackBlocks(perigeeJacobian, szJacobian);
        HelixUtil::transport(jacobian, m_helixCovariance);
        return arcLength2D;
      }
 
    private:
      Helix m_helix;
 
      HelixCovariance m_helixCovariance = HelixUtil::identity();
 
      double m_chi2 = 0.0;
 
      size_t m_ndf = 0.0;
 
    };
 
    std::ostream& operator<<(std::ostream& output, const UncertainHelix& uncertainHelix);
  }
}