Vector2D.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/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 <framework/geometry/VectorUtil.h>
 
#include <Math/Vector2D.h>
 
#include <utility>
#include <string>
#include <iosfwd>
#include <cmath>
 
namespace Belle2 {
  namespace TrackingUtilities {
    class Vector2D {
 
    public:
      Vector2D() = default;
 
      // /// Constructor translating from a ROOT::Math::XYVector instance
      // TODO / FIXME:
      //   had to comment this explicit constructor because of compilation issues that I did not
      //   fully understand nor really wanted to investigate and solve - just using the non-explicit
      //   version solved all the issues, but for the future I think it would be better to solve this
      //   issue and use the explicit constructor
      // explicit Vector2D(const ROOT::Math::XYVector& xyVector)
      Vector2D(const ROOT::Math::XYVector& xyVector);

      Vector2D(const double x, const double y)
        : m_x(x)
        , m_y(y)
      {
      }

      Vector2D& operator=(const ROOT::Math::XYVector& xyVector);

      operator const ROOT::Math::XYVector() const;

      Vector2D(const Vector2D& coordinateVec, const double parallelCoor, const double orthoCoor)
        : m_x(coordinateVec.x() * parallelCoor - coordinateVec.y() * orthoCoor)
        , m_y(coordinateVec.y() * parallelCoor + coordinateVec.x() * orthoCoor)
      {
      }

      static Vector2D Phi(const double phi)
      {
        return std::isnan(phi) ? Vector2D(0.0, 0.0) : Vector2D(cos(phi), sin(phi));
      }


      static Vector2D
      compose(const Vector2D& coordinateVec, const double parallelCoor, const double orthoCoor)
      {
        return Vector2D(coordinateVec, parallelCoor, orthoCoor);
      }


      static Vector2D average(const Vector2D& one, const Vector2D& two)
      {
        if (one.hasNAN()) {
          return two;
        } else if (two.hasNAN()) {
          return one;
        } else {
          return Vector2D((one.x() + two.x()) / 2.0, (one.y() + two.y()) / 2.0);
        }
      }


      static Vector2D average(const Vector2D& one, const Vector2D& two, const Vector2D& three)
      {
 
        if (one.hasNAN()) {
          return average(two, three);
        } else if (two.hasNAN()) {
          return average(one, three);
        } else if (three.hasNAN()) {
          return average(one, two);
        } else {
          return Vector2D((one.x() + two.x() + three.x()) / 3.0,
                          (one.y() + two.y() + three.y()) / 3.0);
        }
      }

      bool operator==(const Vector2D& rhs) const
      {
        return x() == rhs.x() and y() == rhs.y();
      }


      bool operator<(const Vector2D& rhs) const
      {
        return normSquared() < rhs.normSquared() or
               (normSquared() == rhs.normSquared() and (phi() < rhs.phi()));
      }


      static Vector2D getLowest()
      {
        return Vector2D(0.0, 0.0);
      }

      bool isNull() const
      {
        return x() == 0.0 and y() == 0.0;
      }

      bool hasNAN() const
      {
        return std::isnan(x()) or std::isnan(y());
      }

      std::string __str__() const;

      double dot(const Vector2D& rhs) const
      {
        return x() * rhs.x() + y() * rhs.y();
      }

      double Dot(const Vector2D& rhs) const
      {
        return dot(rhs);
      }

      double cross(const Vector2D& rhs) const
      {
        return x() * rhs.y() - y() * rhs.x();
      }

      double normSquared() const
      {
        return x() * x() + y() * y();
      }

      double Mag2() const
      {
        return normSquared();
      }

      double norm() const
      {
        return hypot2(x(), y());
      }

      double cosWith(const Vector2D& rhs) const
      {
        return dot(rhs) / (norm() * rhs.norm());
      }

      double sinWith(const Vector2D& rhs) const
      {
        return cross(rhs) / (norm() * rhs.norm());
      }

      double angleWith(const Vector2D& rhs) const
      {
        return atan2(cross(rhs), dot(rhs));
      }


      double distance(const Vector2D& rhs = Vector2D(0.0, 0.0)) const
      {
        double deltaX = x() - rhs.x();
        double deltaY = y() - rhs.y();
        return hypot2(deltaX, deltaY);
      }

      Vector2D& scale(const double factor)
      {
        m_x *= factor;
        m_y *= factor;
        return *this;
      }

