CoreFlows/Models/src/utilitaire_algebre.cxx

   1 #include "utilitaire_algebre.h"
   2 #include <iostream>
   3 #include <cfloat>
   4
   5 using namespace std;
   6
   7 roots_polynoms::roots_polynoms(): smalno(DBL_MIN), infin(DBL_MAX), //Parameters associated to the type double.
   8                                   eta(DBL_EPSILON), base(FLT_RADIX)
   9 {
  10 }
  11
  12 int roots_polynoms::rpoly(double *op, int degree, double *zeror, double *zeroi)
  13 {
  14   double t,aa,bb,cc,*temp,factor,rot;
  15   double *pt;
  16   double lo,max,min,xx,yy,cosr,sinr,xxx,x,sc,bnd;
  17   double xm,ff,df,dx;
  18   int cnt,nz,i,j,jj,l,nm1,zerok;
  19
  20   /*  The following statements set machine constants. */
  21   are = eta;
  22   mre = eta;
  23   lo = smalno/eta;
  24   /*  Initialization of constants for shift rotation. */
  25   xx = sqrt(0.5);
  26   yy = -xx;
  27   rot = 94.0;
  28   rot *= 0.017453293;
  29   cosr = cos(rot);
  30   sinr = sin(rot);
  31   n = degree;
  32   /*  Algorithm fails of the leading coefficient is zero. */
  33   if (fabs(op[0]) <=  smalno) return -1;
  34   /*  Remove the zeros at the origin, if any. */
  35   while (op[n] == 0.0) {
  36     j = degree - n;
  37     zeror[j] = 0.0;
  38     zeroi[j] = 0.0;
  39     n--;
  40   }
  41   if (n < 1) return -1;
  42
  43   /*
  44    *  Allocate memory here
  45    */
  46   temp = new double [degree+1];
  47   pt = new double [degree+1];
  48   p = new double [degree+1];
  49   qp = new double [degree+1];
  50   k = new double [degree+1];
  51   qk = new double [degree+1];
  52   svk = new double [degree+1];
  53   /*  Make a copy of the coefficients. */
  54   for (i=0;i<=n;i++)
  55     p[i] = op[i];
  56   /*  Start the algorithm for one zero. */
  57  _40:
  58   if (n == 1) {
  59     zeror[degree-1] = -p[1]/p[0];
  60     zeroi[degree-1] = 0.0;
  61     n -= 1;
  62     goto _99;
  63   }
  64   /*  Calculate the final zero or pair of zeros. */
  65   if (n == 2) {
  66     quad(p[0],p[1],p[2],&zeror[degree-2],&zeroi[degree-2],
  67          &zeror[degree-1],&zeroi[degree-1]);
  68     n -= 2;
  69     goto _99;
  70   }
  71   /*  Find largest and smallest moduli of coefficients. */
  72   max = 0.0;
  73   min = infin;
  74   for (i=0;i<=n;i++) {
  75     x = fabs(p[i]);
  76     if (x > max) max = x;
  77     if (x != 0.0 && x < min){ min = x;
  78     //cerr << " x= " << x << " smalno " << smalno << endl;
  79     }
  80   }
  81   /*  Scale if there are large or very small coefficients.
  82    *  Computes a scale factor to multiply the coefficients of the
  83    *  polynomial. The scaling si done to avoid overflow and to
  84    *  avoid undetected underflow interfering with the convergence
  85    *  criterion. The factor is a power of the base.
  86    */
  87   sc = lo/min;
  88   if (sc > 1.0 && infin/sc < max) goto _110;
  89   if (sc <= 1.0) {
  90     if (max < 10.0) goto _110;
  91     if (sc == 0.0)
  92       sc = smalno;
  93   }
  94   l = (int)(log(sc)/log(base) + 0.5);
  95   factor = pow(base*1.0,l);
  96   if (factor != 1.0) {
  97     for (i=0;i<=n;i++)
  98       p[i] = factor*p[i];     /* Scale polynomial. */
  99   }
 100  _110:
 101   /*  Compute lower bound on moduli of roots. */
 102   for (i=0;i<=n;i++) {
 103     pt[i] = (fabs(p[i]));
 104   }
 105   pt[n] = - pt[n];
 106   /*  Compute upper estimate of bound. */
 107   x = exp((log(-pt[n])-log(pt[0])) / (double)n);
 108   /*  If Newton step at the origin is better, use it. */
 109   //if (pt[n-1]  !=  0.0) {
 110   if (fabs(pt[n-1] ) >  smalno*fabs(pt[n])) {
 111     xm = -pt[n]/pt[n-1];
 112     if (xm < x)  x = xm;
 113   }
 114   /*  Chop the interval (0,x) until ff <= 0 */
 115   while (1) {
 116     xm = x*0.1;
 117     ff = pt[0];
 118     for (i=1;i<=n;i++)
 119       ff = ff*xm + pt[i];
 120     if (ff <= 0.0) break;
 121     x = xm;
 122   }
 123   dx = x;
 124   /*  Do Newton interation until x converges to two
 125    *  decimal places.
 126    */
 127   while (fabs(dx/x) > 0.005) {
 128     ff = pt[0];
 129     df = ff;
 130     for (i=1;i<n;i++) {
 131       ff = ff*x + pt[i];
 132       df = df*x + ff;
 133     }
 134     ff = ff*x + pt[n];
 135     dx = ff/df;
 136     x -= dx;
 137   }
 138   bnd = x;
 139   /*  Compute the derivative as the initial k polynomial
 140    *  and do 5 steps with no shift.
 141    */
 142   nm1 = n - 1;
 143   for (i=1;i<n;i++)
 144     k[i] = (double)(n-i)*p[i]/(double)n;
 145   k[0] = p[0];
 146   aa = p[n];
 147   bb = p[n-1];
 148   zerok = (fabs(k[n-1])< smalno);
 149   for(jj=0;jj<5;jj++) {
 150     cc = k[n-1];
 151     if (!zerok) {//cerr << "cc= " << cc << endl;
 152       /*  Use a scaled form of recurrence if value of k at 0 is nonzero. */
 153       t = -aa/cc;
 154       for (i=0;i<nm1;i++) {
 155         j = n-i-1;
 156         k[j] = t*k[j-1]+p[j];
 157       }
 158       k[0] = p[0];
 159       zerok = (fabs(k[n-1]) <= fabs(bb)*eta*10.0 || (fabs(k[n-1])< smalno));
 160     }
 161     else {
 162       /*  Use unscaled form of recurrence. */
 163       for (i=0;i<nm1;i++) {
 164         j = n-i-1;
 165         k[j] = k[j-1];
 166       }
 167       k[0] = 0.0;
 168       zerok = (fabs(k[n-1])< smalno);
 169     }
 170   }
 171   /*  Save k for restarts with new shifts. */
 172   for (i=0;i<n;i++)
 173     temp[i] = k[i];
 174   /*  Loop to select the quadratic corresponding to each new shift. */
 175   for (cnt = 0;cnt < 20;cnt++) {
 176     /*  Quadratic corresponds to a double shift to a
 177      *  non-real point and its complex conjugate. The point
 178      *  has modulus bnd and amplitude rotated by 94 degrees
 179      *  from the previous shift.
 180      */
 181     xxx = cosr*xx - sinr*yy;
 182     yy = sinr*xx + cosr*yy;
 183     xx = xxx;
 184     sr = bnd*xx;
 185     si = bnd*yy;
 186     u = -2.0 * sr;
 187     v = bnd;
 188     fxshfr(20*(cnt+1),&nz);
 189     if (nz != 0) {
 190       /*  The second stage jumps directly to one of the third
 191        *  stage iterations and returns here if successful.
 192        *  Deflate the polynomial, store the zero or zeros and
 193        *  return to the main algorithm.
 194        */
 195       j = degree - n;
 196       zeror[j] = szr;
 197       zeroi[j] = szi;
 198       n -= nz;
 199       for (i=0;i<=n;i++)
 200         p[i] = qp[i];
 201       if (nz != 1) {
 202         zeror[j+1] = lzr;
 203         zeroi[j+1] = lzi;
 204       }
 205       goto _40;
 206     }
 207     /*  If the iteration is unsuccessful another quadratic
 208      *  is chosen after restoring k.
 209      */
 210     for (i=0;i<n;i++) {
 211       k[i] = temp[i];
 212     }
 213   }
 214   /*  Return with failure if no convergence with 20 shifts. */
 215  _99:
 216   delete [] svk;
 217   delete [] qk;
 218   delete [] k;
 219   delete [] qp;
 220   delete [] p;
 221   delete [] pt;
 222   delete [] temp;
 223
 224 //   cerr << " rpoly roots " << endl;
 225 //   for (int ii=0; ii<degree;ii++) {
 226 //     cerr << " (" << zeror[ii] << "," << zeroi[ii] << ")";
 227 //   }
 228 //   cerr << endl;
 229 //   cerr << " rpoly roots point " << &zeror[0] << " " <<  &zeroi[0] << endl;
 230 //   cerr << " rpoly roots  point 1 " << &zeror[1] << " " <<  &zeroi[1] << endl;
 231
 232
 233   return degree - n;
 234 }
 235
 236 /*  Computes up to L2 fixed shift k-polynomials,
 237  *  testing for convergence in the linear or quadratic
 238  *  case. Initiates one of the variable shift
 239  *  iterations and returns with the number of zeros
 240  *  found.
 241  */
 242  void roots_polynoms::fxshfr(int l2,int *nz)
 243 {
 244   double svu,svv,ui,vi,s;
 245   double betas,betav,oss,ovv,ss,vv,ts,tv;
 246   double ots,otv,tvv,tss;
 247   int type, i,j,iflag,vpass,spass,vtry,stry;
 248
 249   *nz = 0;
 250   betav = 0.25;
 251   betas = 0.25;
 252   oss = sr;
 253   ovv = v;
 254   /*  Evaluate polynomial by synthetic division. */
 255   quadsd(n,&u,&v,p,qp,&a,&b);
 256   calcsc(&type);
 257   for (j=0;j<l2;j++) {
 258     /*  Calculate next k polynomial and estimate v. */
 259     nextk(&type);
 260     calcsc(&type);
 261     newest(type,&ui,&vi);
 262     vv = vi;
 263     /*  Estimate s. */
 264     ss = 0.0;
 265     if (k[n-1] != 0.0) ss = -p[n]/k[n-1];
 266     tv = 1.0;
 267     ts = 1.0;
 268     if (j == 0 || type == 3) goto _70;
 269     /*  Compute relative measures of convergence of s and v sequences. */
 270     if (vv != 0.0) tv = fabs((vv-ovv)/vv);
 271     if (ss != 0.0) ts = fabs((ss-oss)/ss);
 272     /*  If decreasing, multiply two most recent convergence measures. */
 273     tvv = 1.0;
 274     if (tv < otv) tvv = tv*otv;
 275     tss = 1.0;
 276     if (ts < ots) tss = ts*ots;
 277     /*  Compare with convergence criteria. */
 278     vpass = (tvv < betav);
 279     spass = (tss < betas);
 280     if (!(spass || vpass)) goto _70;
 281     /*  At least one sequence has passed the convergence test.
 282      *  Store variables before iterating.
 283      */
 284     svu = u;
 285     svv = v;
 286     for (i=0;i<n;i++) {
 287       svk[i] = k[i];
 288     }
 289     s = ss;
 290     /*  Choose iteration according to the fastest converging
 291      *  sequence.
 292      */
 293     vtry = 0;
 294     stry = 0;
 295     if (spass && (!vpass) || tss < tvv) goto _40;
 296   _20:
 297     quadit(&ui,&vi,nz);
 298     if (*nz > 0) return;
 299     /*  Quadratic iteration has failed. Flag that it has
 300      *  been tried and decrease the convergence criterion.
 301      */
 302     vtry = 1;
 303     betav *= 0.25;
 304     /*  Try linear iteration if it has not been tried and
 305      *  the S sequence is converging.
 306      */
 307     if (stry || !spass) goto _50;
 308     for (i=0;i<n;i++) {
 309       k[i] = svk[i];
 310     }
 311   _40:
 312     realit(&s,nz,&iflag);
 313     if (*nz > 0) return;
 314     /*  Linear iteration has failed. Flag that it has been
 315      *  tried and decrease the convergence criterion.
 316      */
 317     stry = 1;
 318     betas *=0.25;
 319     if (iflag == 0) goto _50;
 320     /*  If linear iteration signals an almost double real
 321      *  zero attempt quadratic iteration.
 322      */
 323     ui = -(s+s);
 324     vi = s*s;
 325     goto _20;
 326     /*  Restore variables. */
 327   _50:
 328     u = svu;
 329     v = svv;
 330     for (i=0;i<n;i++) {
 331       k[i] = svk[i];
 332     }
 333     /*  Try quadratic iteration if it has not been tried
 334      *  and the V sequence is convergin.
 335      */
 336     if (vpass && !vtry) goto _20;
 337     /*  Recompute QP and scalar values to continue the
 338      *  second stage.
 339      */
 340     quadsd(n,&u,&v,p,qp,&a,&b);
 341     calcsc(&type);
 342   _70:
 343     ovv = vv;
 344     oss = ss;
 345     otv = tv;
 346     ots = ts;
 347   }
 348 }
 349 /*  Variable-shift k-polynomial iteration for a
 350  *  quadratic factor converges only if the zeros are
 351  *  equimodular or nearly so.
 352  *  uu, vv - coefficients of starting quadratic.
 353  *  nz - number of zeros found.
 354  */
 355  void roots_polynoms::quadit(double *uu,double *vv,int *nz)
 356 {
 357   double ui,vi;
 358   double mp,omp,ee,relstp,t,zm;
 359   int type,i,j,tried;
 360
 361   *nz = 0;
 362   tried = 0;
 363   u = *uu;
 364   v = *vv;
 365   j = 0;
 366   /*  Main loop. */
 367  _10:
 368   quad(1.0,u,v,&szr,&szi,&lzr,&lzi);
 369   /*  Return if roots of the quadratic are real and not
 370    *  close to multiple or nearly equal and of opposite
 371    *  sign.
 372    */
 373   if (fabs(fabs(szr)-fabs(lzr)) > 0.01 * fabs(lzr)) return;
 374   /*  Evaluate polynomial by quadratic synthetic division. */
 375   quadsd(n,&u,&v,p,qp,&a,&b);
 376   mp = fabs(a-szr*b) + fabs(szi*b);
 377   /*  Compute a rigorous bound on the rounding error in
 378    *  evaluating p.
 379    */
 380   zm = sqrt(fabs(v));
 381   ee = 2.0*fabs(qp[0]);
 382   t = -szr*b;
 383   for (i=1;i<n;i++) {
 384     ee = ee*zm + fabs(qp[i]);
 385   }
 386   ee = ee*zm + fabs(a+t);
 387   ee *= (5.0 *mre + 4.0*are);
 388   ee = ee - (5.0*mre+2.0*are)*(fabs(a+t)+fabs(b)*zm)+2.0*are*fabs(t);
 389   /*  Iteration has converged sufficiently if the
 390    *  polynomial value is less than 20 times this bound.
 391    */
 392   if (mp <= 20.0*ee) {
 393     *nz = 2;
 394     return;
 395   }
 396   j++;
 397   /*  Stop iteration after 20 steps. */
 398   if (j > 20) return;
 399   if (j < 2) goto _50;
 400   if (relstp > 0.01 || mp < omp || tried) goto _50;
 401   /*  A cluster appears to be stalling the convergence.
 402    *  Five fixed shift steps are taken with a u,v close
 403    *  to the cluster.
 404    */
 405   if (relstp < eta) relstp = eta;
 406   relstp = sqrt(relstp);
 407   u = u - u*relstp;
 408   v = v + v*relstp;
 409   quadsd(n,&u,&v,p,qp,&a,&b);
 410   for (i=0;i<5;i++) {
 411     calcsc(&type);
 412     nextk(&type);
 413   }
 414   tried = 1;
 415   j = 0;
 416  _50:
 417   omp = mp;
 418   /*  Calculate next k polynomial and new u and v. */
 419   calcsc(&type);
 420   nextk(&type);
 421   calcsc(&type);
 422   newest(type,&ui,&vi);
 423   /*  If vi is zero the iteration is not converging. */
 424   if (vi == 0.0) return;
 425   relstp = fabs((vi-v)/vi);
 426   u = ui;
 427   v = vi;
 428   goto _10;
 429 }
 430 /*  Variable-shift H polynomial iteration for a real zero.
 431  *  sss - starting iterate
 432  *  nz  - number of zeros found
 433  *  iflag - flag to indicate a pair of zeros near real axis.
 434  */
 435  void roots_polynoms::realit(double *sss, int *nz, int *iflag)
 436 {
 437   double pv,kv,t,s;
 438   double ms,mp,omp,ee;
 439   int i,j;
 440
 441   *nz = 0;
 442   s = *sss;
 443   *iflag = 0;
 444   j = 0;
 445   /*  Main loop */
 446   while (1) {
 447     pv = p[0];
 448     /*  Evaluate p at s. */
 449     qp[0] = pv;
 450     for (i=1;i<=n;i++) {
 451       pv = pv*s + p[i];
 452       qp[i] = pv;
 453     }
 454     mp = fabs(pv);
 455     /*  Compute a rigorous bound on the error in evaluating p. */
 456     ms = fabs(s);
 457     ee = (mre/(are+mre))*fabs(qp[0]);
 458     for (i=1;i<=n;i++) {
 459       ee = ee*ms + fabs(qp[i]);
 460     }
 461     /*  Iteration has converged sufficiently if the polynomial
 462      *  value is less than 20 times this bound.
 463      */
 464     if (mp <= 20.0*((are+mre)*ee-mre*mp)) {
 465       *nz = 1;
 466       szr = s;
 467       szi = 0.0;
 468       return;
 469     }
 470     j++;
 471     /*  Stop iteration after 10 steps. */
 472     if (j > 10) return;
 473     if (j < 2) goto _50;
 474     if (fabs(t) > 0.001*fabs(s-t) || mp < omp) goto _50;
 475     /*  A cluster of zeros near the real axis has been
 476      *  encountered. Return with iflag set to initiate a
 477      *  quadratic iteration.
 478      */
 479     *iflag = 1;
 480     *sss = s;
 481     return;
 482     /*  Return if the polynomial value has increased significantly. */
 483   _50:
 484     omp = mp;
 485     /*  Compute t, the next polynomial, and the new iterate. */
 486     kv = k[0];
 487     qk[0] = kv;
 488     for (i=1;i<n;i++) {
 489       kv = kv*s + k[i];
 490       qk[i] = kv;
 491     }
 492     if (fabs(kv) <= fabs(k[n-1])*10.0*eta) {
 493       /*  Use unscaled form. */
 494       k[0] = 0.0;
 495       for (i=1;i<n;i++) {
 496         k[i] = qk[i-1];
 497       }
 498     }
 499     else {
 500       /*  Use the scaled form of the recurrence if the value
 501        *  of k at s is nonzero.
 502        */
 503       t = -pv/kv;
 504       k[0] = qp[0];
 505       for (i=1;i<n;i++) {
 506         k[i] = t*qk[i-1] + qp[i];
 507       }
 508     }
 509     kv = k[0];
 510     for (i=1;i<n;i++) {
 511       kv = kv*s + k[i];
 512     }
 513     t = 0.0;
 514     if (fabs(kv) > fabs(k[n-1]*10.0*eta)) t = -pv/kv;
 515     s += t;
 516   }
 517 }
 518
 519 /*  This routine calculates scalar quantities used to
 520  *  compute the next k polynomial and new estimates of
 521  *  the quadratic coefficients.
 522  *  type - integer variable set here indicating how the
 523  *  calculations are normalized to avoid overflow.
 524  */
 525  void roots_polynoms::calcsc(int *type)
 526 {
 527   /*  Synthetic division of k by the quadratic 1,u,v */
 528   quadsd(n-1,&u,&v,k,qk,&c,&d);
 529   if (fabs(c) > fabs(k[n-1]*100.0*eta)) goto _10;
 530   if (fabs(d) > fabs(k[n-2]*100.0*eta)) goto _10;
 531   *type = 3;
 532   /*  Type=3 indicates the quadratic is almost a factor of k. */
 533   return;
 534  _10:
 535   if (fabs(d) < fabs(c)) {
 536     *type = 1;
 537     /*  Type=1 indicates that all formulas are divided by c. */
 538     e = a/c;
 539     f = d/c;
 540     g = u*e;
 541     h = v*b;
 542     a3 = a*e + (h/c+g)*b;
 543     a1 = b - a*(d/c);
 544     a7 = a + g*d + h*f;
 545     return;
 546   }
 547   *type = 2;
 548   /*  Type=2 indicates that all formulas are divided by d. */
 549   e = a/d;
 550   f = c/d;
 551   g = u*b;
 552   h = v*b;
 553   a3 = (a+g)*e + h*(b/d);
 554   a1 = b*f-a;
 555   a7 = (f+u)*a + h;
 556 }
 557 /*  Computes the next k polynomials using scalars
 558  *  computed in calcsc.
 559  */
 560  void roots_polynoms::nextk(int *type)
 561 {
 562   double temp;
 563   int i;
 564
 565   if (*type == 3) {
 566     /*  Use unscaled form of the recurrence if type is 3. */
 567     k[0] = 0.0;
 568     k[1] = 0.0;
 569     for (i=2;i<n;i++) {
 570       k[i] = qk[i-2];
 571     }
 572     return;
 573   }
 574   temp = a;
 575   if (*type == 1) temp = b;
 576   if (fabs(a1) <= fabs(temp)*eta*10.0) {
 577     /*  If a1 is nearly zero then use a special form of the
 578      *  recurrence.
 579      */
 580     k[0] = 0.0;
 581     k[1] = -a7*qp[0];
 582     for(i=2;i<n;i++) {
 583       k[i] = a3*qk[i-2] - a7*qp[i-1];
 584     }
 585     return;
 586   }
 587   /*  Use scaled form of the recurrence. */
 588   a7 /= a1;
 589   a3 /= a1;
 590   k[0] = qp[0];
 591   k[1] = qp[1] - a7*qp[0];
 592   for (i=2;i<n;i++) {
 593     k[i] = a3*qk[i-2] - a7*qp[i-1] + qp[i];
 594   }
 595 }
 596 /*  Compute new estimates of the quadratic coefficients
 597  *  using the scalars computed in calcsc.
 598  */
 599  void roots_polynoms::newest(int type,double *uu,double *vv)
 600 {
 601   double a4,a5,b1,b2,c1,c2,c3,c4,temp;
 602   //  double smalno = 1.2e-38;
 603   //  double smalno = 0.0;
 604
 605   /* Use formulas appropriate to setting of type. */
 606   if (type == 3) {
 607     /*  If type=3 the quadratic is zeroed. */
 608     *uu = 0.0;
 609     *vv = 0.0;
 610     return;
 611   }
 612   if (type == 2) {
 613     a4 = (a+g)*f + h;
 614     a5 = (f+u)*c + v*d;
 615   }
 616   else {
 617     a4 = a + u*b +h*f;
 618     a5 = c + (u+v*f)*d;
 619   }
 620   /*  Evaluate new quadratic coefficients. */
 621   b1 = -k[n-1]/p[n];
 622   b2 = -(k[n-2]+b1*p[n-1])/p[n];
 623   c1 = v*b2*a1;
 624   c2 = b1*a7;
 625   c3 = b1*b1*a3;
 626   c4 = c1 - c2 - c3;
 627   temp = a5 + b1*a4 - c4;
 628   if (fabs(temp) <= smalno) {
 629     *uu = 0.0;
 630     *vv = 0.0;
 631     return;
 632   }
 633   *uu = u - (u*(c3+c2)+v*(b1*a1+b2*a7))/temp;
 634   *vv = v*(1.0+c4/temp);
 635   return;
 636 }
 637
 638 /*  Divides p by the quadratic 1,u,v placing the quotient
 639  *  in q and the remainder in a,b.
 640  */
 641  void roots_polynoms::quadsd(int my_n,double *my_u,double *my_v,
 642                                          double *my_p,double *my_q,
 643                                          double *my_a,double *my_b)
 644 {
 645   double my_c;
 646   int i;
 647   *my_b = my_p[0];
 648   my_q[0] = *my_b;
 649   *my_a = my_p[1] - (*my_b)*(*my_u);
 650   my_q[1] = *my_a;
 651   for (i=2;i<=my_n;i++) {
 652     my_c = my_p[i] - (*my_a)*(*my_u) - (*my_b)*(*my_v);
 653     my_q[i] = my_c;
 654     *my_b = *my_a;
 655     *my_a = my_c;
 656   }
 657 }
 658 /*  Calculate the zeros of the quadratic a*z^2 + b1*z + c.
 659  *  The quadratic formula, modified to avoid overflow, is used
 660  *  to find the larger zero if the zeros are real and both
 661  *  are complex. The smaller real zero is found directly from
 662  *  the product of the zeros c/a.
 663  */
 664  void roots_polynoms::quad(double my_a,double my_b1,double my_c,double *my_sr,double *my_si,
 665                                        double *my_lr,double *my_li)
 666 {
 667   double my_b,my_d,my_e;
 668   //  double smalno = 1.2e-38;
 669
 670   if (fabs(my_a) <= smalno) {         /* less than two roots */
 671     if (fabs(my_b1) >= smalno)
 672       *my_sr = -my_c/my_b1;
 673     else
 674     *my_sr = 0.0;
 675     *my_lr = 0.0;
 676     *my_si = 0.0;
 677     *my_li = 0.0;
 678     return;
 679   }
 680   if (fabs(my_c) <= smalno) {         /* one real root, one zero root */
 681     *my_sr = 0.0;
 682     *my_lr = -my_b1/my_a;
 683     *my_si = 0.0;
 684     *my_li = 0.0;
 685     return;
 686   }
 687   /* Compute discriminant avoiding overflow. */
 688   my_b = my_b1/2.0;
 689   if (fabs(my_b) < fabs(my_c)) {
 690     if (my_c < 0.0)
 691       my_e = -my_a;
 692     else
 693       my_e = my_a;
 694     my_e = my_b*(my_b/fabs(my_c)) - my_e;
 695     my_d = sqrt(fabs(my_e))*sqrt(fabs(my_c));
 696   }
 697   else {
 698     my_e = 1.0 - (my_a/my_b)*(my_c/my_b);
 699     my_d = sqrt(fabs(my_e))*fabs(my_b);
 700   }
 701   if (my_e < 0.0) {      /* complex conjugate zeros */
 702     *my_sr = -my_b/my_a;
 703     *my_lr = *my_sr;
 704     *my_si = fabs(my_d/my_a);
 705     *my_li = -(*my_si);
 706   }
 707   else {
 708     if (my_b >= 0.0)   /* real zeros. */
 709       my_d = -my_d;
 710     *my_lr = (-my_b+my_d)/my_a;
 711     *my_sr = 0.0;
 712     if (*my_lr != 0.0)
 713       *my_sr = (my_c/ *my_lr)/my_a;
 714     *my_si = 0.0;
 715     *my_li = 0.0;
 716   }
 717 }
 718
 719  //qques outils d'algebre lineaire
 720
 721  /** \fn add
 722   * \brief Calcule la somme de 2vecteurs ( U=U+V)
 723   * @param *U,*V les 2 vecteurs
 724   * @param N taille des 2 vecteurs
 725   * @return la somme de U et V dans U*/
 726  void Polynoms::add
 727  (
 728                 double *U,              //vecteur auquel on ajoute
 729                 int N,                  //taille du vecteur
 730                 const double *V //ajout
 731  )
 732  {
 733         for(int i=0;i<N;i++) U[i] += V[i];
 734  };
 735
 736  /** \fn tensor
 737   * \brief Calcule le tenseur de 2 vecteurs
 738   * @param *a le 1er vecteur (*double)
 739   * @param N la taille du premier vecteur (int)
 740   * @param *b le 2ieme vecteur (*double)
 741   * @param M la taille du 2ieme vecteur (int)
 742   * @param *ab le resultat
 743   * @return */
 744
 745  void Polynoms::tensor
 746  (      const double *a,        //vecteur gauche
 747                 int N,                  //taille du vecteur gauche
 748                 const double *b,        //vecteur droit
 749                 int M,                  //taille du vecteur droit
 750                 double *ab              //conteneur de sortie
 751  )
 752  {
 753         for(int i=0;i<N;i++)
 754                 for(int j=0;j<M;j++)
 755                         ab[i*M+j] = a[i]*b[j];
 756  };
 757
 758  /** \fn matrixProduct
 759   * \brief calcule le produit de 2 matrices .
 760   * @param *Matrix1 la 1ere matrice
 761   * @param R1,C1 la taille de la matrice1 Nbr de ligne et le nbr de colonne
 762   * @param Matrix2 la 2ieme matrice
 763   * @param R2,C2 la taille de la matrice2 Nbr de ligne et le nbr de colonne
 764   * @param out le resultat de Matrix1*Matrix2
 765   * @return */
 766  void Polynoms::matrixProduct
 767  (
 768                 const double *Matrix1,
 769                 const int &R1,
 770                 const int &C1,
 771                 const double *Matrix2,
 772                 const int &R2,
 773                 const int &C2,
 774                 double *out
 775  )
 776  {
 777         for(int i=0; i<R1; i++)
 778         {
 779                 for(int j=0; j<C2; j++)
 780                 {
 781                         out[i*C2+j] = 0;
 782                         for(int k=0; k<C1; k++)
 783                         {
 784                                 out[i*C2+j] += Matrix1[i*C1+k]*Matrix2[k*C2+j];
 785                         }
 786                 }
 787         }
 788  };
 789  /** \fn matrixProdVec
 790   * \brief Calcule le produit Matrice vecteur
 791   * @param *Matrix correspond à la matrice du produit
 792   * @param R1,C1 la taille de la matrice( Nbr de ligne et le nbr de colonne)
 793   * @param Vector correspond au vecteur du produit
 794   * @param out le resultat de Matrix*Vector
 795   * @return   */
 796  void Polynoms::matrixProdVec
 797  (      const double *Matrix,
 798                 const int &R1,
 799                 const int &C1,
 800                 const double *Vector,
 801                 double *out
 802  )
 803  {
 804         for(int i=0; i<R1; i++)
 805         {
 806                 out[i] = 0;
 807                 for(int j=0; j<C1; j++)
 808                 {
 809                         out[i] += Matrix[i*C1+j]*Vector[j];
 810                 }
 811         }
 812  };
 813
 814  /** \fn shift_diagonal
 815   * \brief Calcule la trace d'une matrice carrée
 816   * @param *Matrix correspond  à la matrice
 817   * @param N taille de la matrice
 818   * @param shift résultat
 819   * @return renvoie la trace de la matrice Matrix dans shift */
 820  void Polynoms::shift_diagonal( double *Matrix, const int N, double shift)
 821  {
 822         for (int i=0; i<N; i++)
 823                 Matrix[i*N + i] += shift;
 824  };
 825  //qques outils d'algebre des polynomes
 826
 827  /** \fn remplir_trinome
 828   * \brief remplie un trinome
 829   */
 830  void Polynoms::remplir_trinome(double coeff, double u, double alpha, vector< double >& trinome)
 831  {
 832         trinome[2] = coeff;
 833         coeff *= u;
 834         trinome[1] = -2*coeff;
 835         coeff *= u;
 836         trinome[0] = coeff - alpha;
 837  };
 838
 839
 840  /** \fn additionne
 841   * \brief Additionne 2 vecteurs de tailles differentes
 842   * @param P,Q les 2 vecteurs à additionner
 843   * @param n,m les 2 tailles respectives des vecteurs P et Q
 844   * @return un vecteur qui correspond a P+Q*/
 845  vector< double > Polynoms::additionne(const vector< double >& P, const vector< double >& Q, int n, int m)
 846  {
 847         if(m < n) {
 848                 vector< double > P_plus_Q = P;
 849                 for(int j = 0; j<m+1; j++){
 850                         P_plus_Q[j] += Q[j];
 851                 }
 852                 return P_plus_Q;
 853         }
 854         else{
 855                 vector< double > P_plus_Q = Q;
 856                 for(int j = 0; j<n+1; j++){
 857                         P_plus_Q[j] += P[j];
 858                 }
 859                 return P_plus_Q;
 860         }
 861  };
 862
 863
 864  /** \fn multiplie
 865   * \brief Calcule le produit scalaire de 2 vecteurs de tailles differentes
 866   * @param P,Q les 2 vecteurs
 867   * @param n,m les 2 tailles respectives des 2 vecteurs
 868   * @return un vecteur correspond à (P,Q) */
 869  vector< double > Polynoms::multiplie(const vector< double >& P, const vector< double >& Q, int n, int m)
 870  {
 871         vector< double > P_fois_Q (n+m+1,0.);
 872         for(int i = 0; i<n+1; i++){
 873                 for(int j = 0; j<m+1; j++){
 874                         P_fois_Q[i+j] += P[i]*Q[j];
 875                 }
 876         }
 877         return P_fois_Q;
 878  }
 879
 880
 881  /** \fn polynome_caracteristique
 882   * \brief Calcule le polynome caracteristique
 883   * @return */
 884  vector< double > Polynoms::polynome_caracteristique(double alpha_v, double alpha_l, double Uv, double Ul, double rhov, double rhol, double   cv_2, double cl_2, double dPiv, double dPil)//c= inverse vitesse du son!
 885  {
 886         vector< double > trinome_u(3);
 887         vector< double > trinome_c(3);
 888         vector< double > pol_char;
 889
 890         remplir_trinome(alpha_v*cv_2,Uv,alpha_v,trinome_c);
 891         remplir_trinome(rhol,Ul,dPil,trinome_u);
 892
 893         pol_char = multiplie(trinome_u, trinome_c, 2, 2);
 894
 895         remplir_trinome(alpha_l*cl_2,Ul,alpha_l,trinome_c);
 896         remplir_trinome(rhov,Uv,dPiv,trinome_u);
 897
 898         pol_char = additionne(pol_char,multiplie(trinome_u, trinome_c, 2, 2),4,4);
 899
 900         return pol_char;
 901  }
 902
 903  /** \fn polynome_caracteristique
 904   * \brief
 905   * @param alpha1
 906   * @param alpha2
 907   * @param u1
 908   * @param u2
 909   * @param rho1
 910   * @param rho2
 911   * @param invc1sq
 912   * @param invc2sq
 913   * @param dpi1
 914   * @param dpi2
 915   * @param g2press
 916   * @param g2alpha
 917   * @param g2
 918   * @param epsilon
 919   * @return */
 920  vector< double > Polynoms::polynome_caracteristique(double alpha1, double alpha2, double u1, double u2, double rho1, double rho2, double invc1sq, double invc2sq, double dpi1, double dpi2, double g2press, double g2alpha, double g2, double epsilon)
 921  {
 922         vector< double > trinome1(3);
 923         vector< double > trinome2(3);
 924         vector< double > pol_char;
 925         double K1,K2,K3;
 926
 927         /* (x-u1)^2(x-u2)^2-K1(x-u2)^2-K2(x-u1)^2+K3 */
 928         K1=alpha1*rho2*g2press + dpi1*alpha2*invc2sq*g2alpha;
 929         K2=alpha2*rho1*g2press + dpi2*alpha1*invc1sq*g2alpha;
 930         K3=(alpha1*dpi2+alpha2*dpi1)*g2alpha*g2press/g2;
 931         //cout<<"K1= " <<K1<<", K2= " <<K2<<", K3= " <<K3<<endl;
 932
 933         if(fabs(K1)>epsilon && fabs(K2)>epsilon )
 934         {
 935                 remplir_trinome(1/K1,u1,1,trinome1);
 936                 //cout<<"coeff constant trinome1= " << trinome1[0]<<endl;
 937                 remplir_trinome(1/K2,u2,1,trinome2);
 938                 //cout<<"coeff constant trinome2= " << trinome2[0]<<endl;
 939
 940                 pol_char = multiplie(trinome1, trinome2, 2, 2);
 941                 //cout<<"coeff constant produit= " << pol_char[0]<<endl;
 942
 943                 pol_char[0]+=K3/K2/K1-1;
 944                 //cout<<"coeff constant apres soustraction= " << pol_char[0]<<endl;
 945         }
 946         else
 947         {
 948                 remplir_trinome(1,u1,K1,trinome1);
 949                 remplir_trinome(1,u2,K2,trinome2);
 950
 951                 pol_char = multiplie(trinome1, trinome2, 2, 2);
 952
 953                 pol_char[0]+=K3-K2*K1;
 954         }
 955
 956         /*   for(int ct =0; ct<pol_char.size()-1; ct++)  */
 957         /*     if(fabs(pol_char[ct])< epsilon)  */
 958         /*       pol_char[ct]=0.; */
 959
 960         return pol_char;
 961  }
 962
 963  //Pour le tri des valeurs propres
 964
 965  /** \fn module
 966   * \brief calcule le module d'un nombre complexe
 967   * @param z est un nombre complexe
 968   * @return  calcule le module de z*/
 969  double Polynoms::module(complex< double > z)
 970  {
 971         return abs(z);
 972  }
 973
 974  /** \fn modulec calcule le module² de z */
 975  double Polynoms::modulec(complex< double > z)
 976  {
 977         return norm(z);
 978  }
 979
 980  /** \fn abs_generalise
 981   * \brief calcule la valeur absolue d'un nombre complexe
 982   * \Details calcule la valeur absolue d'un nombre complexe en prenant en compte
 983   * que la partie réelle c-a-d si z= a+ib abs_generalize() renvoie |a|+ib
 984   * @param z
 985   * @return si z = a+ib elle renvoie |a|+ib */
 986  complex< double > Polynoms::abs_generalise(complex< double > z)
 987  {
 988         if(z.real()>=0)
 989                 return z ;
 990         else
 991                 return -z ;
 992  }
 993  void Polynoms::ordre_croissant_abs(vector< complex< double > > &L, int n)
 994  {
 995         vector< complex< double > > copieL = L;
 996         int i_max;
 997         double Lmaxc;
 998         double Lmax_iterc;
 999
1000         for(int j=0; j<n; j++){
1001                 i_max = 0;
1002                 Lmaxc = copieL[0].real();
1003                 for (int i=1; i<n; i++){
1004                         Lmax_iterc = copieL[i].real();
1005                         if( Lmax_iterc > Lmaxc ){
1006                                 Lmaxc = Lmax_iterc;
1007                                 i_max = i;
1008                         }
1009                 }
1010                 L[n-1-j] = copieL[i_max];
1011                 copieL[i_max] = -INFINITY;
1012         }
1013  }
1014
1015  int Polynoms::new_tri_selectif(vector< complex< double > > &L, int n, double epsilon)
1016  {
1017         int size=1;
1018         /*
1019         cout<<"avant ordre croissant"<<endl;
1020         for(int i=0;i<n;i++)
1021                 cout<<L[i]<<", "<<endl;
1022          */
1023         ordre_croissant_abs(L,n);
1024         /*
1025         cout<<"après ordre croissant"<<endl;
1026         for(int i=0;i<n;i++)
1027                 cout<<L[i]<<", "<<endl;
1028          */
1029         vector< complex< double > >result(1,L[0]);
1030         for(int i=1;i<n;i++)
1031                 if(fabs(L[i].real()-result[size-1].real())>epsilon || fabs(L[i].imag())>epsilon)
1032                 {
1033                         result.push_back(L[i]);
1034                         size++;
1035                         /*
1036                         cout<<"tri selectif step "<< i<<endl;
1037                         for(int j=0;j<size;j++)
1038                                 cout<<result[j]<<", "<<endl;
1039                          */
1040                 }
1041         L=result;
1042         return size;
1043  }
1044
1045  /** \fn tri_selectif
1046   * \brief
1047   * @param L
1048   * @param n
1049   * @param epsilon
1050   * @return */
1051  template< typename T >
1052  int Polynoms::tri_selectif(T& L, int n, double epsilon)
1053  {
1054         int imin;
1055         int size_vp =n;
1056         int i=0;
1057         int j;
1058         complex< double > Lmin;
1059
1060         for(i = 0 ; i<size_vp ; i++){//On travaille sur le sous vecteur des indices de  i � size_vp-1
1061                 imin = i;
1062                 Lmin = L[i];
1063
1064                 for(j = i+1 ; j < size_vp ; j++)
1065                         if( Lmin.real() > L[j].real() )
1066                         {Lmin = L[j]; imin = j;}
1067
1068                 //on met la vp � zero si elle est trop petite
1069                 if( fabs(Lmin.real()) < epsilon) Lmin.real()=0;
1070                 if( fabs(Lmin.imag()) < epsilon) Lmin.imag()=0;
1071
1072                 switch (i) {
1073                 case 0: {
1074                         if (imin != i) {
1075                                 L[imin]=L[i];
1076                                 L[i]=Lmin;
1077                         }
1078                         break;
1079                 }
1080                 case 1: {
1081                         if ( module(L[i-1] - Lmin) > epsilon )  {
1082                                 if (imin != i) {
1083                                         L[imin]=L[i];
1084                                         L[i]=Lmin;
1085                                 }
1086
1087                         } else {
1088                                 L[imin]=L[size_vp - 1];
1089                                 L[size_vp - 1]=Lmin;
1090
1091                                 size_vp--;i--;
1092                         }
1093                         break;
1094                 }
1095                 default : {
1096                         if ( (module(L[i-1] - Lmin) > epsilon) &&  (module(L[i-2] - Lmin) > epsilon) )  {
1097                                 if (imin != i) {
1098                                         L[imin]=L[i];
1099                                         L[i]=Lmin;
1100                                 }
1101
1102                         } else {
1103                                 L[imin]=L[size_vp - 1];
1104                                 L[size_vp - 1]=Lmin;
1105
1106                                 size_vp--;i--;
1107                         }
1108                 }
1109                 }
1110         }
1111
1112         return size_vp;
1113  }
1114
1115  //Calcul des coefficients du polynome d'interpolation x->y dans la base de Newton
1116  template<class T>
1117
1118  /** \fn dif_div
1119   * \brief
1120   * @param n
1121   * @param x
1122   * @param y
1123   * @param epsilon
1124   * @return */
1125  vector< complex< double > > Polynoms::dif_div(int n, const vector< complex< double > >& x, const vector< T >& y, double epsilon)//n=nb valeurs à interpoler
1126  {
1127         complex< double > tab_dif_div[n*n];//La matrice de taille n*n
1128         vector< complex< double > > dif_div (n);
1129         int i, j;
1130         complex< double > delta_x;
1131
1132         for( i=0 ; i<n ; i++) {
1133                 tab_dif_div[i*n]=y[i];
1134                 //   cerr << tab_dif_div[i*n+0] << endl;
1135         }
1136         for(j=1; j<n ; j++){
1137                 for(i=0;i < n-j;i++) {
1138                         delta_x = x[i+j]-x[i];
1139                         if(module(delta_x) < epsilon) {
1140                                 cout << " attention, dif_div, delta_x <epsilon, " << " delta_x= " << delta_x <<" epsilon= "<<epsilon<<endl;
1141                                 cout<<" vp= " <<endl;
1142                                 for(int k=0; k<n; k++)
1143                                         cout << x[k]<<", " << endl;
1144                         }
1145                         tab_dif_div[i*n+j]=(tab_dif_div[(i+1)*n+j-1]-tab_dif_div[i*n+j-1])/delta_x;
1146                 }
1147         }
1148         for(i=0 ; i<n ; i++)
1149                 dif_div[i] = tab_dif_div[i*n+n-1-i];
1150         return dif_div;
1151  }
1152  //attention, Les vp complexes conjugu�es doivent se suivre dans la liste x
1153  void Polynoms::appliquer_dif_div(int n, const vector< complex< double > >& dif_div, const vector< complex< double > >& x, const double* A, const int sizeA, double epsilon, double *p)//p=result
1154  {
1155         int i, debut = 1;
1156         double Id[sizeA*sizeA], aux[sizeA*sizeA], matProd[sizeA*sizeA];
1157
1158         for(int k=0; k<sizeA*sizeA; k++)
1159                 Id[k]=0;
1160         for(int k=0; k<sizeA; k++)
1161                 Id[k*sizeA+k]=1;
1162         /*   for(int k=0; k<sizeA*sizeA; k++) */
1163         /*     aux[k] = A[k] ; */
1164         //cerr << x << endl;
1165         if( fabs(x[0].imag()) > epsilon){
1166                 for(int k=0; k<sizeA*sizeA; k++)
1167                         p[k] = A[k] * dif_div[0].real() ;
1168                 shift_diagonal(p, sizeA,(dif_div[1] - dif_div[0]*x[1]).real());
1169                 debut = 2;
1170         }else{
1171                 for(int k=0; k<sizeA*sizeA; k++)
1172                         p[k] = Id[k] * dif_div[0].real() ;
1173         }
1174         for(i=debut ; i<n ; i++){
1175                 for(int k=0; k<sizeA*sizeA; k++)
1176                         aux[k] = A[k];
1177                 //cerr << " on traite la racine reele " << x[i] << endl;
1178                 if ( fabs(x[i].imag()) < epsilon ) {
1179                         shift_diagonal(aux, sizeA, (- x[i].real()));
1180                         matrixProduct( p, sizeA, sizeA, aux, sizeA, sizeA, matProd);
1181                         for(int k=0; k<sizeA*sizeA; k++)
1182                                 p[k] = matProd[k] ;
1183                         shift_diagonal(p, sizeA, dif_div[i].real());
1184                 }
1185                 else{ //couple de conjugees
1186                         if(fabs(x[i].imag() + x[i+1].imag()) < epsilon){
1187                                 matrixProduct( aux, sizeA, sizeA, aux, sizeA, sizeA, matProd);
1188                                 for(int k=0; k<sizeA*sizeA; k++)
1189                                         aux[k] = matProd[k];
1190                                 for(int k=0; k<sizeA*sizeA; k++)
1191                                         aux[k]+=(-2*x[i].real()) * A[k];
1192                                 shift_diagonal(aux, sizeA, modulec(x[i]));
1193
1194                                 matrixProduct( p, sizeA, sizeA, aux, sizeA, sizeA, matProd);
1195                                 for(int k=0; k<sizeA*sizeA; k++)
1196                                         p[k] = matProd[k] ;
1197                                 for(int k=0; k<sizeA*sizeA; k++)
1198                                         p[k]+= dif_div[i].real() * A[k];
1199                                 shift_diagonal(p, sizeA, (dif_div[i+1] - dif_div[i]*x[i+1]).real());
1200                                 i++;
1201                         }
1202                         else{
1203                                 cout << " appliquer_dif_div : les racines complexes conjuguees ne se suivent pas " <<  endl;
1204                                 for(int k=0; k<n; k++)
1205                                         cout << x[k]<<", " << endl;
1206                                 throw(" appliquer_dif_div : les racines complexes conjuguees ne se suivent pas ");
1207                         }
1208                 }
1209         }
1210  }
1211
1212  void Polynoms::abs_par_interp_directe(int n,  vector< complex< double > > vp,   double * A, int sizeA, double epsilon, double *result,vector< complex< double > > y)
1213  {
1214         vector< complex< double > > differences_divisees = dif_div(n, vp, y, epsilon);
1215         /*
1216         cout<<"valeurs propres"<<endl;
1217         for( int i=0 ; i<n ; i++)
1218                 cout<<vp[i] <<", "<<endl;
1219         cout<<"valeurs à atteindre"<<endl;
1220         for( int i=0 ; i<n ; i++)
1221                 cout<<y[i] <<", "<<endl;
1222         cout<<"differences_divisees= "<<endl;
1223         for(int i=0 ; i<n ; i++)
1224                 cout<<differences_divisees[i]<<", ";
1225         cout<<endl;
1226          */
1227         appliquer_dif_div(n, differences_divisees, vp, A, sizeA, epsilon, result);
1228  }
1229
1230  bool Polynoms::belongTo(complex< double > a , vector< complex <double > > v, double epsilon)
1231  {
1232         bool result = false;
1233         int size = v.size();
1234         int i = 0;
1235         double epsilon2 = epsilon*epsilon;
1236
1237         while(i<size && !result)
1238         {
1239                 result = modulec(a-v[i]) < epsilon2;
1240                 i++;
1241         }
1242         return result;
1243  }
1244  double Polynoms::avr(double a, double b)
1245  {
1246          return 0.5*(a+b);
1247  }