MatrixTools.h

Go to the documentation of this file.

// SPDX-FileCopyrightText: The Bio++ Development Group
//
// SPDX-License-Identifier: CECILL-2.1

#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)
  {
    if ((a == 1) && (b == 0))
      return;

    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);
        mult(tmp, tmp, 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