func.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 <cmath>
#include <Math/Vector3D.h>
 
 
namespace Belle2 {
  namespace TOP {

    namespace func {

      inline long lround(double x)
      {
        // 2^52: above it every double is already an integer, and the conversion below could overflow
        if (std::abs(x) < 4503599627370496.0) {
          x += 0.5;
          long n = static_cast<long>(std::abs(x)); // truncation towards zero
          if (x > 0) return n;
          return -(n + 1);
        }
        return std::lround(x); // also takes care of NaN and infinity
      }
 

      inline double unfold(double x, int nx, double A)
      {
        if (nx % 2 == 0) return (nx * A + x);
        else return (nx * A - x);
      }
 

      inline double unfold(double kx, int nx)
      {
        if (nx % 2 == 0) return kx;
        else return -kx;
      }
 

      inline void fold(double xu, double A, double& x, double& kx, int& nx)
      {
        nx = func::lround(xu / A);
        x = xu - nx * A;
        if (nx % 2 != 0) {
          x = -x;
          kx = -kx;
        }
      }

      inline double clip(double x, int Nx, double A, double xmi, double xma)
      {
        x = unfold(x, Nx, A);
        x = std::max(std::min(x, xma), xmi) - Nx * A;
        if (Nx % 2 != 0) x = -x;
        return x;
      }

      inline int getNumOfEven(int j1, int j2)
      {
        return (j2 - j1 + 1 - std::abs(j1) % 2) / 2;
      }

      inline double within2PI(double angle)
      {
        angle = fmod(angle, 2 * M_PI);
        if (angle < 0) angle += 2 * M_PI;
        return angle;
      }

      inline void rotateUz(ROOT::Math::XYZVector& vec, const ROOT::Math::XYZVector& z_Axis)
      {
        auto zAxis = z_Axis.Unit();
        double cth = zAxis.Z();
        double sth = sqrt(1 - cth * cth);
        if (sth == 0) {
          if (cth < 0) vec = -vec;
          return;
        }
        double cfi = zAxis.X() / sth;
        double sfi = zAxis.Y() / sth;
        // rotation by theta around y then by phi around z
        double x = cth * vec.X() + sth * vec.Z();
        double y = vec.Y();
        double z = -sth * vec.X() + cth * vec.Z();
        vec.SetX(cfi * x - sfi * y);
        vec.SetY(sfi * x + cfi * y);
        vec.SetZ(z);
      }
 
 
    } // func
  } // TOP
} // Belle2