src/math/Eigenvalue.hpp

#ifndef JAMA_EIG_H
#define JAMA_EIG_H

#include "math/DynamicRectMatrix.hpp"

#include <algorithm>
// for min(), max() below
#include <cmath>
// for abs() below

using namespace oopse;
using namespace std;

namespace JAMA
{
  
  /** 
      
  Computes eigenvalues and eigenvectors of a real (non-complex)
  matrix. 
  <P>
  If A is symmetric, then A = V*D*V' where the eigenvalue matrix D is
  diagonal and the eigenvector matrix V is orthogonal. That is,
  the diagonal values of D are the eigenvalues, and
  V*V' = I, where I is the identity matrix.  The columns of V 
  represent the eigenvectors in the sense that A*V = V*D.
  
  <P>
  If A is not symmetric, then the eigenvalue matrix D is block diagonal
  with the real eigenvalues in 1-by-1 blocks and any complex eigenvalues,
  a + i*b, in 2-by-2 blocks, [a, b; -b, a].  That is, if the complex
  eigenvalues look like
<pre>

          u + iv     .        .          .      .    .
            .      u - iv     .          .      .    .
            .        .      a + ib       .      .    .
            .        .        .        a - ib   .    .
            .        .        .          .      x    .
            .        .        .          .      .    y
</pre>
  then D looks like
<pre>

            u        v        .          .      .    .
           -v        u        .          .      .    . 
            .        .        a          b      .    .
            .        .       -b          a      .    .
            .        .        .          .      x    .
            .        .        .          .      .    y
</pre>
  This keeps V a real matrix in both symmetric and non-symmetric
  cases, and A*V = V*D.

  <p>
  The matrix V may be badly
  conditioned, or even singular, so the validity of the equation
  A = V*D*inverse(V) depends upon the condition number of V.
  
  <p>
  (Adapted from JAMA, a Java Matrix Library, developed by jointly 
  by the Mathworks and NIST; see  http://math.nist.gov/javanumerics/jama).
  **/
  
  template <class Real>
  class Eigenvalue
  {
    
    
    /** Row and column dimension (square matrix).  */
    int n;
    
    int issymmetric; /* boolean*/
    
    /** Arrays for internal storage of eigenvalues. */
    
    DynamicVector<Real> d;         /* real part */
    DynamicVector<Real> e;         /* img part */
    
    /** Array for internal storage of eigenvectors. */
    DynamicRectMatrix<Real> V;
    
    /** Array for internal storage of nonsymmetric Hessenberg form.
        @serial internal storage of nonsymmetric Hessenberg form.
    */
    DynamicRectMatrix<Real> H;
    
    
    /** Working storage for nonsymmetric algorithm.
        @serial working storage for nonsymmetric algorithm.
    */
    DynamicVector<Real> ort;
    
    
    // Symmetric Householder reduction to tridiagonal form.
    
    void tred2() {
      
      //  This is derived from the Algol procedures tred2 by
      //  Bowdler, Martin, Reinsch, and Wilkinson, Handbook for
      //  Auto. Comp., Vol.ii-Linear Algebra, and the corresponding
      //  Fortran subroutine in EISPACK.
      
      for (int j = 0; j < n; j++) {
        d(j) = V(n-1,j);
      }
      
      // Householder reduction to tridiagonal form.
      
      for (int i = n-1; i > 0; i--) {
        
        // Scale to avoid under/overflow.
        
        Real scale = 0.0;
        Real h = 0.0;
        for (int k = 0; k < i; k++) {
          scale = scale + abs(d(k));
        }
        if (scale == 0.0) {
          e(i) = d(i-1);
          for (int j = 0; j < i; j++) {
            d(j) = V(i-1,j);
            V(i,j) = 0.0;
            V(j,i) = 0.0;
          }
        } else {
          
          // Generate Householder vector.
          
          for (int k = 0; k < i; k++) {
            d(k) /= scale;
            h += d(k) * d(k);
          }
          Real f = d(i-1);
          Real g = sqrt(h);
          if (f > 0) {
            g = -g;
          }
          e(i) = scale * g;
          h = h - f * g;
          d(i-1) = f - g;
          for (int j = 0; j < i; j++) {
            e(j) = 0.0;
          }
          
          // Apply similarity transformation to remaining columns.
          
          for (int j = 0; j < i; j++) {
            f = d(j);
            V(j,i) = f;
            g = e(j) + V(j,j) * f;
            for (int k = j+1; k <= i-1; k++) {
              g += V(k,j) * d(k);
              e(k) += V(k,j) * f;
            }
            e(j) = g;
          }
          f = 0.0;
          for (int j = 0; j < i; j++) {
            e(j) /= h;
            f += e(j) * d(j);
          }
          Real hh = f / (h + h);
          for (int j = 0; j < i; j++) {
            e(j) -= hh * d(j);
          }
          for (int j = 0; j < i; j++) {
            f = d(j);
            g = e(j);
            for (int k = j; k <= i-1; k++) {
              V(k,j) -= (f * e(k) + g * d(k));
            }
            d(j) = V(i-1,j);
            V(i,j) = 0.0;
          }
        }
        d(i) = h;
      }
      
      // Accumulate transformations.
      
      for (int i = 0; i < n-1; i++) {
        V(n-1,i) = V(i,i);
        V(i,i) = 1.0;
        Real h = d(i+1);
        if (h != 0.0) {
          for (int k = 0; k <= i; k++) {
            d(k) = V(k,i+1) / h;
          }
          for (int j = 0; j <= i; j++) {
            Real g = 0.0;
            for (int k = 0; k <= i; k++) {
              g += V(k,i+1) * V(k,j);
            }
            for (int k = 0; k <= i; k++) {
              V(k,j) -= g * d(k);
            }
          }
        }
        for (int k = 0; k <= i; k++) {
          V(k,i+1) = 0.0;
        }
      }
      for (int j = 0; j < n; j++) {
        d(j) = V(n-1,j);
        V(n-1,j) = 0.0;
      }
      V(n-1,n-1) = 1.0;
      e(0) = 0.0;
    } 
    
    // Symmetric tridiagonal QL algorithm.
    
