Matrix.cpp

#include "MantidKernel/Matrix.h"
#include "MantidKernel/V3D.h"
#include "MantidKernel/Exception.h"
#include "MantidKernel/TimeSeriesProperty.h"
#include "MantidKernel/MersenneTwister.h"
#include <iomanip>
#include <iostream>

using Mantid::Kernel::TimeSeriesProperty;


namespace Mantid
{

namespace Kernel
{
  // 
  #define fabs(x) std::fabs((x)*1.0)

  namespace
  {
    //-------------------------------------------------------------------------
    // Utility methods and function objects in anonymous namespace
    //-------------------------------------------------------------------------
    /**
    \class PIndex
    \author S. Ansell
    \date Aug 2005
    \version 1.0
    \brief Class  to fill an index with a progressive count
    */
    template<typename T>
    struct PIndex
    {
    private:
      int count;    ///< counter
    public:
      /// Constructor
      PIndex() : count(0) { }
      /// functional
      std::pair<T,int> operator()(const T& A) {  return std::pair<T,int>(A,count++); }
    };

    /**
    \class PSep
    \author S. Ansell
    \date Aug 2005
    \version 1.0
    \brief Class to access the second object in index pair.
    */
    template<typename T>
    struct PSep
    {
      /// Functional to the second object
      int operator()(const std::pair<T,int>& A) {  return A.second; }
    };

