diff --git a/resources/ui_layout/default/print.ui b/resources/ui_layout/default/print.ui index 8db71216cd6..41d64848fa2 100644 --- a/resources/ui_layout/default/print.ui +++ b/resources/ui_layout/default/print.ui @@ -226,6 +226,9 @@ group:Advanced Infill options setting:label_width$8:width$5:fill_smooth_distribution setting:label_width$26:label$Spacing between ironing lines:width$5:sidetext_width$7:fill_smooth_width end_line +group:Small Area Infill Flow Compensation (beta) + setting:small_area_infill_flow_compensation + setting:height$15:small_area_infill_flow_compensation_model group:title_width$19:Ironing post-process (This will go on top of infills and perimeters) line:Enable ironing post-process setting:label$_:sidetext_width$0:ironing diff --git a/resources/ui_layout/example/print.ui b/resources/ui_layout/example/print.ui index 5d90c54bdaf..d737804a976 100644 --- a/resources/ui_layout/example/print.ui +++ b/resources/ui_layout/example/print.ui @@ -214,6 +214,9 @@ group:Advanced Infill options setting:label_width$8:width$5:fill_smooth_distribution setting:label_width$26:label$Spacing between ironing lines:width$5:sidetext_width$7:fill_smooth_width end_line +group:Small Area Infill Flow Compensation (beta) + setting:small_area_infill_flow_compensation + setting:height$15:small_area_infill_flow_compensation_model group:title_width$19:Ironing post-process (This will go on top of infills and perimeters) line:Enable ironing post-process setting:label$_:sidetext_width$0:ironing diff --git a/src/libslic3r/CMakeLists.txt b/src/libslic3r/CMakeLists.txt index 84be9c12e5c..5126d18799f 100644 --- a/src/libslic3r/CMakeLists.txt +++ b/src/libslic3r/CMakeLists.txt @@ -141,6 +141,8 @@ add_library(libslic3r STATIC GCode/GCodeProcessor.hpp GCode/AvoidCrossingPerimeters.cpp GCode/AvoidCrossingPerimeters.hpp + GCode/SmallAreaInfillFlowCompensator.cpp + GCode/SmallAreaInfillFlowCompensator.hpp GCode.cpp GCode.hpp GCodeReader.cpp diff --git a/src/libslic3r/GCode.cpp b/src/libslic3r/GCode.cpp index 840fcd16c38..764e9eb8d47 100644 --- a/src/libslic3r/GCode.cpp +++ b/src/libslic3r/GCode.cpp @@ -8,6 +8,7 @@ #include "GCode/PrintExtents.hpp" #include "GCode/Thumbnails.hpp" #include "GCode/WipeTower.hpp" +#include "GCode/SmallAreaInfillFlowCompensator.hpp" #include "ShortestPath.hpp" #include "PrintConfig.hpp" #include "Utils.hpp" @@ -5496,15 +5497,28 @@ std::vector cut_corner_cache = { 0.381436735764648,0.398363940736199,0.416256777189962,0.435193636891737,0.455261618934834 }; -void GCode::_extrude_line(std::string& gcode_str, const Line& line, const double e_per_mm, const std::string& comment) { +void GCode::_extrude_line(std::string& gcode_str, const Line& line, const double e_per_mm, const std::string& comment, + ExtrusionRole role) { if (line.a.coincides_with_epsilon(line.b)) { assert(false); // todo: investigate if it happens (it happens in perimeters) return; } + std::string comment_copy = comment; + double unscaled_line_length = unscaled(line.length()); + double extrusion_value = e_per_mm * unscaled_line_length; + if (!this->on_first_layer() && m_small_area_infill_flow_compensator && m_config.small_area_infill_flow_compensation.value) { + double new_extrusion_value = m_small_area_infill_flow_compensator ->modify_flow(unscaled_line_length, extrusion_value, role); + if (new_extrusion_value > 0.0 && new_extrusion_value != extrusion_value) { + extrusion_value = new_extrusion_value; + if (m_config.gcode_comments) { + comment_copy += Slic3r::format(_(L(" | Old Flow Value: %0.5f Length: %0.5f")), extrusion_value, unscaled_line_length); + } + } + } gcode_str += m_writer.extrude_to_xy( this->point_to_gcode(line.b), - e_per_mm * unscaled(line.length()), - comment); + extrusion_value, + comment_copy); } void GCode::_extrude_line_cut_corner(std::string& gcode_str, const Line& line, const double e_per_mm, const std::string& comment, Point& last_pos, const double path_width) { @@ -5604,6 +5618,11 @@ std::string GCode::_extrude(const ExtrusionPath &path, const std::string &descri comment); }; + if (m_config.small_area_infill_flow_compensation.value && + !m_config.small_area_infill_flow_compensation_model.empty()) { + m_small_area_infill_flow_compensator = std::make_unique(m_config); + } + // calculate extrusion length per distance unit double e_per_mm = path.mm3_per_mm * m_writer.tool()->e_per_mm3() @@ -5623,7 +5642,7 @@ std::string GCode::_extrude(const ExtrusionPath &path, const std::string &descri for (const Line& line : path.polyline.lines()) { if (path.role() != erExternalPerimeter || config().external_perimeter_cut_corners.value == 0) { // normal & legacy pathcode - _extrude_line(gcode, line, e_per_mm, comment); + _extrude_line(gcode, line, e_per_mm, comment, path.role()); } else { _extrude_line_cut_corner(gcode, line, e_per_mm, comment, last_pos, path.width); } @@ -5641,7 +5660,7 @@ std::string GCode::_extrude(const ExtrusionPath &path, const std::string &descri const Line line = Line(path.polyline.get_points()[point_index - 1], path.polyline.get_points()[point_index]); if (path.role() != erExternalPerimeter || config().external_perimeter_cut_corners.value == 0) { // normal & legacy pathcode - _extrude_line(gcode, line, e_per_mm, comment); + _extrude_line(gcode, line, e_per_mm, comment, path.role()); } else { _extrude_line_cut_corner(gcode, line, e_per_mm, comment, last_pos, path.width); } diff --git a/src/libslic3r/GCode.hpp b/src/libslic3r/GCode.hpp index 607f6470db3..0caec824ff4 100644 --- a/src/libslic3r/GCode.hpp +++ b/src/libslic3r/GCode.hpp @@ -20,6 +20,7 @@ #include "GCode/SeamPlacer.hpp" #include "GCode/GCodeProcessor.hpp" #include "GCode/ThumbnailData.hpp" +#include "GCode/SmallAreaInfillFlowCompensator.hpp" #include #include @@ -485,6 +486,7 @@ class GCode : ExtrusionVisitorConst { std::unique_ptr m_find_replace; std::unique_ptr m_pressure_equalizer; std::unique_ptr m_wipe_tower; + std::unique_ptr m_small_area_infill_flow_compensator; // Heights (print_z) at which the skirt has already been extruded. std::vector m_skirt_done; @@ -515,7 +517,7 @@ class GCode : ExtrusionVisitorConst { std::function m_throw_if_canceled = [](){}; std::string _extrude(const ExtrusionPath &path, const std::string &description, double speed = -1); - void _extrude_line(std::string& gcode_str, const Line& line, const double e_per_mm, const std::string& comment); + void _extrude_line(std::string& gcode_str, const Line& line, const double e_per_mm, const std::string& comment, ExtrusionRole role); void _extrude_line_cut_corner(std::string& gcode_str, const Line& line, const double e_per_mm, const std::string& comment, Point& last_pos, const double path_width); std::string _before_extrude(const ExtrusionPath &path, const std::string &description, double speed = -1); double_t _compute_speed_mm_per_sec(const ExtrusionPath& path, double speed = -1); diff --git a/src/libslic3r/GCode/SmallAreaInfillFlowCompensator.cpp b/src/libslic3r/GCode/SmallAreaInfillFlowCompensator.cpp new file mode 100644 index 00000000000..60c3a0a6c0d --- /dev/null +++ b/src/libslic3r/GCode/SmallAreaInfillFlowCompensator.cpp @@ -0,0 +1,103 @@ +#include "SmallAreaInfillFlowCompensator.hpp" + +#include "../libslic3r.h" + +namespace Slic3r{ + +bool nearly_equal_floating_point(double a, double b) { + return std::nextafter(a, std::numeric_limits::lowest()) <= b && + std::nextafter(a, std::numeric_limits::max()) >= b; +} + +void SmallAreaInfillFlowCompensator::check_model_parameter_correctness() { + if (extrusionLengths.empty()) { + throw Slic3r::InvalidArgument( + "Small area infill compensation model is misconfigured: no lengths have been set" + ); + } + if (flowCompensationFactors.empty()) { + throw Slic3r::InvalidArgument( + "Small area infill compensation model is misconfigured: no compensation factors have been set" + ); + } + + if (extrusionLengths.size() != flowCompensationFactors.size()) { + throw Slic3r::InvalidArgument("Small area infill compensation model is misconfigured: " + "Different size of lengths and compensation factors"); + } + + if (!nearly_equal_floating_point(extrusionLengths[0], 0.0)) { + throw Slic3r::InvalidArgument( + "First extrusion length for small area infill compensation model must be 0" + ); + } + + for (int i = 1; i < extrusionLengths.size(); i++) { + if (nearly_equal_floating_point(extrusionLengths[i], 0.0)) { + throw Slic3r::InvalidArgument("Only the first extrusion length for small area " + "infill compensation model can be 0"); + } + if (extrusionLengths[i] <= extrusionLengths[i - 1]) { + throw Slic3r::InvalidArgument( + "Extrusion lengths for subsequent points must be in increasing order" + ); + } + } + + if (!nearly_equal_floating_point(flowCompensationFactors.back(), 1.0)) { + throw Slic3r::InvalidArgument( + "Final compensation factor for small area infill flow compensation model must be 1.0" + ); + } +} + +void SmallAreaInfillFlowCompensator::read_config_parameters(const Slic3r::FullPrintConfig& config) { + for (auto &line : config.small_area_infill_flow_compensation_model.values) { + std::istringstream iss(line); + std::string value_str; + double extrusion_length = 0.0; + + if (std::getline(iss, value_str, ',')) { + try { + extrusion_length = std::stod(value_str); + if (std::getline(iss, value_str, ',')) { + extrusionLengths.push_back(extrusion_length); + flowCompensationFactors.push_back(std::stod(value_str)); + } + } catch (...) { + std::stringstream ss; + ss << "Error parsing data point in small area infill compensation model:" << line + << std::endl; + + throw Slic3r::InvalidArgument(ss.str()); + } + } + } +} + + +SmallAreaInfillFlowCompensator::SmallAreaInfillFlowCompensator(const Slic3r::FullPrintConfig& config) { + read_config_parameters(config); + check_model_parameter_correctness(); + flowModel.set_points(extrusionLengths, flowCompensationFactors); +} + +double SmallAreaInfillFlowCompensator::flow_comp_model(const double line_length) { + if (line_length == 0 || line_length > max_modified_length()) { + return 1.0; + } + + return flowModel(line_length); +} + +double SmallAreaInfillFlowCompensator::modify_flow( + const double line_length, const double dE, const ExtrusionRole role +) { + if (role == ExtrusionRole::erSolidInfill || role == ExtrusionRole::erTopSolidInfill) { + return dE * flow_comp_model(line_length); + } + + return dE; +} + +} // namespace Slic3r \ No newline at end of file diff --git a/src/libslic3r/GCode/SmallAreaInfillFlowCompensator.hpp b/src/libslic3r/GCode/SmallAreaInfillFlowCompensator.hpp new file mode 100644 index 00000000000..a67ca0ac3d1 --- /dev/null +++ b/src/libslic3r/GCode/SmallAreaInfillFlowCompensator.hpp @@ -0,0 +1,38 @@ +#ifndef slic3r_SmallAreaInfillFlowCompensator_hpp_ +#define slic3r_SmallAreaInfillFlowCompensator_hpp_ + +#include "../libslic3r.h" +#include "../PrintConfig.hpp" +#include "../ExtrusionEntity.hpp" +#include "spline/spline.h" + +namespace Slic3r { + + +class SmallAreaInfillFlowCompensator +{ +private: + // Model points + std::vector extrusionLengths; + std::vector flowCompensationFactors; + + tk::spline flowModel; + +private: + double flow_comp_model(const double line_length); + + double max_modified_length() { return extrusionLengths.back(); } + + void check_model_parameter_correctness(); + + void read_config_parameters(const Slic3r::FullPrintConfig& config); + +public: + explicit SmallAreaInfillFlowCompensator(const Slic3r::FullPrintConfig& config); + + double modify_flow(const double line_length, const double dE, const ExtrusionRole role); +}; + +} // namespace Slic3r + +#endif /* slic3r_SmallAreaInfillFlowCompensator_hpp_ */ diff --git a/src/libslic3r/Preset.cpp b/src/libslic3r/Preset.cpp index ca667080cfa..d2e4fe8f647 100644 --- a/src/libslic3r/Preset.cpp +++ b/src/libslic3r/Preset.cpp @@ -723,7 +723,8 @@ static std::vector s_Preset_print_options { "milling_speed", //Arachne "perimeter_generator", "wall_transition_length", "wall_transition_filter_deviation", "wall_transition_angle", - "wall_distribution_count", "min_feature_size", "min_bead_width" + "wall_distribution_count", "min_feature_size", "min_bead_width", + "small_area_infill_flow_compensation", "small_area_infill_flow_compensation_model" }; static std::vector s_Preset_filament_options { diff --git a/src/libslic3r/PrintConfig.cpp b/src/libslic3r/PrintConfig.cpp index 058bbb0a2fa..bace96ca9ff 100644 --- a/src/libslic3r/PrintConfig.cpp +++ b/src/libslic3r/PrintConfig.cpp @@ -2504,6 +2504,30 @@ void PrintConfigDef::init_fff_params() def->sidetext = L("%"); def->set_default_value(new ConfigOptionPercent(10)); + def = this->add("small_area_infill_flow_compensation", coBool); + def->label = L("Enable small area flow compensation"); + def->category = OptionCategory::infill; + def->tooltip = L("Enable flow compensation for small infill areas"); + def->mode = comExpert | comSuSi; + def->set_default_value(new ConfigOptionBool(false)); + + def = this->add("small_area_infill_flow_compensation_model", coStrings); + def->label = L("Flow Compensation Model"); + def->category = OptionCategory::infill; + def->tooltip = L("Flow Compensation Model, used to adjust the flow for small infill " + "areas. The model is expressed as a comma separated pair of values for " + "extrusion length and flow correction factors, one per line, in the " + "following format: \"1.234,5.678\""); + def->mode = comExpert | comSuSi; + def->gui_flags = "serialized"; + def->multiline = true; + def->full_width = false; + def->height = 15; + def->set_default_value(new ConfigOptionStrings{ + "0,0", "0.2,0.4444", "0.4,0.6145", "0.6,0.7059", "0.8,0.7619", "1.5,0.8571", + "2,0.8889", "3,0.9231", "5,0.9520", "10,1" + }); + def = this->add("first_layer_acceleration", coFloatOrPercent); def->label = L("Max"); def->full_label = L("First layer acceleration"); @@ -8249,6 +8273,8 @@ std::unordered_set prusa_export_to_remove_keys = { "skirt_brim", "skirt_distance_from_brim", "skirt_extrusion_width", +"small_area_infill_flow_compensation" +"small_area_infill_flow_compensation_model" "small_perimeter_max_length", "small_perimeter_min_length", "solid_fill_pattern", diff --git a/src/libslic3r/PrintConfig.hpp b/src/libslic3r/PrintConfig.hpp index d08278c77e1..c0afb4e8069 100644 --- a/src/libslic3r/PrintConfig.hpp +++ b/src/libslic3r/PrintConfig.hpp @@ -808,6 +808,7 @@ PRINT_CONFIG_CLASS_DEFINE( ((ConfigOptionFloat, xy_size_compensation)) ((ConfigOptionFloat, xy_inner_size_compensation)) ((ConfigOptionBool, wipe_into_objects)) + ((ConfigOptionBool, small_area_infill_flow_compensation)) ) // This object is mapped to Perl as Slic3r::Config::PrintRegion. @@ -1287,6 +1288,7 @@ PRINT_CONFIG_CLASS_DERIVED_DEFINE( ((ConfigOptionFloats, wiping_volumes_extruders)) ((ConfigOptionFloat, z_offset)) ((ConfigOptionFloat, init_z_rotate)) + ((ConfigOptionStrings, small_area_infill_flow_compensation_model)) ) diff --git a/src/libslic3r/PrintObject.cpp b/src/libslic3r/PrintObject.cpp index 18d68c51f4a..43a16d5b41b 100644 --- a/src/libslic3r/PrintObject.cpp +++ b/src/libslic3r/PrintObject.cpp @@ -866,6 +866,9 @@ bool PrintObject::invalidate_state_by_config_options( || opt_key == "perimeter_loop" || opt_key == "perimeter_loop_seam") { steps.emplace_back(posPerimeters); + } else if (opt_key == "small_area_infill_flow_compensation" + || opt_key == "small_area_infill_flow_compensation_model") { + steps.emplace_back(posSlice); } else if ( opt_key == "gap_fill_enabled" || opt_key == "gap_fill_speed") { diff --git a/src/slic3r/GUI/ConfigManipulation.cpp b/src/slic3r/GUI/ConfigManipulation.cpp index 741a97146d9..c09cc2e2c8e 100644 --- a/src/slic3r/GUI/ConfigManipulation.cpp +++ b/src/slic3r/GUI/ConfigManipulation.cpp @@ -378,6 +378,9 @@ void ConfigManipulation::toggle_print_fff_options(DynamicPrintConfig* config) toggle_field("perimeter_loop_seam", config->opt_bool("perimeter_loop")); + bool have_small_area_infill_flow_compensation = config->opt_bool("small_area_infill_flow_compensation"); + toggle_field("small_area_infill_flow_compensation_model", have_small_area_infill_flow_compensation); + bool have_notch = have_perimeters && (config->option("seam_notch_all")->get_float() != 0 || config->option("seam_notch_inner")->get_float() != 0 || config->option("seam_notch_outer")->get_float() != 0); diff --git a/src/spline/spline.h b/src/spline/spline.h new file mode 100644 index 00000000000..ad035cceaca --- /dev/null +++ b/src/spline/spline.h @@ -0,0 +1,885 @@ +/* + * 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(); + } + } + + // 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> m_upper; // upper band + 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 < n - 1; i++) { + const double h = m_x[i + 1] - m_x[i]; + // from continuity and differentiability condition + m_c[i] = (3.0 * (m_y[i + 1] - m_y[i]) / h - (2.0 * m_b[i] + m_b[i + 1])) / h; + // from differentiability condition + m_d[i] = ((m_b[i + 1] - m_b[i]) / (3.0 * h) - 2.0 / 3.0 * m_c[i]) / h; + } + + // for left extrapolation coefficients + m_c0 = (m_left == first_deriv) ? 0.0 : m_c[0]; +} + +void spline::set_points( + const std::vector &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 < n - 1; i++) { + assert(m_x[i] < m_x[i + 1]); + } + + if (type == linear) { + // linear interpolation + m_d.resize(n); + m_c.resize(n); + m_b.resize(n); + for (int i = 0; i < n - 1; i++) { + m_d[i] = 0.0; + m_c[i] = 0.0; + m_b[i] = (m_y[i + 1] - m_y[i]) / (m_x[i + 1] - m_x[i]); + } + // ignore boundary conditions, set slope equal to the last segment + m_b[n - 1] = m_b[n - 2]; + m_c[n - 1] = 0.0; + m_d[n - 1] = 0.0; + } else if (type == cspline) { + // classical cubic splines which are C^2 (twice cont differentiable) + // this requires solving an equation system + + // setting up the matrix and right hand side of the equation system + // for the parameters b[] + int n_upper = (m_left == spline::not_a_knot) ? 2 : 1; + int n_lower = (m_right == spline::not_a_knot) ? 2 : 1; + internal::band_matrix A(n, n_upper, n_lower); + std::vector rhs(n); + for (int i = 1; i < n - 1; i++) { + A(i, i - 1) = 1.0 / 3.0 * (x[i] - x[i - 1]); + A(i, i) = 2.0 / 3.0 * (x[i + 1] - x[i - 1]); + A(i, i + 1) = 1.0 / 3.0 * (x[i + 1] - x[i]); + rhs[i] = (y[i + 1] - y[i]) / (x[i + 1] - x[i]) - (y[i] - y[i - 1]) / (x[i] - x[i - 1]); + } + // boundary conditions + if (m_left == spline::second_deriv) { + // 2*c[0] = f'' + A(0, 0) = 2.0; + A(0, 1) = 0.0; + rhs[0] = m_left_value; + } else if (m_left == spline::first_deriv) { + // b[0] = f', needs to be re-expressed in terms of c: + // (2c[0]+c[1])(x[1]-x[0]) = 3 ((y[1]-y[0])/(x[1]-x[0]) - f') + A(0, 0) = 2.0 * (x[1] - x[0]); + A(0, 1) = 1.0 * (x[1] - x[0]); + rhs[0] = 3.0 * ((y[1] - y[0]) / (x[1] - x[0]) - m_left_value); + } else if (m_left == spline::not_a_knot) { + // f'''(x[1]) exists, i.e. d[0]=d[1], or re-expressed in c: + // -h1*c[0] + (h0+h1)*c[1] - h0*c[2] = 0 + A(0, 0) = -(x[2] - x[1]); + A(0, 1) = x[2] - x[0]; + A(0, 2) = -(x[1] - x[0]); + rhs[0] = 0.0; + } else { + assert(false); + } + if (m_right == spline::second_deriv) { + // 2*c[n-1] = f'' + A(n - 1, n - 1) = 2.0; + A(n - 1, n - 2) = 0.0; + rhs[n - 1] = m_right_value; + } else if (m_right == spline::first_deriv) { + // b[n-1] = f', needs to be re-expressed in terms of c: + // (c[n-2]+2c[n-1])(x[n-1]-x[n-2]) + // = 3 (f' - (y[n-1]-y[n-2])/(x[n-1]-x[n-2])) + A(n - 1, n - 1) = 2.0 * (x[n - 1] - x[n - 2]); + A(n - 1, n - 2) = 1.0 * (x[n - 1] - x[n - 2]); + rhs[n - 1] = 3.0 * (m_right_value - (y[n - 1] - y[n - 2]) / (x[n - 1] - x[n - 2])); + } else if (m_right == spline::not_a_knot) { + // f'''(x[n-2]) exists, i.e. d[n-3]=d[n-2], or re-expressed in c: + // -h_{n-2}*c[n-3] + (h_{n-3}+h_{n-2})*c[n-2] - h_{n-3}*c[n-1] = 0 + A(n - 1, n - 3) = -(x[n - 1] - x[n - 2]); + A(n - 1, n - 2) = x[n - 1] - x[n - 3]; + A(n - 1, n - 1) = -(x[n - 2] - x[n - 3]); + rhs[0] = 0.0; + } else { + assert(false); + } + + // solve the equation system to obtain the parameters c[] + m_c = A.lu_solve(rhs); + + // calculate parameters b[] and d[] based on c[] + m_d.resize(n); + m_b.resize(n); + for (int i = 0; i < n - 1; i++) { + m_d[i] = 1.0 / 3.0 * (m_c[i + 1] - m_c[i]) / (x[i + 1] - x[i]); + m_b[i] = (y[i + 1] - y[i]) / (x[i + 1] - x[i]) - + 1.0 / 3.0 * (2.0 * m_c[i] + m_c[i + 1]) * (x[i + 1] - x[i]); + } + // for the right extrapolation coefficients (zero cubic term) + // f_{n-1}(x) = y_{n-1} + b*(x-x_{n-1}) + c*(x-x_{n-1})^2 + double h = x[n - 1] - x[n - 2]; + // m_c[n-1] is determined by the boundary condition + m_d[n - 1] = 0.0; + m_b[n - 1] = 3.0 * m_d[n - 2] * h * h + 2.0 * m_c[n - 2] * h + + m_b[n - 2]; // = f'_{n-2}(x_{n-1}) + if (m_right == first_deriv) + m_c[n - 1] = 0.0; // force linear extrapolation + + } else if (type == cspline_hermite) { + // hermite cubic splines which are C^1 (cont. differentiable) + // and derivatives are specified on each grid point + // (here we use 3-point finite differences) + m_b.resize(n); + m_c.resize(n); + m_d.resize(n); + // set b to match 1st order derivative finite difference + for (int i = 1; i < n - 1; i++) { + const double h = m_x[i + 1] - m_x[i]; + const double hl = m_x[i] - m_x[i - 1]; + m_b[i] = -h / (hl * (hl + h)) * m_y[i - 1] + (h - hl) / (hl * h) * m_y[i] + + hl / (h * (hl + h)) * m_y[i + 1]; + } + // boundary conditions determine b[0] and b[n-1] + if (m_left == first_deriv) { + m_b[0] = m_left_value; + } else if (m_left == second_deriv) { + const double h = m_x[1] - m_x[0]; + m_b[0] = 0.5 * (-m_b[1] - 0.5 * m_left_value * h + 3.0 * (m_y[1] - m_y[0]) / h); + } else if (m_left == not_a_knot) { + // f''' continuous at x[1] + const double h0 = m_x[1] - m_x[0]; + const double h1 = m_x[2] - m_x[1]; + m_b[0] = -m_b[1] + 2.0 * (m_y[1] - m_y[0]) / h0 + + h0 * h0 / (h1 * h1) * (m_b[1] + m_b[2] - 2.0 * (m_y[2] - m_y[1]) / h1); + } else { + assert(false); + } + if (m_right == first_deriv) { + m_b[n - 1] = m_right_value; + m_c[n - 1] = 0.0; + } else if (m_right == second_deriv) { + const double h = m_x[n - 1] - m_x[n - 2]; + m_b[n - 1] = 0.5 * + (-m_b[n - 2] + 0.5 * m_right_value * h + 3.0 * (m_y[n - 1] - m_y[n - 2]) / h); + m_c[n - 1] = 0.5 * m_right_value; + } else if (m_right == not_a_knot) { + // f''' continuous at x[n-2] + const double h0 = m_x[n - 2] - m_x[n - 3]; + const double h1 = m_x[n - 1] - m_x[n - 2]; + m_b[n - 1] = -m_b[n - 2] + 2.0 * (m_y[n - 1] - m_y[n - 2]) / h1 + + h1 * h1 / (h0 * h0) * + (m_b[n - 3] + m_b[n - 2] - 2.0 * (m_y[n - 2] - m_y[n - 3]) / h0); + // f'' continuous at x[n-1]: c[n-1] = 3*d[n-2]*h[n-2] + c[n-1] + m_c[n - 1] = (m_b[n - 2] + 2.0 * m_b[n - 1]) / h1 - + 3.0 * (m_y[n - 1] - m_y[n - 2]) / (h1 * h1); + } else { + assert(false); + } + m_d[n - 1] = 0.0; + + // parameters c and d are determined by continuity and differentiability + set_coeffs_from_b(); + + } else { + assert(false); + } + + // for left extrapolation coefficients + m_c0 = (m_left == first_deriv) ? 0.0 : m_c[0]; +} + +bool spline::make_monotonic() { + assert(m_x.size() == m_y.size()); + assert(m_x.size() == m_b.size()); + assert(m_x.size() > 2); + 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 < n; i++) { + int im1 = std::max(i - 1, 0); + int ip1 = std::min(i + 1, n - 1); + if (((m_y[im1] <= m_y[i]) && (m_y[i] <= m_y[ip1]) && m_b[i] < 0.0) || + ((m_y[im1] >= 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 < n - 1; i++) { + double h = m_x[i + 1] - m_x[i]; + double avg = (m_y[i + 1] - m_y[i]) / h; + if (avg == 0.0 && (m_b[i] != 0.0 || m_b[i + 1] != 0.0)) { + modified = true; + m_b[i] = 0.0; + m_b[i + 1] = 0.0; + } else if ((m_b[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 (x < m_x[0]) { + // extrapolation to the left + interpol = (m_c0 * h + m_b[0]) * h + m_y[0]; + } else if (x > m_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 (x < m_x[0]) { + // extrapolation to the left + switch (order) { + case 1: interpol = 2.0 * m_c0 * h + m_b[0]; break; + case 2: interpol = 2.0 * m_c0; break; + default: interpol = 0.0; break; + } + } else if (x > m_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; j < root.size(); j++) { + if (root[j] < 0.0) { + x.push_back(m_x[0] + root[j]); + } + } + } + + // brute force check if piecewise cubic has roots in their resp. segment + // TODO: make more efficient + for (size_t i = 0; i < n - 1; i++) { + root = internal::solve_cubic(m_y[i] - y, m_b[i], m_c[i], m_d[i], 1); + for (size_t j = 0; j < root.size(); j++) { + double h = (i > 0) ? (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] < m_x[i + 1] - m_x[i])) { + double new_root = m_x[i] + root[j]; + if (x.size() > 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; j < root.size(); j++) { + if (0.0 <= root[j]) { + x.push_back(m_x[n - 1] + root[j]); + } + } + } + + return x; +}; + +#ifdef HAVE_SSTREAM +std::string spline::info() const { + std::stringstream ss; + ss << "type " << m_type << ", left boundary deriv " << m_left << " = "; + ss << m_left_value << ", right boundary deriv " << m_right << " = "; + ss << m_right_value << std::endl; + if (m_made_monotonic) { + ss << "(spline has been adjusted for piece-wise monotonicity)"; + } + return ss.str(); +} +#endif // HAVE_SSTREAM + +namespace internal { + +// band_matrix implementation +// ------------------------- + +band_matrix::band_matrix(int dim, int n_u, int n_l) { resize(dim, n_u, n_l); } +void band_matrix::resize(int dim, int n_u, int n_l) { + assert(dim > 0); + 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; i < m_upper.size(); i++) { + m_upper[i].resize(dim); + } + for (size_t i = 0; i < m_lower.size(); i++) { + m_lower[i].resize(dim); + } +} +int band_matrix::dim() const { + if (m_upper.size() > 0) { + 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 < dim()) && (j >= 0) && (j < dim())); + assert((-num_lower() <= k) && (k <= num_upper())); + // k=0 -> 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 < dim()) && (j >= 0) && (j < dim())); + assert((-num_lower() <= k) && (k <= num_upper())); + // k=0 -> 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 < dim())); + return m_lower[0][i]; +} +double &band_matrix::saved_diag(int i) { + assert((i >= 0) && (i < dim())); + return m_lower[0][i]; +} + +// LR-Decomposition of a band matrix +void band_matrix::lu_decompose() { + int i_max, j_max; + int j_min; + double x; + + // preconditioning + // normalize column i so that a_ii=1 + for (int i = 0; i < this->dim(); 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; k < this->dim(); 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; i < this->dim(); i++) { + sum = 0; + j_start = std::max(0, i - this->num_lower()); + for (int j = j_start; j < i; j++) + sum += this->operator()(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; i < x.size(); i++) { + for (int k = 0; k < newton_iter; k++) { + double f = (c * x[i] + b) * x[i] + a; + double f1 = 2.0 * c * x[i] + b; + // only adjust if slope is large enough + if (fabs(f1) > 1e-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; i < z.size(); i++) { + // convert depressed cubic roots to original cubic: x = z - c/3 + z[i] -= (1.0 / 3.0) * c; + // improve solution via newton steps + for (int k = 0; k < newton_iter; k++) { + double f = ((z[i] + c) * z[i] + b) * z[i] + a; + double f1 = (3.0 * z[i] + 2.0 * c) * z[i] + b; + // only adjust if slope is large enough + if (fabs(f1) > 1e-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; i < z.size(); i++) { + if (xmin > fabs(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 + +#pragma GCC diagnostic pop + +#endif /* TK_SPLINE_H */ \ No newline at end of file