MatrixTools.h

Go to the documentation of this file.

//
// File: MatrixTools.h
// Authors:
//   Julien Dutheil
// Created: 2004-01-19 16:42:25
//
 
/*
  Copyright or © or Copr. Bio++ Development Team, (November 17, 2004)
  
  This software is a computer program whose purpose is to provide classes
  for numerical calculus. This file is part of the Bio++ project.
  
  This software is governed by the CeCILL license under French law and
  abiding by the rules of distribution of free software. You can use,
  modify and/ or redistribute the software under the terms of the CeCILL
  license as circulated by CEA, CNRS and INRIA at the following URL
  "http://www.cecill.info".
  
  As a counterpart to the access to the source code and rights to copy,
  modify and redistribute granted by the license, users are provided only
  with a limited warranty and the software's author, the holder of the
  economic rights, and the successive licensors have only limited
  liability.
  
  In this respect, the user's attention is drawn to the risks associated
  with loading, using, modifying and/or developing or reproducing the
  software by the user in light of its specific status of free software,
  that may mean that it is complicated to manipulate, and that also
  therefore means that it is reserved for developers and experienced
  professionals having in-depth computer knowledge. Users are therefore
  encouraged to load and test the software's suitability as regards their
  requirements in conditions enabling the security of their systems and/or
  data to be ensured and, more generally, to use and operate it in the
  same conditions as regards security.
  
  The fact that you are presently reading this means that you have had
  knowledge of the CeCILL license and that you accept its terms.
*/
 
#ifndef BPP_NUMERIC_MATRIX_MATRIXTOOLS_H
#define BPP_NUMERIC_MATRIX_MATRIXTOOLS_H
 
#include <cstdio>
#include <iostream>
 
#include "../../Io/OutputStream.h"
#include "../VectorTools.h"
#include "EigenValue.h"
#include "LUDecomposition.h"
#include "Matrix.h"
 
namespace bpp
{
class MatrixTools
{
public:
  MatrixTools() {}
  ~MatrixTools() {}
 
public:
  template<class MatrixA, class MatrixO>
  static void copy(const MatrixA& A, MatrixO& O)
  {
    O.resize(A.getNumberOfRows(), A.getNumberOfColumns());
    for (size_t i = 0; i < A.getNumberOfRows(); i++)
    {
      for (size_t j = 0; j < A.getNumberOfColumns(); j++)
      {
        O(i, j) = A(i, j);
      }
    }
  }
 
  template<class MatrixA, class MatrixO>
  static void copyUp(const MatrixA& A, MatrixO& O)
  {
    size_t nr(A.getNumberOfRows()), nc(A.getNumberOfColumns());
    O.resize(nr, nc);
    for (size_t i = 0; i < nr - 1; i++)
    {
      for (size_t j = 0; j < nc; j++)
      {
        O(i, j) = A(i + 1, j);
      }
    }
    for (size_t j = 0; j < nc; j++)
    {
      O(nr - 1, j) = 0;
    }
  }
 
  template<class MatrixA, class MatrixO>
  static void copyDown(const MatrixA& A, MatrixO& O)
  {
    size_t nr(A.getNumberOfRows()), nc(A.getNumberOfColumns());
    O.resize(nr, nc);
    for (size_t i = 1; i < nr; i++)
    {
      for (size_t j = 0; j < nc; j++)
      {
        O(i, j) = A(i - 1, j);
      }
    }
    for (size_t j = 0; j < nc; j++)
    {
      O(0, j) = 0;
    }
  }
 
  template<class Matrix>
  static void getId(size_t n, Matrix& O)
  {
    O.resize(n, n);
    for (size_t i = 0; i < n; i++)
    {
      for (size_t j = 0; j < n; j++)
      {
        O(i, j) = (i == j) ? 1 : 0;
      }
    }
  }
 
  template<class Scalar>
  static void diag(const std::vector<Scalar>& D, Matrix<Scalar>& O)
  {
    size_t n = D.size();
    O.resize(n, n);
    for (size_t i = 0; i < n; i++)
    {
      for (size_t j = 0; j < n; j++) { O(i, j) = (i == j) ? D[i] : 0;}
    }
  }
 
  template<class Scalar>
  static void diag(const Scalar x, size_t n, Matrix<Scalar>& O)
  {
    O.resize(n, n);
    for (size_t i = 0; i < n; i++)
    {
      for (size_t j = 0; j < n; j++) { O(i, j) = (i == j) ? x : 0;}
    }
  }
 
  template<class Scalar>
  static void diag(const Matrix<Scalar>& M, std::vector<Scalar>& O)
  {
    size_t nc = M.getNumberOfColumns();
    size_t nr = M.getNumberOfRows();
    if (nc != nr) throw DimensionException("MatrixTools::diag(). M must be a square matrix.", nr, nc);
    O.resize(nc);
    for (size_t i = 0; i < nc; i++) { O[i] = M(i, i);}
  }
 
  template<class Matrix, class Scalar>
  static void fill(Matrix& M, Scalar x)
  {
    for (size_t i = 0; i < M.getNumberOfRows(); i++)
    {
      for (size_t j = 0; j < M.getNumberOfColumns(); j++)
      {
        M(i, j) = x;
      }
    }
  }
 
