PerigeeCircle.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/GeneralizedCircle.h>
#include <tracking/trackingUtilities/geometry/Line2D.h>
 
#include <tracking/trackingUtilities/geometry/PerigeeParameters.h>
 
#include <tracking/trackingUtilities/geometry/VectorUtil.h>
 
#include <tracking/trackingUtilities/numerics/EForwardBackward.h>
#include <tracking/trackingUtilities/numerics/ERightLeft.h>
#include <tracking/trackingUtilities/numerics/ERotation.h>
#include <tracking/trackingUtilities/numerics/ESign.h>
 
#include <Math/Vector2D.h>
 
#include <utility>
#include <cmath>
#include <iosfwd>
 
namespace Belle2 {
 
  namespace TrackingUtilities {
    class Circle2D;

    class PerigeeCircle {
 
    public:
      PerigeeCircle();

      PerigeeCircle(double curvature, const ROOT::Math::XYVector& phi0Vec, double impact);

      PerigeeCircle(double curvature, double phi0, double impact);

      explicit PerigeeCircle(const PerigeeParameters& perigeeParameters);
 
    private:
      PerigeeCircle(double curvature, double phi0, const ROOT::Math::XYVector& phi0Vec, double impact);
 
    public:
      explicit PerigeeCircle(const Line2D& n012);

      explicit PerigeeCircle(const GeneralizedCircle& n0123);

      explicit PerigeeCircle(const Circle2D& circle);

      static PerigeeCircle fromN(double n0, double n1, double n2, double n3 = 0);

      static PerigeeCircle fromN(double n0, const ROOT::Math::XYVector& n12, double n3 = 0);

      static PerigeeCircle
      fromCenterAndRadius(const ROOT::Math::XYVector& center,
                          double absRadius,
                          ERotation orientation = ERotation::c_CounterClockwise);
 
    public:
      void invalidate();

      bool isInvalid() const;

      bool isValid() const
      {
        return not isInvalid();
      }

      void reverse();

      PerigeeCircle reversed() const;
 
    public:
      void conformalTransform();

      PerigeeCircle conformalTransformed() const;

      void passiveMoveBy(const ROOT::Math::XYVector& by);

      PerigeeJacobian passiveMoveByJacobian(const ROOT::Math::XYVector& by) const;

      void passiveMoveByJacobian(const ROOT::Math::XYVector& by, PerigeeJacobian& jacobian) const;

      ROOT::Math::XYVector atArcLength(double arcLength) const;

      double arcLengthTo(const ROOT::Math::XYVector& point) const;

      double arcLengthBetween(const ROOT::Math::XYVector& from, const ROOT::Math::XYVector& to) const;

      double arcLengthToCylindricalR(double cylindricalR) const;

      std::pair<ROOT::Math::XYVector, ROOT::Math::XYVector> atCylindricalR(double cylindricalR) const;

      ROOT::Math::XYVector atCylindricalRForwardOf(const ROOT::Math::XYVector& startPoint, double cylindricalR) const;

      EForwardBackward isForwardOrBackwardOf(const ROOT::Math::XYVector& from, const ROOT::Math::XYVector& to) const
      {
        return VectorUtil::isForwardOrBackwardOf(tangential(from), (to - from));
      }

      EForwardBackward isForwardOrBackward(const ROOT::Math::XYVector& to) const
      {
        return VectorUtil::isForwardOrBackwardOf(tangential(), to);
      }

      ROOT::Math::XYVector
      chooseNextForwardOf(const ROOT::Math::XYVector& start, const ROOT::Math::XYVector& end1, const ROOT::Math::XYVector& end2) const;
      ROOT::Math::XYVector closest(const ROOT::Math::XYVector& point) const;

      double distance(const ROOT::Math::XYVector& point) const
      {
        return distance(fastDistance(point));
      }

      double distance(double fastDistance) const;

      double fastDistance(const ROOT::Math::XYVector& point) const;

      double fastImpact() const
      {
        return fastDistance(impact());
      }

      double fastDistance(double distance) const
      {
        return distance * (1.0 + distance * curvature() / 2);
      }

      ERightLeft isRightOrLeft(const ROOT::Math::XYVector& point) const
      {
        return static_cast<ERightLeft>(sign(fastDistance(point)));
      }

      bool isLine() const
      {
        return curvature() == 0.0;
      }

      bool isCircle() const
      {
        return curvature() != 0.0;
      }

      ERotation orientation() const
      {
        return static_cast<ERotation>(sign(curvature()));
      }

      ROOT::Math::XYVector gradient(const ROOT::Math::XYVector& point) const
      {
        return (point - perigee()) * curvature() - VectorUtil::Orthogonal(phi0Vec());
      }

