10 #ifndef EIGEN_COMPANION_H
11 #define EIGEN_COMPANION_H
21 #ifndef EIGEN_PARSED_BY_DOXYGEN
24 T radix(){
return 2; }
27 T radix2(){
return radix<T>()*radix<T>(); }
30 struct decrement_if_fixed_size
38 template<
typename _Scalar,
int _Deg >
42 EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(_Scalar,_Deg==
Dynamic ?
Dynamic : _Deg)
46 Deg_1=decrement_if_fixed_size<Deg>::ret
49 typedef _Scalar Scalar;
50 typedef typename NumTraits<Scalar>::Real RealScalar;
51 typedef Matrix<Scalar, Deg, 1> RightColumn;
53 typedef Matrix<Scalar, Deg_1, 1> BottomLeftDiagonal;
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;
60 typedef DenseIndex
Index;
63 EIGEN_STRONG_INLINE
const _Scalar operator()(
Index row,
Index col )
const
65 if( m_bl_diag.rows() > col )
67 if( 0 < row ){
return m_bl_diag[col]; }
70 else{
return m_monic[row]; }
74 template<
typename VectorType>
75 void setPolynomial(
const VectorType& poly )
77 const Index deg = poly.size()-1;
78 m_monic = -poly.head(deg)/poly[deg];
79 m_bl_diag.setOnes(deg-1);
82 template<
typename VectorType>
83 companion(
const VectorType& poly ){
84 setPolynomial( poly ); }
87 DenseCompanionMatrixType denseMatrix()
const
89 const Index deg = m_monic.size();
90 const Index deg_1 = deg-1;
91 DenseCompanionMatrixType companMat(deg,deg);
93 ( LeftBlock(deg,deg_1)
109 bool balanced( RealScalar colNorm, RealScalar rowNorm,
110 bool& isBalanced, RealScalar& colB, RealScalar& rowB );
118 bool balancedR( RealScalar colNorm, RealScalar rowNorm,
119 bool& isBalanced, RealScalar& colB, RealScalar& rowB );
134 BottomLeftDiagonal m_bl_diag;
139 template<
typename _Scalar,
int _Deg >
141 bool companion<_Scalar,_Deg>::balanced( RealScalar colNorm, RealScalar rowNorm,
142 bool& isBalanced, RealScalar& colB, RealScalar& rowB )
144 if( RealScalar(0) == colNorm || RealScalar(0) == rowNorm ){
return true; }
152 rowB = rowNorm / radix<RealScalar>();
153 colB = RealScalar(1);
154 const RealScalar s = colNorm + rowNorm;
156 while (colNorm < rowB)
158 colB *= radix<RealScalar>();
159 colNorm *= radix2<RealScalar>();
162 rowB = rowNorm * radix<RealScalar>();
164 while (colNorm >= rowB)
166 colB /= radix<RealScalar>();
167 colNorm /= radix2<RealScalar>();
171 if ((rowNorm + colNorm) < RealScalar(0.95) * s * colB)
174 rowB = RealScalar(1) / colB;
182 template<
typename _Scalar,
int _Deg >
184 bool companion<_Scalar,_Deg>::balancedR( RealScalar colNorm, RealScalar rowNorm,
185 bool& isBalanced, RealScalar& colB, RealScalar& rowB )
187 if( RealScalar(0) == colNorm || RealScalar(0) == rowNorm ){
return true; }
194 const RealScalar q = colNorm/rowNorm;
195 if( !isApprox( q, _Scalar(1) ) )
197 rowB =
sqrt( colNorm/rowNorm );
198 colB = RealScalar(1)/rowB;
209 template<
typename _Scalar,
int _Deg >
210 void companion<_Scalar,_Deg>::balance()
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;
217 bool hasConverged=
false;
218 while( !hasConverged )
221 RealScalar colNorm,rowNorm;
222 RealScalar colB,rowB;
226 colNorm =
abs(m_bl_diag[0]);
227 rowNorm =
abs(m_monic[0]);
230 if( !balanced( colNorm, rowNorm, hasConverged, colB, rowB ) )
232 m_bl_diag[0] *= colB;
238 for(
Index i=1; i<deg_1; ++i )
241 colNorm =
abs(m_bl_diag[i]);
244 rowNorm =
abs(m_bl_diag[i-1]) +
abs(m_monic[i]);
247 if( !balanced( colNorm, rowNorm, hasConverged, colB, rowB ) )
249 m_bl_diag[i] *= colB;
250 m_bl_diag[i-1] *= rowB;
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] );
263 if( !balanced( colNorm, rowNorm, hasConverged, colB, rowB ) )
266 m_bl_diag[ebl] *= rowB;
static const ConstantReturnType Zero()
static const IdentityReturnType Identity()
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 Eigen::CwiseUnaryOp< Eigen::internal::scalar_sqrt_op< typename Derived::Scalar >, const Derived > sqrt(const Eigen::ArrayBase< Derived > &x)