  template<class Matrix, class Scalar>
  static void fillDiag(Matrix& M, Scalar x)
  {
    for (size_t i = 0; i < M.getNumberOfRows(); i++)
    {
      M(i, i) = x;
    }
  }
 
  template<class Matrix, class Scalar>
  static void scale(Matrix& A, Scalar a, Scalar b = 0)
  {
    for (size_t i = 0; i < A.getNumberOfRows(); i++)
    {
      for (size_t j = 0; j < A.getNumberOfColumns(); j++)
      {
        A(i, j) = a * A(i, j) + b;
      }
    }
  }
 
  template<class Scalar>
  static void mult(const Matrix<Scalar>& A, const Matrix<Scalar>& B, Matrix<Scalar>& O)
  {
    size_t ncA = A.getNumberOfColumns();
    size_t nrA = A.getNumberOfRows();
    size_t nrB = B.getNumberOfRows();
    size_t ncB = B.getNumberOfColumns();
    if (ncA != nrB) throw DimensionException("MatrixTools::mult(). nrows B != ncols A.", nrB, ncA);
    O.resize(nrA, ncB);
    for (size_t i = 0; i < nrA; i++)
    {
      for (size_t j = 0; j < ncB; j++)
      {
        O(i, j) = 0;
        for (size_t k = 0; k < ncA; k++)
        {
          O(i, j) += A(i, k) * B(k, j);
        }
      }
    }
  }
 
  template<class Scalar>
  static void mult(const Matrix<Scalar>& A, const Matrix<Scalar>& iA, const Matrix<Scalar>& B, const Matrix<Scalar>& iB, Matrix<Scalar>& O, Matrix<Scalar>& iO)
  {
    size_t ncA = A.getNumberOfColumns();
    size_t nrA = A.getNumberOfRows();
    size_t nrB = B.getNumberOfRows();
    size_t ncB = B.getNumberOfColumns();
    if (ncA != nrB) throw DimensionException("MatrixTools::mult(). nrows B != ncols A.", nrB, ncA);
    O.resize(nrA, ncB);
    iO.resize(nrA, ncB);
    for (size_t i = 0; i < nrA; i++)
    {
      for (size_t j = 0; j < ncB; j++)
      {
        O(i, j) = 0;
        iO(i, j) = 0;
        for (size_t k = 0; k < ncA; k++)
        {
          O(i, j) += A(i, k) * B(k, j) - iA(i, k) * iB(k, j);
          iO(i, j) += A(i, k) * iB(k, j) + iA(i, k) * B(k, j);
        }
      }
    }
  }
 
  template<class Scalar>
  static void mult(const Matrix<Scalar>& A, const std::vector<Scalar>& D, const Matrix<Scalar>& B, Matrix<Scalar>& O)
  {
    size_t ncA = A.getNumberOfColumns();
    size_t nrA = A.getNumberOfRows();
    size_t nrB = B.getNumberOfRows();
    size_t ncB = B.getNumberOfColumns();
    if (ncA != nrB) throw DimensionException("MatrixTools::mult(). nrows B != ncols A.", nrB, ncA);
    if (ncA != D.size()) throw DimensionException("MatrixTools::mult(). Vector size is not equal to matrix size.", D.size(), ncA);
    O.resize(nrA, ncB);
    for (size_t i = 0; i < nrA; i++)
    {
      for (size_t j = 0; j < ncB; j++)
      {
        O(i, j) = 0;
        for (size_t k = 0; k < ncA; k++)
        {
          O(i, j) += A(i, k) * B(k, j) * D[k];
        }
      }
    }
  }
 
  template<class Scalar>
  static void mult(const Matrix<Scalar>& A, const Matrix<Scalar>& iA, const std::vector<Scalar>& D, const std::vector<Scalar>& iD, const Matrix<Scalar>& B, const Matrix<Scalar>& iB, Matrix<Scalar>& O, Matrix<Scalar>& iO)
  {
    size_t ncA = A.getNumberOfColumns();
    size_t nrA = A.getNumberOfRows();
    size_t nrB = B.getNumberOfRows();
    size_t ncB = B.getNumberOfColumns();
    if (ncA != nrB) throw DimensionException("MatrixTools::mult(). nrows B != ncols A.", nrB, ncA);
    if (ncA != D.size()) throw DimensionException("MatrixTools::mult(). Vector size is not equal to matrix size.", D.size(), ncA);
    O.resize(nrA, ncB);
    Scalar ab, iaib, iab, aib;
 
    for (size_t i = 0; i < nrA; i++)
    {
      for (size_t j = 0; j < ncB; j++)
      {
        O(i, j) = 0;
        iO(i, j) = 0;
        for (size_t k = 0; k < ncA; k++)
        {
          ab = A(i, k) * B(k, j) - iA(i, k) * iB(k, j);
          aib = A(i, k) * iB(k, j) + iA(i, k) * B(k, j);
 
          O(i, j) += ab * D[k] - aib * iD[k];
          iO(i, j) += ab * iD[k] + aib * D[k];
        }
      }
    }
  }
 
