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 // Eigen is free software; you can redistribute it and/or
7 // modify it under the terms of the GNU Lesser General Public
8 // License as published by the Free Software Foundation; either
9 // version 3 of the License, or (at your option) any later version.
10 //
11 // Alternatively, you can redistribute it and/or
12 // modify it under the terms of the GNU General Public License as
13 // published by the Free Software Foundation; either version 2 of
14 // the License, or (at your option) any later version.
15 //
16 // Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
17 // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
18 // FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
19 // GNU General Public License for more details.
20 //
21 // You should have received a copy of the GNU Lesser General Public
22 // License and a copy of the GNU General Public License along with
23 // Eigen. If not, see <http://www.gnu.org/licenses/>.
24 
25 #ifndef EIGEN_COMPANION_H
26 #define EIGEN_COMPANION_H
27 
28 // This file requires the user to include
29 // * Eigen/Core
30 // * Eigen/src/PolynomialSolver.h
31 
32 namespace Eigen {
33 
34 namespace internal {
35 
36 #ifndef EIGEN_PARSED_BY_DOXYGEN
37 
38 template <typename T>
39 T radix(){ return 2; }
40 
41 template <typename T>
42 T radix2(){ return radix<T>()*radix<T>(); }
43 
44 template<int Size>
45 struct decrement_if_fixed_size
46 {
47  enum {
48  ret = (Size == Dynamic) ? Dynamic : Size-1 };
49 };
50 
51 #endif
52 
53 template< typename _Scalar, int _Deg >
54 class companion
55 {
56  public:
57  EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(_Scalar,_Deg==Dynamic ? Dynamic : _Deg)
58 
59  enum {
60  Deg = _Deg,
61  Deg_1=decrement_if_fixed_size<Deg>::ret
62  };
63 
64  typedef _Scalar Scalar;
65  typedef typename NumTraits<Scalar>::Real RealScalar;
66  typedef Matrix<Scalar, Deg, 1> RightColumn;
67  //typedef DiagonalMatrix< Scalar, Deg_1, Deg_1 > BottomLeftDiagonal;
68  typedef Matrix<Scalar, Deg_1, 1> BottomLeftDiagonal;
69 
70  typedef Matrix<Scalar, Deg, Deg> DenseCompanionMatrixType;
71  typedef Matrix< Scalar, _Deg, Deg_1 > LeftBlock;
72  typedef Matrix< Scalar, Deg_1, Deg_1 > BottomLeftBlock;
73  typedef Matrix< Scalar, 1, Deg_1 > LeftBlockFirstRow;
74 
75  typedef DenseIndex Index;
76 
77  public:
78  EIGEN_STRONG_INLINE const _Scalar operator()(Index row, Index col ) const
79  {
80  if( m_bl_diag.rows() > col )
81  {
82  if( 0 < row ){ return m_bl_diag[col]; }
83  else{ return 0; }
84  }
85  else{ return m_monic[row]; }
86  }
87 
88  public:
89  template<typename VectorType>
90  void setPolynomial( const VectorType& poly )
91  {
92  const Index deg = poly.size()-1;
93  m_monic = -1/poly[deg] * poly.head(deg);
94  //m_bl_diag.setIdentity( deg-1 );
95  m_bl_diag.setOnes(deg-1);
96  }
97 
98  template<typename VectorType>
99  companion( const VectorType& poly ){
100  setPolynomial( poly ); }
101 
102  public:
103  DenseCompanionMatrixType denseMatrix() const
104  {
105  const Index deg = m_monic.size();
106  const Index deg_1 = deg-1;
107  DenseCompanionMatrixType companion(deg,deg);
108  companion <<
109  ( LeftBlock(deg,deg_1)
110  << LeftBlockFirstRow::Zero(1,deg_1),
111  BottomLeftBlock::Identity(deg-1,deg-1)*m_bl_diag.asDiagonal() ).finished()
112  , m_monic;
113  return companion;
114  }
115 
116 
117 
118  protected:
125  bool balanced( Scalar colNorm, Scalar rowNorm,
126  bool& isBalanced, Scalar& colB, Scalar& rowB );
127 
134  bool balancedR( Scalar colNorm, Scalar rowNorm,
135  bool& isBalanced, Scalar& colB, Scalar& rowB );
136 
137  public:
146  void balance();
147 
148  protected:
149  RightColumn m_monic;
150  BottomLeftDiagonal m_bl_diag;
151 };
152 
153 
154 
155 template< typename _Scalar, int _Deg >
156 inline
157 bool companion<_Scalar,_Deg>::balanced( Scalar colNorm, Scalar rowNorm,
158  bool& isBalanced, Scalar& colB, Scalar& rowB )
159 {
160  if( Scalar(0) == colNorm || Scalar(0) == rowNorm ){ return true; }
161  else
162  {
163  //To find the balancing coefficients, if the radix is 2,
164  //one finds \f$ \sigma \f$ such that
165  // \f$ 2^{2\sigma-1} < rowNorm / colNorm \le 2^{2\sigma+1} \f$
166  // then the balancing coefficient for the row is \f$ 1/2^{\sigma} \f$
167  // and the balancing coefficient for the column is \f$ 2^{\sigma} \f$
168  rowB = rowNorm / radix<Scalar>();
169  colB = Scalar(1);
170  const Scalar s = colNorm + rowNorm;
171 
172  while (colNorm < rowB)
173  {
174  colB *= radix<Scalar>();
175  colNorm *= radix2<Scalar>();
176  }
177 
178  rowB = rowNorm * radix<Scalar>();
179 
180  while (colNorm >= rowB)
181  {
182  colB /= radix<Scalar>();
183  colNorm /= radix2<Scalar>();
184  }
185 
186  //This line is used to avoid insubstantial balancing
187  if ((rowNorm + colNorm) < Scalar(0.95) * s * colB)
188  {
189  isBalanced = false;
190  rowB = Scalar(1) / colB;
191  return false;
192  }
193  else{
194  return true; }
195  }
196 }
197 
198 template< typename _Scalar, int _Deg >
199 inline
200 bool companion<_Scalar,_Deg>::balancedR( Scalar colNorm, Scalar rowNorm,
201  bool& isBalanced, Scalar& colB, Scalar& rowB )
202 {
203  if( Scalar(0) == colNorm || Scalar(0) == rowNorm ){ return true; }
204  else
205  {
210  const _Scalar q = colNorm/rowNorm;
211  if( !isApprox( q, _Scalar(1) ) )
212  {
213  rowB = sqrt( colNorm/rowNorm );
214  colB = Scalar(1)/rowB;
215 
216  isBalanced = false;
217  return false;
218  }
219  else{
220  return true; }
221  }
222 }
223 
224 
225 template< typename _Scalar, int _Deg >
226 void companion<_Scalar,_Deg>::balance()
227 {
228  EIGEN_STATIC_ASSERT( Deg == Dynamic || 1 < Deg, YOU_MADE_A_PROGRAMMING_MISTAKE );
229  const Index deg = m_monic.size();
230  const Index deg_1 = deg-1;
231 
232  bool hasConverged=false;
233  while( !hasConverged )
234  {
235  hasConverged = true;
236  Scalar colNorm,rowNorm;
237  Scalar colB,rowB;
238 
239  //First row, first column excluding the diagonal
240  //==============================================
241  colNorm = abs(m_bl_diag[0]);
242  rowNorm = abs(m_monic[0]);
243 
244  //Compute balancing of the row and the column
245  if( !balanced( colNorm, rowNorm, hasConverged, colB, rowB ) )
246  {
247  m_bl_diag[0] *= colB;
248  m_monic[0] *= rowB;
249  }
250 
251  //Middle rows and columns excluding the diagonal
252  //==============================================
253  for( Index i=1; i<deg_1; ++i )
254  {
255  // column norm, excluding the diagonal
256  colNorm = abs(m_bl_diag[i]);
257 
258  // row norm, excluding the diagonal
259  rowNorm = abs(m_bl_diag[i-1]) + abs(m_monic[i]);
260 
261  //Compute balancing of the row and the column
262  if( !balanced( colNorm, rowNorm, hasConverged, colB, rowB ) )
263  {
264  m_bl_diag[i] *= colB;
265  m_bl_diag[i-1] *= rowB;
266  m_monic[i] *= rowB;
267  }
268  }
269 
270  //Last row, last column excluding the diagonal
271  //============================================
272  const Index ebl = m_bl_diag.size()-1;
273  VectorBlock<RightColumn,Deg_1> headMonic( m_monic, 0, deg_1 );
274  colNorm = headMonic.array().abs().sum();
275  rowNorm = abs( m_bl_diag[ebl] );
276 
277  //Compute balancing of the row and the column
278  if( !balanced( colNorm, rowNorm, hasConverged, colB, rowB ) )
279  {
280  headMonic *= colB;
281  m_bl_diag[ebl] *= rowB;
282  }
283  }
284 }
285 
286 } // end namespace internal
287 
288 } // end namespace Eigen
289 
290 #endif // EIGEN_COMPANION_H