Yoji KURODA / Eigen

Eigne Matrix Class Library

Dependents:   Eigen_test Odometry_test AttitudeEstimation_usingTicker MPU9250_Quaternion_Binary_Serial ... more

Eigen Matrix Class Library for mbed.

Finally, you can use Eigen on your mbed!!!

src/Eigenvalues/RealQZ.h@0:13a5d365ba16, 2016-10-13 (annotated)

Committer:
ykuroda
Date:
Thu Oct 13 04:07:23 2016 +0000
Revision:
0:13a5d365ba16
First commint, Eigne Matrix Class Library

Who changed what in which revision?

UserRevisionLine numberNew 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