ParameterLine2D.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/VectorUtil.h>
 
#include <tracking/trackingUtilities/numerics/Quadratic.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 ParameterLine2D {
 
    public:
      ParameterLine2D()
        : m_support(0.0, 0.0)
        , m_tangential(0.0, 0.0)
      {
      }

      ParameterLine2D(const ROOT::Math::XYVector& support, const ROOT::Math::XYVector& tangential)
        : m_support(support)
        , m_tangential(tangential)
      {
      }


      static ParameterLine2D fromSlopeIntercept(const double slope, const double intercept)
      {
        return ParameterLine2D(ROOT::Math::XYVector(0.0, intercept), ROOT::Math::XYVector(1.0, slope));
      }


      static ParameterLine2D fromSlopeIntercept(const double slope,
                                                const double intercept,
                                                const EForwardBackward orientation)
      {
        return ParameterLine2D(ROOT::Math::XYVector(0.0, intercept),
                               ROOT::Math::XYVector(orientation, static_cast<double>(orientation) * slope));
      }


      static ParameterLine2D throughPoints(const ROOT::Math::XYVector& start, const ROOT::Math::XYVector& end)
      {
        return ParameterLine2D(start, end - start);
      }


      explicit ParameterLine2D(const Line2D& line)
        : m_support(line.support())
        , m_tangential(line.tangential())
      {
      }


      static ParameterLine2D touchingCircles(const ROOT::Math::XYVector& fromCenter,
                                             double fromSignedRadius,
                                             const ROOT::Math::XYVector& toCenter,
                                             double toSignedRadius);


      operator Line2D()
      {
        return Line2D(distanceToOrigin(), VectorUtil::unit(normal()));
      } // not optimal yet. tangential.R() is getting calculated two times.
 
    public:
      const ROOT::Math::XYVector& tangential() const
      {
        return m_tangential;
      }

      ROOT::Math::XYVector normal() const
      {
        return VectorUtil::Orthogonal(tangential(), ERotation::c_Clockwise);
      }

      const ROOT::Math::XYVector& support() const
      {
        return m_support;
      }

      ROOT::Math::XYVector at(const double parameter) const
      {
        return tangential() * parameter += support();
      }

      EForwardBackward alignedWithFirst() const
      {
        return static_cast<EForwardBackward>(sign(tangential().X()));
      }

      EForwardBackward alignedWithSecond() const
      {
        return static_cast<EForwardBackward>(sign(tangential().Y()));
      }


      void normalize()
      {
        if (m_tangential.R() != 0.0) {
          m_tangential *= (1. / m_tangential.R());
        }
      }

      void invalidate()
      {
        m_support.SetXY(0, 0);
        m_tangential.SetXY(0, 0);
      }

      bool isInvalid() const
      {
        return VectorUtil::isNull(m_tangential);
      }

      void reverse()
      {
        m_tangential = -m_tangential;
      }

      ParameterLine2D reversed() const
      {
        return ParameterLine2D(support(), -tangential());
      }
 
    public:

      double distance(const ROOT::Math::XYVector& point) const
      {
        return distanceToOrigin() - VectorUtil::orthogonalComp(point, tangential());
      }


      double distance(const double first, const double second) const
      {
        return distance(ROOT::Math::XYVector(first, second));
      }

      double distanceToOrigin() const
      {
        return VectorUtil::orthogonalComp(support(), tangential());
      }

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

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

      bool isLeft(const ROOT::Math::XYVector& rhs) const
      {
        return isRightOrLeft(rhs) == ERightLeft::c_Left;
      }

      bool isRight(const ROOT::Math::XYVector& rhs) const
      {
        return isRightOrLeft(rhs) == ERightLeft::c_Right;
      }

      ROOT::Math::XYVector closest(const ROOT::Math::XYVector& point) const
      {
        double norm_squared = tangential().Mag2();
        return VectorUtil::compose(tangential(),
                                   tangential().Dot(point) / norm_squared,
                                   VectorUtil::Cross(tangential(), support()) / norm_squared);
      }

      double closestAt(const ROOT::Math::XYVector& point) const
      {
        return (tangential().Dot(point) - tangential().Dot(support())) / tangential().Mag2();
      }

      ROOT::Math::XYVector closestToOrigin() const
      {
        return VectorUtil::Orthogonal(tangential()) *
               (VectorUtil::Cross(tangential(), support()) / tangential().Mag2());
      }

      double closestToOriginAt() const
      {
        return -tangential().Dot(support()) / tangential().Mag2();
      }

      double lengthOnCurve(const ROOT::Math::XYVector& from, const ROOT::Math::XYVector& to) const
      {
        return (to.Dot(tangential()) - from.Dot(tangential())) / tangential().R();
      }

      double intersectionAt(const Line2D& line) const
      {
        return -(line.n0() + support().Dot(line.normal())) / tangential().Dot(line.normal());
      }

      double intersectionAt(const ParameterLine2D& line) const
      {
        return (VectorUtil::Cross(line.tangential(), support()) - VectorUtil::Cross(line.tangential(), line.support())) /
               VectorUtil::Cross(tangential(), line.tangential());
      }


      std::pair<double, double> intersectionsAt(const GeneralizedCircle& genCircle) const
      {
        double a = genCircle.n3() * tangential().Mag2();
        double b = tangential().Dot(genCircle.gradient(support()));
        double c = genCircle.fastDistance(support());
 
        return solveQuadraticABC(a, b, c);
      }

      ROOT::Math::XYVector intersection(const Line2D& line) const
      {
        return at(intersectionAt(line));
      }

      ROOT::Math::XYVector intersection(const ParameterLine2D& line) const
      {
        return at(intersectionAt(line));
      }


      void passiveMoveAtBy(const double delta)
      {
        m_support += tangential() * delta;
      }

      void moveBy(const ROOT::Math::XYVector& by)
      {
        m_support += by;
      }

      void moveAlongFirst(const double first)
      {
        m_support.SetX(m_support.X() + first);
      }

      void moveAlongSecond(const double second)
      {
        m_support.SetY(m_support.Y() + second);
      }

      void passiveMoveBy(const ROOT::Math::XYVector& by)
      {
        m_support -= by;
      }

      void passiveMoveAlongFirst(const double first)
      {
        m_support.SetX(m_support.X() - first);
      }

      void passiveMoveAlongSecond(const double second)
      {
        m_support.SetY(m_support.Y() - second);
      }



      double slope() const
      {
        return tangential().Y() / tangential().X();
      }

      double inverseSlope() const
      {
        return tangential().X() / tangential().Y();
      }

      double intercept() const
      {
        return support().Y() - slope() * support().X();
      }

      double zero() const
      {
        return support().X() - inverseSlope() * support().Y();
      }

      double map(const double first) const
      {
        return support().Y() + slope() * (first - support().X());
      }

      double operator()(const double first) const
      {
        return map(first);
      }

      double inverseMap(const double second) const
      {
        return support().X() + inverseSlope() * (second - support().Y());
      }

      void invert()
      {
        m_tangential.SetXY(m_tangential.Y(), m_tangential.X());
        m_support.SetXY(m_support.Y(), m_support.X());
      }

      ParameterLine2D inverted() const
      {
        return ParameterLine2D(ROOT::Math::XYVector(support().Y(), support().X()),
                               ROOT::Math::XYVector(tangential().Y(), tangential().X()));
      }

 
    private:
      ROOT::Math::XYVector m_support;

      ROOT::Math::XYVector m_tangential;
    };

    std::ostream& operator<<(std::ostream& output, const ParameterLine2D& line);
  }
}