-
Eoin Clerkin authored
Decision to not use doxygen for licence headers. Removes doxygen formatting and file tag.
Eoin Clerkin authoredDecision to not use doxygen for licence headers. Removes doxygen formatting and file tag.
L1Extrapolation.h 31.57 KiB
/* Copyright (C) 2007-2017 GSI Helmholtzzentrum fuer Schwerionenforschung, Darmstadt
SPDX-License-Identifier: GPL-3.0-only
Authors: Maxim Zyzak, Sergey Gorbunov [committer], Igor Kulakov */
#ifndef L1Extrapolation_h
#define L1Extrapolation_h
#include "CbmL1Def.h"
#include "L1Field.h"
#include "L1TrackPar.h"
//#define cnst static const fvec
#define cnst const fvec
// #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;
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]) & 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)
{
//
// 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
)
{
cnst c_light = 29.9792458;
T.t += sqrt((T.tx * T.tx) + (T.ty * T.ty) + 1) * dz / c_light;
const fvec k1 = T.tx * dz / (c_light * sqrt((T.tx * T.tx) + (T.ty * T.ty) + 1));
const fvec k2 = T.ty * dz / (c_light * sqrt((T.tx * T.tx) + (T.ty * T.ty) + 1));
fvec c52 = T.C52;
fvec c53 = T.C53;
// cout<<k1<<" k1 "<<k2<<" k2 "<<endl;
// fvec ha = T.C53;
// fvec ha2 = T.C54;
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);
// cout<<ha<<" c53 "<<T.C53<<endl;
// cout<<ha2<<" c54 "<<T.C54<<endl;
}
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;
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;
}
inline void L1ExtrapolateXC00Line(const L1TrackPar& T, fvec z_out, fvec& x, fvec& C00)
{
const fvec dz = (z_out - T.z);
x = T.x + T.tx * dz;
C00 = T.C00 + dz * (2 * T.C20 + dz * T.C22);
}
inline void L1ExtrapolateYC11Line(const L1TrackPar& T, fvec z_out, fvec& y, fvec& C11)
{
const fvec dz = (z_out - T.z); y = T.y + T.ty * dz;
C11 = T.C11 + dz * (2 * T.C31 + dz * T.C33);
}
inline void L1ExtrapolateC10Line(const L1TrackPar& T, fvec z_out, fvec& C10)
{
const fvec dz = (z_out - T.z);
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