/// /// @primary authors: S.Gorbunov; I.Kisel /// @ authors: H.Pabst et al. (Intel); M.Zyzak; I.Kulakov /// #ifndef FitFunctional_H #define FitFunctional_H /// Abstract class. Contains discription of basic Fit Functional, which is used by any fit method ( Fit, Smother, DAF ) #include using namespace arbb; #include "fit_ct.h" #define INF 0.01 #define INF2 0.0001 #define ZERO 0.0 #define ONE 1. #define c_light 0.000299792458 #define c_light_i 1./c_light const ftype PipeRadThick = 0.0009; template class Jacobian_t{ // jacobian elements // j[0][0] - j[3][2] are j02 - j34 public: dense &operator()(int i, int j) { assert( i>=0 && j>=2 ); return fj[i][j-2]; }; Jacobian_t(){ for( int i = 0; i < 4; ++i) for( int j = 0; j < 3; ++j) fj[i][j] = fill(0,1); }; private: // 1 0 ? ? ? // 0 1 ? ? ? // j = 0 0 ? ? ? // 0 0 ? ? ? // 0 0 0 0 1 dense fj[4][3]; }; inline int IC( int i, int j ){ // returns index in VecC from row and column numbers of CovMatrix return ( j<=i ) ? i*(i+1)/2+j :j*(j+1)/2+i; } template class FitFunctional { // base class for all approaches public: /// extrapolates track parameters virtual void Extrapolate( FitResults& results, const W &zOut, // extrapolate to this z position dense& qp0, // use Q/p linearisation at this value const FieldRegionCt &F, dense w ) const = 0; protected: /// initial aproximation void GuessVec( TracksCt &ts, StationsCt &ss, FitResults& results, bool dir ) const; virtual void Filter( TracksCt &ts, StationsCt &ss, const HitInfoCt &info, usize iS, const dense &u, const dense w, FitResults& results ) const = 0; // filter first mesurement virtual void FilterFirst( TracksCt &ts, StationsCt &ss, usize iStation, FitResults& results ) const = 0; virtual void AddMaterial( StationsCt &ss, usize iStation, dense &qp0, FitResults& results, bool isPipe ) const; void AddPipeMaterial( dense &qp0, FitResults& results ) const; void AddHalfMaterial( StationsCt &ss, usize iStation, dense &qp0, FitResults& results ) const; virtual void ExtrapolateWithMaterial( FitResults& results, const W &zOut, dense& qp0, const FieldRegionCt &F, StationsCt &ss, usize iStation, bool isPipe, dense w ) const = 0; // ------------ help functions ------------ virtual void AddMaterial( CovV &C, dense Q22, dense Q32, dense Q33 ) const = 0; /// extrapolates track parameters and returns jacobian for extrapolation of CovMatrix void ExtrapolateJ ( dense T[6], // input track parameters (x,y,tx,ty,Q/p) and cov.matrix const W& z_out , // extrapolate to this z position dense& qp0 , // use Q/p linearisation at this value const FieldRegionCt &F, Jacobian_t &j, dense w ) const; /// calculate covMatrix for Multiple Scattering void GetMSMatrix( const dense tx, const dense ty, const U radThick, const U logRadThick, dense qp0, dense& Q22, dense& Q32, dense& Q33 ) const; private: /// extrapolates track parameters and returns jacobian for extrapolation of CovMatrix #if defined(ANALYTIC_EXTRAPOLATION) void ExtrapolateJAnalytic #elif defined(RK4_EXTRAPOLATION) void ExtrapolateJRK4 #endif ( dense *T, // input track parameters (x,y,tx,ty,Q/p) and cov.matrix const W &z_out , // extrapolate to this z position dense qp0 , // use Q/p linearisation at this value const FieldRegionCt &F, Jacobian_t &j, dense w ) const; }; template void FitFunctional::GuessVec( TracksCt &ts, StationsCt &ss, FitResults& results, bool dir ) const { dense* T = results.VecT; U z0; dense S; if( !dir ) { S = ss.SyL; z0 = ss.zhit[vStations.nStations - 1]; } else { S = ss.SyF; z0 = ss.zhit[0]; } //! Initialize parameters. U w = 1; //h.w, seems that it's alway be 1, so here we use a const instead. U A0 = static_cast::type>(vStations.nStations) * w; dense x = ts.hitsX; dense y = ts.hitsY; dense z = ss.zhit - z0; dense z2d = repeat_row( z, vTracks.nTracks ); dense S2d = repeat_row( S, vTracks.nTracks ); U A1 = add_reduce( z * w ); U A2 = add_reduce( z * z * w ); U A3 = add_reduce( S * w ); U A4 = add_reduce( S * z * w ); U A5 = add_reduce( S * S * w ); // optReduce of Vec now returns a Scalar dense a0 = add_reduce( x * w ); dense a1 = add_reduce( x * z2d * w ); dense a2 = add_reduce( x * S2d * w ); dense b0 = add_reduce( y * w ); dense b1 = add_reduce( y * z2d * w ); dense b2 = add_reduce( y * S2d * w ); U A3A3 = A3 * A3; U A3A4 = A3 * A4; U A1A5 = A1 * A5; U A2A5 = A2 * A5; U A4A4 = A4 * A4; U det = rcp( -A2 * A3A3 + A1 * ( A3A4 * 2 - A1A5 ) + A0 * ( A2A5 - A4A4 ) ); U Ai0 = -A4A4 + A2A5; U Ai1 = A3A4 - A1A5; U Ai2 = -A3A3 + A0 * A5; U Ai3 = -A2 * A3 + A1 * A4; U Ai4 = A1 * A3 - A0 * A4; U Ai5 = -A1 * A1 + A0 * A2; T[0] = ( Ai0 * a0 + Ai1 * a1 + Ai3 * a2 ) * det; T[2] = ( Ai1 * a0 + Ai2 * a1 + Ai4 * a2 ) * det; dense txtx1 = T[2] * T[2] + 1; dense L = ( Ai3 * a0 + Ai4 * a1 + Ai5 * a2 ) * det * rcp ( txtx1 ); dense L1 = L * T[2]; dense A1V = A1 + A3 * L1; dense A2V = A2 + ( A4 * (U)2 + A5 * L1 ) * L1; b1 += b2 * L1; dense detV = rcp( -A1V * A1V + A0 * A2V ); T[1] = ( A2V * b0 - A1V * b1 ) * detV; T[3] = ( -A1V * b0 + A0 * b1 ) * detV; T[4] = -L * _CLIGHT_I * rsqrt( txtx1 + T[3] * T[3] ); T[5] = fill( (U)z0, vTracks.nTracks ); } #if defined(ANALYTIC_EXTRAPOLATION) template inline void FitFunctional::ExtrapolateJAnalytic // extrapolates track parameters and returns jacobian for extrapolation of CovMatrix ( dense T [], // input track parameters (x,y,tx,ty,Q/p) const W& z_out , // extrapolate to this z position dense qp0 , // use Q/p linearisation at this value const FieldRegionCt &F, Jacobian_t &j, dense w ) const { //cout<<"Extrapolation..."< qp = T[4]; const dense dz = (z_out - T[5]); const dense dz2 = dz*dz; const dense dz3 = dz2*dz; // construct coefficients const dense x = T[2]; const dense y = T[3]; const dense xx = x*x; const dense xy = x*y; const dense yy = y*y; const dense y2 = y*c2; const dense x2 = x*c2; const dense x4 = x*c4; const dense xx31 = xx*c3+c1; const dense xx159 = xx*c15+c9; const dense Ay = -xx-c1; const dense Ayy = x*(xx*c3+c3); const dense Ayz = -c2*xy; const dense Ayyy = -(c15*xx*xx+c18*xx+c3); const dense Ayy_x = c3*xx31; const dense Ayyy_x = -x4*xx159; const dense Bx = yy+c1; const dense Byy = y*xx31; const dense Byz = c2*xx+c1; const dense Byyy = -xy*xx159; const dense Byy_x = c6*xy; const dense Byyy_x = -y*(c45*xx+c9); const dense Byyy_y = -x*xx159; // end of coefficients calculation const dense t2 = c1 + xx + yy; const dense t = sqrt( t2 ); const dense h = qp0*c_light; const dense ht = h*t; // get field integrals const dense ddz = T[5]-F.z; const dense Fx0 = F.x0 + F.x1*ddz + F.x2*ddz*ddz; const dense Fx1 = (F.x1 + c2*F.x2*ddz)*dz; const dense Fx2 = F.x2*dz2; const dense Fy0 = F.y0 + F.y1*ddz + F.y2*ddz*ddz; const dense Fy1 = (F.y1 + c2*F.y2*ddz)*dz; const dense Fy2 = F.y2*dz2; const dense Fz0 = F.z0 + F.z1*ddz + F.z2*ddz*ddz; const dense Fz1 = (F.z1 + c2*F.z2*ddz)*dz; const dense Fz2 = F.z2*dz2; // const dense sx = ( Fx0 + Fx1*c2i + Fx2*c3i ); const dense sy = ( Fy0 + Fy1*c2i + Fy2*c3i ); const dense sz = ( Fz0 + Fz1*c2i + Fz2*c3i ); const dense Sx = ( Fx0*c2i + Fx1*c6i + Fx2*c12i ); const dense Sy = ( Fy0*c2i + Fy1*c6i + Fy2*c12i ); const dense Sz = ( Fz0*c2i + Fz1*c6i + Fz2*c12i ); dense syz; { const U 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) ; } dense Syz; { const U 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 dense syy = sy*sy*c2i; const dense syyy = syy*sy*c3i; dense Syy ; { const U 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 ; } dense Syyy; { const U 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 dense 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 dense sA1 = sx*xy + sy*Ay + sz*y ; const dense sA1_x = sx*y - sy*x2 ; const dense sA1_y = sx*x + sz ; const dense sB1 = sx*Bx - sy*xy - sz*x ; const dense sB1_x = -sy*y - sz ; const dense sB1_y = sx*y2 - sy*x ; const dense SA1 = Sx*xy + Sy*Ay + Sz*y ; const dense SA1_x = Sx*y - Sy*x2 ; const dense SA1_y = Sx*x + Sz; const dense SB1 = Sx*Bx - Sy*xy - Sz*x ; const dense SB1_x = -Sy*y - Sz; const dense SB1_y = Sx*y2 - Sy*x; const dense sA2 = syy*Ayy + syz*Ayz ; const dense sA2_x = syy*Ayy_x - syz*y2 ; const dense sA2_y = -syz*x2 ; const dense sB2 = syy*Byy + syz*Byz ; const dense sB2_x = syy*Byy_x + syz*x4 ; const dense sB2_y = syy*xx31 ; const dense SA2 = Syy*Ayy + Syz*Ayz ; const dense SA2_x = Syy*Ayy_x - Syz*y2 ; const dense SA2_y = -Syz*x2 ; const dense SB2 = Syy*Byy + Syz*Byz ; const dense SB2_x = Syy*Byy_x + Syz*x4 ; const dense SB2_y = Syy*xx31 ; const dense sA3 = syyy*Ayyy ; const dense sA3_x = syyy*Ayyy_x; const dense sB3 = syyy*Byyy ; const dense sB3_x = syyy*Byyy_x; const dense sB3_y = syyy*Byyy_y; const dense SA3 = Syyy*Ayyy ; const dense SA3_x = Syyy*Ayyy_x; const dense SB3 = Syyy*Byyy ; const dense SB3_x = Syyy*Byyy_x; const dense SB3_y = Syyy*Byyy_y; const dense ht1 = ht*dz; const dense ht2 = ht*ht*dz2; const dense ht3 = ht*ht*ht*dz3; const dense ht1sA1 = ht1*sA1; const dense ht1sB1 = ht1*sB1; const dense ht1SA1 = ht1*SA1; const dense ht1SB1 = ht1*SB1; const dense ht2sA2 = ht2*sA2; const dense ht2SA2 = ht2*SA2; const dense ht2sB2 = ht2*sB2; const dense ht2SB2 = ht2*SB2; const dense ht3sA3 = ht3*sA3; const dense ht3sB3 = ht3*sB3; const dense ht3SA3 = ht3*SA3; const dense ht3SB3 = ht3*SB3; const dense initialised = ( 0 >= w ); T[0] += select(initialised, ((x + ht1SA1 + ht2SA2 + ht3SA3)*dz), 0) ; T[1] += select(initialised, ((y + ht1SB1 + ht2SB2 + ht3SB3)*dz), 0) ; T[2] += select(initialised, (ht1sA1 + ht2sA2 + ht3sA3), 0); T[3] += select(initialised, (ht1sB1 + ht2sB2 + ht3sB3), 0); T[5] += select(initialised, dz, 0); const dense ctdz = c_light*t*dz; const dense ctdz2 = c_light*t*dz2; const dense dqp = qp - qp0; const dense t2i = c1*rcp(t2);// /t2; const dense xt2i = x*t2i; const dense yt2i = y*t2i; const dense tmp0 = ht1SA1 + c2*ht2SA2 + c3*ht3SA3; const dense tmp1 = ht1SB1 + c2*ht2SB2 + c3*ht3SB3; const dense tmp2 = ht1sA1 + c2*ht2sA2 + c3*ht3sA3; const dense tmp3 = ht1sB1 + c2*ht2sB2 + c3*ht3sB3; // 1 0 ? ? ? // 0 1 ? ? ? // j = 0 0 ? ? ? // 0 0 ? ? ? // 0 0 0 0 1 j(0,2) = dz*(c1 + xt2i*tmp0 + ht1*SA1_x + ht2*SA2_x + ht3*SA3_x); j(1,2) = dz*( xt2i*tmp1 + ht1*SB1_x + ht2*SB2_x + ht3*SB3_x); j(2,2) = c1 + xt2i*tmp2 + ht1*sA1_x + ht2*sA2_x + ht3*sA3_x ; j(3,2) = xt2i*tmp3 + ht1*sB1_x + ht2*sB2_x + ht3*sB3_x ; j(0,3) = dz*( yt2i*tmp0 + ht1*SA1_y + ht2*SA2_y ); j(1,3) = dz*(c1 + yt2i*tmp1 + ht1*SB1_y + ht2*SB2_y + ht3*SB3_y ); j(2,3) = yt2i*tmp2 + ht1*sA1_y + ht2*sA2_y ; j(3,3) = c1 + yt2i*tmp3 + ht1*sB1_y + ht2*sB2_y + ht3*sB3_y ; j(0,4) = ctdz2*( SA1 + c2*ht1*SA2 + c3*ht2*SA3 ); j(1,4) = ctdz2*( SB1 + c2*ht1*SB2 + c3*ht2*SB3 ); j(2,4) = ctdz *( sA1 + c2*ht1*sA2 + c3*ht2*sA3 ); j(3,4) = ctdz *( sB1 + c2*ht1*sB2 + c3*ht2*sB3 ); // extrapolate inverse momentum T[0] += select(initialised, (j(0,4)*dqp), 0); T[1] += select(initialised, (j(1,4)*dqp), 0); T[2] += select(initialised, (j(2,4)*dqp), 0); T[3] += select(initialised, (j(3,4)*dqp), 0); } #elif defined(RK4_EXTRAPOLATION) template inline void FitFunctional::ExtrapolateJRK4 // extrapolates track parameters and returns jacobian for extrapolation of CovMatrix ( dense *T, // input track parameters (x,y,tx,ty,Q/p) and cov.matrix const W &z_out , // extrapolate to this z position dense qp0 , // use Q/p linearisation at this value const FieldRegionCt &F, Jacobian_t &j, dense w ) const { // // 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 // //======================================================================== const dense initialised = ( 0 >= w ); const U a[4] = {0.0, 0.5, 0.5, 1.0}; const U c[4] = {1.0/6.0, 1.0/3.0, 1.0/3.0, 1.0/6.0}; const U b[4] = {0.0, 0.5, 0.5, 1.0}; dense k[16],x0[4],x[4],k1[16]; dense Ax[4],Ay[4],Ax_tx[4],Ay_tx[4],Ax_ty[4],Ay_ty[4]; //---------------------------------------------------------------- dense qp_in = T[4]; dense z_in = T[5]; dense h = (z_out - T[5]); dense hC = h * c_light; x0[0] = T[0]; x0[1] = T[1]; x0[2] = T[2]; x0[3] = T[3]; // // Runge-Kutta step // for (int step = 0; step < 4; ++step) { for(int i=0; i < 4; ++i) { if(step == 0) { x[i] = x0[i]; } else { x[i] = x0[i] + b[step] * k[step*4-4+i]; } } dense B[3]; F.get( z_in + a[step] * h, B ); dense tx = x[2]; dense ty = x[3]; dense tx2 = tx * tx; dense ty2 = ty * ty; dense txty = tx * ty; dense tx2ty21= 1.0 + tx2 + ty2; // if( tx2ty21 > 1.e4 ) return 1; dense I_tx2ty21 = 1.0 / tx2ty21 * qp0; dense tx2ty2 = sqrt(tx2ty21 ) ; // dense I_tx2ty2 = qp0 * hC / tx2ty2 ; unsused ??? tx2ty2 *= hC; dense tx2ty2qp = tx2ty2 * qp0; Ax[step] = ( txty*B[0] + ty*B[2] - ( 1.0 + tx2 )*B[1] ) * tx2ty2; Ay[step] = (-txty*B[1] - tx*B[2] + ( 1.0 + ty2 )*B[0] ) * tx2ty2; Ax_tx[step] = Ax[step]*tx*I_tx2ty21 + ( ty*B[0]-2.0*tx*B[1])*tx2ty2qp; Ax_ty[step] = Ax[step]*ty*I_tx2ty21 + ( tx*B[0]+ B[2])*tx2ty2qp; Ay_tx[step] = Ay[step]*tx*I_tx2ty21 + (-ty*B[1]- B[2])*tx2ty2qp; Ay_ty[step] = Ay[step]*ty*I_tx2ty21 + (-tx*B[1]+2.0*ty*B[0])*tx2ty2qp; const int 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[0] = select(initialised, (x0[0]+c[0]*k[0]+c[1]*k[4+0]+c[2]*k[8+0]+c[3]*k[12+0]), T[0]); T[1] = select(initialised, (x0[1]+c[0]*k[1]+c[1]*k[4+1]+c[2]*k[8+1]+c[3]*k[12+1]), T[1]); T[2] = select(initialised, (x0[2]+c[0]*k[2]+c[1]*k[4+2]+c[2]*k[8+2]+c[3]*k[12+2]), T[2]); T[3] = select(initialised, (x0[3]+c[0]*k[3]+c[1]*k[4+3]+c[2]*k[8+3]+c[3]*k[12+3]), T[3]); T[5] = select(initialised, z_out, T[5]); } const dense Zero = fill( 0., x0[0].length() ); const dense One = fill( 1., x0[0].length() ); // // Derivatives dx/dqp // x0[0] = x0[1] = x0[2] = x0[3] = Zero; // // Runge-Kutta step for derivatives dx/dqp for (int step = 0; step < 4; ++step) { for(int i=0; i < 4; ++i) { if(step == 0) { x[i] = x0[i]; } else { x[i] = x0[i] + b[step] * k1[step*4-4+i]; } } int 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 dense J[25]; for (int 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] = One; // // end of derivatives dx/dqp // // Derivatives dx/tx // x0[0] = x0[1] = Zero; x0[2] = One; x0[3] = Zero; // // Runge-Kutta step for derivatives dx/dtx // for (int step = 0; step < 4; ++step) { for(int 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]; } } const int 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 (int 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] = One; J[14] = Zero; // Derivatives dx/ty // x0[0] = x0[1] = x0[2] = Zero; x0[3] = One; // // Runge-Kutta step for derivatives dx/dty // for (int step = 0; step < 4; ++step) { for(int 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]; } } const int 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 (int 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] = One; J[19] = Zero; // // derivatives dx/dx and dx/dy for(int i = 0; i < 10; ++i){ J[i] = Zero;} J[0] = J[6] = One; dense dqp = qp_in - qp0; { // update parameters T[0] += select(initialised, (J[5*4+0]*dqp), 0); T[1] += select(initialised, (J[5*4+1]*dqp), 0); T[2] += select(initialised, (J[5*4+2]*dqp), 0); T[3] += select(initialised, (J[5*4+3]*dqp), 0); } // 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]; } #endif // _EXTRAPOLATION template inline void FitFunctional::ExtrapolateJ // extrapolates track parameters and returns jacobian for extrapolation of CovMatrix ( dense T[6], // input track parameters (x,y,tx,ty,Q/p) and cov.matrix const W& z_out , // extrapolate to this z position dense& qp0 , // use Q/p linearisation at this value const FieldRegionCt &F, Jacobian_t &j, dense w ) const { #if defined(ANALYTIC_EXTRAPOLATION) ExtrapolateJAnalytic( T, z_out, qp0, F, j, w ); #elif defined(RK4_EXTRAPOLATION) ExtrapolateJRK4( T, z_out, qp0, F, j, w ); #endif // _EXTRAPOLATION } /// calculate covMatrix for Multiple Scattering template inline void FitFunctional::GetMSMatrix( const dense tx, const dense ty, const U radThick, const U logRadThick, dense qp0, dense& Q22, dense& Q32, dense& Q33 ) const { U mass2 = 0.1396*0.1396; dense txtx = tx*tx; dense tyty = ty*ty; dense txtx1 = txtx + ONE; dense h = txtx + tyty; dense t = sqrt(txtx1 + tyty); dense h2 = h*h; dense qp0t = qp0*t; dense s0 = (CC1 + CC2 * logRadThick + CC3 * h + h2 * (CC4 + CC5 * h + CC6 * h2) ) * qp0t; dense a = (ONE+mass2*qp0*qp0t)*radThick*s0*s0; Q22 = txtx1*a; Q32 = tx*ty*a; Q33 = (ONE + tyty)*a; } template inline void FitFunctional::AddMaterial( StationsCt &st, usize iStation, dense &qp0, FitResults& results, bool isPipe ) const { dense Q22, Q32, Q33; if (isPipe) GetMSMatrix( results.VecT[2], results.VecT[3], st.RadThick[iStation] + PipeRadThick, log(st.RadThick[iStation] + PipeRadThick), qp0, Q22, Q32, Q33 ); else GetMSMatrix( results.VecT[2], results.VecT[3], st.RadThick[iStation], st.logRadThick[iStation], qp0, Q22, Q32, Q33 ); AddMaterial( results.VecC, Q22, Q32, Q33 ); } template inline void FitFunctional::AddPipeMaterial( dense &qp0, FitResults& results ) const { dense Q22, Q32, Q33; GetMSMatrix( results.VecT[2], results.VecT[3], PipeRadThick, log(PipeRadThick), qp0, Q22, Q32, Q33 ); AddMaterial( results.VecC, Q22, Q32, Q33 ); } template inline void FitFunctional::AddHalfMaterial( StationsCt &st, usize iStation, dense &qp0, FitResults& results ) const { dense Q22, Q32, Q33; GetMSMatrix( results.VecT[2], results.VecT[3], st.RadThick[iStation]*0.5, st.logRadThick[iStation]+log(0.5), qp0, Q22, Q32, Q33 ); AddMaterial( results.VecC, Q22, Q32, Q33 ); } #endif