RealQZ.h

// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2012 Alexey Korepanov <kaikaikai@yandex.ru>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.

#ifndef EIGEN_REAL_QZ_H
#define EIGEN_REAL_QZ_H

namespace Eigen {

  template<typename _MatrixType> class RealQZ
  {
    public:
      typedef _MatrixType MatrixType;
      enum {
        RowsAtCompileTime = MatrixType::RowsAtCompileTime,
        ColsAtCompileTime = MatrixType::ColsAtCompileTime,
        Options = MatrixType::Options,
        MaxRowsAtCompileTime = MatrixType::MaxRowsAtCompileTime,
        MaxColsAtCompileTime = MatrixType::MaxColsAtCompileTime
      };
      typedef typename MatrixType::Scalar Scalar;
      typedef std::complex<typename NumTraits<Scalar>::Real> ComplexScalar;
      typedef Eigen::Index Index; 

      typedef Matrix<ComplexScalar, ColsAtCompileTime, 1, Options & ~RowMajor, MaxColsAtCompileTime, 1> EigenvalueType;
      typedef Matrix<Scalar, ColsAtCompileTime, 1, Options & ~RowMajor, MaxColsAtCompileTime, 1> ColumnVectorType;

      explicit RealQZ(Index size = RowsAtCompileTime==Dynamic ? 1 : RowsAtCompileTime) :
        m_S(size, size),
        m_T(size, size),
        m_Q(size, size),
        m_Z(size, size),
        m_workspace(size*2),
        m_maxIters(400),
        m_isInitialized(false)
        { }

      RealQZ(const MatrixType& A, const MatrixType& B, bool computeQZ = true) :
        m_S(A.rows(),A.cols()),
        m_T(A.rows(),A.cols()),
        m_Q(A.rows(),A.cols()),
        m_Z(A.rows(),A.cols()),
        m_workspace(A.rows()*2),
        m_maxIters(400),
        m_isInitialized(false) {
          compute(A, B, computeQZ);
        }

      const MatrixType& matrixQ() const {
        eigen_assert(m_isInitialized && "RealQZ is not initialized.");
        eigen_assert(m_computeQZ && "The matrices Q and Z have not been computed during the QZ decomposition.");
        return m_Q;
      }

      const MatrixType& matrixZ() const {
        eigen_assert(m_isInitialized && "RealQZ is not initialized.");
        eigen_assert(m_computeQZ && "The matrices Q and Z have not been computed during the QZ decomposition.");
        return m_Z;
      }

      const MatrixType& matrixS() const {
        eigen_assert(m_isInitialized && "RealQZ is not initialized.");
        return m_S;
      }

      const MatrixType& matrixT() const {
        eigen_assert(m_isInitialized && "RealQZ is not initialized.");
        return m_T;
      }

      RealQZ& compute(const MatrixType& A, const MatrixType& B, bool computeQZ = true);

      ComputationInfo info() const
      {
        eigen_assert(m_isInitialized && "RealQZ is not initialized.");
        return m_info;
      }

      Index iterations() const
      {
        eigen_assert(m_isInitialized && "RealQZ is not initialized.");
        return m_global_iter;
      }

      RealQZ& setMaxIterations(Index maxIters)
      {
        m_maxIters = maxIters;
        return *this;
      }

    private:

      MatrixType m_S, m_T, m_Q, m_Z;
      Matrix<Scalar,Dynamic,1> m_workspace;
      ComputationInfo m_info;
      Index m_maxIters;
      bool m_isInitialized;
      bool m_computeQZ;
      Scalar m_normOfT, m_normOfS;
      Index m_global_iter;

      typedef Matrix<Scalar,3,1> Vector3s;
      typedef Matrix<Scalar,2,1> Vector2s;
      typedef Matrix<Scalar,2,2> Matrix2s;
      typedef JacobiRotation<Scalar> JRs;

      void hessenbergTriangular();
      void computeNorms();
      Index findSmallSubdiagEntry(Index iu);
      Index findSmallDiagEntry(Index f, Index l);
      void splitOffTwoRows(Index i);
      void pushDownZero(Index z, Index f, Index l);
      void step(Index f, Index l, Index iter);

  }; // RealQZ

