spline_curve_fitting.h

/********************************************************************
 * Copyright (C) 2015 Liangliang Nan <liangliang.nan@gmail.com>
 * https://3d.bk.tudelft.nl/liangliang/
 *
 * This file is part of Easy3D. If it is useful in your research/work,
 * I would be grateful if you show your appreciation by citing it:
 * ------------------------------------------------------------------
 *      Liangliang Nan.
 *      Easy3D: a lightweight, easy-to-use, and efficient C++ library
 *      for processing and rendering 3D data.
 *      Journal of Open Source Software, 6(64), 3255, 2021.
 * ------------------------------------------------------------------
 *
 * Easy3D is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License Version 3
 * as published by the Free Software Foundation.
 *
 * Easy3D 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 <http://www.gnu.org/licenses/>.
 ********************************************************************/
 
#ifndef EASY3D_CORE_SPLINE_CURVE_FITTING_H
#define EASY3D_CORE_SPLINE_CURVE_FITTING_H
 
 
#include <vector>
#include <cassert>
 
 
namespace easy3d {
 

    template <template <size_t, class> class Point, size_t N, typename T>
    class SplineCurveFitting {
    public:
        enum Node_e {
            eUNIFORM,     
            eOPEN_UNIFORM 
        };
 
    public:
        explicit SplineCurveFitting(int k = 2, Node_e node_type = eOPEN_UNIFORM);

        void set_ctrl_points(const std::vector<Point<N, T>> &points);

        void get_ctrl_points(std::vector<Point<N, T>> &points) const;

        void set_node_type(Node_e type);

        Point<N, T> eval_f(T u) const;

        Point<N, T> eval_df(T u) const;

        int get_order() const { return _k; }

        std::vector<T> get_equally_spaced_parameters(std::size_t steps) const;
 
    private:
        // -------------------------------------------------------------------------
        // -------------------------------------------------------------------------

        void assert_splines() const;

        void set_nodal_vector();

        void set_node_to_uniform();

        void set_node_to_open_uniform();

        Point<N, T> eval(T u,
                     const std::vector<Point<N, T>> &point,
                     int k,
                     const std::vector<T> &node,
                     int off = 0) const;
        Point<N, T> eval_rec(T u,
                         std::vector<Point<N, T>> p_in,
                         int k,
                         std::vector<T> node_in) const;
 
        // -------------------------------------------------------------------------
        // -------------------------------------------------------------------------
 
        Node_e _node_type;                  
        int _k;                             
        std::vector<Point<N, T>> _point;    
        std::vector<Point<N, T>> _vec;      
        std::vector<T> _node;               
    };
 
 
    template <template <size_t, class> class Point, size_t N, typename T>
    SplineCurveFitting<Point, N, T>::SplineCurveFitting(int k, Node_e node_type):
            _node_type(node_type),
            _k(k),
            _point(_k),
            _vec(_k - 1),
            _node(_k + _point.size()) {
        assert_splines();
    }
 
    // -----------------------------------------------------------------------------
 
    template <template <size_t, class> class Point, size_t N, typename T>
    void SplineCurveFitting<Point, N, T>::set_ctrl_points(const std::vector<Point<N, T>> &point) {
        _point = point;
        _vec.resize(_point.size() - 1);
        for (int i = 0; i < (int) _vec.size(); ++i)
            _vec[i] = _point[i + 1] - _point[i];
        set_nodal_vector();
        assert_splines();
 
        for (int i = 0; i < (int) _vec.size(); ++i)
            _vec[i] /= _node[_k + i] - _node[i + 1];
    }
 
    // -----------------------------------------------------------------------------
 
    template <template <size_t, class> class Point, size_t N, typename T>
    void SplineCurveFitting<Point, N, T>::get_ctrl_points(std::vector<Point<N, T>> &points) const {
        points = _point;
    }
 
    // -----------------------------------------------------------------------------

    template <template <size_t, class> class Point, size_t N, typename T>
    void SplineCurveFitting<Point, N, T>::set_node_type(Node_e type) {
        _node_type = type;
        set_nodal_vector();
        assert_splines();
    }
 
    // -----------------------------------------------------------------------------
 
    template <template <size_t, class> class Point, size_t N, typename T>
    Point<N, T> SplineCurveFitting<Point, N, T>::eval_f(T u) const {
        u = std::max(std::min(u, (T) 1), (T) 0); // clamp between [0 1]
        return eval(u, _point, _k, _node);
    }
 
    // -----------------------------------------------------------------------------
 
    template <template <size_t, class> class Point, size_t N, typename T>
    Point<N, T> SplineCurveFitting<Point, N, T>::eval_df(T u) const {
        u = std::max(std::min(u, (T) 1), (T) 0); // clamp between [0 1]
        return eval(u, _vec, (_k - 1), _node, 1) * (T) (_k - 1);
    }
 
    // -----------------------------------------------------------------------------
 