  template<class Scalar>
  static void mult(const Matrix<Scalar>& A, const std::vector<Scalar>& D, const std::vector<Scalar>& U, const std::vector<Scalar>& L, const Matrix<Scalar>& B, Matrix<Scalar>& O)
  {
    size_t ncA = A.getNumberOfColumns();
    size_t nrA = A.getNumberOfRows();
    size_t nrB = B.getNumberOfRows();
    size_t ncB = B.getNumberOfColumns();
    if (ncA != nrB) throw DimensionException("MatrixTools::mult(). nrows B != ncols A.", nrB, ncA);
    if (ncA != D.size()) throw DimensionException("MatrixTools::mult(). Vector size is not equal to matrix size.", D.size(), ncA);
    if (ncA != U.size() + 1) throw DimensionException("MatrixTools::mult(). Vector size is not equal to matrix size-1.", U.size(), ncA);
    if (ncA != L.size() + 1) throw DimensionException("MatrixTools::mult(). Vector size is not equal to matrix size-1.", L.size(), ncA);
    O.resize(nrA, ncB);
    for (size_t i = 0; i < nrA; i++)
    {
      for (size_t j = 0; j < ncB; j++)
      {
        O(i, j) = A(i, 0) * D[0] * B(0, j);
        if (nrB > 1)
          O(i, j) += A(i, 0) * U[0] * B(1, j);
        for (size_t k = 1; k < ncA - 1; k++)
        {
          O(i, j) += A(i, k) * (L[k - 1] * B(k - 1, j) + D[k] * B(k, j) + U[k] * B(k + 1, j));
        }
        if (ncA >= 2)
          O(i, j) += A(i, ncA - 1) * L[ncA - 2] * B(ncA - 2, j);
        O(i, j) += A(i, ncA - 1) * D[ncA - 1] * B(ncA - 1, j);
      }
    }
  }
 
  template<class MatrixA, class MatrixB>
  static void add(MatrixA& A, const MatrixB& B)
  {
    size_t ncA = A.getNumberOfColumns();
    size_t nrA = A.getNumberOfRows();
    size_t nrB = B.getNumberOfRows();
    size_t ncB = B.getNumberOfColumns();
    if (ncA > ncB) throw DimensionException("MatrixTools::operator+=(). A and B must have the same number of columns.", ncB, ncA);
    if (nrA > nrB) throw DimensionException("MatrixTools::operator+=(). A and B must have the same number of rows.", nrB, nrA);
 
 
    for (size_t i = 0; i < nrA; i++)
    {
      for (size_t j = 0; j < ncA; j++)
      {
        A(i, j) += B(i, j);
      }
    }
  }
 
  template<class MatrixA, class MatrixB, class Scalar>
  static void add(MatrixA& A, Scalar& x, const MatrixB& B)
  {
    size_t ncA = A.getNumberOfColumns();
    size_t nrA = A.getNumberOfRows();
    size_t nrB = B.getNumberOfRows();
    size_t ncB = B.getNumberOfColumns();
    if (ncA != ncB) throw DimensionException("MatrixTools::operator+(). A and B must have the same number of columns.", ncB, ncA);
    if (nrA != nrB) throw DimensionException("MatrixTools::operator+(). A and B must have the same number of rows.", nrB, nrA);
 
    for (size_t i = 0; i < nrA; i++)
    {
      for (size_t j = 0; j < ncA; j++)
      {
        A(i, j) += x * B(i, j);
      }
    }
  }
 
  template<class Matrix>
  static void pow(const Matrix& A, size_t p, Matrix& O)
  {
    size_t n = A.getNumberOfRows();
    if (n != A.getNumberOfColumns()) throw DimensionException("MatrixTools::pow(). nrows != ncols.", A.getNumberOfColumns(), A.getNumberOfRows());
    switch (p)
    {
    case 0:
      getId<Matrix>(n, O);
      break;
    case 1:
      copy(A, O);
      break;
    case 2:
      mult(A, A, O);
      break;
    default:
      Matrix tmp;
      if (p % 2)
      {
        pow(A, p / 2, tmp);
        pow(tmp, 2, O);
      }
      else
      {
        pow(A, (p - 1) / 2, tmp);
        pow(tmp, 2, O);
        mult(A, O, tmp);
        copy(tmp, O);
      }
    }
  }
 
  template<class Scalar>
  static void pow(const Matrix<Scalar>& A, double p, Matrix<Scalar>& O)
  {
    size_t n = A.getNumberOfRows();
    if (n != A.getNumberOfColumns()) throw DimensionException("MatrixTools::pow(). nrows != ncols.", A.getNumberOfColumns(), A.getNumberOfRows());
    EigenValue<Scalar> eigen(A);
    RowMatrix<Scalar> rightEV, leftEV;
    rightEV = eigen.getV();
    inv(rightEV, leftEV);
    mult(rightEV, VectorTools::pow(eigen.getRealEigenValues(), p), leftEV, O);
  }
 
