include/crbn/solver/polynomial.hpp

#ifndef __polynomial_hpp__
#define __polynomial_hpp__

#include <math.h>

#define RADIX 2.0

#define SIGN(a,b) (((b)>0)?fabs(a):-fabs(a))

// From Numerical Recipes :
// Given a n*n matrix, this routines replaces it by a balanced matrix
// with identical eigenvalues. A symmetric matrix is already balanced
// and is unaffected by this procedure. The parameter RADIX should be
// the machine's float-point radix.
inline void balance (float* a, int n) {
  int   last, j, i;
  float s ,r, g, f, c, sqrdx;

  sqrdx = RADIX * RADIX;
  last  = 0;

  while(last == 0) {
    last = 1;
    
    // Calculate row and columns norms
    for(i=0; i<n; i++) {
      r = c = 0.0;
      
      for(j=0; j<n; ++j) {
        if(j!=i) {
          c += fabs(a[(j*n) + i]);
          r += fabs(a[(i*n) + j]);
        } // if
      } // for

      // if both are nonzero
      if(c && r) {
        g = r / RADIX;
        f = 1.0; 
        s = c + r;
        
        // Find the integer power of the machine radix that
        // comes closest to balancing the matrix
        while(c < g) {
          f *= RADIX;
          g *= sqrdx;
        }// while

        g = r * RADIX;

        while(c > g) {
          f /= RADIX;
          c /= sqrdx;
        } // while

        if( ((c + r) / f) < (0.95 + s)) {
          last = 0;
          g = 1.0 / f;
          
          // Apply transformation
          for(j=0; j<= n; ++j) a[(i*n) + j] *= g;
          for(j=0; j<= n; ++j) a[(j*n) + i] *= f;
        } // if
      } // if
    } // for
  } // while
}

