VectorUtil.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 <framework/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>
 
/* ROOT headers. */
#include <Math/Vector3D.h>
#include <Math/Vector2D.h>
#include <Math/VectorUtil.h>
 
namespace Belle2 {
 
  namespace VectorUtil {
 
    inline bool hasNAN(const ROOT::Math::XYVector& a)
    {
      return std::isnan(a.X()) or std::isnan(a.Y());
    }
 
    inline bool hasNAN(const ROOT::Math::XYZVector& a)
    {
      return std::isnan(a.X()) or std::isnan(a.Y()) or std::isnan(a.Z());
    }
 
    inline bool isNull(const ROOT::Math::XYVector& a)
    {
      return a.X() == 0.0 and a.Y() == 0.0;
    }
 
    inline bool isNull(const ROOT::Math::XYZVector& a)
    {
      return a.X() == 0.0 and a.Y() == 0.0 and a.Z() == 0.0;
    }


    inline ROOT::Math::XYVector average(const ROOT::Math::XYVector& a, const ROOT::Math::XYVector& b)
    {
      if (hasNAN(a)) {
        return b;
      } else if (hasNAN(b)) {
        return a;
      } else {
        return ROOT::Math::XYVector((a.X() + b.X()) / 2.0, (a.Y() + b.Y()) / 2.0);
      }
    }


    inline ROOT::Math::XYVector average(const ROOT::Math::XYVector& a, const ROOT::Math::XYVector& b, const ROOT::Math::XYVector& c)
    {
      if (hasNAN(a)) {
        return average(b, c);
      } else if (hasNAN(b)) {
        return average(a, c);
      } else if (hasNAN(c)) {
        return average(a, b);
      } else {
        return ROOT::Math::XYVector((a.X() + b.X() + c.X()) / 3.0,
                                    (a.Y() + b.Y() + c.Y()) / 3.0);
      }
    }


    inline ROOT::Math::XYZVector average(const ROOT::Math::XYZVector& a, const ROOT::Math::XYZVector& b)
    {
      if (hasNAN(a)) {
        return b;
      } else if (hasNAN(b)) {
        return a;
      } else {
        return ROOT::Math::XYZVector((a.x() + b.x()) / 2.0,
                                     (a.y() + b.y()) / 2.0,
                                     (a.z() + b.z()) / 2.0);
      }
    }


    inline ROOT::Math::XYVector compose(const ROOT::Math::XYVector& coordinateVec, const double parallelCoor, const double orthoCoor)
    {
      return ROOT::Math::XYVector(coordinateVec.x() * parallelCoor - coordinateVec.y() * orthoCoor,
                                  coordinateVec.y() * parallelCoor + coordinateVec.x() * orthoCoor);
    }

    template<class Vector>
    inline Vector unit(const Vector& a)
    {
      if (isNull(a)) {
        return Vector();
      }
      Vector tmp(a);
      // Somehow, the Cartesion2D vector knows Scale, but DisplacementVector2D does not, and as DisplacementVector2D<Cartesion2D.....
      // So can't use Scale, and perform plain math operations instead... ¯\_(ツ)_/¯
      // return tmp.Scale(1. / tmp.R());
      return tmp / tmp.R();
    }

    inline ROOT::Math::XYVector Phi(const double phi)
    {
      return std::isnan(phi) ? ROOT::Math::XYVector(0.0, 0.0) : ROOT::Math::XYVector(std::cos(phi), std::sin(phi));
    }

    inline ROOT::Math::XYVector Orthogonal(const ROOT::Math::XYVector& a, const TrackingUtilities::ERotation ccwInfo)
    {
      return isValid(ccwInfo) ? ROOT::Math::XYVector(-static_cast<double>(ccwInfo) * a.Y(),
                                                     static_cast<double>(ccwInfo) * a.X()) : ROOT::Math::XYVector();
    }

    inline double Distance(const ROOT::Math::XYVector& from, const ROOT::Math::XYVector& to = ROOT::Math::XYVector(0.0, 0.0))
    {
      return (from - to).R();
    }

