ParameterLine2D.h

/**************************************************************************
 * BASF2 (Belle Analysis Framework 2)                                     *
 * Copyright(C) 2012 - 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/GeneralizedCircle.h>
#include <tracking/trackFindingCDC/geometry/Line2D.h>
#include <tracking/trackFindingCDC/geometry/Vector2D.h>
 
#include <tracking/trackFindingCDC/numerics/Quadratic.h>
#include <tracking/trackFindingCDC/numerics/EForwardBackward.h>
#include <tracking/trackFindingCDC/numerics/ERightLeft.h>
#include <tracking/trackFindingCDC/numerics/ERotation.h>
#include <tracking/trackFindingCDC/numerics/ESign.h>
 
#include <utility>
#include <cmath>
#include <iosfwd>
 
namespace Belle2 {
  namespace TrackFindingCDC {
 
 
    class ParameterLine2D {
 
    public:
      ParameterLine2D()
        : m_support(0.0, 0.0)
        , m_tangential(0.0, 0.0)
      {
      }
 
      ParameterLine2D(const Vector2D& support, const Vector2D& tangential)
        : m_support(support)
        , m_tangential(tangential)
      {
      }
 
 
      static ParameterLine2D fromSlopeIntercept(const double slope, const double intercept)
      {
        return ParameterLine2D(Vector2D(0.0, intercept), Vector2D(1.0, slope));
      }
 
 
      static ParameterLine2D fromSlopeIntercept(const double slope,
                                                const double intercept,
                                                const EForwardBackward orientation)
      {
        return ParameterLine2D(Vector2D(0.0, intercept),
                               Vector2D(orientation, orientation * slope));
      }
 
 
      static ParameterLine2D throughPoints(const Vector2D& start, const Vector2D& end)
      {
        return ParameterLine2D(start, end - start);
      }
 
 
      explicit ParameterLine2D(const Line2D& line)
        : m_support(line.support())
        , m_tangential(line.tangential())
      {
      }
 
 
      static ParameterLine2D touchingCircles(const Vector2D& fromCenter,
                                             double fromSignedRadius,
                                             const Vector2D& toCenter,
                                             double toSignedRadius);
 
 
      operator Line2D()
      {
        return Line2D(distanceToOrigin(), normal().unit());
      } // not optimal yet. tangential.norm() is getting calculated two times.
 
    public:
      const Vector2D& tangential() const
      {
        return m_tangential;
      }
      Vector2D normal() const
      {
        return tangential().orthogonal(ERotation::c_Clockwise);
      }
 
      const Vector2D& support() const
      {
        return m_support;
      }
 
      Vector2D at(const double parameter) const
      {
        return tangential() * parameter += support();
      }
 
      EForwardBackward alignedWithFirst() const
      {
        return static_cast<EForwardBackward>(sign(tangential().first()));
      }
 
      EForwardBackward alignedWithSecond() const
      {
        return static_cast<EForwardBackward>(sign(tangential().second()));
      }
 
 
      void normalize()
      {
        m_tangential.normalize();
      }
 
      void invalidate()
      {
        m_support.set(0, 0);
        m_tangential.set(0, 0);
      }
 
      bool isInvalid() const
      {
        return m_tangential.isNull();
      }
 
      void reverse()
      {
        m_tangential.reverse();
      }
 
      ParameterLine2D reversed() const
      {
        return ParameterLine2D(support(), -tangential());
      }
 
    public:
 
      double distance(const Vector2D& point) const
      {
        return distanceToOrigin() - point.orthogonalComp(tangential());
      }
 
 
      double distance(const double first, const double second) const
      {
        return distance(Vector2D(first, second));
      }
 
      double distanceToOrigin() const
      {
        return support().orthogonalComp(tangential());
      }
 
      double absoluteDistance(const Vector2D& point) const
      {
        return fabs(distance(point));
      }
 
      ERightLeft isRightOrLeft(const Vector2D& point) const
      {
        return static_cast<ERightLeft>(sign(distance(point)));
      }
 
      bool isLeft(const Vector2D& rhs) const
      {
        return isRightOrLeft(rhs) == ERightLeft::c_Left;
      }
 
      bool isRight(const Vector2D& rhs) const
      {
        return isRightOrLeft(rhs) == ERightLeft::c_Right;
      }
 
      Vector2D closest(const Vector2D& point) const
      {
        double norm_squared = tangential().normSquared();
        return Vector2D(tangential(),
                        tangential().dot(point) / norm_squared,
                        tangential().cross(support()) / norm_squared);
      }
 
      double closestAt(const Vector2D& point) const
      {
        return (tangential().dot(point) - tangential().dot(support())) / tangential().normSquared();
      }
 
      Vector2D closestToOrigin() const
      {
        return tangential().orthogonal() *=
                 (tangential().cross(support()) / tangential().normSquared());
      }
 
      double closestToOriginAt() const
      {
        return -tangential().dot(support()) / tangential().normSquared();
      }
 
      double lengthOnCurve(const Vector2D& from, const Vector2D& to) const
      {
        return (to.dot(tangential()) - from.dot(tangential())) / tangential().norm();
      }
 
      double intersectionAt(const Line2D& line) const
      {
        return -(line.n0() + support().dot(line.normal())) / tangential().dot(line.normal());
      }
 
      double intersectionAt(const ParameterLine2D& line) const
      {
        return (line.tangential().cross(support()) - line.tangential().cross(line.support())) /
               tangential().cross(line.tangential());
      }
 
 
      std::pair<double, double> intersectionsAt(const GeneralizedCircle& genCircle) const
      {
        double a = genCircle.n3() * tangential().normSquared();
        double b = tangential().dot(genCircle.gradient(support()));
        double c = genCircle.fastDistance(support());
 
        return solveQuadraticABC(a, b, c);
      }
 
      Vector2D intersection(const Line2D& line) const
      {
        return at(intersectionAt(line));
      }
 
      Vector2D intersection(const ParameterLine2D& line) const
      {
        return at(intersectionAt(line));
      }
 
 
      void passiveMoveAtBy(const double delta)
      {
        m_support += tangential() * delta;
      }
 
      void moveBy(const Vector2D& by)
      {
        m_support += by;
      }
 
      void moveAlongFirst(const double first)
      {
        m_support.setFirst(m_support.first() + first);
      }
 
      void moveAlongSecond(const double second)
      {
        m_support.setSecond(m_support.second() + second);
      }
 
      void passiveMoveBy(const Vector2D& by)
      {
        m_support -= by;
      }
 
      void passiveMoveAlongFirst(const double first)
      {
        m_support.setFirst(m_support.first() - first);
      }
 
      void passiveMoveAlongSecond(const double second)
      {
        m_support.setSecond(m_support.second() - second);
      }
      double slope() const
      {
        return tangential().second() / tangential().first();
      }
 
      double inverseSlope() const
      {
        return tangential().first() / tangential().second();
      }
 
      double intercept() const
      {
        return support().second() - slope() * support().first();
      }
 
      double zero() const
      {
        return support().first() - inverseSlope() * support().second();
      }
 
      double map(const double first) const
      {
        return support().second() + slope() * (first - support().first());
      }
 
      double operator()(const double first) const
      {
        return map(first);
      }
 
      double inverseMap(const double second) const
      {
        return support().first() + inverseSlope() * (second - support().second());
      }
 
      void invert()
      {
        m_tangential.swapCoordinates();
        m_support.swapCoordinates();
      }
 
      ParameterLine2D inverted() const
      {
        return ParameterLine2D(Vector2D(support().second(), support().first()),
                               Vector2D(tangential().second(), tangential().first()));
      }
 
    private:
      Vector2D m_support;
 
      Vector2D m_tangential;
    };
 
    std::ostream& operator<<(std::ostream& output, const ParameterLine2D& line);
  }
}