TrkHelixUtils.cxx

Go to the documentation of this file.

//--------------------------------------------------------------------------
// File and Version Information:
//      $Id: TrkHelixUtils.cxx,v 1.4 2007/12/07 07:04:14 zhangy Exp $
//
// Description:
//
//
// Environment:
//      Software developed for the BaBar Detector at the SLAC B-Factory.
//
// Author(s): Steve Schaffner, code stolen from TrkParms class
//
//------------------------------------------------------------------------
// #include "BaBar/BaBar.h"
#include "TrkBase/TrkHelixUtils.h"
// #include "PatBField/BField.h"
#include "CLHEP/Geometry/Point3D.h"
#include "CLHEP/Matrix/Matrix.h"
#include "CLHEP/Matrix/SymMatrix.h"
#include "CLHEP/Vector/ThreeVector.h"
#include "MdcGeom/BesAngle.h"
#include "MdcGeom/Constants.h"
#include "MdcRecoUtil/BesPointErr.h"
#include "MdcRecoUtil/BesVectorErr.h"
#include "MdcRecoUtil/DifArray.h"
#include "MdcRecoUtil/DifNumber.h"
#include "TrkBase/NeutParams.h"
#include "TrkBase/TrkExchangePar.h"
#include <math.h>
const double pi = Constants::pi;
 
using MdcPatRec::BField;
 
//------------------------------------------------------------------------
HepMatrix TrkHelixUtils::jacobianExtrapolate( const TrkExchangePar& par, double fltNew ) {
  //------------------------------------------------------------------------
 
  //----------------------------------------------------------------------
  //      Compute and return the jacobian that takes the covariance matrix
  //    from fltOld to fltNew
  //
  //      "fltLen" -- the signed 3-d pathlength a particle travels
  //                     along the orbit starting from the point on the orbit
  //                     close to the origin.
  //
  //      Each element in this matrix is a partial derivative of an orbit
  //      parameter at some reference point to an orbit parameter at
  //      the fit point.  What is kept fixed in taking these partial
  //      derivatives is that the orbit parameters are those at the point
  //      on the orbit that is closest in the x-y plane to the reference point.
  //      This transform matrix has the property that transform(-arclength)
  //      is the inverse of transform(arclength).
  //      (repaired by Gerry Lynch, I think -- sfs)
  //----------------------------------------------------------------------
 
  HepMatrix transform( 5, 5, 0 );
 
  double tanDip = par.tanDip();
  double omega  = par.omega();
  // Convert to 2-d arclength
  double darc    = fltNew / sqrt( 1. + tanDip * tanDip );
  double dphi    = omega * darc;
  double cosDphi = cos( dphi );
  double sinDphi = sin( dphi );
 
  // partials of d0
 
  transform[0][0] = cosDphi;
  transform[0][1] = sinDphi / omega;
  transform[0][2] = ( 1.0 - cosDphi ) / ( omega * omega );
 
  // partials of phi0
 
  transform[1][0] = -sinDphi * omega;
  transform[1][1] = cosDphi;
  transform[1][2] = sinDphi / omega;
 
  // partials of omega
 
  transform[2][2] = 1.0;
 
  // partials of z0
 
  transform[3][0] = -tanDip * sinDphi;
  transform[3][1] = -tanDip * ( 1.0 - cosDphi ) / omega;
  transform[3][2] = -tanDip * ( dphi - sinDphi ) / ( omega * omega );
  transform[3][3] = 1.;
  transform[3][4] = darc;
 
  // partials of tanDip
 
  transform[4][4] = 1.;
 
  return transform;
}
 
//----------------------------------------------------------------------
HepSymMatrix TrkHelixUtils::extrapolateCov( TrkExchangePar& par, double fltNew ) {
  //----------------------------------------------------------------------
 
  return par.covariance().similarity( jacobianExtrapolate( par, fltNew ) );
}
 
//----------------------------------------------------------------------
TrkExchangePar TrkHelixUtils::helixFromMom( const HepPoint3D& pos, const Hep3Vector& pmom,
                                            double sign, const BField& fieldmap ) {
  //----------------------------------------------------------------------
  // As documented in
  // R.~Luchsinger and C.~Grab, Comp.~Phys.~Comm.~{\bf 76}, 263-280 (1993).
  // Equation 14 on page 270.
  // Note: We use the opposite sign for d0.
  // We use tandip and not the dip angle itself.
 
  register double  phip, gamma, rho, pt;
  static HepVector pars( 5 );
 
  double px = pmom.x();
  double py = pmom.y();
  pt        = sqrt( px * px + py * py );
 
  if ( pt < 1.e-7 ) pt = 1.e-7;                                 // hack to avoid pt=0 tracks
  if ( fabs( px ) < 1.e-7 ) px = ( px < 0.0 ) ? -1.e-7 : 1.e-7; // hack to avoid pt=0 tracks
 
  double Bval = fieldmap.bFieldNominal();
 
  pars( 3 ) = -BField::cmTeslaToGeVc * Bval * sign / pt; // omega
  pars( 5 ) = pmom.z() / pt;                             // tandip
 
  rho            = 1. / pars( 3 );
  phip           = atan2( py, px );
  double cosphip = cos( phip );
  double sinphip = sin( phip );
 
  gamma =
      atan( ( pos.x() * cosphip + pos.y() * sinphip ) /
            ( pos.x() * sinphip - pos.y() * cosphip - rho ) ); // NOTE: do NOT use atan2 here!
 
  pars( 2 ) = BesAngle( phip + gamma ).rad();                                       // phi0
  pars( 1 ) = ( rho + pos.y() * cosphip - pos.x() * sinphip ) / cos( gamma ) - rho; // d0
  pars( 4 ) = pos.z() + rho * gamma * pars( 5 );                                    // z0
 
  return TrkExchangePar( pars );
}
 
