From 4b895388d12b160f486c8b49b3296b15eeed7cc2 Mon Sep 17 00:00:00 2001
From: Mikko Rasa <tdb@tdb.fi>
Date: Sun, 2 Jun 2019 19:13:14 +0300
Subject: [PATCH] Add an interpolation sub-library

Initially including polynomials and splines, with cubic Hermite splines
being the only implemented spline type.
---
 Build                              |   1 +
 source/interpolate/dummy.cpp       |   3 +
 source/interpolate/hermitespline.h | 109 +++++++++++++++
 source/interpolate/polynomial.h    | 168 +++++++++++++++++++++++
 source/interpolate/spline.h        | 206 +++++++++++++++++++++++++++++
 5 files changed, 487 insertions(+)
 create mode 100644 source/interpolate/dummy.cpp
 create mode 100644 source/interpolate/hermitespline.h
 create mode 100644 source/interpolate/polynomial.h
 create mode 100644 source/interpolate/spline.h

diff --git a/Build b/Build
index 20875c4..ff363a4 100644
--- a/Build
+++ b/Build
@@ -4,6 +4,7 @@ package "mspmath"
 
 	library "mspmath"
 	{
+		source "source/interpolate";
 		source "source/linal";
 		source "source/geometry";
 		install true;
diff --git a/source/interpolate/dummy.cpp b/source/interpolate/dummy.cpp
new file mode 100644
index 0000000..ec10aec
--- /dev/null
+++ b/source/interpolate/dummy.cpp
@@ -0,0 +1,3 @@
+#include "hermitespline.h"
+#include "polynomial.h"
+#include "spline.h"
diff --git a/source/interpolate/hermitespline.h b/source/interpolate/hermitespline.h
new file mode 100644
index 0000000..057a9c4
--- /dev/null
+++ b/source/interpolate/hermitespline.h
@@ -0,0 +1,109 @@
+#ifndef MSP_INTERPOLATE_HERMITESPLINE_H_
+#define MSP_INTERPOLATE_HERMITESPLINE_H_
+
+#include "spline.h"
+
+namespace Msp {
+namespace Interpolate {
+
+template<typename T>
+struct Hermite
+{
+	static const Polynomial<T, 3> &h00();
+	static const Polynomial<T, 3> &h10();
+	static const Polynomial<T, 3> &h01();
+	static const Polynomial<T, 3> &h11();
+	static Polynomial<T, 3> make(T, T, T, T);
+};
+
+/**
+Implements a cubic Hermite spline.  It is parametrized by the value and slope
+at each knot.  The first derivative is guaranteed to be continuous, making it
+particularly useful as an interpolator.
+*/
+template<typename T, unsigned N = 1>
+class HermiteSpline: public Spline<T, 3, N>
+{
+public:
+	using typename Spline<T, 3, N>::Value;
+
+	struct Knot: Spline<T, 3, N>::Knot
+	{
+		Value dy;
+
+		Knot(T x_, Value y_, Value d): Spline<T, 3, N>::Knot(x_, y_), dy(d) { }
+	};
+
+	HermiteSpline(const std::vector<Knot> &);
+	HermiteSpline(Value, Value, Value, Value);
+};
+
+template<typename T>
+inline const Polynomial<T, 3> &Hermite<T>::h00()
+{
+	static Polynomial<T, 3> p(LinAl::Vector<T, 4>(2, -3, 0, 1));
+	return p;
+}
+
+template<typename T>
+inline const Polynomial<T, 3> &Hermite<T>::h10()
+{
+	static Polynomial<T, 3> p(LinAl::Vector<T, 4>(1, -2, 1, 0));
+	return p;
+}
+
+template<typename T>
+inline const Polynomial<T, 3> &Hermite<T>::h01()
+{
+	static Polynomial<T, 3> p(LinAl::Vector<T, 4>(-2, 3, 0, 0));
+	return p;
+}
+
+template<typename T>
+inline const Polynomial<T, 3> &Hermite<T>::h11()
+{
+	static Polynomial<T, 3> p(LinAl::Vector<T, 4>(1, -1, 0, 0));
+	return p;
+}
+
+template<typename T>
+inline Polynomial<T, 3> Hermite<T>::make(T p0, T m0, T p1, T m1)
+{
+	return h00()*p0 + h10()*m0 + h01()*p1 + h11()*m1;
+}
+
+
+template<typename T, unsigned N>
+inline HermiteSpline<T, N>::HermiteSpline(const std::vector<Knot> &k):
+	Spline<T, 3, N>(k.front())
+{
+	typedef SplineValue<T, N> SV;
+
+	this->reserve(k.size()-1);
+	for(unsigned i=1; i<k.size(); ++i)
+	{
+		T dx = k[i].x-k[i-1].x;
+		Polynomial<T, 1> t(LinAl::Vector<T, 2>(T(1)/dx, -k[i-1].x/dx));
+		Polynomial<T, 3> p[N];
+		for(unsigned j=0; j<N; ++j)
+			p[j] = Hermite<T>::make(SV::get(k[i-1].y, j), SV::get(k[i-1].dy, j)*dx, SV::get(k[i].y, j), SV::get(k[i].dy, j)*dx)(t);
+		this->add_segment(p, k[i].x);
+	}
+}
+
+template<typename T, unsigned N>
+inline HermiteSpline<T, N>::HermiteSpline(Value p0, Value m0, Value p1, Value m1):
+	Spline<T, 3, N>(Knot(0, p0, m0))
+{
+	typedef SplineValue<T, N> SV;
+
+	this->reserve(1);
+	Polynomial<T, 3> p[N];
+	for(unsigned i=0; i<N; ++i)
+		p[i] = Hermite<T>::make(SV::get(p0, i), SV::get(m0, i), SV::get(p1, i), SV::get(m1, i));
+	this->add_segment(p, T(1));
+}
+} // namespace Interpolate
+} // namespace Msp
+
+#endif
diff --git a/source/interpolate/polynomial.h b/source/interpolate/polynomial.h
new file mode 100644
index 0000000..859eea8
--- /dev/null
+++ b/source/interpolate/polynomial.h
@@ -0,0 +1,168 @@
+#ifndef MSP_INTERPOLATE_POLYNOMIAL_H_
+#define MSP_INTERPOLATE_POLYNOMIAL_H_
+
+#include <algorithm>
+#include <msp/linal/vector.h>
+
+namespace Msp {
+namespace Interpolate {
+
+/**
+Represents a polynomial of degree D with a single variable.  The coefficients
+are stored as a vector with the highest degree term first.
+*/
+template<typename T, unsigned D>
+class Polynomial
+{
+private:
+	LinAl::Vector<T, D+1> coeff;
+
+public:
+	Polynomial() { }
+	Polynomial(const LinAl::Vector<T, D+1> &c): coeff(c) { }
+	template<typename U, unsigned G>
+	Polynomial(const Polynomial<U, G> &);
+
+	unsigned degree() const { return D; }
+
+	LinAl::Vector<T, D+1> coefficients() { return coeff; }
+	const LinAl::Vector<T, D+1> coefficients() const { return coeff; }
+	T &coefficient(unsigned i) { return coeff[i]; }
+	const T &coefficient(unsigned i) const { return coeff[i]; }
+
+	Polynomial &operator+=(const Polynomial &);
+	Polynomial &operator-=(const Polynomial &);
+	Polynomial &operator*=(T);
+	Polynomial &operator/=(T);
+
+	/// Calculates the value of the polynomial at a particular point.
+	T operator()(T) const;
+
+	/** Substitutes the variable of the polynomial with another polynomial.  The
+	result is a new polynomial. */
+	template<unsigned G>
+	Polynomial<T, D*G> operator()(const Polynomial<T, G> &) const;
+};
+
+template<typename T, unsigned D>
+template<typename U, unsigned G>
+inline Polynomial<T, D>::Polynomial(const Polynomial<U, G> &other)
+{
+	char source_cannot_be_larger[D>=G ? 1 : -1];
+	for(unsigned i=0; i<=G; ++i)
+		coeff[D-i] = other.coefficient(G-i);
+}
+
+template<typename T, unsigned D>
+inline Polynomial<T, D> &Polynomial<T, D>::operator+=(const Polynomial<T, D> &p)
+{
+	coeff += p.coeff;
+	return *this;
+}
+
+template<typename T, unsigned D>
+inline Polynomial<T, D> operator+(const Polynomial<T, D> &p, const Polynomial<T, D> &q)
+{
+	Polynomial<T, D> r(p);
+	return r += q;
+}
+
+template<typename T, unsigned D>
+inline Polynomial<T, D> &Polynomial<T, D>::operator-=(const Polynomial<T, D> &p)
+{
+	coeff -= p.coeff;
+	return *this;
+}
+
+template<typename T, unsigned D>
+inline Polynomial<T, D> operator-(const Polynomial<T, D> &p, const Polynomial<T, D> &q)
+{
+	Polynomial<T, D> r(p);
+	return r -= q;
+}
+
+template<typename T, unsigned D>
+inline Polynomial<T, D> &Polynomial<T, D>::operator*=(T s)
+{
+	coeff *= s;
+	return *this;
+}
+
+template<typename T, unsigned D>
+inline Polynomial<T, D> operator*(const Polynomial<T, D> &p, T s)
+{
+	Polynomial<T, D> r(p);
+	return r *= s;
+}
+
+template<typename T, unsigned D>
+inline Polynomial<T, D> operator*(T s, const Polynomial<T, D> &p)
+{
+	return p*s;
+}
+
+template<typename T, unsigned D, unsigned G>
+inline Polynomial<T, D+G> operator*(const Polynomial<T, D> &p, const Polynomial<T, G> &q)
+{
+	LinAl::Vector<T, D+G+1> coeff;
+	for(unsigned i=0; i<=D; ++i)
+		for(unsigned j=0; j<=G; ++j)
+			coeff[i+j] += p.coefficient(i)*q.coefficient(j);
+	return Polynomial<T, D+G>(coeff);
+}
+
+template<typename T, unsigned D>
+inline Polynomial<T, D> &Polynomial<T, D>::operator/=(T s)
+{
+	coeff /= s;
+	return *this;
+}
+
+template<typename T, unsigned D>
+inline Polynomial<T, D> operator/(const Polynomial<T, D> &p, T s)
+{
+	Polynomial<T, D> r(p);
+	return r /= s;
+}
+
+template<typename T, unsigned D>
+inline T Polynomial<T, D>::operator()(T x) const
+{
+	T r = T();
+	T x_i = T(1);
+	for(int i=D; i>=0; --i, x_i*=x)
+		r += coeff[i]*x_i;
+	return r;
+}
+
+template<typename T, unsigned D>
+template<unsigned G>
+inline Polynomial<T, D*G> Polynomial<T, D>::operator()(const Polynomial<T, G> &p) const
+{
+	Polynomial<T, D*G> r;
+	r.coefficient(D*G) = coeff[D];
+
+	LinAl::Vector<T, D*G+1> cf;
+	cf[0] = T(1);
+
+	for(unsigned i=1; i<=D; ++i)
+	{
+		for(int j=i*G; j>=0; --j)
+		{
+			T c = T();
+			for(unsigned k=0; (k<=static_cast<unsigned>(j) && k<=G); ++k)
+				c += cf[j-k]*p.coefficient(G-k);
+			cf[j] = c;
+		}
+
+		for(unsigned j=0; j<=i*G; ++j)
+			r.coefficient(D*G-j) += coeff[D-i]*cf[j];
+	}
+
+	return r;
+}
+
+} // namespace Interpolate
+} // namespace Msp
+
+#endif
diff --git a/source/interpolate/spline.h b/source/interpolate/spline.h
new file mode 100644
index 0000000..67c8df0
--- /dev/null
+++ b/source/interpolate/spline.h
@@ -0,0 +1,206 @@
+#ifndef MSP_INTERPOLATE_SPLINE_H_
+#define MSP_INTERPOLATE_SPLINE_H_
+
+#include <algorithm>
+#include <vector>
+#include "polynomial.h"
+
+namespace Msp {
+namespace Interpolate {
+
+template<typename T, unsigned N>
+struct SplineValue
+{
+	typedef LinAl::Vector<T, N> Type;
+	static T get(const Type &v, unsigned i) { return v[i]; }
+	static Type make(const T *v) { return Type(v); }
+};
+
+template<typename T>
+struct SplineValue<T, 1>
+{
+	typedef T Type;
+	static T get(const Type &v, unsigned) { return v; }
+	static Type make(const T *v) { return *v; }
+};
+
+/**
+Stores a spline of degree D.  It is a piecewise polynomial function with value
+continuity.  Derivatives are not guaranteed to be continuous.
+
+The template parameter N controls the dimensionality of the spline's values.
+Multi-dimensional splines can be used to implement parametric curves.
+
+While this class contains everything needed to store and evaluate splines, it
+cannot be used to construct non-empty splines.  See also HermiteSpline.
+*/
+template<typename T, unsigned D, unsigned N = 1>
+class Spline
+{
+public:
+	typedef typename SplineValue<T, N>::Type Value;
+
+	struct Knot
+	{
+		T x;
+		Value y;
+
+		Knot(): x(T()) { }
+		Knot(T x_, const Value &y_): x(x_), y(y_) { }
+	};
+
+	struct Segment
+	{
+		Knot start_knot;
+		Polynomial<T, D> polynomials[N];
+		Knot end_knot;
+
+		Value operator()(T) const;
+	};
+
+private:
+	enum { STRIDE = sizeof(Segment)-sizeof(Knot) };
+
+	unsigned short n_segments;
+	unsigned short capacity;
+	char *segments;
+
+public:
+	Spline();
+protected:
+	Spline(const Knot &);
+public:
+	Spline(const Spline &);
+	Spline &operator=(const Spline &);
+	~Spline();
+
+private:
+	static unsigned data_size(unsigned n) { return sizeof(Knot)+n*STRIDE; }
+protected:
+	void reserve(unsigned);
+	void add_segment(const Polynomial<T, D> [N], T);
+
+public:
+	unsigned degree() const { return D; }
+	unsigned size() const { return n_segments; }
+	const Segment &segment(unsigned i) const;
+	const Knot &knot(unsigned i) const { return i==0 ? segment(0).start_knot : segment(i-1).end_knot; }
+	const Polynomial<T, D> &polynomial(unsigned i, unsigned j = 0) const { return segment(i).polynomials[j]; }
+
+	Value operator()(T) const;
+};
+
+template<typename T, unsigned D, unsigned N>
+inline Spline<T, D, N>::Spline():
+	n_segments(0),
+	capacity(0),
+	segments(0)
+{ }
+
+template<typename T, unsigned D, unsigned N>
+inline Spline<T, D, N>::Spline(const Knot &k):
+	n_segments(0),
+	capacity(0),
+	segments(new char[sizeof(Knot)])
+{
+	new(segments) Knot(k);
+}
+
+template<typename T, unsigned D, unsigned N>
+inline Spline<T, D, N>::Spline(const Spline &s):
+	n_segments(0),
+	capacity(0),
+	segments(0)
+{
+	*this = s;
+}
+
+template<typename T, unsigned D, unsigned N>
+inline Spline<T, D, N> &Spline<T, D, N>::operator=(const Spline &s)
+{
+	if(s.segments)
+	{
+		reserve(s.n_segments);
+		std::copy(s.segments, s.segments+data_size(s.n_segments), segments);
+		n_segments = s.n_segments;
+	}
+	else
+	{
+		delete[] segments;
+		n_segments = 0;
+		capacity = 0;
+		segments = 0;
+	}
+}
+
+template<typename T, unsigned D, unsigned N>
+inline Spline<T, D, N>::~Spline()
+{
+	delete[] segments;
+}
+
+template<typename T, unsigned D, unsigned N>
+inline void Spline<T, D, N>::reserve(unsigned n)
+{
+	if(n<capacity)
+		return;
+
+	char *new_segs = new char[data_size(n)];
+	if(segments)
+	{
+		std::copy(segments, segments+data_size(n_segments), new_segs);
+		delete[] segments;
+	}
+	segments = new_segs;
+	capacity = n;
+}
+
+template<typename T, unsigned D, unsigned N>
+inline void Spline<T, D, N>::add_segment(const Polynomial<T, D> polys[N], T end_x)
+{
+	if(!segments)
+		throw std::logic_error("Spline::add_segment");
+
+	if(n_segments==capacity)
+		reserve(n_segments+1);
+	++n_segments;
+
+	Segment &seg = *reinterpret_cast<Segment *>(segments+(n_segments-1)*STRIDE);
+	std::copy(polys, polys+N, seg.polynomials);
+	seg.end_knot.x = end_x;
+	seg.end_knot.y = seg(end_x);
+}
+
+template<typename T, unsigned D, unsigned N>
+inline const typename Spline<T, D, N>::Segment &Spline<T, D, N>::segment(unsigned i) const
+{
+	return *reinterpret_cast<const Segment *>(segments+i*STRIDE);
+}
+
+template<typename T, unsigned D, unsigned N>
+inline typename Spline<T, D, N>::Value Spline<T, D, N>::operator()(T x) const
+{
+	if(!n_segments)
+		return Value();
+
+	const Segment *seg = &segment(0);
+	for(unsigned i=1; (i<n_segments && x>seg->end_knot.x); ++i)
+		seg = &segment(i);
+
+	return (*seg)(x);
+}
+
+template<typename T, unsigned D, unsigned N>
+inline typename Spline<T, D, N>::Value Spline<T, D, N>::Segment::operator()(T x) const
+{
+	T v[N];
+	for(unsigned i=0; i<N; ++i)
+		v[i] = polynomials[i](x);
+
+	return SplineValue<T, N>::make(v);
+}
+
+} // namespace Interpolate
+} // namespace Msp
+
+#endif
-- 
2.45.2