source: Sophya/trunk/SophyaLib/TArray/sopemtx.cc@ 804

//                         C.Magneville, R.Ansari        03/2000

#include "machdefs.h"
#include <stdio.h>
#include <iostream.h>
#include <complex>
#include <math.h>
#include "sopemtx.h"
#include "smathconst.h"

////////////////////////////////////////////////////////////////
// -------------------------------------------------------------
//   La classe de gestion des lignes et colonnes d'une matrice
// -------------------------------------------------------------
////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////
// Classe de lignes/colonnes de matrices
enum TRCKind {TmatrixRow=0, TmatrixCol=1, TmatrixDiag=2};
template <class T>
class TMatrixRC {
public:
  TMatrixRC();
  TMatrixRC(TMatrix<T>& m, TRCKind kind, uint_4 index=0);
  virtual ~TMatrixRC() {}

  // Acces aux rangees et colonnes de matrices
  static TMatrixRC<T> Row(TMatrix<T> & m, uint_4 r);
  static TMatrixRC<T> Col(TMatrix<T> & m, uint_4 c);
  static TMatrixRC<T> Diag(TMatrix<T> & m);

  // ---- A virer   $CHECK$ Reza 03/2000
  //  int_4 Next();
  //  int_4 Prev();
  int_4 SetCol(int_4 c);
  int_4 SetRow(int_4 r);
  int_4 SetDiag();

  static uint_4 Step(const TMatrix<T>& m, TRCKind rckind);
  static T* Org(const TMatrix<T>&, TRCKind rckind, uint_4 ind=0);

  TRCKind Kind() const { return kind; }
  uint_4 NElts() const;
  T& operator()(uint_4 i);
  T  operator()(uint_4 i) const;


   TMatrixRC<T>& operator = (const TMatrixRC<T>& rc);

// ---- A virer   $CHECK$ Reza 03/2000
//   TVector<T> GetVect() const;
//   TMatrixRC<T>& operator += (const TMatrixRC<T>& rc);
//   TMatrixRC<T>& operator -= (const TMatrixRC<T>& rc);

  TMatrixRC<T>& operator *= (T x);
  TMatrixRC<T>& operator /= (T x);

//   TMatrixRC<T>& operator -= (T x);
//   TMatrixRC<T>& operator += (T x);
  

  TMatrixRC<T>& LinComb(T a, T b, const TMatrixRC& rc, uint_4 first=0);
  TMatrixRC<T>& LinComb(T b, const TMatrixRC<T>& rc, uint_4 first=0);

  uint_4 IMaxAbs(uint_4 first=0);

  static void Swap(TMatrixRC<T>& rc1, TMatrixRC<T>& rc2);

  inline static double Abs_Value(uint_1 v) {return (double) v;}
  inline static double Abs_Value(uint_2 v) {return (double) v;}
  inline static double Abs_Value(int_2 v)  {return (v>0)? (double) v: (double) -v;}
  inline static double Abs_Value(int_4 v)  {return (v>0)? (double) v: (double) -v;}
  inline static double Abs_Value(int_8 v)  {return (v>0)? (double) v: (double) -v;}
  inline static double Abs_Value(uint_4 v) {return (double) v;}
  inline static double Abs_Value(uint_8 v) {return (double) v;}
  inline static double Abs_Value(r_4 v)    {return (double) fabsf(v);}
  inline static double Abs_Value(r_8 v)    {return fabs(v);}
  inline static double Abs_Value(complex<float> v)
                {return sqrt(v.real()*v.real()+v.imag()*v.imag());}
  inline static double Abs_Value(complex<double> v)
                {return sqrt(v.real()*v.real()+v.imag()*v.imag());}

protected:
  TMatrix<T>* matrix;
  T*          data;
  int_4       index;
  uint_4      step;
  TRCKind     kind;
};



template <class T>
inline T operator * (const TMatrixRC<T>& a, const TMatrixRC<T>& b)
  {
  if ( a.NElts() != b.NElts() )
    throw(SzMismatchError("TMatrixRC::operator * size mismatch\n"));
  if ( a.Kind() != b.Kind() )
    throw(SzMismatchError("TMatrixRC::operator * type mismatch\n"));
  T sum = 0;
  for(uint_4 i=0; i<a.NElts(); i++) sum += a(i)*b(i);
  return sum;
  }