  template<class Scalar>
  static void exp(const Matrix<Scalar>& A, Matrix<Scalar>& O)
  {
    size_t n = A.getNumberOfRows();
    if (n != A.getNumberOfColumns()) throw DimensionException("MatrixTools::exp(). nrows != ncols.", A.getNumberOfColumns(), A.getNumberOfRows());
    EigenValue<Scalar> eigen(A);
    RowMatrix<Scalar> rightEV, leftEV;
    rightEV = eigen.getV();
    inv(rightEV, leftEV);
    mult(rightEV, VectorTools::exp(eigen.getRealEigenValues()), leftEV, O);
  }
 
  template<class Matrix, class Scalar>
  static void Taylor(const Matrix& A, size_t p, std::vector< RowMatrix<Scalar> >& vO)
  {
    size_t n = A.getNumberOfRows();
    if (n != A.getNumberOfColumns())
      throw DimensionException("MatrixTools::pow(). nrows != ncols.", A.getNumberOfColumns(), A.getNumberOfRows());
    vO.resize(p + 1);
    getId<Matrix>(n, vO[0]);
    copy(A, vO[1]);
 
    for (size_t i = 1; i < p; i++)
    {
      mult(vO[i], A, vO[i + 1]);
    }
  }
 
  template<class Matrix>
  static std::vector<size_t> whichMax(const Matrix& m)
  {
    size_t nrows = m.getNumberOfRows();
    size_t ncols = m.getNumberOfColumns();
    std::vector<size_t> pos(2);
    size_t imax = 0;
    size_t jmax = 0;
    double currentMax = std::log(0.);
    for (size_t i = 0; i < nrows; i++)
    {
      for (size_t j = 0; j < ncols; j++)
      {
        double currentValue = m(i, j);
        // cout << currentValue << "\t" << (currentValue > currentMax) << endl;
        if (currentValue > currentMax)
        {
          imax = i;
          jmax = j;
          currentMax = currentValue;
        }
      }
    }
    pos[0] = imax;
    pos[1] = jmax;
    return pos;
  }
 
  template<class Matrix>
  static std::vector<size_t> whichMin(const Matrix& m)
  {
    size_t nrows = m.getNumberOfRows();
    size_t ncols = m.getNumberOfColumns();
    std::vector<size_t> pos(2);
    size_t imin = 0;
    size_t jmin = 0;
    double currentMin = -std::log(0.);
    for (size_t i = 0; i < nrows; i++)
    {
      for (size_t j = 0; j < ncols; j++)
      {
        double currentValue = m(i, j);
        if (currentValue < currentMin)
        {
          imin = i;
          jmin = j;
          currentMin = currentValue;
        }
      }
    }
    pos[0] = imin;
    pos[1] = jmin;
    return pos;
  }
 
  template<class Real>
  static Real max(const Matrix<Real>& m)
  {
    size_t nrows = m.getNumberOfRows();
    size_t ncols = m.getNumberOfColumns();
    Real currentMax = std::log(0.);
    for (size_t i = 0; i < nrows; i++)
    {
      for (size_t j = 0; j < ncols; j++)
      {
        Real currentValue = m(i, j);
        // cout << currentValue << "\t" << (currentValue > currentMax) << endl;
        if (currentValue > currentMax)
        {
          currentMax = currentValue;
        }
      }
    }
    return currentMax;
  }
 
 
  template<class Real>
  static Real min(const Matrix<Real>& m)
  {
    size_t nrows = m.getNumberOfRows();
    size_t ncols = m.getNumberOfColumns();
    Real currentMin = -std::log(0.);
    for (size_t i = 0; i < nrows; i++)
    {
      for (size_t j = 0; j < ncols; j++)
      {
        Real currentValue = m(i, j);
        if (currentValue < currentMin)
        {
          currentMin = currentValue;
        }
      }
    }
    return currentMin;
  }
 
  template<class Matrix>
  static void print(const Matrix& m, std::ostream& out = std::cout)
  {
    out << m.getNumberOfRows() << "x" << m.getNumberOfColumns() << std::endl;
    out << "[" << std::endl;
    for (size_t i = 0; i < m.getNumberOfRows(); i++)
    {
      out << "[";
      for (size_t j = 0; j < m.getNumberOfColumns() - 1; j++)
      {
        out << m(i, j) << ", ";
      }
      if (m.getNumberOfColumns() > 0) out << m(i, m.getNumberOfColumns() - 1) << "]" << std::endl;
    }
    out << "]" << std::endl;
  }
 
  template<class Matrix>
  static void print(const Matrix& m, bpp::OutputStream& out, char pIn = '(', char pOut = ')')
  {
    out << pIn;
 
    for (size_t i = 0; i < m.getNumberOfRows(); i++)
    {
      if (i != 0)
        out << ",";
 
      out << pIn;
      for (size_t j = 0; j < m.getNumberOfColumns() - 1; j++)
      {
        out << m(i, j) << ", ";
      }
      if (m.getNumberOfColumns() > 0) out << m(i, m.getNumberOfColumns() - 1) << pOut;
    }
    out << pOut;
  }
 
