12 #include "GaudiKernel/MsgStream.h"
24 bool iterate(
double x,
double y,
double c0x,
double c0y,
double c2x,
double c2y,
double rr)
const
30 int numNodes = 4 <<
t;
33 double c1x = (c0x + c2x) * innerPolygonCoef[
t];
34 double c1y = (c0y + c2y) * innerPolygonCoef[
t];
37 if (
tx *
tx + ty * ty <=
rr) {
40 double t2x = c2x - c1x;
41 double t2y = c2y - c1y;
42 if (
tx * t2x + ty * t2y >= 0 &&
tx * t2x + ty * t2y <= t2x * t2x + t2y * t2y &&
43 (ty * t2x -
tx * t2y >= 0 ||
rr * (t2x * t2x + t2y * t2y) >= (ty * t2x -
tx * t2y) * (ty * t2x -
tx * t2y))) {
46 double t0x = c0x - c1x;
47 double t0y = c0y - c1y;
48 if (
tx * t0x + ty * t0y >= 0 &&
tx * t0x + ty * t0y <= t0x * t0x + t0y * t0y &&
49 (ty * t0x -
tx * t0y <= 0 ||
rr * (t0x * t0x + t0y * t0y) >= (ty * t0x -
tx * t0y) * (ty * t0x -
tx * t0y))) {
53 double c3x = (c0x + c1x) * outerPolygonCoef[
t];
54 double c3y = (c0y + c1y) * outerPolygonCoef[
t];
55 if ((c3x -
x) * (c3x -
x) + (c3y -
y) * (c3y -
y) <
rr) {
60 double c4x = c1x - c3x + c1x;
61 double c4y = c1y - c3y + c1y;
62 if ((c4x -
x) * (c4x -
x) + (c4y -
y) * (c4y -
y) <
rr) {
67 double t3x = c3x - c1x;
68 double t3y = c3y - c1y;
69 if (ty * t3x -
tx * t3y <= 0 ||
rr * (t3x * t3x + t3y * t3y) > (ty * t3x -
tx * t3y) * (ty * t3x -
tx * t3y)) {
70 if (
tx * t3x + ty * t3y > 0) {
71 if (std::abs(
tx * t3x + ty * t3y) <= t3x * t3x + t3y * t3y ||
72 (
x - c3x) * (c0x - c3x) + (
y - c3y) * (c0y - c3y) >= 0) {
77 }
else if (-(
tx * t3x + ty * t3y) <= t3x * t3x + t3y * t3y ||
78 (
x - c4x) * (c2x - c4x) + (
y - c4y) * (c2y - c4y) >= 0) {
92 bool collide(
double x0,
double y0,
double w,
double h,
double x1,
double y1,
double r)
const
94 double x = std::abs(
x1 - x0);
95 double y = std::abs(
y1 - y0);
100 if (
x *
x + (
h -
y) * (
h -
y) <=
r *
r || (
w -
x) * (
w -
x) +
y *
y <=
r *
r ||
103 x *
w -
y *
h <=
w *
w)) {
106 if ((
x -
w) * (
x -
w) + (
y -
h) * (
y -
h) <=
r *
r || (
x <=
w &&
y -
r <=
h) || (
y <=
h &&
x -
r <=
w)) {
113 double localCos =
x /
R;
114 double deltaR = std::sqrt(
h *
h + (
w *
w -
h *
h) * localCos * localCos);
174 std::vector<TDD_real_t> XX;
180 std::vector<TDD_real_t> YY;
187 m_maxXout = *std::max_element(XX.begin(), XX.end());
188 m_minXout = *std::min_element(XX.begin(), XX.end());
189 m_maxYout = *std::max_element(YY.begin(), YY.end());
190 m_minYout = *std::min_element(YY.begin(), YY.end());
215 if (localY > (m_maxYout + tol2) || localY < (m_minYout - tol2))
219 if (localX > (m_maxXout + tol1) || localX < (m_minXout - tol1))
222 if (localX > (m_minXin - tol1) && localX < (m_maxXin + tol1) && localY > (m_minYin - tol2) &&
223 localY < (m_maxYin + tol2))
229 double localR2 = localX * localX + localY * localY;
230 double localR = std::sqrt(localR2);
231 double localCos = localX / localR;
232 double localSin = localY / localR;
233 double deltaR = std::sqrt(tol2 * tol2 * localSin * localSin + tol1 * tol1 * localCos * localCos);
238 bool condRad = (localR < maxR + deltaR && localR > minR -
deltaR);
240 (isRight(locpo, tol1, tol2, m_solution_L_max[0], m_solution_L_max[1], m_solution_L_min[0], m_solution_L_min[1]));
242 (isLeft(locpo, tol1, tol2, m_solution_R_max[0], m_solution_R_max[1], m_solution_R_min[0], m_solution_R_min[1]));
244 return (condRad && condL && condR);
259 AmgMatrix(2, 2) lCovarianceCar = bchk.lCovariance;
262 double w = bchk.
nSigmas * std::sqrt(lCovarianceCar(0, 0));
263 double h = bchk.
nSigmas * std::sqrt(lCovarianceCar(1, 1));
269 if (localY > (m_maxYout + maxTol) || localY < (m_minYout - maxTol))
273 if (localX > (m_maxXout + maxTol) || localX < (m_minXout - maxTol))
276 if (localX > (m_minXin - minTol) && localX < (m_maxXin + minTol) && localY > (m_minYin - minTol) &&
277 localY < (m_maxYin + minTol))
282 float theta = (lCovarianceCar(1, 0) != 0 && (lCovarianceCar(1, 1) - lCovarianceCar(0, 0)) != 0)
283 ? .5 * bchk.
FastArcTan(2 * lCovarianceCar(1, 0) / (lCovarianceCar(1, 1) - lCovarianceCar(0, 0)))
287 rotMatrix << scResult.
cosC, scResult.
sinC,
290 double x1 =
tmp(0, 0);
291 double y1 =
tmp(1, 0);
295 bool condR = (
test.collide(x0, y0,
w,
h,
x1,
y1, -minR) &&
test.collide(x0, y0,
w,
h,
x1,
y1, maxR));
298 std::vector<KDOP> elementKDOP(4);
299 std::vector<Amg::Vector2D> elementP(4);
303 normal << 0, -1, 1, 0;
307 p << m_solution_L_min[0], m_solution_L_min[1];
308 elementP[0] = (rotMatrix * (
p - locpo));
310 p << m_solution_L_max[0], m_solution_L_max[1];
311 elementP[1] = (rotMatrix * (
p - locpo));
313 p << m_solution_R_max[0], m_solution_R_max[1];
314 elementP[2] = (rotMatrix * (
p - locpo));
316 p << m_solution_R_min[0], m_solution_R_min[1];
317 elementP[3] = (rotMatrix * (
p - locpo));
319 std::vector<Amg::Vector2D>
axis = { normal * (elementP[0] - elementP[1]),
320 normal * (elementP[1] - elementP[2]),
321 normal * (elementP[2] - elementP[3]),
322 normal * (elementP[3] - elementP[0]) };
325 std::vector<KDOP> errelipseKDOP(4);
328 bool condSide = bchk.
TestKDOPKDOP(elementKDOP, errelipseKDOP);
331 (isAbove(locpo, 0, 0, m_solution_L_max[0], m_solution_L_max[1], m_solution_R_max[0], m_solution_R_max[1]) &&
332 isRight(locpo, 0, 0, m_solution_L_max[0], m_solution_L_max[1], m_solution_L_min[0], m_solution_L_min[1]) &&
333 isLeft(locpo, 0, 0, m_solution_R_max[0], m_solution_R_max[1], m_solution_R_min[0], m_solution_R_min[1]));
339 return (condR && condSide);
356 double sign =
k > 0. ? -1. : +1.;
379 EllipseIntersectLine(locpo, tol1, tol2,
x1,
y1,
x2,
y2));
383 EllipseIntersectLine(locpo, tol1, tol2,
x1,
y1,
x2,
y2));
389 EllipseIntersectLine(locpo, tol1, tol2,
x1,
y1,
x2,
y2));
409 EllipseIntersectLine(locpo, tol1, tol2,
x1,
y1,
x2,
y2));
413 EllipseIntersectLine(locpo, tol1, tol2,
x1,
y1,
x2,
y2));
419 EllipseIntersectLine(locpo, tol1, tol2,
x1,
y1,
x2,
y2));
431 double distLine_L = distanceToLine(locpo, m_solution_L_min, m_solution_L_max);
432 double distLine_R = distanceToLine(locpo, m_solution_R_min, m_solution_R_max);
434 double dist =
std::min(distLine_L, distLine_R);
437 double distMin = distanceToArc(locpo, minR, m_solution_L_min, m_solution_R_min);
438 double distMax = distanceToArc(locpo, maxR, m_solution_L_max, m_solution_R_max);
440 double distArc =
std::min(distMin, distMax);
444 if (
inside(locpo, 0., 0.)){
465 std::vector<double> solution;
474 double delta = 4. *
k *
d *
k *
d - 4. * (1 +
k *
k) * (
d *
d - R * R);
481 x1 = (-2. *
k *
d - std::sqrt(delta)) / (2. * (1 +
k *
k));
482 x2 = (-2. *
k *
d + std::sqrt(delta)) / (2. * (1 +
k *
k));
489 solution.push_back(
x1);
490 solution.push_back(
y1);
492 solution.push_back(
x2);
493 solution.push_back(
y2);
501 const std::vector<TDD_real_t>& P1,
502 const std::vector<TDD_real_t>& P2)
509 double A = P2x - P1x;
510 double B = P2y - P1y;
517 double u = (
A * (
X - P1x) + B * (
Y - P1y)) / (
A *
A + B * B);
528 return std::sqrt((
X - P3x) * (
X - P3x) + (
Y - P3y) * (
Y - P3y));
535 const std::vector<TDD_real_t>& sL,
536 const std::vector<TDD_real_t>& sR)
542 double tanlocPhi =
X /
Y;
543 double tanPhi_L = sL[0] / sL[1];
544 double tanPhi_R = sR[0] / sR[1];
546 if (tanlocPhi > tanPhi_L && tanlocPhi < tanPhi_R){
547 return std::fabs(std::sqrt(
X *
X +
Y *
Y) - R);
573 if (
h == 0 &&
k == 0)
594 return (std::fabs(
c) <
k);
596 return (std::fabs(
c /
m) <
h);
599 s = 2 *
m *
c *
h *
h;
602 d =
s *
s - 4 *
r *
t;
609 d = std::fabs(
x1) -
h;
617 std::array<std::pair<double, double>, 4> corners;
619 corners[0]=std::make_pair(m_solution_R_max.at(0),m_solution_R_max.at(1));
620 corners[1]=std::make_pair(m_solution_R_min.at(0),m_solution_R_min.at(1));
621 corners[2]=std::make_pair(m_solution_L_min.at(0),m_solution_L_min.at(1));
622 corners[3]=std::make_pair(m_solution_L_max.at(0),m_solution_L_max.at(1));
630 sl << std::setiosflags(std::ios::fixed);
631 sl << std::setprecision(7);
632 sl <<
"Trk::AnnulusBounds: (minR, maxR, phi) = "
635 sl << std::setprecision(-1);
642 sl << std::setiosflags(std::ios::fixed);
643 sl << std::setprecision(7);
644 sl <<
"Trk::AnnulusBounds: (minR, maxR, phi) = "
647 sl << std::setprecision(-1);
660 std::array<TDD_real_t,4> returnArr{};