  template<typename MatrixType>
    void RealQZ<MatrixType>::hessenbergTriangular()
    {

      const Index dim = m_S.cols();

      // perform QR decomposition of T, overwrite T with R, save Q
      HouseholderQR<MatrixType> qrT(m_T);
      m_T = qrT.matrixQR();
      m_T.template triangularView<StrictlyLower>().setZero();
      m_Q = qrT.householderQ();
      // overwrite S with Q* S
      m_S.applyOnTheLeft(m_Q.adjoint());
      // init Z as Identity
      if (m_computeQZ)
        m_Z = MatrixType::Identity(dim,dim);
      // reduce S to upper Hessenberg with Givens rotations
      for (Index j=0; j<=dim-3; j++) {
        for (Index i=dim-1; i>=j+2; i--) {
          JRs G;
          // kill S(i,j)
          if(m_S.coeff(i,j) != 0)
          {
            G.makeGivens(m_S.coeff(i-1,j), m_S.coeff(i,j), &m_S.coeffRef(i-1, j));
            m_S.coeffRef(i,j) = Scalar(0.0);
            m_S.rightCols(dim-j-1).applyOnTheLeft(i-1,i,G.adjoint());
            m_T.rightCols(dim-i+1).applyOnTheLeft(i-1,i,G.adjoint());
            // update Q
            if (m_computeQZ)
              m_Q.applyOnTheRight(i-1,i,G);
          }
          // kill T(i,i-1)
          if(m_T.coeff(i,i-1)!=Scalar(0))
          {
            G.makeGivens(m_T.coeff(i,i), m_T.coeff(i,i-1), &m_T.coeffRef(i,i));
            m_T.coeffRef(i,i-1) = Scalar(0.0);
            m_S.applyOnTheRight(i,i-1,G);
            m_T.topRows(i).applyOnTheRight(i,i-1,G);
            // update Z
            if (m_computeQZ)
              m_Z.applyOnTheLeft(i,i-1,G.adjoint());
          }
        }
      }
    }

  template<typename MatrixType>
    inline void RealQZ<MatrixType>::computeNorms()
    {
      const Index size = m_S.cols();
      m_normOfS = Scalar(0.0);
      m_normOfT = Scalar(0.0);
      for (Index j = 0; j < size; ++j)
      {
        m_normOfS += m_S.col(j).segment(0, (std::min)(size,j+2)).cwiseAbs().sum();
        m_normOfT += m_T.row(j).segment(j, size - j).cwiseAbs().sum();
      }
    }


  template<typename MatrixType>
    inline Index RealQZ<MatrixType>::findSmallSubdiagEntry(Index iu)
    {
      using std::abs;
      Index res = iu;
      while (res > 0)
      {
        Scalar s = abs(m_S.coeff(res-1,res-1)) + abs(m_S.coeff(res,res));
        if (s == Scalar(0.0))
          s = m_normOfS;
        if (abs(m_S.coeff(res,res-1)) < NumTraits<Scalar>::epsilon() * s)
          break;
        res--;
      }
      return res;
    }

  template<typename MatrixType>
    inline Index RealQZ<MatrixType>::findSmallDiagEntry(Index f, Index l)
    {
      using std::abs;
      Index res = l;
      while (res >= f) {
        if (abs(m_T.coeff(res,res)) <= NumTraits<Scalar>::epsilon() * m_normOfT)
          break;
        res--;
      }
      return res;
    }

