source: Sophya/trunk/SophyaLib/TArray/tmatrix.cc@ 762

// $Id: tmatrix.cc,v 1.1.1.1 2000-03-02 16:48:24 ansari Exp $
//                         C.Magneville          04/99
#include "machdefs.h"
#include <stdio.h>
#include <stdlib.h>
#include <iostream.h>
#include <values.h>
#include <complex>
#include "pexceptions.h"
#include "tmatrix.h"
#include "objfio.h"

////////////////////////////////////////////////////////////////
//**** Createur, Destructeur

template <class T>
TMatrix<T>::TMatrix()
// Constructeur par defaut.
: mNr(0), mNc(0), mNDBlock()
{
}

template <class T>
TMatrix<T>::TMatrix(uint_4 r,uint_4 c)
// Construit une matrice de r lignes et c colonnes.
: mNr(r), mNc(c), mNDBlock(r*c)
{
}

template <class T>
TMatrix<T>::TMatrix(uint_4 r,uint_4 c,T* values,Bridge* br)
// Construit une matrice de r lignes et c colonnes. On fournit
// le tableau des valeurs et eventuellement un Bridge.
: mNr(r), mNc(c), mNDBlock(r*c,values,br)
{
}

template <class T>
TMatrix<T>::TMatrix(const TMatrix<T>& a)
// Constructeur par copie (partage si "a" temporaire).
: mNr(a.mNr), mNc(a.mNc), mNDBlock(a.mNDBlock)
{
}

template <class T>
TMatrix<T>::TMatrix(const TMatrix<T>& a,bool share)
// Constructeur par copie avec possibilite de forcer le partage ou non.
: mNr(a.mNr), mNc(a.mNc), mNDBlock(a.mNDBlock,share)
{
}

template <class T>
TMatrix<T>::~TMatrix()
// Destructeur
{
}

////////////////////////////////////////////////////////////////
// Operations matricielles
template <class T>
TMatrix<T> TMatrix<T>::Transpose(void) const
// Transposition
{
TMatrix<T> a; a.Clone(*this); a.SetTemp(true);
a.mNr = mNc; a.mNc = mNr;
{for(uint_4 i=0; i<mNr; i++)
  for(uint_4 j=0; j<mNc; j++) {
    a(j,i) = (*this)(i,j);
}}
return a;
}

////////////////////////////////////////////////////////////////
//**** Impression

template <class T>
void TMatrix<T>::Print(ostream& os,int lp
                 ,uint_4 i0,uint_4 ni,uint_4 j0,uint_4 nj) const
// Impression de la sous-matrice (i:i+ni-1,i:j+nj-1)
{
os<<"TMatrix::Print("<<mNr<<","<<mNc<<")"<<endl;
if(lp>0)
  {os<<"       this="<<this<<endl; mNDBlock.Print(0,0);}
if(mNr==0 || mNc==0) return;
if(i0>=mNr || j0>=mNc || ni==0 || nj==0) return;
uint_4 i1 = i0+ni; if(i1>mNr) i1 = mNr;
uint_4 j1 = j0+nj; if(j1>mNc) j1 = mNc;
for(uint_4 i=i0;i<i1;i++) {
  for(uint_4 j=j0;j<j1;j++) cout<<" "<<(*this)(i,j);
  cout<<endl;
}
}

////////////////////////////////////////////////////////////////
//**** Surcharge de *= (INPLACE): TMatrix *= TMatrix;