  template<class Matrix>
  static void printForR(const Matrix& m, const std::string& variableName = "x", std::ostream& out = std::cout)
  {
    out.precision(12);
    out << variableName << "<-matrix(c(";
    for (size_t i = 0; i < m.getNumberOfRows(); i++)
    {
      if (i > 0)
        out << std::endl;
 
      for (size_t j = 0; j < m.getNumberOfColumns(); j++)
      {
        if (i > 0 || j > 0)
          out << ", ";
        out << m(i, j);
      }
    }
    out << "), nrow=" << m.getNumberOfRows() << ", byrow=TRUE)" << std::endl;
  }
 
 
  template<class Real>
  static void print(const std::vector<Real>& v, std::ostream& out = std::cout)
  {
    out << v.size() << std::endl;
    out << "[";
    for (size_t i = 0; i < v.size() - 1; i++)
    {
      out << v[i] << ", ";
    }
    if (v.size() > 0) out << v[v.size() - 1];
    out << "]" << std::endl;
  }
 
  template<class Matrix>
  static bool isSquare(const Matrix& A) { return A.getNumberOfRows() == A.getNumberOfColumns(); }
 
  template<class Scalar>
  static Scalar inv(const Matrix<Scalar>& A, Matrix<Scalar>& O)
  {
    if (!isSquare(A)) throw DimensionException("MatrixTools::inv(). Matrix A is not a square matrix.", A.getNumberOfRows(), A.getNumberOfColumns());
    LUDecomposition<Scalar> lu(A);
    RowMatrix<Scalar> I;
    getId(A.getNumberOfRows(), I);
    return lu.solve(I, O);
  }
 
  template<class Scalar>
  static double det(const Matrix<Scalar>& A)
  {
    if (!isSquare(A)) throw DimensionException("MatrixTools::det(). Matrix A is not a square matrix.", A.getNumberOfRows(), A.getNumberOfColumns());
    LUDecomposition<Scalar> lu(A);
    return lu.det();
  }
 
  template<class MatrixA, class MatrixO>
  static void transpose(const MatrixA& A, MatrixO& O)
  {
    O.resize(A.getNumberOfColumns(), A.getNumberOfRows());
    for (size_t i = 0; i < A.getNumberOfColumns(); i++)
    {
      for (size_t j = 0; j < A.getNumberOfRows(); j++)
      {
        O(i, j) = A(j, i);
      }
    }
  }
 
  template<class MatrixA>
  static bool isSymmetric(const MatrixA& A)
  {
    if (A.getNumberOfColumns() != A.getNumberOfRows())
      return false;
 
    for (size_t i = 0; i < A.getNumberOfColumns(); i++)
    {
      for (size_t j = i + 1; j < A.getNumberOfRows(); j++)
      {
        if (A(i, j) != A(j, i))
          return false;
      }
    }
    return true;
  }
 
  template<class Scalar>
  static void covar(const Matrix<Scalar>& A, Matrix<Scalar>& O)
  {
    size_t r = A.getNumberOfRows();
    size_t n = A.getNumberOfColumns();
    O.resize(r, r);
    RowMatrix<Scalar> tA;
    transpose(A, tA);
    mult(A, tA, O);
    scale(O, 1. / static_cast<double>(n));
    RowMatrix<Scalar> mean(r, 1);
    for (size_t i = 0; i < r; i++)
    {
      for (size_t j = 0; j < n; j++)
      {
        mean(i, 0) += A(i, j);
      }
      mean(i, 0) /= static_cast<double>(n);
    }
    RowMatrix<Scalar> tMean;
    transpose(mean, tMean);
    RowMatrix<Scalar> meanMat;
    mult(mean, tMean, meanMat);
    scale(meanMat, -1.);
    add(O, meanMat);
  }
 
  template<class Scalar>
  static void kroneckerMult(const Matrix<Scalar>& A, const Matrix<Scalar>& B, Matrix<Scalar>& O, bool check = true)
  {
    size_t ncA = A.getNumberOfColumns();
    size_t nrA = A.getNumberOfRows();
    size_t nrB = B.getNumberOfRows();
    size_t ncB = B.getNumberOfColumns();
 
    if (check)
      O.resize(nrA * nrB, ncA * ncB);
 
    for (size_t ia = 0; ia < nrA; ia++)
    {
      for (size_t ja = 0; ja < ncA; ja++)
      {
        Scalar aij = A(ia, ja);
        for (size_t ib = 0; ib < nrB; ib++)
        {
          for (size_t jb = 0; jb < ncB; jb++)
          {
            O(ia * nrB + ib, ja * ncB + jb) = aij * B(ib, jb);
          }
        }
      }
    }
  }
 
  template<class Scalar>
  static void kroneckerMult(const Matrix<Scalar>& A, size_t dim, const Scalar& v, Matrix<Scalar>& O, bool check = true)
  {
    size_t ncA = A.getNumberOfColumns();
    size_t nrA = A.getNumberOfRows();
 
    if (check)
      O.resize(nrA * dim, ncA * dim);
 
    for (size_t ia = 0; ia < nrA; ia++)
    {
      for (size_t ja = 0; ja < ncA; ja++)
      {
        Scalar aij = A(ia, ja);
        for (size_t ib = 0; ib < dim; ib++)
        {
          for (size_t jb = 0; jb < dim; jb++)
          {
            O(ia * dim + ib, ja * dim + jb) = aij * ((ib == jb) ? v : 0);
          }
        }
      }
    }
  }
 
