RKPropagatorInS.cc

Go to the documentation of this file.

#include "TrackPropagation/RungeKutta/interface/RKPropagatorInS.h"
#include "RKCartesianDistance.h"
#include "CartesianLorentzForce.h"
#include "RKLocalFieldProvider.h"
#include "DataFormats/GeometrySurface/interface/Plane.h"
#include "DataFormats/GeometrySurface/interface/Cylinder.h"
#include "RKAdaptiveSolver.h"
#include "RKOne4OrderStep.h"
#include "RKOneCashKarpStep.h"
#include "PathToPlane2Order.h"
#include "CartesianStateAdaptor.h"
#include "TrackingTools/GeomPropagators/interface/StraightLineCylinderCrossing.h"
#include "TrackingTools/GeomPropagators/interface/StraightLineBarrelCylinderCrossing.h"
#include "MagneticField/VolumeGeometry/interface/MagVolume.h"
#include "TrackingTools/GeomPropagators/interface/PropagationDirectionFromPath.h"
#include "FrameChanger.h"
#include "TrackingTools/GeomPropagators/interface/StraightLinePlaneCrossing.h"
#include "AnalyticalErrorPropagation.h"
#include "GlobalParametersWithPath.h"
#include "TrackingTools/GeomPropagators/interface/PropagationExceptions.h"

#include "FWCore/MessageLogger/interface/MessageLogger.h"
#include "FWCore/Utilities/interface/Likely.h"


std::pair<TrajectoryStateOnSurface,double>
RKPropagatorInS::propagateWithPath(const FreeTrajectoryState& fts, 
                   const Plane& plane) const
{
  GlobalParametersWithPath gp =  propagateParametersOnPlane( fts, plane);
  if UNLIKELY(!gp) return TsosWP(TrajectoryStateOnSurface(),0.);

  SurfaceSideDefinition::SurfaceSide side = PropagationDirectionFromPath()(gp.s(),propagationDirection())==alongMomentum 
    ? SurfaceSideDefinition::beforeSurface : SurfaceSideDefinition::afterSurface;
  return  analyticalErrorPropagation( fts, plane, side, gp.parameters(),gp.s());
}

std::pair< TrajectoryStateOnSurface, double> 
RKPropagatorInS::propagateWithPath (const FreeTrajectoryState& fts, const Cylinder& cyl) const
{
  GlobalParametersWithPath gp =  propagateParametersOnCylinder( fts, cyl);
  if  UNLIKELY(!gp) return TsosWP(TrajectoryStateOnSurface(),0.);

  SurfaceSideDefinition::SurfaceSide side = PropagationDirectionFromPath()(gp.s(),propagationDirection())==alongMomentum 
    ? SurfaceSideDefinition::beforeSurface : SurfaceSideDefinition::afterSurface;
  return analyticalErrorPropagation( fts, cyl, side, gp.parameters(),gp.s());
  
}

