AnalyticalCurvilinearJacobian.cc

Go to the documentation of this file.

#include "TrackingTools/AnalyticalJacobians/interface/AnalyticalCurvilinearJacobian.h"
#include "TrackingTools/TrajectoryParametrization/interface/GlobalTrajectoryParameters.h"
#include <vdt/vdtMath.h>


AnalyticalCurvilinearJacobian::AnalyticalCurvilinearJacobian 
(const GlobalTrajectoryParameters& globalParameters,
 const GlobalPoint& x, 
 const GlobalVector& p, 
 const double& s) :  theJacobian(AlgebraicMatrixID())
{
  //
  // helix: calculate full jacobian
  //
  if ( s*s*fabs(globalParameters.transverseCurvature())>1.e-5 ) { 
    // GlobalPoint xStart = globalParameters.position();
    // GlobalVector h  = globalParameters.magneticFieldInInverseGeV(xStart);
    GlobalVector h  = globalParameters.magneticFieldInInverseGeV();
    computeFullJacobian(globalParameters,x,p,h,s);
  }
  //
  // straight line approximation, error in RPhi about 0.1um
  //
  else
    computeStraightLineJacobian(globalParameters,x,p,s);
   //dbg::dbg_trace(1,"ACJ1", globalParameters.vector(),x,p,s,theJacobian);
}


AnalyticalCurvilinearJacobian::AnalyticalCurvilinearJacobian
(const GlobalTrajectoryParameters& globalParameters,
 const GlobalPoint& x, 
 const GlobalVector& p, 
 const GlobalVector& h, // h is the magnetic Field in Inverse GeV
 const double& s) :  theJacobian(AlgebraicMatrixID())
{
  //
  // helix: calculate full jacobian
  //
  if ( s*s*fabs(globalParameters.transverseCurvature())>1.e-5 )
    computeFullJacobian(globalParameters,x,p,h,s);
  //
  // straight line approximation, error in RPhi about 0.1um
  //
  else
    computeStraightLineJacobian(globalParameters,x,p,s);
  
  //dbg::dbg_trace(1,"ACJ2", globalParameters.vector(),x,p,s,theJacobian);
}


#if defined(USE_SSEVECT) && !defined(TRPRFN_SCALAR)
#include "AnalyticalCurvilinearJacobianSSE.icc"
#elif defined(USE_EXTVECT) && !defined(TRPRFN_SCALAR)
#include "AnalyticalCurvilinearJacobianEXT.icc"

#else

