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
82 MagField::AtlasFieldCache fieldCache;
84
85 double magnFieldVect[3];
86 double posXYZ[3];
87 posXYZ[0] = firsttrack.getPerigee().associatedSurface().center().x();
88 posXYZ[1] = firsttrack.getPerigee().associatedSurface().center().y();
89 posXYZ[2] = firsttrack.getPerigee().associatedSurface().center().z();
90 fieldCache.
getField(posXYZ,magnFieldVect);
91
92
93
94 const double a_Bz=magnFieldVect[2]*299.792;
95
96
97 const double a_Rt = getRadiusOfCurvature(firsttrack.getPerigee(),a_Bz);
98 const double a_cotantheta = 1./
tan(firsttrack.getPerigee().parameters()[
Trk::theta]);
99
100#ifdef TrkDistance_DEBUG
101 ATH_MSG_DEBUG(
"a_Rt" << a_Rt <<
" a_cotantheta " << a_cotantheta );
102 ATH_MSG_DEBUG(
"Magnetic field at perigee is " << a_Bz <<
"GeV/mm " );
103#endif
104
105
106 const double b_phi0 = secondtrack.getPerigee().parameters()[
Trk::phi0];
107 const double b_cosphi0 = -
sin(b_phi0);
108 const double b_sinphi0 =
cos(b_phi0);
109
110
111 const double b_x0=secondtrack.getPerigee().associatedSurface().center().x() +
112 secondtrack.getPerigee().parameters()[
Trk::d0]*b_cosphi0;
113 const double b_y0=secondtrack.getPerigee().associatedSurface().center().y() +
114 secondtrack.getPerigee().parameters()[
Trk::d0]*b_sinphi0;
115 const double b_z0=secondtrack.getPerigee().associatedSurface().center().z() +
116 secondtrack.getPerigee().parameters()[
Trk::z0];
117
118#ifdef TrkDistance_DEBUG
119 ATH_MSG_DEBUG(
"b_x0 " << b_x0 <<
" b_y0 " << b_y0 <<
" b_z0 " << b_z0 );
121#endif
122
123
124 posXYZ[0] = secondtrack.getPerigee().associatedSurface().center().x();
125 posXYZ[1] = secondtrack.getPerigee().associatedSurface().center().y();
126 posXYZ[2] = secondtrack.getPerigee().associatedSurface().center().z();
127 fieldCache.
getField(posXYZ,magnFieldVect);
128
129
130 const double b_Bz = magnFieldVect[2]*299.792;
131
132
133
134 const double b_Rt = getRadiusOfCurvature(secondtrack.getPerigee(),b_Bz);
135 const double b_cotantheta = 1./
tan(secondtrack.getPerigee().parameters()[
Trk::theta]);
136
137#ifdef TrkDistance_DEBUG
138 ATH_MSG_DEBUG(
"b_Rt" << b_Rt <<
" b_cotantheta " << b_cotantheta );
139 ATH_MSG_DEBUG(
"Magnetic field at perigee is " << b_Bz <<
" GeV/mm " );
140#endif
141
142
143
144 const double ab_Dx0 = a_x0-b_x0-a_Rt*a_cosphi0+b_Rt*b_cosphi0;
145 const double ab_Dy0 = a_y0-b_y0-a_Rt*a_sinphi0+b_Rt*b_sinphi0;
146 const double ab_Dz0 = a_z0-b_z0+a_Rt*a_cotantheta*a_phi0-b_Rt*b_cotantheta*b_phi0;
147
148#ifdef TrkDistance_DEBUG
149 ATH_MSG_DEBUG(
"ab_Dx0 " << ab_Dx0 <<
" ab_Dy0 " << ab_Dy0 <<
" ab_Dz0 " << ab_Dz0 );
150#endif
151
152
153
154
155
156
157 double a_phi = firsttrack.getPhiPoint();
158 double b_phi = secondtrack.getPhiPoint();
159
160
161 double a_cosphi = -
sin(a_phi);
162 double a_sinphi =
cos(a_phi);
163 double b_cosphi = -
sin(b_phi);
164 double b_sinphi =
cos(b_phi);
165
166
167#ifdef TrkDistance_DEBUG
168 ATH_MSG_DEBUG(
"Beginning phi is a_phi: " << a_phi <<
" b_phi " << b_phi );
170#endif
171
172
173 int loopsnumber = 0;
174
176
177 while (!isok) {
178
179#ifdef TrkDistance_DEBUG
181 ATH_MSG_DEBUG(
"actual value of a_phi: " << a_phi <<
" of b_phi " << b_phi );
182#endif
183
184
185
186 ++loopsnumber;
187
188
189#ifdef TrkDistance_DEBUG
196
198 ATH_MSG_DEBUG(
"real Dx0 " << ab_Dx0+a_Rt*a_cosphi-b_Rt*b_cosphi );
199 ATH_MSG_DEBUG(
"real Dy0 " << ab_Dy0+a_Rt*a_sinphi-b_Rt*b_sinphi );
200#endif
201
202
203 const double d1da_phi =
204 (ab_Dx0-b_Rt*b_cosphi)*(-a_Rt*a_sinphi)+
205 (ab_Dy0-b_Rt*b_sinphi)*a_cosphi*a_Rt+
206 (ab_Dz0-a_Rt*a_cotantheta*a_phi+b_Rt*b_cotantheta*b_phi)*(-a_Rt*a_cotantheta);
207
208
209
210 const double d1db_phi =
211 (ab_Dx0+a_Rt*a_cosphi)*b_Rt*b_sinphi-
212 (ab_Dy0+a_Rt*a_sinphi)*b_cosphi*b_Rt+
213 (ab_Dz0-a_Rt*a_cotantheta*a_phi+b_Rt*b_cotantheta*b_phi)*(+b_Rt*b_cotantheta);
214
215
216
217 const double d2da_phi2 =
218 (ab_Dx0-b_Rt*b_cosphi)*(-a_Rt*a_cosphi)+
219 (ab_Dy0-b_Rt*b_sinphi)*(-a_Rt*a_sinphi)+
220 +a_Rt*a_Rt*(a_cotantheta*a_cotantheta);
221
222 const double d2db_phi2 =
223 (ab_Dx0+a_Rt*a_cosphi)*(+b_Rt*b_cosphi)+
224 (ab_Dy0+a_Rt*a_sinphi)*(+b_Rt*b_sinphi)+
225 +b_Rt*b_Rt*(b_cotantheta*b_cotantheta);
226
227
228 const double d2da_phib_phi = -a_Rt*b_Rt*(a_sinphi*b_sinphi+a_cosphi*b_cosphi+a_cotantheta*b_cotantheta);
229
230
231
232 const double det = d2da_phi2*d2db_phi2-d2da_phib_phi*d2da_phib_phi;
233
234
235#ifdef TrkDistance_DEBUG
236 ATH_MSG_DEBUG(
"d1da_phi " << d1da_phi <<
" d1db_phi " << d1db_phi <<
" d2da_phi2 " << d2da_phi2 <<
" d2db_phi2 " << d2db_phi2
237 << " d2da_phib_phi " << d2da_phib_phi << " det " << det );
238#endif
239
240
241
242 if (det<0) {
244 return "Hessian is negative";
245 }
246 if (det>0&&d2da_phi2<0) {
247 ATH_MSG_DEBUG(
"Hessian indicates a maximum: derivative will be zero but result incorrect" );
248 return "Maximum point found";
249 }
250
251
252 const double deltaa_phi = -(d2db_phi2*d1da_phi-d2da_phib_phi*d1db_phi)/det;
253 const double deltab_phi = -(-d2da_phib_phi*d1da_phi+d2da_phi2*d1db_phi)/det;
254
255#ifdef TrkDistance_DEBUG
258#endif
259
260
261 a_phi += deltaa_phi;
262 b_phi += deltab_phi;
263
264
265 a_cosphi = -
sin(a_phi);
266 a_sinphi =
cos(a_phi);
267 b_cosphi = -
sin(b_phi);
268 b_sinphi =
cos(b_phi);
269
270 if (std::sqrt(std::pow(deltaa_phi,2)+std::pow(deltab_phi,2))<
m_precision ||
272
273 }
274
276 return "Could not find minimum distance: max loops number reached";
277 }
278
279
281 a_y0+a_Rt*(a_sinphi-a_sinphi0),
282 a_z0-a_Rt*(a_phi-a_phi0)*a_cotantheta),
284 b_y0+b_Rt*(b_sinphi-b_sinphi0),
285 b_z0-b_Rt*(b_phi-b_phi0)*b_cotantheta));
286}
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