include/realroot/linear_householder.hpp

Go to the documentation of this file.

/********************************************************************
 *   This file is part of the source code of the realroot library.
 *   Author(s): J.P. Pavone, GALAAD, INRIA
 *   $Id: householder.hpp,v 1.1 2005/07/11 10:03:55 jppavone Exp $
 ********************************************************************/
#ifndef realroot_SOLVE_SBDSLV_LINEAR_HOUSEHOLDER_HPP
#define realroot_SOLVE_SBDSLV_LINEAR_HOUSEHOLDER_HPP
//--------------------------------------------------------------------
#include <math.h>
#include <realroot/system_descartes1d.hpp>
//--------------------------------------------------------------------
namespace mmx {
//--------------------------------------------------------------------
namespace linear
{
  /*
   *  M: matrice ring_sparseetrique a reduire, a la fin contient la matrice de transformation
   *  d: elements diagonaux
   *  e: au dessus de la diagonale (e[0] = 0 )
   */
  template< typename real_t > 
  void householder( real_t * M, real_t * d, real_t * e, int n )
  {
    int i,j,k,l;
    real_t scale, hh, h, g, f;
    real_t * mi = M + (n-1)*n;
    for ( i = n-1; i >= 1; i--, mi -= n )
      {
        l = i-1;
        h = scale = 0.0;
        if ( l > 0 ) 
          {
            for ( k = 0; k <= l; scale += fabs(mi[k]), k ++ ) ;
            if ( scale == 0.0 ) e[i] = mi[l];
            else
              {
                for ( k = 0; k <= l; mi[k] /= scale, h += mi[k]*mi[k], k ++ ) ;
                f = mi[l];
                g = (f >= 0.0 ? -std::sqrt(h) : std::sqrt(h));
                e[i] = scale*g;
                h -= f*g;
                mi[l] = f - g;
                f = 0.0;
                real_t * mj = M;
                for ( j = 0; j <= l; j ++, mj += n ) {
                  mj[i] = mi[j]/h;
                  g = 0.0;
                  for ( k = 0; k <= j; g += mj[k]*mi[k], k ++ ) ;
                  for ( k = j+1; k<= l; g += M[k*n+j]*mi[k], k ++ ) ;
                  e[j] = g/h;
                  f += e[j]*mi[j];
                };
                hh = f /(h+h);
                for ( j = 0; j <= l; j ++ )
                  {
                    f =  mi[j];
                    e[j] = g = e[j] - hh*f;
                    for ( k = 0; k <= j; M[j*n+k] -= (f*e[k]+g*mi[k]), k ++ ) ;
                  };
              }
          }
        else
          e[i] = mi[l];
        d[i] = h;
      };
    
    d[0] = 0;
    e[0] = 0;
    mi = M;
    for ( i = 0; i < n; i ++, mi += n )
      {
        l = i - 1;
        if ( d[i] ) 
          for ( j = 0; j <= l; j ++ )
            {
              g = 0.0;
              for ( k = 0; k <= l; g += mi[k]*M[k*n+j], k ++ ) ;
              for ( k = 0; k <= l; M[k*n+j] -= g*M[k*n+i], k ++ ) ;
            };
        d[i] = mi[i];
        mi[i] = 1.0;
        for ( j = 0; j <= l; j ++ ) M[j*n+i] = mi[j] = 0.0;
      };
  };
  
