synaps/linalg/MATRIX.m

/*********************************************************************
*      This file is part of the source code of SYNAPS kernel.        *
*   Author(s): B. Mourrain, GALAAD, INRIA                            *
**********************************************************************
History: 
$Id: MATRIX.h,v 1.2 2005/09/28 06:40:50 mourrain Exp $
**********************************************************************/
#ifndef synaps_linalg_MATRIX_H
#define synaps_linalg_MATRIX_H
/*********************************************************************/
#include <iostream>
#include <cassert>
#include <synaps/init.h>
#include <synaps/linalg/MethodName.h>
#include <synaps/base/Range.h>
/*********************************************************************/
__BEGIN_NAMESPACE_SYNAPS
//====================================================================
// namespace let 
// {
//   template<class A, class B> inline void assign(A& a, const B& b);
// }
namespace linalg {template<class R> struct rep1d;}
struct Bareiss;
template<class C, class O, class R> struct MPol;
//====================================================================
namespace MATRIX 
{
  typedef unsigned size_t;
  #define TOL 1.e-9 

  //----------------------------------------------------------------------
  template <class R>
  inline std::ostream & print(std::ostream & os,const R & m) 
  {
    typedef typename R::size_type  size_type;
    for(size_type i=0; i <m.nbrow(); ++i){
      os << "[ ";
      for(size_type j=0; j<m.nbcol(); j++) os<< m(i,j) <<" ";
      os << "]"<< std::endl;
    }  
    return os;
  }
  //----------------------------------------------------------------------
  template<class M>  inline
  std::istream & read( std::istream & is, M & m, ByRow br = ByRow() ){
    typedef typename M::size_type size_type;
    size_type nrw,ncl;
    is >> nrw >> ncl;
    m.resize(nrw,ncl);
    for( size_type i = 0; i < nrw && is.good(); i++ )
      for( size_type j = 0; j < ncl && is.good(); j++ )
        is >> m(i,j);
    return(is);
  }
  
  template<class M>  inline
  std::istream & read( std::istream & is, M & m, ByCol ){
    typedef typename M::size_type size_type;
    size_type nrw,ncl;
    is >> nrw >> ncl;
    m.resize(nrw,ncl);
    for( size_type j = 0; j < ncl && is.good(); j++ )
      for( size_type i = 0; i < nrw && is.good(); i++ )
        is >> m(i,j);
    return(is);
  }

  //----------------------------------------------------------------------
  template<class R, class C> inline
  void init(R & a, const C & c) 
  { 
    typedef typename R::size_type size_type;
    for(size_type i=0; i<a.nbrow(); i++) 
      for(size_type j=0; j <a.nbcol(); j++) 
        a(i,j)=c;
  } 
  
  template<class M> inline
  void init( M & m, const typename M::value_type * data, ByRow )
  {
    for( int i = 0; i < m.nbrow() ; i++ )
      for( int j = 0; j < m.nbcol() ; m(i,j) = *data++, j++ );
  };
  
  template<class M> inline
  void init( M & m,  const typename M::value_type * data, ByCol )
  {
    for( int j = 0; j < m.nbcol() ; j++ )
      for( int i = 0; i < m.nbrow() ; m(i,j) = *data++, i++ );
  }
  
  template<class M> inline
  void init( M & m, std::istream & ins, ByRow )
  {
    for( int i = 0; i < m.nbrow() ; i++ )
      for( int j = 0; j < m.nbcol() ; j ++ )
        {
          if ( ! ins.good() ) return;
          ins >> m(i,j);
        };
  };
  
  template<class M> inline
  void init( M & m, std::istream & ins, ByCol )
  {
    for( int j = 0; j < m.nbcol() ; j++ )
      for( int i = 0; i < m.nbrow() ; i ++ )
        {
          if ( ! ins.good() ) return;
          ins >> m(i,j);
        };
  };
  
  template<class M, class Data, class Order> inline
  void init( M & m, 
             typename M::size_type nrw, 
             typename M::size_type ncl, 
             Data & data, Order )
  {
    m.resize(nrw,ncl);
    init(m,data,Order());
  };
  
  
  //----------------------------------------------------------------------
  template<class R, class Int> inline
  void reserve(R & a, const Int & m, const Int & n) 
  { 
    a.resize(m,n);
  }
  
  template<class M, class C>
  void mul_ext( M & m, const C & c )
  {
    for ( int i = 0; i < m.nbrow(); i ++ )
      for ( int j = 0; j < m.nbcol(); j ++ )
        m(i,j) = m(i,j) * c;
  };