void
AnalyticalCurvilinearJacobian::computeFullJacobian
(const GlobalTrajectoryParameters& globalParameters,
 const GlobalPoint& x, 
 const GlobalVector& p, 
 const GlobalVector& h, 
 const double& s)
{    
  //GlobalVector p1 = fts.momentum().unit();
  GlobalVector p1 = globalParameters.momentum().unit();
  GlobalVector p2 = p.unit();
  //GlobalPoint xStart = fts.position();
  GlobalPoint xStart = globalParameters.position();
  GlobalVector dx = xStart - x;
  //GlobalVector h  = MagneticField::inInverseGeV(xStart);
  // Martijn: field is now given as parameter.. GlobalVector h  = globalParameters.magneticFieldInInverseGeV(xStart);

  //double qbp = fts.signedInverseMomentum();
  double qbp = globalParameters.signedInverseMomentum();
  double absS = s;
  
  // calculate transport matrix
  // Origin: TRPRFN
  double t11 = p1.x(); double t12 = p1.y(); double t13 = p1.z();
  double t21 = p2.x(); double t22 = p2.y(); double t23 = p2.z();
  double cosl0 = p1.perp(); double cosl1 = 1./p2.perp();
  //AlgebraicMatrix a(5,5,1);
  // define average magnetic field and gradient 
  // at initial point - inlike TRPRFN
  GlobalVector hn = h.unit();
  double qp = -h.mag();
//   double q = -h.mag()*qbp;
  double q = qp*qbp;
  double theta = q*absS; double sint,cost;   vdt::fast_sincos(theta,sint,cost);
  double hn1 = hn.x(); double hn2 = hn.y(); double hn3 = hn.z();
  double dx1 = dx.x(); double dx2 = dx.y(); double dx3 = dx.z();
  double gamma = hn1*t21 + hn2*t22 + hn3*t23;
  double an1 = hn2*t23 - hn3*t22;
  double an2 = hn3*t21 - hn1*t23;
  double an3 = hn1*t22 - hn2*t21;
  double au = 1./sqrt(t11*t11 + t12*t12);
  double u11 = -au*t12; double u12 = au*t11;
  double v11 = -t13*u12; double v12 = t13*u11; double v13 = t11*u12 - t12*u11;
  au = 1./sqrt(t21*t21 + t22*t22);
  double u21 = -au*t22; double u22 = au*t21;
  double v21 = -t23*u22; double v22 = t23*u21; double v23 = t21*u22 - t22*u21;
  // now prepare the transport matrix
  // pp only needed in high-p case (WA)
//   double pp = 1./qbp;
// moved up (where -h.mag() is needed()
//   double qp = q*pp;
  double anv = -(hn1*u21 + hn2*u22          );
  double anu =  (hn1*v21 + hn2*v22 + hn3*v23);
  double omcost = 1. - cost; double tmsint = theta - sint;
  
  double hu1 =         - hn3*u12;
  double hu2 = hn3*u11;
  double hu3 = hn1*u12 - hn2*u11;
  
  double hv1 = hn2*v13 - hn3*v12;
  double hv2 = hn3*v11 - hn1*v13;
  double hv3 = hn1*v12 - hn2*v11;
  
  //   1/p - doesn't change since |p1| = |p2|
  theJacobian(0,0) = 1.;  for (auto i=1;i<5; ++i) theJacobian(0,i)=0.;  
  //   lambda
  
  theJacobian(1,0) = -qp*anv*(t21*dx1 + t22*dx2 + t23*dx3);
  
  theJacobian(1,1) = cost*(v11*v21 + v12*v22 + v13*v23) +
                      sint*(hv1*v21 + hv2*v22 + hv3*v23) +
                    omcost*(hn1*v11 + hn2*v12 + hn3*v13) *
                           (hn1*v21 + hn2*v22 + hn3*v23) +
                anv*(-sint*(v11*t21 + v12*t22 + v13*t23) +
                    omcost*(v11*an1 + v12*an2 + v13*an3) -
              tmsint*gamma*(hn1*v11 + hn2*v12 + hn3*v13) );

  theJacobian(1,2) = cost*(u11*v21 + u12*v22          ) +
                      sint*(hu1*v21 + hu2*v22 + hu3*v23) +
                    omcost*(hn1*u11 + hn2*u12          ) *
                           (hn1*v21 + hn2*v22 + hn3*v23) +
                anv*(-sint*(u11*t21 + u12*t22          ) +
                    omcost*(u11*an1 + u12*an2          ) -
              tmsint*gamma*(hn1*u11 + hn2*u12          ) );
  theJacobian(1,2) *= cosl0;

  theJacobian(1,3) = -q*anv*(u11*t21 + u12*t22          );

  theJacobian(1,4) = -q*anv*(v11*t21 + v12*t22 + v13*t23);

  //   phi

  theJacobian(2,0) = -qp*anu*(t21*dx1 + t22*dx2 + t23*dx3)*cosl1;

  theJacobian(2,1) = cost*(v11*u21 + v12*u22          ) +
                      sint*(hv1*u21 + hv2*u22          ) +
                    omcost*(hn1*v11 + hn2*v12 + hn3*v13) *
                           (hn1*u21 + hn2*u22          ) +
                anu*(-sint*(v11*t21 + v12*t22 + v13*t23) +
                    omcost*(v11*an1 + v12*an2 + v13*an3) -
              tmsint*gamma*(hn1*v11 + hn2*v12 + hn3*v13) );
  theJacobian(2,1) *= cosl1;

  theJacobian(2,2) = cost*(u11*u21 + u12*u22          ) +
                      sint*(hu1*u21 + hu2*u22          ) +
                    omcost*(hn1*u11 + hn2*u12          ) *
                           (hn1*u21 + hn2*u22          ) +
                anu*(-sint*(u11*t21 + u12*t22          ) +
                    omcost*(u11*an1 + u12*an2          ) -
              tmsint*gamma*(hn1*u11 + hn2*u12          ) );
  theJacobian(2,2) *= cosl1*cosl0;

  theJacobian(2,3) = -q*anu*(u11*t21 + u12*t22          )*cosl1;

  theJacobian(2,4) = -q*anu*(v11*t21 + v12*t22 + v13*t23)*cosl1;

  //   yt

  //double cutCriterion = abs(s/fts.momentum().mag());
  double cutCriterion = fabs(s/globalParameters.momentum().mag());
  const double limit = 5.; // valid for propagations with effectively float precision

  if (cutCriterion > limit) {
    double pp = 1./qbp;
    theJacobian(3,0) = pp*(u21*dx1 + u22*dx2            );
    theJacobian(4,0) = pp*(v21*dx1 + v22*dx2 + v23*dx3);
  }
  else {
    double hp11 = hn2*t13 - hn3*t12;
    double hp12 = hn3*t11 - hn1*t13;
    double hp13 = hn1*t12 - hn2*t11;
    double temp1 = hp11*u21 + hp12*u22;
    double s2 = s*s;
    double secondOrder41 = 0.5 * qp * temp1 * s2;
    double ghnmp1 = gamma*hn1 - t11;
    double ghnmp2 = gamma*hn2 - t12;
    double ghnmp3 = gamma*hn3 - t13;
    double temp2 = ghnmp1*u21 + ghnmp2*u22;
    double s3 = s2 * s;
    double s4 = s3 * s;
    double h1 = h.mag();
    double h2 = h1 * h1;
    double h3 = h2 * h1;
    double qbp2 = qbp * qbp;
    //                           s*qp*s* (qp*s *qbp)
    double thirdOrder41 = 1./3 * h2 * s3 * qbp * temp2;
    //                           -qp * s * qbp  * above
    double fourthOrder41 = 1./8 * h3 * s4 * qbp2 * temp1;
    theJacobian(3,0) = secondOrder41 + (thirdOrder41 + fourthOrder41);

    double temp3 = hp11*v21 + hp12*v22 + hp13*v23;
    double secondOrder51 = 0.5 * qp * temp3 * s2;
    double temp4 = ghnmp1*v21 + ghnmp2*v22 + ghnmp3*v23;
    double thirdOrder51 = 1./3 * h2 * s3 * qbp * temp4;
    double fourthOrder51 = 1./8 * h3 * s4 * qbp2 * temp3;
    theJacobian(4,0) = secondOrder51 + (thirdOrder51 + fourthOrder51);
  }

  theJacobian(3,1) = (sint*(v11*u21 + v12*u22          ) +
                     omcost*(hv1*u21 + hv2*u22          ) +
                     tmsint*(hn1*u21 + hn2*u22          ) *
                            (hn1*v11 + hn2*v12 + hn3*v13))/q;

  theJacobian(3,2) = (sint*(u11*u21 + u12*u22          ) +
                     omcost*(hu1*u21 + hu2*u22          ) +
                     tmsint*(hn1*u21 + hn2*u22          ) *
                            (hn1*u11 + hn2*u12          ))*cosl0/q;

  theJacobian(3,3) = (u11*u21 + u12*u22          );
  
  theJacobian(3,4) = (v11*u21 + v12*u22          );

  //   zt

  theJacobian(4,1) = (sint*(v11*v21 + v12*v22 + v13*v23) +
                     omcost*(hv1*v21 + hv2*v22 + hv3*v23) +
                     tmsint*(hn1*v21 + hn2*v22 + hn3*v23) *
                            (hn1*v11 + hn2*v12 + hn3*v13))/q;

  theJacobian(4,2) = (sint*(u11*v21 + u12*v22          ) +
                     omcost*(hu1*v21 + hu2*v22 + hu3*v23) +
                     tmsint*(hn1*v21 + hn2*v22 + hn3*v23) *
                    (hn1*u11 + hn2*u12          ))*cosl0/q;

  theJacobian(4,3) = (u11*v21 + u12*v22          );

  theJacobian(4,4) = (v11*v21 + v12*v22 + v13*v23);
  // end of TRPRFN
}

