Matrix.h

Go to the documentation of this file.

#ifndef GCP_UTIL_MATRIX_H
#define GCP_UTIL_MATRIX_H

#include <iostream>
#include <cmath>

#include "gcp/util/common/Exception.h"
#include "gcp/util/common/LogStream.h"
#include "gcp/util/common/Vector.h"

namespace gcp {
  namespace util {
    
    // Some C++ ugliness:  To declare non-member friend template
    // functions under the new standard, the function templates must
    // be forward-declared before appearing in the class definition.
    // I suspect this won't even compile with gcc versions < 3.2.2
    
    template<class type>
      class Matrix;
    
    template<class type>
      std::ostream& operator<<(std::ostream& os, 
                      Matrix<type>& mat);
    
    template<class type>
      std::ostringstream& operator<<(std::ostringstream& os, 
                        Matrix<type>& mat);
    
    template<class type>
      Vector<type> operator*(Vector<type>& vec,
                    Matrix<type>& mat);
    
    // Multiplication operators
    
    template<class type>
      Matrix<type> operator*(unsigned fac, Matrix<type>& mat);
    
    template<class type>
      Matrix<type> operator*(int fac, Matrix<type>& mat);
    
    template<class type>
      Matrix<type> operator*(float fac, Matrix<type>& mat);
    
    template<class type>
      Matrix<type> operator*(double fac, Matrix<type>& mat);
    
    template<class type>
      class Matrix {
      
      public:
      
      Matrix();
      
      Matrix(const Matrix<type>& mat);
      
      Matrix(unsigned nRow, unsigned nCol);
      
      virtual ~Matrix();
      
      Vector<type>& operator[](unsigned iRow);
      
      Matrix<type> operator*(Matrix<type>& mat);
      
      void operator=(const Matrix<type>& mat);
      
      template <class T>
    Matrix<type> operator*(T);
      
      template <class T>
    Matrix<type> operator/(T);
      
      template <class T>
    Matrix<type> operator+(T);
      
      template <class T>
    Matrix<type> operator-(T);
      
      Vector<type> operator*( Vector<type>& vec);
      
      Matrix<type> reduce(unsigned iRow, unsigned iCol);
      
      Matrix<type> transpose();
      Matrix<type> trans();
      
      Matrix<type> adjoint();
      Matrix<type> adj();
      
      Matrix<type> inverse();
      Matrix<type> inv();
      
      type determinant();
      type det();
      type determinant(unsigned i, unsigned j);
      type det(unsigned i, unsigned j);
      
      type trace();
      
      type cofactor(unsigned iRow, unsigned iCol);
      
      
      //------------------------------------------------------------
      // Non-member friends
      //------------------------------------------------------------
      
      friend  Vector<type> gcp::util::operator * <>
    ( Vector<type>& vec,  Matrix<type>& mat);
      
      friend std::ostream& gcp::util::operator << <>
    (std::ostream& os, Matrix<type>& mat);
      
      friend std::ostringstream& gcp::util::operator << <>
    (std::ostringstream& os, Matrix<type>& mat);
      
      private:
      
      // Privately, Matrix is implemented as a vector of vectors
      
      Vector<Vector<type> > data_;
      
      // Store the size
      
      unsigned nRow_;
      unsigned nCol_;
      
    }; // End class Matrix
    
    //------------------------------------------------------------
    // Method bodies
    //------------------------------------------------------------
    
    template<class type>
      Matrix<type>::Matrix() 
      {
    nRow_ = 0;
    nCol_ = 0;
      }
    
    template<class type>
      Matrix<type>::Matrix(const Matrix<type>& mat) 
      {
    if(&mat == this)
      return;
    
    nRow_ = mat.nRow_;
    nCol_ = mat.nCol_;
    
    data_ = mat.data_;
      }
    
    template<class type>
      void Matrix<type>::operator=(const Matrix<type>& mat) 
      {
    if(&mat == this)
      return;
    
    nRow_ = mat.nRow_;
    nCol_ = mat.nCol_;
    
    data_.resize(nRow_);
    for(unsigned irow=0; irow < nRow_; irow++) {
      data_[irow].resize(nCol_);
    }
    
    data_ = mat.data_;
      }
    
