40 {
41
42 double b_phi0=chargedtrk.parameters()[
Trk::phi0];
43 double b_cosphi0=
cos(b_phi0);
44 double b_sinphi0=
sin(b_phi0);
46 chargedtrk.parameters()[
Trk::d0]*b_sinphi0;
48 chargedtrk.parameters()[
Trk::d0]*b_cosphi0;
50 chargedtrk.parameters()[
Trk::z0];
51
54
55
56 double Bz=magnFieldVect.z()*299.792;
57
58 double b_Rt=getRadiusOfCurvature(chargedtrk,Bz);
59
60 double b_cottheta=1./
tan(chargedtrk.parameters()[
Trk::theta]);
61
62
63 double a_x0=neutraltrk.
position()[0];
64 double a_y0=neutraltrk.
position()[1];
65 double a_z0=neutraltrk.
position()[2];
66 double a_px=neutraltrk.
momentum()[0];
67 double a_py=neutraltrk.
momentum()[1];
68 double a_pz=neutraltrk.
momentum()[2];
69
70
71
72 double DxNoVar=b_x0+b_Rt*b_sinphi0-a_x0;
73 double DyNoVar=b_y0-b_Rt*b_cosphi0-a_y0;
74 double DzNoVar=b_z0+b_Rt*b_cottheta*b_phi0-a_z0;
75
76
77 double b_phi=b_phi0;
78 double b_cosphi=
cos(b_phi);
79 double b_sinphi=
sin(b_phi);
80 double a_lambda=0.;
81
82 double deltab_phi=0.;
83 double deltaa_lambda=0.;
84
85 int loopsnumber=0;
86
88
89 bool secondTimeSaddleProblem=false;
90
91 while (!isok) {
92
93
94
95
96
97
98 double x1fin=b_x0+b_Rt*(
sin(b_phi0)-
sin(b_phi));
99 double y1fin=b_y0+b_Rt*(
cos(b_phi)-
cos(b_phi0));
100 double z1fin=b_z0+b_Rt*b_cottheta*(b_phi0-b_phi);
101
102 double x2fin=a_x0+a_px*a_lambda;
103 double y2fin=a_y0+a_py*a_lambda;
104 double z2fin=a_z0+a_pz*a_lambda;
105
106 ATH_MSG_VERBOSE (
"x1fin " << x1fin <<
"y1fin " << y1fin <<
"z1fin " << z1fin);
107 ATH_MSG_VERBOSE (
"x2fin " << x2fin <<
"y2fin " << y2fin <<
"z2fin " << z2fin);
108
109 double distance = sqrt((x1fin-x2fin)*(x1fin-x2fin)+
110 (y1fin-y2fin)*(y1fin-y2fin)+
111 (z1fin-z2fin)*(z1fin-z2fin));
112
114
115
116
117 loopsnumber+=1;
118
119 double d1db_phi=((DxNoVar-a_px*a_lambda)*(-b_Rt*b_cosphi)+
120 (DyNoVar-a_py*a_lambda)*(-b_Rt*b_sinphi)+
121 (DzNoVar-b_Rt*b_phi*b_cottheta-a_pz*a_lambda)*(-b_Rt*b_cottheta));
122
123 double d1da_lambda=((DxNoVar-b_Rt*b_sinphi-a_px*a_lambda)*(-a_px)+
124 (DyNoVar+b_Rt*b_cosphi-a_py*a_lambda)*(-a_py)+
125 (DzNoVar-b_Rt*b_cottheta*b_phi-a_pz*a_lambda)*(-a_pz));
126
127
128
129 double d2db_phi2=((DxNoVar-a_px*a_lambda)*(b_Rt*b_sinphi)+
130 (DyNoVar-a_py*a_lambda)*(-b_Rt*b_cosphi)+
131 (b_Rt*b_cottheta*b_Rt*b_cottheta));
132
133 double d2dadb_lambdaphi=a_px*b_Rt*b_cosphi+a_py*b_Rt*b_sinphi+a_pz*b_Rt*b_cottheta;
134
135
136
137
138
139
140
141 double d2da_lambda2=((a_px*a_px)+
142 (a_py*a_py)+
143 (a_pz*a_pz));
144
145
146 ATH_MSG_VERBOSE (
" d1db_phi: " << d1db_phi <<
" d1da_lambda " << d1da_lambda);
147
148 ATH_MSG_VERBOSE (
" d2phi2 " << d2db_phi2 <<
" d2lambda2 " <<d2da_lambda2 <<
149 " d2lambdaphi " << d2dadb_lambdaphi);
150
151
152 double det=d2da_lambda2*d2db_phi2-d2dadb_lambdaphi*d2dadb_lambdaphi;
153
154 if (det<0) {
155 if (!secondTimeSaddleProblem)
156 {
157 secondTimeSaddleProblem=true;
158
161 double firstTerm=(DxNoVar*a_px+DyNoVar*a_py)*denominator;
162 double toSquare=(a_px*DyNoVar-a_py*DxNoVar);
163 double toSqrt=b_Rt*b_Rt/
denominator-toSquare*toSquare;
164 if (toSqrt<0)
165 {
166 ATH_MSG_WARNING (
"No intersection between neutral and charged track can be found. SKipping...");
167 continue;
168 }
169
171 double lambda1=firstTerm+secondTerm;
172 double lambda2=firstTerm-secondTerm;
173
174 ATH_MSG_VERBOSE (
"first solution: " << lambda1 <<
" second solution " << lambda2);
175
176 double phi1=TMath::ATan2((DxNoVar-a_px*lambda1)/b_Rt,-(DyNoVar-a_py*lambda1)/b_Rt);
177 double phi2=TMath::ATan2((DxNoVar-a_px*lambda2)/b_Rt,-(DyNoVar-a_py*lambda2)/b_Rt);
178
179 double x1case1=b_x0+b_Rt*(
sin(b_phi0)-
sin(phi1));
180 double y1case1=b_y0+b_Rt*(
cos(phi1)-
cos(b_phi0));
181 double z1case1=b_z0+b_Rt*b_cottheta*(b_phi0-phi1);
182
183 double x2case1=a_x0+a_px*lambda1;
184 double y2case1=a_y0+a_py*lambda1;
185 double z2case1=a_z0+a_pz*lambda1;
186
187 double x1case2=b_x0+b_Rt*(
sin(b_phi0)-
sin(phi2));
188 double y1case2=b_y0+b_Rt*(
cos(phi2)-
cos(b_phi0));
189 double z1case2=b_z0+b_Rt*b_cottheta*(b_phi0-phi2);
190
191 double x2case2=a_x0+a_px*lambda2;
192 double y2case2=a_y0+a_py*lambda2;
193 double z2case2=a_z0+a_pz*lambda2;
194
195 ATH_MSG_VERBOSE (
"solution1 x1: " << x1case1 <<
" x2: " << x2case1 <<
" y1: " << y1case1 <<
" y2: " << y2case1);
196 ATH_MSG_VERBOSE (
"solution2 x1: " << x1case2 <<
" x2: " << x2case2 <<
" y1: " << y1case2 <<
" y2: " << y2case2);
197
198 double deltaz1=fabs(z2case1-z1case1);
199 double deltaz2=fabs(z2case2-z1case2);
200
202
203 if (deltaz1<=deltaz2)
204 {
205 b_phi=phi1;
206 a_lambda=lambda1;
207 }
208 else
209 {
210 b_phi=phi2;
211 a_lambda=lambda2;
212 }
213
216 continue;
217 }
218
219
221 throw Error::NewtonProblem("Hessian is negative");
222
223 }
224 if (det>0&&d2da_lambda2<0) {
225 ATH_MSG_WARNING(
"Hessian indicates a maximum: derivative will be zero but result incorrect");
226 throw Error::NewtonProblem("Maximum point found");
227 }
228
229
230 deltaa_lambda=-(d2db_phi2*d1da_lambda-d2dadb_lambdaphi*d1db_phi)/det;
231 deltab_phi=-(-d2dadb_lambdaphi*d1da_lambda+d2da_lambda2*d1db_phi)/det;
232
233 a_lambda+=deltaa_lambda;
234 b_phi+=deltab_phi;
235
236
239
240 if (sqrt(std::pow(deltaa_lambda,2)+std::pow(deltab_phi,2))<
m_precision ||
242
243 }
244
245 if (loopsnumber>
m_maxloopnumber)
throw Error::NewtonProblem(
"Could not find minimum distance: max loops number reached");
246
247 double x1fin=b_x0+b_Rt*(
sin(b_phi0)-
sin(b_phi));
248 double y1fin=b_y0+b_Rt*(
cos(b_phi)-
cos(b_phi0));
249 double z1fin=b_z0+b_Rt*b_cottheta*(b_phi0-b_phi);
250
251 double x2fin=a_x0+a_px*a_lambda;
252 double y2fin=a_y0+a_py*a_lambda;
253 double z2fin=a_z0+a_pz*a_lambda;
254
255 ATH_MSG_VERBOSE (
"x1fin " << x1fin <<
"y1fin " << y1fin <<
"z1fin " << z1fin);
256 ATH_MSG_VERBOSE (
"x2fin " << x2fin <<
"y2fin " << y2fin <<
"z2fin " << z2fin);
257
258 double distance = sqrt((x1fin-x2fin)*(x1fin-x2fin)+
259 (y1fin-y2fin)*(y1fin-y2fin)+
260 (z1fin-z2fin)*(z1fin-z2fin));
261
263 << "fitted dist from primary vertex " <<
264 a_lambda/fabs(a_lambda)*sqrt(std::pow(a_px*a_lambda,2)+
265 std::pow(a_py*a_lambda,2)+std::pow(a_pz*a_lambda,2)) );
266
267 distanceOnAxis=a_lambda/fabs(a_lambda)*sqrt(std::pow(a_px*a_lambda,2)+
268 std::pow(a_py*a_lambda,2)+
269 std::pow(a_pz*a_lambda,2));
270
273
274 return std::pair<Amg::Vector3D,double>(r2,distance);
275
276}
#define ATH_MSG_VERBOSE(x)
#define ATH_MSG_WARNING(x)
void getField(const double *ATH_RESTRICT xyz, double *ATH_RESTRICT bxyz, double *ATH_RESTRICT deriv=nullptr)
get B field value at given position xyz[3] is in mm, bxyz[3] is in kT if deriv[9] is given,...
const Amg::Vector3D & momentum() const
const Amg::Vector3D & position() const
virtual const S & associatedSurface() const override final
Access to the Surface method.
const Amg::Vector3D & center() const
Returns the center position of the Surface.
float distance(const Amg::Vector3D &p1, const Amg::Vector3D &p2)
calculates the distance between two point in 3D space
Eigen::Matrix< double, 3, 1 > Vector3D