AnalyticalJacobians/src/AnalyticalCurvilinearJacobianSSE.icc

0001 void AnalyticalCurvilinearJacobian::computeFullJacobian(  //NOLINT(misc-definitions-in-headers)
0002     const GlobalTrajectoryParameters& globalParameters,
0003     const GlobalPoint& x,
0004     const GlobalVector& p,
0005     const GlobalVector& h,
0006     const double& s) {
0007   using mathSSE::Vec2D;
0008   using mathSSE::Vec3D;
0009   using mathSSE::Vec4D;
0010
0011   //GlobalVector p1 = fts.momentum().unit();
0012   GlobalVector p1 = globalParameters.momentum().unit();
0013   GlobalVector p2 = p.unit();
0014   //GlobalPoint xStart = fts.position();
0015   GlobalPoint xStart = globalParameters.position();
0016   GlobalVector dxf = xStart - x;
0017   //GlobalVector h  = MagneticField::inInverseGeV(xStart);
0018   // Martijn: field is now given as parameter.. GlobalVector h  = globalParameters.magneticFieldInInverseGeV(xStart);
0019
0020   //double qbp = fts.signedInverseMomentum();
0021   double qbp = globalParameters.signedInverseMomentum();
0022   double absS = s;
0023
0024   // calculate transport matrix
0025   // Origin: TRPRFN
0026   double cosl0 = p1.perp();
0027   double cosl1 = 1. / p2.perp();
0028
0029   // define average magnetic field and gradient
0030   // at initial point - inlike TRPRFN
0031   GlobalVector hnf = h.unit();
0032   double qp = -h.mag();
0033   //   double q = -h.mag()*qbp;
0034   double q = qp * qbp;
0035
0036   double theta = q * absS;
0037   double sint, cost;
0038   vdt::fast_sincos(theta, sint, cost);
0039
0040   Vec3D t1(p1.basicVector().v);
0041   Vec3D t2(p2.basicVector().v);
0042
0043   Vec3D hn(hnf.basicVector().v);
0044   Vec3D dx(dxf.basicVector().v);
0045
0046   double gamma = dot(hn, t2);
0047   Vec3D an = cross(hn, t2);
0048
0049   Vec4D t(t1.xy(), t2.xy());
0050   Vec4D tt = t * t;
0051   tt = mathSSE::sqrt(hadd(tt, tt));
0052
0053 #ifdef CMS_USE_AVX2
0054   const __m256d neg = _mm256_setr_pd(-0.0, 0.0, -0.0, 0.0);
0055   Vec4D res(_mm256_xor_pd(neg, _mm256_div_pd(_mm256_shuffle_pd(t.vec, t.vec, 5), tt.vec)));
0056   Vec2D u1(res.xy());
0057   Vec2D u2(res.zw());
0058 #else
0059   // __m128d t = _mm_mul_pd(t1.xy().vec,t1.xy().vec);
0060   //  t = _mm_sqrt_pd(_mm_add_pd(t,_mm_shuffle_pd(t,t,1)));
0061   // t = _mm_sqrt_pd(_mm_hadd_pd(t,t));
0062   const __m128d neg = _mm_set_pd(0.0, -0.0);  // mind: set not setr
0063   Vec2D u1(_mm_xor_pd(neg, _mm_div_pd(_mm_shuffle_pd(t1.xy().vec, t1.xy().vec, 1), tt.xy().vec)));
0064   // t = _mm_mul_pd(t2.xy().vec,t2.xy().vec);
0065   //  t = _mm_sqrt_pd(_mm_add_pd(t,_mm_shuffle_pd(t,t,1)));
0066   //t = _mm_sqrt_pd(_mm_hadd_pd(t,t));
0067
0068   Vec2D u2(_mm_xor_pd(neg, _mm_div_pd(_mm_shuffle_pd(t2.xy().vec, t2.xy().vec, 1), tt.zw().vec)));
0069
0070 #endif
0071
0072   Vec3D u13(u1);
0073   Vec3D v1 = cross(t1, u13);
0074   Vec3D u23(u2);
0075   Vec3D v2 = cross(t2, u23);
0076
0077   // now prepare the transport matrix
0078   // pp only needed in high-p case (WA)
0079   //   double pp = 1./qbp;
0080   ////    double pp = fts.momentum().mag();
0081   // moved up (where -h.mag() is needed()
0082   //   double qp = q*pp;
0083
0084   double anv = -dot(hn, u23);
0085   double anu = dot(hn, v2);
0086
0087   double omcost = 1. - cost;
0088   double tmsint = theta - sint;
0089
0090   Vec3D hu = cross(hn, u13);
0091   Vec3D hv = cross(hn, v1);
0092
0093   //   1/p - doesn't change since |p1| = |p2|
0094   theJacobian(0, 0) = 1.;
0095   for (auto i = 1; i < 5; ++i)
0096     theJacobian(0, i) = 0.;
0097   //   lambda
0098
0099   theJacobian(1, 0) = -qp * anv * dot(t2, dx);
0100
0101   theJacobian(1, 1) = cost * dot(v1, v2) + sint * dot(hv, v2) + omcost * dot(hn, v1) * dot(hn, v2) +
0102                       anv * (-sint * dot(v1, t2) + omcost * dot(v1, an) - tmsint * gamma * dot(hn, v1));
0103
0104   theJacobian(1, 2) = cost * dot(u1, v2.xy()) + sint * dot(hu, v2) + omcost * dot(hn.xy(), u1) * dot(hn, v2) +
0105                       anv * (-sint * dot(u1, t2.xy()) + omcost * dot(u1, an.xy()) - tmsint * gamma * dot(hn.xy(), u1));
0106   theJacobian(1, 2) *= cosl0;
0107
0108   theJacobian(1, 3) = -q * anv * dot(u1, t2.xy());
0109
0110   theJacobian(1, 4) = -q * anv * dot(v1, t2);
0111
0112   //   phi
0113
0114   theJacobian(2, 0) = -qp * anu * cosl1 * dot(t2, dx);
0115
0116   theJacobian(2, 1) = cost * dot(v1.xy(), u2) + sint * dot(hv.xy(), u2) + omcost * dot(hn, v1) * dot(hn.xy(), u2) +
0117                       anu * (-sint * dot(v1, t2) + omcost * dot(v1, an) - tmsint * gamma * dot(hn, v1));
0118   theJacobian(2, 1) *= cosl1;
0119
0120   theJacobian(2, 2) = cost * dot(u1, u2) + sint * dot(hu.xy(), u2) + omcost * dot(hn.xy(), u1) * dot(hn.xy(), u2) +
0121                       anu * (-sint * dot(u1, t2.xy()) + omcost * dot(u1, an.xy()) - tmsint * gamma * dot(hn.xy(), u1));
0122   theJacobian(2, 2) *= cosl1 * cosl0;
0123
0124   theJacobian(2, 3) = -q * anu * cosl1 * dot(u1, t2.xy());
0125
0126   theJacobian(2, 4) = -q * anu * cosl1 * dot(v1, t2);
0127
0128   //   yt
0129
0130   double overQ = 1. / q;
0131
0132   theJacobian(3, 1) =
0133       (sint * dot(v1.xy(), u2) + omcost * dot(hv.xy(), u2) + tmsint * dot(hn, v1) * dot(hn.xy(), u2)) * overQ;
0134
0135   theJacobian(3, 2) =
0136       (sint * dot(u1, u2) + omcost * dot(hu.xy(), u2) + tmsint * dot(hn.xy(), u1) * dot(hn.xy(), u2)) * (cosl0 * overQ);
0137
0138   theJacobian(3, 3) = dot(u1, u2);
0139
0140   theJacobian(3, 4) = dot(v1.xy(), u2);
0141
0142   //   zt
0143
0144   theJacobian(4, 1) = (sint * dot(v1, v2) + omcost * dot(hv, v2) + tmsint * dot(hn, v1) * dot(hn, v2)) * overQ;
0145
0146   theJacobian(4, 2) =
0147       (sint * dot(u1, v2.xy()) + omcost * dot(hu, v2) + tmsint * dot(hn.xy(), u1) * dot(hn, v2)) * (cosl0 * overQ);
0148
0149   theJacobian(4, 3) = dot(u1, v2.xy());
0150
0151   theJacobian(4, 4) = dot(v1, v2);
0152
0153   //double cutCriterion = abs(s/fts.momentum().mag());
0154   //  double cutCriterion = fabs(s/globalParameters.momentum().mag());
0155   double cutCriterion = std::abs(s * qbp);
0156   const double limit = 5.;  // valid for propagations with effectively float precision
0157
0158   if (cutCriterion > limit) {
0159     double pp = 1. / qbp;
0160     theJacobian(3, 0) = pp * dot(u2, dx.xy());
0161     //    theJacobian(4,0) = -pp*dot(v2,dx);  // was wrong sign???
0162     /*
0163 It seems so.
0164 The effect was noticed analysing the distribution of reduced chi2 of general tracks
0165 vs eta. A +3% difference was noticed whem using the - sign instead of the plus,
0166 in the region .75 < |eta| < 1.5, in particular for <1GeV tracks.
0167 Indeed, the reduced chi2 is only one of the symptoms: many other quantities
0168 were affected (momentum for example). In addition additional tracks were reconstructed.
0169 */
0170     theJacobian(4, 0) = pp * dot(v2, dx);
0171
0172   } else {
0173     Vec3D hp1 = cross(hn, t1);
0174     double temp1 = dot(hp1.xy(), u2);
0175     Vec3D ghnmp = gamma * hn - t1;
0176     double temp2 = dot(ghnmp.xy(), u2);
0177
0178     double qps = qp * s;
0179     double h2 = qps * qbp;
0180     double h3 = (-1. / 8.) * h2;
0181
0182     double secondOrder41 = 0.5 * temp1;
0183     double thirdOrder41 = (1. / 3.) * temp2;
0184     double fourthOrder41 = h3 * temp1;
0185     theJacobian(3, 0) = (s * qps) * (secondOrder41 + h2 * (thirdOrder41 + fourthOrder41));
0186
0187     double temp3 = dot(hp1, v2);
0188     double temp4 = dot(ghnmp, v2);
0189
0190     double secondOrder51 = 0.5 * temp3;
0191     double thirdOrder51 = (1. / 3.) * temp4;
0192     double fourthOrder51 = h3 * temp3;
0193     theJacobian(4, 0) = (s * qps) * (secondOrder51 + h2 * (thirdOrder51 + fourthOrder51));
0194   }
0195
0196   // end of TRPRFN
0197 }