Acts/Visualization/Interpolation3D.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
0013 #include <algorithm>
0014
0015 #include <unsupported/Eigen/Splines>
0016
0017 namespace Acts::Interpolation3D {
0018
0019 /// @brief Helper function to interpolate points using a spline
0020 /// from Eigen
0021 ///
0022 /// The only requirement is that the input trajectory type has
0023 /// a method empty() and size() and that the elements can be
0024 /// accessed with operator[] and have themselves a operator[] to
0025 /// access the coordinates.
0026 ///
0027 /// @tparam input_trajectory_type input trajectory type
0028 ///
0029 /// @param inputsRaw input vector points
0030 /// @param nPoints number of interpolation points
0031 /// @param keepOriginalHits keep the original hits in the trajectory
0032 ///
0033 /// @return std::vector<Acts::Vector3> interpolated points
0034 template <typename trajectory_type>
0035 trajectory_type spline(const trajectory_type& inputsRaw, std::size_t nPoints,
0036                        bool keepOriginalHits = false) {
0037   trajectory_type output;
0038   if (inputsRaw.empty()) {
0039     return output;
0040   }
0041
0042   using InputVectorType = typename trajectory_type::value_type;
0043
0044   std::vector<Vector3> inputs;
0045   // If input type is a vector of Vector3 we can use it directly
0046   if constexpr (std::is_same_v<trajectory_type, std::vector<Vector3>>) {
0047     inputs = inputsRaw;
0048   } else {
0049     inputs.reserve(inputsRaw.size());
0050     for (const auto& input : inputsRaw) {
0051       inputs.push_back(Vector3(input[0], input[1], input[2]));
0052     }
0053   }
0054
0055   // Don't do anything if we have less than 3 points or less interpolation
0056   // points than input points
0057   if (inputsRaw.size() < 3 || nPoints <= inputsRaw.size()) {
0058     return inputsRaw;
0059   } else {
0060     Eigen::MatrixXd points(3, inputs.size());
0061     for (std::size_t i = 0; i < inputs.size(); ++i) {
0062       points.col(i) = inputs[i].transpose();
0063     }
0064
0065     // MARK: fpeMaskBegin(FLTDIV, 1, #4024)
0066     // MARK: fpeMaskBegin(FLTINV, 1, #4024)
0067     Eigen::Spline<double, 3> spline3D =
0068         Eigen::SplineFitting<Eigen::Spline<double, 3>>::Interpolate(points, 2);
0069     // MARK: fpeMaskEnd(FLTDIV)
0070     // MARK: fpeMaskEnd(FLTINV)
0071
0072     double step = 1. / (nPoints - 1);
0073     for (std::size_t i = 0; i < nPoints; ++i) {
0074       double t = i * step;
0075       InputVectorType point;
0076       point[0] = spline3D(t)[0];
0077       point[1] = spline3D(t)[1];
0078       point[2] = spline3D(t)[2];
0079       output.push_back(point);
0080     }
0081   }
0082   // If we want to keep the original hits, we add them to the output
0083   // (first and last are there anyway)
0084   if (keepOriginalHits) {
0085     output.insert(output.begin(), inputsRaw.begin() + 1, inputsRaw.end() - 1);
0086     // We need to sort the output in distance to first
0087     std::ranges::sort(output, [&inputs](const auto& a, const auto& b) {
0088       const auto ifront = inputs.front();
0089       double da2 = (a[0] - ifront[0]) * (a[0] - ifront[0]) +
0090                    (a[1] - ifront[1]) * (a[1] - ifront[1]) +
0091                    (a[2] - ifront[2]) * (a[2] - ifront[2]);
0092       double db2 = (b[0] - ifront[0]) * (b[0] - ifront[0]) +
0093                    (b[1] - ifront[1]) * (b[1] - ifront[1]) +
0094                    (b[2] - ifront[2]) * (b[2] - ifront[2]);
0095       return da2 < db2;
0096     });
0097   }
0098
0099   return output;
0100 }
0101
0102 }  // namespace Acts::Interpolation3D