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 //