      Vector2D& operator*=(const double factor)
      {
        return scale(factor);
      }

      Vector2D scaled(const double factor) const
      {
        return Vector2D(x() * factor, y() * factor);
      }

      friend Vector2D operator*(const Vector2D& vec2D, const double factor)
      {
        return vec2D.scaled(factor);
      }

      Vector2D& divide(const double denominator)
      {
        m_x /= denominator;
        m_y /= denominator;
        return *this;
      }

      Vector2D& operator/=(const double denominator)
      {
        return divide(denominator);
      }

      Vector2D divided(const double denominator) const
      {
        return Vector2D(x() / denominator, y() / denominator);
      }

      Vector2D operator/(const double denominator) const
      {
        return divided(denominator);
      }

      Vector2D& add(const Vector2D& rhs)
      {
        m_x += rhs.x();
        m_y += rhs.y();
        return *this;
      }

      Vector2D& operator+=(const Vector2D& rhs)
      {
        return add(rhs);
      }

      Vector2D& subtract(const Vector2D& rhs)
      {
        m_x -= rhs.x();
        m_y -= rhs.y();
        return *this;
      }

      Vector2D& operator-=(const Vector2D& rhs)
      {
        return subtract(rhs);
      }

      Vector2D orthogonal() const
      {
        return Vector2D(-y(), x());
      }

      Vector2D orthogonal(const ERotation ccwInfo) const
      {
        return isValid(ccwInfo) ? Vector2D(-static_cast<double>(ccwInfo) * y(), static_cast<double>(ccwInfo) * x()) : Vector2D();
      }


      double normalize()
      {
        double originalLength = norm();
        if (originalLength != 0.0) divide(originalLength);
        return originalLength;
      }


      double normalizeTo(const double toLength)
      {
        double originalLength = norm();
        if (originalLength != 0.0) scale(toLength / originalLength);
        return originalLength;
      }

      Vector2D unit() const
      {
        return isNull() ? Vector2D(0.0, 0.0) : divided(norm());
      }

      Vector2D& reverse()
      {
        scale(-1.0);
        return *this;
      }

      Vector2D reversed() const
      {
        return scaled(-1.0);
      }

      Vector2D operator-() const
      {
        return reversed();
      }

      void flipFirst()
      {
        m_x = -x();
      }

      void flipSecond()
      {
        m_y = -y();
      }

      Vector2D flippedFirst() const
      {
        return Vector2D(-x(), y());
      }

      Vector2D flippedSecond() const
      {
        return Vector2D(x(), -y());
      }

      Vector2D flippedOver(const Vector2D& reflectionLine) const
      {
        return *this - orthogonalVector(reflectionLine) * 2;
      }

      Vector2D flippedAlong(const Vector2D& flippingDirection) const
      {
        return *this - Belle2::VectorUtil::parallelVector(*this, flippingDirection) * 2;
      }


      void conformalTransform()
      {
        divide(normSquared());
      }


      Vector2D conformalTransformed() const
      {
        return divided(normSquared());
      }

      Vector2D operator+(const Vector2D& rhs) const
      {
        return Vector2D(x() + rhs.x(), y() + rhs.y());
      }

      Vector2D operator-(const Vector2D& rhs) const
      {
        return Vector2D(x() - rhs.x(), y() - rhs.y());
      }

      Vector2D operator-(const ROOT::Math::XYVector& rhs) const
      {
        return Vector2D(x() - rhs.X(), y() - rhs.Y());
      }

      double parallelComp(const Vector2D& relativTo) const
      {
        return relativTo.dot(*this) / relativTo.norm();
      }


      double unnormalizedParallelComp(const Vector2D& relativTo) const
      {
        return relativTo.dot(*this);
      }


      double orthogonalComp(const Vector2D& relativTo) const
      {
        return relativTo.cross(*this) / relativTo.norm();
      }

      Vector2D orthogonalVector(const Vector2D& relativTo) const
      {
        return relativTo.scaled(relativTo.cross(*this) / relativTo.normSquared()).orthogonal();
      }


      double unnormalizedOrthogonalComp(const Vector2D& relativTo) const
      {
        return relativTo.cross(*this);
      }

      ERightLeft isRightOrLeftOf(const Vector2D& rhs) const
      {
        return static_cast<ERightLeft>(-sign(unnormalizedOrthogonalComp(rhs)));
      }

