splines.h

Go to the documentation of this file.

 
 
#ifndef THEORETICA_SPLINES_H
#define THEORETICA_SPLINES_H
 
#include "../core/constants.h"
#include "../algebra/algebra_types.h"
 
 
namespace theoretica {
 
 
    inline real lerp(real x1, real x2, real interp) {
        return (x1 + interp * (x2 - x1));
    }
 
 
    template<unsigned int N>
    inline vec<real, N> lerp(
        const vec<real, N>& P1, const vec<real, N>& P2, real interp) {
 
        return (P1 + (P2 - P1) * interp);
    }
 
 
    inline real invlerp(real x1, real x2, real value) {
        return (value - x1) / (x2 - x1);
    }
 
 
    template<unsigned int N>
    inline vec<real, N> invlerp(
        const vec<real, N>& P1, const vec<real, N>& P2, real value) {
 
        real t = invlerp(P1.get(0), P2.get(0), value);
 
        // Check that all computed t_i are the same
        for (int i = 1; i < N; ++i) {
 
            real t_new = invlerp(P1.get(i), P2.get(i), value);
 
            if(t != t_new) {
                TH_MATH_ERROR("invlerp", t_new, MathError::OutOfDomain);
                return nan();
            }
        }
 
        return t;
    }
 
 
    inline real remap(real iFrom, real iTo, real oFrom, real oTo, real value) {
        return lerp(oFrom, oTo, invlerp(iFrom, iTo, value));
    }
 
 
    template<unsigned int N>
    inline vec<real, N> remap(
        const vec<real, N>& iFrom, const vec<real, N>& iTo,
        const vec<real, N>& oFrom, const vec<real, N>& oTo, real value) {
        return lerp(oFrom, oTo, invlerp(iFrom, iTo, value));
    }
 
 
    template<unsigned int N>
    inline vec<real, N> nlerp(
        const vec<real, N>& P1, const vec<real, N>& P2, real interp) {
        
        return (P1 + (P2 - P1) * interp).normalized();
    }
 
 
    template<unsigned int N>
    inline vec<real, N> slerp(
        const vec<real, N>& P1, const vec<real, N>& P2, real t) {
 
        // Compute (only once) the length
        // of the input vectors
        const real P1_l = P1.norm();
        const real P2_l = P2.norm();
 
        // Check whether one of the vectors is null,
        // which would make the computation impossible
        if(P1_l == 0 || P2_l == 0) {
            TH_MATH_ERROR("slerp", P1_l ? P2_l : P1_l, MathError::ImpossibleOperation);
            return vec<real, N>(nan());
        }
 
        // Angle between P1 and P2 (from the dot product)
        real omega = acos((P1 * P2) / (P1_l * P2_l));
        real s = sin(omega);
 
        // Check that the sine of the angle is not zero
        if(s == 0) {
            TH_MATH_ERROR("slerp", s, MathError::DivByZero);
            return vec<real, N>(nan());
        }
 
        return (P1 * sin((1 - t) * omega) + P2 * sin(t * omega)) / s;
    }
 
 
    // Sigmoid-like interpolation
 
 
    inline real smoothstep(real x1, real x2, real interp) {
 
        if(x1 == x2) {
            TH_MATH_ERROR("smoothstep", x1, MathError::DivByZero);
            return nan();
        }
 
        // Clamp x between 0 and 1
        const real x = clamp((interp - x1) / (x2 - x1), 0.0, 1.0);
 
        // 3x^2 - 2x^3
        return x * x * (3 - 2 * x);
    }
 
 
    inline real smootherstep(real x1, real x2, real interp) {
 
        if(x1 == x2) {
            TH_MATH_ERROR("smootherstep", x1, MathError::DivByZero);
            return nan();
        }
 
        // Clamp x between 0 and 1
        const real x = clamp((interp - x1) / (x2 - x1), 0.0, 1.0);
 
        // 6x^5 - 15x^4 + 10x^3
        return x * x * x * (x * (x * 6 - 15) + 10);
    }
 
 
    // Bezier curves
 
 
    template<unsigned int N>
    inline vec<real, N> quadratic_bezier(
        const vec<real, N>& P0, const vec<real, N>& P1,
        const vec<real, N>& P2, real t) {
 
        return lerp(lerp(P0, P1, t), lerp(P1, P2, t), t);
    }
 
 
    template<unsigned int N>
    inline vec<real, N> cubic_bezier(
        const vec<real, N>& P0, const vec<real, N>& P1,
        const vec<real, N>& P2, vec<real, N> P3, real t) {
 
        const vec<real, N> A = lerp(P0, P1, t);
        const vec<real, N> B = lerp(P1, P2, t);
        const vec<real, N> C = lerp(P2, P3, t);
 
        const vec<real, N> D = lerp(A, B, t);
        const vec<real, N> E = lerp(B, C, t);
 
        return lerp(D, E, t);
    }
 
 
    template<unsigned int N>
    inline vec<real, N> bezier(const std::vector<vec<real, N>>& points, real t) {
        
        if(points.size() < 2) {
            TH_MATH_ERROR("bezier", points.size(), MathError::InvalidArgument);
            return vec<real, N>(nan());
        }
 
        if(t < 0 || t > 1) {
            TH_MATH_ERROR("bezier", t, MathError::InvalidArgument);
            return vec<real, N>(nan());
        }
 
        std::vector<vec<real, N>> p = points;
 
        for (int index = p.size(); index > 1; --index) {
 
            for (int i = 0; i < index - 1; ++i)
                p[i] = lerp(p[i], p[i + 1], t);
        }
 
        return p[0];
    }
 
 
    struct spline_node {
 
