util.cpp 1.41 KB
Newer Older
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56
#include "util.h"
#include <cassert>
#include <cmath>

namespace loadl {

//https://en.wikipedia.org/wiki/Monotone_cubic_interpolation
monotonic_interpolator::monotonic_interpolator(const std::vector<double>& x, const std::vector<double>& y) : x_(x), y_(y), m_(x.size()) {
	assert(x.size() == y.size());
	assert(x.size() > 1);

	std::vector<double> d(x.size());
	for(size_t i = 0; i < x.size()-1; i++) {
		d[i] = (y[i+1]-y[i])/(x[i+1]-x[i]);
	}
	m_[0] = d[0];
	m_[x.size()-1] = d[x.size()-2];
	for(size_t i = 1; i < x.size()-1; i++) {
		m_[i] = (d[i-1]+d[i])/2;

		if(d[i-1]*d[i] <= 0) {
			m_[i] = 0;
		}
	}
	for(size_t i = 0; i < x.size()-1; i++) {
		double a = m_[i]/d[i];
		double b = m_[i+1]/d[i];

		double r = a*a + b*b;
		if(r > 9) {
			m_[i] *= 3/sqrt(r);
			m_[i+1] *= 3/sqrt(r);
		}
	}
}

static double h00(double t) {
	return (1+2*t)*(1-t)*(1-t);
}

static double h10(double t) {
	return t*(1-t)*(1-t);
}

static double h01(double t) {
	return t*t*(3-2*t);
}

static double h11(double t) {
	return t*t*(t-1);
}

double monotonic_interpolator::operator()(double x0) {
	// replace by binary search if necessary!
	size_t idx = 0;
	assert(x0 < x_[x_.size()-1]);
57 58 59 60
	for(; idx < x_.size()-1; idx++) {
		if((x0-x_[idx])*(x0-x_[idx+1]) <= 0) {
			break;
		}
61 62 63 64 65 66 67 68 69
	}
	assert(idx < x_.size()-1);

	double del = x_[idx+1]-x_[idx];
	double t = (x0-x_[idx])/del;

	return y_[idx]*h00(t)+del*m_[idx]*h10(t)+y_[idx+1]*h01(t)+del*m_[idx+1]*h11(t);
}
}