    void tql2 () {
      
      //  This is derived from the Algol procedures tql2, by
      //  Bowdler, Martin, Reinsch, and Wilkinson, Handbook for
      //  Auto. Comp., Vol.ii-Linear Algebra, and the corresponding
      //  Fortran subroutine in EISPACK.
      
      for (int i = 1; i < n; i++) {
        e(i-1) = e(i);
      }
      e(n-1) = 0.0;
      
      Real f = 0.0;
      Real tst1 = 0.0;
      Real eps = pow(2.0,-52.0);
      for (int l = 0; l < n; l++) {
        
        // Find small subdiagonal element
        
        tst1 = max(tst1,abs(d(l)) + abs(e(l)));
        int m = l;
        
        // Original while-loop from Java code
        while (m < n) {
          if (abs(e(m)) <= eps*tst1) {
            break;
          }
          m++;
        }
        
        
        // If m == l, d(l) is an eigenvalue,
        // otherwise, iterate.
        
        if (m > l) {
          int iter = 0;
          do {
            iter = iter + 1;  // (Could check iteration count here.)
            
            // Compute implicit shift
            
            Real g = d(l);
            Real p = (d(l+1) - g) / (2.0 * e(l));
            Real r = hypot(p,1.0);
            if (p < 0) {
              r = -r;
            }
            d(l) = e(l) / (p + r);
            d(l+1) = e(l) * (p + r);
            Real dl1 = d(l+1);
            Real h = g - d(l);
            for (int i = l+2; i < n; i++) {
              d(i) -= h;
            }
            f = f + h;
            
            // Implicit QL transformation.
            
            p = d(m);
            Real c = 1.0;
            Real c2 = c;
            Real c3 = c;
            Real el1 = e(l+1);
            Real s = 0.0;
            Real s2 = 0.0;
            for (int i = m-1; i >= l; i--) {
              c3 = c2;
              c2 = c;
              s2 = s;
              g = c * e(i);
              h = c * p;
              r = hypot(p,e(i));
              e(i+1) = s * r;
              s = e(i) / r;
              c = p / r;
              p = c * d(i) - s * g;
              d(i+1) = h + s * (c * g + s * d(i));
              
              // Accumulate transformation.
              
              for (int k = 0; k < n; k++) {
                h = V(k,i+1);
                V(k,i+1) = s * V(k,i) + c * h;
                V(k,i) = c * V(k,i) - s * h;
              }
            }
            p = -s * s2 * c3 * el1 * e(l) / dl1;
            e(l) = s * p;
            d(l) = c * p;
            
            // Check for convergence.
            
          } while (abs(e(l)) > eps*tst1);
        }
        d(l) = d(l) + f;
        e(l) = 0.0;
      }
      
      // Sort eigenvalues and corresponding vectors.
      
      for (int i = 0; i < n-1; i++) {
        int k = i;
        Real p = d(i);
        for (int j = i+1; j < n; j++) {
          if (d(j) < p) {
            k = j;
            p = d(j);
          }
        }
        if (k != i) {
          d(k) = d(i);
          d(i) = p;
          for (int j = 0; j < n; j++) {
            p = V(j,i);
            V(j,i) = V(j,k);
            V(j,k) = p;
          }
        }
      }
    }
    
    // Nonsymmetric reduction to Hessenberg form.
    
    void orthes () {
      
      //  This is derived from the Algol procedures orthes and ortran,
      //  by Martin and Wilkinson, Handbook for Auto. Comp.,
      //  Vol.ii-Linear Algebra, and the corresponding
      //  Fortran subroutines in EISPACK.
   
      int low = 0;
      int high = n-1;
      
      for (int m = low+1; m <= high-1; m++) {
        
        // Scale column.
        
        Real scale = 0.0;
        for (int i = m; i <= high; i++) {
          scale = scale + abs(H(i,m-1));
        }
        if (scale != 0.0) {
          
          // Compute Householder transformation.
          
          Real h = 0.0;
          for (int i = high; i >= m; i--) {
            ort(i) = H(i,m-1)/scale;
            h += ort(i) * ort(i);
          }
          Real g = sqrt(h);
          if (ort(m) > 0) {
            g = -g;
          }
          h = h - ort(m) * g;
          ort(m) = ort(m) - g;
          
          // Apply Householder similarity transformation
          // H = (I-u*u'/h)*H*(I-u*u')/h)
          
          for (int j = m; j < n; j++) {
            Real f = 0.0;
            for (int i = high; i >= m; i--) {
              f += ort(i)*H(i,j);
            }
            f = f/h;
            for (int i = m; i <= high; i++) {
              H(i,j) -= f*ort(i);
            }
          }
          
          for (int i = 0; i <= high; i++) {
            Real f = 0.0;
            for (int j = high; j >= m; j--) {
              f += ort(j)*H(i,j);
            }
            f = f/h;
            for (int j = m; j <= high; j++) {
              H(i,j) -= f*ort(j);
            }
          }
          ort(m) = scale*ort(m);
          H(m,m-1) = scale*g;
        }
      }
      
      // Accumulate transformations (Algol's ortran).
      
      for (int i = 0; i < n; i++) {
        for (int j = 0; j < n; j++) {
          V(i,j) = (i == j ? 1.0 : 0.0);
        }
      }
      
      for (int m = high-1; m >= low+1; m--) {
        if (H(m,m-1) != 0.0) {
          for (int i = m+1; i <= high; i++) {
            ort(i) = H(i,m-1);
          }
          for (int j = m; j <= high; j++) {
            Real g = 0.0;
            for (int i = m; i <= high; i++) {
              g += ort(i) * V(i,j);
            }
            // Double division avoids possible underflow
            g = (g / ort(m)) / H(m,m-1);
            for (int i = m; i <= high; i++) {
              V(i,j) += g * ort(i);
            }
          }
        }
      }
    }
    
    
    // Complex scalar division.
    
    Real cdivr, cdivi;
    void cdiv(Real xr, Real xi, Real yr, Real yi) {
      Real r,d;
      if (abs(yr) > abs(yi)) {
        r = yi/yr;
        d = yr + r*yi;
        cdivr = (xr + r*xi)/d;
        cdivi = (xi - r*xr)/d;
      } else {
        r = yr/yi;
        d = yi + r*yr;
        cdivr = (r*xr + xi)/d;
        cdivi = (r*xi - xr)/d;
      }
    }
    
    
    // Nonsymmetric reduction from Hessenberg to real Schur form.
    
