library "mspmath"
{
+ source "source/interpolate";
source "source/linal";
source "source/geometry";
install true;
--- /dev/null
+#include "hermitespline.h"
+#include "polynomial.h"
+#include "spline.h"
--- /dev/null
+#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
--- /dev/null
+#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
--- /dev/null
+#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
projects / libs / math.git / commitdiff