        real x;
 
        real a, b, c, d;
 
        spline_node() : x(0), a(0), b(0), c(0), d(0) {}
 
        spline_node(real x, real a, real b, real c, real d)
            : x(x), a(a), b(b), c(c), d(d) {}
 
 
        inline real operator()(real X) const {
            const real h = X - x;
            return a + h * (b + h * (c + h * d));
        }
 
 
        inline real deriv(real X) const {
            const real h = X - x;
            return b + h * (c * 2 + h * d * 3);
        }
    };
 
 
    template<typename DataPoints = std::vector<vec2>>
    inline std::vector<spline_node> cubic_splines(DataPoints p) {
 
        if(p.size() < 2) {
            TH_MATH_ERROR("cubic_splines", p.size(), MathError::InvalidArgument);
            return {spline_node(nan(), nan(), nan(), nan(), nan())};
        }
 
        const unsigned int n = p.size() - 1;
 
        std::vector<real> dx(n);
        std::vector<real> delta(n);
 
        delta[0] = 0;
 
        std::vector<real> alpha(n + 1);
        std::vector<real> beta(n + 1);
        std::vector<real> gamma(n + 1);
 
        alpha[0] = 1;
        beta[0] = 0;
        gamma[0] = 0;
 
        std::vector<real> b(n);
        std::vector<real> c(n + 1);
        std::vector<real> d(n);
 
        for (unsigned int i = 0; i < n; ++i)
            dx[i] = p[i + 1][0] - p[i][0];
 
        for (unsigned int i = 1; i < n; ++i)
            delta[i] = 3 * (
                ((p[i + 1][1] - p[i][1]) / dx[i])
                    - (p[i][1] - p[i - 1][1]) / dx[i - 1]);
 
        for (unsigned int i = 1; i < n; ++i) {
            alpha[i] = 2 * (p[i + 1][0] - p[i - 1][0]) - dx[i - 1] * beta[i - 1];
            beta[i] = dx[i] / alpha[i];
            gamma[i] = (delta[i] - dx[i] * gamma[i - 1]) / alpha[i];
        }
 
        // Apply boundary conditions
        alpha[n] = 1;
        gamma[n] = 0;
        c[n] = 0;
 
        // Solve the associated tridiagonal system
        // using back-substitution
        for (int i = n - 1; i >= 0; --i) {
            
            c[i] = gamma[i] - beta[i] * c[i + 1];
            b[i] = (p[i + 1][1] - p[i][1]) / dx[i]
                - dx[i] * (c[i + 1] + 2 * c[i]) / 3.0;
            d[i] = (c[i + 1] - c[i]) / (3.0 * dx[i]);
        }
 
        std::vector<spline_node> nodes(n);
 
        for (unsigned int i = 0; i < n; ++i)
            nodes[i] = spline_node(p[i][0], p[i][1], b[i], c[i], d[i]);
 
        return nodes;
    }
 
 
    template<typename Dataset1, typename Dataset2>
    inline std::vector<spline_node> cubic_splines(
        const Dataset1& x, const Dataset2& y) {
 
        if(x.size() < 2) {
            TH_MATH_ERROR("cubic_splines", x.size(), MathError::InvalidArgument);
            return {spline_node(nan(), nan(), nan(), nan(), nan())};
        }
 
        if(x.size() != y.size()) {
            TH_MATH_ERROR("cubic_splines", x.size(), MathError::InvalidArgument);
            return {spline_node(nan(), nan(), nan(), nan(), nan())};
        }
 
        const unsigned int n = x.size() - 1;
 
        std::vector<real> dx(n);
        std::vector<real> delta(n);
 
        delta[0] = 0;
 
        std::vector<real> alpha(n + 1);
        std::vector<real> beta(n + 1);
        std::vector<real> gamma(n + 1);
 
        alpha[0] = 1;
        beta[0] = 0;
        gamma[0] = 0;
 
        std::vector<real> b(n);
        std::vector<real> c(n + 1);
        std::vector<real> d(n);
 
        for (unsigned int i = 0; i < n; ++i)
            dx[i] = x[i + 1] - x[i];
 
        for (unsigned int i = 1; i < n; ++i)
            delta[i] = 3 * (
                ((y[i + 1] - y[i]) / dx[i])
                    - (y[i] - y[i - 1]) / dx[i - 1]);
 
        for (unsigned int i = 1; i < n; ++i) {
            alpha[i] = 2 * (x[i + 1] - x[i - 1]) - dx[i - 1] * beta[i - 1];
            beta[i] = dx[i] / alpha[i];
            gamma[i] = (delta[i] - dx[i] * gamma[i - 1]) / alpha[i];
        }
 
        // Apply boundary conditions
        alpha[n] = 1;
        gamma[n] = 0;
        c[n] = 0;
 
        // Solve the associated tridiagonal system
        // using back-substitution
        for (int i = n - 1; i >= 0; --i) {
            
            c[i] = gamma[i] - beta[i] * c[i + 1];
            b[i] = (y[i + 1] - y[i]) / dx[i]
                - dx[i] * (c[i + 1] + 2 * c[i]) / 3.0;
            d[i] = (c[i + 1] - c[i]) / (3.0 * dx[i]);
        }
 
        std::vector<spline_node> nodes(n);
 
        for (unsigned int i = 0; i < n; ++i)
            nodes[i] = spline_node(x[i], y[i], b[i], c[i], d[i]);
 
        return nodes;
    }
 
 
    class spline {
    public:
 
        std::vector<spline_node> nodes;
 
 
        template<typename DataPoints = std::vector<vec2>>
        spline(const DataPoints& p) {
            nodes = cubic_splines(p);
        }
 
 
        template<typename Dataset1, typename Dataset2>
        spline(const Dataset1& X, const Dataset2& Y) {
            nodes = cubic_splines(X, Y);
        }
 
 
        inline spline& operator=(const spline& other) {
            nodes = other.nodes;
            return *this;
        }
 
 
        inline real operator()(real x) const {
            
            for (int i = int(nodes.size()) - 1; i > 0; --i)
                if(x >= nodes[i].x)
                    return nodes[i](x);
 
            // Extrapolation for x < x_0
            return nodes[0](x);
        }
 
 
        inline real deriv(real x) const {
            
            for (unsigned int i = nodes.size() - 1; i > 0; --i)
                if(x >= nodes[i].x)
                    return nodes[i].deriv(x);
 
            // Extrapolation for x < x_0
            return nodes[0].deriv(x);
        }
 
 
        inline auto begin() {
            return nodes.begin();
        }
 
 
        inline auto end() {
            return nodes.end();
        }
    };
 
    
}
 
#endif