flyopunta/src/src_uti/gmatrix.cpp

/***************************************************************************
                          GPmrg->h  -  description
                             -------------------
    begin                : Febbraio 2006
    copyright            : (C) 2000 by Giuseppe M Pierazzini
    email                : pierazzini@unipi.it
 ***************************************************************************/
//----------------------   vector -----------------------------
#define GVECTOR
#include "parm.h"
#include "gmatrix.h"
#include "evento.h"
#include "gvet.h"

using namespace std;


//--------------------  c o s t r u t t o r i
// costruttore


gmatrix::gmatrix()
{
        // matrice vuota con dimensioni nulle
        Row =Col=Elem=0;
        ValM= 0;
        detrm=0.0;
        okflag=0;

}




gmatrix::gmatrix ( int r, int c )
{
// it generates an empty matrix of rXc dimension 
        Row = r;
        Col = c;
        Elem=Row*Col;
        ValM = new double *  [Row];
        for ( int i=0;i<Row;i++ )
                ValM[i]=new double [Col];
        Reset();

}

gmatrix::gmatrix ( int r, double aa)
{
 // it generates an empty matrix of rXr dimension with the diagonal terms  filled with aa
       
        Row = r;
        Col = r;
        Elem=Row*Col;
        ValM = new double *  [Row];
        for ( int i=0;i<Row;i++ )
                ValM[i]=new double [Col];
        Reset();
        for ( int i=0;i<Row;i++ )ValM[i][i]=aa;
        Det();
}




gmatrix::gmatrix ( const gmatrix & m )
{
// generate a new matrix equal to m
        Row = m.Row;
        Col = m.Col;
        Elem=Row*Col;
        ValM = new double *  [Row];
        for ( int i=0;i<Row;i++ )
                ValM[i]=new double [Col];
        for ( int i=0;i<Row;i++ )
                for ( int j=0;j<Col;j++ )
                        ValM[i][j]=m.ValM[i][j];
        detrm=m.detrm;
        okflag=0;

}

gmatrix::gmatrix ( const gvet & v )
{
// genertes a matrix from the vector (gvet)  v    
   Elem=Row=Col=0;
  *this=v;  // see the "=" operator
}


inline void gmatrix::Clone ( const gmatrix & m )
{
        // matrice copia di m ; Clone
        Row = m.Row;
        Col = m.Col;
        Elem=Row*Col;
        ValM = new double *  [Row];
        for ( int i=0;i<Row;i++ )
                ValM[i]=new double [Col];
        for ( int i=0;i<Row;i++ )
                for ( int j=0;j<Col;j++ )
                        ValM[i][j]=m.ValM[i][j];
        detrm=m.detrm;
  okflag=0;
}
gmatrix::~gmatrix()
{
        for ( int i=0; i < Row; i++ ) delete [] ValM[i];
        delete [] ValM;
}
//===================================

gmatrix   gmatrix::Proj(const gvet &v)
{ // definisce in this il  tensore (proiettore  |v><v| ) dal vettore gvet v;

   if ( Row != 3 || Col != 3 )
                REPORT_ERROR ( 0,"gmatrix::operator+= : Inconsistent matrix sizes in addition!" ); 
   gmatrix gv(v);
   *this= gv*(~gv);
   return *this;
}


gmatrix gmatrix::operator= ( const gmatrix& m )
{   // copies a matrix m into this
        if ( Elem==0 )  //Matrice vuota...appena nata
        {
                Row = m.Row;
                Col = m.Col;
                Elem=Row*Col;
                ValM = new double *  [Row];
                for ( int i=0;i<Row;i++ )
                        ValM[i]=new double [Col];
        }
        else if ( Row != m.Row || Col != m.Col )
                REPORT_ERROR ( 0,"gmatrix::operator = : Inconsistent matrix sizes !" );
        for ( int i=0;i<Row;i++ )
                for ( int j=0;j<Col;j++ )
                        ValM[i][j]=m.ValM[i][j];
        detrm=m.detrm;
  okflag=m.okflag;
        return *this;
}

//   gmatrix da Gvet 