  template<class V, class M>
  void getrow( V & v, const M & m, int i )
  { for ( int k = 0; k < m.nbcol(); k ++ ) let::assign(v[k],m(i,k)); };

  template<class V, class M>
  void getcol( V & v, const M & m, int i )
  { for ( int k = 0; k < m.nbrow(); k ++ ) let::assign(v[k],m(k,i)); };

  
  template<class SM, class M>
  void submatrix( SM & sm, const M & m, const Range2d & rg )
  {
    using let::assign;
    typedef typename M::size_type sz_t;
    sz_t i1 = rg.i1, i2 = rg.i2, j1 = rg.j1, j2 = rg.j2;
    sm.resize(i2-i1+1,j2-j1+1);
    for( sz_t  i = i1; i < i2+1; i++ ) 
      for( sz_t j = j1; j < j2+1; j++ ) 
        assign(sm(i-i1,j-j1),m(i,j));
  };

  template<class M, class Ma, class C>
  void mul_ext( M & m, const Ma & a, const C & c )
  {
    m.resize( a.nbrow(), a.nbcol() );
    for ( int i = 0; i < m.nbrow(); i ++ )
      for ( int j = 0; j < m.nbcol(); j ++ )
        m(i,j) = a(i,j) * c;
  };

  template<class M, class C>
  void div_ext( M & m, const C & c )
  {
    for ( int i = 0; i < m.nbrow(); i ++ )
      for ( int j = 0; j < m.nbcol(); j ++ )
        m(i,j) /= c;
  };

  template<class M, class Ma, class C>
  void div_ext( M & m, const Ma & a, const C & c )
  {
    m.resize( a.nbrow(), a.nbcol() );
    for ( int i = 0; i < m.nbrow(); i ++ )
      for ( int j = 0; j < m.nbcol(); j ++ )
        m(i,j) = a(i,j) / c;
  };

