Spline.hpp

#include <NDEVR/Angle.h>
#include <NDEVR/Buffer.h>
#include <NDEVR/Vertex.h>
#include <NDEVR/BandMatrix.h>
#include <NDEVR/SplineBoundaryType.h>
namespace NDEVR
{
    template<class t_point_type>
    class Spline
    {
    protected:
        Buffer<fltp08> m_x;
        Buffer<t_point_type> 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
        Buffer<t_point_type> m_b;
        Buffer<t_point_type> m_c;
        Buffer<t_point_type> m_d; // spline coefficients
        t_point_type m_c0; // for left extrapolation
        SplineType m_type = SplineType::e_cspline;
        SplineBoundaryType m_left = SplineBoundaryType::e_second_deriv;
        SplineBoundaryType m_right = SplineBoundaryType::e_second_deriv;
        fltp08 m_left_value = 0.0;
        fltp08 m_right_value = 0.0;
        bool m_made_monotonic = false;
    public:
        // default constructor: set boundary condition to be zero curvature
        // at both ends, i.e. natural splines
        Spline()
        {}
 
        Spline(const Buffer<t_point_type>& Y
            , SplineType type = SplineType::e_cspline
            , SplineBoundaryType left = SplineBoundaryType::e_second_deriv
            , fltp08 left_value = 0.0
            , SplineBoundaryType right = SplineBoundaryType::e_second_deriv
            , fltp08 right_value = 0.0)
            : m_type(type)
            , m_left(left)
            , m_right(right)
            , m_left_value(left_value)
            , m_right_value(right_value)
        {
            Buffer<fltp08> X;
            X.setSize(Y.size());
            for (uint04 i = 0; i < Y.size(); i++)
                X[i] = cast<fltp08>(i);
            setPoints(X, Y, m_type);
        }
        Spline(const Buffer<fltp08>& X
            , const Buffer<t_point_type>& Y
            , SplineType type = SplineType::e_cspline
            , bool make_monotonic = false
            , SplineBoundaryType left  = SplineBoundaryType::e_second_deriv
            , fltp08 left_value = 0.0
            , SplineBoundaryType right = SplineBoundaryType::e_second_deriv
            , fltp08 right_value = 0.0) 
            : m_type(type)
            , m_left(left)
            , m_right(right)
            , m_left_value(left_value)
            , m_right_value(right_value)
        {
            setPoints(X, Y, m_type);
            if (make_monotonic)
                makeMonotonic();
        }
        
