ATLAS Offline Software
Tracking/TrkVertexFitter/TrkVKalVrtCore/src/Utilities.cxx
Go to the documentation of this file.
1 /*
2  Copyright (C) 2002-2023 CERN for the benefit of the ATLAS collaboration
3 */
4 
5 #include "TrkVKalVrtCore/Utilities.h"
6 #include "TrkVKalVrtCore/VtDeriv.h"
7 #include "TrkVKalVrtCore/cfNewP.h"
8 #include "TrkVKalVrtCore/TrkVKalVrtCoreBase.h"
9 #include <algorithm>
10 #include <cmath>
11 
12 namespace {
13 // Helpers go to the anonymous
14 
15 inline int sIndexVK(int i, int j) {
16  return i > j ? i * (i + 1) / 2 + j : j * (j + 1) / 2 + i;
17 }
18 } // namespace
19 
20 namespace Trk {
21 
22 // Chi2 calculation for 5-par. track
23 // XYZT(3)-vertex , ICH - charge , PART(3) -track pass. XYZT vertex
24 // PAR0(5) - fitted track
25 // ------------------------------------------------------------
26 
27 double cfchi2(double *xyzt, const long int ich, double *part,
28  const double *par0, double *wgt, double *rmnd)
29 {
30  double phif, epsf, d1, d2, d3, d4, d5, sr, uu, vv, res, zpf;
31 
32  /* Parameter adjustments */
33  --wgt;
34  --part;
35  --xyzt;
36 
37  uu = xyzt[1] * cos(part[2]) + xyzt[2] * sin(part[2]);
38  vv = xyzt[2] * cos(part[2]) - xyzt[1] * sin(part[2]);
39  sr = part[3] * std::abs(ich);
40  epsf = -vv - (uu * uu + vv * vv) * sr / 2.;
41  zpf = xyzt[3] - uu * (1. - vv * sr) / tan(part[1]);
42  phif = part[2] - uu * sr;
43  d1 = par0[0] - epsf;
44  d2 = par0[1] - zpf;
45  d3 = par0[2] - part[1];
46  d4 = par0[3] - phif;
47  d5 = par0[4] - part[3];
48  while(d4 > M_PI)d4-=2.*M_PI;
49  while(d4 < -M_PI)d4+=2.*M_PI;
50 // -----------------------Check of propagation
51 // double paro[5],parn[5],s,ref[3],peri[3];
52 // paro[0]=0.; paro[1]=0.; paro[2]=part[1]; paro[3]=part[2]; paro[4]=part[3];
53 // ref[0]=-xyzt[1]; ref[1]=-xyzt[2]; ref[2]=-xyzt[3];
54 // cfnewp(ich, paro, ref, &s, parn, peri);
55 // d1 = par0[0] - parn[0]; d2 = par0[1] - parn[1]; d3 = par0[2] - parn[2];
56 // d4 = par0[3] - parn[3]; d5 = par0[4] - parn[4];
57 // std::cout<<" testp0="<<parn[0]<<", "<<parn[1]<<", "<<parn[3]<<", "<<s<<'\n';
58 // std::cout<<" testp1="<<(*ich)<<", "<<epsf<<", "<<zpf<<", "<<phif<<", "<<res<<'\n';
59 //--------------------------------------------------------------------
60  res = wgt[1] * (d1 * d1) + wgt[3] * (d2 * d2) + wgt[6] * (d3 * d3)
61  + wgt[10] * (d4 * d4) + wgt[15] * (d5 * d5) + (d2 *
62  d1 * wgt[2] + d3 * (d1 * wgt[4] + d2 * wgt[5]) + d4 * (d1 * wgt[7]
63  + d2 * wgt[8] + d3 * wgt[9]) + d5 * (d1 * wgt[11] + d2 * wgt[12]
64  + d3 * wgt[13] + d4 * wgt[14])) * 2;
65  rmnd[0] = d1;
66  rmnd[1] = d2;
67  rmnd[2] = d3;
68  rmnd[3] = d4;
69  rmnd[4] = d5;
70  return (std::abs(res) < 1.e12 ? std::abs(res) : 1.e12);
71 }
72 
73 
74 // Chi2 calculation for 5-par. track
75 // VRTT(3)-vertex , PART(3) - parameters of track pass. through VRTT vertex
76 // Procedure assumes that VRTT(3) is in system where track reference point is (0,0,0).
77 // So it transfers fitted parameters to (0,0,0) and compares with reference track parameters
78 // ------------------------------------------------------------
79 
80 double cfchi2(const double *vrtt, const double * part, VKTrack * trk)
81 {
82 
83  double d1, d2, d3, d4, d5;
84  double theta_f = part[0];
85  double phi_f = part[1];
86  double invR_f = part[2];
87  double uu = vrtt[0] * cos(phi_f) + vrtt[1] * sin(phi_f );
88  double vv = vrtt[1] * cos(phi_f) - vrtt[0] * sin(phi_f);
89  double sr = invR_f * abs(trk->Charge); // Zero in case of zero charge
90  double epsf = -vv - (uu * uu + vv * vv) * sr / 2.;
91  double zpf = vrtt[2] - uu * (1. - vv * sr) / tan(theta_f);
92  double phif = phi_f - uu * sr;
93  d1 = epsf - trk->Perig[0];
94  d2 = zpf - trk->Perig[1];
95  d3 = theta_f - trk->Perig[2];
96  d4 = phif - trk->Perig[3];
97  d5 = invR_f - trk->Perig[4];
98  if(d4 > M_PI)d4-=2.*M_PI;
99  if(d4 < -M_PI)d4+=2.*M_PI;
100  double res = trk->WgtM[0] * (d1*d1)
101  + trk->WgtM[2] * (d2*d2)
102  + trk->WgtM[5] * (d3*d3)
103  + trk->WgtM[9] * (d4*d4)
104  + trk->WgtM[14]* (d5*d5)
105  +(d2 * d1*trk->WgtM[1]
106  + d3 * (d1*trk->WgtM[3] + d2*trk->WgtM[4])
107  + d4 * (d1*trk->WgtM[6] + d2*trk->WgtM[7] + d3*trk->WgtM[8])
108  + d5 * (d1*trk->WgtM[10] + d2*trk->WgtM[11] + d3*trk->WgtM[12] + d4*trk->WgtM[13])) * 2;
109  trk->rmnd[0] = d1;
110  trk->rmnd[1] = d2;
111  trk->rmnd[2] = d3;
112  trk->rmnd[3] = d4;
113  trk->rmnd[4] = d5;
114 //std::cout<<__func__<<":"<<d1<<", "<<d2<<", "<<d3<<", "<<d4<<", "<<d5<<'\n';
115 //std::cout<<trk->Perig[0]<<", "<<trk->Perig[1]<<", "<<trk->Perig[2]<<", "<<trk->Perig[3]<<", "<<trk->Perig[4]<<'\n';
116 //std::cout<<theta_f<<", "<<phi_f<<", "<<invR_f<<'\n';
117 //std::cout<<trk->WgtM[0]<<", "<<trk->WgtM[2]<<", "<<trk->WgtM[5]<<", "<<trk->WgtM[9]<<", "<<trk->WgtM[14]<<'\n';
118  return (std::abs(res) < 1.e12 ? std::abs(res) : 1.e12);
119 }
120 
121 
122 
123 
124 //
125 // Function returns a position of minimum X in parabolic approximation
126 // based on 3 measured points (X,Y)
127 //----------------------------------------------------------------------------
128 double finter(double y0, double y1, double y2, double x0, double x1, double x2)
129 {
130  double ret_val;
131  volatile double b1,b2; // To guarantee AMD==Intel
132 
133 /* ------------------------*/
134 /* Function interpolation */
135 /* Author: V.Kostyukhin */
136 /* ------------------------*/
137  b1 = (y1 - y0) / (x1 - x0);
138  b2 = (y2 - y0 - b1 * (x2 - x0)) / (x2 - x0) / (x2 - x1);
139  if (std::abs(b2) < 1e-8) {
140  if (y2 <= y0 && y2 < y1) {
141  ret_val = x2;
142  } else if (y1 <= y0 && y1 < y2) {
143  ret_val = x1;
144  } else if (y0 < y1 && y0 < y2) {
145  ret_val = x0 + (x2 - x0) * .001;
146  } else {
147  ret_val = x2;
148  }
149  return ret_val;
150  }
151  ret_val = (x1 + x0) - ( b1 / b2 );
152 //VK 14.08.2007 Check now is after call of this subroutine
153 // double NegativeLim = x0 + (x2 - x0) * (-0.3);
154 // if (ret_val <= x0) {
155 // if (ret_val <= NegativeLim) ret_val = NegativeLim;
156 // }
157  return ret_val/2.;
158 }
159 
160 
161 
162 // R=A*S*A' where A.i.j == A(j,i) -> summation over first index of A
163 // Dimension S(N,N)!!! OBSOLETE!
164 //---------------------------------------------------------------------
165 /*void tdasat(double *a, double *s, double *r__, long int M, long int N)
166 {
167 
168  long int imax, i__, k, ia, mn, ir, is, iaa, ind;
169  double sum;
170  --r__;
171  --s;
172  --a;
173 
174  imax = (M * M + M) / 2;
175  for (i__ = 1; i__ <= imax; ++i__) {r__[i__] = 0.;}
176  mn = M * N;
177  ind = 0;
178  i__ = 0;
179 
180 L5:
181  ind += i__;
182  ia = 0;
183  ir = 0;
184 
185 L10:
186  is = ind;
187  sum = 0.;
188  k = 0;
189 
190 L15:
191  if (k > i__) goto L20;
192  ++is;
193  goto L30;
194 L20:
195  is += k;
196 L30:
197  ++ia;
198  sum += s[is] * a[ia];
199  ++k;
200  if (k < N) goto L15;
201  iaa = i__ + 1;
202 L40:
203  ++ir;
204  r__[ir] += sum * a[iaa];
205  iaa += N;
206  if (iaa <= ia) goto L40;
207  if (ia < mn) goto L10;
208 
209  ++i__;
210  if (i__ < N) goto L5;
211 
212  return;
213 }*/
214 
215 //
216 // Dimensions input: CovI(N,N), output: CovF(M,M), input: Der(N,M)-first index runs first
217 //
218 void tdasatVK(const double *Der, const double *CovI, double *CovF, long int M, long int N)
219 {
220  int i,j,k,f;
221  for( k=0; k<M; k++) { // sycle on output index
222  for( f=0; f<=k; f++ ) { //
223  double sum=0.;
224  for( i=0; i<N; i++){
225  double tmp=0.;
226  for( j=0; j<N; j++) tmp += CovI[sIndexVK(i,j)]*Der[j+f*N];
227  sum += Der[i+k*N]*tmp;
228  }
229  CovF[sIndexVK(k,f)]=sum;
230  }
231  }
232 }
233 
234 // Zero symmetric matrix and set up diagonal elevents to VALUE
235 //------------------------------------------------------------
236 void cfsetdiag(long int n, double *matr, double value) noexcept
237 {
238  long int i, j, ij;
239  --matr;
240 
241  ij = 0;
242  for (i = 1; i <= n; ++i) {
243  for (j = 1; j <= i; ++j) {
244  ++ij;
245  matr[ij] = 0.;
246  if (i == j) {
247  matr[ij] = value;
248  }
249  }
250  }
251 }
252 
253 // Functions for 2D parabolic fit
254 //
255 // Decomposition of Chi2 function: G(x,y)
256 //double Chi2Taylor(double a, double b, double c, double d, double e, double f,
257 //double x, double y) { return a + b*x + d*y + c*x*x + e*y*y + f*x*y;}
258 
259 
260 void abcCoef(double g1, double g2, double g3,
261  double &a, double &b, double &c)
262 /* Function assumes g1(x=0), g2(x=0.5), g3(x=1.) */
263 {
264  a=g1;
265  c=2.*(g3-g1) - 4.*(g2-g1);
266  b=(g3-g1) - c;
267 }
268 
269 void efdCoef(double Ga0, double Gamb, double Gab, double Gw0, double Gwb,
270  double alf, double bet, double w,
271  double &d, double &e, double &f)
272 /* Function assumes Ga0(x=alf, y=0), Gamb(x=alf, y=-bet), Gab(x=alf, y=+bet),*/
273 /* Gw0(x=w, y=0), Gwb( x=w, y=bet)*/
274 {
275 
276  e = (Gab + Gamb - 2.*Ga0)/bet/bet/2.;
277  f = (Gwb + Gamb - 2.*e*bet*bet - Gw0 - Ga0)/bet/(w-alf);
278  d = (Gab - Ga0 - e*bet*bet - f*alf*bet)/bet;
279 }
280 
281 
282 void ParaMin( double b, double c, double d, double e, double f,
283  double &xmin, double &ymin)
284 {
285  ymin = (f*b-2.*c*d)/(4.*c*e - f*f);
286  if( std::abs(f) > std::abs(c) ) { xmin = - (2.*e*ymin + d)/f;}
287  else { xmin = - (f*ymin+b)/(2.*c);}
288  }
289 
290 void cfTrkCovarCorr(double *cov){
291 // std::cout<<cov[1]*cov[1]/cov[0]/cov[2]<<'\n';
292 // std::cout<<cov[3]*cov[3]/cov[0]/cov[5]<<", "<<cov[4]*cov[4]/cov[2]/cov[5]<<'\n';
293 // std::cout<<cov[6]*cov[6]/cov[0]/cov[9]<<", "<<cov[7]*cov[7]/cov[2]/cov[9]<<", "<<cov[8]*cov[8]/cov[5]/cov[9]<<'\n';
294 // std::cout<<cov[10]*cov[10]/cov[0]/cov[14]<<", "<<cov[11]*cov[11]/cov[2]/cov[14]<<", "<<cov[12]*cov[12]/cov[5]/cov[14]<<
295 // ", "<<cov[13]*cov[13]/cov[9]/cov[14]<<'\n';
296  double Lim=0.99; double Lim2=Lim*Lim;
297  bool i0,i1,i2,i3,i4; i0=i1=i2=i3=i4=false;
298  if( std::abs(cov[1]*cov[1])/cov[0]/cov[2] > Lim2 ){ i0=true; i1=true;}
299  if( std::abs(cov[3]*cov[3])/cov[0]/cov[5] > Lim2 ){ i0=true; i2=true;}
300  if( std::abs(cov[4]*cov[4])/cov[2]/cov[5] > Lim2 ){ i1=true; i2=true;}
301  if( std::abs(cov[6]*cov[6])/cov[0]/cov[9] > Lim2 ){ i0=true; i3=true;}
302  if( std::abs(cov[7]*cov[7])/cov[2]/cov[9] > Lim2 ){ i1=true; i3=true;}
303  if( std::abs(cov[8]*cov[8])/cov[5]/cov[9] > Lim2 ){ i2=true; i3=true;}
304  if( std::abs(cov[10]*cov[10])/cov[0]/cov[14] > Lim2 ){ i0=true; i4=true;}
305  if( std::abs(cov[11]*cov[11])/cov[2]/cov[14] > Lim2 ){ i1=true; i4=true;}
306  if( std::abs(cov[12]*cov[12])/cov[5]/cov[14] > Lim2 ){ i2=true; i4=true;}
307  if( std::abs(cov[13]*cov[13])/cov[9]/cov[14] > Lim2 ){ i3=true; i4=true;}
308 
309  if(i0){ cov[1]*=Lim; cov[3]*=Lim; cov[6]*=Lim; cov[10]*=Lim; }
310  if(i1){ cov[1]*=Lim; cov[4]*=Lim; cov[7]*=Lim; cov[11]*=Lim; }
311  if(i2){ cov[3]*=Lim; cov[4]*=Lim; cov[8]*=Lim; cov[12]*=Lim; }
312  if(i3){ cov[6]*=Lim; cov[7]*=Lim; cov[8]*=Lim; cov[13]*=Lim; }
313  if(i4){ cov[10]*=Lim; cov[11]*=Lim; cov[12]*=Lim; cov[13]*=Lim; }
314 }
315 
316 } /*End of namespace */
317 
LArG4FSStartPointFilter.part
part
Definition: LArG4FSStartPointFilter.py:21
Trk::finter
double finter(double y0, double y1, double y2, double x0, double x1, double x2)
Definition: Tracking/TrkVertexFitter/TrkVKalVrtCore/src/Utilities.cxx:128
plotBeamSpotCompare.x1
x1
Definition: plotBeamSpotCompare.py:216
Trk::VKTrack::WgtM
double WgtM[15]
Definition: TrkVKalVrtCoreBase.h:87
python.CaloRecoConfig.f
f
Definition: CaloRecoConfig.py:127
ymin
double ymin
Definition: listroot.cxx:63
hist_file_dump.d
d
Definition: hist_file_dump.py:137
plotBeamSpotCompare.x2
x2
Definition: plotBeamSpotCompare.py:218
plotBeamSpotVxVal.cov
cov
Definition: plotBeamSpotVxVal.py:201
M_PI
#define M_PI
Definition: ActiveFraction.h:11
dq_defect_virtual_defect_validation.d1
d1
Definition: dq_defect_virtual_defect_validation.py:79
athena.value
value
Definition: athena.py:122
JetTiledMap::N
@ N
Definition: TiledEtaPhiMap.h:44
Trk::VKTrack
Definition: TrkVKalVrtCoreBase.h:64
Trk::VKTrack::Charge
int Charge
Definition: TrkVKalVrtCoreBase.h:73
drawFromPickle.cos
cos
Definition: drawFromPickle.py:36
Trk::VKTrack::Perig
double Perig[5]
Definition: TrkVKalVrtCoreBase.h:86
VtDeriv.h
makeTRTBarrelCans.y1
tuple y1
Definition: makeTRTBarrelCans.py:15
TrkVKalVrtCoreBase.h
Trk::abcCoef
void abcCoef(double g1, double g2, double g3, double &a, double &b, double &c)
Definition: Tracking/TrkVertexFitter/TrkVKalVrtCore/src/Utilities.cxx:260
python.SystemOfUnits.sr
int sr
Definition: SystemOfUnits.py:113
convertTimingResiduals.sum
sum
Definition: convertTimingResiduals.py:55
fitman.g1
g1
Definition: fitman.py:619
lumiFormat.i
int i
Definition: lumiFormat.py:92
xmin
double xmin
Definition: listroot.cxx:60
Trk::VKTrack::rmnd
double rmnd[5]
Definition: TrkVKalVrtCoreBase.h:91
beamspotman.n
n
Definition: beamspotman.py:731
makeTRTBarrelCans.y2
tuple y2
Definition: makeTRTBarrelCans.py:18
res
std::pair< std::vector< unsigned int >, bool > res
Definition: JetGroupProductTest.cxx:14
drawFromPickle.tan
tan
Definition: drawFromPickle.py:36
DeMoUpdate.tmp
string tmp
Definition: DeMoUpdate.py:1167
Trk::tdasatVK
void tdasatVK(const double *Der, const double *CovI, double *CovF, long int M, long int N)
Definition: Tracking/TrkVertexFitter/TrkVKalVrtCore/src/Utilities.cxx:218
Trk
Ensure that the ATLAS eigen extensions are properly loaded.
Definition: FakeTrackBuilder.h:9
fitman.g2
g2
Definition: fitman.py:624
plotBeamSpotMon.b
b
Definition: plotBeamSpotMon.py:77
Trk::ParaMin
void ParaMin(double b, double c, double d, double e, double f, double &xmin, double &ymin)
Definition: Tracking/TrkVertexFitter/TrkVKalVrtCore/src/Utilities.cxx:282
xyzt
#define xyzt
DiTauMassTools::MaxHistStrategyV2::e
e
Definition: PhysicsAnalysis/TauID/DiTauMassTools/DiTauMassTools/HelperFunctions.h:26
a
TList * a
Definition: liststreamerinfos.cxx:10
dq_defect_virtual_defect_validation.d2
d2
Definition: dq_defect_virtual_defect_validation.py:81
Trk::cfchi2
double cfchi2(double *xyzt, const long int ich, double *part, const double *par0, double *wgt, double *rmnd)
Definition: Tracking/TrkVertexFitter/TrkVKalVrtCore/src/Utilities.cxx:27
cfNewP.h
drawFromPickle.sin
sin
Definition: drawFromPickle.py:36
python.IoTestsLib.w
def w
Definition: IoTestsLib.py:200
Utilities.h
PlotCalibFromCool.vv
vv
Definition: PlotCalibFromCool.py:716
python.compressB64.c
def c
Definition: compressB64.py:93
Trk::cfTrkCovarCorr
void cfTrkCovarCorr(double *cov)
Definition: Tracking/TrkVertexFitter/TrkVKalVrtCore/src/Utilities.cxx:290
Trk::efdCoef
void efdCoef(double Ga0, double Gamb, double Gab, double Gw0, double Gwb, double alf, double bet, double w, double &d, double &e, double &f)
Definition: Tracking/TrkVertexFitter/TrkVKalVrtCore/src/Utilities.cxx:269
fitman.k
k
Definition: fitman.py:528
Trk::cfsetdiag
void cfsetdiag(long int n, double *matr, double value) noexcept
Definition: Tracking/TrkVertexFitter/TrkVKalVrtCore/src/Utilities.cxx:236
Generated on Fri Jul 5 2024 21:32:20 for ATLAS Offline Software by   doxygen 1.8.18