matrix_utils.h

#ifndef MATRIX_ALGEBRA_H
#define MATRIX_ALGEBRA_H
 
#include <eigen3/Eigen/Eigen>
 
namespace math {
Eigen::MatrixXd kron(const Eigen::MatrixXd &m1, const Eigen::MatrixXd &m2) {
  uint32_t m1r = m1.rows();
  uint32_t m1c = m1.cols();
  uint32_t m2r = m2.rows();
  uint32_t m2c = m2.cols();
 
  Eigen::MatrixXd m3(m1r * m2r, m1c * m2c);
 
  for (int i = 0; i < m1r; i++) {
    for (int j = 0; j < m1c; j++) {
      m3.block(i * m2r, j * m2c, m2r, m2c) = m1(i, j) * m2;
    }
  }
 
  return m3;
}
 
Eigen::MatrixXd block_diag(const Eigen::MatrixXd &m1,
                           const Eigen::MatrixXd &m2) {
  uint32_t m1r = m1.rows();
  uint32_t m1c = m1.cols();
  uint32_t m2r = m2.rows();
  uint32_t m2c = m2.cols();
 
  Eigen::MatrixXd mf = Eigen::MatrixXd::Zero(m1r + m2r, m1c + m2c);
  mf.block(0, 0, m1r, m1c) = m1;
  mf.block(m1r, m1c, m2r, m2c) = m2;
 
  return mf;
}
 
Eigen::MatrixXd block_diag(const Eigen::MatrixXd &m1, const Eigen::MatrixXd &m2,
                           const Eigen::MatrixXd &m3) {
  uint32_t m1r = m1.rows();
  uint32_t m1c = m1.cols();
  uint32_t m2r = m2.rows();
  uint32_t m2c = m2.cols();
  uint32_t m3r = m3.rows();
  uint32_t m3c = m3.cols();
 
  Eigen::MatrixXd bdm = Eigen::MatrixXd::Zero(m1r + m2r + m3r, m1c + m2c + m3c);
  bdm.block(0, 0, m1r, m1c) = m1;
  bdm.block(m1r, m1c, m2r, m2c) = m2;
  bdm.block(m1r + m2r, m1c + m2c, m3r, m3c) = m3;
 
  return bdm;
}
 
Eigen::MatrixXd reshape(Eigen::MatrixXd x, uint32_t r, uint32_t c) {
  Eigen::Map<Eigen::MatrixXd> rx(x.data(), r, c);
 
  return rx;
}
}  // namespace math
#endif