  template<class Scalar>
  static void kroneckerMult(const Matrix<Scalar>& A, const Matrix<Scalar>& B, const Scalar& dA, const Scalar& dB, Matrix<Scalar>& O, bool check = true)
  {
    size_t ncA = A.getNumberOfColumns();
    size_t nrA = A.getNumberOfRows();
    size_t nrB = B.getNumberOfRows();
    size_t ncB = B.getNumberOfColumns();
 
    if (check)
      O.resize(nrA * nrB, ncA * ncB);
 
    for (size_t ia = 0; ia < nrA; ia++)
    {
      for (size_t ja = 0; ja < ncA; ja++)
      {
        const Scalar& aij = (ia == ja) ? dA : A(ia, ja);
 
        for (size_t ib = 0; ib < nrB; ib++)
        {
          for (size_t jb = 0; jb < ncB; jb++)
          {
            O(ia * nrB + ib, ja * ncB + jb) = aij * ((ib == jb) ? dB : B(ib, jb));
          }
        }
      }
    }
  }
 
  template<class Scalar>
  static void hadamardMult(const Matrix<Scalar>& A, const Matrix<Scalar>& B, Matrix<Scalar>& O)
  {
    size_t ncA = A.getNumberOfColumns();
    size_t nrA = A.getNumberOfRows();
    size_t nrB = B.getNumberOfRows();
    size_t ncB = B.getNumberOfColumns();
    if (nrA != nrB) throw DimensionException("MatrixTools::hadamardMult(). nrows A != nrows B.", nrA, nrB);
    if (ncA != ncB) throw DimensionException("MatrixTools::hadamardMult(). ncols A != ncols B.", ncA, ncB);
    O.resize(nrA, ncA);
    for (size_t i = 0; i < nrA; i++)
    {
      for (size_t j = 0; j < ncA; j++)
      {
        O(i, j) = A(i, j) * B(i, j);
      }
    }
  }
 
  template<class Scalar>
  static void hadamardMult(const Matrix<Scalar>& A, const Matrix<Scalar>& iA, const Matrix<Scalar>& B, const Matrix<Scalar>& iB, Matrix<Scalar>& O, Matrix<Scalar>& iO)
  {
    size_t ncA = A.getNumberOfColumns();
    size_t nrA = A.getNumberOfRows();
    size_t nrB = B.getNumberOfRows();
    size_t ncB = B.getNumberOfColumns();
    if (nrA != nrB) throw DimensionException("MatrixTools::hadamardMult(). nrows A != nrows B.", nrA, nrB);
    if (ncA != ncB) throw DimensionException("MatrixTools::hadamardMult(). ncols A != ncols B.", ncA, ncB);
    O.resize(nrA, ncA);
    iO.resize(nrA, ncA);
    for (size_t i = 0; i < nrA; i++)
    {
      for (size_t j = 0; j < ncA; j++)
      {
        O(i, j) = A(i, j) * B(i, j) -  iA(i, j) * iB(i, j);
        iO(i, j) = iA(i, j) * B(i, j) +  A(i, j) * iB(i, j);
      }
    }
  }
 
  template<class Scalar>
  static void hadamardMult(const Matrix<Scalar>& A, const std::vector<Scalar>& B, Matrix<Scalar>& O, bool row = true)
  {
    size_t ncA = A.getNumberOfColumns();
    size_t nrA = A.getNumberOfRows();
    size_t sB = B.size();
    if (row == true && nrA != sB) throw DimensionException("MatrixTools::hadamardMult(). nrows A != size of B.", nrA, sB);
    if (row == false && ncA != sB) throw DimensionException("MatrixTools::hadamardMult(). ncols A != size of B.", ncA, sB);
    O.resize(nrA, ncA);
    if (row)
    {
      for (size_t i = 0; i < nrA; i++)
      {
        for (size_t j = 0; j < ncA; j++)
        {
          O(i, j) = A(i, j) * B[i];
        }
      }
    }
    else
    {
      for (size_t i = 0; i < nrA; i++)
      {
        for (size_t j = 0; j < ncA; j++)
        {
          O(i, j) = A(i, j) * B[j];
        }
      }
    }
  }
 
  template<class Scalar>
  static void directSum(const Matrix<Scalar>& A, const Matrix<Scalar>& B, Matrix<Scalar>& O)
  {
    size_t ncA = A.getNumberOfColumns();
    size_t nrA = A.getNumberOfRows();
    size_t nrB = B.getNumberOfRows();
    size_t ncB = B.getNumberOfColumns();
    O.resize(nrA + nrB, ncA + ncB);
 
    for (size_t ia = 0; ia < nrA; ia++)
    {
      for (size_t ja = 0; ja < ncA; ja++)
      {
        O(ia, ja) = A(ia, ja);
      }
    }
 
    for (size_t ia = 0; ia < nrA; ia++)
    {
      for (size_t jb = 0; jb < ncB; jb++)
      {
        O(ia, ncA + jb) = 0;
      }
    }
 
    for (size_t ib = 0; ib < nrB; ib++)
    {
      for (size_t ja = 0; ja < ncA; ja++)
      {
        O(nrA + ib, ja) = 0;
      }
    }
 
    for (size_t ib = 0; ib < nrB; ib++)
    {
      for (size_t jb = 0; jb < nrB; jb++)
      {
        O(nrA + ib, ncA + jb) = B(ib, jb);
      }
    }
  }
 