  template<class M>
  void swaprow( M & m, int i, int j )
  { if(i!=j) for(int l=0; l< m.nbcol();l++) std::swap(m(i,l),m(j,l)); };
  template<class M>
  void swapcol( M & m, int i, int j )
  { if(i!=j) for(int l=0; l< m.nbrow();l++) std::swap(m(l,i),m(l,j)); };
  template<class M, class C>
  void addrow( M & m, typename M::size_type i, typename M::size_type j, const C & c )
  { for( int l=0; l < m.nbcol();l++) m(i,l) += m(j,l)*c; };
  template<class M, class C>
  void addcol( M & m, int i, int j, const C & c ) 
  { for( int l=0; l < m.nbrow();l++) m(l,i) += m(l,j)*c; };
  template<class M, class C>
  void multrow( M & m, int i, const C & c )
  { for( int l=0; l< m.nbcol();l++)  m(i,l)*=c; };
  template<class M, class C>
  void multcol( M & m, int i, const C & c )
  { for( int l=0; l< m.nbrow();l++)  m(l,i)*=c; };
  //----------------------------------------------------------------------
  template <class R1, class R2>
  inline void assign(R1 & r, const R2 & m) 
  {
    using let::assign;
    typedef typename R1::size_type  size_type;
    for(size_type i=0; i <m.nbrow(); ++i)
      for(size_type j=0; j<m.nbcol(); j++) 
        assign(r(i,j),m(i,j));
  }
  //----------------------------------------------------------------------
  template<class R> inline
  typename R::value_type trace(const R & M)
  {
    typename R::value_type tr(0);
    for (typename R::size_type i=0; i<M.nbrow(); i++) tr+=M(i,i);
    return tr;
  }
  //----------------------------------------------------------------------
  template<class R,class S> inline
  void add(R & a, const S & b )
  {
    typedef typename R::size_type size_type;
    for(size_type i=0; i<a.nbrow(); i++) 
      for(size_type j=0; j <a.nbcol(); j++) 
        a(i,j) += b(i,j);
  }
  //----------------------------------------------------------------------
  template<class R,class S,class T> inline
  void add(R & a, const S & b, const T & c)
  {
    assert((b.nbrow()==c.nbrow()) && (b.nbcol()==c.nbcol()) );
    typedef typename R::size_type size_type;
    
    for(size_type i=0; i<a.nbrow(); i++) 
      for(size_type j=0; j <a.nbcol(); j++)
        a(i,j) = b(i,j)+c(i,j);
  }
  //----------------------------------------------------------------------
  template<class R,class S> inline
  void sub(R & a, const S & b )
  {
    typedef typename R::size_type size_type;
    for(size_type i=0; i<a.nbrow(); i++) 
      for(size_type j=0; j <a.nbcol(); j++)
        a(i,j) -= b(i,j);
  }
  //----------------------------------------------------------------------
  template<class R> inline
  void sub(R & a, const R & b, const R & c)
  {
    assert((b.nbrow()==c.nbrow()) && (b.nbcol()==c.nbcol()) );
    typedef typename R::size_type size_type;

    for(size_type i=0; i<a.nbrow(); i++) 
      for(size_type j=0; j <a.nbcol(); j++)
        a(i,j) = b(i,j)-c(i,j);
  }
  //----------------------------------------------------------------------
  template<class R,class S,class T> inline
  void mul(R & a, const S & b, const T & c)
  {
    assert((&a != &c) && (&a != &b));
    typedef typename R::size_type size_type;
    typedef typename R::value_type value_type;
    for(size_type i=0; i<b.nbrow(); i++) 
      for(size_type j=0; j< c.nbcol(); j++){
        for(size_type k=0; k < b.nbcol(); k++) {
          a(i,j) += b(i,k)*c(k,j);
        }
      }
  }
  //----------------------------------------------------------------------
  template<class R,class S> inline
  void mul(R & a, const S & b)
  {
    assert((&a != &b));
    assert((a.nbcol()==b.nbrow()));
    typedef typename R::size_type size_type;
    typedef typename R::value_type value_type;

    R at(a);
    a.resize(a.nbrow(),b.nbcol());
    init(a,0);
    //    linalg::rep1d<value_type> r(b.nbcol());
    for(size_type i=0; i<a.nbrow(); i++) 
      {
        //      for(size_type j=0; j< b.nbcol(); j++) r[j]=0;
        for(size_type j=0; j< b.nbcol(); j++)
          for(size_type k=0; k < at.nbcol(); k++) {
            a(i,j) += at(i,k)*b(k,j);
          }
        //      for(size_type j=0; j< b.nbcol(); j++) a(i,j)=r[j];
      }
  }
  //----------------------------------------------------------------------
  template<class V,class M,class W> inline
  void mul_vect(V & a, const M & b, const W & v)
  {
    assert(&a != &v);
    typedef typename M::size_type  size_type;
    typedef typename V::value_type value_type;
    for(size_type i=0; i<b.nbrow(); i++)
      { 
        a[i] = 0; 
        for(size_type j=0; j< b.nbcol(); j++)
          a[i] += b(i,j)*v;
      };
  }
  
  //----------------------------------------------------------------------
  template <class R> 
  void transpose(R & M) 
  {
    typedef typename R::size_type size_type;
    
    if (M.nbrow()==M.nbcol())
      for(size_type i = 0; i < M.nbrow(); i++)
        for(size_type j = i+1; j < M.nbcol(); j++) 
          std::swap(M(i, j), M(j, i));
    else {
      R tmp(M.nbcol(),M.nbrow());
      for(size_type i = 0; i < M.nbrow(); i++)
        for(size_type j = 0; j < M.nbcol(); j++) 
          tmp(j, i)= M(i, j);
      M=tmp;
    }
  }
  template<class R>
  bool equal( const R & a, const R & b )
  {
    if (a.nbrow() != b.nbrow() || a.nbcol() != b.nbcol()) return false;
    
    unsigned i;
    for(i=0;i<a.nbrow(); i++)
      {
        unsigned j;
        for(j=0;j<a.nbcol() && a(i,j)==b(i,j); j++); 
        if(j!=a.nbcol()) return false;
      }
    if(i!=a.nbrow())  return false;
    
    return true;
  };
  
  //----------------------------------------------------------------------
  template<class R>
  void setupper(R & A) 
  {
    for(unsigned j=0 ; j< A.nbcol(); j++)
      for(unsigned i=j+1; i < A.nbrow(); i++)
        A(i,j) =0;
  }
  //----------------------------------------------------------------------
  template<class R>
  void setlower(R & A) 
  {
    for(unsigned j=0 ; j< A.nbcol(); j++)
      for(unsigned i=0; i < j && i < A.nbrow(); i++)
        A(i,j) =0;
  }
  //----------------------------------------------------------------------
  /* This SVD routine is based on pgs 30-48 of "Compact Numerical Methods
     for Computers" by J.C. Nash (1990), used to compute the pseudoinverse.
     First implementation by bryant@sioux.stanford.edu (Bryant Marks).
     Modification by Bernard.Mourrain@sophia.inria.fr for genericity.
  */
  