    void hqr2 () {
      
      //  This is derived from the Algol procedure hqr2,
      //  by Martin and Wilkinson, Handbook for Auto. Comp.,
      //  Vol.ii-Linear Algebra, and the corresponding
      //  Fortran subroutine in EISPACK.
      
      // Initialize
      
      int nn = this->n;
      int n = nn-1;
      int low = 0;
      int high = nn-1;
      Real eps = pow(2.0,-52.0);
      Real exshift = 0.0;
      Real p=0,q=0,r=0,s=0,z=0,t,w,x,y;
      
      // Store roots isolated by balanc and compute matrix norm
      
      Real norm = 0.0;
      for (int i = 0; i < nn; i++) {
        if ((i < low) || (i > high)) {
          d(i) = H(i,i);
          e(i) = 0.0;
        }
        for (int j = max(i-1,0); j < nn; j++) {
          norm = norm + abs(H(i,j));
        }
      }
      
      // Outer loop over eigenvalue index
   
      int iter = 0;
      while (n >= low) {
        
        // Look for single small sub-diagonal element
        
        int l = n;
        while (l > low) {
          s = abs(H(l-1,l-1)) + abs(H(l,l));
          if (s == 0.0) {
            s = norm;
          }
          if (abs(H(l,l-1)) < eps * s) {
            break;
          }
          l--;
        }
        
        // Check for convergence
        // One root found
        
        if (l == n) {
          H(n,n) = H(n,n) + exshift;
          d(n) = H(n,n);
          e(n) = 0.0;
          n--;
          iter = 0;
          
          // Two roots found
          
        } else if (l == n-1) {
          w = H(n,n-1) * H(n-1,n);
          p = (H(n-1,n-1) - H(n,n)) / 2.0;
          q = p * p + w;
          z = sqrt(abs(q));
          H(n,n) = H(n,n) + exshift;
          H(n-1,n-1) = H(n-1,n-1) + exshift;
          x = H(n,n);
          
          // Real pair
          
          if (q >= 0) {
            if (p >= 0) {
              z = p + z;
            } else {
              z = p - z;
            }
            d(n-1) = x + z;
            d(n) = d(n-1);
            if (z != 0.0) {
              d(n) = x - w / z;
            }
            e(n-1) = 0.0;
            e(n) = 0.0;
            x = H(n,n-1);
            s = abs(x) + abs(z);
            p = x / s;
            q = z / s;
            r = sqrt(p * p+q * q);
            p = p / r;
            q = q / r;
            
            // Row modification
            
            for (int j = n-1; j < nn; j++) {
              z = H(n-1,j);
              H(n-1,j) = q * z + p * H(n,j);
              H(n,j) = q * H(n,j) - p * z;
            }
            
            // Column modification
            
            for (int i = 0; i <= n; i++) {
              z = H(i,n-1);
              H(i,n-1) = q * z + p * H(i,n);
              H(i,n) = q * H(i,n) - p * z;
            }
            
            // Accumulate transformations
            
            for (int i = low; i <= high; i++) {
              z = V(i,n-1);
              V(i,n-1) = q * z + p * V(i,n);
              V(i,n) = q * V(i,n) - p * z;
            }
            
            // Complex pair
            
          } else {
            d(n-1) = x + p;
            d(n) = x + p;
            e(n-1) = z;
            e(n) = -z;
          }
          n = n - 2;
          iter = 0;
          
          // No convergence yet
          
        } else {
          
          // Form shift
          
          x = H(n,n);
          y = 0.0;
          w = 0.0;
          if (l < n) {
            y = H(n-1,n-1);
            w = H(n,n-1) * H(n-1,n);
          }
          
          // Wilkinson's original ad hoc shift
          
          if (iter == 10) {
            exshift += x;
            for (int i = low; i <= n; i++) {
              H(i,i) -= x;
            }
            s = abs(H(n,n-1)) + abs(H(n-1,n-2));
            x = y = 0.75 * s;
            w = -0.4375 * s * s;
          }
          
          // MATLAB's new ad hoc shift
          
          if (iter == 30) {
            s = (y - x) / 2.0;
            s = s * s + w;
            if (s > 0) {
              s = sqrt(s);
              if (y < x) {
                s = -s;
              }
              s = x - w / ((y - x) / 2.0 + s);
              for (int i = low; i <= n; i++) {
                H(i,i) -= s;
              }
              exshift += s;
              x = y = w = 0.964;
            }
          }
          
          iter = iter + 1;   // (Could check iteration count here.)
          
          // Look for two consecutive small sub-diagonal elements
          
          int m = n-2;
          while (m >= l) {
            z = H(m,m);
            r = x - z;
            s = y - z;
            p = (r * s - w) / H(m+1,m) + H(m,m+1);
            q = H(m+1,m+1) - z - r - s;
            r = H(m+2,m+1);
            s = abs(p) + abs(q) + abs(r);
            p = p / s;
            q = q / s;
            r = r / s;
            if (m == l) {
              break;
            }
            if (abs(H(m,m-1)) * (abs(q) + abs(r)) <
                eps * (abs(p) * (abs(H(m-1,m-1)) + abs(z) +
                                 abs(H(m+1,m+1))))) {
              break;
            }
            m--;
          }
          
          for (int i = m+2; i <= n; i++) {
            H(i,i-2) = 0.0;
            if (i > m+2) {
              H(i,i-3) = 0.0;
            }
          }
          
          // Double QR step involving rows l:n and columns m:n
          
          for (int k = m; k <= n-1; k++) {
            int notlast = (k != n-1);
            if (k != m) {
              p = H(k,k-1);
              q = H(k+1,k-1);
              r = (notlast ? H(k+2,k-1) : 0.0);
              x = abs(p) + abs(q) + abs(r);
              if (x != 0.0) {
                p = p / x;
                q = q / x;
                r = r / x;
              }
            }
            if (x == 0.0) {
              break;
            }
            s = sqrt(p * p + q * q + r * r);
            if (p < 0) {
              s = -s;
            }
            if (s != 0) {
              if (k != m) {
                H(k,k-1) = -s * x;
              } else if (l != m) {
                H(k,k-1) = -H(k,k-1);
              }
              p = p + s;
              x = p / s;
              y = q / s;
              z = r / s;
              q = q / p;
              r = r / p;
              
              // Row modification
              
              for (int j = k; j < nn; j++) {
                p = H(k,j) + q * H(k+1,j);
                if (notlast) {
                  p = p + r * H(k+2,j);
                  H(k+2,j) = H(k+2,j) - p * z;
                }
                H(k,j) = H(k,j) - p * x;
                H(k+1,j) = H(k+1,j) - p * y;
              }
              
              // Column modification
              
              for (int i = 0; i <= min(n,k+3); i++) {
                p = x * H(i,k) + y * H(i,k+1);
                if (notlast) {
                  p = p + z * H(i,k+2);
                  H(i,k+2) = H(i,k+2) - p * r;
                }
                H(i,k) = H(i,k) - p;
                H(i,k+1) = H(i,k+1) - p * q;
              }
              
              // Accumulate transformations
              
              for (int i = low; i <= high; i++) {
                p = x * V(i,k) + y * V(i,k+1);
                if (notlast) {
                  p = p + z * V(i,k+2);
                  V(i,k+2) = V(i,k+2) - p * r;
                }
                V(i,k) = V(i,k) - p;
                V(i,k+1) = V(i,k+1) - p * q;
              }
            }  // (s != 0)
          }  // k loop
        }  // check convergence
      }  // while (n >= low)
      
      // Backsubstitute to find vectors of upper triangular form
      
      if (norm == 0.0) {
        return;
      }
      
      for (n = nn-1; n >= 0; n--) {
        p = d(n);
        q = e(n);
        
        // Real vector
        
        if (q == 0) {
          int l = n;
          H(n,n) = 1.0;
          for (int i = n-1; i >= 0; i--) {
            w = H(i,i) - p;
            r = 0.0;
            for (int j = l; j <= n; j++) {
              r = r + H(i,j) * H(j,n);
            }
            if (e(i) < 0.0) {
              z = w;
              s = r;
            } else {
              l = i;
              if (e(i) == 0.0) {
                if (w != 0.0) {
                  H(i,n) = -r / w;
                } else {
                  H(i,n) = -r / (eps * norm);
                }
                
                // Solve real equations
                
              } else {
                x = H(i,i+1);
                y = H(i+1,i);
                q = (d(i) - p) * (d(i) - p) + e(i) * e(i);
                t = (x * s - z * r) / q;
                H(i,n) = t;
                if (abs(x) > abs(z)) {
                  H(i+1,n) = (-r - w * t) / x;
                } else {
                  H(i+1,n) = (-s - y * t) / z;
                }
              }
              
              // Overflow control
              
              t = abs(H(i,n));
              if ((eps * t) * t > 1) {
                for (int j = i; j <= n; j++) {
                  H(j,n) = H(j,n) / t;
                }
              }
            }
          }
          
          // Complex vector
          
        } else if (q < 0) {
          int l = n-1;
          
          // Last vector component imaginary so matrix is triangular
          
          if (abs(H(n,n-1)) > abs(H(n-1,n))) {
            H(n-1,n-1) = q / H(n,n-1);
            H(n-1,n) = -(H(n,n) - p) / H(n,n-1);
          } else {
            cdiv(0.0,-H(n-1,n),H(n-1,n-1)-p,q);
            H(n-1,n-1) = cdivr;
            H(n-1,n) = cdivi;
          }
          H(n,n-1) = 0.0;
          H(n,n) = 1.0;
          for (int i = n-2; i >= 0; i--) {
            Real ra,sa,vr,vi;
            ra = 0.0;
            sa = 0.0;
            for (int j = l; j <= n; j++) {
              ra = ra + H(i,j) * H(j,n-1);
              sa = sa + H(i,j) * H(j,n);
            }
            w = H(i,i) - p;
            
            if (e(i) < 0.0) {
              z = w;
              r = ra;
              s = sa;
            } else {
              l = i;
              if (e(i) == 0) {
                cdiv(-ra,-sa,w,q);
                H(i,n-1) = cdivr;
                H(i,n) = cdivi;
              } else {
                
                // Solve complex equations
                
                x = H(i,i+1);
                y = H(i+1,i);
                vr = (d(i) - p) * (d(i) - p) + e(i) * e(i) - q * q;
                vi = (d(i) - p) * 2.0 * q;
                if ((vr == 0.0) && (vi == 0.0)) {
                  vr = eps * norm * (abs(w) + abs(q) +
                                     abs(x) + abs(y) + abs(z));
                }
                cdiv(x*r-z*ra+q*sa,x*s-z*sa-q*ra,vr,vi);
                H(i,n-1) = cdivr;
                H(i,n) = cdivi;
                if (abs(x) > (abs(z) + abs(q))) {
                  H(i+1,n-1) = (-ra - w * H(i,n-1) + q * H(i,n)) / x;
                  H(i+1,n) = (-sa - w * H(i,n) - q * H(i,n-1)) / x;
                } else {
                  cdiv(-r-y*H(i,n-1),-s-y*H(i,n),z,q);
                  H(i+1,n-1) = cdivr;
                  H(i+1,n) = cdivi;
                }
              }
              
              // Overflow control
              
              t = max(abs(H(i,n-1)),abs(H(i,n)));
              if ((eps * t) * t > 1) {
                for (int j = i; j <= n; j++) {
                  H(j,n-1) = H(j,n-1) / t;
                  H(j,n) = H(j,n) / t;
                }
              }
            }
          }
        }
      }
      
      // Vectors of isolated roots
      
      for (int i = 0; i < nn; i++) {
        if (i < low || i > high) {
          for (int j = i; j < nn; j++) {
            V(i,j) = H(i,j);
          }
        }
      }
      
      // Back transformation to get eigenvectors of original matrix
      
      for (int j = nn-1; j >= low; j--) {
        for (int i = low; i <= high; i++) {
          z = 0.0;
          for (int k = low; k <= min(j,high); k++) {
            z = z + V(i,k) * H(k,j);
            }
          V(i,j) = z;
        }
      }
    }
    
