199 lines
4.1 KiB
C++
199 lines
4.1 KiB
C++
#pragma once
|
|
#include <cgv/math/polynomial.h>
|
|
|
|
namespace cgv {
|
|
namespace math {
|
|
|
|
|
|
/**
|
|
* bivariate polynomials are stored in matrices in the following order:
|
|
* [a,b,c,d a*x1^3*x2^3 + b*x1^2*x2^3 + c*x1*x2^3 + d*x2^3 +
|
|
* e,f,g,h e*x1^3*x2^2 + f*x1^2*x2^2 + g*x1*x2^2 + h*x2^2 +
|
|
* i,j,k,l i*x1^3*x2 + j*x1^2*x2 + k*x1*x2 + l*x2 +
|
|
* m,n,o,p] m*x1^3 + n*x1^2 + o*x1 + p
|
|
*/
|
|
|
|
/// evaluate a bivariate polynomial p at (x1,x2)
|
|
/// p is the coefficients matrix of the bivariate polynomial
|
|
/// evaluation is done by using the horner scheme
|
|
template <typename T>
|
|
T bipoly_val(const mat<T>& bp, const T& x1, const T& x2)
|
|
{
|
|
unsigned n = bp.ncols();
|
|
unsigned m = bp.nrows();
|
|
assert(n > 0);
|
|
assert(m > 0);
|
|
|
|
vec<T> r(m);
|
|
for(unsigned i = 0; i < m;i++)
|
|
r(i) = poly_val(bp.row(i),x1);
|
|
|
|
return poly_val(r,x2);
|
|
}
|
|
|
|
/// evaluate a bivariate polynomial p at 2d position x
|
|
/// p is the coefficients matrix of the bivariate polynomial
|
|
/// evaluation is done by using the horner scheme
|
|
template <typename T>
|
|
T bipoly_val(const mat<T>& bp, const vec<T>& x)
|
|
{
|
|
unsigned n = bp.ncols();
|
|
unsigned m = bp.nrows();
|
|
assert(n > 0);
|
|
assert(m > 0);
|
|
assert(v.size() == 2)
|
|
|
|
vec<T> r(n);
|
|
for(unsigned i = 0; i < m;i++)
|
|
r(i) = poly_val(bp.row(i),x(0));
|
|
|
|
return poly_val(r,x(1));
|
|
}
|
|
|
|
|
|
//returns vandermonde matrix for bivariate polynomial fitting
|
|
template <typename T>
|
|
cgv::math::mat<T> vander2(unsigned degree1, unsigned degree2,
|
|
const vec<T>& X1,const vec<T> X2)
|
|
{
|
|
assert(X1.size()>0);
|
|
assert(X1.size() == X2.size());
|
|
unsigned n = X1.size();
|
|
unsigned m = (degree1+1)*(degree2+1);
|
|
cgv::math::mat<T> Vxy(n,m);
|
|
|
|
cgv::math::mat<T> Vx = vander(degree1,X1);
|
|
cgv::math::mat<T> Vy = vander(degree2,X2);
|
|
|
|
for(unsigned r = 0; r < n; r++)
|
|
{
|
|
for(unsigned i = 0; i < degree2+1; i++)
|
|
for(unsigned j =0; j < degree1+1; j++)
|
|
Vxy(r,j*(degree2+1)+i) = Vy(r,i)*Vx(r,j);
|
|
|
|
}
|
|
|
|
|
|
return Vxy;
|
|
|
|
}
|
|
|
|
|
|
|
|
//fit a bivariate polynomial
|
|
template <typename T>
|
|
mat<T> bipoly_fit(unsigned degree1, unsigned degree2,
|
|
const vec<T>& X1,const vec<T> X2,const vec<T> Y)
|
|
{
|
|
|
|
assert(X1.size() == X2.size());
|
|
assert(X1.size() == Y.size());
|
|
|
|
|
|
mat<T> V = cgv::math::vander2(degree1,degree2,X1,X2);
|
|
mat<T> A;
|
|
vec<T> b,bp;
|
|
AtA(V,A);
|
|
Atx(V,Y,b);
|
|
svd_solve(A,b,bp);
|
|
|
|
return reshape(bp,degree2+1,degree1+1);
|
|
|
|
}
|
|
|
|
|
|
/*
|
|
template <typename T>
|
|
mat<T> bipoly_fit2(unsigned degree1, unsigned degree2,
|
|
const vec<T>& X1,const vec<T> X2,const vec<T> Y)
|
|
{
|
|
|
|
assert(X1.size() == X2.size());
|
|
assert(X1.size() == Y.size());
|
|
|
|
vec<T> C = poly_fit<T>(degree2,X2,Y);
|
|
//std::cout << C << "\n";
|
|
mat<T> B = dyad(cgv::math::ones<T>(X1.size()),C);
|
|
mat<T> V = cgv::math::vander(degree1,X1);
|
|
mat<T> A;
|
|
|
|
AtA(V,A);
|
|
|
|
mat<T> D,bp;
|
|
AtB(V,B,D);
|
|
|
|
svd_solve(A,D,bp);
|
|
//std::cout <<":\n"<< A*bp-D;
|
|
|
|
return bp;
|
|
}*/
|
|
|
|
////analytic derivation of bivariate polynomial dp(x1,x2)/dx1
|
|
template <typename T>
|
|
mat<T> bipoly_der_x1(const mat<T>& bp)
|
|
{
|
|
|
|
assert(bp.ncols() > 0 && bp.nrows() > 0);
|
|
if(bp.ncols() == 1)
|
|
return zeros<T>(1,1);
|
|
|
|
mat<T> m(bp.nrows(),bp.ncols()-1);
|
|
for(unsigned i = 0; i < bp.nrows(); i++)
|
|
m.set_row(i,poly_der(bp.row(i)));
|
|
return m;
|
|
}
|
|
|
|
////analytic derivation of bivariate polynomial dp(x1,x2)/dx2
|
|
template <typename T>
|
|
mat<T> bipoly_der_x2(const mat<T>& bp)
|
|
{
|
|
|
|
assert(bp.ncols() > 0 && bp.nrows() > 0);
|
|
if(bp.nrows() == 1)
|
|
return zeros<T>(1,1);
|
|
T n = (T)(bp.nrows()-1);
|
|
|
|
mat<T> m(bp.nrows()-1,bp.ncols());
|
|
for(unsigned i = 0; i < bp.nrows()-1; i++)
|
|
m.set_row(i,(n-(T)i)* bp.row(i));
|
|
return m;
|
|
}
|
|
|
|
///analytic integration of bivariate polynomial int p(x1,x2) dx1
|
|
template <typename T>
|
|
mat<T> bipoly_int_x1(const mat<T>& bp,const T& k=0)
|
|
{
|
|
mat<T> m(bp.nrows(),bp.ncols()+1);
|
|
for(unsigned i = 0; i < bp.nrows();i++)
|
|
m.set_row(i,poly_int(bp.row(i),k));
|
|
return m;
|
|
}
|
|
|
|
///analytic integration of bivariate polynomial int p(x1,x2) dx2
|
|
template <typename T>
|
|
mat<T> bipoly_int_x2(const mat<T>& bp,const T& k=0)
|
|
{
|
|
|
|
mat<T> v(1,bp.ncols());
|
|
v.zeros();
|
|
v(0,bp.ncols()-1)=k;
|
|
mat<T> m = vertcat(bp,v);
|
|
|
|
for(unsigned i = 0; i < m.nrows()-2;i++)
|
|
for(unsigned j = 0; j < m.ncols();j++)
|
|
m(i,j)=m(i,j)/(m.nrows()-1-i);
|
|
|
|
|
|
|
|
return m;
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
}
|
|
} |