sparse.hpp

Go to the documentation of this file.

// Copyright (C) 2022 Petter Nilsson. MIT License.
 
#pragma once
 
#include <numeric>
#include <optional>
 
#include <Eigen/Sparse>
 
namespace smooth::feedback {
 
template<typename Source, int Options>
  requires(std::is_base_of_v<Eigen::EigenBase<Source>, Source>)
inline void block_add(
  Eigen::SparseMatrix<double, Options> & dest,
  Eigen::Index row0,
  Eigen::Index col0,
  const Source & source,
  double scale    = 1,
  bool upper_only = false)
{
  for (auto c = 0; c < source.outerSize(); ++c) {
    for (Eigen::InnerIterator it(source, c); it; ++it) {
      if (!upper_only || row0 + it.row() <= col0 + it.col()) {
        dest.coeffRef(row0 + it.row(), col0 + it.col()) += scale * it.value();
      }
    }
  }
}
 
template<typename Source, int Options>
  requires(std::is_base_of_v<Eigen::EigenBase<Source>, Source>)
inline void block_write(
  Eigen::SparseMatrix<double, Options> & dest,
  Eigen::Index row0,
  Eigen::Index col0,
  const Source & source,
  double scale    = 1,
  bool upper_only = false)
{
  for (auto c = 0; c < source.outerSize(); ++c) {
    for (Eigen::InnerIterator it(source, c); it; ++it) {
      if (!upper_only || row0 + it.row() <= col0 + it.col()) {
        dest.coeffRef(row0 + it.row(), col0 + it.col()) = scale * it.value();
      }
    }
  }
}
 
template<int Options>
inline void block_add_identity(
  Eigen::SparseMatrix<double, Options> & dest, Eigen::Index row0, Eigen::Index col0, Eigen::Index n, double scale = 1)
{
  for (auto k = 0u; k < n; ++k) { dest.coeffRef(row0 + k, col0 + k) += scale; }
}
 
template<int Options>
inline void block_write_identity(
  Eigen::SparseMatrix<double, Options> & dest, Eigen::Index row0, Eigen::Index col0, Eigen::Index n, double scale = 1)
{
  for (auto k = 0u; k < n; ++k) { dest.coeffRef(row0 + k, col0 + k) = scale; }
}
 
template<typename SparseMat>
  requires(std::is_base_of_v<Eigen::SparseCompressedBase<std::decay_t<SparseMat>>, std::decay_t<SparseMat>>)
inline void set_zero(SparseMat && mat)
{
  if (mat.isCompressed()) {
    mat.coeffs().setZero();
  } else {
    for (auto i = 0; i < mat.outerSize(); ++i) {
      for (typename std::decay_t<decltype(mat)>::InnerIterator it(mat, i); it; ++it) { it.valueRef() = 0; }
    }
  }
}
 
template<typename Mat>
  requires(std::is_base_of_v<Eigen::EigenBase<Mat>, Mat>)
uint64_t count_explicit_zeros(const Mat & mat)
{
  uint64_t ret = 0;
  for (auto c = 0; c < mat.outerSize(); ++c) {
    for (Eigen::InnerIterator it(mat, c); it; ++it) {
      if (it.value() == 0) { ++ret; }
    }
  }
  return ret;
}
 
template<typename Mat>
  requires(std::is_base_of_v<Eigen::EigenBase<Mat>, Mat>)
Eigen::MatrixX<typename Mat::Scalar> mark_explicit_zeros(const Mat & mat)
{
  Eigen::MatrixX<typename Mat::Scalar> ret;
  ret.setConstant(mat.rows(), mat.cols(), 0);
  for (auto c = 0; c < mat.outerSize(); ++c) {
    for (Eigen::InnerIterator it(mat, c); it; ++it) {
      if (it.value() == 0) {
        ret(it.row(), it.col()) = 9;
      } else {
        ret(it.row(), it.col()) = 1;
      }
    }
  }
  return ret;
}
 
template<typename S1, typename S2, typename S3, typename S4>
  requires(std::is_base_of_v<Eigen::EigenBase<S1>, S1> && std::is_base_of_v<Eigen::EigenBase<S2>, S2> &&
             std::is_base_of_v<Eigen::EigenBase<S3>, S3> && std::is_base_of_v<Eigen::EigenBase<S4>, S4>)
inline void d2r_fog(
  Eigen::SparseMatrix<double> & out,
  const S1 & Jf,
  const S2 & Hf,
  const S3 & Jg,
  const S4 & Hg,
  Eigen::Index r0 = 0,
  Eigen::Index c0 = 0)
{
  const auto No = Jf.rows();
  const auto Ny = Jf.cols();
 
  [[maybe_unused]] const auto Ni = Jg.rows();
  const auto Nx                  = Jg.cols();
 
  // check some dimensions
  assert(Ny == Ni);
  assert(Hf.rows() == Ny);
  assert(Hf.cols() == No * Ny);
  assert(Hg.rows() == Nx);
  assert(Hg.cols() == Ni * Nx);
 
  for (auto no = 0u; no < No; ++no) {
    // TODO sparse-sparse-sparse product is expensive and allocates temporary
    block_add(out, r0, c0 + no * Nx, Jg.transpose() * Hf.middleCols(no * Ny, Ny) * Jg);
  }
 
  for (auto i = 0u; i < Jf.outerSize(); ++i) {
    for (Eigen::InnerIterator it(Jf, i); it; ++it) {
      block_add(out, r0, c0 + it.row() * Nx, Hg.middleCols(it.col() * Nx, Nx), it.value());
    }
  }
}
 
}  // namespace smooth::feedback