template <class T>
TMatrix<T>& TMatrix<T>::operator *= (const TMatrix<T>& a)
// A = A*B  -> A(n,m) = A(n,m)*B(m,m)
{
uint_4 ndata = mNr*mNc;
if(ndata==0 || mNc!=a.mNr || a.mNr!=a.mNc)
  throw(SzMismatchError("TMatrix::operator*=A size mismatch"));
// A(i,j) = Sum(k) A(i,k)*B(k,j) ... il faut sauver la ligne "i" de A
// Vecteur oi   : vecteur ou est sauve la ligne i de la matrice *this
//                oi,oe = pointeur de debut et de fin du vecteur temporaire
//                oij   = pointeur parcourant le vecteur oi
// Matrice *this: i     = pointeur du debut de la ligne i
//                ij    = pointeur parcourant la ligne i
// Matrice a    : aj    = pointeur de debut de la colonne j
//                aji   = pointeur parcourant la colonne j
T* oi = new T[mNc]; T* oe = oi+mNc;
for(T *i=Data(); i<Data()+ndata; i+=mNc) {
  {for(T *oij=oi,*ij=i; oij<oe;) *oij++ = *ij++;}
  {for(T *ij=i,*aj=const_cast<T *>(a.Data()); aj<a.Data()+a.mNc; ij++,aj++) {
    T sum = 0;
    for(T *oij=oi,*aji=aj; oij<oe; oij++,aji+=a.mNc) sum += *oij * *aji;
    *ij = sum;
  }}
}
delete [] oi;
return *this;
}

////////////////////////////////////////////////////////////////
//**** Pour surcharge d'operateurs C = A (+,-,*,/) B

template <class T> TMatrix<T> TMatrix<T>::Add(const TMatrix<T>& b) const
{
if(mNr!=b.mNr || mNc!=b.mNc)
    throw(SzMismatchError("TMatrix operator C=A+B size mismatch\n"));
TMatrix<T> result; result.SetTemp(true); result.mNr=mNr; result.mNc=mNc;
result.mNDBlock = mNDBlock+b.mNDBlock;
return result;
}

template <class T> TMatrix<T> TMatrix<T>::Sub(const TMatrix<T>& b) const
{
if(mNr!=b.mNr || mNc!=b.mNc)
    throw(SzMismatchError("TMatrix operator C=A-B size mismatch\n"));
TMatrix<T> result; result.SetTemp(true); result.mNr=mNr; result.mNc=mNc;
result.mNDBlock = mNDBlock-b.mNDBlock;
return result;
}

template <class T> TMatrix<T> TMatrix<T>::Mul(const TMatrix<T>& b) const
// C = A(this)*B : Cij = Aik Bkj (allocation forcee dans tous les cas)
{
if(mNr==0 || mNc==0 || b.mNc==0 || mNc!=b.mNr)
  throw(SzMismatchError("TMatrix operator C=A*B size mismatch\n"));
TMatrix<T> r; r.SetTemp(true); r.ReSize(mNr,b.mNc);
T *ai,*aik,*bj,*bkj,*ri,*rij;
for(ri=const_cast<T *>(r.Data()),ai=const_cast<T *>(Data());
    ri<r.Data()+r.mNr*r.mNc;ri+=r.mNc,ai+=mNc) {
  for(rij=ri,bj=const_cast<T *>(b.Data());rij<ri+r.mNc;rij++,bj++) {
    *rij = 0;
    for(aik=ai,bkj=bj;aik<ai+mNc;aik++,bkj+=b.mNc) *rij += *aik * *bkj;
  }
}
return r;
}

////////////////////////////////////////////////////////////////
//**** Pour gestion des lignes et des colonnes

template <class T> TMatrixRC<T> TMatrix<T>::Row(uint_4 r) const
{
TMatrixRC<T> rc((TMatrix<T>&)*this, TmatrixRow, r);
return rc;
}

template <class T> TMatrixRC<T> TMatrix<T>::Col(uint_4 c) const
{
TMatrixRC<T> rc((TMatrix<T>&)*this, TmatrixCol, c);
return rc;
}

template <class T> TMatrixRC<T> TMatrix<T>::Diag() const
{
TMatrixRC<T> rc((TMatrix<T>&)*this, TmatrixDiag);
return rc;
}

////////////////////////////////////////////////////////////////
//**** Pour inversion
#ifndef M_LN2
#define M_LN2 0.69314718055994530942
#endif
#ifndef LN_MINDOUBLE
#define LN_MINDOUBLE  (M_LN2 * (DMINEXP - 1))
#endif
#ifndef LN_MAXDOUBLE
#define LN_MAXDOUBLE  (M_LN2 * DMAXEXP)
#endif