//----------------------------------------------------------------------
TrkExchangePar TrkHelixUtils::helixFromMomErr( const BesPointErr&  pos,
                                               const BesVectorErr& pmom, const HepMatrix& cxp,
                                               double sign, const BField& fieldmap ) {
  //----------------------------------------------------------------------
 
  DifNumber xDF( pos.x(), 1, 6 ), yDF( pos.y(), 2, 6 ), zDF( pos.z(), 3, 6 );
  DifNumber pxDF( pmom.x(), 4, 6 );
  DifNumber pyDF( pmom.y(), 5, 6 );
  DifNumber pzDF( pmom.z(), 6, 6 );
 
  static DifNumber phip, cosphip, sinphip, gamma;
  static DifArray  pars( 5, 6 );
 
  DifNumber invpt = pxDF;
  invpt *= pxDF;
  invpt += pyDF * pyDF;
  invpt = sqrt( invpt );
 
  if ( invpt < 1.e-7 ) invpt = 1.e-7;                         // hack to avoid pt=0 tracks
  if ( fabs( pxDF ) < 1.e-7 ) pxDF = pxDF < 0 ? -1e-7 : 1e-7; // hack to avoid pt=0 tracks
  invpt = 1. / invpt;
 
  // omega
  double Bval = fieldmap.bFieldNominal();
  //  if (fabs(Bval) < 1.e-7) {  // hack to avoid B=0 tracks (very far Z)
  //    Bval = 1.e-7;
  //  }
 
  //  pars(3) = -BField::cmTeslaToGeVc*Bval*sign/pt;
  pars( 3 ) = invpt;
  pars( 3 ) *= sign;
  pars( 3 ) *= Bval;
  pars( 3 ) *= -BField::cmTeslaToGeVc;
 
  //  pars(5) = pzDF / pt;  //tandip
  pars( 5 ) = pzDF;
  pars( 5 ) *= invpt;
 
  DifNumber rho = 1. / pars[2];
  phip          = atan2( pyDF, pxDF );
  phip.cosAndSin( cosphip, sinphip );
 
  //  pars(1) = (rho + yDF*cosphip - xDF*sinphip)/cos(gamma) - rho;//d0
  pars( 1 ) = rho;
  pars( 1 ) += yDF * cosphip;
  pars( 1 ) -= xDF * sinphip; // continued below ...
 
  gamma =
      atan( ( xDF * cosphip + yDF * sinphip ) / -pars( 1 ) ); // NOTE: do NOT use atan2 here
 
  pars( 1 ) /= cos( gamma );
  pars( 1 ) -= rho;
 
  //  pars(2) = phip+gamma;  //phi0
  pars( 2 ) = phip;
  pars( 2 ) += gamma;
  //  pars(2).mod(0., Constants::twoPi);
 
  //  pars(4) = zDF + rho*gamma*pars[4];  //z0
  pars( 4 ) = pars[4]; // weird
  pars( 4 ) *= gamma;
  pars( 4 ) *= rho;
  pars( 4 ) += zDF;
 
  // Get error matrix for position and momentum
 
  static HepSymMatrix posandmomErr( 6 );
  static HepVector    parsVec( 5 );
 
  int i;
  for ( i = 1; i <= 3; ++i )
  {
    int j;
    for ( j = 1; j <= i; ++j )
    {
      // with "fast" access, row should be >= col
      posandmomErr.fast( i, j )         = pos.covMatrix().fast( i, j );
      posandmomErr.fast( i + 3, j + 3 ) = pmom.covMatrix().fast( i, j );
    }
    for ( j = 1; j <= 3; ++j ) { posandmomErr.fast( j + 3, i ) = cxp( i, j ); }
  }
  for ( i = 1; i <= 5; ++i )
  {
    // make the array of DifNums into a HepVector
    // (needed for TrkExchangePar init)
    parsVec( i ) = pars( i ).number();
  }
  // Now calculate error on the helix pars--the real calculation
 
  return TrkExchangePar( parsVec, posandmomErr.similarity( pars.jacobian() ) );
}
//----------------------------------------------------------------------
NeutParams TrkHelixUtils::lineFromMomErr( const BesPointErr& pos, const BesVectorErr& pmom,
                                          const HepMatrix& cxp, double sign,
                                          const BField& fieldmap ) {
  //----------------------------------------------------------------------
 
  DifNumber xDF( pos.x(), 1, 6 ), yDF( pos.y(), 2, 6 ), zDF( pos.z(), 3, 6 );
  DifNumber pxDF( pmom.x(), 4, 6 );
  DifNumber pyDF( pmom.y(), 5, 6 );
  DifNumber pzDF( pmom.z(), 6, 6 );
 
  static DifArray pars( 6, 6 );
 
  DifNumber pt = pxDF;
  pt *= pxDF;
  pt += pyDF * pyDF;
 
  if ( pt < 1.e-14 ) pt = 1.e-14;                             // hack to avoid pt=0 tracks
  if ( fabs( pxDF ) < 1.e-7 ) pxDF = pxDF < 0 ? -1e-7 : 1e-7; // hack to avoid pt=0 tracks
 
  //  pars(3) = sqrt(pt*pt+pzDF*pzDF); // Magnitude of p
  pars( 3 ) = pzDF;
  pars( 3 ) *= pzDF;
  pars( 3 ) += pt;
  pars( 3 ) = sqrt( pars( 3 ) );
 
  pt              = sqrt( pt );
  DifNumber invpt = 1. / pt;
 
  // Next lines modified by Eugenio Paoloni 19-XI-98
 
  //  DifNumber pvxDF=pxDF/pt; // versor along pt x
  DifNumber pvxDF = pxDF;
  pvxDF *= invpt;
 
  //  DifNumber pvyDF=pyDF/pt; // and y component
  DifNumber pvyDF = pyDF;
  pvyDF *= invpt;
 
  //  pars(5) = pzDF / pt;  //tandip
  pars( 5 ) = pzDF;
  pars( 5 ) *= invpt;
 
  pars( 2 ) = atan2( pyDF, pxDF ); // phi0
 
  //  pars(1) = yDF*pvxDF - xDF*pvyDF;//d0
  pars( 1 ) = yDF;
  pars( 1 ) *= pvxDF;
  pars( 1 ) -= xDF * pvyDF;
 
  //  pars(4) = zDF -pars(5)*(xDF*pvxDF+yDF*pvyDF) ;  //z0
  pars( 4 ) = xDF;
  pars( 4 ) *= pvxDF;
  pars( 4 ) += yDF * pvyDF;
 
  // insert this in the middle for speed
  //  pars(6) = (xDF*pvxDF+yDF*pvyDF)*pars(3)/pt;//s0
  pars( 6 ) = pars( 4 );
  pars( 6 ) *= pars( 3 );
  pars( 6 ) *= invpt;
 
  pars( 4 ) *= -pars( 5 );
  pars( 4 ) += zDF;
 
  // Get error matrix for position and momentum
 
  static HepSymMatrix posandmomErr( 6 );
  static HepVector    parsVec( 6 );
 
  int i;
  for ( i = 1; i <= 3; ++i )
  {
    int j;
    for ( j = 1; j <= i; ++j )
    {
      // with "fast" access, row should be >= col
      posandmomErr.fast( i, j )         = pos.covMatrix().fast( i, j );
      posandmomErr.fast( i + 3, j + 3 ) = pmom.covMatrix().fast( i, j );
    }
    for ( j = 1; j <= 3; ++j ) { posandmomErr.fast( j + 3, i ) = cxp( i, j ); }
  }
  for ( i = 1; i <= 6; ++i )
  {
    // make the array of DifNums into a HepVector
    // (needed for TrkExchangePar init)
    parsVec( i ) = pars( i ).number();
  }
  // Now calculate error on the helix pars--the real calculation
  return NeutParams( parsVec, posandmomErr.similarity( pars.jacobian() ) );
}
 
