CbmRoot
L1TrackFitter.cxx
Go to the documentation of this file.
1 /*
2  *====================================================================
3  *
4  * SIMD Kalman filter(KF) track fitter
5  *
6  * Authors: M.Zyzak, I.Kulakov
7  * Primary authors: I.Kisel, S.Gorbunov
8  *
9  * e-mail : I.Kisel@gsi.de, M.Zyzak@gsi.de
10  *
11  *====================================================================
12  */
13 
14 
15 #include "L1AddMaterial.h"
16 #include "L1Algo.h"
17 #include "L1Extrapolation.h"
18 #include "L1Filtration.h" // for KFTrackFitter_simple
19 #include "L1TrackPar.h"
20 #include "L1TrackParFit.h"
21 
22 #include <iostream>
23 #include <vector>
24 
25 using std::cout;
26 using std::endl;
27 using std::vector;
28 
29 
30 namespace NS_L1TrackFitter {
31  const fvec c_light = 0.000299792458, c_light_i = 1. / c_light;
32  const fvec ZERO = 0.;
33  const fvec ONE = 1.;
34  const fvec vINF = 0.1;
35 } // namespace NS_L1TrackFitter
36 using namespace NS_L1TrackFitter;
37 
39 void L1Algo::KFTrackFitter_simple() // TODO: Add pipe.
40 {
41  // cout << " Start KF Track Fitter " << endl;
42  int start_hit = 0; // for interation in vRecoHits[]
43 
44  for (unsigned int itrack = 0; itrack < NTracksIsecAll; itrack++) {
45  L1Track& t = vTracks[itrack]; // current track
46 
47  // get hits of current track
48  std::vector<unsigned short int>
49  hits; // array of indeses of hits of current track
50  hits.clear();
51  int nHits = t.NHits;
52  for (int i = 0; i < nHits; i++) {
53  hits.push_back(vRecoHits[start_hit++]);
54  }
55 
56  L1TrackPar T; // fitting parametr coresponding to current track
57 
58  fvec qp0 = 0.25;
59  //fvec qp0 = 2./t.Momentum;
60  for (int iter = 0; iter < 3; iter++) {
61  //cout<<" Back 1"<<endl;
62  { // fit backward
63  const L1StsHit& hit0 = (*vStsHits)[hits[nHits - 1]];
64  const L1StsHit& hit1 = (*vStsHits)[hits[nHits - 2]];
65  const L1StsHit& hit2 = (*vStsHits)[hits[nHits - 3]];
66 
67  int ista0 = (*vSFlag)[hit0.f] / 4;
68  int ista1 = (*vSFlag)[hit1.f] / 4;
69  int ista2 = (*vSFlag)[hit2.f] / 4;
70 
71  //cout<<"back: ista012="<<ista0<<" "<<ista1<<" "<<ista2<<endl;
72  L1Station& sta0 = vStations[ista0];
73  L1Station& sta1 = vStations[ista1];
74  L1Station& sta2 = vStations[ista2];
75 
76  fvec u0 = static_cast<fscal>((*vStsStrips)[hit0.f]);
77  fvec v0 = static_cast<fscal>((*vStsStripsB)[hit0.b]);
78  fvec x0, y0;
79  StripsToCoor(u0, v0, x0, y0, sta0);
80  fvec z0 = (*vStsZPos)[hit0.iz];
81 
82  fvec u1 = static_cast<fscal>((*vStsStrips)[hit1.f]);
83  fvec v1 = static_cast<fscal>((*vStsStripsB)[hit1.b]);
84  fvec x1, y1;
85  StripsToCoor(u1, v1, x1, y1, sta1);
86  fvec z1 = (*vStsZPos)[hit1.iz];
87 
88  fvec u2 = static_cast<fscal>((*vStsStrips)[hit2.f]);
89  fvec v2 = static_cast<fscal>((*vStsStripsB)[hit2.b]);
90  fvec x2, y2;
91  StripsToCoor(u2, v2, x2, y2, sta2);
92  // fvec z2 = (*vStsZPos)[hit2.iz];
93 
94  fvec dzi = 1. / (z1 - z0);
95 
96  // const fvec vINF = .1;
97  T.x = x0;
98  T.y = y0;
99  if (iter == 0) {
100  T.tx = (x1 - x0) * dzi;
101  T.ty = (y1 - y0) * dzi;
102  }
103 
104  T.qp = qp0;
105  T.z = z0;
106  T.chi2 = 0.;
107  T.NDF = 2.;
108  T.C00 = sta0.XYInfo.C00;
109  T.C10 = sta0.XYInfo.C10;
110  T.C11 = sta0.XYInfo.C11;
111 
112  T.C20 = T.C21 = 0;
113  T.C30 = T.C31 = T.C32 = 0;
114  T.C40 = T.C41 = T.C42 = T.C43 = 0;
115  T.C22 = T.C33 = vINF;
116  T.C44 = 1.;
117 
118  // static L1FieldValue fB0, fB1, fB2 _fvecalignment;
119  // static L1FieldRegion fld _fvecalignment;
120  L1FieldValue fB0, fB1, fB2 _fvecalignment;
121  L1FieldRegion fld _fvecalignment;
122  fvec fz0 = sta1.z; // suppose field is smoth
123  fvec fz1 = sta2.z;
124  fvec fz2 = sta0.z;
125 
126 
127  sta1.fieldSlice.GetFieldValue(x1, y1, fB0);
128  sta2.fieldSlice.GetFieldValue(x2, y2, fB1);
129  sta0.fieldSlice.GetFieldValue(x0, y0, fB2);
130 
131  fld.Set(fB2, fz2, fB1, fz1, fB0, fz0);
132 
133  int ista = ista2;
134  //cout<<"\nfit, iter=:"<<iter<<endl;
135  for (int i = nHits - 2; i >= 0; i--) {
136  // if( fabs(T.qp[0])>2. ) break; // iklm. Don't know it need for
137  const L1StsHit& hit = (*vStsHits)[hits[i]];
138  ista = (*vSFlag)[hit.f] / 4;
139 
140  L1Station& sta = vStations[ista];
141 
142  // L1Extrapolate( T, (*vStsZPos)[hit.iz], qp0, fld );
143  L1ExtrapolateLine(T, (*vStsZPos)[hit.iz]);
144 // T.L1Extrapolate( sta.z, qp0, fld );
145 // L1Extrapolate( T, hit.z, qp0, fld );
146 #ifdef USE_RL_TABLE
147  L1AddMaterial(T, fRadThick[i].GetRadThick(T.x, T.y), qp0, ONE);
148 #else
149  L1AddMaterial(T, sta.materialInfo, qp0, ONE);
150 #endif
151 
152  // if (ista==NMvdStations-1) L1AddPipeMaterial( T, qp0);
153 
154  fvec u = static_cast<fscal>((*vStsStrips)[hit.f]);
155  fvec v = static_cast<fscal>((*vStsStripsB)[hit.b]);
156  L1Filter(T, sta.frontInfo, u);
157  L1Filter(T, sta.backInfo, v);
158  fB0 = fB1;
159  fB1 = fB2;
160  fz0 = fz1;
161  fz1 = fz2;
162  fvec x, y;
163  StripsToCoor(u, v, x, y, sta);
164  sta.fieldSlice.GetFieldValue(x, y, fB2);
165 
166  fz2 = sta.z;
167  fld.Set(fB2, fz2, fB1, fz1, fB0, fz0);
168  } // i
169 
170  // write received parametres in track
171  t.TFirst[0] = T.x[0];
172  t.TFirst[1] = T.y[0];
173  t.TFirst[2] = T.tx[0];
174  t.TFirst[3] = T.ty[0];
175  t.TFirst[4] = T.qp[0];
176  t.TFirst[5] = T.z[0];
177 
178  t.CFirst[0] = T.C00[0];
179  t.CFirst[1] = T.C10[0];
180  t.CFirst[2] = T.C11[0];
181  t.CFirst[3] = T.C20[0];
182  t.CFirst[4] = T.C21[0];
183  t.CFirst[5] = T.C22[0];
184  t.CFirst[6] = T.C30[0];
185  t.CFirst[7] = T.C31[0];
186  t.CFirst[8] = T.C32[0];
187  t.CFirst[9] = T.C33[0];
188  t.CFirst[10] = T.C40[0];
189  t.CFirst[11] = T.C41[0];
190  t.CFirst[12] = T.C42[0];
191  t.CFirst[13] = T.C43[0];
192  t.CFirst[14] = T.C44[0];
193 
194  t.chi2 = T.chi2[0];
195  t.NDF = static_cast<int>(T.NDF[0]);
196  qp0 = T.qp[0];
197  } // fit backward
198 
199  // fit forward
200  {
201  //T.qp = first_trip->GetQpOrig(MaxInvMom);
202 
203  const L1StsHit& hit0 = (*vStsHits)[hits[0]];
204  const L1StsHit& hit1 = (*vStsHits)[hits[1]];
205  const L1StsHit& hit2 = (*vStsHits)[hits[2]];
206 
207  int ista0 = GetFStation((*vSFlag)[hit0.f]);
208  int ista1 = GetFStation((*vSFlag)[hit1.f]);
209  int ista2 = GetFStation((*vSFlag)[hit2.f]);
210 
211  L1Station& sta0 = vStations[ista0];
212  L1Station& sta1 = vStations[ista1];
213  L1Station& sta2 = vStations[ista2];
214 
215  fvec u0 = static_cast<fscal>((*vStsStrips)[hit0.f]);
216  fvec v0 = static_cast<fscal>((*vStsStripsB)[hit0.b]);
217  fvec x0, y0;
218  StripsToCoor(u0, v0, x0, y0, sta0);
219  fvec z0 = (*vStsZPos)[hit0.iz];
220 
221  fvec u1 = static_cast<fscal>((*vStsStrips)[hit1.f]);
222  fvec v1 = static_cast<fscal>((*vStsStripsB)[hit1.b]);
223  fvec x1, y1;
224  StripsToCoor(u1, v1, x1, y1, sta1);
225  // fvec z1 = (*vStsZPos)[hit1.iz];
226 
227  fvec u2 = static_cast<fscal>((*vStsStrips)[hit2.f]);
228  fvec v2 = static_cast<fscal>((*vStsStripsB)[hit2.b]);
229  fvec x2, y2;
230  StripsToCoor(u2, v2, x2, y2, sta2);
231  // fvec z2 = (*vStsZPos)[hit2.iz];
232 
233  // fvec dzi = 1./(z1-z0);
234 
235  //fvec qp0 = first_trip->GetQpOrig(MaxInvMom);
236 
237  // const fvec vINF = .1;
238  T.chi2 = 0.;
239  T.NDF = 2.;
240  T.x = x0;
241  T.y = y0;
242  // T.tx = (x1-x0)*dzi;
243  // T.ty = (y1-y0)*dzi;
244  // qp0 = 0;
245  T.qp = qp0;
246  T.z = z0;
247  T.C00 = sta0.XYInfo.C00;
248  T.C10 = sta0.XYInfo.C10;
249  T.C11 = sta0.XYInfo.C11;
250  T.C20 = T.C21 = 0;
251  T.C30 = T.C31 = T.C32 = 0;
252  T.C40 = T.C41 = T.C42 = T.C43 = 0;
253  T.C22 = T.C33 = vINF;
254  T.C44 = 1.;
255 
256  // static L1FieldValue fB0, fB1, fB2 _fvecalignment;
257  // static L1FieldRegion fld _fvecalignment;
258  L1FieldValue fB0, fB1, fB2 _fvecalignment;
259  L1FieldRegion fld _fvecalignment;
260  fvec fz0 = sta1.z;
261  fvec fz1 = sta2.z;
262  fvec fz2 = sta0.z;
263 
264  sta1.fieldSlice.GetFieldValue(x1, y1, fB0);
265  sta2.fieldSlice.GetFieldValue(x2, y2, fB1);
266  sta0.fieldSlice.GetFieldValue(x0, y0, fB2);
267 
268  fld.Set(fB2, fz2, fB1, fz1, fB0, fz0);
269  int ista = ista2;
270 
271  for (int i = 1; i < nHits; i++) {
272  const L1StsHit& hit = (*vStsHits)[hits[i]];
273  ista = (*vSFlag)[hit.f] / 4;
274  L1Station& sta = vStations[ista];
275  fvec u = static_cast<fscal>((*vStsStrips)[hit.f]);
276  fvec v = static_cast<fscal>((*vStsStripsB)[hit.b]);
277 
278 
279  // L1Extrapolate( T, (*vStsZPos)[hit.iz], qp0, fld );
280  L1ExtrapolateLine(T, (*vStsZPos)[hit.iz]);
281 // T.L1Extrapolate( sta.z, qp0, fld );
282 // L1Extrapolate( T, hit.z, qp0, fld );
283 #ifdef USE_RL_TABLE
284  L1AddMaterial(T, fRadThick[i].GetRadThick(T.x, T.y), qp0, ONE);
285 #else
286  L1AddMaterial(T, sta.materialInfo, qp0, ONE);
287 #endif
288  // if (ista==NMvdStations) L1AddPipeMaterial( T, qp0);
289  L1Filter(T, sta.frontInfo, u);
290  L1Filter(T, sta.backInfo, v);
291 
292  fB0 = fB1;
293  fB1 = fB2;
294  fz0 = fz1;
295  fz1 = fz2;
296  fvec x, y;
297  StripsToCoor(u, v, x, y, sta);
298  sta.fieldSlice.GetFieldValue(x, y, fB2);
299  fz2 = sta.z;
300  fld.Set(fB2, fz2, fB1, fz1, fB0, fz0);
301  }
302 
303  // write received parametres in track
304  t.TLast[0] = T.x[0];
305  t.TLast[1] = T.y[0];
306  t.TLast[2] = T.tx[0];
307  t.TLast[3] = T.ty[0];
308  t.TLast[4] = T.qp[0];
309  t.TLast[5] = T.z[0];
310 
311  t.CLast[0] = T.C00[0];
312  t.CLast[1] = T.C10[0];
313  t.CLast[2] = T.C11[0];
314  t.CLast[3] = T.C20[0];
315  t.CLast[4] = T.C21[0];
316  t.CLast[5] = T.C22[0];
317  t.CLast[6] = T.C30[0];
318  t.CLast[7] = T.C31[0];
319  t.CLast[8] = T.C32[0];
320  t.CLast[9] = T.C33[0];
321  t.CLast[10] = T.C40[0];
322  t.CLast[11] = T.C41[0];
323  t.CLast[12] = T.C42[0];
324  t.CLast[13] = T.C43[0];
325  t.CLast[14] = T.C44[0];
326 
327  t.chi2 += T.chi2[0];
328  t.NDF += static_cast<int>(T.NDF[0]);
329  }
330  qp0 = T.qp[0];
331  }
332  } // for(int itrack
333 }
334 
335 void L1Algo::L1KFTrackFitter() {
336  // cout << " Start L1 Track Fitter " << endl;
337  int start_hit = 0; // for interation in vRecoHits[]
338 
339  // static L1FieldValue fB0, fB1, fB2 _fvecalignment;
340  // static L1FieldRegion fld _fvecalignment;
341  L1FieldValue fB0, fB1, fB2 _fvecalignment;
342  L1FieldRegion fld _fvecalignment;
343 
344 
345  L1FieldValue fB01, fB11, fB21 _fvecalignment;
346  L1FieldRegion fld1 _fvecalignment;
347 
348  const int nHits = NStations;
349  int iVec = 0, i = 0;
350  int nTracks_SIMD = fvecLen;
351  L1TrackPar T; // fitting parametr coresponding to current track
352 
353  L1TrackParFit T1; // fitting parametr coresponding to current track
354 
355  L1Track* t[fvecLen];
356 
357  L1Station* sta = vStations;
358  L1Station staFirst, staLast;
359  fvec x[MaxNStations], u[MaxNStations], v[MaxNStations], y[MaxNStations],
360  time[MaxNStations], timeEr[MaxNStations], z[MaxNStations];
361  fvec d_x[MaxNStations], d_y[MaxNStations], d_xy[MaxNStations],
362  d_u[MaxNStations], d_v[MaxNStations];
363  fvec x_first, y_first, time_first, x_last, y_last, time_last, time_er_first,
364  time_er_last, d_x_fst, d_y_fst, d_xy_fst, time_er_lst, d_x_lst, d_y_lst,
365  d_xy_lst;
366  fvec Sy[MaxNStations], w[MaxNStations], w_time[MaxNStations];
367  fvec y_temp, x_temp;
368  fvec fz0, fz1, fz2, z_start, z_end;
369  L1FieldValue fB[MaxNStations], fB_temp _fvecalignment;
370 
371  fvec ZSta[MaxNStations];
372  for (int iHit = 0; iHit < nHits; iHit++) {
373  ZSta[iHit] = sta[iHit].z;
374  }
375 
376  unsigned short N_vTracks = NTracksIsecAll;
377  const fvec mass2 = 0.1395679f * 0.1395679f;
378 
379  for (unsigned short itrack = 0; itrack < N_vTracks; itrack += fvecLen) {
380  if (N_vTracks - itrack < static_cast<unsigned short>(fvecLen))
381  nTracks_SIMD = N_vTracks - itrack;
382 
383  for (i = 0; i < nTracks_SIMD; i++)
384  t[i] = &vTracks[itrack + i]; // current track
385 
386  // get hits of current track
387  for (i = 0; i < nHits; i++) {
388  w[i] = ZERO;
389  w_time[i] = ZERO;
390  z[i] = ZSta[i];
391  }
392 
393  for (iVec = 0; iVec < nTracks_SIMD; iVec++) {
394  int nHitsTrack = t[iVec]->NHits;
395  int iSta[MaxNStations];
396  for (i = 0; i < nHitsTrack; i++) {
397  const L1StsHit& hit = (*vStsHits)[vRecoHits[start_hit++]];
398  const int ista = (*vSFlag)[hit.f] / 4;
399  iSta[i] = ista;
400  w[ista][iVec] = 1.;
401  if (ista > NMvdStations) w_time[ista][iVec] = 1.;
402 
403  u[ista][iVec] = (*vStsStrips)[hit.f];
404  v[ista][iVec] = (*vStsStripsB)[hit.b];
405  d_u[ista][iVec] = hit.du;
406  d_v[ista][iVec] = hit.dv;
407  StripsToCoor(u[ista], v[ista], x_temp, y_temp, sta[ista]);
408  x[ista][iVec] = x_temp[iVec];
409  y[ista][iVec] = y_temp[iVec];
410  time[ista][iVec] = hit.t_reco;
411  timeEr[ista][iVec] = hit.t_er;
412  z[ista][iVec] = (*vStsZPos)[hit.iz];
413  sta[ista].fieldSlice.GetFieldValue(x[ista], y[ista], fB_temp);
414  dUdV_to_dX(d_u[ista], d_v[ista], d_x[ista], sta[ista]);
415  dUdV_to_dY(d_u[ista], d_v[ista], d_y[ista], sta[ista]);
416  dUdV_to_dXdY(d_u[ista], d_v[ista], d_xy[ista], sta[ista]);
417  fB[ista].x[iVec] = fB_temp.x[iVec];
418  fB[ista].y[iVec] = fB_temp.y[iVec];
419  fB[ista].z[iVec] = fB_temp.z[iVec];
420  if (i == 0) {
421  z_start[iVec] = z[ista][iVec];
422  x_first[iVec] = x[ista][iVec];
423  y_first[iVec] = y[ista][iVec];
424  time_first[iVec] = time[ista][iVec];
425  time_er_first[iVec] = timeEr[ista][iVec];
426  d_x_fst[iVec] = d_x[ista][iVec];
427  d_y_fst[iVec] = d_y[ista][iVec];
428  d_xy_fst[iVec] = d_xy[ista][iVec];
429  staFirst.XYInfo.C00[iVec] = sta[ista].XYInfo.C00[iVec];
430  staFirst.XYInfo.C10[iVec] = sta[ista].XYInfo.C10[iVec];
431  staFirst.XYInfo.C11[iVec] = sta[ista].XYInfo.C11[iVec];
432  } else if (i == nHitsTrack - 1) {
433  z_end[iVec] = z[ista][iVec];
434  x_last[iVec] = x[ista][iVec];
435  y_last[iVec] = y[ista][iVec];
436  d_x_lst[iVec] = d_x[ista][iVec];
437  d_y_lst[iVec] = d_y[ista][iVec];
438  d_xy_lst[iVec] = d_xy[ista][iVec];
439  time_last[iVec] = time[ista][iVec];
440  time_er_last[iVec] = timeEr[ista][iVec];
441  staLast.XYInfo.C00[iVec] = sta[ista].XYInfo.C00[iVec];
442  staLast.XYInfo.C10[iVec] = sta[ista].XYInfo.C10[iVec];
443  staLast.XYInfo.C11[iVec] = sta[ista].XYInfo.C11[iVec];
444  }
445  }
446 
447  fscal prevZ = z_end[iVec];
448  fscal cursy = 0., curSy = 0.;
449  for (i = nHitsTrack - 1; i >= 0; i--) {
450  const int ista = iSta[i];
451  L1Station& st = vStations[ista];
452  const fscal curZ = z[ista][iVec];
453  fscal dZ = curZ - prevZ;
454  fscal By = st.fieldSlice.cy[0][0];
455  curSy += dZ * cursy + dZ * dZ * By / 2.;
456  cursy += dZ * By;
457  Sy[ista][iVec] = curSy;
458  prevZ = curZ;
459  }
460  }
461 
462 
463  //fit backward
464 
465  GuessVec(T, x, y, z, Sy, w, nHits, &z_end);
466  GuessVec(T1, x, y, z, Sy, w, nHits, &z_end, time, w_time);
467 
468  for (int iter = 0; iter < 2; iter++) { // 1.5 iterations
469 
470  fvec qp0 = T.qp;
471 
472  fvec qp01 = T1.fqp;
473 
474  i = nHits - 1;
475 
476  FilterFirst(T, x_last, y_last, staLast);
477  // FilterFirst( T1, x_last, y_last, time_last, time_er_last, staLast );
478 
479  FilterFirst(T1,
480  x_last,
481  y_last,
482  time_last,
483  time_er_last,
484  staLast,
485  d_x_lst,
486  d_y_lst,
487  d_xy_lst);
488 
489  T1.Filter(time[i], timeEr[i], w_time[i]);
490 
491  // L1AddMaterial( T, sta[i].materialInfo, qp0 );
492 
493  fz1 = z[i];
494 
495  sta[i].fieldSlice.GetFieldValue(T.x, T.y, fB1);
496 
497 
498  fB1.Combine(fB[i], w[i]);
499 
500  fz2 = z[i - 2];
501  fvec dz = fz2 - fz1;
502  sta[i].fieldSlice.GetFieldValue(T.x + T.tx * dz, T.y + T.ty * dz, fB2);
503  fB2.Combine(fB[i - 2], w[i - 2]);
504  fld.Set(fB2, fz2, fB1, fz1, fB0, fz0);
505  for (--i; i >= 0; i--) {
506 
507  fz0 = z[i];
508  dz = (fz1 - fz0);
509  sta[i].fieldSlice.GetFieldValue(T.x - T.tx * dz, T.y - T.ty * dz, fB0);
510  fB0.Combine(fB[i], w[i]);
511  fld.Set(fB0, fz0, fB1, fz1, fB2, fz2);
512 
513  fvec initialised = fvec(z[i] < z_end) & fvec(z_start <= z[i]);
514  fvec w1 = (w[i] & (initialised));
515  fvec w1_time = (w_time[i] & (initialised));
516  fvec wIn = (ONE & (initialised));
517 
518  fld1 = fld;
519 
520 
521  T1.Extrapolate(z[i], qp01, fld1, &w1);
522  // T1.ExtrapolateLine(z[i]);
523 
524  L1Extrapolate(T, z[i], qp0, fld, &w1);
525 
526  if (i == NMvdStations - 1) {
527 
528  L1AddPipeMaterial(T, qp0, wIn);
529  EnergyLossCorrection(T, mass2, PipeRadThick, qp0, fvec(1.f), wIn);
530 
531 
532  T1.L1AddPipeMaterial(qp01, wIn);
533  T1.EnergyLossCorrection(mass2, PipeRadThick, qp01, fvec(1.f), wIn);
534  }
535 #ifdef USE_RL_TABLE
536  L1AddMaterial(T, fRadThick[i].GetRadThick(T.x, T.y), qp0, wIn);
537  EnergyLossCorrection(
538  T, mass2, fRadThick[i].GetRadThick(T.x, T.y), qp0, fvec(1.f), wIn);
539 
540  T1.L1AddMaterial(fRadThick[i].GetRadThick(T1.fx, T1.fy), qp01, wIn);
541  T1.EnergyLossCorrection(
542  mass2, fRadThick[i].GetRadThick(T1.fx, T1.fy), qp01, fvec(1.f), wIn);
543 #else
544  L1AddMaterial(T, sta[i].materialInfo, qp0, wIn);
545  T1.L1AddMaterial(sta[i].materialInfo, qp01, wIn);
546 #endif
547  L1UMeasurementInfo info = sta[i].frontInfo;
548  info.sigma2 = d_u[i] * d_u[i];
549 
550  L1Filter(T, sta[i].frontInfo, u[i], w1);
551  T1.Filter(info, u[i], w1);
552 
553  info = sta[i].backInfo;
554  info.sigma2 = d_v[i] * d_v[i];
555 
556  L1Filter(T, sta[i].backInfo, v[i], w1);
557  T1.Filter(info, v[i], w1);
558 
559  T1.Filter(time[i], timeEr[i], w1_time);
560 
561 
562  fB2 = fB1;
563  fz2 = fz1;
564  fB1 = fB0;
565  fz1 = fz0;
566  }
567  // L1AddHalfMaterial( T, sta[i].materialInfo, qp0 );
568 
569  for (iVec = 0; iVec < nTracks_SIMD; iVec++) {
570  t[iVec]->TFirst[0] = T1.fx[iVec];
571  t[iVec]->TFirst[1] = T1.fy[iVec];
572  t[iVec]->TFirst[2] = T1.ftx[iVec];
573  t[iVec]->TFirst[3] = T1.fty[iVec];
574  t[iVec]->TFirst[4] = T1.fqp[iVec];
575  t[iVec]->TFirst[5] = T1.fz[iVec];
576  t[iVec]->TFirst[6] = T1.ft[iVec];
577 
578  t[iVec]->CFirst[0] = T1.C00[iVec];
579  t[iVec]->CFirst[1] = T1.C10[iVec];
580  t[iVec]->CFirst[2] = T1.C11[iVec];
581  t[iVec]->CFirst[3] = T1.C20[iVec];
582  t[iVec]->CFirst[4] = T1.C21[iVec];
583  t[iVec]->CFirst[5] = T1.C22[iVec];
584  t[iVec]->CFirst[6] = T1.C30[iVec];
585  t[iVec]->CFirst[7] = T1.C31[iVec];
586  t[iVec]->CFirst[8] = T1.C32[iVec];
587  t[iVec]->CFirst[9] = T1.C33[iVec];
588  t[iVec]->CFirst[10] = T1.C40[iVec];
589  t[iVec]->CFirst[11] = T1.C41[iVec];
590  t[iVec]->CFirst[12] = T1.C42[iVec];
591  t[iVec]->CFirst[13] = T1.C43[iVec];
592  t[iVec]->CFirst[14] = T1.C44[iVec];
593  t[iVec]->CFirst[15] = T1.C50[iVec];
594  t[iVec]->CFirst[16] = T1.C51[iVec];
595  t[iVec]->CFirst[17] = T1.C52[iVec];
596  t[iVec]->CFirst[18] = T1.C53[iVec];
597  t[iVec]->CFirst[19] = T1.C54[iVec];
598  t[iVec]->CFirst[20] = T1.C55[iVec];
599 
600  t[iVec]->chi2 = T1.chi2[iVec];
601  t[iVec]->NDF = (int) T1.NDF[iVec];
602  }
603 
604  // extrapolate to the PV region
605  L1TrackParFit T1PV = T1;
606  T1PV.Extrapolate(0.f, T1PV.fqp, fld);
607 
608  // T1PV.ExtrapolateLine(0.f);
609 
610  for (iVec = 0; iVec < nTracks_SIMD; iVec++) {
611  t[iVec]->Tpv[0] = T1PV.fx[iVec];
612  t[iVec]->Tpv[1] = T1PV.fy[iVec];
613  t[iVec]->Tpv[2] = T1PV.ftx[iVec];
614  t[iVec]->Tpv[3] = T1PV.fty[iVec];
615  t[iVec]->Tpv[4] = T1PV.fqp[iVec];
616  t[iVec]->Tpv[5] = T1PV.fz[iVec];
617  t[iVec]->Tpv[6] = T1PV.ft[iVec];
618 
619  t[iVec]->Cpv[0] = T1PV.C00[iVec];
620  t[iVec]->Cpv[1] = T1PV.C10[iVec];
621  t[iVec]->Cpv[2] = T1PV.C11[iVec];
622  t[iVec]->Cpv[3] = T1PV.C20[iVec];
623  t[iVec]->Cpv[4] = T1PV.C21[iVec];
624  t[iVec]->Cpv[5] = T1PV.C22[iVec];
625  t[iVec]->Cpv[6] = T1PV.C30[iVec];
626  t[iVec]->Cpv[7] = T1PV.C31[iVec];
627  t[iVec]->Cpv[8] = T1PV.C32[iVec];
628  t[iVec]->Cpv[9] = T1PV.C33[iVec];
629  t[iVec]->Cpv[10] = T1PV.C40[iVec];
630  t[iVec]->Cpv[11] = T1PV.C41[iVec];
631  t[iVec]->Cpv[12] = T1PV.C42[iVec];
632  t[iVec]->Cpv[13] = T1PV.C43[iVec];
633  t[iVec]->Cpv[14] = T1PV.C44[iVec];
634  t[iVec]->Cpv[15] = T1PV.C50[iVec];
635  t[iVec]->Cpv[16] = T1PV.C51[iVec];
636  t[iVec]->Cpv[17] = T1PV.C52[iVec];
637  t[iVec]->Cpv[18] = T1PV.C53[iVec];
638  t[iVec]->Cpv[19] = T1PV.C54[iVec];
639  t[iVec]->Cpv[20] = T1PV.C55[iVec];
640  }
641 
642 
643  if (iter == 1) continue; // only 1.5 iterations
644  // fit forward
645 
646  i = 0;
647 
648  FilterFirst(T, x_first, y_first, staFirst);
649  // FilterFirst( T1, x_first, y_first, time_first, time_er_first, staFirst);
650 
651  FilterFirst(T1,
652  x_first,
653  y_first,
654  time_first,
655  time_er_first,
656  staFirst,
657  d_x_fst,
658  d_y_fst,
659  d_xy_fst);
660 
661  T1.Filter(time[i], timeEr[i], w_time[i]);
662 
663  // L1AddMaterial( T, sta[i].materialInfo, qp0 );
664  qp0 = T.qp;
665  qp01 = T1.fqp;
666 
667  fz1 = z[i];
668  sta[i].fieldSlice.GetFieldValue(T.x, T.y, fB1);
669  fB1.Combine(fB[i], w[i]);
670 
671  fz2 = z[i + 2];
672  dz = fz2 - fz1;
673  sta[i].fieldSlice.GetFieldValue(T.x + T.tx * dz, T.y + T.ty * dz, fB2);
674  fB2.Combine(fB[i + 2], w[i + 2]);
675  fld.Set(fB2, fz2, fB1, fz1, fB0, fz0);
676 
677  for (++i; i < nHits; i++) {
678  fz0 = z[i];
679  dz = (fz1 - fz0);
680  sta[i].fieldSlice.GetFieldValue(T.x - T.tx * dz, T.y - T.ty * dz, fB0);
681  fB0.Combine(fB[i], w[i]);
682  fld.Set(fB0, fz0, fB1, fz1, fB2, fz2);
683 
684  fvec initialised = fvec(z[i] <= z_end) & fvec(z_start < z[i]);
685  fvec w1 = (w[i] & (initialised));
686  fvec wIn = (ONE & (initialised));
687  fvec w1_time = (w_time[i] & (initialised));
688 
689  L1Extrapolate(T, z[i], qp0, fld, &w1);
690 
691  // L1ExtrapolateLine( T, z[i]);
692 
693  T1.Extrapolate(z[i], qp0, fld, &w1);
694 
695  // T1.ExtrapolateLine( z[i]);
696 
697  if (i == NMvdStations) {
698  L1AddPipeMaterial(T, qp0, wIn);
699  EnergyLossCorrection(T, mass2, PipeRadThick, qp0, fvec(-1.f), wIn);
700 
701  T1.L1AddPipeMaterial(qp01, wIn);
702  T1.EnergyLossCorrection(mass2, PipeRadThick, qp01, fvec(-1.f), wIn);
703  }
704 #ifdef USE_RL_TABLE
705  L1AddMaterial(T, fRadThick[i].GetRadThick(T.x, T.y), qp0, wIn);
706  EnergyLossCorrection(
707  T, mass2, fRadThick[i].GetRadThick(T.x, T.y), qp0, fvec(-1.f), wIn);
708 
709  T1.L1AddMaterial(fRadThick[i].GetRadThick(T1.fx, T1.fy), qp01, wIn);
710  T1.EnergyLossCorrection(
711  mass2, fRadThick[i].GetRadThick(T1.fx, T1.fy), qp01, fvec(-1.f), wIn);
712 #else
713  L1AddMaterial(T, sta[i].materialInfo, qp0, wIn);
714 #endif
715  L1Filter(T, sta[i].frontInfo, u[i], w1);
716  L1Filter(T, sta[i].backInfo, v[i], w1);
717 
718  L1UMeasurementInfo info = sta[i].frontInfo;
719  info.sigma2 = d_u[i] * d_u[i];
720 
721  T1.Filter(info, u[i], w1);
722 
723  info = sta[i].backInfo;
724  info.sigma2 = d_v[i] * d_v[i];
725 
726  T1.Filter(info, v[i], w1);
727 
728  T1.Filter(time[i], timeEr[i], w1_time);
729 
730  fB2 = fB1;
731  fz2 = fz1;
732  fB1 = fB0;
733  fz1 = fz0;
734  }
735  // L1AddHalfMaterial( T, sta[i].materialInfo, qp0 );
736 
737  for (iVec = 0; iVec < nTracks_SIMD; iVec++) {
738  t[iVec]->TLast[0] = T1.fx[iVec];
739  t[iVec]->TLast[1] = T1.fy[iVec];
740  t[iVec]->TLast[2] = T1.ftx[iVec];
741  t[iVec]->TLast[3] = T1.fty[iVec];
742  t[iVec]->TLast[4] = T1.fqp[iVec];
743  t[iVec]->TLast[5] = T1.fz[iVec];
744  t[iVec]->TLast[6] = T1.ft[iVec];
745 
746  t[iVec]->CLast[0] = T1.C00[iVec];
747  t[iVec]->CLast[1] = T1.C10[iVec];
748  t[iVec]->CLast[2] = T1.C11[iVec];
749  t[iVec]->CLast[3] = T1.C20[iVec];
750  t[iVec]->CLast[4] = T1.C21[iVec];
751  t[iVec]->CLast[5] = T1.C22[iVec];
752  t[iVec]->CLast[6] = T1.C30[iVec];
753  t[iVec]->CLast[7] = T1.C31[iVec];
754  t[iVec]->CLast[8] = T1.C32[iVec];
755  t[iVec]->CLast[9] = T1.C33[iVec];
756  t[iVec]->CLast[10] = T1.C40[iVec];
757  t[iVec]->CLast[11] = T1.C41[iVec];
758  t[iVec]->CLast[12] = T1.C42[iVec];
759  t[iVec]->CLast[13] = T1.C43[iVec];
760  t[iVec]->CLast[14] = T1.C44[iVec];
761  t[iVec]->CLast[15] = T1.C50[iVec];
762  t[iVec]->CLast[16] = T1.C51[iVec];
763  t[iVec]->CLast[17] = T1.C52[iVec];
764  t[iVec]->CLast[18] = T1.C53[iVec];
765  t[iVec]->CLast[19] = T1.C54[iVec];
766  t[iVec]->CLast[20] = T1.C55[iVec];
767 
768  t[iVec]->chi2 = T1.chi2[iVec];
769  t[iVec]->NDF = (int) T1.NDF[iVec];
770  }
771  } // iter
772  }
773 }
774 
775 void L1Algo::L1KFTrackFitterMuch() {
776  // cout << " Start L1 Track Fitter " << endl;
777  int start_hit = 0; // for interation in vRecoHits[]
778 
779  // static L1FieldValue fB0, fB1, fB2 _fvecalignment;
780  // static L1FieldRegion fld _fvecalignment;
781  L1FieldValue fB0, fB1, fB2 _fvecalignment;
782  L1FieldRegion fld _fvecalignment;
783 
784 
785  L1FieldValue fB01, fB11, fB21 _fvecalignment;
786  L1FieldRegion fld1 _fvecalignment;
787 
788  const int nHits = NStations;
789  int iVec = 0, i = 0;
790  int nTracks_SIMD = fvecLen;
791  L1TrackPar T; // fitting parametr coresponding to current track
792 
793  L1TrackParFit T1; // fitting parametr coresponding to current track
794 
795  L1Track* t[fvecLen];
796 
797  L1Station* sta = vStations;
798  L1Station staFirst, staLast;
799  fvec x[MaxNStations], u[MaxNStations], v[MaxNStations], y[MaxNStations],
800  time[MaxNStations], timeEr[MaxNStations], z[MaxNStations];
801  fvec d_x[MaxNStations], d_y[MaxNStations], d_xy[MaxNStations],
802  d_u[MaxNStations], d_v[MaxNStations];
803  fvec x_first, y_first, time_first, x_last, y_last, time_last, time_er_fst,
804  d_x_fst, d_y_fst, d_xy_fst, time_er_lst, d_x_lst, d_y_lst, d_xy_lst, dz;
805  int iSta[MaxNStations];
806  fvec Sy[MaxNStations], w[MaxNStations];
807  fvec y_temp, x_temp;
808  fvec fz0, fz1, fz2, z_start, z_end;
809  L1FieldValue fB[MaxNStations], fB_temp _fvecalignment;
810 
811  fvec ZSta[MaxNStations];
812  for (int iHit = 0; iHit < nHits; iHit++) {
813  ZSta[iHit] = sta[iHit].z;
814  }
815 
816  unsigned short N_vTracks = NTracksIsecAll;
817  const fvec mass2 = 0.10565f * 0.10565f;
818 
819  for (unsigned short itrack = 0; itrack < N_vTracks; itrack += fvecLen) {
820  if (N_vTracks - itrack < static_cast<unsigned short>(fvecLen))
821  nTracks_SIMD = N_vTracks - itrack;
822 
823  for (i = 0; i < nTracks_SIMD; i++)
824  t[i] = &vTracks[itrack + i]; // current track
825 
826  // get hits of current track
827  for (i = 0; i < nHits; i++) {
828  w[i] = ZERO;
829  z[i] = ZSta[i];
830  }
831 
832  for (iVec = 0; iVec < nTracks_SIMD; iVec++) {
833  for (i = 0; i < NStations; i++) {
834 
835  d_x[i][iVec] = 0;
836  d_y[i][iVec] = 0;
837  }
838  }
839 
840  for (iVec = 0; iVec < nTracks_SIMD; iVec++) {
841  int nHitsTrack = t[iVec]->NHits;
842  int nHitsTrackSts = 0;
843  for (i = 0; i < nHitsTrack; i++) {
844  const L1StsHit& hit = (*vStsHits)[vRecoHits[start_hit++]];
845  const int ista = (*vSFlag)[hit.f] / 4;
846  if (ista < 8) nHitsTrackSts++;
847  iSta[i] = ista;
848  w[ista][iVec] = 1.;
849 
850  d_x[i][iVec] = 0;
851  d_y[i][iVec] = 0;
852 
853  u[ista][iVec] = (*vStsStrips)[hit.f];
854  v[ista][iVec] = (*vStsStripsB)[hit.b];
855  StripsToCoor(u[ista], v[ista], x_temp, y_temp, sta[ista]);
856  x[ista][iVec] = x_temp[iVec];
857  y[ista][iVec] = y_temp[iVec];
858  time[ista][iVec] = hit.t_reco;
859  timeEr[ista][iVec] = hit.t_er;
860  d_u[ista][iVec] = hit.du;
861  d_v[ista][iVec] = hit.dv;
862  dUdV_to_dX(d_u[ista], d_v[ista], d_x[ista], sta[ista]);
863  dUdV_to_dY(d_u[ista], d_v[ista], d_y[ista], sta[ista]);
864  dUdV_to_dXdY(d_u[ista], d_v[ista], d_xy[ista], sta[ista]);
865  // mom[ista][iVec] = hit.p;
866  z[ista][iVec] = (*vStsZPos)[hit.iz];
867  sta[ista].fieldSlice.GetFieldValue(x[ista], y[ista], fB_temp);
868  fB[ista].x[iVec] = fB_temp.x[iVec];
869  fB[ista].y[iVec] = fB_temp.y[iVec];
870  fB[ista].z[iVec] = fB_temp.z[iVec];
871  if (i == 0) {
872  z_start[iVec] = z[ista][iVec];
873  x_first[iVec] = x[ista][iVec];
874  y_first[iVec] = y[ista][iVec];
875  time_first[iVec] = time[ista][iVec];
876  time_er_fst[iVec] = timeEr[ista][iVec];
877  d_x_fst[iVec] = d_x[ista][iVec];
878  d_y_fst[iVec] = d_y[ista][iVec];
879  d_xy_fst[iVec] = d_xy[ista][iVec];
880  staFirst.XYInfo.C00[iVec] = sta[ista].XYInfo.C00[iVec];
881  staFirst.XYInfo.C10[iVec] = sta[ista].XYInfo.C10[iVec];
882  staFirst.XYInfo.C11[iVec] = sta[ista].XYInfo.C11[iVec];
883  } else if (i == nHitsTrack - 1) {
884  z_end[iVec] = z[ista][iVec];
885  x_last[iVec] = x[ista][iVec];
886  y_last[iVec] = y[ista][iVec];
887  time_last[iVec] = time[ista][iVec];
888  time_er_lst[iVec] = timeEr[ista][iVec];
889  d_x_lst[iVec] = d_x[ista][iVec];
890  d_y_lst[iVec] = d_y[ista][iVec];
891  d_xy_lst[iVec] = d_xy[ista][iVec];
892  staLast.XYInfo.C00[iVec] = sta[ista].XYInfo.C00[iVec];
893  staLast.XYInfo.C10[iVec] = sta[ista].XYInfo.C10[iVec];
894  staLast.XYInfo.C11[iVec] = sta[ista].XYInfo.C11[iVec];
895  }
896  }
897 
898  // fscal prevZ = z_end[iVec];
899  fscal prevZ = z_start[iVec];
900  fscal cursy = 0., curSy = 0.;
901  // for(i = nHitsTrack - 1; i >= 0; i-- ){
902  for (i = 0; i < nHitsTrackSts; i++) {
903  const int ista = iSta[i];
904  L1Station& st = vStations[ista];
905  const fscal curZ = z[ista][iVec];
906  fscal dZ = curZ - prevZ;
907  fscal By = st.fieldSlice.cy[0][0];
908  curSy += dZ * cursy + dZ * dZ * By / 2.;
909  cursy += dZ * By;
910  Sy[ista][iVec] = curSy;
911  prevZ = curZ;
912  }
913  }
914 
915  //fit backward
916 
917  int nHitsSts = 0;
918 
919  for (i = 0; i < nHits; i++)
920  if (iSta[i] < 8) nHitsSts++;
921 
922  GuessVec(T, x, y, z, Sy, w, nHitsSts, &z_start);
923 
924 
925  GuessVec(T1, x, y, z, Sy, w, nHitsSts, &z_start);
926 
927 
928  for (int iter = 0; iter < 2; iter++) { // 1.5 iterations
929 
930  fvec qp0 = T.qp;
931 
932  fvec qp01 = T1.fqp;
933 
934  i = 0;
935 
936  FilterFirst(T, x_first, y_first, staFirst);
937 
938  FilterFirst(T1,
939  x_first,
940  y_first,
941  time_first,
942  time_er_fst,
943  staFirst,
944  d_x_fst,
945  d_y_fst,
946  d_xy_fst);
947 
948  fz1 = z[i];
949 
950  sta[i].fieldSlice.GetFieldValue(T.x, T.y, fB1);
951 
952  fB1.Combine(fB[i], w[i]);
953 
954  fz2 = z[i + 2];
955  dz = fz2 - fz1;
956  sta[i].fieldSlice.GetFieldValue(T.x + T.tx * dz, T.y + T.ty * dz, fB2);
957  fB2.Combine(fB[i + 2], w[i + 2]);
958 
959  fld.Set(fB2, fz2, fB1, fz1, fB0, fz0);
960 
961  for (++i; i < nHits; i++) {
962  fvec initialised = fvec(z[i] <= z_end) & fvec(z_start < z[i]);
963  fvec w1 = (w[i] & (initialised));
964  fvec wIn = (ONE & (initialised));
965 
966  fz0 = z[i];
967  dz = (fz1 - fz0);
968 
969 
970  if (i < 8) {
971 
972  sta[i].fieldSlice.GetFieldValue(
973  T.x - T.tx * dz, T.y - T.ty * dz, fB0);
974  fB0.Combine(fB[i], w[i]);
975  fld.Set(fB0, fz0, fB1, fz1, fB2, fz2);
976 
977  L1Extrapolate(T, z[i], qp0, fld, &w1);
978 
979  T1.Extrapolate(z[i], qp01, fld, &w1);
980 
981  if (i == NMvdStations) {
982  L1AddPipeMaterial(T, qp0, wIn);
983  EnergyLossCorrection(T, mass2, PipeRadThick, qp0, fvec(-1.f), wIn);
984 
985  T1.L1AddPipeMaterial(qp01, wIn);
986  T1.EnergyLossCorrection(mass2, PipeRadThick, qp01, fvec(-1.f), wIn);
987  }
988 
989  fB2 = fB1;
990  fz2 = fz1;
991  fB1 = fB0;
992  fz1 = fz0;
993 #ifdef USE_RL_TABLE
994  T1.EnergyLossCorrection(mass2,
995  fRadThick[i].GetRadThick(T1.fx, T1.fy),
996  qp01,
997  fvec(-1.f),
998  wIn);
999 
1000  T1.L1AddThickMaterial(fRadThick[i].GetRadThick(T1.fx, T1.fy),
1001  qp01,
1002  wIn,
1003  sta[i].materialInfo.thick,
1004  1);
1005 #else
1006  // L1AddMaterial( T, sta[i].materialInfo, qp0, wIn );
1007 #endif
1008 
1009  L1UMeasurementInfo info = sta[i].frontInfo;
1010  info.sigma2 = d_x[i] * d_x[i];
1011  L1Filter(T, info, u[i], w1);
1012  T1.Filter(info, u[i], w1);
1013 
1014  info = sta[i].backInfo;
1015  info.sigma2 = d_y[i] * d_y[i];
1016 
1017  L1Filter(T, info, v[i], w1);
1018  T1.Filter(info, v[i], w1);
1019 
1020 
1021  T1.Filter(time[i], timeEr[i], w1);
1022  }
1023 
1024  if (i >= 8) {
1025 
1026  fvec z_last = z[i];
1027 
1028  // T1.ExtrapolateLine( T1.fz + 10, &w1);
1029  // L1ExtrapolateLine( T, T.z + 10);
1030 
1031  fvec d_z = T1.fz - z_last;
1032 
1033  const fvec st = fvec(10);
1034 
1035  fvec nofSteps = (fabs(d_z) / 10);
1036 
1037  int max_steps = 0;
1038 
1039  for (int j = 0; j < 4; j++) {
1040  nofSteps[j] = int(fabs(d_z[j]) / 10);
1041  if (max_steps < nofSteps[j]) max_steps = nofSteps[j];
1042  }
1043 
1044  const fvec mask = (nofSteps < fvec(1)) & fvec(1);
1045 
1046  fvec nofSteps1 = fvec(0);
1047 
1048  fvec one = fvec(1);
1049 
1050  fvec stepSize = (((mask) *d_z) + ((one - mask) * st)) * w1;
1051 
1052  fvec z_cur = T1.fz;
1053 
1054  fvec w2 = w1;
1055 
1056 
1057  for (int iStep = 0; iStep < max_steps + 1; iStep++) {
1058 
1059  const fvec mask1 = (nofSteps == nofSteps1) & fvec(1);
1060 
1061  z_cur = (one - mask1) * (stepSize + T1.fz) + mask1 * z_last;
1062 
1063  // fvec v_mc = fabs(1/qp01)/sqrt(mass2+fabs(1/qp01)*fabs(1/qp01));
1064  // T1.ExtrapolateLine1( z, &w2, v_mc);
1065 
1066  T1.ExtrapolateLine(z_cur, &w2);
1067 
1068  nofSteps1 = nofSteps1 + (one - mask1);
1069 
1070  w2 = w2 & (one - mask1);
1071 
1072 // T1.ExtrapolateLine( z_last, &w1);
1073 // L1ExtrapolateLine( T, z_last);
1074 #ifdef USE_RL_TABLE
1075  if (i == 11 || i == 14 || i == 17)
1076  T1.EnergyLossCorrectionIron(mass2,
1077  fRadThick[i].GetRadThick(T1.fx, T1.fy)
1078  / (nofSteps + 1),
1079  qp01,
1080  fvec(-1.f),
1081  wIn);
1082  if (i == 8)
1083  T1.EnergyLossCorrectionCarbon(
1084  mass2,
1085  fRadThick[i].GetRadThick(T1.fx, T1.fy) / (nofSteps + 1),
1086  qp01,
1087  fvec(-1.f),
1088  wIn);
1089  if (i == 9 || i == 10 || i == 12 || i == 13 || i == 15 || i == 16
1090  || i == 18 || i == 19)
1091  T1.EnergyLossCorrectionAl(mass2,
1092  fRadThick[i].GetRadThick(T1.fx, T1.fy)
1093  / (nofSteps + 1),
1094  qp01,
1095  fvec(-1.f),
1096  wIn);
1097 
1098  T1.L1AddThickMaterial(
1099  fRadThick[i].GetRadThick(T1.fx, T1.fy) / (nofSteps + fvec(1)),
1100  qp01,
1101  wIn,
1102  sta[i].materialInfo.thick / (nofSteps + fvec(1)),
1103  1);
1104 
1105  wIn = wIn & (one - mask1);
1106 
1107 #else
1108  L1AddMaterial(T, sta[i].materialInfo, qp0, wIn);
1109 #endif
1110  }
1111 
1112  // T1.ExtrapolateLine( z_last, &w1);
1113  //
1114  L1UMeasurementInfo info = sta[i].frontInfo;
1115  info.sigma2 = d_x[i] * d_x[i];
1116  L1Filter(T, info, u[i], w1);
1117  T1.Filter(info, u[i], w1);
1118 
1119  info = sta[i].backInfo;
1120  info.sigma2 = d_y[i] * d_y[i];
1121  L1Filter(T, info, v[i], w1);
1122  T1.Filter(info, v[i], w1);
1123 
1124  T1.Filter(time[i], timeEr[i], w1);
1125  }
1126  }
1127  // L1AddHalfMaterial( T, sta[i].materialInfo, qp0 );
1128 
1129  for (iVec = 0; iVec < nTracks_SIMD; iVec++) {
1130 
1131  t[iVec]->TLast[0] = T1.fx[iVec];
1132  t[iVec]->TLast[1] = T1.fy[iVec];
1133  t[iVec]->TLast[2] = T1.ftx[iVec];
1134  t[iVec]->TLast[3] = T1.fty[iVec];
1135  t[iVec]->TLast[4] = T1.fqp[iVec];
1136  t[iVec]->TLast[5] = T1.fz[iVec];
1137  t[iVec]->TLast[6] = T1.ft[iVec];
1138 
1139  t[iVec]->CLast[0] = T1.C00[iVec];
1140  t[iVec]->CLast[1] = T1.C10[iVec];
1141  t[iVec]->CLast[2] = T1.C11[iVec];
1142  t[iVec]->CLast[3] = T1.C20[iVec];
1143  t[iVec]->CLast[4] = T1.C21[iVec];
1144  t[iVec]->CLast[5] = T1.C22[iVec];
1145  t[iVec]->CLast[6] = T1.C30[iVec];
1146  t[iVec]->CLast[7] = T1.C31[iVec];
1147  t[iVec]->CLast[8] = T1.C32[iVec];
1148  t[iVec]->CLast[9] = T1.C33[iVec];
1149  t[iVec]->CLast[10] = T1.C40[iVec];
1150  t[iVec]->CLast[11] = T1.C41[iVec];
1151  t[iVec]->CLast[12] = T1.C42[iVec];
1152  t[iVec]->CLast[13] = T1.C43[iVec];
1153  t[iVec]->CLast[14] = T1.C44[iVec];
1154  t[iVec]->CLast[15] = T1.C50[iVec];
1155  t[iVec]->CLast[16] = T1.C51[iVec];
1156  t[iVec]->CLast[17] = T1.C52[iVec];
1157  t[iVec]->CLast[18] = T1.C53[iVec];
1158  t[iVec]->CLast[19] = T1.C54[iVec];
1159  t[iVec]->CLast[20] = T1.C55[iVec];
1160 
1161 
1162  t[iVec]->chi2 = T1.chi2[iVec];
1163  t[iVec]->NDF = (int) T1.NDF[iVec];
1164  }
1165 
1166  if (iter == 1) continue; // only 1.5 iterations
1167  // fit backward
1168 
1169  i = nHits - 1; //0;
1170 
1171  FilterFirst(T, x_last, y_last, staLast);
1172 
1173  FilterFirstL(T1,
1174  x_last,
1175  y_last,
1176  time_last,
1177  time_er_lst,
1178  staLast,
1179  d_x_lst,
1180  d_y_lst,
1181  d_xy_lst);
1182 
1183  qp0 = T.qp;
1184  qp01 = T1.fqp;
1185 
1186 
1187  // fvec initialised = fvec(z[i] < z_end) & fvec(z_start <= z[i]);
1188  // fvec wIn = (ONE & (initialised));
1189 
1190  // T1.EnergyLossCorrectionAl( mass2, fRadThick[i].GetRadThick(T1.fx, T1.fy), qp01, fvec(1.f), wIn);
1191  //
1192  // T1.L1AddThickMaterial(fRadThick[i].GetRadThick(T1.fx, T1.fy), qp01, ONE, sta[i].materialInfo.thick, 0);
1193 
1194  for (--i; i >= 0; i--) {
1195 
1196  fvec initialised = fvec(z[i] < z_end) & fvec(z_start <= z[i]);
1197  fvec w1 = (w[i] & (initialised));
1198 
1199  fvec wIn = (ONE & (initialised));
1200 
1201 
1202  if (i >= 7) {
1203 
1204  fvec z_last = z[i];
1205 
1206  // T1.ExtrapolateLine( T1.fz + 10, &w1);
1207  // L1ExtrapolateLine( T, T.z + 10);
1208 
1209 
1210  fvec d_z = T1.fz - z_last;
1211  const fvec st = fvec(10);
1212  fvec nofSteps = (fabs(d_z) / 10);
1213 
1214  int max_steps = 0;
1215 
1216  for (int j = 0; j < 4; j++) {
1217  nofSteps[j] = int(fabs(d_z[j]) / 10); //*w1[i];
1218  if (max_steps < nofSteps[j]) max_steps = nofSteps[j];
1219  }
1220 
1221  const fvec mask = (nofSteps < fvec(1)) & fvec(1);
1222  fvec nofSteps1 = fvec(0);
1223  fvec one = fvec(1);
1224  fvec stepSize = (((mask) *d_z) + ((one - mask) * st)) * wIn;
1225  fvec z_cur = T1.fz;
1226  fvec w2 = wIn;
1227 
1228  for (int iStep = 0; iStep < max_steps + 1; iStep++) {
1229 
1230  const fvec mask1 = (nofSteps == nofSteps1) & fvec(1);
1231 
1232  z_cur = (one - mask1) * (T1.fz - stepSize) + mask1 * z_last;
1233 
1234  // fvec v_mc = fabs(1/qp01)/sqrt(mass2+fabs(1/qp01)*fabs(1/qp01));
1235  // T1.ExtrapolateLine1( z_cur, &w2, v_mc);
1236 
1237  T1.ExtrapolateLine(z_cur, &w2);
1238  nofSteps1 = nofSteps1 + (one - mask1);
1239 
1240 #ifdef USE_RL_TABLE
1241 
1242  if (i == 11 || i == 14 || i == 17)
1243  T1.EnergyLossCorrectionIron(mass2,
1244  fRadThick[i].GetRadThick(T1.fx, T1.fy)
1245  / (nofSteps + 1),
1246  qp01,
1247  fvec(1.f),
1248  w2);
1249  if (i == 8)
1250  T1.EnergyLossCorrectionCarbon(
1251  mass2,
1252  fRadThick[i].GetRadThick(T1.fx, T1.fy) / (nofSteps + 1),
1253  qp01,
1254  fvec(1.f),
1255  w2);
1256  if (i == 9 || i == 10 || i == 12 || i == 13 || i == 15 || i == 16
1257  || i == 18)
1258  T1.EnergyLossCorrectionAl(mass2,
1259  fRadThick[i].GetRadThick(T1.fx, T1.fy)
1260  / (nofSteps + 1),
1261  qp01,
1262  fvec(1.f),
1263  w2);
1264 
1265  T1.L1AddThickMaterial(
1266  fRadThick[i].GetRadThick(T1.fx, T1.fy) / (nofSteps + fvec(1)),
1267  qp01,
1268  w2,
1269  sta[i].materialInfo.thick / (nofSteps + fvec(1)),
1270  0);
1271 
1272  w2 = w2 & (one - mask1);
1273 
1274 #else
1275  L1AddMaterial(T, sta[i].materialInfo, qp0, w2);
1276 #endif
1277  }
1278 
1279  //fvec v_mc = fabs(1/qp01)/sqrt(mass2+fabs(1/qp01)*fabs(1/qp01));
1280  // T1.ExtrapolateLine1( z_last, &wIn, v_mc);
1281 
1282  T1.ExtrapolateLine(z_last, &wIn);
1283  L1ExtrapolateLine(T, z_last);
1284 
1285 
1286  L1UMeasurementInfo info = sta[i].frontInfo;
1287  info.sigma2 = d_x[i] * d_x[i];
1288 
1289  L1Filter(T, info, u[i], w1);
1290  T1.Filter(info, u[i], w1);
1291 
1292  info = sta[i].backInfo;
1293  info.sigma2 = d_y[i] * d_y[i];
1294 
1295  L1Filter(T, info, v[i], w1);
1296  T1.Filter(info, v[i], w1);
1297 
1298  T1.Filter(time[i], timeEr[i], w1);
1299  }
1300 
1301  if (i < 7) {
1302 
1303  if (i == 6) {
1304 
1305  T1.ExtrapolateLine(z[7]);
1306 
1307  int i_sts = i + 1;
1308  fz1 = z[i_sts]; //7
1309  sta[i_sts].fieldSlice.GetFieldValue(T1.fx, T1.fy, fB1);
1310  fB1.Combine(fB[i_sts], w[i_sts]);
1311 
1312  fz2 = z[i_sts - 2]; //5
1313  dz = fz2 - fz1;
1314 
1315  sta[i_sts].fieldSlice.GetFieldValue(
1316  T1.fx + T1.ftx * dz, T1.fy + T1.fty * dz, fB2);
1317  fB2.Combine(fB[i_sts - 2], w[i_sts - 2]);
1318  }
1319 
1320 
1321  fz0 = z[i];
1322  dz = (fz1 - fz0);
1323  sta[i].fieldSlice.GetFieldValue(
1324  T1.fx - T1.ftx * dz, T1.fy - T1.fty * dz, fB0);
1325  fB0.Combine(fB[i], w[i]);
1326 
1327  fld.Set(fB0, fz0, fB1, fz1, fB2, fz2);
1328 
1329  // fvec v_mc = fabs(1/qp01)/sqrt(mass2+fabs(1/qp01)*fabs(1/qp01));
1330 
1331  T1.Extrapolate(z[i], qp01, fld, &w1);
1332  // L1Extrapolate( T, z[i], qp0, fld,&w1 );
1333 
1334 
1335 #ifdef USE_RL_TABLE
1336  T1.EnergyLossCorrection(mass2,
1337  fRadThick[i].GetRadThick(T1.fx, T1.fy),
1338  qp01,
1339  fvec(1.f),
1340  wIn);
1341  T1.L1AddThickMaterial(fRadThick[i].GetRadThick(T1.fx, T1.fy),
1342  qp01,
1343  wIn,
1344  sta[i].materialInfo.thick,
1345  0);
1346 
1347 #else
1348  L1AddMaterial(T, sta[i].materialInfo, qp0, wIn);
1349 #endif
1350 
1351  L1UMeasurementInfo info = sta[i].frontInfo;
1352  // info.sigma2 = d_u[i] * d_u[i];
1353  T1.Filter(info, u[i], w1);
1354 
1355  info = sta[i].backInfo;
1356  // info.sigma2 = d_v[i] * d_v[i];
1357  T1.Filter(info, v[i], w1);
1358 
1359  T1.Filter(time[i], timeEr[i], w1);
1360 
1361 
1362  fB2 = fB1;
1363  fz2 = fz1;
1364  fB1 = fB0;
1365  fz1 = fz0;
1366  }
1367  }
1368  // L1AddHalfMaterial( T, sta[i].materialInfo, qp0 );
1369 
1370 
1371  for (iVec = 0; iVec < nTracks_SIMD; iVec++) {
1372 
1373  t[iVec]->TFirst[0] = T1.fx[iVec];
1374  t[iVec]->TFirst[1] = T1.fy[iVec];
1375  t[iVec]->TFirst[2] = T1.ftx[iVec];
1376  t[iVec]->TFirst[3] = T1.fty[iVec];
1377  t[iVec]->TFirst[4] = T1.fqp[iVec];
1378  t[iVec]->TFirst[5] = T1.fz[iVec];
1379  t[iVec]->TFirst[6] = T1.ft[iVec];
1380 
1381  t[iVec]->CFirst[0] = T1.C00[iVec];
1382  t[iVec]->CFirst[1] = T1.C10[iVec];
1383  t[iVec]->CFirst[2] = T1.C11[iVec];
1384  t[iVec]->CFirst[3] = T1.C20[iVec];
1385  t[iVec]->CFirst[4] = T1.C21[iVec];
1386  t[iVec]->CFirst[5] = T1.C22[iVec];
1387  t[iVec]->CFirst[6] = T1.C30[iVec];
1388  t[iVec]->CFirst[7] = T1.C31[iVec];
1389  t[iVec]->CFirst[8] = T1.C32[iVec];
1390  t[iVec]->CFirst[9] = T1.C33[iVec];
1391  t[iVec]->CFirst[10] = T1.C40[iVec];
1392  t[iVec]->CFirst[11] = T1.C41[iVec];
1393  t[iVec]->CFirst[12] = T1.C42[iVec];
1394  t[iVec]->CFirst[13] = T1.C43[iVec];
1395  t[iVec]->CFirst[14] = T1.C44[iVec];
1396  t[iVec]->CFirst[15] = T1.C50[iVec];
1397  t[iVec]->CFirst[16] = T1.C51[iVec];
1398  t[iVec]->CFirst[17] = T1.C52[iVec];
1399  t[iVec]->CFirst[18] = T1.C53[iVec];
1400  t[iVec]->CFirst[19] = T1.C54[iVec];
1401  t[iVec]->CFirst[20] = T1.C55[iVec];
1402 
1403  t[iVec]->chi2 = T1.chi2[iVec];
1404  t[iVec]->NDF = (int) T1.NDF[iVec];
1405  }
1406 
1407 
1408  // extrapolate to the PV region
1409  L1TrackParFit T1PV = T1;
1410  T1PV.Extrapolate(0.f, T1PV.fqp, fld);
1411  for (iVec = 0; iVec < nTracks_SIMD; iVec++) {
1412  t[iVec]->Tpv[0] = T1PV.fx[iVec];
1413  t[iVec]->Tpv[1] = T1PV.fy[iVec];
1414  t[iVec]->Tpv[2] = T1PV.ftx[iVec];
1415  t[iVec]->Tpv[3] = T1PV.fty[iVec];
1416  t[iVec]->Tpv[4] = T1PV.fqp[iVec];
1417  t[iVec]->Tpv[5] = T1PV.fz[iVec];
1418  t[iVec]->Tpv[6] = T1PV.ft[iVec];
1419 
1420  t[iVec]->Cpv[0] = T1PV.C00[iVec];
1421  t[iVec]->Cpv[1] = T1PV.C10[iVec];
1422  t[iVec]->Cpv[2] = T1PV.C11[iVec];
1423  t[iVec]->Cpv[3] = T1PV.C20[iVec];
1424  t[iVec]->Cpv[4] = T1PV.C21[iVec];
1425  t[iVec]->Cpv[5] = T1PV.C22[iVec];
1426  t[iVec]->Cpv[6] = T1PV.C30[iVec];
1427  t[iVec]->Cpv[7] = T1PV.C31[iVec];
1428  t[iVec]->Cpv[8] = T1PV.C32[iVec];
1429  t[iVec]->Cpv[9] = T1PV.C33[iVec];
1430  t[iVec]->Cpv[10] = T1PV.C40[iVec];
1431  t[iVec]->Cpv[11] = T1PV.C41[iVec];
1432  t[iVec]->Cpv[12] = T1PV.C42[iVec];
1433  t[iVec]->Cpv[13] = T1PV.C43[iVec];
1434  t[iVec]->Cpv[14] = T1PV.C44[iVec];
1435  t[iVec]->Cpv[15] = T1PV.C50[iVec];
1436  t[iVec]->Cpv[16] = T1PV.C51[iVec];
1437  t[iVec]->Cpv[17] = T1PV.C52[iVec];
1438  t[iVec]->Cpv[18] = T1PV.C53[iVec];
1439  t[iVec]->Cpv[19] = T1PV.C54[iVec];
1440  t[iVec]->Cpv[20] = T1PV.C55[iVec];
1441  }
1442  } // iter
1443  }
1444 }
1445 
1446 
1447 void L1Algo::GuessVec(L1TrackPar& t,
1448  fvec* xV,
1449  fvec* yV,
1450  fvec* zV,
1451  fvec* Sy,
1452  fvec* wV,
1453  int NHits,
1454  fvec* zCur)
1455 // gives nice initial approximation for x,y,tx,ty - almost same as KF fit. qp - is shifted by 4%, resid_ual - ~3.5% (KF fit resid_ual - 1%).
1456 {
1457  fvec A0, A1 = ZERO, A2 = ZERO, A3 = ZERO, A4 = ZERO, A5 = ZERO, a0, a1 = ZERO,
1458  a2 = ZERO, b0, b1 = ZERO, b2 = ZERO;
1459  fvec z0, x, y, z, S, w, wz, wS;
1460 
1461  int i = NHits - 1;
1462  if (zCur)
1463  z0 = *zCur;
1464  else
1465  z0 = zV[i];
1466  w = wV[i];
1467  A0 = w;
1468  a0 = w * xV[i];
1469  b0 = w * yV[i];
1470  for (i = 0; i < NHits; i++) {
1471  x = xV[i];
1472  y = yV[i];
1473  w = wV[i];
1474  z = zV[i] - z0;
1475  S = Sy[i];
1476  wz = w * z;
1477  wS = w * S;
1478  A0 += w;
1479  A1 += wz;
1480  A2 += wz * z;
1481  A3 += wS;
1482  A4 += wS * z;
1483  A5 += wS * S;
1484  a0 += w * x;
1485  a1 += wz * x;
1486  a2 += wS * x;
1487  b0 += w * y;
1488  b1 += wz * y;
1489  b2 += wS * y;
1490  }
1491 
1492  fvec A3A3 = A3 * A3;
1493  fvec A3A4 = A3 * A4;
1494  fvec A1A5 = A1 * A5;
1495  fvec A2A5 = A2 * A5;
1496  fvec A4A4 = A4 * A4;
1497 
1498  fvec det = rcp(-A2 * A3A3 + A1 * (A3A4 + A3A4 - A1A5) + A0 * (A2A5 - A4A4));
1499  fvec Ai0 = (-A4A4 + A2A5);
1500  fvec Ai1 = (A3A4 - A1A5);
1501  fvec Ai2 = (-A3A3 + A0 * A5);
1502  fvec Ai3 = (-A2 * A3 + A1 * A4);
1503  fvec Ai4 = (A1 * A3 - A0 * A4);
1504  fvec Ai5 = (-A1 * A1 + A0 * A2);
1505 
1506  fvec L, L1;
1507  t.x = (Ai0 * a0 + Ai1 * a1 + Ai3 * a2) * det;
1508  t.tx = (Ai1 * a0 + Ai2 * a1 + Ai4 * a2) * det;
1509  fvec txtx1 = 1. + t.tx * t.tx;
1510  L = (Ai3 * a0 + Ai4 * a1 + Ai5 * a2) * det * rcp(txtx1);
1511  L1 = L * t.tx;
1512  A1 = A1 + A3 * L1;
1513  A2 = A2 + (A4 + A4 + A5 * L1) * L1;
1514  b1 += b2 * L1;
1515  det = rcp(-A1 * A1 + A0 * A2);
1516 
1517  t.y = (A2 * b0 - A1 * b1) * det;
1518  t.ty = (-A1 * b0 + A0 * b1) * det;
1519  t.qp = -L * c_light_i * rsqrt(txtx1 + t.ty * t.ty);
1520  t.z = z0;
1521 }
1522 
1523 void L1Algo::GuessVec(L1TrackParFit& t,
1524  fvec* xV,
1525  fvec* yV,
1526  fvec* zV,
1527  fvec* Sy,
1528  fvec* wV,
1529  int NHits,
1530  fvec* zCur,
1531  fvec* timeV,
1532  fvec* w_time)
1533 // gives nice initial approximation for x,y,tx,ty - almost same as KF fit. qp - is shifted by 4%, resid_ual - ~3.5% (KF fit resid_ual - 1%).
1534 {
1535  fvec A0, A1 = ZERO, A2 = ZERO, A3 = ZERO, A4 = ZERO, A5 = ZERO, a0, a1 = ZERO,
1536  a2 = ZERO, b0, b1 = ZERO, b2 = ZERO;
1537  fvec z0, x, y, z, S, w, wz, wS, time;
1538 
1539  time = 0;
1540 
1541  int i = NHits - 1;
1542  if (zCur)
1543  z0 = *zCur;
1544  else
1545  z0 = zV[i];
1546  w = wV[i];
1547  A0 = w;
1548  a0 = w * xV[i];
1549  b0 = w * yV[i];
1550 
1551  fvec nhits = 0;
1552  for (i = 0; i < NHits; i++) {
1553  x = xV[i];
1554  y = yV[i];
1555  w = wV[i];
1556  if (timeV) nhits = nhits + w_time[i];
1557  z = zV[i] - z0;
1558  S = Sy[i];
1559  wz = w * z;
1560  wS = w * S;
1561  A0 += w;
1562  A1 += wz;
1563  A2 += wz * z;
1564  A3 += wS;
1565  A4 += wS * z;
1566  A5 += wS * S;
1567  a0 += w * x;
1568  a1 += wz * x;
1569  a2 += wS * x;
1570  b0 += w * y;
1571  b1 += wz * y;
1572  b2 += wS * y;
1573  if (timeV)
1574  time += w_time[i] * (timeV[i] - sqrt(x * x + y * y + z * z) / 30.f);
1575  }
1576  time = time / nhits;
1577 
1578  fvec A3A3 = A3 * A3;
1579  fvec A3A4 = A3 * A4;
1580  fvec A1A5 = A1 * A5;
1581  fvec A2A5 = A2 * A5;
1582  fvec A4A4 = A4 * A4;
1583 
1584  fvec det = rcp(-A2 * A3A3 + A1 * (A3A4 + A3A4 - A1A5) + A0 * (A2A5 - A4A4));
1585  fvec Ai0 = (-A4A4 + A2A5);
1586  fvec Ai1 = (A3A4 - A1A5);
1587  fvec Ai2 = (-A3A3 + A0 * A5);
1588  fvec Ai3 = (-A2 * A3 + A1 * A4);
1589  fvec Ai4 = (A1 * A3 - A0 * A4);
1590  fvec Ai5 = (-A1 * A1 + A0 * A2);
1591 
1592  fvec L, L1;
1593  t.fx = (Ai0 * a0 + Ai1 * a1 + Ai3 * a2) * det;
1594  t.ftx = (Ai1 * a0 + Ai2 * a1 + Ai4 * a2) * det;
1595  fvec txtx1 = 1. + t.ftx * t.ftx;
1596  L = (Ai3 * a0 + Ai4 * a1 + Ai5 * a2) * det * rcp(txtx1);
1597  L1 = L * t.ftx;
1598  A1 = A1 + A3 * L1;
1599  A2 = A2 + (A4 + A4 + A5 * L1) * L1;
1600  b1 += b2 * L1;
1601  det = rcp(-A1 * A1 + A0 * A2);
1602 
1603  t.fy = (A2 * b0 - A1 * b1) * det;
1604  t.fty = (-A1 * b0 + A0 * b1) * det;
1605  t.fqp = -L * c_light_i * rsqrt(txtx1 + t.fty * t.fty);
1606  if (timeV) t.ft = time & (nhits > 0);
1607 
1608  t.fz = z0;
1609 }
1610 
1611 void L1Algo::FilterFirst(L1TrackPar& track, fvec& x, fvec& y, L1Station& st) {
1612  // initialize covariance matrix
1613  track.C00 = st.XYInfo.C00;
1614  track.C10 = st.XYInfo.C10;
1615  track.C11 = st.XYInfo.C11;
1616  track.C20 = ZERO;
1617  track.C21 = ZERO;
1618  track.C22 = vINF;
1619  track.C30 = ZERO;
1620  track.C31 = ZERO;
1621  track.C32 = ZERO;
1622  track.C33 = vINF;
1623  track.C40 = ZERO;
1624  track.C41 = ZERO;
1625  track.C42 = ZERO;
1626  track.C43 = ZERO;
1627  track.C44 = ONE;
1628 
1629  track.x = x;
1630  track.y = y;
1631  track.NDF = -3.0;
1632  track.chi2 = ZERO;
1633 }
1634 
1635 void L1Algo::FilterFirst(L1TrackParFit& track,
1636  fvec& x,
1637  fvec& y,
1638  fvec& t,
1639  L1Station& st) {
1640  // initialize covariance matrix
1641  track.C00 = st.XYInfo.C00;
1642  track.C10 = st.XYInfo.C10;
1643  track.C11 = st.XYInfo.C11;
1644  track.C20 = ZERO;
1645  track.C21 = ZERO;
1646  track.C22 = vINF;
1647  track.C30 = ZERO;
1648  track.C31 = ZERO;
1649  track.C32 = ZERO;
1650  track.C33 = vINF;
1651  track.C40 = ZERO;
1652  track.C41 = ZERO;
1653  track.C42 = ZERO;
1654  track.C43 = ZERO;
1655  track.C44 = ONE;
1656  track.C50 = ZERO;
1657  track.C51 = ZERO;
1658  track.C52 = ZERO;
1659  track.C53 = ZERO;
1660  track.C54 = ZERO;
1661  track.C55 = 2.6f * 2.6f;
1662 
1663  track.fx = x;
1664  track.fy = y;
1665  track.ft = t;
1666  track.NDF = -3.0;
1667  track.chi2 = ZERO;
1668 }
1669 void L1Algo::FilterFirst(L1TrackParFit& track,
1670  fvec& x,
1671  fvec& y,
1672  fvec& t,
1673  fvec& t_er,
1674  L1Station& st,
1675  fvec& /*d_x*/,
1676  fvec& /*d_y*/,
1677  fvec& /*d_xy*/) {
1678  // initialize covariance matrix
1679  // track.C00= d_x*d_x;
1680  // track.C10= d_xy; track.C11= d_y*d_y;
1681 
1682 
1683  track.C00 = st.XYInfo.C00;
1684  track.C10 = st.XYInfo.C10;
1685  track.C11 = st.XYInfo.C11;
1686  track.C20 = ZERO;
1687  track.C21 = ZERO;
1688  track.C22 = vINF;
1689  track.C30 = ZERO;
1690  track.C31 = ZERO;
1691  track.C32 = ZERO;
1692  track.C33 = vINF;
1693  track.C40 = ZERO;
1694  track.C41 = ZERO;
1695  track.C42 = ZERO;
1696  track.C43 = ZERO;
1697  track.C44 = ONE;
1698  track.C50 = ZERO;
1699  track.C51 = ZERO;
1700  track.C52 = ZERO;
1701  track.C53 = ZERO;
1702  track.C54 = ZERO;
1703  track.C55 = t_er * t_er;
1704 
1705  track.fx = x;
1706  track.fy = y;
1707  track.ft = t;
1708  track.NDF = -3.0;
1709  track.chi2 = ZERO;
1710 }
1711 
1712 void L1Algo::FilterFirst(L1TrackParFit& track,
1713  fvec& x,
1714  fvec& y,
1715  fvec& /*t*/,
1716  fvec& t_er,
1717  L1Station& st) {
1718  // initialize covariance matrix
1719  // track.C00= d_x*d_x;
1720  // track.C10= d_xy; track.C11= d_y*d_y;
1721 
1722  track.C00 = st.XYInfo.C00;
1723  track.C10 = st.XYInfo.C10;
1724  track.C11 = st.XYInfo.C11;
1725  track.C20 = ZERO;
1726  track.C21 = ZERO;
1727  track.C22 = vINF;
1728  track.C30 = ZERO;
1729  track.C31 = ZERO;
1730  track.C32 = ZERO;
1731  track.C33 = vINF;
1732  track.C40 = ZERO;
1733  track.C41 = ZERO;
1734  track.C42 = ZERO;
1735  track.C43 = ZERO;
1736  track.C44 = ONE;
1737  track.C50 = ZERO;
1738  track.C51 = ZERO;
1739  track.C52 = ZERO;
1740  track.C53 = ZERO;
1741  track.C54 = ZERO;
1742  track.C55 = t_er * t_er;
1743 
1744  track.fx = x;
1745  track.fy = y;
1746  track.NDF = -3.0;
1747  track.chi2 = ZERO;
1748 }
1749 
1750 
1751 void L1Algo::FilterFirstL(L1TrackParFit& track,
1752  fvec& x,
1753  fvec& y,
1754  fvec& /*t*/,
1755  fvec& t_er,
1756  L1Station& /*st*/,
1757  fvec& d_x,
1758  fvec& d_y,
1759  fvec& d_xy) {
1760  // initialize covariance matrix
1761  track.C00 = d_x * d_x;
1762  track.C10 = d_xy;
1763  track.C11 = d_y * d_y;
1764  // track.C00= st.XYInfo.C00;
1765  // track.C10= st.XYInfo.C10; track.C11= st.XYInfo.C11;
1766  track.C20 = ZERO;
1767  track.C21 = ZERO;
1768  track.C22 = vINF;
1769  track.C30 = ZERO;
1770  track.C31 = ZERO;
1771  track.C32 = ZERO;
1772  track.C33 = vINF;
1773  track.C40 = ZERO;
1774  track.C41 = ZERO;
1775  track.C42 = ZERO;
1776  track.C43 = ZERO;
1777  track.C44 = ONE;
1778  track.C50 = ZERO;
1779  track.C51 = ZERO;
1780  track.C52 = ZERO;
1781  track.C53 = ZERO;
1782  track.C54 = ZERO;
1783  track.C55 = t_er * t_er;
1784 
1785  track.fx = x;
1786  track.fy = y;
1787  // track.ft = t;
1788  track.NDF = -3.0;
1789  track.chi2 = ZERO;
1790 }
L1ExtrapolateLine
void L1ExtrapolateLine(L1TrackPar &T, fvec z_out)
Definition: L1Extrapolation.h:814
L1TrackPar::C10
fvec C10
Definition: L1TrackPar.h:9
fscal
float fscal
Definition: L1/vectors/P4_F32vec4.h:250
L1TrackPar::qp
fvec qp
Definition: L1TrackPar.h:9
L1TrackParFit::Extrapolate
void Extrapolate(fvec z_out, fvec qp0, const L1FieldRegion &F, fvec *w=0)
Definition: L1TrackParFit.cxx:326
L1Algo::GuessVec
void GuessVec(L1TrackPar &t, fvec *xV, fvec *yV, fvec *zV, fvec *Sy, fvec *wV, int NHits, fvec *zCur=0)
---— Subroutines used by L1Algo::KFTrackFitter() ---—
Definition: L1TrackFitter.cxx:1447
L1TrackParFit::L1AddPipeMaterial
void L1AddPipeMaterial(fvec qp0, fvec w=1, fvec mass2=0.1395679f *0.1395679f)
Definition: L1TrackParFit.cxx:764
L1TrackParFit::C31
fvec C31
Definition: L1TrackParFit.h:15
f
float f
Definition: L1/vectors/P4_F32vec4.h:24
L1Algo.h
L1TrackPar::C20
fvec C20
Definition: L1TrackPar.h:9
L1TrackParFit::C43
fvec C43
Definition: L1TrackParFit.h:16
L1TrackParFit::C41
fvec C41
Definition: L1TrackParFit.h:16
_fvecalignment
#define _fvecalignment
Definition: L1/vectors/P4_F32vec4.h:254
F32vec4
Definition: L1/vectors/P4_F32vec4.h:47
L1TrackParFit::ft
fvec ft
Definition: L1TrackParFit.h:15
L1Station::materialInfo
L1MaterialInfo materialInfo
Definition: L1Station.h:29
L1XYMeasurementInfo::C11
fvec C11
Definition: L1XYMeasurementInfo.h:11
L1XYMeasurementInfo::C10
fvec C10
Definition: L1XYMeasurementInfo.h:11
L1TrackParFit::C50
fvec C50
Definition: L1TrackParFit.h:16
L1TrackPar::C41
fvec C41
Definition: L1TrackPar.h:10
L1Station
Definition: L1Station.h:9
L1Station::fieldSlice
L1FieldSlice fieldSlice
Definition: L1Station.h:30
sqrt
friend F32vec4 sqrt(const F32vec4 &a)
Definition: L1/vectors/P4_F32vec4.h:41
L1TrackPar::C30
fvec C30
Definition: L1TrackPar.h:9
L1TrackParFit::chi2
fvec chi2
Definition: L1TrackParFit.h:16
L1TrackPar::C31
fvec C31
Definition: L1TrackPar.h:9
fvec
F32vec4 fvec
Definition: L1/vectors/P4_F32vec4.h:249
L1FieldValue::z
fvec z
Definition: L1Field.h:17
L1Extrapolate
void L1Extrapolate(L1TrackPar &T, fvec z_out, fvec qp0, const L1FieldRegion &F, fvec *w=0)
Definition: L1Extrapolation.h:314
L1TrackParFit::fy
fvec fy
Definition: L1TrackParFit.h:15
rcp
friend F32vec4 rcp(const F32vec4 &a)
Definition: L1/vectors/P4_F32vec4.h:52
i
int i
Definition: L1/vectors/P4_F32vec4.h:25
L1TrackParFit::L1AddThickMaterial
void L1AddThickMaterial(fvec radThick, fvec qp0, fvec w=1, fvec mass2=0.1395679f *0.1395679f, fvec thickness=0, bool fDownstream=1)
Definition: L1TrackParFit.cxx:822
L1TrackPar::C42
fvec C42
Definition: L1TrackPar.h:10
L1TrackParFit::C30
fvec C30
Definition: L1TrackParFit.h:15
L1TrackParFit::EnergyLossCorrectionAl
void EnergyLossCorrectionAl(const fvec &mass2, const fvec &radThick, fvec &qp0, fvec direction, fvec w=1)
Definition: L1TrackParFit.cxx:1070
L1Algo::KFTrackFitter_simple
void KFTrackFitter_simple()
Track fitting procedures.
Definition: L1TrackFitter.cxx:39
L1Station::backInfo
L1UMeasurementInfo backInfo
Definition: L1Station.h:31
L1TrackPar::z
fvec z
Definition: L1TrackPar.h:9
L1TrackParFit::C10
fvec C10
Definition: L1TrackParFit.h:15
L1UMeasurementInfo::sigma2
fvec sigma2
Definition: L1UMeasurementInfo.h:12
L1TrackParFit::L1AddMaterial
void L1AddMaterial(L1MaterialInfo &info, fvec qp0, fvec w=1, fvec mass2=0.1395679f *0.1395679f)
Definition: L1TrackParFit.cxx:867
L1StsHit
Definition: L1StsHit.h:12
L1TrackParFit::C33
fvec C33
Definition: L1TrackParFit.h:16
NStations
const int NStations
Definition: L1AlgoPulls.h:9
L1StsHit::t_er
float t_er
Definition: L1StsHit.h:18
L1Track::CFirst
fscal CFirst[21]
Definition: L1Track.h:25
L1TrackParFit::C42
fvec C42
Definition: L1TrackParFit.h:16
L1FieldSlice::cy
fvec cy[21]
Definition: L1Field.h:34
L1TrackPar::ty
fvec ty
Definition: L1TrackPar.h:9
L1TrackPar::C32
fvec C32
Definition: L1TrackPar.h:9
L1AddMaterial
void L1AddMaterial(L1TrackPar &T, fvec radThick, fvec qp0, fvec w=1, fvec mass2=0.10565f *0.10565f)
Definition: L1AddMaterial.h:559
L1Track::NDF
short int NDF
Definition: L1Track.h:26
L1Track::Cpv
fscal Cpv[21]
Definition: L1Track.h:25
L1TrackParFit::C54
fvec C54
Definition: L1TrackParFit.h:16
L1StsHit::b
TStripI b
Definition: L1StsHit.h:15
L1TrackPar::y
fvec y
Definition: L1TrackPar.h:9
L1StsHit::du
float du
Definition: L1StsHit.h:16
L1Track::Tpv
fscal Tpv[7]
Definition: L1Track.h:25
L1TrackParFit::C00
fvec C00
Definition: L1TrackParFit.h:15
L1TrackParFit::C40
fvec C40
Definition: L1TrackParFit.h:16
L1FieldValue::y
fvec y
Definition: L1Field.h:17
L1Algo::L1KFTrackFitterMuch
void L1KFTrackFitterMuch()
Definition: L1TrackFitter.cxx:775
L1Algo::FilterFirstL
void FilterFirstL(L1TrackParFit &track, fvec &x, fvec &y, fvec &t, fvec &t_er, L1Station &st, fvec &dx, fvec &dy, fvec &dxy)
Definition: L1TrackFitter.cxx:1751
L1TrackPar::C44
fvec C44
Definition: L1TrackPar.h:10
L1FieldRegion
Definition: L1Field.h:85
fvecLen
const int fvecLen
Definition: L1/vectors/P4_F32vec4.h:251
EnergyLossCorrection
void EnergyLossCorrection(L1TrackPar &T, const fvec &mass2, const fvec &radThick, fvec &qp0, fvec direction, fvec w=1, fvec=0)
Definition: L1AddMaterial.h:235
L1Station::z
fvec z
Definition: L1Station.h:28
L1AddPipeMaterial
void L1AddPipeMaterial(L1TrackPar &T, fvec qp0, fvec w=1, fvec mass2=0.10565f *0.10565f)
Definition: L1AddMaterial.h:737
L1TrackPar::chi2
fvec chi2
Definition: L1TrackPar.h:10
L1TrackParFit::C20
fvec C20
Definition: L1TrackParFit.h:15
L1Filter
void L1Filter(L1TrackPar &T, L1UMeasurementInfo &info, fvec u, fvec w=1.)
Definition: L1Filtration.h:80
L1TrackPar::NDF
fvec NDF
Definition: L1TrackPar.h:10
L1StsHit::dv
float dv
Definition: L1StsHit.h:16
L1TrackParFit.h
L1Track::NHits
unsigned char NHits
Definition: L1Track.h:22
L1TrackParFit::fx
fvec fx
Definition: L1TrackParFit.h:15
L1TrackPar::tx
fvec tx
Definition: L1TrackPar.h:9
MaxNStations
const int MaxNStations
Definition: L1AlgoEfficiencyPerformance.h:24
L1TrackParFit::C52
fvec C52
Definition: L1TrackParFit.h:16
L1TrackPar.h
rsqrt
friend F32vec4 rsqrt(const F32vec4 &a)
Definition: L1/vectors/P4_F32vec4.h:44
L1TrackParFit::Filter
void Filter(L1UMeasurementInfo &info, fvec u, fvec w=1.)
Definition: L1TrackParFit.cxx:6
L1TrackParFit::C51
fvec C51
Definition: L1TrackParFit.h:16
NS_L1TrackFitter::c_light_i
const fvec c_light_i
Definition: L1TrackFitter.cxx:31
L1TrackParFit::C11
fvec C11
Definition: L1TrackParFit.h:15
L1TrackParFit::ftx
fvec ftx
Definition: L1TrackParFit.h:15
L1TrackPar::C21
fvec C21
Definition: L1TrackPar.h:9
L1TrackParFit::ExtrapolateLine
void ExtrapolateLine(fvec z_out, fvec *w=0)
Definition: L1TrackParFit.cxx:204
L1TrackPar::x
fvec x
Definition: L1TrackPar.h:9
L1TrackPar::C00
fvec C00
Definition: L1TrackPar.h:9
L1StsHit::t_reco
float t_reco
Definition: L1StsHit.h:17
L1FieldSlice::GetFieldValue
void GetFieldValue(const fvec &x, const fvec &y, L1FieldValue &B) const
Definition: L1Field.h:41
L1Extrapolation.h
L1UMeasurementInfo
Definition: L1UMeasurementInfo.h:7
L1Track::CLast
fscal CLast[21]
Definition: L1Track.h:25
L1Track::chi2
fscal chi2
Definition: L1Track.h:25
L1TrackParFit::C21
fvec C21
Definition: L1TrackParFit.h:15
L1TrackPar::C33
fvec C33
Definition: L1TrackPar.h:9
L1Track
Definition: L1Track.h:20
L1TrackParFit::NDF
fvec NDF
Definition: L1TrackParFit.h:16
L1TrackParFit::C32
fvec C32
Definition: L1TrackParFit.h:16
v
__m128 v
Definition: L1/vectors/P4_F32vec4.h:1
L1AddMaterial.h
L1TrackParFit::C53
fvec C53
Definition: L1TrackParFit.h:16
L1Track::TLast
fscal TLast[7]
Definition: L1Track.h:25
L1Algo::L1KFTrackFitter
void L1KFTrackFitter()
Definition: L1TrackFitter.cxx:335
x
Double_t x
Definition: CbmMvdSensorDigiToHitTask.cxx:68
L1TrackPar
Definition: L1TrackPar.h:6
fabs
friend F32vec4 fabs(const F32vec4 &a)
Definition: L1/vectors/P4_F32vec4.h:60
L1MaterialInfo::thick
fvec thick
Definition: L1MaterialInfo.h:9
y
Double_t y
Definition: CbmMvdSensorDigiToHitTask.cxx:68
L1TrackPar::C40
fvec C40
Definition: L1TrackPar.h:10
L1Track::TFirst
fscal TFirst[7]
Definition: L1Track.h:25
NS_L1TrackFitter::vINF
const fvec vINF
Definition: L1TrackFitter.cxx:34
hits
static vector< vector< QAHit > > hits
Definition: CbmTofHitFinderTBQA.cxx:114
L1TrackPar::C11
fvec C11
Definition: L1TrackPar.h:9
L1TrackParFit::fqp
fvec fqp
Definition: L1TrackParFit.h:15
L1TrackParFit
Definition: L1TrackParFit.h:12
L1TrackParFit::C55
fvec C55
Definition: L1TrackParFit.h:16
L1TrackParFit::C44
fvec C44
Definition: L1TrackParFit.h:16
z1
Double_t z1[nSects1]
Definition: pipe_v16a_mvdsts100.h:6
L1TrackParFit::EnergyLossCorrectionIron
void EnergyLossCorrectionIron(const fvec &mass2, const fvec &radThick, fvec &qp0, fvec direction, fvec w=1)
Definition: L1TrackParFit.cxx:928
PipeRadThick
const fvec PipeRadThick
Definition: L1AddMaterial.h:11
L1TrackPar::C43
fvec C43
Definition: L1TrackPar.h:10
NS_L1TrackFitter
Definition: L1TrackFitter.cxx:30
L1FieldValue
Definition: L1Field.h:11
L1XYMeasurementInfo::C00
fvec C00
Definition: L1XYMeasurementInfo.h:11
L1StsHit::f
TStripI f
Definition: L1StsHit.h:15
NS_L1TrackFitter::ZERO
const fvec ZERO
Definition: L1TrackFitter.cxx:32
L1StsHit::iz
TZPosI iz
Definition: L1StsHit.h:24
L1FieldValue::Combine
void Combine(L1FieldValue &B, fvec w)
Definition: L1Field.h:19
NS_L1TrackFitter::ONE
const fvec ONE
Definition: L1TrackFitter.cxx:33
L1Filtration.h
L1TrackParFit::fz
fvec fz
Definition: L1TrackParFit.h:15
L1TrackParFit::EnergyLossCorrectionCarbon
void EnergyLossCorrectionCarbon(const fvec &mass2, const fvec &radThick, fvec &qp0, fvec direction, fvec w=1)
Definition: L1TrackParFit.cxx:999
L1Station::frontInfo
L1UMeasurementInfo frontInfo
Definition: L1Station.h:31
L1TrackParFit::EnergyLossCorrection
void EnergyLossCorrection(const fvec &mass2, const fvec &radThick, fvec &qp0, fvec direction, fvec w=1)
Definition: L1TrackParFit.cxx:898
L1FieldValue::x
fvec x
Definition: L1Field.h:15
L1TrackParFit::fty
fvec fty
Definition: L1TrackParFit.h:15
L1TrackPar::C22
fvec C22
Definition: L1TrackPar.h:9
L1Algo::FilterFirst
void FilterFirst(L1TrackPar &track, fvec &x, fvec &y, L1Station &st)
Definition: L1TrackFitter.cxx:1611
NS_L1TrackFitter::c_light
const fvec c_light
Definition: L1TrackFitter.cxx:31
L1Station::XYInfo
L1XYMeasurementInfo XYInfo
Definition: L1Station.h:32
L1TrackParFit::C22
fvec C22
Definition: L1TrackParFit.h:15