r_8 TMatrix<r_8>::GausPiv(TMatrix<r_8>& a, TMatrix<r_8>& b)
// Pivot de Gauss
// * Attention: egcs impose que cette fonction soit mise dans le .cc
//              avant ::Inverse() (car Inverse() l'utilise)
// {TMatrix A(a); TMatrix B(b); return (r_8) TMatrix::GausPiv(A,B);}
{
uint_4 n = a.NRows();
if(n!=b.NRows())
  throw(SzMismatchError("TMatrix::GausPiv size mismatch\n"));
// On fait une normalisation un peu brutale...
double vmin=MAXDOUBLE;
double vmax=0;
for(uint_4 iii=0; iii<a.NRows(); iii++)
  for(uint_4 jjj=0; jjj<a.NCols(); jjj++) {
    double v = TMatrixRC<r_8>::Abs_Value(a(iii,jjj));
    if(v>vmax) vmax = v;
    if(v<vmin && v>0) vmin = v;
}
double nrm = sqrt(vmin*vmax);
if(nrm > 1.e5 || nrm < 1.e-5) {
  a /= nrm;
  b /= nrm;
  //cout << "normalisation matrice " << nrm << endl;
} else nrm=1;

double det = 1.0;
if(nrm != 1) {
  double ld = a.NRows() * log(nrm);
  if (ld <= LN_MINDOUBLE || ld >= LN_MAXDOUBLE) {
   // cerr << "TMatrix warning, overflow for det" << endl;
  } else {
    det = exp(ld);
  }
}

TMatrixRC<r_8> pivRowa(a,TmatrixRow);
TMatrixRC<r_8> pivRowb(b,TmatrixRow);

for(uint_4 k=0; k<n-1; k++) {
  uint_4 iPiv = a.Col(k).IMaxAbs(k);
  if(iPiv != k) {
    TMatrixRC<r_8> aIPiv(a.Row(iPiv));
    TMatrixRC<r_8> aK(a.Row(k));
    TMatrixRC<r_8>::Swap(aIPiv,aK);
    TMatrixRC<r_8> bIPiv(b.Row(iPiv));
    TMatrixRC<r_8> bK(b.Row(k));
    TMatrixRC<r_8>::Swap(bIPiv,bK);
  }
  double pivot = a(k,k);
  if (fabs(pivot) < 1.e-50) return 0.0;
  //det *= pivot;
  pivRowa.SetRow(k); // to avoid constructors
  pivRowb.SetRow(k);
  for (uint_4 i=k+1; i<n; i++) {
    double r = -a(i,k)/pivot;
    a.Row(i).LinComb(r, pivRowa); // + rapide que -= r * pivRowa
    b.Row(i).LinComb(r, pivRowb);
  }
}
det *= a(n-1, n-1);

// on remonte
for(uint_4 kk=n-1; kk>0; kk--) {
  double pivot = a(kk,kk);
  if (fabs(pivot) <= 1.e-50) return 0.0;
  pivRowa.SetRow(kk); // to avoid constructors
  pivRowb.SetRow(kk);
  for(uint_4 jj=0; jj<kk; jj++) {
    double r = -a(jj,kk)/pivot;
    a.Row(jj).LinComb(r, pivRowa);
    b.Row(jj).LinComb(r, pivRowb);
  }
}

for(uint_4 l=0; l<n; l++) {
  if (fabs(a(l,l)) <= 1.e-50) return 0.0;
  b.Row(l) /= a(l,l);
}

return det;
}

TMatrix<r_8> TMatrix<r_8>::Inverse() const
// Inversion
{
TMatrix<r_8> b(mNc,mNr); TMatrix<r_8> a(*this); b = 1.;
if(fabs(TMatrix<r_8>::GausPiv(a,b)) < 1.e-50)
  throw(MathExc("TMatrix Inverse() Singular OMatrix"));
return b;
}


///////////////////////////////////////////////////////////
// --------------------------------------------------------
//   Les objets delegues pour la gestion de persistance 
// --------------------------------------------------------
///////////////////////////////////////////////////////////

template <class T>
FIO_TMatrix<T>::FIO_TMatrix()
{
dobj=new TMatrix<T>;
ownobj=true;
}

template <class T>
FIO_TMatrix<T>::FIO_TMatrix(string const & filename) 
{
dobj=new TMatrix<T>;
ownobj=true; 
Read(filename);
}