    template<class type>
      Matrix<type>::Matrix(unsigned nRow, unsigned nCol)
      {
    data_.resize(nRow);
    
    for(unsigned irow=0; irow < nRow; irow++)
      data_[irow].resize(nCol);
    
    nRow_ = nRow;
    nCol_ = nCol;
      }
    
    template<class type>
      Matrix<type>::~Matrix() {}
    
    template<class type>
      Vector<type>& Matrix<type>::operator[](unsigned iRow)
      {
    gcp::util::LogStream errStr;
    
    if(iRow > data_.size()-1) {
      errStr.initMessage(true);
      errStr << "Matrix has no row: " << iRow;
      errStr.report();
      throw Error(errStr);
    }
    
    return data_[iRow];
      }
    
    // Matrix
    
    template<class type>
      Matrix<type> 
      Matrix<type>::operator*(Matrix<type>& mat)
      {
    LogStream errStr;
    
    // Make sure no dimension is zero
    
    if(mat.data_.size() == 0 || data_.size() == 0) {
      errStr.appendMessage(true, "Zero-size dimension encountered");
      errStr.report();
      throw Error(errStr);
    }
    
    // Make sure all rows have the same number of columns
    
    for(unsigned iRow=0; iRow < mat.data_.size(); iRow++)
      if(mat.data_[iRow].size() != mat.nCol_) {
        errStr.appendMessage(true, "Matrix has variable dimensions");
        break;
      }
    
    for(unsigned iRow=0; iRow < data_.size(); iRow++)
      if(data_[iRow].size() != nCol_) {
        errStr.appendMessage(true, "Matrix has variable dimensions");
        break;
      }
    
    // Make sure the matrices can in fact be multiplied.
    
    if(nRow_ != mat.nCol_ || nCol_ != mat.nRow_)
      errStr.appendMessage(true, "Matrices have incompatible dimensions");
    
    if(errStr.isError()) {
      errStr.report();
      throw Error(errStr);
    }
    
    Matrix<type> result(nRow_, mat.nCol_);
    
    bool first;
    for(unsigned iRow=0; iRow < nRow_; iRow++)
      for(unsigned iCol=0; iCol < mat.nCol_; iCol++) {
        first = true;
        for(unsigned j=0; j < nCol_; j++) {
          if(first) {
        result[iRow][iCol] = data_[iRow][j] * mat.data_[j][iCol];
        first = false;
          } else {
        result[iRow][iCol] += data_[iRow][j] * mat.data_[j][iCol];
          }
        }
      }
    return result;
      }
    
    template<class type>
      Vector<type> 
      Matrix<type>::operator*( Vector<type>& vec)
      {
    LogStream errStr;
    
    if(nCol_==0 || vec.size()==0) {
      errStr.appendMessage(true, "zero dimension encountered");
      errStr.report();
      throw Error(errStr);
    }
    
    if(nCol_ != vec.size()) {
      errStr.appendMessage(true, "vector has incompatible dimensions");
      errStr.report();
      throw Error(errStr);
    }
    
    // Now do the calculation
    
    Vector<type> result;
    
    result.resize(nRow_);
    
    bool first = true;
    
    for(unsigned iRow=0; iRow < nRow_; iRow++)
      for(unsigned j=0; j < nCol_; j++) {
        if(first) {
          result[iRow]  = data_[iRow][j] * vec[j];
          first = false;
        } else {
          result[iRow] += data_[iRow][j] * vec[j];
        }
      }
    
    return result;
      }
    
    // Factors
    
    template<class type>
      template<class T>
      Matrix<type> Matrix<type>::operator*(T fac)
      {
    Matrix<type> result = *this;
    
    for(unsigned iRow=0; iRow < nRow_; iRow++)
      for(unsigned iCol=0; iCol < nCol_; iCol++) 
        result[iRow][iCol] *= fac;
    
    return result;
      }
    