//------------------------------------------------------------------------
double TrkHelixUtils::fltToRad( const TrkExchangePar& hel, double rad ) {
  //------------------------------------------------------------------------
  double d0     = hel.d0();
  double omega  = hel.omega();
  double tanDip = hel.tanDip();
  // std::cout<< "omega  "<<omega << std::endl;
  //  GS To be able to use this with straight lines:
  //  if( fabs(omega) < 0.0001 ) omega=copysign(0.0001,omega);  // assume the pt= 1000 GeV
  if ( fabs( omega ) < 0.0001 ) omega = ( omega < 0.0 ) ? -0.0001 : 0.0001;
 
  double stuff = ( rad * rad - d0 * d0 ) / ( 1 + omega * d0 );
  if ( stuff <= 0.0 ) return 0.;
  // std::cout<< "in TrkHelixUtils::fltToRad  "<< stuff << std::endl;
  if ( omega == 0. ) return sqrt( stuff );
  double sinAngle = 0.5 * omega * sqrt( stuff );
  double dist2d   = 0;
  if ( sinAngle < -1.0 || sinAngle > 1.0 ) { dist2d = Constants::pi / fabs( omega ); }
  else { dist2d = 2. * asin( sinAngle ) / omega; }
  // std::cout<< "in TrkHelixUtils::fltToRad  dist2d "<< dist2d << std::endl;
  return dist2d * sqrt( 1. + tanDip * tanDip );
}