text: // This file is part of Eigen, a lightweight C++ template library
  // for linear algebra.
  //
  // Copyright (C) 2010 Manuel Yguel <manuel.yguel@gmail.com>
  //
  // This Source Code Form is subject to the terms of the Mozilla
  // Public License v. 2.0. If a copy of the MPL was not distributed
  // with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
  
  #ifndef EIGEN_COMPANION_H
  #define EIGEN_COMPANION_H
  
  // This file requires the user to include
  // * Eigen/Core
  // * Eigen/src/PolynomialSolver.h
  
  namespace Eigen { 
  
  namespace internal {
  
  #ifndef EIGEN_PARSED_BY_DOXYGEN
  
  template <typename T>
  T radix(){ return 2; }
  
  template <typename T>
  T radix2(){ return radix<T>()*radix<T>(); }
  
  template<int Size>
  struct decrement_if_fixed_size
  {
    enum {
      ret = (Size == Dynamic) ? Dynamic : Size-1 };
  };
  
  #endif
  
  template< typename _Scalar, int _Deg >
  class companion
  {
    public:
      EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(_Scalar,_Deg==Dynamic ? Dynamic : _Deg)
  
      enum {
        Deg = _Deg,
        Deg_1=decrement_if_fixed_size<Deg>::ret
      };
  
      typedef _Scalar                                Scalar;
      typedef typename NumTraits<Scalar>::Real       RealScalar;
      typedef Matrix<Scalar, Deg, 1>                 RightColumn;
      //typedef DiagonalMatrix< Scalar, Deg_1, Deg_1 > BottomLeftDiagonal;
      typedef Matrix<Scalar, Deg_1, 1>               BottomLeftDiagonal;
  
      typedef Matrix<Scalar, Deg, Deg>               DenseCompanionMatrixType;
      typedef Matrix< Scalar, _Deg, Deg_1 >          LeftBlock;
      typedef Matrix< Scalar, Deg_1, Deg_1 >         BottomLeftBlock;
      typedef Matrix< Scalar, 1, Deg_1 >             LeftBlockFirstRow;
  
      typedef DenseIndex Index;
  
    public:
      EIGEN_STRONG_INLINE const _Scalar operator()(Index row, Index col ) const
      {
        if( m_bl_diag.rows() > col )
        {
          if( 0 < row ){ return m_bl_diag[col]; }
          else{ return 0; }
        }
        else{ return m_monic[row]; }
      }
  
    public:
      template<typename VectorType>
      void setPolynomial( const VectorType& poly )
      {
        const Index deg = poly.size()-1;
        m_monic = -poly.head(deg)/poly[deg];
        m_bl_diag.setOnes(deg-1);
      }
  
      template<typename VectorType>
      companion( const VectorType& poly ){
        setPolynomial( poly ); }
  
    public:
      DenseCompanionMatrixType denseMatrix() const
      {
        const Index deg   = m_monic.size();
        const Index deg_1 = deg-1;
        DenseCompanionMatrixType companMat(deg,deg);
        companMat <<
          ( LeftBlock(deg,deg_1)
            << LeftBlockFirstRow::Zero(1,deg_1),
            BottomLeftBlock::Identity(deg-1,deg-1)*m_bl_diag.asDiagonal() ).finished()
          , m_monic;
        return companMat;
      }
  
  
  
    protected:
      /** Helper function for the balancing algorithm.
       * \returns true if the row and the column, having colNorm and rowNorm
       * as norms, are balanced, false otherwise.
       * colB and rowB are repectively the multipliers for
       * the column and the row in order to balance them.
       * */
      bool balanced( RealScalar colNorm, RealScalar rowNorm,
          bool& isBalanced, RealScalar& colB, RealScalar& rowB );
  
      /** Helper function for the balancing algorithm.
       * \returns true if the row and the column, having colNorm and rowNorm
       * as norms, are balanced, false otherwise.
       * colB and rowB are repectively the multipliers for
       * the column and the row in order to balance them.
       * */
      bool balancedR( RealScalar colNorm, RealScalar rowNorm,
          bool& isBalanced, RealScalar& colB, RealScalar& rowB );
  
    public:
      /**
       * Balancing algorithm from B. N. PARLETT and C. REINSCH (1969)
       * "Balancing a matrix for calculation of eigenvalues and eigenvectors"
       * adapted to the case of companion matrices.
       * A matrix with non zero row and non zero column is balanced
       * for a certain norm if the i-th row and the i-th column
       * have same norm for all i.
       */
      void balance();
  
    protected:
        RightColumn                m_monic;
        BottomLeftDiagonal         m_bl_diag;
  };
  
  
  
  template< typename _Scalar, int _Deg >
  inline
  bool companion<_Scalar,_Deg>::balanced( RealScalar colNorm, RealScalar rowNorm,
      bool& isBalanced, RealScalar& colB, RealScalar& rowB )
  {
    if( RealScalar(0) == colNorm || RealScalar(0) == rowNorm ){ return true; }
    else
    {
      //To find the balancing coefficients, if the radix is 2,
      //one finds \f$ \sigma \f$ such that
      // \f$ 2^{2\sigma-1} < rowNorm / colNorm \le 2^{2\sigma+1} \f$
      // then the balancing coefficient for the row is \f$ 1/2^{\sigma} \f$
      // and the balancing coefficient for the column is \f$ 2^{\sigma} \f$
      rowB = rowNorm / radix<RealScalar>();
      colB = RealScalar(1);
      const RealScalar s = colNorm + rowNorm;
  
      while (colNorm < rowB)
      {
        colB *= radix<RealScalar>();
        colNorm *= radix2<RealScalar>();
      }
  
      rowB = rowNorm * radix<RealScalar>();
  
      while (colNorm >= rowB)
      {
        colB /= radix<RealScalar>();
        colNorm /= radix2<RealScalar>();
      }
  
      //This line is used to avoid insubstantial balancing
      if ((rowNorm + colNorm) < RealScalar(0.95) * s * colB)
      {
        isBalanced = false;
        rowB = RealScalar(1) / colB;
        return false;
      }
      else{
        return true; }
    }
  }
  
  template< typename _Scalar, int _Deg >
  inline
  bool companion<_Scalar,_Deg>::balancedR( RealScalar colNorm, RealScalar rowNorm,
      bool& isBalanced, RealScalar& colB, RealScalar& rowB )
  {
    if( RealScalar(0) == colNorm || RealScalar(0) == rowNorm ){ return true; }
    else
    {
      /**
       * Set the norm of the column and the row to the geometric mean
       * of the row and column norm
       */
      const RealScalar q = colNorm/rowNorm;
      if( !isApprox( q, _Scalar(1) ) )
      {
        rowB = sqrt( colNorm/rowNorm );
        colB = RealScalar(1)/rowB;
  
        isBalanced = false;
        return false;
      }
      else{
        return true; }
    }
  }
  
  
  template< typename _Scalar, int _Deg >
  void companion<_Scalar,_Deg>::balance()
  {
    using std::abs;
    EIGEN_STATIC_ASSERT( Deg == Dynamic || 1 < Deg, YOU_MADE_A_PROGRAMMING_MISTAKE );
    const Index deg   = m_monic.size();
    const Index deg_1 = deg-1;
  
    bool hasConverged=false;
    while( !hasConverged )
    {
      hasConverged = true;
      RealScalar colNorm,rowNorm;
      RealScalar colB,rowB;
  
      //First row, first column excluding the diagonal
      //==============================================
      colNorm = abs(m_bl_diag[0]);
      rowNorm = abs(m_monic[0]);
  
      //Compute balancing of the row and the column
      if( !balanced( colNorm, rowNorm, hasConverged, colB, rowB ) )
      {
        m_bl_diag[0] *= colB;
        m_monic[0] *= rowB;
      }
  
      //Middle rows and columns excluding the diagonal
      //==============================================
      for( Index i=1; i<deg_1; ++i )
      {
        // column norm, excluding the diagonal
        colNorm = abs(m_bl_diag[i]);
  
        // row norm, excluding the diagonal
        rowNorm = abs(m_bl_diag[i-1]) + abs(m_monic[i]);
  
        //Compute balancing of the row and the column
        if( !balanced( colNorm, rowNorm, hasConverged, colB, rowB ) )
        {
          m_bl_diag[i]   *= colB;
          m_bl_diag[i-1] *= rowB;
          m_monic[i]     *= rowB;
        }
      }
  
      //Last row, last column excluding the diagonal
      //============================================
      const Index ebl = m_bl_diag.size()-1;
      VectorBlock<RightColumn,Deg_1> headMonic( m_monic, 0, deg_1 );
      colNorm = headMonic.array().abs().sum();
      rowNorm = abs( m_bl_diag[ebl] );
  
      //Compute balancing of the row and the column
      if( !balanced( colNorm, rowNorm, hasConverged, colB, rowB ) )
      {
        headMonic      *= colB;
        m_bl_diag[ebl] *= rowB;
      }
    }
  }
  
  } // end namespace internal
  
  } // end namespace Eigen
  
  #endif // EIGEN_COMPANION_H
  