    template<class type>
      template<class T>
      Matrix<type> Matrix<type>::operator/(T fac)
      {
    Matrix<type> result = *this;
    
    for(unsigned iRow=0; iRow < nRow_; iRow++)
      for(unsigned iCol=0; iCol < nCol_; iCol++) 
        result[iRow][iCol] /= fac;
    
    return result;
      }
    
    template<class type>
      template<class T>
      Matrix<type> Matrix<type>::operator+(T fac)
      {
    Matrix<type> result = *this;
    
    for(unsigned iRow=0; iRow < nRow_; iRow++)
      for(unsigned iCol=0; iCol < nCol_; iCol++) 
        result[iRow][iCol] += fac;
    
    return result;
      }
    
    template<class type>
      template<class T>
      Matrix<type> Matrix<type>::operator-(T fac)
      {
    Matrix<type> result = *this;
    
    for(unsigned iRow=0; iRow < nRow_; iRow++)
      for(unsigned iCol=0; iCol < nCol_; iCol++) 
        result[iRow][iCol] -= fac;
    
    return result;
      }
    
    //------------------------------------------------------------
    // Non-member operators
    //------------------------------------------------------------
    
    template<class type>
      Vector<type> operator*( Vector<type>& vec,
                     Matrix<type>& mat)
      {
    LogStream errStr;
    
    if(mat.nRow_==0 || vec.size()==0) {
      errStr.appendMessage(true, "zero dimension encountered");
      errStr.report();
      throw Error(errStr);
    }
    
    if(mat.nRow_ != vec.size()) {
      errStr.appendMessage(true, "vector has incompatible dimensions");
      errStr.report();
      throw Error(errStr);
    }
    
    // Now do the calculation
    
    Vector<type> result;
    
    result.resize(mat.nCol_);
    
    bool first = true;
    
    for(unsigned iCol=0; iCol < mat.nCol_; iCol++)
      for(unsigned j=0; j < mat.nRow_; j++) {
        if(first) {
          result[iCol]  = mat.data_[j][iCol] * vec[j];
          first = false;
        } else {
          result[iCol] += mat.data_[j][iCol] * vec[j];
        }
      }
    
    return result;
      }
    
    template<class type>
      Matrix<type> operator*(unsigned fac, Matrix<type>& mat)
      {
    return mat * fac; // Member operator is already defined
      }
    
    template<class type>
      Matrix<type> operator*(int fac, Matrix<type>& mat)
      {
    return mat * fac; // Member operator is already defined
      }
    
    template<class type>
      Matrix<type> operator*(float fac, Matrix<type>& mat)
      {
    return mat * fac; // Member operator is already defined
      }
    
    template<class type>
      Matrix<type> operator*(double fac, Matrix<type>& mat)
      {
    return mat * fac; // Member operator is already defined
      }
    
    template<class type>
      std::ostream& operator<<(std::ostream& os, 
                      Matrix<type>& mat)
      {
    for(unsigned iRow=0; iRow < mat.nRow_; iRow++) {
      os << "|";
      for(unsigned iCol=0; iCol < mat.nCol_; iCol++) {
        if(iCol != 0)
          os << " ";
        os << std::setw(10) << std::setprecision(4) << mat.data_[iRow][iCol];
      }
      os << "|" << std::endl;
    }
    os << std::ends;
    return os;
      }
    
    template<class type>
      std::ostringstream& operator<<(std::ostringstream& os, 
                        Matrix<type>& mat)
      {
    for(unsigned iRow=0; iRow < mat.nRow_; iRow++) {
      os << "|";
      for(unsigned iCol=0; iCol < mat.nCol_; iCol++) {
        if(iCol != 0)
          os << " ";
        os << mat.data_[iRow][iCol];
      }
      os << "|" << std::endl;
    }
    os << std::ends;
    return os;
      }
    
    template<class type>
      Matrix<type> Matrix<type>::transpose()
      {
    Matrix<type> result(nCol_, nRow_);
    
    for(unsigned i=0; i < nRow_; i++)
      for(unsigned j=0; j < nCol_; j++)
        result.data_[j][i] = data_[i][j];
    
    return result;
      }
    
    template<class type>
      Matrix<type> Matrix<type>::trans()
      {
    return transpose();
      }
    
