cubic_spline.h

Go to the documentation of this file.

 
#ifndef __SABLIB_CUBIC_SPLINE_H__
#define __SABLIB_CUBIC_SPLINE_H__
 
#include <stdexcept>
#include <type_traits>
#include <vector>
#include <Eigen/Eigen>
 
namespace sablib {

template <typename Scalar>
class CubicSpline final
{
    static_assert(std::is_floating_point_v<Scalar>, "Scalar must be a floating-point type.");
 
public:
    CubicSpline() = default;

    CubicSpline(const Eigen::VectorX<Scalar> & x, const Eigen::VectorX<Scalar> & y);

    void Fit(const Eigen::VectorX<Scalar> & x, const Eigen::VectorX<Scalar> & y);

    Scalar Interpolate(const double x) const;

    Scalar operator()(const double x) const
    {
        return Interpolate(x);
    }
 
private:
    Eigen::VectorX<Scalar> a, b, c, d;
    Eigen::VectorX<Scalar> sp_x, sp_y;

    Eigen::VectorX<Scalar> SolveTridiagonal(
        const Eigen::VectorX<Scalar>& lower, const Eigen::VectorX<Scalar>& diag,
        const Eigen::VectorX<Scalar>& upper, const Eigen::VectorX<Scalar>& rhs
    );
};
 
//
// Implementation of constructor
//
template <typename Scalar>
inline CubicSpline<Scalar>::CubicSpline(const Eigen::VectorX<Scalar>& x, const Eigen::VectorX<Scalar>& y)
{
    Fit(x, y);
}
 
//
// Implementation of Fit() method
//
template <typename Scalar>
void CubicSpline<Scalar>::Fit(const Eigen::VectorX<Scalar> & x, const Eigen::VectorX<Scalar> & y)
{
    if (x.size() != y.size()) {
        throw std::invalid_argument("CubicSpline::Fit(): x and y must have the same size.");
    }
 
    if (x.size() < 2) {
        throw std::invalid_argument("CubicSpline::Fit(): At least 2 points are required.");
    }
 
    sp_x = x;
    sp_y = y;
 
    int n = x.size() - 1;
    Eigen::VectorX<Scalar> h(n);
 
    for(int i = 0; i < n; i++) {
        h(i) = x(i + 1) - x(i);
        if (h(i) <= 0) {
            throw std::invalid_argument("CubicSpline::Fit(): x must be strictly increasing.");
        }
    }
 
    Eigen::VectorX<Scalar> lower = Eigen::VectorX<Scalar>::Zero(n + 1);
    Eigen::VectorX<Scalar> diag  = Eigen::VectorX<Scalar>::Ones(n + 1);
    Eigen::VectorX<Scalar> upper = Eigen::VectorX<Scalar>::Zero(n + 1);
    Eigen::VectorX<Scalar> rhs   = Eigen::VectorX<Scalar>::Zero(n + 1);
 
    for(int i = 1; i < n; i++) {
        lower(i) = h(i - 1);
        diag(i)  = 2 * (h(i - 1) + h(i));
        upper(i) = h(i);
        rhs(i)   = 6 * ((y(i + 1) - y(i)) / h(i) - (y(i) - y(i - 1)) / h(i - 1));
    }
 
    Eigen::VectorX<Scalar> m = SolveTridiagonal(lower, diag, upper, rhs);
 
    a.resize(n);
    b.resize(n);
    c.resize(n);
    d.resize(n);
 
    for(int i=0;i<n;i++) {
        a(i) = y(i);
        b(i) = (y(i + 1) - y(i)) / h(i) - h(i) * (2 * m(i) + m(i + 1)) / 6.0;
        c(i) = m(i) / 2.0;
        d(i) = (m(i + 1) - m(i)) / (6.0 * h(i));
    }
}
 
//
// Implementation of Thomas' algorithm (TDMA)
//
template <typename Scalar>
Eigen::VectorX<Scalar> CubicSpline<Scalar>::SolveTridiagonal(
    const Eigen::VectorX<Scalar>& lower, const Eigen::VectorX<Scalar>& diag,
    const Eigen::VectorX<Scalar>& upper, const Eigen::VectorX<Scalar>& rhs
)
{
    int n = diag.size();
    Eigen::VectorX<Scalar> cp = Eigen::VectorX<Scalar>::Zero(n);
    Eigen::VectorX<Scalar> dp = Eigen::VectorX<Scalar>::Zero(n);
    Eigen::VectorX<Scalar> x  = Eigen::VectorX<Scalar>::Zero(n);
 
    // Forward elimination
    cp(0) = upper(0) / diag(0);
    dp(0) = rhs(0) / diag(0);
 
    for (int i = 1; i < n; i++) {
        Scalar m = diag(i) - lower(i) * cp(i - 1);
        cp(i) = upper(i) / m;
        dp(i) = (rhs(i) - lower(i) * dp(i - 1)) / m;
    }
 
    // Backward substitution
    x(n - 1) = dp(n - 1);
    for (int i = n - 2; i >= 0; i--) {
        x(i) = dp(i) - cp(i) * x(i + 1);
    }
 
    return x;
}
 
//
// Implementation of Interpolate() method
//
template <typename Scalar>
Scalar CubicSpline<Scalar>::Interpolate(const double x) const
{
    int n = sp_x.size() - 1;
    int i = n - 1;
 
    for(int j = 0; j < n; j++) {
        if(x >= sp_x(j) && x <= sp_x(j + 1)) {
            i = j;
            break;
        }
    }
 
    double dx = x - sp_x(i);
 
    return a(i) + b(i) * dx + c(i) * dx * dx + d(i) * dx * dx * dx;
}
 
}; // namespace sablib
 
#endif // __SABLIB_CUBIC_SPLINE_H__