Skip to content

Commit

Permalink
[JMT] Add smooth_pos, smooth_neg and smooth_clamp second derivatives …
Browse files Browse the repository at this point in the history 
…+ test
  • Loading branch information
Juan Manzanero committed Mar 18, 2024
1 parent 32fbd3e commit d063853
Show file tree
Hide file tree
Showing 3 changed files with 194 additions and 14 deletions.
9 changes: 9 additions & 0 deletions lion/foundation/utils.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -131,12 +131,18 @@ constexpr T smooth_pos(const T &x, S eps2);
template<bool ActuallySmooth = true, typename T, typename S>
constexpr T smooth_pos_derivative(const T &x, S eps2);

template<bool ActuallySmooth = true, typename T, typename S>
constexpr T smooth_pos_second_derivative(const T &x, S eps2);

template<bool ActuallySmooth = true, typename T, typename S>
constexpr T smooth_neg(const T &x, S eps2);

template<bool ActuallySmooth = true, typename T, typename S>
constexpr T smooth_neg_derivative(const T &x, S eps2);

template<bool ActuallySmooth = true, typename T, typename S>
constexpr T smooth_neg_second_derivative(const T &x, S eps2);

template<bool ActuallySmooth = true, bool UseSinAtanFormula = true, typename T, typename S>
constexpr T smooth_sign(const T &x, S eps);

Expand All @@ -161,6 +167,9 @@ constexpr T smooth_clamp(const T &x, const T1 &lo, const T2 &hi, S eps2);
template<bool ActuallySmooth = true, typename T, typename T1, typename T2, typename S>
constexpr T smooth_clamp_derivative(const T &x, const T1 &lo, const T2 &hi, S eps2);

template<bool ActuallySmooth = true, typename T, typename T1, typename T2, typename S>
constexpr T smooth_clamp_second_derivative(const T &x, const T1 &lo, const T2 &hi, S eps2);

template<bool ActuallySmooth = true, bool UseSinAtanFormula = true, typename T, typename S>
constexpr T smooth_step(const T &x, S eps);

Expand Down
63 changes: 63 additions & 0 deletions lion/foundation/utils.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -233,6 +233,31 @@ constexpr T smooth_pos_derivative(const T &x, S eps2)
}


template<bool ActuallySmooth, typename T, typename S>
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<bool ActuallySmooth, typename T, typename S>
constexpr T smooth_neg(const T &x, S eps2)
{
Expand Down Expand Up @@ -263,6 +288,21 @@ constexpr T smooth_neg_derivative(const T &x, S eps2)
}


template<bool ActuallySmooth, typename T, typename S>
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<ActuallySmooth>(-x, eps2);
}


template<bool ActuallySmooth, bool UseSmoothAbsFormula, typename T, typename S>
constexpr T smooth_sign(const T &x, S eps)
{
Expand Down Expand Up @@ -468,6 +508,29 @@ constexpr T smooth_clamp_derivative(const T &x, const T1 &lo, const T2 &hi, S ep
}


template<bool ActuallySmooth, typename T, typename T1, typename T2, typename S>
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<bool ActuallySmooth, bool UseSmoothAbsFormula, typename T, typename S>
constexpr T smooth_step(const T &x, S eps)
{
Expand Down
136 changes: 122 additions & 14 deletions test/foundation/utils_test.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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";
Expand Down Expand Up @@ -37,7 +37,8 @@ TEST(utils_test, string_to_double_vector)
}
}

TEST(utils_test, linspace)

TEST(Utils_test, linspace)
{
const auto x = linspace<scalar>(0,1,11);

Expand All @@ -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
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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)_;
Expand Down Expand Up @@ -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<double> v(n);
Expand All @@ -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<double> v(n);
Expand All @@ -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;
Expand All @@ -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;
Expand All @@ -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;
Expand All @@ -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.
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -851,4 +851,112 @@ TEST(utils_test, nearest_in_sorted_range_test)
EXPECT_EQ(static_cast<std::size_t>(i), i_alt);
}
}
}
}


TEST(Utils_test, smooth_pos_derivatives)
{
//
// Check that the derivatives of the smooth_pos_function
// are correct
//

using AD = CppAD::AD<scalar>;


// tape the smooth_pos function
auto a_x = std::vector<AD>{AD{ 0 }};
CppAD::Independent(a_x);

auto a_y = std::vector<AD>{smooth_pos(a_x[0], scalar{ 0.01 })};

auto f = CppAD::ADFun<scalar>{};
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<scalar>{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<scalar>{x}, std::vector<scalar>{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<scalar>;


// tape the smooth_neg function
auto a_x = std::vector<AD>{AD{ 0 }};
CppAD::Independent(a_x);

auto a_y = std::vector<AD>{smooth_neg(a_x[0], scalar{ 0.01 })};

auto f = CppAD::ADFun<scalar>{};
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<scalar>{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<scalar>{x}, std::vector<scalar>{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<scalar>;


// tape the smooth_clamp function
auto a_x = std::vector<AD>{AD{ 0 }};
CppAD::Independent(a_x);

auto a_y = std::vector<AD>{smooth_clamp(a_x[0], scalar{-3}, scalar{5}, scalar{ 0.01 })};

auto f = CppAD::ADFun<scalar>{};
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<scalar>{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<scalar>{x}, std::vector<scalar>{scalar{1}})[0];
EXPECT_NEAR(d2ydx2_analytical, d2ydx2_ad, 2.0e-15);
}
}

0 comments on commit d063853

Please sign in to comment.