  template<class Vect, class Mat>
  void svd(Vect & S, const Mat & A, Mat & Us, Mat & V)
  {
#ifdef VERBATIM
    std::cout <<"Using the  generic svd" << std::endl;
#endif
    typedef typename Vect::size_type   index_t;
    typedef typename Vect::value_type  C;
    
    int i, j, k, EstColRank, RotCount, SweepCount, slimit;
    C eps, e2, tol, vt, p, x0, y0, q, r, c0, s0=0, d1, d2;
    int  nRow = A.nbrow(), nCol = A.nbcol();
    eps = C(TOL);
    slimit = nCol/4;
    
    Us=A;  // Copy of A in Us;
    V.resize(nCol,nCol);
    
    if (slimit < 6.0)
      slimit = 6;
    SweepCount = 0;
    e2 = C(10.0)*C(nRow)*eps*eps;
    tol = eps*C(.1);
    EstColRank = nCol;
    for (i=0; i<nCol; i++)
      for (j=0; j<nCol; j++)
        {
          V(j,i) = C(0.0);
          V(i,i) = C(1.0);
        }
    
    RotCount = EstColRank*(EstColRank-1)/2;
    while (RotCount != 0 && SweepCount <= slimit)
      {
        RotCount = EstColRank*(EstColRank-1)/2;
        SweepCount++;
        for (j=0; j<EstColRank-1; j++)
          {
            for (k=j+1; k<EstColRank; k++)
              {
                p = q = r = C(0.0);
                for (i=0; i<nRow; i++)
                  {
                    x0 = Us(i,j); y0 = Us(i,k);
                    p += x0*y0; q += x0*x0; r += y0*y0;
                  }
                S = q; S = r;
                if (q >= r)
                  {
                    if (q<=e2*S[0] || (Norm(p)<=(tol*q))) RotCount--;
                    else
                      {
                        p /= q; r = -r/q+1; vt = sqrt(p*p*4+r*r);
                        c0 = sqrt(Norm((r/vt+1)*C(.5))); 
                        s0 = p/(vt*c0);
                        for (i=0; i<nRow; i++)
                          {
                            d1 = Us(i,j); d2 = Us(i,k);
                            Us(i,j) = d1*c0+d2*s0; Us(i,k) = -d1*s0+d2*c0; 
                          }
                        for(i=0; i<nCol; i++)        
                          {
                            d1 = V(j,i); d2 = V(k,i);
                            V(j,i) = d1*c0+d2*s0; 
                            V(k,i) = -d1*s0+d2*c0; 
                          }
                      }
                  }
                else
                  {
                    p /= r; q = q/r-C(1); vt = sqrt(p*p*4+q*q);
                    s0 = sqrt(Norm(C(.5)*(-q/vt+1)));
                    if (p<0) s0 = -s0;
                    c0 = p/(vt*s0);
                    for (i=0; i<nRow; i++)
                      {
                        d1 = Us(i,j); d2 = Us(i,k);
                        Us(i,j) = d1*c0+d2*s0; Us(i,k) = -d1*s0+d2*c0;
                      }
                    for (i=0; i<nCol; i++)
                      {
                        d1 = V(j,i); d2 = V(k,i);
                        V(j,i) = d1*c0+d2*s0; V(k,i) = -d1*s0+d2*c0;
                      }
                  }
              }
          }
        while (EstColRank>=3 && S[(EstColRank-1)]<=S[0]*tol+tol*tol)
          EstColRank--;
      }
#if DEBUG
    //  if (SweepCount > slimit)
    //    fprintf(stderr, "Sweeps = %d\n", SweepCount);
#endif
    for(index_t i=0;i<S.size();i++) S[i]=sqrt(S[i]);
    
  }