template <class T>
FIO_TMatrix<T>::FIO_TMatrix(const TMatrix<T> & obj) 
{ 
dobj = new TMatrix<T>(obj);
ownobj=true; 
}

template <class T>
FIO_TMatrix<T>::FIO_TMatrix(TMatrix<T> * obj) 
{ 
dobj = obj;
ownobj=false; 
}

template <class T>
FIO_TMatrix<T>::~FIO_TMatrix()
{
if (ownobj && dobj) delete dobj;
}

template <class T>
AnyDataObj* FIO_TMatrix<T>::DataObj()
{
return(dobj);
}

template <class T>
void FIO_TMatrix<T>::SetDataObj(AnyDataObj & o)
{
TMatrix<T> * po = dynamic_cast< TMatrix<T> * >(&o);
if (po == NULL) return;
if (ownobj && dobj) delete dobj;
dobj = po; ownobj = false;
} 

template <class T>
void FIO_TMatrix<T>::ReadSelf(PInPersist& is)
{
// On lit les 3 premiers uint_4
//  0: Numero de version,  1 : NRows,  2 : NCol
uint_4 itab[3];
is.Get(itab,3);
if (dobj == NULL) dobj = new TMatrix<T>(itab[1],itab[2]);
else dobj->ReSize(itab[1],itab[2]);
// On lit le NDataBlock
FIO_NDataBlock<T> fio_nd(&dobj->DataBlock());
fio_nd.Read(is);
}

template <class T>
void FIO_TMatrix<T>::WriteSelf(POutPersist& os) const
{
if (dobj == NULL)   return;
//  On ecrit 3 uint_4 .... 
//  0: Numero de version,  1 : NRows,  2 : NCol
uint_4 itab[3];
 itab[0] = 1;  // Numero de version a 1
itab[1] = dobj->NRows();
itab[2] = dobj->NCols();
os.Put(itab,3);
// On ecrit le NDataBlock
FIO_NDataBlock<T> fio_nd(&dobj->DataBlock());
fio_nd.Write(os);
}

////////////////////////////////////////////////////////////////
// -------------------------------------------------------------
//   La classe de gestion des lignes et colonnes d'une matrice
// -------------------------------------------------------------
////////////////////////////////////////////////////////////////
#include "tvector.h"

template <class T> TMatrixRC<T>::TMatrixRC()
: matrix(NULL), data(NULL), index(0), step(0)
{}

template <class T> TMatrixRC<T>::TMatrixRC(TMatrix<T>& m,TRCKind rckind,uint_4 ind)
: matrix(&m), data(Org(m,rckind,ind)),
  index(ind), step(Step(m,rckind)), kind(rckind)
{
if (kind == TmatrixDiag && m.mNc != m.mNr)
  throw(SzMismatchError("TMatrixRC::TMatrixRC(...,TmatrixDiag,...) size mismatch\n"));
}

template <class T> int_4 TMatrixRC<T>::Next()
{
if (!matrix || kind==TmatrixDiag) return -1;
index++;
if(kind == TmatrixRow) {
  if(index > (int_4)matrix->mNr) {index = (int_4)matrix->mNr; return -1;}
  data += matrix->mNc;
} else {
  if (index > (int_4)matrix->mNc) {index = (int_4)matrix->mNc; return -1;}
  data++;
}
return index;
}

template <class T> int_4 TMatrixRC<T>::Prev()
{
if (!matrix || kind == TmatrixDiag) return -1;
index--;
if(index < 0) {index = 0; return -1;}
if(kind == TmatrixRow) data -= matrix->mNc;
else data--;
return index;
}

template <class T> int_4 TMatrixRC<T>::SetCol(int_4 c)
{
if(!matrix) return -1;
if(c<0 || c>(int_4)matrix->mNc) return -1;
kind = TmatrixCol;
index = c;
step = Step(*matrix, TmatrixCol);
data = Org(*matrix, TmatrixCol, c);
return c;
}

template <class T> int_4 TMatrixRC<T>::SetRow(int_4 r)
{
if(!matrix) return -1;
if(r<0 && r>(int_4)matrix->mNr) return -1;
kind = TmatrixRow;
index = r;
step = Step(*matrix, TmatrixRow);
data = Org(*matrix, TmatrixRow, r);
return r;
}

