00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025 #ifndef EIGEN_COMPANION_H
00026 #define EIGEN_COMPANION_H
00027
00028
00029
00030
00031
00032 namespace Eigen {
00033
00034 namespace internal {
00035
00036 #ifndef EIGEN_PARSED_BY_DOXYGEN
00037
00038 template <typename T>
00039 T radix(){ return 2; }
00040
00041 template <typename T>
00042 T radix2(){ return radix<T>()*radix<T>(); }
00043
00044 template<int Size>
00045 struct decrement_if_fixed_size
00046 {
00047 enum {
00048 ret = (Size == Dynamic) ? Dynamic : Size-1 };
00049 };
00050
00051 #endif
00052
00053 template< typename _Scalar, int _Deg >
00054 class companion
00055 {
00056 public:
00057 EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(_Scalar,_Deg==Dynamic ? Dynamic : _Deg)
00058
00059 enum {
00060 Deg = _Deg,
00061 Deg_1=decrement_if_fixed_size<Deg>::ret
00062 };
00063
00064 typedef _Scalar Scalar;
00065 typedef typename NumTraits<Scalar>::Real RealScalar;
00066 typedef Matrix<Scalar, Deg, 1> RightColumn;
00067
00068 typedef Matrix<Scalar, Deg_1, 1> BottomLeftDiagonal;
00069
00070 typedef Matrix<Scalar, Deg, Deg> DenseCompanionMatrixType;
00071 typedef Matrix< Scalar, _Deg, Deg_1 > LeftBlock;
00072 typedef Matrix< Scalar, Deg_1, Deg_1 > BottomLeftBlock;
00073 typedef Matrix< Scalar, 1, Deg_1 > LeftBlockFirstRow;
00074
00075 typedef DenseIndex Index;
00076
00077 public:
00078 EIGEN_STRONG_INLINE const _Scalar operator()(Index row, Index col ) const
00079 {
00080 if( m_bl_diag.rows() > col )
00081 {
00082 if( 0 < row ){ return m_bl_diag[col]; }
00083 else{ return 0; }
00084 }
00085 else{ return m_monic[row]; }
00086 }
00087
00088 public:
00089 template<typename VectorType>
00090 void setPolynomial( const VectorType& poly )
00091 {
00092 const Index deg = poly.size()-1;
00093 m_monic = -1/poly[deg] * poly.head(deg);
00094
00095 m_bl_diag.setOnes(deg-1);
00096 }
00097
00098 template<typename VectorType>
00099 companion( const VectorType& poly ){
00100 setPolynomial( poly ); }
00101
00102 public:
00103 DenseCompanionMatrixType denseMatrix() const
00104 {
00105 const Index deg = m_monic.size();
00106 const Index deg_1 = deg-1;
00107 DenseCompanionMatrixType companion(deg,deg);
00108 companion <<
00109 ( LeftBlock(deg,deg_1)
00110 << LeftBlockFirstRow::Zero(1,deg_1),
00111 BottomLeftBlock::Identity(deg-1,deg-1)*m_bl_diag.asDiagonal() ).finished()
00112 , m_monic;
00113 return companion;
00114 }
00115
00116
00117
00118 protected:
00125 bool balanced( Scalar colNorm, Scalar rowNorm,
00126 bool& isBalanced, Scalar& colB, Scalar& rowB );
00127
00134 bool balancedR( Scalar colNorm, Scalar rowNorm,
00135 bool& isBalanced, Scalar& colB, Scalar& rowB );
00136
00137 public:
00146 void balance();
00147
00148 protected:
00149 RightColumn m_monic;
00150 BottomLeftDiagonal m_bl_diag;
00151 };
00152
00153
00154
00155 template< typename _Scalar, int _Deg >
00156 inline
00157 bool companion<_Scalar,_Deg>::balanced( Scalar colNorm, Scalar rowNorm,
00158 bool& isBalanced, Scalar& colB, Scalar& rowB )
00159 {
00160 if( Scalar(0) == colNorm || Scalar(0) == rowNorm ){ return true; }
00161 else
00162 {
00163
00164
00165
00166
00167
00168 rowB = rowNorm / radix<Scalar>();
00169 colB = Scalar(1);
00170 const Scalar s = colNorm + rowNorm;
00171
00172 while (colNorm < rowB)
00173 {
00174 colB *= radix<Scalar>();
00175 colNorm *= radix2<Scalar>();
00176 }
00177
00178 rowB = rowNorm * radix<Scalar>();
00179
00180 while (colNorm >= rowB)
00181 {
00182 colB /= radix<Scalar>();
00183 colNorm /= radix2<Scalar>();
00184 }
00185
00186
00187 if ((rowNorm + colNorm) < Scalar(0.95) * s * colB)
00188 {
00189 isBalanced = false;
00190 rowB = Scalar(1) / colB;
00191 return false;
00192 }
00193 else{
00194 return true; }
00195 }
00196 }
00197
00198 template< typename _Scalar, int _Deg >
00199 inline
00200 bool companion<_Scalar,_Deg>::balancedR( Scalar colNorm, Scalar rowNorm,
00201 bool& isBalanced, Scalar& colB, Scalar& rowB )
00202 {
00203 if( Scalar(0) == colNorm || Scalar(0) == rowNorm ){ return true; }
00204 else
00205 {
00210 const _Scalar q = colNorm/rowNorm;
00211 if( !isApprox( q, _Scalar(1) ) )
00212 {
00213 rowB = sqrt( colNorm/rowNorm );
00214 colB = Scalar(1)/rowB;
00215
00216 isBalanced = false;
00217 return false;
00218 }
00219 else{
00220 return true; }
00221 }
00222 }
00223
00224
00225 template< typename _Scalar, int _Deg >
00226 void companion<_Scalar,_Deg>::balance()
00227 {
00228 EIGEN_STATIC_ASSERT( Deg == Dynamic || 1 < Deg, YOU_MADE_A_PROGRAMMING_MISTAKE );
00229 const Index deg = m_monic.size();
00230 const Index deg_1 = deg-1;
00231
00232 bool hasConverged=false;
00233 while( !hasConverged )
00234 {
00235 hasConverged = true;
00236 Scalar colNorm,rowNorm;
00237 Scalar colB,rowB;
00238
00239
00240
00241 colNorm = abs(m_bl_diag[0]);
00242 rowNorm = abs(m_monic[0]);
00243
00244
00245 if( !balanced( colNorm, rowNorm, hasConverged, colB, rowB ) )
00246 {
00247 m_bl_diag[0] *= colB;
00248 m_monic[0] *= rowB;
00249 }
00250
00251
00252
00253 for( Index i=1; i<deg_1; ++i )
00254 {
00255
00256 colNorm = abs(m_bl_diag[i]);
00257
00258
00259 rowNorm = abs(m_bl_diag[i-1]) + abs(m_monic[i]);
00260
00261
00262 if( !balanced( colNorm, rowNorm, hasConverged, colB, rowB ) )
00263 {
00264 m_bl_diag[i] *= colB;
00265 m_bl_diag[i-1] *= rowB;
00266 m_monic[i] *= rowB;
00267 }
00268 }
00269
00270
00271
00272 const Index ebl = m_bl_diag.size()-1;
00273 VectorBlock<RightColumn,Deg_1> headMonic( m_monic, 0, deg_1 );
00274 colNorm = headMonic.array().abs().sum();
00275 rowNorm = abs( m_bl_diag[ebl] );
00276
00277
00278 if( !balanced( colNorm, rowNorm, hasConverged, colB, rowB ) )
00279 {
00280 headMonic *= colB;
00281 m_bl_diag[ebl] *= rowB;
00282 }
00283 }
00284 }
00285
00286 }
00287
00288 }
00289
00290 #endif // EIGEN_COMPANION_H