21 static const InterfaceID IID_IVertexPointEstimator(
"InDet::VertexPointEstimator", 1, 0);
28 declareInterface<VertexPointEstimator>(
this);
33 return IID_IVertexPointEstimator;
38 return StatusCode::SUCCESS;
43 return StatusCode::SUCCESS;
69 decors[
"deltaPhiTracks"],
76 return {
"deltaPhiTracks",
"DR1R2"};
102 double minArcLength(0.);
103 double maxArcLength(0.);
120 double d0[2],
z0[2],phi[2],
cotTheta[2],qOverPt[2],RC[2],XC[2],YC[2],RA[2],AB[2];
121 double XSVI[2],YSVI[2],ZSVI[2],RVI[2],DZSVI[2],SS1[2],SS2[2],ZZ1[2],ZZ2[2];
123 double X=0.,
Y=0.,Z=0.;
124 for (
int I=0;
I<2; ++
I) {
139 DZSVI[
I] = 2.0*DZMAX;
147 const AmgVector(5)& perParam1 = per1->parameters();
149 const AmgVector(5)& perParam2 = per2->parameters();
159 RC[0] = 10.*0.5/(qOverPt[0]*aconst); RC[1] = 10.*0.5/(qOverPt[1]*aconst);
161 XC[0] = -1*U*
sin(phi[0]);
162 YC[0] = U*
cos(phi[0]);
165 XC[1] = -1*U*
sin(phi[1]);
166 YC[1] = U*
cos(phi[1]);
176 RC[0] = 10.*0.5/(qOverPt[0]*aconst); RC[1] = 10.*0.5/(qOverPt[1]*aconst);
177 double U = RC[0] +
d0[0];
178 XC[0] = -1*U*
sin(phi[0]);
179 YC[0] = U*
cos(phi[0]);
182 XC[1] = -1*U*
sin(phi[1]);
183 YC[1] = U*
cos(phi[1]);
188 double DX = XC[1] - XC[0];
189 double DY = YC[1] - YC[0];
190 double D = DX*DX + DY*DY;
193 ATH_MSG_DEBUG(
"Concentric circles, should not happen return (0,0,0)");
197 U = D - RA[0] - RA[1];
202 double hl =
areaVar(XC[0], YC[0], RA[0], XC[1], YC[1], RA[1],
PHI);
206 double Y0 = (-1*XC[0]*YC[1] + XC[1]*YC[0])/D;
207 for (
int I=0;
I<2; ++
I) {
208 AB[
I] = COST*XC[
I] + SINT*YC[
I];
210 U = (XC[1] + XC[0])*(XC[1] - XC[0]) + (YC[1] + YC[0])*(YC[1] - YC[0]);
211 double V = (RA[1] + RA[0])*(RA[1] - RA[0]);
212 double XX = 0.5*(U - V)/D;
213 if ((XX - AB[0])*(XX - AB[0]) > 0.) {
214 U = sqrt((XX - AB[0])*(XX - AB[0]));
216 double YY2 = (RA[0] + U)*(RA[0] - U);
224 for (
int I=0;
I<2; ++
I) {
226 XSVI[
I] = COST*XX - SINT*U;
227 YSVI[
I] = SINT*XX + COST*U;
236 U = D - RA[0] - RA[1];
242 if (AB[0] < AB[1]) XX = AB[0] + RA[0];
247 V = AB[0] - RA[0] - XX;
251 V = AB[1] + RA[1] - XX;
255 XSVI[0] = COST*XX - SINT*Y0;
256 YSVI[0] = SINT*XX + COST*Y0;
260 ATH_MSG_DEBUG(
"XY distance of minimum approach is too large, return (0,0,0)");
266 for (
int J=0; J<nsol; ++J) {
267 U = (XSVI[J] - XC[0])/RC[0];
268 V = -1*(YSVI[J] - YC[0])/RC[0];
269 U = atan2(U,V) - phi[0];
274 U = (XSVI[J] - XC[1])/RC[1];
275 V = -1*(YSVI[J] - YC[1])/RC[1];
276 U = atan2(U,V) - phi[1];
281 RVI[J] = sqrt(XSVI[J]*XSVI[J] + YSVI[J]*YSVI[J]);
282 DZSVI[J] = ZZ2[J] - ZZ1[J];
284 ZSVI[0] = 0.5*(ZZ1[0] + ZZ2[0]);
285 ZSVI[1] = 0.5*(ZZ1[1] + ZZ2[1]);
287 if (
std::min(RVI[0],RVI[1]) > RVMAX) {
292 if (
std::min(fabs(DZSVI[0]),fabs(DZSVI[1])) > DZMAX) {
306 if ( nsol == 2 && (fabs(DZSVI[1]) < fabs(DZSVI[0])) ) J = 1;
311 intPoint(0)=
X; intPoint(1)=
Y; intPoint(2)=Z;
338 double ret = -999999;
339 double xi1, yi1, xi2, yi2;
342 double h = 0.5*(sqrt(
pow(xi1-xi2,2) +
pow(yi1-yi2,2) ));
343 double l = sqrt(
pow(xc1-xc2,2) +
pow(yc1-yc2,2) );
346 double norm1 = sqrt((xi1-xc1)*(xi1-xc1)+(yi1-yc1)*(yi1-yc1));
347 double norm2 = sqrt((xi1-xc2)*(xi1-xc2)+(yi1-yc2)*(yi1-yc2));
348 if((norm1!=0.) && (norm2!=0.)){
352 double xa = (xi1-xc1)*norm1;
353 double ya = (yi1-yc1)*norm1;
354 double xb = (xi1-xc2)*norm2;
355 double yb = (yi1-yc2)*norm2;
356 double costheta =
xa*
xb + ya*yb;
357 phi =
M_PI-std::acos(costheta);
366 if (
x < -1.)
return M_PI;
367 if (
x > 1.)
return 0;
375 double ret = -999999;
376 double xi1, yi1, xi2, yi2;
385 h = 0.5*(sqrt(
pow(xi1-xi2,2) +
pow(yi1-yi2,2) ));
387 double l = sqrt(
pow(xc1-xc2,2) +
pow(yc1-yc2,2) );
395 double norm1 = sqrt(
pow(xi1-xc1,2)+
pow(yi1-yc1,2));
396 double norm2 = sqrt(
pow(xi1-xc2,2)+
pow(yi1-yc2,2));
397 if ((norm1 != 0) && (norm2 != 0)){
399 double xa = (xi1 - xc1)/norm1;
400 double ya = (yi1 - yc1)/norm1;
401 double xb = (xi1 - xc2)/norm2;
402 double yb = (yi1 - yc2)/norm2;
403 double costheta =
xa*
xb +ya*yb;
414 double xc2,
double yc2,
double r2,
415 double& xi1,
double& yi1,
416 double& xi2,
double& yi2)
429 double A = (xc1 - xc2) / (yc2- yc1);
430 double B = (r1*r1 - r2*r2 - xc1*xc1 + xc2*xc2 - yc1*yc1 + yc2*yc2) / 2. / ( yc2 -yc1);
432 double b = 2*
A*B - 2*xc1 -2*
A*yc1;
433 double c = B*B - 2*B*yc1 + xc1*xc1 + yc1*yc1 - r1*r1;
442 double A = (yc1 - yc2) / (xc2- xc1);
443 double B = (r1*r1 - r2*r2 - xc1*xc1 + xc2*xc2 - yc1*yc1 + yc2*yc2) / 2. / ( xc2 -xc1);
445 double b = 2*
A*B - 2*yc1 -2*
A*xc1;
446 double c = B*B - 2*B*xc1 + xc1*xc1 + yc1*yc1 - r1*r1;
466 double discr =
b*
b - 4*
a*
c;
467 if (discr < 0)
return false;
468 y1 = (-
b+sqrt(discr))/2./
a;
469 y2 = (-
b-sqrt(discr))/2./
a;
477 double g,
double h) {