  public:

    
    /** Check for symmetry, then construct the eigenvalue decomposition
        @param A    Square real (non-complex) matrix
    */    
    Eigenvalue(const DynamicRectMatrix<Real> &A) {
      n = A.getNCol();
      V = DynamicRectMatrix<Real>(n,n);
      d = DynamicVector<Real>(n);
      e = DynamicVector<Real>(n);
      
      issymmetric = 1;
      for (int j = 0; (j < n) && issymmetric; j++) {
        for (int i = 0; (i < n) && issymmetric; i++) {
          issymmetric = (A(i,j) == A(j,i));
        }
      }
      
      if (issymmetric) {
        for (int i = 0; i < n; i++) {
          for (int j = 0; j < n; j++) {
            V(i,j) = A(i,j);
          }
        }
        
        // Tridiagonalize.
        tred2();
        
        // Diagonalize.
        tql2();
        
      } else {
        H = DynamicRectMatrix<Real>(n,n);
        ort = DynamicVector<Real>(n);
        
        for (int j = 0; j < n; j++) {
          for (int i = 0; i < n; i++) {
            H(i,j) = A(i,j);
          }
        }
        
        // Reduce to Hessenberg form.
        orthes();
        
        // Reduce Hessenberg to real Schur form.
        hqr2();
      }
    }
    
    
    /** Return the eigenvector matrix
        @return     V
    */    
    void getV (DynamicRectMatrix<Real> &V_) {
      V_ = V;
      return;
    }
    
    /** Return the real parts of the eigenvalues
        @return     real(diag(D))
    */    
    void getRealEigenvalues (DynamicVector<Real> &d_) {
      d_ = d;
      return ;
    }
    
    /** Return the imaginary parts of the eigenvalues
        in parameter e_.
        
        @pararm e_: new matrix with imaginary parts of the eigenvalues.
    */
    void getImagEigenvalues (DynamicVector<Real> &e_) {
      e_ = e;
      return;
    }
    
   
    /** 
        Computes the block diagonal eigenvalue matrix.
        If the original matrix A is not symmetric, then the eigenvalue 
        matrix D is block diagonal with the real eigenvalues in 1-by-1 
        blocks and any complex eigenvalues,
        a + i*b, in 2-by-2 blocks, (a, b; -b, a).  That is, if the complex
        eigenvalues look like
<pre>

          u + iv     .        .          .      .    .
            .      u - iv     .          .      .    .
            .        .      a + ib       .      .    .
            .        .        .        a - ib   .    .
            .        .        .          .      x    .
            .        .        .          .      .    y
</pre>
        then D looks like
<pre>

            u        v        .          .      .    .
           -v        u        .          .      .    . 
            .        .        a          b      .    .
            .        .       -b          a      .    .
            .        .        .          .      x    .
            .        .        .          .      .    y
</pre>
    This keeps V a real matrix in both symmetric and non-symmetric
    cases, and A*V = V*D.

        @param D: upon return, the matrix is filled with the block diagonal 
        eigenvalue matrix.
        
*/
    void getD (DynamicRectMatrix<Real> &D) {
      D = DynamicRectMatrix<Real>(n,n);
      for (int i = 0; i < n; i++) {
        for (int j = 0; j < n; j++) {
          D(i,j) = 0.0;
        }
        D(i,i) = d(i);
        if (e(i) > 0) {
          D(i,i+1) = e(i);
        } else if (e(i) < 0) {
          D(i,i-1) = e(i);
        }
      }
    }
  };
  
} //namespace JAMA