  template<typename MatrixType>
    inline void RealQZ<MatrixType>::splitOffTwoRows(Index i)
    {
      using std::abs;
      using std::sqrt;
      const Index dim=m_S.cols();
      if (abs(m_S.coeff(i+1,i))==Scalar(0))
        return;
      Index j = findSmallDiagEntry(i,i+1);
      if (j==i-1)
      {
        // block of (S T^{-1})
        Matrix2s STi = m_T.template block<2,2>(i,i).template triangularView<Upper>().
          template solve<OnTheRight>(m_S.template block<2,2>(i,i));
        Scalar p = Scalar(0.5)*(STi(0,0)-STi(1,1));
        Scalar q = p*p + STi(1,0)*STi(0,1);
        if (q>=0) {
          Scalar z = sqrt(q);
          // one QR-like iteration for ABi - lambda I
          // is enough - when we know exact eigenvalue in advance,
          // convergence is immediate
          JRs G;
          if (p>=0)
            G.makeGivens(p + z, STi(1,0));
          else
            G.makeGivens(p - z, STi(1,0));
          m_S.rightCols(dim-i).applyOnTheLeft(i,i+1,G.adjoint());
          m_T.rightCols(dim-i).applyOnTheLeft(i,i+1,G.adjoint());
          // update Q
          if (m_computeQZ)
            m_Q.applyOnTheRight(i,i+1,G);

          G.makeGivens(m_T.coeff(i+1,i+1), m_T.coeff(i+1,i));
          m_S.topRows(i+2).applyOnTheRight(i+1,i,G);
          m_T.topRows(i+2).applyOnTheRight(i+1,i,G);
          // update Z
          if (m_computeQZ)
            m_Z.applyOnTheLeft(i+1,i,G.adjoint());

          m_S.coeffRef(i+1,i) = Scalar(0.0);
          m_T.coeffRef(i+1,i) = Scalar(0.0);
        }
      }
      else
      {
        pushDownZero(j,i,i+1);
      }
    }

  template<typename MatrixType>
    inline void RealQZ<MatrixType>::pushDownZero(Index z, Index f, Index l)
    {
      JRs G;
      const Index dim = m_S.cols();
      for (Index zz=z; zz<l; zz++)
      {
        // push 0 down
        Index firstColS = zz>f ? (zz-1) : zz;
        G.makeGivens(m_T.coeff(zz, zz+1), m_T.coeff(zz+1, zz+1));
        m_S.rightCols(dim-firstColS).applyOnTheLeft(zz,zz+1,G.adjoint());
        m_T.rightCols(dim-zz).applyOnTheLeft(zz,zz+1,G.adjoint());
        m_T.coeffRef(zz+1,zz+1) = Scalar(0.0);
        // update Q
        if (m_computeQZ)
          m_Q.applyOnTheRight(zz,zz+1,G);
        // kill S(zz+1, zz-1)
        if (zz>f)
        {
          G.makeGivens(m_S.coeff(zz+1, zz), m_S.coeff(zz+1,zz-1));
          m_S.topRows(zz+2).applyOnTheRight(zz, zz-1,G);
          m_T.topRows(zz+1).applyOnTheRight(zz, zz-1,G);
          m_S.coeffRef(zz+1,zz-1) = Scalar(0.0);
          // update Z
          if (m_computeQZ)
            m_Z.applyOnTheLeft(zz,zz-1,G.adjoint());
        }
      }
      // finally kill S(l,l-1)
      G.makeGivens(m_S.coeff(l,l), m_S.coeff(l,l-1));
      m_S.applyOnTheRight(l,l-1,G);
      m_T.applyOnTheRight(l,l-1,G);
      m_S.coeffRef(l,l-1)=Scalar(0.0);
      // update Z
      if (m_computeQZ)
        m_Z.applyOnTheLeft(l,l-1,G.adjoint());
    }