gmatrix gmatrix::operator= ( const gvet & m )
{
        if ( Elem==0 )  //Matrice vuota...appena nata
        {
                Row = 3;
                Col = 1;
                Elem=3;
                ValM = new double *  [Row];
                for ( int i=0;i<Row;i++ )
                        ValM[i]=new double [Col];
        }
        else if ( Row != 3 || Col !=1 )
                REPORT_ERROR ( 0,"gmatrix::operator = : Inconsistent matrix sizes !" );
        ValM[0][0]=m.x;ValM[1][0]=m.y;ValM[2][0]=m.z;
        detrm=0.0;
  okflag=0;
        return *this;
}


 gvet   gmatrix::operator % (const gvet &v) 
 { // moltiplica un vettore per una matrice(3,3)
   // restituyisce un vettore gvet
   gmatrix Vg(v);
   gvet vout;
   Vg = (*this*Vg);
   vout=Vg;
   return vout;
   }




//=====================================================
// annulla la matrice eliminando  gli elementi! OK
void gmatrix::Null()
{
for ( int i=0; i < Row; i++ )  delete [] ValM[i];
delete [] ValM;
Row =Col=Elem=0;
ValM=0;
detrm=0.0;
okflag=0;

}
void gmatrix::Set_Size ( int r, int c )
{
        if ( Row==r && Col==c )
                {Reset(); return;} // nothing to do
        for ( int i=0; i < Row; i++ )  delete [] ValM[i];
        delete [] ValM;
        Row = r;
        Col = c;
        Elem=Row*Col;
        ValM = new double *  [Row];
        for ( int i=0;i<Row;i++ )
                ValM[i]=new double [Col];
        Reset();
}




//printout OK
void gmatrix::Print ( const char * title )
{

        Gout<< " \n\nPrint < "<<title<<" >  di "<<Elem <<"  elementi e  dim:"
        <<" ( "<<Row<<"X"<<Col<<" ) Det "<<detrm<<endl;

        for ( int  i=0; i < Row; i++ )
        {
                for ( int  j=0; j < Col; j++ )
                {
      Gout.setf(ios::scientific);
                        Gout<<setw ( 11 ) <<setprecision ( 5 ) <<ValM[i][j]<< '\t';
                }
                Gout<<endl;
        }
  

        Gout<<"=== e n d  < "<<title<<" >\n "<<endl;

}



// Unary operators : cambio segno OK
gmatrix  gmatrix::operator - ()
{
        gmatrix temp ( Row,Col );
        for ( int i=0;i<Row;i++ )
                for ( int j=0;j<Col;j++ )
                        temp.ValM[i][j]=-ValM[i][j];
        return temp;
}


// unary transpose operator OK
gmatrix gmatrix::operator ~ ()
{
        gmatrix temp ( Col,Row );
        for ( int i=0; i < Col; i++ )
        {
                for ( int j=0; j < Row; j++ )
                {
                        temp ( i,j ) =ValM[j][i];
                }
                temp.detrm=detrm;
        }
        return temp;
}



// combined addition and assignment operator  OK
gmatrix & gmatrix ::operator+= ( const gmatrix & m )
{
        if ( Row != m.Row || Col != m.Col )
                REPORT_ERROR ( 0,"gmatrix::operator+= : Inconsistent matrix sizes in addition!" );
        for ( int i=0;i<Row;i++ )
                for ( int j=0;j<Col;j++ )
                        ValM[i][j]+=m.ValM[i][j];
        detrm=0.0;
        return *this;
}


// combined subtraction and assignment operator   OK
gmatrix & gmatrix ::operator-= ( const gmatrix & m )
{
        if ( Row != m.Row || Col != m.Col )
        {
                Gout<<"\n Dim: "<<Row<<"X"<<Col<<" "<<m.Row<<"X"<<m.Col<<endl;
                REPORT_ERROR ( 0,"gmatrix::operator-= : Inconsistent matrix sizes in subctration!" );
        }

        for ( int i=0;i<Row;i++ )
                for ( int j=0;j<Col;j++ )
                        ValM[i][j]-=m.ValM[i][j];
        detrm=0.0;

        return *this;
}




// combined product  and assignment operator for a double.OK
gmatrix& gmatrix::operator*= ( const double & a )
{

        for ( int i=0;i<Row;i++ )
        {
                detrm*=a;
                for ( int j=0;j<Col;j++ )
                        ValM[i][j]*=a;
        }
        return *this;
}

