10#ifndef EIGEN_COMPANION_H
11#define EIGEN_COMPANION_H
21#ifndef EIGEN_PARSED_BY_DOXYGEN
24struct decrement_if_fixed_size
27 ret = (Size ==
Dynamic) ? Dynamic : Size-1 };
32template<
typename _Scalar,
int _Deg >
36 EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(_Scalar,_Deg==
Dynamic ?
Dynamic : _Deg)
40 Deg_1=decrement_if_fixed_size<Deg>::ret
43 typedef _Scalar Scalar;
44 typedef typename NumTraits<Scalar>::Real RealScalar;
45 typedef Matrix<Scalar, Deg, 1> RightColumn;
47 typedef Matrix<Scalar, Deg_1, 1> BottomLeftDiagonal;
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;
54 typedef DenseIndex
Index;
57 EIGEN_STRONG_INLINE
const _Scalar operator()(Index row, Index col )
const
59 if( m_bl_diag.rows() > col )
61 if( 0 < row ){
return m_bl_diag[col]; }
64 else{
return m_monic[row]; }
68 template<
typename VectorType>
69 void setPolynomial(
const VectorType& poly )
71 const Index deg = poly.size()-1;
72 m_monic = -poly.head(deg)/poly[deg];
73 m_bl_diag.setOnes(deg-1);
76 template<
typename VectorType>
77 companion(
const VectorType& poly ){
78 setPolynomial( poly ); }
81 DenseCompanionMatrixType denseMatrix()
const
83 const Index deg = m_monic.size();
84 const Index deg_1 = deg-1;
85 DenseCompanionMatrixType companMat(deg,deg);
87 ( LeftBlock(deg,deg_1)
88 << LeftBlockFirstRow::Zero(1,deg_1),
89 BottomLeftBlock::Identity(deg-1,deg-1)*m_bl_diag.asDiagonal() ).finished()
103 bool balanced( RealScalar colNorm, RealScalar rowNorm,
104 bool& isBalanced, RealScalar& colB, RealScalar& rowB );
112 bool balancedR( RealScalar colNorm, RealScalar rowNorm,
113 bool& isBalanced, RealScalar& colB, RealScalar& rowB );
128 BottomLeftDiagonal m_bl_diag;
133template<
typename _Scalar,
int _Deg >
135bool companion<_Scalar,_Deg>::balanced( RealScalar colNorm, RealScalar rowNorm,
136 bool& isBalanced, RealScalar& colB, RealScalar& rowB )
138 if( RealScalar(0) == colNorm || RealScalar(0) == rowNorm
139 || !(numext::isfinite)(colNorm) || !(numext::isfinite)(rowNorm)){
149 const RealScalar radix = RealScalar(2);
150 const RealScalar radix2 = RealScalar(4);
152 rowB = rowNorm / radix;
153 colB = RealScalar(1);
154 const RealScalar s = colNorm + rowNorm;
157 RealScalar scout = colNorm;
166 scout = colNorm * (colB / radix) * colB;
167 while (scout >= rowNorm)
174 if ((rowNorm + radix * scout) < RealScalar(0.95) * s * colB)
177 rowB = RealScalar(1) / colB;
187template<
typename _Scalar,
int _Deg >
189bool companion<_Scalar,_Deg>::balancedR( RealScalar colNorm, RealScalar rowNorm,
190 bool& isBalanced, RealScalar& colB, RealScalar& rowB )
192 if( RealScalar(0) == colNorm || RealScalar(0) == rowNorm ){
return true; }
199 const RealScalar q = colNorm/rowNorm;
200 if( !isApprox( q, _Scalar(1) ) )
202 rowB =
sqrt( colNorm/rowNorm );
203 colB = RealScalar(1)/rowB;
214template<
typename _Scalar,
int _Deg >
215void companion<_Scalar,_Deg>::balance()
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;
222 bool hasConverged=
false;
223 while( !hasConverged )
226 RealScalar colNorm,rowNorm;
227 RealScalar colB,rowB;
231 colNorm =
abs(m_bl_diag[0]);
232 rowNorm =
abs(m_monic[0]);
235 if( !balanced( colNorm, rowNorm, hasConverged, colB, rowB ) )
237 m_bl_diag[0] *= colB;
243 for(
Index i=1; i<deg_1; ++i )
246 colNorm =
abs(m_bl_diag[i]);
249 rowNorm =
abs(m_bl_diag[i-1]) +
abs(m_monic[i]);
252 if( !balanced( colNorm, rowNorm, hasConverged, colB, rowB ) )
254 m_bl_diag[i] *= colB;
255 m_bl_diag[i-1] *= rowB;
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] );
268 if( !balanced( colNorm, rowNorm, hasConverged, colB, rowB ) )
271 m_bl_diag[ebl] *= rowB;
Namespace containing all symbols from the Eigen library.
const Eigen::CwiseUnaryOp< Eigen::internal::scalar_sqrt_op< typename Derived::Scalar >, const Derived > sqrt(const Eigen::ArrayBase< Derived > &x)
const Eigen::CwiseUnaryOp< Eigen::internal::scalar_abs_op< typename Derived::Scalar >, const Derived > abs(const Eigen::ArrayBase< Derived > &x)
EIGEN_DEFAULT_DENSE_INDEX_TYPE Index