  template<typename MatrixType>
    inline void RealQZ<MatrixType>::step(Index f, Index l, Index iter)
    {
      using std::abs;
      const Index dim = m_S.cols();

      // x, y, z
      Scalar x, y, z;
      if (iter==10)
      {
        // Wilkinson ad hoc shift
        const Scalar
          a11=m_S.coeff(f+0,f+0), a12=m_S.coeff(f+0,f+1),
          a21=m_S.coeff(f+1,f+0), a22=m_S.coeff(f+1,f+1), a32=m_S.coeff(f+2,f+1),
          b12=m_T.coeff(f+0,f+1),
          b11i=Scalar(1.0)/m_T.coeff(f+0,f+0),
          b22i=Scalar(1.0)/m_T.coeff(f+1,f+1),
          a87=m_S.coeff(l-1,l-2),
          a98=m_S.coeff(l-0,l-1),
          b77i=Scalar(1.0)/m_T.coeff(l-2,l-2),
          b88i=Scalar(1.0)/m_T.coeff(l-1,l-1);
        Scalar ss = abs(a87*b77i) + abs(a98*b88i),
               lpl = Scalar(1.5)*ss,
               ll = ss*ss;
        x = ll + a11*a11*b11i*b11i - lpl*a11*b11i + a12*a21*b11i*b22i
          - a11*a21*b12*b11i*b11i*b22i;
        y = a11*a21*b11i*b11i - lpl*a21*b11i + a21*a22*b11i*b22i 
          - a21*a21*b12*b11i*b11i*b22i;
        z = a21*a32*b11i*b22i;
      }
      else if (iter==16)
      {
        // another exceptional shift
        x = m_S.coeff(f,f)/m_T.coeff(f,f)-m_S.coeff(l,l)/m_T.coeff(l,l) + m_S.coeff(l,l-1)*m_T.coeff(l-1,l) /
          (m_T.coeff(l-1,l-1)*m_T.coeff(l,l));
        y = m_S.coeff(f+1,f)/m_T.coeff(f,f);
        z = 0;
      }
      else if (iter>23 && !(iter%8))
      {
        // extremely exceptional shift
        x = internal::random<Scalar>(-1.0,1.0);
        y = internal::random<Scalar>(-1.0,1.0);
        z = internal::random<Scalar>(-1.0,1.0);
      }
      else
      {
        // Compute the shifts: (x,y,z,0...) = (AB^-1 - l1 I) (AB^-1 - l2 I) e1
        // where l1 and l2 are the eigenvalues of the 2x2 matrix C = U V^-1 where
        // U and V are 2x2 bottom right sub matrices of A and B. Thus:
        //  = AB^-1AB^-1 + l1 l2 I - (l1+l2)(AB^-1)
        //  = AB^-1AB^-1 + det(M) - tr(M)(AB^-1)
        // Since we are only interested in having x, y, z with a correct ratio, we have:
        const Scalar
          a11 = m_S.coeff(f,f),     a12 = m_S.coeff(f,f+1),
          a21 = m_S.coeff(f+1,f),   a22 = m_S.coeff(f+1,f+1),
                                    a32 = m_S.coeff(f+2,f+1),

          a88 = m_S.coeff(l-1,l-1), a89 = m_S.coeff(l-1,l),
          a98 = m_S.coeff(l,l-1),   a99 = m_S.coeff(l,l),

          b11 = m_T.coeff(f,f),     b12 = m_T.coeff(f,f+1),
                                    b22 = m_T.coeff(f+1,f+1),

          b88 = m_T.coeff(l-1,l-1), b89 = m_T.coeff(l-1,l),
                                    b99 = m_T.coeff(l,l);

        x = ( (a88/b88 - a11/b11)*(a99/b99 - a11/b11) - (a89/b99)*(a98/b88) + (a98/b88)*(b89/b99)*(a11/b11) ) * (b11/a21)
          + a12/b22 - (a11/b11)*(b12/b22);
        y = (a22/b22-a11/b11) - (a21/b11)*(b12/b22) - (a88/b88-a11/b11) - (a99/b99-a11/b11) + (a98/b88)*(b89/b99);
        z = a32/b22;
      }

      JRs G;

      for (Index k=f; k<=l-2; k++)
      {
        // variables for Householder reflections
        Vector2s essential2;
        Scalar tau, beta;

        Vector3s hr(x,y,z);

        // Q_k to annihilate S(k+1,k-1) and S(k+2,k-1)
        hr.makeHouseholderInPlace(tau, beta);
        essential2 = hr.template bottomRows<2>();
        Index fc=(std::max)(k-1,Index(0));  // first col to update
        m_S.template middleRows<3>(k).rightCols(dim-fc).applyHouseholderOnTheLeft(essential2, tau, m_workspace.data());
        m_T.template middleRows<3>(k).rightCols(dim-fc).applyHouseholderOnTheLeft(essential2, tau, m_workspace.data());
        if (m_computeQZ)
          m_Q.template middleCols<3>(k).applyHouseholderOnTheRight(essential2, tau, m_workspace.data());
        if (k>f)
          m_S.coeffRef(k+2,k-1) = m_S.coeffRef(k+1,k-1) = Scalar(0.0);

        // Z_{k1} to annihilate T(k+2,k+1) and T(k+2,k)
        hr << m_T.coeff(k+2,k+2),m_T.coeff(k+2,k),m_T.coeff(k+2,k+1);
        hr.makeHouseholderInPlace(tau, beta);
        essential2 = hr.template bottomRows<2>();
        {
          Index lr = (std::min)(k+4,dim); // last row to update
          Map<Matrix<Scalar,Dynamic,1> > tmp(m_workspace.data(),lr);
          // S
          tmp = m_S.template middleCols<2>(k).topRows(lr) * essential2;
          tmp += m_S.col(k+2).head(lr);
          m_S.col(k+2).head(lr) -= tau*tmp;
          m_S.template middleCols<2>(k).topRows(lr) -= (tau*tmp) * essential2.adjoint();
          // T
          tmp = m_T.template middleCols<2>(k).topRows(lr) * essential2;
          tmp += m_T.col(k+2).head(lr);
          m_T.col(k+2).head(lr) -= tau*tmp;
          m_T.template middleCols<2>(k).topRows(lr) -= (tau*tmp) * essential2.adjoint();
        }
        if (m_computeQZ)
        {
          // Z
          Map<Matrix<Scalar,1,Dynamic> > tmp(m_workspace.data(),dim);
          tmp = essential2.adjoint()*(m_Z.template middleRows<2>(k));
          tmp += m_Z.row(k+2);
          m_Z.row(k+2) -= tau*tmp;
          m_Z.template middleRows<2>(k) -= essential2 * (tau*tmp);
        }
        m_T.coeffRef(k+2,k) = m_T.coeffRef(k+2,k+1) = Scalar(0.0);

        // Z_{k2} to annihilate T(k+1,k)
        G.makeGivens(m_T.coeff(k+1,k+1), m_T.coeff(k+1,k));
        m_S.applyOnTheRight(k+1,k,G);
        m_T.applyOnTheRight(k+1,k,G);
        // update Z
        if (m_computeQZ)
          m_Z.applyOnTheLeft(k+1,k,G.adjoint());
        m_T.coeffRef(k+1,k) = Scalar(0.0);

        // update x,y,z
        x = m_S.coeff(k+1,k);
        y = m_S.coeff(k+2,k);
        if (k < l-2)
          z = m_S.coeff(k+3,k);
      } // loop over k

      // Q_{n-1} to annihilate y = S(l,l-2)
      G.makeGivens(x,y);
      m_S.applyOnTheLeft(l-1,l,G.adjoint());
      m_T.applyOnTheLeft(l-1,l,G.adjoint());
      if (m_computeQZ)
        m_Q.applyOnTheRight(l-1,l,G);
      m_S.coeffRef(l,l-2) = Scalar(0.0);

      // Z_{n-1} to annihilate T(l,l-1)
      G.makeGivens(m_T.coeff(l,l),m_T.coeff(l,l-1));
      m_S.applyOnTheRight(l,l-1,G);
      m_T.applyOnTheRight(l,l-1,G);
      if (m_computeQZ)
        m_Z.applyOnTheLeft(l,l-1,G.adjoint());
      m_T.coeffRef(l,l-1) = Scalar(0.0);
    }