// combined matrix multiplication and assignment operator OK
gmatrix& gmatrix::operator*= ( const gmatrix& m )
{
        if ( Col != m.Row )
        {
                Gout<<"\n Dim: "<<Row<<"X"<<Col<<" "<<m.Row<<"X"<<m.Col<<endl;
                REPORT_ERROR ( 0, "gmatrix::operator*= : Inconsistent matrix sizes in multiplication!" );
        }

        gmatrix temp ( Row,m.Col );

        for ( int i=0; i < Row; i++ )
                for ( int j=0; j < m.Col; j++ )
                {
                        temp.ValM[i][j] = 0.0;
                        for ( int k=0; k < Col; k++ )
                                temp.ValM[i][j] +=ValM[i][k] * m.ValM[k][j];
                }
        temp.detrm= detrm*m.detrm;
        *this = temp;
        return *this;
}

// binary matrix multiplication operator  OK
gmatrix  gmatrix::operator * ( const gmatrix& m )
{
        if ( Col != m.Row )
        {
                Gout<<"\n Dim: "<<Row<<"X"<<Col<<" "<<m.Row<<"X"<<m.Col<<endl;
                REPORT_ERROR ( 0, "gmatrix::operator* : Inconsistent matrix sizes!" );

        }
        gmatrix temp ( Row,m.Col );

        for ( int i=0; i < Row; i++ )
                for ( int j=0; j < m.Col; j++ )
                {
                        temp.ValM[i][j] = 0.0;
                        for ( int k=0; k < Col; k++ )
                                temp.ValM[i][j] +=ValM[i][k] * m.ValM[k][j];
                }
        temp.detrm= detrm*m.detrm;
        return temp;
}
// prodotto con un vettore gvet 3x3 e stituisce un vettore



// binary addition operator   OK
gmatrix gmatrix::operator + ( const gmatrix& m )
{
        gmatrix temp = *this;
        temp += m;
        return temp;
}

// binary subctration operator  OK
gmatrix gmatrix::operator - ( const gmatrix& m )
{
        gmatrix temp =*this;
        temp -= m;
        return temp;
}

// binary scalar multiplication operator OK
gmatrix gmatrix::operator * ( const double &a )
{
        gmatrix temp = *this;
        temp *= a;
        return temp;
}

// combined power and assignment operator OK
gmatrix& gmatrix::operator ^= ( const int& pow )
{
        gmatrix temp ( *this );
        for ( int i=2; i <= pow; i++ )
                *this = ( *this * temp );
        return *this;
}






// unary inversion operator   OK
gmatrix gmatrix::operator ! ()
{
        gmatrix temp = *this;

        return temp.Inv();
}



gmatrix gmatrix::Inv ()
{
        // attenzione cambia this...
        int i,j,k;
        double a1,a2,*rowptr;
        okflag=1;
        if ( Row != Col )
                REPORT_ERROR ( 0, "gmatrix::operator!: Inversion of a non-square matrix" );
        gmatrix temp ( Row,Col );
        temp.Unit();
        for ( k=0; k < Row; k++ )
        {
                int indx = pivot ( k );
                if ( indx == -1 )
                {
                        REPORT_ERROR ( 2,"gmatrix::Inversion of singular matrix!" );
      temp.Unit(); temp.okflag=-1;
      return temp;
                }

                if ( indx != 0 )
                {
                        rowptr = temp.ValM[k];
                        temp.ValM[k] = temp.ValM[indx];
                        temp.ValM[indx] = rowptr;
                }
                a1 =ValM[k][k];
                for ( j=0; j < Row; j++ )
                {
                        ValM[k][j] /= a1;
                        temp.ValM[k][j] /= a1;
                }
                for ( i=0; i < Row; i++ )
                        if ( i != k )
                        {
                                a2 =ValM[i][k];
                                for ( j=0; j < Row; j++ )
                                {
                                        ValM[i][j] -= a2 *ValM[k][j];
                                        temp.ValM[i][j] -= a2 * temp.ValM[k][j];
                                }
                        }
        }
        if ( detrm!=0.0 ) temp.detrm=1./detrm; else temp.detrm=0.0;
        return temp;
}


