irplib_polynomial.c

00001 /* $Id: irplib_polynomial.c,v 1.35 2013-01-29 08:43:33 jtaylor Exp $
00002  *
00003  * This file is part of the ESO Common Pipeline Library
00004  * Copyright (C) 2001-2004 European Southern Observatory
00005  *
00006  * This program is free software; you can redistribute it and/or modify
00007  * it under the terms of the GNU General Public License as published by
00008  * the Free Software Foundation; either version 2 of the License, or
00009  * (at your option) any later version.
00010  *
00011  * This program is distributed in the hope that it will be useful,
00012  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00013  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00014  * GNU General Public License for more details.
00015  *
00016  * You should have received a copy of the GNU General Public License
00017  * along with this program; if not, write to the Free Software
00018  * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02111-1307  USA
00019  */
00020 
00021 /*
00022  * $Author: jtaylor $
00023  * $Date: 2013-01-29 08:43:33 $
00024  * $Revision: 1.35 $
00025  * $Name: not supported by cvs2svn $
00026  */
00027 
00028 #ifdef HAVE_CONFIG_H
00029 #include <config.h>
00030 #endif
00031 
00032 /*-----------------------------------------------------------------------------
00033                                    Includes
00034  -----------------------------------------------------------------------------*/
00035 
00036 #include "irplib_polynomial.h"
00037 #include <assert.h>
00038 #include <math.h>
00039 /* DBL_MAX: */
00040 #include <float.h>
00041 
00042 /*----------------------------------------------------------------------------*/
00048 /*----------------------------------------------------------------------------*/
00051 /*-----------------------------------------------------------------------------
00052                                    Macro definitions
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() /* Trace */
00061 #endif
00062 
00063 /*-----------------------------------------------------------------------------
00064                                    Static functions
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, cpl_size *,
00097                                                    double *, double *,
00098                                                    double *, double *);
00099 
00100 static cpl_error_code irplib_polynomial_solve_1d_nonzero(cpl_polynomial *,
00101                                                          cpl_vector *,
00102                                                          cpl_size *);
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                               Function codes
00113  -----------------------------------------------------------------------------*/
00114 
00115 /*----------------------------------------------------------------------------*/
00141 /*----------------------------------------------------------------------------*/
00142 cpl_error_code irplib_polynomial_solve_1d_all(const cpl_polynomial * self,
00143                                               cpl_vector * roots,
00144                                               cpl_size * preal)
00145 {
00146 
00147     cpl_error_code error = CPL_ERROR_NONE;
00148     cpl_polynomial * p;
00149 
00150     cpl_ensure_code(self  != NULL, CPL_ERROR_NULL_INPUT);
00151     cpl_ensure_code(roots != NULL, CPL_ERROR_NULL_INPUT);
00152     cpl_ensure_code(preal != NULL, CPL_ERROR_NULL_INPUT);
00153     cpl_ensure_code(cpl_polynomial_get_dimension(self) == 1,
00154                     CPL_ERROR_INVALID_TYPE);
00155     cpl_ensure_code(cpl_polynomial_get_degree(self) > 0,
00156                     CPL_ERROR_DATA_NOT_FOUND);
00157     cpl_ensure_code(cpl_polynomial_get_degree(self) ==
00158                     cpl_vector_get_size(roots), CPL_ERROR_INCOMPATIBLE_INPUT);
00159 
00160     *preal = 0;
00161 
00162     p = cpl_polynomial_duplicate(self);
00163 
00164     error = irplib_polynomial_solve_1d_nonzero(p, roots, preal);
00165 
00166     cpl_polynomial_delete(p);
00167 
00168     return error;
00169 
00170 }
00171 
00174 /*----------------------------------------------------------------------------*/
00201 /*----------------------------------------------------------------------------*/
00202 static cpl_error_code irplib_polynomial_solve_1d_nonzero(cpl_polynomial * self,
00203                                                          cpl_vector * roots,
00204                                                          cpl_size * preal)
00205 {
00206     cpl_error_code error   = CPL_ERROR_NONE;
00207     const cpl_size ncoeffs = 1 + cpl_polynomial_get_degree(self);
00208 
00209     cpl_ensure_code(self  != NULL,  CPL_ERROR_NULL_INPUT);
00210     cpl_ensure_code(roots != NULL,  CPL_ERROR_NULL_INPUT);
00211     cpl_ensure_code(preal != NULL,  CPL_ERROR_NULL_INPUT);
00212     cpl_ensure_code(cpl_polynomial_get_dimension(self) == 1,
00213                     CPL_ERROR_INVALID_TYPE);
00214     cpl_ensure_code(ncoeffs   > 1,  CPL_ERROR_DATA_NOT_FOUND);
00215     cpl_ensure_code(*preal >= 0,    CPL_ERROR_ILLEGAL_INPUT);
00216     cpl_ensure_code(ncoeffs + *preal == 1+cpl_vector_get_size(roots),
00217                     CPL_ERROR_INCOMPATIBLE_INPUT);
00218 
00219     switch (ncoeffs) {
00220 
00221     case 2 : {
00222         const cpl_size i1 = 1;
00223         const cpl_size i0 = 0;
00224         const double   p1 = cpl_polynomial_get_coeff(self, &i1);
00225         const double   p0 = cpl_polynomial_get_coeff(self, &i0);
00226 
00227         cpl_vector_set(roots, (*preal)++, -p0/p1);
00228         break;
00229     }
00230     case 3 : {
00231         const cpl_size i2 = 2;
00232         const cpl_size i1 = 1;
00233         const cpl_size i0 = 0;
00234         const double   p2 = cpl_polynomial_get_coeff(self, &i2);
00235         const double   p1 = cpl_polynomial_get_coeff(self, &i1);
00236         const double   p0 = cpl_polynomial_get_coeff(self, &i0);
00237         double         x1, x2;
00238 
00239         if (irplib_polynomial_solve_1d_2(p2, p1, p0, &x1, &x2)) {
00240             /* This is the complex root in the upper imaginary half-plane */
00241             cpl_vector_set(roots, (*preal)  , x1);
00242             cpl_vector_set(roots, (*preal)+1, x2);
00243         } else {
00244             cpl_vector_set(roots, (*preal)++, x1);
00245             cpl_vector_set(roots, (*preal)++, x2);
00246         }
00247         break;
00248     }
00249     case 4 : {
00250         const cpl_size i3 = 3;
00251         const cpl_size i2 = 2;
00252         const cpl_size i1 = 1;
00253         const cpl_size i0 = 0;
00254         const double   p3 = cpl_polynomial_get_coeff(self, &i3);
00255         const double   p2 = cpl_polynomial_get_coeff(self, &i2);
00256         const double   p1 = cpl_polynomial_get_coeff(self, &i1);
00257         const double   p0 = cpl_polynomial_get_coeff(self, &i0);
00258         double         x1, x2, x3;
00259 
00260         if (irplib_polynomial_solve_1d_3(p3, p2, p1, p0, &x1, &x2, &x3,
00261                                          NULL, NULL)) {
00262             cpl_vector_set(roots, (*preal)++, x1);
00263             /* This is the complex root in the upper imaginary half-plane */
00264             cpl_vector_set(roots, (*preal)  , x2);
00265             cpl_vector_set(roots, (*preal)+1, x3);
00266         } else {
00267             cpl_vector_set(roots, (*preal)++, x1);
00268             cpl_vector_set(roots, (*preal)++, x2);
00269             cpl_vector_set(roots, (*preal)++, x3);
00270         }
00271         break;
00272     }
00273     case 5 : {
00274         const cpl_size i4 = 4;
00275         const cpl_size i3 = 3;
00276         const cpl_size i2 = 2;
00277         const cpl_size i1 = 1;
00278         const cpl_size i0 = 0;
00279         const double   p4 = cpl_polynomial_get_coeff(self, &i4);
00280         const double   p3 = cpl_polynomial_get_coeff(self, &i3);
00281         const double   p2 = cpl_polynomial_get_coeff(self, &i2);
00282         const double   p1 = cpl_polynomial_get_coeff(self, &i1);
00283         const double   p0 = cpl_polynomial_get_coeff(self, &i0);
00284         double         x1, x2, x3, x4;
00285         cpl_size       nreal;
00286 
00287         error = irplib_polynomial_solve_1d_4(p4, p3, p2, p1, p0, &nreal,
00288                                              &x1, &x2, &x3, &x4);
00289         if (!error) {
00290             cpl_vector_set(roots, (*preal)  , x1);
00291             cpl_vector_set(roots, (*preal)+1, x2);
00292             cpl_vector_set(roots, (*preal)+2, x3);
00293             cpl_vector_set(roots, (*preal)+3, x4);
00294 
00295             *preal += nreal;
00296         }
00297         break;
00298     }
00299 
00300     default: {
00301 
00302         /* Try to reduce the problem by finding a single root */
00303 #ifndef IPRLIB_POLYNOMIAL_USE_MONOMIAL_ROOT
00304         const cpl_size    n0 = ncoeffs-1;
00305         const double pn0 = cpl_polynomial_get_coeff(self, &n0);
00306         const cpl_size    n1 = ncoeffs-2;
00307         const double pn1 = cpl_polynomial_get_coeff(self, &n1);
00308         /* First guess of root is the root average.
00309            FIXME: May need refinement, e.g. via bisection */
00310         const double rmean = -pn1 / (pn0 * n0);
00311         double root = rmean;
00312 #else
00313         /* Try an analytical solution to a (shifted) monomial */
00314         cpl_polynomial * copy = cpl_polynomial_duplicate(self);
00315         const cpl_size    i0 = 0;
00316         const double rmean = irplib_polynomial_depress_1d(copy);
00317         const double c0 = cpl_polynomial_get_coeff(copy, &i0);
00318         double root = rmean + ((n0&1) && c0 < 0.0 ? -1.0 : 1.0)
00319             * pow(fabs(c0), 1.0/n0);
00320 
00321         cpl_polynomial_delete(copy);
00322 #endif
00323 
00324         error = cpl_polynomial_solve_1d(self, root, &root, 1);
00325 
00326         if (!error) {
00327 
00328             cpl_vector_set(roots, (*preal)++, root);
00329 
00330             irplib_polynomial_divide_1d_root(self, root, NULL);
00331 
00332             error = irplib_polynomial_solve_1d_nonzero(self, roots, preal);
00333 
00334             if (!error && *preal > 1) {
00335                 /* Sort the real roots */
00336 
00337                 /* FIXME: Assumes that all roots found so far are real */
00338 
00339                 cpl_vector * reals = cpl_vector_wrap(*preal,
00340                                                      cpl_vector_get_data(roots));
00341                 cpl_vector_sort(reals, 1);
00342                 (void)cpl_vector_unwrap(reals);
00343             }
00344         }
00345         break;
00346     }
00347     }
00348 
00349     return error;
00350 }
00351 
00352 /*----------------------------------------------------------------------------*/
00364 /*----------------------------------------------------------------------------*/
00365 static cpl_boolean irplib_polynomial_solve_1d_2(double p2, double p1, double p0,
00366                                                 double * px1,
00367                                                 double * px2) {
00368 
00369     const double sqrtD = sqrt(fabs(p1 * p1 - 4.0 * p2 * p0));
00370     cpl_boolean is_complex = CPL_FALSE;
00371     double x1 = -0.5 * p1 / p2; /* Double root */
00372     double x2;
00373 
00374     /* Compute residual, assuming D == 0 */
00375     double res0 = irplib_polynomial_eval_2_max(p2, p1, p0, CPL_FALSE, x1, x1);
00376     double res;
00377 
00378     assert(px1 != NULL );
00379     assert(px2 != NULL );
00380 
00381     *px2 = *px1 = x1;
00382 
00383     /* Compute residual, assuming D > 0 */
00384 
00385     /* x1 is the root with largest absolute value */
00386     if (p1 > 0.0) {
00387         x1 = -0.5 * (p1 + sqrtD);
00388         irplib_trace(); /* OK */
00389     } else {
00390         x1 = -0.5 * (p1 - sqrtD);
00391         irplib_trace(); /* OK */
00392     }
00393     /* Compute smaller root via division to avoid
00394        loss of precision due to cancellation */
00395     x2 = p0 / x1;
00396     x1 /= p2; /* Scale x1 with leading coefficient */
00397 
00398     res = irplib_polynomial_eval_2_max(p2, p1, p0, CPL_FALSE, x1, x2);
00399 
00400     if (res < res0) {
00401         res0 = res;
00402         if (x2 > x1) {
00403             *px1 = x1;
00404             *px2 = x2;
00405             irplib_trace(); /* OK */
00406         } else {
00407             *px1 = x2;
00408             *px2 = x1;
00409             irplib_trace(); /* OK */
00410         }
00411     }
00412 
00413     /* Compute residual, assuming D < 0 */
00414 
00415     x1 = -0.5 * p1 / p2;          /* Real part of complex root */
00416     x2 =  0.5 * sqrtD / fabs(p2); /* Positive, imaginary part of root */
00417 
00418     res  = irplib_polynomial_eval_2_max(p2, p1, p0, CPL_TRUE,  x1, x2);
00419 
00420     if (res < res0) {
00421         *px1 = x1;
00422         *px2 = x2;
00423         is_complex = CPL_TRUE;
00424         irplib_trace(); /* OK */
00425     }
00426 
00427     return is_complex;
00428 
00429 }
00430 
00431 
00432 /*----------------------------------------------------------------------------*/
00445 /*----------------------------------------------------------------------------*/
00446 static double irplib_polynomial_eval_2_max(double p2, double p1, double p0,
00447                                            cpl_boolean is_c,
00448                                            double x1, double x2)
00449 {
00450     double res;
00451 
00452     if (is_c) {
00453         res = fabs(p0 + x1 * (p1 + x1 * p2) - p2 * x2 * x2);
00454         irplib_trace(); /* OK */
00455     } else {
00456         const double r1 = fabs(p0 + x1 * (p1 + x1 * p2));
00457         const double r2 = fabs(p0 + x2 * (p1 + x2 * p2));
00458 
00459         res = r1 > r2 ? r1 : r2;
00460         irplib_trace(); /* OK */
00461     }
00462 
00463     return res;
00464 }
00465 
00466 
00467 /*----------------------------------------------------------------------------*/
00482 /*----------------------------------------------------------------------------*/
00483 static double irplib_polynomial_eval_3_max(double p3, double p2,
00484                                            double p1, double p0,
00485                                            cpl_boolean is_c,
00486                                            double x1, double x2, double x3)
00487 {
00488     const double r1 = fabs(p0 + x1 * (p1 + x1 * (p2 + x1 * p3)));
00489     double res;
00490 
00491     if (is_c) {
00492         const double r2 = fabs(p0 + x2 * (p1 + x2 * (p2 + x2 * p3))
00493                                - x3 * x3 * ( 3.0 * p3 * x2 + p2));
00494 
00495         res = r1 > r2 ? r1 : r2;
00496         irplib_trace(); /* OK */
00497     } else {
00498         const double r2 = fabs(p0 + x2 * (p1 + x2 * (p2 + x2 * p3)));
00499         const double r3 = fabs(p0 + x3 * (p1 + x3 * (p2 + x3 * p3)));
00500         res = r1 > r2 ? (r1 > r3 ? r1 : r3) : (r2 > r3 ? r2 : r3);
00501         irplib_trace(); /* OK */
00502     }
00503 
00504     /* cpl_msg_info(cpl_func, "%d: %g (%g)", __LINE__, res, r1); */
00505 
00506     return res;
00507 }
00508 
00509 
00510 /*----------------------------------------------------------------------------*/
00529 /*----------------------------------------------------------------------------*/
00530 static cpl_boolean irplib_polynomial_solve_1d_3(double p3, double p2, double p1,
00531                                                 double p0,
00532                                                 double * px1,
00533                                                 double * px2,
00534                                                 double * px3,
00535                                                 cpl_boolean * pdbl1,
00536                                                 cpl_boolean * pdbl2) {
00537     cpl_boolean is_complex = CPL_FALSE;
00538     const double a = p2/p3;
00539     const double b = p1/p3;
00540     const double c = p0/p3;
00541 
00542     const double q = (a * a - 3.0 * b);
00543     const double r = (a * (2.0 * a * a - 9.0 * b) + 27.0 * c);
00544 
00545     const double Q = q / 9.0;
00546     const double R = r / 54.0;
00547 
00548     const double Q3 = Q * Q * Q;
00549     const double R2 = R * R;
00550 
00551     double x1 = DBL_MAX; /* Fix (false) uninit warning */
00552     double x2 = DBL_MAX; /* Fix (false) uninit warning */
00553     double x3 = DBL_MAX; /* Fix (false) uninit warning */
00554     double xx1 = DBL_MAX; /* Fix (false) uninit warning */
00555     double xx2 = DBL_MAX; /* Fix (false) uninit warning */
00556     double xx3 = DBL_MAX; /* Fix (false) uninit warning */
00557 
00558     double resx = DBL_MAX;
00559     double res  = DBL_MAX;
00560     cpl_boolean is_first = CPL_TRUE;
00561 
00562     cpl_boolean dbl2;
00563 
00564 
00565     assert(px1 != NULL );
00566 
00567     if (pdbl1 != NULL) *pdbl1 = CPL_FALSE;
00568     if (pdbl2 != NULL) *pdbl2 = CPL_FALSE;
00569 
00570     dbl2 = CPL_FALSE;
00571 
00572     /*
00573       All branches (for which the roots are defined) are evaluated, and
00574       the branch with the smallest maximum-residual is chosen.
00575       When two maximum-residual are identical, preference is given to
00576       the purely real solution and if necessary to the solution with a
00577       double root.
00578     */
00579 
00580     if ((R2 >= Q3 && R != 0.0) || R2 > Q3) {
00581 
00582         cpl_boolean is_c = CPL_FALSE;
00583 
00584         irplib_polynomial_solve_1d_3c(a, c, Q, Q3, R, R2, &x1, &x2, &x3,
00585                                       &is_c, &dbl2);
00586 
00587 
00588         res = resx = irplib_polynomial_eval_3_max(p3, p2, p1, p0, is_c,
00589                                             x1, x2, x3);
00590 
00591         is_first = CPL_FALSE;
00592 
00593         if (pdbl1 != NULL) *pdbl1 = CPL_FALSE;
00594         if (!is_c && pdbl2 != NULL) *pdbl2 = dbl2;
00595         is_complex = is_c;
00596         irplib_trace(); /* OK */
00597    
00598     }
00599 
00600     if (Q > 0.0 && fabs(R / (Q * sqrt(Q))) <= 1.0) {
00601 
00602         /* this test is actually R2 < Q3, written in a form suitable
00603            for exact computation with integers */
00604 
00605         /* assert( Q > 0.0 ); */
00606 
00607         irplib_polynomial_solve_1d_3r(a, c, Q, R, &xx1, &xx2, &xx3);
00608 
00609         resx = irplib_polynomial_eval_3_max(p3, p2, p1, p0, CPL_FALSE,
00610                                             xx1, xx2, xx3);
00611 
00612         if (is_first || (dbl2 ? resx < res : resx <= res)) {
00613             is_first = CPL_FALSE;
00614             res = resx;
00615             x1 = xx1;
00616             x2 = xx2;
00617             x3 = xx3;
00618             if (pdbl1 != NULL) *pdbl1 = CPL_FALSE;
00619             if (pdbl2 != NULL) *pdbl2 = CPL_FALSE;
00620             is_complex = CPL_FALSE;
00621             irplib_trace(); /* OK */
00622         }
00623     }
00624 
00625     if (Q >= 0) {
00626         cpl_boolean dbl1 = CPL_FALSE;
00627 
00628 
00629         irplib_polynomial_solve_1d_32(a, c, Q, &xx1, &xx2, &xx3, &dbl2);
00630 
00631         resx = irplib_polynomial_eval_3_max(p3, p2, p1, p0, CPL_FALSE,
00632                                             xx1, xx2, xx3);
00633         /*
00634         cpl_msg_info(cpl_func, "%d: %g = %g - %g (%u)", __LINE__,
00635                      res - resx, res, resx, is_complex);
00636         */
00637 
00638         if (is_first || resx <= res) {
00639             is_first = CPL_FALSE;
00640             res = resx;
00641             x1 = xx1;
00642             x2 = xx2;
00643             x3 = xx3;
00644             if (pdbl1 != NULL) *pdbl1 = CPL_FALSE;
00645             if (pdbl2 != NULL) *pdbl2 = dbl2;
00646             is_complex = CPL_FALSE;
00647             irplib_trace(); /* OK */
00648         }
00649 
00650 
00651         /* This branch also covers the case where the depressed cubic
00652            polynomial has zero as triple root (i.e. Q == R == 0) */
00653 
00654         irplib_polynomial_solve_1d_31(a, Q, &xx1, &xx2, &xx3, &dbl1);
00655 
00656         resx = irplib_polynomial_eval_3_max(p3, p2, p1, p0, CPL_FALSE,
00657                                             xx1, xx2, xx3);
00658 
00659         if (resx <= res) {
00660             is_first = CPL_FALSE;
00661             res = resx;
00662             x1 = xx1;
00663             x2 = xx2;
00664             x3 = xx3;
00665             if (pdbl1 != NULL) *pdbl1 = dbl1;
00666             if (pdbl2 != NULL) *pdbl2 = CPL_FALSE;
00667             is_complex = CPL_FALSE;
00668             irplib_trace(); /* OK */
00669         }
00670 
00671     }
00672 
00673     if (px2 != NULL && px3 != NULL) {
00674         *px1 = x1;
00675         *px2 = x2;
00676         *px3 = x3;
00677         irplib_trace(); /* OK */
00678     } else if (is_complex) {
00679         *px1 = x1;
00680         irplib_trace(); /* OK */
00681     } else {
00682         *px1 = x3;
00683         irplib_trace(); /* OK */
00684     }
00685 
00686     return is_complex;
00687 }
00688 
00689 /*----------------------------------------------------------------------------*/
00703 /*----------------------------------------------------------------------------*/
00704 static void irplib_polynomial_solve_1d_31(double a, double Q,
00705                                           double * px1, double * px2,
00706                                           double * px3, cpl_boolean * pdbl1)
00707 {
00708 
00709     const double sqrtQ = sqrt (Q);
00710 
00711     double x1, x2, x3;
00712 
00713     x2 = x1 = -sqrtQ - a / 3.0;
00714     x3 = 2.0 * sqrtQ - a / 3.0;
00715     if (pdbl1 != NULL) *pdbl1 = CPL_TRUE;
00716 
00717     *px1 = x1;
00718     *px2 = x2;
00719     *px3 = x3;
00720 
00721     irplib_trace(); /* OK */
00722     return;
00723 }
00724 
00725 /*----------------------------------------------------------------------------*/
00740 /*----------------------------------------------------------------------------*/
00741 static void irplib_polynomial_solve_1d_32(double a, double c, double Q,
00742                                           double * px1, double * px2,
00743                                           double * px3, cpl_boolean * pdbl2)
00744 {
00745 
00746     const double sqrtQ = sqrt (Q);
00747 
00748     double x1 = DBL_MAX;
00749     double x2 = DBL_MAX;
00750     double x3 = DBL_MAX;
00751 
00752     if (a > 0.0) {
00753         /* a and sqrt(Q) have same sign - or Q is zero */
00754         x1 = -2.0 * sqrtQ - a / 3.0;
00755         /* FIXME: Two small roots with opposite signs may
00756            end up here, with the sign lost for one of them */
00757         x3 = x2 = -a < x1 ? -sqrt(fabs(c / x1)) : sqrt(fabs(c / x1));
00758         if (pdbl2 != NULL) *pdbl2 = CPL_TRUE;
00759         irplib_trace(); /* OK */
00760     } else if (a < 0.0) {
00761         /* a and sqrt(Q) have opposite signs - or Q is zero */
00762         x3 = x2 = sqrtQ - a / 3.0;
00763         x1 = -c / (x2 * x2);
00764         if (pdbl2 != NULL) *pdbl2 = CPL_TRUE;
00765         irplib_trace(); /* OK */
00766     } else {
00767         x1 = -2.0 * sqrtQ;
00768         x3 = x2 = sqrtQ;
00769         if (pdbl2 != NULL) *pdbl2 = CPL_TRUE;
00770         irplib_trace(); /* OK */
00771     }
00772 
00773     *px1 = x1;
00774     *px2 = x2;
00775     *px3 = x3;
00776 
00777     return;
00778 }
00779 
00780 /*----------------------------------------------------------------------------*/
00800 /*----------------------------------------------------------------------------*/
00801 static void irplib_polynomial_solve_1d_3c(double a, double c,
00802                                           double Q, double Q3,
00803                                           double R, double R2,
00804                                           double * px1,
00805                                           double * px2, double * px3,
00806                                           cpl_boolean * pis_c,
00807                                           cpl_boolean * pdbl2)
00808 {
00809 
00810     /* Due to finite precision some double roots may be missed, and
00811        will be considered to be a pair of complex roots z = x +/-
00812        epsilon i close to the real axis. */
00813 
00814     /* Another case: A double root, which is small relative to the
00815        last root, may cause this branch to be taken - with the
00816        imaginary part eventually being truncated to zero. */
00817 
00818     const double sgnR = (R >= 0 ? 1.0 : -1.0);
00819     const double A = -sgnR * pow (fabs (R) + sqrt (R2 - Q3), 1.0 / 3.0);
00820     const double B = Q / A;
00821 
00822     double x1 = DBL_MAX;
00823     double x2 = DBL_MAX;
00824     double x3 = DBL_MAX;
00825     cpl_boolean is_complex = CPL_FALSE;
00826 
00827     if (( A > -B && a > 0.0) || (A < -B && a < 0.0)) {
00828         /* A+B has same sign as a */
00829 
00830         /* Real part of complex conjugate */
00831         x2 = -0.5 * (A + B) - a / 3.0; /* No cancellation */
00832         /* Positive, imaginary part of complex conjugate */
00833         x3 = 0.5 * CPL_MATH_SQRT3 * fabs(A - B);
00834 
00835         x1 = -c / (x2 * x2 + x3 * x3);
00836         irplib_trace(); /* OK */
00837     } else {
00838         /* A+B and a have opposite signs - or exactly one is zero */
00839         x1 = A + B - a / 3.0;
00840         /* Positive, imaginary part of complex conjugate */
00841         x3 = 0.5 * CPL_MATH_SQRT3 * fabs(A - B);
00842 
00843         if (x3 > 0.0) {
00844             /* Real part of complex conjugate */
00845             x2 = -0.5 * (A + B) - a / 3.0; /* FIXME: Cancellation */
00846             irplib_trace(); /* OK */
00847         } else {
00848 
00849             x2 = -a < x1 ? -sqrt(fabs(c / x1)) : sqrt(fabs(c / x1));
00850             x3 = 0.0;
00851             irplib_trace(); /* OK */
00852         }
00853     }
00854 
00855     if (x3 > 0.0) {
00856         is_complex = CPL_TRUE;
00857         irplib_trace(); /* OK */
00858     } else {
00859         /* Whoaa, the imaginary part was truncated to zero
00860            - return a real, double root */
00861         x3 = x2;
00862         if (pdbl2 != NULL) *pdbl2 = CPL_TRUE;
00863         irplib_trace(); /* OK */
00864     }
00865 
00866     *px1 = x1;
00867     *px2 = x2;
00868     *px3 = x3;
00869     *pis_c = is_complex;
00870 
00871     return;
00872 }
00873 
00874 /*----------------------------------------------------------------------------*/
00889 /*----------------------------------------------------------------------------*/
00890 static void irplib_polynomial_solve_1d_3r(double a, double c,
00891                                           double Q, double R,
00892                                           double * px1,
00893                                           double * px2, double * px3)
00894 {
00895 
00896     const double sqrtQ = sqrt(Q);
00897     const double theta = acos (R / (Q * sqrtQ)); /* theta in range [0; pi] */
00898 
00899     /* -1.0 <= cos((theta + CPL_MATH_2PI) / 3.0) <= -0.5
00900        -0.5 <= cos((theta - CPL_MATH_2PI) / 3.0) <=  0.5
00901         0.5 <= cos((theta               ) / 3.0) <=  1.0 */
00902 
00903 #define TR1 (-2.0 * sqrtQ * cos( theta                 / 3.0))
00904 #define TR2 (-2.0 * sqrtQ * cos((theta - CPL_MATH_2PI) / 3.0))
00905 #define TR3 (-2.0 * sqrtQ * cos((theta + CPL_MATH_2PI) / 3.0))
00906 
00907     /* TR1 < TR2 < TR3, except when theta == 0, then TR2 == TR3 */
00908 
00909     /* The three roots must be transformed back via subtraction with a/3.
00910        To prevent loss of precision due to cancellation, the root which
00911        is closest to a/3 is computed using the relation
00912        p3 * x1 * x2 * x3 = -p0 */
00913 
00914     double x1 = DBL_MAX;
00915     double x2 = DBL_MAX;
00916     double x3 = DBL_MAX;
00917 
00918     if (a > 0.0) {
00919         x1 = TR1 - a / 3.0;
00920         if (TR2 > 0.0 && (TR2 + TR3) > 2.0 * a) {
00921             /* FIXME: Cancellation may still effect x3 ? */
00922             x3 = TR3 - a / 3.0;
00923             x2 = -c / ( x1 * x3 );
00924             irplib_trace(); /* OK */
00925         } else {
00926             /* FIXME: Cancellation may still effect x2, especially
00927                if x2, x3 is (almost) a double root, i.e.
00928                if theta is close to zero. */
00929             x2 = TR2 - a / 3.0;
00930  
00931             x3 = -c / ( x1 * x2 );
00932             irplib_trace(); /* OK */
00933         }
00934     } else if (a < 0.0) {
00935         x3 = TR3 - a / 3.0;
00936         if (TR2 < 0.0 && (TR1 + TR2) > 2.0 * a) {
00937             x1 = TR1 - a / 3.0;
00938             x2 = -c / ( x1 * x3 );
00939             irplib_trace(); /* OK */
00940         } else {
00941             x2 = TR2 - a / 3.0;
00942             x1 = -c / ( x2 * x3 );
00943             irplib_trace(); /* OK */
00944         }
00945     } else {
00946         x1 = TR1;
00947         x2 = TR2;
00948         x3 = TR3;
00949         irplib_trace(); /* OK */
00950     }
00951 
00952     assert(x1 < x3);
00953 
00954     if (x1 > x2) {
00955         /* In absence of round-off:
00956            theta == PI: x1 == x2,
00957            theta  < PI: x1 <  x2,
00958 
00959            The only way x1 could exceed x2 would be due to round-off when
00960            theta is close to PI */
00961      
00962         x1 = x2 = 0.5 * ( x1 + x2 );
00963         irplib_trace(); /* OK, tested only for x1 == x2 */
00964     } else if (x2 > x3) {
00965         /* In absence of round-off:
00966            theta == 0: x2 == x3,
00967            theta  > 0: x2 <  x3,
00968 
00969            For small theta:
00970            Round-off can cause x2 to become greater than x3 */
00971      
00972         x3 = x2 = 0.5 * ( x2 + x3 );
00973         irplib_trace(); /* OK */
00974     }
00975 
00976     *px1 = x1;
00977     *px2 = x2;
00978     *px3 = x3;
00979 
00980     return;
00981 }
00982 
00983 /*----------------------------------------------------------------------------*/
01001 /*----------------------------------------------------------------------------*/
01002 static cpl_error_code irplib_polynomial_solve_1d_4(double p4, double p3,
01003                                                    double p2, double p1,
01004                                                    double p0, cpl_size * preal,
01005                                                    double * px1, double * px2,
01006                                                    double * px3, double * px4)
01007 {
01008 
01009     /* Construct the monic, depressed quartic using Horners scheme on 1 / p4 */
01010     const double a = (p2 - 0.375 * p3 * p3 / p4) / p4;
01011     const double b = (p1 - 0.5 * (p2 - 0.25 * p3 * p3 / p4 ) * p3 / p4 ) / p4;
01012     const double c =
01013         (p0 - 0.25 * (p1 - 0.25 * (p2 - 0.1875 * p3 * p3 / p4 ) * p3 / p4
01014                       ) * p3 / p4 ) / p4;
01015 
01016     double x1 = DBL_MAX; /* Fix (false) uninit warning */
01017     double x2 = DBL_MAX; /* Fix (false) uninit warning */
01018     double x3 = DBL_MAX; /* Fix (false) uninit warning */
01019     double x4 = DBL_MAX; /* Fix (false) uninit warning */
01020 
01021     assert(preal != NULL );
01022     assert(px1   != NULL );
01023     assert(px2   != NULL );
01024     assert(px3   != NULL );
01025     assert(px4   != NULL );
01026 
01027     *preal = 4;
01028 
01029     if (c == 0.0) {
01030         /* The depressed quartic has zero as root */
01031         /* Since the sum of the roots is zero, at least one is negative
01032            and at least one is positive - unless they are all zero */
01033         cpl_boolean dbl1, dbl2;
01034         const cpl_boolean is_real =
01035             !irplib_polynomial_solve_1d_3(1.0, 0.0, a, b, &x1, &x3, &x4,
01036                                           &dbl1, &dbl2);
01037 
01038         x1 -= 0.25 * p3 / p4;
01039         x2 = -0.25 * p3 / p4;
01040         x3 -= 0.25 * p3 / p4;
01041         if (is_real) {
01042 
01043             if (dbl2) {
01044                 x4 = x3;
01045                 assert( x1 <= x2);
01046                 assert( x2 <= x3);
01047             } else {
01048                 x4 -= 0.25 * p3 / p4;
01049                 /* Need (only) a guarded swap of x2, x3 */
01050                 if (x2 > x3) {
01051                     IRPLIB_SWAP(x2, x3);
01052                 }
01053                 if (dbl1) {
01054                     assert( x1 <= x2); /* The cubic may have 0 as triple root */
01055                     assert( x2 <= x3);
01056                     assert( x2 <= x4);
01057                 } else {
01058                     assert( x1 < x2);
01059                     assert( x2 < x4);
01060                 }
01061             }
01062         } else {
01063             *preal = 2;
01064 
01065             if (x1 > x2) {
01066                 assert( x3 <= x2 ); /* Don't swap a complex root */
01067 
01068                 IRPLIB_SWAP(x1, x2);
01069             } else {
01070                 assert( x3 >= x2 );
01071             }
01072         }
01073 
01074     } else if (b == 0.0) {
01075         /* The monic, depressed quartic is a monic, biquadratic equation */
01076         double u1, u2;
01077         const cpl_boolean is_complex = irplib_polynomial_solve_1d_2(1.0, a, c,
01078                                                                     &u1, &u2);
01079 
01080         if (is_complex) {
01081             /* All four roots are conjugate, complex */
01082             const double norm = sqrt(u1*u1 + u2*u2);
01083             const double   v1 = sqrt(0.5*(norm+u1));
01084             const double   v2 = u2 / sqrt(2.0*(norm+u1));
01085 
01086 
01087             x1 = -0.25 * p3 / p4 - v1;
01088             x3 = -0.25 * p3 / p4 + v1;
01089 
01090             x4 = x2 = v2;
01091 
01092             *preal = 0;
01093 
01094         } else if (u1 >= 0.0) {
01095             /* All four roots are real */
01096             const double sv1 = sqrt(u1);
01097             const double sv2 = sqrt(u2);
01098 
01099 
01100             *preal = 4;
01101 
01102             x1 = -0.25 * p3 / p4 - sv2;
01103             x2 = -0.25 * p3 / p4 - sv1;
01104             x3 = -0.25 * p3 / p4 + sv1;
01105             x4 = -0.25 * p3 / p4 + sv2;
01106         } else if (u2 < 0.0) {
01107             /* All four roots are conjugate, complex */
01108             const double sv1 = sqrt(-u2);
01109             const double sv2 = sqrt(-u1);
01110 
01111 
01112             *preal = 0;
01113 
01114             x1 = x3 = -0.25 * p3 / p4;
01115 
01116             x2 = sv1;
01117             x4 = sv2;
01118         } else {
01119             /* Two roots are real, two roots are conjugate, complex */
01120             const double sv1 = sqrt(-u1);
01121             const double sv2 = sqrt(u2);
01122 
01123 
01124             *preal = 2;
01125 
01126             x1 = -0.25 * p3 / p4 - sv2;
01127             x2 = -0.25 * p3 / p4 + sv2;
01128 
01129             x3 = -0.25 * p3 / p4;
01130             x4 = sv1;
01131         }
01132     } else {
01133         /* Need a root from the nested, monic cubic */
01134         const double q2 = -a;
01135         const double q1 = -4.0 * c;
01136         const double q0 = 4.0 * a * c - b * b;
01137         double u1, sqrtd, sqrtrd;
01138         double z1, z2, z3, z4;
01139 
01140         cpl_boolean is_complex1, is_complex2;
01141 
01142         /* Largest cubic root ensures real square roots when solving the
01143            quartic equation */
01144         (void)irplib_polynomial_solve_1d_3(1.0, q2, q1, q0, &u1, NULL, NULL,
01145                                            NULL, NULL);
01146 
01147 
01148         assert( u1 > a );
01149 
01150         sqrtd = sqrt(u1 - a);
01151 
01152         sqrtrd = 0.5 * b/sqrtd;
01153 
01154         is_complex1 = irplib_polynomial_solve_1d_2(1.0,  sqrtd, 0.5*u1 - sqrtrd,
01155                                                    &z1, &z2);
01156 
01157         is_complex2 = irplib_polynomial_solve_1d_2(1.0, -sqrtd, 0.5*u1 + sqrtrd,
01158                                                    &z3, &z4);
01159 
01160         z1 -= 0.25 * p3 / p4;
01161         z3 -= 0.25 * p3 / p4;
01162         if (!is_complex1) z2 -= 0.25 * p3 / p4;
01163         if (!is_complex2) z4 -= 0.25 * p3 / p4;
01164 
01165         if (!is_complex1 && is_complex2) {
01166             *preal = 2;
01167             x1 = z1;
01168             x2 = z2;
01169             x3 = z3;
01170             x4 = z4;
01171         } else if (is_complex1 && !is_complex2) {
01172             *preal = 2;
01173             x1 = z3;
01174             x2 = z4;
01175             x3 = z1;
01176             x4 = z2;
01177         } else if (is_complex1 && is_complex2) {
01178             *preal = 0;
01179 
01180             if (z1 < z3 || (z1 == z3 && z2 <= z4)) {
01181                 x1 = z1;
01182                 x2 = z2;
01183                 x3 = z3;
01184                 x4 = z4;
01185             } else {
01186                 x1 = z3;
01187                 x2 = z4;
01188                 x3 = z1;
01189                 x4 = z2;
01190             }
01191         } else {
01192             *preal = 4;
01193 
01194             if (z3 >= z2) {
01195                 x1 = z1;
01196                 x2 = z2;
01197                 x3 = z3;
01198                 x4 = z4;
01199             } else if (z4 <= z1) {
01200                 x1 = z3;
01201                 x2 = z4;
01202                 x3 = z1;
01203                 x4 = z2;
01204             } else if (z2 > z4) {
01205                 x1 = z3;
01206                 x2 = z1;
01207                 x3 = z4;
01208                 x4 = z2;
01209             } else {
01210                 x1 = z1;
01211                 x2 = z3;
01212                 x3 = z2;
01213                 x4 = z4;
01214             }
01215         }
01216     }
01217 
01218     *px1 = x1;
01219     *px2 = x2;
01220     *px3 = x3;
01221     *px4 = x4;
01222 
01223     return CPL_ERROR_NONE;
01224 }
01225 
01226 #ifdef IPRLIB_POLYNOMIAL_USE_MONOMIAL_ROOT
01227 /*----------------------------------------------------------------------------*/
01235 /*----------------------------------------------------------------------------*/
01236 static double irplib_polynomial_depress_1d(cpl_polynomial * self)
01237 {
01238 
01239     const cpl_size degree = cpl_polynomial_get_degree(self);
01240     const cpl_size nc1    = degree - 1;
01241     const double   an     = cpl_polynomial_get_coeff(self, &degree);
01242     const double   an1    = cpl_polynomial_get_coeff(self, &nc1);
01243     double         rmean;
01244     cpl_size       i;
01245 
01246 
01247     cpl_ensure(degree > 0,   CPL_ERROR_DATA_NOT_FOUND, 0.0);
01248 
01249     assert( an != 0.0 );
01250 
01251     rmean = -an1/(an * (double)degree);
01252 
01253     if (rmean != 0.0) {
01254 
01255         cpl_polynomial_shift_1d(self, 0, rmean);
01256 
01257         cpl_polynomial_set_coeff(self, &nc1, 0.0); /* Round-off... */
01258 
01259     }
01260 
01261     /* Set leading coefficient to one. */
01262     for (i = 0; i < degree-1; i++) {
01263         const double ai = cpl_polynomial_get_coeff(self, &i) / an;
01264         cpl_polynomial_set_coeff(self, &i, ai);
01265     }
01266 
01267     cpl_polynomial_set_coeff(self, &degree, 1.0); /* Round-off... */
01268 
01269     return rmean;
01270 }
01271 #endif
01272 
01273 /*----------------------------------------------------------------------------*/
01288 /*----------------------------------------------------------------------------*/
01289 static
01290 cpl_error_code irplib_polynomial_divide_1d_root(cpl_polynomial * p, double r,
01291                                                 double * pres)
01292 {
01293 
01294     const cpl_size n = cpl_polynomial_get_degree(p);
01295     double         sum;
01296     cpl_size       i;
01297 
01298 
01299     cpl_ensure_code(p != NULL, CPL_ERROR_NULL_INPUT);
01300     cpl_ensure_code(cpl_polynomial_get_dimension(p) == 1,
01301                     CPL_ERROR_INVALID_TYPE);
01302     cpl_ensure_code(n > 0, CPL_ERROR_DATA_NOT_FOUND);
01303 
01304     sum = cpl_polynomial_get_coeff(p, &n);
01305     cpl_polynomial_set_coeff(p, &n, 0.0);
01306 
01307     for (i = n-1; i >= 0; i--) {
01308         const double coeff = cpl_polynomial_get_coeff(p, &i);
01309 
01310         cpl_polynomial_set_coeff(p, &i, sum);
01311 
01312         sum = coeff + r * sum;
01313 
01314     }
01315 
01316     if (pres != NULL) *pres = sum;
01317 
01318     return CPL_ERROR_NONE;
01319 }
Generated on Mon Feb 17 15:01:44 2014 for NACO Pipeline Reference Manual by  doxygen 1.6.3