115 lines
2.8 KiB
C++
115 lines
2.8 KiB
C++
#include "pch.h"
|
|
#include "ZZ_Math.h"
|
|
|
|
void ZZ_MATH::PolyFit::Eigen_Polyfit(const std::vector<double> &xv, const std::vector<double> &yv, std::vector<double> &coeff, int order)
|
|
{
|
|
Eigen::MatrixXd A(xv.size(), order + 1);
|
|
Eigen::VectorXd yv_mapped = Eigen::VectorXd::Map(&yv.front(), yv.size());
|
|
Eigen::VectorXd result;
|
|
|
|
assert(xv.size() == yv.size());
|
|
assert(xv.size() >= order + 1);
|
|
|
|
|
|
for (size_t i = 0; i < xv.size(); i++)
|
|
{
|
|
for (size_t j = 0; j < order + 1; j++)
|
|
{
|
|
A(i, j) = pow(xv.at(i), j);
|
|
}
|
|
}
|
|
|
|
|
|
result = A.householderQr().solve(yv_mapped);
|
|
|
|
coeff.resize(order + 1);
|
|
for (size_t i = 0; i < order + 1; i++)
|
|
{
|
|
coeff[i] = result[i];
|
|
}
|
|
}
|
|
|
|
double ZZ_MATH::PolyFit::Eigen_Polyeval(std::vector<double> coeffs, double x)
|
|
{
|
|
double result = 0.0;
|
|
|
|
for (int i = 0; i < coeffs.size(); i++)
|
|
{
|
|
result += coeffs[i] * pow(x, i);
|
|
}
|
|
|
|
return result;
|
|
}
|
|
|
|
|
|
Eigen::VectorXd ZZ_MATH::SplineFit::Eigen_Normalize(const Eigen::VectorXd &x)
|
|
{
|
|
using namespace Eigen;
|
|
VectorXd x_norm;
|
|
x_norm.resize(x.size());
|
|
const double min = x.minCoeff();
|
|
const double max = x.maxCoeff();
|
|
|
|
for (int k = 0; k < x.size(); k++)
|
|
{
|
|
x_norm(k) = (x(k) - min) / (max - min);
|
|
}
|
|
|
|
return x_norm;
|
|
|
|
}
|
|
|
|
void ZZ_MATH::SplineFit::Test(std::vector<double> const &x_vec, std::vector<double> const &y_vec)
|
|
{
|
|
typedef Spline<double, 1> Spline1D;
|
|
typedef SplineFitting<Spline1D> Spline1DFitting;
|
|
|
|
|
|
//VectorXd vx,vy;
|
|
//vx.resize(x_vec.size());
|
|
//vy.resize(y_vec.size());
|
|
//vx.Map(&x_vec.front(), x_vec.size());
|
|
//vy.Map(&y_vec.front(), y_vec.size());
|
|
|
|
Eigen::VectorXd vx = Eigen::VectorXd::Map(&x_vec.front(), 5/*x_vec.size()*/);
|
|
Eigen::VectorXd vy = Eigen::VectorXd::Map(&y_vec.front(), 5/*y_vec.size()*/);
|
|
|
|
const double scale = 1 / (vx.maxCoeff() - vx.minCoeff());
|
|
const double scale_sq = scale * scale;
|
|
|
|
//VectorXd knots = Eigen_Normalize(vx);
|
|
//Spline1D spline = Spline1DFitting::Interpolate(vy.transpose(),3, knots);
|
|
//double a;
|
|
// a = spline.derivatives(0,1)(0);
|
|
//Eigen::VectorXd xvals(5);
|
|
//Eigen::VectorXd yvals(xvals.rows());
|
|
//xvals << 0, 1, 2,3,4;
|
|
//yvals << 0, 1, 4,9,16;
|
|
|
|
SplineInterpolation s(vx, vy);
|
|
return;
|
|
}
|
|
|
|
ZZ_MATH::SplineFit::SplineInterpolation::SplineInterpolation(Eigen::VectorXd const &x_vec, Eigen::VectorXd const &y_vec):
|
|
x_min(x_vec.minCoeff()), x_max(x_vec.maxCoeff()),
|
|
spline_(Eigen::SplineFitting<Eigen::Spline<double, 1>>::
|
|
Interpolate(y_vec.transpose(), 3, scaled_values(x_vec)))
|
|
{
|
|
|
|
}
|
|
|
|
double ZZ_MATH::SplineFit::SplineInterpolation::operator()(double x) const
|
|
{
|
|
return spline_(scaled_value(x))(0);
|
|
}
|
|
|
|
double ZZ_MATH::SplineFit::SplineInterpolation::scaled_value(double x) const
|
|
{
|
|
return (x - x_min) / (x_max - x_min);
|
|
}
|
|
|
|
Eigen::RowVectorXd ZZ_MATH::SplineFit::SplineInterpolation::scaled_values(Eigen::VectorXd const &x_vec) const
|
|
{
|
|
return x_vec.unaryExpr([this](double x) { return scaled_value(x); }).transpose();
|
|
}
|