// private partial pivoting method OK
int gmatrix::pivot ( int  row )
{
        int k = int ( row );
        double amax,temp;
        amax = -1.;
        for ( int  i=row; i < Row; i++ )
                if ( ( temp = abs ( ValM[i][row] ) ) > amax && temp != 0.0 )
                {
                        amax = temp;
                        k = i;
                }

        if ( ValM[k][row] == 0.0 )
                return -1;
        if ( k != int ( row ) )
        {
                double* rowptr =ValM[k];
                ValM[k] =ValM[row];
                ValM[row] = rowptr;
                return k;
        }
        return 0;
}

// calculate the determinant of a matrix
double gmatrix::Det ()
{
        int  i,j,k;
        double piv,detVal = 1.0;
        if ( Row != Col )
                REPORT_ERROR ( 0, "gmatrix::Det(): Determinant a non-square matrix!" );

        gmatrix temp ( *this );
        for ( k=0; k < Row; k++ )
        {
                int indx = temp.pivot ( k );
                if ( indx == -1 )
                        return 0;
                if ( indx != 0 )
                        detVal = - detVal;
                detVal = temp.ValM[k][k]*detVal ;
                for ( i=k+1; i < Row; i++ )
                {
                        piv = temp.ValM[i][k] / temp.ValM[k][k];
                        for ( j=k+1; j < Row; j++ )
                                temp.ValM[i][j] -= piv * temp.ValM[k][j];
                }
        }
        detrm=detVal;
        return detVal;
}


// solve simultaneous equation  OK
gmatrix gmatrix::Solve ( const gmatrix& v ) const
{
        int i,j,k;
        double a1;

        if ( ! ( Row == Col && Col == v.Row ) )
                REPORT_ERROR ( 0, "gmatrix::Solve():Inconsistent matrices!" );

        gmatrix temp ( Row,Col+v.Col );
        for ( i=0; i < Row; i++ )
        {
                for ( j=0; j < Col; j++ )
                        temp.ValM[i][j] =ValM[i][j];
                for ( k=0; k < v.Col; k++ )
                        temp.ValM[i][Col+k] = v.ValM[i][k];
        }
        for ( k=0; k < Row; k++ )
        {
                int indx = temp.pivot ( k );
                if ( indx == -1 )
                        REPORT_ERROR ( 0, "gmatrix::Solve(): Singular matrix!" );

                a1 = temp.ValM[k][k];
                for ( j=k; j < temp.Col; j++ )
                        temp.ValM[k][j] /= a1;

                for ( i=k+1; i < Row; i++ )
                {
                        a1 = temp.ValM[i][k];
                        for ( j=k; j < temp.Col; j++ )
                                temp.ValM[i][j] -= a1 * temp.ValM[k][j];
                }
        }
        gmatrix s ( v.Row,v.Col );
        for ( k=0; k < v.Col; k++ )
                for ( int m=int ( Row )-1; m >= 0; m-- )
                {
                        s.ValM[m][k] = temp.ValM[m][Col+k];
                        for ( j=m+1; j < Col; j++ )
                                s.ValM[m][k] -= temp.ValM[m][j] * s.ValM[j][k];
                }
        return s;
}
// ================================


 gvet & gmatrix::Get_Vcol(gvet &V)
 {  // restituisce il vettore V definito come la colonna 0 della matrice this.
 if ( Row < 3 )REPORT_ERROR ( 0, "gmatrix::Get_Vcol: Less than 3 rows!" );
  V.x= ValM[0][0];
  V.y= ValM[1][0];
  V.z= ValM[2][0];
 return V; 
 }
//===================================================================
// set this matrix to unity
void gmatrix::Unit ()
{
        if ( Row != Col )
                REPORT_ERROR ( 0, "gmatrix::Unit: Non-square matrix!" );

        for ( int i=0; i < Row; i++ )
                for ( int j=0; j < Col; j++ )
                        ValM[i][j] = i == j ? 1.0 : 0.0;
                detrm=1.;
        return;
}

// calculate the norm of a matrix
inline double gmatrix::Norm ()
{
        double retVal = 0.0;

        for ( int  i=0; i < Row; i++ )
                for ( int  j=0; j < Col; j++ )
                        retVal +=ValM[i][j] *ValM[i][j];
        retVal = sqrt ( retVal );

        return retVal;
}
//    private.............
// calculate the condition number of a matrix
inline double gmatrix::Cond ()
{
        gmatrix inv = ! ( *this );
        return ( Norm() * inv.Norm() );
}