      bool isLeftOf(const Vector2D& rhs) const
      {
        return isRightOrLeftOf(rhs) == ERightLeft::c_Left;
      }

      bool isRightOf(const Vector2D& rhs) const
      {
        return isRightOrLeftOf(rhs) == ERightLeft::c_Right;
      }

      ERotation isCCWOrCWOf(const Vector2D& rhs) const
      {
        return static_cast<ERotation>(sign(unnormalizedOrthogonalComp(rhs)));
      }

      bool isCCWOf(const Vector2D& rhs) const
      {
        return isCCWOrCWOf(rhs) == ERotation::c_CounterClockwise;
      }

      bool isCWOf(const Vector2D& rhs) const
      {
        return isCCWOrCWOf(rhs) == ERotation::c_Clockwise;
      }

      EForwardBackward isForwardOrBackwardOf(const Vector2D& rhs) const
      {
        return static_cast<EForwardBackward>(sign(unnormalizedParallelComp(rhs)));
      }

      bool isForwardOf(const Vector2D& rhs) const
      {
        return isForwardOrBackwardOf(rhs) == EForwardBackward::c_Forward;
      }

      bool isBackwardOf(const Vector2D& rhs) const
      {
        return isForwardOrBackwardOf(rhs) == EForwardBackward::c_Backward;
      }
 
    private:
      static bool sameSign(float n1, float n2, float n3)
      {
        return ((n1 > 0 and n2 > 0 and n3 > 0) or (n1 < 0 and n2 < 0 and n3 < 0));
      }
 
    public:
      bool isBetween(const Vector2D& lower, const Vector2D& upper) const
      {
        // Set up a linear (nonorthogonal) transformation that maps
        // lower -> (1, 0)
        // upper -> (0, 1)
        // Check whether this transformation is orientation conserving
        // If yes this vector must lie in the first quadrant to be between lower and upper
        // If no it must lie in some other quadrant.
        double det = lower.cross(upper);
        if (det == 0) {
          // lower and upper are coaligned
          return isRightOf(lower) and isLeftOf(upper);
        } else {
          bool flipsOrientation = det < 0;
 
          double transformedX = cross(upper);
          double transformedY = -cross(lower);
          bool inFirstQuadrant = sameSign(det, transformedX, transformedY);
          if (flipsOrientation) {
            inFirstQuadrant = not inFirstQuadrant;
          }
          return inFirstQuadrant;
        }
      }

      void swapCoordinates()
      {
        std::swap(m_x, m_y);
      }

      double cylindricalR() const
      {
        return hypot2(x(), y());
      }

      void setCylindricalR(const double cylindricalR)
      {
        scale(cylindricalR / norm());
      }

      double phi() const
      {
        return isNull() ? NAN : atan2(y(), x());
      }

      void passiveMoveBy(const Vector2D& by)
      {
        subtract(by);
      }

      Vector2D passiveMovedBy(const Vector2D& by) const
      {
        return *this - by;
      }

      Vector2D passiveRotatedBy(const Vector2D& phiVec) const
      {
        return Vector2D(unnormalizedParallelComp(phiVec), unnormalizedOrthogonalComp(phiVec));
      }

      double x() const
      {
        return m_x;
      }

      double X() const
      {
        return m_x;
      }

      void setX(const double x)
      {
        m_x = x;
      }

      double y() const
      {
        return m_y;
      }

      double Y() const
      {
        return m_y;
      }

      void setY(const double y)
      {
        m_y = y;
      }

      void setXY(const double x, const double y)
      {
        setX(x);
        setY(y);
      }

      void setXY(const Vector2D& xy)
      {
        m_x = xy.x();
        m_y = xy.y();
      }

      double first() const
      {
        return m_x;
      }

      void setFirst(const double first)
      {
        m_x = first;
      }

      double second() const
      {
        return m_y;
      }

      void setSecond(const double second)
      {
        m_y = second;
      }

      void set(const double first, const double second)
      {
        setX(first);
        setY(second);
      }

      void set(const Vector2D& both)
      {
        m_x = both.x();
        m_y = both.y();
      }
 
    private:
      double m_x = 0.0;

      double m_y = 0.0;
    };

    template<class Vector>
    Vector2D operator- (const Vector& a, const Vector2D& b)
    {
      return Vector2D(a.X() - b.X(), a.Y() - b.Y());
    }

    std::ostream& operator<<(std::ostream& output, const Vector2D& vector2D);
  }
}