59{
60
61 const double a_phi0 = firsttrack.getPerigee().parameters()[
Trk::phi0];
62 const double a_cosphi0 = -
sin(a_phi0);
63 const double a_sinphi0 =
cos(a_phi0);
64
65
66 const double a_x0=firsttrack.getPerigee().associatedSurface().center().x() +
67 firsttrack.getPerigee().parameters()[
Trk::d0]*a_cosphi0;
68 const double a_y0=firsttrack.getPerigee().associatedSurface().center().y() +
69 firsttrack.getPerigee().parameters()[
Trk::d0]*a_sinphi0;
70 const double a_z0=firsttrack.getPerigee().associatedSurface().center().z() +
71 firsttrack.getPerigee().parameters()[
Trk::z0];
72
73#ifdef TrkDistance_DEBUG
74 ATH_MSG_DEBUG(
"a_x0 " << a_x0 <<
" a_y0 " << a_y0 <<
" a_z0 " << a_z0 );
76#endif
77
78
80 const AtlasFieldCacheCondObj* fieldCondObj{*readHandle};
81 if (!fieldCondObj){
83 return "fieldCondObj is nullptr";
84 }
85 MagField::AtlasFieldCache fieldCache;
87
88 double magnFieldVect[3];
89 double posXYZ[3];
90 posXYZ[0] = firsttrack.getPerigee().associatedSurface().center().x();
91 posXYZ[1] = firsttrack.getPerigee().associatedSurface().center().y();
92 posXYZ[2] = firsttrack.getPerigee().associatedSurface().center().z();
93 fieldCache.
getField(posXYZ,magnFieldVect);
94
95
96
97 const double a_Bz=magnFieldVect[2]*299.792;
98
99
100 const double a_Rt = getRadiusOfCurvature(firsttrack.getPerigee(),a_Bz);
101 const double a_cotantheta = 1./
tan(firsttrack.getPerigee().parameters()[
Trk::theta]);
102
103#ifdef TrkDistance_DEBUG
104 ATH_MSG_DEBUG(
"a_Rt" << a_Rt <<
" a_cotantheta " << a_cotantheta );
105 ATH_MSG_DEBUG(
"Magnetic field at perigee is " << a_Bz <<
"GeV/mm " );
106#endif
107
108
109 const double b_phi0 = secondtrack.getPerigee().parameters()[
Trk::phi0];
110 const double b_cosphi0 = -
sin(b_phi0);
111 const double b_sinphi0 =
cos(b_phi0);
112
113
114 const double b_x0=secondtrack.getPerigee().associatedSurface().center().x() +
115 secondtrack.getPerigee().parameters()[
Trk::d0]*b_cosphi0;
116 const double b_y0=secondtrack.getPerigee().associatedSurface().center().y() +
117 secondtrack.getPerigee().parameters()[
Trk::d0]*b_sinphi0;
118 const double b_z0=secondtrack.getPerigee().associatedSurface().center().z() +
119 secondtrack.getPerigee().parameters()[
Trk::z0];
120
121#ifdef TrkDistance_DEBUG
122 ATH_MSG_DEBUG(
"b_x0 " << b_x0 <<
" b_y0 " << b_y0 <<
" b_z0 " << b_z0 );
124#endif
125
126
127 posXYZ[0] = secondtrack.getPerigee().associatedSurface().center().x();
128 posXYZ[1] = secondtrack.getPerigee().associatedSurface().center().y();
129 posXYZ[2] = secondtrack.getPerigee().associatedSurface().center().z();
130 fieldCache.
getField(posXYZ,magnFieldVect);
131
132
133 const double b_Bz = magnFieldVect[2]*299.792;
134
135
136
137 const double b_Rt = getRadiusOfCurvature(secondtrack.getPerigee(),b_Bz);
138 const double b_cotantheta = 1./
tan(secondtrack.getPerigee().parameters()[
Trk::theta]);
139
140#ifdef TrkDistance_DEBUG
141 ATH_MSG_DEBUG(
"b_Rt" << b_Rt <<
" b_cotantheta " << b_cotantheta );
142 ATH_MSG_DEBUG(
"Magnetic field at perigee is " << b_Bz <<
" GeV/mm " );
143#endif
144
145
146
147 const double ab_Dx0 = a_x0-b_x0-a_Rt*a_cosphi0+b_Rt*b_cosphi0;
148 const double ab_Dy0 = a_y0-b_y0-a_Rt*a_sinphi0+b_Rt*b_sinphi0;
149 const double ab_Dz0 = a_z0-b_z0+a_Rt*a_cotantheta*a_phi0-b_Rt*b_cotantheta*b_phi0;
150
151#ifdef TrkDistance_DEBUG
152 ATH_MSG_DEBUG(
"ab_Dx0 " << ab_Dx0 <<
" ab_Dy0 " << ab_Dy0 <<
" ab_Dz0 " << ab_Dz0 );
153#endif
154
155
156
157
158
159
160 double a_phi = firsttrack.getPhiPoint();
161 double b_phi = secondtrack.getPhiPoint();
162
163
164 double a_cosphi = -
sin(a_phi);
165 double a_sinphi =
cos(a_phi);
166 double b_cosphi = -
sin(b_phi);
167 double b_sinphi =
cos(b_phi);
168
169
170#ifdef TrkDistance_DEBUG
171 ATH_MSG_DEBUG(
"Beginning phi is a_phi: " << a_phi <<
" b_phi " << b_phi );
173#endif
174
175
176 int loopsnumber = 0;
177
179
180 while (!isok) {
181
182#ifdef TrkDistance_DEBUG
184 ATH_MSG_DEBUG(
"actual value of a_phi: " << a_phi <<
" of b_phi " << b_phi );
185#endif
186
187
188
189 ++loopsnumber;
190
191
192#ifdef TrkDistance_DEBUG
199
201 ATH_MSG_DEBUG(
"real Dx0 " << ab_Dx0+a_Rt*a_cosphi-b_Rt*b_cosphi );
202 ATH_MSG_DEBUG(
"real Dy0 " << ab_Dy0+a_Rt*a_sinphi-b_Rt*b_sinphi );
203#endif
204
205
206 const double d1da_phi =
207 (ab_Dx0-b_Rt*b_cosphi)*(-a_Rt*a_sinphi)+
208 (ab_Dy0-b_Rt*b_sinphi)*a_cosphi*a_Rt+
209 (ab_Dz0-a_Rt*a_cotantheta*a_phi+b_Rt*b_cotantheta*b_phi)*(-a_Rt*a_cotantheta);
210
211
212
213 const double d1db_phi =
214 (ab_Dx0+a_Rt*a_cosphi)*b_Rt*b_sinphi-
215 (ab_Dy0+a_Rt*a_sinphi)*b_cosphi*b_Rt+
216 (ab_Dz0-a_Rt*a_cotantheta*a_phi+b_Rt*b_cotantheta*b_phi)*(+b_Rt*b_cotantheta);
217
218
219
220 const double d2da_phi2 =
221 (ab_Dx0-b_Rt*b_cosphi)*(-a_Rt*a_cosphi)+
222 (ab_Dy0-b_Rt*b_sinphi)*(-a_Rt*a_sinphi)+
223 +a_Rt*a_Rt*(a_cotantheta*a_cotantheta);
224
225 const double d2db_phi2 =
226 (ab_Dx0+a_Rt*a_cosphi)*(+b_Rt*b_cosphi)+
227 (ab_Dy0+a_Rt*a_sinphi)*(+b_Rt*b_sinphi)+
228 +b_Rt*b_Rt*(b_cotantheta*b_cotantheta);
229
230
231 const double d2da_phib_phi = -a_Rt*b_Rt*(a_sinphi*b_sinphi+a_cosphi*b_cosphi+a_cotantheta*b_cotantheta);
232
233
234
235 const double det = d2da_phi2*d2db_phi2-d2da_phib_phi*d2da_phib_phi;
236
237
238#ifdef TrkDistance_DEBUG
239 ATH_MSG_DEBUG(
"d1da_phi " << d1da_phi <<
" d1db_phi " << d1db_phi <<
" d2da_phi2 " << d2da_phi2 <<
" d2db_phi2 " << d2db_phi2
240 << " d2da_phib_phi " << d2da_phib_phi << " det " << det );
241#endif
242
243
244
245 if (det<0) {
247 return "Hessian is negative";
248 }
249 if (det>0&&d2da_phi2<0) {
250 ATH_MSG_DEBUG(
"Hessian indicates a maximum: derivative will be zero but result incorrect" );
251 return "Maximum point found";
252 }
255 return "Hessian is zero";
256 }
257
258 const double deltaa_phi = -(d2db_phi2*d1da_phi-d2da_phib_phi*d1db_phi)/det;
259 const double deltab_phi = -(-d2da_phib_phi*d1da_phi+d2da_phi2*d1db_phi)/det;
260
261#ifdef TrkDistance_DEBUG
264#endif
265
266
267 a_phi += deltaa_phi;
268 b_phi += deltab_phi;
269
270
271 a_cosphi = -
sin(a_phi);
272 a_sinphi =
cos(a_phi);
273 b_cosphi = -
sin(b_phi);
274 b_sinphi =
cos(b_phi);
275
276 if (std::sqrt(std::pow(deltaa_phi,2)+std::pow(deltab_phi,2))<
m_precision ||
278
279 }
280
282 return "Could not find minimum distance: max loops number reached";
283 }
284
285
287 a_y0+a_Rt*(a_sinphi-a_sinphi0),
288 a_z0-a_Rt*(a_phi-a_phi0)*a_cotantheta),
290 b_y0+b_Rt*(b_sinphi-b_sinphi0),
291 b_z0-b_Rt*(b_phi-b_phi0)*b_cotantheta));
292}
void getInitializedCache(MagField::AtlasFieldCache &cache) const
get B field cache for evaluation as a function of 2-d or 3-d position.
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,...
SG::ReadCondHandleKey< AtlasFieldCacheCondObj > m_fieldCacheCondObjInputKey
Eigen::Matrix< double, 3, 1 > Vector3D
std::pair< Amg::Vector3D, Amg::Vector3D > TwoPoints