Add an interpolation sub-library

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

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