GlobalParametersWithPath
RKPropagatorInS::propagateParametersOnPlane( const FreeTrajectoryState& ts, 
                         const Plane& plane) const
{

  GlobalPoint gpos( ts.position());
  GlobalVector gmom( ts.momentum());
  double startZ = plane.localZ(gpos);
  // (transverse) curvature
  double rho = ts.transverseCurvature();
  //
  // Straight line approximation? |rho|<1.e-10 equivalent to ~ 1um 
  // difference in transversal position at 10m.
  //
  if  UNLIKELY( fabs(rho)<1.e-10 ) {
    //
    // Instantiate auxiliary object for finding intersection.
    // Frame-independant point and vector are created explicitely to 
    // avoid confusing gcc (refuses to compile with temporary objects
    // in the constructor).
    //
    LogDebug("RKPropagatorInS")<<" startZ = "<<startZ;

    if UNLIKELY(fabs(startZ) < 1e-5){  
      LogDebug("RKPropagatorInS")<< "Propagation is not performed: state is already on final surface.";
      GlobalTrajectoryParameters res( gpos, gmom, ts.charge(), 
                      theVolume);
      return GlobalParametersWithPath( res, 0.0);
    }

    StraightLinePlaneCrossing::PositionType pos(gpos);
    StraightLinePlaneCrossing::DirectionType dir(gmom);
    StraightLinePlaneCrossing planeCrossing(pos,dir, propagationDirection());
    //
    // get solution
    //
    std::pair<bool,double> propResult = planeCrossing.pathLength(plane);
    if LIKELY( propResult.first && theVolume !=nullptr) {
      double s = propResult.second;
      // point (reconverted to GlobalPoint)
      GlobalPoint x (planeCrossing.position(s));
      GlobalTrajectoryParameters res( x, gmom, ts.charge(), theVolume);
      return GlobalParametersWithPath( res, s);
      }
    //do someting
    LogDebug("RKPropagatorInS")<< "Straight line propgation to plane failed !!"; 
    return GlobalParametersWithPath( );

  }


#ifdef EDM_ML_DEBUG
  if (theVolume != 0) {
    LogDebug("RKPropagatorInS")  << "RKPropagatorInS: starting prop to plane in volume with pos " << theVolume->position()
          << " Z axis " << theVolume->toGlobal( LocalVector(0,0,1)) ;

    LogDebug("RKPropagatorInS")  << "The starting position is " << ts.position() << " (global) "
          << theVolume->toLocal(ts.position()) << " (local) " ;
  
    FrameChanger changer;
    auto localPlane = changer.transformPlane( plane, *theVolume);
    LogDebug("RKPropagatorInS")  << "The plane position is " << plane.position() << " (global) "
          << localPlane.position() << " (local) " ;

    LogDebug("RKPropagatorInS")  << "The initial distance to plane is " << plane.localZ( ts.position()) ;

    StraightLinePlaneCrossing cross( ts.position().basicVector(), ts.momentum().basicVector());
    std::pair<bool,double> res3 = cross.pathLength(plane);
    LogDebug("RKPropagatorInS")  << "straight line distance " << res3.first << " " << res3.second ;
  }
#endif

  typedef RKAdaptiveSolver<double,RKOneCashKarpStep, 6>   Solver;
  typedef Solver::Vector                                  RKVector;
  
  RKLocalFieldProvider field( fieldProvider());
  PathToPlane2Order pathLength( field, &field.frame());
  CartesianLorentzForce deriv(field, ts.charge());

  RKCartesianDistance dist;
  double eps = theTolerance;
  Solver solver;
  double stot = 0;
  PropagationDirection currentDirection = propagationDirection();

  // in magVolume frame
  RKVector start( CartesianStateAdaptor::rkstate( rkPosition(gpos), rkMomentum(gmom)));
  int safeGuard = 0;
  while (safeGuard++<100) {
    CartesianStateAdaptor startState(start);

    std::pair<bool,double> path = pathLength( plane, startState.position(), 
                          startState.momentum(), 
                          (double) ts.charge(), currentDirection);
    if  UNLIKELY(!path.first) { 
      LogDebug("RKPropagatorInS")  << "RKPropagatorInS: Path length calculation to plane failed!" 
                   << "...distance to plane " << plane.localZ( globalPosition(startState.position()))
                   << "...Local starting position in volume " << startState.position() 
                   << "...Magnetic field " << field.inTesla( startState.position()) ;


      return GlobalParametersWithPath();
    }

    LogDebug("RKPropagatorInS")  << "RKPropagatorInS: Path lenght to plane is " << path.second ;
   

    double sstep = path.second;
    if  UNLIKELY( std::abs(sstep) < eps) {
      LogDebug("RKPropagatorInS")  << "On-surface accuracy not reached, but pathLength calculation says we are there! "
         << "path " << path.second << " distance to plane is " << startZ ;
      GlobalTrajectoryParameters res( gtpFromVolumeLocal( startState, ts.charge()));
      return GlobalParametersWithPath( res, stot);
    }

    LogDebug("RKPropagatorInS")  << "RKPropagatorInS: Solving for " << sstep 
          << " current distance to plane is " << startZ ;

    RKVector rkresult = solver( 0, start, sstep, deriv, dist, eps);
    stot += sstep;
    CartesianStateAdaptor cur( rkresult);
    double remainingZ = plane.localZ( globalPosition(cur.position()));

    if ( fabs(remainingZ) < eps) {
      LogDebug("RKPropagatorInS")  << "On-surface accuracy reached! " << remainingZ ;
      GlobalTrajectoryParameters res( gtpFromVolumeLocal( cur, ts.charge()));
      return GlobalParametersWithPath( res, stot);
    }

    start = rkresult;

    if (remainingZ * startZ > 0) {
      LogDebug("RKPropagatorInS")  << "Accuracy not reached yet, trying in same direction again " 
        << remainingZ ;
    }
    else {
      LogDebug("RKPropagatorInS")  << "Accuracy not reached yet, trying in opposite direction " 
        << remainingZ ;
      currentDirection = invertDirection( currentDirection);
    }
    startZ = remainingZ;
  }

  edm::LogError("FailedPropagation") << " too many iterations trying to reach plane ";
  return GlobalParametersWithPath();
}

