Add an interpolation sub-library

[libs/math.git] / source / interpolate / hermitespline.h

diff --git a/source/interpolate/hermitespline.h b/source/interpolate/hermitespline.h
new file mode 100644 (file)
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