34#ifndef ADAPTED_STEP_NULL_TOLERANCE
35 #define ADAPTED_STEP_NULL_TOLERANCE 1e-6
39#ifndef ADAPTED_STEP_MAX_ROOTFINDING_STEPS
40 #define ADAPTED_STEP_MAX_ROOTFINDING_STEPS 1e6
56static float p4_eval(
float x,
float a4,
float a3,
float a2,
float a1,
float a0)
58 return a0 + x * (a1 + x * (a2 + x * (a3 + x * (a4))));
76static float p4_halley(
float a4,
float a3,
float a2,
float a1,
float a0,
float tol,
float init,
int max_steps)
79 float p_x =
p4_eval(x, a4, a3, a2, a1, a0);
91 float a2_dd = 3 * a3_d;
92 float a1_dd = 2 * a2_d;
97 while (ABS(p_x) > tol &&
step < max_steps) {
98 float p_xd =
p4_eval(x, a4_d, a3_d, a2_d, a1_d, a0_d);
99 float p_xdd =
p4_eval(x, a4_dd, a3_dd, a2_dd, a1_dd, a0_dd);
100 p_x =
p4_eval(x, a4, a3, a2, a1, a0);
101 x = x - (2 * p_x * p_xd) / (2 * p_xd * p_xd - p_x * p_xdd);
108float step_adaptation(
float ds,
float f1d,
float f2d,
float f3d,
float f1dd,
float f2dd,
float f3dd)
110 float a4 = (f1dd * f1dd + f2dd * f2dd + f3dd * f3dd) / 4.;
111 float a3 = (f1d * f1dd + f2d * f2dd + f3d * f3dd);
112 float a2 = (f1d * f1d + f2d * f2d + f3d * f3d);
114 float a0 = - ds * ds;
125 return ds / sqrtf(a2);
130 float output = sqrtf(ABS(ds)) / sqrtf(sqrtf(a4));
149 float discr = a2_d * a2_d - 4 * a3_d * a1_d;
153 init = ds / sqrtf(a2);
160 init = (-a2_d - sqrtf(discr)) / (4 * a3_d);
163 init = ds / sqrtf(a2);
168 init = ds / sqrtf(a2);
171 init = (-a2_d + sqrtf(discr)) / (4 * a3_d);
179 if (result * ds < 0) {
float step_adaptation(float ds, float f1d, float f2d, float f3d, float f1dd, float f2dd, float f3dd)
Compute the adapted parametric step given the wanted geometric distance.
#define ADAPTED_STEP_NULL_TOLERANCE
static float p4_halley(float a4, float a3, float a2, float a1, float a0, float tol, float init, int max_steps)
Implementation of Halley's method for degree 4 polynomial root finding.
#define ADAPTED_STEP_MAX_ROOTFINDING_STEPS
static float p4_eval(float x, float a4, float a3, float a2, float a1, float a0)
Horner's method for fast degree 4 polynomial evaluation.
Dynamic parametric step adaptation for the GVF algorithm.