Image Component Library (ICL)
|
00001 /******************************************************************** 00002 ** Image Component Library (ICL) ** 00003 ** ** 00004 ** Copyright (C) 2006-2013 CITEC, University of Bielefeld ** 00005 ** Neuroinformatics Group ** 00006 ** Website: www.iclcv.org and ** 00007 ** http://opensource.cit-ec.de/projects/icl ** 00008 ** ** 00009 ** File : ICLUtils/src/ICLMath/PolynomialSolver.h ** 00010 ** Module : ICLMath ** 00011 ** Authors: Sergius Gaulik ** 00012 ** ** 00013 ** ** 00014 ** GNU LESSER GENERAL PUBLIC LICENSE ** 00015 ** This file may be used under the terms of the GNU Lesser General ** 00016 ** Public License version 3.0 as published by the ** 00017 ** ** 00018 ** Free Software Foundation and appearing in the file LICENSE.LGPL ** 00019 ** included in the packaging of this file. Please review the ** 00020 ** following information to ensure the license requirements will ** 00021 ** be met: http://www.gnu.org/licenses/lgpl-3.0.txt ** 00022 ** ** 00023 ** The development of this software was supported by the ** 00024 ** Excellence Cluster EXC 277 Cognitive Interaction Technology. ** 00025 ** The Excellence Cluster EXC 277 is a grant of the Deutsche ** 00026 ** Forschungsgemeinschaft (DFG) in the context of the German ** 00027 ** Excellence Initiative. ** 00028 ** ** 00029 ********************************************************************/ 00030 00031 #pragma once 00032 00033 #include <ICLUtils/BasicTypes.h> 00034 #include <float.h> 00035 00036 #if (defined _MSC_VER && _MSC_VER < 1800) 00037 #pragma WARNING("This compiler does not support the functions ilogb and scalbln.") 00038 #else 00039 00040 namespace icl{ 00041 namespace math{ 00042 00043 #ifdef WIN32 00044 typedef std::complex<double> xcomplex; 00045 #define __real__(C) (C)._Val[0] 00046 #define __imag__(C) (C)._Val[1] 00047 #else 00048 typedef _Complex double xcomplex; 00049 #endif 00050 00051 /**************************************************************************** 00052 * 00053 * cpoly.C René Hartung 00054 * Laurent Bartholdi 00055 * 00056 * @(#)$id: fr_dll.c,v 1.18 2010/10/26 05:19:40 gap exp $ 00057 * 00058 * Copyright (c) 2011, Laurent Bartholdi 00059 * 00060 **************************************************************************** 00061 * 00062 * template for root-finding of complex polynomial 00063 * 00064 * The ICL developers applied only minor changes to this file 00065 ****************************************************************************/ 00066 00067 typedef double xreal; 00068 static const struct { double ZERO, INFIN; int MIN_EXP, MAX_EXP; } 00069 xdata = { 0.0, DBL_MAX, DBL_MIN_EXP, DBL_MAX_EXP }; 00070 static xreal xnorm(xcomplex z) { return __real__(z)*__real__(z) + __imag__(z)*__imag__(z); } 00071 static xreal xabs(xcomplex z) { return sqrt(xnorm(z)); } 00072 static xreal xroot(xreal x, int n) { return pow(x, 1.0 / n); } 00073 static int xlogb(xcomplex z) { return ilogb(xnorm(z)) / 2; } 00074 #define xbits(z) DBL_MANT_DIG 00075 #define xeta(z) DBL_EPSILON 00076 static void xscalbln(xcomplex *z, int e) { 00077 __real__(*z) = scalbln(__real__(*z), e); 00078 __imag__(*z) = scalbln(__imag__(*z), e); 00079 } 00080 00081 // CAUCHY COMPUTES A LOWER BOUND ON THE MODULI OF THE ZEROS OF A 00082 // POLYNOMIAL - PT IS THE MODULUS OF THE COEFFICIENTS. 00083 // 00084 static xcomplex cauchy(const int deg, xcomplex *P) 00085 { 00086 xreal x, xm, f, dx, df; 00087 xreal *tmp = new xreal[deg + 1]; 00088 00089 for(int i = 0; i<=deg; i++){ tmp[i] = xabs(P[i]); }; 00090 00091 // Compute upper estimate bound 00092 x = xroot(tmp[deg],deg) / xroot(tmp[0],deg); 00093 if(tmp[deg - 1] != 0.0) { 00094 // Newton step at the origin is better, use it 00095 xm = tmp[deg] / tmp[deg-1]; 00096 if (xm < x) x = xm; 00097 } 00098 00099 tmp[deg] = -tmp[deg]; 00100 00101 // Chop the interval (0,x) until f < 0 00102 while(1) { 00103 xm = x * 0.1; 00104 // Evaluate the polynomial <tmp> at <xm> 00105 f = tmp[0]; 00106 for(int i = 1; i <= deg; i++) 00107 f = f * xm + tmp[i]; 00108 00109 if(f <= 0.0) break; 00110 x = xm; 00111 } 00112 dx = x; 00113 00114 // Do Newton iteration until x converges to two decimal places 00115 while(fabs(dx / x) > 0.005) { 00116 f = tmp[0]; 00117 df = 0.0; 00118 for(int i = 1; i <= deg; i++){ 00119 df = df * x + f; 00120 f = f * x + tmp[i]; 00121 } 00122 00123 dx = f / df; 00124 x -= dx; // Newton step 00125 } 00126 00127 delete tmp; 00128 00129 return (xcomplex)(x); 00130 } 00131 00132 // RETURNS A SCALE FACTOR TO MULTIPLY THE COEFFICIENTS OF THE POLYNOMIAL. 00133 // THE SCALING IS DONE TO AVOID OVERFLOW AND TO AVOID UNDETECTED UNDERFLOW 00134 // INTERFERING WITH THE CONVERGENCE CRITERION. THE FACTOR IS A POWER OF THE 00135 //int BASE. 00136 // PT - MODULUS OF COEFFICIENTS OF P 00137 // ETA, INFIN, SMALNO, BASE - CONSTANTS DESCRIBING THE FLOATING POINT ARITHMETIC. 00138 // 00139 static void scale(const int deg, xcomplex* P) 00140 { 00141 int hi, lo, max, min, x, sc; 00142 00143 // Find largest and smallest moduli of coefficients 00144 hi = (int)(xdata.MAX_EXP / 2.0); 00145 lo = (int)(xdata.MIN_EXP - xbits(P[0])); 00146 max = xlogb(P[0]); // leading coefficient does not vanish! 00147 min = xlogb(P[0]); 00148 00149 for(int i = 0; i <= deg; i++) { 00150 if (P[i] != xdata.ZERO){ 00151 x = xlogb(P[i]); 00152 if(x > max) max = x; 00153 if(x < min) min = x; 00154 } 00155 } 00156 00157 // Scale only if there are very large or very small components 00158 if(min >= lo && max <= hi) return; 00159 00160 x = lo - min; 00161 if(x <= 0) 00162 sc = -(max+min) / 2; 00163 else { 00164 sc = x; 00165 if(xdata.MAX_EXP - sc > max) sc = 0; 00166 } 00167 00168 // Scale the polynomial 00169 for(int i = 0; i<= deg; i++){ xscalbln(&P[i],sc); } 00170 } 00171 00172 // COMPUTES THE DERIVATIVE POLYNOMIAL AS THE INITIAL H 00173 // POLYNOMIAL AND COMPUTES L1 NO-SHIFT H POLYNOMIALS. 00174 // 00175 static void noshft(const int l1, int deg, xcomplex *P, xcomplex *H) 00176 { 00177 int i, j, jj; 00178 xcomplex t, tmp; 00179 00180 // compute the first H-polynomial as the (normed) derivative of P 00181 for (i = 0; i < deg; i++) { 00182 tmp *= (deg - i); 00183 tmp /= deg; 00184 H[i] = tmp; 00185 } 00186 00187 for(jj = 1; jj <= l1; jj++) { 00188 if(xnorm(H[deg - 1]) > xeta(P[deg-1])*xeta(P[deg-1])* 10*10 * xnorm(P[deg - 1])) { 00189 t = -P[deg] / H[deg-1]; 00190 for(i = 0; i < deg-1; i++){ 00191 j = deg - i - 1; 00192 H[j] = t * H[j-1] + P[j]; 00193 } 00194 H[0] = P[0]; 00195 } else { 00196 // if the constant term is essentially zero, shift H coefficients 00197 for(i = 0; i < deg-1; i++) { 00198 j = deg - i - 1; 00199 H[j] = H[j - 1]; 00200 } 00201 H[0] = xdata.ZERO; 00202 } 00203 } 00204 } 00205 00206 // EVALUATES A POLYNOMIAL P AT S BY THE HORNER RECURRENCE 00207 // PLACING THE PARTIAL SUMS IN Q AND THE COMPUTED VALUE IN PV. 00208 // 00209 static xcomplex polyev(const int deg, const xcomplex s, const xcomplex *Q, xcomplex *q) { 00210 q[0] = Q[0]; 00211 for(int i = 1; i <= deg; i++){ q[i] = q[i-1] * s + Q[i]; }; 00212 return q[deg]; 00213 } 00214 00215 // COMPUTES T = -P(S)/H(S). 00216 // BOOL - LOGICAL, SET TRUE IF H(S) IS ESSENTIALLY ZERO. 00217 // 00218 static xcomplex calct(bool *bol, int deg, xcomplex Ps, xcomplex *H, xcomplex *h, xcomplex s){ 00219 xcomplex Hs; 00220 Hs = polyev(deg-1, s, H, h); 00221 *bol = xnorm(Hs) <= xeta(H[deg-1])*xeta(H[deg-1]) * 10*10 * xnorm(H[deg-1]); 00222 if(!*bol) 00223 return -Ps / Hs; 00224 else 00225 return xdata.ZERO; 00226 } 00227 00228 // CALCULATES THE NEXT SHIFTED H POLYNOMIAL. 00229 // BOOL - LOGICAL, IF .TRUE. H(S) IS ESSENTIALLY ZERO 00230 // 00231 static void nexth(const bool bol, int deg, xcomplex t, xcomplex *H, xcomplex *h, xcomplex *p){ 00232 if(!bol){ 00233 for(int j = 1; j < deg; j++) 00234 H[j] = t * h[j-1] + p[j]; 00235 H[0] = p[0]; 00236 } 00237 else { 00238 // If h[s] is zero replace H with qh 00239 for(int j = 1; j < deg; j++) 00240 H[j] = h[j - 1]; 00241 h[0] = xdata.ZERO; 00242 } 00243 } 00244 00245 // BOUNDS THE ERROR IN EVALUATING THE POLYNOMIAL BY THE HORNER RECURRENCE. 00246 // QR,QI - THE PARTIAL SUMS 00247 // MS -MODULUS OF THE POINT 00248 // MP -MODULUS OF POLYNOMIAL VALUE 00249 // ARE, MRE -ERROR BOUNDS ON COMPLEX ADDITION AND MULTIPLICATION 00250 // 00251 static xreal errev(const int deg, const xcomplex *p, const xreal ms, const xreal mp){ 00252 xreal MRE = 2.0 * sqrt(2.0) * xeta(p[0]); 00253 xreal e = xabs(p[0]) * MRE / (xeta(p[0]) + MRE); 00254 00255 for(int i = 0; i <= deg; i++) 00256 e = e * ms + xabs(p[i]); 00257 00258 return e * (xeta(p[0]) + MRE) - MRE * mp; 00259 } 00260 00261 // CARRIES OUT THE THIRD STAGE ITERATION. 00262 // L3 - LIMIT OF STEPS IN STAGE 3. 00263 // ZR,ZI - ON ENTRY CONTAINS THE INITIAL ITERATE, IF THE 00264 // ITERATION CONVERGES IT CONTAINS THE FINAL ITERATE ON EXIT. 00265 // CONV - .TRUE. IF ITERATION CONVERGES 00266 // 00267 static bool vrshft(const int l3, int deg, xcomplex *P, xcomplex *p, xcomplex *H, xcomplex *h, xcomplex *zero, xcomplex *s){ 00268 bool bol, conv, b; 00269 int i, j; 00270 xcomplex Ps, t; 00271 xreal mp, ms, omp = 0.0, relstp = 0.0, tp; 00272 00273 conv = b = false; 00274 *s = *zero; 00275 00276 // Main loop for stage three 00277 for(i = 1; i <= l3; i++) { 00278 // Evaluate P at S and test for convergence 00279 Ps = polyev(deg, *s, P, p); 00280 mp = xabs(Ps); 00281 ms = xabs(*s); 00282 if(mp <= 20 * errev(deg, p, ms, mp)) { 00283 // Polynomial value is smaller in value than a bound on the error 00284 // in evaluating P, terminate the iteration 00285 conv = true; 00286 *zero = *s; 00287 return conv; 00288 } 00289 00290 if(i != 1) { 00291 if(!(b || mp < omp || relstp >= 0.05)){ 00292 // if(!(b || xlogb(mp) < omp || real(relstp) >= 0.05)){ 00293 // Iteration has stalled. Probably a cluster of zeros. Do 5 fixed 00294 // shift steps into the cluster to force one zero to dominate 00295 tp = relstp; 00296 b = true; 00297 if(relstp < xeta(P[0])) tp = xeta(P[0]); 00298 00299 #ifdef WIN32 00300 *s *= 1.0 + xcomplex(1.0, 1.0)*sqrt(tp); 00301 #else 00302 *s *= 1.0 + (1.0 + 1.0i)*sqrt(tp); 00303 #endif 00304 00305 Ps = polyev(deg, *s, P, p); 00306 for(j = 1; j <= 5; j++){ 00307 t = calct(&bol, deg, Ps, H, h, *s); 00308 nexth(bol, deg, t, H, h, p); 00309 } 00310 omp = xdata.INFIN; 00311 goto _20; 00312 } 00313 00314 // Exit if polynomial value increase significantly 00315 if(mp * 0.1 > omp) return conv; 00316 } 00317 00318 omp = mp; 00319 00320 // Calculate next iterate 00321 _20: t = calct(&bol, deg, Ps, H, h, *s); 00322 nexth(bol, deg, t, H, h, p); 00323 t = calct(&bol, deg, Ps, H, h, *s); 00324 if(!bol) { 00325 relstp = xabs(t) / xabs(*s); 00326 *s += t; 00327 } 00328 } // end for 00329 00330 return conv; 00331 } 00332 00333 // COMPUTES L2 FIXED-SHIFT H POLYNOMIALS AND TESTS FOR CONVERGENCE. 00334 // INITIATES A VARIABLE-SHIFT ITERATION AND RETURNS WITH THE 00335 // APPROXIMATE ZERO IF SUCCESSFUL. 00336 // L2 - LIMIT OF FIXED SHIFT STEPS 00337 // ZR,ZI - APPROXIMATE ZERO IF CONV IS .TRUE. 00338 // CONV - LOGICAL INDICATING CONVERGENCE OF STAGE 3 ITERATION 00339 // 00340 static bool fxshft(const int l2, int deg, xcomplex *P, xcomplex *p, xcomplex *H, xcomplex *h, xcomplex *zero, xcomplex *s){ 00341 bool bol, conv; // boolean for convergence of stage 2 00342 bool test, pasd; 00343 xcomplex old_T, old_S, Ps, t; 00344 xcomplex *Tmp = new xcomplex[deg+1]; 00345 00346 Ps = polyev(deg, *s, P, p); 00347 test = true; 00348 pasd = false; 00349 00350 // Calculate first T = -P(S)/H(S) 00351 t = calct(&bol, deg, Ps, H, h, *s); 00352 00353 // Main loop for second stage 00354 for(int j = 1; j <= l2; j++){ 00355 old_T = t; 00356 00357 // Compute the next H Polynomial and new t 00358 nexth(bol, deg, t, H, h, p); 00359 t = calct(&bol, deg, Ps, H, h, *s); 00360 *zero = *s + t; 00361 00362 // Test for convergence unless stage 3 has failed once or this 00363 // is the last H Polynomial 00364 if(!(bol || !test || j == l2)){ 00365 if(xabs(t - old_T) < 0.5 * xabs(*zero)) { 00366 if(pasd) { 00367 // The weak convergence test has been passwed twice, start the third stage 00368 // Iteration, after saving the current H polynomial and shift 00369 for(int i = 0; i < deg; i++) 00370 Tmp[i] = H[i]; 00371 old_S = *s; 00372 00373 conv = vrshft(10, deg, P, p, H, h, zero, s); 00374 if(conv) return conv; 00375 00376 //The iteration failed to converge. Turn off testing and restore h,s,pv and T 00377 test = false; 00378 for(int i = 0; i < deg; i++) 00379 H[i] = Tmp[i]; 00380 *s = old_S; 00381 00382 Ps = polyev(deg, *s, P, p); 00383 t = calct(&bol, deg, Ps, H, h, *s); 00384 continue; 00385 } 00386 pasd = true; 00387 } 00388 else 00389 pasd = false; 00390 } 00391 } 00392 00393 // Attempt an iteration with final H polynomial from second stage 00394 conv = vrshft(10, deg, P, p, H, h, zero, s); 00395 00396 delete Tmp; 00397 00398 return conv; 00399 } 00400 00401 // Main function 00402 // 00403 int cpoly(int degree, const xcomplex poly[], xcomplex Roots[]) 00404 { 00405 #ifdef WIN32 00406 xcomplex PhiDiff = xcomplex(-0.069756473, 0.99756405); 00407 xcomplex PhiRand = xcomplex(1.0, -1.0) /sqrt(2.0); 00408 #else 00409 xcomplex PhiDiff = -0.069756473 + 0.99756405i; 00410 xcomplex PhiRand = (1.0 - 1.0i) / sqrt(2.0); 00411 #endif 00412 xcomplex *P = new xcomplex[degree + 1], *H = new xcomplex[degree + 1]; 00413 xcomplex *h = new xcomplex[degree + 1], *p = new xcomplex[degree + 1]; 00414 xcomplex zero, s, bnd; 00415 unsigned int conv = 0; 00416 00417 while(poly[0] == xdata.ZERO) { 00418 poly++; 00419 degree--; 00420 if (degree < 0) 00421 return -1; 00422 }; 00423 00424 int deg = degree; 00425 00426 // Remove the zeros at the origin if any 00427 while(poly[deg] == xdata.ZERO){ 00428 Roots[degree - deg] = xdata.ZERO; 00429 deg--; 00430 } 00431 00432 if (deg == 0) return degree; 00433 00434 // Make a copy of the coefficients 00435 for(int i = 0; i <= deg; i++) { P[i] = poly[i]; } 00436 00437 scale(deg, P); 00438 00439 search: 00440 00441 if(deg <= 1){ 00442 Roots[degree-1] = - P[1] / P[0]; 00443 return degree; 00444 } 00445 00446 // compute a bound of the moduli of the roots (Newton-Raphson) 00447 bnd = cauchy(deg, P); 00448 00449 // Outer loop to control 2 Major passes with different sequences of shifts 00450 for(int cnt1 = 1; cnt1 <= 2; cnt1++) { 00451 // First stage calculation , no shift 00452 noshft(5, deg, P, H); 00453 00454 // Inner loop to select a shift 00455 for(int cnt2 = 1; cnt2 <= 9; cnt2++) { 00456 // Shift is chosen with modulus bnd and amplitude rotated by 94 degree from the previous shif 00457 PhiRand = PhiDiff * PhiRand; 00458 s = bnd * PhiRand; 00459 00460 // Second stage calculation, fixed shift 00461 conv = fxshft(10 * cnt2, deg, P, p, H, h, &zero, &s); 00462 if(conv) { 00463 // The second stage jumps directly to the third stage iteration 00464 // If successful the zero is stored and the polynomial deflated 00465 Roots[degree - deg] = zero; 00466 00467 // continue with the remaining polynomial 00468 deg--; 00469 for(int i = 0; i <= deg; i++){ P[i] = p[i]; }; 00470 goto search; 00471 } 00472 // if the iteration is unsuccessful another shift is chosen 00473 } 00474 00475 // if 9 shifts fail, the outer loop is repeated with another sequence of shifts 00476 } 00477 00478 delete P; 00479 delete H; 00480 delete h; 00481 delete p; 00482 00483 // The zerofinder has failed on two major passes 00484 // return empty handed with the number of roots found (less than the original degree) 00485 return degree - deg; 00486 } 00487 00488 // Wrapper function: 00489 // This function calls the one made by Laurent Bartholdi 00490 int solve_poly(int degree, const double *in_real, const double *in_imag, double *out_real, double *out_imag) 00491 { 00492 int nroots; 00493 xcomplex *poly = new xcomplex[degree+1]; 00494 xcomplex *roots = new xcomplex[degree+1]; 00495 00496 for (int i = 0; i <= degree; ++i) { 00497 __real__(poly[i]) = in_real[i]; 00498 __imag__(poly[i]) = in_imag[i]; 00499 } 00500 00501 nroots = cpoly(degree, poly, roots); 00502 00503 for (int i = 0; i < nroots; ++i) { 00504 out_real[i] = __real__(roots[i]); 00505 out_imag[i] = __imag__(roots[i]); 00506 } 00507 00508 delete[] poly; 00509 delete[] roots; 00510 00511 return nroots; 00512 } 00513 } 00514 } 00515 00516 #endif