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
 
/* ROOT headers. */
#include <Math/Vector3D.h>
#include <Math/Vector2D.h>
#include <Math/VectorUtil.h>
#include <TVector3.h>
 
namespace Belle2 {

  static constexpr auto XYZToTVector = [](const ROOT::Math::XYZVector& a)
  {
    return TVector3(a.X(), a.Y(), a.Z());
  };
 
  namespace VectorUtil {

    inline void setMagThetaPhi(ROOT::Math::XYZVector& vector,
                               double mag, double theta, double phi)
    {
      const double amag = std::abs(mag);
      const double sinTheta = std::sin(theta);
      const double x = amag * sinTheta * std::cos(phi);
      const double y = amag * sinTheta * std::sin(phi);
      const double z = amag * std::cos(theta);
      vector.SetXYZ(x, y, z);
    }

    inline void setMag(ROOT::Math::XYZVector& vector, double mag)
    {
      setMagThetaPhi(vector, mag, vector.Theta(), vector.Phi());
    }

    inline void setTheta(ROOT::Math::XYZVector& vector, double theta)
    {
      setMagThetaPhi(vector, vector.R(), theta, vector.Phi());
    }

    inline void setPhi(ROOT::Math::XYZVector& vector, double phi)
    {
      setMagThetaPhi(vector, vector.R(), vector.Theta(), phi);
    }

    inline void setPtThetaPhi(ROOT::Math::XYZVector& vector,
                              double pt, double theta, double phi)
    {
      const double aPt = std::abs(pt);
      const double x = aPt * std::cos(phi);
      const double y = aPt * std::sin(phi);
      const double tanTheta = std::tan(theta);
      const double z = tanTheta ? aPt / tanTheta : 0;
      vector.SetXYZ(x, y, z);
    }

    template <class Vector>
    double CosTheta(const Vector&  v1, const Vector& v2)
    {
      const double v1_r2 = v1.Mag2();
      const double v2_r2 = v2.Mag2();
      const double ptot2 = v1_r2 * v2_r2;
      if (ptot2 == 0) {
        return 0.0;
      }
      const double pdot = v1.Dot(v2);
      double arg = pdot / std::sqrt(ptot2);
      if (arg >  1.0)
        arg =  1.0;
      if (arg < -1.0)
        arg = -1.0;
      return arg;
    }
 

    template <class Vector>
    double Angle(const Vector&  v1, const Vector& v2)
    {
      return std::acos(CosTheta(v1, v2));
    }

    inline ROOT::Math::XYVector orthogonalVector(const ROOT::Math::XYVector& v1, const ROOT::Math::XYVector& v2)
    {
      const double cross = v1.X() * v2.Y() - v1.Y() - v2.X();     // = relativTo.cross(*this)
      const ROOT::Math::XYVector tmp = v2 * (cross / v2.Mag2());  // = relativTo.scaled(cross / relativTo.normSquared())
      return ROOT::Math::XYVector(-tmp.Y(), tmp.X());             // = .orthogonal()
    }

    template<class aVector>
    inline aVector parallelVector(const aVector& v1, const aVector& v2)
    {
      const double v2Mag2 = v2.Mag2();
      if (v2Mag2 == 0)
        return aVector();
      const double dotp = v1.Dot(v2);           // = relativTo.dot(*this)
      const aVector tmp = v2 * (dotp / v2Mag2); // = relativTo.scaled(dotp / relativTo.normSquared())
      return tmp;
    }
 
  }

}