Helix.cc

/**************************************************************************
 * 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.                  *
 **************************************************************************/
#include <tracking/trackingUtilities/geometry/Helix.h>
 
#include <tracking/trackingUtilities/geometry/HelixParameters.h>
#include <tracking/trackingUtilities/geometry/PerigeeCircle.h>
#include <tracking/trackingUtilities/geometry/PerigeeParameters.h>
#include <tracking/trackingUtilities/geometry/SZLine.h>
#include <tracking/trackingUtilities/geometry/SZParameters.h>
 
#include <tracking/trackingUtilities/geometry/VectorUtil.h>
 
#include <Math/Vector3D.h>
 
#include <boost/math/tools/minima.hpp>
 
#include <utility>
#include <algorithm>
#include <limits>
#include <ostream>
#include <cstdint>
 
using namespace Belle2;
using namespace TrackingUtilities;
 
double Helix::arcLength2DToClosest(const ROOT::Math::XYZVector& point, bool firstPeriod) const
{
  // The point may happen to lie in the center of the helix.
  double loc_d0 = circleXY().distance(VectorUtil::getXYVector(point));
  double denominator = 1 + curvatureXY() * loc_d0;
  if (denominator == 0) {
    // When this happens we can optimise the z distance for the closest point
    double arcLength2D = (point.z() - z0()) / tanLambda();
    if (not std::isfinite(arcLength2D)) {
      return 0.0;
    } else {
      return arcLength2D;
    }
  }
 
  // First approximation optimising the xy distance.
  double arcLength2D = circleXY().arcLengthTo(VectorUtil::getXYVector(point));
  // In case the helix is a flat circle with no extend into z this is the actual solution
  if (tanLambda() == 0) {
    return arcLength2D;
  }
 
  double deltaZ = point.z() - szLine().map(arcLength2D);
  if (not firstPeriod) {
    if (fabs(deltaZ) > zPeriod() / 2) {
      double newDeltaZ = std::remainder(deltaZ, zPeriod());
      double nPeriodShift = (deltaZ - newDeltaZ) / zPeriod();
      arcLength2D += nPeriodShift * perimeterXY();
      deltaZ = newDeltaZ;
    }
  }
 
  using boost::math::tools::brent_find_minima;
 
  auto distance3D = [this, &point](const double & s) -> double {
    ROOT::Math::XYZVector pos = atArcLength2D(s);
    return VectorUtil::Distance(pos, point);
  };
 
  double searchWidth = std::fmin(perimeterXY(), 2 * deltaZ / tanLambda());
 
  double lowerS = arcLength2D - searchWidth;
  double upperS = arcLength2D + searchWidth;
 
  int bits = std::numeric_limits<double>::digits;
  size_t nMaxIter = 100;
 
  std::pair<double, double> sBounds = brent_find_minima(distance3D, lowerS, upperS, bits, nMaxIter);
 
  // Stopped before iterations were exhausted?
  // bool converged = nMaxIter > 0;
 
  double firstDistance = distance3D(sBounds.first);
  double secondDistance = distance3D(sBounds.second);
  if (firstDistance < secondDistance) {
    return sBounds.first;
  } else {
    return sBounds.second;
  }
}
 
HelixJacobian Helix::passiveMoveByJacobian(const ROOT::Math::XYZVector& by) const
{
  // Fills the upper left 3x3 corner.
  PerigeeJacobian perigeeJacobian = circleXY().passiveMoveByJacobian(VectorUtil::getXYVector(by));
  SZJacobian szJacobian = SZUtil::identity();
  HelixJacobian jacobian = HelixUtil::stackBlocks(perigeeJacobian, szJacobian);
 
  double curv = curvatureXY();
  double tanL = tanLambda();
  double sArc = circleXY().arcLengthTo(VectorUtil::getXYVector(by));
 
  using namespace NHelixParameterIndices;
  jacobian(c_Z0, c_Curv) = tanL * (jacobian(c_Phi0, c_Curv) - sArc) / curv;
  jacobian(c_Z0, c_Phi0) = tanL * (jacobian(c_Phi0, c_Phi0) - 1.) / curv;
  jacobian(c_Z0, c_I) = tanL * jacobian(c_Phi0, c_I) / curv;
  jacobian(c_Z0, c_TanL) = sArc;
 
  return jacobian;
}
 
std::ostream& TrackingUtilities::operator<<(std::ostream& output, const Helix& helix)
{
  return output << "Helix("
         << "curv=" << helix.curvatureXY() << ","
         << "phi0=" << helix.phi0() << ","
         << "impact=" << helix.impactXY() << ","
         << "tanL=" << helix.tanLambda() << ","
         << "z0=" << helix.z0() << ")";
}