From bfcd161161648d7829ab9206697443ebd2852fea Mon Sep 17 00:00:00 2001 From: Nagi70 Date: Mon, 19 Aug 2024 12:33:25 +0900 Subject: [PATCH] fix: unusedFunction Signed-off-by: Nagi70 --- .../spline_interpolation_points_2d.hpp | 3 --- .../src/spline_interpolation_points_2d.cpp | 22 ------------------- 2 files changed, 25 deletions(-) diff --git a/common/interpolation/include/interpolation/spline_interpolation_points_2d.hpp b/common/interpolation/include/interpolation/spline_interpolation_points_2d.hpp index 398d841c54710..b2170cf83bde2 100644 --- a/common/interpolation/include/interpolation/spline_interpolation_points_2d.hpp +++ b/common/interpolation/include/interpolation/spline_interpolation_points_2d.hpp @@ -21,9 +21,6 @@ namespace interpolation { -std::array, 3> slerp2dFromXY( - const std::vector & base_keys, const std::vector & base_x_values, - const std::vector & base_y_values, const std::vector & query_keys); template std::vector splineYawFromPoints(const std::vector & points); diff --git a/common/interpolation/src/spline_interpolation_points_2d.cpp b/common/interpolation/src/spline_interpolation_points_2d.cpp index 4d6d1639f2ac7..95fde68b171bd 100644 --- a/common/interpolation/src/spline_interpolation_points_2d.cpp +++ b/common/interpolation/src/spline_interpolation_points_2d.cpp @@ -71,28 +71,6 @@ std::array, 4> getBaseValues( namespace interpolation { -std::array, 3> slerp2dFromXY( - const std::vector & base_keys, const std::vector & base_x_values, - const std::vector & base_y_values, const std::vector & query_keys) -{ - // calculate spline coefficients - SplineInterpolation interpolator_x(base_keys, base_x_values); - SplineInterpolation interpolator_y(base_keys, base_y_values); - const auto diff_x = interpolator_x.getSplineInterpolatedDiffValues(query_keys); - const auto diff_y = interpolator_y.getSplineInterpolatedDiffValues(query_keys); - - // calculate yaw - std::vector yaw_vec; - for (size_t i = 0; i < diff_x.size(); i++) { - double yaw = std::atan2(diff_y[i], diff_x[i]); - yaw_vec.push_back(yaw); - } - // interpolate base_keys at query_keys - return { - interpolator_x.getSplineInterpolatedValues(query_keys), - interpolator_y.getSplineInterpolatedValues(query_keys), yaw_vec}; -} - template std::vector splineYawFromPoints(const std::vector & points) {