src/Surfaces/AnnulusBounds.cpp

0001 // This file is part of the ACTS project.
0002 //
0003 // Copyright (C) 2016 CERN for the benefit of the ACTS project
0004 //
0005 // This Source Code Form is subject to the terms of the Mozilla Public
0006 // License, v. 2.0. If a copy of the MPL was not distributed with this
0007 // file, You can obtain one at https://mozilla.org/MPL/2.0/.
0008
0009 #include "Acts/Surfaces/AnnulusBounds.hpp"
0010
0011 #include "Acts/Surfaces/BoundaryTolerance.hpp"
0012 #include "Acts/Surfaces/detail/VerticesHelper.hpp"
0013 #include "Acts/Utilities/VectorHelpers.hpp"
0014 #include "Acts/Utilities/detail/periodic.hpp"
0015
0016 #include <algorithm>
0017 #include <cmath>
0018 #include <iomanip>
0019 #include <iostream>
0020 #include <limits>
0021 #include <stdexcept>
0022
0023 namespace Acts {
0024
0025 namespace {
0026
0027 Vector2 closestOnSegment(const Vector2& a, const Vector2& b, const Vector2& p,
0028                          const SquareMatrix2& weight) {
0029   // connecting vector
0030   auto n = b - a;
0031   // squared norm of line
0032   auto f = (n.transpose() * weight * n).value();
0033   // weighted scalar product of line to point and segment line
0034   auto u = ((p - a).transpose() * weight * n).value() / f;
0035   // clamp to [0, 1], convert to point
0036   return std::clamp(u, 0., 1.) * n + a;
0037 }
0038
0039 double squaredNorm(const Vector2& v, const SquareMatrix2& weight) {
0040   return (v.transpose() * weight * v).value();
0041 }
0042
0043 }  // namespace
0044
0045 AnnulusBounds::AnnulusBounds(const std::array<double, eSize>& values) noexcept(
0046     false)
0047     : m_values(values), m_moduleOrigin({values[eOriginX], values[eOriginY]}) {
0048   checkConsistency();
0049   m_rotationStripPC = Translation2(Vector2(0, -get(eAveragePhi)));
0050   m_translation = Translation2(m_moduleOrigin);
0051
0052   m_shiftXY = m_moduleOrigin * -1;
0053   m_shiftPC =
0054       Vector2(VectorHelpers::perp(m_shiftXY), VectorHelpers::phi(m_shiftXY));
0055
0056   // we need the corner points of the module to do the inside
0057   // checking, calculate them here once, they don't change
0058
0059   // find inner outer radius at edges in STRIP PC
0060   auto circIx = [](double O_x, double O_y, double r, double phi) -> Vector2 {
0061     //                      _____________________________________________
0062     //                     /      2  2                    2    2  2    2
0063     //     O_x + O_y*m - \/  - O_x *m  + 2*O_x*O_y*m - O_y  + m *r  + r
0064     // x = --------------------------------------------------------------
0065     //                                  2
0066     //                                 m  + 1
0067     //
0068     // y = m*x
0069     //
0070     double m = std::tan(phi);
0071     Vector2 dir(std::cos(phi), std::sin(phi));
0072     double x1 = (O_x + O_y * m -
0073                  std::sqrt(-std::pow(O_x, 2) * std::pow(m, 2) +
0074                            2 * O_x * O_y * m - std::pow(O_y, 2) +
0075                            std::pow(m, 2) * std::pow(r, 2) + std::pow(r, 2))) /
0076                 (std::pow(m, 2) + 1);
0077     double x2 = (O_x + O_y * m +
0078                  std::sqrt(-std::pow(O_x, 2) * std::pow(m, 2) +
0079                            2 * O_x * O_y * m - std::pow(O_y, 2) +
0080                            std::pow(m, 2) * std::pow(r, 2) + std::pow(r, 2))) /
0081                 (std::pow(m, 2) + 1);
0082
0083     Vector2 v1(x1, m * x1);
0084     if (v1.dot(dir) > 0) {
0085       return v1;
0086     }
0087     return {x2, m * x2};
0088   };
0089
0090   // calculate corners in STRIP XY, keep them we need them for minDistance()
0091   m_outLeftStripXY =
0092       circIx(m_moduleOrigin[0], m_moduleOrigin[1], get(eMaxR), get(eMaxPhiRel));
0093   m_inLeftStripXY =
0094       circIx(m_moduleOrigin[0], m_moduleOrigin[1], get(eMinR), get(eMaxPhiRel));
0095   m_outRightStripXY =
0096       circIx(m_moduleOrigin[0], m_moduleOrigin[1], get(eMaxR), get(eMinPhiRel));
0097   m_inRightStripXY =
0098       circIx(m_moduleOrigin[0], m_moduleOrigin[1], get(eMinR), get(eMinPhiRel));
0099
0100   m_outLeftStripPC = {m_outLeftStripXY.norm(),
0101                       VectorHelpers::phi(m_outLeftStripXY)};
0102   m_inLeftStripPC = {m_inLeftStripXY.norm(),
0103                      VectorHelpers::phi(m_inLeftStripXY)};
0104   m_outRightStripPC = {m_outRightStripXY.norm(),
0105                        VectorHelpers::phi(m_outRightStripXY)};
0106   m_inRightStripPC = {m_inRightStripXY.norm(),
0107                       VectorHelpers::phi(m_inRightStripXY)};
0108
0109   m_outLeftModulePC = stripXYToModulePC(m_outLeftStripXY);
0110   m_inLeftModulePC = stripXYToModulePC(m_inLeftStripXY);
0111   m_outRightModulePC = stripXYToModulePC(m_outRightStripXY);
0112   m_inRightModulePC = stripXYToModulePC(m_inRightStripXY);
0113 }
0114
0115 std::vector<double> AnnulusBounds::values() const {
0116   return {m_values.begin(), m_values.end()};
0117 }
0118
0119 void AnnulusBounds::checkConsistency() noexcept(false) {
0120   if (get(eMinR) < 0. || get(eMaxR) < 0. || get(eMinR) > get(eMaxR) ||
0121       std::abs(get(eMinR) - get(eMaxR)) < s_epsilon) {
0122     throw std::invalid_argument("AnnulusBounds: invalid radial setup.");
0123   }
0124   if (get(eMinPhiRel) != detail::radian_sym(get(eMinPhiRel)) ||
0125       get(eMaxPhiRel) != detail::radian_sym(get(eMaxPhiRel)) ||
0126       get(eMinPhiRel) > get(eMaxPhiRel)) {
0127     throw std::invalid_argument("AnnulusBounds: invalid phi boundary setup.");
0128   }
0129   if (get(eAveragePhi) != detail::radian_sym(get(eAveragePhi))) {
0130     throw std::invalid_argument("AnnulusBounds: invalid phi positioning.");
0131   }
0132 }
0133
0134 std::vector<Vector2> AnnulusBounds::corners() const {
0135   auto rot = m_rotationStripPC.inverse();
0136
0137   return {rot * m_outRightStripPC, rot * m_outLeftStripPC,
0138           rot * m_inLeftStripPC, rot * m_inRightStripPC};
0139 }
0140
0141 std::vector<Vector2> AnnulusBounds::vertices(
0142     unsigned int quarterSegments) const {
0143   if (quarterSegments > 0u) {
0144     using VectorHelpers::phi;
0145
0146     double phiMinInner = phi(m_inRightStripXY - m_moduleOrigin);
0147     double phiMaxInner = phi(m_inLeftStripXY - m_moduleOrigin);
0148
0149     double phiMinOuter = phi(m_outRightStripXY - m_moduleOrigin);
0150     double phiMaxOuter = phi(m_outLeftStripXY - m_moduleOrigin);
0151
0152     // Inner bow from phi_min -> phi_max (needs to be reversed)
0153     std::vector<Vector2> rvertices =
0154         detail::VerticesHelper::segmentVertices<Vector2, Transform2>(
0155             {get(eMinR), get(eMinR)}, phiMinInner, phiMaxInner, {},
0156             quarterSegments);
0157     std::reverse(rvertices.begin(), rvertices.end());
0158
0159     // Outer bow from phi_min -> phi_max
0160     auto overtices =
0161         detail::VerticesHelper::segmentVertices<Vector2, Transform2>(
0162             {get(eMaxR), get(eMaxR)}, phiMinOuter, phiMaxOuter, {},
0163             quarterSegments);
0164     rvertices.insert(rvertices.end(), overtices.begin(), overtices.end());
0165
0166     std::for_each(rvertices.begin(), rvertices.end(),
0167                   [&](Vector2& rv) { rv += m_moduleOrigin; });
0168     return rvertices;
0169   }
0170   return {m_inLeftStripXY, m_inRightStripXY, m_outRightStripXY,
0171           m_outLeftStripXY};
0172 }
0173
0174 bool AnnulusBounds::inside(const Vector2& lposition, double tolR,
0175                            double tolPhi) const {
0176   // locpo is PC in STRIP SYSTEM
0177   // need to perform internal rotation induced by average phi
0178   Vector2 locpo_rotated = m_rotationStripPC * lposition;
0179   double phiLoc = locpo_rotated[1];
0180   double rLoc = locpo_rotated[0];
0181
0182   if (phiLoc < (get(eMinPhiRel) - tolPhi) ||
0183       phiLoc > (get(eMaxPhiRel) + tolPhi)) {
0184     return false;
0185   }
0186
0187   // calculate R in MODULE SYSTEM to evaluate R-bounds
0188   if (tolR == 0.) {
0189     // don't need R, can use R^2
0190     double r_mod2 = m_shiftPC[0] * m_shiftPC[0] + rLoc * rLoc +
0191                     2 * m_shiftPC[0] * rLoc * cos(phiLoc - m_shiftPC[1]);
0192
0193     if (r_mod2 < get(eMinR) * get(eMinR) || r_mod2 > get(eMaxR) * get(eMaxR)) {
0194       return false;
0195     }
0196   } else {
0197     // use R
0198     double r_mod = sqrt(m_shiftPC[0] * m_shiftPC[0] + rLoc * rLoc +
0199                         2 * m_shiftPC[0] * rLoc * cos(phiLoc - m_shiftPC[1]));
0200
0201     if (r_mod < (get(eMinR) - tolR) || r_mod > (get(eMaxR) + tolR)) {
0202       return false;
0203     }
0204   }
0205   return true;
0206 }
0207
0208 bool AnnulusBounds::inside(const Vector2& lposition,
0209                            const BoundaryTolerance& boundaryTolerance) const {
0210   using enum BoundaryTolerance::ToleranceMode;
0211   if (boundaryTolerance.isInfinite()) {
0212     return true;
0213   }
0214
0215   if (boundaryTolerance.isNone()) {
0216     return inside(lposition, 0., 0.);
0217   }
0218
0219   if (auto absoluteBound = boundaryTolerance.asAbsoluteBoundOpt();
0220       absoluteBound.has_value()) {
0221     return inside(lposition, absoluteBound->tolerance0,
0222                   absoluteBound->tolerance1);
0223   }
0224
0225   bool insideStrict = inside(lposition, 0., 0.);
0226
0227   BoundaryTolerance::ToleranceMode mode = boundaryTolerance.toleranceMode();
0228   if (mode == None) {
0229     // first check if inside if we're in None tolerance mode. We don't need to
0230     // look into the covariance if inside
0231     return insideStrict;
0232   }
0233
0234   if (mode == Extend && insideStrict) {
0235     return true;
0236   }
0237
0238   // locpo is PC in STRIP SYSTEM
0239   // we need to rotate the locpo
0240   Vector2 locpo_rotated = m_rotationStripPC * lposition;
0241
0242   // covariance is given in STRIP SYSTEM in PC we need to convert the
0243   // covariance to the MODULE SYSTEM in PC via jacobian. The following
0244   // transforms into STRIP XY, does the shift into MODULE XY, and then
0245   // transforms into MODULE PC
0246   double dphi = get(eAveragePhi);
0247   double phi_strip = locpo_rotated[1];
0248   double r_strip = locpo_rotated[0];
0249   double O_x = m_shiftXY[0];
0250   double O_y = m_shiftXY[1];
0251
0252   auto closestPointDistanceBound = [&](const SquareMatrix2& weight) {
0253     // For a transformation from cartesian into polar coordinates
0254     //
0255     //              [         _________      ]
0256     //              [        /  2    2       ]
0257     //              [      \/  x  + y        ]
0258     //     [ r' ]   [                        ]
0259     // v = [    ] = [      /       y        \]
0260     //     [phi']   [2*atan|----------------|]
0261     //              [      |       _________|]
0262     //              [      |      /  2    2 |]
0263     //              [      \x + \/  x  + y  /]
0264     //
0265     // Where x, y are polar coordinates that can be rotated by dPhi
0266     //
0267     // [x]   [O_x + r*cos(dPhi - phi)]
0268     // [ ] = [                       ]
0269     // [y]   [O_y - r*sin(dPhi - phi)]
0270     //
0271     // The general jacobian is:
0272     //
0273     //        [d        d      ]
0274     //        [--(f_x)  --(f_x)]
0275     //        [dx       dy     ]
0276     // Jgen = [                ]
0277     //        [d        d      ]
0278     //        [--(f_y)  --(f_y)]
0279     //        [dx       dy     ]
0280     //
0281     // which means in this case:
0282     //
0283     //     [     d                   d           ]
0284     //     [ ----------(rMod)    ---------(rMod) ]
0285     //     [ dr_{strip}          dphiStrip       ]
0286     // J = [                                     ]
0287     //     [    d                   d            ]
0288     //     [----------(phiMod)  ---------(phiMod)]
0289     //     [dr_{strip}          dphiStrip        ]
0290     //
0291     // Performing the derivative one gets:
0292     //
0293     //     [B*O_x + C*O_y + rStrip  rStrip*(B*O_y + O_x*sin(dPhi - phiStrip))]
0294     //     [----------------------  -----------------------------------------]
0295     //     [          ___                               ___                  ]
0296     //     [        \/ A                              \/ A                   ]
0297     // J = [                                                                 ]
0298     //     [  -(B*O_y - C*O_x)           rStrip*(B*O_x + C*O_y + rStrip)     ]
0299     //     [  -----------------          -------------------------------     ]
0300     //     [          A                                 A                    ]
0301     //
0302     // where
0303     //        2                                          2
0304     // A = O_x  + 2*O_x*rStrip*cos(dPhi - phiStrip) + O_y
0305     //                                                 2
0306     //     - 2*O_y*rStrip*sin(dPhi - phiStrip) + rStrip
0307     // B = cos(dPhi - phiStrip)
0308     // C = -sin(dPhi - phiStrip)
0309
0310     double cosDPhiPhiStrip = std::cos(dphi - phi_strip);
0311     double sinDPhiPhiStrip = std::sin(dphi - phi_strip);
0312
0313     double A = O_x * O_x + 2 * O_x * r_strip * cosDPhiPhiStrip + O_y * O_y -
0314                2 * O_y * r_strip * sinDPhiPhiStrip + r_strip * r_strip;
0315     double sqrtA = std::sqrt(A);
0316
0317     double B = cosDPhiPhiStrip;
0318     double C = -sinDPhiPhiStrip;
0319     SquareMatrix2 jacobianStripPCToModulePC;
0320     jacobianStripPCToModulePC(0, 0) = (B * O_x + C * O_y + r_strip) / sqrtA;
0321     jacobianStripPCToModulePC(0, 1) =
0322         r_strip * (B * O_y + O_x * sinDPhiPhiStrip) / sqrtA;
0323     jacobianStripPCToModulePC(1, 0) = -(B * O_y - C * O_x) / A;
0324     jacobianStripPCToModulePC(1, 1) =
0325         r_strip * (B * O_x + C * O_y + r_strip) / A;
0326
0327     // Mahalanobis distance uses inverse covariance as weights
0328     const auto& weightStripPC = weight;
0329     auto weightModulePC = jacobianStripPCToModulePC.transpose() *
0330                           weightStripPC * jacobianStripPCToModulePC;
0331
0332     double minDist = std::numeric_limits<double>::max();
0333     Vector2 delta;
0334
0335     Vector2 currentClosest;
0336     double currentDist = 0;
0337
0338     // do projection in STRIP PC
0339
0340     // first: STRIP system. locpo is in STRIP PC already
0341     currentClosest = closestOnSegment(m_inLeftStripPC, m_outLeftStripPC,
0342                                       locpo_rotated, weightStripPC);
0343     currentDist = squaredNorm(locpo_rotated - currentClosest, weightStripPC);
0344     minDist = currentDist;
0345     delta = locpo_rotated - currentClosest;
0346     currentClosest = closestOnSegment(m_inRightStripPC, m_outRightStripPC,
0347                                       locpo_rotated, weightStripPC);
0348     currentDist = squaredNorm(locpo_rotated - currentClosest, weightStripPC);
0349     if (currentDist < minDist) {
0350       minDist = currentDist;
0351       delta = locpo_rotated - currentClosest;
0352     }
0353
0354     // now: MODULE system. Need to transform locpo to MODULE PC
0355     //  transform is STRIP PC -> STRIP XY -> MODULE XY -> MODULE PC
0356     Vector2 locpoStripXY(
0357         locpo_rotated[eBoundLoc0] * std::cos(locpo_rotated[eBoundLoc1]),
0358         locpo_rotated[eBoundLoc0] * std::sin(locpo_rotated[eBoundLoc1]));
0359     Vector2 locpoModulePC = stripXYToModulePC(locpoStripXY);
0360
0361     // now check edges in MODULE PC (inner and outer circle) assuming
0362     // Mahalanobis distances are of same unit if covariance is correctly
0363     // transformed
0364     currentClosest = closestOnSegment(m_inLeftModulePC, m_inRightModulePC,
0365                                       locpoModulePC, weightModulePC);
0366     currentDist = squaredNorm(locpoModulePC - currentClosest, weightModulePC);
0367     if (currentDist < minDist) {
0368       minDist = currentDist;
0369       delta = jacobianStripPCToModulePC.inverse() *
0370               (currentClosest - locpoModulePC);
0371     }
0372
0373     currentClosest = closestOnSegment(m_outLeftModulePC, m_outRightModulePC,
0374                                       locpoModulePC, weightModulePC);
0375     currentDist = squaredNorm(locpoModulePC - currentClosest, weightModulePC);
0376     if (currentDist < minDist) {
0377       minDist = currentDist;
0378       delta = jacobianStripPCToModulePC.inverse() *
0379               (currentClosest - locpoModulePC);
0380     }
0381
0382     return std::tuple{delta, minDist};
0383   };
0384
0385   if (boundaryTolerance.hasChi2Bound()) {
0386     const auto& boundaryToleranceChi2 = boundaryTolerance.asChi2Bound();
0387
0388     // Calculate minDist based on weight from the boundary tolerance object.
0389     // That weight matrix is in STRIP PC
0390     auto [delta, minDist] =
0391         closestPointDistanceBound(boundaryToleranceChi2.weight);
0392
0393     // compare resulting Mahalanobis distance to configured "number of sigmas"
0394     // we square it b/c we never took the square root of the distance
0395     if (mode == Extend) {
0396       return minDist <
0397              boundaryToleranceChi2.maxChi2 * boundaryToleranceChi2.maxChi2;
0398     } else if (mode == Shrink) {
0399       return minDist > boundaryToleranceChi2.maxChi2 *
0400                            boundaryToleranceChi2.maxChi2 &&
0401              insideStrict;
0402     }
0403   } else if (boundaryTolerance.hasAbsoluteEuclidean()) {
0404     const auto& boundaryToleranceAbsoluteEuclidean =
0405         boundaryTolerance.asAbsoluteEuclidean();
0406
0407     SquareMatrix2 jacobianPCToXY;
0408     jacobianPCToXY(0, 0) = std::cos(phi_strip);
0409     jacobianPCToXY(0, 1) = -r_strip * std::sin(phi_strip);
0410     jacobianPCToXY(1, 0) = std::sin(phi_strip);
0411     jacobianPCToXY(1, 1) = r_strip * std::cos(phi_strip);
0412
0413     // This is J.T * J but we can also calculate it directly
0414     SquareMatrix2 weightStripPC;
0415     weightStripPC(0, 0) = 1;
0416     weightStripPC(0, 1) = 0;
0417     weightStripPC(1, 0) = 0;
0418     weightStripPC(1, 1) = r_strip * r_strip;
0419
0420     auto [delta, minDist] = closestPointDistanceBound(weightStripPC);
0421
0422     Vector2 cartesianDistance = jacobianPCToXY * delta;
0423
0424     if (mode == Extend) {
0425       return cartesianDistance.squaredNorm() <
0426              boundaryToleranceAbsoluteEuclidean.tolerance *
0427                  boundaryToleranceAbsoluteEuclidean.tolerance;
0428     } else if (mode == Shrink) {
0429       return cartesianDistance.squaredNorm() >
0430                  boundaryToleranceAbsoluteEuclidean.tolerance *
0431                      boundaryToleranceAbsoluteEuclidean.tolerance &&
0432              insideStrict;
0433     }
0434   }
0435
0436   throw std::logic_error("not implemented");
0437 }
0438
0439 Vector2 AnnulusBounds::stripXYToModulePC(const Vector2& vStripXY) const {
0440   Vector2 vecModuleXY = vStripXY + m_shiftXY;
0441   return {vecModuleXY.norm(), VectorHelpers::phi(vecModuleXY)};
0442 }
0443
0444 Vector2 AnnulusBounds::moduleOrigin() const {
0445   return Eigen::Rotation2D<double>(get(eAveragePhi)) * m_moduleOrigin;
0446 }
0447
0448 std::ostream& AnnulusBounds::toStream(std::ostream& sl) const {
0449   sl << std::setiosflags(std::ios::fixed);
0450   sl << std::setprecision(7);
0451   sl << "Acts::AnnulusBounds:  (innerRadius, outerRadius, minPhi, maxPhi) = ";
0452   sl << "(" << get(eMinR) << ", " << get(eMaxR) << ", " << phiMin() << ", "
0453      << phiMax() << ")" << '\n';
0454   sl << " - shift xy = " << m_shiftXY.x() << ", " << m_shiftXY.y() << '\n';
0455   sl << " - shift pc = " << m_shiftPC.x() << ", " << m_shiftPC.y() << '\n';
0456   sl << std::setprecision(-1);
0457   return sl;
0458 }
0459
0460 }  // namespace Acts