10#ifndef EIGEN_GENERALIZED_SCHUR_REORDERING_H
11#define EIGEN_GENERALIZED_SCHUR_REORDERING_H
14#include "./InternalHeaderCheck.h"
25template <
typename MatrixType>
26Index generalized_schur_block_size(
const MatrixType& S, Index k) {
27 if (NumTraits<typename MatrixType::Scalar>::IsComplex)
return 1;
28 return (k + 1 < S.rows() && !numext::is_exactly_zero(S.coeff(k + 1, k))) ? 2 : 1;
39template <
typename MatrixType>
40bool swap_generalized_schur_1x1(MatrixType& S, MatrixType& T, MatrixType* Q, MatrixType& Z, Index j) {
41 using Scalar =
typename MatrixType::Scalar;
42 using RealScalar =
typename NumTraits<Scalar>::Real;
43 using Matrix2 = Matrix<Scalar, 2, 2>;
44 const Matrix2 a = S.template block<2, 2>(j, j);
45 const Matrix2 b = T.template block<2, 2>(j, j);
46 const Scalar f = a(1, 1) * b(0, 0) - b(1, 1) * a(0, 0);
47 const Scalar g = a(1, 1) * b(0, 1) - b(1, 1) * a(0, 1);
49 if (numext::is_exactly_zero(f) && numext::is_exactly_zero(g))
return false;
50 JacobiRotation<Scalar> gz, gq;
53 an.applyOnTheRight(0, 1, gz);
54 bn.applyOnTheRight(0, 1, gz);
55 if (numext::abs(a(1, 1)) * numext::abs(b(0, 0)) >= numext::abs(a(0, 0)) * numext::abs(b(1, 1)))
56 gq.makeGivens(an(0, 0), an(1, 0));
58 gq.makeGivens(bn(0, 0), bn(1, 0));
59 an.applyOnTheLeft(0, 1, gq.adjoint());
60 bn.applyOnTheLeft(0, 1, gq.adjoint());
63 const RealScalar eps = NumTraits<RealScalar>::epsilon();
64 const RealScalar smlnum = (std::numeric_limits<RealScalar>::min)() / eps;
65 const RealScalar threshA = numext::maxi(RealScalar(20) * eps * a.stableNorm(), smlnum);
66 const RealScalar threshB = numext::maxi(RealScalar(20) * eps * b.stableNorm(), smlnum);
67 if (!(numext::abs(an(1, 0)) <= threshA && numext::abs(bn(1, 0)) <= threshB))
return false;
71 ra.applyOnTheLeft(0, 1, gq);
72 rb.applyOnTheLeft(0, 1, gq);
73 ra.applyOnTheRight(0, 1, gz.adjoint());
74 rb.applyOnTheRight(0, 1, gz.adjoint());
75 if (!((ra - a).stableNorm() <= threshA && (rb - b).stableNorm() <= threshB))
return false;
77 S.template block<2, 2>(j, j) = an;
78 T.template block<2, 2>(j, j) = bn;
79 const Index right = S.cols() - j - 2;
81 S.rightCols(right).applyOnTheLeft(j, j + 1, gq.adjoint());
82 T.rightCols(right).applyOnTheLeft(j, j + 1, gq.adjoint());
85 S.topRows(j).applyOnTheRight(j, j + 1, gz);
86 T.topRows(j).applyOnTheRight(j, j + 1, gz);
88 if (Q !=
nullptr) Q->applyOnTheRight(j, j + 1, gq);
89 Z.applyOnTheRight(j, j + 1, gz);
105template <
typename MatrixType>
106bool swap_generalized_schur_sylvester(MatrixType& S, MatrixType& T, MatrixType* Q, MatrixType& Z, Index j, Index n1,
108 using Scalar =
typename MatrixType::Scalar;
109 using RealScalar =
typename NumTraits<Scalar>::Real;
110 using LocalMatrix = Matrix<Scalar, Dynamic, Dynamic, ColMajor, 4, 4>;
111 using SylvesterMatrix = Matrix<Scalar, Dynamic, Dynamic, ColMajor, 8, 8>;
112 using SylvesterVector = Matrix<Scalar, Dynamic, 1, ColMajor, 8, 1>;
113 const Index m = n1 + n2;
114 const Index k = n1 * n2;
115 const LocalMatrix A = S.block(j, j, m, m);
116 const LocalMatrix B = T.block(j, j, m, m);
120 SylvesterMatrix K = SylvesterMatrix::Zero(2 * k, 2 * k);
121 for (Index c = 0; c < n2; ++c) {
122 K.block(c * n1, c * n1, n1, n1) = A.topLeftCorner(n1, n1);
123 K.block(k + c * n1, c * n1, n1, n1) = B.topLeftCorner(n1, n1);
124 for (Index i = 0; i < n2; ++i) {
125 K.block(c * n1, k + i * n1, n1, n1).diagonal().setConstant(-A(n1 + i, n1 + c));
126 K.block(k + c * n1, k + i * n1, n1, n1).diagonal().setConstant(-B(n1 + i, n1 + c));
129 SylvesterVector rhs(2 * k);
130 rhs << -A.topRightCorner(n1, n2).reshaped(), -B.topRightCorner(n1, n2).reshaped();
131 const FullPivLU<SylvesterMatrix> sylvester(K);
132 if (!sylvester.isInvertible())
return false;
133 const SylvesterVector sol = sylvester.solve(rhs);
135 LocalMatrix basis(m, n2);
136 basis.topRows(n1) = sol.head(k).reshaped(n1, n2);
137 basis.bottomRows(n2).setIdentity();
138 const LocalMatrix Zl = HouseholderQR<LocalMatrix>(basis).householderQ();
139 basis.topRows(n1) = sol.tail(k).reshaped(n1, n2);
140 const LocalMatrix Ql = HouseholderQR<LocalMatrix>(basis).householderQ();
141 const LocalMatrix An = Ql.adjoint() * A * Zl;
142 const LocalMatrix Bn = Ql.adjoint() * B * Zl;
145 const LocalMatrix H = HouseholderQR<LocalMatrix>(Bn).householderQ();
146 LocalMatrix Aq = H.adjoint() * An;
149 const LocalMatrix W = HouseholderQR<LocalMatrix>(Bn.adjoint().rowwise().reverse()).householderQ();
150 const LocalMatrix V = W.rowwise().reverse();
151 LocalMatrix Ar = An * V;
153 const RealScalar eps = NumTraits<RealScalar>::epsilon();
154 const RealScalar smlnum = (std::numeric_limits<RealScalar>::min)() / eps;
155 const RealScalar threshA = numext::maxi(RealScalar(20) * eps * A.stableNorm(), smlnum);
156 const RealScalar threshB = numext::maxi(RealScalar(20) * eps * B.stableNorm(), smlnum);
157 const RealScalar qrA21 = Aq.bottomLeftCorner(n1, n2).stableNorm();
158 const RealScalar rqA21 = Ar.bottomLeftCorner(n1, n2).stableNorm();
161 LocalMatrix Qn, Zn, Sn, Tn;
162 if (qrA21 <= rqA21 && qrA21 <= threshA) {
163 Qn.noalias() = Ql * H;
166 Tn.noalias() = H.adjoint() * Bn;
167 }
else if (rqA21 <= threshA) {
169 Zn.noalias() = Zl * V;
171 Tn.noalias() = Bn * V;
175 Sn.bottomLeftCorner(n1, n2).setZero();
176 Tn.template triangularView<StrictlyLower>().setZero();
179 if (!((Qn * Sn * Zn.adjoint() - A).stableNorm() <= threshA && (Qn * Tn * Zn.adjoint() - B).stableNorm() <= threshB))
182 S.block(j, j, m, m) = Sn;
183 T.block(j, j, m, m) = Tn;
184 const Index right = S.cols() - j - m;
186 S.block(j, j + m, m, right) = Qn.adjoint() * S.block(j, j + m, m, right);
187 T.block(j, j + m, m, right) = Qn.adjoint() * T.block(j, j + m, m, right);
190 S.block(0, j, j, m) = S.block(0, j, j, m) * Zn;
191 T.block(0, j, j, m) = T.block(0, j, j, m) * Zn;
193 if (Q !=
nullptr) Q->middleCols(j, m) = Q->middleCols(j, m) * Qn;
194 Z.middleCols(j, m) = Z.middleCols(j, m) * Zn;
208template <
typename MatrixType>
209bool swap_generalized_schur_blocks(MatrixType& S, MatrixType& T, MatrixType* Q, MatrixType& Z, Index j, Index n1,
211 eigen_assert(n1 >= 1 && n1 <= 2 && n2 >= 1 && n2 <= 2 && j >= 0 && j + n1 + n2 <= S.rows());
212 if (n1 == 1 && n2 == 1)
return swap_generalized_schur_1x1(S, T, Q, Z, j);
213 return swap_generalized_schur_sylvester(S, T, Q, Z, j, n1, n2);
221template <
typename MatrixType>
222void clean_generalized_schur_form(MatrixType& S, MatrixType& T) {
223 const Index n = S.rows();
224 T.template triangularView<StrictlyLower>().setZero();
225 if (NumTraits<typename MatrixType::Scalar>::IsComplex)
226 S.template triangularView<StrictlyLower>().setZero();
228 S.bottomLeftCorner(n - 2, n - 2).template triangularView<Lower>().setZero();
239template <
typename MatrixType>
240Index reorder_generalized_schur_inside_unit_circle(MatrixType& S, MatrixType& T, MatrixType* Q, MatrixType& Z) {
241 using Scalar =
typename MatrixType::Scalar;
242 const Index n = S.rows();
243 clean_generalized_schur_form(S, T);
246 for (Index k = 0; k < n;) {
247 const Index bs = generalized_schur_block_size(S, k);
250 inside = numext::abs(S(k, k)) < numext::abs(T(k, k));
253 inside = numext::abs(S.template block<2, 2>(k, k).determinant()) < numext::abs(T(k, k) * T(k + 1, k + 1));
257 for (Index pos = k; pos > selected;) {
259 (!NumTraits<Scalar>::IsComplex && pos - 2 >= selected && !numext::is_exactly_zero(S(pos - 1, pos - 2))) ? 2
261 if (!swap_generalized_schur_blocks(S, T, Q, Z, pos - pbs, pbs, bs))
return -1;
Matrix< Type, 2, 2 > Matrix2
Namespace containing all symbols from the Eigen library.