      ROOT::Math::XYVector normal(const ROOT::Math::XYVector& point) const
      {
        return VectorUtil::unit(gradient(point));
      }

      ROOT::Math::XYVector tangential(const ROOT::Math::XYVector& point) const
      {
        return VectorUtil::Orthogonal(normal(point));
      }

      const ROOT::Math::XYVector& tangential() const
      {
        return phi0Vec();
      }

      ROOT::Math::XYVector perigee() const
      {
        return VectorUtil::Orthogonal(phi0Vec()) * impact();
      }

      ROOT::Math::XYVector center() const
      {
        return VectorUtil::Orthogonal(phi0Vec()) * (impact() + radius());
      }

      ROOT::Math::XYVector apogee() const
      {
        return VectorUtil::Orthogonal(phi0Vec()) * (impact() + 2 * radius());
      }

      double minimalCylindricalR() const
      {
        return fabs(impact());
      }

      double maximalCylindricalR() const
      {
        return fabs(impact() + 2 * radius());
      }

      double arcLengthPeriod() const
      {
        return std::fabs(perimeter());
      }

      double perimeter() const
      {
        return 2 * M_PI * radius();
      }

      double radius() const
      {
        return 1 / curvature();
      }

      double absRadius() const
      {
        return fabs(radius());
      }

      void setCenterAndRadius(const ROOT::Math::XYVector& center,
                              double absRadius,
                              ERotation orientation = ERotation::c_CounterClockwise);

      double n0() const
      {
        return impact() * (impact() * curvature() / 2.0 + 1.0);
      }

      ROOT::Math::XYVector n12() const
      {
        return -VectorUtil::Orthogonal(phi0Vec()) * (1 + curvature() * impact());
      }

      double n1() const
      {
        return n12().x();
      }
      //{ return phi0Vec().y() * (1 + curvature() * impact()); }

      double n2() const
      {
        return n12().y();
      }
      //{ return -(phi0Vec().x()) * (1 + curvature() * impact()); }

      double n3() const
      {
        return curvature() / 2.0;
      }

      void setN(double n0, double n1, double n2, double n3 = 0.0)
      {
        setN(n0, ROOT::Math::XYVector(n1, n2), n3);
      }

      void setN(double n0, const ROOT::Math::XYVector& n12, double n3 = 0.0);

      void setN(const Line2D& n012)
      {
        setN(n012.n0(), n012.n12());
      }

      void setN(const GeneralizedCircle& n0123)
      {
        setN(n0123.n0(), n0123.n12(), n0123.n3());
      }

      double omega() const
      {
        return -curvature();
      }

      double d0() const
      {
        return -impact();
      }

      double curvature() const
      {
        return m_curvature;
      }

      double phi0() const
      {
        return m_phi0;
      }

      const ROOT::Math::XYVector& phi0Vec() const
      {
        return m_phi0Vec;
      }

      double impact() const
      {
        return m_impact;
      }

      PerigeeParameters perigeeParameters() const
      {
        using namespace NPerigeeParameterIndices;
        PerigeeParameters result;
        result(c_Curv) = curvature();
        result(c_Phi0) = phi0();
        result(c_I) = impact();
        return result;
      }

      void setCurvature(double curvature)
      {
        m_curvature = curvature;
      }

      void setPhi0(double phi0)
      {
        m_phi0 = phi0;
        m_phi0Vec = VectorUtil::Phi(phi0);
      }

      void setPhi0(const ROOT::Math::XYVector& phi0Vec)
      {
        m_phi0 = phi0Vec.Phi();
        m_phi0Vec = VectorUtil::unit(phi0Vec);
      }

      void setImpact(double impact)
      {
        m_impact = impact;
      }

      void setPerigeeParameters(double curvature, const ROOT::Math::XYVector& phi0Vec, double impact)
      {
        m_impact = impact;
        m_phi0 = phi0Vec.Phi();
        m_phi0Vec = VectorUtil::unit(phi0Vec);
        m_curvature = curvature;
      }

      void setPerigeeParameters(double curvature, double phi0, double impact)
      {
        m_impact = impact;
        m_phi0 = phi0;
        m_phi0Vec = VectorUtil::Phi(phi0);
        m_curvature = curvature;
      }
 
    private:
      double arcLengthAtDeltaLength(double delta, double dr) const;

      double arcLengthAtSecantLength(double secantLength) const;
 
    private:
      double m_curvature = 0.0;

      double m_phi0 = NAN;

      ROOT::Math::XYVector m_phi0Vec;

      double m_impact = 0.0;
 
    };

    std::ostream& operator<<(std::ostream& output, const PerigeeCircle& circle);
  }
}