-
Sergey Gorbunov authoredSergey Gorbunov authored
L1Extrapolation.h 31.57 KiB
/* Copyright (C) 2007-2017 GSI Helmholtzzentrum fuer Schwerionenforschung, Darmstadt
SPDX-License-Identifier: GPL-3.0-only
Authors: Maksym Zyzak, Sergey Gorbunov [committer], Igor Kulakov */
#ifndef L1Extrapolation_h
#define L1Extrapolation_h
#include "L1Def.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