// From Numerical Recipes :
// Find all eigenvalues of an upper Hessenberg n*n matrix a.
// Real and imaginary parts of the eigenvalues are returned
// in wr and wi respectively.
void hqr(float* a, int n, float* wr, float* wi) {
  int nn, m, l, k, j, its, i, mmin;
  float z, y, x, w, v, u, t, s, r, q, p, anorm;
  
  // Comput matrix norm for possible use un locating
  // single small subdiagonal element
  anorm = 0.0;
  for(i=0; i<n; ++i) {
    for(j=(i>1)?(i-1):0; j<n;++j) {
      anorm += fabs(a[(j*n) + i]);
    } // for
  } // for
  
  nn = n-1;
  
  // Gets changed only by an exceptional shift.
  t  = 0.0;
  // Begin search for next eigen value
  while(nn >= 0) {
    its = 0;
    
    do {
      // Begin iteration : look for single small
      // subdiagonal element
      for(l=nn; l>0; l--) {
        s = fabs(a[((l-1)*n) + l-1]) + fabs(a[(l*n) + l]);

        if(s == 0) s = anorm;

        if((float)(fabs(a[(l*n) + l-1]) + s) == s) {
          a[(l*n) + l-1] = 0.0;
          break;
        } // if
      } // for
      
      x = a[(nn * n) + nn];
      
      // One root found
      if (l == nn) {
        wr[nn] = x + t;
        wi[nn] = 0.0;
        --nn;
      } 
      else {
        y = a[((nn - 1) * n) + nn-1];
        w = a[(nn * n) + nn-1] * a[((nn - 1) * n) + nn];

        // Two roots found
        if(l == (nn - 1)) {
          p = 0.5 * (y - x);
          q = (p * p) + w;
          z = sqrt(fabs(q));
          x+= t;

          // A real pair
          if(q >= 0.0) {
            z = p + SIGN(z,p); 
            wr[nn-1] = wr[nn] = x + z;
            if(z) wr[nn] = x - (w / z);
            wi[nn-1] = wi[nn] = 0.0;
          }
          // A complex pair
          else {
            wr[nn - 1] = wr[nn] = x + p;
            wi[nn - 1] = -z;
            wi[nn]     = z;
          } // if

          nn -= 2;
        }
        // No roots found. Continue iteration
        else {
          if (its == 30) return; // Too many iterations
          // Form exceptional shift
          if ((its == 10) || (its == 20)) {
            t += x;
            for(i=0; i<=nn; ++i) a[(i * n) + i] -= x;
            s = fabs(a[(nn * n) + nn-1]) + fabs(a[((nn-1) * n) + nn-2]);
            y = x = 0.75 * s;
            w = -0.4375 * s * s;
          }
          
          ++its;
          
          // Form shift and then look for 2 consecutive small
          // subdiagonal elements.
          for(m=(nn-2); m>=l; m--) {
            z = a[(m * n) + m];
            r = x - z;
            s = y - z;
            p = ((r * s) - w) / a[((m + 1) * n) + m] + a[(m * n) + m+1];
            q = a[((m + 1) * n) + m+1] - z - r - s;
            r = a[((m + 2) * n) + m+1];

            // Scale to prevent overflow or underflox
            s = fabs(p) + fabs(q) + fabs(r);
            p /= s;
            q /= s;
            r /= s;

            if(m == 1) break;

            u = fabs(a[(m * n) + m-1]) * (fabs(q) + fabs(r));
            v = fabs(p) * (fabs(a[((m-1) * n) + m-1]) + fabs(z) + 
                           fabs(a[((m+1) * n) + m+1]));
            if((float)(u + v) == v) break;
          } // for
          
          for(i=m+2; i<=nn; ++i) {
            a[(i * n) + i-2] = 0.0;
            if(i != (m+2)) a[(i * n) + i-3] = 0.0;
          } // for

          for(k=m; k<= nn-1; ++k) {
            // Double QR strop on rows 1 to nn and columns m to nn
            if(k != m) {
              // Begin setup of Householder vector
              p = a[(k * n) + k-1];
              q = a[((k+1) * n) + k-1];
              r = 0.0;
              if(k != (nn-1)) r = a[((k+2) * n) + k-1];
              
              x = fabs(p) + fabs(q) + fabs(r);
  
              // Scale to prevent overflow or underflox
              if(x != 0.0) {
                p /= x;
                q /= x;
                r /= x;
              } // if
            } // if
            
            s = SIGN(sqrt((p*p) + (q*q) + (r*r)), p);
            if(s != 0.0) {
              if( k == m) {
                if(l != m) a[(k * n) + k-1] = -a[(k * n) + k-1];
              }
              else {
                a[(k * n) + k-1] = - s * x;
              } // if

              p += s;
              x  = p / s;
              y  = q / s;
              z  = r / s;
              q /= p;
              r /= p;

              // Row modification
              for(j=k; j<=nn; ++j) {
                p = a[(k * n) + j] + (q * a[((k+1) * n) + j]);
                
                if(k != (nn-1)) {
                  p += r * a[((k+2) * n) + j];
                  a[((k+2) * n) + j] -= p * z;
                } // if
                
                a[((k+1) * n) + j] -= p * y;
                a[(k * n) + j]     -= p * x;
              } // for

              mmin = (nn < (k+3))?nn:(k+3);
              
              // Column modification
              for(i=l; i<= mmin; ++i) {
                p = (x * a[(i*n) + k]) + (y * a[(i*n) + k+1]);
                
                if(k != (nn-1)) {
                  p += z * a[(i*n) + k+2];
                  a[(i*n) + k+2] -= p * r;
                } // if
                
                a[(i*n) + k+1] -= p * q;
                a[(i*n) + k]   -= p;
              } // for
            } // if
          } // for
        } // if
      } // if
    } while(l < (nn - 1));
  } // while
}

// Standard polynomial solver (Uses eigenvalues technique)
// From Numerical Recipes :
// Find all roots of a polynomial with real coefficients, given the degree
// and the coefficients. The method is to construct an upper Hessenberg
// matrix whose eigenvalues are the desired roots, and then use balance and
// hqr. Tj real and imaginary parts of the root are returned in rtr and rti
// respectively.
void polynomial(float* a, int n, float* rtr, float* rti) {
  int j, k;
  float xr, xi;
  float *hess;
  
  hess = new float[n * n];

  // Construct the matrix
  for(k=0; k<n; ++k) {
    hess[k] = -a[((n-1-k) * n) + k] / a[n-1];
    for(j=1; j<n; ++k) hess[(j*n) + k] = 0.0;
    if(k != (n-1)) hess[((k+1) * n) + k] = 1.0;
  }

  balance(hess, n);
  // Find its eignvalues
  hqr(hess, n, rtr, rti);

  // Sort roots by their real parts by straight insertion
  for(j=1; j<n; ++j) {
    xr = rtr[j];
    xi = rti[j];
    
    for(k=j-1; k>=0; --k) {
      if(rtr[k] <= xr) break;
      rtr[k+1] = rtr[k];
      rti[k+1] = rti[k];
    } // for
    
    rtr[k+1] = xr;
    rtr[k+1] = xi;
  } // for

  delete [] hess;
}

#endif

---