template <class T> int_4 TMatrixRC<T>::SetDiag()
{
if (!matrix) return -1;
if (matrix->mNc != matrix->mNr)
  throw(SzMismatchError("TMatrixRC::SetDiag size mismatch\n"));
kind = TmatrixDiag;
index = 0;
step = Step(*matrix, TmatrixDiag);
data = Org(*matrix, TmatrixDiag);
return 0;
}


template <class T> TMatrixRC<T>& TMatrixRC<T>::operator = (const TMatrixRC<T>& rc)
{
matrix = rc.matrix;
data   = rc.data;
index  = rc.index;
step   = rc.step;
kind   = rc.kind;
return *this;
}

template <class T> TVector<T> TMatrixRC<T>::GetVect() const
{
TVector<T> v(NElts());
for (uint_4 i=0; i<NElts(); i++) v(i) = (*this)(i);
return v;
}

template <class T> TMatrixRC<T>& TMatrixRC<T>::operator += (const TMatrixRC<T>& rc)
{
if ( NElts() != rc.NElts() )
  throw(SzMismatchError("TMatrixRC::operator+= size mismatch\n"));
if ( kind != rc.kind )
  throw(SzMismatchError("TMatrixRC::operator+= type mismatch\n"));
for (uint_4 i=0; i<NElts(); i++) (*this)(i) += rc(i);
return *this;
}

template <class T> TMatrixRC<T>& TMatrixRC<T>::operator -= (const TMatrixRC<T>& rc)
{
if( NElts() != rc.NElts() )
  throw(SzMismatchError("TMatrixRC::operator-= size mismatch\n"));
if( kind != rc.kind )
  throw(SzMismatchError("TMatrixRC::operator-= type mismatch\n"));
for(uint_4 i=0; i<NElts(); i++) (*this)(i) -= rc(i);
return *this;
}


template <class T> TMatrixRC<T>& TMatrixRC<T>::operator *= (T x)
{
for(uint_4 i=0; i<NElts(); i++) (*this)(i) *= x;
return *this;
}

template <class T> TMatrixRC<T>& TMatrixRC<T>::operator /= (T x)
{
for(uint_4 i=0; i<NElts(); i++) (*this)(i) /= x;
return *this;
}


template <class T> TMatrixRC<T>& TMatrixRC<T>::operator -= (T x)
{
for(uint_4 i=0; i<NElts(); i++) (*this)(i) -= x;
return *this;
}

template <class T> TMatrixRC<T>& TMatrixRC<T>::operator += (T x)
{
for(uint_4 i=0; i<NElts(); i++) (*this)(i) += x;
return *this;
}

template <class T>
TMatrixRC<T>& TMatrixRC<T>::LinComb(T a, T b, const TMatrixRC<T>& rc, uint_4 first)
{
if ( NElts() != rc.NElts() )
  throw(SzMismatchError("TMatrixRC::LinComb size mismatch\n"));
if ( kind != rc.kind )
  throw(SzMismatchError("TMatrixRC::LinComb type mismatch\n"));
for(uint_4 i=first; i<NElts(); i++) (*this)(i) = (*this)(i)*a + rc(i)*b;
return *this;
}

template <class T>
TMatrixRC<T>& TMatrixRC<T>::LinComb(T b, const TMatrixRC<T>& rc, uint_4 first)
{
if ( NElts() != rc.NElts() )
  throw(SzMismatchError("TMatrixRC::LinComb size mismatch\n"));
if ( kind != rc.kind )
  throw(SzMismatchError("TMatrixRC::LinComb type mismatch\n"));
for(uint_4 i=first; i<NElts(); i++) (*this)(i) += rc(i)*b;
return *this;
}

template <class T> uint_4 TMatrixRC<T>::IMaxAbs(uint_4 first)
{
if (first>NElts())
  throw(SzMismatchError("TMatrixRC::IMaxAbs size mismatch\n"));
uint_4 imax = first;
double vmax = Abs_Value((*this)(first));
for(uint_4 i=first+1; i<NElts(); i++) {
  double v = Abs_Value((*this)(i));
  if(v > vmax) {vmax = v; imax = i;}
}
return imax;
}

