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