  template< typename real_t > 
  real_t pythag( const real_t& a, const real_t& b)
  {
    real_t fa = std::abs(a);
    real_t fb = std::abs(b);
    real_t c;
    if ( fa > fb ) 
      {
        c = fb/fa;
        return fa * std::sqrt(1.0+c*c);
      }
    else
      {
        c = fa/fb;
        return fb * std::sqrt(1.0+c*c);
      };
   };

  
  template< typename real_t > 
  void tqli( real_t * d, real_t * e, int n, real_t * z )
  {
    int m,l,iter,i,k;
    real_t s,r,p,g,f,dd,c,b;
    for ( i = 1; i <  n; i ++ ) e[i-1] = e[i];
    e[n-1] = 0.0;
    for ( l = 0; l < n; l++ )//for ( l = 1; l <= n; l ++ )
      {

        iter = 0;
        do
          {
           
            for ( m = l; m < n-1; m++ )//           for ( m = 1; m <=n-1; m++ )
              {
                dd = std::abs(d[m])+std::abs(d[m+1]);
                if ( (real_t)(std::abs(e[m])+dd) == dd ) 
                  { break; };
              };
            if ( m != l )
              {
                iter++;
                assert(iter<30);
                g = (d[l+1]-d[l])/(2.0*e[l]);
                r = pythag(g,(real_t)1.0);
                if ( g >= 0.0 ) g = d[m]-d[l]+e[l]/(g+std::abs(r));
                else            g = d[m]-d[l]+e[l]/(g-std::abs(r));
                s = c = 1.0;
                p = 0.0;
                for ( i = m-1; i >= l; i-- )
                  {
                    f = s*e[i];
                    b = c*e[i];
                    e[i+1] = (r=pythag(f,g));
                    if ( r == 0.0 )
                      {
                        d[i+1] -= p;
                        e[m] = 0.0;
                        break;
                      };
                    s = f/r;
                    c = g/r;
                    g = d[i+1]-p;
                    r=(d[i]-g)*s+2.0*c*b;
                    d[i+1]= g +(p=s*r);
                    g = c*r-b;
                    for ( k = 0; k < n; k++ )//             for ( k = 1; k <= n; k++ )
                      {
                        f = z[k*n+i+1];//f = z[k][i+1];
                        z[k*n+i+1] = s*z[k*n+i]+c*f;//z[k][i+1] = s*z[k][i]+c*f;
                        z[k*n+i]  = c*z[k*n+i]-s*f;//z[k][i]   = c*z[k][i]-s*f;
                      };
                  }
                if ( r == 0.0 && i >= l ) continue;
                d[l] -= p;
                e[l]  = g;
                e[m]  = 0.0;
              };
          }
        while( m != l );
      };
  };
  
  template< class real_t >
  void symeigen( real_t * M, real_t * d, int n )
  {
    real_t e[n];
    householder( M, d, e, n );
    tqli( d, e, n, M );
  };
  
  template< typename real_t > 
  void cpolynom( real_t * p, real_t * M, int n )
  {
    real_t   cp1[n+1];
    real_t * p1 = cp1;  
    real_t * p0 = p;
    int i,k,c;
    
    for ( k = 0; k <= n; p0[k] = p1[k] = 0, k ++ ) ; 

    p0[0] =  1; 
    p1[0] =  M[n*(n-1)+(n-1)];
    p1[1] = -1;

    c = 0;

    for ( i = n-2; i >= 0; i -- )
      {
        real_t a = M[i*n+i];
        real_t b = M[i*n+i+1];

        for ( k = 0; k <= n-2-i; k ++ ) p0[k] = -p0[k]*(b*b);
        for ( k = 0; k <= n-1-i; k ++ ) p0[k] += a*p1[k];
        for ( k = 1; k <= n-i; k ++ )
          p0[k] -= p1[k-1];
//      for ( k = 0; k <= n-1-i; k ++ )
//        p0[k] +=  a*p1[k];
//      for ( k = 1; k <= n-i; k ++ )
//        p0[k] -= p1[k-1];
        std::swap(p0,p1);
      };
    
    if ( p1 != p )
      std::copy( p1, p1 + n+1, p );
    //    if ( c ) std::copy( p1, p1 + n+1, p0 );
  };
  
  template< typename real_t >
  void caract_polynom( real_t * p, real_t * M, int n )
  {
    real_t d[n];
    real_t e[n];
    householder( M, d, e, n );
    cpolynom( p, M, n );
  };

  /*
  struct slv1dprm : std::vector< real_t >
  {
    void output( const real_t& a, const real_t& b )
    { push_back( (a+b)/2.0 ); };
  };
  
  template< typename real_t >
  void symeigen( real_t * vp, real_t * base, real_t * m, 
                 int n, const real_t& eps = 1e-12 )
  {
    real_t tmp[n*n];
    std::copy(m,m+n*n,tmp);
    real_t p[n+1];
    caract_polynom(p, m, n );
    typedef solvers::descartes_solver< real_t, solvers::bsearch_newton<real_t> > solver_t;
    solver_t tmp(n+1,eps);
    slv1dprm svp;
    tmp.solve( svp, p );
    for ( unsigned i = 0; i < svp.size(); i ++ )
      
  };
  */
};
//--------------------------------------------------------------------
} //namespace mmx
/********************************************************************/
#endif //

---