00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028 #ifdef HAVE_CONFIG_H
00029 #include <config.h>
00030 #endif
00031
00032
00033
00034
00035
00036 #include "irplib_polynomial.h"
00037 #include <assert.h>
00038 #include <math.h>
00039
00040 #include <float.h>
00041
00042
00048
00051
00052
00053
00054
00055 #define IRPLIB_SWAP(a,b) { const double t=(a);(a)=(b);(b)=t; }
00056
00057 #if 0
00058 #define irplib_trace() cpl_msg_info(cpl_func, "%d: Trace", __LINE__)
00059 #else
00060 #define irplib_trace()
00061 #endif
00062
00063
00064
00065
00066
00067 static double irplib_polynomial_eval_2_max(double, double, double, cpl_boolean,
00068 double, double);
00069
00070 static double irplib_polynomial_eval_3_max(double, double, double, double,
00071 cpl_boolean, double, double, double);
00072
00073
00074 static cpl_boolean irplib_polynomial_solve_1d_2(double, double, double,
00075 double *, double *);
00076 static cpl_boolean irplib_polynomial_solve_1d_3(double, double, double, double,
00077 double *, double *, double *,
00078 cpl_boolean *,
00079 cpl_boolean *);
00080
00081 static void irplib_polynomial_solve_1d_31(double, double, double *, double *,
00082 double *, cpl_boolean *);
00083
00084 static void irplib_polynomial_solve_1d_32(double, double, double, double *,
00085 double *, double *, cpl_boolean *);
00086
00087 static void irplib_polynomial_solve_1d_3r(double, double, double, double,
00088 double *, double *, double *);
00089
00090 static void irplib_polynomial_solve_1d_3c(double, double, double,
00091 double, double, double,
00092 double *, double *, double *,
00093 cpl_boolean *, cpl_boolean *);
00094
00095 static cpl_error_code irplib_polynomial_solve_1d_4(double, double, double,
00096 double, double, int *,
00097 double *, double *,
00098 double *, double *);
00099
00100 static cpl_error_code irplib_polynomial_solve_1d_nonzero(cpl_polynomial *,
00101 cpl_vector *,
00102 int *);
00103
00104 static cpl_error_code irplib_polynomial_divide_1d_root(cpl_polynomial *, double,
00105 double *);
00106
00107 #ifdef IPRLIB_POLYNOMIAL_USE_MONOMIAL_ROOT
00108 static double irplib_polynomial_depress_1d(cpl_polynomial *);
00109 #endif
00110
00111
00112
00113
00114
00115
00116
00131
00132 cpl_error_code irplib_polynomial_add(cpl_polynomial * self,
00133 const cpl_polynomial * first,
00134 const cpl_polynomial * second)
00135 {
00136 int degree0 = cpl_polynomial_get_degree(self);
00137 const int degree1 = cpl_polynomial_get_degree(first);
00138 const int degree2 = cpl_polynomial_get_degree(second);
00139 const int maxdeg = degree1 > degree2 ? degree1 : degree2;
00140
00141
00142 cpl_ensure_code(self != NULL, CPL_ERROR_NULL_INPUT);
00143 cpl_ensure_code(first != NULL, CPL_ERROR_NULL_INPUT);
00144 cpl_ensure_code(second != NULL, CPL_ERROR_NULL_INPUT);
00145
00146 cpl_ensure_code(cpl_polynomial_get_dimension(self) ==
00147 cpl_polynomial_get_dimension(first),
00148 CPL_ERROR_INCOMPATIBLE_INPUT);
00149 cpl_ensure_code(cpl_polynomial_get_dimension(self) ==
00150 cpl_polynomial_get_dimension(second),
00151 CPL_ERROR_INCOMPATIBLE_INPUT);
00152
00153
00154 cpl_ensure_code(cpl_polynomial_get_dimension(self) == 1,
00155 CPL_ERROR_UNSUPPORTED_MODE);
00156
00157 if (degree0 < maxdeg) {
00158 degree0 = maxdeg;
00159 } else {
00160
00161 for (; degree0 > maxdeg; degree0--) {
00162 cpl_polynomial_set_coeff(self, °ree0, 0.0);
00163 }
00164 }
00165
00166
00167
00168 for (; degree0 >= 0; degree0--) {
00169 const double val1 = cpl_polynomial_get_coeff(first, °ree0);
00170 const double val2 = cpl_polynomial_get_coeff(second, °ree0);
00171 cpl_polynomial_set_coeff(self, °ree0, val1 + val2);
00172 }
00173
00174 return CPL_ERROR_NONE;
00175 }
00176
00177
00192
00193 cpl_error_code irplib_polynomial_subtract(cpl_polynomial * self,
00194 const cpl_polynomial * first,
00195 const cpl_polynomial * second)
00196 {
00197 int degree0 = cpl_polynomial_get_degree(self);
00198 const int degree1 = cpl_polynomial_get_degree(first);
00199 const int degree2 = cpl_polynomial_get_degree(second);
00200 const int maxdeg = degree1 > degree2 ? degree1 : degree2;
00201
00202
00203 cpl_ensure_code(self != NULL, CPL_ERROR_NULL_INPUT);
00204 cpl_ensure_code(first != NULL, CPL_ERROR_NULL_INPUT);
00205 cpl_ensure_code(second != NULL, CPL_ERROR_NULL_INPUT);
00206
00207 cpl_ensure_code(cpl_polynomial_get_dimension(self) ==
00208 cpl_polynomial_get_dimension(first),
00209 CPL_ERROR_INCOMPATIBLE_INPUT);
00210 cpl_ensure_code(cpl_polynomial_get_dimension(self) ==
00211 cpl_polynomial_get_dimension(second),
00212 CPL_ERROR_INCOMPATIBLE_INPUT);
00213
00214
00215 cpl_ensure_code(cpl_polynomial_get_dimension(self) == 1,
00216 CPL_ERROR_UNSUPPORTED_MODE);
00217
00218 if (degree0 < maxdeg) {
00219 degree0 = maxdeg;
00220 } else {
00221
00222 for (; degree0 > maxdeg; degree0--) {
00223 cpl_polynomial_set_coeff(self, °ree0, 0.0);
00224 }
00225 }
00226
00227
00228
00229 for (; degree0 >= 0; degree0--) {
00230 const double val1 = cpl_polynomial_get_coeff(first, °ree0);
00231 const double val2 = cpl_polynomial_get_coeff(second, °ree0);
00232 cpl_polynomial_set_coeff(self, °ree0, val1 - val2);
00233 }
00234
00235 return CPL_ERROR_NONE;
00236 }
00237
00238
00249
00250 cpl_error_code irplib_polynomial_multiply_scalar(cpl_polynomial * self,
00251 double factor)
00252 {
00253
00254 int degree0;
00255
00256 cpl_ensure_code(self != NULL, CPL_ERROR_NULL_INPUT);
00257
00258 cpl_ensure_code(cpl_polynomial_get_dimension(self) == 1,
00259 CPL_ERROR_UNSUPPORTED_MODE);
00260
00261 degree0 = cpl_polynomial_get_degree(self);
00262
00263 for (; degree0 >=0; degree0--) {
00264 const double val = cpl_polynomial_get_coeff(self, °ree0);
00265 cpl_polynomial_set_coeff(self, °ree0, factor * val);
00266 }
00267
00268 return CPL_ERROR_NONE;
00269 }
00270
00271
00297
00298 cpl_error_code irplib_polynomial_solve_1d_all(const cpl_polynomial * self,
00299 cpl_vector * roots, int * preal)
00300 {
00301
00302 cpl_error_code error = CPL_ERROR_NONE;
00303 cpl_polynomial * p;
00304
00305 cpl_ensure_code(self != NULL, CPL_ERROR_NULL_INPUT);
00306 cpl_ensure_code(roots != NULL, CPL_ERROR_NULL_INPUT);
00307 cpl_ensure_code(preal != NULL, CPL_ERROR_NULL_INPUT);
00308 cpl_ensure_code(cpl_polynomial_get_dimension(self) == 1,
00309 CPL_ERROR_INVALID_TYPE);
00310 cpl_ensure_code(cpl_polynomial_get_degree(self) > 0,
00311 CPL_ERROR_DATA_NOT_FOUND);
00312 cpl_ensure_code(cpl_polynomial_get_degree(self) ==
00313 cpl_vector_get_size(roots), CPL_ERROR_INCOMPATIBLE_INPUT);
00314
00315 *preal = 0;
00316
00317 p = cpl_polynomial_duplicate(self);
00318
00319 error = irplib_polynomial_solve_1d_nonzero(p, roots, preal);
00320
00321 cpl_polynomial_delete(p);
00322
00323 return error;
00324
00325 }
00326
00329
00355
00356 static cpl_error_code irplib_polynomial_solve_1d_nonzero(cpl_polynomial * self,
00357 cpl_vector * roots,
00358 int * preal)
00359 {
00360 cpl_error_code error = CPL_ERROR_NONE;
00361 const int ncoeffs = 1 + cpl_polynomial_get_degree(self);
00362
00363 cpl_ensure_code(self != NULL, CPL_ERROR_NULL_INPUT);
00364 cpl_ensure_code(roots != NULL, CPL_ERROR_NULL_INPUT);
00365 cpl_ensure_code(preal != NULL, CPL_ERROR_NULL_INPUT);
00366 cpl_ensure_code(cpl_polynomial_get_dimension(self) == 1,
00367 CPL_ERROR_INVALID_TYPE);
00368 cpl_ensure_code(ncoeffs > 1, CPL_ERROR_DATA_NOT_FOUND);
00369 cpl_ensure_code(*preal >= 0, CPL_ERROR_ILLEGAL_INPUT);
00370 cpl_ensure_code(ncoeffs + *preal == 1+cpl_vector_get_size(roots),
00371 CPL_ERROR_INCOMPATIBLE_INPUT);
00372
00373 switch (ncoeffs) {
00374
00375 case 2 : {
00376 const int i1 = 1;
00377 const double p1 = cpl_polynomial_get_coeff(self, &i1);
00378 const int i0 = 0;
00379 const double p0 = cpl_polynomial_get_coeff(self, &i0);
00380
00381 cpl_vector_set(roots, (*preal)++, -p0/p1);
00382 break;
00383 }
00384 case 3 : {
00385 const int i2 = 2;
00386 const int i1 = 1;
00387 const int i0 = 0;
00388 const double p2 = cpl_polynomial_get_coeff(self, &i2);
00389 const double p1 = cpl_polynomial_get_coeff(self, &i1);
00390 const double p0 = cpl_polynomial_get_coeff(self, &i0);
00391 double x1, x2;
00392
00393 if (irplib_polynomial_solve_1d_2(p2, p1, p0, &x1, &x2)) {
00394
00395 cpl_vector_set(roots, (*preal) , x1);
00396 cpl_vector_set(roots, (*preal)+1, x2);
00397 } else {
00398 cpl_vector_set(roots, (*preal)++, x1);
00399 cpl_vector_set(roots, (*preal)++, x2);
00400 }
00401 break;
00402 }
00403 case 4 : {
00404 const int i3 = 3;
00405 const int i2 = 2;
00406 const int i1 = 1;
00407 const int i0 = 0;
00408 const double p3 = cpl_polynomial_get_coeff(self, &i3);
00409 const double p2 = cpl_polynomial_get_coeff(self, &i2);
00410 const double p1 = cpl_polynomial_get_coeff(self, &i1);
00411 const double p0 = cpl_polynomial_get_coeff(self, &i0);
00412 double x1, x2, x3;
00413
00414 if (irplib_polynomial_solve_1d_3(p3, p2, p1, p0, &x1, &x2, &x3,
00415 NULL, NULL)) {
00416 cpl_vector_set(roots, (*preal)++, x1);
00417
00418 cpl_vector_set(roots, (*preal) , x2);
00419 cpl_vector_set(roots, (*preal)+1, x3);
00420 } else {
00421 cpl_vector_set(roots, (*preal)++, x1);
00422 cpl_vector_set(roots, (*preal)++, x2);
00423 cpl_vector_set(roots, (*preal)++, x3);
00424 }
00425 break;
00426 }
00427 case 5 : {
00428 const int i4 = 4;
00429 const int i3 = 3;
00430 const int i2 = 2;
00431 const int i1 = 1;
00432 const int i0 = 0;
00433 const double p4 = cpl_polynomial_get_coeff(self, &i4);
00434 const double p3 = cpl_polynomial_get_coeff(self, &i3);
00435 const double p2 = cpl_polynomial_get_coeff(self, &i2);
00436 const double p1 = cpl_polynomial_get_coeff(self, &i1);
00437 const double p0 = cpl_polynomial_get_coeff(self, &i0);
00438 double x1, x2, x3, x4;
00439 int nreal;
00440
00441 error = irplib_polynomial_solve_1d_4(p4, p3, p2, p1, p0, &nreal,
00442 &x1, &x2, &x3, &x4);
00443 if (!error) {
00444 cpl_vector_set(roots, (*preal) , x1);
00445 cpl_vector_set(roots, (*preal)+1, x2);
00446 cpl_vector_set(roots, (*preal)+2, x3);
00447 cpl_vector_set(roots, (*preal)+3, x4);
00448
00449 *preal += nreal;
00450 }
00451 break;
00452 }
00453
00454 default: {
00455
00456
00457 #ifndef IPRLIB_POLYNOMIAL_USE_MONOMIAL_ROOT
00458 const int n0 = ncoeffs-1;
00459 const double pn0 = cpl_polynomial_get_coeff(self, &n0);
00460 const int n1 = ncoeffs-2;
00461 const double pn1 = cpl_polynomial_get_coeff(self, &n1);
00462
00463
00464 const double rmean = -pn1 / (pn0 * n0);
00465 double root = rmean;
00466 #else
00467
00468 cpl_polynomial * copy = cpl_polynomial_duplicate(self);
00469 const int i0 = 0;
00470 const double rmean = irplib_polynomial_depress_1d(copy);
00471 const double c0 = cpl_polynomial_get_coeff(copy, &i0);
00472 double root = rmean + ((n0&1) && c0 < 0.0 ? -1.0 : 1.0)
00473 * pow(fabs(c0), 1.0/n0);
00474
00475 cpl_polynomial_delete(copy);
00476 #endif
00477
00478 error = cpl_polynomial_solve_1d(self, root, &root, 1);
00479
00480 if (!error) {
00481
00482 cpl_vector_set(roots, (*preal)++, root);
00483
00484 irplib_polynomial_divide_1d_root(self, root, NULL);
00485
00486 error = irplib_polynomial_solve_1d_nonzero(self, roots, preal);
00487
00488 if (!error && *preal > 1) {
00489
00490
00491
00492
00493 cpl_vector * reals = cpl_vector_wrap(*preal,
00494 cpl_vector_get_data(roots));
00495 cpl_vector_sort(reals, 1);
00496 (void)cpl_vector_unwrap(reals);
00497 }
00498 }
00499 break;
00500 }
00501 }
00502
00503 return error;
00504 }
00505
00506
00518
00519 static cpl_boolean irplib_polynomial_solve_1d_2(double p2, double p1, double p0,
00520 double * px1,
00521 double * px2) {
00522
00523 const double sqrtD = sqrt(fabs(p1 * p1 - 4.0 * p2 * p0));
00524 cpl_boolean is_complex = CPL_FALSE;
00525 double x1 = -0.5 * p1 / p2;
00526 double x2;
00527
00528
00529 double res0 = irplib_polynomial_eval_2_max(p2, p1, p0, CPL_FALSE, x1, x1);
00530 double res;
00531
00532 assert(px1 != NULL );
00533 assert(px2 != NULL );
00534
00535 *px2 = *px1 = x1;
00536
00537
00538
00539
00540 if (p1 > 0.0) {
00541 x1 = -0.5 * (p1 + sqrtD);
00542 irplib_trace();
00543 } else {
00544 x1 = -0.5 * (p1 - sqrtD);
00545 irplib_trace();
00546 }
00547
00548
00549 x2 = p0 / x1;
00550 x1 /= p2;
00551
00552 res = irplib_polynomial_eval_2_max(p2, p1, p0, CPL_FALSE, x1, x2);
00553
00554 if (res < res0) {
00555 res0 = res;
00556 if (x2 > x1) {
00557 *px1 = x1;
00558 *px2 = x2;
00559 irplib_trace();
00560 } else {
00561 *px1 = x2;
00562 *px2 = x1;
00563 irplib_trace();
00564 }
00565 }
00566
00567
00568
00569 x1 = -0.5 * p1 / p2;
00570 x2 = 0.5 * sqrtD / fabs(p2);
00571
00572 res = irplib_polynomial_eval_2_max(p2, p1, p0, CPL_TRUE, x1, x2);
00573
00574 if (res < res0) {
00575 *px1 = x1;
00576 *px2 = x2;
00577 is_complex = CPL_TRUE;
00578 irplib_trace();
00579 }
00580
00581 return is_complex;
00582
00583 }
00584
00585
00586
00599
00600 static double irplib_polynomial_eval_2_max(double p2, double p1, double p0,
00601 cpl_boolean is_c,
00602 double x1, double x2)
00603 {
00604 double res;
00605
00606 if (is_c) {
00607 res = fabs(p0 + x1 * (p1 + x1 * p2) - p2 * x2 * x2);
00608 irplib_trace();
00609 } else {
00610 const double r1 = fabs(p0 + x1 * (p1 + x1 * p2));
00611 const double r2 = fabs(p0 + x2 * (p1 + x2 * p2));
00612
00613 res = r1 > r2 ? r1 : r2;
00614 irplib_trace();
00615 }
00616
00617 return res;
00618 }
00619
00620
00621
00636
00637 static double irplib_polynomial_eval_3_max(double p3, double p2,
00638 double p1, double p0,
00639 cpl_boolean is_c,
00640 double x1, double x2, double x3)
00641 {
00642 const double r1 = fabs(p0 + x1 * (p1 + x1 * (p2 + x1 * p3)));
00643 double res;
00644
00645 if (is_c) {
00646 const double r2 = fabs(p0 + x2 * (p1 + x2 * (p2 + x2 * p3))
00647 - x3 * x3 * ( 3.0 * p3 * x2 + p2));
00648
00649 res = r1 > r2 ? r1 : r2;
00650 irplib_trace();
00651 } else {
00652 const double r2 = fabs(p0 + x2 * (p1 + x2 * (p2 + x2 * p3)));
00653 const double r3 = fabs(p0 + x3 * (p1 + x3 * (p2 + x3 * p3)));
00654 res = r1 > r2 ? (r1 > r3 ? r1 : r3) : (r2 > r3 ? r2 : r3);
00655 irplib_trace();
00656 }
00657
00658
00659
00660 return res;
00661 }
00662
00663
00664
00683
00684 static cpl_boolean irplib_polynomial_solve_1d_3(double p3, double p2, double p1,
00685 double p0,
00686 double * px1,
00687 double * px2,
00688 double * px3,
00689 cpl_boolean * pdbl1,
00690 cpl_boolean * pdbl2) {
00691 cpl_boolean is_complex = CPL_FALSE;
00692 const double a = p2/p3;
00693 const double b = p1/p3;
00694 const double c = p0/p3;
00695
00696 const double q = (a * a - 3.0 * b);
00697 const double r = (a * (2.0 * a * a - 9.0 * b) + 27.0 * c);
00698
00699 const double Q = q / 9.0;
00700 const double R = r / 54.0;
00701
00702 const double Q3 = Q * Q * Q;
00703 const double R2 = R * R;
00704
00705 double x1 = DBL_MAX;
00706 double x2 = DBL_MAX;
00707 double x3 = DBL_MAX;
00708 double xx1 = DBL_MAX;
00709 double xx2 = DBL_MAX;
00710 double xx3 = DBL_MAX;
00711
00712 double resx = DBL_MAX;
00713 double res = DBL_MAX;
00714 cpl_boolean is_first = CPL_TRUE;
00715
00716 cpl_boolean dbl2;
00717
00718
00719 assert(px1 != NULL );
00720
00721 if (pdbl1 != NULL) *pdbl1 = CPL_FALSE;
00722 if (pdbl2 != NULL) *pdbl2 = CPL_FALSE;
00723
00724 dbl2 = CPL_FALSE;
00725
00726
00727
00728
00729
00730
00731
00732
00733
00734 if ((R2 >= Q3 && R != 0.0) || R2 > Q3) {
00735
00736 cpl_boolean is_c = CPL_FALSE;
00737
00738 irplib_polynomial_solve_1d_3c(a, c, Q, Q3, R, R2, &x1, &x2, &x3,
00739 &is_c, &dbl2);
00740
00741
00742 res = resx = irplib_polynomial_eval_3_max(p3, p2, p1, p0, is_c,
00743 x1, x2, x3);
00744
00745 is_first = CPL_FALSE;
00746
00747 if (pdbl1 != NULL) *pdbl1 = CPL_FALSE;
00748 if (!is_c && pdbl2 != NULL) *pdbl2 = dbl2;
00749 is_complex = is_c;
00750 irplib_trace();
00751
00752 }
00753
00754 if (Q > 0.0 && fabs(R / (Q * sqrt(Q))) <= 1.0) {
00755
00756
00757
00758
00759
00760
00761 irplib_polynomial_solve_1d_3r(a, c, Q, R, &xx1, &xx2, &xx3);
00762
00763 resx = irplib_polynomial_eval_3_max(p3, p2, p1, p0, CPL_FALSE,
00764 xx1, xx2, xx3);
00765
00766 if (is_first || (dbl2 ? resx < res : resx <= res)) {
00767 is_first = CPL_FALSE;
00768 res = resx;
00769 x1 = xx1;
00770 x2 = xx2;
00771 x3 = xx3;
00772 if (pdbl1 != NULL) *pdbl1 = CPL_FALSE;
00773 if (pdbl2 != NULL) *pdbl2 = CPL_FALSE;
00774 is_complex = CPL_FALSE;
00775 irplib_trace();
00776 }
00777 }
00778
00779 if (Q >= 0) {
00780 cpl_boolean dbl1 = CPL_FALSE;
00781
00782
00783 irplib_polynomial_solve_1d_32(a, c, Q, &xx1, &xx2, &xx3, &dbl2);
00784
00785 resx = irplib_polynomial_eval_3_max(p3, p2, p1, p0, CPL_FALSE,
00786 xx1, xx2, xx3);
00787
00788
00789
00790
00791
00792 if (is_first || resx <= res) {
00793 is_first = CPL_FALSE;
00794 res = resx;
00795 x1 = xx1;
00796 x2 = xx2;
00797 x3 = xx3;
00798 if (pdbl1 != NULL) *pdbl1 = CPL_FALSE;
00799 if (pdbl2 != NULL) *pdbl2 = dbl2;
00800 is_complex = CPL_FALSE;
00801 irplib_trace();
00802 }
00803
00804
00805
00806
00807
00808 irplib_polynomial_solve_1d_31(a, Q, &xx1, &xx2, &xx3, &dbl1);
00809
00810 resx = irplib_polynomial_eval_3_max(p3, p2, p1, p0, CPL_FALSE,
00811 xx1, xx2, xx3);
00812
00813 if (resx <= res) {
00814 is_first = CPL_FALSE;
00815 res = resx;
00816 x1 = xx1;
00817 x2 = xx2;
00818 x3 = xx3;
00819 if (pdbl1 != NULL) *pdbl1 = dbl1;
00820 if (pdbl2 != NULL) *pdbl2 = CPL_FALSE;
00821 is_complex = CPL_FALSE;
00822 irplib_trace();
00823 }
00824
00825 }
00826
00827 if (px2 != NULL && px3 != NULL) {
00828 *px1 = x1;
00829 *px2 = x2;
00830 *px3 = x3;
00831 irplib_trace();
00832 } else if (is_complex) {
00833 *px1 = x1;
00834 irplib_trace();
00835 } else {
00836 *px1 = x3;
00837 irplib_trace();
00838 }
00839
00840 return is_complex;
00841 }
00842
00843
00857
00858 static void irplib_polynomial_solve_1d_31(double a, double Q,
00859 double * px1, double * px2,
00860 double * px3, cpl_boolean * pdbl1)
00861 {
00862
00863 const double sqrtQ = sqrt (Q);
00864
00865 double x1, x2, x3;
00866
00867 x2 = x1 = -sqrtQ - a / 3.0;
00868 x3 = 2.0 * sqrtQ - a / 3.0;
00869 if (pdbl1 != NULL) *pdbl1 = CPL_TRUE;
00870
00871 *px1 = x1;
00872 *px2 = x2;
00873 *px3 = x3;
00874
00875 irplib_trace();
00876 return;
00877 }
00878
00879
00894
00895 static void irplib_polynomial_solve_1d_32(double a, double c, double Q,
00896 double * px1, double * px2,
00897 double * px3, cpl_boolean * pdbl2)
00898 {
00899
00900 const double sqrtQ = sqrt (Q);
00901
00902 double x1 = DBL_MAX;
00903 double x2 = DBL_MAX;
00904 double x3 = DBL_MAX;
00905
00906 if (a > 0.0) {
00907
00908 x1 = -2.0 * sqrtQ - a / 3.0;
00909
00910
00911 x3 = x2 = -a < x1 ? -sqrt(fabs(c / x1)) : sqrt(fabs(c / x1));
00912 if (pdbl2 != NULL) *pdbl2 = CPL_TRUE;
00913 irplib_trace();
00914 } else if (a < 0.0) {
00915
00916 x3 = x2 = sqrtQ - a / 3.0;
00917 x1 = -c / (x2 * x2);
00918 if (pdbl2 != NULL) *pdbl2 = CPL_TRUE;
00919 irplib_trace();
00920 } else {
00921 x1 = -2.0 * sqrtQ;
00922 x3 = x2 = sqrtQ;
00923 if (pdbl2 != NULL) *pdbl2 = CPL_TRUE;
00924 irplib_trace();
00925 }
00926
00927 *px1 = x1;
00928 *px2 = x2;
00929 *px3 = x3;
00930
00931 return;
00932 }
00933
00934
00954
00955 static void irplib_polynomial_solve_1d_3c(double a, double c,
00956 double Q, double Q3,
00957 double R, double R2,
00958 double * px1,
00959 double * px2, double * px3,
00960 cpl_boolean * pis_c,
00961 cpl_boolean * pdbl2)
00962 {
00963
00964
00965
00966
00967
00968
00969
00970
00971
00972 const double sgnR = (R >= 0 ? 1.0 : -1.0);
00973 const double A = -sgnR * pow (fabs (R) + sqrt (R2 - Q3), 1.0 / 3.0);
00974 const double B = Q / A;
00975
00976 double x1 = DBL_MAX;
00977 double x2 = DBL_MAX;
00978 double x3 = DBL_MAX;
00979 cpl_boolean is_complex = CPL_FALSE;
00980
00981 if (( A > -B && a > 0.0) || (A < -B && a < 0.0)) {
00982
00983
00984
00985 x2 = -0.5 * (A + B) - a / 3.0;
00986
00987 x3 = 0.5 * CPL_MATH_SQRT3 * fabs(A - B);
00988
00989 x1 = -c / (x2 * x2 + x3 * x3);
00990 irplib_trace();
00991 } else {
00992
00993 x1 = A + B - a / 3.0;
00994
00995 x3 = 0.5 * CPL_MATH_SQRT3 * fabs(A - B);
00996
00997 if (x3 > 0.0) {
00998
00999 x2 = -0.5 * (A + B) - a / 3.0;
01000 irplib_trace();
01001 } else {
01002
01003 x2 = -a < x1 ? -sqrt(fabs(c / x1)) : sqrt(fabs(c / x1));
01004 x2 = -0.5 * (A + B) - a / 3.0;
01005 x3 = 0.0;
01006 irplib_trace();
01007 }
01008 }
01009
01010 if (x3 > 0.0) {
01011 is_complex = CPL_TRUE;
01012 irplib_trace();
01013 } else {
01014
01015
01016 x3 = x2;
01017 if (pdbl2 != NULL) *pdbl2 = CPL_TRUE;
01018 irplib_trace();
01019 }
01020
01021 *px1 = x1;
01022 *px2 = x2;
01023 *px3 = x3;
01024 *pis_c = is_complex;
01025
01026 return;
01027 }
01028
01029
01044
01045 static void irplib_polynomial_solve_1d_3r(double a, double c,
01046 double Q, double R,
01047 double * px1,
01048 double * px2, double * px3)
01049 {
01050
01051 const double sqrtQ = sqrt(Q);
01052 const double theta = acos (R / (Q * sqrtQ));
01053
01054
01055
01056
01057
01058 #define TR1 (-2.0 * sqrtQ * cos( theta / 3.0))
01059 #define TR2 (-2.0 * sqrtQ * cos((theta - CPL_MATH_2PI) / 3.0))
01060 #define TR3 (-2.0 * sqrtQ * cos((theta + CPL_MATH_2PI) / 3.0))
01061
01062
01063
01064
01065
01066
01067
01068
01069 double x1 = DBL_MAX;
01070 double x2 = DBL_MAX;
01071 double x3 = DBL_MAX;
01072
01073 if (a > 0.0) {
01074 x1 = TR1 - a / 3.0;
01075 if (TR2 > 0.0 && (TR2 + TR3) > 2.0 * a) {
01076
01077 x3 = TR3 - a / 3.0;
01078 x2 = -c / ( x1 * x3 );
01079 irplib_trace();
01080 } else {
01081
01082
01083
01084 x2 = TR2 - a / 3.0;
01085
01086 x3 = -c / ( x1 * x2 );
01087 irplib_trace();
01088 }
01089 } else if (a < 0.0) {
01090 x3 = TR3 - a / 3.0;
01091 if (TR2 < 0.0 && (TR1 + TR2) > 2.0 * a) {
01092 x1 = TR1 - a / 3.0;
01093 x2 = -c / ( x1 * x3 );
01094 irplib_trace();
01095 } else {
01096 x2 = TR2 - a / 3.0;
01097 x1 = -c / ( x2 * x3 );
01098 irplib_trace();
01099 }
01100 } else {
01101 x1 = TR1;
01102 x2 = TR2;
01103 x3 = TR3;
01104 irplib_trace();
01105 }
01106
01107 assert(x1 < x3);
01108
01109 if (x1 > x2) {
01110
01111
01112
01113
01114
01115
01116
01117 x1 = x2 = 0.5 * ( x1 + x2 );
01118 irplib_trace();
01119 } else if (x2 > x3) {
01120
01121
01122
01123
01124
01125
01126
01127 x3 = x2 = 0.5 * ( x2 + x3 );
01128 irplib_trace();
01129 }
01130
01131 *px1 = x1;
01132 *px2 = x2;
01133 *px3 = x3;
01134
01135 return;
01136 }
01137
01138
01156
01157 static cpl_error_code irplib_polynomial_solve_1d_4(double p4, double p3,
01158 double p2, double p1,
01159 double p0, int * preal,
01160 double * px1, double * px2,
01161 double * px3, double * px4)
01162 {
01163
01164
01165 const double a = (p2 - 0.375 * p3 * p3 / p4) / p4;
01166 const double b = (p1 - 0.5 * (p2 - 0.25 * p3 * p3 / p4 ) * p3 / p4 ) / p4;
01167 const double c =
01168 (p0 - 0.25 * (p1 - 0.25 * (p2 - 0.1875 * p3 * p3 / p4 ) * p3 / p4
01169 ) * p3 / p4 ) / p4;
01170
01171 double x1 = DBL_MAX;
01172 double x2 = DBL_MAX;
01173 double x3 = DBL_MAX;
01174 double x4 = DBL_MAX;
01175
01176 assert(preal != NULL );
01177 assert(px1 != NULL );
01178 assert(px2 != NULL );
01179 assert(px3 != NULL );
01180 assert(px4 != NULL );
01181
01182 *preal = 4;
01183
01184 if (c == 0.0) {
01185
01186
01187
01188 cpl_boolean dbl1, dbl2;
01189 const cpl_boolean is_real =
01190 !irplib_polynomial_solve_1d_3(1.0, 0.0, a, b, &x1, &x3, &x4,
01191 &dbl1, &dbl2);
01192
01193 x1 -= 0.25 * p3 / p4;
01194 x2 = -0.25 * p3 / p4;
01195 x3 -= 0.25 * p3 / p4;
01196 if (is_real) {
01197
01198 if (dbl2) {
01199 x4 = x3;
01200 assert( x1 <= x2);
01201 assert( x2 <= x3);
01202 } else {
01203 x4 -= 0.25 * p3 / p4;
01204
01205 if (x2 > x3) {
01206 IRPLIB_SWAP(x2, x3);
01207 }
01208 if (dbl1) {
01209 assert( x1 <= x2);
01210 assert( x2 <= x3);
01211 assert( x2 <= x4);
01212 } else {
01213 assert( x1 < x2);
01214 assert( x2 < x4);
01215 }
01216 }
01217 } else {
01218 *preal = 2;
01219
01220 if (x1 > x2) {
01221 assert( x3 <= x2 );
01222
01223 IRPLIB_SWAP(x1, x2);
01224 } else {
01225 assert( x3 >= x2 );
01226 }
01227 }
01228
01229 } else if (b == 0.0) {
01230
01231 double u1, u2;
01232 const cpl_boolean is_complex = irplib_polynomial_solve_1d_2(1.0, a, c,
01233 &u1, &u2);
01234
01235 if (is_complex) {
01236
01237 const double norm = sqrt(u1*u1 + u2*u2);
01238 const double v1 = sqrt(0.5*(norm+u1));
01239 const double v2 = u2 / sqrt(2.0*(norm+u1));
01240
01241
01242 x1 = -0.25 * p3 / p4 - v1;
01243 x3 = -0.25 * p3 / p4 + v1;
01244
01245 x4 = x2 = v2;
01246
01247 *preal = 0;
01248
01249 } else if (u1 >= 0.0) {
01250
01251 const double sv1 = sqrt(u1);
01252 const double sv2 = sqrt(u2);
01253
01254
01255 *preal = 4;
01256
01257 x1 = -0.25 * p3 / p4 - sv2;
01258 x2 = -0.25 * p3 / p4 - sv1;
01259 x3 = -0.25 * p3 / p4 + sv1;
01260 x4 = -0.25 * p3 / p4 + sv2;
01261 } else if (u2 < 0.0) {
01262
01263 const double sv1 = sqrt(-u2);
01264 const double sv2 = sqrt(-u1);
01265
01266
01267 *preal = 0;
01268
01269 x1 = x3 = -0.25 * p3 / p4;
01270
01271 x2 = sv1;
01272 x4 = sv2;
01273 } else {
01274
01275 const double sv1 = sqrt(-u1);
01276 const double sv2 = sqrt(u2);
01277
01278
01279 *preal = 2;
01280
01281 x1 = -0.25 * p3 / p4 - sv2;
01282 x2 = -0.25 * p3 / p4 + sv2;
01283
01284 x3 = -0.25 * p3 / p4;
01285 x4 = sv1;
01286 }
01287 } else {
01288
01289 const double q2 = -a;
01290 const double q1 = -4.0 * c;
01291 const double q0 = 4.0 * a * c - b * b;
01292 double u1, sqrtd, sqrtrd;
01293 double z1, z2, z3, z4;
01294
01295 cpl_boolean is_complex1, is_complex2;
01296
01297
01298
01299 (void)irplib_polynomial_solve_1d_3(1.0, q2, q1, q0, &u1, NULL, NULL,
01300 NULL, NULL);
01301
01302
01303 assert( u1 > a );
01304
01305 sqrtd = sqrt(u1 - a);
01306
01307 sqrtrd = 0.5 * b/sqrtd;
01308
01309 is_complex1 = irplib_polynomial_solve_1d_2(1.0, sqrtd, 0.5*u1 - sqrtrd,
01310 &z1, &z2);
01311
01312 is_complex2 = irplib_polynomial_solve_1d_2(1.0, -sqrtd, 0.5*u1 + sqrtrd,
01313 &z3, &z4);
01314
01315 z1 -= 0.25 * p3 / p4;
01316 z3 -= 0.25 * p3 / p4;
01317 if (!is_complex1) z2 -= 0.25 * p3 / p4;
01318 if (!is_complex2) z4 -= 0.25 * p3 / p4;
01319
01320 if (!is_complex1 && is_complex2) {
01321 *preal = 2;
01322 x1 = z1;
01323 x2 = z2;
01324 x3 = z3;
01325 x4 = z4;
01326 } else if (is_complex1 && !is_complex2) {
01327 *preal = 2;
01328 x1 = z3;
01329 x2 = z4;
01330 x3 = z1;
01331 x4 = z2;
01332 } else if (is_complex1 && is_complex2) {
01333 *preal = 0;
01334
01335 if (z1 < z3 || (z1 == z3 && z2 <= z4)) {
01336 x1 = z1;
01337 x2 = z2;
01338 x3 = z3;
01339 x4 = z4;
01340 } else {
01341 x1 = z3;
01342 x2 = z4;
01343 x3 = z1;
01344 x4 = z2;
01345 }
01346 } else {
01347 *preal = 4;
01348
01349 if (z3 >= z2) {
01350 x1 = z1;
01351 x2 = z2;
01352 x3 = z3;
01353 x4 = z4;
01354 } else if (z4 <= z1) {
01355 x1 = z3;
01356 x2 = z4;
01357 x3 = z1;
01358 x4 = z2;
01359 } else if (z2 > z4) {
01360 x1 = z3;
01361 x2 = z1;
01362 x3 = z4;
01363 x4 = z2;
01364 } else {
01365 x1 = z1;
01366 x2 = z3;
01367 x3 = z2;
01368 x4 = z4;
01369 }
01370 }
01371 }
01372
01373 *px1 = x1;
01374 *px2 = x2;
01375 *px3 = x3;
01376 *px4 = x4;
01377
01378 return CPL_ERROR_NONE;
01379 }
01380
01381 #ifdef IPRLIB_POLYNOMIAL_USE_MONOMIAL_ROOT
01382
01390
01391 static double irplib_polynomial_depress_1d(cpl_polynomial * self)
01392 {
01393
01394 const int degree = cpl_polynomial_get_degree(self);
01395 const int nc1 = degree - 1;
01396 const double an = cpl_polynomial_get_coeff(self, °ree);
01397 const double an1 = cpl_polynomial_get_coeff(self, &nc1);
01398 double rmean;
01399 int i;
01400
01401
01402 cpl_ensure(degree > 0, CPL_ERROR_DATA_NOT_FOUND, 0.0);
01403
01404 assert( an != 0.0 );
01405
01406 rmean = -an1/(an * (double)degree);
01407
01408 if (rmean != 0.0) {
01409
01410 cpl_polynomial_shift_1d(self, 0, rmean);
01411
01412 cpl_polynomial_set_coeff(self, &nc1, 0.0);
01413
01414 }
01415
01416
01417 for (i = 0; i < degree-1; i++) {
01418 const double ai = cpl_polynomial_get_coeff(self, &i) / an;
01419 cpl_polynomial_set_coeff(self, &i, ai);
01420 }
01421
01422 cpl_polynomial_set_coeff(self, °ree, 1.0);
01423
01424 return rmean;
01425 }
01426 #endif
01427
01428
01443
01444 static
01445 cpl_error_code irplib_polynomial_divide_1d_root(cpl_polynomial * p, double r,
01446 double * pres)
01447 {
01448
01449 const int n = cpl_polynomial_get_degree(p);
01450 double sum;
01451 int i;
01452
01453
01454 cpl_ensure_code(p != NULL, CPL_ERROR_NULL_INPUT);
01455 cpl_ensure_code(cpl_polynomial_get_dimension(p) == 1,
01456 CPL_ERROR_INVALID_TYPE);
01457 cpl_ensure_code(n > 0, CPL_ERROR_DATA_NOT_FOUND);
01458
01459 sum = cpl_polynomial_get_coeff(p, &n);
01460 cpl_polynomial_set_coeff(p, &n, 0.0);
01461
01462 for (i = n-1; i >= 0; i--) {
01463 const double coeff = cpl_polynomial_get_coeff(p, &i);
01464
01465 cpl_polynomial_set_coeff(p, &i, sum);
01466
01467 sum = coeff + r * sum;
01468
01469 }
01470
01471 if (pres != NULL) *pres = sum;
01472
01473 return CPL_ERROR_NONE;
01474 }