  template<class Scalar>
  static void directSum(const std::vector< Matrix<Scalar>*>& vA, Matrix<Scalar>& O)
  {
    size_t nr = 0;
    size_t nc = 0;
    for (size_t k = 0; k < vA.size(); k++)
    {
      nr += vA[k]->getNumberOfRows();
      nc += vA[k]->getNumberOfColumns();
    }
    O.resize(nr, nc);
    for (size_t k = 0; k < nr; k++)
    {
      for (size_t k2 = 0; k2 < nc; k2++)
      {
        O(k, k2) = 0;
      }
    }
 
    size_t rk = 0; // Row counter
    size_t ck = 0; // Col counter
    for (size_t k = 0; k < vA.size(); k++)
    {
      const Matrix<Scalar>* Ak = vA[k];
      for (size_t i = 0; i < Ak->getNumberOfRows(); i++)
      {
        for (size_t j = 0; j < Ak->getNumberOfColumns(); j++)
        {
          O(rk + i, ck + j) = (*Ak)(i, j);
        }
      }
      rk += Ak->getNumberOfRows();
      ck += Ak->getNumberOfColumns();
    }
  }
 
  template<class Scalar>
  static void toVVdouble(const Matrix<Scalar>& M, std::vector< std::vector<Scalar> >& vO)
  {
    size_t n = M.getNumberOfRows();
    size_t m = M.getNumberOfColumns();
    vO.resize(n);
    for (size_t i = 0; i < n; i++)
    {
      vO[i].resize(m);
      for (size_t j = 0; j < m; j++)
      {
        vO[i][j] = M(i, j);
      }
    }
  }
 