    /**
    * Function to take a vector and sort the vector 
    * so as to produce an index. Leaves the vector unchanged.
    * @param pVec :: Input vector
    * @param Index :: Output vector
    */
    template<typename T>
    void
      indexSort(const std::vector<T>& pVec,std::vector<int>& Index)
    {
      Index.resize(pVec.size());
      std::vector<typename std::pair<T,int> > PartList;
      PartList.resize(pVec.size());

      transform(pVec.begin(),pVec.end(),PartList.begin(),PIndex<T>());
      sort(PartList.begin(),PartList.end());
      transform(PartList.begin(),PartList.end(),Index.begin(),PSep<T>());
      return;
    }
    template void indexSort(const std::vector<double>&,std::vector<int>&);
    template void indexSort(const std::vector<float>&,std::vector<int>&);
    template void indexSort(const std::vector<int>&,std::vector<int>&);
  }



template<typename T>
std::vector<T> 
Matrix<T>::getVector()const
{
  std::vector<T> rez(nx*ny);
  size_t ic(0);
  for(size_t i=0;i<nx;i++)
  {
    for(size_t j=0;j<ny;j++)
    {
      rez[ic]=V[i][j];
      ic++;
    }
  }
  return rez;
}
//
template<typename T>
Matrix<T>::Matrix(const size_t nrow, const size_t ncol, const bool makeIdentity)
  : nx(0),ny(0),V(0)
  /**
    Constructor with pre-set sizes. Matrix is zeroed
    @param nrow :: number of rows
    @param ncol :: number of columns
    @param makeIdentity :: flag for the constructor to return an identity matrix
  */
{
  // Note:: nx,ny zeroed so setMem always works
  setMem(nrow,ncol);
  zeroMatrix();
  if(makeIdentity==true) identityMatrix();
}

template<typename T>
Matrix<T>::Matrix(const std::vector<T>& A,const std::vector<T>& B)
  : nx(0),ny(0),V(0)
  /**
    Constructor to take two vectors and multiply them to 
    construct a matrix. (assuming that we have columns x row
    vector.
    @param A :: Column vector to multiply
    @param B :: Row vector to multiply
  */
{
  // Note:: nx,ny zeroed so setMem always works
  setMem(A.size(),B.size());
  for(size_t i=0;i<nx;i++)
  {
    for(size_t j=0;j<ny;j++)
    {
      V[i][j]=A[i]*B[j];
    }
  }  
}
//
template<typename T>
Matrix<T>::Matrix(const std::vector<T>&data) : nx(0),ny(0),V(0)
{
        size_t numel = data.size();
        size_t nxt =(size_t)sqrt(double(numel));
        size_t test = nxt*nxt;
        if(test != numel){
                throw(std::invalid_argument("number of elements in input vector have to be square of some value"));
        }

        setMem(nxt,nxt);

        size_t ic(0);
        for(size_t i=0;i<nx;i++)
        {
                for(size_t j=0;j<ny;j++)
                {
                        V[i][j]=data[ic];
                        ic++;
                }
        }  
}

template<typename T>
Matrix<T>::Matrix(const Matrix<T>& A,const size_t nrow,const size_t ncol)
  : nx(A.nx-1),ny(A.ny-1),V(0)
  /**
    Constructor with for a missing row/column.
    @param A :: The input matrix
    @param nrow :: number of row to miss
    @param ncol :: number of column to miss
  */
{
  // Note:: nx,ny zeroed so setMem always works
  if (nrow>nx)
    throw Kernel::Exception::IndexError(nrow,A.nx, "Matrix::Constructor without col");
  if (ncol>ny)
    throw Kernel::Exception::IndexError(ncol,A.ny, "Matrix::Constructor without col");
  setMem(nx,ny);
  size_t iR(0);
  for(size_t i=0;i<=nx;i++)
  {
    if (i!=nrow)
    {
      size_t jR(0);
      for(size_t j=0;j<=ny;j++)
      {
        if (j!=ncol)
        {
          V[iR][jR]=A.V[i][j];
          jR++;
        }
      }
          iR++;
    }
  }
}


template<typename T>
Matrix<T>::Matrix(const Matrix<T>& A)
  : nx(0),ny(0),V(0)
  /** 
    Simple copy constructor
    @param A :: Object to copy
  */
{
  // Note:: nx,ny zeroed so setMem always works
  setMem(A.nx,A.ny);
  if (nx*ny)
  {
    for(size_t i=0;i<nx;i++)
    {
      for(size_t j=0;j<ny;j++)
      {
        V[i][j]=A.V[i][j];
      }
    }
  }
}


template<typename T>
Matrix<T>&
Matrix<T>::operator=(const Matrix<T>& A)
  /** 
    Simple assignment operator 
    @param A :: Object to copy
    @return the copied object
  */
{
  if (&A!=this)
    {
      setMem(A.nx,A.ny);
      if (nx*ny)
      {
        for(size_t i=0;i<nx;i++)
        {
          for(size_t j=0;j<ny;j++)
          {
            V[i][j]=A.V[i][j];
          }
        }
      }
    }
  return *this;
}

template<typename T>
Matrix<T>::~Matrix()
  /**
    Delete operator :: removes memory for 
    matrix
  */
{
  deleteMem();
}


template<typename T>
Matrix<T>&
Matrix<T>::operator+=(const Matrix<T>& A)
   /** 
     Matrix addition THIS + A  
     If the size is different then 0 is added where appropiate
     Matrix A is not expanded.
     @param A :: Matrix to add
     @return Matrix(this + A)
   */
{
  const size_t Xpt((nx>A.nx) ? A.nx : nx);
  const size_t Ypt((ny>A.ny) ? A.ny : ny);
  for(size_t i=0;i<Xpt;i++)
  {
    for(size_t j=0;j<Ypt;j++)
    {
      V[i][j]+=A.V[i][j];
    }
  }

  return *this;
}

template<typename T>
Matrix<T>&
Matrix<T>::operator-=(const Matrix<T>& A)
   /** 
     Matrix subtractoin THIS - A  
     If the size is different then 0 is added where appropiate
     Matrix A is not expanded.
     @param A :: Matrix to add
     @return Ma
   */
{
  const size_t Xpt((nx>A.nx) ? A.nx : nx);
  const size_t Ypt((ny>A.ny) ? A.ny : ny);
  for(size_t i=0;i<Xpt;i++)
  {
    for(size_t j=0;j<Ypt;j++)
    {
      V[i][j]-=A.V[i][j];
    }
  }

  return *this;
}

template<typename T>
Matrix<T>
Matrix<T>::operator+(const Matrix<T>& A) const
   /** 
     Matrix addition THIS + A  
     If the size is different then 0 is added where appropiate
     Matrix A is not expanded.
     @param A :: Matrix to add
     @return Matrix(this + A)
   */
{
  Matrix<T> X(*this);
  return X+=A;
}

template<typename T>
Matrix<T>
Matrix<T>::operator-(const Matrix<T>& A) const
   /** 
     Matrix subtraction THIS - A  
     If the size is different then 0 is subtracted where 
     appropiate. This matrix determines the size 
     @param A :: Matrix to add
     @return Matrix(this + A)
   */
{
  Matrix<T> X(*this);
  return X-=A;
}
  

template<typename T>
Matrix<T>
Matrix<T>::operator*(const Matrix<T>& A) const
  /** 
    Matrix multiplication THIS * A  
    @param A :: Matrix to multiply by  (this->row must == A->columns)
    @throw MisMatch<size_t> if there is a size mismatch.
    @return Matrix(This * A) 
 */
{
  if (ny!=A.nx)
    throw Kernel::Exception::MisMatch<size_t>(ny,A.nx,"Matrix::operator*(Matrix)");
  Matrix<T> X(nx,A.ny);
  for(size_t i=0;i<nx;i++)
  {
    for(size_t j=0;j<A.ny;j++)
    {
      for(size_t kk=0;kk<ny;kk++)
      {
              X.V[i][j]+=V[i][kk]*A.V[kk][j];
      }
    }
  }
  return X;
}

template<typename T>
std::vector<T>
Matrix<T>::operator*(const std::vector<T>& Vec) const
  /** 
    Matrix multiplication THIS * Vec to produce a vec  
    @param Vec :: size of vector > this->nrows
    @throw MisMatch<size_t> if there is a size mismatch.
    @return Matrix(This * Vec)
  */
{
  std::vector<T> Out;
  if (ny>Vec.size())
    throw Kernel::Exception::MisMatch<size_t>(ny,Vec.size(),"Matrix::operator*(Vec)");

  Out.resize(nx);
  for(size_t i=0;i<nx;i++)
  {
    Out[i]=0;
    for(size_t j=0;j<ny;j++)
    {
      Out[i]+=V[i][j]*Vec[j];
    }
  }
  return Out;
}

template<typename T>
V3D 
Matrix<T>::operator*(const V3D& Vx) const
  /** 
    Matrix multiplication THIS * V
    @param Vx :: Colunm vector to multiply by
    @throw MisMatch<size_t> if there is a size mismatch.
    @return Matrix(This * A) 
 */
{
  if (ny!=3)
    throw Kernel::Exception::MisMatch<size_t>(ny,3,"Matrix::operator*(V3D)");
  V3D X;
  for(size_t i=0;i<nx;i++)
  {
    for(size_t kk=0;kk<ny;kk++)
    {
      X[i]+=V[i][kk]*Vx[kk];
    }
  }
  return X;
}

template<typename T>
Matrix<T>
Matrix<T>::operator*(const T& Value) const
   /** 
     Matrix multiplication THIS * Value  
     @param Value :: Scalar to multiply by
     @return V * (this)
   */
{
  Matrix<T> X(*this);
  for(size_t i=0;i<nx;i++)
  {
    for(size_t j=0;j<ny;j++)
    {
      X.V[i][j]*=Value;
    }
  }
  return X;
}

/**
  Matrix multiplication THIS *= A
  Note that we call operator* to avoid the problem
  of changing matrix size.
 @param A :: Matrix to multiply by  (this->row must == A->columns)
 @return This *= A
*/
template<typename T>
Matrix<T>&
Matrix<T>::operator*=(const Matrix<T>& A)
{
  if (ny!=A.nx)
    throw Kernel::Exception::MisMatch<size_t>(ny,A.nx,"Matrix*=(Matrix<T>)");
  // This construct to avoid the problem of changing size
  *this = this->operator*(A);
  return *this;
}

template<typename T>
Matrix<T>&
Matrix<T>::operator*=(const T& Value)
   /** 
     Matrix multiplication THIS * Value  
     @param Value :: Scalar to multiply matrix by
     @return *this
   */
{
  for(size_t i=0;i<nx;i++)
  {
    for(size_t j=0;j<ny;j++)
    {
      V[i][j]*=Value;
    }
  }
  return *this;
}

template<typename T>
Matrix<T>&
Matrix<T>::operator/=(const T& Value)
   /** 
     Matrix divishio THIS / Value  
     @param Value :: Scalar to multiply matrix by
     @return *this
   */
{
  for(size_t i=0;i<nx;i++)
  {
    for(size_t j=0;j<ny;j++)
    {
      V[i][j]/=Value;
    }
  }
  return *this;
}

template<typename T>
bool
Matrix<T>::operator!=(const Matrix<T>& A) const
/**
Element by Element comparison
@param A :: Matrix to check
@return true :: on succees
@return false :: failure
*/
{
  return (this->operator==(A));
}

template<typename T>
bool Matrix<T>::operator==(const Matrix<T>& A) const
/** 
Element by element comparison within tolerance.
Tolerance means that the value must be > tolerance
and less than (diff/max)>tolerance 

Always returns 0 if the Matrix have different sizes
@param A :: matrix to check.
@return true on success 
*/
{
  return this->equals(A, 1e-8);
}


//---------------------------------------------------------------------------------------
template<typename T>
bool Matrix<T>::equals(const Matrix<T>& A, const double Tolerance) const
/**
Element by element comparison within tolerance.
Tolerance means that the value must be > tolerance
and less than (diff/max)>tolerance

Always returns 0 if the Matrix have different sizes
@param A :: matrix to check.
@param Tolerance :: tolerance in comparing elements
@return true on success
*/
{
  if (&A!=this)       // this == A == always true
  {
    if(A.nx!=nx || A.ny!=ny)
      return false;

    double maxS(0.0);
    double maxDiff(0.0);   // max di
    for(size_t i=0;i<nx;i++)
      for(size_t j=0;j<ny;j++)
      {
        const T diff=(V[i][j]-A.V[i][j]);
        if (fabs(diff)>maxDiff)
          maxDiff=fabs(diff);
        if (fabs(V[i][j])>maxS)
          maxS=fabs(V[i][j]);
      }
      if (maxDiff<Tolerance)
        return true;
      if (maxS>1.0 && (maxDiff/maxS)<Tolerance)
        return true;
      return false;
  }
  //this == this is true
  return true;
}

//---------------------------------------------------------------------------------------
/** Element by element comparison of <
Always returns false if the Matrix have different sizes
@param A :: matrix to check.
@return true if this < A
*/
template<typename T>
bool Matrix<T>::operator<(const Matrix<T>& A) const
{
  if (&A == this)       // this < A == always false
    return false;

  if(A.nx!=nx || A.ny!=ny)
    return false;

  for(size_t i=0;i<nx;i++)
    for(size_t j=0;j<ny;j++)
    {
      if (V[i][j] >= A.V[i][j])
        return false;
    }
  return true;
}


//---------------------------------------------------------------------------------------
/** Element by element comparison of >=
Always returns false if the Matrix have different sizes
@param A :: matrix to check.
@return true if this >= A
*/
template<typename T>
bool Matrix<T>::operator>=(const Matrix<T>& A) const
{
  if (&A == this)
    return true;

  if(A.nx!=nx || A.ny!=ny)
    return false;

  for(size_t i=0;i<nx;i++)
    for(size_t j=0;j<ny;j++)
    {
      if (V[i][j] < A.V[i][j])
        return false;
    }
  return true;
}


//---------------------------------------------------------------------------------------
template<typename T>
void
Matrix<T>::deleteMem()
  /**
    Deletes the memory held in matrix 
  */
{
  if (V)
    {
      delete [] *V;
      delete [] V;
      V=0;
    }
  nx=0;
  ny=0;
  return;
}


/**
  Sets the memory held in matrix
  @param a :: number of rows
  @param b :: number of columns
*/
template<typename T>
void
Matrix<T>::setMem(const size_t a,const size_t b)
{
  if (a==nx && b==ny) 
    return;

  deleteMem();
  if (a<=0 || b<=0)
    return;

  nx=a;
  ny=b;  
  if (nx*ny)
    {
      T* tmpX=new T[nx*ny];
      V=new T*[nx];
      for (size_t i=0;i<nx;i++)
      {
        V[i]=tmpX + (i*ny);
      }
    }
  return;
}  


/**
  Swap rows I and J
  @param RowI :: row I to swap
  @param RowJ :: row J to swap
*/
template<typename T> 
void
Matrix<T>::swapRows(const size_t RowI,const size_t RowJ)
{
  if (nx*ny && RowI<nx && RowJ<nx && RowI!=RowJ) 
  {
    for(size_t k=0;k<ny;k++)
    {
      T tmp=V[RowI][k];
      V[RowI][k]=V[RowJ][k];
      V[RowJ][k]=tmp;
    }
  }
  return;
}


/**
  Swap columns I and J
  @param colI :: col I to swap
  @param colJ :: col J to swap
*/
template<typename T> 
void
Matrix<T>::swapCols(const size_t colI, const size_t colJ)
{
  if (nx*ny && colI<ny && colJ<ny && colI!=colJ) 
  {
    for(size_t k=0;k<nx;k++)
    {
      T tmp=V[k][colI];
      V[k][colI]=V[k][colJ];
      V[k][colJ]=tmp;
    }
  }
  return;
}

template<typename T> 
void
Matrix<T>::zeroMatrix()
  /** 
    Zeros all elements of the matrix 
  */
{
  if (nx*ny)
  {
    for(size_t i=0;i<nx;i++)
    {
      for(size_t j=0;j<ny;j++)
      {
        V[i][j]=static_cast<T>(0);
      }
    }
  }
  return;
}

template<typename T> 
void
Matrix<T>::identityMatrix()
  /**
    Makes the matrix an idenity matrix.
    Zeros all the terms outside of the square
  */
{
  if (nx*ny)
  {
    for(size_t i=0;i<nx;i++)
    {
      for(size_t j=0;j<ny;j++)
      {
        V[i][j]=(T)((j==i) ? 1 : 0);
      }
    }
  }
  return;
}
template<typename T> 
void 
Matrix<T>::setColumn(const size_t nCol,const std::vector<T> &newCol)
{
        if(nCol>=this->ny){
                throw(std::invalid_argument("nCol requested> nCol availible"));
        }
        size_t nxM = newCol.size();
        if(nx < nxM) nxM=nx;
        for(size_t i=0; i< nxM;i++)
  {
                V[i][nCol]=newCol[i];
        }

}
template<typename T> 
void 
Matrix<T>::setRow(const size_t nRow,const std::vector<T> &newRow)
{
        if(nRow>=this->nx){
                throw(std::invalid_argument("nRow requested> nRow availible"));
        }
        size_t nyM = newRow.size();
        if(ny < nyM) nyM=ny;
        for(size_t j=0; j < nyM;j++)
  {
                V[nRow][j]=newRow[j];
        }

}

template<typename T>
void
Matrix<T>::rotate(const double tau,const double s,const int i,const int j,
                  const int k,const int m)
  /**
    Applies a rotation to a particular point of tan(theta)=tau.
    Note that you need both sin(theta) and tan(theta) because of 
    sign preservation. 
    @param tau :: tan(theta) 
    @param s :: sin(theta)
    @param i ::  first index (xpos) 
    @param j ::  first index (ypos) 
    @param k ::  second index (xpos) 
    @param m ::  second index (ypos) 
   */
{
  const T gg=V[i][j];
  const T hh=V[k][m]; 
  V[i][j]=static_cast<T>(gg-s*(hh+gg*tau));
  V[k][m]=static_cast<T>(hh+s*(gg-hh*tau));
  return;
}

template<typename T>
Matrix<T>
Matrix<T>::fDiagonal(const std::vector<T>& Dvec) const
  /**
    Calculate the forward diagonal product.
    Construct a matrix based on  Dvec * This, 
    where Dvec is made into a diagonal matrix.
    @param Dvec :: diagonal matrix (just centre points)
    @return D*this
  */
{
  // Note:: nx,ny zeroed so setMem always works
  if (Dvec.size()!=nx)
    {
      std::ostringstream cx;
      cx<<"Matrix::fDiagonal Size: "<<Dvec.size()<<" "<<nx<<" "<<ny;
      throw std::runtime_error(cx.str());
    }
  Matrix<T> X(Dvec.size(),ny);
  for(size_t i=0; i < Dvec.size();i++)
  {
    for(size_t j=0;j<ny;j++)
    {
      X.V[i][j]=Dvec[i]*V[i][j];
    }
  }
  return X;
}

template<typename T>
Matrix<T>
Matrix<T>::bDiagonal(const std::vector<T>& Dvec) const
  /**
    Calculate the backward diagonal product.
    Construct a matrix based on 
    This * Dvec, where Dvec is made into a diagonal 
    matrix.
    @param Dvec :: diagonal matrix (just centre points)
    @return this*D
  */
{
  // Note:: nx,ny zeroed so setMem always works
  if (Dvec.size()!=ny)
    {
      std::ostringstream cx;
      cx<<"Error Matrix::bDiaognal size:: "<<Dvec.size()
        <<" "<<nx<<" "<<ny;
      throw std::runtime_error(cx.str());
    }

  Matrix<T> X(nx,Dvec.size());
  for(size_t i=0;i<nx;i++)
  {
    for(size_t j=0;j<Dvec.size();j++)
    {
      X.V[i][j]=Dvec[j]*V[i][j];
    }
  }
  return X;
}

template<typename T>
Matrix<T>
Matrix<T>::Tprime() const
  /** 
    Transpose the matrix : 
    Has transpose for a square matrix case.
    @return M^T
  */
{
  if (!nx*ny)
    return *this;

  if (nx==ny)   // inplace transpose
    {
      Matrix<T> MT(*this);
      MT.Transpose();
      return MT;
    }

  // irregular matrix
  Matrix<T> MT(ny,nx);
  for(size_t i=0;i<nx;i++)
    for(size_t j=0;j<ny;j++)
      MT.V[j][i]=V[i][j];

  return MT;
}



template<typename T>
Matrix<T>&
Matrix<T>::Transpose()
  /** 
    Transpose the matrix : 
    Has a inplace transpose for a square matrix case.
    @return this^T
  */
{
  if (!nx*ny)
    return *this;
  if (nx==ny)   // inplace transpose
  {
    for(size_t i=0;i<nx;i++)
    {
      for(size_t j=i+1;j<ny;j++)
      {
        T tmp=V[i][j];
        V[i][j]=V[j][i];
        V[j][i]=tmp;
      }
    }
    return *this;
  }
  // irregular matrix
  // get some memory
  T* tmpX=new T[ny*nx];
  T** Vt=new T*[ny];
  for (size_t i=0;i<ny;i++)
  {
    Vt[i] = tmpX + (i*nx);
  } 
  for(size_t i=0;i<nx;i++)
  {
    for(size_t j=0;j<ny;j++)
    {
      Vt[j][i]=V[i][j];
    }
  }
  // remove old memory
  const size_t tx=nx;
  const size_t ty=ny;
  deleteMem();  // resets nx,ny
  // replace memory
  V=Vt;
  nx=ty;
  ny=tx;

  return *this;
}

template<>
int
Matrix<int>::GaussJordan(Kernel::Matrix<int>&)
  /**
    Not valid for Integer
    @return zero
  */
{
  return 0;
}

template<typename T>
int
Matrix<T>::GaussJordan(Matrix<T>& B)
  /**
    Invert this Matrix and solve the 
    form such that if A.x=B then  solve to generate x.  
    This requires that B is B[A.nx][Any]
    The result is placed back in B
    @return the calculation result
   */
{
  // check for input errors
  if (nx!=ny || B.nx!=nx)
    return -1;
  
  // pivoted rows 
  std::vector<int> pivoted(nx);
  fill(pivoted.begin(),pivoted.end(),0);

  std::vector<int> indxcol(nx);   // Column index
  std::vector<int> indxrow(nx);   // row index

  size_t irow(0),icol(0);
  for(size_t i=0;i<nx;i++)
  {
    // Get Biggest non-pivoted item
    double bigItem=0.0;         // get point to pivot over
    for(size_t j=0;j<nx;j++)
        {
            if (pivoted[j]!= 1)  //check only non-pivots
            {
              for(size_t k=0;k<nx;k++)
        {
                if (!pivoted[k])
                      {
                        if (fabs(V[j][k]) >=bigItem)
                        {
                                bigItem=fabs(V[j][k]);
                                irow=j;
                                icol=k;
                        }
                  }
        }
      }
            else if (pivoted[j]>1)
      {
              throw std::runtime_error("Error doing G-J elem on a singular matrix");
      }
    }
    pivoted[icol]++;
    // Swap in row/col to make a diagonal item 
    if (irow != icol)
        {