AnalyticalJacobians/src/AnalyticalCurvilinearJacobian.cc

0001 #include "TrackingTools/AnalyticalJacobians/interface/AnalyticalCurvilinearJacobian.h"
0002 #include "TrackingTools/TrajectoryParametrization/interface/GlobalTrajectoryParameters.h"
0003 #include <vdt/vdtMath.h>
0004
0005 AnalyticalCurvilinearJacobian::AnalyticalCurvilinearJacobian(const GlobalTrajectoryParameters& globalParameters,
0006                                                              const GlobalPoint& x,
0007                                                              const GlobalVector& p,
0008                                                              const double& s)
0009     : theJacobian(AlgebraicMatrixID()) {
0010   //
0011   // helix: calculate full jacobian
0012   //
0013   if (s * s * fabs(globalParameters.transverseCurvature()) > 1.e-5) {
0014     // GlobalPoint xStart = globalParameters.position();
0015     // GlobalVector h  = globalParameters.magneticFieldInInverseGeV(xStart);
0016     GlobalVector h = globalParameters.magneticFieldInInverseGeV();
0017     computeFullJacobian(globalParameters, x, p, h, s);
0018   }
0019   //
0020   // straight line approximation, error in RPhi about 0.1um
0021   //
0022   else
0023     computeStraightLineJacobian(globalParameters, x, p, s);
0024   //dbg::dbg_trace(1,"ACJ1", globalParameters.vector(),x,p,s,theJacobian);
0025 }
0026
0027 AnalyticalCurvilinearJacobian::AnalyticalCurvilinearJacobian(
0028     const GlobalTrajectoryParameters& globalParameters,
0029     const GlobalPoint& x,
0030     const GlobalVector& p,
0031     const GlobalVector& h,  // h is the magnetic Field in Inverse GeV
0032     const double& s)
0033     : theJacobian(AlgebraicMatrixID()) {
0034   //
0035   // helix: calculate full jacobian
0036   //
0037   if (s * s * fabs(globalParameters.transverseCurvature()) > 1.e-5)
0038     computeFullJacobian(globalParameters, x, p, h, s);
0039   //
0040   // straight line approximation, error in RPhi about 0.1um
0041   //
0042   else
0043     computeStraightLineJacobian(globalParameters, x, p, s);
0044
0045   //dbg::dbg_trace(1,"ACJ2", globalParameters.vector(),x,p,s,theJacobian);
0046 }
0047
0048 #if defined(USE_SSEVECT) && !defined(TRPRFN_SCALAR)
0049 #include "AnalyticalCurvilinearJacobianSSE.icc"
0050 #elif defined(USE_EXTVECT) && !defined(TRPRFN_SCALAR)
0051 #include "AnalyticalCurvilinearJacobianEXT.icc"
0052
0053 #else
0054
0055 void AnalyticalCurvilinearJacobian::computeFullJacobian(const GlobalTrajectoryParameters& globalParameters,
0056                                                         const GlobalPoint& x,
0057                                                         const GlobalVector& p,
0058                                                         const GlobalVector& h,
0059                                                         const double& s) {
0060   //GlobalVector p1 = fts.momentum().unit();
0061   GlobalVector p1 = globalParameters.momentum().unit();
0062   GlobalVector p2 = p.unit();
0063   //GlobalPoint xStart = fts.position();
0064   GlobalPoint xStart = globalParameters.position();
0065   GlobalVector dx = xStart - x;
0066   //GlobalVector h  = MagneticField::inInverseGeV(xStart);
0067   // Martijn: field is now given as parameter.. GlobalVector h  = globalParameters.magneticFieldInInverseGeV(xStart);
0068
0069   //double qbp = fts.signedInverseMomentum();
0070   double qbp = globalParameters.signedInverseMomentum();
0071   double absS = s;
0072
0073   // calculate transport matrix
0074   // Origin: TRPRFN
0075   double t11 = p1.x();
0076   double t12 = p1.y();
0077   double t13 = p1.z();
0078   double t21 = p2.x();
0079   double t22 = p2.y();
0080   double t23 = p2.z();
0081   double cosl0 = p1.perp();
0082   double cosl1 = 1. / p2.perp();
0083   //AlgebraicMatrix a(5,5,1);
0084   // define average magnetic field and gradient
0085   // at initial point - inlike TRPRFN
0086   GlobalVector hn = h.unit();
0087   double qp = -h.mag();
0088   //   double q = -h.mag()*qbp;
0089   double q = qp * qbp;
0090   double theta = q * absS;
0091   double sint, cost;
0092   vdt::fast_sincos(theta, sint, cost);
0093   double hn1 = hn.x();
0094   double hn2 = hn.y();
0095   double hn3 = hn.z();
0096   double dx1 = dx.x();
0097   double dx2 = dx.y();
0098   double dx3 = dx.z();
0099   double gamma = hn1 * t21 + hn2 * t22 + hn3 * t23;
0100   double an1 = hn2 * t23 - hn3 * t22;
0101   double an2 = hn3 * t21 - hn1 * t23;
0102   double an3 = hn1 * t22 - hn2 * t21;
0103   double au = 1. / sqrt(t11 * t11 + t12 * t12);
0104   double u11 = -au * t12;
0105   double u12 = au * t11;
0106   double v11 = -t13 * u12;
0107   double v12 = t13 * u11;
0108   double v13 = t11 * u12 - t12 * u11;
0109   au = 1. / sqrt(t21 * t21 + t22 * t22);
0110   double u21 = -au * t22;
0111   double u22 = au * t21;
0112   double v21 = -t23 * u22;
0113   double v22 = t23 * u21;
0114   double v23 = t21 * u22 - t22 * u21;
0115   // now prepare the transport matrix
0116   // pp only needed in high-p case (WA)
0117   //   double pp = 1./qbp;
0118   ////    double pp = fts.momentum().mag();
0119   // moved up (where -h.mag() is needed()
0120   //   double qp = q*pp;
0121   double anv = -(hn1 * u21 + hn2 * u22);
0122   double anu = (hn1 * v21 + hn2 * v22 + hn3 * v23);
0123   double omcost = 1. - cost;
0124   double tmsint = theta - sint;
0125
0126   double hu1 = -hn3 * u12;
0127   double hu2 = hn3 * u11;
0128   double hu3 = hn1 * u12 - hn2 * u11;
0129
0130   double hv1 = hn2 * v13 - hn3 * v12;
0131   double hv2 = hn3 * v11 - hn1 * v13;
0132   double hv3 = hn1 * v12 - hn2 * v11;
0133
0134   //   1/p - doesn't change since |p1| = |p2|
0135   theJacobian(0, 0) = 1.;
0136   for (auto i = 1; i < 5; ++i)
0137     theJacobian(0, i) = 0.;
0138   //   lambda
0139
0140   theJacobian(1, 0) = -qp * anv * (t21 * dx1 + t22 * dx2 + t23 * dx3);
0141
0142   theJacobian(1, 1) =
0143       cost * (v11 * v21 + v12 * v22 + v13 * v23) + sint * (hv1 * v21 + hv2 * v22 + hv3 * v23) +
0144       omcost * (hn1 * v11 + hn2 * v12 + hn3 * v13) * (hn1 * v21 + hn2 * v22 + hn3 * v23) +
0145       anv * (-sint * (v11 * t21 + v12 * t22 + v13 * t23) + omcost * (v11 * an1 + v12 * an2 + v13 * an3) -
0146              tmsint * gamma * (hn1 * v11 + hn2 * v12 + hn3 * v13));
0147
0148   theJacobian(1, 2) = cost * (u11 * v21 + u12 * v22) + sint * (hu1 * v21 + hu2 * v22 + hu3 * v23) +
0149                       omcost * (hn1 * u11 + hn2 * u12) * (hn1 * v21 + hn2 * v22 + hn3 * v23) +
0150                       anv * (-sint * (u11 * t21 + u12 * t22) + omcost * (u11 * an1 + u12 * an2) -
0151                              tmsint * gamma * (hn1 * u11 + hn2 * u12));
0152   theJacobian(1, 2) *= cosl0;
0153
0154   theJacobian(1, 3) = -q * anv * (u11 * t21 + u12 * t22);
0155
0156   theJacobian(1, 4) = -q * anv * (v11 * t21 + v12 * t22 + v13 * t23);
0157
0158   //   phi
0159
0160   theJacobian(2, 0) = -qp * anu * (t21 * dx1 + t22 * dx2 + t23 * dx3) * cosl1;
0161
0162   theJacobian(2, 1) =
0163       cost * (v11 * u21 + v12 * u22) + sint * (hv1 * u21 + hv2 * u22) +
0164       omcost * (hn1 * v11 + hn2 * v12 + hn3 * v13) * (hn1 * u21 + hn2 * u22) +
0165       anu * (-sint * (v11 * t21 + v12 * t22 + v13 * t23) + omcost * (v11 * an1 + v12 * an2 + v13 * an3) -
0166              tmsint * gamma * (hn1 * v11 + hn2 * v12 + hn3 * v13));
0167   theJacobian(2, 1) *= cosl1;
0168
0169   theJacobian(2, 2) = cost * (u11 * u21 + u12 * u22) + sint * (hu1 * u21 + hu2 * u22) +
0170                       omcost * (hn1 * u11 + hn2 * u12) * (hn1 * u21 + hn2 * u22) +
0171                       anu * (-sint * (u11 * t21 + u12 * t22) + omcost * (u11 * an1 + u12 * an2) -
0172                              tmsint * gamma * (hn1 * u11 + hn2 * u12));
0173   theJacobian(2, 2) *= cosl1 * cosl0;
0174
0175   theJacobian(2, 3) = -q * anu * (u11 * t21 + u12 * t22) * cosl1;
0176
0177   theJacobian(2, 4) = -q * anu * (v11 * t21 + v12 * t22 + v13 * t23) * cosl1;
0178
0179   //   yt
0180
0181   //double cutCriterion = abs(s/fts.momentum().mag());
0182   double cutCriterion = fabs(s / globalParameters.momentum().mag());
0183   const double limit = 5.;  // valid for propagations with effectively float precision
0184
0185   if (cutCriterion > limit) {
0186     double pp = 1. / qbp;
0187     theJacobian(3, 0) = pp * (u21 * dx1 + u22 * dx2);
0188     theJacobian(4, 0) = pp * (v21 * dx1 + v22 * dx2 + v23 * dx3);
0189   } else {
0190     double hp11 = hn2 * t13 - hn3 * t12;
0191     double hp12 = hn3 * t11 - hn1 * t13;
0192     double hp13 = hn1 * t12 - hn2 * t11;
0193     double temp1 = hp11 * u21 + hp12 * u22;
0194     double s2 = s * s;
0195     double secondOrder41 = 0.5 * qp * temp1 * s2;
0196     double ghnmp1 = gamma * hn1 - t11;
0197     double ghnmp2 = gamma * hn2 - t12;
0198     double ghnmp3 = gamma * hn3 - t13;
0199     double temp2 = ghnmp1 * u21 + ghnmp2 * u22;
0200     double s3 = s2 * s;
0201     double s4 = s3 * s;
0202     double h1 = h.mag();
0203     double h2 = h1 * h1;
0204     double h3 = h2 * h1;
0205     double qbp2 = qbp * qbp;
0206     //                           s*qp*s* (qp*s *qbp)
0207     double thirdOrder41 = 1. / 3 * h2 * s3 * qbp * temp2;
0208     //                           -qp * s * qbp  * above
0209     double fourthOrder41 = 1. / 8 * h3 * s4 * qbp2 * temp1;
0210     theJacobian(3, 0) = secondOrder41 + (thirdOrder41 + fourthOrder41);
0211
0212     double temp3 = hp11 * v21 + hp12 * v22 + hp13 * v23;
0213     double secondOrder51 = 0.5 * qp * temp3 * s2;
0214     double temp4 = ghnmp1 * v21 + ghnmp2 * v22 + ghnmp3 * v23;
0215     double thirdOrder51 = 1. / 3 * h2 * s3 * qbp * temp4;
0216     double fourthOrder51 = 1. / 8 * h3 * s4 * qbp2 * temp3;
0217     theJacobian(4, 0) = secondOrder51 + (thirdOrder51 + fourthOrder51);
0218   }
0219
0220   theJacobian(3, 1) = (sint * (v11 * u21 + v12 * u22) + omcost * (hv1 * u21 + hv2 * u22) +
0221                        tmsint * (hn1 * u21 + hn2 * u22) * (hn1 * v11 + hn2 * v12 + hn3 * v13)) /
0222                       q;
0223
0224   theJacobian(3, 2) = (sint * (u11 * u21 + u12 * u22) + omcost * (hu1 * u21 + hu2 * u22) +
0225                        tmsint * (hn1 * u21 + hn2 * u22) * (hn1 * u11 + hn2 * u12)) *
0226                       cosl0 / q;
0227
0228   theJacobian(3, 3) = (u11 * u21 + u12 * u22);
0229
0230   theJacobian(3, 4) = (v11 * u21 + v12 * u22);
0231
0232   //   zt
0233
0234   theJacobian(4, 1) = (sint * (v11 * v21 + v12 * v22 + v13 * v23) + omcost * (hv1 * v21 + hv2 * v22 + hv3 * v23) +
0235                        tmsint * (hn1 * v21 + hn2 * v22 + hn3 * v23) * (hn1 * v11 + hn2 * v12 + hn3 * v13)) /
0236                       q;
0237
0238   theJacobian(4, 2) = (sint * (u11 * v21 + u12 * v22) + omcost * (hu1 * v21 + hu2 * v22 + hu3 * v23) +
0239                        tmsint * (hn1 * v21 + hn2 * v22 + hn3 * v23) * (hn1 * u11 + hn2 * u12)) *
0240                       cosl0 / q;
0241
0242   theJacobian(4, 3) = (u11 * v21 + u12 * v22);
0243
0244   theJacobian(4, 4) = (v11 * v21 + v12 * v22 + v13 * v23);
0245   // end of TRPRFN
0246 }
0247
0248 #endif
0249
0250 void AnalyticalCurvilinearJacobian::computeInfinitesimalJacobian(const GlobalTrajectoryParameters& globalParameters,
0251                                                                  const GlobalPoint&,
0252                                                                  const GlobalVector& p,
0253                                                                  const GlobalVector& h,
0254                                                                  const double& s) {
0255   /*
0256    * origin  TRPROP
0257    *
0258    C *** ERROR PROPAGATION ALONG A PARTICLE TRAJECTORY IN A MAGNETIC FIELD
0259    C     ROUTINE ASSUMES THAT IN THE INTERVAL (X1,X2) THE QUANTITIES 1/P
0260    C     AND (HX,HY,HZ) ARE RATHER CONSTANT. DELTA(PHI) MUST NOT BE TOO LARGE
0261    C
0262    C     Authors: A. Haas and W. Wittek
0263    C
0264
0265   */
0266
0267   double qbp = globalParameters.signedInverseMomentum();
0268   double absS = s;
0269
0270   // average momentum
0271   GlobalVector tn = (globalParameters.momentum() + p).unit();
0272   double sinl = tn.z();
0273   double cosl = std::sqrt(1. - sinl * sinl);
0274   double cosl1 = 1. / cosl;
0275   double tgl = sinl * cosl1;
0276   double sinp = tn.y() * cosl1;
0277   double cosp = tn.x() * cosl1;
0278
0279   // define average magnetic field and gradient
0280   // at initial point - inlike TRPROP
0281   double b0 = h.x() * cosp + h.y() * sinp;
0282   double b2 = -h.x() * sinp + h.y() * cosp;
0283   double b3 = -b0 * sinl + h.z() * cosl;
0284
0285   theJacobian = AlgebraicMatrixID();
0286
0287   theJacobian(3, 2) = absS * cosl;
0288   theJacobian(4, 1) = absS;
0289
0290   theJacobian(1, 0) = absS * b2;
0291   //if ( qbp<0) theJacobian(1,0) = -theJacobian(1,0);
0292   theJacobian(1, 2) = -b0 * (absS * qbp);
0293   theJacobian(1, 3) = b3 * (b2 * qbp * (absS * qbp));
0294   theJacobian(1, 4) = -b2 * (b2 * qbp * (absS * qbp));
0295
0296   theJacobian(2, 0) = -absS * b3 * cosl1;
0297   // if ( qbp<0) theJacobian(2,0) = -theJacobian(2,0);
0298   theJacobian(2, 1) = b0 * (absS * qbp) * cosl1 * cosl1;
0299   theJacobian(2, 2) = 1. + tgl * b2 * (absS * qbp);
0300   theJacobian(2, 3) = -b3 * (b3 * qbp * (absS * qbp) * cosl1);
0301   theJacobian(2, 4) = b2 * (b3 * qbp * (absS * qbp) * cosl1);
0302
0303   theJacobian(3, 4) = -b3 * tgl * (absS * qbp);
0304   theJacobian(4, 3) = b3 * tgl * (absS * qbp);
0305 }
0306
0307 void AnalyticalCurvilinearJacobian::computeStraightLineJacobian(const GlobalTrajectoryParameters& globalParameters,
0308                                                                 const GlobalPoint&,
0309                                                                 const GlobalVector&,
0310                                                                 const double& s) {
0311   //
0312   // matrix: elements =1 on diagonal and =0 are already set
0313   // in initialisation
0314   //
0315   theJacobian = AlgebraicMatrixID();
0316   GlobalVector p1 = globalParameters.momentum().unit();
0317   double cosl0 = p1.perp();
0318   theJacobian(3, 2) = cosl0 * s;
0319   theJacobian(4, 1) = s;
0320 }