interpolate.h

#pragma once
 
#include <functional>
#include <memory>
#include <vector>
 
#include "interval.h"
#include "compare_float.h"
 
namespace cgv {
namespace math {
 
namespace detail {

template<typename ParamT = float>
ParamT map_to_domain(ParamT t, const interval<ParamT>& domain = { 0.0f, 1.0f }) {
    ParamT domain_size = domain.size();
    if(!is_zero(domain_size))
        return (t - domain.lower_bound) / domain_size;
    return domain.lower_bound;
}

template<typename PointT, typename InterpolationOp, typename ParamT = float>
PointT interpolate_piece(const std::vector<PointT>& points, ParamT t, InterpolationOp operation) {
    if(t <= ParamT(0))
        return points.front();
 
    if(t >= ParamT(1))
        return points.back();
 
    size_t num_pairs = points.size() - 1;
    size_t index = static_cast<size_t>(t * static_cast<ParamT>(num_pairs));
    ParamT step = ParamT(1) / static_cast<ParamT>(num_pairs);
    ParamT prev = static_cast<ParamT>(index) * step;
    ParamT curr = static_cast<ParamT>(index + 1) * step;
    t = (t - prev) / (curr - prev);
    return operation(points[index], points[index + 1], t);
}

template<typename PointT, typename InterpolationOp, typename ParamT = float>
PointT interpolate_piece(const std::vector<std::pair<ParamT, PointT>>& points, ParamT t, InterpolationOp operation) {
    using pair_type = std::pair<ParamT, PointT>;
    
    if(t <= points.front().first)
        return points.front().second;
 
    auto it = std::lower_bound(points.begin(), points.end(), t, [](const pair_type& point, float value) { return point.first < value; });
    if(it == points.end())
        return points.back().second;
 
    // "it" points to the first point whose parameter position is larger than t
    const pair_type& left = *(it == points.begin() ? it : it - 1);
    const pair_type& right = *it;
    t = (t - left.first) / (right.first - left.first);
    return operation(left.second, right.second, t);
}

template<typename PointT, typename InterpolationOp, typename ParamT = float>
std::vector<PointT> interpolate_n(const std::vector<std::pair<ParamT, PointT>>& points, InterpolationOp operation, size_t n) {
    using pair_type = std::pair<ParamT, PointT>;
 
    if(n == 0 || points.size() == 0)
        return {};
 
    if(points.size() == 1)
        return std::vector<PointT>(n, points.front().second);
 
    std::vector<PointT> res;
    res.reserve(n);
 
    size_t l = 0;
    size_t r = 1;
    ParamT size = points.back().first - points.front().first;
    ParamT step = size / static_cast<ParamT>(n - 1);
    for(size_t i = 0; i < n; ++i) {
        ParamT x = points.front().first + step * static_cast<ParamT>(i);
 
        while(x > points[r].first && l < points.size() - 1) {
            l = r;
            r = std::min(l + 1, points.size() - 1);
        }
 
        if(l == r) {
            res.push_back(points[r].second);
        } else {
            const pair_type& p0 = points[l];
            const pair_type& p1 = points[r];
            ParamT t = (x - p0.first) / (p1.first - p0.first);
            res.push_back(operation(p0.second, p1.second, t));
        }
    }
    return res;
}
 
} // namespace detail

template<typename ParamT = float, typename OutputIt, typename UnaryOp>
static void sequence_transform(OutputIt output_first, UnaryOp operation, size_t n, ParamT a = ParamT(0), ParamT b = ParamT(1)) {
    if(n == 1) {
        *output_first = operation(ParamT(0.5) * (a + b));
        ++output_first;
    } else if(n > 1) {
        ParamT size = b - a;
        ParamT step = size / static_cast<ParamT>(n - 1);
        for(size_t i = 0; i < n; ++i) {
            ParamT t = a + step * static_cast<ParamT>(i);
            *output_first = operation(t);
            ++output_first;
        }
    }
}

template<typename PointT, typename ParamT = float>
PointT interpolate_nearest(const PointT& p0, const PointT& p1, ParamT t) {
    return t < ParamT(0.5) ? p0 : p1;
};

template<typename PointT, typename ParamT = float>
PointT interpolate_nearest(const std::vector<PointT>& points, ParamT t, const interval<ParamT>& domain = { 0.0f, 1.0f }) {
    return detail::interpolate_piece(points, t, static_cast<PointT(*)(const PointT&, const PointT&, ParamT)>(&interpolate_nearest<PointT, ParamT>));
};

template<typename PointT, typename ParamT = float>
PointT interpolate_nearest(const std::vector<std::pair<ParamT, PointT>>& points, ParamT t) {
    return detail::interpolate_piece(points, t, static_cast<PointT(*)(const PointT&, const PointT&, ParamT)>(&interpolate_nearest<PointT, ParamT>));
}

template<typename PointT, typename ParamT = float>
std::vector<PointT> interpolate_nearest_n(const std::vector<PointT>& points, size_t n) {
    std::vector<PointT> res;
    res.reserve(n);
    sequence_transform(std::back_inserter(res), [&points](float t) { return interpolate_nearest(points, t); }, n);
    return res;
};

template<typename PointT, typename ParamT = float>
std::vector<PointT> interpolate_nearest_n(const std::vector<std::pair<ParamT, PointT>>& points, size_t n) {
    return detail::interpolate_n(points, static_cast<PointT(*)(const PointT&, const PointT&, ParamT)>(&interpolate_nearest<PointT, ParamT>), n);
}

template<typename PointT, typename ParamT = float>
static PointT interpolate_linear(const PointT& p0, const PointT& p1, ParamT t) {
    PointT v0 = (ParamT(1) - t) * p0;
    PointT v1 = t * p1;
    return v0 + v1;
}

template<typename PointT, typename ParamT = float>
PointT interpolate_linear(const std::vector<PointT>& points, ParamT t, const interval<ParamT>& domain = { 0.0f, 1.0f }) {
    t = detail::map_to_domain(t, domain);
    return detail::interpolate_piece(points, t, static_cast<PointT(*)(const PointT&, const PointT&, ParamT)>(&interpolate_linear<PointT, ParamT>));
};

template<typename PointT, typename ParamT = float>
PointT interpolate_linear(const std::vector<std::pair<ParamT, PointT>>& points, ParamT t) {
    return detail::interpolate_piece(points, t, static_cast<PointT(*)(const PointT&, const PointT&, ParamT)>(&interpolate_linear<PointT, ParamT>));
}

template<typename PointT, typename ParamT = float>
std::vector<PointT> interpolate_linear_n(const std::vector<PointT>& points, size_t n) {
    std::vector<PointT> res;
    res.reserve(n);
    sequence_transform(std::back_inserter(res), [&points](float t) { return interpolate_linear(points, t); }, n);
    return res;
};

template<typename PointT, typename ParamT = float>
std::vector<PointT> interpolate_linear_n(const std::vector<std::pair<ParamT, PointT>>& points, size_t n) {
    return detail::interpolate_n(points, static_cast<PointT(*)(const PointT&, const PointT&, ParamT)>(&interpolate_linear<PointT, ParamT>), n);
}

template<typename PointT, typename ParamT = float>
static PointT interpolate_quadratic_bezier(const PointT& p0, const PointT& p1, const PointT& p2, ParamT t) {
    ParamT mt = ParamT(1) - t;
    PointT v0 = mt * mt * p0;
    PointT v1 = ParamT(2) * mt * t * p1;
    PointT v2 = t * t * p2;
    return v0 + v1 + v2;
}

template<typename PointT, typename ParamT = float>
static PointT interpolate_cubic_bezier(const PointT& p0, const PointT& p1, const PointT& p2, const PointT& p3, ParamT t) {
    ParamT mt = ParamT(1) - t;
    PointT v0 = mt * mt * mt * p0;
    PointT v1 = ParamT(3) * mt * mt * t * p1;
    PointT v2 = ParamT(3) * mt * t * t * p2;
    PointT v3 = t * t * t * p3;
    return v0 + v1 + v2 + v3;
}

template<typename PointT, typename ParamT = float>
static PointT interpolate_cubic_hermite(const PointT& p0, const PointT& m0, const PointT& p1, const PointT& m1, ParamT t) {
    ParamT t2 = t * t;
    ParamT t3 = t * t * t;
    PointT w0 = ParamT(2) * t3 - ParamT(3) * t2 + ParamT(1);
    PointT w1 = t3 - ParamT(2) * t2 + t;
    PointT w2 = ParamT(-2) * t3 + ParamT(3) * t2;
    PointT w3 = t3 - t2;
    return w0 * p0 + w1 * m0 + w2 * p1 + w3 * m1;
}

template<typename PointT, typename ParamT = float>
static PointT interpolate_cubic_basis(const PointT& b0, const PointT& b1, const PointT& b2, const PointT& b3, ParamT t) {
    ParamT t2 = t * t;
    ParamT t3 = t * t * t;
    PointT v0 = (ParamT(1) - ParamT(3) * t + ParamT(3) * t2 - t3) * b0;
    PointT v1 = (ParamT(4) - ParamT(6) * t2 + ParamT(3) * t3) * b1;
    PointT v2 = (ParamT(1) + ParamT(3) * t + ParamT(3) * t2 - ParamT(3) * t3) * b2;
    PointT v3 = t3 * b3;
    return (ParamT(1) / ParamT(6)) * (v0 + v1 + v2 + v3);
}

template<typename PointT, typename ParamT = float>
PointT interpolate_smooth_cubic(const std::vector<PointT>& points, ParamT t, const interval<ParamT>& domain = { 0.0f, 1.0f }) {
    t = detail::map_to_domain(t, domain);
 
    size_t num_pairs = points.size() - 1;
 
    size_t i = 0;
    if(t <= ParamT(0)) {
        t = ParamT(0);
    } else if(t >= ParamT(1)) {
        t = ParamT(1);
        i = num_pairs - 1;
    } else {
        i = static_cast<size_t>(t * static_cast<float>(num_pairs));
    }
 
    PointT v1 = points[i];
    PointT v2 = points[i + 1];
    PointT v0 = i > 0 ? points[i - 1] : PointT(2) * v1 - v2;
    PointT v3 = i < num_pairs - 1 ? points[i + 2] : PointT(2) * v2 - v1;
    t = (t - static_cast<float>(i) / static_cast<float>(num_pairs)) * static_cast<float>(num_pairs);
    return interpolate_cubic_basis(v0, v1, v2, v3, t);
};

template<typename PointT, typename ParamT = float>
PointT interpolate_smooth_cubic(const std::vector<std::pair<ParamT, PointT>>& points, ParamT t) {
    using pair_type = std::pair<ParamT, PointT>;
 
    size_t num_pairs = points.size() - 1;
 
    if(t <= ParamT(0))
        return points.front().second;
 
    auto it = std::lower_bound(points.begin(), points.end(), t, [](const pair_type& point, float value) { return point.first < value; });
    if(it == points.end())
        return points.back().second;
 
    if(it != points.begin())
        --it;
 
    size_t i = std::distance(points.begin(), it);
 
    pair_type v1 = points[i];
    pair_type v2 = points[i + 1];
    PointT v0 = i > 0 ? points[i - 1].second : PointT(2) * v1.second - v2.second;
    PointT v3 = i < num_pairs - 1 ? points[i + 2].second : PointT(2) * v2.second - v1.second;
    t = (t - v1.first) / (v2.first - v1.first);
    return interpolate_cubic_basis(v0, v1.second, v2.second, v3, t);
}

template<typename PointT, typename ParamT = float>
std::vector<PointT> interpolate_smooth_cubic_n(const std::vector<PointT>& points, size_t n) {
    std::vector<PointT> res;
    res.reserve(n);
    sequence_transform(std::back_inserter(res), [&points](float t) { return interpolate_smooth_cubic(points, t); }, n);
    return res;
}

template<typename PointT, typename ParamT = float>
std::vector<PointT> interpolate_smooth_cubic_n(const std::vector<std::pair<ParamT, PointT>>& points, size_t n) {
    std::vector<PointT> res;
    res.reserve(n);
    sequence_transform(std::back_inserter(res), [&points](float t) { return interpolate_smooth_cubic(points, t); }, n);
    return res;
}

template<typename X, typename Y>
struct uniform_piecewise_linear_function {
    interval<X> domain = { X(0), X(1) };
    std::vector<Y> breakpoints;
    