GlobalParametersWithPath
RKPropagatorInS::propagateParametersOnCylinder( const FreeTrajectoryState& ts, 
                        const Cylinder& cyl) const {

  typedef RKAdaptiveSolver<double,RKOneCashKarpStep, 6>   Solver;
  typedef Solver::Vector                                  RKVector;
  
  
  const GlobalPoint& sp = cyl.position();
  if UNLIKELY(sp.x()!=0. || sp.y()!=0.) {
      throw PropagationException("Cannot propagate to an arbitrary cylinder");
    }
  
  GlobalPoint gpos( ts.position());
  GlobalVector gmom( ts.momentum());
  LocalPoint pos(cyl.toLocal(gpos));
  LocalVector mom(cyl.toLocal(gmom));
  double startR = cyl.radius() - pos.perp();
  
  // LogDebug("RKPropagatorInS")  << "RKPropagatorInS: starting from FTS " << ts ;
  
  
  // (transverse) curvature
  double rho = ts.transverseCurvature();
  //
  // Straight line approximation? |rho|<1.e-10 equivalent to ~ 1um 
  // difference in transversal position at 10m.
  //
  if  UNLIKELY( fabs(rho)<1.e-10 ) {
      //
      // Instantiate auxiliary object for finding intersection.
      // Frame-independant point and vector are created explicitely to 
      // avoid confusing gcc (refuses to compile with temporary objects
      // in the constructor).
      //
      
      StraightLineBarrelCylinderCrossing cylCrossing(gpos,gmom,propagationDirection());
      
      //
      // get solution
      //
      std::pair<bool,double> propResult = cylCrossing.pathLength(cyl);
      if  LIKELY( propResult.first && theVolume !=nullptr) {
      double s = propResult.second;
      // point (reconverted to GlobalPoint)
      GlobalPoint x (cylCrossing.position(s));
      GlobalTrajectoryParameters res( x, gmom, ts.charge(), theVolume);
      LogDebug("RKPropagatorInS")  << "Straight line propagation to cylinder succeeded !!";
      return GlobalParametersWithPath( res, s);
    } 
      
      //do someting 
      edm::LogError("RKPropagatorInS")  <<"Straight line propagation to cylinder failed !!";
      return GlobalParametersWithPath( );
      
    }
  
  
  RKLocalFieldProvider field( fieldProvider(cyl));
  // StraightLineCylinderCrossing pathLength( pos, mom, propagationDirection());
  CartesianLorentzForce deriv(field, ts.charge());
  
  RKCartesianDistance dist;
  double eps = theTolerance;
  Solver solver;
  double stot = 0;
  PropagationDirection currentDirection = propagationDirection();
  
  RKVector start( CartesianStateAdaptor::rkstate( pos.basicVector(), mom.basicVector()));
  int safeGuard = 0;
  while (safeGuard++<100) {
    CartesianStateAdaptor startState(start);
    StraightLineCylinderCrossing pathLength( LocalPoint(startState.position()), 
                         LocalVector(startState.momentum()), 
                         currentDirection, eps);
    
    std::pair<bool,double> path = pathLength.pathLength( cyl);
    if  UNLIKELY(!path.first) { 
    LogDebug("RKPropagatorInS")  << "RKPropagatorInS: Path length calculation to cylinder failed!" 
                     << "Radius " << cyl.radius() << " pos.perp() " << LocalPoint(startState.position()).perp() ;
    return GlobalParametersWithPath();
      }
    
    LogDebug("RKPropagatorInS")  << "RKPropagatorInS: Path lenght to cylinder is " << path.second 
                 << " from point (R,z) " << startState.position().perp() 
                 << ", " << startState.position().z()
                 << " to R " << cyl.radius() 
      ;
    
    
    double sstep = path.second;
    if  UNLIKELY( std::abs(sstep) < eps) {
    LogDebug("RKPropagatorInS")  << "accuracy not reached, but pathLength calculation says we are there! "
                     << path.second ;
    
    GlobalTrajectoryParameters res( gtpFromLocal( startState.position(), 
                              startState.momentum(), 
                              ts.charge(), cyl));
    return GlobalParametersWithPath( res, stot);
      }
    
    LogDebug("RKPropagatorInS")  << "RKPropagatorInS: Solving for " << sstep 
                 << " current distance to cylinder is " << startR ;
    
    RKVector rkresult = solver( 0, start, sstep, deriv, dist, eps);
    stot += sstep;
    CartesianStateAdaptor cur( rkresult);
    double remainingR = cyl.radius() - cur.position().perp();

    if ( fabs(remainingR) < eps) {
      LogDebug("RKPropagatorInS")  << "Accuracy reached! " << remainingR ;
      GlobalTrajectoryParameters res( gtpFromLocal( cur.position(), 
                            cur.momentum(), 
                            ts.charge(), cyl));
      return GlobalParametersWithPath( res, stot);
    }
    
    start = rkresult;
    if (remainingR * startR > 0) {
      LogDebug("RKPropagatorInS")  << "Accuracy not reached yet, trying in same direction again " 
                   << remainingR ;
    }
    else {
      LogDebug("RKPropagatorInS")  << "Accuracy not reached yet, trying in opposite direction " 
                   << remainingR ;
      currentDirection = invertDirection( currentDirection);
    }
    startR = remainingR;
  }
  
  edm::LogError("FailedPropagation") << " too many iterations trying to reach cylinder ";
  return GlobalParametersWithPath();
}