  template<typename MatrixType>
    RealQZ<MatrixType>& RealQZ<MatrixType>::compute(const MatrixType& A_in, const MatrixType& B_in, bool computeQZ)
    {

      const Index dim = A_in.cols();

      eigen_assert (A_in.rows()==dim && A_in.cols()==dim 
          && B_in.rows()==dim && B_in.cols()==dim 
          && "Need square matrices of the same dimension");

      m_isInitialized = true;
      m_computeQZ = computeQZ;
      m_S = A_in; m_T = B_in;
      m_workspace.resize(dim*2);
      m_global_iter = 0;

      // entrance point: hessenberg triangular decomposition
      hessenbergTriangular();
      // compute L1 vector norms of T, S into m_normOfS, m_normOfT
      computeNorms();

      Index l = dim-1, 
            f, 
            local_iter = 0;

      while (l>0 && local_iter<m_maxIters)
      {
        f = findSmallSubdiagEntry(l);
        // now rows and columns f..l (including) decouple from the rest of the problem
        if (f>0) m_S.coeffRef(f,f-1) = Scalar(0.0);
        if (f == l) // One root found
        {
          l--;
          local_iter = 0;
        }
        else if (f == l-1) // Two roots found
        {
          splitOffTwoRows(f);
          l -= 2;
          local_iter = 0;
        }
        else // No convergence yet
        {
          // if there's zero on diagonal of T, we can isolate an eigenvalue with Givens rotations
          Index z = findSmallDiagEntry(f,l);
          if (z>=f)
          {
            // zero found
            pushDownZero(z,f,l);
          }
          else
          {
            // We are sure now that S.block(f,f, l-f+1,l-f+1) is underuced upper-Hessenberg 
            // and T.block(f,f, l-f+1,l-f+1) is invertible uper-triangular, which allows to
            // apply a QR-like iteration to rows and columns f..l.
            step(f,l, local_iter);
            local_iter++;
            m_global_iter++;
          }
        }
      }
      // check if we converged before reaching iterations limit
      m_info = (local_iter<m_maxIters) ? Success : NoConvergence;

      // For each non triangular 2x2 diagonal block of S,
      //    reduce the respective 2x2 diagonal block of T to positive diagonal form using 2x2 SVD.
      // This step is not mandatory for QZ, but it does help further extraction of eigenvalues/eigenvectors,
      // and is in par with Lapack/Matlab QZ.
      if(m_info==Success)
      {
        for(Index i=0; i<dim-1; ++i)
        {
          if(m_S.coeff(i+1, i) != Scalar(0))
          {
            JacobiRotation<Scalar> j_left, j_right;
            internal::real_2x2_jacobi_svd(m_T, i, i+1, &j_left, &j_right);

            // Apply resulting Jacobi rotations
            m_S.applyOnTheLeft(i,i+1,j_left);
            m_S.applyOnTheRight(i,i+1,j_right);
            m_T.applyOnTheLeft(i,i+1,j_left);
            m_T.applyOnTheRight(i,i+1,j_right);
            m_T(i+1,i) = m_T(i,i+1) = Scalar(0);

            if(m_computeQZ) {
              m_Q.applyOnTheRight(i,i+1,j_left.transpose());
              m_Z.applyOnTheLeft(i,i+1,j_right.transpose());
            }

            i++;
          }
        }
      }

      return *this;
    } // end compute

} // end namespace Eigen

#endif //EIGEN_REAL_QZ