    Y evaluate(X x) const {
        x = detail::map_to_domain(x, domain);
 
        if(x <= X(0))
            return breakpoints.front();
 
        if(x >= X(1))
            return breakpoints.back();
 
        size_t num_pieces = breakpoints.size() - 1;
        size_t index = static_cast<size_t>(x * static_cast<X>(num_pieces));
 
        X step = X(1) / static_cast<X>(num_pieces);
        X prev = static_cast<X>(index) * step;
        X curr = static_cast<X>(index + 1) * step;
        X t = (x - prev) / (curr - prev);
        return interpolate_linear(breakpoints[index], breakpoints[index + 1], t);
    }
};

template<typename ValueT, typename ParamT = float>
class interpolator {
public:
    virtual ~interpolator() = default;
 
    virtual std::unique_ptr<interpolator> clone() const = 0;

    virtual ValueT at(ParamT t) const = 0;

    virtual std::vector<ValueT> quantize(size_t n) const {
        std::vector<ValueT> samples;
        samples.reserve(n);
        sequence_transform<ParamT>(std::back_inserter(samples), [this](ParamT t) { return at(t); }, n);
        return samples;
    }
};
 
template<typename PointT, typename ValueT, typename ParamT = float>
class piecewise_interpolator_storage : public interpolator<ValueT, ParamT> {
public:
    using point_type = PointT;
 