#endif
// JAMA_EIG_H
Revision:	1358
Committed:	Thu Aug 13 21:21:51 2009 UTC (16 years, 2 months ago) by gezelter
File size:	27568 byte(s)
Log Message:	Adding hybrid analytic / companion matrix polynomial root solver adapted from David Eberly's Wild Magic code
#	User	Rev	Content
1	gezelter	1358	#ifndef JAMA_EIG_H
2			#define JAMA_EIG_H
3
4			#include "math/DynamicRectMatrix.hpp"
5
6			#include <algorithm>
7			// for min(), max() below
8			#include <cmath>
9			// for abs() below
10
11			using namespace oopse;
12			using namespace std;
13
14			namespace JAMA
15			{
16
17			/**
18
19			Computes eigenvalues and eigenvectors of a real (non-complex)
20			matrix.
21			<P>
22			If A is symmetric, then A = VDV' where the eigenvalue matrix D is
23			diagonal and the eigenvector matrix V is orthogonal. That is,
24			the diagonal values of D are the eigenvalues, and
25			V*V' = I, where I is the identity matrix. The columns of V
26			represent the eigenvectors in the sense that AV = VD.
27
28			<P>
29			If A is not symmetric, then the eigenvalue matrix D is block diagonal
30			with the real eigenvalues in 1-by-1 blocks and any complex eigenvalues,
31			a + i*b, in 2-by-2 blocks, [a, b; -b, a]. That is, if the complex
32			eigenvalues look like
33			<pre>
34
35			u + iv . . . . .
36			. u - iv . . . .
37			. . a + ib . . .
38			. . . a - ib . .
39			. . . . x .
40			. . . . . y
41			</pre>
42			then D looks like
43			<pre>
44
45			u v . . . .
46			-v u . . . .
47			. . a b . .
48			. . -b a . .
49			. . . . x .
50			. . . . . y
51			</pre>
52			This keeps V a real matrix in both symmetric and non-symmetric
53			cases, and AV = VD.
54
55			<p>
56			The matrix V may be badly
57			conditioned, or even singular, so the validity of the equation
58			A = VDinverse(V) depends upon the condition number of V.
59
60			<p>
61			(Adapted from JAMA, a Java Matrix Library, developed by jointly
62			by the Mathworks and NIST; see http://math.nist.gov/javanumerics/jama).
63			**/
64
65			template <class Real>
66			class Eigenvalue
67			{
68
69
70			/** Row and column dimension (square matrix). */
71			int n;
72
73			int issymmetric; /* boolean*/
74
75			/** Arrays for internal storage of eigenvalues. */
76
77			DynamicVector<Real> d; /* real part */
78			DynamicVector<Real> e; /* img part */
79
80			/** Array for internal storage of eigenvectors. */
81			DynamicRectMatrix<Real> V;
82
83			/** Array for internal storage of nonsymmetric Hessenberg form.
84			@serial internal storage of nonsymmetric Hessenberg form.
85			*/
86			DynamicRectMatrix<Real> H;
87
88
89			/** Working storage for nonsymmetric algorithm.
90			@serial working storage for nonsymmetric algorithm.
91			*/
92			DynamicVector<Real> ort;
93
94
95			// Symmetric Householder reduction to tridiagonal form.
96
97			void tred2() {
98
99			// This is derived from the Algol procedures tred2 by
100			// Bowdler, Martin, Reinsch, and Wilkinson, Handbook for
101			// Auto. Comp., Vol.ii-Linear Algebra, and the corresponding
102			// Fortran subroutine in EISPACK.
103
104			for (int j = 0; j < n; j++) {
105			d(j) = V(n-1,j);
106			}
107
108			// Householder reduction to tridiagonal form.
109
110			for (int i = n-1; i > 0; i--) {
111
112			// Scale to avoid under/overflow.
113
114			Real scale = 0.0;
115			Real h = 0.0;
116			for (int k = 0; k < i; k++) {
117			scale = scale + abs(d(k));
118			}
119			if (scale == 0.0) {
120			e(i) = d(i-1);
121			for (int j = 0; j < i; j++) {
122			d(j) = V(i-1,j);
123			V(i,j) = 0.0;
124			V(j,i) = 0.0;
125			}
126			} else {
127
128			// Generate Householder vector.
129
130			for (int k = 0; k < i; k++) {
131			d(k) /= scale;
132			h += d(k) * d(k);
133			}
134			Real f = d(i-1);
135			Real g = sqrt(h);
136			if (f > 0) {
137			g = -g;
138			}
139			e(i) = scale * g;
140			h = h - f * g;
141			d(i-1) = f - g;
142			for (int j = 0; j < i; j++) {
143			e(j) = 0.0;
144			}
145
146			// Apply similarity transformation to remaining columns.
147
148			for (int j = 0; j < i; j++) {
149			f = d(j);
150			V(j,i) = f;
151			g = e(j) + V(j,j) * f;
152			for (int k = j+1; k <= i-1; k++) {
153			g += V(k,j) * d(k);
154			e(k) += V(k,j) * f;
155			}
156			e(j) = g;
157			}
158			f = 0.0;
159			for (int j = 0; j < i; j++) {
160			e(j) /= h;
161			f += e(j) * d(j);
162			}
163			Real hh = f / (h + h);
164			for (int j = 0; j < i; j++) {
165			e(j) -= hh * d(j);
166			}
167			for (int j = 0; j < i; j++) {
168			f = d(j);
169			g = e(j);
170			for (int k = j; k <= i-1; k++) {
171			V(k,j) -= (f * e(k) + g * d(k));
172			}
173			d(j) = V(i-1,j);
174			V(i,j) = 0.0;
175			}
176			}
177			d(i) = h;
178			}
179
180			// Accumulate transformations.
181
182			for (int i = 0; i < n-1; i++) {
183			V(n-1,i) = V(i,i);
184			V(i,i) = 1.0;
185			Real h = d(i+1);
186			if (h != 0.0) {
187			for (int k = 0; k <= i; k++) {
188			d(k) = V(k,i+1) / h;
189			}
190			for (int j = 0; j <= i; j++) {
191			Real g = 0.0;
192			for (int k = 0; k <= i; k++) {
193			g += V(k,i+1) * V(k,j);
194			}
195			for (int k = 0; k <= i; k++) {
196			V(k,j) -= g * d(k);
197			}
198			}
199			}
200			for (int k = 0; k <= i; k++) {
201			V(k,i+1) = 0.0;
202			}
203			}
204			for (int j = 0; j < n; j++) {
205			d(j) = V(n-1,j);
206			V(n-1,j) = 0.0;
207			}
208			V(n-1,n-1) = 1.0;
209			e(0) = 0.0;
210			}
211
212			// Symmetric tridiagonal QL algorithm.
213
214			void tql2 () {
215
216			// This is derived from the Algol procedures tql2, by
217			// Bowdler, Martin, Reinsch, and Wilkinson, Handbook for
218			// Auto. Comp., Vol.ii-Linear Algebra, and the corresponding
219			// Fortran subroutine in EISPACK.
220
221			for (int i = 1; i < n; i++) {
222			e(i-1) = e(i);
223			}
224			e(n-1) = 0.0;
225
226			Real f = 0.0;
227			Real tst1 = 0.0;
228			Real eps = pow(2.0,-52.0);
229			for (int l = 0; l < n; l++) {
230
231			// Find small subdiagonal element
232
233			tst1 = max(tst1,abs(d(l)) + abs(e(l)));
234			int m = l;
235
236			// Original while-loop from Java code
237			while (m < n) {
238			if (abs(e(m)) <= eps*tst1) {
239			break;
240			}
241			m++;
242			}
243
244
245			// If m == l, d(l) is an eigenvalue,
246			// otherwise, iterate.
247
248			if (m > l) {
249			int iter = 0;
250			do {
251			iter = iter + 1; // (Could check iteration count here.)
252
253			// Compute implicit shift
254
255			Real g = d(l);
256			Real p = (d(l+1) - g) / (2.0 * e(l));
257			Real r = hypot(p,1.0);
258			if (p < 0) {
259			r = -r;
260			}
261			d(l) = e(l) / (p + r);
262			d(l+1) = e(l) * (p + r);
263			Real dl1 = d(l+1);
264			Real h = g - d(l);
265			for (int i = l+2; i < n; i++) {
266			d(i) -= h;
267			}
268			f = f + h;
269
270			// Implicit QL transformation.
271
272			p = d(m);
273			Real c = 1.0;
274			Real c2 = c;
275			Real c3 = c;
276			Real el1 = e(l+1);
277			Real s = 0.0;
278			Real s2 = 0.0;
279			for (int i = m-1; i >= l; i--) {
280			c3 = c2;
281			c2 = c;
282			s2 = s;
283			g = c * e(i);
284			h = c * p;
285			r = hypot(p,e(i));
286			e(i+1) = s * r;
287			s = e(i) / r;
288			c = p / r;
289			p = c * d(i) - s * g;
290			d(i+1) = h + s * (c * g + s * d(i));
291
292			// Accumulate transformation.
293
294			for (int k = 0; k < n; k++) {
295			h = V(k,i+1);
296			V(k,i+1) = s * V(k,i) + c * h;
297			V(k,i) = c * V(k,i) - s * h;
298			}
299			}
300			p = -s * s2 * c3 * el1 * e(l) / dl1;
301			e(l) = s * p;
302			d(l) = c * p;
303
304			// Check for convergence.
305
306			} while (abs(e(l)) > eps*tst1);
307			}
308			d(l) = d(l) + f;
309			e(l) = 0.0;
310			}
311
312			// Sort eigenvalues and corresponding vectors.
313
314			for (int i = 0; i < n-1; i++) {
315			int k = i;
316			Real p = d(i);
317			for (int j = i+1; j < n; j++) {
318			if (d(j) < p) {
319			k = j;
320			p = d(j);
321			}
322			}
323			if (k != i) {
324			d(k) = d(i);
325			d(i) = p;
326			for (int j = 0; j < n; j++) {
327			p = V(j,i);
328			V(j,i) = V(j,k);
329			V(j,k) = p;
330			}
331			}
332			}
333			}
334
335			// Nonsymmetric reduction to Hessenberg form.
336
337			void orthes () {
338
339			// This is derived from the Algol procedures orthes and ortran,
340			// by Martin and Wilkinson, Handbook for Auto. Comp.,
341			// Vol.ii-Linear Algebra, and the corresponding
342			// Fortran subroutines in EISPACK.
343
344			int low = 0;
345			int high = n-1;
346
347			for (int m = low+1; m <= high-1; m++) {
348
349			// Scale column.
350
351			Real scale = 0.0;
352			for (int i = m; i <= high; i++) {
353			scale = scale + abs(H(i,m-1));
354			}
355			if (scale != 0.0) {
356
357			// Compute Householder transformation.
358
359			Real h = 0.0;
360			for (int i = high; i >= m; i--) {
361			ort(i) = H(i,m-1)/scale;
362			h += ort(i) * ort(i);
363			}
364			Real g = sqrt(h);
365			if (ort(m) > 0) {
366			g = -g;
367			}
368			h = h - ort(m) * g;
369			ort(m) = ort(m) - g;
370
371			// Apply Householder similarity transformation
372			// H = (I-uu'/h)H(I-uu')/h)
373
374			for (int j = m; j < n; j++) {
375			Real f = 0.0;
376			for (int i = high; i >= m; i--) {
377			f += ort(i)*H(i,j);
378			}
379			f = f/h;
380			for (int i = m; i <= high; i++) {
381			H(i,j) -= f*ort(i);
382			}
383			}
384
385			for (int i = 0; i <= high; i++) {
386			Real f = 0.0;
387			for (int j = high; j >= m; j--) {
388			f += ort(j)*H(i,j);
389			}
390			f = f/h;
391			for (int j = m; j <= high; j++) {
392			H(i,j) -= f*ort(j);
393			}
394			}
395			ort(m) = scale*ort(m);
396			H(m,m-1) = scale*g;
397			}
398			}
399
400			// Accumulate transformations (Algol's ortran).
401
402			for (int i = 0; i < n; i++) {
403			for (int j = 0; j < n; j++) {
404			V(i,j) = (i == j ? 1.0 : 0.0);
405			}
406			}
407
408			for (int m = high-1; m >= low+1; m--) {
409			if (H(m,m-1) != 0.0) {
410			for (int i = m+1; i <= high; i++) {
411			ort(i) = H(i,m-1);
412			}
413			for (int j = m; j <= high; j++) {
414			Real g = 0.0;
415			for (int i = m; i <= high; i++) {
416			g += ort(i) * V(i,j);
417			}
418			// Double division avoids possible underflow
419			g = (g / ort(m)) / H(m,m-1);
420			for (int i = m; i <= high; i++) {
421			V(i,j) += g * ort(i);
422			}
423			}
424			}
425			}
426			}
427
428
429			// Complex scalar division.
430
431			Real cdivr, cdivi;
432			void cdiv(Real xr, Real xi, Real yr, Real yi) {
433			Real r,d;
434			if (abs(yr) > abs(yi)) {
435			r = yi/yr;
436			d = yr + r*yi;
437			cdivr = (xr + r*xi)/d;
438			cdivi = (xi - r*xr)/d;
439			} else {
440			r = yr/yi;
441			d = yi + r*yr;
442			cdivr = (r*xr + xi)/d;
443			cdivi = (r*xi - xr)/d;
444			}
445			}
446
447
448			// Nonsymmetric reduction from Hessenberg to real Schur form.
449
450			void hqr2 () {
451
452			// This is derived from the Algol procedure hqr2,
453			// by Martin and Wilkinson, Handbook for Auto. Comp.,
454			// Vol.ii-Linear Algebra, and the corresponding
455			// Fortran subroutine in EISPACK.
456
457			// Initialize
458
459			int nn = this->n;
460			int n = nn-1;
461			int low = 0;
462			int high = nn-1;
463			Real eps = pow(2.0,-52.0);
464			Real exshift = 0.0;
465			Real p=0,q=0,r=0,s=0,z=0,t,w,x,y;
466
467			// Store roots isolated by balanc and compute matrix norm
468
469			Real norm = 0.0;
470			for (int i = 0; i < nn; i++) {
471			if ((i < low) \|\| (i > high)) {
472			d(i) = H(i,i);
473			e(i) = 0.0;
474			}
475			for (int j = max(i-1,0); j < nn; j++) {
476			norm = norm + abs(H(i,j));
477			}
478			}
479
480			// Outer loop over eigenvalue index
481
482			int iter = 0;
483			while (n >= low) {
484
485			// Look for single small sub-diagonal element
486
487			int l = n;
488			while (l > low) {
489			s = abs(H(l-1,l-1)) + abs(H(l,l));
490			if (s == 0.0) {
491			s = norm;
492			}
493			if (abs(H(l,l-1)) < eps * s) {
494			break;
495			}
496			l--;
497			}
498
499			// Check for convergence
500			// One root found
501
502			if (l == n) {
503			H(n,n) = H(n,n) + exshift;
504			d(n) = H(n,n);
505			e(n) = 0.0;
506			n--;
507			iter = 0;
508
509			// Two roots found
510
511			} else if (l == n-1) {
512			w = H(n,n-1) * H(n-1,n);
513			p = (H(n-1,n-1) - H(n,n)) / 2.0;
514			q = p * p + w;
515			z = sqrt(abs(q));
516			H(n,n) = H(n,n) + exshift;
517			H(n-1,n-1) = H(n-1,n-1) + exshift;
518			x = H(n,n);
519
520			// Real pair
521
522			if (q >= 0) {
523			if (p >= 0) {
524			z = p + z;
525			} else {
526			z = p - z;
527			}
528			d(n-1) = x + z;
529			d(n) = d(n-1);
530			if (z != 0.0) {
531			d(n) = x - w / z;
532			}
533			e(n-1) = 0.0;
534			e(n) = 0.0;
535			x = H(n,n-1);
536			s = abs(x) + abs(z);
537			p = x / s;
538			q = z / s;
539			r = sqrt(p * p+q * q);
540			p = p / r;
541			q = q / r;
542
543			// Row modification
544
545			for (int j = n-1; j < nn; j++) {
546			z = H(n-1,j);
547			H(n-1,j) = q * z + p * H(n,j);
548			H(n,j) = q * H(n,j) - p * z;
549			}
550
551			// Column modification
552
553			for (int i = 0; i <= n; i++) {
554			z = H(i,n-1);
555			H(i,n-1) = q * z + p * H(i,n);
556			H(i,n) = q * H(i,n) - p * z;
557			}
558
559			// Accumulate transformations
560
561			for (int i = low; i <= high; i++) {
562			z = V(i,n-1);
563			V(i,n-1) = q * z + p * V(i,n);
564			V(i,n) = q * V(i,n) - p * z;
565			}
566
567			// Complex pair
568
569			} else {
570			d(n-1) = x + p;
571			d(n) = x + p;
572			e(n-1) = z;
573			e(n) = -z;
574			}
575			n = n - 2;
576			iter = 0;
577
578			// No convergence yet
579
580			} else {
581
582			// Form shift
583
584			x = H(n,n);
585			y = 0.0;
586			w = 0.0;
587			if (l < n) {
588			y = H(n-1,n-1);
589			w = H(n,n-1) * H(n-1,n);
590			}
591
592			// Wilkinson's original ad hoc shift
593
594			if (iter == 10) {
595			exshift += x;
596			for (int i = low; i <= n; i++) {
597			H(i,i) -= x;
598			}
599			s = abs(H(n,n-1)) + abs(H(n-1,n-2));
600			x = y = 0.75 * s;
601			w = -0.4375 * s * s;
602			}
603
604			// MATLAB's new ad hoc shift
605
606			if (iter == 30) {
607			s = (y - x) / 2.0;
608			s = s * s + w;
609			if (s > 0) {
610			s = sqrt(s);
611			if (y < x) {
612			s = -s;
613			}
614			s = x - w / ((y - x) / 2.0 + s);
615			for (int i = low; i <= n; i++) {
616			H(i,i) -= s;
617			}
618			exshift += s;
619			x = y = w = 0.964;
620			}
621			}
622
623			iter = iter + 1; // (Could check iteration count here.)
624
625			// Look for two consecutive small sub-diagonal elements
626
627			int m = n-2;
628			while (m >= l) {
629			z = H(m,m);
630			r = x - z;
631			s = y - z;
632			p = (r * s - w) / H(m+1,m) + H(m,m+1);
633			q = H(m+1,m+1) - z - r - s;
634			r = H(m+2,m+1);
635			s = abs(p) + abs(q) + abs(r);
636			p = p / s;
637			q = q / s;
638			r = r / s;
639			if (m == l) {
640			break;
641			}
642			if (abs(H(m,m-1)) * (abs(q) + abs(r)) <
643			eps * (abs(p) * (abs(H(m-1,m-1)) + abs(z) +
644			abs(H(m+1,m+1))))) {
645			break;
646			}
647			m--;
648			}
649
650			for (int i = m+2; i <= n; i++) {
651			H(i,i-2) = 0.0;
652			if (i > m+2) {
653			H(i,i-3) = 0.0;
654			}
655			}
656
657			// Double QR step involving rows l:n and columns m:n
658
659			for (int k = m; k <= n-1; k++) {
660			int notlast = (k != n-1);
661			if (k != m) {
662			p = H(k,k-1);
663			q = H(k+1,k-1);
664			r = (notlast ? H(k+2,k-1) : 0.0);
665			x = abs(p) + abs(q) + abs(r);
666			if (x != 0.0) {
667			p = p / x;
668			q = q / x;
669			r = r / x;
670			}
671			}
672			if (x == 0.0) {
673			break;
674			}
675			s = sqrt(p * p + q * q + r * r);
676			if (p < 0) {
677			s = -s;
678			}
679			if (s != 0) {
680			if (k != m) {
681			H(k,k-1) = -s * x;
682			} else if (l != m) {
683			H(k,k-1) = -H(k,k-1);
684			}
685			p = p + s;
686			x = p / s;
687			y = q / s;
688			z = r / s;
689			q = q / p;
690			r = r / p;
691
692			// Row modification
693
694			for (int j = k; j < nn; j++) {
695			p = H(k,j) + q * H(k+1,j);
696			if (notlast) {
697			p = p + r * H(k+2,j);
698			H(k+2,j) = H(k+2,j) - p * z;
699			}
700			H(k,j) = H(k,j) - p * x;
701			H(k+1,j) = H(k+1,j) - p * y;
702			}
703
704			// Column modification
705
706			for (int i = 0; i <= min(n,k+3); i++) {
707			p = x * H(i,k) + y * H(i,k+1);
708			if (notlast) {
709			p = p + z * H(i,k+2);
710			H(i,k+2) = H(i,k+2) - p * r;
711			}
712			H(i,k) = H(i,k) - p;
713			H(i,k+1) = H(i,k+1) - p * q;
714			}
715
716			// Accumulate transformations
717
718			for (int i = low; i <= high; i++) {
719			p = x * V(i,k) + y * V(i,k+1);
720			if (notlast) {
721			p = p + z * V(i,k+2);
722			V(i,k+2) = V(i,k+2) - p * r;
723			}
724			V(i,k) = V(i,k) - p;
725			V(i,k+1) = V(i,k+1) - p * q;
726			}
727			} // (s != 0)
728			} // k loop
729			} // check convergence
730			} // while (n >= low)
731
732			// Backsubstitute to find vectors of upper triangular form
733
734			if (norm == 0.0) {
735			return;
736			}
737
738			for (n = nn-1; n >= 0; n--) {
739			p = d(n);
740			q = e(n);
741
742			// Real vector
743
744			if (q == 0) {
745			int l = n;
746			H(n,n) = 1.0;
747			for (int i = n-1; i >= 0; i--) {
748			w = H(i,i) - p;
749			r = 0.0;
750			for (int j = l; j <= n; j++) {
751			r = r + H(i,j) * H(j,n);
752			}
753			if (e(i) < 0.0) {
754			z = w;
755			s = r;
756			} else {
757			l = i;
758			if (e(i) == 0.0) {
759			if (w != 0.0) {
760			H(i,n) = -r / w;
761			} else {
762			H(i,n) = -r / (eps * norm);
763			}
764
765			// Solve real equations
766
767			} else {
768			x = H(i,i+1);
769			y = H(i+1,i);
770			q = (d(i) - p) * (d(i) - p) + e(i) * e(i);
771			t = (x * s - z * r) / q;
772			H(i,n) = t;
773			if (abs(x) > abs(z)) {
774			H(i+1,n) = (-r - w * t) / x;
775			} else {
776			H(i+1,n) = (-s - y * t) / z;
777			}
778			}
779
780			// Overflow control
781
782			t = abs(H(i,n));
783			if ((eps * t) * t > 1) {
784			for (int j = i; j <= n; j++) {
785			H(j,n) = H(j,n) / t;
786			}
787			}
788			}
789			}
790
791			// Complex vector
792
793			} else if (q < 0) {
794			int l = n-1;
795
796			// Last vector component imaginary so matrix is triangular
797
798			if (abs(H(n,n-1)) > abs(H(n-1,n))) {
799			H(n-1,n-1) = q / H(n,n-1);
800			H(n-1,n) = -(H(n,n) - p) / H(n,n-1);
801			} else {
802			cdiv(0.0,-H(n-1,n),H(n-1,n-1)-p,q);
803			H(n-1,n-1) = cdivr;
804			H(n-1,n) = cdivi;
805			}
806			H(n,n-1) = 0.0;
807			H(n,n) = 1.0;
808			for (int i = n-2; i >= 0; i--) {
809			Real ra,sa,vr,vi;
810			ra = 0.0;
811			sa = 0.0;
812			for (int j = l; j <= n; j++) {
813			ra = ra + H(i,j) * H(j,n-1);
814			sa = sa + H(i,j) * H(j,n);
815			}
816			w = H(i,i) - p;
817
818			if (e(i) < 0.0) {
819			z = w;
820			r = ra;
821			s = sa;
822			} else {
823			l = i;
824			if (e(i) == 0) {
825			cdiv(-ra,-sa,w,q);
826			H(i,n-1) = cdivr;
827			H(i,n) = cdivi;
828			} else {
829
830			// Solve complex equations
831
832			x = H(i,i+1);
833			y = H(i+1,i);
834			vr = (d(i) - p) * (d(i) - p) + e(i) * e(i) - q * q;
835			vi = (d(i) - p) * 2.0 * q;
836			if ((vr == 0.0) && (vi == 0.0)) {
837			vr = eps * norm * (abs(w) + abs(q) +
838			abs(x) + abs(y) + abs(z));
839			}
840			cdiv(xr-zra+qsa,xs-zsa-qra,vr,vi);
841			H(i,n-1) = cdivr;
842			H(i,n) = cdivi;
843			if (abs(x) > (abs(z) + abs(q))) {
844			H(i+1,n-1) = (-ra - w * H(i,n-1) + q * H(i,n)) / x;
845			H(i+1,n) = (-sa - w * H(i,n) - q * H(i,n-1)) / x;
846			} else {
847			cdiv(-r-yH(i,n-1),-s-yH(i,n),z,q);
848			H(i+1,n-1) = cdivr;
849			H(i+1,n) = cdivi;
850			}
851			}
852
853			// Overflow control
854
855			t = max(abs(H(i,n-1)),abs(H(i,n)));
856			if ((eps * t) * t > 1) {
857			for (int j = i; j <= n; j++) {
858			H(j,n-1) = H(j,n-1) / t;
859			H(j,n) = H(j,n) / t;
860			}
861			}
862			}
863			}
864			}
865			}
866
867			// Vectors of isolated roots
868
869			for (int i = 0; i < nn; i++) {
870			if (i < low \|\| i > high) {
871			for (int j = i; j < nn; j++) {
872			V(i,j) = H(i,j);
873			}
874			}
875			}
876
877			// Back transformation to get eigenvectors of original matrix
878
879			for (int j = nn-1; j >= low; j--) {
880			for (int i = low; i <= high; i++) {
881			z = 0.0;
882			for (int k = low; k <= min(j,high); k++) {
883			z = z + V(i,k) * H(k,j);
884			}
885			V(i,j) = z;
886			}
887			}
888			}
889
890			public:
891
892
893			/** Check for symmetry, then construct the eigenvalue decomposition
894			@param A Square real (non-complex) matrix
895			*/
896			Eigenvalue(const DynamicRectMatrix<Real> &A) {
897			n = A.getNCol();
898			V = DynamicRectMatrix<Real>(n,n);
899			d = DynamicVector<Real>(n);
900			e = DynamicVector<Real>(n);
901
902			issymmetric = 1;
903			for (int j = 0; (j < n) && issymmetric; j++) {
904			for (int i = 0; (i < n) && issymmetric; i++) {
905			issymmetric = (A(i,j) == A(j,i));
906			}
907			}
908
909			if (issymmetric) {
910			for (int i = 0; i < n; i++) {
911			for (int j = 0; j < n; j++) {
912			V(i,j) = A(i,j);
913			}
914			}
915
916			// Tridiagonalize.
917			tred2();
918
919			// Diagonalize.
920			tql2();
921
922			} else {
923			H = DynamicRectMatrix<Real>(n,n);
924			ort = DynamicVector<Real>(n);
925
926			for (int j = 0; j < n; j++) {
927			for (int i = 0; i < n; i++) {
928			H(i,j) = A(i,j);
929			}
930			}
931
932			// Reduce to Hessenberg form.
933			orthes();
934
935			// Reduce Hessenberg to real Schur form.
936			hqr2();
937			}
938			}
939
940
941			/** Return the eigenvector matrix
942			@return V
943			*/
944			void getV (DynamicRectMatrix<Real> &V_) {
945			V_ = V;
946			return;
947			}
948
949			/** Return the real parts of the eigenvalues
950			@return real(diag(D))
951			*/
952			void getRealEigenvalues (DynamicVector<Real> &d_) {
953			d_ = d;
954			return ;
955			}
956
957			/** Return the imaginary parts of the eigenvalues
958			in parameter e_.
959
960			@pararm e_: new matrix with imaginary parts of the eigenvalues.
961			*/
962			void getImagEigenvalues (DynamicVector<Real> &e_) {
963			e_ = e;
964			return;
965			}
966
967
968			/**
969			Computes the block diagonal eigenvalue matrix.
970			If the original matrix A is not symmetric, then the eigenvalue
971			matrix D is block diagonal with the real eigenvalues in 1-by-1
972			blocks and any complex eigenvalues,
973			a + i*b, in 2-by-2 blocks, (a, b; -b, a). That is, if the complex
974			eigenvalues look like
975			<pre>
976
977			u + iv . . . . .
978			. u - iv . . . .
979			. . a + ib . . .
980			. . . a - ib . .
981			. . . . x .
982			. . . . . y
983			</pre>
984			then D looks like
985			<pre>
986
987			u v . . . .
988			-v u . . . .
989			. . a b . .
990			. . -b a . .
991			. . . . x .
992			. . . . . y
993			</pre>
994			This keeps V a real matrix in both symmetric and non-symmetric
995			cases, and AV = VD.
996
997			@param D: upon return, the matrix is filled with the block diagonal
998			eigenvalue matrix.
999
1000			*/
1001			void getD (DynamicRectMatrix<Real> &D) {
1002			D = DynamicRectMatrix<Real>(n,n);
1003			for (int i = 0; i < n; i++) {
1004			for (int j = 0; j < n; j++) {
1005			D(i,j) = 0.0;
1006			}
1007			D(i,i) = d(i);
1008			if (e(i) > 0) {
1009			D(i,i+1) = e(i);
1010			} else if (e(i) < 0) {
1011			D(i,i-1) = e(i);
1012			}
1013			}
1014			}
1015			};
1016
1017			} //namespace JAMA
1018
1019
1020			#endif
1021			// JAMA_EIG_H