Eigne Matrix Class Library
Dependents: MPC_current_control HydraulicControlBoard_SW AHRS Test_ekf ... more
src/Eigenvalues/RealQZ.h@1:3b8049da21b8, 2019-09-24 (annotated)
- Committer:
- jsoh91
- Date:
- Tue Sep 24 00:18:23 2019 +0000
- Revision:
- 1:3b8049da21b8
- Parent:
- 0:13a5d365ba16
ignore and revise some of error parts
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
ykuroda | 0:13a5d365ba16 | 1 | // This file is part of Eigen, a lightweight C++ template library |
ykuroda | 0:13a5d365ba16 | 2 | // for linear algebra. |
ykuroda | 0:13a5d365ba16 | 3 | // |
ykuroda | 0:13a5d365ba16 | 4 | // Copyright (C) 2012 Alexey Korepanov <kaikaikai@yandex.ru> |
ykuroda | 0:13a5d365ba16 | 5 | // |
ykuroda | 0:13a5d365ba16 | 6 | // This Source Code Form is subject to the terms of the Mozilla |
ykuroda | 0:13a5d365ba16 | 7 | // Public License v. 2.0. If a copy of the MPL was not distributed |
ykuroda | 0:13a5d365ba16 | 8 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. |
ykuroda | 0:13a5d365ba16 | 9 | |
ykuroda | 0:13a5d365ba16 | 10 | #ifndef EIGEN_REAL_QZ_H |
ykuroda | 0:13a5d365ba16 | 11 | #define EIGEN_REAL_QZ_H |
ykuroda | 0:13a5d365ba16 | 12 | |
ykuroda | 0:13a5d365ba16 | 13 | namespace Eigen { |
ykuroda | 0:13a5d365ba16 | 14 | |
ykuroda | 0:13a5d365ba16 | 15 | /** \eigenvalues_module \ingroup Eigenvalues_Module |
ykuroda | 0:13a5d365ba16 | 16 | * |
ykuroda | 0:13a5d365ba16 | 17 | * |
ykuroda | 0:13a5d365ba16 | 18 | * \class RealQZ |
ykuroda | 0:13a5d365ba16 | 19 | * |
ykuroda | 0:13a5d365ba16 | 20 | * \brief Performs a real QZ decomposition of a pair of square matrices |
ykuroda | 0:13a5d365ba16 | 21 | * |
ykuroda | 0:13a5d365ba16 | 22 | * \tparam _MatrixType the type of the matrix of which we are computing the |
ykuroda | 0:13a5d365ba16 | 23 | * real QZ decomposition; this is expected to be an instantiation of the |
ykuroda | 0:13a5d365ba16 | 24 | * Matrix class template. |
ykuroda | 0:13a5d365ba16 | 25 | * |
ykuroda | 0:13a5d365ba16 | 26 | * Given a real square matrices A and B, this class computes the real QZ |
ykuroda | 0:13a5d365ba16 | 27 | * decomposition: \f$ A = Q S Z \f$, \f$ B = Q T Z \f$ where Q and Z are |
ykuroda | 0:13a5d365ba16 | 28 | * real orthogonal matrixes, T is upper-triangular matrix, and S is upper |
ykuroda | 0:13a5d365ba16 | 29 | * quasi-triangular matrix. An orthogonal matrix is a matrix whose |
ykuroda | 0:13a5d365ba16 | 30 | * inverse is equal to its transpose, \f$ U^{-1} = U^T \f$. A quasi-triangular |
ykuroda | 0:13a5d365ba16 | 31 | * matrix is a block-triangular matrix whose diagonal consists of 1-by-1 |
ykuroda | 0:13a5d365ba16 | 32 | * blocks and 2-by-2 blocks where further reduction is impossible due to |
ykuroda | 0:13a5d365ba16 | 33 | * complex eigenvalues. |
ykuroda | 0:13a5d365ba16 | 34 | * |
ykuroda | 0:13a5d365ba16 | 35 | * The eigenvalues of the pencil \f$ A - z B \f$ can be obtained from |
ykuroda | 0:13a5d365ba16 | 36 | * 1x1 and 2x2 blocks on the diagonals of S and T. |
ykuroda | 0:13a5d365ba16 | 37 | * |
ykuroda | 0:13a5d365ba16 | 38 | * Call the function compute() to compute the real QZ decomposition of a |
ykuroda | 0:13a5d365ba16 | 39 | * given pair of matrices. Alternatively, you can use the |
ykuroda | 0:13a5d365ba16 | 40 | * RealQZ(const MatrixType& B, const MatrixType& B, bool computeQZ) |
ykuroda | 0:13a5d365ba16 | 41 | * constructor which computes the real QZ decomposition at construction |
ykuroda | 0:13a5d365ba16 | 42 | * time. Once the decomposition is computed, you can use the matrixS(), |
ykuroda | 0:13a5d365ba16 | 43 | * matrixT(), matrixQ() and matrixZ() functions to retrieve the matrices |
ykuroda | 0:13a5d365ba16 | 44 | * S, T, Q and Z in the decomposition. If computeQZ==false, some time |
ykuroda | 0:13a5d365ba16 | 45 | * is saved by not computing matrices Q and Z. |
ykuroda | 0:13a5d365ba16 | 46 | * |
ykuroda | 0:13a5d365ba16 | 47 | * Example: \include RealQZ_compute.cpp |
ykuroda | 0:13a5d365ba16 | 48 | * Output: \include RealQZ_compute.out |
ykuroda | 0:13a5d365ba16 | 49 | * |
ykuroda | 0:13a5d365ba16 | 50 | * \note The implementation is based on the algorithm in "Matrix Computations" |
ykuroda | 0:13a5d365ba16 | 51 | * by Gene H. Golub and Charles F. Van Loan, and a paper "An algorithm for |
ykuroda | 0:13a5d365ba16 | 52 | * generalized eigenvalue problems" by C.B.Moler and G.W.Stewart. |
ykuroda | 0:13a5d365ba16 | 53 | * |
ykuroda | 0:13a5d365ba16 | 54 | * \sa class RealSchur, class ComplexSchur, class EigenSolver, class ComplexEigenSolver |
ykuroda | 0:13a5d365ba16 | 55 | */ |
ykuroda | 0:13a5d365ba16 | 56 | |
ykuroda | 0:13a5d365ba16 | 57 | template<typename _MatrixType> class RealQZ |
ykuroda | 0:13a5d365ba16 | 58 | { |
ykuroda | 0:13a5d365ba16 | 59 | public: |
ykuroda | 0:13a5d365ba16 | 60 | typedef _MatrixType MatrixType; |
ykuroda | 0:13a5d365ba16 | 61 | enum { |
ykuroda | 0:13a5d365ba16 | 62 | RowsAtCompileTime = MatrixType::RowsAtCompileTime, |
ykuroda | 0:13a5d365ba16 | 63 | ColsAtCompileTime = MatrixType::ColsAtCompileTime, |
ykuroda | 0:13a5d365ba16 | 64 | Options = MatrixType::Options, |
ykuroda | 0:13a5d365ba16 | 65 | MaxRowsAtCompileTime = MatrixType::MaxRowsAtCompileTime, |
ykuroda | 0:13a5d365ba16 | 66 | MaxColsAtCompileTime = MatrixType::MaxColsAtCompileTime |
ykuroda | 0:13a5d365ba16 | 67 | }; |
ykuroda | 0:13a5d365ba16 | 68 | typedef typename MatrixType::Scalar Scalar; |
ykuroda | 0:13a5d365ba16 | 69 | typedef std::complex<typename NumTraits<Scalar>::Real> ComplexScalar; |
ykuroda | 0:13a5d365ba16 | 70 | typedef typename MatrixType::Index Index; |
ykuroda | 0:13a5d365ba16 | 71 | |
ykuroda | 0:13a5d365ba16 | 72 | typedef Matrix<ComplexScalar, ColsAtCompileTime, 1, Options & ~RowMajor, MaxColsAtCompileTime, 1> EigenvalueType; |
ykuroda | 0:13a5d365ba16 | 73 | typedef Matrix<Scalar, ColsAtCompileTime, 1, Options & ~RowMajor, MaxColsAtCompileTime, 1> ColumnVectorType; |
ykuroda | 0:13a5d365ba16 | 74 | |
ykuroda | 0:13a5d365ba16 | 75 | /** \brief Default constructor. |
ykuroda | 0:13a5d365ba16 | 76 | * |
ykuroda | 0:13a5d365ba16 | 77 | * \param [in] size Positive integer, size of the matrix whose QZ decomposition will be computed. |
ykuroda | 0:13a5d365ba16 | 78 | * |
ykuroda | 0:13a5d365ba16 | 79 | * The default constructor is useful in cases in which the user intends to |
ykuroda | 0:13a5d365ba16 | 80 | * perform decompositions via compute(). The \p size parameter is only |
ykuroda | 0:13a5d365ba16 | 81 | * used as a hint. It is not an error to give a wrong \p size, but it may |
ykuroda | 0:13a5d365ba16 | 82 | * impair performance. |
ykuroda | 0:13a5d365ba16 | 83 | * |
ykuroda | 0:13a5d365ba16 | 84 | * \sa compute() for an example. |
ykuroda | 0:13a5d365ba16 | 85 | */ |
ykuroda | 0:13a5d365ba16 | 86 | RealQZ(Index size = RowsAtCompileTime==Dynamic ? 1 : RowsAtCompileTime) : |
ykuroda | 0:13a5d365ba16 | 87 | m_S(size, size), |
ykuroda | 0:13a5d365ba16 | 88 | m_T(size, size), |
ykuroda | 0:13a5d365ba16 | 89 | m_Q(size, size), |
ykuroda | 0:13a5d365ba16 | 90 | m_Z(size, size), |
ykuroda | 0:13a5d365ba16 | 91 | m_workspace(size*2), |
ykuroda | 0:13a5d365ba16 | 92 | m_maxIters(400), |
ykuroda | 0:13a5d365ba16 | 93 | m_isInitialized(false) |
ykuroda | 0:13a5d365ba16 | 94 | { } |
ykuroda | 0:13a5d365ba16 | 95 | |
ykuroda | 0:13a5d365ba16 | 96 | /** \brief Constructor; computes real QZ decomposition of given matrices |
ykuroda | 0:13a5d365ba16 | 97 | * |
ykuroda | 0:13a5d365ba16 | 98 | * \param[in] A Matrix A. |
ykuroda | 0:13a5d365ba16 | 99 | * \param[in] B Matrix B. |
ykuroda | 0:13a5d365ba16 | 100 | * \param[in] computeQZ If false, A and Z are not computed. |
ykuroda | 0:13a5d365ba16 | 101 | * |
ykuroda | 0:13a5d365ba16 | 102 | * This constructor calls compute() to compute the QZ decomposition. |
ykuroda | 0:13a5d365ba16 | 103 | */ |
ykuroda | 0:13a5d365ba16 | 104 | RealQZ(const MatrixType& A, const MatrixType& B, bool computeQZ = true) : |
ykuroda | 0:13a5d365ba16 | 105 | m_S(A.rows(),A.cols()), |
ykuroda | 0:13a5d365ba16 | 106 | m_T(A.rows(),A.cols()), |
ykuroda | 0:13a5d365ba16 | 107 | m_Q(A.rows(),A.cols()), |
ykuroda | 0:13a5d365ba16 | 108 | m_Z(A.rows(),A.cols()), |
ykuroda | 0:13a5d365ba16 | 109 | m_workspace(A.rows()*2), |
ykuroda | 0:13a5d365ba16 | 110 | m_maxIters(400), |
ykuroda | 0:13a5d365ba16 | 111 | m_isInitialized(false) { |
ykuroda | 0:13a5d365ba16 | 112 | compute(A, B, computeQZ); |
ykuroda | 0:13a5d365ba16 | 113 | } |
ykuroda | 0:13a5d365ba16 | 114 | |
ykuroda | 0:13a5d365ba16 | 115 | /** \brief Returns matrix Q in the QZ decomposition. |
ykuroda | 0:13a5d365ba16 | 116 | * |
ykuroda | 0:13a5d365ba16 | 117 | * \returns A const reference to the matrix Q. |
ykuroda | 0:13a5d365ba16 | 118 | */ |
ykuroda | 0:13a5d365ba16 | 119 | const MatrixType& matrixQ() const { |
ykuroda | 0:13a5d365ba16 | 120 | eigen_assert(m_isInitialized && "RealQZ is not initialized."); |
ykuroda | 0:13a5d365ba16 | 121 | eigen_assert(m_computeQZ && "The matrices Q and Z have not been computed during the QZ decomposition."); |
ykuroda | 0:13a5d365ba16 | 122 | return m_Q; |
ykuroda | 0:13a5d365ba16 | 123 | } |
ykuroda | 0:13a5d365ba16 | 124 | |
ykuroda | 0:13a5d365ba16 | 125 | /** \brief Returns matrix Z in the QZ decomposition. |
ykuroda | 0:13a5d365ba16 | 126 | * |
ykuroda | 0:13a5d365ba16 | 127 | * \returns A const reference to the matrix Z. |
ykuroda | 0:13a5d365ba16 | 128 | */ |
ykuroda | 0:13a5d365ba16 | 129 | const MatrixType& matrixZ() const { |
ykuroda | 0:13a5d365ba16 | 130 | eigen_assert(m_isInitialized && "RealQZ is not initialized."); |
ykuroda | 0:13a5d365ba16 | 131 | eigen_assert(m_computeQZ && "The matrices Q and Z have not been computed during the QZ decomposition."); |
ykuroda | 0:13a5d365ba16 | 132 | return m_Z; |
ykuroda | 0:13a5d365ba16 | 133 | } |
ykuroda | 0:13a5d365ba16 | 134 | |
ykuroda | 0:13a5d365ba16 | 135 | /** \brief Returns matrix S in the QZ decomposition. |
ykuroda | 0:13a5d365ba16 | 136 | * |
ykuroda | 0:13a5d365ba16 | 137 | * \returns A const reference to the matrix S. |
ykuroda | 0:13a5d365ba16 | 138 | */ |
ykuroda | 0:13a5d365ba16 | 139 | const MatrixType& matrixS() const { |
ykuroda | 0:13a5d365ba16 | 140 | eigen_assert(m_isInitialized && "RealQZ is not initialized."); |
ykuroda | 0:13a5d365ba16 | 141 | return m_S; |
ykuroda | 0:13a5d365ba16 | 142 | } |
ykuroda | 0:13a5d365ba16 | 143 | |
ykuroda | 0:13a5d365ba16 | 144 | /** \brief Returns matrix S in the QZ decomposition. |
ykuroda | 0:13a5d365ba16 | 145 | * |
ykuroda | 0:13a5d365ba16 | 146 | * \returns A const reference to the matrix S. |
ykuroda | 0:13a5d365ba16 | 147 | */ |
ykuroda | 0:13a5d365ba16 | 148 | const MatrixType& matrixT() const { |
ykuroda | 0:13a5d365ba16 | 149 | eigen_assert(m_isInitialized && "RealQZ is not initialized."); |
ykuroda | 0:13a5d365ba16 | 150 | return m_T; |
ykuroda | 0:13a5d365ba16 | 151 | } |
ykuroda | 0:13a5d365ba16 | 152 | |
ykuroda | 0:13a5d365ba16 | 153 | /** \brief Computes QZ decomposition of given matrix. |
ykuroda | 0:13a5d365ba16 | 154 | * |
ykuroda | 0:13a5d365ba16 | 155 | * \param[in] A Matrix A. |
ykuroda | 0:13a5d365ba16 | 156 | * \param[in] B Matrix B. |
ykuroda | 0:13a5d365ba16 | 157 | * \param[in] computeQZ If false, A and Z are not computed. |
ykuroda | 0:13a5d365ba16 | 158 | * \returns Reference to \c *this |
ykuroda | 0:13a5d365ba16 | 159 | */ |
ykuroda | 0:13a5d365ba16 | 160 | RealQZ& compute(const MatrixType& A, const MatrixType& B, bool computeQZ = true); |
ykuroda | 0:13a5d365ba16 | 161 | |
ykuroda | 0:13a5d365ba16 | 162 | /** \brief Reports whether previous computation was successful. |
ykuroda | 0:13a5d365ba16 | 163 | * |
ykuroda | 0:13a5d365ba16 | 164 | * \returns \c Success if computation was succesful, \c NoConvergence otherwise. |
ykuroda | 0:13a5d365ba16 | 165 | */ |
ykuroda | 0:13a5d365ba16 | 166 | ComputationInfo info() const |
ykuroda | 0:13a5d365ba16 | 167 | { |
ykuroda | 0:13a5d365ba16 | 168 | eigen_assert(m_isInitialized && "RealQZ is not initialized."); |
ykuroda | 0:13a5d365ba16 | 169 | return m_info; |
ykuroda | 0:13a5d365ba16 | 170 | } |
ykuroda | 0:13a5d365ba16 | 171 | |
ykuroda | 0:13a5d365ba16 | 172 | /** \brief Returns number of performed QR-like iterations. |
ykuroda | 0:13a5d365ba16 | 173 | */ |
ykuroda | 0:13a5d365ba16 | 174 | Index iterations() const |
ykuroda | 0:13a5d365ba16 | 175 | { |
ykuroda | 0:13a5d365ba16 | 176 | eigen_assert(m_isInitialized && "RealQZ is not initialized."); |
ykuroda | 0:13a5d365ba16 | 177 | return m_global_iter; |
ykuroda | 0:13a5d365ba16 | 178 | } |
ykuroda | 0:13a5d365ba16 | 179 | |
ykuroda | 0:13a5d365ba16 | 180 | /** Sets the maximal number of iterations allowed to converge to one eigenvalue |
ykuroda | 0:13a5d365ba16 | 181 | * or decouple the problem. |
ykuroda | 0:13a5d365ba16 | 182 | */ |
ykuroda | 0:13a5d365ba16 | 183 | RealQZ& setMaxIterations(Index maxIters) |
ykuroda | 0:13a5d365ba16 | 184 | { |
ykuroda | 0:13a5d365ba16 | 185 | m_maxIters = maxIters; |
ykuroda | 0:13a5d365ba16 | 186 | return *this; |
ykuroda | 0:13a5d365ba16 | 187 | } |
ykuroda | 0:13a5d365ba16 | 188 | |
ykuroda | 0:13a5d365ba16 | 189 | private: |
ykuroda | 0:13a5d365ba16 | 190 | |
ykuroda | 0:13a5d365ba16 | 191 | MatrixType m_S, m_T, m_Q, m_Z; |
ykuroda | 0:13a5d365ba16 | 192 | Matrix<Scalar,Dynamic,1> m_workspace; |
ykuroda | 0:13a5d365ba16 | 193 | ComputationInfo m_info; |
ykuroda | 0:13a5d365ba16 | 194 | Index m_maxIters; |
ykuroda | 0:13a5d365ba16 | 195 | bool m_isInitialized; |
ykuroda | 0:13a5d365ba16 | 196 | bool m_computeQZ; |
ykuroda | 0:13a5d365ba16 | 197 | Scalar m_normOfT, m_normOfS; |
ykuroda | 0:13a5d365ba16 | 198 | Index m_global_iter; |
ykuroda | 0:13a5d365ba16 | 199 | |
ykuroda | 0:13a5d365ba16 | 200 | typedef Matrix<Scalar,3,1> Vector3s; |
ykuroda | 0:13a5d365ba16 | 201 | typedef Matrix<Scalar,2,1> Vector2s; |
ykuroda | 0:13a5d365ba16 | 202 | typedef Matrix<Scalar,2,2> Matrix2s; |
ykuroda | 0:13a5d365ba16 | 203 | typedef JacobiRotation<Scalar> JRs; |
ykuroda | 0:13a5d365ba16 | 204 | |
ykuroda | 0:13a5d365ba16 | 205 | void hessenbergTriangular(); |
ykuroda | 0:13a5d365ba16 | 206 | void computeNorms(); |
ykuroda | 0:13a5d365ba16 | 207 | Index findSmallSubdiagEntry(Index iu); |
ykuroda | 0:13a5d365ba16 | 208 | Index findSmallDiagEntry(Index f, Index l); |
ykuroda | 0:13a5d365ba16 | 209 | void splitOffTwoRows(Index i); |
ykuroda | 0:13a5d365ba16 | 210 | void pushDownZero(Index z, Index f, Index l); |
ykuroda | 0:13a5d365ba16 | 211 | void step(Index f, Index l, Index iter); |
ykuroda | 0:13a5d365ba16 | 212 | |
ykuroda | 0:13a5d365ba16 | 213 | }; // RealQZ |
ykuroda | 0:13a5d365ba16 | 214 | |
ykuroda | 0:13a5d365ba16 | 215 | /** \internal Reduces S and T to upper Hessenberg - triangular form */ |
ykuroda | 0:13a5d365ba16 | 216 | template<typename MatrixType> |
ykuroda | 0:13a5d365ba16 | 217 | void RealQZ<MatrixType>::hessenbergTriangular() |
ykuroda | 0:13a5d365ba16 | 218 | { |
ykuroda | 0:13a5d365ba16 | 219 | |
ykuroda | 0:13a5d365ba16 | 220 | const Index dim = m_S.cols(); |
ykuroda | 0:13a5d365ba16 | 221 | |
ykuroda | 0:13a5d365ba16 | 222 | // perform QR decomposition of T, overwrite T with R, save Q |
ykuroda | 0:13a5d365ba16 | 223 | HouseholderQR<MatrixType> qrT(m_T); |
ykuroda | 0:13a5d365ba16 | 224 | m_T = qrT.matrixQR(); |
ykuroda | 0:13a5d365ba16 | 225 | m_T.template triangularView<StrictlyLower>().setZero(); |
ykuroda | 0:13a5d365ba16 | 226 | m_Q = qrT.householderQ(); |
ykuroda | 0:13a5d365ba16 | 227 | // overwrite S with Q* S |
ykuroda | 0:13a5d365ba16 | 228 | m_S.applyOnTheLeft(m_Q.adjoint()); |
ykuroda | 0:13a5d365ba16 | 229 | // init Z as Identity |
ykuroda | 0:13a5d365ba16 | 230 | if (m_computeQZ) |
ykuroda | 0:13a5d365ba16 | 231 | m_Z = MatrixType::Identity(dim,dim); |
ykuroda | 0:13a5d365ba16 | 232 | // reduce S to upper Hessenberg with Givens rotations |
ykuroda | 0:13a5d365ba16 | 233 | for (Index j=0; j<=dim-3; j++) { |
ykuroda | 0:13a5d365ba16 | 234 | for (Index i=dim-1; i>=j+2; i--) { |
ykuroda | 0:13a5d365ba16 | 235 | JRs G; |
ykuroda | 0:13a5d365ba16 | 236 | // kill S(i,j) |
ykuroda | 0:13a5d365ba16 | 237 | if(m_S.coeff(i,j) != 0) |
ykuroda | 0:13a5d365ba16 | 238 | { |
ykuroda | 0:13a5d365ba16 | 239 | G.makeGivens(m_S.coeff(i-1,j), m_S.coeff(i,j), &m_S.coeffRef(i-1, j)); |
ykuroda | 0:13a5d365ba16 | 240 | m_S.coeffRef(i,j) = Scalar(0.0); |
ykuroda | 0:13a5d365ba16 | 241 | m_S.rightCols(dim-j-1).applyOnTheLeft(i-1,i,G.adjoint()); |
ykuroda | 0:13a5d365ba16 | 242 | m_T.rightCols(dim-i+1).applyOnTheLeft(i-1,i,G.adjoint()); |
ykuroda | 0:13a5d365ba16 | 243 | // update Q |
ykuroda | 0:13a5d365ba16 | 244 | if (m_computeQZ) |
ykuroda | 0:13a5d365ba16 | 245 | m_Q.applyOnTheRight(i-1,i,G); |
ykuroda | 0:13a5d365ba16 | 246 | } |
ykuroda | 0:13a5d365ba16 | 247 | // kill T(i,i-1) |
ykuroda | 0:13a5d365ba16 | 248 | if(m_T.coeff(i,i-1)!=Scalar(0)) |
ykuroda | 0:13a5d365ba16 | 249 | { |
ykuroda | 0:13a5d365ba16 | 250 | G.makeGivens(m_T.coeff(i,i), m_T.coeff(i,i-1), &m_T.coeffRef(i,i)); |
ykuroda | 0:13a5d365ba16 | 251 | m_T.coeffRef(i,i-1) = Scalar(0.0); |
ykuroda | 0:13a5d365ba16 | 252 | m_S.applyOnTheRight(i,i-1,G); |
ykuroda | 0:13a5d365ba16 | 253 | m_T.topRows(i).applyOnTheRight(i,i-1,G); |
ykuroda | 0:13a5d365ba16 | 254 | // update Z |
ykuroda | 0:13a5d365ba16 | 255 | if (m_computeQZ) |
ykuroda | 0:13a5d365ba16 | 256 | m_Z.applyOnTheLeft(i,i-1,G.adjoint()); |
ykuroda | 0:13a5d365ba16 | 257 | } |
ykuroda | 0:13a5d365ba16 | 258 | } |
ykuroda | 0:13a5d365ba16 | 259 | } |
ykuroda | 0:13a5d365ba16 | 260 | } |
ykuroda | 0:13a5d365ba16 | 261 | |
ykuroda | 0:13a5d365ba16 | 262 | /** \internal Computes vector L1 norms of S and T when in Hessenberg-Triangular form already */ |
ykuroda | 0:13a5d365ba16 | 263 | template<typename MatrixType> |
ykuroda | 0:13a5d365ba16 | 264 | inline void RealQZ<MatrixType>::computeNorms() |
ykuroda | 0:13a5d365ba16 | 265 | { |
ykuroda | 0:13a5d365ba16 | 266 | const Index size = m_S.cols(); |
ykuroda | 0:13a5d365ba16 | 267 | m_normOfS = Scalar(0.0); |
ykuroda | 0:13a5d365ba16 | 268 | m_normOfT = Scalar(0.0); |
ykuroda | 0:13a5d365ba16 | 269 | for (Index j = 0; j < size; ++j) |
ykuroda | 0:13a5d365ba16 | 270 | { |
ykuroda | 0:13a5d365ba16 | 271 | m_normOfS += m_S.col(j).segment(0, (std::min)(size,j+2)).cwiseAbs().sum(); |
ykuroda | 0:13a5d365ba16 | 272 | m_normOfT += m_T.row(j).segment(j, size - j).cwiseAbs().sum(); |
ykuroda | 0:13a5d365ba16 | 273 | } |
ykuroda | 0:13a5d365ba16 | 274 | } |
ykuroda | 0:13a5d365ba16 | 275 | |
ykuroda | 0:13a5d365ba16 | 276 | |
ykuroda | 0:13a5d365ba16 | 277 | /** \internal Look for single small sub-diagonal element S(res, res-1) and return res (or 0) */ |
ykuroda | 0:13a5d365ba16 | 278 | template<typename MatrixType> |
ykuroda | 0:13a5d365ba16 | 279 | inline typename MatrixType::Index RealQZ<MatrixType>::findSmallSubdiagEntry(Index iu) |
ykuroda | 0:13a5d365ba16 | 280 | { |
ykuroda | 0:13a5d365ba16 | 281 | using std::abs; |
ykuroda | 0:13a5d365ba16 | 282 | Index res = iu; |
ykuroda | 0:13a5d365ba16 | 283 | while (res > 0) |
ykuroda | 0:13a5d365ba16 | 284 | { |
ykuroda | 0:13a5d365ba16 | 285 | Scalar s = abs(m_S.coeff(res-1,res-1)) + abs(m_S.coeff(res,res)); |
ykuroda | 0:13a5d365ba16 | 286 | if (s == Scalar(0.0)) |
ykuroda | 0:13a5d365ba16 | 287 | s = m_normOfS; |
ykuroda | 0:13a5d365ba16 | 288 | if (abs(m_S.coeff(res,res-1)) < NumTraits<Scalar>::epsilon() * s) |
ykuroda | 0:13a5d365ba16 | 289 | break; |
ykuroda | 0:13a5d365ba16 | 290 | res--; |
ykuroda | 0:13a5d365ba16 | 291 | } |
ykuroda | 0:13a5d365ba16 | 292 | return res; |
ykuroda | 0:13a5d365ba16 | 293 | } |
ykuroda | 0:13a5d365ba16 | 294 | |
ykuroda | 0:13a5d365ba16 | 295 | /** \internal Look for single small diagonal element T(res, res) for res between f and l, and return res (or f-1) */ |
ykuroda | 0:13a5d365ba16 | 296 | template<typename MatrixType> |
ykuroda | 0:13a5d365ba16 | 297 | inline typename MatrixType::Index RealQZ<MatrixType>::findSmallDiagEntry(Index f, Index l) |
ykuroda | 0:13a5d365ba16 | 298 | { |
ykuroda | 0:13a5d365ba16 | 299 | using std::abs; |
ykuroda | 0:13a5d365ba16 | 300 | Index res = l; |
ykuroda | 0:13a5d365ba16 | 301 | while (res >= f) { |
ykuroda | 0:13a5d365ba16 | 302 | if (abs(m_T.coeff(res,res)) <= NumTraits<Scalar>::epsilon() * m_normOfT) |
ykuroda | 0:13a5d365ba16 | 303 | break; |
ykuroda | 0:13a5d365ba16 | 304 | res--; |
ykuroda | 0:13a5d365ba16 | 305 | } |
ykuroda | 0:13a5d365ba16 | 306 | return res; |
ykuroda | 0:13a5d365ba16 | 307 | } |
ykuroda | 0:13a5d365ba16 | 308 | |
ykuroda | 0:13a5d365ba16 | 309 | /** \internal decouple 2x2 diagonal block in rows i, i+1 if eigenvalues are real */ |
ykuroda | 0:13a5d365ba16 | 310 | template<typename MatrixType> |
ykuroda | 0:13a5d365ba16 | 311 | inline void RealQZ<MatrixType>::splitOffTwoRows(Index i) |
ykuroda | 0:13a5d365ba16 | 312 | { |
ykuroda | 0:13a5d365ba16 | 313 | using std::abs; |
ykuroda | 0:13a5d365ba16 | 314 | using std::sqrt; |
ykuroda | 0:13a5d365ba16 | 315 | const Index dim=m_S.cols(); |
ykuroda | 0:13a5d365ba16 | 316 | if (abs(m_S.coeff(i+1,i))==Scalar(0)) |
ykuroda | 0:13a5d365ba16 | 317 | return; |
ykuroda | 0:13a5d365ba16 | 318 | Index z = findSmallDiagEntry(i,i+1); |
ykuroda | 0:13a5d365ba16 | 319 | if (z==i-1) |
ykuroda | 0:13a5d365ba16 | 320 | { |
ykuroda | 0:13a5d365ba16 | 321 | // block of (S T^{-1}) |
ykuroda | 0:13a5d365ba16 | 322 | Matrix2s STi = m_T.template block<2,2>(i,i).template triangularView<Upper>(). |
ykuroda | 0:13a5d365ba16 | 323 | template solve<OnTheRight>(m_S.template block<2,2>(i,i)); |
ykuroda | 0:13a5d365ba16 | 324 | Scalar p = Scalar(0.5)*(STi(0,0)-STi(1,1)); |
ykuroda | 0:13a5d365ba16 | 325 | Scalar q = p*p + STi(1,0)*STi(0,1); |
ykuroda | 0:13a5d365ba16 | 326 | if (q>=0) { |
ykuroda | 0:13a5d365ba16 | 327 | Scalar z = sqrt(q); |
ykuroda | 0:13a5d365ba16 | 328 | // one QR-like iteration for ABi - lambda I |
ykuroda | 0:13a5d365ba16 | 329 | // is enough - when we know exact eigenvalue in advance, |
ykuroda | 0:13a5d365ba16 | 330 | // convergence is immediate |
ykuroda | 0:13a5d365ba16 | 331 | JRs G; |
ykuroda | 0:13a5d365ba16 | 332 | if (p>=0) |
ykuroda | 0:13a5d365ba16 | 333 | G.makeGivens(p + z, STi(1,0)); |
ykuroda | 0:13a5d365ba16 | 334 | else |
ykuroda | 0:13a5d365ba16 | 335 | G.makeGivens(p - z, STi(1,0)); |
ykuroda | 0:13a5d365ba16 | 336 | m_S.rightCols(dim-i).applyOnTheLeft(i,i+1,G.adjoint()); |
ykuroda | 0:13a5d365ba16 | 337 | m_T.rightCols(dim-i).applyOnTheLeft(i,i+1,G.adjoint()); |
ykuroda | 0:13a5d365ba16 | 338 | // update Q |
ykuroda | 0:13a5d365ba16 | 339 | if (m_computeQZ) |
ykuroda | 0:13a5d365ba16 | 340 | m_Q.applyOnTheRight(i,i+1,G); |
ykuroda | 0:13a5d365ba16 | 341 | |
ykuroda | 0:13a5d365ba16 | 342 | G.makeGivens(m_T.coeff(i+1,i+1), m_T.coeff(i+1,i)); |
ykuroda | 0:13a5d365ba16 | 343 | m_S.topRows(i+2).applyOnTheRight(i+1,i,G); |
ykuroda | 0:13a5d365ba16 | 344 | m_T.topRows(i+2).applyOnTheRight(i+1,i,G); |
ykuroda | 0:13a5d365ba16 | 345 | // update Z |
ykuroda | 0:13a5d365ba16 | 346 | if (m_computeQZ) |
ykuroda | 0:13a5d365ba16 | 347 | m_Z.applyOnTheLeft(i+1,i,G.adjoint()); |
ykuroda | 0:13a5d365ba16 | 348 | |
ykuroda | 0:13a5d365ba16 | 349 | m_S.coeffRef(i+1,i) = Scalar(0.0); |
ykuroda | 0:13a5d365ba16 | 350 | m_T.coeffRef(i+1,i) = Scalar(0.0); |
ykuroda | 0:13a5d365ba16 | 351 | } |
ykuroda | 0:13a5d365ba16 | 352 | } |
ykuroda | 0:13a5d365ba16 | 353 | else |
ykuroda | 0:13a5d365ba16 | 354 | { |
ykuroda | 0:13a5d365ba16 | 355 | pushDownZero(z,i,i+1); |
ykuroda | 0:13a5d365ba16 | 356 | } |
ykuroda | 0:13a5d365ba16 | 357 | } |
ykuroda | 0:13a5d365ba16 | 358 | |
ykuroda | 0:13a5d365ba16 | 359 | /** \internal use zero in T(z,z) to zero S(l,l-1), working in block f..l */ |
ykuroda | 0:13a5d365ba16 | 360 | template<typename MatrixType> |
ykuroda | 0:13a5d365ba16 | 361 | inline void RealQZ<MatrixType>::pushDownZero(Index z, Index f, Index l) |
ykuroda | 0:13a5d365ba16 | 362 | { |
ykuroda | 0:13a5d365ba16 | 363 | JRs G; |
ykuroda | 0:13a5d365ba16 | 364 | const Index dim = m_S.cols(); |
ykuroda | 0:13a5d365ba16 | 365 | for (Index zz=z; zz<l; zz++) |
ykuroda | 0:13a5d365ba16 | 366 | { |
ykuroda | 0:13a5d365ba16 | 367 | // push 0 down |
ykuroda | 0:13a5d365ba16 | 368 | Index firstColS = zz>f ? (zz-1) : zz; |
ykuroda | 0:13a5d365ba16 | 369 | G.makeGivens(m_T.coeff(zz, zz+1), m_T.coeff(zz+1, zz+1)); |
ykuroda | 0:13a5d365ba16 | 370 | m_S.rightCols(dim-firstColS).applyOnTheLeft(zz,zz+1,G.adjoint()); |
ykuroda | 0:13a5d365ba16 | 371 | m_T.rightCols(dim-zz).applyOnTheLeft(zz,zz+1,G.adjoint()); |
ykuroda | 0:13a5d365ba16 | 372 | m_T.coeffRef(zz+1,zz+1) = Scalar(0.0); |
ykuroda | 0:13a5d365ba16 | 373 | // update Q |
ykuroda | 0:13a5d365ba16 | 374 | if (m_computeQZ) |
ykuroda | 0:13a5d365ba16 | 375 | m_Q.applyOnTheRight(zz,zz+1,G); |
ykuroda | 0:13a5d365ba16 | 376 | // kill S(zz+1, zz-1) |
ykuroda | 0:13a5d365ba16 | 377 | if (zz>f) |
ykuroda | 0:13a5d365ba16 | 378 | { |
ykuroda | 0:13a5d365ba16 | 379 | G.makeGivens(m_S.coeff(zz+1, zz), m_S.coeff(zz+1,zz-1)); |
ykuroda | 0:13a5d365ba16 | 380 | m_S.topRows(zz+2).applyOnTheRight(zz, zz-1,G); |
ykuroda | 0:13a5d365ba16 | 381 | m_T.topRows(zz+1).applyOnTheRight(zz, zz-1,G); |
ykuroda | 0:13a5d365ba16 | 382 | m_S.coeffRef(zz+1,zz-1) = Scalar(0.0); |
ykuroda | 0:13a5d365ba16 | 383 | // update Z |
ykuroda | 0:13a5d365ba16 | 384 | if (m_computeQZ) |
ykuroda | 0:13a5d365ba16 | 385 | m_Z.applyOnTheLeft(zz,zz-1,G.adjoint()); |
ykuroda | 0:13a5d365ba16 | 386 | } |
ykuroda | 0:13a5d365ba16 | 387 | } |
ykuroda | 0:13a5d365ba16 | 388 | // finally kill S(l,l-1) |
ykuroda | 0:13a5d365ba16 | 389 | G.makeGivens(m_S.coeff(l,l), m_S.coeff(l,l-1)); |
ykuroda | 0:13a5d365ba16 | 390 | m_S.applyOnTheRight(l,l-1,G); |
ykuroda | 0:13a5d365ba16 | 391 | m_T.applyOnTheRight(l,l-1,G); |
ykuroda | 0:13a5d365ba16 | 392 | m_S.coeffRef(l,l-1)=Scalar(0.0); |
ykuroda | 0:13a5d365ba16 | 393 | // update Z |
ykuroda | 0:13a5d365ba16 | 394 | if (m_computeQZ) |
ykuroda | 0:13a5d365ba16 | 395 | m_Z.applyOnTheLeft(l,l-1,G.adjoint()); |
ykuroda | 0:13a5d365ba16 | 396 | } |
ykuroda | 0:13a5d365ba16 | 397 | |
ykuroda | 0:13a5d365ba16 | 398 | /** \internal QR-like iterative step for block f..l */ |
ykuroda | 0:13a5d365ba16 | 399 | template<typename MatrixType> |
ykuroda | 0:13a5d365ba16 | 400 | inline void RealQZ<MatrixType>::step(Index f, Index l, Index iter) |
ykuroda | 0:13a5d365ba16 | 401 | { |
ykuroda | 0:13a5d365ba16 | 402 | using std::abs; |
ykuroda | 0:13a5d365ba16 | 403 | const Index dim = m_S.cols(); |
ykuroda | 0:13a5d365ba16 | 404 | |
ykuroda | 0:13a5d365ba16 | 405 | // x, y, z |
ykuroda | 0:13a5d365ba16 | 406 | Scalar x, y, z; |
ykuroda | 0:13a5d365ba16 | 407 | if (iter==10) |
ykuroda | 0:13a5d365ba16 | 408 | { |
ykuroda | 0:13a5d365ba16 | 409 | // Wilkinson ad hoc shift |
ykuroda | 0:13a5d365ba16 | 410 | const Scalar |
ykuroda | 0:13a5d365ba16 | 411 | a11=m_S.coeff(f+0,f+0), a12=m_S.coeff(f+0,f+1), |
ykuroda | 0:13a5d365ba16 | 412 | a21=m_S.coeff(f+1,f+0), a22=m_S.coeff(f+1,f+1), a32=m_S.coeff(f+2,f+1), |
ykuroda | 0:13a5d365ba16 | 413 | b12=m_T.coeff(f+0,f+1), |
ykuroda | 0:13a5d365ba16 | 414 | b11i=Scalar(1.0)/m_T.coeff(f+0,f+0), |
ykuroda | 0:13a5d365ba16 | 415 | b22i=Scalar(1.0)/m_T.coeff(f+1,f+1), |
ykuroda | 0:13a5d365ba16 | 416 | a87=m_S.coeff(l-1,l-2), |
ykuroda | 0:13a5d365ba16 | 417 | a98=m_S.coeff(l-0,l-1), |
ykuroda | 0:13a5d365ba16 | 418 | b77i=Scalar(1.0)/m_T.coeff(l-2,l-2), |
ykuroda | 0:13a5d365ba16 | 419 | b88i=Scalar(1.0)/m_T.coeff(l-1,l-1); |
ykuroda | 0:13a5d365ba16 | 420 | Scalar ss = abs(a87*b77i) + abs(a98*b88i), |
ykuroda | 0:13a5d365ba16 | 421 | lpl = Scalar(1.5)*ss, |
ykuroda | 0:13a5d365ba16 | 422 | ll = ss*ss; |
ykuroda | 0:13a5d365ba16 | 423 | x = ll + a11*a11*b11i*b11i - lpl*a11*b11i + a12*a21*b11i*b22i |
ykuroda | 0:13a5d365ba16 | 424 | - a11*a21*b12*b11i*b11i*b22i; |
ykuroda | 0:13a5d365ba16 | 425 | y = a11*a21*b11i*b11i - lpl*a21*b11i + a21*a22*b11i*b22i |
ykuroda | 0:13a5d365ba16 | 426 | - a21*a21*b12*b11i*b11i*b22i; |
ykuroda | 0:13a5d365ba16 | 427 | z = a21*a32*b11i*b22i; |
ykuroda | 0:13a5d365ba16 | 428 | } |
ykuroda | 0:13a5d365ba16 | 429 | else if (iter==16) |
ykuroda | 0:13a5d365ba16 | 430 | { |
ykuroda | 0:13a5d365ba16 | 431 | // another exceptional shift |
ykuroda | 0:13a5d365ba16 | 432 | 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) / |
ykuroda | 0:13a5d365ba16 | 433 | (m_T.coeff(l-1,l-1)*m_T.coeff(l,l)); |
ykuroda | 0:13a5d365ba16 | 434 | y = m_S.coeff(f+1,f)/m_T.coeff(f,f); |
ykuroda | 0:13a5d365ba16 | 435 | z = 0; |
ykuroda | 0:13a5d365ba16 | 436 | } |
ykuroda | 0:13a5d365ba16 | 437 | else if (iter>23 && !(iter%8)) |
ykuroda | 0:13a5d365ba16 | 438 | { |
ykuroda | 0:13a5d365ba16 | 439 | // extremely exceptional shift |
ykuroda | 0:13a5d365ba16 | 440 | x = internal::random<Scalar>(-1.0,1.0); |
ykuroda | 0:13a5d365ba16 | 441 | y = internal::random<Scalar>(-1.0,1.0); |
ykuroda | 0:13a5d365ba16 | 442 | z = internal::random<Scalar>(-1.0,1.0); |
ykuroda | 0:13a5d365ba16 | 443 | } |
ykuroda | 0:13a5d365ba16 | 444 | else |
ykuroda | 0:13a5d365ba16 | 445 | { |
ykuroda | 0:13a5d365ba16 | 446 | // Compute the shifts: (x,y,z,0...) = (AB^-1 - l1 I) (AB^-1 - l2 I) e1 |
ykuroda | 0:13a5d365ba16 | 447 | // where l1 and l2 are the eigenvalues of the 2x2 matrix C = U V^-1 where |
ykuroda | 0:13a5d365ba16 | 448 | // U and V are 2x2 bottom right sub matrices of A and B. Thus: |
ykuroda | 0:13a5d365ba16 | 449 | // = AB^-1AB^-1 + l1 l2 I - (l1+l2)(AB^-1) |
ykuroda | 0:13a5d365ba16 | 450 | // = AB^-1AB^-1 + det(M) - tr(M)(AB^-1) |
ykuroda | 0:13a5d365ba16 | 451 | // Since we are only interested in having x, y, z with a correct ratio, we have: |
ykuroda | 0:13a5d365ba16 | 452 | const Scalar |
ykuroda | 0:13a5d365ba16 | 453 | a11 = m_S.coeff(f,f), a12 = m_S.coeff(f,f+1), |
ykuroda | 0:13a5d365ba16 | 454 | a21 = m_S.coeff(f+1,f), a22 = m_S.coeff(f+1,f+1), |
ykuroda | 0:13a5d365ba16 | 455 | a32 = m_S.coeff(f+2,f+1), |
ykuroda | 0:13a5d365ba16 | 456 | |
ykuroda | 0:13a5d365ba16 | 457 | a88 = m_S.coeff(l-1,l-1), a89 = m_S.coeff(l-1,l), |
ykuroda | 0:13a5d365ba16 | 458 | a98 = m_S.coeff(l,l-1), a99 = m_S.coeff(l,l), |
ykuroda | 0:13a5d365ba16 | 459 | |
ykuroda | 0:13a5d365ba16 | 460 | b11 = m_T.coeff(f,f), b12 = m_T.coeff(f,f+1), |
ykuroda | 0:13a5d365ba16 | 461 | b22 = m_T.coeff(f+1,f+1), |
ykuroda | 0:13a5d365ba16 | 462 | |
ykuroda | 0:13a5d365ba16 | 463 | b88 = m_T.coeff(l-1,l-1), b89 = m_T.coeff(l-1,l), |
ykuroda | 0:13a5d365ba16 | 464 | b99 = m_T.coeff(l,l); |
ykuroda | 0:13a5d365ba16 | 465 | |
ykuroda | 0:13a5d365ba16 | 466 | x = ( (a88/b88 - a11/b11)*(a99/b99 - a11/b11) - (a89/b99)*(a98/b88) + (a98/b88)*(b89/b99)*(a11/b11) ) * (b11/a21) |
ykuroda | 0:13a5d365ba16 | 467 | + a12/b22 - (a11/b11)*(b12/b22); |
ykuroda | 0:13a5d365ba16 | 468 | y = (a22/b22-a11/b11) - (a21/b11)*(b12/b22) - (a88/b88-a11/b11) - (a99/b99-a11/b11) + (a98/b88)*(b89/b99); |
ykuroda | 0:13a5d365ba16 | 469 | z = a32/b22; |
ykuroda | 0:13a5d365ba16 | 470 | } |
ykuroda | 0:13a5d365ba16 | 471 | |
ykuroda | 0:13a5d365ba16 | 472 | JRs G; |
ykuroda | 0:13a5d365ba16 | 473 | |
ykuroda | 0:13a5d365ba16 | 474 | for (Index k=f; k<=l-2; k++) |
ykuroda | 0:13a5d365ba16 | 475 | { |
ykuroda | 0:13a5d365ba16 | 476 | // variables for Householder reflections |
ykuroda | 0:13a5d365ba16 | 477 | Vector2s essential2; |
ykuroda | 0:13a5d365ba16 | 478 | Scalar tau, beta; |
ykuroda | 0:13a5d365ba16 | 479 | |
ykuroda | 0:13a5d365ba16 | 480 | Vector3s hr(x,y,z); |
ykuroda | 0:13a5d365ba16 | 481 | |
ykuroda | 0:13a5d365ba16 | 482 | // Q_k to annihilate S(k+1,k-1) and S(k+2,k-1) |
ykuroda | 0:13a5d365ba16 | 483 | hr.makeHouseholderInPlace(tau, beta); |
ykuroda | 0:13a5d365ba16 | 484 | essential2 = hr.template bottomRows<2>(); |
ykuroda | 0:13a5d365ba16 | 485 | Index fc=(std::max)(k-1,Index(0)); // first col to update |
ykuroda | 0:13a5d365ba16 | 486 | m_S.template middleRows<3>(k).rightCols(dim-fc).applyHouseholderOnTheLeft(essential2, tau, m_workspace.data()); |
ykuroda | 0:13a5d365ba16 | 487 | m_T.template middleRows<3>(k).rightCols(dim-fc).applyHouseholderOnTheLeft(essential2, tau, m_workspace.data()); |
ykuroda | 0:13a5d365ba16 | 488 | if (m_computeQZ) |
ykuroda | 0:13a5d365ba16 | 489 | m_Q.template middleCols<3>(k).applyHouseholderOnTheRight(essential2, tau, m_workspace.data()); |
ykuroda | 0:13a5d365ba16 | 490 | if (k>f) |
ykuroda | 0:13a5d365ba16 | 491 | m_S.coeffRef(k+2,k-1) = m_S.coeffRef(k+1,k-1) = Scalar(0.0); |
ykuroda | 0:13a5d365ba16 | 492 | |
ykuroda | 0:13a5d365ba16 | 493 | // Z_{k1} to annihilate T(k+2,k+1) and T(k+2,k) |
ykuroda | 0:13a5d365ba16 | 494 | hr << m_T.coeff(k+2,k+2),m_T.coeff(k+2,k),m_T.coeff(k+2,k+1); |
ykuroda | 0:13a5d365ba16 | 495 | hr.makeHouseholderInPlace(tau, beta); |
ykuroda | 0:13a5d365ba16 | 496 | essential2 = hr.template bottomRows<2>(); |
ykuroda | 0:13a5d365ba16 | 497 | { |
ykuroda | 0:13a5d365ba16 | 498 | Index lr = (std::min)(k+4,dim); // last row to update |
ykuroda | 0:13a5d365ba16 | 499 | Map<Matrix<Scalar,Dynamic,1> > tmp(m_workspace.data(),lr); |
ykuroda | 0:13a5d365ba16 | 500 | // S |
ykuroda | 0:13a5d365ba16 | 501 | tmp = m_S.template middleCols<2>(k).topRows(lr) * essential2; |
ykuroda | 0:13a5d365ba16 | 502 | tmp += m_S.col(k+2).head(lr); |
ykuroda | 0:13a5d365ba16 | 503 | m_S.col(k+2).head(lr) -= tau*tmp; |
ykuroda | 0:13a5d365ba16 | 504 | m_S.template middleCols<2>(k).topRows(lr) -= (tau*tmp) * essential2.adjoint(); |
ykuroda | 0:13a5d365ba16 | 505 | // T |
ykuroda | 0:13a5d365ba16 | 506 | tmp = m_T.template middleCols<2>(k).topRows(lr) * essential2; |
ykuroda | 0:13a5d365ba16 | 507 | tmp += m_T.col(k+2).head(lr); |
ykuroda | 0:13a5d365ba16 | 508 | m_T.col(k+2).head(lr) -= tau*tmp; |
ykuroda | 0:13a5d365ba16 | 509 | m_T.template middleCols<2>(k).topRows(lr) -= (tau*tmp) * essential2.adjoint(); |
ykuroda | 0:13a5d365ba16 | 510 | } |
ykuroda | 0:13a5d365ba16 | 511 | if (m_computeQZ) |
ykuroda | 0:13a5d365ba16 | 512 | { |
ykuroda | 0:13a5d365ba16 | 513 | // Z |
ykuroda | 0:13a5d365ba16 | 514 | Map<Matrix<Scalar,1,Dynamic> > tmp(m_workspace.data(),dim); |
ykuroda | 0:13a5d365ba16 | 515 | tmp = essential2.adjoint()*(m_Z.template middleRows<2>(k)); |
ykuroda | 0:13a5d365ba16 | 516 | tmp += m_Z.row(k+2); |
ykuroda | 0:13a5d365ba16 | 517 | m_Z.row(k+2) -= tau*tmp; |
ykuroda | 0:13a5d365ba16 | 518 | m_Z.template middleRows<2>(k) -= essential2 * (tau*tmp); |
ykuroda | 0:13a5d365ba16 | 519 | } |
ykuroda | 0:13a5d365ba16 | 520 | m_T.coeffRef(k+2,k) = m_T.coeffRef(k+2,k+1) = Scalar(0.0); |
ykuroda | 0:13a5d365ba16 | 521 | |
ykuroda | 0:13a5d365ba16 | 522 | // Z_{k2} to annihilate T(k+1,k) |
ykuroda | 0:13a5d365ba16 | 523 | G.makeGivens(m_T.coeff(k+1,k+1), m_T.coeff(k+1,k)); |
ykuroda | 0:13a5d365ba16 | 524 | m_S.applyOnTheRight(k+1,k,G); |
ykuroda | 0:13a5d365ba16 | 525 | m_T.applyOnTheRight(k+1,k,G); |
ykuroda | 0:13a5d365ba16 | 526 | // update Z |
ykuroda | 0:13a5d365ba16 | 527 | if (m_computeQZ) |
ykuroda | 0:13a5d365ba16 | 528 | m_Z.applyOnTheLeft(k+1,k,G.adjoint()); |
ykuroda | 0:13a5d365ba16 | 529 | m_T.coeffRef(k+1,k) = Scalar(0.0); |
ykuroda | 0:13a5d365ba16 | 530 | |
ykuroda | 0:13a5d365ba16 | 531 | // update x,y,z |
ykuroda | 0:13a5d365ba16 | 532 | x = m_S.coeff(k+1,k); |
ykuroda | 0:13a5d365ba16 | 533 | y = m_S.coeff(k+2,k); |
ykuroda | 0:13a5d365ba16 | 534 | if (k < l-2) |
ykuroda | 0:13a5d365ba16 | 535 | z = m_S.coeff(k+3,k); |
ykuroda | 0:13a5d365ba16 | 536 | } // loop over k |
ykuroda | 0:13a5d365ba16 | 537 | |
ykuroda | 0:13a5d365ba16 | 538 | // Q_{n-1} to annihilate y = S(l,l-2) |
ykuroda | 0:13a5d365ba16 | 539 | G.makeGivens(x,y); |
ykuroda | 0:13a5d365ba16 | 540 | m_S.applyOnTheLeft(l-1,l,G.adjoint()); |
ykuroda | 0:13a5d365ba16 | 541 | m_T.applyOnTheLeft(l-1,l,G.adjoint()); |
ykuroda | 0:13a5d365ba16 | 542 | if (m_computeQZ) |
ykuroda | 0:13a5d365ba16 | 543 | m_Q.applyOnTheRight(l-1,l,G); |
ykuroda | 0:13a5d365ba16 | 544 | m_S.coeffRef(l,l-2) = Scalar(0.0); |
ykuroda | 0:13a5d365ba16 | 545 | |
ykuroda | 0:13a5d365ba16 | 546 | // Z_{n-1} to annihilate T(l,l-1) |
ykuroda | 0:13a5d365ba16 | 547 | G.makeGivens(m_T.coeff(l,l),m_T.coeff(l,l-1)); |
ykuroda | 0:13a5d365ba16 | 548 | m_S.applyOnTheRight(l,l-1,G); |
ykuroda | 0:13a5d365ba16 | 549 | m_T.applyOnTheRight(l,l-1,G); |
ykuroda | 0:13a5d365ba16 | 550 | if (m_computeQZ) |
ykuroda | 0:13a5d365ba16 | 551 | m_Z.applyOnTheLeft(l,l-1,G.adjoint()); |
ykuroda | 0:13a5d365ba16 | 552 | m_T.coeffRef(l,l-1) = Scalar(0.0); |
ykuroda | 0:13a5d365ba16 | 553 | } |
ykuroda | 0:13a5d365ba16 | 554 | |
ykuroda | 0:13a5d365ba16 | 555 | |
ykuroda | 0:13a5d365ba16 | 556 | template<typename MatrixType> |
ykuroda | 0:13a5d365ba16 | 557 | RealQZ<MatrixType>& RealQZ<MatrixType>::compute(const MatrixType& A_in, const MatrixType& B_in, bool computeQZ) |
ykuroda | 0:13a5d365ba16 | 558 | { |
ykuroda | 0:13a5d365ba16 | 559 | |
ykuroda | 0:13a5d365ba16 | 560 | const Index dim = A_in.cols(); |
ykuroda | 0:13a5d365ba16 | 561 | |
ykuroda | 0:13a5d365ba16 | 562 | eigen_assert (A_in.rows()==dim && A_in.cols()==dim |
ykuroda | 0:13a5d365ba16 | 563 | && B_in.rows()==dim && B_in.cols()==dim |
ykuroda | 0:13a5d365ba16 | 564 | && "Need square matrices of the same dimension"); |
ykuroda | 0:13a5d365ba16 | 565 | |
ykuroda | 0:13a5d365ba16 | 566 | m_isInitialized = true; |
ykuroda | 0:13a5d365ba16 | 567 | m_computeQZ = computeQZ; |
ykuroda | 0:13a5d365ba16 | 568 | m_S = A_in; m_T = B_in; |
ykuroda | 0:13a5d365ba16 | 569 | m_workspace.resize(dim*2); |
ykuroda | 0:13a5d365ba16 | 570 | m_global_iter = 0; |
ykuroda | 0:13a5d365ba16 | 571 | |
ykuroda | 0:13a5d365ba16 | 572 | // entrance point: hessenberg triangular decomposition |
ykuroda | 0:13a5d365ba16 | 573 | hessenbergTriangular(); |
ykuroda | 0:13a5d365ba16 | 574 | // compute L1 vector norms of T, S into m_normOfS, m_normOfT |
ykuroda | 0:13a5d365ba16 | 575 | computeNorms(); |
ykuroda | 0:13a5d365ba16 | 576 | |
ykuroda | 0:13a5d365ba16 | 577 | Index l = dim-1, |
ykuroda | 0:13a5d365ba16 | 578 | f, |
ykuroda | 0:13a5d365ba16 | 579 | local_iter = 0; |
ykuroda | 0:13a5d365ba16 | 580 | |
ykuroda | 0:13a5d365ba16 | 581 | while (l>0 && local_iter<m_maxIters) |
ykuroda | 0:13a5d365ba16 | 582 | { |
ykuroda | 0:13a5d365ba16 | 583 | f = findSmallSubdiagEntry(l); |
ykuroda | 0:13a5d365ba16 | 584 | // now rows and columns f..l (including) decouple from the rest of the problem |
ykuroda | 0:13a5d365ba16 | 585 | if (f>0) m_S.coeffRef(f,f-1) = Scalar(0.0); |
ykuroda | 0:13a5d365ba16 | 586 | if (f == l) // One root found |
ykuroda | 0:13a5d365ba16 | 587 | { |
ykuroda | 0:13a5d365ba16 | 588 | l--; |
ykuroda | 0:13a5d365ba16 | 589 | local_iter = 0; |
ykuroda | 0:13a5d365ba16 | 590 | } |
ykuroda | 0:13a5d365ba16 | 591 | else if (f == l-1) // Two roots found |
ykuroda | 0:13a5d365ba16 | 592 | { |
ykuroda | 0:13a5d365ba16 | 593 | splitOffTwoRows(f); |
ykuroda | 0:13a5d365ba16 | 594 | l -= 2; |
ykuroda | 0:13a5d365ba16 | 595 | local_iter = 0; |
ykuroda | 0:13a5d365ba16 | 596 | } |
ykuroda | 0:13a5d365ba16 | 597 | else // No convergence yet |
ykuroda | 0:13a5d365ba16 | 598 | { |
ykuroda | 0:13a5d365ba16 | 599 | // if there's zero on diagonal of T, we can isolate an eigenvalue with Givens rotations |
ykuroda | 0:13a5d365ba16 | 600 | Index z = findSmallDiagEntry(f,l); |
ykuroda | 0:13a5d365ba16 | 601 | if (z>=f) |
ykuroda | 0:13a5d365ba16 | 602 | { |
ykuroda | 0:13a5d365ba16 | 603 | // zero found |
ykuroda | 0:13a5d365ba16 | 604 | pushDownZero(z,f,l); |
ykuroda | 0:13a5d365ba16 | 605 | } |
ykuroda | 0:13a5d365ba16 | 606 | else |
ykuroda | 0:13a5d365ba16 | 607 | { |
ykuroda | 0:13a5d365ba16 | 608 | // We are sure now that S.block(f,f, l-f+1,l-f+1) is underuced upper-Hessenberg |
ykuroda | 0:13a5d365ba16 | 609 | // and T.block(f,f, l-f+1,l-f+1) is invertible uper-triangular, which allows to |
ykuroda | 0:13a5d365ba16 | 610 | // apply a QR-like iteration to rows and columns f..l. |
ykuroda | 0:13a5d365ba16 | 611 | step(f,l, local_iter); |
ykuroda | 0:13a5d365ba16 | 612 | local_iter++; |
ykuroda | 0:13a5d365ba16 | 613 | m_global_iter++; |
ykuroda | 0:13a5d365ba16 | 614 | } |
ykuroda | 0:13a5d365ba16 | 615 | } |
ykuroda | 0:13a5d365ba16 | 616 | } |
ykuroda | 0:13a5d365ba16 | 617 | // check if we converged before reaching iterations limit |
ykuroda | 0:13a5d365ba16 | 618 | m_info = (local_iter<m_maxIters) ? Success : NoConvergence; |
ykuroda | 0:13a5d365ba16 | 619 | return *this; |
ykuroda | 0:13a5d365ba16 | 620 | } // end compute |
ykuroda | 0:13a5d365ba16 | 621 | |
ykuroda | 0:13a5d365ba16 | 622 | } // end namespace Eigen |
ykuroda | 0:13a5d365ba16 | 623 | |
ykuroda | 0:13a5d365ba16 | 624 | #endif //EIGEN_REAL_QZ |