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 <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 = -poly.head(deg)/poly[deg];
79  m_bl_diag.setOnes(deg-1);
80  }
81 
82  template<typename VectorType>
83  companion( const VectorType& poly ){
84  setPolynomial( poly ); }
85 
86  public:
87  DenseCompanionMatrixType denseMatrix() const
88  {
89  const Index deg = m_monic.size();
90  const Index deg_1 = deg-1;
91  DenseCompanionMatrixType companMat(deg,deg);
92  companMat <<
93  ( LeftBlock(deg,deg_1)
94  << LeftBlockFirstRow::Zero(1,deg_1),
95  BottomLeftBlock::Identity(deg-1,deg-1)*m_bl_diag.asDiagonal() ).finished()
96  , m_monic;
97  return companMat;
98  }
99 
100 
101 
102  protected:
109  bool balanced( RealScalar colNorm, RealScalar rowNorm,
110  bool& isBalanced, RealScalar& colB, RealScalar& rowB );
111 
118  bool balancedR( RealScalar colNorm, RealScalar rowNorm,
119  bool& isBalanced, RealScalar& colB, RealScalar& rowB );
120 
121  public:
130  void balance();
131 
132  protected:
133  RightColumn m_monic;
134  BottomLeftDiagonal m_bl_diag;
135 };
136 
137 
138 
139 template< typename _Scalar, int _Deg >
140 inline
141 bool companion<_Scalar,_Deg>::balanced( RealScalar colNorm, RealScalar rowNorm,
142  bool& isBalanced, RealScalar& colB, RealScalar& rowB )
143 {
144  if( RealScalar(0) == colNorm || RealScalar(0) == rowNorm ){ return true; }
145  else
146  {
147  //To find the balancing coefficients, if the radix is 2,
148  //one finds \f$ \sigma \f$ such that
149  // \f$ 2^{2\sigma-1} < rowNorm / colNorm \le 2^{2\sigma+1} \f$
150  // then the balancing coefficient for the row is \f$ 1/2^{\sigma} \f$
151  // and the balancing coefficient for the column is \f$ 2^{\sigma} \f$
152  rowB = rowNorm / radix<RealScalar>();
153  colB = RealScalar(1);
154  const RealScalar s = colNorm + rowNorm;
155 
156  while (colNorm < rowB)
157  {
158  colB *= radix<RealScalar>();
159  colNorm *= radix2<RealScalar>();
160  }
161 
162  rowB = rowNorm * radix<RealScalar>();
163 
164  while (colNorm >= rowB)
165  {
166  colB /= radix<RealScalar>();
167  colNorm /= radix2<RealScalar>();
168  }
169 
170  //This line is used to avoid insubstantial balancing
171  if ((rowNorm + colNorm) < RealScalar(0.95) * s * colB)
172  {
173  isBalanced = false;
174  rowB = RealScalar(1) / colB;
175  return false;
176  }
177  else{
178  return true; }
179  }
180 }
181 
182 template< typename _Scalar, int _Deg >
183 inline
184 bool companion<_Scalar,_Deg>::balancedR( RealScalar colNorm, RealScalar rowNorm,
185  bool& isBalanced, RealScalar& colB, RealScalar& rowB )
186 {
187  if( RealScalar(0) == colNorm || RealScalar(0) == rowNorm ){ return true; }
188  else
189  {
194  const RealScalar q = colNorm/rowNorm;
195  if( !isApprox( q, _Scalar(1) ) )
196  {
197  rowB = sqrt( colNorm/rowNorm );
198  colB = RealScalar(1)/rowB;
199 
200  isBalanced = false;
201  return false;
202  }
203  else{
204  return true; }
205  }
206 }
207 
208 
209 template< typename _Scalar, int _Deg >
210 void companion<_Scalar,_Deg>::balance()
211 {
212  using std::abs;
213  EIGEN_STATIC_ASSERT( Deg == Dynamic || 1 < Deg, YOU_MADE_A_PROGRAMMING_MISTAKE );
214  const Index deg = m_monic.size();
215  const Index deg_1 = deg-1;
216 
217  bool hasConverged=false;
218  while( !hasConverged )
219  {
220  hasConverged = true;
221  RealScalar colNorm,rowNorm;
222  RealScalar colB,rowB;
223 
224  //First row, first column excluding the diagonal
225  //==============================================
226  colNorm = abs(m_bl_diag[0]);
227  rowNorm = abs(m_monic[0]);
228 
229  //Compute balancing of the row and the column
230  if( !balanced( colNorm, rowNorm, hasConverged, colB, rowB ) )
231  {
232  m_bl_diag[0] *= colB;
233  m_monic[0] *= rowB;
234  }
235 
236  //Middle rows and columns excluding the diagonal
237  //==============================================
238  for( Index i=1; i<deg_1; ++i )
239  {
240  // column norm, excluding the diagonal
241  colNorm = abs(m_bl_diag[i]);
242 
243  // row norm, excluding the diagonal
244  rowNorm = abs(m_bl_diag[i-1]) + abs(m_monic[i]);
245 
246  //Compute balancing of the row and the column
247  if( !balanced( colNorm, rowNorm, hasConverged, colB, rowB ) )
248  {
249  m_bl_diag[i] *= colB;
250  m_bl_diag[i-1] *= rowB;
251  m_monic[i] *= rowB;
252  }
253  }
254 
255  //Last row, last column excluding the diagonal
256  //============================================
257  const Index ebl = m_bl_diag.size()-1;
258  VectorBlock<RightColumn,Deg_1> headMonic( m_monic, 0, deg_1 );
259  colNorm = headMonic.array().abs().sum();
260  rowNorm = abs( m_bl_diag[ebl] );
261 
262  //Compute balancing of the row and the column
263  if( !balanced( colNorm, rowNorm, hasConverged, colB, rowB ) )
264  {
265  headMonic *= colB;
266  m_bl_diag[ebl] *= rowB;
267  }
268  }
269 }
270 
271 } // end namespace internal
272 
273 } // end namespace Eigen
274 
275 #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.
static const ConstantReturnType Zero()
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)
static const IdentityReturnType Identity()
const int Dynamic