src/Surfaces/IntersectionHelper2D.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/detail/IntersectionHelper2D.hpp"
0010
0011 #include "Acts/Definitions/Tolerance.hpp"
0012 #include "Acts/Utilities/VectorHelpers.hpp"
0013 #include "Acts/Utilities/detail/RealQuadraticEquation.hpp"
0014
0015 #include <cmath>
0016
0017 namespace Acts::detail {
0018
0019 Intersection2D IntersectionHelper2D::intersectSegment(const Vector2& s0,
0020                                                       const Vector2& s1,
0021                                                       const Vector2& origin,
0022                                                       const Vector2& dir,
0023                                                       bool boundCheck) {
0024   using Line = Eigen::ParametrizedLine<double, 2>;
0025   using Plane = Eigen::Hyperplane<double, 2>;
0026
0027   Vector2 edge(s1 - s0);
0028   double det = edge.x() * dir.y() - edge.y() * dir.x();
0029   if (std::abs(det) < s_epsilon) {
0030     return Intersection2D::invalid();
0031   }
0032
0033   auto line = Line(origin, dir);
0034   auto d = line.intersectionParameter(Plane::Through(s0, s1));
0035
0036   Vector2 intersection(origin + d * dir);
0037   IntersectionStatus status = IntersectionStatus::reachable;
0038   if (boundCheck) {
0039     auto edgeToSol = intersection - s0;
0040     if (edgeToSol.dot(edge) < 0. || edgeToSol.norm() > (edge).norm()) {
0041       status = IntersectionStatus::unreachable;
0042     }
0043   }
0044   return Intersection2D(intersection, d, status);
0045 }
0046
0047 std::array<Intersection2D, 2> IntersectionHelper2D::intersectEllipse(
0048     double Rx, double Ry, const Vector2& origin, const Vector2& dir) {
0049   auto createSolution =
0050       [&](const Vector2& sol,
0051           const Vector2& alt) -> std::array<Intersection2D, 2> {
0052     Vector2 toSolD(sol - origin);
0053     Vector2 toAltD(alt - origin);
0054
0055     double solD = std::copysign(toSolD.norm(), toSolD.dot(dir));
0056     double altD = std::copysign(toAltD.norm(), toAltD.dot(dir));
0057
0058     if (std::abs(solD) < std::abs(altD)) {
0059       return {Intersection2D(sol, solD, IntersectionStatus::reachable),
0060               Intersection2D(alt, altD, IntersectionStatus::reachable)};
0061     }
0062     return {Intersection2D(alt, altD, IntersectionStatus::reachable),
0063             Intersection2D(sol, solD, IntersectionStatus::reachable)};
0064   };
0065
0066   // Special cases first
0067   if (std::abs(dir.x()) < s_epsilon) {
0068     double solx = origin.x();
0069     double D = 1. - solx * solx / (Rx * Rx);
0070     if (D > 0.) {
0071       double sqrtD = std::sqrt(D);
0072       Vector2 sol(solx, Ry * sqrtD);
0073       Vector2 alt(solx, -Ry * sqrtD);
0074       return createSolution(sol, alt);
0075     } else if (std::abs(D) < s_epsilon) {
0076       return {Intersection2D(Vector2(solx, 0.), -origin.y(),
0077                              IntersectionStatus::reachable),
0078               Intersection2D::invalid()};
0079     }
0080     return {Intersection2D::invalid(), Intersection2D::invalid()};
0081   } else if (std::abs(dir.y()) < s_epsilon) {
0082     double soly = origin.y();
0083     double D = 1. - soly * soly / (Ry * Ry);
0084     if (D > 0.) {
0085       double sqrtD = std::sqrt(D);
0086       Vector2 sol(Rx * sqrtD, soly);
0087       Vector2 alt(-Rx * sqrtD, soly);
0088       return createSolution(sol, alt);
0089     } else if (std::abs(D) < s_epsilon) {
0090       return {Intersection2D(Vector2(0., soly), -origin.x(),
0091                              IntersectionStatus::reachable),
0092               Intersection2D::invalid()};
0093     }
0094     return {Intersection2D::invalid(), Intersection2D::invalid()};
0095   }
0096   // General solution
0097   double k = dir.y() / dir.x();
0098   double d = origin.y() - k * origin.x();
0099   double Ry2 = Ry * Ry;
0100   double alpha = 1. / (Rx * Rx) + k * k / Ry2;
0101   double beta = 2. * k * d / Ry2;
0102   double gamma = d * d / Ry2 - 1;
0103   RealQuadraticEquation solver(alpha, beta, gamma);
0104   if (solver.solutions == 1) {
0105     double x = solver.first;
0106     Vector2 sol(x, k * x + d);
0107     Vector2 toSolD(sol - origin);
0108     double solD = std::copysign(toSolD.norm(), toSolD.dot(dir));
0109     return {Intersection2D(sol, solD, IntersectionStatus::reachable),
0110             Intersection2D::invalid()};
0111   } else if (solver.solutions > 1) {
0112     double x0 = solver.first;
0113     double x1 = solver.second;
0114     Vector2 sol(x0, k * x0 + d);
0115     Vector2 alt(x1, k * x1 + d);
0116     return createSolution(sol, alt);
0117   }
0118   return {Intersection2D::invalid(), Intersection2D::invalid()};
0119 }
0120
0121 Intersection2D IntersectionHelper2D::intersectCircleSegment(
0122     double R, double phiMin, double phiMax, const Vector2& origin,
0123     const Vector2& dir) {
0124   auto intersections = intersectCircle(R, origin, dir);
0125   for (const auto& candidate : intersections) {
0126     if (candidate.pathLength() > 0.) {
0127       double phi = VectorHelpers::phi(candidate.position());
0128       if (phi > phiMin && phi < phiMax) {
0129         return candidate;
0130       }
0131     }
0132   }
0133   return Intersection2D::invalid();
0134 }
0135
0136 }  // namespace Acts::detail