polynomial.h

#pragma once
 
#include <iostream>
#include <cmath>
#include <cassert>
#include <cgv/math/vec.h>
#include <cgv/math/mat.h>
#include <cgv/math/functions.h>
#include <cgv/math/lin_solve.h>
 
namespace cgv {
namespace math {
 
 
template <typename T>
 
T poly_val(const vec<T>& p, const T& x)
{
    assert(p.size() > 0);
    T y=p(0);
 
    for(unsigned i = 1; i<p.size(); i++)
        y= y*x +p(i);
 
    return y;
}
 
template <typename T>
vec<T> poly_val(const vec<T>& p, const vec<T>& x)
 
{
 
    assert(p.size() > 0);
    vec<T> y(x.size());
    y.fill(p(0));
 
    for(unsigned i = 1; i<p.size(); i++)
        y= x*y + p(i);
 
    return y;
 
}
 
template <typename T>
vec<T> poly_mult(const vec<T>& u, const vec<T>& v)
{
    unsigned s =u.size() + v.size()-1;
    vec<double> w(s);
    for(unsigned k = 1; k <= s; k++)
    {
        w(k-1) = 0;
        int j = 0;
        
        int jmin = (int)k+1-(int)v.size();
        if(jmin < 1)
            jmin = 1;
 
        int jmax = (int)u.size();
        if (jmax > (int)k)
            jmax=k;
        
 
        for(int j = jmin; j <=jmax; j++)
            w(k-1)+=u(j-1)*v(k-j);
    }
    return w;
}
 
template <typename T>
bool poly_div(const vec<T>& f, const vec<T>& g, vec<T>& q, vec<T>& r)
{
    if(g.size() > f.size())
    {
        q.resize(1);
        q(0)=0;
        r=f;
        return false;
    }
    else
    {
        vec<T> p = f;
        //std::cout << p << std::endl;
        q.resize(f.size()-g.size()+1);
        for(unsigned j = 0; g.size()+j <=p.size();j++)
        {
            q(j) = p(j)/g(0);
            for(unsigned i = j; i < g.size()+j; i++)
            {
                p(i)= p(i)- q(j)*g(i-j);
            }
            //std::cout <<p<<std::endl;
        }
        unsigned s=g.size();
        unsigned a=p.size()-g.size();
        while(p(a) == 0 && s > 0)
        {
            a++;s--;
        }
 
        if(s == 0)
        {
            r.resize(1);
            r(0)=0;
            return true;
        }
        else
        {
            r=p.sub_vec(a,s);       
            return false;
        }
    }
    
    
}
 
//returns addition of polynomials u and v
template <typename T>
vec<T> poly_add(const vec<T>& u, const vec<T>& v)
{
    unsigned n = u.size();
    unsigned m = v.size();
 
    vec<double> r;
    if(n >= m)
    {
        r.resize(n);
        unsigned i = 0;
        unsigned d = n-m;
        for(;i < d;i++)
            r(i) = u(i);
        
        for(;i < n;i++)
            r(i) = u(i)+v(i-d);
    }else
    {
        r.resize(m);
        unsigned i = 0;
        unsigned d = m-n;
        for(;i < d;i++)
            r(i) = v(i);    
        for(;i < m;i++)
            r(i) = v(i)+u(i-d);
    }
        
    return r;
}
 
//returns subtraction of polynomials u and v
template <typename T>
vec<T> poly_sub(const vec<T>& u, const vec<T>& v)
{
    unsigned n = u.size();
    unsigned m = v.size();
 
    vec<double> r;
    if(n >= m)
    {
        r.resize(n);
        unsigned i = 0;
        unsigned d = n-m;
        for(;i < d;i++)
            r(i) = u(i);
        
        for(;i < n;i++)
            r(i) = u(i)-v(i-d);
    }else
    {
        r.resize(m);
        unsigned i = 0;
        unsigned d = m-n;
        for(;i < d;i++)
            r(i) = -v(i);   
        for(;i < m;i++)
            r(i) = u(i-d)-v(i);
    }
        
    return r;
}
 
 
//analytic derivation of polynomial p 
template <typename T>
vec<T> poly_der(const vec<T>& p)
{
    assert(p.size() > 0);
    unsigned n = p.size(); 
 
    if(n <= 1)
    {
        vec<T> q(1);
        q(0) = 0;
        return q;
    }
    else
    {
        unsigned m = n-1;
        vec<T> q(m);
        for(unsigned i = 0; i< m; i++)
            q(i)= p(i)*(m-i);
        return q;
    }
}
 
//analytic integrate polynomial p using integration constant k
template <typename T>
vec<T> poly_int(const vec<T>& p, const T& k=0)
{
    assert(p.size() > 0);
    unsigned m = p.size()+1; 
    
    vec<T> q(m);
    for(unsigned i = 0; i < m-1; i++)
        q(i)= p(i)/(m-i-1);
        
    
    q(m-1)=k;
    return q;
}
 
template <typename T>
vec<T> bernstein_polynomial(unsigned j, unsigned g)
{
    vec<T> p;
    p.zeros(j+1);
    p(0)=1.0;// p = t^j 
 
    
    for(unsigned i = 0; i < g-j; i++)   
        p = poly_mult(p, vec<T>(-1.0,1));   //p=p*(1-t) 
    
    return nchoosek(g,j)*p;
}
 
 
//returns lagrange basis polynomial
template <typename T>
vec<T> lagrange_basis_polynomial(unsigned i, const vec<T>& u)
{
    //polynomial p = 1
    vec<T> p(1);
    p(0)=1.0;   
 
    unsigned g = u.size()-1;
 
    for(unsigned j = 0; j <= g; j++)
    {
        if(i == j) continue;    
        p = poly_mult(p, vec<T>(1.0,-u(j))) / (u(i)-u(j));      
    }
    return p;
}
 
 
template <typename T>
vec<T> newton_basis_polynomial(unsigned i, const vec<T>& u)
{
    //polynomial p = 1
    vec<T> p(1);
    p(0)=1.0;   
 
    unsigned g = u.size()-1;
 
    for(unsigned j = 0; j <= g; j++)
    {
        
        p = poly_mult(p, vec<T>(1.0,-u(j)));        
    }
    return p;
}
 
 
//returns the vandermonde matrix which can be used for fitting 
//polynomials (for large order polynomials use orthogonal basis instead)
//x = [ 1 2 3 4] -> V(i,j) x(i)^(n-j) V is n x n with n = x.size()
 
template <typename T>
mat<T> vander(const vec<T>& x)
{
    return vander(x.size()-1,x);
}
 
//returns vandermonde matrix 
template <typename T>
mat<T> vander(unsigned degree,const vec<T>& x)
{
    assert(x.size() >= 1);
    unsigned n = x.size();
    cgv::math::mat<T> V(n,degree+1);
    
    for(unsigned i = 0; i < n; i++)
    {
        V(i,degree) = (T)1;
        for(int j = (int)degree-1; j>= 0; j--)
            V(i,j)=V(i,j+1)*x(i);
    }
 
    return  V;
}
 
//fit a polynomial of degree n into data such that E = sum_i ||p(X(i)) - Y(i)||² is minimized
//uses vandermonde matrix and svd solver 
//large degrees can be numerically instable keep n < 10
//otherwise use orthogonal basis for fitting
template <typename T>
vec<T> poly_fit(unsigned n,const vec<T>&X, const vec<T>&Y)
{
    mat<T> V = cgv::math::vander(n,X);
    mat<T> A;
    vec<T> b,p;
    AtA(V,A);
    Atx(V,Y,b);
    svd_solve(A,b,p);
    return p;
}
 
}
}