Rpoly.cpp
Go to the documentation of this file.
00001 #include "Rpoly.h"
00002 
00003 #include <iostream>
00004 #include <fstream>
00005 #include <cctype>
00006 #include <cmath>
00007 #include <cfloat>
00008 
00009 using namespace std;
00010 
00011 void rpoly_ak1(double op[MDP1], int* Degree, double zeror[MAXDEGREE], double zeroi[MAXDEGREE]){
00012     int i, j, jj, l, N, NM1, NN, NZ, zerok;
00013 
00014     double K[MDP1], p[MDP1], pt[MDP1], qp[MDP1], temp[MDP1];
00015     double bnd, df, dx, factor, ff, moduli_max, moduli_min, sc, x, xm;
00016     double aa, bb, cc, lzi, lzr, sr, szi, szr, t, u, xx, xxx, yy;
00017 
00018     const double RADFAC = 3.14159265358979323846/180; // Degrees-to-radians conversion factor = pi/180
00019     const double lb2 = log(2.0); // Dummy variable to avoid re-calculating this value in loop below
00020     const double lo = DBL_MIN/DBL_EPSILON;
00021     const double cosr = cos(94.0*RADFAC); // = -0.069756474
00022     const double sinr = sin(94.0*RADFAC); // = 0.99756405
00023 
00024     if ((*Degree) > MAXDEGREE){
00025         cout << "\nThe entered Degree is greater than MAXDEGREE. Exiting rpoly. No further action taken.\n";
00026         *Degree = -1;
00027         return;
00028     } // End ((*Degree) > MAXDEGREE)
00029 
00030     //Do a quick check to see if leading coefficient is 0
00031     if (op[0] != 0){
00032 
00033         N = *Degree;
00034         xx = sqrt(0.5); // = 0.70710678
00035         yy = -xx;
00036 
00037         // Remove zeros at the origin, if any
00038         j = 0;
00039         while (op[N] == 0){
00040             zeror[j] = zeroi[j] = 0.0;
00041             N--;
00042             j++;
00043         } // End while (op[N] == 0)
00044 
00045         NN = N + 1;
00046 
00047         // Make a copy of the coefficients
00048         for (i = 0; i < NN; i++)
00049             p[i] = op[i];
00050 
00051         while (N >= 1){ // Main loop
00052             // Start the algorithm for one zero
00053             if (N <= 2){
00054             // Calculate the final zero or pair of zeros
00055                 if (N < 2){
00056                     zeror[(*Degree) - 1] = -(p[1]/p[0]);
00057                     zeroi[(*Degree) - 1] = 0.0;
00058                 } // End if (N < 2)
00059                 else { // else N == 2
00060                     Quad_ak1(p[0], p[1], p[2], &zeror[(*Degree) - 2], &zeroi[(*Degree) - 2], &zeror[(*Degree) - 1], &zeroi[(*Degree) - 1]);
00061                 } // End else N == 2
00062                 break;
00063             } // End if (N <= 2)
00064 
00065             // Find the largest and smallest moduli of the coefficients
00066 
00067             moduli_max = 0.0;
00068             moduli_min = DBL_MAX;
00069 
00070             for (i = 0; i < NN; i++){
00071                 x = fabs(p[i]);
00072                 if (x > moduli_max)   moduli_max = x;
00073                 if ((x != 0) && (x < moduli_min))   moduli_min = x;
00074             } // End for i
00075 
00076             // Scale if there are large or very small coefficients
00077             // Computes a scale factor to multiply the coefficients of the polynomial. The scaling
00078             // is done to avoid overflow and to avoid undetected underflow interfering with the
00079             // convergence criterion.
00080             // The factor is a power of the base.
00081 
00082             sc = lo/moduli_min;
00083 
00084             if (((sc <= 1.0) && (moduli_max >= 10)) || ((sc > 1.0) && (DBL_MAX/sc >= moduli_max))){
00085                 sc = ((sc == 0) ? DBL_MIN : sc);
00086                 l = (int)(log(sc)/lb2 + 0.5);
00087                 factor = pow(2.0, l);
00088                 if (factor != 1.0){
00089                     for (i = 0; i < NN; i++)   p[i] *= factor;
00090                 } // End if (factor != 1.0)
00091             } // End if (((sc <= 1.0) && (moduli_max >= 10)) || ((sc > 1.0) && (DBL_MAX/sc >= moduli_max)))
00092 
00093             // Compute lower bound on moduli of zeros
00094 
00095             for (i = 0; i < NN; i++)   pt[i] = fabs(p[i]);
00096             pt[N] = -(pt[N]);
00097 
00098             NM1 = N - 1;
00099 
00100             // Compute upper estimate of bound
00101 
00102             x = exp((log(-pt[N]) - log(pt[0]))/(double)N);
00103 
00104             if (pt[NM1] != 0) {
00105                 // If Newton step at the origin is better, use it
00106                 xm = -pt[N]/pt[NM1];
00107                 x = ((xm < x) ? xm : x);
00108             } // End if (pt[NM1] != 0)
00109 
00110             // Chop the interval (0, x) until ff <= 0
00111 
00112             xm = x;
00113             do {
00114                 x = xm;
00115                 xm = 0.1*x;
00116                 ff = pt[0];
00117                 for (i = 1; i < NN; i++)   ff = ff *xm + pt[i];
00118             } while (ff > 0); // End do-while loop
00119 
00120             dx = x;
00121 
00122             // Do Newton iteration until x converges to two decimal places
00123 
00124             do {
00125                 df = ff = pt[0];
00126                 for (i = 1; i < N; i++){
00127                     ff = x*ff + pt[i];
00128                     df = x*df + ff;
00129                 } // End for i
00130                 ff = x*ff + pt[N];
00131                 dx = ff/df;
00132                 x -= dx;
00133             } while (fabs(dx/x) > 0.005); // End do-while loop
00134 
00135             bnd = x;
00136 
00137             // Compute the derivative as the initial K polynomial and do 5 steps with no shift
00138 
00139             for (i = 1; i < N; i++)   K[i] = (double)(N - i)*p[i]/((double)N);
00140             K[0] = p[0];
00141 
00142             aa = p[N];
00143             bb = p[NM1];
00144             zerok = ((K[NM1] == 0) ? 1 : 0);
00145 
00146             for (jj = 0; jj < 5; jj++) {
00147                 cc = K[NM1];
00148                 if (zerok){
00149                     // Use unscaled form of recurrence
00150                     for (i = 0; i < NM1; i++){
00151                         j = NM1 - i;
00152                         K[j] = K[j - 1];
00153                     } // End for i
00154                     K[0] = 0;
00155                    zerok = ((K[NM1] == 0) ? 1 : 0);
00156                 } // End if (zerok)
00157 
00158                 else { // else !zerok
00159                     // Used scaled form of recurrence if value of K at 0 is nonzero
00160                     t = -aa/cc;
00161                     for (i = 0; i < NM1; i++){
00162                         j = NM1 - i;
00163                         K[j] = t*K[j - 1] + p[j];
00164                     } // End for i
00165                     K[0] = p[0];
00166                     zerok = ((fabs(K[NM1]) <= fabs(bb)*DBL_EPSILON*10.0) ? 1 : 0);
00167                 } // End else !zerok
00168 
00169             } // End for jj
00170 
00171             // Save K for restarts with new shifts
00172             for (i = 0; i < N; i++)   temp[i] = K[i];
00173 
00174             // Loop to select the quadratic corresponding to each new shift
00175 
00176             for (jj = 1; jj <= 20; jj++){
00177 
00178                 // Quadratic corresponds to a double shift to a non-real point and its
00179                 // complex conjugate. The point has modulus BND and amplitude rotated
00180                 // by 94 degrees from the previous shift.
00181 
00182                 xxx = -(sinr*yy) + cosr*xx;
00183                 yy = sinr*xx + cosr*yy;
00184                 xx = xxx;
00185                 sr = bnd*xx;
00186                 u = -(2.0*sr);
00187 
00188                 // Second stage calculation, fixed quadratic
00189 
00190                 Fxshfr_ak1(20*jj, &NZ, sr, bnd, K, N, p, NN, qp, u, &lzi, &lzr, &szi, &szr);
00191 
00192                 if (NZ != 0){
00193 
00194                     // The second stage jumps directly to one of the third stage iterations and
00195                     // returns here if successful. Deflate the polynomial, store the zero or
00196                     // zeros, and return to the main algorithm.
00197 
00198                     j = (*Degree) - N;
00199                     zeror[j] = szr;
00200                     zeroi[j] = szi;
00201                     NN = NN - NZ;
00202                     N = NN - 1;
00203                     for (i = 0; i < NN; i++)   p[i] = qp[i];
00204                     if (NZ != 1){
00205                         zeror[j + 1] = lzr;
00206                         zeroi[j + 1] = lzi;
00207                     } // End if (NZ != 1)
00208                     break;
00209                 } // End if (NZ != 0)
00210                 else { // Else (NZ == 0)
00211 
00212                     // If the iteration is unsuccessful, another quadratic is chosen after restoring K
00213                     for (i = 0; i < N; i++)   K[i] = temp[i];
00214                 } // End else (NZ == 0)
00215 
00216             } // End for jj
00217 
00218             // Return with failure if no convergence with 20 shifts
00219 
00220             if (jj > 20) {
00221                 cout << "\nFailure. No convergence after 20 shifts. Program terminated.\n";
00222                 *Degree -= N;
00223                 break;
00224             } // End if (jj > 20)
00225 
00226         } // End while (N >= 1)
00227     } // End if op[0] != 0
00228     else { // else op[0] == 0
00229         cout << "\nThe leading coefficient is zero. No further action taken. Program terminated.\n";
00230         *Degree = 0;
00231     } // End else op[0] == 0
00232 
00233     return;
00234 } // End rpoly_ak1
00235 
00236 void Fxshfr_ak1(int L2, int* NZ, double sr, double v, double K[MDP1], int N, double p[MDP1], int NN, double qp[MDP1], double u, double* lzi, double* lzr, double* szi, double* szr)
00237 {
00238 
00239     // Computes up to L2 fixed shift K-polynomials, testing for convergence in the linear or
00240     // quadratic case. Initiates one of the variable shift iterations and returns with the
00241     // number of zeros found.
00242 
00243     // L2 limit of fixed shift steps
00244     // NZ number of zeros found
00245 
00246     int fflag, i, iFlag = 1, j, spass, stry, tFlag, vpass, vtry;
00247     double a, a1, a3, a7, b, betas, betav, c, d, e, f, g, h, oss, ots=0, otv=0, ovv, s, ss, ts, tss, tv, tvv, ui, vi, vv;
00248     double qk[MDP1], svk[MDP1];
00249 
00250     *NZ = 0;
00251     betav = betas = 0.25;
00252     oss = sr;
00253     ovv = v;
00254 
00255     //Evaluate polynomial by synthetic division
00256     QuadSD_ak1(NN, u, v, p, qp, &a, &b);
00257 
00258     tFlag = calcSC_ak1(N, a, b, &a1, &a3, &a7, &c, &d, &e, &f, &g, &h, K, u, v, qk);
00259 
00260     for (j = 0; j < L2; j++){
00261 
00262         fflag = 1;
00263         //Calculate next K polynomial and estimate v
00264         nextK_ak1(N, tFlag, a, b, a1, &a3, &a7, K, qk, qp);
00265         tFlag = calcSC_ak1(N, a, b, &a1, &a3, &a7, &c, &d, &e, &f, &g, &h, K, u, v, qk);
00266         newest_ak1(tFlag, &ui, &vi, a, a1, a3, a7, b, c, d, f, g, h, u, v, K, N, p);
00267 
00268         vv = vi;
00269 
00270         // Estimate s
00271 
00272         ss = ((K[N - 1] != 0.0) ? -(p[N]/K[N - 1]) : 0.0);
00273 
00274         ts = tv = 1.0;
00275 
00276         if ((j != 0) && (tFlag != 3)){
00277 
00278            // Compute relative measures of convergence of s and v sequences
00279 
00280             tv = ((vv != 0.0) ? fabs((vv - ovv)/vv) : tv);
00281             ts = ((ss != 0.0) ? fabs((ss - oss)/ss) : ts);
00282 
00283             // If decreasing, multiply the two most recent convergence measures
00284 
00285             tvv = ((tv < otv) ? tv*otv : 1.0);
00286             tss = ((ts < ots) ? ts*ots : 1.0);
00287 
00288             // Compare with convergence criteria
00289 
00290             vpass = ((tvv < betav) ? 1 : 0);
00291             spass = ((tss < betas) ? 1 : 0);
00292 
00293             if ((spass) || (vpass)){
00294 
00295                 // At least one sequence has passed the convergence test.
00296                 // Store variables before iterating
00297 
00298                 for (i = 0; i < N; i++)   svk[i] = K[i];
00299 
00300                 s = ss;
00301 
00302                 // Choose iteration according to the fastest converging sequence
00303 
00304                 stry = vtry = 0;
00305 
00306                 for ( ; ; ) {
00307 
00308                     if ((fflag && ((fflag = 0) == 0)) && ((spass) && (!vpass || (tss < tvv)))){
00309                         ; // Do nothing. Provides a quick "short circuit".
00310                     } // End if (fflag)
00311 
00312                     else { // else !fflag
00313                         QuadIT_ak1(N, NZ, ui, vi, szr, szi, lzr, lzi, qp, NN, &a, &b, p, qk, &a1, &a3, &a7, &c, &d, &e, &f, &g, &h, K);
00314 
00315                         if ((*NZ) > 0)   return;
00316 
00317                         // Quadratic iteration has failed. Flag that it has been tried and decrease the
00318                         // convergence criterion
00319 
00320                         iFlag = vtry = 1;
00321                         betav *= 0.25;
00322 
00323                         // Try linear iteration if it has not been tried and the s sequence is converging
00324                         if (stry || (!spass)){
00325                             iFlag = 0;
00326                         } // End if (stry || (!spass))
00327                         else {
00328                             for (i = 0; i < N; i++)   K[i] = svk[i];
00329                         } // End if (stry || !spass)
00330 
00331                     } // End else fflag
00332 
00333                     if (iFlag != 0){
00334                         RealIT_ak1(&iFlag, NZ, &s, N, p, NN, qp, szr, szi, K, qk);
00335 
00336                         if ((*NZ) > 0)   return;
00337 
00338                         // Linear iteration has failed. Flag that it has been tried and decrease the
00339                         // convergence criterion
00340 
00341                         stry = 1;
00342                         betas *= 0.25;
00343 
00344                         if (iFlag != 0){
00345 
00346                             // If linear iteration signals an almost double real zero, attempt quadratic iteration
00347 
00348                             ui = -(s + s);
00349                             vi = s*s;
00350                             continue;
00351 
00352                         } // End if (iFlag != 0)
00353                     } // End if (iFlag != 0)
00354 
00355                     // Restore variables
00356                     for (i = 0; i < N; i++)   K[i] = svk[i];
00357 
00358                     // Try quadratic iteration if it has not been tried and the v sequence is converging
00359 
00360                     if (!vpass || vtry)   break; // Break out of infinite for loop
00361 
00362                 } // End infinite for loop
00363 
00364                 // Re-compute qp and scalar values to continue the second stage
00365 
00366                 QuadSD_ak1(NN, u, v, p, qp, &a, &b);
00367                 tFlag = calcSC_ak1(N, a, b, &a1, &a3, &a7, &c, &d, &e, &f, &g, &h, K, u, v, qk);
00368 
00369             } // End if ((spass) || (vpass))
00370 
00371         } // End if ((j != 0) && (tFlag != 3))
00372 
00373         ovv = vv;
00374         oss = ss;
00375         otv = tv;
00376         ots = ts;
00377     } // End for j
00378 
00379     return;
00380 } // End Fxshfr_ak1
00381 
00382 void QuadSD_ak1(int NN, double u, double v, double p[MDP1], double q[MDP1], double* a, double* b)
00383 {
00384     // Divides p by the quadratic 1, u, v placing the quotient in q and the remainder in a, b
00385 
00386     int i;
00387 
00388     q[0] = *b = p[0];
00389     q[1] = *a = -((*b)*u) + p[1];
00390 
00391     for (i = 2; i < NN; i++){
00392         q[i] = -((*a)*u + (*b)*v) + p[i];
00393         *b = (*a);
00394         *a = q[i];
00395     } // End for i
00396 
00397     return;
00398 } // End QuadSD_ak1
00399 
00400 int calcSC_ak1(int N, double a, double b, double* a1, double* a3, double* a7, double* c, double* d, double* e, double* f, double* g, double* h, double K[MDP1], double u, double v, double qk[MDP1])
00401 {
00402 
00403     // This routine calculates scalar quantities used to compute the next K polynomial and
00404     // new estimates of the quadratic coefficients.
00405 
00406     // calcSC - integer variable set here indicating how the calculations are normalized
00407     // to avoid overflow.
00408 
00409     int dumFlag = 3; // TYPE = 3 indicates the quadratic is almost a factor of K
00410 
00411     // Synthetic division of K by the quadratic 1, u, v
00412     QuadSD_ak1(N, u, v, K, qk, c, d);
00413 
00414     if (fabs((*c)) <= (100.0*DBL_EPSILON*fabs(K[N - 1]))) {
00415         if (fabs((*d)) <= (100.0*DBL_EPSILON*fabs(K[N - 2])))   return dumFlag;
00416     } // End if (fabs(c) <= (100.0*DBL_EPSILON*fabs(K[N - 1])))
00417 
00418     *h = v*b;
00419     if (fabs((*d)) >= fabs((*c))){
00420         dumFlag = 2; // TYPE = 2 indicates that all formulas are divided by d
00421         *e = a/(*d);
00422         *f = (*c)/(*d);
00423         *g = u*b;
00424         *a3 = (*e)*((*g) + a) + (*h)*(b/(*d));
00425         *a1 = -a + (*f)*b;
00426         *a7 = (*h) + ((*f) + u)*a;
00427     } // End if(fabs(d) >= fabs(c))
00428     else {
00429         dumFlag = 1; // TYPE = 1 indicates that all formulas are divided by c;
00430         *e = a/(*c);
00431         *f = (*d)/(*c);
00432         *g = (*e)*u;
00433         *a3 = (*e)*a + ((*g) + (*h)/(*c))*b;
00434         *a1 = -(a*((*d)/(*c))) + b;
00435         *a7 = (*g)*(*d) + (*h)*(*f) + a;
00436     } // End else
00437 
00438     return dumFlag;
00439 } // End calcSC_ak1
00440 
00441 void nextK_ak1(int N, int tFlag, double a, double b, double a1, double* a3, double* a7, double K[MDP1], double qk[MDP1], double qp[MDP1])
00442 {
00443     // Computes the next K polynomials using the scalars computed in calcSC_ak1
00444 
00445     int i;
00446     double temp;
00447 
00448     if (tFlag == 3){ // Use unscaled form of the recurrence
00449         K[1] = K[0] = 0.0;
00450 
00451         for (i = 2; i < N; i++)   K[i] = qk[i - 2];
00452 
00453         return;
00454     } // End if (tFlag == 3)
00455 
00456     temp = ((tFlag == 1) ? b : a);
00457 
00458     if (fabs(a1) > (10.0*DBL_EPSILON*fabs(temp))){
00459         // Use scaled form of the recurrence
00460 
00461         (*a7) /= a1;
00462         (*a3) /= a1;
00463         K[0] = qp[0];
00464         K[1] = -((*a7)*qp[0]) + qp[1];
00465 
00466         for (i = 2; i < N; i++)   K[i] = -((*a7)*qp[i - 1]) + (*a3)*qk[i - 2] + qp[i];
00467 
00468     } // End if (fabs(a1) > (10.0*DBL_EPSILON*fabs(temp)))
00469     else {
00470         // If a1 is nearly zero, then use a special form of the recurrence
00471 
00472         K[0] = 0.0;
00473         K[1] = -(*a7)*qp[0];
00474 
00475         for (i = 2; i < N; i++)   K[i] = -((*a7)*qp[i - 1]) + (*a3)*qk[i - 2];
00476     } // End else
00477 
00478     return;
00479 } // End nextK_ak1
00480 
00481 void newest_ak1(int tFlag, double* uu, double* vv, double a, double a1, double a3, double a7, double b, double c, double d, double f, double g, double h, double u, double v, double K[MDP1], int N, double p[MDP1])
00482 {
00483     // Compute new estimates of the quadratic coefficients using the scalars computed in calcSC_ak1
00484 
00485     double a4, a5, b1, b2, c1, c2, c3, c4, temp;
00486 
00487     (*vv) = (*uu) = 0.0; // The quadratic is zeroed
00488 
00489     if (tFlag != 3){
00490 
00491         if (tFlag != 2){
00492             a4 = a + u*b + h*f;
00493             a5 = c + (u + v*f)*d;
00494         } // End if (tFlag != 2)
00495         else { // else tFlag == 2
00496             a4 = (a + g)*f + h;
00497             a5 = (f + u)*c + v*d;
00498         } // End else tFlag == 2
00499 
00500         // Evaluate new quadratic coefficients
00501 
00502         b1 = -K[N - 1]/p[N];
00503         b2 = -(K[N - 2] + b1*p[N - 1])/p[N];
00504         c1 = v*b2*a1;
00505         c2 = b1*a7;
00506         c3 = b1*b1*a3;
00507         c4 = -(c2 + c3) + c1;
00508         temp = -c4 + a5 + b1*a4;
00509         if (temp != 0.0) {
00510             *uu= -((u*(c3 + c2) + v*(b1*a1 + b2*a7))/temp) + u;
00511             *vv = v*(1.0 + c4/temp);
00512         } // End if (temp != 0)
00513 
00514     } // End if (tFlag != 3)
00515 
00516     return;
00517 } // End newest_ak1
00518 
00519 void QuadIT_ak1(int N, int* NZ, double uu, double vv, double* szr, double* szi, double* lzr, double* lzi, double qp[MDP1], int NN, double* a, double* b, double p[MDP1], double qk[MDP1], double* a1, double* a3, double* a7, double* c, double* d, double* e, double* f, double* g, double* h, double K[MDP1])
00520 {
00521     // Variable-shift K-polynomial iteration for a quadratic factor converges only if the
00522     // zeros are equimodular or nearly so.
00523 
00524     int i, j = 0, tFlag, triedFlag = 0;
00525     double ee, mp, omp, relstp=0, t, u, ui, v, vi, zm;
00526 
00527     *NZ = 0; // Number of zeros found
00528     u = uu; // uu and vv are coefficients of the starting quadratic
00529     v = vv;
00530 
00531     do {
00532         Quad_ak1(1.0, u, v, szr, szi, lzr, lzi);
00533 
00534         // Return if roots of the quadratic are real and not close to multiple or nearly
00535         // equal and of opposite sign.
00536 
00537         if (fabs(fabs(*szr) - fabs(*lzr)) > 0.01*fabs(*lzr))   break;
00538 
00539         // Evaluate polynomial by quadratic synthetic division
00540 
00541         QuadSD_ak1(NN, u, v, p, qp, a, b);
00542 
00543         mp = fabs(-((*szr)*(*b)) + (*a)) + fabs((*szi)*(*b));
00544 
00545         // Compute a rigorous bound on the rounding error in evaluating p
00546 
00547         zm = sqrt(fabs(v));
00548         ee = 2.0*fabs(qp[0]);
00549         t = -((*szr)*(*b));
00550 
00551         for (i = 1; i < N; i++)   ee = ee*zm + fabs(qp[i]);
00552 
00553         ee = ee*zm + fabs((*a) + t);
00554         ee = (9.0*ee + 2.0*fabs(t) - 7.0*(fabs((*a) + t) + zm*fabs((*b))))*DBL_EPSILON;
00555 
00556         // Iteration has converged sufficiently if the polynomial value is less than 20 times this bound
00557 
00558         if (mp <= 20.0*ee){
00559             *NZ = 2;
00560             break;
00561         } // End if (mp <= 20.0*ee)
00562 
00563         j++;
00564 
00565         // Stop iteration after 20 steps
00566         if (j > 20)   break;
00567 
00568         if (j >= 2){
00569             if ((relstp <= 0.01) && (mp >= omp) && (!triedFlag)){
00570             // A cluster appears to be stalling the convergence. Five fixed shift
00571             // steps are taken with a u, v close to the cluster.
00572 
00573             relstp = ((relstp < DBL_EPSILON) ? sqrt(DBL_EPSILON) : sqrt(relstp));
00574 
00575             u -= u*relstp;
00576             v += v*relstp;
00577 
00578             QuadSD_ak1(NN, u, v, p, qp, a, b);
00579 
00580             for (i = 0; i < 5; i++){
00581                 tFlag = calcSC_ak1(N, *a, *b, a1, a3, a7, c, d, e, f, g, h, K, u, v, qk);
00582                 nextK_ak1(N, tFlag, *a, *b, *a1, a3, a7, K, qk, qp);
00583             } // End for i
00584 
00585             triedFlag = 1;
00586             j = 0;
00587 
00588             } // End if ((relstp <= 0.01) && (mp >= omp) && (!triedFlag))
00589 
00590         } // End if (j >= 2)
00591 
00592         omp = mp;
00593 
00594         // Calculate next K polynomial and new u and v
00595 
00596         tFlag = calcSC_ak1(N, *a, *b, a1, a3, a7, c, d, e, f, g, h, K, u, v, qk);
00597         nextK_ak1(N, tFlag, *a, *b, *a1, a3, a7, K, qk, qp);
00598         tFlag = calcSC_ak1(N, *a, *b, a1, a3, a7, c, d, e, f, g, h, K, u, v, qk);
00599         newest_ak1(tFlag, &ui, &vi, *a, *a1, *a3, *a7, *b, *c, *d, *f, *g, *h, u, v, K, N, p);
00600 
00601         // If vi is zero, the iteration is not converging
00602         if (vi != 0){
00603             relstp = fabs((-v + vi)/vi);
00604             u = ui;
00605             v = vi;
00606         } // End if (vi != 0)
00607     } while (vi != 0); // End do-while loop
00608 
00609     return;
00610 
00611 } //End QuadIT_ak1
00612 
00613 void RealIT_ak1(int* iFlag, int* NZ, double* sss, int N, double p[MDP1], int NN, double qp[MDP1], double* szr, double* szi, double K[MDP1], double qk[MDP1])
00614 {
00615 
00616     // Variable-shift H-polynomial iteration for a real zero
00617 
00618     // sss - starting iterate
00619     // NZ - number of zeros found
00620     // iFlag - flag to indicate a pair of zeros near real axis
00621 
00622     int i, j = 0, nm1 = N - 1;
00623     double ee, kv, mp, ms, omp, pv, s, t=0;
00624 
00625     *iFlag = *NZ = 0;
00626     s = *sss;
00627 
00628     for ( ; ; ) {
00629         pv = p[0];
00630 
00631         // Evaluate p at s
00632         qp[0] = pv;
00633         for (i = 1; i < NN; i++)   qp[i] = pv = pv*s + p[i];
00634 
00635         mp = fabs(pv);
00636 
00637         // Compute a rigorous bound on the error in evaluating p
00638 
00639         ms = fabs(s);
00640         ee = 0.5*fabs(qp[0]);
00641         for (i = 1; i < NN; i++)   ee = ee*ms + fabs(qp[i]);
00642 
00643         // Iteration has converged sufficiently if the polynomial value is less than
00644         // 20 times this bound
00645 
00646         if (mp <= 20.0*DBL_EPSILON*(2.0*ee - mp)){
00647             *NZ = 1;
00648             *szr = s;
00649             *szi = 0.0;
00650             break;
00651         } // End if (mp <= 20.0*DBL_EPSILON*(2.0*ee - mp))
00652 
00653         j++;
00654 
00655         // Stop iteration after 10 steps
00656 
00657         if (j > 10)   break;
00658 
00659         if (j >= 2){
00660             if ((fabs(t) <= 0.001*fabs(-t + s)) && (mp > omp)){
00661                 // A cluster of zeros near the real axis has been encountered;
00662                 // Return with iFlag set to initiate a quadratic iteration
00663 
00664                 *iFlag = 1;
00665                 *sss = s;
00666                 break;
00667             } // End if ((fabs(t) <= 0.001*fabs(s - t)) && (mp > omp))
00668 
00669         } //End if (j >= 2)
00670 
00671         // Return if the polynomial value has increased significantly
00672 
00673         omp = mp;
00674 
00675         // Compute t, the next polynomial and the new iterate
00676         qk[0] = kv = K[0];
00677         for (i = 1; i < N; i++)   qk[i] = kv = kv*s + K[i];
00678 
00679         if (fabs(kv) > fabs(K[nm1])*10.0*DBL_EPSILON){
00680             // Use the scaled form of the recurrence if the value of K at s is non-zero
00681             t = -(pv/kv);
00682             K[0] = qp[0];
00683             for (i = 1; i < N; i++)   K[i] = t*qk[i - 1] + qp[i];
00684         } // End if (fabs(kv) > fabs(K[nm1])*10.0*DBL_EPSILON)
00685         else { // else (fabs(kv) <= fabs(K[nm1])*10.0*DBL_EPSILON)
00686             // Use unscaled form
00687             K[0] = 0.0;
00688             for (i = 1; i < N; i++)   K[i] = qk[i - 1];
00689         } // End else (fabs(kv) <= fabs(K[nm1])*10.0*DBL_EPSILON)
00690 
00691         kv = K[0];
00692         for (i = 1; i < N; i++)   kv = kv*s + K[i];
00693 
00694         t = ((fabs(kv) > (fabs(K[nm1])*10.0*DBL_EPSILON)) ? -(pv/kv) : 0.0);
00695 
00696         s += t;
00697 
00698     } // End infinite for loop
00699 
00700     return;
00701 
00702 } // End RealIT_ak1
00703 
00704 void Quad_ak1(double a, double b1, double c, double* sr, double* si, double* lr, double* li)
00705 {
00706     // Calculates the zeros of the quadratic a*Z^2 + b1*Z + c
00707     // The quadratic formula, modified to avoid overflow, is used to find the larger zero if the
00708     // zeros are real and both zeros are complex. The smaller real zero is found directly from
00709     // the product of the zeros c/a.
00710 
00711     double b, d, e;
00712 
00713     *sr = *si = *lr = *li = 0.0;
00714 
00715     if (a == 0) {
00716         *sr = ((b1 != 0) ? -(c/b1) : *sr);
00717         return;
00718     } // End if (a == 0))
00719 
00720     if (c == 0){
00721         *lr = -(b1/a);
00722         return;
00723     } // End if (c == 0)
00724 
00725     // Compute discriminant avoiding overflow
00726 
00727     b = b1/2.0;
00728     if (fabs(b) < fabs(c)){
00729         e = ((c >= 0) ? a : -a);
00730         e = -e + b*(b/fabs(c));
00731         d = sqrt(fabs(e))*sqrt(fabs(c));
00732     } // End if (fabs(b) < fabs(c))
00733     else { // Else (fabs(b) >= fabs(c))
00734         e = -((a/b)*(c/b)) + 1.0;
00735         d = sqrt(fabs(e))*(fabs(b));
00736     } // End else (fabs(b) >= fabs(c))
00737 
00738     if (e >= 0) {
00739         // Real zeros
00740 
00741         d = ((b >= 0) ? -d : d);
00742         *lr = (-b + d)/a;
00743         *sr = ((*lr != 0) ? (c/(*lr))/a : *sr);
00744     } // End if (e >= 0)
00745     else { // Else (e < 0)
00746         // Complex conjugate zeros
00747 
00748         *lr = *sr = -(b/a);
00749         *si = fabs(d/a);
00750         *li = -(*si);
00751     } // End else (e < 0)
00752 
00753     return;
00754 } // End Quad_ak1
00755 /*
00756 int main()
00757 {
00758     char rflag = 0; //Readiness flag
00759 
00760 cout << "                                           rpoly_ak1 (14 July 2007)\n";
00761 cout << "=========================================================================== \n";
00762 cout << "This program calculates the roots of a polynomial of real coefficients:\n";
00763 cout << "\nop[0]*x^N + op[1]*x^(N-1) + op[2]*x^(N-2) + . . . + op[N]*x^0 = 0 \n";
00764 cout << "\n--------------------------------------------------------------------------- \n";
00765 cout << "\nThis program can accept polynomials of degree 100 or less, specified by the\n";
00766 cout << "constant MAXDEGREE. If a higher order polynomial is to be input, redefine\n";
00767 cout << "MAXDEGREE and re-compile the program.\n";
00768 cout << "\n--------------------------------------------------------------------------- \n";
00769 cout << "\nAll relevant data for the polynomial whose roots will be sought should have\n";
00770 cout << "been saved beforehand in a file named rpoly_ak1dat.txt.\n";
00771 cout << "rpoly_ak1dat.txt should be in the same folder as the rpoly_ak1 executable. \n";
00772 cout << "--------------------------------------------------------------------------- \n";
00773 cout << "\nThe first entry of this file must be the degree, N, of the polynomial for\n";
00774 cout << "which the roots are to be calculated.\n";
00775 cout << "Entries for the coefficients of the polynomial should follow, starting with\n";
00776 cout << "the coefficient for the highest power of x and working down to the coefficient\n";
00777 cout << "for the x^0 term.\n";
00778 cout << "\nThe data is assumed to be of type double. Variables used within this program\n";
00779 cout << "are type double.\n";
00780 cout << "\n--------------------------------------------------------------------------- \n";
00781 cout << "\nThe output is written to the file rpoly_ak1out.txt.\n";
00782 cout << "\nNote the returned value of the variable Degree.\n";
00783 cout << "If Degree > 0, it specifies the number of zeros found.\n";
00784 cout << "If Degree = 0, the leading coefficient of the input polynomial was 0.\n";
00785 cout << "If Degree = -1, the input value of Degree was greater than MAXDEGREE.\n";
00786 cout << "\n--------------------------------------------------------------------------- \n";
00787 cout << "\nAdditional information is posted at the following URL:\n";
00788 cout << "http://www.akiti.ca/rpoly_ak1_Intro.html\n";
00789 cout << "--------------------------------------------------------------------------- \n";
00790 cout << "\nIs everything ready (are you ready to continue?)? If yes, Enter y. \n";
00791 cout << "Otherwise Enter any other key. \n";
00792 cin >> rflag;
00793 
00794 if (toupper(rflag) == 'Y') {
00795 
00796     int Degree; // The degree of the polynomial to be solved
00797     cout << "Appear to be ready. \n";
00798 
00799     ifstream in("rpoly_ak1dat.txt", ios::in);
00800 
00801     if (!in) {
00802         cout << "Cannot open the input file.\n";
00803         return 0;
00804     }
00805 
00806     in >> Degree; //Input the polynomial degree from the file
00807     if (Degree < 0) {
00808         cout << "Invalid polynomial degree entered. Program terminated. \n";
00809         in.close(); //Close the input file before terminating
00810         return 0;
00811     }
00812 
00813     ofstream out("rpoly_ak1out.txt", ios::out);
00814     if (!out) {
00815         cout << "Cannot open the output file. Program terminated.\n";
00816         in.close(); //Close the input file before terminating
00817         return 0;
00818     }
00819 
00820     double op[MDP1], zeroi[MAXDEGREE], zeror[MAXDEGREE]; // Coefficient vectors
00821     int i; // vector index
00822 
00823     //Input the polynomial coefficients from the file and put them in the op vector
00824     for (i = 0; i < (Degree+1); i++){
00825         in >> op[i];
00826     }//End for i
00827 
00828     in.close(); //Close the input file
00829 
00830     rpoly_ak1(op, &Degree, zeror, zeroi);
00831 
00832     out << "Degree = " << Degree << ".\n";
00833     out << "\n";
00834 
00835     if (Degree <= 0){
00836         cout << "\nReturned from rpoly_ak1 and Degree had a value <= 0.\n";
00837     } // End if (Degree <= 0)
00838     else { // else Degree > 0
00839         out.precision(DBL_DIG);
00840         out << "The roots follow:\n";
00841         out << "\n";
00842         for (i = 0; i < Degree; i++){
00843             out << zeror[i] << " + " << zeroi[i] << "i" << " \n";
00844         }//End for i
00845     } // End else Degree > 0
00846 
00847     out.close(); // Close the output file
00848 } //End if rflag = 'Y'
00849 else cout << "\nNot ready. Try again when ready with information. \n";
00850 cout << "\nEnter any key to continue. \n";
00851 cin >> rflag;
00852 return 0;
00853 } // End main program.
00854 */


fiducial_pose
Author(s): Jim Vaughan
autogenerated on Thu Jun 6 2019 18:08:11