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