Workspace.h

Go to the documentation of this file.

/* Copyright 2020 CNRS-AIST JRL */
 
#pragma once
 
#include <Eigen/Core>
 
#include <jrl-qp/api.h>
 
namespace jrl::qp::internal
{
struct NotConst
{
};
 
template<typename Scalar = double>
class Workspace
{
public:
  Workspace() : buffer_(0) {}
  Workspace(int size) : buffer_(size) {}
  Workspace(int rows, int cols) : Workspace(rows * cols) {}
 
  void resize(int size, bool fit = false)
  {
    if(size > buffer_.size() || (size < buffer_.size() && fit))
    {
      buffer_.resize(size);
    }
  }
 
  int size() const
  {
    return static_cast<int>(buffer_.size());
  }
 
  void resize(int rows, int cols, bool fit = false)
  {
    resize(rows * cols, fit);
  }
 
  void changeLd(int rows, int cols, int oldLd, int newLd)
  {
    if(oldLd < newLd)
    {
      assert(cols * newLd < buffer_.size());
      for(int i = cols - 1; i > 0; --i)
      {
        void * src = reinterpret_cast<void *>(&buffer_[i * oldLd]);
        void * dst = reinterpret_cast<void *>(&buffer_[i * newLd]);
        memmove(dst, src, rows * sizeof(double));
      }
    }
    else if(oldLd > newLd)
    {
      for(int i = 1; i < cols; ++i)
      {
        void * src = reinterpret_cast<void *>(&buffer_[i * oldLd]);
        void * dst = reinterpret_cast<void *>(&buffer_[i * newLd]);
        memmove(dst, src, rows * sizeof(double));
      }
    }
  }
 
  auto asVector(int size, NotConst = {})
  {
    assert(size <= buffer_.size());
    return buffer_.head(size);
  }
 
  auto asVector(int size) const
  {
    assert(size <= buffer_.size());
    return buffer_.head(size);
  }
 
  auto asMatrix(int rows, int cols, int ld, NotConst = {})
  {
    assert(ld * cols <= buffer_.size());
    assert(ld >= rows);
    return Eigen::Map<Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>, 0, Eigen::Stride<Eigen::Dynamic, 1>>(
        buffer_.data(), rows, cols, Eigen::Stride<Eigen::Dynamic, 1>(ld, 1));
  }
 
  auto asMatrix(int rows, int cols, int ld) const
  {
    assert(ld * cols <= buffer_.size());
    assert(ld >= rows);
    return Eigen::Map<const Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>, 0, Eigen::Stride<Eigen::Dynamic, 1>>(
        buffer_.data(), rows, cols, Eigen::Stride<Eigen::Dynamic, 1>(ld, 1));
  }
 
  void setZero()
  {
    buffer_.setZero();
  }
 
private:
  Eigen::Matrix<Scalar, Eigen::Dynamic, 1> buffer_;
};
} // namespace jrl::qp::internal
 
namespace jrl::qp
{
using WVector = decltype(internal::Workspace<double>().asVector(0));
using WConstVector = decltype(std::add_const_t<internal::Workspace<double>>().asVector(0));
} // namespace jrl::qp