    template <template <size_t, class> class Point, size_t N, typename T>
    std::vector<T> SplineCurveFitting<Point, N, T>::get_equally_spaced_parameters(std::size_t steps) const {
        assert_splines();
 
        std::vector<T> lengths(steps, 0);
        std::vector<T> parameters(steps, 0);
        Point<N, T> prev_point;
        for (std::size_t i = 0; i < steps; ++i) {
            const T u = static_cast<T>(i) / static_cast<T>(steps - 1);
            parameters[i] = u;
            const Point<N, T> pos = eval_f(u);
            if (i > 0)
                lengths[i] = lengths[i - 1] + distance(pos, prev_point);
            prev_point = pos;
        }
        T total_length = lengths.back();
 
        // given a \c value and a sorted sequence of values, find the two indices such that
        // sorted_values[left_index] <= value and sorted_values[right_index] >= value.
        auto find_closest_less_or_equal = [](const std::vector<T> &sorted_values, T value,
                                             std::size_t start) -> std::size_t {
            assert(value >= sorted_values.front() && value <= sorted_values.back());
            std::size_t index = start;
            while (sorted_values[index] < value)
                ++index;
            while (sorted_values[index] > value)
                --index;
            return index;
        };
 
        std::vector<T> U(steps);
        for (std::size_t i = 0; i < steps; ++i) {
            T u = static_cast<T>(i) / static_cast<T>(steps - 1);
            T length = total_length * u;
            std::size_t left_index = find_closest_less_or_equal(lengths, length, i);
            std::size_t right_index = (lengths[left_index] == length) ? left_index : left_index + 1;
            if (lengths[left_index] == lengths[right_index])
                u = parameters[left_index];
            else { // linear interpolation
                T left_u = parameters[left_index];
                T right_u = parameters[right_index];
                T w = (length - lengths[left_index]) / (lengths[right_index] - lengths[left_index]);
                u = (1.0f - w) * left_u + w * right_u;
            }
            U[i] = u;
        }
        return U;
    }
 
    // -----------------------------------------------------------------------------
 
    template <template <size_t, class> class Point, size_t N, typename T>
    void SplineCurveFitting<Point, N, T>::assert_splines() const {
        assert(_k > 1);
        assert((int) _point.size() >= _k);
        assert(_node.size() == (_k + _point.size()));
        assert(_point.size() == (_vec.size() + 1));
    }
 
    // -----------------------------------------------------------------------------
 
    template <template <size_t, class> class Point, size_t N, typename T>
    void SplineCurveFitting<Point, N, T>::set_nodal_vector() {
        if (_node_type == eOPEN_UNIFORM)
            set_node_to_open_uniform();
        else if (_node_type == eUNIFORM)
            set_node_to_uniform();
    }
 
    // -----------------------------------------------------------------------------
 
    template <template <size_t, class> class Point, size_t N, typename T>
    void SplineCurveFitting<Point, N, T>::set_node_to_uniform() {
        const std::size_t n = _point.size() - 1;
        _node.resize(_k + n + 1);
 
        T step = (T) 1 / (T) (n - _k + 2);
        for (std::size_t i = 0; i < _node.size(); ++i) {
            _node[i] = ((T) i) * step - step * (T) (_k - 1);
        }
    }
 
    // -----------------------------------------------------------------------------
 
    template <template <size_t, class> class Point, size_t N, typename T>
    void SplineCurveFitting<Point, N, T>::set_node_to_open_uniform() {
        _node.resize(_k + _point.size());
 
        int acc = 1;
        for (int i = 0; i < (int) _node.size(); ++i) {
            if (i < _k)
                _node[i] = 0.;
            else if (i >= ((int) _point.size() + 1))
                _node[i] = 1.;
            else {
                _node[i] = (T) acc / (T) (_point.size() + 1 - _k);
                acc++;
            }
        }
    }
 
    // -----------------------------------------------------------------------------
 
    template <template <size_t, class> class Point, size_t N, typename T>
    Point<N, T> SplineCurveFitting<Point, N, T>::eval(T u,
                                  const std::vector<Point<N, T>> &point,
                                  int k,
                                  const std::vector<T> &node,
                                  int off) const {
        assert(k > 1);
        assert((int) point.size() >= k);
        assert_splines();
        int dec = 0;
        // TODO: better search with dychotomi ?
        // TODO: check for overflow
        while (u > node[dec + k + off])
            dec++;
 
        // TODO: use buffers in attributes for better performances ?
        std::vector<Point<N, T>> p_rec(k, Point<N, T>());
        for (int i = dec, j = 0; i < (dec + k); ++i, ++j)
            p_rec[j] = point[i];
 
        std::vector<T> node_rec(k + k - 2, (T) 0);
        for (int i = (dec + 1), j = 0; i < (dec + k + k - 1); ++i, ++j)
            node_rec[j] = node[i + off];
 
        return eval_rec(u, p_rec, k, node_rec);
    }
 
    // -----------------------------------------------------------------------------
 
    template <template <size_t, class> class Point, size_t N, typename T>
    Point<N, T> SplineCurveFitting<Point, N, T>::eval_rec(T u,
                                      std::vector<Point<N, T>> p_in,
                                      int k,
                                      std::vector<T> node_in) const {
        if (p_in.size() == 1)
            return p_in[0];
 
        // TODO: use buffers in attributes for better performances ?
        std::vector<Point<N, T>> p_out(k - 1, Point<N, T>());
        for (int i = 0; i < (k - 1); ++i) {
            const T n0 = node_in[i + k - 1];
            const T n1 = node_in[i];
            const T f0 = (n0 - u) / (n0 - n1);
            const T f1 = (u - n1) / (n0 - n1);
 
            p_out[i] = p_in[i] * f0 + p_in[i + 1] * f1;
        }
 
        std::vector<T> node_out(node_in.size() - 2);
        for (int i = 1, j = 0; i < ((int) node_in.size() - 1); ++i, ++j)
            node_out[j] = node_in[i];
 
        return eval_rec(u, p_out, (k - 1), node_out);
    }
 
}   // namespace easy3d
 
 
#endif  // EASY3D_CORE_SPLINE_CURVE_FITTING_H