  template<class Scalar>
  static Scalar sumElements(const Matrix<Scalar>& M)
  {
    Scalar sum = 0;
    for (size_t i = 0; i < M.getNumberOfRows(); i++)
    {
      for (size_t j = 0; j < M.getNumberOfColumns(); j++)
      {
        sum += M(i, j);
      }
    }
    return sum;
  }
 
 
  template<class Scalar>
  static Scalar lap(Matrix<Scalar>& assignCost,
                    std::vector<int>& rowSol,
                    std::vector<int>& colSol,
                    std::vector<Scalar>& u,
                    std::vector<Scalar>& v)
  {
    size_t dim = assignCost.getNumberOfRows();
    if (assignCost.getNumberOfColumns() != dim)
      throw Exception("MatrixTools::lap. Cost matrix should be scare.");
 
    bool unassignedFound;
    size_t i, iMin;
    size_t numFree = 0, previousNumFree, f, k, freeRow;
    int i0;
    std::vector<size_t> free(dim); // list of unassigned rows.
    std::vector<size_t> pred(dim); // row-predecessor of column in augmenting/alternating path.
    size_t j, j1, j2, endOfPath, last, low, up;
    std::vector<size_t> colList(dim); // list of columns to be scanned in various ways.
    std::vector<short int> matches(dim, 0); // counts how many times a row could be assigned.
    Scalar min;
    Scalar h;
    size_t uMin, uSubMin;
    Scalar v2;
    std::vector<Scalar> d(dim); // 'cost-distance' in augmenting path calculation.
 
    // Column reduction
    for (j = dim; j > 0; j--)    // reverse order gives better results.
    {
      // find minimum cost over rows.
      min = assignCost(0, j - 1);
      iMin = 0;
      for (i = 1; i < dim; ++i)
      {
        if (assignCost(i, j - 1) < min)
        {
          min = assignCost(i, j - 1);
          iMin = i;
        }
      }
      v[j - 1] = min;
 
      if (++matches[iMin] == 1)
      {
        // init assignment if minimum row assigned for first time.
        rowSol[iMin] = static_cast<int>(j - 1);
        colSol[j - 1] = static_cast<int>(iMin);
      }
      else
        colSol[j - 1] = -1;                                       // row already assigned, column not assigned.
    }
 
    // Reduction tranfer
    for (i = 0; i < dim; i++)
    {
      if (matches[i] == 0)                                  // fill list of unassigned 'free' rows.
        free[numFree++] = i;
      else
      {
        if (matches[i] == 1)   // transfer reduction from rows that are assigned once.
        {
          j1 = static_cast<size_t>(rowSol[i]); // rowSol[i] is >= 0 here
          min = -std::log(0);
          for (j = 0; j < dim; j++)
          {
            if (j != j1)
              if (assignCost(i, j - 1) - v[j] < min)
                min = assignCost(i, j - 1) - v[j - 1];
          }
          v[j1] = v[j1] - min;
        }
      }
    }
 
    // Augmenting row reduction
    short loopcnt = 0;           // do-loop to be done twice.
    do
    {
      loopcnt++;
 
      // scan all free rows.
      // in some cases, a free row may be replaced with another one to be scanned next.
      k = 0;
      previousNumFree = numFree;
      numFree = 0;             // start list of rows still free after augmenting row reduction.
      while (k < previousNumFree)
      {
        i = free[k];
        k++;
 
        // find minimum and second minimum reduced cost over columns.
        uMin = assignCost(i, 0) - v[0];
        j1 = 0;
        uSubMin = static_cast<size_t>(-std::log(0));
        for (j = 1; j < dim; j++)
        {
          h = assignCost(i, j) - v[j];
          if (h < uSubMin)
          {
            if (h >= uMin)
            {
              uSubMin = h;
              j2 = j;
            }
            else
            {
              uSubMin = uMin;
              uMin = h;
              j2 = j1;
              j1 = j;
            }
          }
        }
 
        i0 = colSol[j1];
        if (uMin < uSubMin)
        {
          // change the reduction of the minimum column to increase the minimum
          // reduced cost in the row to the subminimum.
          v[j1] = v[j1] - (uSubMin - uMin);
        }
        else                    // minimum and subminimum equal.
        {
          if (i0 >= 0)         // minimum column j1 is assigned.
          {
            // swap columns j1 and j2, as j2 may be unassigned.
            j1 = j2;
            i0 = colSol[j2];
          }
        }
 
        // (re-)assign i to j1, possibly de-assigning an i0.
        rowSol[i] = static_cast<int>(j1);
        colSol[j1] = static_cast<int>(i);
 
        if (i0 >= 0)            // minimum column j1 assigned earlier.
        {
          if (uMin < uSubMin)
          {
            // put in current k, and go back to that k.
            // continue augmenting path i - j1 with i0.
            free[--k] = static_cast<size_t>(i0);
          }
          else
          {
            // no further augmenting reduction possible.
            // store i0 in list of free rows for next phase.
            free[numFree++] = static_cast<size_t>(i0);
          }
        }
      }
    }
    while (loopcnt < 2);       // repeat once.
 
    // Augment solution for each free row.
    for (f = 0; f < numFree; f++)
    {
      freeRow = free[f];       // start row of augmenting path.
 
      // Dijkstra shortest path algorithm.
      // runs until unassigned column added to shortest path tree.
      for (j = 0; j < dim; j++)
      {
        d[j] = assignCost(freeRow, j) - v[j];
        pred[j] = freeRow;
        colList[j] = j;        // init column list.
      }
 
      low = 0; // columns in 0..low-1 are ready, now none.
      up = 0;  // columns in low..up-1 are to be scanned for current minimum, now none.
      // columns in up..dim-1 are to be considered later to find new minimum,
      // at this stage the list simply contains all columns
      unassignedFound = false;
      do
      {
        if (up == low)         // no more columns to be scanned for current minimum.
        {
          last = low - 1;
 
          // scan columns for up..dim-1 to find all indices for which new minimum occurs.
          // store these indices between low..up-1 (increasing up).
          min = d[colList[up++]];
          for (k = up; k < dim; k++)
          {
            j = colList[k];
            h = d[j];
            if (h <= min)
            {
              if (h < min)     // new minimum.
              {
                up = low;      // restart list at index low.
                min = h;
              }
              // new index with same minimum, put on undex up, and extend list.
              colList[k] = colList[up];
              colList[up++] = j;
            }
          }
 
          // check if any of the minimum columns happens to be unassigned.
          // if so, we have an augmenting path right away.
          for (k = low; k < up; k++)
          {
            if (colSol[colList[k]] < 0)
            {
              endOfPath = colList[k];
              unassignedFound = true;
              break;
            }
          }
        }
 
        if (!unassignedFound)
        {
          // update 'distances' between freerow and all unscanned columns, via next scanned column.
          j1 = colList[low];
          low++;
          i = static_cast<size_t>(colSol[j1]);
          h = assignCost(i, j1) - v[j1] - min;
 
          for (k = up; k < dim; k++)
          {
            j = colList[k];
            v2 = assignCost(i, j) - v[j] - h;
            if (v2 < d[j])
            {
              pred[j] = i;
              if (v2 == min)    // new column found at same minimum value
              {
                if (colSol[j] < 0)
                {
                  // if unassigned, shortest augmenting path is complete.
                  endOfPath = j;
                  unassignedFound = true;
                  break;
                }
                // else add to list to be scanned right away.
                else
                {
                  colList[k] = colList[up];
                  colList[up++] = j;
                }
              }
              d[j] = v2;
            }
          }
        }
      }
      while (!unassignedFound);
 
      // update column prices.
      for (k = 0; k <= last; k++)
      {
        j1 = colList[k];
        v[j1] = v[j1] + d[j1] - min;
      }
 
      // reset row and column assignments along the alternating path.
      do
      {
        i = pred[endOfPath];
        colSol[endOfPath] = static_cast<int>(i);
        j1 = endOfPath;
        endOfPath = static_cast<size_t>(rowSol[i]);
        rowSol[i] = static_cast<int>(j1);
      }
      while (i != freeRow);
    }
 
    // calculate optimal cost.
    Scalar lapCost = 0;
    for (i = 0; i < dim; i++)
    {
      j = static_cast<size_t>(rowSol[i]);
      u[i] = assignCost(i, j) - v[j];
      lapCost = lapCost + assignCost(i, j);
    }
 
    return lapCost;
  }
};
} // end of namespace bpp.
#endif // BPP_NUMERIC_MATRIX_MATRIXTOOLS_H