#endif

void AnalyticalCurvilinearJacobian::computeInfinitesimalJacobian 
(const GlobalTrajectoryParameters& globalParameters,
 const GlobalPoint&, 
 const GlobalVector& p, 
 const GlobalVector& h, 
 const double& s) {
  /*
   * origin  TRPROP
   *
   C *** ERROR PROPAGATION ALONG A PARTICLE TRAJECTORY IN A MAGNETIC FIELD
   C     ROUTINE ASSUMES THAT IN THE INTERVAL (X1,X2) THE QUANTITIES 1/P
   C     AND (HX,HY,HZ) ARE RATHER CONSTANT. DELTA(PHI) MUST NOT BE TOO LARGE
   C
   C     Authors: A. Haas and W. Wittek
   C
   
  */
  
  
  double qbp = globalParameters.signedInverseMomentum();
  double absS = s;
  
  // average momentum
  GlobalVector tn = (globalParameters.momentum()+p).unit(); 
  double sinl = tn.z(); 
  double cosl = std::sqrt(1.-sinl*sinl); 
  double cosl1 = 1./cosl;
  double tgl=sinl*cosl1;
  double sinp = tn.y()*cosl1;
  double cosp = tn.x()*cosl1;

  // define average magnetic field and gradient 
  // at initial point - inlike TRPROP
  double b0= h.x()*cosp+h.y()*sinp;
  double b2=-h.x()*sinp+h.y()*cosp;
  double b3=-b0*sinl+h.z()*cosl;

  theJacobian = AlgebraicMatrixID();

  theJacobian(3,2)=absS*cosl;
  theJacobian(4,1)=absS;


  theJacobian(1,0) =  absS*b2;
  //if ( qbp<0) theJacobian(1,0) = -theJacobian(1,0);
  theJacobian(1,2) = -b0*(absS*qbp);
  theJacobian(1,3) =  b3*(b2*qbp*(absS*qbp));
  theJacobian(1,4) = -b2*(b2*qbp*(absS*qbp));
  
  theJacobian(2,0) = -absS*b3*cosl1;
  // if ( qbp<0) theJacobian(2,0) = -theJacobian(2,0);
  theJacobian(2,1) = b0*(absS*qbp)*cosl1*cosl1;
  theJacobian(2,2) = 1.+tgl*b2*(absS*qbp);
  theJacobian(2,3) = -b3*(b3*qbp*(absS*qbp)*cosl1);
  theJacobian(2,4) =  b2*(b3*qbp*(absS*qbp)*cosl1);
  
  theJacobian(3,4) = -b3*tgl*(absS*qbp);
  theJacobian(4,3) =  b3*tgl*(absS*qbp);


}

void
AnalyticalCurvilinearJacobian::computeStraightLineJacobian
(const GlobalTrajectoryParameters& globalParameters,
 const GlobalPoint&, const GlobalVector&, const double& s)
{
  //
  // matrix: elements =1 on diagonal and =0 are already set
  // in initialisation
  //
  theJacobian = AlgebraicMatrixID();
  GlobalVector p1 = globalParameters.momentum().unit();
  double cosl0 = p1.perp();
  theJacobian(3,2) = cosl0 * s;
  theJacobian(4,1) = s;
}