template <class T>
inline uint_4 TMatrixRC<T>::Step(const TMatrix<T>& m, TRCKind rckind)
  { switch (rckind) { case TmatrixRow  : return m.Step(m.RowsKA());
                      case TmatrixCol  : return m.Step(m.ColsKA());
                      case TmatrixDiag : return (m.Step(m.RowsKA())+m.Step(m.ColsKA())); }
    return 0; }

template <class T>
inline T* TMatrixRC<T>::Org(const TMatrix<T>& m, TRCKind rckind, uint_4 index)
  { switch (rckind) { case TmatrixRow  : return const_cast<T *>(&(m(index,0)));
                      case TmatrixCol  : return const_cast<T *>(&(m(0,index)));
                      case TmatrixDiag : return const_cast<T *>(m.Data()); }
    return NULL; }

template <class T> inline uint_4 TMatrixRC<T>::NElts() const
  { if (!matrix) return 0;
    switch (kind) { case TmatrixRow  : return matrix->NCols();
                    case TmatrixCol  : return matrix->NRows();
                    case TmatrixDiag : return matrix->NCols(); }
    return 0; }

template <class T>
inline T& TMatrixRC<T>::operator()(uint_4 i) {return data[i*step];}
template <class T>
inline T  TMatrixRC<T>::operator()(uint_4 i) const {return data[i*step];}

////////////////////////////////////////////////////////////////
// Typedef pour simplifier et compatibilite Peida
typedef TMatrixRC<r_8> MatrixRC;


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.NCols() != m.NRows())
  throw(SzMismatchError("TMatrixRC::TMatrixRC(...,TmatrixDiag,...) size mismatch\n"));
}

////////////////////////////////////////////////////////////////
// Acces aux rangees et colonnes de matrices

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

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

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


// ---- A virer   $CHECK$ Reza 03/2000
// template <class T> int_4 TMatrixRC<T>::Next()
// {
// if (!matrix || kind==TmatrixDiag) return -1;
// index++;
// if(kind == TmatrixRow) {
//   if(index > (int_4)matrix->NRows()) {index = (int_4)matrix->NRows(); return -1;}
//   data += matrix->NCols();
// } else {
//   if (index > (int_4)matrix->NCols()) {index = (int_4)matrix->NCols(); 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->NCols();
// 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->NCols()) 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->NRows()) 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->NCols() != matrix->NRows())
  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;
}

// ---- A virer   $CHECK$ Reza 03/2000
// 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;
}


// ---- A virer   $CHECK$ Reza 03/2000
// 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;}
}




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

