diff --git a/reco/L1/CMakeLists.txt b/reco/L1/CMakeLists.txt
index 03f109afa61e8b2e914c2c0fd6e25ee50b46e106..1d7f583d1fbd5a98134a2dd16d254de2a2c0ed3f 100644
--- a/reco/L1/CMakeLists.txt
+++ b/reco/L1/CMakeLists.txt
@@ -138,6 +138,8 @@ L1Algo/L1Event.cxx
L1Algo/L1EventMatch.cxx
L1Algo/L1MCEvent.cxx
L1Algo/L1Fit.cxx
+L1Algo/L1FitMaterial.cxx
+L1Algo/L1Extrapolation.cxx
CbmL1MCTrack.cxx
L1Algo/L1MaterialInfo.cxx
L1Algo/L1UMeasurementInfo.cxx
diff --git a/reco/L1/L1Algo/L1ClonesMerger.cxx b/reco/L1/L1Algo/L1ClonesMerger.cxx
index 8192ad776957a4ec669a95898f3661b6986db738..76299dc9ca00e8ea1298e748b5d771cc85bd8362 100644
--- a/reco/L1/L1Algo/L1ClonesMerger.cxx
+++ b/reco/L1/L1Algo/L1ClonesMerger.cxx
@@ -297,9 +297,8 @@ void L1ClonesMerger::InvertCholesky(fvec a[15])
uud += u[j][i] * u[j][i] * d[j];
uud = a[i * (i + 3) / 2] - uud;
- fvec smallval = 1.e-12;
- fvec initialised = fvec(uud < smallval);
- uud = ((!initialised) & uud) + (smallval & initialised);
+ fvec smallval(1.e-12);
+ uud = if3(uud >= smallval, uud, smallval);
d[i] = uud / fabs(uud);
u[i][i] = sqrt(fabs(uud));
diff --git a/reco/L1/L1Algo/L1Extrapolation.cxx b/reco/L1/L1Algo/L1Extrapolation.cxx
new file mode 100644
index 0000000000000000000000000000000000000000..58cc92e8ac03f52734aa0829a0bbd3e0ff3658e0
--- /dev/null
+++ b/reco/L1/L1Algo/L1Extrapolation.cxx
@@ -0,0 +1,914 @@
+/* Copyright (C) 2007-2017 GSI Helmholtzzentrum fuer Schwerionenforschung, Darmstadt
+ SPDX-License-Identifier: GPL-3.0-only
+ Authors: Maksym Zyzak, Sergey Gorbunov [committer], Igor Kulakov */
+
+#include "L1Extrapolation.h"
+
+#include "L1Def.h"
+#include "L1Field.h"
+#include "L1TrackPar.h"
+
+
+//#define cnst static const fvec
+#define cnst const fvec
+
+void L1ExtrapolateAnalytic(L1TrackPar& T, fvec z_out, fvec qp0, const L1FieldRegion& F, const fvec* w);
+void L1ExtrapolateRungeKutta(L1TrackPar& T, fvec z_out, fvec qp0, const L1FieldRegion& F, const fvec* w);
+
+void L1Extrapolate // extrapolates track parameters and returns jacobian for extrapolation of CovMatrix
+ (L1TrackPar& T, // input track parameters (x,y,tx,ty,Q/p) and cov.matrix
+ fvec z_out, // extrapolate to this z position
+ fvec qp0, // use Q/p linearisation at this value
+ const L1FieldRegion& F, fvec* w)
+{
+ L1ExtrapolateRungeKutta(T, z_out, qp0, F, w);
+ //L1ExtrapolateAnalytic(T, z_out, qp0, F, w);
+}
+
+
+void L1ExtrapolateAnalytic(L1TrackPar& T, // input track parameters (x,y,tx,ty,Q/p) and cov.matrix
+ fvec z_out, // extrapolate to this z position
+ fvec qp0, // use Q/p linearisation at this value
+ const L1FieldRegion& F, const fvec* w)
+{
+ //cout<<"Extrapolation..."<<endl;
+ //
+ // Part of the analytic extrapolation formula with error (c_light*B*dz)^4/4!
+ //
+
+ cnst c_light = 0.000299792458;
+
+ cnst c1 = 1., c2 = 2., c3 = 3., c4 = 4., c6 = 6., c9 = 9., c15 = 15., c18 = 18., c45 = 45., c2i = 1. / 2.,
+ c3i = 1. / 3., c6i = 1. / 6., c12i = 1. / 12.;
+
+ const fvec qp = T.qp;
+ fvec dz = (z_out - T.z);
+
+ if (w) { dz = masked(dz, fvec::Zero() < *w); }
+
+ const fvec dz2 = dz * dz;
+ const fvec dz3 = dz2 * dz;
+
+ // construct coefficients
+
+ const fvec x = T.tx;
+ const fvec y = T.ty;
+ const fvec xx = x * x;
+ const fvec xy = x * y;
+ const fvec yy = y * y;
+ const fvec y2 = y * c2;
+ const fvec x2 = x * c2;
+ const fvec x4 = x * c4;
+ const fvec xx31 = xx * c3 + c1;
+ const fvec xx159 = xx * c15 + c9;
+
+ const fvec Ay = -xx - c1;
+ const fvec Ayy = x * (xx * c3 + c3);
+ const fvec Ayz = -c2 * xy;
+ const fvec Ayyy = -(c15 * xx * xx + c18 * xx + c3);
+
+ const fvec Ayy_x = c3 * xx31;
+ const fvec Ayyy_x = -x4 * xx159;
+
+ const fvec Bx = yy + c1;
+ const fvec Byy = y * xx31;
+ const fvec Byz = c2 * xx + c1;
+ const fvec Byyy = -xy * xx159;
+
+ const fvec Byy_x = c6 * xy;
+ const fvec Byyy_x = -y * (c45 * xx + c9);
+ const fvec Byyy_y = -x * xx159;
+
+ // end of coefficients calculation
+
+ const fvec t2 = c1 + xx + yy;
+ const fvec t = sqrt(t2);
+ const fvec h = qp0 * c_light;
+ const fvec ht = h * t;
+
+ // get field integrals
+ const fvec ddz = T.z - F.z0;
+ const fvec Fx0 = F.cx0 + F.cx1 * ddz + F.cx2 * ddz * ddz;
+ const fvec Fx1 = (F.cx1 + c2 * F.cx2 * ddz) * dz;
+ const fvec Fx2 = F.cx2 * dz2;
+ const fvec Fy0 = F.cy0 + F.cy1 * ddz + F.cy2 * ddz * ddz;
+ const fvec Fy1 = (F.cy1 + c2 * F.cy2 * ddz) * dz;
+ const fvec Fy2 = F.cy2 * dz2;
+ const fvec Fz0 = F.cz0 + F.cz1 * ddz + F.cz2 * ddz * ddz;
+ const fvec Fz1 = (F.cz1 + c2 * F.cz2 * ddz) * dz;
+ const fvec Fz2 = F.cz2 * dz2;
+
+ //
+
+ const fvec sx = (Fx0 + Fx1 * c2i + Fx2 * c3i);
+ const fvec sy = (Fy0 + Fy1 * c2i + Fy2 * c3i);
+ const fvec sz = (Fz0 + Fz1 * c2i + Fz2 * c3i);
+
+ const fvec Sx = (Fx0 * c2i + Fx1 * c6i + Fx2 * c12i);
+ const fvec Sy = (Fy0 * c2i + Fy1 * c6i + Fy2 * c12i);
+ const fvec Sz = (Fz0 * c2i + Fz1 * c6i + Fz2 * c12i);
+
+ fvec syz;
+ {
+ cnst d = 1. / 360., c00 = 30. * 6. * d, c01 = 30. * 2. * d, c02 = 30. * d, c10 = 3. * 40. * d, c11 = 3. * 15. * d,
+ c12 = 3. * 8. * d, c20 = 2. * 45. * d, c21 = 2. * 2. * 9. * d, c22 = 2. * 2. * 5. * d;
+ syz = Fy0 * (c00 * Fz0 + c01 * Fz1 + c02 * Fz2) + Fy1 * (c10 * Fz0 + c11 * Fz1 + c12 * Fz2)
+ + Fy2 * (c20 * Fz0 + c21 * Fz1 + c22 * Fz2);
+ }
+
+ fvec Syz;
+ {
+ cnst d = 1. / 2520., c00 = 21. * 20. * d, c01 = 21. * 5. * d, c02 = 21. * 2. * d, c10 = 7. * 30. * d,
+ c11 = 7. * 9. * d, c12 = 7. * 4. * d, c20 = 2. * 63. * d, c21 = 2. * 21. * d, c22 = 2. * 10. * d;
+ Syz = Fy0 * (c00 * Fz0 + c01 * Fz1 + c02 * Fz2) + Fy1 * (c10 * Fz0 + c11 * Fz1 + c12 * Fz2)
+ + Fy2 * (c20 * Fz0 + c21 * Fz1 + c22 * Fz2);
+ }
+
+ const fvec syy = sy * sy * c2i;
+ const fvec syyy = syy * sy * c3i;
+
+ fvec Syy;
+ {
+ cnst d = 1. / 2520., c00 = 420. * d, c01 = 21. * 15. * d, c02 = 21. * 8. * d, c03 = 63. * d, c04 = 70. * d,
+ c05 = 20. * d;
+ Syy = Fy0 * (c00 * Fy0 + c01 * Fy1 + c02 * Fy2) + Fy1 * (c03 * Fy1 + c04 * Fy2) + c05 * Fy2 * Fy2;
+ }
+
+ fvec Syyy;
+ {
+ cnst d = 1. / 181440., c000 = 7560 * d, c001 = 9 * 1008 * d, c002 = 5 * 1008 * d, c011 = 21 * 180 * d,
+ c012 = 24 * 180 * d, c022 = 7 * 180 * d, c111 = 540 * d, c112 = 945 * d, c122 = 560 * d, c222 = 112 * d;
+ const fvec Fy22 = Fy2 * Fy2;
+ Syyy = Fy0 * (Fy0 * (c000 * Fy0 + c001 * Fy1 + c002 * Fy2) + Fy1 * (c011 * Fy1 + c012 * Fy2) + c022 * Fy22)
+ + Fy1 * (Fy1 * (c111 * Fy1 + c112 * Fy2) + c122 * Fy22) + c222 * Fy22 * Fy2;
+ }
+
+
+ const fvec sA1 = sx * xy + sy * Ay + sz * y;
+ const fvec sA1_x = sx * y - sy * x2;
+ const fvec sA1_y = sx * x + sz;
+
+ const fvec sB1 = sx * Bx - sy * xy - sz * x;
+ const fvec sB1_x = -sy * y - sz;
+ const fvec sB1_y = sx * y2 - sy * x;
+
+ const fvec SA1 = Sx * xy + Sy * Ay + Sz * y;
+ const fvec SA1_x = Sx * y - Sy * x2;
+ const fvec SA1_y = Sx * x + Sz;
+
+ const fvec SB1 = Sx * Bx - Sy * xy - Sz * x;
+ const fvec SB1_x = -Sy * y - Sz;
+ const fvec SB1_y = Sx * y2 - Sy * x;
+
+
+ const fvec sA2 = syy * Ayy + syz * Ayz;
+ const fvec sA2_x = syy * Ayy_x - syz * y2;
+ const fvec sA2_y = -syz * x2;
+ const fvec sB2 = syy * Byy + syz * Byz;
+ const fvec sB2_x = syy * Byy_x + syz * x4;
+ const fvec sB2_y = syy * xx31;
+
+ const fvec SA2 = Syy * Ayy + Syz * Ayz;
+ const fvec SA2_x = Syy * Ayy_x - Syz * y2;
+ const fvec SA2_y = -Syz * x2;
+ const fvec SB2 = Syy * Byy + Syz * Byz;
+ const fvec SB2_x = Syy * Byy_x + Syz * x4;
+ const fvec SB2_y = Syy * xx31;
+
+ const fvec sA3 = syyy * Ayyy;
+ const fvec sA3_x = syyy * Ayyy_x;
+ const fvec sB3 = syyy * Byyy;
+ const fvec sB3_x = syyy * Byyy_x;
+ const fvec sB3_y = syyy * Byyy_y;
+
+
+ const fvec SA3 = Syyy * Ayyy;
+ const fvec SA3_x = Syyy * Ayyy_x;
+ const fvec SB3 = Syyy * Byyy;
+ const fvec SB3_x = Syyy * Byyy_x;
+ const fvec SB3_y = Syyy * Byyy_y;
+
+ const fvec ht1 = ht * dz;
+ const fvec ht2 = ht * ht * dz2;
+ const fvec ht3 = ht * ht * ht * dz3;
+ const fvec ht1sA1 = ht1 * sA1;
+ const fvec ht1sB1 = ht1 * sB1;
+ const fvec ht1SA1 = ht1 * SA1;
+ const fvec ht1SB1 = ht1 * SB1;
+ const fvec ht2sA2 = ht2 * sA2;
+ const fvec ht2SA2 = ht2 * SA2;
+ const fvec ht2sB2 = ht2 * sB2;
+ const fvec ht2SB2 = ht2 * SB2;
+ const fvec ht3sA3 = ht3 * sA3;
+ const fvec ht3sB3 = ht3 * sB3;
+ const fvec ht3SA3 = ht3 * SA3;
+ const fvec ht3SB3 = ht3 * SB3;
+
+ T.x += (x + ht1SA1 + ht2SA2 + ht3SA3) * dz;
+ T.y += (y + ht1SB1 + ht2SB2 + ht3SB3) * dz;
+ T.tx += ht1sA1 + ht2sA2 + ht3sA3;
+ T.ty += ht1sB1 + ht2sB2 + ht3sB3;
+ T.z += dz;
+
+ const fvec ctdz = c_light * t * dz;
+ const fvec ctdz2 = c_light * t * dz2;
+
+ const fvec dqp = qp - qp0;
+ const fvec t2i = c1 * rcp(t2); // /t2;
+ const fvec xt2i = x * t2i;
+ const fvec yt2i = y * t2i;
+ const fvec tmp0 = ht1SA1 + c2 * ht2SA2 + c3 * ht3SA3;
+ const fvec tmp1 = ht1SB1 + c2 * ht2SB2 + c3 * ht3SB3;
+ const fvec tmp2 = ht1sA1 + c2 * ht2sA2 + c3 * ht3sA3;
+ const fvec tmp3 = ht1sB1 + c2 * ht2sB2 + c3 * ht3sB3;
+
+ const fvec j02 = dz * (c1 + xt2i * tmp0 + ht1 * SA1_x + ht2 * SA2_x + ht3 * SA3_x);
+ const fvec j12 = dz * (xt2i * tmp1 + ht1 * SB1_x + ht2 * SB2_x + ht3 * SB3_x);
+ const fvec j22 = c1 + xt2i * tmp2 + ht1 * sA1_x + ht2 * sA2_x + ht3 * sA3_x;
+ const fvec j32 = xt2i * tmp3 + ht1 * sB1_x + ht2 * sB2_x + ht3 * sB3_x;
+
+ const fvec j03 = dz * (yt2i * tmp0 + ht1 * SA1_y + ht2 * SA2_y);
+ const fvec j13 = dz * (c1 + yt2i * tmp1 + ht1 * SB1_y + ht2 * SB2_y + ht3 * SB3_y);
+ const fvec j23 = yt2i * tmp2 + ht1 * sA1_y + ht2 * sA2_y;
+ const fvec j33 = c1 + yt2i * tmp3 + ht1 * sB1_y + ht2 * sB2_y + ht3 * sB3_y;
+
+ const fvec j04 = ctdz2 * (SA1 + c2 * ht1 * SA2 + c3 * ht2 * SA3);
+ const fvec j14 = ctdz2 * (SB1 + c2 * ht1 * SB2 + c3 * ht2 * SB3);
+ const fvec j24 = ctdz * (sA1 + c2 * ht1 * sA2 + c3 * ht2 * sA3);
+ const fvec j34 = ctdz * (sB1 + c2 * ht1 * sB2 + c3 * ht2 * sB3);
+
+
+ // extrapolate inverse momentum
+
+ T.x += j04 * dqp;
+ T.y += j14 * dqp;
+ T.tx += j24 * dqp;
+ T.ty += j34 * dqp;
+
+ // covariance matrix transport
+
+ const fvec c42 = T.C42, c43 = T.C43;
+
+ const fvec cj00 = T.C00 + T.C20 * j02 + T.C30 * j03 + T.C40 * j04;
+ // const fvec cj10 = T.C10 + T.C21*j02 + T.C31*j03 + T.C41*j04;
+ const fvec cj20 = T.C20 + T.C22 * j02 + T.C32 * j03 + c42 * j04;
+ const fvec cj30 = T.C30 + T.C32 * j02 + T.C33 * j03 + c43 * j04;
+
+ const fvec cj01 = T.C10 + T.C20 * j12 + T.C30 * j13 + T.C40 * j14;
+ const fvec cj11 = T.C11 + T.C21 * j12 + T.C31 * j13 + T.C41 * j14;
+ const fvec cj21 = T.C21 + T.C22 * j12 + T.C32 * j13 + c42 * j14;
+ const fvec cj31 = T.C31 + T.C32 * j12 + T.C33 * j13 + c43 * j14;
+
+ // const fvec cj02 = T.C20*j22 + T.C30*j23 + T.C40*j24;
+ // const fvec cj12 = T.C21*j22 + T.C31*j23 + T.C41*j24;
+ const fvec cj22 = T.C22 * j22 + T.C32 * j23 + c42 * j24;
+ const fvec cj32 = T.C32 * j22 + T.C33 * j23 + c43 * j24;
+
+ // const fvec cj03 = T.C20*j32 + T.C30*j33 + T.C40*j34;
+ // const fvec cj13 = T.C21*j32 + T.C31*j33 + T.C41*j34;
+ const fvec cj23 = T.C22 * j32 + T.C32 * j33 + c42 * j34;
+ const fvec cj33 = T.C32 * j32 + T.C33 * j33 + c43 * j34;
+
+ T.C40 += c42 * j02 + c43 * j03 + T.C44 * j04; // cj40
+ T.C41 += c42 * j12 + c43 * j13 + T.C44 * j14; // cj41
+ T.C42 = c42 * j22 + c43 * j23 + T.C44 * j24; // cj42
+ T.C43 = c42 * j32 + c43 * j33 + T.C44 * j34; // cj43
+
+ T.C00 = cj00 + j02 * cj20 + j03 * cj30 + j04 * T.C40;
+ T.C10 = cj01 + j02 * cj21 + j03 * cj31 + j04 * T.C41;
+ T.C11 = cj11 + j12 * cj21 + j13 * cj31 + j14 * T.C41;
+
+ T.C20 = j22 * cj20 + j23 * cj30 + j24 * T.C40;
+ T.C30 = j32 * cj20 + j33 * cj30 + j34 * T.C40;
+ T.C21 = j22 * cj21 + j23 * cj31 + j24 * T.C41;
+ T.C31 = j32 * cj21 + j33 * cj31 + j34 * T.C41;
+ T.C22 = j22 * cj22 + j23 * cj32 + j24 * T.C42;
+ T.C32 = j32 * cj22 + j33 * cj32 + j34 * T.C42;
+ T.C33 = j32 * cj23 + j33 * cj33 + j34 * T.C43;
+ //cout<<"Extrapolation ok"<<endl;
+}
+
+
+void L1ExtrapolateRungeKutta // extrapolates track parameters and returns jacobian for extrapolation of CovMatrix
+ (L1TrackPar& T, // input track parameters (x,y,tx,ty,Q/p) and cov.matrix
+ fvec z_out, // extrapolate to this z position
+ fvec qp0, // use Q/p linearisation at this value
+ const L1FieldRegion& F, const fvec* w)
+{
+ //
+ // Forth-order Runge-Kutta method for solution of the equation
+ // of motion of a particle with parameter qp = Q /P
+ // in the magnetic field B()
+ //
+ // | x | tx
+ // | y | ty
+ // d/dz = | tx| = ft * ( ty * ( B(3)+tx*b(1) ) - (1+tx**2)*B(2) )
+ // | ty| ft * (-tx * ( B(3)+ty+b(2) - (1+ty**2)*B(1) ) ,
+ //
+ // where ft = c_light*qp*sqrt ( 1 + tx**2 + ty**2 ) .
+ //
+ // In the following for RK step :
+ //
+ // x=x[0], y=x[1], tx=x[3], ty=x[4].
+ //
+ //========================================================================
+ //
+ // NIM A395 (1997) 169-184; NIM A426 (1999) 268-282.
+ //
+ // the routine is based on LHC(b) utility code
+ //
+ //========================================================================
+
+
+ cnst c_light = 0.000299792458;
+
+ const fvec a[4] = {0.0f, 0.5f, 0.5f, 1.0f};
+ const fvec c[4] = {1.0f / 6.0f, 1.0f / 3.0f, 1.0f / 3.0f, 1.0f / 6.0f};
+ const fvec b[4] = {0.0f, 0.5f, 0.5f, 1.0f};
+
+ int step4;
+ fvec k[16], x0[4], x[4], k1[16];
+ fvec Ax[4], Ay[4], Ax_tx[4], Ay_tx[4], Ax_ty[4], Ay_ty[4];
+
+ //----------------------------------------------------------------
+
+ if (w) { z_out = if3(fvec::Zero() < *w, z_out, T.z); }
+
+ fvec qp_in = T.qp;
+ const fvec z_in = T.z;
+ const fvec h = (z_out - T.z);
+ fvec hC = h * c_light;
+ x0[0] = T.x;
+ x0[1] = T.y;
+ x0[2] = T.tx;
+ x0[3] = T.ty;
+ //
+ // Runge-Kutta step
+ //
+
+ int step;
+ int i;
+
+ fvec B[4][3];
+ for (step = 0; step < 4; ++step) {
+ F.Get(z_in + a[step] * h, B[step]);
+ }
+
+ for (step = 0; step < 4; ++step) {
+ for (i = 0; i < 4; ++i) {
+ if (step == 0) { x[i] = x0[i]; }
+ else {
+ x[i] = x0[i] + b[step] * k[step * 4 - 4 + i];
+ }
+ }
+
+ fvec tx = x[2];
+ fvec ty = x[3];
+ fvec tx2 = tx * tx;
+ fvec ty2 = ty * ty;
+ fvec txty = tx * ty;
+ fvec tx2ty21 = 1.f + tx2 + ty2;
+ // if( tx2ty21 > 1.e4 ) return 1;
+ fvec I_tx2ty21 = 1.f / tx2ty21 * qp0;
+ fvec tx2ty2 = sqrt(tx2ty21);
+ // fvec I_tx2ty2 = qp0 * hC / tx2ty2 ; unsused ???
+ tx2ty2 *= hC;
+ fvec tx2ty2qp = tx2ty2 * qp0;
+ Ax[step] = (txty * B[step][0] + ty * B[step][2] - (1.f + tx2) * B[step][1]) * tx2ty2;
+ Ay[step] = (-txty * B[step][1] - tx * B[step][2] + (1.f + ty2) * B[step][0]) * tx2ty2;
+
+ Ax_tx[step] = Ax[step] * tx * I_tx2ty21 + (ty * B[step][0] - 2.f * tx * B[step][1]) * tx2ty2qp;
+ Ax_ty[step] = Ax[step] * ty * I_tx2ty21 + (tx * B[step][0] + B[step][2]) * tx2ty2qp;
+ Ay_tx[step] = Ay[step] * tx * I_tx2ty21 + (-ty * B[step][1] - B[step][2]) * tx2ty2qp;
+ Ay_ty[step] = Ay[step] * ty * I_tx2ty21 + (-tx * B[step][1] + 2.f * ty * B[step][0]) * tx2ty2qp;
+
+ step4 = step * 4;
+ k[step4] = tx * h;
+ k[step4 + 1] = ty * h;
+ k[step4 + 2] = Ax[step] * qp0;
+ k[step4 + 3] = Ay[step] * qp0;
+
+ } // end of Runge-Kutta steps
+
+ {
+ T.x = x0[0] + c[0] * k[0] + c[1] * k[4 + 0] + c[2] * k[8 + 0] + c[3] * k[12 + 0];
+ T.y = x0[1] + c[0] * k[1] + c[1] * k[4 + 1] + c[2] * k[8 + 1] + c[3] * k[12 + 1];
+ T.tx = x0[2] + c[0] * k[2] + c[1] * k[4 + 2] + c[2] * k[8 + 2] + c[3] * k[12 + 2];
+ T.ty = x0[3] + c[0] * k[3] + c[1] * k[4 + 3] + c[2] * k[8 + 3] + c[3] * k[12 + 3];
+ T.z = z_out;
+ }
+
+ //
+ // Derivatives dx/dqp
+ //
+
+ x0[0] = 0.f;
+ x0[1] = 0.f;
+ x0[2] = 0.f;
+ x0[3] = 0.f;
+
+ //
+ // Runge-Kutta step for derivatives dx/dqp
+
+ for (step = 0; step < 4; ++step) {
+ for (i = 0; i < 4; ++i) {
+ if (step == 0) { x[i] = x0[i]; }
+ else {
+ x[i] = x0[i] + b[step] * k1[step * 4 - 4 + i];
+ }
+ }
+ step4 = step * 4;
+ k1[step4] = x[2] * h;
+ k1[step4 + 1] = x[3] * h;
+ k1[step4 + 2] = Ax[step] + Ax_tx[step] * x[2] + Ax_ty[step] * x[3];
+ k1[step4 + 3] = Ay[step] + Ay_tx[step] * x[2] + Ay_ty[step] * x[3];
+
+ } // end of Runge-Kutta steps for derivatives dx/dqp
+
+ fvec J[25];
+
+ for (i = 0; i < 4; ++i) {
+ J[20 + i] = x0[i] + c[0] * k1[i] + c[1] * k1[4 + i] + c[2] * k1[8 + i] + c[3] * k1[12 + i];
+ }
+ J[24] = 1.;
+ //
+ // end of derivatives dx/dqp
+ //
+
+ // Derivatives dx/tx
+ //
+
+ x0[0] = 0.f;
+ x0[1] = 0.f;
+ x0[2] = 1.f;
+ x0[3] = 0.f;
+
+ //
+ // Runge-Kutta step for derivatives dx/dtx
+ //
+
+ for (step = 0; step < 4; ++step) {
+ for (i = 0; i < 4; ++i) {
+ if (step == 0) { x[i] = x0[i]; }
+ else if (i != 2) {
+ x[i] = x0[i] + b[step] * k1[step * 4 - 4 + i];
+ }
+ }
+ step4 = step * 4;
+ k1[step4] = x[2] * h;
+ k1[step4 + 1] = x[3] * h;
+ // k1[step4+2] = Ax_tx[step] * x[2] + Ax_ty[step] * x[3];
+ k1[step4 + 3] = Ay_tx[step] * x[2] + Ay_ty[step] * x[3];
+
+ } // end of Runge-Kutta steps for derivatives dx/dtx
+
+ for (i = 0; i < 4; ++i) {
+ if (i != 2) { J[10 + i] = x0[i] + c[0] * k1[i] + c[1] * k1[4 + i] + c[2] * k1[8 + i] + c[3] * k1[12 + i]; }
+ }
+ // end of derivatives dx/dtx
+ J[12] = 1.f;
+ J[14] = 0.f;
+
+ // Derivatives dx/ty
+ //
+
+ x0[0] = 0.f;
+ x0[1] = 0.f;
+ x0[2] = 0.f;
+ x0[3] = 1.f;
+
+ //
+ // Runge-Kutta step for derivatives dx/dty
+ //
+
+ for (step = 0; step < 4; ++step) {
+ for (i = 0; i < 4; ++i) {
+ if (step == 0) {
+ x[i] = x0[i]; // ty fixed
+ }
+ else if (i != 3) {
+ x[i] = x0[i] + b[step] * k1[step * 4 - 4 + i];
+ }
+ }
+ step4 = step * 4;
+ k1[step4] = x[2] * h;
+ k1[step4 + 1] = x[3] * h;
+ k1[step4 + 2] = Ax_tx[step] * x[2] + Ax_ty[step] * x[3];
+ // k1[step4+3] = Ay_tx[step] * x[2] + Ay_ty[step] * x[3];
+
+ } // end of Runge-Kutta steps for derivatives dx/dty
+
+ for (i = 0; i < 3; ++i) {
+ J[15 + i] = x0[i] + c[0] * k1[i] + c[1] * k1[4 + i] + c[2] * k1[8 + i] + c[3] * k1[12 + i];
+ }
+ // end of derivatives dx/dty
+ J[18] = 1.;
+ J[19] = 0.;
+
+ //
+ // derivatives dx/dx and dx/dy
+
+ for (i = 0; i < 10; ++i) {
+ J[i] = 0.;
+ }
+ J[0] = 1.;
+ J[6] = 1.;
+
+ fvec dqp = qp_in - qp0;
+
+ { // update parameters
+ T.x += J[5 * 4 + 0] * dqp;
+ T.y += J[5 * 4 + 1] * dqp;
+ T.tx += J[5 * 4 + 2] * dqp;
+ T.ty += J[5 * 4 + 3] * dqp;
+ }
+
+ // covariance matrix transport
+
+ // // if ( C_in&&C_out ) CbmKFMath::multQtSQ( 5, J, C_in, C_out); // TODO
+ // j(0,2) = J[5*2 + 0];
+ // j(1,2) = J[5*2 + 1];
+ // j(2,2) = J[5*2 + 2];
+ // j(3,2) = J[5*2 + 3];
+ //
+ // j(0,3) = J[5*3 + 0];
+ // j(1,3) = J[5*3 + 1];
+ // j(2,3) = J[5*3 + 2];
+ // j(3,3) = J[5*3 + 3];
+ //
+ // j(0,4) = J[5*4 + 0];
+ // j(1,4) = J[5*4 + 1];
+ // j(2,4) = J[5*4 + 2];
+ // j(3,4) = J[5*4 + 3];
+
+ const fvec c42 = T.C42, c43 = T.C43;
+
+ const fvec cj00 = T.C00 + T.C20 * J[5 * 2 + 0] + T.C30 * J[5 * 3 + 0] + T.C40 * J[5 * 4 + 0];
+ // const fvec cj10 = T.C10 + T.C21*J[5*2 + 0] + T.C31*J[5*3 + 0] + T.C41*J[5*4 + 0];
+ const fvec cj20 = T.C20 + T.C22 * J[5 * 2 + 0] + T.C32 * J[5 * 3 + 0] + c42 * J[5 * 4 + 0];
+ const fvec cj30 = T.C30 + T.C32 * J[5 * 2 + 0] + T.C33 * J[5 * 3 + 0] + c43 * J[5 * 4 + 0];
+
+ const fvec cj01 = T.C10 + T.C20 * J[5 * 2 + 1] + T.C30 * J[5 * 3 + 1] + T.C40 * J[5 * 4 + 1];
+ const fvec cj11 = T.C11 + T.C21 * J[5 * 2 + 1] + T.C31 * J[5 * 3 + 1] + T.C41 * J[5 * 4 + 1];
+ const fvec cj21 = T.C21 + T.C22 * J[5 * 2 + 1] + T.C32 * J[5 * 3 + 1] + c42 * J[5 * 4 + 1];
+ const fvec cj31 = T.C31 + T.C32 * J[5 * 2 + 1] + T.C33 * J[5 * 3 + 1] + c43 * J[5 * 4 + 1];
+
+ // const fvec cj02 = T.C20*J[5*2 + 2] + T.C30*J[5*3 + 2] + T.C40*J[5*4 + 2];
+ // const fvec cj12 = T.C21*J[5*2 + 2] + T.C31*J[5*3 + 2] + T.C41*J[5*4 + 2];
+ const fvec cj22 = T.C22 * J[5 * 2 + 2] + T.C32 * J[5 * 3 + 2] + c42 * J[5 * 4 + 2];
+ const fvec cj32 = T.C32 * J[5 * 2 + 2] + T.C33 * J[5 * 3 + 2] + c43 * J[5 * 4 + 2];
+
+ // const fvec cj03 = T.C20*J[5*2 + 3] + T.C30*J[5*3 + 3] + T.C40*J[5*4 + 3];
+ // const fvec cj13 = T.C21*J[5*2 + 3] + T.C31*J[5*3 + 3] + T.C41*J[5*4 + 3];
+ const fvec cj23 = T.C22 * J[5 * 2 + 3] + T.C32 * J[5 * 3 + 3] + c42 * J[5 * 4 + 3];
+ const fvec cj33 = T.C32 * J[5 * 2 + 3] + T.C33 * J[5 * 3 + 3] + c43 * J[5 * 4 + 3];
+
+ T.C40 += c42 * J[5 * 2 + 0] + c43 * J[5 * 3 + 0] + T.C44 * J[5 * 4 + 0]; // cj40
+ T.C41 += c42 * J[5 * 2 + 1] + c43 * J[5 * 3 + 1] + T.C44 * J[5 * 4 + 1]; // cj41
+ T.C42 = c42 * J[5 * 2 + 2] + c43 * J[5 * 3 + 2] + T.C44 * J[5 * 4 + 2];
+ T.C43 = c42 * J[5 * 2 + 3] + c43 * J[5 * 3 + 3] + T.C44 * J[5 * 4 + 3];
+
+ T.C00 = cj00 + J[5 * 2 + 0] * cj20 + J[5 * 3 + 0] * cj30 + J[5 * 4 + 0] * T.C40;
+ T.C10 = cj01 + J[5 * 2 + 0] * cj21 + J[5 * 3 + 0] * cj31 + J[5 * 4 + 0] * T.C41;
+ T.C11 = cj11 + J[5 * 2 + 1] * cj21 + J[5 * 3 + 1] * cj31 + J[5 * 4 + 1] * T.C41;
+
+ T.C20 = J[5 * 2 + 2] * cj20 + J[5 * 3 + 2] * cj30 + J[5 * 4 + 2] * T.C40;
+ T.C30 = J[5 * 2 + 3] * cj20 + J[5 * 3 + 3] * cj30 + J[5 * 4 + 3] * T.C40;
+ T.C21 = J[5 * 2 + 2] * cj21 + J[5 * 3 + 2] * cj31 + J[5 * 4 + 2] * T.C41;
+ T.C31 = J[5 * 2 + 3] * cj21 + J[5 * 3 + 3] * cj31 + J[5 * 4 + 3] * T.C41;
+ T.C22 = J[5 * 2 + 2] * cj22 + J[5 * 3 + 2] * cj32 + J[5 * 4 + 2] * T.C42;
+ T.C32 = J[5 * 2 + 3] * cj22 + J[5 * 3 + 3] * cj32 + J[5 * 4 + 3] * T.C42;
+ T.C33 = J[5 * 2 + 3] * cj23 + J[5 * 3 + 3] * cj33 + J[5 * 4 + 3] * T.C43;
+}
+
+
+void L1Extrapolate0(L1TrackPar& T, // input track parameters (x,y,tx,ty,Q/p) and cov.matrix
+ fvec z_out, // extrapolate to this z position
+ L1FieldRegion& F) // magneti
+{
+ //
+ // Part of the analytic extrapolation formula with error (c_light*B*dz)^4/4!
+ //
+
+ cnst c_light = 0.000299792458;
+
+ cnst c1 = 1., c2i = 1. / 2., c3i = 1. / 3., c6i = 1. / 6., c12i = 1. / 12.;
+
+ const fvec qp = T.qp;
+
+
+ const fvec dz = (z_out - T.z);
+ const fvec dz2 = dz * dz;
+
+ // construct coefficients
+
+ const fvec x = T.tx;
+ const fvec y = T.ty;
+ // const fvec z = T.z;
+ const fvec xx = x * x;
+ const fvec xy = x * y;
+ const fvec yy = y * y;
+ const fvec Ay = -xx - c1;
+ const fvec Bx = yy + c1;
+ const fvec ct = c_light * sqrt(c1 + xx + yy);
+
+ const fvec dzc2i = dz * c2i;
+ const fvec dz2c3i = dz2 * c3i;
+ const fvec dzc6i = dz * c6i;
+ const fvec dz2c12i = dz2 * c12i;
+
+ const fvec sx = (F.cx0 + F.cx1 * dzc2i + F.cx2 * dz2c3i);
+ const fvec sy = (F.cy0 + F.cy1 * dzc2i + F.cy2 * dz2c3i);
+ const fvec sz = (F.cz0 + F.cz1 * dzc2i + F.cz2 * dz2c3i);
+
+ const fvec Sx = (F.cx0 * c2i + F.cx1 * dzc6i + F.cx2 * dz2c12i);
+ const fvec Sy = (F.cy0 * c2i + F.cy1 * dzc6i + F.cy2 * dz2c12i);
+ const fvec Sz = (F.cz0 * c2i + F.cz1 * dzc6i + F.cz2 * dz2c12i);
+
+
+ const fvec ctdz = ct * dz;
+ const fvec ctdz2 = ct * dz2;
+
+ const fvec j04 = ctdz2 * (Sx * xy + Sy * Ay + Sz * y);
+ const fvec j14 = ctdz2 * (Sx * Bx - Sy * xy - Sz * x);
+ const fvec j24 = ctdz * (sx * xy + sy * Ay + sz * y);
+ const fvec j34 = ctdz * (sx * Bx - sy * xy - sz * x);
+
+ T.x += x * dz + j04 * qp;
+ T.y += y * dz + j14 * qp;
+ T.tx += j24 * qp;
+ T.ty += j34 * qp;
+ T.z += dz;
+ //T.t += sqrt((x-T.x)*(x-T.x)+(y-T.y)*(y-T.y)+dz*dz)/29.9792458f;
+
+ // covariance matrix transport
+
+ const fvec cj00 = T.C00 + T.C20 * dz + T.C40 * j04;
+ // const fvec cj10 = T.C10 + T.C21*dz + T.C41*j04;
+ const fvec cj20 = T.C20 + T.C22 * dz + T.C42 * j04;
+ const fvec cj30 = T.C30 + T.C32 * dz + T.C43 * j04;
+
+ const fvec cj01 = T.C10 + T.C30 * dz + T.C40 * j14;
+ const fvec cj11 = T.C11 + T.C31 * dz + T.C41 * j14;
+ const fvec cj21 = T.C21 + T.C32 * dz + T.C42 * j14;
+ const fvec cj31 = T.C31 + T.C33 * dz + T.C43 * j14;
+
+ // const fvec cj02 = T.C20 + T.C40*j24;
+ // const fvec cj12 = T.C21 + T.C41*j24;
+ const fvec cj22 = T.C22 + T.C42 * j24;
+ const fvec cj32 = T.C32 + T.C43 * j24;
+
+ // const fvec cj03 = T.C30 + T.C40*j34;
+ // const fvec cj13 = T.C31 + T.C41*j34;
+ // const fvec cj23 = T.C32 + T.C42*j34;
+ const fvec cj33 = T.C33 + T.C43 * j34;
+
+ T.C40 += T.C42 * dz + T.C44 * j04; // cj40
+ T.C41 += T.C43 * dz + T.C44 * j14; // cj41
+ T.C42 += T.C44 * j24; // cj42
+ T.C43 += T.C44 * j34; // cj43
+
+ T.C00 = cj00 + dz * cj20 + j04 * T.C40;
+ T.C10 = cj01 + dz * cj21 + j04 * T.C41;
+ T.C11 = cj11 + dz * cj31 + j14 * T.C41;
+
+ T.C20 = cj20 + j24 * T.C40;
+ T.C30 = cj30 + j34 * T.C40;
+ T.C21 = cj21 + j24 * T.C41;
+ T.C31 = cj31 + j34 * T.C41;
+ T.C22 = cj22 + j24 * T.C42;
+ T.C32 = cj32 + j34 * T.C42;
+ T.C33 = cj33 + j34 * T.C43;
+}
+
+
+void L1ExtrapolateTime(L1TrackPar& T, // input track parameters (x,y,tx,ty,Q/p) and cov.matrix
+ fvec dz, // extrapolate to this z position
+ fvec timeInfo)
+{
+ cnst c_light = 29.9792458;
+ dz = masked(dz, timeInfo >= 0);
+
+ T.t += dz * sqrt((T.tx * T.tx) + (T.ty * T.ty) + 1) / c_light;
+
+ const fvec k1 = dz * T.tx / (c_light * sqrt((T.tx * T.tx) + (T.ty * T.ty) + 1.));
+ const fvec k2 = dz * T.ty / (c_light * sqrt((T.tx * T.tx) + (T.ty * T.ty) + 1.));
+
+ fvec c52 = T.C52;
+ fvec c53 = T.C53;
+
+ T.C50 += k1 * T.C20 + k2 * T.C30;
+ T.C51 += k1 * T.C21 + k2 * T.C31;
+ T.C52 += k1 * T.C22 + k2 * T.C32;
+ T.C53 += k1 * T.C32 + k2 * T.C33;
+ T.C54 += k1 * T.C42 + k2 * T.C43;
+ T.C55 += k1 * (T.C52 + c52) + k2 * (T.C53 + c53);
+}
+
+void L1ExtrapolateLine(L1TrackPar& T, fvec z_out)
+{
+ fvec dz = (z_out - T.z);
+
+ T.x += T.tx * dz;
+ T.y += T.ty * dz;
+ T.z += dz;
+
+ const fvec dzC32_in = dz * T.C32;
+
+ T.C21 += dzC32_in;
+ T.C10 += dz * (T.C21 + T.C30);
+
+ const fvec C20_in = T.C20;
+
+ T.C20 += dz * T.C22;
+ T.C00 += dz * (T.C20 + C20_in);
+
+ const fvec C31_in = T.C31;
+
+ T.C31 += dz * T.C33;
+ T.C11 += dz * (T.C31 + C31_in);
+ T.C30 += dzC32_in;
+
+ T.C40 += dz * T.C42;
+ T.C41 += dz * T.C43;
+}
+
+
+void L1ExtrapolateJXY // is not used currently
+ (fvec& tx, fvec& ty,
+ fvec& qp, // input track parameters
+ fvec dz, // extrapolate to this dz
+ L1FieldRegion& F, fvec& extrDx, fvec& extrDy, fvec extrJ[])
+{
+ //
+ // Part of the analytic extrapolation formula with error (c_light*B*dz)^4/4!
+ //
+
+ // cnst ZERO = 0.0, ONE = 1.;
+ cnst c_light = 0.000299792458;
+
+ cnst c1 = 1., c2 = 2., c3 = 3., c4 = 4., c6 = 6., c9 = 9., c15 = 15., c18 = 18., c45 = 45., c2i = 1. / 2.,
+ c6i = 1. / 6., c12i = 1. / 12.;
+
+ fvec dz2 = dz * dz;
+ fvec dz3 = dz2 * dz;
+
+ // construct coefficients
+
+ fvec x = tx;
+ fvec y = ty;
+ fvec xx = x * x;
+ fvec xy = x * y;
+ fvec yy = y * y;
+ fvec y2 = y * c2;
+ fvec x2 = x * c2;
+ fvec x4 = x * c4;
+ fvec xx31 = xx * c3 + c1;
+ fvec xx159 = xx * c15 + c9;
+
+ fvec Ay = -xx - c1;
+ fvec Ayy = x * (xx * c3 + c3);
+ fvec Ayz = -c2 * xy;
+ fvec Ayyy = -(c15 * xx * xx + c18 * xx + c3);
+
+ fvec Ayy_x = c3 * xx31;
+ fvec Ayyy_x = -x4 * xx159;
+
+ fvec Bx = yy + c1;
+ fvec Byy = y * xx31;
+ fvec Byz = c2 * xx + c1;
+ fvec Byyy = -xy * xx159;
+
+ fvec Byy_x = c6 * xy;
+ fvec Byyy_x = -y * (c45 * xx + c9);
+ fvec Byyy_y = -x * xx159;
+
+ // end of coefficients calculation
+
+ fvec t2 = c1 + xx + yy;
+ fvec t = sqrt(t2);
+ fvec h = qp * c_light;
+ fvec ht = h * t;
+
+ // get field integrals
+ fvec Fx0 = F.cx0;
+ fvec Fx1 = F.cx1 * dz;
+ fvec Fx2 = F.cx2 * dz2;
+ fvec Fy0 = F.cy0;
+ fvec Fy1 = F.cy1 * dz;
+ fvec Fy2 = F.cy2 * dz2;
+ fvec Fz0 = F.cz0;
+ fvec Fz1 = F.cz1 * dz;
+ fvec Fz2 = F.cz2 * dz2;
+
+ //
+
+ fvec Sx = (Fx0 * c2i + Fx1 * c6i + Fx2 * c12i);
+ fvec Sy = (Fy0 * c2i + Fy1 * c6i + Fy2 * c12i);
+ fvec Sz = (Fz0 * c2i + Fz1 * c6i + Fz2 * c12i);
+
+ fvec Syz;
+ {
+ cnst d = 1. / 2520., c00 = 21. * 20. * d, c01 = 21. * 5. * d, c02 = 21. * 2. * d, c10 = 7. * 30. * d,
+ c11 = 7. * 9. * d, c12 = 7. * 4. * d, c20 = 2. * 63. * d, c21 = 2. * 21. * d, c22 = 2. * 10. * d;
+ Syz = Fy0 * (c00 * Fz0 + c01 * Fz1 + c02 * Fz2) + Fy1 * (c10 * Fz0 + c11 * Fz1 + c12 * Fz2)
+ + Fy2 * (c20 * Fz0 + c21 * Fz1 + c22 * Fz2);
+ }
+
+ fvec Syy;
+ {
+ cnst d = 1. / 2520., c00 = 420. * d, c01 = 21. * 15. * d, c02 = 21. * 8. * d, c03 = 63. * d, c04 = 70. * d,
+ c05 = 20. * d;
+ Syy = Fy0 * (c00 * Fy0 + c01 * Fy1 + c02 * Fy2) + Fy1 * (c03 * Fy1 + c04 * Fy2) + c05 * Fy2 * Fy2;
+ }
+
+ fvec Syyy;
+ {
+ cnst d = 1. / 181440., c000 = 7560 * d, c001 = 9 * 1008 * d, c002 = 5 * 1008 * d, c011 = 21 * 180 * d,
+ c012 = 24 * 180 * d, c022 = 7 * 180 * d, c111 = 540 * d, c112 = 945 * d, c122 = 560 * d, c222 = 112 * d;
+ fvec Fy22 = Fy2 * Fy2;
+ Syyy = Fy0 * (Fy0 * (c000 * Fy0 + c001 * Fy1 + c002 * Fy2) + Fy1 * (c011 * Fy1 + c012 * Fy2) + c022 * Fy22)
+ + Fy1 * (Fy1 * (c111 * Fy1 + c112 * Fy2) + c122 * Fy22) + c222 * Fy22 * Fy2;
+ }
+
+ fvec SA1 = Sx * xy + Sy * Ay + Sz * y;
+ fvec SA1_x = Sx * y - Sy * x2;
+ fvec SA1_y = Sx * x + Sz;
+
+ fvec SB1 = Sx * Bx - Sy * xy - Sz * x;
+ fvec SB1_x = -Sy * y - Sz;
+ fvec SB1_y = Sx * y2 - Sy * x;
+
+ fvec SA2 = Syy * Ayy + Syz * Ayz;
+ fvec SA2_x = Syy * Ayy_x - Syz * y2;
+ fvec SA2_y = -Syz * x2;
+ fvec SB2 = Syy * Byy + Syz * Byz;
+ fvec SB2_x = Syy * Byy_x + Syz * x4;
+ fvec SB2_y = Syy * xx31;
+
+ fvec SA3 = Syyy * Ayyy;
+ fvec SA3_x = Syyy * Ayyy_x;
+ fvec SB3 = Syyy * Byyy;
+ fvec SB3_x = Syyy * Byyy_x;
+ fvec SB3_y = Syyy * Byyy_y;
+
+ fvec ht1 = ht * dz;
+ fvec ht2 = ht * ht * dz2;
+ fvec ht3 = ht * ht * ht * dz3;
+ fvec ht1SA1 = ht1 * SA1;
+ fvec ht1SB1 = ht1 * SB1;
+ fvec ht2SA2 = ht2 * SA2;
+ fvec ht2SB2 = ht2 * SB2;
+ fvec ht3SA3 = ht3 * SA3;
+ fvec ht3SB3 = ht3 * SB3;
+
+ extrDx = (x + ht1SA1 + ht2SA2 + ht3SA3) * dz;
+ extrDy = (y + ht1SB1 + ht2SB2 + ht3SB3) * dz;
+
+ fvec ctdz2 = c_light * t * dz2;
+
+ fvec t2i = c1 / t2;
+ fvec xt2i = x * t2i;
+ fvec yt2i = y * t2i;
+ fvec tmp0 = ht1SA1 + c2 * ht2SA2 + c3 * ht3SA3;
+ fvec tmp1 = ht1SB1 + c2 * ht2SB2 + c3 * ht3SB3;
+
+ extrJ[0] = dz * (c1 + xt2i * tmp0 + ht1 * SA1_x + ht2 * SA2_x + ht3 * SA3_x); // j02
+ extrJ[1] = dz * (yt2i * tmp0 + ht1 * SA1_y + ht2 * SA2_y); // j03
+ extrJ[2] = ctdz2 * (SA1 + c2 * ht1 * SA2 + c3 * ht2 * SA3); // j04
+ extrJ[3] = dz * (xt2i * tmp1 + ht1 * SB1_x + ht2 * SB2_x + ht3 * SB3_x); // j12
+ extrJ[4] = dz * (c1 + yt2i * tmp1 + ht1 * SB1_y + ht2 * SB2_y + ht3 * SB3_y); // j13
+ extrJ[5] = ctdz2 * (SB1 + c2 * ht1 * SB2 + c3 * ht2 * SB3); // j14
+}
+
+void L1ExtrapolateJXY0(fvec& tx,
+ fvec& ty, // input track slopes
+ fvec dz, // extrapolate to this dz position
+ L1FieldRegion& F, fvec& extrDx, fvec& extrDy, fvec& J04, fvec& J14)
+{
+ cnst c_light = 0.000299792458, c1 = 1., c2i = 0.5, c6i = 1. / 6., c12i = 1. / 12.;
+
+ fvec dz2 = dz * dz;
+ fvec dzc6i = dz * c6i;
+ fvec dz2c12i = dz2 * c12i;
+
+ fvec xx = tx * tx;
+ fvec yy = ty * ty;
+ fvec xy = tx * ty;
+
+ fvec Ay = -xx - c1;
+ fvec Bx = yy + c1;
+
+ fvec ctdz2 = c_light * sqrt(c1 + xx + yy) * dz2;
+
+ fvec Sx = F.cx0 * c2i + F.cx1 * dzc6i + F.cx2 * dz2c12i;
+ fvec Sy = F.cy0 * c2i + F.cy1 * dzc6i + F.cy2 * dz2c12i;
+ fvec Sz = F.cz0 * c2i + F.cz1 * dzc6i + F.cz2 * dz2c12i;
+
+ extrDx = tx * dz;
+ extrDy = ty * dz;
+ J04 = ctdz2 * (Sx * xy + Sy * Ay + Sz * ty);
+ J14 = ctdz2 * (Sx * Bx - Sy * xy - Sz * tx);
+}
+
+#undef cnst
diff --git a/reco/L1/L1Algo/L1Extrapolation.h b/reco/L1/L1Algo/L1Extrapolation.h
index d71177c41d50b50ca03cf602d3bfeba9e1654527..4887f7769fc315681ced4d096b0011ae0664bf1d 100644
--- a/reco/L1/L1Algo/L1Extrapolation.h
+++ b/reco/L1/L1Algo/L1Extrapolation.h
@@ -9,748 +9,33 @@
#include "L1Field.h"
#include "L1TrackPar.h"
+void L1Extrapolate // extrapolates track parameters and returns jacobian for extrapolation of CovMatrix
+ (L1TrackPar& T, // input track parameters (x,y,tx,ty,Q/p) and cov.matrix
+ fvec z_out, // extrapolate to this z position
+ fvec qp0, // use Q/p linearisation at this value
+ const L1FieldRegion& F, fvec* w = nullptr);
-//#define cnst static const fvec
-#define cnst const fvec
+void L1Extrapolate0(L1TrackPar& T, // input track parameters (x,y,tx,ty,Q/p) and cov.matrix
+ fvec z_out, // extrapolate to this z position
+ L1FieldRegion& F);
-// #define ANALYTICEXTRAPOLATION
-#ifdef ANALYTICEXTRAPOLATION
-inline void L1Extrapolate(L1TrackPar& T, // input track parameters (x,y,tx,ty,Q/p) and cov.matrix
- fvec z_out, // extrapolate to this z position
- fvec qp0, // use Q/p linearisation at this value
- L1FieldRegion& F, fvec* w = 0)
-{
- //cout<<"Extrapolation..."<<endl;
- //
- // Part of the analytic extrapolation formula with error (c_light*B*dz)^4/4!
- //
-
- cnst ZERO = 0.0, ONE = 1.;
- cnst c_light = 0.000299792458, c_light_i = 1. / c_light;
-
- cnst c1 = 1., c2 = 2., c3 = 3., c4 = 4., c6 = 6., c9 = 9., c15 = 15., c18 = 18., c45 = 45., c2i = 1. / 2.,
- c3i = 1. / 3., c6i = 1. / 6., c12i = 1. / 12.;
-
- const fvec qp = T.qp;
- const fvec dz = (z_out - T.z);
- const fvec dz2 = dz * dz;
- const fvec dz3 = dz2 * dz;
-
- // construct coefficients
-
- const fvec x = T.tx;
- const fvec y = T.ty;
- const fvec xx = x * x;
- const fvec xy = x * y;
- const fvec yy = y * y;
- const fvec y2 = y * c2;
- const fvec x2 = x * c2;
- const fvec x4 = x * c4;
- const fvec xx31 = xx * c3 + c1;
- const fvec xx159 = xx * c15 + c9;
-
- const fvec Ay = -xx - c1;
- const fvec Ayy = x * (xx * c3 + c3);
- const fvec Ayz = -c2 * xy;
- const fvec Ayyy = -(c15 * xx * xx + c18 * xx + c3);
-
- const fvec Ayy_x = c3 * xx31;
- const fvec Ayyy_x = -x4 * xx159;
-
- const fvec Bx = yy + c1;
- const fvec Byy = y * xx31;
- const fvec Byz = c2 * xx + c1;
- const fvec Byyy = -xy * xx159;
-
- const fvec Byy_x = c6 * xy;
- const fvec Byyy_x = -y * (c45 * xx + c9);
- const fvec Byyy_y = -x * xx159;
-
- // end of coefficients calculation
-
- const fvec t2 = c1 + xx + yy;
- const fvec t = sqrt(t2);
- const fvec h = qp0 * c_light;
- const fvec ht = h * t;
-
- // get field integrals
- const fvec ddz = T.z - F.z0;
- const fvec Fx0 = F.cx0 + F.cx1 * ddz + F.cx2 * ddz * ddz;
- const fvec Fx1 = (F.cx1 + c2 * F.cx2 * ddz) * dz;
- const fvec Fx2 = F.cx2 * dz2;
- const fvec Fy0 = F.cy0 + F.cy1 * ddz + F.cy2 * ddz * ddz;
- const fvec Fy1 = (F.cy1 + c2 * F.cy2 * ddz) * dz;
- const fvec Fy2 = F.cy2 * dz2;
- const fvec Fz0 = F.cz0 + F.cz1 * ddz + F.cz2 * ddz * ddz;
- const fvec Fz1 = (F.cz1 + c2 * F.cz2 * ddz) * dz;
- const fvec Fz2 = F.cz2 * dz2;
-
- //
-
- const fvec sx = (Fx0 + Fx1 * c2i + Fx2 * c3i);
- const fvec sy = (Fy0 + Fy1 * c2i + Fy2 * c3i);
- const fvec sz = (Fz0 + Fz1 * c2i + Fz2 * c3i);
-
- const fvec Sx = (Fx0 * c2i + Fx1 * c6i + Fx2 * c12i);
- const fvec Sy = (Fy0 * c2i + Fy1 * c6i + Fy2 * c12i);
- const fvec Sz = (Fz0 * c2i + Fz1 * c6i + Fz2 * c12i);
-
- fvec syz;
- {
- cnst d = 1. / 360., c00 = 30. * 6. * d, c01 = 30. * 2. * d, c02 = 30. * d, c10 = 3. * 40. * d, c11 = 3. * 15. * d,
- c12 = 3. * 8. * d, c20 = 2. * 45. * d, c21 = 2. * 2. * 9. * d, c22 = 2. * 2. * 5. * d;
- syz = Fy0 * (c00 * Fz0 + c01 * Fz1 + c02 * Fz2) + Fy1 * (c10 * Fz0 + c11 * Fz1 + c12 * Fz2)
- + Fy2 * (c20 * Fz0 + c21 * Fz1 + c22 * Fz2);
- }
-
- fvec Syz;
- {
- cnst d = 1. / 2520., c00 = 21. * 20. * d, c01 = 21. * 5. * d, c02 = 21. * 2. * d, c10 = 7. * 30. * d,
- c11 = 7. * 9. * d, c12 = 7. * 4. * d, c20 = 2. * 63. * d, c21 = 2. * 21. * d, c22 = 2. * 10. * d;
- Syz = Fy0 * (c00 * Fz0 + c01 * Fz1 + c02 * Fz2) + Fy1 * (c10 * Fz0 + c11 * Fz1 + c12 * Fz2)
- + Fy2 * (c20 * Fz0 + c21 * Fz1 + c22 * Fz2);
- }
-
- const fvec syy = sy * sy * c2i;
- const fvec syyy = syy * sy * c3i;
-
- fvec Syy;
- {
- cnst d = 1. / 2520., c00 = 420. * d, c01 = 21. * 15. * d, c02 = 21. * 8. * d, c03 = 63. * d, c04 = 70. * d,
- c05 = 20. * d;
- Syy = Fy0 * (c00 * Fy0 + c01 * Fy1 + c02 * Fy2) + Fy1 * (c03 * Fy1 + c04 * Fy2) + c05 * Fy2 * Fy2;
- }
-
- fvec Syyy;
- {
- cnst d = 1. / 181440., c000 = 7560 * d, c001 = 9 * 1008 * d, c002 = 5 * 1008 * d, c011 = 21 * 180 * d,
- c012 = 24 * 180 * d, c022 = 7 * 180 * d, c111 = 540 * d, c112 = 945 * d, c122 = 560 * d, c222 = 112 * d;
- const fvec Fy22 = Fy2 * Fy2;
- Syyy = Fy0 * (Fy0 * (c000 * Fy0 + c001 * Fy1 + c002 * Fy2) + Fy1 * (c011 * Fy1 + c012 * Fy2) + c022 * Fy22)
- + Fy1 * (Fy1 * (c111 * Fy1 + c112 * Fy2) + c122 * Fy22) + c222 * Fy22 * Fy2;
- }
-
-
- const fvec sA1 = sx * xy + sy * Ay + sz * y;
- const fvec sA1_x = sx * y - sy * x2;
- const fvec sA1_y = sx * x + sz;
-
- const fvec sB1 = sx * Bx - sy * xy - sz * x;
- const fvec sB1_x = -sy * y - sz;
- const fvec sB1_y = sx * y2 - sy * x;
-
- const fvec SA1 = Sx * xy + Sy * Ay + Sz * y;
- const fvec SA1_x = Sx * y - Sy * x2;
- const fvec SA1_y = Sx * x + Sz;
-
- const fvec SB1 = Sx * Bx - Sy * xy - Sz * x;
- const fvec SB1_x = -Sy * y - Sz;
- const fvec SB1_y = Sx * y2 - Sy * x;
-
-
- const fvec sA2 = syy * Ayy + syz * Ayz;
- const fvec sA2_x = syy * Ayy_x - syz * y2;
- const fvec sA2_y = -syz * x2;
- const fvec sB2 = syy * Byy + syz * Byz;
- const fvec sB2_x = syy * Byy_x + syz * x4;
- const fvec sB2_y = syy * xx31;
-
- const fvec SA2 = Syy * Ayy + Syz * Ayz;
- const fvec SA2_x = Syy * Ayy_x - Syz * y2;
- const fvec SA2_y = -Syz * x2;
- const fvec SB2 = Syy * Byy + Syz * Byz;
- const fvec SB2_x = Syy * Byy_x + Syz * x4;
- const fvec SB2_y = Syy * xx31;
-
- const fvec sA3 = syyy * Ayyy;
- const fvec sA3_x = syyy * Ayyy_x;
- const fvec sB3 = syyy * Byyy;
- const fvec sB3_x = syyy * Byyy_x;
- const fvec sB3_y = syyy * Byyy_y;
-
-
- const fvec SA3 = Syyy * Ayyy;
- const fvec SA3_x = Syyy * Ayyy_x;
- const fvec SB3 = Syyy * Byyy;
- const fvec SB3_x = Syyy * Byyy_x;
- const fvec SB3_y = Syyy * Byyy_y;
-
- const fvec ht1 = ht * dz;
- const fvec ht2 = ht * ht * dz2;
- const fvec ht3 = ht * ht * ht * dz3;
- const fvec ht1sA1 = ht1 * sA1;
- const fvec ht1sB1 = ht1 * sB1;
- const fvec ht1SA1 = ht1 * SA1;
- const fvec ht1SB1 = ht1 * SB1;
- const fvec ht2sA2 = ht2 * sA2;
- const fvec ht2SA2 = ht2 * SA2;
- const fvec ht2sB2 = ht2 * sB2;
- const fvec ht2SB2 = ht2 * SB2;
- const fvec ht3sA3 = ht3 * sA3;
- const fvec ht3sB3 = ht3 * sB3;
- const fvec ht3SA3 = ht3 * SA3;
- const fvec ht3SB3 = ht3 * SB3;
-
- fvec initialised = ZERO;
- if (w) //TODO use operator {?:}
- {
- const fvec zero = ZERO;
- initialised = fvec(zero < *w);
- }
- else {
- const fvec one = ONE;
- const fvec zero = ZERO;
- initialised = fvec(zero < one);
- }
-
- T.x += (((x + ht1SA1 + ht2SA2 + ht3SA3) * dz) & initialised);
- T.y += (((y + ht1SB1 + ht2SB2 + ht3SB3) * dz) & initialised);
- T.tx += ((ht1sA1 + ht2sA2 + ht3sA3) & initialised);
- T.ty += ((ht1sB1 + ht2sB2 + ht3sB3) & initialised);
- T.z += ((dz) &initialised);
-
- const fvec ctdz = c_light * t * dz;
- const fvec ctdz2 = c_light * t * dz2;
-
- const fvec dqp = qp - qp0;
- const fvec t2i = c1 * rcp(t2); // /t2;
- const fvec xt2i = x * t2i;
- const fvec yt2i = y * t2i;
- const fvec tmp0 = ht1SA1 + c2 * ht2SA2 + c3 * ht3SA3;
- const fvec tmp1 = ht1SB1 + c2 * ht2SB2 + c3 * ht3SB3;
- const fvec tmp2 = ht1sA1 + c2 * ht2sA2 + c3 * ht3sA3;
- const fvec tmp3 = ht1sB1 + c2 * ht2sB2 + c3 * ht3sB3;
-
- const fvec j02 = dz * (c1 + xt2i * tmp0 + ht1 * SA1_x + ht2 * SA2_x + ht3 * SA3_x);
- const fvec j12 = dz * (xt2i * tmp1 + ht1 * SB1_x + ht2 * SB2_x + ht3 * SB3_x);
- const fvec j22 = c1 + xt2i * tmp2 + ht1 * sA1_x + ht2 * sA2_x + ht3 * sA3_x;
- const fvec j32 = xt2i * tmp3 + ht1 * sB1_x + ht2 * sB2_x + ht3 * sB3_x;
-
- const fvec j03 = dz * (yt2i * tmp0 + ht1 * SA1_y + ht2 * SA2_y);
- const fvec j13 = dz * (c1 + yt2i * tmp1 + ht1 * SB1_y + ht2 * SB2_y + ht3 * SB3_y);
- const fvec j23 = yt2i * tmp2 + ht1 * sA1_y + ht2 * sA2_y;
- const fvec j33 = c1 + yt2i * tmp3 + ht1 * sB1_y + ht2 * sB2_y + ht3 * sB3_y;
-
- const fvec j04 = ctdz2 * (SA1 + c2 * ht1 * SA2 + c3 * ht2 * SA3);
- const fvec j14 = ctdz2 * (SB1 + c2 * ht1 * SB2 + c3 * ht2 * SB3);
- const fvec j24 = ctdz * (sA1 + c2 * ht1 * sA2 + c3 * ht2 * sA3);
- const fvec j34 = ctdz * (sB1 + c2 * ht1 * sB2 + c3 * ht2 * sB3);
-
-
- // extrapolate inverse momentum
-
- T.x += ((j04 * dqp) & initialised);
- T.y += ((j14 * dqp) & initialised);
- T.tx += ((j24 * dqp) & initialised);
- T.ty += ((j34 * dqp) & initialised);
-
- // covariance matrix transport
-
- const fvec c42 = T.C42, c43 = T.C43;
-
- const fvec cj00 = T.C00 + T.C20 * j02 + T.C30 * j03 + T.C40 * j04;
- // const fvec cj10 = T.C10 + T.C21*j02 + T.C31*j03 + T.C41*j04;
- const fvec cj20 = T.C20 + T.C22 * j02 + T.C32 * j03 + c42 * j04;
- const fvec cj30 = T.C30 + T.C32 * j02 + T.C33 * j03 + c43 * j04;
-
- const fvec cj01 = T.C10 + T.C20 * j12 + T.C30 * j13 + T.C40 * j14;
- const fvec cj11 = T.C11 + T.C21 * j12 + T.C31 * j13 + T.C41 * j14;
- const fvec cj21 = T.C21 + T.C22 * j12 + T.C32 * j13 + c42 * j14;
- const fvec cj31 = T.C31 + T.C32 * j12 + T.C33 * j13 + c43 * j14;
-
- // const fvec cj02 = T.C20*j22 + T.C30*j23 + T.C40*j24;
- // const fvec cj12 = T.C21*j22 + T.C31*j23 + T.C41*j24;
- const fvec cj22 = T.C22 * j22 + T.C32 * j23 + c42 * j24;
- const fvec cj32 = T.C32 * j22 + T.C33 * j23 + c43 * j24;
-
- // const fvec cj03 = T.C20*j32 + T.C30*j33 + T.C40*j34;
- // const fvec cj13 = T.C21*j32 + T.C31*j33 + T.C41*j34;
- const fvec cj23 = T.C22 * j32 + T.C32 * j33 + c42 * j34;
- const fvec cj33 = T.C32 * j32 + T.C33 * j33 + c43 * j34;
-
- T.C40 += ((c42 * j02 + c43 * j03 + T.C44 * j04) & initialised); // cj40
- T.C41 += ((c42 * j12 + c43 * j13 + T.C44 * j14) & initialised); // cj41
- T.C42 = ((c42 * j22 + c43 * j23 + T.C44 * j24) & initialised) + (T.C42 & (!initialised)); // cj42
- T.C43 = ((c42 * j32 + c43 * j33 + T.C44 * j34) & initialised) + (T.C43 & (!initialised)); // cj43
-
- T.C00 = ((cj00 + j02 * cj20 + j03 * cj30 + j04 * T.C40) & initialised) + (T.C00 & (!initialised));
- T.C10 = ((cj01 + j02 * cj21 + j03 * cj31 + j04 * T.C41) & initialised) + (T.C10 & (!initialised));
- T.C11 = ((cj11 + j12 * cj21 + j13 * cj31 + j14 * T.C41) & initialised) + (T.C11 & (!initialised));
-
- T.C20 = ((j22 * cj20 + j23 * cj30 + j24 * T.C40) & initialised) + (T.C20 & (!initialised));
- T.C30 = ((j32 * cj20 + j33 * cj30 + j34 * T.C40) & initialised) + (T.C30 & (!initialised));
- T.C21 = ((j22 * cj21 + j23 * cj31 + j24 * T.C41) & initialised) + (T.C21 & (!initialised));
- T.C31 = ((j32 * cj21 + j33 * cj31 + j34 * T.C41) & initialised) + (T.C31 & (!initialised));
- T.C22 = ((j22 * cj22 + j23 * cj32 + j24 * T.C42) & initialised) + (T.C22 & (!initialised));
- T.C32 = ((j32 * cj22 + j33 * cj32 + j34 * T.C42) & initialised) + (T.C32 & (!initialised));
- T.C33 = ((j32 * cj23 + j33 * cj33 + j34 * T.C43) & initialised) + (T.C33 & (!initialised));
- //cout<<"Extrapolation ok"<<endl;
-}
-#else
-inline void L1Extrapolate // extrapolates track parameters and returns jacobian for extrapolation of CovMatrix
- (L1TrackPar& T, // input track parameters (x,y,tx,ty,Q/p) and cov.matrix
- fvec z_out, // extrapolate to this z position
- fvec qp0, // use Q/p linearisation at this value
- const L1FieldRegion& F, fvec* w = 0)
-{
- //
- // Forth-order Runge-Kutta method for solution of the equation
- // of motion of a particle with parameter qp = Q /P
- // in the magnetic field B()
- //
- // | x | tx
- // | y | ty
- // d/dz = | tx| = ft * ( ty * ( B(3)+tx*b(1) ) - (1+tx**2)*B(2) )
- // | ty| ft * (-tx * ( B(3)+ty+b(2) - (1+ty**2)*B(1) ) ,
- //
- // where ft = c_light*qp*sqrt ( 1 + tx**2 + ty**2 ) .
- //
- // In the following for RK step :
- //
- // x=x[0], y=x[1], tx=x[3], ty=x[4].
- //
- //========================================================================
- //
- // NIM A395 (1997) 169-184; NIM A426 (1999) 268-282.
- //
- // the routine is based on LHC(b) utility code
- //
- //========================================================================
-
-
- cnst ZERO = 0.0, ONE = 1.;
- cnst c_light = 0.000299792458;
-
- const fvec a[4] = {0.0f, 0.5f, 0.5f, 1.0f};
- const fvec c[4] = {1.0f / 6.0f, 1.0f / 3.0f, 1.0f / 3.0f, 1.0f / 6.0f};
- const fvec b[4] = {0.0f, 0.5f, 0.5f, 1.0f};
-
- int step4;
- fvec k[16], x0[4], x[4], k1[16];
- fvec Ax[4], Ay[4], Ax_tx[4], Ay_tx[4], Ax_ty[4], Ay_ty[4];
-
- //----------------------------------------------------------------
-
- fvec qp_in = T.qp;
- const fvec z_in = T.z;
- const fvec h = (z_out - T.z);
- fvec hC = h * c_light;
- x0[0] = T.x;
- x0[1] = T.y;
- x0[2] = T.tx;
- x0[3] = T.ty;
- //
- // Runge-Kutta step
- //
-
- int step;
- int i;
-
- fvec B[4][3];
- for (step = 0; step < 4; ++step) {
- F.Get(z_in + a[step] * h, B[step]);
- }
-
- for (step = 0; step < 4; ++step) {
- for (i = 0; i < 4; ++i) {
- if (step == 0) { x[i] = x0[i]; }
- else {
- x[i] = x0[i] + b[step] * k[step * 4 - 4 + i];
- }
- }
-
- fvec tx = x[2];
- fvec ty = x[3];
- fvec tx2 = tx * tx;
- fvec ty2 = ty * ty;
- fvec txty = tx * ty;
- fvec tx2ty21 = 1.f + tx2 + ty2;
- // if( tx2ty21 > 1.e4 ) return 1;
- fvec I_tx2ty21 = 1.f / tx2ty21 * qp0;
- fvec tx2ty2 = sqrt(tx2ty21);
- // fvec I_tx2ty2 = qp0 * hC / tx2ty2 ; unsused ???
- tx2ty2 *= hC;
- fvec tx2ty2qp = tx2ty2 * qp0;
- Ax[step] = (txty * B[step][0] + ty * B[step][2] - (1.f + tx2) * B[step][1]) * tx2ty2;
- Ay[step] = (-txty * B[step][1] - tx * B[step][2] + (1.f + ty2) * B[step][0]) * tx2ty2;
-
- Ax_tx[step] = Ax[step] * tx * I_tx2ty21 + (ty * B[step][0] - 2.f * tx * B[step][1]) * tx2ty2qp;
- Ax_ty[step] = Ax[step] * ty * I_tx2ty21 + (tx * B[step][0] + B[step][2]) * tx2ty2qp;
- Ay_tx[step] = Ay[step] * tx * I_tx2ty21 + (-ty * B[step][1] - B[step][2]) * tx2ty2qp;
- Ay_ty[step] = Ay[step] * ty * I_tx2ty21 + (-tx * B[step][1] + 2.f * ty * B[step][0]) * tx2ty2qp;
-
- step4 = step * 4;
- k[step4] = tx * h;
- k[step4 + 1] = ty * h;
- k[step4 + 2] = Ax[step] * qp0;
- k[step4 + 3] = Ay[step] * qp0;
-
- } // end of Runge-Kutta steps
-
- fvec initialised = ZERO;
- if (w) //TODO use operator {?:}
- {
- const fvec zero = ZERO;
- initialised = fvec(zero < *w);
- }
- else {
- const fvec one = ONE;
- const fvec zero = ZERO;
- initialised = fvec(zero < one);
- }
-
- {
- T.x = ((x0[0] + c[0] * k[0] + c[1] * k[4 + 0] + c[2] * k[8 + 0] + c[3] * k[12 + 0]) & initialised)
- + ((!initialised) & T.x);
- T.y = ((x0[1] + c[0] * k[1] + c[1] * k[4 + 1] + c[2] * k[8 + 1] + c[3] * k[12 + 1]) & initialised)
- + ((!initialised) & T.y);
- T.tx = ((x0[2] + c[0] * k[2] + c[1] * k[4 + 2] + c[2] * k[8 + 2] + c[3] * k[12 + 2]) & initialised)
- + ((!initialised) & T.tx);
- T.ty = ((x0[3] + c[0] * k[3] + c[1] * k[4 + 3] + c[2] * k[8 + 3] + c[3] * k[12 + 3]) & initialised)
- + ((!initialised) & T.ty);
- T.z = (z_out & initialised) + ((!initialised) & T.z);
- }
-
- //
- // Derivatives dx/dqp
- //
-
- x0[0] = 0.f;
- x0[1] = 0.f;
- x0[2] = 0.f;
- x0[3] = 0.f;
-
- //
- // Runge-Kutta step for derivatives dx/dqp
-
- for (step = 0; step < 4; ++step) {
- for (i = 0; i < 4; ++i) {
- if (step == 0) { x[i] = x0[i]; }
- else {
- x[i] = x0[i] + b[step] * k1[step * 4 - 4 + i];
- }
- }
- step4 = step * 4;
- k1[step4] = x[2] * h;
- k1[step4 + 1] = x[3] * h;
- k1[step4 + 2] = Ax[step] + Ax_tx[step] * x[2] + Ax_ty[step] * x[3];
- k1[step4 + 3] = Ay[step] + Ay_tx[step] * x[2] + Ay_ty[step] * x[3];
-
- } // end of Runge-Kutta steps for derivatives dx/dqp
-
- fvec J[25];
-
- for (i = 0; i < 4; ++i) {
- J[20 + i] = x0[i] + c[0] * k1[i] + c[1] * k1[4 + i] + c[2] * k1[8 + i] + c[3] * k1[12 + i];
- }
- J[24] = 1.;
- //
- // end of derivatives dx/dqp
- //
-
- // Derivatives dx/tx
- //
-
- x0[0] = 0.f;
- x0[1] = 0.f;
- x0[2] = 1.f;
- x0[3] = 0.f;
-
- //
- // Runge-Kutta step for derivatives dx/dtx
- //
-
- for (step = 0; step < 4; ++step) {
- for (i = 0; i < 4; ++i) {
- if (step == 0) { x[i] = x0[i]; }
- else if (i != 2) {
- x[i] = x0[i] + b[step] * k1[step * 4 - 4 + i];
- }
- }
- step4 = step * 4;
- k1[step4] = x[2] * h;
- k1[step4 + 1] = x[3] * h;
- // k1[step4+2] = Ax_tx[step] * x[2] + Ax_ty[step] * x[3];
- k1[step4 + 3] = Ay_tx[step] * x[2] + Ay_ty[step] * x[3];
-
- } // end of Runge-Kutta steps for derivatives dx/dtx
-
- for (i = 0; i < 4; ++i) {
- if (i != 2) { J[10 + i] = x0[i] + c[0] * k1[i] + c[1] * k1[4 + i] + c[2] * k1[8 + i] + c[3] * k1[12 + i]; }
- }
- // end of derivatives dx/dtx
- J[12] = 1.f;
- J[14] = 0.f;
-
- // Derivatives dx/ty
- //
-
- x0[0] = 0.f;
- x0[1] = 0.f;
- x0[2] = 0.f;
- x0[3] = 1.f;
-
- //
- // Runge-Kutta step for derivatives dx/dty
- //
-
- for (step = 0; step < 4; ++step) {
- for (i = 0; i < 4; ++i) {
- if (step == 0) {
- x[i] = x0[i]; // ty fixed
- }
- else if (i != 3) {
- x[i] = x0[i] + b[step] * k1[step * 4 - 4 + i];
- }
- }
- step4 = step * 4;
- k1[step4] = x[2] * h;
- k1[step4 + 1] = x[3] * h;
- k1[step4 + 2] = Ax_tx[step] * x[2] + Ax_ty[step] * x[3];
- // k1[step4+3] = Ay_tx[step] * x[2] + Ay_ty[step] * x[3];
-
- } // end of Runge-Kutta steps for derivatives dx/dty
-
- for (i = 0; i < 3; ++i) {
- J[15 + i] = x0[i] + c[0] * k1[i] + c[1] * k1[4 + i] + c[2] * k1[8 + i] + c[3] * k1[12 + i];
- }
- // end of derivatives dx/dty
- J[18] = 1.;
- J[19] = 0.;
-
- //
- // derivatives dx/dx and dx/dy
-
- for (i = 0; i < 10; ++i) {
- J[i] = 0.;
- }
- J[0] = 1.;
- J[6] = 1.;
-
- fvec dqp = qp_in - qp0;
-
- { // update parameters
- T.x += ((J[5 * 4 + 0] * dqp) & initialised);
- T.y += ((J[5 * 4 + 1] * dqp) & initialised);
- T.tx += ((J[5 * 4 + 2] * dqp) & initialised);
- T.ty += ((J[5 * 4 + 3] * dqp) & initialised);
- }
-
- // covariance matrix transport
-
- // // if ( C_in&&C_out ) CbmKFMath::multQtSQ( 5, J, C_in, C_out); // TODO
- // j(0,2) = J[5*2 + 0];
- // j(1,2) = J[5*2 + 1];
- // j(2,2) = J[5*2 + 2];
- // j(3,2) = J[5*2 + 3];
- //
- // j(0,3) = J[5*3 + 0];
- // j(1,3) = J[5*3 + 1];
- // j(2,3) = J[5*3 + 2];
- // j(3,3) = J[5*3 + 3];
- //
- // j(0,4) = J[5*4 + 0];
- // j(1,4) = J[5*4 + 1];
- // j(2,4) = J[5*4 + 2];
- // j(3,4) = J[5*4 + 3];
-
- const fvec c42 = T.C42, c43 = T.C43;
+void L1ExtrapolateTime(L1TrackPar& T, // input track parameters (x,y,tx,ty,Q/p) and cov.matrix
+ fvec dz, // extrapolate to this z position
+ fvec timeInfo = fvec::One());
- const fvec cj00 = T.C00 + T.C20 * J[5 * 2 + 0] + T.C30 * J[5 * 3 + 0] + T.C40 * J[5 * 4 + 0];
- // const fvec cj10 = T.C10 + T.C21*J[5*2 + 0] + T.C31*J[5*3 + 0] + T.C41*J[5*4 + 0];
- const fvec cj20 = T.C20 + T.C22 * J[5 * 2 + 0] + T.C32 * J[5 * 3 + 0] + c42 * J[5 * 4 + 0];
- const fvec cj30 = T.C30 + T.C32 * J[5 * 2 + 0] + T.C33 * J[5 * 3 + 0] + c43 * J[5 * 4 + 0];
+void L1ExtrapolateLine(L1TrackPar& T, fvec z_out);
- const fvec cj01 = T.C10 + T.C20 * J[5 * 2 + 1] + T.C30 * J[5 * 3 + 1] + T.C40 * J[5 * 4 + 1];
- const fvec cj11 = T.C11 + T.C21 * J[5 * 2 + 1] + T.C31 * J[5 * 3 + 1] + T.C41 * J[5 * 4 + 1];
- const fvec cj21 = T.C21 + T.C22 * J[5 * 2 + 1] + T.C32 * J[5 * 3 + 1] + c42 * J[5 * 4 + 1];
- const fvec cj31 = T.C31 + T.C32 * J[5 * 2 + 1] + T.C33 * J[5 * 3 + 1] + c43 * J[5 * 4 + 1];
-
- // const fvec cj02 = T.C20*J[5*2 + 2] + T.C30*J[5*3 + 2] + T.C40*J[5*4 + 2];
- // const fvec cj12 = T.C21*J[5*2 + 2] + T.C31*J[5*3 + 2] + T.C41*J[5*4 + 2];
- const fvec cj22 = T.C22 * J[5 * 2 + 2] + T.C32 * J[5 * 3 + 2] + c42 * J[5 * 4 + 2];
- const fvec cj32 = T.C32 * J[5 * 2 + 2] + T.C33 * J[5 * 3 + 2] + c43 * J[5 * 4 + 2];
-
- // const fvec cj03 = T.C20*J[5*2 + 3] + T.C30*J[5*3 + 3] + T.C40*J[5*4 + 3];
- // const fvec cj13 = T.C21*J[5*2 + 3] + T.C31*J[5*3 + 3] + T.C41*J[5*4 + 3];
- const fvec cj23 = T.C22 * J[5 * 2 + 3] + T.C32 * J[5 * 3 + 3] + c42 * J[5 * 4 + 3];
- const fvec cj33 = T.C32 * J[5 * 2 + 3] + T.C33 * J[5 * 3 + 3] + c43 * J[5 * 4 + 3];
-
- T.C40 += ((c42 * J[5 * 2 + 0] + c43 * J[5 * 3 + 0] + T.C44 * J[5 * 4 + 0]) & initialised); // cj40
- T.C41 += ((c42 * J[5 * 2 + 1] + c43 * J[5 * 3 + 1] + T.C44 * J[5 * 4 + 1]) & initialised); // cj41
- T.C42 = ((c42 * J[5 * 2 + 2] + c43 * J[5 * 3 + 2] + T.C44 * J[5 * 4 + 2]) & initialised) + ((!initialised) & T.C42);
- T.C43 = ((c42 * J[5 * 2 + 3] + c43 * J[5 * 3 + 3] + T.C44 * J[5 * 4 + 3]) & initialised) + ((!initialised) & T.C43);
-
- T.C00 = ((cj00 + J[5 * 2 + 0] * cj20 + J[5 * 3 + 0] * cj30 + J[5 * 4 + 0] * T.C40) & initialised)
- + ((!initialised) & T.C00);
- T.C10 = ((cj01 + J[5 * 2 + 0] * cj21 + J[5 * 3 + 0] * cj31 + J[5 * 4 + 0] * T.C41) & initialised)
- + ((!initialised) & T.C10);
- T.C11 = ((cj11 + J[5 * 2 + 1] * cj21 + J[5 * 3 + 1] * cj31 + J[5 * 4 + 1] * T.C41) & initialised)
- + ((!initialised) & T.C11);
-
- T.C20 = ((J[5 * 2 + 2] * cj20 + J[5 * 3 + 2] * cj30 + J[5 * 4 + 2] * T.C40) & initialised) + ((!initialised) & T.C20);
- T.C30 = ((J[5 * 2 + 3] * cj20 + J[5 * 3 + 3] * cj30 + J[5 * 4 + 3] * T.C40) & initialised) + ((!initialised) & T.C30);
- T.C21 = ((J[5 * 2 + 2] * cj21 + J[5 * 3 + 2] * cj31 + J[5 * 4 + 2] * T.C41) & initialised) + ((!initialised) & T.C21);
- T.C31 = ((J[5 * 2 + 3] * cj21 + J[5 * 3 + 3] * cj31 + J[5 * 4 + 3] * T.C41) & initialised) + ((!initialised) & T.C31);
- T.C22 = ((J[5 * 2 + 2] * cj22 + J[5 * 3 + 2] * cj32 + J[5 * 4 + 2] * T.C42) & initialised) + ((!initialised) & T.C22);
- T.C32 = ((J[5 * 2 + 3] * cj22 + J[5 * 3 + 3] * cj32 + J[5 * 4 + 3] * T.C42) & initialised) + ((!initialised) & T.C32);
- T.C33 = ((J[5 * 2 + 3] * cj23 + J[5 * 3 + 3] * cj33 + J[5 * 4 + 3] * T.C43) & initialised) + ((!initialised) & T.C33);
-}
-#endif //ANALYTICEXTRAPOLATION
-
-inline void L1Extrapolate0(L1TrackPar& T, // input track parameters (x,y,tx,ty,Q/p) and cov.matrix
- fvec z_out, // extrapolate to this z position
- L1FieldRegion& F) // magneti
-{
- //
- // Part of the analytic extrapolation formula with error (c_light*B*dz)^4/4!
- //
-
- cnst c_light = 0.000299792458;
-
- cnst c1 = 1., c2i = 1. / 2., c3i = 1. / 3., c6i = 1. / 6., c12i = 1. / 12.;
-
- const fvec qp = T.qp;
-
-
- const fvec dz = (z_out - T.z);
- const fvec dz2 = dz * dz;
-
- // construct coefficients
-
- const fvec x = T.tx;
- const fvec y = T.ty;
- // const fvec z = T.z;
- const fvec xx = x * x;
- const fvec xy = x * y;
- const fvec yy = y * y;
- const fvec Ay = -xx - c1;
- const fvec Bx = yy + c1;
- const fvec ct = c_light * sqrt(c1 + xx + yy);
-
- const fvec dzc2i = dz * c2i;
- const fvec dz2c3i = dz2 * c3i;
- const fvec dzc6i = dz * c6i;
- const fvec dz2c12i = dz2 * c12i;
-
- const fvec sx = (F.cx0 + F.cx1 * dzc2i + F.cx2 * dz2c3i);
- const fvec sy = (F.cy0 + F.cy1 * dzc2i + F.cy2 * dz2c3i);
- const fvec sz = (F.cz0 + F.cz1 * dzc2i + F.cz2 * dz2c3i);
-
- const fvec Sx = (F.cx0 * c2i + F.cx1 * dzc6i + F.cx2 * dz2c12i);
- const fvec Sy = (F.cy0 * c2i + F.cy1 * dzc6i + F.cy2 * dz2c12i);
- const fvec Sz = (F.cz0 * c2i + F.cz1 * dzc6i + F.cz2 * dz2c12i);
-
-
- const fvec ctdz = ct * dz;
- const fvec ctdz2 = ct * dz2;
-
- const fvec j04 = ctdz2 * (Sx * xy + Sy * Ay + Sz * y);
- const fvec j14 = ctdz2 * (Sx * Bx - Sy * xy - Sz * x);
- const fvec j24 = ctdz * (sx * xy + sy * Ay + sz * y);
- const fvec j34 = ctdz * (sx * Bx - sy * xy - sz * x);
-
- T.x += x * dz + j04 * qp;
- T.y += y * dz + j14 * qp;
- T.tx += j24 * qp;
- T.ty += j34 * qp;
- T.z += dz;
- //T.t += sqrt((x-T.x)*(x-T.x)+(y-T.y)*(y-T.y)+dz*dz)/29.9792458f;
-
- // covariance matrix transport
-
- const fvec cj00 = T.C00 + T.C20 * dz + T.C40 * j04;
- // const fvec cj10 = T.C10 + T.C21*dz + T.C41*j04;
- const fvec cj20 = T.C20 + T.C22 * dz + T.C42 * j04;
- const fvec cj30 = T.C30 + T.C32 * dz + T.C43 * j04;
-
- const fvec cj01 = T.C10 + T.C30 * dz + T.C40 * j14;
- const fvec cj11 = T.C11 + T.C31 * dz + T.C41 * j14;
- const fvec cj21 = T.C21 + T.C32 * dz + T.C42 * j14;
- const fvec cj31 = T.C31 + T.C33 * dz + T.C43 * j14;
-
- // const fvec cj02 = T.C20 + T.C40*j24;
- // const fvec cj12 = T.C21 + T.C41*j24;
- const fvec cj22 = T.C22 + T.C42 * j24;
- const fvec cj32 = T.C32 + T.C43 * j24;
-
- // const fvec cj03 = T.C30 + T.C40*j34;
- // const fvec cj13 = T.C31 + T.C41*j34;
- // const fvec cj23 = T.C32 + T.C42*j34;
- const fvec cj33 = T.C33 + T.C43 * j34;
-
- T.C40 += T.C42 * dz + T.C44 * j04; // cj40
- T.C41 += T.C43 * dz + T.C44 * j14; // cj41
- T.C42 += T.C44 * j24; // cj42
- T.C43 += T.C44 * j34; // cj43
-
- T.C00 = cj00 + dz * cj20 + j04 * T.C40;
- T.C10 = cj01 + dz * cj21 + j04 * T.C41;
- T.C11 = cj11 + dz * cj31 + j14 * T.C41;
-
- T.C20 = cj20 + j24 * T.C40;
- T.C30 = cj30 + j34 * T.C40;
- T.C21 = cj21 + j24 * T.C41;
- T.C31 = cj31 + j34 * T.C41;
- T.C22 = cj22 + j24 * T.C42;
- T.C32 = cj32 + j34 * T.C42;
- T.C33 = cj33 + j34 * T.C43;
-}
-
-
-inline void L1ExtrapolateTime(L1TrackPar& T, // input track parameters (x,y,tx,ty,Q/p) and cov.matrix
- fvec dz, // extrapolate to this z position
- fvec timeInfo = 1.)
-{
-
- cnst c_light = 29.9792458;
-
- const fvec mask = (timeInfo > 0);
- fvec dz_mask = mask & dz;
-
- T.t += sqrt((T.tx * T.tx) + (T.ty * T.ty) + 1) * dz_mask / c_light;
-
- const fvec k1 = dz_mask * T.tx / (c_light * sqrt((T.tx * T.tx) + (T.ty * T.ty) + 1));
- const fvec k2 = dz_mask * T.ty / (c_light * sqrt((T.tx * T.tx) + (T.ty * T.ty) + 1));
-
- fvec c52 = T.C52;
- fvec c53 = T.C53;
-
- T.C50 += k1 * T.C20 + k2 * T.C30;
- T.C51 += k1 * T.C21 + k2 * T.C31;
- T.C52 += k1 * T.C22 + k2 * T.C32;
- T.C53 += k1 * T.C32 + k2 * T.C33;
- T.C54 += k1 * T.C42 + k2 * T.C43;
- T.C55 += k1 * (T.C52 + c52) + k2 * (T.C53 + c53);
-}
-
-inline void L1ExtrapolateLine(L1TrackPar& T, fvec z_out)
-{
- fvec dz = (z_out - T.z);
-
- T.x += T.tx * dz;
- T.y += T.ty * dz;
- T.z += dz;
-
- const fvec dzC32_in = dz * T.C32;
-
- T.C21 += dzC32_in;
- T.C10 += dz * (T.C21 + T.C30);
-
- const fvec C20_in = T.C20;
-
- T.C20 += dz * T.C22;
- T.C00 += dz * (T.C20 + C20_in);
-
- const fvec C31_in = T.C31;
+void L1ExtrapolateJXY // is not used currently
+ (fvec& tx, fvec& ty,
+ fvec& qp, // input track parameters
+ fvec dz, // extrapolate to this dz
+ L1FieldRegion& F, fvec& extrDx, fvec& extrDy, fvec extrJ[]);
- T.C31 += dz * T.C33;
- T.C11 += dz * (T.C31 + C31_in);
- T.C30 += dzC32_in;
+void L1ExtrapolateJXY0(fvec& tx,
+ fvec& ty, // input track slopes
+ fvec dz, // extrapolate to this dz position
+ L1FieldRegion& F, fvec& extrDx, fvec& extrDy, fvec& J04, fvec& J14);
- T.C40 += dz * T.C42;
- T.C41 += dz * T.C43;
-}
inline void L1ExtrapolateXC00Line(const L1TrackPar& T, fvec z_out, fvec& x, fvec& C00)
{
@@ -772,183 +57,4 @@ inline void L1ExtrapolateC10Line(const L1TrackPar& T, fvec z_out, fvec& C10)
C10 = T.C10 + dz * (T.C21 + T.C30 + dz * T.C32);
}
-inline void L1ExtrapolateJXY // is not used currently
- (fvec& tx, fvec& ty,
- fvec& qp, // input track parameters
- fvec dz, // extrapolate to this dz
- L1FieldRegion& F, fvec& extrDx, fvec& extrDy, fvec extrJ[])
-{
- //
- // Part of the analytic extrapolation formula with error (c_light*B*dz)^4/4!
- //
-
- // cnst ZERO = 0.0, ONE = 1.;
- cnst c_light = 0.000299792458;
-
- cnst c1 = 1., c2 = 2., c3 = 3., c4 = 4., c6 = 6., c9 = 9., c15 = 15., c18 = 18., c45 = 45., c2i = 1. / 2.,
- c6i = 1. / 6., c12i = 1. / 12.;
-
- fvec dz2 = dz * dz;
- fvec dz3 = dz2 * dz;
-
- // construct coefficients
-
- fvec x = tx;
- fvec y = ty;
- fvec xx = x * x;
- fvec xy = x * y;
- fvec yy = y * y;
- fvec y2 = y * c2;
- fvec x2 = x * c2;
- fvec x4 = x * c4;
- fvec xx31 = xx * c3 + c1;
- fvec xx159 = xx * c15 + c9;
-
- fvec Ay = -xx - c1;
- fvec Ayy = x * (xx * c3 + c3);
- fvec Ayz = -c2 * xy;
- fvec Ayyy = -(c15 * xx * xx + c18 * xx + c3);
-
- fvec Ayy_x = c3 * xx31;
- fvec Ayyy_x = -x4 * xx159;
-
- fvec Bx = yy + c1;
- fvec Byy = y * xx31;
- fvec Byz = c2 * xx + c1;
- fvec Byyy = -xy * xx159;
-
- fvec Byy_x = c6 * xy;
- fvec Byyy_x = -y * (c45 * xx + c9);
- fvec Byyy_y = -x * xx159;
-
- // end of coefficients calculation
-
- fvec t2 = c1 + xx + yy;
- fvec t = sqrt(t2);
- fvec h = qp * c_light;
- fvec ht = h * t;
-
- // get field integrals
- fvec Fx0 = F.cx0;
- fvec Fx1 = F.cx1 * dz;
- fvec Fx2 = F.cx2 * dz2;
- fvec Fy0 = F.cy0;
- fvec Fy1 = F.cy1 * dz;
- fvec Fy2 = F.cy2 * dz2;
- fvec Fz0 = F.cz0;
- fvec Fz1 = F.cz1 * dz;
- fvec Fz2 = F.cz2 * dz2;
-
- //
-
- fvec Sx = (Fx0 * c2i + Fx1 * c6i + Fx2 * c12i);
- fvec Sy = (Fy0 * c2i + Fy1 * c6i + Fy2 * c12i);
- fvec Sz = (Fz0 * c2i + Fz1 * c6i + Fz2 * c12i);
-
- fvec Syz;
- {
- cnst d = 1. / 2520., c00 = 21. * 20. * d, c01 = 21. * 5. * d, c02 = 21. * 2. * d, c10 = 7. * 30. * d,
- c11 = 7. * 9. * d, c12 = 7. * 4. * d, c20 = 2. * 63. * d, c21 = 2. * 21. * d, c22 = 2. * 10. * d;
- Syz = Fy0 * (c00 * Fz0 + c01 * Fz1 + c02 * Fz2) + Fy1 * (c10 * Fz0 + c11 * Fz1 + c12 * Fz2)
- + Fy2 * (c20 * Fz0 + c21 * Fz1 + c22 * Fz2);
- }
-
- fvec Syy;
- {
- cnst d = 1. / 2520., c00 = 420. * d, c01 = 21. * 15. * d, c02 = 21. * 8. * d, c03 = 63. * d, c04 = 70. * d,
- c05 = 20. * d;
- Syy = Fy0 * (c00 * Fy0 + c01 * Fy1 + c02 * Fy2) + Fy1 * (c03 * Fy1 + c04 * Fy2) + c05 * Fy2 * Fy2;
- }
-
- fvec Syyy;
- {
- cnst d = 1. / 181440., c000 = 7560 * d, c001 = 9 * 1008 * d, c002 = 5 * 1008 * d, c011 = 21 * 180 * d,
- c012 = 24 * 180 * d, c022 = 7 * 180 * d, c111 = 540 * d, c112 = 945 * d, c122 = 560 * d, c222 = 112 * d;
- fvec Fy22 = Fy2 * Fy2;
- Syyy = Fy0 * (Fy0 * (c000 * Fy0 + c001 * Fy1 + c002 * Fy2) + Fy1 * (c011 * Fy1 + c012 * Fy2) + c022 * Fy22)
- + Fy1 * (Fy1 * (c111 * Fy1 + c112 * Fy2) + c122 * Fy22) + c222 * Fy22 * Fy2;
- }
-
- fvec SA1 = Sx * xy + Sy * Ay + Sz * y;
- fvec SA1_x = Sx * y - Sy * x2;
- fvec SA1_y = Sx * x + Sz;
-
- fvec SB1 = Sx * Bx - Sy * xy - Sz * x;
- fvec SB1_x = -Sy * y - Sz;
- fvec SB1_y = Sx * y2 - Sy * x;
-
- fvec SA2 = Syy * Ayy + Syz * Ayz;
- fvec SA2_x = Syy * Ayy_x - Syz * y2;
- fvec SA2_y = -Syz * x2;
- fvec SB2 = Syy * Byy + Syz * Byz;
- fvec SB2_x = Syy * Byy_x + Syz * x4;
- fvec SB2_y = Syy * xx31;
-
- fvec SA3 = Syyy * Ayyy;
- fvec SA3_x = Syyy * Ayyy_x;
- fvec SB3 = Syyy * Byyy;
- fvec SB3_x = Syyy * Byyy_x;
- fvec SB3_y = Syyy * Byyy_y;
-
- fvec ht1 = ht * dz;
- fvec ht2 = ht * ht * dz2;
- fvec ht3 = ht * ht * ht * dz3;
- fvec ht1SA1 = ht1 * SA1;
- fvec ht1SB1 = ht1 * SB1;
- fvec ht2SA2 = ht2 * SA2;
- fvec ht2SB2 = ht2 * SB2;
- fvec ht3SA3 = ht3 * SA3;
- fvec ht3SB3 = ht3 * SB3;
-
- extrDx = (x + ht1SA1 + ht2SA2 + ht3SA3) * dz;
- extrDy = (y + ht1SB1 + ht2SB2 + ht3SB3) * dz;
-
- fvec ctdz2 = c_light * t * dz2;
-
- fvec t2i = c1 * rcp(t2); // /t2;
- fvec xt2i = x * t2i;
- fvec yt2i = y * t2i;
- fvec tmp0 = ht1SA1 + c2 * ht2SA2 + c3 * ht3SA3;
- fvec tmp1 = ht1SB1 + c2 * ht2SB2 + c3 * ht3SB3;
-
- extrJ[0] = dz * (c1 + xt2i * tmp0 + ht1 * SA1_x + ht2 * SA2_x + ht3 * SA3_x); // j02
- extrJ[1] = dz * (yt2i * tmp0 + ht1 * SA1_y + ht2 * SA2_y); // j03
- extrJ[2] = ctdz2 * (SA1 + c2 * ht1 * SA2 + c3 * ht2 * SA3); // j04
- extrJ[3] = dz * (xt2i * tmp1 + ht1 * SB1_x + ht2 * SB2_x + ht3 * SB3_x); // j12
- extrJ[4] = dz * (c1 + yt2i * tmp1 + ht1 * SB1_y + ht2 * SB2_y + ht3 * SB3_y); // j13
- extrJ[5] = ctdz2 * (SB1 + c2 * ht1 * SB2 + c3 * ht2 * SB3); // j14
-}
-
-inline void L1ExtrapolateJXY0(fvec& tx,
- fvec& ty, // input track slopes
- fvec dz, // extrapolate to this dz position
- L1FieldRegion& F, fvec& extrDx, fvec& extrDy, fvec& J04, fvec& J14)
-{
- cnst c_light = 0.000299792458, c1 = 1., c2i = 0.5, c6i = 1. / 6., c12i = 1. / 12.;
-
- fvec dz2 = dz * dz;
- fvec dzc6i = dz * c6i;
- fvec dz2c12i = dz2 * c12i;
-
- fvec xx = tx * tx;
- fvec yy = ty * ty;
- fvec xy = tx * ty;
-
- fvec Ay = -xx - c1;
- fvec Bx = yy + c1;
-
- fvec ctdz2 = c_light * sqrt(c1 + xx + yy) * dz2;
-
- fvec Sx = F.cx0 * c2i + F.cx1 * dzc6i + F.cx2 * dz2c12i;
- fvec Sy = F.cy0 * c2i + F.cy1 * dzc6i + F.cy2 * dz2c12i;
- fvec Sz = F.cz0 * c2i + F.cz1 * dzc6i + F.cz2 * dz2c12i;
-
- extrDx = (tx) *dz;
- extrDy = (ty) *dz;
- J04 = ctdz2 * (Sx * xy + Sy * Ay + Sz * ty);
- J14 = ctdz2 * (Sx * Bx - Sy * xy - Sz * tx);
-}
-
-#undef cnst
-
#endif
diff --git a/reco/L1/L1Algo/L1Field.cxx b/reco/L1/L1Algo/L1Field.cxx
index 1dd1e163f7724888dc9d663326b6eda0fc51a190..c3f7bd12f5dd02b2218e0b5830309ff40dccb031 100644
--- a/reco/L1/L1Algo/L1Field.cxx
+++ b/reco/L1/L1Algo/L1Field.cxx
@@ -67,21 +67,21 @@ void L1FieldSlice::CheckConsistency() const
{
/* Check SIMD data vectors for consistent initialization */
for (int i = 0; i < L1Constants::size::kMaxNFieldApproxCoefficients; ++i) {
- if (!cx[i].IsHorizontallyEqual()) {
+ if (!IsHorizontallyEqual(cx[i])) {
std::stringstream msg;
msg << "L1FieldSlice: \"cx[" << i
<< "]\" SIMD vector is inconsistent not all of the words are equal each other: " << cx[i];
throw std::logic_error(msg.str());
}
- if (!cy[i].IsHorizontallyEqual()) {
+ if (!IsHorizontallyEqual(cy[i])) {
std::stringstream msg;
msg << "L1FieldSlice: \"cy[" << i
<< "]\" SIMD vector is inconsistent not all of the words are equal each other: " << cy[i];
throw std::logic_error(msg.str());
}
- if (!cz[i].IsHorizontallyEqual()) {
+ if (!IsHorizontallyEqual(cz[i])) {
std::stringstream msg;
msg << "L1FieldSlice: \"cz[" << i
<< "]\" SIMD vector is inconsistent not all of the words are equal each other: " << cz[i];
@@ -190,7 +190,7 @@ void L1FieldRegion::CheckConsistency() const
// TODO: Should it be inline? (S.Zharko)
L1FieldValue L1FieldRegion::Get(const fvec z)
{
- fvec dz = (z - z0);
+ fvec dz = z - z0;
fvec dz2 = dz * dz;
L1FieldValue B;
B.x = cx0 + cx1 * dz + cx2 * dz2;
@@ -203,7 +203,7 @@ L1FieldValue L1FieldRegion::Get(const fvec z)
// TODO: Should it be inline? (S.Zharko)
void L1FieldRegion::Get(const fvec z_, fvec* B) const
{
- fvec dz = (z_ - z0);
+ fvec dz = z_ - z0;
fvec dz2 = dz * dz;
B[0] = cx0 + cx1 * dz + cx2 * dz2;
B[1] = cy0 + cy1 * dz + cy2 * dz2;
@@ -216,8 +216,10 @@ void L1FieldRegion::Set(const L1FieldValue& b0, const fvec b0z, const L1FieldVal
const L1FieldValue& b2, const fvec b2z)
{
z0 = b0z;
- fvec dz1 = b1z - b0z, dz2 = b2z - b0z;
- fvec det = rcp(fvec(dz1 * dz2 * (dz2 - dz1)));
+ fvec dz1 = b1z - b0z;
+ fvec dz2 = b2z - b0z;
+ fvec det = rcp(dz1 * dz2 * (dz2 - dz1));
+
fvec w21 = -dz2 * det;
fvec w22 = dz1 * det;
fvec w11 = -dz2 * w21;
@@ -246,8 +248,8 @@ void L1FieldRegion::Set(const L1FieldValue& b0, const fvec b0z, const L1FieldVal
// TODO: Should it be inline? (S.Zharko)
void L1FieldRegion::Set(const L1FieldValue& b0, const fvec b0z, const L1FieldValue& b1, const fvec b1z)
{
- z0 = b0z[0];
- fvec dzi = rcp(fvec(b1z - b0z));
+ z0 = b0z;
+ fvec dzi = rcp(b1z - b0z);
cx0 = b0.x;
cy0 = b0.y;
cz0 = b0.z;
@@ -267,7 +269,7 @@ void L1FieldRegion::Shift(fvec z)
fvec cx2dz = cx2 * dz;
fvec cy2dz = cy2 * dz;
fvec cz2dz = cz2 * dz;
- z0 = z[0];
+ z0 = z;
cx0 += (cx1 + cx2dz) * dz;
cy0 += (cy1 + cy2dz) * dz;
cz0 += (cz1 + cz2dz) * dz;
diff --git a/reco/L1/L1Algo/L1Filtration.h b/reco/L1/L1Algo/L1Filtration.h
index 08229ffb8e1d06d91e823827168acd5f191a68a3..8193d11382c545fb9698a473012c504218c08794 100644
--- a/reco/L1/L1Algo/L1Filtration.h
+++ b/reco/L1/L1Algo/L1Filtration.h
@@ -14,7 +14,7 @@
#define cnst const fvec
-inline void FilterTime(L1TrackPar& T, fvec t, fvec dt, fvec timeInfo = 1., fvec w = 1.)
+inline void FilterTime(L1TrackPar& T, fvec t, fvec dt, fvec timeInfo = fvec::One(), fvec w = fvec::One())
{
// filter track with a time measurement
@@ -28,7 +28,7 @@ inline void FilterTime(L1TrackPar& T, fvec t, fvec dt, fvec timeInfo = 1., fvec
fvec HCH = T.C55;
- w = w & (timeInfo > 0.f);
+ w = masked(w, timeInfo > fvec::Zero());
fvec dt2 = dt * dt;
@@ -42,7 +42,7 @@ inline void FilterTime(L1TrackPar& T, fvec t, fvec dt, fvec timeInfo = 1., fvec
fvec wi = w / (dt2 + 1.0000001f * HCH);
fvec zeta = T.t - t;
- fvec zetawi = w * zeta / ((maskDoFilter & dt2) + HCH);
+ fvec zetawi = w * zeta / (masked(dt2, maskDoFilter) + HCH);
//T.chi2 += maskDoFilter & (zeta * zetawi);
T.chi2 += zeta * zeta * wi;
@@ -113,9 +113,9 @@ inline void L1Filter(L1TrackPar& T, const L1UMeasurementInfo& info, fvec u, fvec
// with respect to HCH that it disappears due to the roundoff error
//
fvec wi = w / (info.sigma2 + 1.0000001f * HCH);
- fvec zetawi = w * zeta / ((maskDoFilter & info.sigma2) + HCH);
+ fvec zetawi = w * zeta / (masked(info.sigma2, maskDoFilter) + HCH);
- // T.chi2 += maskDoFilter & (zeta * zetawi);
+ // T.chi2 += masked( zeta * zetawi, maskDoFilter);
T.chi2 += zeta * zeta * wi;
T.NDF += w;
@@ -158,7 +158,7 @@ inline void L1Filter(L1TrackPar& T, const L1UMeasurementInfo& info, fvec u, fvec
inline void L1FilterNoField(L1TrackPar& T, const L1UMeasurementInfo& info, fvec u, fvec w = 1.)
{
- fvec wi, zeta, zetawi, HCH;
+ fvec zeta, HCH;
fvec F0, F1, F2, F3, F4, F5;
fvec K1, K2, K3, K4, K5;
@@ -175,19 +175,16 @@ inline void L1FilterNoField(L1TrackPar& T, const L1UMeasurementInfo& info, fvec
F4 = info.cos_phi * T.C40 + info.sin_phi * T.C41;
F5 = info.cos_phi * T.C50 + info.sin_phi * T.C51;
-#if 0 // use mask
- const fvec mask = (HCH < info.sigma2 * 16.);
- wi = w/( (mask & info.sigma2) +HCH );
- zetawi = zeta *wi;
- T.chi2 += mask & (zeta * zetawi);
-#else
- wi = w / (info.sigma2 + HCH);
- zetawi = zeta * wi;
+ //const fmask maskDoFilter = (HCH < info.sigma2 * 16.f);
+ const fvec maskDoFilter(fvec::MaskOne());
- T.chi2 += zeta * zetawi;
+ //TODO: SG: try this
+ //fvec wi = w / (info.sigma2 + 1.0000001f * HCH);
+ fvec wi = w / (info.sigma2 + HCH);
+ fvec zetawi = w * zeta / (masked(info.sigma2, maskDoFilter) + HCH);
-#endif // 0
+ T.chi2 += zeta * zeta * wi;
T.NDF += w;
K1 = F1 * wi;
diff --git a/reco/L1/L1Algo/L1Fit.h b/reco/L1/L1Algo/L1Fit.h
index 7577d1df009a0c76f7ec6a753498d0ff650acc5e..e7807c348ec7bfd1c4380469cba2cbd8f9426f01 100644
--- a/reco/L1/L1Algo/L1Fit.h
+++ b/reco/L1/L1Algo/L1Fit.h
@@ -55,6 +55,10 @@ public:
void EnergyLossCorrection(L1TrackPar& T, const fvec& radThick, fvec& qp0, fvec direction, fvec w);
+ template<int atomicZ>
+ void EnergyLossCorrection(float atomicA, float rho, float radLen, L1TrackPar& T, const fvec& radThick, fvec& qp0,
+ fvec direction, fvec w);
+
void EnergyLossCorrectionAl(L1TrackPar& T, const fvec& radThick, fvec& qp0, fvec direction, fvec w);
@@ -86,6 +90,4 @@ private:
//ClassDefNV(L1Fit, 0)
};
-#include "L1FitMaterial.h"
-
#endif
diff --git a/reco/L1/L1Algo/L1FitMaterial.h b/reco/L1/L1Algo/L1FitMaterial.cxx
similarity index 71%
rename from reco/L1/L1Algo/L1FitMaterial.h
rename to reco/L1/L1Algo/L1FitMaterial.cxx
index e5a2410c2da1aad2f77acf06cbac4d4829bfcb5a..6619648eebd21b9d4d53a05fa6660f1c49d8702d 100644
--- a/reco/L1/L1Algo/L1FitMaterial.h
+++ b/reco/L1/L1Algo/L1FitMaterial.cxx
@@ -2,10 +2,8 @@
SPDX-License-Identifier: GPL-3.0-only
Authors: Sergey Gorbunov [committer] */
-#ifndef L1FitMaterial_h
-#define L1FitMaterial_h
-
#include "L1Def.h"
+#include "L1Fit.h"
#include "L1MaterialInfo.h"
#include "L1TrackPar.h"
@@ -15,8 +13,7 @@
//const fvec PipeRadThick = 7.87e-3f; // 0.7 mm Aluminium
//const fvec TargetRadThick = 3.73e-2f * 2; // 250 mum Gold
-
-inline fvec L1Fit::BetheBlochIron(const float qp)
+fvec L1Fit::BetheBlochIron(const float qp)
{
float K = 0.000307075; // [GeV*cm^2/g]
@@ -49,7 +46,7 @@ inline fvec L1Fit::BetheBlochIron(const float qp)
return K * z * z * (Z / A) * (1. / betaSq) * (0.5 * log(2 * me * betaSq * gammaSq * Tmax / (I * I)) - betaSq - dc);
}
-inline fvec L1Fit::BetheBlochCarbon(const float qp)
+fvec L1Fit::BetheBlochCarbon(const float qp)
{
constexpr float K = 0.000307075; // GeV * g^-1 * cm^2
@@ -82,7 +79,7 @@ inline fvec L1Fit::BetheBlochCarbon(const float qp)
return K * z * z * (Z / A) * (1. / betaSq) * (0.5 * log(2 * me * betaSq * gammaSq * Tmax / (I * I)) - betaSq - dc);
}
-inline fvec L1Fit::BetheBlochAl(const float qp)
+fvec L1Fit::BetheBlochAl(const float qp)
{
constexpr float K = 0.000307075; // GeV * g^-1 * cm^2
@@ -115,7 +112,7 @@ inline fvec L1Fit::BetheBlochAl(const float qp)
return K * z * z * (Z / A) * (1. / betaSq) * (0.5 * log(2 * me * betaSq * gammaSq * Tmax / (I * I)) - betaSq - dc);
}
-inline fvec L1Fit::ApproximateBetheBloch(const fvec& bg2)
+fvec L1Fit::ApproximateBetheBloch(const fvec& bg2)
{
//
// This is the parameterization of the Bethe-Bloch formula inspired by Geant.
@@ -151,19 +148,18 @@ inline fvec L1Fit::ApproximateBetheBloch(const fvec& bg2)
const fvec x = 0.5f * log(bg2);
const fvec lhwI = log(28.816f * 1e-9f * sqrt(rho * mZA) / mI);
- fvec init = x > x1;
-
- d2 = fvec(init & (lhwI + x - 0.5f));
+ fvec init = x > x1;
+ d2 = masked(lhwI + x - 0.5f, init);
const fvec r = (x1 - x) / (x1 - x0);
init = (x > x0) & (x1 > x);
- d2 = fvec(init & (lhwI + x - 0.5f + (0.5f - lhwI - x0) * r * r * r)) + fvec((!init) & d2);
+ d2 = if3(init, lhwI + x - 0.5f + (0.5f - lhwI - x0) * r * r * r, d2);
return mK * mZA * (fvec(1.f) + bg2) / bg2
* (0.5f * log(_2me * bg2 * maxT / (mI * mI)) - bg2 / (fvec(1.f) + bg2) - d2);
}
-inline fvec L1Fit::ApproximateBetheBloch(const fvec& bg2, const fvec& kp0, const fvec& kp1, const fvec& kp2,
- const fvec& kp3, const fvec& kp4)
+fvec L1Fit::ApproximateBetheBloch(const fvec& bg2, const fvec& kp0, const fvec& kp1, const fvec& kp2, const fvec& kp3,
+ const fvec& kp4)
{
//
// This is the parameterization of the Bethe-Bloch formula inspired by Geant.
@@ -199,19 +195,18 @@ inline fvec L1Fit::ApproximateBetheBloch(const fvec& bg2, const fvec& kp0, const
const fvec x = 0.5f * log(bg2);
const fvec lhwI = log(28.816f * 1e-9f * sqrt(rho * mZA) / mI);
- fvec init = x > x1;
-
- d2 = fvec(init & (lhwI + x - 0.5f));
+ fvec init = x > x1;
+ d2 = masked(lhwI + x - 0.5f, init);
const fvec r = (x1 - x) / (x1 - x0);
init = (x > x0) & (x1 > x);
- d2 = fvec(init & (lhwI + x - 0.5f + (0.5f - lhwI - x0) * r * r * r)) + fvec((!init) & d2);
+ d2 = if3(init, lhwI + x - 0.5f + (0.5f - lhwI - x0) * r * r * r, d2);
return mK * mZA * (fvec(1.f) + bg2) / bg2
* (0.5f * log(_2me * bg2 * maxT / (mI * mI)) - bg2 / (fvec(1.f) + bg2) - d2);
}
-inline void L1Fit::EnergyLossCorrection(L1TrackPar& T, const fvec& radThick, fvec& qp0, fvec direction, fvec w)
+void L1Fit::EnergyLossCorrection(L1TrackPar& T, const fvec& radThick, fvec& qp0, fvec direction, fvec w)
{
const fvec p2 = 1.f / (T.qp * T.qp);
const fvec E2 = fMass2 + p2;
@@ -233,9 +228,8 @@ inline void L1Fit::EnergyLossCorrection(L1TrackPar& T, const fvec& radThick, fve
const fvec E2Corrected = (sqrt(E2) + direction * dE) * (sqrt(E2) + direction * dE);
fvec corr = sqrt(p2 / (E2Corrected - fMass2));
- fvec init = fvec(!(corr == corr)) | fvec(w < 1);
- corr = fvec(fvec(1.f) & init) + fvec(corr & fvec(!(init)));
-
+ fvec ok = (corr == corr) & (fvec::Zero() < w);
+ corr = if3(ok, corr, fvec::One());
qp0 *= corr;
// fvec dqp = CalcQpAfterEloss(qp[0], (direction*dE)[0], fMass2[0]);
@@ -249,24 +243,15 @@ inline void L1Fit::EnergyLossCorrection(L1TrackPar& T, const fvec& radThick, fve
T.C44 *= corr * corr;
}
-inline void L1Fit::EnergyLossCorrectionAl(L1TrackPar& T, const fvec& radThick, fvec& qp0, fvec direction, fvec w)
+template<int atomicZ>
+void L1Fit::EnergyLossCorrection(float atomicA, float rho, float radLen, L1TrackPar& T, const fvec& radThick, fvec& qp0,
+ fvec direction, fvec w)
{
const fvec p2 = 1.f / (T.qp * T.qp);
const fvec E2 = fMass2 + p2;
fvec qp = T.qp;
- constexpr int atomicZ = 13;
- constexpr float atomicA = 26.981f;
- constexpr float rho = 2.70f;
- constexpr float radLen = 2.265f;
-
- // int atomicZ = 18;
- // float atomicA = 39.95;
- // float rho = 0.001780;
- // float radLen = 1.097e+04;
-
-
fvec i;
if (atomicZ < 13) i = (12. * atomicZ + 7.) * 1.e-9;
else
@@ -278,17 +263,15 @@ inline void L1Fit::EnergyLossCorrectionAl(L1TrackPar& T, const fvec& radThick, f
// fvec bethe2 = BetheBlochAl(qp[0]);
-
fvec tr = sqrt(fvec(1.f) + T.tx * T.tx + T.ty * T.ty);
// fvec dE2 = 2*bethe * radThick*tr * 9.34961f*2.33f ;
fvec dE = bethe * radThick * tr * radLen * rho;
-
const fvec E2Corrected = (sqrt(E2) + direction * dE) * (sqrt(E2) + direction * dE);
fvec corr = sqrt(p2 / (E2Corrected - fMass2));
- fvec init = fvec(!(corr == corr)) | fvec(w < 1);
- corr = fvec(fvec(1.f) & init) + fvec(corr & fvec(!(init)));
+ fvec ok = (corr == corr) & (fvec::Zero() < w);
+ corr = if3(ok, corr, fvec::One());
qp0 *= corr;
// fvec dqp = CalcQpAfterEloss(qp[0], (direction*dE)[0], fMass2[0]);
@@ -339,176 +322,35 @@ inline void L1Fit::EnergyLossCorrectionAl(L1TrackPar& T, const fvec& radThick, f
// T.C44 *= corr*corr;
}
-
-inline void L1Fit::EnergyLossCorrectionCarbon(L1TrackPar& T, const fvec& radThick, fvec& qp0, fvec direction, fvec w)
+void L1Fit::EnergyLossCorrectionAl(L1TrackPar& T, const fvec& radThick, fvec& qp0, fvec direction, fvec w)
{
- const fvec p2 = 1.f / (T.qp * T.qp);
- const fvec E2 = fMass2 + p2;
-
- fvec qp = T.qp;
-
+ constexpr int atomicZ = 13;
+ constexpr float atomicA = 26.981f;
+ constexpr float rho = 2.70f;
+ constexpr float radLen = 2.265f;
+ EnergyLossCorrection<atomicZ>(atomicA, rho, radLen, T, radThick, qp0, direction, w);
+}
+void L1Fit::EnergyLossCorrectionCarbon(L1TrackPar& T, const fvec& radThick, fvec& qp0, fvec direction, fvec w)
+{
constexpr int atomicZ = 6;
constexpr float atomicA = 12.011f;
constexpr float rho = 2.265;
constexpr float radLen = 18.76f;
-
- fvec i;
- if (atomicZ < 13) i = (12. * atomicZ + 7.) * 1.e-9;
- else
- i = (9.76 * atomicZ + 58.8 * std::pow(atomicZ, -0.19)) * 1.e-9;
-
- const fvec bethe = ApproximateBetheBloch(p2 / fMass2, rho, 0.20, 3.00, i, atomicZ / atomicA);
-
-
- // const fvec bethe = ApproximateBetheBloch( p2/fMass2 );
-
- // fvec bethe2 = BetheBlochCarbon(qp[0]);
-
-
- fvec tr = sqrt(fvec(1.f) + T.tx * T.tx + T.ty * T.ty);
-
-
- fvec dE = bethe * radThick * tr * rho * radLen;
- // fvec dE2 = bethe2 * radThick*tr*2.70f* 18.76f;
-
- const fvec E2Corrected = (sqrt(E2) + direction * dE) * (sqrt(E2) + direction * dE);
- fvec corr = sqrt(p2 / (E2Corrected - fMass2));
- fvec init = fvec(!(corr == corr)) | fvec(w < 1);
- corr = fvec(fvec(1.f) & init) + fvec(corr & fvec(!(init)));
-
- qp0 *= corr;
- T.qp *= corr;
-
-
- float P = fabs(1. / qp[0]); // GeV
-
- float Z = atomicZ;
- float A = atomicA;
- float RHO = rho;
-
- fvec STEP = radThick * tr * radLen;
- fvec EMASS = 0.511 * 1e-3; // GeV
-
- fvec BETA = P / sqrt(E2Corrected);
- fvec GAMMA = sqrt(E2) / fMass;
-
- // Calculate xi factor (KeV).
- fvec XI = (153.5 * Z * STEP * RHO) / (A * BETA * BETA);
-
- // Maximum energy transfer to atomic electron (KeV).
- fvec ETA = BETA * GAMMA;
- fvec ETASQ = ETA * ETA;
- fvec RATIO = EMASS / fMass;
- fvec F1 = 2. * EMASS * ETASQ;
- fvec F2 = 1. + 2. * RATIO * GAMMA + RATIO * RATIO;
- fvec EMAX = 1e6 * F1 / F2;
-
- fvec DEDX2 = XI * EMAX * (1. - (BETA * BETA / 2.)) * 1e-12;
-
- float P2 = P * P;
- fvec SDEDX = ((E2) *DEDX2) / (P2 * P2 * P2);
-
- // fvec dqp = CalcQpAfterEloss(qp[0], (direction*dE)[0], fMass2[0]);
- // qp0 = dqp;
- // T.qp = dqp;
-
-
- // T.C40 *= corr;
- // T.C41 *= corr;
- // T.C42 *= corr;
- // T.C43 *= corr;
- // T.C44 *= corr*corr;
- T.C44 += fabs(SDEDX);
-
- // T.C40 *= corr;
- // T.C41 *= corr;
- // T.C42 *= corr;
- // T.C43 *= corr;
- // T.C44 *= corr*corr;
+ EnergyLossCorrection<atomicZ>(atomicA, rho, radLen, T, radThick, qp0, direction, w);
}
-
-inline void L1Fit::EnergyLossCorrectionIron(L1TrackPar& T, const fvec& radThick, fvec& qp0, fvec direction, fvec w)
+void L1Fit::EnergyLossCorrectionIron(L1TrackPar& T, const fvec& radThick, fvec& qp0, fvec direction, fvec w)
{
- const fvec p2 = 1.f / (T.qp * T.qp);
- const fvec E2 = fMass2 + p2;
-
- fvec qp = T.qp;
-
constexpr int atomicZ = 26;
constexpr float atomicA = 55.845f;
constexpr float rho = 7.87;
constexpr float radLen = 1.758f;
-
- fvec i;
- if (atomicZ < 13) i = (12. * atomicZ + 7.) * 1.e-9;
- else
- i = (9.76 * atomicZ + 58.8 * std::pow(atomicZ, -0.19)) * 1.e-9;
-
- const fvec bethe = ApproximateBetheBloch(p2 / fMass2, rho, 0.20, 3.00, i, atomicZ / atomicA);
-
- // fvec bethe2 = BetheBlochIron(T.qp[0]);
-
-
- fvec tr = sqrt(fvec(1.f) + T.tx * T.tx + T.ty * T.ty);
-
-
- fvec dE = bethe * radThick * tr * radLen * rho;
- //fvec dE2 = bethe2 * radThick*tr * 1.758f*2.33;
-
- const fvec E2Corrected = (sqrt(E2) + direction * dE) * (sqrt(E2) + direction * dE);
- fvec corr = sqrt(p2 / (E2Corrected - fMass2));
- fvec init = fvec(!(corr == corr)) | fvec(w < 1);
- corr = fvec(fvec(1.f) & init) + fvec(corr & fvec(!(init)));
-
- qp0 *= corr;
- T.qp *= corr;
-
- float P = fabs(1. / qp[0]); // GeV
-
- float Z = atomicZ;
- float A = atomicA;
- float RHO = rho;
-
- fvec STEP = radThick * tr * radLen;
- fvec EMASS = 0.511 * 1e-3; // GeV
-
- fvec BETA = P / sqrt(E2Corrected);
- fvec GAMMA = sqrt(E2) / fMass;
-
- // Calculate xi factor (KeV).
- fvec XI = (153.5 * Z * STEP * RHO) / (A * BETA * BETA);
-
- // Maximum energy transfer to atomic electron (KeV).
- fvec ETA = BETA * GAMMA;
- fvec ETASQ = ETA * ETA;
- fvec RATIO = EMASS / sqrt(fMass2);
- fvec F1 = 2. * EMASS * ETASQ;
- fvec F2 = 1. + 2. * RATIO * GAMMA + RATIO * RATIO;
- fvec EMAX = 1e6 * F1 / F2;
-
- fvec DEDX2 = XI * EMAX * (1. - (BETA * BETA / 2.)) * 1e-12;
-
- float P2 = P * P;
- fvec SDEDX = ((E2) *DEDX2) / (P2 * P2 * P2);
-
- // T.C40 *= corr;
- // T.C41 *= corr;
- // T.C42 *= corr;
- // T.C43 *= corr;
- // T.C44 *= corr*corr;
- T.C44 += fabs(SDEDX);
-
- // T.C40 *= corr;
- // T.C41 *= corr;
- // T.C42 *= corr;
- // T.C43 *= corr;
- // T.C44 *= corr*corr;
+ EnergyLossCorrection<atomicZ>(atomicA, rho, radLen, T, radThick, qp0, direction, w);
}
-inline void L1Fit::L1AddMaterial(L1TrackPar& T, fvec radThick, fvec qp0, fvec w)
+void L1Fit::L1AddMaterial(L1TrackPar& T, fvec radThick, fvec qp0, fvec w)
{
cnst ONE = 1.;
@@ -533,7 +375,7 @@ inline void L1Fit::L1AddMaterial(L1TrackPar& T, fvec radThick, fvec qp0, fvec w)
T.C33 += w * (ONE + tyty) * a;
}
-inline void L1Fit::L1AddMaterial(L1TrackPar& T, const L1MaterialInfo& info, fvec qp0, fvec w)
+void L1Fit::L1AddMaterial(L1TrackPar& T, const L1MaterialInfo& info, fvec qp0, fvec w)
{
cnst ONE = 1.f;
@@ -600,7 +442,7 @@ inline void L1Fit::L1AddMaterial(L1TrackPar& T, const L1MaterialInfo& info, fvec
//
// }
-inline void L1Fit::L1AddThickMaterial(L1TrackPar& T, fvec radThick, fvec qp0, fvec w, fvec thickness, bool fDownstream)
+void L1Fit::L1AddThickMaterial(L1TrackPar& T, fvec radThick, fvec qp0, fvec w, fvec thickness, bool fDownstream)
{
cnst ONE = 1.;
@@ -640,7 +482,7 @@ inline void L1Fit::L1AddThickMaterial(L1TrackPar& T, fvec radThick, fvec qp0, fv
}
-inline void L1Fit::L1AddHalfMaterial(L1TrackPar& T, const L1MaterialInfo& info, fvec qp0)
+void L1Fit::L1AddHalfMaterial(L1TrackPar& T, const L1MaterialInfo& info, fvec qp0)
{
cnst ONE = 1.f;
fvec tx = T.tx;
@@ -664,7 +506,7 @@ inline void L1Fit::L1AddHalfMaterial(L1TrackPar& T, const L1MaterialInfo& info,
T.C33 += (ONE + tyty) * a;
}
-inline void L1Fit::L1AddPipeMaterial(L1TrackPar& T, fvec qp0, fvec w)
+void L1Fit::L1AddPipeMaterial(L1TrackPar& T, fvec qp0, fvec w)
{
cnst ONE = 1.f;
@@ -693,7 +535,7 @@ inline void L1Fit::L1AddPipeMaterial(L1TrackPar& T, fvec qp0, fvec w)
T.C33 += w * (ONE + tyty) * a;
}
-inline void L1Fit::L1AddTargetMaterial(L1TrackPar& T, fvec qp0, fvec w)
+void L1Fit::L1AddTargetMaterial(L1TrackPar& T, fvec qp0, fvec w)
{
cnst ONE = 1.f;
@@ -719,5 +561,3 @@ inline void L1Fit::L1AddTargetMaterial(L1TrackPar& T, fvec qp0, fvec w)
}
#undef cnst
-
-#endif
diff --git a/reco/L1/L1Algo/L1NaN.h b/reco/L1/L1Algo/L1NaN.h
index a83807d8f0da02e910d3d2a8f9d086c4e028c3ba..667ce9d4c474fbe2b40a7409dccdd0ffc1255612 100644
--- a/reco/L1/L1Algo/L1NaN.h
+++ b/reco/L1/L1Algo/L1NaN.h
@@ -64,7 +64,7 @@ namespace L1NaN
template<typename T, typename std::enable_if<std::is_same<T, fvec>::value, T>::type* = nullptr>
bool IsNaN(T value)
{
- return value.IsNanAny(); // NOTE: Here we consider fvec a NaN if at least one of its words is NaN
+ return IsNanAny(value); // NOTE: Here we consider fvec a NaN if at least one of its words is NaN
}
}; // namespace L1NaN
diff --git a/reco/L1/L1Algo/L1TrackFitter.cxx b/reco/L1/L1Algo/L1TrackFitter.cxx
index a7f9cd30e48f51ca61305a978e9a23d38f6ab99e..ec45faa181d97558f972a72d648e8a9d1e7b5b76 100644
--- a/reco/L1/L1Algo/L1TrackFitter.cxx
+++ b/reco/L1/L1Algo/L1TrackFitter.cxx
@@ -47,7 +47,7 @@ void L1Algo::KFTrackFitter_simple() // TODO: Add pipe.
L1Fit fit;
fit.SetParticleMass(GetDefaultParticleMass());
- fvec qp0 = 0.25;
+ fvec qp0(0.25);
//fvec qp0 = 2./t.Momentum;
for (int iter = 0; iter < 3; iter++) {
//cout<<" Back 1"<<endl;
@@ -66,18 +66,18 @@ void L1Algo::KFTrackFitter_simple() // TODO: Add pipe.
const L1Station& sta1 = fParameters.GetStation(ista1);
const L1Station& sta2 = fParameters.GetStation(ista2);
- fvec u0 = hit0.u;
- fvec v0 = hit0.v;
+ fvec u0 = hit0.u;
+ fvec v0 = hit0.v;
auto [x0, y0] = sta0.ConvUVtoXY<fvec>(u0, v0);
- fvec z0 = hit0.z;
+ fvec z0 = hit0.z;
- fvec u1 = hit1.u;
- fvec v1 = hit1.v;
+ fvec u1 = hit1.u;
+ fvec v1 = hit1.v;
auto [x1, y1] = sta1.ConvUVtoXY<fvec>(u1, v1);
- fvec z1 = hit1.z;
+ fvec z1 = hit1.z;
- fvec u2 = hit2.u;
- fvec v2 = hit2.v;
+ fvec u2 = hit2.u;
+ fvec v2 = hit2.v;
auto [x2, y2] = sta1.ConvUVtoXY<fvec>(u2, v2);
// fvec z2 = hit2.z;
@@ -93,17 +93,17 @@ void L1Algo::KFTrackFitter_simple() // TODO: Add pipe.
T.qp = qp0;
T.z = z0;
- T.chi2 = 0.;
- T.NDF = 2.;
+ T.chi2 = fvec(0.);
+ T.NDF = fvec(2.);
T.C00 = sta0.XYInfo.C00;
T.C10 = sta0.XYInfo.C10;
T.C11 = sta0.XYInfo.C11;
- T.C20 = T.C21 = 0;
- T.C30 = T.C31 = T.C32 = 0;
- T.C40 = T.C41 = T.C42 = T.C43 = 0;
+ T.C20 = T.C21 = fvec(0.);
+ T.C30 = T.C31 = T.C32 = fvec(0.);
+ T.C40 = T.C41 = T.C42 = T.C43 = fvec(0.);
T.C22 = T.C33 = vINF;
- T.C44 = 1.;
+ T.C44 = fvec(1.);
// static L1FieldValue fldB0, fldB1, fldB2 _fvecalignment;
// static L1FieldRegion fld _fvecalignment;
@@ -142,8 +142,8 @@ void L1Algo::KFTrackFitter_simple() // TODO: Add pipe.
// if (ista == fNstationsBeforePipe - 1) fit.L1AddPipeMaterial( T, qp0, 1);
- fvec u = hit.u;
- fvec v = hit.v;
+ fvec u = hit.u;
+ fvec v = hit.v;
auto [x, y] = sta.ConvUVtoXY<fvec>(u, v);
L1Filter(T, sta.frontInfo, u);
L1Filter(T, sta.backInfo, v);
@@ -183,7 +183,7 @@ void L1Algo::KFTrackFitter_simple() // TODO: Add pipe.
t.chi2 = T.chi2[0];
t.NDF = static_cast<int>(T.NDF[0]);
- qp0 = T.qp[0];
+ qp0 = T.qp;
} // fit backward
// fit forward
@@ -201,18 +201,18 @@ void L1Algo::KFTrackFitter_simple() // TODO: Add pipe.
const L1Station& sta1 = fParameters.GetStation(ista1);
const L1Station& sta2 = fParameters.GetStation(ista2);
- fvec u0 = hit0.u;
- fvec v0 = hit0.v;
+ fvec u0 = hit0.u;
+ fvec v0 = hit0.v;
auto [x0, y0] = sta0.ConvUVtoXY<fvec>(u0, v0);
- fvec z0 = hit0.z;
+ fvec z0 = hit0.z;
- fvec u1 = hit1.u;
- fvec v1 = hit1.v;
+ fvec u1 = hit1.u;
+ fvec v1 = hit1.v;
auto [x1, y1] = sta1.ConvUVtoXY<fvec>(u1, v1);
// fvec z1 = hit1.z;
- fvec u2 = hit2.u;
- fvec v2 = hit2.v;
+ fvec u2 = hit2.u;
+ fvec v2 = hit2.v;
auto [x2, y2] = sta2.ConvUVtoXY<fvec>(u2, v2);
// fvec z2 = hit2.z;
@@ -221,8 +221,8 @@ void L1Algo::KFTrackFitter_simple() // TODO: Add pipe.
//fvec qp0 = first_trip->GetQpOrig(MaxInvMom);
// const fvec vINF = .1;
- T.chi2 = 0.;
- T.NDF = 2.;
+ T.chi2 = fvec(0.);
+ T.NDF = fvec(2.);
T.x = x0;
T.y = y0;
// T.tx = (x1-x0)*dzi;
@@ -312,7 +312,7 @@ void L1Algo::KFTrackFitter_simple() // TODO: Add pipe.
t.chi2 += T.chi2[0];
t.NDF += static_cast<int>(T.NDF[0]);
}
- qp0 = T.qp[0];
+ qp0 = T.qp;
}
} // for(int itrack
}
@@ -428,16 +428,16 @@ void L1Algo::L1KFTrackFitter()
w[ista][iVec] = 1.;
if (ista > fNstationsBeforePipe) w_time[ista][iVec] = 1.;
- u[ista][iVec] = hit.u;
- v[ista][iVec] = hit.v;
- d_u[ista][iVec] = hit.du;
- d_v[ista][iVec] = hit.dv;
+ u[ista][iVec] = hit.u;
+ v[ista][iVec] = hit.v;
+ d_u[ista][iVec] = hit.du;
+ d_v[ista][iVec] = hit.dv;
std::tie(x_temp, y_temp) = sta[ista].ConvUVtoXY<fvec>(u[ista], v[ista]);
- x[ista][iVec] = x_temp[iVec];
- y[ista][iVec] = y_temp[iVec];
- time[ista][iVec] = hit.t;
- timeEr[ista][iVec] = hit.dt;
- z[ista][iVec] = hit.z;
+ x[ista][iVec] = x_temp[iVec];
+ y[ista][iVec] = y_temp[iVec];
+ time[ista][iVec] = hit.t;
+ timeEr[ista][iVec] = hit.dt;
+ z[ista][iVec] = hit.z;
sta[ista].fieldSlice.GetFieldValue(x[ista], y[ista], fB_temp);
std::tie(d_xx[ista], d_xy[ista], d_yy[ista]) = sta[ista].FormXYCovarianceMatrix(d_u[ista], d_v[ista]);
@@ -530,10 +530,10 @@ void L1Algo::L1KFTrackFitter()
fldB0.Combine(fB[i], w[i]);
fld.Set(fldB0, fldZ0, fldB1, fldZ1, fldB2, fldZ2);
- fvec initialised = fvec(z[i] < z_end) & fvec(z_start <= z[i]);
- fvec w1 = (w[i] & (initialised));
- fvec w1_time = (w_time[i] & (initialised));
- fvec wIn = (ONE & (initialised));
+ fvec initialised = (z[i] < z_end) & (z_start <= z[i]);
+ fvec w1 = masked(w[i], initialised);
+ fvec w1_time = masked(w_time[i], initialised);
+ fvec wIn = masked(fvec::One(), initialised);
fld1 = fld;
@@ -692,10 +692,10 @@ void L1Algo::L1KFTrackFitter()
fldB0.Combine(fB[i], w[i]);
fld.Set(fldB0, fldZ0, fldB1, fldZ1, fldB2, fldZ2);
- fvec initialised = fvec(z[i] <= z_end) & fvec(z_start < z[i]);
- fvec w1 = (w[i] & (initialised));
- fvec wIn = (ONE & (initialised));
- fvec w1_time = (w_time[i] & (initialised));
+ fvec initialised = (z[i] <= z_end) & (z_start < z[i]);
+ fvec w1 = masked(w[i], initialised);
+ fvec w1_time = masked(w_time[i], initialised);
+ fvec wIn = masked(fvec::One(), initialised);
L1Extrapolate(T, z[i], qp0, fld, &w1);
@@ -901,15 +901,15 @@ void L1Algo::L1KFTrackFitterMuch()
d_xx[i][iVec] = 0;
d_yy[i][iVec] = 0;
- u[ista][iVec] = hit.u;
- v[ista][iVec] = hit.v;
+ u[ista][iVec] = hit.u;
+ v[ista][iVec] = hit.v;
std::tie(x_temp, y_temp) = sta[ista].ConvUVtoXY<fvec>(u[ista], v[ista]);
- x[ista][iVec] = x_temp[iVec];
- y[ista][iVec] = y_temp[iVec];
- time[ista][iVec] = hit.t;
- timeEr[ista][iVec] = hit.dt;
- d_u[ista][iVec] = hit.du;
- d_v[ista][iVec] = hit.dv;
+ x[ista][iVec] = x_temp[iVec];
+ y[ista][iVec] = y_temp[iVec];
+ time[ista][iVec] = hit.t;
+ timeEr[ista][iVec] = hit.dt;
+ d_u[ista][iVec] = hit.du;
+ d_v[ista][iVec] = hit.dv;
std::tie(d_xx[ista], d_xy[ista], d_yy[ista]) = sta[ista].FormXYCovarianceMatrix(d_u[ista], d_v[ista]);
// mom[ista][iVec] = hit.p;
@@ -1002,9 +1002,9 @@ void L1Algo::L1KFTrackFitterMuch()
fld.Set(fldB2, fldZ2, fldB1, fldZ1, fldB0, fldZ0);
for (++i; i < nHits; i++) {
- fvec initialised = fvec(z[i] <= z_end) & fvec(z_start < z[i]);
- fvec w1 = (w[i] & (initialised));
- fvec wIn = (ONE & (initialised));
+ fvec initialised = (z[i] <= z_end) & (z_start < z[i]);
+ fvec w1 = masked(w[i], initialised);
+ fvec wIn = masked(fvec::One(), initialised);
fldZ0 = z[i];
dz = (fldZ1 - fldZ0);
@@ -1078,33 +1078,25 @@ void L1Algo::L1KFTrackFitterMuch()
if (max_steps < nofSteps[j]) max_steps = nofSteps[j];
}
- const fvec mask = (nofSteps < fvec(1)) & fvec(1);
- fvec nofSteps1 = fvec(0);
+ fvec nofSteps1 = fvec::Zero();
- fvec one = fvec(1);
-
- fvec stepSize = (((mask) *d_z) + ((one - mask) * st)) * w1;
-
- fvec z_cur = T1.fz;
-
- fvec w2 = w1;
+ fvec stepSize = if3(nofSteps < fvec::One(), d_z, st) * w1;
+ fvec z_cur = T1.fz;
+ fvec w2 = w1;
for (int iStep = 0; iStep < max_steps + 1; iStep++) {
- const fvec mask1 = (nofSteps == nofSteps1) & fvec(1);
-
- z_cur = (one - mask1) * (stepSize + T1.fz) + mask1 * z_last;
+ const fvec maskLastStep = (nofSteps == nofSteps1);
+ z_cur = if3(maskLastStep, z_last, T1.fz + stepSize);
// fvec v_mc = fabs(1/qp01)/sqrt(mass2+fabs(1/qp01)*fabs(1/qp01));
// T1.ExtrapolateLine1( z, &w2, v_mc);
T1.ExtrapolateLine(z_cur, &w2);
-
- nofSteps1 = nofSteps1 + (one - mask1);
-
- w2 = w2 & (one - mask1);
+ nofSteps1 += masked(fvec::One(), !maskLastStep);
+ w2 = masked(w2, !maskLastStep);
// T1.ExtrapolateLine( z_last, &w1);
// L1ExtrapolateLine( T, z_last);
@@ -1123,7 +1115,7 @@ void L1Algo::L1KFTrackFitterMuch()
T1.L1AddThickMaterial(fParameters.GetMaterialThickness(i, T1.fx, T1.fy) / (nofSteps + fvec(1)), qp01, wIn,
sta[i].materialInfo.thick / (nofSteps + fvec(1)), 1);
- wIn = wIn & (one - mask1);
+ wIn = masked(wIn, !maskLastStep);
}
else {
fit.L1AddMaterial(T, sta[i].materialInfo, qp0, wIn);
@@ -1206,11 +1198,9 @@ void L1Algo::L1KFTrackFitterMuch()
for (--i; i >= 0; i--) {
- fvec initialised = fvec(z[i] < z_end) & fvec(z_start <= z[i]);
- fvec w1 = (w[i] & (initialised));
-
- fvec wIn = (ONE & (initialised));
-
+ fvec initialised = (z[i] < z_end) & (z_start <= z[i]);
+ fvec w1 = masked(w[i], initialised);
+ fvec wIn = masked(fvec::One(), initialised);
if (i >= fNfieldStations - 1) {
@@ -1226,29 +1216,27 @@ void L1Algo::L1KFTrackFitterMuch()
int max_steps = 0;
- for (int j = 0; j < 4; j++) {
+ for (int j = 0; j < fvecLen; j++) {
nofSteps[j] = int(fabs(d_z[j]) / 10); //*w1[i];
if (max_steps < nofSteps[j]) max_steps = nofSteps[j];
}
- const fvec mask = (nofSteps < fvec(1)) & fvec(1);
- fvec nofSteps1 = fvec(0);
- fvec one = fvec(1);
- fvec stepSize = (((mask) *d_z) + ((one - mask) * st)) * wIn;
- fvec z_cur = T1.fz;
- fvec w2 = wIn;
+ fvec nofSteps1 = fvec(0);
+ fvec stepSize = wIn * if3((nofSteps < fvec::One()), d_z, st);
+ fvec z_cur = T1.fz;
+ fvec w2 = wIn;
for (int iStep = 0; iStep < max_steps + 1; iStep++) {
- const fvec mask1 = (nofSteps == nofSteps1) & fvec(1);
-
- z_cur = (one - mask1) * (T1.fz - stepSize) + mask1 * z_last;
+ const fvec maskLastStep = (nofSteps == nofSteps1);
+ z_cur = if3(maskLastStep, z_last, T1.fz - stepSize);
// fvec v_mc = fabs(1/qp01)/sqrt(mass2+fabs(1/qp01)*fabs(1/qp01));
// T1.ExtrapolateLine1( z_cur, &w2, v_mc);
T1.ExtrapolateLine(z_cur, &w2);
- nofSteps1 = nofSteps1 + (one - mask1);
+ nofSteps1 += masked(fvec::One(), !maskLastStep);
+
// TODO: Unify the selection of energy loss correction! (S.Zharko)
if constexpr (L1Constants::control::kIfUseRadLengthTable) {
if (i == 11 || i == 14 || i == 17)
@@ -1264,7 +1252,7 @@ void L1Algo::L1KFTrackFitterMuch()
T1.L1AddThickMaterial(fParameters.GetMaterialThickness(i, T1.fx, T1.fy) / (nofSteps + fvec(1)), qp01, w2,
sta[i].materialInfo.thick / (nofSteps + fvec(1)), 0);
- w2 = w2 & (one - mask1);
+ w2 = masked(w2, !maskLastStep);
}
else {
fit.L1AddMaterial(T, sta[i].materialInfo, qp0, w2);
@@ -1568,7 +1556,7 @@ void L1Algo::GuessVec(L1TrackParFit& t, fvec* xV, fvec* yV, fvec* zV, fvec* Sy,
t.fy = (A2 * b0 - A1 * b1) * det;
t.fty = (-A1 * b0 + A0 * b1) * det;
t.fqp = -L * c_light_i * rsqrt(txtx1 + t.fty * t.fty);
- if (timeV) t.ft = time & (nhits > 0);
+ if (timeV) { t.ft = masked(time, nhits > 0); }
t.fz = z0;
}
diff --git a/reco/L1/L1Algo/L1TrackParFit.cxx b/reco/L1/L1Algo/L1TrackParFit.cxx
index e11876b3e6eca808ae562014bc2e31b055b5ebe1..7aaacce2836772b3a6f7b69bcafd9af1f7ee5c67 100644
--- a/reco/L1/L1Algo/L1TrackParFit.cxx
+++ b/reco/L1/L1Algo/L1TrackParFit.cxx
@@ -217,123 +217,94 @@ void L1TrackParFit::Filter(fvec t0, fvec dt0, fvec w, fvec timeInfo)
void L1TrackParFit::ExtrapolateLine(fvec z_out, fvec* w)
{
- cnst ZERO = 0.0, ONE = 1.;
cnst c_light = 29.9792458;
- fvec initialised = ZERO;
- if (w) //TODO use operator {?:}
- {
- const fvec zero = ZERO;
- initialised = fvec(zero < *w);
- }
- else {
- const fvec one = ONE;
- const fvec zero = ZERO;
- initialised = fvec(zero < one);
- }
-
fvec dz = (z_out - fz);
+ if (w) { dz = dz & (fvec(0.f) < *w); }
- fx += (ftx * dz & initialised);
- fy += (fty * dz & initialised);
- fz += (dz & initialised);
- ft += ((dz * sqrt(1 + ftx * ftx + fty * fty) / c_light) & initialised);
+ fx += dz * ftx;
+ fy += dz * fty;
+ fz += dz;
+ ft += dz * sqrt(1. + ftx * ftx + fty * fty) / c_light;
- const fvec k1 = ftx * dz / (c_light * sqrt((ftx * ftx) + (fty * fty) + 1));
- const fvec k2 = fty * dz / (c_light * sqrt((ftx * ftx) + (fty * fty) + 1));
+ const fvec k1 = dz * ftx / (c_light * sqrt(ftx * ftx + fty * fty + 1.));
+ const fvec k2 = dz * fty / (c_light * sqrt(ftx * ftx + fty * fty + 1.));
const fvec dzC32_in = dz * C32;
- C21 += (dzC32_in & initialised);
- C10 += (dz * (C21 + C30) & initialised);
+ C21 += dzC32_in;
+ C10 += dz * (C21 + C30);
const fvec C20_in = C20;
- C20 += ((dz * C22) & initialised);
- C00 += (dz * (C20 + C20_in) & initialised);
+ C20 += dz * C22;
+ C00 += dz * (C20 + C20_in);
const fvec C31_in = C31;
- C31 += ((dz * C33) & initialised);
- C11 += ((dz * (C31 + C31_in)) & initialised);
- C30 += dzC32_in & initialised;
+ C31 += dz * C33;
+ C11 += dz * (C31 + C31_in);
+ C30 += dzC32_in;
- C40 += (dz * C42 & initialised);
- C41 += (dz * C43 & initialised);
+ C40 += dz * C42;
+ C41 += dz * C43;
fvec c52 = C52;
fvec c53 = C53;
- C50 = ((k1 * C20 + k2 * C30) & initialised) + C50;
- C51 = ((k1 * C21 + k2 * C31) & initialised) + C51;
- C52 = ((k1 * C22 + k2 * C32) & initialised) + C52;
- C53 = ((k1 * C32 + k2 * C33) & initialised) + C53;
- C54 = ((k1 * C42 + k2 * C43) & initialised) + C54;
- C55 = ((k1 * (C52 + c52) + k2 * (C53 + c53)) & initialised) + C55;
+ C50 += k1 * C20 + k2 * C30;
+ C51 += k1 * C21 + k2 * C31;
+ C52 += k1 * C22 + k2 * C32;
+ C53 += k1 * C32 + k2 * C33;
+ C54 += k1 * C42 + k2 * C43;
+ C55 += k1 * (C52 + c52) + k2 * (C53 + c53);
}
void L1TrackParFit::ExtrapolateLine1(fvec z_out, fvec* w, fvec v)
{
- cnst ZERO = 0.0, ONE = 1.;
cnst c_light = 29.9792458;
- fvec initialised = ZERO;
- if (w) //TODO use operator {?:}
- {
- const fvec zero = ZERO;
- initialised = fvec(zero < *w);
- }
- else {
- const fvec one = ONE;
- const fvec zero = ZERO;
- initialised = fvec(zero < one);
- }
-
fvec dz = (z_out - fz);
+ if (w) { dz = dz & (fvec(0.f) < *w); }
+ fx += dz * ftx;
+ fy += dz * fty;
+ fz += dz;
- fx += (ftx * dz & initialised);
- fy += (fty * dz & initialised);
- fz += (dz & initialised);
-
-
- ft += ((dz * sqrt(1 + ftx * ftx + fty * fty) / (v * c_light)) & initialised);
+ ft += dz * sqrt(1. + ftx * ftx + fty * fty) / (v * c_light);
- const fvec k1 = ftx * dz / ((v * c_light) * sqrt((ftx * ftx) + (fty * fty) + 1));
- const fvec k2 = fty * dz / ((v * c_light) * sqrt((ftx * ftx) + (fty * fty) + 1));
+ const fvec k1 = dz * ftx / ((v * c_light) * sqrt(ftx * ftx + fty * fty + 1.));
+ const fvec k2 = dz * fty / ((v * c_light) * sqrt(ftx * ftx + fty * fty + 1.));
const fvec dzC32_in = dz * C32;
- C21 += (dzC32_in & initialised);
- C10 += (dz * (C21 + C30) & initialised);
+ C21 += dzC32_in;
+ C10 += dz * (C21 + C30);
const fvec C20_in = C20;
- C20 += ((dz * C22) & initialised);
- C00 += (dz * (C20 + C20_in) & initialised);
+ C20 += dz * C22;
+ C00 += dz * (C20 + C20_in);
const fvec C31_in = C31;
- C31 += ((dz * C33) & initialised);
- C11 += ((dz * (C31 + C31_in)) & initialised);
- C30 += dzC32_in & initialised;
+ C31 += dz * C33;
+ C11 += dz * (C31 + C31_in);
+ C30 += dzC32_in;
- C40 += (dz * C42 & initialised);
- C41 += (dz * C43 & initialised);
+ C40 += dz * C42;
+ C41 += dz * C43;
fvec c52 = C52;
fvec c53 = C53;
- C50 = ((k1 * C20 + k2 * C30) & initialised) + C50;
- C51 = ((k1 * C21 + k2 * C31) & initialised) + C51;
- C52 = ((k1 * C22 + k2 * C32) & initialised) + C52;
- C53 = ((k1 * C32 + k2 * C33) & initialised) + C53;
- C54 = ((k1 * C42 + k2 * C43) & initialised) + C54;
- C55 = ((k1 * (C52 + c52) + k2 * (C53 + c53)) & initialised) + C55;
-
- // fz = ( z_out & initialised) + ( (!initialised) & fz); //TEST
- // cout << "fz = " << fz << endl;
+ C50 += k1 * C20 + k2 * C30;
+ C51 += k1 * C21 + k2 * C31;
+ C52 += k1 * C22 + k2 * C32;
+ C53 += k1 * C32 + k2 * C33;
+ C54 += k1 * C42 + k2 * C43;
+ C55 += k1 * (C52 + c52) + k2 * (C53 + c53);
}
void L1TrackParFit::Extrapolate // extrapolates track parameters and returns jacobian for extrapolation of CovMatrix
@@ -379,6 +350,7 @@ void L1TrackParFit::Extrapolate // extrapolates track parameters and returns ja
//----------------------------------------------------------------
+ if (w) { z_out = if3((fvec(0.f) < *w), z_out, fz); }
fvec qp_in = fqp;
const fvec z_in = fz;
const fvec h = (z_out - fz);
@@ -848,8 +820,8 @@ void L1TrackParFit::EnergyLossCorrection(const fvec& radThick, fvec& qp0, fvec d
const fvec E2Corrected = (sqrt(E2) + direction * dE) * (sqrt(E2) + direction * dE);
fvec corr = sqrt(p2 / (E2Corrected - fMass2));
- fvec init = fvec(!(corr == corr)) | fvec(w < 1);
- corr = fvec(fvec(1.f) & init) + fvec(corr & fvec(!(init)));
+ fvec ok = (corr == corr) & (fvec::Zero() < w);
+ corr = if3(ok, corr, fvec::One());
qp0 *= corr;
fqp *= corr;
@@ -861,16 +833,13 @@ void L1TrackParFit::EnergyLossCorrection(const fvec& radThick, fvec& qp0, fvec d
C44 *= corr * corr;
}
-void L1TrackParFit::EnergyLossCorrectionIron(const fvec& radThick, fvec& qp0, fvec direction, fvec w)
+template<int atomicZ>
+void L1TrackParFit::EnergyLossCorrection(float atomicA, float rho, float radLen, const fvec& radThick, fvec& qp0,
+ fvec direction, fvec w)
{
const fvec p2 = 1.f / (qp0 * qp0);
const fvec E2 = fMass2 + p2;
- constexpr int atomicZ = 26;
- constexpr float atomicA = 55.845f;
- constexpr float rho = 7.87;
- constexpr float radLen = 1.758f;
-
fvec i;
if (atomicZ < 13) i = (12. * atomicZ + 7.) * 1.e-9;
else
@@ -880,13 +849,12 @@ void L1TrackParFit::EnergyLossCorrectionIron(const fvec& radThick, fvec& qp0, fv
fvec tr = sqrt(fvec(1.f) + ftx * ftx + fty * fty);
-
fvec dE = bethe * radThick * tr * radLen * rho;
const fvec E2Corrected = (sqrt(E2) + direction * dE) * (sqrt(E2) + direction * dE);
fvec corr = sqrt(p2 / (E2Corrected - fMass2));
- fvec init = fvec(!(corr == corr)) | fvec(w < 1);
- corr = fvec(fvec(1.f) & init) + fvec(corr & fvec(!(init)));
+ fvec ok = (corr == corr) & (fvec::Zero() < w);
+ corr = if3(ok, corr, fvec::One());
qp0 *= corr;
fqp *= corr;
@@ -927,136 +895,33 @@ void L1TrackParFit::EnergyLossCorrectionIron(const fvec& radThick, fvec& qp0, fv
C44 += fabs(SDEDX);
}
-void L1TrackParFit::EnergyLossCorrectionCarbon(const fvec& radThick, fvec& qp0, fvec direction, fvec w)
+
+void L1TrackParFit::EnergyLossCorrectionIron(const fvec& radThick, fvec& qp0, fvec direction, fvec w)
{
- const fvec p2 = 1.f / (qp0 * qp0);
- const fvec E2 = fMass2 + p2;
+ constexpr int atomicZ = 26;
+ constexpr float atomicA = 55.845f;
+ constexpr float rho = 7.87;
+ constexpr float radLen = 1.758f;
+ EnergyLossCorrection<atomicZ>(atomicA, rho, radLen, radThick, qp0, direction, w);
+}
+
+void L1TrackParFit::EnergyLossCorrectionCarbon(const fvec& radThick, fvec& qp0, fvec direction, fvec w)
+{
constexpr int atomicZ = 6;
constexpr float atomicA = 12.011f;
constexpr float rho = 2.265;
constexpr float radLen = 18.76f;
-
- fvec i;
- if (atomicZ < 13) i = (12. * atomicZ + 7.) * 1.e-9;
- else
- i = (9.76 * atomicZ + 58.8 * std::pow(atomicZ, -0.19)) * 1.e-9;
-
- const fvec bethe = L1Fit::ApproximateBetheBloch(p2 / fMass2, rho, 0.20, 3.00, i, atomicZ / atomicA);
-
- fvec tr = sqrt(fvec(1.f) + ftx * ftx + fty * fty);
-
-
- fvec dE = bethe * radThick * tr * radLen * rho;
-
- const fvec E2Corrected = (sqrt(E2) + direction * dE) * (sqrt(E2) + direction * dE);
- fvec corr = sqrt(p2 / (E2Corrected - fMass2));
- fvec init = fvec(!(corr == corr)) | fvec(w < 1);
- corr = fvec(fvec(1.f) & init) + fvec(corr & fvec(!(init)));
-
- qp0 *= corr;
- fqp *= corr;
-
- float P = fabs(1. / qp0[0]); // GeV
-
- float Z = atomicZ;
- float A = atomicA;
- float RHO = rho;
-
- fvec STEP = radThick * tr * radLen;
- fvec EMASS = 0.511 * 1e-3; // GeV
-
- fvec BETA = P / sqrt(E2Corrected);
- fvec GAMMA = sqrt(E2Corrected) / fMass;
-
- // Calculate xi factor (KeV).
- fvec XI = (153.5 * Z * STEP * RHO) / (A * BETA * BETA);
-
- // Maximum energy transfer to atomic electron (KeV).
- fvec ETA = BETA * GAMMA;
- fvec ETASQ = ETA * ETA;
- fvec RATIO = EMASS / fMass;
- fvec F1 = 2. * EMASS * ETASQ;
- fvec F2 = 1. + 2. * RATIO * GAMMA + RATIO * RATIO;
- fvec EMAX = 1e6 * F1 / F2;
-
- fvec DEDX2 = XI * EMAX * (1. - (BETA * BETA / 2.)) * 1e-12;
-
- float P2 = P * P;
- fvec SDEDX = ((E2) *DEDX2) / (P2 * P2 * P2);
-
- // T.C40 *= corr;
- // T.C41 *= corr;
- // T.C42 *= corr;
- // T.C43 *= corr;
- // T.C44 *= corr*corr;
- C44 += fabs(SDEDX);
+ EnergyLossCorrection<atomicZ>(atomicA, rho, radLen, radThick, qp0, direction, w);
}
void L1TrackParFit::EnergyLossCorrectionAl(const fvec& radThick, fvec& qp0, fvec direction, fvec w)
{
- const fvec p2 = 1.f / (qp0 * qp0);
- const fvec E2 = fMass2 + p2;
-
constexpr int atomicZ = 13;
constexpr float atomicA = 26.981f;
constexpr float rho = 2.70f;
constexpr float radLen = 2.265f;
-
- fvec i;
- if (atomicZ < 13) i = (12. * atomicZ + 7.) * 1.e-9;
- else
- i = (9.76 * atomicZ + 58.8 * std::pow(atomicZ, -0.19)) * 1.e-9;
-
- const fvec bethe = L1Fit::ApproximateBetheBloch(p2 / fMass2, rho, 0.20, 3.00, i, atomicZ / atomicA);
-
- fvec tr = sqrt(fvec(1.f) + ftx * ftx + fty * fty);
-
-
- fvec dE = bethe * radThick * tr * radLen * rho;
-
- const fvec E2Corrected = (sqrt(E2) + direction * dE) * (sqrt(E2) + direction * dE);
- fvec corr = sqrt(p2 / (E2Corrected - fMass2));
- fvec init = fvec(!(corr == corr)) | fvec(w < 1);
- corr = fvec(fvec(1.f) & init) + fvec(corr & fvec(!(init)));
-
- qp0 *= corr;
- fqp *= corr;
-
- float P = fabs(1. / qp0[0]); // GeV
-
- float Z = atomicZ;
- float A = atomicA;
- float RHO = rho;
-
- fvec STEP = radThick * tr * radLen;
- fvec EMASS = 0.511 * 1e-3; // GeV
-
- fvec BETA = P / sqrt(E2Corrected);
- fvec GAMMA = sqrt(E2Corrected) / fMass;
-
- // Calculate xi factor (KeV).
- fvec XI = (153.5 * Z * STEP * RHO) / (A * BETA * BETA);
-
- // Maximum energy transfer to atomic electron (KeV).
- fvec ETA = BETA * GAMMA;
- fvec ETASQ = ETA * ETA;
- fvec RATIO = EMASS / fMass;
- fvec F1 = 2. * EMASS * ETASQ;
- fvec F2 = 1. + 2. * RATIO * GAMMA + RATIO * RATIO;
- fvec EMAX = 1e6 * F1 / F2;
-
- fvec DEDX2 = XI * EMAX * (1. - (BETA * BETA / 2.)) * 1e-12;
-
- float P2 = P * P;
- fvec SDEDX = ((E2) *DEDX2) / (P2 * P2 * P2);
-
- // T.C40 *= corr;
- // T.C41 *= corr;
- // T.C42 *= corr;
- // T.C43 *= corr;
- // T.C44 *= corr*corr;
- C44 += fabs(SDEDX);
+ EnergyLossCorrection<atomicZ>(atomicA, rho, radLen, radThick, qp0, direction, w);
}
#undef cnst
diff --git a/reco/L1/L1Algo/L1TrackParFit.h b/reco/L1/L1Algo/L1TrackParFit.h
index ce9e5634c9ac5f01ece13510c1a9ae8d262f41b3..2712c44af512c0bc1a9ec29b17e90538cf82e7fe 100644
--- a/reco/L1/L1Algo/L1TrackParFit.h
+++ b/reco/L1/L1Algo/L1TrackParFit.h
@@ -112,16 +112,24 @@ public:
void ExtrapolateLine(fvec z_out, fvec* w = 0);
void ExtrapolateLine1(fvec z_out, fvec* w = 0, fvec v = 0);
void Compare(L1TrackPar& T);
+
void EnergyLossCorrection(const fvec& radThick, fvec& qp0, fvec direction, fvec w);
- void L1AddMaterial(const L1MaterialInfo& info, fvec qp0, fvec w = 1);
+
+ template<int atomicZ>
+ void EnergyLossCorrection(float atomicA, float rho, float radLen, const fvec& radThick, fvec& qp0, fvec direction,
+ fvec w);
+
+ void EnergyLossCorrectionIron(const fvec& radThick, fvec& qp0, fvec direction, fvec w);
+ void EnergyLossCorrectionCarbon(const fvec& radThick, fvec& qp0, fvec direction, fvec w);
+ void EnergyLossCorrectionAl(const fvec& radThick, fvec& qp0, fvec direction, fvec w);
+
+ void L1AddMaterial(const L1MaterialInfo& info, fvec qp0, fvec w);
void L1AddMaterial(fvec radThick, fvec qp0, fvec w = 1);
void L1AddThickMaterial(fvec radThick, fvec qp0, fvec w, fvec thickness, bool fDownstream);
void L1AddPipeMaterial(fvec qp0, fvec w = 1);
- void EnergyLossCorrectionIron(const fvec& radThick, fvec& qp0, fvec direction, fvec w = 1);
- void EnergyLossCorrectionCarbon(const fvec& radThick, fvec& qp0, fvec direction, fvec w = 1);
- void EnergyLossCorrectionAl(const fvec& radThick, fvec& qp0, fvec direction, fvec w = 1);
+
// void L1Extrapolate
// (
// // L1TrackParFit &T, // input track parameters (x,y,tx,ty,Q/p) and cov.matrix
diff --git a/reco/L1/L1Algo/L1Utils.h b/reco/L1/L1Algo/L1Utils.h
index 7b053e5065bddb35f558344f2e0abbaa1a1afee8..740c713c86564fe228e5021595a115cc9365b5cf 100644
--- a/reco/L1/L1Algo/L1Utils.h
+++ b/reco/L1/L1Algo/L1Utils.h
@@ -44,7 +44,7 @@ namespace L1Utils
[[gnu::always_inline]] inline void CheckSimdVectorEquality(fvec v, const char* name)
{
- if (!v.IsHorizontallyEqual()) {
+ if (!IsHorizontallyEqual(v)) {
std::stringstream msg;
msg << name << " SIMD vector is inconsistent, not all of the words are equal each other: " << v;
throw std::logic_error(msg.str());
diff --git a/reco/L1/OffLineInterface/CbmL1RichENNRingFinderParallel.cxx b/reco/L1/OffLineInterface/CbmL1RichENNRingFinderParallel.cxx
index 2858b1c4badd39e617292b1fd5ffd7ce0ef8033a..a357adb554202926ccc141be247c0f5320de13fe 100644
--- a/reco/L1/OffLineInterface/CbmL1RichENNRingFinderParallel.cxx
+++ b/reco/L1/OffLineInterface/CbmL1RichENNRingFinderParallel.cxx
@@ -305,7 +305,7 @@ void CbmL1RichENNRingFinderParallel::ENNRingFinder(const int NHits, nsL1vector<E
fvec S0, S1, S2, S3, S4, S5, S6, S7;
fvec X = 0, Y = 0, R = 0, R2 = 0;
- fvec validRing = (fvec(0) <= fvec(0)); // mask of the valid rings
+ fvec validRing(fvec::MaskOne()); // mask of the valid rings
fvec SearchAreaSize = 0; // number of hits to fit and search ring
fvec PickUpAreaSize = 0;
@@ -470,7 +470,7 @@ void CbmL1RichENNRingFinderParallel::ENNRingFinder(const int NHits, nsL1vector<E
S0 = S1 = S2 = S3 = S4 = S5 = S6 = S7 = 0.0;
for (THitIndex ih = 0; ih < MaxSearchAreaSize; ih++) {
ENNSearchHitV& sHit = SearchArea[ih];
- const fvec w = bool2int(SearchArea[ih].on_ring);
+ const fvec w = mask2int(SearchArea[ih].on_ring);
S0 += w * sHit.S0;
S1 += w * sHit.S1;
S2 += w * sHit.S2;
@@ -532,7 +532,7 @@ void CbmL1RichENNRingFinderParallel::ENNRingFinder(const int NHits, nsL1vector<E
validHit = validHit & ( d <= HitSize );
ringV.chi2 += d*d;
ringV.localIHits.push_back( if3( validHit, sHit.localIndex, -1 ) );
- ringV.NHits += bool2int(validHit);
+ ringV.NHits += mask2int(validHit);
validHit = validHit & ( d <= ShadowSize ); // TODO check *4
if ( Empty (validHit) ) continue; // CHECKME
Shadow.push_back( if3( validHit, sHit.localIndex, -1 ) );
@@ -548,7 +548,7 @@ void CbmL1RichENNRingFinderParallel::ENNRingFinder(const int NHits, nsL1vector<E
if ( Empty (validHit) ) continue;
ringV.chi2 += d*d;
ringV.localIHits.push_back( if3( validHit, puHit.localIndex, -1 ) );
- ringV.NHits += bool2int(validHit);
+ ringV.NHits += mask2int(validHit);
validHit = validHit & ( d <= ShadowSize ); // TODO check *4
if ( Empty (validHit) ) continue; // CHECKME
Shadow.push_back( if3( validHit, puHit.localIndex, -1 ) );
diff --git a/reco/L1/OffLineInterface/CbmL1RichENNRingFinderParallel.h b/reco/L1/OffLineInterface/CbmL1RichENNRingFinderParallel.h
index 731d8cac98c5d41393aaba28febf36a61ff320ed..e6a3889ff8ed74fb4eda73a6683d268a559214ad 100644
--- a/reco/L1/OffLineInterface/CbmL1RichENNRingFinderParallel.h
+++ b/reco/L1/OffLineInterface/CbmL1RichENNRingFinderParallel.h
@@ -114,7 +114,7 @@ class CbmL1RichENNRingFinderParallel : public CbmRichRingFinder {
, Cx(0)
, Cy(0)
, // coefficients for the parameter space
- on_ring(0) {};
+ on_ring(fvec::MaskOne()) {};
// variables for local search:
fvec lx, ly, lr2; // local coordinates
diff --git a/reco/L1/ParticleFinder/CbmL1PFFitter.cxx b/reco/L1/ParticleFinder/CbmL1PFFitter.cxx
index 64c0b43332c8d8060508e6dfa50d044b2351e360..98873e494cebcda4599300301c358ff2923fcce9 100644
--- a/reco/L1/ParticleFinder/CbmL1PFFitter.cxx
+++ b/reco/L1/ParticleFinder/CbmL1PFFitter.cxx
@@ -252,9 +252,9 @@ void CbmL1PFFitter::Fit(vector<CbmStsTrack>& Tracks, vector<int>& pidHypo)
b0.Combine(fB[i], w[i]);
fld.Set(b0, z0, b1, z1, b2, z2);
- fvec initialised = fvec(z[i] <= z_end) & fvec(z_start < z[i]);
- fvec w1 = (w[i] & (initialised));
- fvec wIn = (ONE & (initialised));
+ fvec initialised = (z[i] <= z_end) & (z_start < z[i]);
+ fvec w1 = masked(w[i], initialised);
+ fvec wIn = mask2int(initialised);
L1Extrapolate(T, z[i], qp0, fld, &w1);
if (i == NMvdStations) { // TODO: How a hit can be also a station? (S.Zharko)
@@ -329,9 +329,9 @@ void CbmL1PFFitter::Fit(vector<CbmStsTrack>& Tracks, vector<int>& pidHypo)
b0.Combine(fB[i], w[i]);
fld.Set(b0, z0, b1, z1, b2, z2);
- fvec initialised = fvec(z[i] < z_end) & fvec(z_start <= z[i]);
- fvec w1 = (w[i] & (initialised));
- fvec wIn = (ONE & (initialised));
+ fvec initialised = (z[i] < z_end) & (z_start <= z[i]);
+ fvec w1 = masked(w[i], initialised);
+ fvec wIn = mask2int(initialised);
L1Extrapolate(T, z[i], qp0, fld, &w1);
if (i == NMvdStations - 1) {
@@ -409,7 +409,7 @@ void CbmL1PFFitter::GetChiToVertex(vector<CbmStsTrack>& Tracks, vector<L1FieldRe
int nStations = CbmL1::Instance()->fpAlgo->GetParameters()->GetNstationsActive();
int NMvdStations = CbmL1::Instance()->fpAlgo->GetNstationsBeforePipe();
const L1Station* sta = CbmL1::Instance()->fpAlgo->GetParameters()->GetStations().begin();
- fvec* zSta = new fvec[nStations];
+ fvec* zSta = new fvec[nStations];
for (int iSta = 0; iSta < nStations; iSta++)
zSta[iSta] = sta[iSta].z;
@@ -496,9 +496,8 @@ void CbmL1PFFitter::GetChiToVertex(vector<CbmStsTrack>& Tracks, vector<L1FieldRe
field.push_back(fld);
for (int iSt = nStations - 4; iSt >= 0; iSt--) {
- fvec w = ONE;
- fvec initialized = fvec(T.z > (zSta[iSt] + 2.5));
- w = fvec(w & initialized);
+
+ fvec w = mask2int(T.z > (zSta[iSt] + 2.5));
L1Extrapolate(T, zSta[iSt], T.qp, fld, &w);
if (iSt == NMvdStations - 1) {
@@ -521,15 +520,16 @@ void CbmL1PFFitter::GetChiToVertex(vector<CbmStsTrack>& Tracks, vector<L1FieldRe
fvec dx = T.x - primVtx.GetRefX();
fvec dy = T.y - primVtx.GetRefY();
fvec c[3] = {T.C00, T.C10, T.C11};
- c[0] += Cv[0];
- c[1] += Cv[1];
- c[2] += Cv[2];
- fvec d = c[0] * c[2] - c[1] * c[1];
- fvec chi = sqrt(fabs(0.5 * (dx * dx * c[0] - 2 * dx * dy * c[1] + dy * dy * c[2]) / d));
- fvec isNull = fvec(fabs(d) < 1.e-20);
- chi = fvec(fvec(!isNull) & chi) + fvec(isNull & fvec(0));
- for (int iVec = 0; iVec < nTracks_SIMD; iVec++)
+ c[0] += fvec(Cv[0]);
+ c[1] += fvec(Cv[1]);
+ c[2] += fvec(Cv[2]);
+ fvec d = c[0] * c[2] - c[1] * c[1];
+ fvec chi = sqrt(fabs(0.5 * (dx * dx * c[0] - 2 * dx * dy * c[1] + dy * dy * c[2]) / d));
+ chi = masked(chi, fabs(d) >= 1.e-20);
+
+ for (int iVec = 0; iVec < nTracks_SIMD; iVec++) {
chiToVtx.push_back(chi[iVec]);
+ }
if (chiPrim > 0) {
for (int iVec = 0; iVec < nTracks_SIMD; iVec++) {
diff --git a/reco/L1/vectors/P4_F32vec4.h b/reco/L1/vectors/P4_F32vec4.h
index 765a33d6872c9f236cdb96226937a64c1c7da922..a18457bd06705b51c93b21ddf6aecda5216d66d7 100644
--- a/reco/L1/vectors/P4_F32vec4.h
+++ b/reco/L1/vectors/P4_F32vec4.h
@@ -143,16 +143,17 @@ public:
return _mm_cmpeq_ps(a, b);
}
-#define if3(a, b, c) ((a) & (b)) | ((!(a)) & (c)) // analog (a) ? b : c
+ // #define if3(a, b, c) ((a) & (b)) | ((!(a)) & (c)) // analog (a) ? b : c
+
+ static F32vec4 One() { return F32vec4(1.); }
+ static F32vec4 Zero() { return F32vec4(0.); }
+ static F32vec4 MaskOne() { return _f32vec4_true; }
+ static F32vec4 MaskZero() { return _f32vec4_false; }
#define NotEmptyFvec(a) bool((a)[0]) | bool((a)[1]) | bool((a)[2]) | bool((a)[3])
#define EmptyFvec(a) !(bool((a)[0]) | bool((a)[1]) | bool((a)[2]) | bool((a)[3]))
// bool NotEmpty(const F32vec4 &a) { return a[0]||a[1]||a[2]||a[3]; }
// bool Empty(const F32vec4 &a) { return !(a[0]||a[1]||a[2]||a[3]); } // optimize
- friend F32vec4 bool2int(const F32vec4& a)
- { // mask returned
- return if3(a, 1, 0);
- }
/* Define all operators for consistensy */
@@ -228,23 +229,6 @@ public:
return strm;
}
- /// Checks, if all bands are equal
- /// NOTE: two values defined as signaling_NaN() are not equal, thus if there are all or one
- /// of the words are kNaN, the function returns false
- bool IsHorizontallyEqual() const { return v[0] == v[1] && v[1] == v[2] && v[2] == v[3]; }
-
- /// Checks, if any of the bands is NaN
- bool IsNanAny() const { return std::isnan(v[0]) || std::isnan(v[1]) || std::isnan(v[2]) || std::isnan(v[3]); }
-
- /// Checks, if all of the bands is NaN
- bool IsNanAll() const { return std::isnan(v[0]) && std::isnan(v[1]) && std::isnan(v[2]) && std::isnan(v[3]); }
-
- /// Checks, if all of the bands are finite
- bool IsFiniteAll() const
- {
- return std::isfinite(v[0]) && std::isfinite(v[1]) && std::isfinite(v[2]) && std::isfinite(v[3]);
- }
-
} __attribute__((aligned(16)));
@@ -255,6 +239,41 @@ const int fvecLen = 4;
//#define fvec_false _f32vec4_false
#define _fvecalignment __attribute__((aligned(16)))
+inline fvec if3(const fvec& mask, const fvec& b, const fvec& c)
+{
+ // return (a ?b :c);
+ return (b & mask) + (c & (!mask));
+}
+
+inline fvec masked(const fvec& a, const fvec& mask) { return if3(mask, a, fvec::Zero()); }
+
+inline fvec mask2int(const fvec& mask)
+{ // mask returned
+ return if3(mask, fvec::One(), fvec::Zero());
+}
+
+/// Checks, if all bands are equal
+/// NOTE: two values defined as signaling_NaN() are not equal, thus if there are all or one
+/// of the words are kNaN, the function returns false
+inline bool IsHorizontallyEqual(const fvec& v)
+{
+ fscal s = v[0];
+ bool ret = true;
+ for (int i = 1; i < fvecLen; i++) {
+ ret = ret && (v[i] == s);
+ }
+ return ret;
+}
+
+/// Checks, if any of the bands is NaN
+inline bool IsNanAny(const fvec& v)
+{
+ bool ret = false;
+ for (int i = 0; i < fvecLen; i++) {
+ ret = ret || std::isnan(v[i]);
+ }
+ return ret;
+}
#include "std_alloc.h"