// Determine if the matrix is upper triangular
inline bool gmatrix::IsUpperTriangular ()
{
        if ( Row != Col )
                return false;
        for ( int  i=1; i < Row; i++ )
                for ( int  j=0; j < i-1; j++ )
                        if ( ValM[i][j] != 0.0 )
                                return false;
        return true;
}

// Determine if the matrix is lower triangular
inline bool gmatrix::IsLowerTriangular ()
{
        if ( Row != Col )
                return false;

        for ( int  j=1; j < Col; j++ )
                for ( int  i=0; i < j-1; i++ )
                        if ( ValM[i][j] != 0.0 )
                                return false;

        return true;
}

// Determine if the matrix is symmetric
inline bool gmatrix::IsSymmetric ()
{
        if ( Row != Col )
                return false;
        for ( int i=0; i < Row; i++ )
                for ( int j=0; j < Col; j++ )
                        if ( ValM[i][j] !=ValM[j][i] )
                                return false;
        return true;
}

// Determine if the matrix is skew-symmetric
inline bool gmatrix::IsSkewSymmetric ()
{
        if ( Row != Col )
                return false;
        for ( int i=0; i < Row; i++ )
                for ( int j=0; j < Col; j++ )
                        if ( ValM[i][j] != -ValM[j][i] )
                                return false;
        return true;
}
// Determine if the matrix is a unit matrix
inline bool gmatrix::IsUnit ()
{
        if ( IsScalar() &&ValM[0][0] == 1.0 )
                return true;
        return false;
}

// Determine if this is a null matrix
inline bool gmatrix::IsNull ()
{
        for ( int i=0; i < Row; i++ )
                for ( int j=0; j < Col; j++ )
                        if ( ValM[i][j] != 0.0 )
                                return false;
        return true;
}

// calculate adjoin of a matrix
inline gmatrix gmatrix::Adj ()
{
        if ( Row != Col )
                REPORT_ERROR ( 0, "gmatrix::Adj(): Adjoin of a non-square matrix." );

        gmatrix temp ( Row,Col );

        for ( int i=0; i < Row; i++ )
                for ( int j=0; j < Col; j++ )
                        temp.ValM[j][i] = Cofact ( i,j );
        return temp;
}

// calculate the cofactor of a matrix for a given element
inline double gmatrix::Cofact ( int row, int col )
{
        int i,i1,j,j1;

        if ( Row != Col )
                REPORT_ERROR ( 0, "gmatrix::Cofact(): Cofactor of a non-square matrix!" );

        if ( row > Row || col > Col )
                REPORT_ERROR ( 0, "gmatrix::Cofact(): Index out of range!" );

        gmatrix temp ( Row-1,Col-1 );

        for ( i=i1=0; i < Row; i++ )
        {
                if ( i == row )
                        continue;
                for ( j=j1=0; j < Col; j++ )
                {
                        if ( j == col )
                                continue;
                        temp.ValM[i1][j1] =ValM[i][j];
                        j1++;
                }
                i1++;
        }
        double   cof = temp.Det();
        if ( ( row+col ) %2 == 1 )
                cof = -cof;

        return cof;
}

// Determine if the matrix is singular
inline bool gmatrix::IsSingular ()
{
        if ( Row != Col )
                return false;
        return ( Det() == 0.0 );
}

// Determine if the matrix is diagonal
inline bool gmatrix::IsDiagonal ()
{
        if ( Row != Col )
                return false;
        for ( int i=0; i < Row; i++ )
                for ( int j=0; j < Col; j++ )
                        if ( i != j &&ValM[i][j] != 0.0 )
                                return false;
        return true;
}

// Determine if the matrix is scalar
inline bool gmatrix::IsScalar ()
{
        if ( !IsDiagonal() )
                return false;
        double v =ValM[0][0];
        for ( int i=1; i < Row; i++ )
                if ( ValM[i][i] != v )
                        return false;
        return true;
}

// =====
void REPORT_ERROR ( int ko, const char * tit )
{
        Gout<< "\nEv "<<evento_.Gen.Event<<" ERRORE: "<<tit<<endl;
        
        if ( ko<1 )  exit ( 0 );
}