template <class T>
T SimpleMatrixOperation<T>::GausPiv(TMatrix<T>& a, TMatrix<T>& 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 (T) 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<T>::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<T> pivRowa(a,TmatrixRow);
TMatrixRC<T> pivRowb(b,TmatrixRow);

for(uint_4 k=0; k<n-1; k++) {
  uint_4 iPiv = TMatrixRC<T>::Col(a, k).IMaxAbs(k);
  if(iPiv != k) {
    TMatrixRC<T> aIPiv(TMatrixRC<T>::Row(a,iPiv));
    TMatrixRC<T> aK(TMatrixRC<T>::Row(a, k));
    TMatrixRC<T>::Swap(aIPiv,aK);
    TMatrixRC<T> bIPiv(TMatrixRC<T>::Row(b, iPiv));
    TMatrixRC<T> bK(TMatrixRC<T>::Row(b, k));
    TMatrixRC<T>::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;
    TMatrixRC<T>::Row(a, i).LinComb(r, pivRowa); // + rapide que -= r * pivRowa
    TMatrixRC<T>::Row(b, 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;
    TMatrixRC<T>::Row(a, jj).LinComb(r, pivRowa);
    TMatrixRC<T>::Row(b, jj).LinComb(r, pivRowb);
  }
}

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

return det;
}

template <class T>
TMatrix<T> SimpleMatrixOperation<T>::Inverse(TMatrix<T> const & A) 
// Inversion
{
TMatrix<T> a(A);
TMatrix<T> b(a.NCols(),a.NRows());  b = 1.;
if(fabs(GausPiv(a,b)) < 1.e-50)
  throw(MathExc("TMatrix Inverse() Singular OMatrix"));
return b;
}


LinFitter::LinFitter()
{
}

LinFitter::~LinFitter()
{
}

double LinFitter::LinFit(const Vector& x, const Vector& y, int nf, 
                         double (*f)(int, double), Vector& c)
{
  int n = x.NElts();
  if (n != y.NElts()) THROW(sizeMismatchErr);
  
  Matrix fx(nf, n);
  for (int i=0; i<nf; i++)
    for (int j=0; j<n; j++)
      fx(i,j) = f(i,x(j));

  return LinFit(fx,y,c);
}



double LinFitter::LinFit(const Matrix& fx, const Vector& y, Vector& c)
{
  int n = y.NElts();
  if ( n != fx.NCol()) THROW(sizeMismatchErr);

  int nf = fx.NRows();

  Matrix a(nf,nf);

  for (int j=0; j<nf; j++)
    for (int k=j; k<nf; k++)
      a(j,k) = a(k,j) = TMatrixRC<r_8>::Row(const_cast<Matrix &>(fx), j) 

        * TMatrixRC<r_8>::Row(const_cast<Matrix &>(fx), k); /* $CHECK$ Reza 10/3/2000 */
   
  c = fx * y;

  if (SimpleMatrixOperation<r_8>::GausPiv(a,c) == 0) THROW(singMatxErr); /* $CHECK$ Reza 10/3/2000 */

  double xi2 = 0;

  for (int k=0; k<n; k++) {
    double x = 0;
    for (int i=0; i<nf; i++)
      x += c(i) * fx(i,k);
    x -= y(k);
    xi2 += x*x;
  }
  return xi2;
}



double LinFitter::LinFit(const Vector& x, const Vector& y, const Vector& errY2, int nf,
                         double (*f)(int, double), Vector& c, Vector& errC)
{
  int n = x.NElts();
  if (n != y.NElts()) THROW(sizeMismatchErr);
  
  Matrix fx(nf, n);
  for (int i=0; i<nf; i++)
    for (int j=0; j<n; j++)
      fx(i,j) = f(i,x(j));

  return LinFit(fx,y,errY2,c,errC);
}


double LinFitter::LinFit(const Matrix& fx, const Vector& y, const Vector& errY2,
           Vector& c, Vector& errC)
{
  int n = y.NElts();
  if ( n != errY2.NElts()) THROW(sizeMismatchErr);
  if ( n != fx.NCol()) THROW(sizeMismatchErr);

  int nf = fx.NRows();

  Matrix a(nf,nf);

  c.Realloc(nf);
  errC.Realloc(nf);

  for (int j=0; j<nf; j++)
    for (int k=j; k<nf; k++) {
      double x=0;
      for (int l=0; l<n; l++)
        x += fx(j,l) * fx(k,l) / errY2(l);        // Matrice a inverser
      a(j,k) = a(k,j) = x;
    }
 
  Matrix d(nf,nf+1);
  for (int k=0; k<nf; k++) {
    double x=0;
    for (int l=0; l<n; l++)
      x += fx(k,l) * y(l) / errY2(l);             // Second membre 1ere colonne
    d(k,0) = x;
    for (int m=1; m<=nf; m++)
      d(k,m) = (k==m) ? 1 : 0;                // Reste second membre = Id.
  }

  if (SimpleMatrixOperation<r_8>::GausPiv(a,d) == 0) THROW(singMatxErr); /* $CHECK$ Reza 10/3/2000 */

  for (int l=0; l<nf; l++) {
    c(l) = d(l,0);                            // Parametres = 1ere colonne
    errC(l) = d(l,l+1);                        // Erreurs = diag inverse.
  }

  double xi2 = 0;

  for (int jj=0; jj<n; jj++) {
    double x = 0;
    for (int ii=0; ii<nf; ii++)
      x += c(ii) * fx(ii,jj);
    x -= y(jj);
    xi2 += x*x/errY2(jj);
  }
  return xi2;
}




///////////////////////////////////////////////////////////////
#ifdef __CXX_PRAGMA_TEMPLATES__
// Instances gestion lignes/colonnes
#pragma define_template TMatrixRC<int_4>
#pragma define_template TMatrixRC<r_4>
#pragma define_template TMatrixRC<r_8>
#pragma define_template SimpleMatrixOperation<int_4>
#pragma define_template SimpleMatrixOperation<r_4>
#pragma define_template SimpleMatrixOperation<r_8>
#endif

#if defined(ANSI_TEMPLATES) || defined(GNU_TEMPLATES)
// Instances gestion lignes/colonnes
template class TMatrixRC<int_4>;
template class TMatrixRC<r_4>;
template class TMatrixRC<r_8>;
template class SimpleMatrixOperation<int_4>;
template class SimpleMatrixOperation<r_4>;
template class SimpleMatrixOperation<r_8>;
#endif