splinterp.cpp

#include <std::vector>
#include <opencv2/core/core.hpp>
#include <iostream>
#include <stdio.h>
/*
This work by Ryan Muller released under the Creative Commons CC0 License
http://creativecommons.org/publicdomain/zero/1.0/
*/

/**
  Spline interpolation of a parametric function.

  INPUT: std::vector<double> x
        A list of double values that represent sampled
        points. The array index of each point will be
        taken as the parameter t by which x will be
        represented as a function.

  OUTPUT: std::vector<cv::Vec4d> P
        A list of cv::Vec4d representing polynomials. To
        interpret segment [i]:
        x(t) =  P0*a + P1*b + P2*(a^3-a)/6 + P3*(b^3-b)/6
        where a = t-i
              b = i-t+1
  */
std::vector<cv::Vec4d> splinterp(std::vector<double> x){

    std::vector<cv::Vec4d> out;

    // spline size
    int n=x.size();

    // loop counter
    int i;

    // working variables
    double p;
    std::vector<double> u;

    // second derivative
    std::vector<double> z;

    u.resize(n);
    z.resize(n);

    // set the second derivative to 0 at the ends
    z[0] = u[0] = 0;
    z[n-1] = 0;

    // decomposition loop
    for(i=1;i<n-1;i++){
        p = 0.5*z[i-1] + 2.0;
        z[i] = -0.5/p;
        u[i] = x[i+1]+x[i-1]-2*x[i];
        u[i] = (3*u[i]-0.5*u[i-1])/p;
    }

    z[n-1] = u[n-1];
    // back-substitution loop
    for(i=n-2;i>0;i--){
        z[i] = z[i] * z[i+1] + u[i];
    }

    for(i=0;i<n-1;i++){
        out.push_back(cv::Vec4d(
                          x[i+1],
                          x[i],
                          z[i+1],
                          z[i]
                          ));
    }

    return out;

}

/**
  A demo of Splinterp's capabilities.
  Modify the function in the for loop to change
  the sample points. When you run the program
  it will generate a polynomial for each segment
  and output it in text format. Copy and paste
  the output into WxMaxima (andrejv.github.com/wxmaxima/)
  to see a plot of your interpolated curves!
  */
void splinterpDemo(){

    std::vector<double> samples;
    std::vector<cv::Vec4d> spline;
    cv::Vec4d p;
    double x;
    unsigned int i, imax=12;

    for(i=0;i<imax;i++){
        // modify this function
        x = (cos(2*M_PI*i/(imax-1)));
        samples.push_back(x);
    }

    spline = splinterp(samples);



    for(i=0;i<spline.size();i++){
        p = spline.at(i);
        printf("x%d(t):= if t>= %d and t<=%d then %.8f * (t-%d) + %.8f * (%d-t) + "
               "%.8f * ((t-%d)^3-(t-%d))/6 + %.8f * ((%d-t)^3-(%d-t))/6 $\n",
               i,i,i+1,p[0],i,p[1],i+1,p[2],i,i,p[3],i+1,i+1);
    }
    printf("wxplot2d([");
    for(i=0;i<spline.size();i++){
        if(i>0) cout << ",";
        printf("x%d(t)",i);
    }
    printf("],[t,0,%d],[gnuplot_preamble,\"set nokey;\"])$\n",spline.size());
}