1 | // This file is part of Eigen, a lightweight C++ template library
|
---|
2 | // for linear algebra.
|
---|
3 | //
|
---|
4 | // Copyright (C) 2010 Manuel Yguel <manuel.yguel@gmail.com>
|
---|
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_COMPANION_H
|
---|
11 | #define EIGEN_COMPANION_H
|
---|
12 |
|
---|
13 | // This file requires the user to include
|
---|
14 | // * Eigen/Core
|
---|
15 | // * Eigen/src/PolynomialSolver.h
|
---|
16 |
|
---|
17 | namespace Eigen {
|
---|
18 |
|
---|
19 | namespace internal {
|
---|
20 |
|
---|
21 | #ifndef EIGEN_PARSED_BY_DOXYGEN
|
---|
22 |
|
---|
23 | template <typename T>
|
---|
24 | T radix(){ return 2; }
|
---|
25 |
|
---|
26 | template <typename T>
|
---|
27 | T radix2(){ return radix<T>()*radix<T>(); }
|
---|
28 |
|
---|
29 | template<int Size>
|
---|
30 | struct decrement_if_fixed_size
|
---|
31 | {
|
---|
32 | enum {
|
---|
33 | ret = (Size == Dynamic) ? Dynamic : Size-1 };
|
---|
34 | };
|
---|
35 |
|
---|
36 | #endif
|
---|
37 |
|
---|
38 | template< typename _Scalar, int _Deg >
|
---|
39 | class companion
|
---|
40 | {
|
---|
41 | public:
|
---|
42 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(_Scalar,_Deg==Dynamic ? Dynamic : _Deg)
|
---|
43 |
|
---|
44 | enum {
|
---|
45 | Deg = _Deg,
|
---|
46 | Deg_1=decrement_if_fixed_size<Deg>::ret
|
---|
47 | };
|
---|
48 |
|
---|
49 | typedef _Scalar Scalar;
|
---|
50 | typedef typename NumTraits<Scalar>::Real RealScalar;
|
---|
51 | typedef Matrix<Scalar, Deg, 1> RightColumn;
|
---|
52 | //typedef DiagonalMatrix< Scalar, Deg_1, Deg_1 > BottomLeftDiagonal;
|
---|
53 | typedef Matrix<Scalar, Deg_1, 1> BottomLeftDiagonal;
|
---|
54 |
|
---|
55 | typedef Matrix<Scalar, Deg, Deg> DenseCompanionMatrixType;
|
---|
56 | typedef Matrix< Scalar, _Deg, Deg_1 > LeftBlock;
|
---|
57 | typedef Matrix< Scalar, Deg_1, Deg_1 > BottomLeftBlock;
|
---|
58 | typedef Matrix< Scalar, 1, Deg_1 > LeftBlockFirstRow;
|
---|
59 |
|
---|
60 | typedef DenseIndex Index;
|
---|
61 |
|
---|
62 | public:
|
---|
63 | EIGEN_STRONG_INLINE const _Scalar operator()(Index row, Index col ) const
|
---|
64 | {
|
---|
65 | if( m_bl_diag.rows() > col )
|
---|
66 | {
|
---|
67 | if( 0 < row ){ return m_bl_diag[col]; }
|
---|
68 | else{ return 0; }
|
---|
69 | }
|
---|
70 | else{ return m_monic[row]; }
|
---|
71 | }
|
---|
72 |
|
---|
73 | public:
|
---|
74 | template<typename VectorType>
|
---|
75 | void setPolynomial( const VectorType& poly )
|
---|
76 | {
|
---|
77 | const Index deg = poly.size()-1;
|
---|
78 | m_monic = -1/poly[deg] * poly.head(deg);
|
---|
79 | //m_bl_diag.setIdentity( deg-1 );
|
---|
80 | m_bl_diag.setOnes(deg-1);
|
---|
81 | }
|
---|
82 |
|
---|
83 | template<typename VectorType>
|
---|
84 | companion( const VectorType& poly ){
|
---|
85 | setPolynomial( poly ); }
|
---|
86 |
|
---|
87 | public:
|
---|
88 | DenseCompanionMatrixType denseMatrix() const
|
---|
89 | {
|
---|
90 | const Index deg = m_monic.size();
|
---|
91 | const Index deg_1 = deg-1;
|
---|
92 | DenseCompanionMatrixType companion(deg,deg);
|
---|
93 | companion <<
|
---|
94 | ( LeftBlock(deg,deg_1)
|
---|
95 | << LeftBlockFirstRow::Zero(1,deg_1),
|
---|
96 | BottomLeftBlock::Identity(deg-1,deg-1)*m_bl_diag.asDiagonal() ).finished()
|
---|
97 | , m_monic;
|
---|
98 | return companion;
|
---|
99 | }
|
---|
100 |
|
---|
101 |
|
---|
102 |
|
---|
103 | protected:
|
---|
104 | /** Helper function for the balancing algorithm.
|
---|
105 | * \returns true if the row and the column, having colNorm and rowNorm
|
---|
106 | * as norms, are balanced, false otherwise.
|
---|
107 | * colB and rowB are repectively the multipliers for
|
---|
108 | * the column and the row in order to balance them.
|
---|
109 | * */
|
---|
110 | bool balanced( Scalar colNorm, Scalar rowNorm,
|
---|
111 | bool& isBalanced, Scalar& colB, Scalar& rowB );
|
---|
112 |
|
---|
113 | /** Helper function for the balancing algorithm.
|
---|
114 | * \returns true if the row and the column, having colNorm and rowNorm
|
---|
115 | * as norms, are balanced, false otherwise.
|
---|
116 | * colB and rowB are repectively the multipliers for
|
---|
117 | * the column and the row in order to balance them.
|
---|
118 | * */
|
---|
119 | bool balancedR( Scalar colNorm, Scalar rowNorm,
|
---|
120 | bool& isBalanced, Scalar& colB, Scalar& rowB );
|
---|
121 |
|
---|
122 | public:
|
---|
123 | /**
|
---|
124 | * Balancing algorithm from B. N. PARLETT and C. REINSCH (1969)
|
---|
125 | * "Balancing a matrix for calculation of eigenvalues and eigenvectors"
|
---|
126 | * adapted to the case of companion matrices.
|
---|
127 | * A matrix with non zero row and non zero column is balanced
|
---|
128 | * for a certain norm if the i-th row and the i-th column
|
---|
129 | * have same norm for all i.
|
---|
130 | */
|
---|
131 | void balance();
|
---|
132 |
|
---|
133 | protected:
|
---|
134 | RightColumn m_monic;
|
---|
135 | BottomLeftDiagonal m_bl_diag;
|
---|
136 | };
|
---|
137 |
|
---|
138 |
|
---|
139 |
|
---|
140 | template< typename _Scalar, int _Deg >
|
---|
141 | inline
|
---|
142 | bool companion<_Scalar,_Deg>::balanced( Scalar colNorm, Scalar rowNorm,
|
---|
143 | bool& isBalanced, Scalar& colB, Scalar& rowB )
|
---|
144 | {
|
---|
145 | if( Scalar(0) == colNorm || Scalar(0) == rowNorm ){ return true; }
|
---|
146 | else
|
---|
147 | {
|
---|
148 | //To find the balancing coefficients, if the radix is 2,
|
---|
149 | //one finds \f$ \sigma \f$ such that
|
---|
150 | // \f$ 2^{2\sigma-1} < rowNorm / colNorm \le 2^{2\sigma+1} \f$
|
---|
151 | // then the balancing coefficient for the row is \f$ 1/2^{\sigma} \f$
|
---|
152 | // and the balancing coefficient for the column is \f$ 2^{\sigma} \f$
|
---|
153 | rowB = rowNorm / radix<Scalar>();
|
---|
154 | colB = Scalar(1);
|
---|
155 | const Scalar s = colNorm + rowNorm;
|
---|
156 |
|
---|
157 | while (colNorm < rowB)
|
---|
158 | {
|
---|
159 | colB *= radix<Scalar>();
|
---|
160 | colNorm *= radix2<Scalar>();
|
---|
161 | }
|
---|
162 |
|
---|
163 | rowB = rowNorm * radix<Scalar>();
|
---|
164 |
|
---|
165 | while (colNorm >= rowB)
|
---|
166 | {
|
---|
167 | colB /= radix<Scalar>();
|
---|
168 | colNorm /= radix2<Scalar>();
|
---|
169 | }
|
---|
170 |
|
---|
171 | //This line is used to avoid insubstantial balancing
|
---|
172 | if ((rowNorm + colNorm) < Scalar(0.95) * s * colB)
|
---|
173 | {
|
---|
174 | isBalanced = false;
|
---|
175 | rowB = Scalar(1) / colB;
|
---|
176 | return false;
|
---|
177 | }
|
---|
178 | else{
|
---|
179 | return true; }
|
---|
180 | }
|
---|
181 | }
|
---|
182 |
|
---|
183 | template< typename _Scalar, int _Deg >
|
---|
184 | inline
|
---|
185 | bool companion<_Scalar,_Deg>::balancedR( Scalar colNorm, Scalar rowNorm,
|
---|
186 | bool& isBalanced, Scalar& colB, Scalar& rowB )
|
---|
187 | {
|
---|
188 | if( Scalar(0) == colNorm || Scalar(0) == rowNorm ){ return true; }
|
---|
189 | else
|
---|
190 | {
|
---|
191 | /**
|
---|
192 | * Set the norm of the column and the row to the geometric mean
|
---|
193 | * of the row and column norm
|
---|
194 | */
|
---|
195 | const _Scalar q = colNorm/rowNorm;
|
---|
196 | if( !isApprox( q, _Scalar(1) ) )
|
---|
197 | {
|
---|
198 | rowB = sqrt( colNorm/rowNorm );
|
---|
199 | colB = Scalar(1)/rowB;
|
---|
200 |
|
---|
201 | isBalanced = false;
|
---|
202 | return false;
|
---|
203 | }
|
---|
204 | else{
|
---|
205 | return true; }
|
---|
206 | }
|
---|
207 | }
|
---|
208 |
|
---|
209 |
|
---|
210 | template< typename _Scalar, int _Deg >
|
---|
211 | void companion<_Scalar,_Deg>::balance()
|
---|
212 | {
|
---|
213 | using std::abs;
|
---|
214 | EIGEN_STATIC_ASSERT( Deg == Dynamic || 1 < Deg, YOU_MADE_A_PROGRAMMING_MISTAKE );
|
---|
215 | const Index deg = m_monic.size();
|
---|
216 | const Index deg_1 = deg-1;
|
---|
217 |
|
---|
218 | bool hasConverged=false;
|
---|
219 | while( !hasConverged )
|
---|
220 | {
|
---|
221 | hasConverged = true;
|
---|
222 | Scalar colNorm,rowNorm;
|
---|
223 | Scalar colB,rowB;
|
---|
224 |
|
---|
225 | //First row, first column excluding the diagonal
|
---|
226 | //==============================================
|
---|
227 | colNorm = abs(m_bl_diag[0]);
|
---|
228 | rowNorm = abs(m_monic[0]);
|
---|
229 |
|
---|
230 | //Compute balancing of the row and the column
|
---|
231 | if( !balanced( colNorm, rowNorm, hasConverged, colB, rowB ) )
|
---|
232 | {
|
---|
233 | m_bl_diag[0] *= colB;
|
---|
234 | m_monic[0] *= rowB;
|
---|
235 | }
|
---|
236 |
|
---|
237 | //Middle rows and columns excluding the diagonal
|
---|
238 | //==============================================
|
---|
239 | for( Index i=1; i<deg_1; ++i )
|
---|
240 | {
|
---|
241 | // column norm, excluding the diagonal
|
---|
242 | colNorm = abs(m_bl_diag[i]);
|
---|
243 |
|
---|
244 | // row norm, excluding the diagonal
|
---|
245 | rowNorm = abs(m_bl_diag[i-1]) + abs(m_monic[i]);
|
---|
246 |
|
---|
247 | //Compute balancing of the row and the column
|
---|
248 | if( !balanced( colNorm, rowNorm, hasConverged, colB, rowB ) )
|
---|
249 | {
|
---|
250 | m_bl_diag[i] *= colB;
|
---|
251 | m_bl_diag[i-1] *= rowB;
|
---|
252 | m_monic[i] *= rowB;
|
---|
253 | }
|
---|
254 | }
|
---|
255 |
|
---|
256 | //Last row, last column excluding the diagonal
|
---|
257 | //============================================
|
---|
258 | const Index ebl = m_bl_diag.size()-1;
|
---|
259 | VectorBlock<RightColumn,Deg_1> headMonic( m_monic, 0, deg_1 );
|
---|
260 | colNorm = headMonic.array().abs().sum();
|
---|
261 | rowNorm = abs( m_bl_diag[ebl] );
|
---|
262 |
|
---|
263 | //Compute balancing of the row and the column
|
---|
264 | if( !balanced( colNorm, rowNorm, hasConverged, colB, rowB ) )
|
---|
265 | {
|
---|
266 | headMonic *= colB;
|
---|
267 | m_bl_diag[ebl] *= rowB;
|
---|
268 | }
|
---|
269 | }
|
---|
270 | }
|
---|
271 |
|
---|
272 | } // end namespace internal
|
---|
273 |
|
---|
274 | } // end namespace Eigen
|
---|
275 |
|
---|
276 | #endif // EIGEN_COMPANION_H
|
---|