BmnRoot 26.10.0-210-gc52d34b0
Software framework of the BM@N experiment
Loading...
Searching...
No Matches
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#include "L1AddMaterial.h"
15#include "L1Algo.h"
16#include "L1Extrapolation.h"
17#include "L1Filtration.h" // for KFTrackFitter_simple
18#include "L1TrackPar.h"
19
20#include <iostream>
21#include <vector>
22
23using std::cout;
24using std::endl;
25using std::vector;
26
27namespace NS_L1TrackFitter
28{
29const fvec c_light = 0.000299792458, c_light_i = 1. / c_light;
30const fvec ZERO = 0.;
31const fvec ONE = 1.;
32const fvec vINF = 0.1;
33} // namespace NS_L1TrackFitter
34using namespace NS_L1TrackFitter;
35
36//----------------------------------------------------------------------------------------------
37
38/// Fit reconstracted track like it fitted during the reconstruction.
39void L1Algo::KFTrackFitter_simple() // TODO: Add pipe.
40{
41 // cout << " Start KF Track Fitter " << endl;
42 cout << " !!! L1Algo::KFTrackFitter_simple !!! " << endl;
43 exit(9); // AZ-220223 - not called
44 int start_hit = 0; // for interation in vRecoHits[]
45
46 for (unsigned int itrack = 0; itrack < vTracks.size(); itrack++) {
47 L1Track& t = vTracks[itrack]; // current track
48
49 // get hits of current track
50 std::vector<unsigned short int> hits; // array of indeses of hits of current track
51 hits.clear();
52 int nHits = t.NHits;
53 for (int i = 0; i < nHits; i++) {
54 hits.push_back(vRecoHits[start_hit++]);
55 }
56
57 L1TrackPar T; // fitting parametr coresponding to current track
58
59 fvec qp0 = 0;
60 // fvec qp0 = 2./t.Momentum;
61 for (int iter = 0; iter < 3; iter++) {
62 // cout<<" Back 1"<<endl;
63 { // fit backward
64 L1StsHit& hit0 = vStsHits[hits[nHits - 1]];
65 L1StsHit& hit1 = vStsHits[hits[nHits - 2]];
66 L1StsHit& hit2 = vStsHits[hits[nHits - 3]];
67
68 int ista0 = vSFlag[hit0.f] / 4;
69 int ista1 = vSFlag[hit1.f] / 4;
70 int ista2 = vSFlag[hit2.f] / 4;
71
72 // cout<<"back: ista012="<<ista0<<" "<<ista1<<" "<<ista2<<endl;
73 L1Station& sta0 = vStations[ista0];
74 L1Station& sta1 = vStations[ista1];
75 L1Station& sta2 = vStations[ista2];
76
77 fvec u0 = static_cast<fscal>(vStsStrips[hit0.f]);
78 fvec v0 = static_cast<fscal>(vStsStripsB[hit0.b]);
79 fvec x0, y0;
80 StripsToCoor(u0, v0, x0, y0, sta0);
81 fvec z0 = vStsZPos[hit0.iz];
82
83 fvec u1 = static_cast<fscal>(vStsStrips[hit1.f]);
84 fvec v1 = static_cast<fscal>(vStsStripsB[hit1.b]);
85 fvec x1, y1;
86 StripsToCoor(u1, v1, x1, y1, sta1);
87 fvec z1 = vStsZPos[hit1.iz];
88
89 fvec u2 = static_cast<fscal>(vStsStrips[hit2.f]);
90 fvec v2 = static_cast<fscal>(vStsStripsB[hit2.b]);
91 fvec x2, y2;
92 StripsToCoor(u2, v2, x2, y2, sta2);
93 // fvec z2 = vStsZPos[hit2.iz];
94
95 fvec dzi = 1. / (z1 - z0);
96
97 const fvec vINF = .1;
98 T.x = x0;
99 T.y = y0;
100 if (iter == 0) {
101 T.tx = (x1 - x0) * dzi;
102 T.ty = (y1 - y0) * dzi;
103 }
104
105 T.qp = qp0;
106 T.z = z0;
107 T.chi2 = 0.;
108 T.NDF = 2.;
109 T.C00 = sta0.XYInfo.C00;
110 T.C10 = sta0.XYInfo.C10;
111 T.C11 = sta0.XYInfo.C11;
112
113 T.C20 = T.C21 = 0;
114 T.C30 = T.C31 = T.C32 = 0;
115 T.C40 = T.C41 = T.C42 = T.C43 = 0;
116 T.C22 = T.C33 = vINF;
117 T.C44 = 1.;
118
119 // static L1FieldValue fB0, fB1, fB2 _fvecalignment;
120 // static L1FieldRegion fld _fvecalignment;
121 L1FieldValue fB0, fB1, fB2 _fvecalignment;
122 L1FieldRegion fld _fvecalignment;
123 fvec fz0 = sta1.z; // suppose field is smoth
124 fvec fz1 = sta2.z;
125 fvec fz2 = sta0.z;
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 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 // T.L1Extrapolate( sta.z, qp0, fld );
144 // L1Extrapolate( T, hit.z, qp0, fld );
145 L1AddMaterial(T, sta.materialInfo, qp0);
146 // if (ista==NMvdStations-1) L1AddPipeMaterial( T, qp0);
147
148 fvec u = static_cast<fscal>(vStsStrips[hit.f]);
149 fvec v = static_cast<fscal>(vStsStripsB[hit.b]);
150 L1Filter(T, sta.frontInfo, u);
151 L1Filter(T, sta.backInfo, v);
152 fB0 = fB1;
153 fB1 = fB2;
154 fz0 = fz1;
155 fz1 = fz2;
156 fvec x, y;
157 StripsToCoor(u, v, x, y, sta);
158 sta.fieldSlice.GetFieldValue(x, y, fB2);
159
160 fz2 = sta.z;
161 fld.Set(fB2, fz2, fB1, fz1, fB0, fz0);
162 } // i
163
164 // write received parametres in track
165 t.TFirst[0] = T.x[0];
166 t.TFirst[1] = T.y[0];
167 t.TFirst[2] = T.tx[0];
168 t.TFirst[3] = T.ty[0];
169 t.TFirst[4] = T.qp[0];
170 t.TFirst[5] = T.z[0];
171
172 t.CFirst[0] = T.C00[0];
173 t.CFirst[1] = T.C10[0];
174 t.CFirst[2] = T.C11[0];
175 t.CFirst[3] = T.C20[0];
176 t.CFirst[4] = T.C21[0];
177 t.CFirst[5] = T.C22[0];
178 t.CFirst[6] = T.C30[0];
179 t.CFirst[7] = T.C31[0];
180 t.CFirst[8] = T.C32[0];
181 t.CFirst[9] = T.C33[0];
182 t.CFirst[10] = T.C40[0];
183 t.CFirst[11] = T.C41[0];
184 t.CFirst[12] = T.C42[0];
185 t.CFirst[13] = T.C43[0];
186 t.CFirst[14] = T.C44[0];
187
188 t.chi2 = T.chi2[0];
189 t.NDF = static_cast<int>(T.NDF[0]);
190 qp0 = T.qp[0];
191 } // fit backward
192
193 // fit forward
194 {
195 // T.qp = first_trip->GetQpOrig(MaxInvMom);
196
197 L1StsHit& hit0 = vStsHits[hits[0]];
198 L1StsHit& hit1 = vStsHits[hits[1]];
199 L1StsHit& hit2 = vStsHits[hits[2]];
200
201 int ista0 = GetFStation(vSFlag[hit0.f]);
202 int ista1 = GetFStation(vSFlag[hit1.f]);
203 int ista2 = GetFStation(vSFlag[hit2.f]);
204
205 L1Station& sta0 = vStations[ista0];
206 L1Station& sta1 = vStations[ista1];
207 L1Station& sta2 = vStations[ista2];
208
209 fvec u0 = static_cast<fscal>(vStsStrips[hit0.f]);
210 fvec v0 = static_cast<fscal>(vStsStripsB[hit0.b]);
211 fvec x0, y0;
212 StripsToCoor(u0, v0, x0, y0, sta0);
213 fvec z0 = vStsZPos[hit0.iz];
214
215 fvec u1 = static_cast<fscal>(vStsStrips[hit1.f]);
216 fvec v1 = static_cast<fscal>(vStsStripsB[hit1.b]);
217 fvec x1, y1;
218 StripsToCoor(u1, v1, x1, y1, sta1);
219 // fvec z1 = vStsZPos[hit1.iz];
220
221 fvec u2 = static_cast<fscal>(vStsStrips[hit2.f]);
222 fvec v2 = static_cast<fscal>(vStsStripsB[hit2.b]);
223 fvec x2, y2;
224 StripsToCoor(u2, v2, x2, y2, sta2);
225 // fvec z2 = vStsZPos[hit2.iz];
226
227 // fvec dzi = 1./(z1-z0);
228
229 // fvec qp0 = first_trip->GetQpOrig(MaxInvMom);
230
231 const fvec vINF = .1;
232 T.chi2 = 0.;
233 T.NDF = 2.;
234 T.x = x0;
235 T.y = y0;
236 // T.tx = (x1-x0)*dzi;
237 // T.ty = (y1-y0)*dzi;
238 // qp0 = 0;
239 T.qp = qp0;
240 T.z = z0;
241 T.C00 = sta0.XYInfo.C00;
242 T.C10 = sta0.XYInfo.C10;
243 T.C11 = sta0.XYInfo.C11;
244 T.C20 = T.C21 = 0;
245 T.C30 = T.C31 = T.C32 = 0;
246 T.C40 = T.C41 = T.C42 = T.C43 = 0;
247 T.C22 = T.C33 = vINF;
248 T.C44 = 1.;
249
250 // static L1FieldValue fB0, fB1, fB2 _fvecalignment;
251 // static L1FieldRegion fld _fvecalignment;
252 L1FieldValue fB0, fB1, fB2 _fvecalignment;
253 L1FieldRegion fld _fvecalignment;
254 fvec fz0 = sta1.z;
255 fvec fz1 = sta2.z;
256 fvec fz2 = sta0.z;
257
258 sta1.fieldSlice.GetFieldValue(x1, y1, fB0);
259 sta2.fieldSlice.GetFieldValue(x2, y2, fB1);
260 sta0.fieldSlice.GetFieldValue(x0, y0, fB2);
261
262 fld.Set(fB2, fz2, fB1, fz1, fB0, fz0);
263 int ista = ista2;
264
265 for (int i = 1; i < nHits; i++) {
266 L1StsHit& hit = vStsHits[hits[i]];
267 ista = vSFlag[hit.f] / 4;
268 L1Station& sta = vStations[ista];
269 fvec u = static_cast<fscal>(vStsStrips[hit.f]);
270 fvec v = static_cast<fscal>(vStsStripsB[hit.b]);
271
272 L1Extrapolate(T, vStsZPos[hit.iz], qp0, fld);
273 // T.L1Extrapolate( sta.z, qp0, fld );
274 // L1Extrapolate( T, hit.z, qp0, fld );
275 L1AddMaterial(T, sta.materialInfo, qp0);
276 // if (ista==NMvdStations) L1AddPipeMaterial( T, qp0);
277 L1Filter(T, sta.frontInfo, u);
278 L1Filter(T, sta.backInfo, v);
279
280 fB0 = fB1;
281 fB1 = fB2;
282 fz0 = fz1;
283 fz1 = fz2;
284 fvec x, y;
285 StripsToCoor(u, v, x, y, sta);
286 sta.fieldSlice.GetFieldValue(x, y, fB2);
287 fz2 = sta.z;
288 fld.Set(fB2, fz2, fB1, fz1, fB0, fz0);
289 }
290
291 // write received parametres in track
292 t.TLast[0] = T.x[0];
293 t.TLast[1] = T.y[0];
294 t.TLast[2] = T.tx[0];
295 t.TLast[3] = T.ty[0];
296 t.TLast[4] = T.qp[0];
297 t.TLast[5] = T.z[0];
298
299 t.CLast[0] = T.C00[0];
300 t.CLast[1] = T.C10[0];
301 t.CLast[2] = T.C11[0];
302 t.CLast[3] = T.C20[0];
303 t.CLast[4] = T.C21[0];
304 t.CLast[5] = T.C22[0];
305 t.CLast[6] = T.C30[0];
306 t.CLast[7] = T.C31[0];
307 t.CLast[8] = T.C32[0];
308 t.CLast[9] = T.C33[0];
309 t.CLast[10] = T.C40[0];
310 t.CLast[11] = T.C41[0];
311 t.CLast[12] = T.C42[0];
312 t.CLast[13] = T.C43[0];
313 t.CLast[14] = T.C44[0];
314
315 t.chi2 += T.chi2[0];
316 t.NDF += static_cast<int>(T.NDF[0]);
317 }
318 qp0 = T.qp[0];
319 }
320 } // for(int itrack
321}
322
323//----------------------------------------------------------------------------------------------
324
325void L1Algo::L1KFTrackFitter(bool extrapolateToTheEndOfSTS)
326{
327 // cout << " Start L1 Track Fitter " << endl;
328 int start_hit = 0; // for interation in vRecoHits[]
329
330 // static L1FieldValue fB0, fB1, fB2 _fvecalignment;
331 // static L1FieldRegion fld _fvecalignment;
332 L1FieldValue fB0, fB1, fB2 _fvecalignment;
333 L1FieldRegion fld _fvecalignment;
334
335 const int nHits = NStations;
336 int iVec = 0, i = 0;
337 int nTracks_SIMD = fvecLen;
338 L1TrackPar T; // fitting parametr coresponding to current track
339
340 L1Track* t[fvecLen];
341
342 L1Station* sta = vStations;
343 fvec x[MaxNStations], u[MaxNStations], v[MaxNStations], y[MaxNStations], z[MaxNStations];
344 fvec du[MaxNStations], dv[MaxNStations]; // AZ
345 fvec x_first, y_first, x_last, y_last;
346 fvec Sy[MaxNStations], w[MaxNStations];
347 fvec y_temp, x_temp;
348 fvec fz0, fz1, fz2, dz, z_start, z_end;
349 L1FieldValue fB[MaxNStations], fB_temp _fvecalignment;
350
351 fvec ZSta[MaxNStations];
352 for (int iHit = 0; iHit < nHits; iHit++) {
353 ZSta[iHit] = sta[iHit].z;
354 }
355
356 unsigned short N_vTracks = vTracks.size();
357
358 for (unsigned short itrack = 0; itrack < N_vTracks; itrack += fvecLen) {
359 if (N_vTracks - itrack < static_cast<unsigned short>(fvecLen))
360 nTracks_SIMD = N_vTracks - itrack;
361
362 for (i = 0; i < nTracks_SIMD; i++)
363 t[i] = &vTracks[itrack + i]; // current track
364
365 // get hits of current track
366 for (i = 0; i < nHits; i++) {
367 w[i] = ZERO;
368 z[i] = ZSta[i];
369 }
370
371 for (iVec = 0; iVec < nTracks_SIMD; iVec++) {
372 int nHitsTrack = t[iVec]->NHits;
373 // cout << "AZ: hits = " << nHitsTrack << endl;
374 int iSta[MaxNStations];
375 for (i = 0; i < nHitsTrack; i++) {
376 L1StsHit& hit = vStsHits[vRecoHits[start_hit++]];
377 const int ista = vSFlag[hit.f] / 4;
378 iSta[i] = ista;
379 w[ista][iVec] = 1.;
380
381#ifdef AZ
382 // AZ-230223
383 int ihp = vRecoHits[start_hit - 1];
384 L1HitPoint& hp = vAllHitPoints[ihp];
385 x_temp = hp.X();
386 y_temp = hp.Y();
387 u[ista][iVec] = hp.U();
388 v[ista][iVec] = hp.V();
389 du[ista][iVec] = hp.DU() * hp.DU();
390 dv[ista][iVec] = hp.DV() * hp.DV();
391#else
392 u[ista][iVec] = vStsStrips[hit.f];
393 v[ista][iVec] = vStsStripsB[hit.b];
394 du[ista][iVec] = vStsStrips[hit.f].errf() * vStsStrips[hit.f].errf(); // AZ
395 dv[ista][iVec] = vStsStripsB[hit.b].errf() * vStsStripsB[hit.b].errf(); // AZ
396 StripsToCoor(u[ista], v[ista], x_temp, y_temp, sta[ista]);
397#endif
398 x[ista][iVec] = x_temp[iVec];
399 y[ista][iVec] = y_temp[iVec];
400 z[ista][iVec] = vStsZPos[hit.iz];
401 sta[ista].fieldSlice.GetFieldValue(x[ista], y[ista], fB_temp);
402 fB[ista].x[iVec] = fB_temp.x[iVec];
403 fB[ista].y[iVec] = fB_temp.y[iVec];
404 fB[ista].z[iVec] = fB_temp.z[iVec];
405 if (i == 0) {
406 z_start[iVec] = z[ista][iVec];
407 x_first[iVec] = x[ista][iVec];
408 y_first[iVec] = y[ista][iVec];
409 } else if (i == nHitsTrack - 1) {
410 z_end[iVec] = z[ista][iVec];
411 x_last[iVec] = x[ista][iVec];
412 y_last[iVec] = y[ista][iVec];
413 }
414 }
415
416 // get field integrals
417#if 0
418 for(i = nHits - 1; i >= 0; i-- ){
419 L1Station &st = vStations[i];
420 Sy[i] = st.Sy;
421 }
422#else
423 fscal prevZ = z_end[iVec];
424 fscal cursy = 0., curSy = 0.;
425 for (i = nHitsTrack - 1; i >= 0; i--) {
426 const int ista = iSta[i];
427 L1Station& st = vStations[ista];
428 const fscal curZ = z[ista][iVec];
429 fscal dZ = curZ - prevZ;
430 fscal By = st.fieldSlice.cy[0][0];
431 curSy += dZ * cursy + dZ * dZ * By / 2.;
432 cursy += dZ * By;
433 Sy[ista][iVec] = curSy;
434 prevZ = curZ;
435 }
436#endif // 0
437 }
438
439 // fit backward
440
441 GuessVec(T, x, y, z, Sy, w, nHits, &z_end);
442
443 for (int iter = 0; iter < 2; iter++) { // 1.5 iterations
444
445 fvec qp0 = T.qp;
446
447 i = nHits - 1;
448
449 FilterFirst(T, x_last, y_last, sta[i]);
450
451 // L1AddMaterial( T, sta[i].materialInfo, qp0 );
452
453 fz1 = z[i];
454 sta[i].fieldSlice.GetFieldValue(T.x, T.y, fB1);
455 fB1.Combine(fB[i], w[i]);
456
457 fz2 = z[i - 2];
458 dz = fz2 - fz1;
459 sta[i].fieldSlice.GetFieldValue(T.x + T.tx * dz, T.y + T.ty * dz, fB2);
460 fB2.Combine(fB[i - 2], w[i - 2]);
461 fld.Set(fB2, fz2, fB1, fz1, fB0, fz0);
462 for (--i; i >= 0; i--) {
463 fz0 = z[i];
464 dz = (fz1 - fz0);
465 sta[i].fieldSlice.GetFieldValue(T.x - T.tx * dz, T.y - T.ty * dz, fB0);
466 fB0.Combine(fB[i], w[i]);
467 fld.Set(fB0, fz0, fB1, fz1, fB2, fz2);
468
469 // fvec zero = ZERO;
470 fvec initialised = fvec(z[i] < z_end) & fvec(z_start <= z[i]);
471 // cout << z_start << " " << z_end << " " << initialised << endl;
472 fvec w1 = (w[i] & (initialised));
473 fvec wIn = (ONE & (initialised));
474
475 L1Extrapolate(T, z[i], qp0, fld, &w1);
476 if (i == NMvdStations - 1)
477 L1AddPipeMaterial(T, qp0, wIn);
478#ifdef USE_RL_TABLE
479 L1AddMaterial(T, fRadThick[i].GetRadThick(T.x, T.y), qp0, wIn);
480#else
481 L1AddMaterial(T, sta[i].materialInfo, qp0, wIn);
482#endif
483 // L1Filter( T, sta[i].frontInfo, u[i], w1 );
484 // L1Filter( T, sta[i].backInfo, v[i], w1 );
485 L1Filter(T, sta[i].frontInfo, u[i], w1, &du[i]); // AZ
486 L1Filter(T, sta[i].backInfo, v[i], w1, &dv[i]); // AZ
487 fB2 = fB1;
488 fz2 = fz1;
489 fB1 = fB0;
490 fz1 = fz0;
491 }
492 // L1AddHalfMaterial( T, sta[i].materialInfo, qp0 );
493
494 for (iVec = 0; iVec < nTracks_SIMD; iVec++) {
495 t[iVec]->TFirst[0] = T.x[iVec];
496 t[iVec]->TFirst[1] = T.y[iVec];
497 t[iVec]->TFirst[2] = T.tx[iVec];
498 t[iVec]->TFirst[3] = T.ty[iVec];
499 t[iVec]->TFirst[4] = T.qp[iVec];
500 t[iVec]->TFirst[5] = T.z[iVec];
501
502 t[iVec]->CFirst[0] = T.C00[iVec];
503 t[iVec]->CFirst[1] = T.C10[iVec];
504 t[iVec]->CFirst[2] = T.C11[iVec];
505 t[iVec]->CFirst[3] = T.C20[iVec];
506 t[iVec]->CFirst[4] = T.C21[iVec];
507 t[iVec]->CFirst[5] = T.C22[iVec];
508 t[iVec]->CFirst[6] = T.C30[iVec];
509 t[iVec]->CFirst[7] = T.C31[iVec];
510 t[iVec]->CFirst[8] = T.C32[iVec];
511 t[iVec]->CFirst[9] = T.C33[iVec];
512 t[iVec]->CFirst[10] = T.C40[iVec];
513 t[iVec]->CFirst[11] = T.C41[iVec];
514 t[iVec]->CFirst[12] = T.C42[iVec];
515 t[iVec]->CFirst[13] = T.C43[iVec];
516 t[iVec]->CFirst[14] = T.C44[iVec];
517
518 t[iVec]->chi2 = T.chi2[iVec];
519 t[iVec]->NDF = (int)T.NDF[iVec];
520 }
521
522 if (iter == 1)
523 continue; // only 1.5 iterations
524 // fit forward
525
526 i = 0;
527 FilterFirst(T, x_first, y_first, sta[i]); // TODO different station. won't work with MVD
528 // L1AddMaterial( T, sta[i].materialInfo, qp0 );
529 qp0 = T.qp;
530
531 fz1 = z[i];
532 sta[i].fieldSlice.GetFieldValue(T.x, T.y, fB1);
533 fB1.Combine(fB[i], w[i]);
534
535 fz2 = z[i + 2];
536 dz = fz2 - fz1;
537 sta[i].fieldSlice.GetFieldValue(T.x + T.tx * dz, T.y + T.ty * dz, fB2);
538 fB2.Combine(fB[i + 2], w[i + 2]);
539 fld.Set(fB2, fz2, fB1, fz1, fB0, fz0);
540
541 for (++i; i < nHits; i++) {
542 fz0 = z[i];
543 dz = (fz1 - fz0);
544 sta[i].fieldSlice.GetFieldValue(T.x - T.tx * dz, T.y - T.ty * dz, fB0);
545 fB0.Combine(fB[i], w[i]);
546 fld.Set(fB0, fz0, fB1, fz1, fB2, fz2);
547
548 // fvec zero = ZERO;
549 fvec initialised = fvec(z[i] <= z_end) & fvec(z_start < z[i]);
550 fvec w1 = (w[i] & (initialised));
551 fvec wIn = (ONE & (initialised));
552
553 L1Extrapolate(T, z[i], qp0, fld, &w1);
554 if (i == NMvdStations)
555 L1AddPipeMaterial(T, qp0, wIn);
556#ifdef USE_RL_TABLE
557 L1AddMaterial(T, fRadThick[i].GetRadThick(T.x, T.y), qp0, wIn);
558#else
559 L1AddMaterial(T, sta[i].materialInfo, qp0, wIn);
560#endif
561 // L1Filter( T, sta[i].frontInfo, u[i], w1 );
562 // L1Filter( T, sta[i].backInfo, v[i], w1 );
563 L1Filter(T, sta[i].frontInfo, u[i], w1, &du[i]); // AZ
564 L1Filter(T, sta[i].backInfo, v[i], w1, &dv[i]); // AZ
565
566 fB2 = fB1;
567 fz2 = fz1;
568 fB1 = fB0;
569 fz1 = fz0;
570 }
571 // L1AddHalfMaterial( T, sta[i].materialInfo, qp0 );
572
573 { // extrapolate to 1 m
574 L1TrackPar Tout = T;
575 if (extrapolateToTheEndOfSTS) {
576 // extrapolate to the last station
577 i = 0;
578 fz1 = z[i];
579 sta[i].fieldSlice.GetFieldValue(T.x, Tout.y, fB1);
580 fB1.Combine(fB[i], w[i]);
581
582 fz2 = z[i + 2];
583 dz = fz2 - Tout.z;
584 sta[i].fieldSlice.GetFieldValue(Tout.x + Tout.tx * dz, Tout.y + Tout.ty * dz, fB2);
585 fB2.Combine(fB[i + 2], w[i + 2]);
586 fld.Set(fB2, fz2, fB1, fz1, fB0, fz0);
587
588 for (++i; i < NStations; i++) {
589 fz0 = z[i];
590 dz = (Tout.z - fz0);
591 sta[i].fieldSlice.GetFieldValue(Tout.x - Tout.tx * dz, Tout.y - Tout.ty * dz, fB0);
592 fB0.Combine(fB[i], w[i]);
593 fld.Set(fB0, fz0, fB1, fz1, fB2, fz2);
594
595 // fvec zero = ZERO;
596 fvec initialised = fvec(z[i] > z_end);
597 fvec wIn = (ONE & (initialised));
598
599 L1Extrapolate(Tout, z[i], qp0, fld, &wIn);
600 if (i == NMvdStations)
601 L1AddPipeMaterial(Tout, qp0, wIn);
602#ifdef USE_RL_TABLE
603 L1AddMaterial(Tout, fRadThick[i].GetRadThick(Tout.x, Tout.y), qp0, wIn);
604#else
605 L1AddMaterial(Tout, sta[i].materialInfo, qp0, wIn);
606#endif
607
608 fB2 = fB1;
609 fz2 = fz1;
610 fB1 = fB0;
611 fz1 = fz0;
612 }
613 // extrapolate to 1m
614 {
615 const fvec zFinal = 100.f;
616 i = NStations - 1;
617 fvec initialised =
618 fvec(zFinal > Tout.z); // 3cm safe distance, needed because of diff position of sensors
619 L1Extrapolate(Tout, zFinal, qp0, fld, &initialised); // extra with old field
620 }
621 }
622 for (iVec = 0; iVec < nTracks_SIMD; iVec++) {
623 t[iVec]->TLast[0] = Tout.x[iVec];
624 t[iVec]->TLast[1] = Tout.y[iVec];
625 t[iVec]->TLast[2] = Tout.tx[iVec];
626 t[iVec]->TLast[3] = Tout.ty[iVec];
627 t[iVec]->TLast[4] = Tout.qp[iVec];
628 t[iVec]->TLast[5] = Tout.z[iVec];
629
630 t[iVec]->CLast[0] = Tout.C00[iVec];
631 t[iVec]->CLast[1] = Tout.C10[iVec];
632 t[iVec]->CLast[2] = Tout.C11[iVec];
633 t[iVec]->CLast[3] = Tout.C20[iVec];
634 t[iVec]->CLast[4] = Tout.C21[iVec];
635 t[iVec]->CLast[5] = Tout.C22[iVec];
636 t[iVec]->CLast[6] = Tout.C30[iVec];
637 t[iVec]->CLast[7] = Tout.C31[iVec];
638 t[iVec]->CLast[8] = Tout.C32[iVec];
639 t[iVec]->CLast[9] = Tout.C33[iVec];
640 t[iVec]->CLast[10] = Tout.C40[iVec];
641 t[iVec]->CLast[11] = Tout.C41[iVec];
642 t[iVec]->CLast[12] = Tout.C42[iVec];
643 t[iVec]->CLast[13] = Tout.C43[iVec];
644 t[iVec]->CLast[14] = Tout.C44[iVec];
645
646 t[iVec]->chi2 = Tout.chi2[iVec];
647 t[iVec]->NDF = (int)Tout.NDF[iVec];
648 }
649 }
650 } // iter
651 }
652}
653
654//----------------------------------------------------------------------------------------------
655
656void L1Algo::GuessVec(L1TrackPar& t, fvec* xV, fvec* yV, fvec* zV, fvec* Sy, fvec* wV, int NHits, fvec* zCur)
657// gives nice initial approximation for x,y,tx,ty - almost same as KF fit. qp - is shifted by 4%, residual - ~3.5% (KF
658// fit residual - 1%).
659{
660 fvec A0, A1 = ZERO, A2 = ZERO, A3 = ZERO, A4 = ZERO, A5 = ZERO, a0, a1 = ZERO, a2 = ZERO, b0, b1 = ZERO, b2 = ZERO;
661 fvec z0, x, y, z, S, w, wz, wS;
662
663 int i = NHits - 1;
664 if (zCur)
665 z0 = *zCur;
666 else
667 z0 = zV[i];
668 w = wV[i];
669 A0 = w;
670 a0 = w * xV[i];
671 b0 = w * yV[i];
672 for (i = 0; i < NHits; i++) {
673 x = xV[i];
674 y = yV[i];
675 w = wV[i];
676 z = zV[i] - z0;
677 S = Sy[i];
678 wz = w * z;
679 wS = w * S;
680 A0 += w;
681 A1 += wz;
682 A2 += wz * z;
683 A3 += wS;
684 A4 += wS * z;
685 A5 += wS * S;
686 a0 += w * x;
687 a1 += wz * x;
688 a2 += wS * x;
689 b0 += w * y;
690 b1 += wz * y;
691 b2 += wS * y;
692 }
693
694 fvec A3A3 = A3 * A3;
695 fvec A3A4 = A3 * A4;
696 fvec A1A5 = A1 * A5;
697 fvec A2A5 = A2 * A5;
698 fvec A4A4 = A4 * A4;
699
700 fvec det = rcp(-A2 * A3A3 + A1 * (A3A4 + A3A4 - A1A5) + A0 * (A2A5 - A4A4));
701 fvec Ai0 = (-A4A4 + A2A5);
702 fvec Ai1 = (A3A4 - A1A5);
703 fvec Ai2 = (-A3A3 + A0 * A5);
704 fvec Ai3 = (-A2 * A3 + A1 * A4);
705 fvec Ai4 = (A1 * A3 - A0 * A4);
706 fvec Ai5 = (-A1 * A1 + A0 * A2);
707
708 fvec L, L1;
709 t.x = (Ai0 * a0 + Ai1 * a1 + Ai3 * a2) * det;
710 t.tx = (Ai1 * a0 + Ai2 * a1 + Ai4 * a2) * det;
711 fvec txtx1 = 1. + t.tx * t.tx;
712 L = (Ai3 * a0 + Ai4 * a1 + Ai5 * a2) * det * rcp(txtx1);
713 L1 = L * t.tx;
714 A1 = A1 + A3 * L1;
715 A2 = A2 + (A4 + A4 + A5 * L1) * L1;
716 b1 += b2 * L1;
717 det = rcp(-A1 * A1 + A0 * A2);
718
719 t.y = (A2 * b0 - A1 * b1) * det;
720 t.ty = (-A1 * b0 + A0 * b1) * det;
721 t.qp = -L * c_light_i * rsqrt(txtx1 + t.ty * t.ty);
722 t.z = z0;
723}
724
725//----------------------------------------------------------------------------------------------
726
727void L1Algo::FilterFirst(L1TrackPar& track, fvec& x, fvec& y, L1Station& st)
728{
729 // initialize covariance matrix
730 track.C00 = st.XYInfo.C00;
731 track.C10 = st.XYInfo.C10;
732 track.C11 = st.XYInfo.C11;
733 track.C20 = ZERO;
734 track.C21 = ZERO;
735 track.C22 = vINF;
736 track.C30 = ZERO;
737 track.C31 = ZERO;
738 track.C32 = ZERO;
739 track.C33 = vINF;
740 track.C40 = ZERO;
741 track.C41 = ZERO;
742 track.C42 = ZERO;
743 track.C43 = ZERO;
744 track.C44 = ONE;
745
746 track.x = x;
747 track.y = y;
748 track.NDF = -3.0;
749 track.chi2 = ZERO;
750}
L1AddPipeMaterial
void L1AddPipeMaterial(L1TrackPar &T, fvec qp0, fvec w=1, fvec mass2=0.1395679f *0.1395679f)
Definition L1AddMaterial.h:91
L1AddMaterial
void L1AddMaterial(L1TrackPar &T, fvec radThick, fvec qp0, fvec w=1, fvec mass2=0.1395679f *0.1395679f)
Definition L1AddMaterial.h:11
L1Extrapolate
void L1Extrapolate(L1TrackPar &T, fvec z_out, fvec qp0, L1FieldRegion &F, fvec *w=0)
Definition L1Extrapolation.h:11
L1Filter
void L1Filter(L1TrackPar &T, L1UMeasurementInfo &info, fvec u, fvec w=1., fvec *du2=0)
Definition L1Filtration.h:13
fscal
float fscal
Definition P4_F32vec4.h:232
v
__m128 v
Definition P4_F32vec4.h:1
fvecLen
const int fvecLen
Definition P4_F32vec4.h:233
rsqrt
friend F32vec4 rsqrt(const F32vec4 &a)
Definition P4_F32vec4.h:37
i
int i
Definition P4_F32vec4.h:22
rcp
friend F32vec4 rcp(const F32vec4 &a)
Definition P4_F32vec4.h:45
fvec
F32vec4 fvec
Definition P4_F32vec4.h:231
L1Algo::StripsToCoor
void StripsToCoor(const fscal &u, const fscal &v, fscal &x, fscal &y, const L1Station &sta) const
convert strip positions to coordinates
Definition L1Algo.cxx:185
L1Algo::vStsHits
vector< L1StsHit > vStsHits
Definition L1Algo.h:162
L1Algo::vTracks
vector< L1Track > vTracks
--— Output data --—
Definition L1Algo.h:182
L1Algo::_fvecalignment
L1Station vStations[MaxNStations] _fvecalignment
Definition L1Algo.h:155
L1Algo::NStations
int NStations
Definition L1Algo.h:153
L1Algo::vStsZPos
vector< fscal > vStsZPos
Definition L1Algo.h:161
L1Algo::vStsStripsB
vector< L1Strip > vStsStripsB
Definition L1Algo.h:160
L1Algo::vAllHitPoints
vector< L1HitPoint > vAllHitPoints
Definition L1Algo.h:178
L1Algo::GetFStation
unsigned char GetFStation(unsigned char flag)
– Flags routines –
Definition L1Algo.h:271
L1Algo::FilterFirst
void FilterFirst(L1TrackPar &track, fvec &x, fvec &y, L1Station &st)
Definition L1TrackFitter.cxx:727
L1Algo::vSFlag
vector< unsigned char > vSFlag
Definition L1Algo.h:164
L1Algo::vRecoHits
vector< THitI > vRecoHits
Definition L1Algo.h:183
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:656
L1Algo::NMvdStations
int NMvdStations
Definition L1Algo.h:154
L1Algo::vStsStrips
vector< L1Strip > vStsStrips
Definition L1Algo.h:159
L1Algo::L1KFTrackFitter
void L1KFTrackFitter(bool extrapolateToTheEndOfSTS=false)
Definition L1TrackFitter.cxx:325
L1Algo::KFTrackFitter_simple
void KFTrackFitter_simple()
Track fitting procedures.
Definition L1TrackFitter.cxx:39
L1Algo::MaxNStations
@ MaxNStations
Definition L1Algo.h:151
L1Algo::fRadThick
vector< L1Material > fRadThick
Definition L1Algo.h:157
L1FieldSlice::GetFieldValue
void GetFieldValue(const fvec &x, const fvec &y, L1FieldValue &B) const
Definition L1Field.h:47
L1FieldSlice::cy
fvec cy[21]
Definition L1Field.h:39
L1FieldValue::Combine
void Combine(L1FieldValue &B, fvec w)
Definition L1Field.h:22
L1Station::XYInfo
L1XYMeasurementInfo XYInfo
Definition L1Station.h:31
L1Station::backInfo
L1UMeasurementInfo backInfo
Definition L1Station.h:30
L1Station::fieldSlice
L1FieldSlice fieldSlice
Definition L1Station.h:29
L1Station::materialInfo
L1MaterialInfo materialInfo
Definition L1Station.h:28
L1Station::frontInfo
L1UMeasurementInfo frontInfo
Definition L1Station.h:30
L1Station::Sy
fvec Sy
Definition L1Station.h:27
L1StsHit::iz
TZPosI iz
Definition L1StsHit.h:13
L1StsHit::f
TStripI f
Definition L1StsHit.h:12
L1StsHit::b
TStripI b
Definition L1StsHit.h:12
L1Track::TFirst
double TFirst[6]
Definition L1Track.h:25
L1Track::chi2
double chi2
Definition L1Track.h:25
L1Track::NHits
unsigned char NHits
Definition L1Track.h:23
L1Track::NDF
int NDF
Definition L1Track.h:26
L1Track::CFirst
double CFirst[15]
Definition L1Track.h:25
L1Track::CLast
double CLast[15]
Definition L1Track.h:25
L1Track::TLast
double TLast[6]
Definition L1Track.h:25
NS_L1TrackFitter
Definition L1TrackFitter.cxx:28
NS_L1TrackFitter::ZERO
const fvec ZERO
Definition L1TrackFitter.cxx:30
NS_L1TrackFitter::vINF
const fvec vINF
Definition L1TrackFitter.cxx:32
NS_L1TrackFitter::ONE
const fvec ONE
Definition L1TrackFitter.cxx:31
NS_L1TrackFitter::c_light_i
const fvec c_light_i
Definition L1TrackFitter.cxx:29
NS_L1TrackFitter::c_light
const fvec c_light
Definition L1TrackFitter.cxx:29
L1HitPoint
contain strips positions and coordinates of hit
Definition L1HitPoint.h:9
L1HitPoint::V
fscal V() const
Definition L1HitPoint.h:43
L1HitPoint::DU
fscal DU() const
Definition L1HitPoint.h:44
L1HitPoint::U
fscal U() const
Definition L1HitPoint.h:42
L1HitPoint::Y
fscal Y() const
Definition L1HitPoint.h:40
L1HitPoint::X
fscal X() const
Definition L1HitPoint.h:39
L1HitPoint::DV
fscal DV() const
Definition L1HitPoint.h:45