UncertainHelix.h

/**************************************************************************
 * basf2 (Belle II Analysis Software Framework)                           *
 * Author: The Belle II Collaboration                                     *
 *                                                                        *
 * See git log for contributors and copyright holders.                    *
 * This file is licensed under LGPL-3.0, see LICENSE.md.                  *
 **************************************************************************/
#pragma once
 
#include <tracking/trackingUtilities/geometry/Helix.h>
 
#include <tracking/trackingUtilities/geometry/UncertainPerigeeCircle.h>
#include <tracking/trackingUtilities/geometry/UncertainSZLine.h>
#include <tracking/trackingUtilities/geometry/SZLine.h>
 
#include <tracking/trackingUtilities/geometry/HelixParameters.h>
#include <tracking/trackingUtilities/geometry/PerigeeParameters.h>
#include <tracking/trackingUtilities/geometry/SZParameters.h>
 
#include <Math/Vector3D.h>
#include <Math/Vector2D.h>
 
#include <cstddef>
#include <iosfwd>
 
namespace Belle2 {
 
  namespace TrackingUtilities {

    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 ROOT::Math::XYVector& 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 ROOT::Math::XYZVector& 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(ROOT::Math::XYVector(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);
  }
}