Surfaces/detail/LineHelper.hpp

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 #pragma once
0010
0011 #include "Acts/Definitions/Algebra.hpp"
0012 #include "Acts/Utilities/Intersection.hpp"
0013 #include "Acts/Utilities/MathHelpers.hpp"
0014
0015 namespace Acts::detail::LineHelper {
0016 /// @brief Intersect two straight N-dimensional lines with each other or more generally
0017 ///        calculate the point of closest approach of the second line to the
0018 ///        first line.
0019 /// @param linePosA: Arbitrary point on the first line
0020 /// @param lineDirA: Direction of the first line (Unit-length)
0021 /// @param linePosB: Arbitrary point on the second line
0022 /// @param lineDirB: Direction of the second line (Unit-length)
0023 template <int N>
0024 inline Intersection<N> lineIntersect(const ActsVector<N>& linePosA,
0025                                      const ActsVector<N>& lineDirA,
0026                                      const ActsVector<N>& linePosB,
0027                                      const ActsVector<N>& lineDirB)
0028   requires(N >= 2)
0029 {
0030   static_assert(N >= 2, "One dimensional intersect not sensible");
0031   /// Use the formula
0032   ///   A + lambda dirA  = B + mu dirB
0033   ///   (A-B) + lambda dirA = mu dirB
0034   ///   <A-B, dirB> + lambda <dirA,dirB> = mu
0035   ///    A + lambda dirA = B + (<A-B, dirB> + lambda <dirA,dirB>)dirB
0036   ///    <A-B,dirA> + lambda <dirA, dirA> = <A-B, dirB><dirA,dirB> +
0037   ///    lambda<dirA,dirB><dirA,dirB>
0038   ///  -> lambda = -(<A-B, dirA> - <A-B, dirB> * <dirA, dirB>) / (1-
0039   ///  <dirA,dirB>^2)
0040   ///  --> mu    =  (<A-B, dirB> - <A-B, dirA> * <dirA, dirB>) / (1-
0041   ///  <dirA,dirB>^2)
0042   const double dirDots = lineDirA.dot(lineDirB);
0043   const double divisor = (1. - square(dirDots));
0044   /// If the two directions are parallel to each other there's no way of
0045   /// intersection
0046   if (std::abs(divisor) < std::numeric_limits<double>::epsilon()) {
0047     return Intersection<N>::invalid();
0048   }
0049   const ActsVector<N> aMinusB = linePosA - linePosB;
0050   const double pathLength =
0051       (aMinusB.dot(lineDirB) - aMinusB.dot(lineDirA) * dirDots) / divisor;
0052
0053   return Intersection<N>{linePosB + pathLength * lineDirB, pathLength,
0054                          IntersectionStatus::onSurface};
0055 }
0056 /// @brief Intersect the lines of two line surfaces using their respective transforms.
0057 /// @param lineSurfTrf1: local -> global transform of the first surface
0058 /// @param lineSurfTrf2: local -> global transform of the second surface
0059 inline Intersection3D lineSurfaceIntersect(
0060     const Acts::Transform3& lineSurfTrf1,
0061     const Acts::Transform3& lineSurfTrf2) {
0062   return lineIntersect<3>(
0063       lineSurfTrf1.translation(), lineSurfTrf1.linear().col(2),
0064       lineSurfTrf2.translation(), lineSurfTrf2.linear().col(2));
0065 }
0066 /// @brief Calculates the signed distance between two lines in 3D space
0067 /// @param linePosA: Arbitrary point on the first line
0068 /// @param lineDirA: Direction of the first line (Unit-length)
0069 /// @param linePosB: Arbitrary point on the second line
0070 /// @param lineDirB: Direction of the second line (Unit-length)
0071 inline double signedDistance(const Vector3& linePosA, const Vector3& lineDirA,
0072                              const Vector3& linePosB, const Vector3& lineDirB) {
0073   /// Project the first direction onto the second & renormalize to a unit vector
0074   const double dirDots = lineDirA.dot(lineDirB);
0075   const Vector3 aMinusB = linePosA - linePosB;
0076   if (std::abs(dirDots - 1.) < std::numeric_limits<double>::epsilon()) {
0077     return (aMinusB - lineDirA.dot(aMinusB) * lineDirA).norm();
0078   }
0079   const Vector3 projDir = (lineDirA - dirDots * lineDirB).normalized();
0080   return aMinusB.cross(lineDirB).dot(projDir);
0081 }
0082 }  // namespace Acts::detail::LineHelper