    piecewise_interpolator_storage() {}
 
    piecewise_interpolator_storage(std::initializer_list<point_type> points) : points(points) {}
 
    piecewise_interpolator_storage(const std::vector<point_type>& points) : points(points) {}
 
    template<class IteratorT>
    piecewise_interpolator_storage(IteratorT first, IteratorT last) {
        points.assign(first, last);
    }
 
    std::vector<point_type> points;
};
 
template<typename ValueT, typename ParamT = float>
class identity_interpolator : public interpolator<ValueT, ParamT> {
public:
    identity_interpolator() {}
 
    std::unique_ptr<interpolator<ValueT, ParamT>> clone() const override {
        return std::unique_ptr<interpolator<ValueT, ParamT>>(new identity_interpolator(*this));
    }
 
    ValueT at(ParamT t) const override {
        return ValueT(t);
    }
};
 
template<typename ValueT, typename ParamT = float>
class uniform_piecewise_interpolator : public piecewise_interpolator_storage<ValueT, ValueT, ParamT> {
    using base = piecewise_interpolator_storage<ValueT, ValueT, ParamT>;
 
public:
    using base::base;
};
 
template<typename ValueT, typename ParamT = float>
class piecewise_interpolator : public piecewise_interpolator_storage<std::pair<ParamT, ValueT>, ValueT, ParamT> {
    using base = piecewise_interpolator_storage<std::pair<ParamT, ValueT>, ValueT, ParamT>;
 
public:
    using base::base;
};
 
template<typename ValueT, typename ParamT = float>
class discrete_interpolator : public uniform_piecewise_interpolator<ValueT, ParamT> {
    using base = uniform_piecewise_interpolator<ValueT, ParamT>;
 
public:
    using base::base;
 
