From a72ac58a29f5fef73647158a9540125bd4b4c492 Mon Sep 17 00:00:00 2001 From: Gonzalo-Mier Date: Mon, 17 Jun 2024 19:16:56 +0200 Subject: [PATCH] Spline lib replaced by joshhooker/CubicSplineClass lib --- README.rst | 2 +- include/fields2cover.h | 1 + include/fields2cover/utils/spline.h | 74 ++ include/spline/spline.h | 948 ------------------ .../path_planning/path_planning.cpp | 3 +- src/fields2cover/types/Path.cpp | 8 +- src/fields2cover/utils/spline.cpp | 202 ++++ swig/Fields2Cover.i | 1 + tests/cpp/utils/spline_test.cpp | 28 + 9 files changed, 312 insertions(+), 955 deletions(-) create mode 100644 include/fields2cover/utils/spline.h delete mode 100644 include/spline/spline.h create mode 100644 src/fields2cover/utils/spline.cpp create mode 100755 tests/cpp/utils/spline_test.cpp diff --git a/README.rst b/README.rst index 5d77dc82..188a5946 100644 --- a/README.rst +++ b/README.rst @@ -230,7 +230,7 @@ Credits and more info --------------------- This library is only possible thanks to `GDAL `_ who provides the basic types of this library. -Other great libraries that made Fields2Cover possible are `OR-tools `__, `hbanzhaf/steering_functions `_, `nlohmann/json `_, `leethomason/tinyxml2 `_, `ttk592/spline `_ and `alandefreitas/matplotplusplus `_ +Other great libraries that made Fields2Cover possible are `OR-tools `__, `hbanzhaf/steering_functions `_, `nlohmann/json `_, `leethomason/tinyxml2 `_, `joshhooker/CubicSplineClass `_ and `alandefreitas/matplotplusplus `_ This code repository is part of the project Fields2Cover which is (partly) financed by the Dutch Research Council (NWO). diff --git a/include/fields2cover.h b/include/fields2cover.h index 18c92888..f49776c3 100644 --- a/include/fields2cover.h +++ b/include/fields2cover.h @@ -12,6 +12,7 @@ #include "fields2cover/utils/random.h" +#include "fields2cover/utils/spline.h" #include "fields2cover/utils/transformation.h" #include "fields2cover/utils/parser.h" #include "fields2cover/utils/visualizer.h" diff --git a/include/fields2cover/utils/spline.h b/include/fields2cover/utils/spline.h new file mode 100644 index 00000000..de7257dc --- /dev/null +++ b/include/fields2cover/utils/spline.h @@ -0,0 +1,74 @@ +/* + * spline.h + * + * The original version of this file can be found at with: + * https://github.com/joshhooker/CubicSplineClass + * + * This file has MIT license. + * + * Actual version is modified to adapt to Fields2Cover requirements. + * + * --------------------------------------------------------------------- + * Copyright (c) 2017 Joshua Hooker + * + * Permission is hereby granted, free of charge, to any person obtaining + * a copy of this software and associated documentation files (the "Software"), + * to deal in the Software without restriction, including without limitation + * the rights to use, copy, modify, merge, publish, distribute, sublicense, + * and/or sell copies of the Software, and to permit persons to whom the + * Software is furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included + * in all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL + * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING + * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER + * DEALINGS IN THE SOFTWARE. + * --------------------------------------------------------------------- + * + */ + + +#ifndef FIELDS2COVER_UTILS_SPLINE_H_ +#define FIELDS2COVER_UTILS_SPLINE_H_ + +#include +#include + +namespace f2c { + +class CubicSpline { + public: + CubicSpline(); + CubicSpline(const std::vector &x, const std::vector &y, + bool monotonic = false); + + ~CubicSpline(); + + void SetPoints(const std::vector &x, const std::vector &y, + bool monotonic = false); + + double operator()(double x) const; + + + private: + size_t size_{0}; + std::vector x_vec_, y_vec_; + std::vector b_vec_, c_vec_, d_vec_; + + bool monotonic_{false}; + + void SetSpline(); + + void SetSplineCubic(); + + void check_error(bool cond, const std::string& msg) const; +}; + +} // namespace f2c + +#endif // FIELDS2COVER_UTILS_SPLINE_H_ diff --git a/include/spline/spline.h b/include/spline/spline.h deleted file mode 100644 index bedf2a43..00000000 --- a/include/spline/spline.h +++ /dev/null @@ -1,948 +0,0 @@ -/* - * spline.h - * - * simple cubic spline interpolation library without external - * dependencies - * - * --------------------------------------------------------------------- - * Copyright (C) 2011, 2014, 2016, 2021 Tino Kluge (ttk448 at gmail.com) - * - * This program is free software; you can redistribute it and/or - * modify it under the terms of the GNU General Public License - * as published by the Free Software Foundation; either version 2 - * of the License, or (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with this program. If not, see . - * --------------------------------------------------------------------- - * - */ - - -#ifndef TK_SPLINE_H -#define TK_SPLINE_H - -#include -#include -#include -#include -#include -#ifdef HAVE_SSTREAM -#include -#include -#endif // HAVE_SSTREAM - -// not ideal but disable unused-function warnings -// (we get them because we have implementations in the header file, -// and this is because we want to be able to quickly separate them -// into a cpp file if necessary) -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wunused-function" - -// unnamed namespace only because the implementation is in this -// header file and we don't want to export symbols to the obj files -namespace -{ - -namespace tk -{ - -// spline interpolation -class spline -{ -public: - // spline types - enum spline_type { - linear = 10, // linear interpolation - cspline = 30, // cubic splines (classical C^2) - cspline_hermite = 31 // cubic hermite splines (local, only C^1) - }; - - // boundary condition type for the spline end-points - enum bd_type { - first_deriv = 1, - second_deriv = 2, - not_a_knot = 3 - }; - -protected: - std::vector m_x,m_y; // x,y coordinates of points - // interpolation parameters - // f(x) = a_i + b_i*(x-x_i) + c_i*(x-x_i)^2 + d_i*(x-x_i)^3 - // where a_i = y_i, or else it won't go through grid points - std::vector m_b,m_c,m_d; // spline coefficients - double m_c0; // for left extrapolation - spline_type m_type; - bd_type m_left, m_right; - double m_left_value, m_right_value; - bool m_made_monotonic; - void set_coeffs_from_b(); // calculate c_i, d_i from b_i - size_t find_closest(double x) const; // closest idx so that m_x[idx]<=x - -public: - // default constructor: set boundary condition to be zero curvature - // at both ends, i.e. natural splines - spline(): m_type(cspline), - m_left(second_deriv), m_right(second_deriv), - m_left_value(0.0), m_right_value(0.0), m_made_monotonic(false) - { - ; - } - spline(const std::vector& X, const std::vector& Y, - spline_type type = cspline, - bool make_monotonic = false, - bd_type left = second_deriv, double left_value = 0.0, - bd_type right = second_deriv, double right_value = 0.0 - ): - m_type(type), - m_left(left), m_right(right), - m_left_value(left_value), m_right_value(right_value), - m_made_monotonic(false) // false correct here: make_monotonic() sets it - { - this->set_points(X,Y,m_type); - if(make_monotonic) { - this->make_monotonic(); - } - } - ~spline() = default; - - - // modify boundary conditions: if called it must be before set_points() - void set_boundary(bd_type left, double left_value, - bd_type right, double right_value); - - // set all data points (cubic_spline=false means linear interpolation) - void set_points(const std::vector& x, - const std::vector& y, - spline_type type=cspline); - - // adjust coefficients so that the spline becomes piecewise monotonic - // where possible - // this is done by adjusting slopes at grid points by a non-negative - // factor and this will break C^2 - // this can also break boundary conditions if adjustments need to - // be made at the boundary points - // returns false if no adjustments have been made, true otherwise - bool make_monotonic(); - - // evaluates the spline at point x - double operator() (double x) const; - double deriv(int order, double x) const; - - // solves for all x so that: spline(x) = y - std::vector solve(double y, bool ignore_extrapolation=true) const; - - // returns the input data points - std::vector get_x() const { return m_x; } - std::vector get_y() const { return m_y; } - double get_x_min() const { assert(!m_x.empty()); return m_x.front(); } - double get_x_max() const { assert(!m_x.empty()); return m_x.back(); } - -#ifdef HAVE_SSTREAM - // spline info string, i.e. spline type, boundary conditions etc. - std::string info() const; -#endif // HAVE_SSTREAM - -}; - - - -namespace internal -{ - -// band matrix solver -class band_matrix -{ -private: - std::vector< std::vector > m_upper; // upper band - std::vector< std::vector > m_lower; // lower band -public: - band_matrix() {}; // constructor - band_matrix(int dim, int n_u, int n_l); // constructor - ~band_matrix() {}; // destructor - void resize(int dim, int n_u, int n_l); // init with dim,n_u,n_l - int dim() const; // matrix dimension - int num_upper() const - { - return (int)m_upper.size()-1; - } - int num_lower() const - { - return (int)m_lower.size()-1; - } - // access operator - double & operator () (int i, int j); // write - double operator () (int i, int j) const; // read - // we can store an additional diagonal (in m_lower) - double& saved_diag(int i); - double saved_diag(int i) const; - void lu_decompose(); - std::vector r_solve(const std::vector& b) const; - std::vector l_solve(const std::vector& b) const; - std::vector lu_solve(const std::vector& b, - bool is_lu_decomposed=false); - -}; - -double get_eps(); - -std::vector solve_cubic(double a, double b, double c, double d, - int newton_iter=0); - -} // namespace internal - - - - -// --------------------------------------------------------------------- -// implementation part, which could be separated into a cpp file -// --------------------------------------------------------------------- - -// spline implementation -// ----------------------- - -void spline::set_boundary(spline::bd_type left, double left_value, - spline::bd_type right, double right_value) -{ - assert(m_x.size()==0); // set_points() must not have happened yet - m_left=left; - m_right=right; - m_left_value=left_value; - m_right_value=right_value; -} - - -void spline::set_coeffs_from_b() -{ - assert(m_x.size()==m_y.size()); - assert(m_x.size()==m_b.size()); - assert(m_x.size()>2); - size_t n=m_b.size(); - if(m_c.size()!=n) - m_c.resize(n); - if(m_d.size()!=n) - m_d.resize(n); - - for(size_t i=0; i& x, - const std::vector& y, - spline_type type) -{ - assert(x.size()==y.size()); - assert(x.size()>=3); - // not-a-knot with 3 points has many solutions - if(m_left==not_a_knot || m_right==not_a_knot) - assert(x.size()>=4); - m_type=type; - m_made_monotonic=false; - m_x=x; - m_y=y; - int n = (int) x.size(); - // check strict monotonicity of input vector x - for(int i=0; i rhs(n); - for(int i=1; i2); - bool modified = false; - const int n=(int)m_x.size(); - // make sure: input data monotonic increasing --> b_i>=0 - // input data monotonic decreasing --> b_i<=0 - for(int i=0; i=m_y[i]) && (m_y[i]>=m_y[ip1]) && m_b[i]>0.0) ) { - modified=true; - m_b[i]=0.0; - } - } - // if input data is monotonic (b[i], b[i+1], avg have all the same sign) - // ensure a sufficient criteria for monotonicity is satisfied: - // sqrt(b[i]^2+b[i+1]^2) <= 3 |avg|, with avg=(y[i+1]-y[i])/h, - for(int i=0; i=0.0 && m_b[i+1]>=0.0 && avg>0.0) || - (m_b[i]<=0.0 && m_b[i+1]<=0.0 && avg<0.0) ) { - // input data is monotonic - double r = sqrt(m_b[i]*m_b[i]+m_b[i+1]*m_b[i+1])/std::fabs(avg); - if(r>3.0) { - // sufficient criteria for monotonicity: r<=3 - // adjust b[i] and b[i+1] - modified=true; - m_b[i] *= (3.0/r); - m_b[i+1] *= (3.0/r); - } - } - } - - if(modified==true) { - set_coeffs_from_b(); - m_made_monotonic=true; - } - - return modified; -} - -// return the closest idx so that m_x[idx] <= x (return 0 if x::const_iterator it; - it=std::upper_bound(m_x.begin(),m_x.end(),x); // *it > x - size_t idx = std::max( int(it-m_x.begin())-1, 0); // m_x[idx] <= x - return idx; -} - -double spline::operator() (double x) const -{ - // polynomial evaluation using Horner's scheme - // TODO: consider more numerically accurate algorithms, e.g.: - // - Clenshaw - // - Even-Odd method by A.C.R. Newbery - // - Compensated Horner Scheme - size_t n=m_x.size(); - size_t idx=find_closest(x); - - double h=x-m_x[idx]; - double interpol; - if(xm_x[n-1]) { - // extrapolation to the right - interpol=(m_c[n-1]*h + m_b[n-1])*h + m_y[n-1]; - } else { - // interpolation - interpol=((m_d[idx]*h + m_c[idx])*h + m_b[idx])*h + m_y[idx]; - } - return interpol; -} - -double spline::deriv(int order, double x) const -{ - assert(order>0); - size_t n=m_x.size(); - size_t idx = find_closest(x); - - double h=x-m_x[idx]; - double interpol; - if(xm_x[n-1]) { - // extrapolation to the right - switch(order) { - case 1: - interpol=2.0*m_c[n-1]*h + m_b[n-1]; - break; - case 2: - interpol=2.0*m_c[n-1]; - break; - default: - interpol=0.0; - break; - } - } else { - // interpolation - switch(order) { - case 1: - interpol=(3.0*m_d[idx]*h + 2.0*m_c[idx])*h + m_b[idx]; - break; - case 2: - interpol=6.0*m_d[idx]*h + 2.0*m_c[idx]; - break; - case 3: - interpol=6.0*m_d[idx]; - break; - default: - interpol=0.0; - break; - } - } - return interpol; -} - -std::vector spline::solve(double y, bool ignore_extrapolation) const -{ - std::vector x; // roots for the entire spline - std::vector root; // roots for each piecewise cubic - const size_t n=m_x.size(); - - // left extrapolation - if(ignore_extrapolation==false) { - root = internal::solve_cubic(m_y[0]-y,m_b[0],m_c0,0.0,1); - for(size_t j=0; j0) ? (m_x[i]-m_x[i-1]) : 0.0; - double eps = internal::get_eps()*512.0*std::min(h,1.0); - if( (-eps<=root[j]) && (root[j]0 && x.back()+eps > new_root) { - x.back()=new_root; // avoid spurious duplicate roots - } else { - x.push_back(new_root); - } - } - } - } - - // right extrapolation - if(ignore_extrapolation==false) { - root = internal::solve_cubic(m_y[n-1]-y,m_b[n-1],m_c[n-1],0.0,1); - for(size_t j=0; j0); - assert(n_u>=0); - assert(n_l>=0); - m_upper.resize(n_u+1); - m_lower.resize(n_l+1); - for(size_t i=0; i0) { - return m_upper[0].size(); - } else { - return 0; - } -} - - -// defines the new operator (), so that we can access the elements -// by A(i,j), index going from i=0,...,dim()-1 -double & band_matrix::operator () (int i, int j) -{ - int k=j-i; // what band is the entry - assert( (i>=0) && (i=0) && (j diagonal, k<0 lower left part, k>0 upper right part - if(k>=0) return m_upper[k][i]; - else return m_lower[-k][i]; -} -double band_matrix::operator () (int i, int j) const -{ - int k=j-i; // what band is the entry - assert( (i>=0) && (i=0) && (j diagonal, k<0 lower left part, k>0 upper right part - if(k>=0) return m_upper[k][i]; - else return m_lower[-k][i]; -} -// second diag (used in LU decomposition), saved in m_lower -double band_matrix::saved_diag(int i) const -{ - assert( (i>=0) && (i=0) && (idim(); i++) { - assert(this->operator()(i,i)!=0.0); - this->saved_diag(i)=1.0/this->operator()(i,i); - j_min=std::max(0,i-this->num_lower()); - j_max=std::min(this->dim()-1,i+this->num_upper()); - for(int j=j_min; j<=j_max; j++) { - this->operator()(i,j) *= this->saved_diag(i); - } - this->operator()(i,i)=1.0; // prevents rounding errors - } - - // Gauss LR-Decomposition - for(int k=0; kdim(); k++) { - i_max=std::min(this->dim()-1,k+this->num_lower()); // num_lower not a mistake! - for(int i=k+1; i<=i_max; i++) { - assert(this->operator()(k,k)!=0.0); - x=-this->operator()(i,k)/this->operator()(k,k); - this->operator()(i,k)=-x; // assembly part of L - j_max=std::min(this->dim()-1,k+this->num_upper()); - for(int j=k+1; j<=j_max; j++) { - // assembly part of R - this->operator()(i,j)=this->operator()(i,j)+x*this->operator()(k,j); - } - } - } -} -// solves Ly=b -std::vector band_matrix::l_solve(const std::vector& b) const -{ - assert( this->dim()==(int)b.size() ); - std::vector x(this->dim()); - int j_start; - double sum; - for(int i=0; idim(); i++) { - sum=0; - j_start=std::max(0,i-this->num_lower()); - for(int j=j_start; joperator()(i,j)*x[j]; - x[i]=(b[i]*this->saved_diag(i)) - sum; - } - return x; -} -// solves Rx=y -std::vector band_matrix::r_solve(const std::vector& b) const -{ - assert( this->dim()==(int)b.size() ); - std::vector x(this->dim()); - int j_stop; - double sum; - for(int i=this->dim()-1; i>=0; i--) { - sum=0; - j_stop=std::min(this->dim()-1,i+this->num_upper()); - for(int j=i+1; j<=j_stop; j++) sum += this->operator()(i,j)*x[j]; - x[i]=( b[i] - sum ) / this->operator()(i,i); - } - return x; -} - -std::vector band_matrix::lu_solve(const std::vector& b, - bool is_lu_decomposed) -{ - assert( this->dim()==(int)b.size() ); - std::vector x,y; - if(is_lu_decomposed==false) { - this->lu_decompose(); - } - y=this->l_solve(b); - x=this->r_solve(y); - return x; -} - -// machine precision of a double, i.e. the successor of 1 is 1+eps -double get_eps() -{ - //return std::numeric_limits::epsilon(); // __DBL_EPSILON__ - return 2.2204460492503131e-16; // 2^-52 -} - -// solutions for a + b*x = 0 -std::vector solve_linear(double a, double b) -{ - std::vector x; // roots - if(b==0.0) { - if(a==0.0) { - // 0*x = 0 - x.resize(1); - x[0] = 0.0; // any x solves it but we need to pick one - return x; - } else { - // 0*x + ... = 0, no solution - return x; - } - } else { - x.resize(1); - x[0] = -a/b; - return x; - } -} - -// solutions for a + b*x + c*x^2 = 0 -std::vector solve_quadratic(double a, double b, double c, - int newton_iter=0) -{ - if(c==0.0) { - return solve_linear(a,b); - } - // rescale so that we solve x^2 + 2p x + q = (x+p)^2 + q - p^2 = 0 - double p=0.5*b/c; - double q=a/c; - double discr = p*p-q; - const double eps=0.5*internal::get_eps(); - double discr_err = (6.0*(p*p)+3.0*fabs(q)+fabs(discr))*eps; - - std::vector x; // roots - if(fabs(discr)<=discr_err) { - // discriminant is zero --> one root - x.resize(1); - x[0] = -p; - } else if(discr<0) { - // no root - } else { - // two roots - x.resize(2); - x[0] = -p - sqrt(discr); - x[1] = -p + sqrt(discr); - } - - // improve solution via newton steps - for(size_t i=0; i1e-8) { - x[i] -= f/f1; - } - } - } - - return x; -} - -// solutions for the cubic equation: a + b*x +c*x^2 + d*x^3 = 0 -// this is a naive implementation of the analytic solution without -// optimisation for speed or numerical accuracy -// newton_iter: number of newton iterations to improve analytical solution -// see also -// gsl: gsl_poly_solve_cubic() in solve_cubic.c -// octave: roots.m - via eigenvalues of the Frobenius companion matrix -std::vector solve_cubic(double a, double b, double c, double d, - int newton_iter) -{ - if(d==0.0) { - return solve_quadratic(a,b,c,newton_iter); - } - - // convert to normalised form: a + bx + cx^2 + x^3 = 0 - if(d!=1.0) { - a/=d; - b/=d; - c/=d; - } - - // convert to depressed cubic: z^3 - 3pz - 2q = 0 - // via substitution: z = x + c/3 - std::vector z; // roots of the depressed cubic - double p = -(1.0/3.0)*b + (1.0/9.0)*(c*c); - double r = 2.0*(c*c)-9.0*b; - double q = -0.5*a - (1.0/54.0)*(c*r); - double discr=p*p*p-q*q; // discriminant - // calculating numerical round-off errors with assumptions: - // - each operation is precise but each intermediate result x - // when stored has max error of x*eps - // - only multiplication with a power of 2 introduces no new error - // - a,b,c,d and some fractions (e.g. 1/3) have rounding errors eps - // - p_err << |p|, q_err << |q|, ... (this is violated in rare cases) - // would be more elegant to use boost::numeric::interval - const double eps = internal::get_eps(); - double p_err = eps*((3.0/3.0)*fabs(b)+(4.0/9.0)*(c*c)+fabs(p)); - double r_err = eps*(6.0*(c*c)+18.0*fabs(b)+fabs(r)); - double q_err = 0.5*fabs(a)*eps + (1.0/54.0)*fabs(c)*(r_err+fabs(r)*3.0*eps) - + fabs(q)*eps; - double discr_err = (p*p) * (3.0*p_err + fabs(p)*2.0*eps) - + fabs(q) * (2.0*q_err + fabs(q)*eps) + fabs(discr)*eps; - - // depending on the discriminant we get different solutions - if(fabs(discr)<=discr_err) { - // discriminant zero: one or two real roots - if(fabs(p)<=p_err) { - // p and q are zero: single root - z.resize(1); - z[0] = 0.0; // triple root - } else { - z.resize(2); - z[0] = 2.0*q/p; // single root - z[1] = -0.5*z[0]; // double root - } - } else if(discr>0) { - // three real roots: via trigonometric solution - z.resize(3); - double ac = (1.0/3.0) * acos( q/(p*sqrt(p)) ); - double sq = 2.0*sqrt(p); - z[0] = sq * cos(ac); - z[1] = sq * cos(ac-2.0*M_PI/3.0); - z[2] = sq * cos(ac-4.0*M_PI/3.0); - } else if (discr<0.0) { - // single real root: via Cardano's fromula - z.resize(1); - double sgnq = (q >= 0 ? 1 : -1); - double basis = fabs(q) + sqrt(-discr); - double C = sgnq * pow(basis, 1.0/3.0); // c++11 has std::cbrt() - z[0] = C + p/C; - } - for(size_t i=0; i1e-8) { - z[i] -= f/f1; - } - } - } - // ensure if a=0 we get exactly x=0 as root - // TODO: remove this fudge - if(a==0.0) { - assert(z.size()>0); // cubic should always have at least one root - double xmin=fabs(z[0]); - size_t imin=0; - for(size_t i=1; ifabs(z[i])) { - xmin=fabs(z[i]); - imin=i; - } - } - z[imin]=0.0; // replace the smallest absolute value with 0 - } - std::sort(z.begin(), z.end()); - return z; -} - - -} // namespace internal - - -} // namespace tk - - -} // namespace - -#pragma GCC diagnostic pop - -#endif /* TK_SPLINE_H */ diff --git a/src/fields2cover/path_planning/path_planning.cpp b/src/fields2cover/path_planning/path_planning.cpp index f52ba719..f4723ab4 100644 --- a/src/fields2cover/path_planning/path_planning.cpp +++ b/src/fields2cover/path_planning/path_planning.cpp @@ -4,7 +4,6 @@ // BSD-3 License //============================================================================= -#include #include #include "fields2cover/path_planning/path_planning.h" @@ -133,7 +132,7 @@ std::vector> PathPlanning::simplifyConnection( for (int i = 1; i < vp.size() - 1; ++i) { double dist_in = vp[i].distance(vp[i-1]); double dist_out = vp[i].distance(vp[i+1]); - if (dist_in == 0.0 || dist_out == 0.0){ + if (dist_in == 0.0 || dist_out == 0.0) { continue; } double d_in = min(0.5 * dist_in, safe_dist); diff --git a/src/fields2cover/types/Path.cpp b/src/fields2cover/types/Path.cpp index e3ee4298..5fd6657b 100644 --- a/src/fields2cover/types/Path.cpp +++ b/src/fields2cover/types/Path.cpp @@ -4,9 +4,9 @@ // BSD-3 License //============================================================================= -#include #include #include +#include "fields2cover/utils/spline.h" #include "fields2cover/types/Path.h" namespace f2c::types { @@ -372,9 +372,9 @@ Path& Path::populate(int number_points) { std::vector t(len.size()); std::partial_sum(len.begin(), len.end() - 1, t.begin() + 1); - tk::spline s_x(t, x); - tk::spline s_y(t, y); - tk::spline s_ang(t, ang); + CubicSpline s_x(t, x); + CubicSpline s_y(t, y); + CubicSpline s_ang(t, ang); this->states_.clear(); diff --git a/src/fields2cover/utils/spline.cpp b/src/fields2cover/utils/spline.cpp new file mode 100644 index 00000000..071787d1 --- /dev/null +++ b/src/fields2cover/utils/spline.cpp @@ -0,0 +1,202 @@ +/* + * spline.cpp + * + * The original version of this file can be found at with: + * https://github.com/joshhooker/CubicSplineClass + * + * This file has MIT license. + * + * Actual version is modified to adapt to Fields2Cover requirements. + * + * --------------------------------------------------------------------- + * Copyright (c) 2017 Joshua Hooker + * + * Permission is hereby granted, free of charge, to any person obtaining + * a copy of this software and associated documentation files (the "Software"), + * to deal in the Software without restriction, including without limitation + * the rights to use, copy, modify, merge, publish, distribute, sublicense, + * and/or sell copies of the Software, and to permit persons to whom the + * Software is furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included + * in all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL + * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING + * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER + * DEALINGS IN THE SOFTWARE. + * --------------------------------------------------------------------- + * + */ + +#include +#include +#include +#include +#include "fields2cover/utils/spline.h" + + +namespace f2c { + +CubicSpline::CubicSpline() = default; + +CubicSpline::CubicSpline( + const std::vector &x, const std::vector &y, + bool monotonic) { + check_error(x.size() == y.size(), + "In CubicSpline initialization, x vector size != y vector size\n"); + check_error(x.size() > 1, + "In CubicSpline initialization, array size must be larger than 1\n"); + size_ = x.size(); + x_vec_ = x; + y_vec_ = y; + b_vec_.resize(size_); + c_vec_.resize(size_); + d_vec_.resize(size_); + + monotonic_ = monotonic; + + SetSpline(); +} + + +CubicSpline::~CubicSpline() = default; + +void CubicSpline::SetPoints( + const std::vector &x, const std::vector &y, + bool monotonic) { + check_error(x.size() == y.size(), + "In CubicSpline SetPoints, x vector size != y vector size\n"); + check_error(x.size() > 1, + "In CubicSpline initialization, array size must be larger than 1\n"); + size_ = x.size(); + x_vec_ = x; + y_vec_ = y; + b_vec_.resize(size_); + c_vec_.resize(size_); + d_vec_.resize(size_); + + monotonic_ = monotonic; + + SetSpline(); +} + + +double CubicSpline::operator()(double x) const { + const auto xs = static_cast(x); + + int l = 0; + int h = size_; + while (l < h) { + int mid = (l + h) / 2; + if (xs <= x_vec_[mid]) { + h = mid; + } else { + l = mid + 1; + } + } + + size_t idx = l == 0 ? 0 : l - 1; + + double xi = xs - x_vec_[idx]; + double xi2 = xi * xi; + double xi3 = xi2 * xi; + + return y_vec_[idx] + b_vec_[idx] * xi + c_vec_[idx] * xi2 + d_vec_[idx] * xi3; +} + +void CubicSpline::SetSpline() { + SetSplineCubic(); +} + +void CubicSpline::SetSplineCubic() { + std::vector h(size_), alpha(size_), l(size_), z(size_), u(size_); + + h[0] = x_vec_[1] - x_vec_[0]; + l[0] = 1.; + u[0] = 0.; + z[0] = 0.; + l[size_ - 1] = 1.; + u[size_ - 1] = 0.; + c_vec_[size_ - 1] = 0.; + + for (unsigned int i = 1; i < size_ - 1; i++) { + check_error(x_vec_[i + 1] > x_vec_[i], + "In CubicSpline SetSpline, x array is not sorted" + "from smallest to largest\n"); + h[i] = x_vec_[i + 1] - x_vec_[i]; + + alpha[i] = 3. / h[i] * (y_vec_[i + 1] - y_vec_[i]) - 3. / + h[i - 1] * (y_vec_[i] - y_vec_[i - 1]); + l[i] = 2. * (x_vec_[i + 1] - x_vec_[i - 1]) - h[i - 1] * u[i - 1]; + u[i] = h[i] / l[i]; + z[i] = (alpha[i] - h[i - 1] * z[i - 1]) / l[i]; + } + + for (int i = size_ - 2; i > -1; i--) { + c_vec_[i] = z[i] - u[i] * c_vec_[i + 1]; + b_vec_[i] = + (y_vec_[i + 1] - y_vec_[i]) / h[i] - h[i] * (c_vec_[i + 1] + + 2. * c_vec_[i]) / 3.; + d_vec_[i] = (c_vec_[i + 1] - c_vec_[i]) / (3. * h[i]); + } + + // Monotonic correction + if (monotonic_) { + bool changed = false; + for (unsigned int i = 0; i < size_; i++) { + int i_low = std::max(static_cast(i - 1), 0); + int i_high = + std::min(static_cast(i + 1), static_cast(size_) - 1); + + if (((y_vec_[i_low] <= y_vec_[i]) && (y_vec_[i] <= y_vec_[i_high]) && + b_vec_[i] < 0.0) || + ((y_vec_[i_low] >= y_vec_[i]) && (y_vec_[i] >= y_vec_[i_high]) && + b_vec_[i] > 0.0)) { + b_vec_[i] = 0.0; + changed = true; + } + + double slope = (y_vec_[i_high] - y_vec_[i]) / h[i]; + if (slope == 0.0 && (b_vec_[i] != 0.0 || b_vec_[i + 1] != 0.0)) { + b_vec_[i] = 0.0; + b_vec_[i + 1] = 0.0; + changed = true; + } else { + double r = sqrt(b_vec_[i] * b_vec_[i] + b_vec_[i + 1] * b_vec_[i + 1]) / + fabs(slope); + if (r > 3.0) { + b_vec_[i] *= 3.0 / r; + b_vec_[i + 1] *= 3.0 / r; + changed = true; + } + } + } + + if (changed) { + for (unsigned int i = 0; i < size_; i++) { + double inv_h = 1.0 / h[i]; + c_vec_[i] = inv_h * (3.0 * inv_h * (y_vec_[i + 1] - y_vec_[i]) + - 2.0 * b_vec_[i] - b_vec_[i + 1]); + d_vec_[i] = + inv_h * (inv_h * (b_vec_[i + 1] - b_vec_[i]) - 2.0 * c_vec_[i]) / 3.0; + } + } + } + + d_vec_[0] = 0.; + d_vec_[size_ - 1] = 0.; +} + + +void CubicSpline::check_error(bool cond, const std::string& msg) const { + if (!cond) { + throw std::out_of_range(msg); + } +} + +} // namespace f2c + diff --git a/swig/Fields2Cover.i b/swig/Fields2Cover.i index 5cac0047..9fde45df 100644 --- a/swig/Fields2Cover.i +++ b/swig/Fields2Cover.i @@ -178,6 +178,7 @@ typedef long unsigned int size_t; %template(VectorPathSectionType) std::vector; %include "fields2cover/utils/random.h" +%include "fields2cover/utils/spline.h" %include "fields2cover/utils/parser.h" %include "fields2cover/utils/visualizer.h" diff --git a/tests/cpp/utils/spline_test.cpp b/tests/cpp/utils/spline_test.cpp new file mode 100755 index 00000000..d582e142 --- /dev/null +++ b/tests/cpp/utils/spline_test.cpp @@ -0,0 +1,28 @@ +//============================================================================= +// Copyright (C) 2021-2024 Wageningen University - All Rights Reserved +// Author: Gonzalo Mier +// BSD-3 License +//============================================================================= + +#include +#include "fields2cover/utils/spline.h" + +TEST(fields2cover_utils_spline, init) { + std::vector x {1, 2, 4, 5}; + std::vector y1 {1, 2, 4, 5}; + std::vector y2 {-10, -20, -40, -50}; + std::vector y3 {0, 1, 1, 0}; + + f2c::CubicSpline cs1(x, y1); + EXPECT_NEAR(cs1(3), 3.0, 1e-3); + + f2c::CubicSpline cs2(x, y2); + EXPECT_NEAR(cs2(3), -30.0, 1e-3); + + f2c::CubicSpline cs3(x, y3); + EXPECT_GT(cs3(3), 1.0); + EXPECT_LT(cs3(3), 2.0); + EXPECT_NEAR(cs3(1.5), cs3(1.5), 1e-3); +} + +