#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;

}
 // back-substitution loop

for(i=n-1;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());