    std::unique_ptr<interpolator<ValueT, ParamT>> clone() const override {
        return std::unique_ptr<interpolator<ValueT, ParamT>>(new discrete_interpolator(*this));
    }
 
    ValueT at(ParamT t) const override {
        size_t num_points = this->points.size();
 
        if(t <= ParamT(0))
            return this->points.front();
        else if(t >= ParamT(1))
            return this->points.back();
        else
            return this->points[std::min(static_cast<size_t>(t * static_cast<ParamT>(num_points)), num_points - 1)];
    }
};
 
template<typename ValueT, typename ParamT = float>
class uniform_linear_interpolator : public uniform_piecewise_interpolator<ValueT, ParamT> {
    using base = uniform_piecewise_interpolator<ValueT, ParamT>;
 
public:
    using base::base;
 
    std::unique_ptr<interpolator<ValueT, ParamT>> clone() const override {
        return std::unique_ptr<interpolator<ValueT, ParamT>>(new uniform_linear_interpolator(*this));
    }
 
    ValueT at(ParamT t) const override {
        return interpolate_linear(this->points, t);
    }
 
    std::vector<ValueT> quantize(size_t n) const override {
        return interpolate_linear_n(this->points, n);
    }
};
 
template<typename ValueT, typename ParamT = float>
class linear_interpolator : public piecewise_interpolator<ValueT, ParamT> {
    using base = piecewise_interpolator<ValueT, ParamT>;
 
public:
    using base::base;
 
