Companion.h
00001 // This file is part of Eigen, a lightweight C++ template library
00002 // for linear algebra.
00003 //
00004 // Copyright (C) 2010 Manuel Yguel <manuel.yguel@gmail.com>
00005 //
00006 // Eigen is free software; you can redistribute it and/or
00007 // modify it under the terms of the GNU Lesser General Public
00008 // License as published by the Free Software Foundation; either
00009 // version 3 of the License, or (at your option) any later version.
00010 //
00011 // Alternatively, you can redistribute it and/or
00012 // modify it under the terms of the GNU General Public License as
00013 // published by the Free Software Foundation; either version 2 of
00014 // the License, or (at your option) any later version.
00015 //
00016 // Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
00017 // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
00018 // FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
00019 // GNU General Public License for more details.
00020 //
00021 // You should have received a copy of the GNU Lesser General Public
00022 // License and a copy of the GNU General Public License along with
00023 // Eigen. If not, see <http://www.gnu.org/licenses/>.
00024 
00025 #ifndef EIGEN_COMPANION_H
00026 #define EIGEN_COMPANION_H
00027 
00028 // This file requires the user to include
00029 // * Eigen/Core
00030 // * Eigen/src/PolynomialSolver.h
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     //typedef DiagonalMatrix< Scalar, Deg_1, Deg_1 > BottomLeftDiagonal;
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       //m_bl_diag.setIdentity( deg-1 );
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     //To find the balancing coefficients, if the radix is 2,
00164     //one finds \f$ \sigma \f$ such that
00165     // \f$ 2^{2\sigma-1} < rowNorm / colNorm \le 2^{2\sigma+1} \f$
00166     // then the balancing coefficient for the row is \f$ 1/2^{\sigma} \f$
00167     // and the balancing coefficient for the column is \f$ 2^{\sigma} \f$
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     //This line is used to avoid insubstantial balancing
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     //First row, first column excluding the diagonal
00240     //==============================================
00241     colNorm = abs(m_bl_diag[0]);
00242     rowNorm = abs(m_monic[0]);
00243 
00244     //Compute balancing of the row and the column
00245     if( !balanced( colNorm, rowNorm, hasConverged, colB, rowB ) )
00246     {
00247       m_bl_diag[0] *= colB;
00248       m_monic[0] *= rowB;
00249     }
00250 
00251     //Middle rows and columns excluding the diagonal
00252     //==============================================
00253     for( Index i=1; i<deg_1; ++i )
00254     {
00255       // column norm, excluding the diagonal
00256       colNorm = abs(m_bl_diag[i]);
00257 
00258       // row norm, excluding the diagonal
00259       rowNorm = abs(m_bl_diag[i-1]) + abs(m_monic[i]);
00260 
00261       //Compute balancing of the row and the column
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     //Last row, last column excluding the diagonal
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     //Compute balancing of the row and the column
00278     if( !balanced( colNorm, rowNorm, hasConverged, colB, rowB ) )
00279     {
00280       headMonic      *= colB;
00281       m_bl_diag[ebl] *= rowB;
00282     }
00283   }
00284 }
00285 
00286 } // end namespace internal
00287 
00288 } // end namespace Eigen
00289 
00290 #endif // EIGEN_COMPANION_H