36 #include "irplib_polynomial.h"
55 #define IRPLIB_SWAP(a,b) { const double t=(a);(a)=(b);(b)=t; }
58 #define irplib_trace() cpl_msg_info(cpl_func, "%d: Trace", __LINE__)
60 #define irplib_trace()
71 cpl_boolean,
double,
double,
double);
74 static cpl_boolean irplib_polynomial_solve_1d_2(
double,
double,
double,
76 static cpl_boolean irplib_polynomial_solve_1d_3(
double,
double,
double,
double,
77 double *,
double *,
double *,
81 static void irplib_polynomial_solve_1d_31(
double,
double,
double *,
double *,
82 double *, cpl_boolean *);
84 static void irplib_polynomial_solve_1d_32(
double,
double,
double,
double *,
85 double *,
double *, cpl_boolean *);
87 static void irplib_polynomial_solve_1d_3r(
double,
double,
double,
double,
88 double *,
double *,
double *);
90 static void irplib_polynomial_solve_1d_3c(
double,
double,
double,
91 double,
double,
double,
92 double *,
double *,
double *,
93 cpl_boolean *, cpl_boolean *);
95 static cpl_error_code irplib_polynomial_solve_1d_4(
double,
double,
double,
96 double,
double, cpl_size *,
100 static cpl_error_code irplib_polynomial_solve_1d_nonzero(cpl_polynomial *,
104 static cpl_error_code irplib_polynomial_divide_1d_root(cpl_polynomial *,
double,
107 #ifdef IPRLIB_POLYNOMIAL_USE_MONOMIAL_ROOT
108 static double irplib_polynomial_depress_1d(cpl_polynomial *);
147 cpl_error_code error = CPL_ERROR_NONE;
150 cpl_ensure_code(
self != NULL, CPL_ERROR_NULL_INPUT);
151 cpl_ensure_code(roots != NULL, CPL_ERROR_NULL_INPUT);
152 cpl_ensure_code(preal != NULL, CPL_ERROR_NULL_INPUT);
153 cpl_ensure_code(cpl_polynomial_get_dimension(
self) == 1,
154 CPL_ERROR_INVALID_TYPE);
155 cpl_ensure_code(cpl_polynomial_get_degree(
self) > 0,
156 CPL_ERROR_DATA_NOT_FOUND);
157 cpl_ensure_code(cpl_polynomial_get_degree(
self) ==
158 cpl_vector_get_size(roots), CPL_ERROR_INCOMPATIBLE_INPUT);
162 p = cpl_polynomial_duplicate(
self);
164 error = irplib_polynomial_solve_1d_nonzero(p, roots, preal);
166 cpl_polynomial_delete(p);
202 static cpl_error_code irplib_polynomial_solve_1d_nonzero(cpl_polynomial *
self,
206 cpl_error_code error = CPL_ERROR_NONE;
207 const cpl_size ncoeffs = 1 + cpl_polynomial_get_degree(
self);
209 cpl_ensure_code(
self != NULL, CPL_ERROR_NULL_INPUT);
210 cpl_ensure_code(roots != NULL, CPL_ERROR_NULL_INPUT);
211 cpl_ensure_code(preal != NULL, CPL_ERROR_NULL_INPUT);
212 cpl_ensure_code(cpl_polynomial_get_dimension(
self) == 1,
213 CPL_ERROR_INVALID_TYPE);
214 cpl_ensure_code(ncoeffs > 1, CPL_ERROR_DATA_NOT_FOUND);
215 cpl_ensure_code(*preal >= 0, CPL_ERROR_ILLEGAL_INPUT);
216 cpl_ensure_code(ncoeffs + *preal == 1+cpl_vector_get_size(roots),
217 CPL_ERROR_INCOMPATIBLE_INPUT);
222 const cpl_size i1 = 1;
223 const cpl_size i0 = 0;
224 const double p1 = cpl_polynomial_get_coeff(
self, &i1);
225 const double p0 = cpl_polynomial_get_coeff(
self, &i0);
227 cpl_vector_set(roots, (*preal)++, -p0/p1);
231 const cpl_size i2 = 2;
232 const cpl_size i1 = 1;
233 const cpl_size i0 = 0;
234 const double p2 = cpl_polynomial_get_coeff(
self, &i2);
235 const double p1 = cpl_polynomial_get_coeff(
self, &i1);
236 const double p0 = cpl_polynomial_get_coeff(
self, &i0);
239 if (irplib_polynomial_solve_1d_2(p2, p1, p0, &x1, &x2)) {
241 cpl_vector_set(roots, (*preal) , x1);
242 cpl_vector_set(roots, (*preal)+1, x2);
244 cpl_vector_set(roots, (*preal)++, x1);
245 cpl_vector_set(roots, (*preal)++, x2);
250 const cpl_size i3 = 3;
251 const cpl_size i2 = 2;
252 const cpl_size i1 = 1;
253 const cpl_size i0 = 0;
254 const double p3 = cpl_polynomial_get_coeff(
self, &i3);
255 const double p2 = cpl_polynomial_get_coeff(
self, &i2);
256 const double p1 = cpl_polynomial_get_coeff(
self, &i1);
257 const double p0 = cpl_polynomial_get_coeff(
self, &i0);
260 if (irplib_polynomial_solve_1d_3(p3, p2, p1, p0, &x1, &x2, &x3,
262 cpl_vector_set(roots, (*preal)++, x1);
264 cpl_vector_set(roots, (*preal) , x2);
265 cpl_vector_set(roots, (*preal)+1, x3);
267 cpl_vector_set(roots, (*preal)++, x1);
268 cpl_vector_set(roots, (*preal)++, x2);
269 cpl_vector_set(roots, (*preal)++, x3);
274 const cpl_size i4 = 4;
275 const cpl_size i3 = 3;
276 const cpl_size i2 = 2;
277 const cpl_size i1 = 1;
278 const cpl_size i0 = 0;
279 const double p4 = cpl_polynomial_get_coeff(
self, &i4);
280 const double p3 = cpl_polynomial_get_coeff(
self, &i3);
281 const double p2 = cpl_polynomial_get_coeff(
self, &i2);
282 const double p1 = cpl_polynomial_get_coeff(
self, &i1);
283 const double p0 = cpl_polynomial_get_coeff(
self, &i0);
284 double x1, x2, x3, x4;
287 error = irplib_polynomial_solve_1d_4(p4, p3, p2, p1, p0, &nreal,
290 cpl_vector_set(roots, (*preal) , x1);
291 cpl_vector_set(roots, (*preal)+1, x2);
292 cpl_vector_set(roots, (*preal)+2, x3);
293 cpl_vector_set(roots, (*preal)+3, x4);
303 #ifndef IPRLIB_POLYNOMIAL_USE_MONOMIAL_ROOT
304 const cpl_size n0 = ncoeffs-1;
305 const double pn0 = cpl_polynomial_get_coeff(
self, &n0);
306 const cpl_size n1 = ncoeffs-2;
307 const double pn1 = cpl_polynomial_get_coeff(
self, &n1);
310 const double rmean = -pn1 / (pn0 * n0);
314 cpl_polynomial * copy = cpl_polynomial_duplicate(
self);
315 const cpl_size i0 = 0;
316 const double rmean = irplib_polynomial_depress_1d(copy);
317 const double c0 = cpl_polynomial_get_coeff(copy, &i0);
318 double root = rmean + ((n0&1) && c0 < 0.0 ? -1.0 : 1.0)
319 * pow(fabs(c0), 1.0/n0);
321 cpl_polynomial_delete(copy);
324 error = cpl_polynomial_solve_1d(
self, root, &root, 1);
328 cpl_vector_set(roots, (*preal)++, root);
330 irplib_polynomial_divide_1d_root(
self, root, NULL);
332 error = irplib_polynomial_solve_1d_nonzero(
self, roots, preal);
334 if (!error && *preal > 1) {
339 cpl_vector * reals = cpl_vector_wrap(*preal,
340 cpl_vector_get_data(roots));
341 cpl_vector_sort(reals, 1);
342 (void)cpl_vector_unwrap(reals);
365 static cpl_boolean irplib_polynomial_solve_1d_2(
double p2,
double p1,
double p0,
369 const double sqrtD = sqrt(fabs(p1 * p1 - 4.0 * p2 * p0));
370 cpl_boolean is_complex = CPL_FALSE;
371 double x1 = -0.5 * p1 / p2;
378 assert(px1 != NULL );
379 assert(px2 != NULL );
387 x1 = -0.5 * (p1 + sqrtD);
390 x1 = -0.5 * (p1 - sqrtD);
416 x2 = 0.5 * sqrtD / fabs(p2);
423 is_complex = CPL_TRUE;
448 double x1,
double x2)
453 res = fabs(p0 + x1 * (p1 + x1 * p2) - p2 * x2 * x2);
456 const double r1 = fabs(p0 + x1 * (p1 + x1 * p2));
457 const double r2 = fabs(p0 + x2 * (p1 + x2 * p2));
459 res = r1 > r2 ? r1 : r2;
484 double p1,
double p0,
486 double x1,
double x2,
double x3)
488 const double r1 = fabs(p0 + x1 * (p1 + x1 * (p2 + x1 * p3)));
492 const double r2 = fabs(p0 + x2 * (p1 + x2 * (p2 + x2 * p3))
493 - x3 * x3 * ( 3.0 * p3 * x2 + p2));
495 res = r1 > r2 ? r1 : r2;
498 const double r2 = fabs(p0 + x2 * (p1 + x2 * (p2 + x2 * p3)));
499 const double r3 = fabs(p0 + x3 * (p1 + x3 * (p2 + x3 * p3)));
500 res = r1 > r2 ? (r1 > r3 ? r1 : r3) : (r2 > r3 ? r2 : r3);
530 static cpl_boolean irplib_polynomial_solve_1d_3(
double p3,
double p2,
double p1,
536 cpl_boolean * pdbl2) {
537 cpl_boolean is_complex = CPL_FALSE;
538 const double a = p2/p3;
539 const double b = p1/p3;
540 const double c = p0/p3;
542 const double q = (a * a - 3.0 * b);
543 const double r = (a * (2.0 * a * a - 9.0 * b) + 27.0 * c);
545 const double Q = q / 9.0;
546 const double R = r / 54.0;
548 const double Q3 = Q * Q * Q;
549 const double R2 = R * R;
554 double xx1 = DBL_MAX;
555 double xx2 = DBL_MAX;
556 double xx3 = DBL_MAX;
558 double resx = DBL_MAX;
559 double res = DBL_MAX;
560 cpl_boolean is_first = CPL_TRUE;
565 assert(px1 != NULL );
567 if (pdbl1 != NULL) *pdbl1 = CPL_FALSE;
568 if (pdbl2 != NULL) *pdbl2 = CPL_FALSE;
580 if ((R2 >= Q3 && R != 0.0) || R2 > Q3) {
582 cpl_boolean is_c = CPL_FALSE;
584 irplib_polynomial_solve_1d_3c(a, c, Q, Q3, R, R2, &x1, &x2, &x3,
591 is_first = CPL_FALSE;
593 if (pdbl1 != NULL) *pdbl1 = CPL_FALSE;
594 if (!is_c && pdbl2 != NULL) *pdbl2 = dbl2;
600 if (Q > 0.0 && fabs(R / (Q * sqrt(Q))) <= 1.0) {
607 irplib_polynomial_solve_1d_3r(a, c, Q, R, &xx1, &xx2, &xx3);
612 if (is_first || (dbl2 ? resx < res : resx <= res)) {
613 is_first = CPL_FALSE;
618 if (pdbl1 != NULL) *pdbl1 = CPL_FALSE;
619 if (pdbl2 != NULL) *pdbl2 = CPL_FALSE;
620 is_complex = CPL_FALSE;
626 cpl_boolean dbl1 = CPL_FALSE;
629 irplib_polynomial_solve_1d_32(a, c, Q, &xx1, &xx2, &xx3, &dbl2);
638 if (is_first || resx <= res) {
639 is_first = CPL_FALSE;
644 if (pdbl1 != NULL) *pdbl1 = CPL_FALSE;
645 if (pdbl2 != NULL) *pdbl2 = dbl2;
646 is_complex = CPL_FALSE;
654 irplib_polynomial_solve_1d_31(a, Q, &xx1, &xx2, &xx3, &dbl1);
660 is_first = CPL_FALSE;
665 if (pdbl1 != NULL) *pdbl1 = dbl1;
666 if (pdbl2 != NULL) *pdbl2 = CPL_FALSE;
667 is_complex = CPL_FALSE;
673 if (px2 != NULL && px3 != NULL) {
678 }
else if (is_complex) {
704 static void irplib_polynomial_solve_1d_31(
double a,
double Q,
705 double * px1,
double * px2,
706 double * px3, cpl_boolean * pdbl1)
709 const double sqrtQ = sqrt (Q);
713 x2 = x1 = -sqrtQ - a / 3.0;
714 x3 = 2.0 * sqrtQ - a / 3.0;
715 if (pdbl1 != NULL) *pdbl1 = CPL_TRUE;
741 static void irplib_polynomial_solve_1d_32(
double a,
double c,
double Q,
742 double * px1,
double * px2,
743 double * px3, cpl_boolean * pdbl2)
746 const double sqrtQ = sqrt (Q);
754 x1 = -2.0 * sqrtQ - a / 3.0;
757 x3 = x2 = -a < x1 ? -sqrt(fabs(c / x1)) : sqrt(fabs(c / x1));
758 if (pdbl2 != NULL) *pdbl2 = CPL_TRUE;
760 }
else if (a < 0.0) {
762 x3 = x2 = sqrtQ - a / 3.0;
764 if (pdbl2 != NULL) *pdbl2 = CPL_TRUE;
769 if (pdbl2 != NULL) *pdbl2 = CPL_TRUE;
801 static void irplib_polynomial_solve_1d_3c(
double a,
double c,
805 double * px2,
double * px3,
818 const double sgnR = (R >= 0 ? 1.0 : -1.0);
819 const double A = -sgnR * pow (fabs (R) + sqrt (R2 - Q3), 1.0 / 3.0);
820 const double B = Q / A;
825 cpl_boolean is_complex = CPL_FALSE;
827 if (( A > -B && a > 0.0) || (A < -B && a < 0.0)) {
831 x2 = -0.5 * (A + B) - a / 3.0;
833 x3 = 0.5 * CPL_MATH_SQRT3 * fabs(A - B);
835 x1 = -c / (x2 * x2 + x3 * x3);
839 x1 = A + B - a / 3.0;
841 x3 = 0.5 * CPL_MATH_SQRT3 * fabs(A - B);
845 x2 = -0.5 * (A + B) - a / 3.0;
849 x2 = -a < x1 ? -sqrt(fabs(c / x1)) : sqrt(fabs(c / x1));
856 is_complex = CPL_TRUE;
862 if (pdbl2 != NULL) *pdbl2 = CPL_TRUE;
890 static void irplib_polynomial_solve_1d_3r(
double a,
double c,
893 double * px2,
double * px3)
896 const double sqrtQ = sqrt(Q);
897 const double theta = acos (R / (Q * sqrtQ));
903 #define TR1 (-2.0 * sqrtQ * cos( theta / 3.0))
904 #define TR2 (-2.0 * sqrtQ * cos((theta - CPL_MATH_2PI) / 3.0))
905 #define TR3 (-2.0 * sqrtQ * cos((theta + CPL_MATH_2PI) / 3.0))
920 if (TR2 > 0.0 && (TR2 + TR3) > 2.0 * a) {
923 x2 = -c / ( x1 * x3 );
931 x3 = -c / ( x1 * x2 );
934 }
else if (a < 0.0) {
936 if (TR2 < 0.0 && (TR1 + TR2) > 2.0 * a) {
938 x2 = -c / ( x1 * x3 );
942 x1 = -c / ( x2 * x3 );
962 x1 = x2 = 0.5 * ( x1 + x2 );
964 }
else if (x2 > x3) {
972 x3 = x2 = 0.5 * ( x2 + x3 );
1002 static cpl_error_code irplib_polynomial_solve_1d_4(
double p4,
double p3,
1003 double p2,
double p1,
1004 double p0, cpl_size * preal,
1005 double * px1,
double * px2,
1006 double * px3,
double * px4)
1010 const double a = (p2 - 0.375 * p3 * p3 / p4) / p4;
1011 const double b = (p1 - 0.5 * (p2 - 0.25 * p3 * p3 / p4 ) * p3 / p4 ) / p4;
1013 (p0 - 0.25 * (p1 - 0.25 * (p2 - 0.1875 * p3 * p3 / p4 ) * p3 / p4
1016 double x1 = DBL_MAX;
1017 double x2 = DBL_MAX;
1018 double x3 = DBL_MAX;
1019 double x4 = DBL_MAX;
1021 assert(preal != NULL );
1022 assert(px1 != NULL );
1023 assert(px2 != NULL );
1024 assert(px3 != NULL );
1025 assert(px4 != NULL );
1033 cpl_boolean dbl1, dbl2;
1034 const cpl_boolean is_real =
1035 !irplib_polynomial_solve_1d_3(1.0, 0.0, a, b, &x1, &x3, &x4,
1038 x1 -= 0.25 * p3 / p4;
1039 x2 = -0.25 * p3 / p4;
1040 x3 -= 0.25 * p3 / p4;
1048 x4 -= 0.25 * p3 / p4;
1051 IRPLIB_SWAP(x2, x3);
1068 IRPLIB_SWAP(x1, x2);
1074 }
else if (b == 0.0) {
1077 const cpl_boolean is_complex = irplib_polynomial_solve_1d_2(1.0, a, c,
1082 const double norm = sqrt(u1*u1 + u2*u2);
1083 const double v1 = sqrt(0.5*(norm+u1));
1084 const double v2 = u2 / sqrt(2.0*(norm+u1));
1087 x1 = -0.25 * p3 / p4 - v1;
1088 x3 = -0.25 * p3 / p4 + v1;
1094 }
else if (u1 >= 0.0) {
1096 const double sv1 = sqrt(u1);
1097 const double sv2 = sqrt(u2);
1102 x1 = -0.25 * p3 / p4 - sv2;
1103 x2 = -0.25 * p3 / p4 - sv1;
1104 x3 = -0.25 * p3 / p4 + sv1;
1105 x4 = -0.25 * p3 / p4 + sv2;
1106 }
else if (u2 < 0.0) {
1108 const double sv1 = sqrt(-u2);
1109 const double sv2 = sqrt(-u1);
1114 x1 = x3 = -0.25 * p3 / p4;
1120 const double sv1 = sqrt(-u1);
1121 const double sv2 = sqrt(u2);
1126 x1 = -0.25 * p3 / p4 - sv2;
1127 x2 = -0.25 * p3 / p4 + sv2;
1129 x3 = -0.25 * p3 / p4;
1134 const double q2 = -a;
1135 const double q1 = -4.0 * c;
1136 const double q0 = 4.0 * a * c - b * b;
1137 double u1, sqrtd, sqrtrd;
1138 double z1, z2, z3, z4;
1140 cpl_boolean is_complex1, is_complex2;
1144 (void)irplib_polynomial_solve_1d_3(1.0, q2, q1, q0, &u1, NULL, NULL,
1150 sqrtd = sqrt(u1 - a);
1152 sqrtrd = 0.5 * b/sqrtd;
1154 is_complex1 = irplib_polynomial_solve_1d_2(1.0, sqrtd, 0.5*u1 - sqrtrd,
1157 is_complex2 = irplib_polynomial_solve_1d_2(1.0, -sqrtd, 0.5*u1 + sqrtrd,
1160 z1 -= 0.25 * p3 / p4;
1161 z3 -= 0.25 * p3 / p4;
1162 if (!is_complex1) z2 -= 0.25 * p3 / p4;
1163 if (!is_complex2) z4 -= 0.25 * p3 / p4;
1165 if (!is_complex1 && is_complex2) {
1171 }
else if (is_complex1 && !is_complex2) {
1177 }
else if (is_complex1 && is_complex2) {
1180 if (z1 < z3 || (z1 == z3 && z2 <= z4)) {
1199 }
else if (z4 <= z1) {
1204 }
else if (z2 > z4) {
1223 return CPL_ERROR_NONE;
1226 #ifdef IPRLIB_POLYNOMIAL_USE_MONOMIAL_ROOT
1236 static double irplib_polynomial_depress_1d(cpl_polynomial *
self)
1239 const cpl_size degree = cpl_polynomial_get_degree(
self);
1240 const cpl_size nc1 = degree - 1;
1241 const double an = cpl_polynomial_get_coeff(
self, °ree);
1242 const double an1 = cpl_polynomial_get_coeff(
self, &nc1);
1247 cpl_ensure(degree > 0, CPL_ERROR_DATA_NOT_FOUND, 0.0);
1249 assert( an != 0.0 );
1251 rmean = -an1/(an * (double)degree);
1255 cpl_polynomial_shift_1d(
self, 0, rmean);
1257 cpl_polynomial_set_coeff(
self, &nc1, 0.0);
1262 for (i = 0; i < degree-1; i++) {
1263 const double ai = cpl_polynomial_get_coeff(
self, &i) / an;
1264 cpl_polynomial_set_coeff(
self, &i, ai);
1267 cpl_polynomial_set_coeff(
self, °ree, 1.0);
1290 cpl_error_code irplib_polynomial_divide_1d_root(cpl_polynomial * p,
double r,
1294 const cpl_size n = cpl_polynomial_get_degree(p);
1299 cpl_ensure_code(p != NULL, CPL_ERROR_NULL_INPUT);
1300 cpl_ensure_code(cpl_polynomial_get_dimension(p) == 1,
1301 CPL_ERROR_INVALID_TYPE);
1302 cpl_ensure_code(n > 0, CPL_ERROR_DATA_NOT_FOUND);
1304 sum = cpl_polynomial_get_coeff(p, &n);
1305 cpl_polynomial_set_coeff(p, &n, 0.0);
1307 for (i = n-1; i >= 0; i--) {
1308 const double coeff = cpl_polynomial_get_coeff(p, &i);
1310 cpl_polynomial_set_coeff(p, &i, sum);
1312 sum = coeff + r * sum;
1316 if (pres != NULL) *pres = sum;
1318 return CPL_ERROR_NONE;
cpl_error_code irplib_polynomial_solve_1d_all(const cpl_polynomial *self, cpl_vector *roots, cpl_size *preal)
Compute all n roots of p(x) = 0, where p(x) is of degree n, n > 0.
static double irplib_polynomial_eval_2_max(double, double, double, cpl_boolean, double, double)
Find the max residual on a 2nd degree 1D-polynomial on the roots.
static double irplib_polynomial_eval_3_max(double, double, double, double, cpl_boolean, double, double, double)
Find the max residual on a 3rd degree 1D-polynomial on the roots.