  //----------------------------------------------------------------------
  template<class Vect, class Mat>
  void svd(Vect & S, const Mat & A)
  {
    int m=A.nbrow(), n=A.nbcol();
    Mat U(m,m), V(n,n);
    svd(S,A,U,V);
  }
  //----------------------------------------------------------------------
  template <class R>
  bool good_pivot(Bareiss, R & A)
  {
    return A != 0;
  }
  //----------------------------------------------------------------------
  template<class C, class O, class R>
  bool good_pivot(Bareiss, MPol<C,O,R> & A)
  {
    typedef typename MPol<C,O,R>::iterator iterator;
    bool result = false;
    C Precision = 1e-12;
    for(iterator it=A.begin(); it != A.end(); ++it)
      if(it->coeff() > Precision)
        return true;
    return false;
  }
  //----------------------------------------------------------------------
  template<class R>
  typename R::value_type decomp(LU lu, R & A)
  {
    typedef unsigned size_type ;
    typedef typename R::value_type value_type;
    
    size_type n = A.nbrow(), m = A.nbcol();
    size_type rmar = m, r=0, i,j;
    int s = 1;
    value_type d=1;// t;
    
    for(size_type k=0; k< std::min(m, rmar) && r < n; k++) {
      for(i=r; i<n && A(i,k) == 0;i++);
      
      for(j=i+1; j<n; j++) {
        if(A(j,k) !=0 && size(A(j, k)) < size(A(i,k))) {
          i=j;
        }
      }
      if(i < n) {
        if( i != r) {
          s *= -1;
          for(j=k;j<m;j++) {
            std::swap(A(i,j), A(r,j));
          }
        }
        for(i = r+1;i<n;i++) {
          for(j=k+1;j<m;j++) {
            d = A(i,k)/A(r,k);
            A(i,j) -= A(r,j)*d;
          }
          A(i, k) = 0;
        }
        r++;
      }
    }
    if(s==-1)
      return -A(A.nbrow()-1,A.nbcol()-1);
    else
      return  A(A.nbrow()-1,A.nbcol()-1);
  }
  
  //----------------------------------------------------------------------
  template <class R>  
  typename R::value_type decomp(Bareiss, R & A, unsigned & r)
  {
    typedef unsigned size_t;
    typedef typename R::value_type value_type;
    r=0;
    size_t n = A.nbrow(), m = A.nbcol();
    size_t rmar = m, i,j;
    int s = 1;
    value_type d=1;// t;
    for(size_t k=0; k< std::min(m, rmar) && r < n; k++) {
      for(i=r; i<n && (size(A(i,k)) == 0);i++);
      
      for(j=i+1; j<n; j++) {
        if(A(j,k) !=0 && size(A(j, k)) < size(A(i,k))){
          i=j;
        }
      }
      if(i < n) {
        if( i != r) {
          s *= -1;
          for(j=k;j<m;j++) {
            std::swap(A(i,j),A(r,j));
          }
        }
        for(i = r+1;i<n;i++) {
          for(j=k+1;j<m;j++) {
            A(i,j) = (A(i,j)*A(r,k) - A(r,j)*A(i,k));
            A(i,j) /= d;
          }
          A(i, k) = 0;
        }
        d= A(r, k);
        r++;
      }
    }
    if(s==-1)
      return -A(A.nbrow()-1,A.nbcol()-1);
    else
      return  A(A.nbrow()-1,A.nbcol()-1);
  }
  //--------------------------------------------------------------------
  template <class R>  
  typename R::value_type decomp(const Bareiss& mth, R & A)
  {
    unsigned rk=0;
    return decomp(mth,A,rk);
  }
  //----------------------------------------------------------------------
  template<class R>  
  typename R::size_type rank(const R & M)
  {
    typedef typename R::size_type size_type;
    typedef typename R::value_type value_type;
    linalg::rep1d<value_type> S(std::min(M.nbrow(),M.nbcol()));
    svd(S,M);
    size_type i= S.size()-1;
    for(; i>=0 && S*TOL>S;i--);
    return i+1;
  }
  //-------------------------------------------------------------------- 
  template<class R>  
  typename R::size_type rank(const Bareiss & mth, const R & M)
  {
    unsigned rk=0;
    R A=M;
    decomp(mth,A,rk);
    return rk;
  }
  

  //-------------------------------------------------------------------- 
  template<class MAT>  
  void kronecker(MAT& r, const MAT& a, const MAT& b)
  {
    r.resize(a.nbrow()*b.nbrow(),a.nbcol()*b.nbcol());
    for(unsigned i=0;i<a.nbrow();i++)
      for(unsigned j=0;j<a.nbcol();j++)
        for(unsigned k=0;k<b.nbcol();k++)
          for(unsigned l=0;l<b.nbcol();l++)
            r(i*b.nbrow()+k,j*b.nbrow()+l)=a(i,j)*b(k,l);
  }
} //MATRIX
//--------------------------------------------------------------------
__END_NAMESPACE_SYNAPS
/********************************************************************/
#endif // synaps_linalg_MATRIX_H