        void setPoints(const Buffer<fltp08>& x, const Buffer<t_point_type>& y, SplineType type)
        {
            lib_assert(x.size() == y.size(), "Y size must be = X size");
            lib_assert(x.size() >= 3, "X size must be greater");
            if (m_left == SplineBoundaryType::e_not_a_knot || m_right == SplineBoundaryType::e_not_a_knot)
                lib_assert(x.size() >= 4, "not-a-knot with 3 points has multiple solutions");
            m_type = type;
            m_made_monotonic = false;
            m_x = x;
            m_y = y;
            uint04 n = x.size();
            for (uint04 i = 0; i < n - 1; i++) 
                lib_assert(m_x[i] < m_x[i + 1], "x must have monotonicity");
 
 
            if (type == SplineType::e_linear) 
            {
                // e_linear interpolation
                m_d.resize(n);
                m_c.resize(n);
                m_b.resize(n);
                for (uint04 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 == SplineType::e_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 == SplineBoundaryType::e_not_a_knot) ? 2 : 1;
                int n_lower = (m_right == SplineBoundaryType::e_not_a_knot) ? 2 : 1;
                BandMatrix<fltp08> A(n, n_upper, n_lower);
                Buffer<t_point_type> rhs;
                rhs.setSize(n);
                for (uint04 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 == SplineBoundaryType::e_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 == SplineBoundaryType::e_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 == SplineBoundaryType::e_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 
                {
                    lib_assert(false, "unknown state");
                }
                if (m_right == SplineBoundaryType::e_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 == SplineBoundaryType::e_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 == SplineBoundaryType::e_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 
                {
                    lib_assert(false, "unknown state");
                }
 
                // solve the equation system to obtain the parameters c[]
                m_c = A.luSolve(rhs);
 
                // calculate parameters b[] and d[] based on c[]
                m_d.resize(n);
                m_b.resize(n);
                for (uint04 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
                fltp08 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 == SplineBoundaryType::e_first_deriv)
                    m_c[n - 1] = 0.0;   // force e_linear extrapolation
 
            }
            else if (type == SplineType::e_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 (uint04 i = 1; i < n - 1; i++) 
                {
                    const fltp08 h = m_x[i + 1] - m_x[i];
                    const fltp08 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 == SplineBoundaryType::e_first_deriv)
                {
                    m_b[0] = m_left_value;
                }
                else if (m_left == SplineBoundaryType::e_second_deriv) 
                {
                    const fltp08 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 == SplineBoundaryType::e_not_a_knot)
                {
                    // f''' continuous at x[1]
                    const fltp08 h0 = m_x[1] - m_x[0];
                    const fltp08 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 
                {
                    lib_assert(false, "unknown state");
                }
                if (m_right == SplineBoundaryType::e_first_deriv)
                {
                    m_b[n - 1] = m_right_value;
                    m_c[n - 1] = 0.0;
                }
                else if (m_right == SplineBoundaryType::e_second_deriv)
                {
                    const fltp08 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 == SplineBoundaryType::e_not_a_knot)
                {
                    // f''' continuous at x[n-2]
                    const fltp08 h0 = m_x[n - 2] - m_x[n - 3];
                    const fltp08 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 
                {
                    lib_assert(false, "unknown state");
                }
                m_d[n - 1] = 0.0;
 
                // parameters c and d are determined by continuity and differentiability
                setCoeffsFromB();
 
            }
            else 
            {
                lib_assert(false, "unknown state");
            }
 
            // for left extrapolation coefficients
            m_c0 = (m_left == SplineBoundaryType::e_first_deriv) ? t_point_type(0.0) : m_c[0];
        }
 
        bool makeMonotonic()
        {
            lib_assert(m_x.size() == m_y.size(), "unknown state");
            lib_assert(m_x.size() == m_b.size(), "unknown state");
            lib_assert(m_x.size() > 2, "unknown state");
            bool modified = false;
            const uint04 n = m_x.size();
            // make sure: input data monotonic increasing --> b_i>=0
            //          input data monotonic decreasing --> b_i<=0
            for (uint04 i = 0; i < n; i++) 
            {
                uint04 im1 = getMax(i - 1, 0U);
                uint04 ip1 = getMin(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 (uint04 i = 0; i < n - 1; i++)
            {
                fltp08 h = m_x[i + 1] - m_x[i];
                for (uint01 n = 0; n < t_point_type::NumberOfDimensions(); n++)
                {
                    fltp08 avg = (m_y[i + 1][n] - m_y[i][n]) / h;
                    if (avg == 0.0 && (m_b[i][n] != 0.0 || m_b[i + 1][n] != 0.0)) {
                        modified = true;
                        m_b[i][n] = 0.0;
                        m_b[i + 1][n] = 0.0;
                    }
                    else if ((m_b[i][n] >= 0.0 && m_b[i + 1][n] >= 0.0 && avg > 0.0) ||
                        (m_b[i][n] <= 0.0 && m_b[i + 1][n] <= 0.0 && avg < 0.0))
                    {
 
                        // input data is monotonic
                        fltp08 r = sqrt(m_b[i][n] * m_b[i][n] + m_b[i + 1][n] * m_b[i + 1][n]) / abs(avg);
                        if (r > 3.0) {
                            // sufficient criteria for monotonicity: r<=3
                            // adjust b[i] and b[i+1]
                            modified = true;
                            m_b[i][n] *= (3.0 / r);
                            m_b[i + 1][n] *= (3.0 / r);
                        }
                    }
                }
            }
 
            if (modified)
            {
                setCoeffsFromB();
                m_made_monotonic = true;
            }
            return modified;
        }
 
        // return the closest idx so that m_x[idx] <= x (return 0 if x<m_x[0])
        uint04 find_closest(fltp08 x) const
        {
            auto it = std::upper_bound(m_x.begin(), m_x.end(), x);
            return getMax(cast<uint04>(it - m_x.begin() - 1), 0U);
        }
 
        t_point_type operator() (fltp08 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
            uint04 n = m_x.size();
            uint04 idx = find_closest(x);
 
            fltp08 h = x - m_x[idx];
            t_point_type 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;
        }
 
        t_point_type deriv(uint04 order, fltp08 x) const
        {
            lib_assert(order > 0, "unknown state");
            uint04 n = m_x.size();
            uint04 idx = find_closest(x);
 
            fltp08 h = x - m_x[idx];
            t_point_type 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;
        }
 
        Buffer<t_point_type> solve(t_point_type y, bool ignore_extrapolation) const
        {
            Buffer<t_point_type> x;       // roots for the entire spline
            Buffer<t_point_type> root;     // roots for each piecewise cubic
            const uint04 n = m_x.size();
 
            // left extrapolation
            if (ignore_extrapolation == false)
            {
                root = solveCubic(m_y[0] - y, m_b[0], m_c0, t_point_type(0.0), 1);
                for (uint04 j = 0; j < root.size(); j++) 
                {
                    if (root[j] < 0.0)
                        x.add(m_x[0] + root[j]);
                }
            }
 
            // brute force check if piecewise cubic has roots in their resp. segment
            // TODO: make more efficient
            for (uint04 i = 0; i < n - 1; i++) 
            {
                root = solveCubic(m_y[i] - y, m_b[i], m_c[i], m_d[i], 1);
                for (uint04 j = 0; j < root.size(); j++) 
                {
                    fltp08 h = (i > 0) ? (m_x[i] - m_x[i - 1]) : 0.0;
                    fltp08 eps = getEPS() * 512.0 * getMin(h, 1.0);
                    if ((-eps <= root[j]) && (root[j] < m_x[i + 1] - m_x[i]))
                    {
                        t_point_type new_root = m_x[i] + root[j];
                        if (x.size() > 0 && x.last() + eps > new_root) 
                        {
                            x.last() = new_root;      // avoid spurious duplicate roots
                        }
                        else
                        {
                            x.add(new_root);
                        }
                    }
                }
            }
 
            // right extrapolation
            if (!ignore_extrapolation) 
            {
                root = solveCubic(m_y[n - 1] - y, m_b[n - 1], m_c[n - 1], t_point_type(0.0), 1);
                for (uint04 j = 0; j < root.size(); j++) 
                {
                    if (0.0 <= root[j])
                        x.add(m_x[n - 1] + root[j]);
                }
            }
 
            return x;
        };
 
 
        // machine precision of a fltp08, i.e. the successor of 1 is 1+eps
        static fltp08 getEPS()
        {
            return 2.2204460492503131e-16;
        }
 
        // solutions for a + b*x = 0
        static Buffer<t_point_type> solveLinear(t_point_type a
            , t_point_type b)
        {
            Buffer<t_point_type> 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
        static Buffer<t_point_type> solveQuadratic(t_point_type a
            , t_point_type b, t_point_type c
            , int newton_iter = 0)
        {
            if (c == 0.0)
                return solveLinear(a, b);
            // rescale so that we solve x^2 + 2p x + q = (x+p)^2 + q - p^2 = 0
            const t_point_type p = 0.5 * b / c;
            const t_point_type q = a / c;
            const t_point_type discr = p * p - q;
            const fltp08 eps = 0.5 * getEPS();
            const t_point_type discr_err = (6.0 * (p * p) + 3.0 * abs(q) + abs(discr)) * eps;
 
            Buffer<t_point_type> x;   // roots
            if (abs(discr) <= discr_err)
            {
                // discriminant is zero --> one root
                x.resize(1);
                x[0] = -p;
            }
            else if (discr < 0.0) {
                // no root
            }
            else {
                // two roots
                x.resize(2);
                for (uint01 n = 0; n < t_point_type::NumberOfDimensions(); n++)
                {
                    x[0][n] = -p[n] - sqrt(discr[n]);
                    x[1][n] = -p[n] + sqrt(discr[n]);
                }
            }
 
            // improve solution via newton steps
            for (uint04 i = 0; i < x.size(); i++) 
            {
                for (int k = 0; k < newton_iter; k++) 
                {
                    t_point_type f = (c * x[i] + b) * x[i] + a;
                    t_point_type f1 = 2.0 * c * x[i] + b;
                    // only adjust if slope is large enough
                    if (abs(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 solveCubic.c
        //   octave: roots.m - via eigenvalues of the Frobenius companion matrix
        Buffer<t_point_type> solveCubic(t_point_type a
            , t_point_type b, t_point_type c
            , t_point_type d,int newton_iter) const
        {
            if (d == 0.0)
                return solveQuadratic(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
            const t_point_type p = -(1.0 / 3.0) * b + (1.0 / 9.0) * (c * c);
            const t_point_type r = 2.0 * (c * c) - 9.0 * b;
            const t_point_type q = -0.5 * a - (1.0 / 54.0) * (c * r);
            const t_point_type discr = p * p * p - q * q;
            const fltp08 eps = getEPS();
            t_point_type p_err = eps * ((3.0 / 3.0) * abs(b) 
                + (4.0 / 9.0) * (c * c) 
                + abs(p));
            t_point_type r_err = eps * (6.0 * (c * c) 
                + 18.0 * abs(b) 
                + abs(r));
            t_point_type q_err = 0.5 * abs(a) * eps 
                + (1.0 / 54.0) * abs(c) * (r_err + abs(r) * 3.0 * eps)
                + abs(q) * eps;
            t_point_type discr_err = (p * p) * (3.0 * p_err 
                + abs(p) * 2.0 * eps)
                + abs(q) * (2.0 * q_err 
                + abs(q) * eps) 
                + abs(discr) * eps;
 
            Buffer<t_point_type> z;// roots of the depressed cubic
            // depending on the discriminant we get different solutions
            if (abs(discr) <= discr_err) 
            {
                // discriminant zero: one or two real roots
                if (abs(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];    // fltp08 root
                }
            }
            else if (discr > Vertex < 3, fltp08>(0)) 
            {
                // three real roots: via trigonometric solution
                z.resize(3);
                for (uint01 n = 0; n < t_point_type::NumberOfDimensions(); n++)
                {
                    fltp08 ac = (1.0 / 3.0) * acos(q[n] / (p[n] * sqrt(p[n])));
                    fltp08 sq = 2.0 * sqrt(p[n]);
                    z[0][n] = sq * cos(ac);
                    z[1][n] = sq * cos(ac - 2.0 * PI<fltp08>() / 3.0);
                    z[2][n] = sq * cos(ac - 4.0 * PI<fltp08>() / 3.0);
                }
            }
            else if (discr < 0.0) 
            {
                // single real root: via Cardano's fromula
                z.resize(1);
                for (uint01 n = 0; n < t_point_type::NumberOfDimensions(); n++)
                {
                    fltp08 sgnq = (q[n] >= 0 ? 1 : -1);
                    fltp08 basis = abs(q[n]) + sqrt(-discr[n]);
                    fltp08 C = sgnq * pow(basis, 1.0 / 3.0); // c++11 has std::cbrt()
                    z[0][n] = C + p[n] / C;
                }
            }
            for (uint04 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++)
                {
                    t_point_type f = ((z[i] + c) * z[i] + b) * z[i] + a;
                    t_point_type f1 = (3.0 * z[i] + 2.0 * c) * z[i] + b;
                    // only adjust if slope is large enough
                    if (abs(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) 
            {
                lib_assert(z.size() > 0, "cubic should always have at least one root");
                t_point_type xmin = abs(z[0]);
                uint04 imin = 0U;
                for (uint04 i = 1; i < z.size(); i++)
                {
                    if (xmin > abs(z[i])) 
                    {
                        xmin = abs(z[i]);
                        imin = i;
                    }
                }
                z[imin] = 0.0;      // replace the smallest absolute value with 0
            }
            std::sort(z.begin(), z.end());
            return z;
        }
        void setBoundary(SplineBoundaryType left, fltp08 left_value, SplineBoundaryType right, fltp08 right_value)
        {
            lib_assert(m_x.size() == 0, "setPoints() must not have happened yet");
            m_left = left;
            m_right = right;
            m_left_value = left_value;
            m_right_value = right_value;
        }
    protected:
        void setCoeffsFromB()
        {
            lib_assert(m_x.size() == m_y.size(), "m_y size");
            lib_assert(m_x.size() == m_b.size(), "m_b size");
            lib_assert(m_x.size() > 2, "size bust be > 2");
            uint04 n = m_b.size();
            if (m_c.size() != n)
                m_c.resize(n);
            if (m_d.size() != n)
                m_d.resize(n);
 
            for (uint04 i = 0; i < n - 1; i++)
            {
                const fltp08 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 == SplineBoundaryType::e_first_deriv) ? t_point_type(0.0) : m_c[0];
        }
    };
 
}