first commit

ZZ_Math.cpp

@@ -0,0 +1,114 @@
+#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();
+}