    template<class type>
      Matrix<type> Matrix<type>::adjoint()
      {
    Matrix<type> result(nCol_, nRow_);
    int prefac;
    
    for(unsigned i=0; i < nRow_; i++)
      for(unsigned j=0; j < nCol_; j++) {
        prefac = (i+j)%2 == 0 ? 1 : -1;
        result.data_[i][j] = prefac * determinant(i, j);
      }
    
    return result.transpose();
      }
    
    template<class type>
      Matrix<type> Matrix<type>::adj()
      {
    return adjoint();
      }
    
    template<class type>
      Matrix<type> Matrix<type>::inverse()
      {
    Matrix<type> result = adjoint();
    type deter = determinant();
    
    if(::std::isfinite(1.0/((double)deter)))
      return result/deter;
    else {
      ThrowError("Matrix is not invertible.");
    }
      }
    
    template<class type>
      Matrix<type> Matrix<type>::inv()
      {
    return inverse();
      }
    
    template<class type>
      type Matrix<type>::trace()
      {
    if(nRow_ != nCol_) {
      ThrowError("Not an N x N matrix");
    }
    
    Matrix<type> result = data_[0][0];
    
    for(unsigned i=1; i < nRow_; i++)
      result += data_[i][i];
    
    return result;
      }
    
    template<class type>
      type Matrix<type>::cofactor(unsigned i, unsigned j)
      {
    if(nRow_ != nCol_) {
      ThrowError("Not and N x N matrix");
    }
    
    if(nRow_ < 2) {
      ThrowError("Cannot determine cofactors for dimensions < 2");
    }
    
    int prefac = (i+j)%2 == 0 ? 1 : -1;
    
    return prefac * determinant(i, j);
      }
    
    template<class type>
      type Matrix<type>::determinant()
      {
    type sum;
    
    if(nRow_ != nCol_) {
      ThrowError("Not and N x N matrix");
    }
    
    // If this is a N x N matrix with N < 3, the determinant is trivial
    
    if(nRow_ == 1) 
      return data_[0][0];
    else if(nRow_ == 2) 
      return data_[0][0] * data_[1][1] - data_[1][0] * data_[0][1];
    else {
      bool first=true;
      
      for(unsigned iCol=0; iCol < nCol_; iCol++) {
        if(first) {
          sum = data_[0][iCol] * cofactor(0, iCol);
          first = false;
        } else {
          sum += data_[0][iCol] * cofactor(0, iCol);
        }
      }
    }
    return sum;
      }
    
    template<class type>
      type Matrix<type>::det()
      {
    return determinant();
      }
    
    template<class type>
      type Matrix<type>::determinant(unsigned i, unsigned j)
      {
    if(nRow_ != nCol_) {
      ThrowError("Not and N x N matrix");
    }
    
    if(nRow_ < 2) {
      ThrowError("Cannot compute determinant for dimensions < 2");
    }
    
    Matrix<type> reduced = reduce(i, j);
    
    return reduced.determinant();
      }
    
    template<class type>
      type Matrix<type>::det(unsigned i, unsigned j)
      {
    return determinant(i, j);
      }
    
    template<class type>
      Matrix<type> Matrix<type>::reduce(unsigned i, unsigned j)
      {
    if(nCol_ == 1 || nRow_ == 1) {
      ThrowError("Matrix dimensions cannot be reduced");
    }
    
    // Else output the reduced matrix
    
    Matrix<type> result(nRow_-1, nCol_-1);
    
    // Copy the original matrix, exclusive of the specified row and column
    
    for(unsigned iRow=0, iRowRed=0; iRow < nRow_; iRow++) {
      
      if(iRow != i) {
        
        for(unsigned iCol=0, iColRed=0; iCol < nCol_; iCol++) {
          
          if(iCol != j) {
        result.data_[iRowRed][iColRed] = data_[iRow][iCol];
        iColRed++;
          }
        }
        
        iRowRed++;
      }
    }
    return result;
      }
    
  } // End namespace util
} // End namespace gcp




#endif // End #ifndef GCP_UTIL_MATRIX_H