template <class T>
void TMatrixRC<T>::Swap(TMatrixRC<T>& rc1, TMatrixRC<T>& rc2)
{
if(rc1.NElts() != rc2.NElts())
  throw(SzMismatchError("TMatrixRC::Swap size mismatch\n"));
if(rc1.kind != rc2.kind)
  throw(SzMismatchError("TMatrixRC::Swap type mismatch\n"));
if(rc1.data == rc2.data) return;
for(uint_4 i=0; i<rc1.NElts(); i++)
  {T tmp = rc1(i); rc1(i) = rc2(i); rc2(i) = tmp;}
}

///////////////////////////////////////////////////////////////
#ifdef __CXX_PRAGMA_TEMPLATES__
#pragma define_template TMatrix<uint_1>
#pragma define_template TMatrix<uint_2>
#pragma define_template TMatrix<int_2>
#pragma define_template TMatrix<int_4>
#pragma define_template TMatrix<int_8>
#pragma define_template TMatrix<uint_4>
#pragma define_template TMatrix<uint_8>
#pragma define_template TMatrix<r_4>
#pragma define_template TMatrix<r_8>
#pragma define_template TMatrix< complex<r_4> > 
#pragma define_template TMatrix< complex<r_8> > 
// Instances des delegues FileIO (PPersist)
#pragma define_template FIO_TMatrix<uint_1>
#pragma define_template FIO_TMatrix<uint_2>
#pragma define_template FIO_TMatrix<int_2>
#pragma define_template FIO_TMatrix<int_4>
#pragma define_template FIO_TMatrix<int_8>
#pragma define_template FIO_TMatrix<uint_4>
#pragma define_template FIO_TMatrix<uint_8>
#pragma define_template FIO_TMatrix<r_8>
#pragma define_template FIO_TMatrix<r_4>
#pragma define_template FIO_TMatrix< complex<r_4> >
#pragma define_template FIO_TMatrix< complex<r_8> >
// Instances gestion lignes/colonnes
#pragma define_template TMatrixRC<uint_1>
#pragma define_template TMatrixRC<uint_2>
#pragma define_template TMatrixRC<int_2>
#pragma define_template TMatrixRC<int_4>
#pragma define_template TMatrixRC<int_8>
#pragma define_template TMatrixRC<uint_4>
#pragma define_template TMatrixRC<uint_8>
#pragma define_template TMatrixRC<r_4>
#pragma define_template TMatrixRC<r_8>
#pragma define_template TMatrixRC< complex<r_4> > 
#pragma define_template TMatrixRC< complex<r_8> >
#endif

#if defined(ANSI_TEMPLATES) || defined(GNU_TEMPLATES)
template class TMatrix<uint_1>;
template class TMatrix<uint_2>;
template class TMatrix<int_2>;
template class TMatrix<int_4>;
template class TMatrix<int_8>;
template class TMatrix<uint_4>;
template class TMatrix<uint_8>;
template class TMatrix<r_4>;
template class TMatrix<r_8>;
template class TMatrix< complex<r_4> >;
template class TMatrix< complex<r_8> >;
// Instances des delegues FileIO (PPersist)
template class FIO_TMatrix<uint_1>;
template class FIO_TMatrix<uint_2>;
template class FIO_TMatrix<int_2>;
template class FIO_TMatrix<int_4>;
template class FIO_TMatrix<int_8>;
template class FIO_TMatrix<uint_4>;
template class FIO_TMatrix<uint_8>;
template class FIO_TMatrix<r_8>;
template class FIO_TMatrix<r_4>;
template class FIO_TMatrix< complex<r_4> >;
template class FIO_TMatrix< complex<r_8> >;
// Instances gestion lignes/colonnes
template class TMatrixRC<uint_1>;
template class TMatrixRC<uint_2>;
template class TMatrixRC<int_2>;
template class TMatrixRC<int_4>;
template class TMatrixRC<int_8>;
template class TMatrixRC<uint_4>;
template class TMatrixRC<uint_8>;
template class TMatrixRC<r_4>;
template class TMatrixRC<r_8>;
template class TMatrixRC< complex<r_4> >;
template class TMatrixRC< complex<r_8> >;
#endif