src/Navigation/NavigationStream.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/Navigation/NavigationStream.hpp"
0010
0011 #include "Acts/Propagator/NavigationTarget.hpp"
0012 #include "Acts/Surfaces/BoundaryTolerance.hpp"
0013 #include "Acts/Surfaces/Surface.hpp"
0014 #include "Acts/Utilities/Enumerate.hpp"
0015
0016 #include <algorithm>
0017
0018 namespace Acts {
0019
0020 bool NavigationStream::initialize(const GeometryContext& gctx,
0021                                   const QueryPoint& queryPoint,
0022                                   const BoundaryTolerance& cTolerance,
0023                                   const double onSurfaceTolerance) {
0024   // Position and direction from the query point
0025   const Vector3& position = queryPoint.position;
0026   const Vector3& direction = queryPoint.direction;
0027
0028   // A container collecting additional candidates from multiple
0029   // valid intersections
0030   std::vector<NavigationTarget> additionalCandidates = {};
0031   for (auto& candidate : m_candidates) {
0032     // Get the surface from the object intersection
0033     const Surface& surface = candidate.surface();
0034     // Intersect the surface
0035     auto multiIntersection = surface.intersect(gctx, position, direction,
0036                                                cTolerance, onSurfaceTolerance);
0037
0038     bool firstValid = multiIntersection.at(0).isValid();
0039     bool secondValid = multiIntersection.at(1).isValid();
0040     if (firstValid && !secondValid) {
0041       if (multiIntersection.at(0).pathLength() < -onSurfaceTolerance) {
0042         continue;
0043       }
0044       candidate.intersection() = multiIntersection.at(0);
0045       candidate.intersectionIndex() = 0;
0046     } else if (!firstValid && secondValid) {
0047       if (multiIntersection.at(1).pathLength() < -onSurfaceTolerance) {
0048         continue;
0049       }
0050       candidate.intersection() = multiIntersection.at(1);
0051       candidate.intersectionIndex() = 1;
0052     } else {
0053       // Split them into valid intersections, keep track of potentially
0054       // additional candidates
0055       bool originalCandidateUpdated = false;
0056       for (auto [intersectionIndex, intersection] :
0057            enumerate(multiIntersection)) {
0058         // Skip negative solutions, respecting the on surface tolerance
0059         if (intersection.pathLength() < -onSurfaceTolerance) {
0060           continue;
0061         }
0062         // Valid solution is either on surface or updates the distance
0063         if (intersection.isValid()) {
0064           if (!originalCandidateUpdated) {
0065             candidate.intersection() = intersection;
0066             candidate.intersectionIndex() = intersectionIndex;
0067             originalCandidateUpdated = true;
0068           } else {
0069             NavigationTarget additionalCandidate = candidate;
0070             additionalCandidate.intersection() = intersection;
0071             additionalCandidate.intersectionIndex() = intersectionIndex;
0072             additionalCandidates.emplace_back(additionalCandidate);
0073           }
0074         }
0075       }
0076     }
0077   }
0078
0079   // Append the multi intersection candidates
0080   m_candidates.insert(m_candidates.end(), additionalCandidates.begin(),
0081                       additionalCandidates.end());
0082
0083   // Sort the candidates by path length
0084   std::ranges::sort(m_candidates, NavigationTarget::pathLengthOrder);
0085
0086   // If we have duplicates, we expect them to be close by in path length, so we
0087   // don't need to re-sort Remove duplicates on basis of the surface pointer
0088   auto nonUniqueRange = std::ranges::unique(
0089       m_candidates.begin(), m_candidates.end(),
0090       [](const NavigationTarget& a, const NavigationTarget& b) {
0091         return &a.surface() == &b.surface();
0092       });
0093   m_candidates.erase(nonUniqueRange.begin(), nonUniqueRange.end());
0094
0095   // The we find the first invalid candidate
0096   auto firstInvalid = std::ranges::find_if(
0097       m_candidates,
0098       [](const NavigationTarget& a) { return !a.intersection().isValid(); });
0099
0100   // Set the range and initialize
0101   m_candidates.resize(std::distance(m_candidates.begin(), firstInvalid),
0102                       NavigationTarget::None());
0103
0104   m_currentIndex = 0;
0105   if (m_candidates.empty()) {
0106     return false;
0107   }
0108   return true;
0109 }
0110
0111 bool NavigationStream::update(const GeometryContext& gctx,
0112                               const QueryPoint& queryPoint,
0113                               double onSurfaceTolerance) {
0114   // Loop over the (currently valid) candidates and update
0115   for (; m_currentIndex < m_candidates.size(); ++m_currentIndex) {
0116     // Get the candidate, and resolve the tuple
0117     NavigationTarget& candidate = currentCandidate();
0118     // Get the surface from the object intersection
0119     const Surface& surface = candidate.surface();
0120     // (re-)Intersect the surface
0121     auto multiIntersection =
0122         surface.intersect(gctx, queryPoint.position, queryPoint.direction,
0123                           candidate.boundaryTolerance(), onSurfaceTolerance);
0124     // Split them into valid intersections
0125     for (auto [intersectionIndex, intersection] :
0126          enumerate(multiIntersection)) {
0127       // Skip wrong index solution
0128       if (intersectionIndex != candidate.intersectionIndex()) {
0129         continue;
0130       }
0131       // Valid solution is either on surface or updates the distance
0132       if (intersection.isValid()) {
0133         candidate.intersection() = intersection;
0134         return true;
0135       }
0136     }
0137   }
0138   // No candidate was reachable
0139   return false;
0140 }
0141
0142 void NavigationStream::reset() {
0143   m_candidates.clear();
0144   m_currentIndex = 0;
0145 }
0146
0147 void NavigationStream::addSurfaceCandidate(
0148     const Surface& surface, const BoundaryTolerance& bTolerance) {
0149   m_candidates.emplace_back(Intersection3D::Invalid(), 0, surface, bTolerance);
0150 }
0151
0152 void NavigationStream::addSurfaceCandidates(
0153     std::span<const Surface*> surfaces, const BoundaryTolerance& bTolerance) {
0154   m_candidates.reserve(m_candidates.size() + surfaces.size());
0155   std::ranges::for_each(surfaces, [&](const Surface* surface) {
0156     m_candidates.emplace_back(Intersection3D::Invalid(), 0, *surface,
0157                               bTolerance);
0158   });
0159 }
0160
0161 void NavigationStream::addPortalCandidate(const Portal& portal) {
0162   m_candidates.emplace_back(Intersection3D::Invalid(), 0, portal,
0163                             BoundaryTolerance::None());
0164 }
0165
0166 AppendOnlyNavigationStream::AppendOnlyNavigationStream(NavigationStream& stream)
0167     : m_stream{&stream} {}
0168
0169 void AppendOnlyNavigationStream::addPortalCandidate(const Portal& portal) {
0170   m_stream->addPortalCandidate(portal);
0171 }
0172
0173 void AppendOnlyNavigationStream::addSurfaceCandidate(
0174     const Surface& surface, const BoundaryTolerance& bTolerance) {
0175   m_stream->addSurfaceCandidate(surface, bTolerance);
0176 }
0177
0178 }  // namespace Acts