Please, help us to better know about our user community by answering the following short survey: https://forms.gle/wpyrxWi18ox9Z5ae9
Companion.h
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<int Size>
24 struct decrement_if_fixed_size
25 {
26  enum {
27  ret = (Size == Dynamic) ? Dynamic : Size-1 };
28 };
29 
30 #endif
31 
32 template< typename _Scalar, int _Deg >
33 class companion
34 {
35  public:
36  EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(_Scalar,_Deg==Dynamic ? Dynamic : _Deg)
37 
38  enum {
39  Deg = _Deg,
40  Deg_1=decrement_if_fixed_size<Deg>::ret
41  };
42 
43  typedef _Scalar Scalar;
44  typedef typename NumTraits<Scalar>::Real RealScalar;
45  typedef Matrix<Scalar, Deg, 1> RightColumn;
46  //typedef DiagonalMatrix< Scalar, Deg_1, Deg_1 > BottomLeftDiagonal;
47  typedef Matrix<Scalar, Deg_1, 1> BottomLeftDiagonal;
48 
49  typedef Matrix<Scalar, Deg, Deg> DenseCompanionMatrixType;
50  typedef Matrix< Scalar, _Deg, Deg_1 > LeftBlock;
51  typedef Matrix< Scalar, Deg_1, Deg_1 > BottomLeftBlock;
52  typedef Matrix< Scalar, 1, Deg_1 > LeftBlockFirstRow;
53 
54  typedef DenseIndex Index;
55 
56  public:
57  EIGEN_STRONG_INLINE const _Scalar operator()(Index row, Index col ) const
58  {
59  if( m_bl_diag.rows() > col )
60  {
61  if( 0 < row ){ return m_bl_diag[col]; }
62  else{ return 0; }
63  }
64  else{ return m_monic[row]; }
65  }
66 
67  public:
68  template<typename VectorType>
69  void setPolynomial( const VectorType& poly )
70  {
71  const Index deg = poly.size()-1;
72  m_monic = -poly.head(deg)/poly[deg];
73  m_bl_diag.setOnes(deg-1);
74  }
75 
76  template<typename VectorType>
77  companion( const VectorType& poly ){
78  setPolynomial( poly ); }
79 
80  public:
81  DenseCompanionMatrixType denseMatrix() const
82  {
83  const Index deg = m_monic.size();
84  const Index deg_1 = deg-1;
85  DenseCompanionMatrixType companMat(deg,deg);
86  companMat <<
87  ( LeftBlock(deg,deg_1)
88  << LeftBlockFirstRow::Zero(1,deg_1),
89  BottomLeftBlock::Identity(deg-1,deg-1)*m_bl_diag.asDiagonal() ).finished()
90  , m_monic;
91  return companMat;
92  }
93 
94 
95 
96  protected:
103  bool balanced( RealScalar colNorm, RealScalar rowNorm,
104  bool& isBalanced, RealScalar& colB, RealScalar& rowB );
105 
112  bool balancedR( RealScalar colNorm, RealScalar rowNorm,
113  bool& isBalanced, RealScalar& colB, RealScalar& rowB );
114 
115  public:
124  void balance();
125 
126  protected:
127  RightColumn m_monic;
128  BottomLeftDiagonal m_bl_diag;
129 };
130 
131 
132 
133 template< typename _Scalar, int _Deg >
134 inline
135 bool companion<_Scalar,_Deg>::balanced( RealScalar colNorm, RealScalar rowNorm,
136  bool& isBalanced, RealScalar& colB, RealScalar& rowB )
137 {
138  if( RealScalar(0) == colNorm || RealScalar(0) == rowNorm
139  || !(numext::isfinite)(colNorm) || !(numext::isfinite)(rowNorm)){
140  return true;
141  }
142  else
143  {
144  //To find the balancing coefficients, if the radix is 2,
145  //one finds \f$ \sigma \f$ such that
146  // \f$ 2^{2\sigma-1} < rowNorm / colNorm \le 2^{2\sigma+1} \f$
147  // then the balancing coefficient for the row is \f$ 1/2^{\sigma} \f$
148  // and the balancing coefficient for the column is \f$ 2^{\sigma} \f$
149  const RealScalar radix = RealScalar(2);
150  const RealScalar radix2 = RealScalar(4);
151 
152  rowB = rowNorm / radix;
153  colB = RealScalar(1);
154  const RealScalar s = colNorm + rowNorm;
155 
156  // Find sigma s.t. rowNorm / 2 <= 2^(2*sigma) * colNorm
157  RealScalar scout = colNorm;
158  while (scout < rowB)
159  {
160  colB *= radix;
161  scout *= radix2;
162  }
163 
164  // We now have an upper-bound for sigma, try to lower it.
165  // Find sigma s.t. 2^(2*sigma) * colNorm / 2 < rowNorm
166  scout = colNorm * (colB / radix) * colB; // Avoid overflow.
167  while (scout >= rowNorm)
168  {
169  colB /= radix;
170  scout /= radix2;
171  }
172 
173  // This line is used to avoid insubstantial balancing.
174  if ((rowNorm + radix * scout) < RealScalar(0.95) * s * colB)
175  {
176  isBalanced = false;
177  rowB = RealScalar(1) / colB;
178  return false;
179  }
180  else
181  {
182  return true;
183  }
184  }
185 }
186 
187 template< typename _Scalar, int _Deg >
188 inline
189 bool companion<_Scalar,_Deg>::balancedR( RealScalar colNorm, RealScalar rowNorm,
190  bool& isBalanced, RealScalar& colB, RealScalar& rowB )
191 {
192  if( RealScalar(0) == colNorm || RealScalar(0) == rowNorm ){ return true; }
193  else
194  {
199  const RealScalar q = colNorm/rowNorm;
200  if( !isApprox( q, _Scalar(1) ) )
201  {
202  rowB = sqrt( colNorm/rowNorm );
203  colB = RealScalar(1)/rowB;
204 
205  isBalanced = false;
206  return false;
207  }
208  else{
209  return true; }
210  }
211 }
212 
213 
214 template< typename _Scalar, int _Deg >
215 void companion<_Scalar,_Deg>::balance()
216 {
217  using std::abs;
218  EIGEN_STATIC_ASSERT( Deg == Dynamic || 1 < Deg, YOU_MADE_A_PROGRAMMING_MISTAKE );
219  const Index deg = m_monic.size();
220  const Index deg_1 = deg-1;
221 
222  bool hasConverged=false;
223  while( !hasConverged )
224  {
225  hasConverged = true;
226  RealScalar colNorm,rowNorm;
227  RealScalar colB,rowB;
228 
229  //First row, first column excluding the diagonal
230  //==============================================
231  colNorm = abs(m_bl_diag[0]);
232  rowNorm = abs(m_monic[0]);
233 
234  //Compute balancing of the row and the column
235  if( !balanced( colNorm, rowNorm, hasConverged, colB, rowB ) )
236  {
237  m_bl_diag[0] *= colB;
238  m_monic[0] *= rowB;
239  }
240 
241  //Middle rows and columns excluding the diagonal
242  //==============================================
243  for( Index i=1; i<deg_1; ++i )
244  {
245  // column norm, excluding the diagonal
246  colNorm = abs(m_bl_diag[i]);
247 
248  // row norm, excluding the diagonal
249  rowNorm = abs(m_bl_diag[i-1]) + abs(m_monic[i]);
250 
251  //Compute balancing of the row and the column
252  if( !balanced( colNorm, rowNorm, hasConverged, colB, rowB ) )
253  {
254  m_bl_diag[i] *= colB;
255  m_bl_diag[i-1] *= rowB;
256  m_monic[i] *= rowB;
257  }
258  }
259 
260  //Last row, last column excluding the diagonal
261  //============================================
262  const Index ebl = m_bl_diag.size()-1;
263  VectorBlock<RightColumn,Deg_1> headMonic( m_monic, 0, deg_1 );
264  colNorm = headMonic.array().abs().sum();
265  rowNorm = abs( m_bl_diag[ebl] );
266 
267  //Compute balancing of the row and the column
268  if( !balanced( colNorm, rowNorm, hasConverged, colB, rowB ) )
269  {
270  headMonic *= colB;
271  m_bl_diag[ebl] *= rowB;
272  }
273  }
274 }
275 
276 } // end namespace internal
277 
278 } // end namespace Eigen
279 
280 #endif // EIGEN_COMPANION_H
const Eigen::CwiseUnaryOp< Eigen::internal::scalar_sqrt_op< typename Derived::Scalar >, const Derived > sqrt(const Eigen::ArrayBase< Derived > &x)
Namespace containing all symbols from the Eigen library.
EIGEN_DEFAULT_DENSE_INDEX_TYPE Index
const Eigen::CwiseUnaryOp< Eigen::internal::scalar_abs_op< typename Derived::Scalar >, const Derived > abs(const Eigen::ArrayBase< Derived > &x)
const int Dynamic