From d063853502740fc1ed4fe7401c2794c490ea8e68 Mon Sep 17 00:00:00 2001 From: Juan Manzanero Date: Mon, 18 Mar 2024 09:34:10 +0100 Subject: [PATCH] [JMT] Add smooth_pos, smooth_neg and smooth_clamp second derivatives + test --- lion/foundation/utils.h | 9 +++ lion/foundation/utils.hpp | 63 +++++++++++++++ test/foundation/utils_test.cpp | 136 +++++++++++++++++++++++++++++---- 3 files changed, 194 insertions(+), 14 deletions(-) diff --git a/lion/foundation/utils.h b/lion/foundation/utils.h index 5343b2b..d9d34d9 100644 --- a/lion/foundation/utils.h +++ b/lion/foundation/utils.h @@ -131,12 +131,18 @@ constexpr T smooth_pos(const T &x, S eps2); template constexpr T smooth_pos_derivative(const T &x, S eps2); +template +constexpr T smooth_pos_second_derivative(const T &x, S eps2); + template constexpr T smooth_neg(const T &x, S eps2); template constexpr T smooth_neg_derivative(const T &x, S eps2); +template +constexpr T smooth_neg_second_derivative(const T &x, S eps2); + template constexpr T smooth_sign(const T &x, S eps); @@ -161,6 +167,9 @@ constexpr T smooth_clamp(const T &x, const T1 &lo, const T2 &hi, S eps2); template constexpr T smooth_clamp_derivative(const T &x, const T1 &lo, const T2 &hi, S eps2); +template +constexpr T smooth_clamp_second_derivative(const T &x, const T1 &lo, const T2 &hi, S eps2); + template constexpr T smooth_step(const T &x, S eps); diff --git a/lion/foundation/utils.hpp b/lion/foundation/utils.hpp index 8a58b98..500a5a9 100644 --- a/lion/foundation/utils.hpp +++ b/lion/foundation/utils.hpp @@ -233,6 +233,31 @@ constexpr T smooth_pos_derivative(const T &x, S eps2) } +template +constexpr T smooth_pos_second_derivative(const T &x, S eps2) +{ + // + // Returns the positive part of input "x", smoothed + // out near 0 with parameter "eps2" (unless template + // parameter "ActuallySmooth" is false, in that case + // the function returns the strict positive part + // of "x"). + // + + if constexpr (ActuallySmooth) { + using std::sqrt; + + return scalar{0.5} * eps2 / (x * x + eps2) / sqrt(x * x + eps2); + } + else { + (void)eps2; + + return T{ 0 }; + } +} + + + template constexpr T smooth_neg(const T &x, S eps2) { @@ -263,6 +288,21 @@ constexpr T smooth_neg_derivative(const T &x, S eps2) } +template +constexpr T smooth_neg_second_derivative(const T &x, S eps2) +{ + // + // Returns the negative part of input "x", smoothed + // out near 0 with parameter "eps2" (unless template + // parameter "ActuallySmooth" is false, in that case + // the function returns the strict negative part + // of "x"). + // + + return -smooth_pos_second_derivative(-x, eps2); +} + + template constexpr T smooth_sign(const T &x, S eps) { @@ -468,6 +508,29 @@ constexpr T smooth_clamp_derivative(const T &x, const T1 &lo, const T2 &hi, S ep } +template +constexpr T smooth_clamp_second_derivative(const T &x, const T1 &lo, const T2 &hi, S eps2) +{ + // + // Clamps "x" to "[x_lower, x_upper]", using a squared root smooth modulation, + // unless template parameter "ActuallySmooth" is false, in + // that case we strictly clamp the value. + // + + if constexpr (ActuallySmooth) { + using std::sqrt; + + const auto Dxlo = x - lo; + const auto Dxhi = x - hi; + return S{ 0.5 } * (eps2 / (Dxlo * Dxlo + eps2) / sqrt(Dxlo * Dxlo + eps2) - eps2 / (Dxhi * Dxhi + eps2) / sqrt(Dxhi * Dxhi + eps2)); + } + else { + return T{ 0 }; + } +} + + + template constexpr T smooth_step(const T &x, S eps) { diff --git a/test/foundation/utils_test.cpp b/test/foundation/utils_test.cpp index 6314540..0dda769 100644 --- a/test/foundation/utils_test.cpp +++ b/test/foundation/utils_test.cpp @@ -5,7 +5,7 @@ #include "gtest/gtest.h" -TEST(utils_test, string_to_double_vector) +TEST(Utils_test, string_to_double_vector) { { const std::string s = "1.02324355, 5.0624692 \n -7.354634, \n 5.23035234392, 243.242532425"; @@ -37,7 +37,8 @@ TEST(utils_test, string_to_double_vector) } } -TEST(utils_test, linspace) + +TEST(Utils_test, linspace) { const auto x = linspace(0,1,11); @@ -48,7 +49,7 @@ TEST(utils_test, linspace) } -TEST(utils_test, find_closest_point) +TEST(Utils_test, find_closest_point) { // Define a polygon with its (x,y) coordinates @@ -459,7 +460,7 @@ TEST(utils_test, find_closest_point) } -TEST(utils_test, nchoosek_test) +TEST(Utils_test, nchoosek_test) { // for invalid inputs (negative, or n < k), // the result is 0 @@ -590,7 +591,7 @@ TEST(utils_test, nchoosek_test) } -TEST(utils_test, sin_cos_solve_test) +TEST(Utils_test, sin_cos_solve_test) { { const auto [_, valid] = sin_cos_solve(0., 0., 1.); (void)_; @@ -651,7 +652,7 @@ TEST(utils_test, sin_cos_solve_test) } -TEST(utils_test, sumabs_test) +TEST(Utils_test, sumabs_test) { constexpr auto n = 20000; std::vector v(n); @@ -664,7 +665,7 @@ TEST(utils_test, sumabs_test) } -TEST(utils_test, sumsqr_test) +TEST(Utils_test, sumsqr_test) { constexpr auto n = 20000; std::vector v(n); @@ -677,7 +678,7 @@ TEST(utils_test, sumsqr_test) } -TEST(utils_test, smooth_clamp_test) +TEST(Utils_test, smooth_clamp_test) { const auto lb = 0.123456; const auto ub = 0.98523; @@ -693,7 +694,7 @@ TEST(utils_test, smooth_clamp_test) } -TEST(utils_test, smooth_max_test) +TEST(Utils_test, smooth_max_test) { const auto lb = 0.123456; const auto ub = 0.98523; @@ -709,7 +710,7 @@ TEST(utils_test, smooth_max_test) } -TEST(utils_test, smooth_min_test) +TEST(Utils_test, smooth_min_test) { const auto lb = 0.123456; const auto ub = 0.98523; @@ -725,7 +726,7 @@ TEST(utils_test, smooth_min_test) } -TEST(utils_test, grid_vectors2points_flat) +TEST(Utils_test, grid_vectors2points_flat) { // // Tests the "grid_vectors2points_flat" function. @@ -786,8 +787,7 @@ TEST(utils_test, grid_vectors2points_flat) } - -TEST(utils_test, nearest_in_sorted_range_test) +TEST(Utils_test, nearest_in_sorted_range_test) { // // Tests the "nearest_in_sorted_range" function @@ -851,4 +851,112 @@ TEST(utils_test, nearest_in_sorted_range_test) EXPECT_EQ(static_cast(i), i_alt); } } -} \ No newline at end of file +} + + +TEST(Utils_test, smooth_pos_derivatives) +{ + // + // Check that the derivatives of the smooth_pos_function + // are correct + // + + using AD = CppAD::AD; + + + // tape the smooth_pos function + auto a_x = std::vector{AD{ 0 }}; + CppAD::Independent(a_x); + + auto a_y = std::vector{smooth_pos(a_x[0], scalar{ 0.01 })}; + + auto f = CppAD::ADFun{}; + f.Dependent(a_x, a_y); + + const auto x_values = linspace(-10.0, 10.0, 200); + + for (auto& x : x_values) { + + // first derivative + const auto dydx_analytical = smooth_pos_derivative(x, scalar{ 0.01 }); + const auto dydx_ad = f.Jacobian(std::vector{x})[0]; + EXPECT_DOUBLE_EQ(dydx_analytical, dydx_ad); + + // second derivative + const auto d2ydx2_analytical = smooth_pos_second_derivative(x, scalar{ 0.01 });; + const auto d2ydx2_ad = f.Hessian(std::vector{x}, std::vector{scalar{1}})[0]; + EXPECT_NEAR(d2ydx2_analytical, d2ydx2_ad, 2.0e-15); + } +} + + +TEST(Utils_test, smooth_neg_derivatives) +{ + // + // Check that the derivatives of the smooth_neg_function + // are correct + // + + using AD = CppAD::AD; + + + // tape the smooth_neg function + auto a_x = std::vector{AD{ 0 }}; + CppAD::Independent(a_x); + + auto a_y = std::vector{smooth_neg(a_x[0], scalar{ 0.01 })}; + + auto f = CppAD::ADFun{}; + f.Dependent(a_x, a_y); + + const auto x_values = linspace(-10.0, 10.0, 200); + + for (auto& x : x_values) { + + // first derivative + const auto dydx_analytical = smooth_neg_derivative(x, scalar{ 0.01 }); + const auto dydx_ad = f.Jacobian(std::vector{x})[0]; + EXPECT_DOUBLE_EQ(dydx_analytical, dydx_ad); + + // second derivative + const auto d2ydx2_analytical = smooth_neg_second_derivative(x, scalar{ 0.01 });; + const auto d2ydx2_ad = f.Hessian(std::vector{x}, std::vector{scalar{1}})[0]; + EXPECT_NEAR(d2ydx2_analytical, d2ydx2_ad, 2.0e-15); + } +} + + +TEST(Utils_test, smooth_clamp_derivatives) +{ + // + // Check that the derivatives of the smooth_clamp_function + // are correct + // + + using AD = CppAD::AD; + + + // tape the smooth_clamp function + auto a_x = std::vector{AD{ 0 }}; + CppAD::Independent(a_x); + + auto a_y = std::vector{smooth_clamp(a_x[0], scalar{-3}, scalar{5}, scalar{ 0.01 })}; + + auto f = CppAD::ADFun{}; + f.Dependent(a_x, a_y); + + const auto x_values = linspace(-10.0, 10.0, 200); + + for (auto& x : x_values) { + + // first derivative + const auto dydx_analytical = smooth_clamp_derivative(x, scalar{-3}, scalar{5}, scalar{ 0.01 }); + const auto dydx_ad = f.Jacobian(std::vector{x})[0]; + EXPECT_DOUBLE_EQ(dydx_analytical, dydx_ad); + + // second derivative + const auto d2ydx2_analytical = smooth_clamp_second_derivative(x, scalar{-3}, scalar{5}, scalar{ 0.01 });; + const auto d2ydx2_ad = f.Hessian(std::vector{x}, std::vector{scalar{1}})[0]; + EXPECT_NEAR(d2ydx2_analytical, d2ydx2_ad, 2.0e-15); + } +}