Propagator * RKPropagatorInS::clone() const
{
    return new RKPropagatorInS(*this);
}

GlobalTrajectoryParameters RKPropagatorInS::gtpFromLocal( const Basic3DVector<float>& lpos,
                              const Basic3DVector<float>& lmom,
                              TrackCharge ch, const Surface& surf) const
{
    return GlobalTrajectoryParameters( surf.toGlobal( LocalPoint( lpos)),
                       surf.toGlobal( LocalVector( lmom)), ch, theVolume);
}

RKLocalFieldProvider RKPropagatorInS::fieldProvider() const
{
  return RKLocalFieldProvider( *theVolume);
}

RKLocalFieldProvider RKPropagatorInS::fieldProvider( const Cylinder& cyl) const
{
  return RKLocalFieldProvider( *theVolume, cyl);
}

PropagationDirection RKPropagatorInS::invertDirection( PropagationDirection dir) const
{
  if (dir == anyDirection) return dir;
  return ( dir == alongMomentum ? oppositeToMomentum : alongMomentum);
}

Basic3DVector<double> RKPropagatorInS::rkPosition( const GlobalPoint& pos) const
{
  if (theVolume != nullptr) return theVolume->toLocal( pos).basicVector();
  else return pos.basicVector();
}

Basic3DVector<double> RKPropagatorInS::rkMomentum( const GlobalVector& mom) const
{
  if (theVolume != nullptr) return theVolume->toLocal( mom).basicVector();
  else return mom.basicVector();
}

GlobalPoint RKPropagatorInS::globalPosition( const Basic3DVector<float>& pos) const
{
  if (theVolume != nullptr) return theVolume->toGlobal( LocalPoint(pos));
  else return GlobalPoint(pos);
}

GlobalVector RKPropagatorInS::globalMomentum( const Basic3DVector<float>& mom) const

{
  if (theVolume != nullptr) return theVolume->toGlobal( LocalVector(mom));
  else return GlobalVector(mom);
}

GlobalTrajectoryParameters 
RKPropagatorInS::gtpFromVolumeLocal( const CartesianStateAdaptor& state, 
                     TrackCharge charge) const
{
  return GlobalTrajectoryParameters( globalPosition(state.position()), 
                     globalMomentum(state.momentum()), 
                     charge, theVolume);
}