    std::unique_ptr<interpolator<ValueT, ParamT>> clone() const override {
        return std::unique_ptr<interpolator<ValueT, ParamT>>(new linear_interpolator(*this));
    }
 
    ValueT at(ParamT t) const override {
        return interpolate_linear(this->points, t);
    }
 
    std::vector<ValueT> quantize(size_t n) const override {
        return interpolate_linear_n(this->points, n);
    }
};
 
template<typename ValueT, typename ParamT = float>
class uniform_smooth_interpolator : public uniform_piecewise_interpolator<ValueT, ParamT> {
    using base = uniform_piecewise_interpolator<ValueT, ParamT>;
 
public:
    using base::base;
 
    std::unique_ptr<interpolator<ValueT, ParamT>> clone() const override {
        return std::unique_ptr<interpolator<ValueT, ParamT>>(new uniform_smooth_interpolator(*this));
    }
 
    ValueT at(ParamT t) const override {
        return interpolate_smooth_cubic(this->points, t);
    }
};
 
template<typename ValueT, typename ParamT = float>
class smooth_interpolator : public piecewise_interpolator<ValueT, ParamT> {
    using base = piecewise_interpolator<ValueT, ParamT>;
 
public:
    using base::base;
 
    std::unique_ptr<interpolator<ValueT, ParamT>> clone() const override {
        return std::unique_ptr<interpolator<ValueT, ParamT>>(new smooth_interpolator(*this));
    }
 
    ValueT at(ParamT t) const override {
        return interpolate_smooth_cubic(this->points, t);
    }
};
 
template<typename ValueT, typename ParamT = float>
class function_ref_interpolator : public interpolator<ValueT, ParamT> {
public:
    function_ref_interpolator(std::function<ValueT(ParamT)> function) : function(function) {}
 
    std::unique_ptr<interpolator<ValueT, ParamT>> clone() const override {
        return std::unique_ptr<interpolator<ValueT, ParamT>>(new function_ref_interpolator(*this));
    }
 
    ValueT at(ParamT t) const override {
        if(function)
            return function(t);
        return {};
    }
 
    std::function<ValueT(ParamT)> function;
};
 
} // namespace math
} // namespace cgv