3 #include <unsupported/Eigen/Polynomials>
7 for (
unsigned int i = 0; i < x.size(); i++) {
12 double interp1d(
const std::vector<double>* x,
const std::vector<double>* y,
double x0) {
13 std::size_t i, L, R, M;
31 if (i < (*x).size() - 2) {
33 return QuadInterp((*x)[i], (*x)[i + 1], (*x)[i + 2], (*y)[i], (*y)[i + 1], (*y)[i + 2], x0);
36 return QuadInterp((*x)[i], (*x)[i - 1], (*x)[i - 2], (*y)[i], (*y)[i - 1], (*y)[i - 2], x0);
64 for (i = 1; i < y_in; i++) {
65 product = product * x_in;
70 void MatInv_2(
double A[2][2],
double B[2][2]) {
74 Det = A[0][0] * A[1][1] - A[1][0] * A[0][1];
75 B[0][0] = 1.0 / Det * A[1][1];
76 B[1][1] = 1.0 / Det * A[0][0];
77 B[1][0] = -1.0 / Det * A[1][0];
78 B[0][1] = -1.0 / Det * A[0][1];
81 void solve_cubic(
double a,
double b,
double c,
double d,
int& N,
double& x0,
double& x1,
double& x2) {
93 x0 = (-c + sqrt(c * c - 4 * b * d)) / (2 * b);
94 x1 = (-c - sqrt(c * c - 4 * b * d)) / (2 * b);
103 double DELTA = 18 * a * b * c * d - 4 * b * b * b * d + b * b * c * c - 4 * a * c * c * c - 27 * a * a * d * d;
105 double p = (3 * a * c - b * b) / (3 * a * a);
106 double q = (2 * b * b * b - 9 * a * b * c + 27 * a * a * d) / (27 * a * a * a);
111 if (4 * p * p * p + 27 * q * q > 0 && p < 0) {
112 t0 = -2.0 * std::abs(q) / q * sqrt(-p / 3.0) * cosh(1.0 / 3.0 * acosh(-3.0 * std::abs(q) / (2.0 * p) * sqrt(-3.0 / p)));
114 t0 = -2.0 * sqrt(p / 3.0) * sinh(1.0 / 3.0 * asinh(3.0 * q / (2.0 * p) * sqrt(3.0 / p)));
117 x0 = t0 - b / (3 * a);
118 x1 = t0 - b / (3 * a);
119 x2 = t0 - b / (3 * a);
123 double t0 = 2.0 * sqrt(-p / 3.0) * cos(1.0 / 3.0 * acos(3.0 * q / (2.0 * p) * sqrt(-3.0 / p)) - 0 * 2.0 *
M_PI / 3.0);
124 double t1 = 2.0 * sqrt(-p / 3.0) * cos(1.0 / 3.0 * acos(3.0 * q / (2.0 * p) * sqrt(-3.0 / p)) - 1 * 2.0 *
M_PI / 3.0);
125 double t2 = 2.0 * sqrt(-p / 3.0) * cos(1.0 / 3.0 * acos(3.0 * q / (2.0 * p) * sqrt(-3.0 / p)) - 2 * 2.0 *
M_PI / 3.0);
128 x0 = t0 - b / (3 * a);
129 x1 = t1 - b / (3 * a);
130 x2 = t2 - b / (3 * a);
133 void solve_quartic(
double a,
double b,
double c,
double d,
double e,
int& N,
double& x0,
double& x1,
double& x2,
double& x3) {
137 Eigen::PolynomialSolver<double, Eigen::Dynamic> solver;
138 Eigen::VectorXd coeff(5);
139 coeff << e, d, c, b, a;
140 solver.compute(coeff);
142 std::vector<double> realRoots;
143 solver.realRoots(realRoots);
144 N =
static_cast<int>(realRoots.size());
174 if (i == 4)
return false;
191 if (i == 4)
return false;
201 return a * x * x * x +
b * x * x +
c * x +
d;