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
#	Content
1	#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