b2splines.H

//------------------------------------------------------------------------
// b2splines.H --
//
//
// written by Mathias Doreille
//
// Copyright (c) 2007-2012 SMR Engineering & Development SA
//               2502 Bienne, Switzerland
//
// All Rights Reserved.  Proprietary source code.  The contents of
// this file may not be disclosed to third parties, copied or
// duplicated in any form, in whole or in part, without the prior
// written permission of SMR.
//----------------------------------------------------------------------*/
 
#ifndef __B2SPLINES_H__
#define __B2SPLINES_H__
 
#include <vector>
 
#include "utils/b2exception.H"
 
namespace b2000 {
 
class CubicSpline {
public:
    CubicSpline() {}
 
    template <typename T1, typename T2>
    CubicSpline(
          T1 x_begin, T1 x_end, T2 y_begin, T2 y_end, bool natural, double yp1 = 0, double ypn = 0)
        : xa(x_begin, x_end), ya(y_begin, y_end) {
        compute_y2a(natural, yp1, ypn);
    }
 
    template <typename T1>
    CubicSpline(T1 xy_begin, T1 xy_end, bool natural, double yp1 = 0, double ypn = 0) {
        size_t s = (xy_end - xy_begin) / 2;
        xa.reserve(s);
        ya.reserve(s);
        while (xy_begin != xy_end) {
            xa.push_back(*xy_begin);
            ++xy_begin;
            ya.push_back(*xy_begin);
            ++xy_begin;
        }
        compute_y2a(natural, yp1, ypn);
    }
 
    template <typename T1, typename T2>
    CubicSpline(
          size_t s, std::pair<T1, T2> xy_begin, std::pair<T1, T2> xy_end, bool natural,
          double yp1 = 0, double ypn = 0) {
        xa.reserve(s);
        ya.reserve(s);
        while (xy_begin != xy_end) {
            xa.push_back(xy_begin->first);
            ya.push_back(xy_begin->second);
            ++xy_begin;
        }
        compute_y2a(natural, yp1, ypn);
    }
 
    void add_point(double x, double y) {
        xa.push_back(x);
        ya.push_back(y);
    }
 
    void finalise(bool natural, double yp1 = 0, double ypn = 0) { compute_y2a(natural, yp1, ypn); }
 
    double operator()(double x) const {
        if (x < xa.front()) { return ya.front(); }
        if (x > xa.back()) { return ya.back(); }
        size_t klo = 0;
        size_t khi = xa.size() - 1;
        {
            size_t k;
            while (khi - klo > 1) {
                k = (khi + klo) / 2;
                if (xa[k] > x) {
                    khi = k;
                } else {
                    klo = k;
                }
            }
        }
        const double h = xa[khi] - xa[klo];
        if (h == 0) { Exception() << THROW; }
        const double a = (xa[khi] - x) / h;
        const double b = (x - xa[klo]) / h;
        return a * ya[klo] + b * ya[khi]
               + ((a * a * a - a) * y2a[klo] + (b * b * b - b) * y2a[khi]) * (h * h) / 6.0;
    }
 
    void get_value_and_derivative(double x, double& v, double& dv) const {
        if (x < xa.front()) {
            v = ya.front();
            dv = 0;
            return;
        }
        if (x > xa.back()) {
            v = ya.back();
            dv = 0;
            return;
        }
        size_t klo = 0;
        size_t khi = xa.size() - 1;
        {
            size_t k;
            while (khi - klo > 1) {
                k = (khi + klo) / 2;
                if (xa[k] > x) {
                    khi = k;
                } else {
                    klo = k;
                }
            }
        }
        const double h = xa[khi] - xa[klo];
        if (h == 0) { Exception() << THROW; }
        const double a = (xa[khi] - x) / h;
        const double b = (x - xa[klo]) / h;
        v = a * ya[klo] + b * ya[khi]
            + ((a * a * a - a) * y2a[klo] + (b * b * b - b) * y2a[khi]) * (h * h) / 6.0;
        dv = (-ya[klo] + ya[khi]) / h
             + ((1 - 3 * a * a) * y2a[klo] + (3 * b * b - 1) * y2a[khi]) * h / 6.0;
    }
 
private:
    void compute_y2a(bool natural, double yp1 = 0, double ypn = 0) {
        std::vector<double> u(xa.size() - 1);
        y2a.resize(xa.size());
        if (natural) {
            y2a[0] = u[0] = 0.0;
        } else {
            y2a[0] = -0.5;
            u[0] = (3.0 / (xa[1] - xa[0])) * ((ya[1] - ya[0]) / (xa[1] - xa[0]) - yp1);
        }
        const size_t i_max = xa.size() - 1;
        for (size_t i = 1; i != i_max; ++i) {
            const double sig = (xa[i] - xa[i - 1]) / (xa[i + 1] - xa[i - 1]);
            const double p = sig * y2a[i - 1] + 2.0;
            y2a[i] = (sig - 1.0) / p;
            u[i] = (ya[i + 1] - ya[i]) / (xa[i + 1] - xa[i])
                   - (ya[i] - ya[i - 1]) / (xa[i] - xa[i - 1]);
            u[i] = (6.0 * u[i] / (xa[i + 1] - xa[i - 1]) - sig * u[i - 1]) / p;
        }
        {
            double qn, un;
            size_t n = xa.size();
            if (natural) {
                qn = un = 0.0;
            } else {
                qn = 0.5;
                un = (3.0 / (xa[n - 1] - xa[n - 2]))
                     * (ypn - (ya[n - 1] - ya[n - 2]) / (xa[n - 1] - xa[n - 2]));
            }
            y2a[n - 1] = (un - qn * u[n - 2]) / (qn * y2a[n - 2] + 1.0);
        }
        for (size_t i = xa.size() - 1; i > 0; --i) { y2a[i - 1] = y2a[i - 1] * y2a[i] + u[i - 1]; }
    }
 
    std::vector<double> xa;
    std::vector<double> ya;
    std::vector<double> y2a;
};
 
}  // namespace b2000
 
#endif