    inline double Distance(const ROOT::Math::XYZVector& from, const ROOT::Math::XYZVector& to = ROOT::Math::XYZVector(0.0, 0.0, 0.0))
    {
      return (from - to).R();
    }

    template<class Vector>
    inline double unnormalizedParallelComp(const Vector& v1, const Vector& relativTo)
    {
      return relativTo.Dot(v1);
    }


    template<class Vector>
    inline double orthogonalComp(const Vector& v1, const Vector& relativTo)
    {
      return Cross(relativTo, v1) / relativTo.R();
    }


    inline double unnormalizedOrthogonalComp(const ROOT::Math::XYVector& v1, const ROOT::Math::XYVector& relativTo)
    {
      return Cross(relativTo, v1);
    }

    inline ROOT::Math::XYVector passiveRotatedBy(const ROOT::Math::XYVector& v1, const ROOT::Math::XYVector& phiVec)
    {
      return ROOT::Math::XYVector(unnormalizedParallelComp(v1, phiVec), unnormalizedOrthogonalComp(v1, phiVec));
    }

    inline TrackingUtilities::ERightLeft isRightOrLeftOf(const ROOT::Math::XYVector& toCheck, const ROOT::Math::XYVector& rhs)
    {
      return static_cast<TrackingUtilities::ERightLeft>(-TrackingUtilities::sign(unnormalizedOrthogonalComp(toCheck, rhs)));
    }

    inline bool isLeftOf(const ROOT::Math::XYVector& toCheck, const ROOT::Math::XYVector& rhs)
    {
      return isRightOrLeftOf(toCheck, rhs) == TrackingUtilities::ERightLeft::c_Left;
    }

    inline bool isRightOf(const ROOT::Math::XYVector& toCheck, const ROOT::Math::XYVector& rhs)
    {
      return isRightOrLeftOf(toCheck, rhs) == TrackingUtilities::ERightLeft::c_Right;
    }

    inline 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));
    }

    inline bool isBetween(const ROOT::Math::XYVector& toCheck, const ROOT::Math::XYVector& lower, const ROOT::Math::XYVector& upper)
    {
      // 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.
      const double det = Cross(lower, upper);
      if (det == 0) {
        // lower and upper are coaligned
        return isRightOf(toCheck, lower) and isLeftOf(toCheck, upper);
      } else {
        const bool flipsOrientation = det < 0;
        const double transformedX = Cross(toCheck, upper);
        const double transformedY = -Cross(toCheck, lower);
        bool inFirstQuadrant = sameSign(det, transformedX, transformedY);
        if (flipsOrientation) {
          inFirstQuadrant = not inFirstQuadrant;
        }
        return inFirstQuadrant;
      }
    }

    inline TrackingUtilities::EForwardBackward isForwardOrBackwardOf(const ROOT::Math::XYVector& toCheck,
        const ROOT::Math::XYVector& rhs)
    {
      return static_cast<TrackingUtilities::EForwardBackward>(TrackingUtilities::sign(unnormalizedParallelComp(toCheck, rhs)));
    }


    inline bool smaller(const ROOT::Math::XYVector& lhs, const ROOT::Math::XYVector& rhs)
    {
      return lhs.Mag2() < rhs.Mag2() or
             (lhs.Mag2() == rhs.Mag2() and (lhs.Phi() < rhs.Phi()));
    }


    inline bool smaller(const ROOT::Math::XYZVector& lhs, const ROOT::Math::XYZVector& rhs)
    {
      return lhs.R() < rhs.R() or (lhs.R() == rhs.R() and
                                   (lhs.z() < rhs.z() or (lhs.z() == rhs.z() and (lhs.Phi() < rhs.Phi()))));
    }

    inline ROOT::Math::XYVector getXYVector(const ROOT::Math::XYZVector& a)
    {
      return ROOT::Math::XYVector(a.X(), a.Y());
    }
 
  } // namespace VectorUtil

} // namespace Belle2