38 double const pq1 = -a1 / 2.0;
39 double const pq2 = std::sqrt(std::max(0.0, a1 * a1 / 4.0 - a0));
40 double const sol1 = pq1 + pq2;
41 double const sol2 = pq1 - pq2;
44 if (std::abs(sol1 - 0.5) < std::abs(sol2 - 0.5))
66 double const p = a1 - a2 * a2 / 3.0;
67 double const q = a0 - a1 * a2 / 3.0 + a2 * a2 * a2 / 13.5;
68 double const discr = 4.0 * p * p * p + 27.0 * q * q;
73 double const sqrtp3 = std::sqrt(-p / 3.0);
74 double const theta = std::acos(3.0 * q / (-2.0 * p * sqrtp3)) / 3.0;
76 root[2] = -2.0 * sqrtp3 * std::cos(theta) - a2 / 3.0;
77 root[0] = 2.0 * sqrtp3 * std::cos(4.0 * std::atan(1.0) / 3.0 - theta) - a2 / 3.0;
78 root[1] = -(root[0] + root[2] + a2);
81 double the_root = 0.0;
83 for (
int i = 0; i < 3; ++i)
84 if (root[i] >= 0.0 && root[i] <= 1.0)
102 double const v0 = a0 * a3;
103 double const v1 = (a0 + a1 + a2 + 1.0) * a3;
108 else if (discr > 0.0)
112 double const c = std::pow(std::sqrt(discr / 108.0) + std::abs(q) / 2.0, 1.0 / 3.0);
113 double const t = c - p / (3.0 * c);
118 return -t - a2 / 3.0;
141 throw std::runtime_error(
"Unreachable code");
149 double a0 = 0.0, a1 = 0.0, a2 = 0.0, a3 = 0.0;
161 double scale = 2.0 * (v1 - v0) / (d0 + d1);
164 a2 = 3.0 * (v1 - v0) - (2.0 * d0 + d1) * scale;
171 a1 = (d0 - d1) / 2.0 + v1 - v0;
172 a2 = (d1 - d0) / 2.0;
180 a2 = 3.0 * v1 - 3.0 * v0 - 2.0 * d0 - 1.0 * d1;
181 a3 = 2.0 * v0 - 2.0 * v1 + d0 + d1;
186 throw std::runtime_error(
"Invalid interpolation type");
189 return std::min(1.0, std::max(0.0, root));
#define FSSR_NAMESPACE_END
#define FSSR_NAMESPACE_BEGIN
double find_root_square(double a0, double a1, double a2)
Finds the root of a quadratic function f(x) = a0 + a1 * x + a2 * x^2.
double interpolate_root(double v0, double v1, double d0, double d1, InterpolationType type)
Interpolates the root of an unknown function f(x) given value and derivative constraints: f(0) = v0,...
double find_root_linear(double a0, double a1)
Finds the root of a linear function f(x) = a0 + a1(x).
double find_root_cubic(double a0, double a1, double a2, double a3)
Finds the root of a cubic function f(x) = a0 + a1 * x + a2 * x^2 + a3 * x^3.