/****************************************************************************** * Header file for workload Track Fitting based on Kalman Filter. * Ct classes and prodecure defined here. * * $Header$ * ******************************************************************************/ /// /// @primary authors: S.Gorbunov; I.Kisel /// @ authors: H.Pabst et al. (Intel); M.Zyzak; I.Kulakov /// #ifndef __ARBB_EXAMPLE_TRACK_FITTING_FIT_ARBB_HPP_ #define __ARBB_EXAMPLE_TRACK_FITTING_FIT_ARBB_HPP_ #include using namespace arbb; #ifdef SINGLE_PRECISION typedef f32 FTYPE; #else typedef f64 FTYPE; #endif //SINGLE_PRECISION #define MASS2 (FTYPE)(0.1396*0.1396) #define CC1 (FTYPE)( 0.0136 ) #define CC2 (FTYPE)( 0.0136 * 0.038 ) #define CC3 (FTYPE)( 0.0136 * 0.038 * 0.5 ) #define CC4 (FTYPE)( -0.0136 * 0.038 * 0.5 / 2.0 ) #define CC5 (FTYPE)( 0.0136 * 0.038 * 0.5 / 3.0 ) #define CC6 (FTYPE)( -0.0136 * 0.038 * 0.5 / 4.0 ) #define _CLIGHT ( 0.000299792458f ) #define _CLIGHT_I ( 1.f/0.000299792458f ) template struct FieldRegionCt{ dense x0, x1, x2 ; // Hx(Z) = x0 + x1*(Z-z) + x2*(Z-z)^2 dense y0, y1, y2 ; // Hy(Z) = y0 + y1*(Z-z) + y2*(Z-z)^2 dense z0, z1, z2 ; // Hz(Z) = z0 + z1*(Z-z) + z2*(Z-z)^2 FTYPE z; FieldRegionCt(){}; FieldRegionCt(usize nt) { z = FTYPE( 0 ); x0 = x1 = x2 = y0 = y1 = y2 = z0 = z1 = z2 = fill(f32(0.0), nt); }; void get(const dense z_, dense B[3]) const{ const dense dz = (z_ - z); const dense dz2 = dz*dz; B[0] = x0 + x1*dz + x2*dz2; B[1] = y0 + y1*dz + y2*dz2; B[2] = z0 + z1*dz + z2*dz2; } void set( const dense h0[3], const FTYPE &h0z, const dense h1[3], const FTYPE &h1z, const dense h2[3], const FTYPE &h2z){ z = h0z; FTYPE dz1 = h1z-h0z, dz2 = h2z-h0z; FTYPE det = 1.0f/(dz1*dz2*(dz2-dz1)); FTYPE w21 = -dz2*det; FTYPE w22 = dz1*det; FTYPE w11 = -dz2*w21; FTYPE w12 = -dz1*w22; dense dh1 = h1[0] - h0[0]; dense dh2 = h2[0] - h0[0]; x0 = h0[0]; x1 = dh1*w11 + dh2*w12 ; x2 = dh1*w21 + dh2*w22 ; dh1 = h1[1] - h0[1]; dh2 = h2[1] - h0[1]; y0 = h0[1]; y1 = dh1*w11 + dh2*w12 ; y2 = dh1*w21 + dh2*w22 ; dh1 = h1[2] - h0[2]; dh2 = h2[2] - h0[2]; z0 = h0[2]; z1 = dh1*w11 + dh2*w12 ; z2 = dh1*w21 + dh2*w22 ; } //! this function seems will never been used. void shift( const FTYPE &newZ){ FTYPE dz = newZ-z; dense x2dz = x2*dz; dense y2dz = y2*dz; dense z2dz = z2*dz; z = newZ; x0+= (x1 + x2dz)*dz; x1+= x2dz+x2dz; y0+= (y1 + y2dz)*dz; y1+= y2dz+y2dz; z0+= (z1 + z2dz)*dz; z1+= z2dz+z2dz; } }; template class HitInfoCt{ // strip info public: HitInfoCt(){}; HitInfoCt( const HitInfo &st, int nStations ); dense cos_phi, sin_phi, sigma2, sigma216; }; template class HitXYInfoCt{ public: HitXYInfoCt(){}; HitXYInfoCt( const HitXYInfo &st, int nStations ); dense C00, C10, C11; }; //! define stations (SOA) template class StationsCt{ public: dense z, thick, zhit, RL, RadThick, logRadThick, SyF, SyL; dense mapX, mapY, mapZ; // polinom coeff. HitInfoCt UInfo, VInfo; // front and back HitXYInfoCt XYInfo; public: StationsCt(){}; StationsCt( const Stations &st ); StationsCt( const StationsCt& s) : z(s.z), thick(s.thick), zhit(s.zhit), RL(s.RL), RadThick(s.RadThick), logRadThick(s.logRadThick), SyF(s.SyF), SyL(s.SyL), mapX(s.mapX), mapY(s.mapY), mapZ(s.mapZ), UInfo(s.UInfo), VInfo(s.VInfo), XYInfo(s.XYInfo) { } void/*StationsCt&*/ operator=( const StationsCt& s ){ z=s.z; thick=s.thick; zhit=s.zhit; RL=s.RL; RadThick=s.RadThick; logRadThick=s.logRadThick; SyF=s.SyF; SyL=s.SyL; mapX = s.mapX; mapY = s.mapY; mapZ = s.mapZ; UInfo = s.UInfo; VInfo = s.VInfo; XYInfo = s.XYInfo; //return *this; } void field( const usize &i, const dense &x, const dense &y, dense H[3] ){ dense x2 = x*x; dense y2 = y*y; dense xy = x*y; dense x3 = x2*x; dense y3 = y2*y; dense xy2 = x*y2; dense x2y = x2*y; dense x4 = x3*x; dense y4 = y3*y; dense xy3 = x*y3; dense x2y2 = x2*y2; dense x3y = x3*y; dense x5 = x4*x; dense y5 = y4*y; dense xy4 = x*y4; dense x2y3 = x2*y3; dense x3y2 = x3*y2; dense x4y = x4*y; H[0] = mapX(usize(0), i) + mapX(1, i)*x + mapX(2, i)*y + mapX(3, i)*x2 + mapX(4, i)*xy + mapX(5, i)*y2 + mapX(6, i)*x3 + mapX(7, i)*x2y + mapX(8, i)*xy2 + mapX(9, i)*y3 + mapX(10, i)*x4 + mapX(11, i)*x3y + mapX(12, i)*x2y2 + mapX(13, i)*xy3 + mapX(14, i)*y4 + mapX(15, i)*x5 + mapX(16, i)*x4y + mapX(17, i)*x3y2 + mapX(18, i)*x2y3 + mapX(19, i)*xy4 + mapX(20, i)*y5; H[1] = mapY(usize(0), i) + mapY(1, i)*x + mapY(2, i)*y + mapY(3, i)*x2 + mapY(4, i)*xy + mapY(5, i)*y2 + mapY(6, i)*x3 + mapY(7, i)*x2y + mapY(8, i)*xy2 + mapY(9, i)*y3 + mapY(10, i)*x4 + mapY(11, i)*x3y + mapY(12, i)*x2y2 + mapY(13, i)*xy3 + mapY(14, i)*y4 + mapY(15, i)*x5 + mapY(16, i)*x4y + mapY(17, i)*x3y2 + mapY(18, i)*x2y3 + mapY(19, i)*xy4 + mapY(20, i)*y5; H[2] = mapZ(usize(0), i) + mapZ(1, i)*x + mapZ(2, i)*y + mapZ(3, i)*x2 + mapZ(4, i)*xy + mapZ(5, i)*y2 + mapZ(6, i)*x3 + mapZ(7, i)*x2y + mapZ(8, i)*xy2 + mapZ(9, i)*y3 + mapZ(10, i)*x4 + mapZ(11, i)*x3y + mapZ(12, i)*x2y2 + mapZ(13, i)*xy3 + mapZ(14, i)*y4 + mapZ(15, i)*x5 + mapZ(16, i)*x4y + mapZ(17, i)*x3y2 + mapZ(18, i)*x2y3 + mapZ(19, i)*xy4 + mapZ(20, i)*y5; } }; //! Define tracks (SOA) template class TracksCt { public: dense MC_x, MC_y, MC_z, MC_px, MC_py, MC_pz, MC_q; dense nHits, NDF; dense hitsX, hitsY; dense hitsX2, hitsY2; dense hitsIsta; TracksCt(){}; TracksCt( Tracks& t ); TracksCt( const TracksCt& t) : MC_x(t.MC_x), MC_y(t.MC_y), MC_z(t.MC_z), MC_px(t.MC_px), MC_py(t.MC_py), MC_pz(t.MC_pz), MC_q(t.MC_q), nHits(t.nHits), NDF(t.NDF), hitsX(t.hitsX), hitsY(t.hitsY), hitsX2(t.hitsX2), hitsY2(t.hitsY2), hitsIsta(t.hitsIsta) { } TracksCt& operator=( const TracksCt& t) { MC_x = t.MC_x; MC_y = t.MC_y; MC_z = t.MC_z; MC_px = t.MC_px; MC_py = t.MC_py; MC_pz = t.MC_pz; MC_q = t.MC_q; nHits = t.nHits; NDF = t.NDF; hitsX = t.hitsX; hitsY = t.hitsY; hitsX2 = t.hitsX2; hitsY2 = t.hitsY2; hitsIsta = t.hitsIsta; return *this; } //private: //usize nTracks; }; #if defined(CONVENTIONAL) template struct CovV{ dense C00, C10, C11, C20, C21, C22, C30, C31, C32, C33, C40, C41, C42, C43, C44; void SetAll( dense a ){ C00 = C10 = C11 = C20 = C21 = C22 = C30 = C31 = C32 = C33 = C40 = C41 = C42 = C43 = C44 = a; }; void CopyOut( dense a, ftype *CBuf, i32 nt ){ copy_out(C00, CBuf[0], nt); copy_out(C10, CBuf[1], nt); copy_out(C11, CBuf[2], nt); copy_out(C20, CBuf[3], nt); copy_out(C21, CBuf[4], nt); copy_out(C22, CBuf[5], nt); copy_out(C30, CBuf[6], nt); copy_out(C31, CBuf[7], nt); copy_out(C32, CBuf[8], nt); copy_out(C33, CBuf[9], nt); copy_out(C40, CBuf[10], nt); copy_out(C41, CBuf[11], nt); copy_out(C42, CBuf[12], nt); copy_out(C43, CBuf[13], nt); copy_out(C44, CBuf[14], nt); }; }; #else #if defined(SQUARE_ROOT1) || defined(SQUARE_ROOT2) template struct CovV{ dense S00, S10, S11, S20, S21, S22, S30, S31, S32, S33, S40, S41, S42, S43, S44; void SetAll( dense a ){ S00 = S10 = S11 = S20 = S21 = S22 = S30 = S31 = S32 = S33 = S40 = S41 = S42 = S43 = S44 = a; }; CovV &operator*=( const CovV& a ){ S40 = S40*a.S00 + S41*a.S10 + S42*a.S20 + S43*a.S30 + S44*a.S40; S30 = S30*a.S00 + S31*a.S10 + S32*a.S20 + S33*a.S30; S20 = S20*a.S00 + S21*a.S10 + S22*a.S20; S10 = S10*a.S00 + S11*a.S10; S00 = S00*a.S00; S41 = S41*a.S11 + S42*a.S21 + S43*a.S31 + S44*a.S41; S31 = S31*a.S11 + S32*a.S21 + S33*a.S31; S21 = S21*a.S11 + S22*a.S21; S11 = S11*a.S11; S42 = S42*a.S22 + S43*a.S32 + S44*a.S42; S32 = S32*a.S22 + S33*a.S32; S22 = S22*a.S22; S43 = S43*a.S33 + S44*a.S43; S33 = S33*a.S33; S44 = S44*a.S44; return *this; } }; #elif defined(SQUARE_ROOT1S) template struct CovV{ dense S00, S01, S02, S03, S04, S10, S11, S12, S13, S14, S20, S21, S22, S23, S24, S30, S31, S32, S33, S34, S40, S41, S42, S43, S44; void SetAll( dense a ){ S00 = S01 = S02 = S03 = S04 = S10 = S11 = S12 = S13 = S14 = S20 = S21 = S22 = S23 = S24 = S30 = S31 = S32 = S33 = S34 = S40 = S41 = S42 = S43 = S44 = a; }; }; #elif defined(UD_FILTERING) template struct CovV{ dense U01, U02, U03, U04, U12, U13, U14, U23, U24, U34, D00, D11, D22, D33, D44; void SetAll( dense a ){ U01 = U02 = U03 = U04 = U12 = U13 = U14 = U23 = U24 = U34 = D00 = D11 = D22 = D33 = D44 = a; }; }; #endif template struct CovVConventional{ // used for convertion dense C00, C10, C11, C20, C21, C22, C30, C31, C32, C33, C40, C41, C42, C43, C44; CovVConventional() {}; #if defined(SQUARE_ROOT1) || defined(SQUARE_ROOT2) CovVConventional( const CovV& T ) { // C = S*S' C00 = T.S00*T.S00; C10 = T.S10*T.S00; C11 = T.S10*T.S10 + T.S11*T.S11; C20 = T.S20*T.S00; C21 = T.S20*T.S10 + T.S21*T.S11; C22 = T.S20*T.S20 + T.S21*T.S21 + T.S22*T.S22; C30 = T.S30*T.S00; C31 = T.S30*T.S10 + T.S31*T.S11; C32 = T.S30*T.S20 + T.S31*T.S21 + T.S32*T.S22; C33 = T.S30*T.S30 + T.S31*T.S31 + T.S32*T.S32 + T.S33*T.S33; C40 = T.S40*T.S00; C41 = T.S40*T.S10 + T.S41*T.S11; C42 = T.S40*T.S20 + T.S41*T.S21 + T.S42*T.S22; C43 = T.S40*T.S30 + T.S41*T.S31 + T.S42*T.S32 + T.S43*T.S33; C44 = T.S40*T.S40 + T.S41*T.S41 + T.S42*T.S42 + T.S43*T.S43 + T.S44*T.S44; } operator CovV(){ CovV r; r.S00 = sqrt( C00 ); const dense iS00 = 1.f/r.S00; r.S10 = iS00*C10; r.S11 = sqrt( C11 - r.S10*r.S10 ); const dense iS11 = 1.f/r.S11; r.S20 = iS00*C20; r.S21 = iS11*( C21 - r.S20*r.S10 ); r.S22 = sqrt( C22 - r.S20*r.S20 - r.S21*r.S21 ); const dense iS22 = 1.f/r.S22; r.S30 = iS00*C30; r.S31 = iS11*( C31 - r.S30*r.S10 ); r.S32 = iS22*( C32 - r.S30*r.S20 - r.S31*r.S21 ); r.S33 = sqrt( C33 - r.S30*r.S30 - r.S31*r.S31 - r.S32*r.S32 ); const dense iS33 = 1.f/r.S33; r.S40 = iS00*C40; r.S41 = iS11*( C41 - r.S40*r.S10 ); r.S42 = iS22*( C42 - r.S40*r.S20 - r.S41*r.S21 ); r.S43 = iS33*( C43 - r.S40*r.S20 - r.S41*r.S21 - r.S42*r.S32 ); r.S44 = sqrt( C44 - r.S40*r.S40 - r.S41*r.S41 - r.S42*r.S42 - r.S43*r.S43 ); return r; } #elif defined(SQUARE_ROOT1S) CovVConventional( const CovV& T ) { // C = S*S' #define M(i,j) C##i##j = T.S##i##0*T.S##j##0 + T.S##i##1*T.S##j##1 + T.S##i##2*T.S##j##2 + T.S##i##3*T.S##j##3 + T.S##i##4*T.S##j##4 M(0,0); M(1,0); M(1,1); M(2,0); M(2,1); M(2,2); M(3,0); M(3,1); M(3,2); M(3,3); M(4,0); M(4,1); M(4,2); M(4,3); M(4,4); #undef M } #elif defined(UD_FILTERING) CovVConventional( const CovV& T ) { // DU = D*U' // C = U*D*U'= U*DU C44 = T.D44; C43 = T.D44*T.U34; C42 = T.D44*T.U24; C41 = T.D44*T.U14; C40 = T.D44*T.U04; const dense &DU43 = C43; const dense &DU42 = C42; const dense &DU41 = C41; const dense &DU40 = C40; #define DU_DEF(i,j) const dense DU##i##j = T.D##i##i*T.U##j##i DU_DEF(3,2); DU_DEF(3,1); DU_DEF(3,0); DU_DEF(2,1); DU_DEF(2,0); DU_DEF(1,0); #undef DU_DEF C33 = T.D33 + T.U34*DU43; C32 = DU32 + T.U34*DU42; C31 = DU31 + T.U34*DU41; C30 = DU30 + T.U34*DU40; C22 = T.D22 + T.U23*DU32 + T.U24*DU42; C21 = DU21 + T.U23*DU31 + T.U24*DU41; C20 = DU20 + T.U23*DU30 + T.U24*DU40; C11 = T.D11 + T.U12*DU21 + T.U13*DU31 + T.U14*DU41; C10 = DU10 + T.U12*DU20 + T.U13*DU30 + T.U14*DU40; C00 = T.D00 + T.U01*DU10 + T.U02*DU20 + T.U03*DU30 + T.U04*DU40; } operator CovV(){ CovV r; r.D44 = C44; const dense iD44 = 1.f/r.D44; const dense &DU43 = C43; const dense &DU42 = C42; const dense &DU41 = C41; const dense &DU40 = C40; r.U34 = DU43*iD44; r.U24 = DU42*iD44; r.U14 = DU41*iD44; r.U04 = DU40*iD44; r.D33 = C33 - r.U34*DU43; const dense iD33 = 1.f/r.D33; const dense DU32 = C32 - r.U34*DU42; const dense DU31 = C31 - r.U34*DU41; const dense DU30 = C30 - r.U34*DU40; r.U23 = DU32*iD33; r.U13 = DU31*iD33; r.U03 = DU30*iD33; r.D22 = C22 - r.U23*DU32 - r.U24*DU42; const dense iD22 = 1.f/r.D22; const dense DU21 = C21 - r.U23*DU31 - r.U24*DU41; const dense DU20 = C20 - r.U23*DU30 - r.U24*DU40; r.U12 = DU21*iD22; r.U02 = DU20*iD22; r.D11 = C11 - r.U12*DU21 - r.U13*DU31 - r.U14*DU41; const dense DU10 = C10 - r.U12*DU20 - r.U13*DU30 - r.U14*DU40; r.U01 = DU10/r.D11; r.D00 = C00 - r.U01*DU10 - r.U02*DU20 - r.U03*DU30 - r.U04*DU40; return r; } #endif }; #endif // !defined(CONVENTIONAL) template class FitResults { public: dense VecT[6]; CovV VecC; // FieldRegionCt f; // field for extrapolation to vertex FitResults() { } FitResults( usize vSize )//:f(vSize) { for( int i = 0; i < 6; i ++ ){ VecT[i] = fill(f32(0.0f), vSize); } VecC.SetAll(fill(f32(0.0f), vSize)); } FitResults( const FitResults& f ) { assign(f); } FitResults& operator=( const FitResults& f ) { assign(f); return *this; } usize length() const { return VecT[0].length(); }; private: void assign( const FitResults& r) { for( int i = 0; i < 6; i++ ){ VecT[i] = r.VecT[i]; } VecC = r.VecC; // f = r.f; } }; void fitTracksCt( ftype (*T)[6], ftype (*C)[15] ); #endif //__ARBB_EXAMPLE_TRACK_FITTING_FIT_ARBB_HPP_