diff --git a/Eigen/src/Eigenvalues/RealSchur.h b/Eigen/src/Eigenvalues/RealSchur.h index 0526ed958..6a2ac2756 100644 --- a/Eigen/src/Eigenvalues/RealSchur.h +++ b/Eigen/src/Eigenvalues/RealSchur.h @@ -93,7 +93,12 @@ template class RealSchur EigenvalueType m_eivalues; bool m_isInitialized; - void hqr2(); + Scalar computeNormOfT(); + int findSmallSubdiagEntry(int n, Scalar norm); + void computeShift(Scalar& x, Scalar& y, Scalar& w, int l, int n, Scalar& exshift, int iter); + void findTwoSmallSubdiagEntries(Scalar x, Scalar y, Scalar w, int l, int& m, int n, Scalar& p, Scalar& q, Scalar& r); + void doFrancisStep(int l, int m, int n, Scalar p, Scalar q, Scalar r, Scalar x, Scalar* workspace); + void splitOffTwoRows(int n, Scalar exshift); }; @@ -102,66 +107,28 @@ void RealSchur::compute(const MatrixType& matrix) { assert(matrix.cols() == matrix.rows()); - // Reduce to Hessenberg form + // Step 1. Reduce to Hessenberg form // TODO skip Q if skipU = true HessenbergDecomposition hess(matrix); m_matT = hess.matrixH(); m_matU = hess.matrixQ(); - // Reduce to Real Schur form - hqr2(); - - m_isInitialized = true; -} - - -template -void RealSchur::hqr2() -{ + // Step 2. Reduce to real Schur form typedef Matrix ColumnVectorType; - - // 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 - const int size = m_matU.cols(); - int n = size-1; - Scalar exshift = 0.0; - Scalar p=0, q=0, r=0; - - ColumnVectorType workspaceVector(size); + ColumnVectorType workspaceVector(m_matU.cols()); Scalar* workspace = &workspaceVector.coeffRef(0); - // Compute matrix norm - // FIXME to be efficient the following would requires a triangular reduxion code - // Scalar norm = m_matT.upper().cwiseAbs().sum() + m_matT.corner(BottomLeft,n,n).diagonal().cwiseAbs().sum(); - Scalar norm = 0.0; - for (int j = 0; j < size; ++j) - { - norm += m_matT.row(j).segment(std::max(j-1,0), size-std::max(j-1,0)).cwiseAbs().sum(); - } + int n = m_matU.cols() - 1; + Scalar exshift = 0.0; + Scalar norm = computeNormOfT(); - // Outer loop over eigenvalue index int iter = 0; while (n >= 0) { - // Look for single small sub-diagonal element - int l = n; - while (l > 0) - { - Scalar s = ei_abs(m_matT.coeff(l-1,l-1)) + ei_abs(m_matT.coeff(l,l)); - if (s == 0.0) - s = norm; - if (ei_abs(m_matT.coeff(l,l-1)) < NumTraits::epsilon() * s) - break; - l--; - } + int l = findSmallSubdiagEntry(n, norm); // Check for convergence - // One root found - if (l == n) + if (l == n) // One root found { m_matT.coeffRef(n,n) = m_matT.coeff(n,n) + exshift; m_eivalues.coeffRef(n) = ComplexScalar(m_matT.coeff(n,n), 0.0); @@ -170,169 +137,226 @@ void RealSchur::hqr2() } else if (l == n-1) // Two roots found { - Scalar w = m_matT.coeff(n,n-1) * m_matT.coeff(n-1,n); - p = (m_matT.coeff(n-1,n-1) - m_matT.coeff(n,n)) * Scalar(0.5); - q = p * p + w; - Scalar z = ei_sqrt(ei_abs(q)); - m_matT.coeffRef(n,n) = m_matT.coeff(n,n) + exshift; - m_matT.coeffRef(n-1,n-1) = m_matT.coeff(n-1,n-1) + exshift; - Scalar x = m_matT.coeff(n,n); - - // Scalar pair - if (q >= 0) - { - if (p >= 0) - z = p + z; - else - z = p - z; - - m_eivalues.coeffRef(n-1) = ComplexScalar(x + z, 0.0); - m_eivalues.coeffRef(n) = ComplexScalar(z!=0.0 ? x - w / z : m_eivalues.coeff(n-1).real(), 0.0); - - PlanarRotation rot; - rot.makeGivens(z, m_matT.coeff(n, n-1)); - m_matT.block(0, n-1, size, size-n+1).applyOnTheLeft(n-1, n, rot.adjoint()); - m_matT.block(0, 0, n+1, size).applyOnTheRight(n-1, n, rot); - m_matU.applyOnTheRight(n-1, n, rot); - } - else // Complex pair - { - m_eivalues.coeffRef(n-1) = ComplexScalar(x + p, z); - m_eivalues.coeffRef(n) = ComplexScalar(x + p, -z); - } + splitOffTwoRows(n, exshift); n = n - 2; iter = 0; } else // No convergence yet { - // Form shift - Scalar x = m_matT.coeff(n,n); - Scalar y = 0.0; - Scalar w = 0.0; - if (l < n) - { - y = m_matT.coeff(n-1,n-1); - w = m_matT.coeff(n,n-1) * m_matT.coeff(n-1,n); - } - - // Wilkinson's original ad hoc shift - if (iter == 10) - { - exshift += x; - for (int i = 0; i <= n; ++i) - m_matT.coeffRef(i,i) -= x; - Scalar s = ei_abs(m_matT.coeff(n,n-1)) + ei_abs(m_matT.coeff(n-1,n-2)); - x = y = Scalar(0.75) * s; - w = Scalar(-0.4375) * s * s; - } - - // MATLAB's new ad hoc shift - if (iter == 30) - { - Scalar s = Scalar((y - x) / 2.0); - s = s * s + w; - if (s > 0) - { - s = ei_sqrt(s); - if (y < x) - s = -s; - s = Scalar(x - w / ((y - x) / 2.0 + s)); - for (int i = 0; i <= n; ++i) - m_matT.coeffRef(i,i) -= s; - exshift += s; - x = y = w = Scalar(0.964); - } - } - + Scalar p = 0, q = 0, r = 0, x, y, w; + computeShift(x, y, w, l, n, exshift, iter); iter = iter + 1; // (Could check iteration count here.) - - // Look for two consecutive small sub-diagonal elements - int m = n-2; - while (m >= l) - { - Scalar z = m_matT.coeff(m,m); - r = x - z; - Scalar s = y - z; - p = (r * s - w) / m_matT.coeff(m+1,m) + m_matT.coeff(m,m+1); - q = m_matT.coeff(m+1,m+1) - z - r - s; - r = m_matT.coeff(m+2,m+1); - s = ei_abs(p) + ei_abs(q) + ei_abs(r); - p = p / s; - q = q / s; - r = r / s; - if (m == l) { - break; - } - if (ei_abs(m_matT.coeff(m,m-1)) * (ei_abs(q) + ei_abs(r)) < - NumTraits::epsilon() * (ei_abs(p) * (ei_abs(m_matT.coeff(m-1,m-1)) + ei_abs(z) + - ei_abs(m_matT.coeff(m+1,m+1))))) - { - break; - } - m--; - } - - for (int i = m+2; i <= n; ++i) - { - m_matT.coeffRef(i,i-2) = 0.0; - if (i > m+2) - m_matT.coeffRef(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 = m_matT.coeff(k,k-1); - q = m_matT.coeff(k+1,k-1); - r = notlast ? m_matT.coeff(k+2,k-1) : Scalar(0); - x = ei_abs(p) + ei_abs(q) + ei_abs(r); - if (x != 0.0) - { - p = p / x; - q = q / x; - r = r / x; - } - } - - if (x == 0.0) - break; - - Scalar s = ei_sqrt(p * p + q * q + r * r); - - if (p < 0) - s = -s; - - if (s != 0) - { - if (k != m) - m_matT.coeffRef(k,k-1) = -s * x; - else if (l != m) - m_matT.coeffRef(k,k-1) = -m_matT.coeff(k,k-1); - - p = p + s; - - if (notlast) - { - Matrix ess(q/p, r/p); - m_matT.block(k, k, 3, size-k).applyHouseholderOnTheLeft(ess, p/s, workspace); - m_matT.block(0, k, std::min(n,k+3) + 1, 3).applyHouseholderOnTheRight(ess, p/s, workspace); - m_matU.block(0, k, size, 3).applyHouseholderOnTheRight(ess, p/s, workspace); - } - else - { - Matrix ess; - ess.coeffRef(0) = q/p; - m_matT.block(k, k, 2, size-k).applyHouseholderOnTheLeft(ess, p/s, workspace); - m_matT.block(0, k, std::min(n,k+3) + 1, 2).applyHouseholderOnTheRight(ess, p/s, workspace); - m_matU.block(0, k, size, 2).applyHouseholderOnTheRight(ess, p/s, workspace); - } - - } // (s != 0) - } // k loop + int m; + findTwoSmallSubdiagEntries(x, y, w, l, m, n, p, q, r); + doFrancisStep(l, m, n, p, q, r, x, workspace); } // check convergence } // while (n >= 0) + + m_isInitialized = true; +} + +// Compute matrix norm +template +inline typename MatrixType::Scalar RealSchur::computeNormOfT() +{ + const int size = m_matU.cols(); + // FIXME to be efficient the following would requires a triangular reduxion code + // Scalar norm = m_matT.upper().cwiseAbs().sum() + m_matT.corner(BottomLeft,size-1,size-1).diagonal().cwiseAbs().sum(); + Scalar norm = 0.0; + for (int j = 0; j < size; ++j) + norm += m_matT.row(j).segment(std::max(j-1,0), size-std::max(j-1,0)).cwiseAbs().sum(); + return norm; +} + +// Look for single small sub-diagonal element +template +inline int RealSchur::findSmallSubdiagEntry(int n, Scalar norm) +{ + int l = n; + while (l > 0) + { + Scalar s = ei_abs(m_matT.coeff(l-1,l-1)) + ei_abs(m_matT.coeff(l,l)); + if (s == 0.0) + s = norm; + if (ei_abs(m_matT.coeff(l,l-1)) < NumTraits::epsilon() * s) + break; + l--; + } + return l; +} + +template +inline void RealSchur::splitOffTwoRows(int n, Scalar exshift) +{ + const int size = m_matU.cols(); + Scalar w = m_matT.coeff(n,n-1) * m_matT.coeff(n-1,n); + Scalar p = (m_matT.coeff(n-1,n-1) - m_matT.coeff(n,n)) * Scalar(0.5); + Scalar q = p * p + w; + Scalar z = ei_sqrt(ei_abs(q)); + m_matT.coeffRef(n,n) = m_matT.coeff(n,n) + exshift; + m_matT.coeffRef(n-1,n-1) = m_matT.coeff(n-1,n-1) + exshift; + Scalar x = m_matT.coeff(n,n); + + // Scalar pair + if (q >= 0) + { + if (p >= 0) + z = p + z; + else + z = p - z; + + m_eivalues.coeffRef(n-1) = ComplexScalar(x + z, 0.0); + m_eivalues.coeffRef(n) = ComplexScalar(z!=0.0 ? x - w / z : m_eivalues.coeff(n-1).real(), 0.0); + + PlanarRotation rot; + rot.makeGivens(z, m_matT.coeff(n, n-1)); + m_matT.block(0, n-1, size, size-n+1).applyOnTheLeft(n-1, n, rot.adjoint()); + m_matT.block(0, 0, n+1, size).applyOnTheRight(n-1, n, rot); + m_matU.applyOnTheRight(n-1, n, rot); + } + else // Complex pair + { + m_eivalues.coeffRef(n-1) = ComplexScalar(x + p, z); + m_eivalues.coeffRef(n) = ComplexScalar(x + p, -z); + } +} + +// Form shift +template +inline void RealSchur::computeShift(Scalar& x, Scalar& y, Scalar& w, int l, int n, Scalar& exshift, int iter) +{ + x = m_matT.coeff(n,n); + y = 0.0; + w = 0.0; + if (l < n) + { + y = m_matT.coeff(n-1,n-1); + w = m_matT.coeff(n,n-1) * m_matT.coeff(n-1,n); + } + + // Wilkinson's original ad hoc shift + if (iter == 10) + { + exshift += x; + for (int i = 0; i <= n; ++i) + m_matT.coeffRef(i,i) -= x; + Scalar s = ei_abs(m_matT.coeff(n,n-1)) + ei_abs(m_matT.coeff(n-1,n-2)); + x = y = Scalar(0.75) * s; + w = Scalar(-0.4375) * s * s; + } + + // MATLAB's new ad hoc shift + if (iter == 30) + { + Scalar s = Scalar((y - x) / 2.0); + s = s * s + w; + if (s > 0) + { + s = ei_sqrt(s); + if (y < x) + s = -s; + s = Scalar(x - w / ((y - x) / 2.0 + s)); + for (int i = 0; i <= n; ++i) + m_matT.coeffRef(i,i) -= s; + exshift += s; + x = y = w = Scalar(0.964); + } + } +} + +// Look for two consecutive small sub-diagonal elements +template +inline void RealSchur::findTwoSmallSubdiagEntries(Scalar x, Scalar y, Scalar w, int l, int& m, int n, Scalar& p, Scalar& q, Scalar& r) +{ + m = n-2; + while (m >= l) + { + Scalar z = m_matT.coeff(m,m); + r = x - z; + Scalar s = y - z; + p = (r * s - w) / m_matT.coeff(m+1,m) + m_matT.coeff(m,m+1); + q = m_matT.coeff(m+1,m+1) - z - r - s; + r = m_matT.coeff(m+2,m+1); + s = ei_abs(p) + ei_abs(q) + ei_abs(r); + p = p / s; + q = q / s; + r = r / s; + if (m == l) { + break; + } + if (ei_abs(m_matT.coeff(m,m-1)) * (ei_abs(q) + ei_abs(r)) < + NumTraits::epsilon() * (ei_abs(p) * (ei_abs(m_matT.coeff(m-1,m-1)) + ei_abs(z) + + ei_abs(m_matT.coeff(m+1,m+1))))) + { + break; + } + m--; + } + + for (int i = m+2; i <= n; ++i) + { + m_matT.coeffRef(i,i-2) = 0.0; + if (i > m+2) + m_matT.coeffRef(i,i-3) = 0.0; + } +} + +// Double QR step involving rows l:n and columns m:n +template +inline void RealSchur::doFrancisStep(int l, int m, int n, Scalar p, Scalar q, Scalar r, Scalar x, Scalar* workspace) +{ + const int size = m_matU.cols(); + + for (int k = m; k <= n-1; ++k) + { + int notlast = (k != n-1); + if (k != m) { + p = m_matT.coeff(k,k-1); + q = m_matT.coeff(k+1,k-1); + r = notlast ? m_matT.coeff(k+2,k-1) : Scalar(0); + x = ei_abs(p) + ei_abs(q) + ei_abs(r); + if (x != 0.0) + { + p = p / x; + q = q / x; + r = r / x; + } + } + + if (x == 0.0) + break; + + Scalar s = ei_sqrt(p * p + q * q + r * r); + + if (p < 0) + s = -s; + + if (s != 0) + { + if (k != m) + m_matT.coeffRef(k,k-1) = -s * x; + else if (l != m) + m_matT.coeffRef(k,k-1) = -m_matT.coeff(k,k-1); + + p = p + s; + + if (notlast) + { + Matrix ess(q/p, r/p); + m_matT.block(k, k, 3, size-k).applyHouseholderOnTheLeft(ess, p/s, workspace); + m_matT.block(0, k, std::min(n,k+3) + 1, 3).applyHouseholderOnTheRight(ess, p/s, workspace); + m_matU.block(0, k, size, 3).applyHouseholderOnTheRight(ess, p/s, workspace); + } + else + { + Matrix ess; + ess.coeffRef(0) = q/p; + m_matT.block(k, k, 2, size-k).applyHouseholderOnTheLeft(ess, p/s, workspace); + m_matT.block(0, k, std::min(n,k+3) + 1, 2).applyHouseholderOnTheRight(ess, p/s, workspace); + m_matU.block(0, k, size, 2).applyHouseholderOnTheRight(ess, p/s, workspace); + } + } // (s != 0) + } // k loop } #endif // EIGEN_REAL_SCHUR_H