Companion.h
Go to the documentation of this file.
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 // This Source Code Form is subject to the terms of the Mozilla
00007 // Public License v. 2.0. If a copy of the MPL was not distributed
00008 // with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
00009 
00010 #ifndef EIGEN_COMPANION_H
00011 #define EIGEN_COMPANION_H
00012 
00013 // This file requires the user to include
00014 // * Eigen/Core
00015 // * Eigen/src/PolynomialSolver.h
00016 
00017 namespace Eigen { 
00018 
00019 namespace internal {
00020 
00021 #ifndef EIGEN_PARSED_BY_DOXYGEN
00022 
00023 template <typename T>
00024 T radix(){ return 2; }
00025 
00026 template <typename T>
00027 T radix2(){ return radix<T>()*radix<T>(); }
00028 
00029 template<int Size>
00030 struct decrement_if_fixed_size
00031 {
00032   enum {
00033     ret = (Size == Dynamic) ? Dynamic : Size-1 };
00034 };
00035 
00036 #endif
00037 
00038 template< typename _Scalar, int _Deg >
00039 class companion
00040 {
00041   public:
00042     EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(_Scalar,_Deg==Dynamic ? Dynamic : _Deg)
00043 
00044     enum {
00045       Deg = _Deg,
00046       Deg_1=decrement_if_fixed_size<Deg>::ret
00047     };
00048 
00049     typedef _Scalar                                Scalar;
00050     typedef typename NumTraits<Scalar>::Real       RealScalar;
00051     typedef Matrix<Scalar, Deg, 1>                 RightColumn;
00052     //typedef DiagonalMatrix< Scalar, Deg_1, Deg_1 > BottomLeftDiagonal;
00053     typedef Matrix<Scalar, Deg_1, 1>               BottomLeftDiagonal;
00054 
00055     typedef Matrix<Scalar, Deg, Deg>               DenseCompanionMatrixType;
00056     typedef Matrix< Scalar, _Deg, Deg_1 >          LeftBlock;
00057     typedef Matrix< Scalar, Deg_1, Deg_1 >         BottomLeftBlock;
00058     typedef Matrix< Scalar, 1, Deg_1 >             LeftBlockFirstRow;
00059 
00060     typedef DenseIndex Index;
00061 
00062   public:
00063     EIGEN_STRONG_INLINE const _Scalar operator()(Index row, Index col ) const
00064     {
00065       if( m_bl_diag.rows() > col )
00066       {
00067         if( 0 < row ){ return m_bl_diag[col]; }
00068         else{ return 0; }
00069       }
00070       else{ return m_monic[row]; }
00071     }
00072 
00073   public:
00074     template<typename VectorType>
00075     void setPolynomial( const VectorType& poly )
00076     {
00077       const Index deg = poly.size()-1;
00078       m_monic = -1/poly[deg] * poly.head(deg);
00079       //m_bl_diag.setIdentity( deg-1 );
00080       m_bl_diag.setOnes(deg-1);
00081     }
00082 
00083     template<typename VectorType>
00084     companion( const VectorType& poly ){
00085       setPolynomial( poly ); }
00086 
00087   public:
00088     DenseCompanionMatrixType denseMatrix() const
00089     {
00090       const Index deg   = m_monic.size();
00091       const Index deg_1 = deg-1;
00092       DenseCompanionMatrixType companion(deg,deg);
00093       companion <<
00094         ( LeftBlock(deg,deg_1)
00095           << LeftBlockFirstRow::Zero(1,deg_1),
00096           BottomLeftBlock::Identity(deg-1,deg-1)*m_bl_diag.asDiagonal() ).finished()
00097         , m_monic;
00098       return companion;
00099     }
00100 
00101 
00102 
00103   protected:
00110     bool balanced( Scalar colNorm, Scalar rowNorm,
00111         bool& isBalanced, Scalar& colB, Scalar& rowB );
00112 
00119     bool balancedR( Scalar colNorm, Scalar rowNorm,
00120         bool& isBalanced, Scalar& colB, Scalar& rowB );
00121 
00122   public:
00131     void balance();
00132 
00133   protected:
00134       RightColumn                m_monic;
00135       BottomLeftDiagonal         m_bl_diag;
00136 };
00137 
00138 
00139 
00140 template< typename _Scalar, int _Deg >
00141 inline
00142 bool companion<_Scalar,_Deg>::balanced( Scalar colNorm, Scalar rowNorm,
00143     bool& isBalanced, Scalar& colB, Scalar& rowB )
00144 {
00145   if( Scalar(0) == colNorm || Scalar(0) == rowNorm ){ return true; }
00146   else
00147   {
00148     //To find the balancing coefficients, if the radix is 2,
00149     //one finds \f$ \sigma \f$ such that
00150     // \f$ 2^{2\sigma-1} < rowNorm / colNorm \le 2^{2\sigma+1} \f$
00151     // then the balancing coefficient for the row is \f$ 1/2^{\sigma} \f$
00152     // and the balancing coefficient for the column is \f$ 2^{\sigma} \f$
00153     rowB = rowNorm / radix<Scalar>();
00154     colB = Scalar(1);
00155     const Scalar s = colNorm + rowNorm;
00156 
00157     while (colNorm < rowB)
00158     {
00159       colB *= radix<Scalar>();
00160       colNorm *= radix2<Scalar>();
00161     }
00162 
00163     rowB = rowNorm * radix<Scalar>();
00164 
00165     while (colNorm >= rowB)
00166     {
00167       colB /= radix<Scalar>();
00168       colNorm /= radix2<Scalar>();
00169     }
00170 
00171     //This line is used to avoid insubstantial balancing
00172     if ((rowNorm + colNorm) < Scalar(0.95) * s * colB)
00173     {
00174       isBalanced = false;
00175       rowB = Scalar(1) / colB;
00176       return false;
00177     }
00178     else{
00179       return true; }
00180   }
00181 }
00182 
00183 template< typename _Scalar, int _Deg >
00184 inline
00185 bool companion<_Scalar,_Deg>::balancedR( Scalar colNorm, Scalar rowNorm,
00186     bool& isBalanced, Scalar& colB, Scalar& rowB )
00187 {
00188   if( Scalar(0) == colNorm || Scalar(0) == rowNorm ){ return true; }
00189   else
00190   {
00195     const _Scalar q = colNorm/rowNorm;
00196     if( !isApprox( q, _Scalar(1) ) )
00197     {
00198       rowB = sqrt( colNorm/rowNorm );
00199       colB = Scalar(1)/rowB;
00200 
00201       isBalanced = false;
00202       return false;
00203     }
00204     else{
00205       return true; }
00206   }
00207 }
00208 
00209 
00210 template< typename _Scalar, int _Deg >
00211 void companion<_Scalar,_Deg>::balance()
00212 {
00213   using std::abs;
00214   EIGEN_STATIC_ASSERT( Deg == Dynamic || 1 < Deg, YOU_MADE_A_PROGRAMMING_MISTAKE );
00215   const Index deg   = m_monic.size();
00216   const Index deg_1 = deg-1;
00217 
00218   bool hasConverged=false;
00219   while( !hasConverged )
00220   {
00221     hasConverged = true;
00222     Scalar colNorm,rowNorm;
00223     Scalar colB,rowB;
00224 
00225     //First row, first column excluding the diagonal
00226     //==============================================
00227     colNorm = abs(m_bl_diag[0]);
00228     rowNorm = abs(m_monic[0]);
00229 
00230     //Compute balancing of the row and the column
00231     if( !balanced( colNorm, rowNorm, hasConverged, colB, rowB ) )
00232     {
00233       m_bl_diag[0] *= colB;
00234       m_monic[0] *= rowB;
00235     }
00236 
00237     //Middle rows and columns excluding the diagonal
00238     //==============================================
00239     for( Index i=1; i<deg_1; ++i )
00240     {
00241       // column norm, excluding the diagonal
00242       colNorm = abs(m_bl_diag[i]);
00243 
00244       // row norm, excluding the diagonal
00245       rowNorm = abs(m_bl_diag[i-1]) + abs(m_monic[i]);
00246 
00247       //Compute balancing of the row and the column
00248       if( !balanced( colNorm, rowNorm, hasConverged, colB, rowB ) )
00249       {
00250         m_bl_diag[i]   *= colB;
00251         m_bl_diag[i-1] *= rowB;
00252         m_monic[i]     *= rowB;
00253       }
00254     }
00255 
00256     //Last row, last column excluding the diagonal
00257     //============================================
00258     const Index ebl = m_bl_diag.size()-1;
00259     VectorBlock<RightColumn,Deg_1> headMonic( m_monic, 0, deg_1 );
00260     colNorm = headMonic.array().abs().sum();
00261     rowNorm = abs( m_bl_diag[ebl] );
00262 
00263     //Compute balancing of the row and the column
00264     if( !balanced( colNorm, rowNorm, hasConverged, colB, rowB ) )
00265     {
00266       headMonic      *= colB;
00267       m_bl_diag[ebl] *= rowB;
00268     }
00269   }
00270 }
00271 
00272 } // end namespace internal
00273 
00274 } // end namespace Eigen
00275 
00276 #endif // EIGEN_COMPANION_H


acado
Author(s): Milan Vukov, Rien Quirynen
autogenerated on Sat Jun 8 2019 19:36:53