25 #ifndef EIGEN_COMPANION_H
26 #define EIGEN_COMPANION_H
36 #ifndef EIGEN_PARSED_BY_DOXYGEN
39 T radix(){
return 2; }
42 T radix2(){
return radix<T>()*radix<T>(); }
45 struct decrement_if_fixed_size
48 ret = (Size ==
Dynamic) ? Dynamic : Size-1 };
53 template<
typename _Scalar,
int _Deg >
57 EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(_Scalar,_Deg==Dynamic ? Dynamic : _Deg)
61 Deg_1=decrement_if_fixed_size<Deg>::ret
64 typedef _Scalar Scalar;
65 typedef typename NumTraits<Scalar>::Real RealScalar;
66 typedef Matrix<Scalar, Deg, 1> RightColumn;
68 typedef Matrix<Scalar, Deg_1, 1> BottomLeftDiagonal;
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;
75 typedef DenseIndex Index;
78 EIGEN_STRONG_INLINE
const _Scalar operator()(Index row, Index col )
const
80 if( m_bl_diag.rows() > col )
82 if( 0 < row ){
return m_bl_diag[col]; }
85 else{
return m_monic[row]; }
89 template<
typename VectorType>
90 void setPolynomial(
const VectorType& poly )
92 const Index deg = poly.size()-1;
93 m_monic = -1/poly[deg] * poly.head(deg);
95 m_bl_diag.setOnes(deg-1);
98 template<
typename VectorType>
99 companion(
const VectorType& poly ){
100 setPolynomial( poly ); }
103 DenseCompanionMatrixType denseMatrix()
const
105 const Index deg = m_monic.size();
106 const Index deg_1 = deg-1;
107 DenseCompanionMatrixType companion(deg,deg);
109 ( LeftBlock(deg,deg_1)
110 << LeftBlockFirstRow::Zero(1,deg_1),
111 BottomLeftBlock::Identity(deg-1,deg-1)*m_bl_diag.asDiagonal() ).finished()
125 bool balanced( Scalar colNorm, Scalar rowNorm,
126 bool& isBalanced, Scalar& colB, Scalar& rowB );
134 bool balancedR( Scalar colNorm, Scalar rowNorm,
135 bool& isBalanced, Scalar& colB, Scalar& rowB );
150 BottomLeftDiagonal m_bl_diag;
155 template<
typename _Scalar,
int _Deg >
157 bool companion<_Scalar,_Deg>::balanced( Scalar colNorm, Scalar rowNorm,
158 bool& isBalanced, Scalar& colB, Scalar& rowB )
160 if( Scalar(0) == colNorm || Scalar(0) == rowNorm ){
return true; }
168 rowB = rowNorm / radix<Scalar>();
170 const Scalar s = colNorm + rowNorm;
172 while (colNorm < rowB)
174 colB *= radix<Scalar>();
175 colNorm *= radix2<Scalar>();
178 rowB = rowNorm * radix<Scalar>();
180 while (colNorm >= rowB)
182 colB /= radix<Scalar>();
183 colNorm /= radix2<Scalar>();
187 if ((rowNorm + colNorm) < Scalar(0.95) * s * colB)
190 rowB = Scalar(1) / colB;
198 template<
typename _Scalar,
int _Deg >
200 bool companion<_Scalar,_Deg>::balancedR( Scalar colNorm, Scalar rowNorm,
201 bool& isBalanced, Scalar& colB, Scalar& rowB )
203 if( Scalar(0) == colNorm || Scalar(0) == rowNorm ){
return true; }
210 const _Scalar q = colNorm/rowNorm;
211 if( !isApprox( q, _Scalar(1) ) )
213 rowB =
sqrt( colNorm/rowNorm );
214 colB = Scalar(1)/rowB;
225 template<
typename _Scalar,
int _Deg >
226 void companion<_Scalar,_Deg>::balance()
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;
232 bool hasConverged=
false;
233 while( !hasConverged )
236 Scalar colNorm,rowNorm;
241 colNorm =
abs(m_bl_diag[0]);
242 rowNorm =
abs(m_monic[0]);
245 if( !balanced( colNorm, rowNorm, hasConverged, colB, rowB ) )
247 m_bl_diag[0] *= colB;
253 for( Index i=1; i<deg_1; ++i )
256 colNorm =
abs(m_bl_diag[i]);
259 rowNorm =
abs(m_bl_diag[i-1]) +
abs(m_monic[i]);
262 if( !balanced( colNorm, rowNorm, hasConverged, colB, rowB ) )
264 m_bl_diag[i] *= colB;
265 m_bl_diag[i-1] *= rowB;
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] );
278 if( !balanced( colNorm, rowNorm, hasConverged, colB, rowB ) )
281 m_bl_diag[ebl] *= rowB;
290 #endif // EIGEN_COMPANION_H