matrix-view.hh

Go to the documentation of this file.

// Copyright (c) 2017, Joseph Mirabel
// Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
//
 
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
// 1. Redistributions of source code must retain the above copyright
//    notice, this list of conditions and the following disclaimer.
//
// 2. Redistributions in binary form must reproduce the above copyright
// notice, this list of conditions and the following disclaimer in the
// documentation and/or other materials provided with the distribution.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
// DAMAGE.
 
#ifndef HPP_CONSTRAINTS_MATRIX_VIEW_HH
#define HPP_CONSTRAINTS_MATRIX_VIEW_HH
 
#include <Eigen/Core>
#include <hpp/constraints/fwd.hh>
#include <hpp/pinocchio/util.hh>
#include <hpp/util/indent.hh>
#include <iostream>
#include <vector>
 
#define HPP_EIGEN_USE_EVALUATOR EIGEN_VERSION_AT_LEAST(3, 2, 92)
 
namespace Eigen {


struct BlockIndex {
  typedef hpp::constraints::size_type size_type;
  typedef hpp::constraints::segment_t segment_t;
  typedef hpp::constraints::segments_t segments_t;

  static size_type cardinal(const segments_t& a);

  template <typename Derived>
  static segments_t fromLogicalExpression(
      const Eigen::ArrayBase<Derived>& array);

  static void sort(segments_t& a);

  static void shrink(segments_t& a);

  static bool overlap(const segment_t& a, const segment_t& b);

  static segments_t sum(const segment_t& a, const segment_t& b);

  static void add(segments_t& a, const segment_t& b);

  static void add(segments_t& a, const segments_t& b);

  static segments_t difference(const segment_t& a, const segment_t& b);

  static segments_t difference(const segments_t& a, const segment_t& b);

  static segments_t difference(const segment_t& a, const segments_t& b);

  static segments_t difference(const segments_t& a, const segments_t& b);

  static segments_t split(segments_t& segments, const size_type& cardinal);

  static segments_t extract(const segments_t& segments, size_type start,
                            size_type cardinal);
};  // struct BlockIndex
 
template <typename ArgType, int _Rows, int _Cols, bool _allRows, bool _allCols>
class MatrixBlockView;

template <bool _allRows = false, bool _allCols = false>
class MatrixBlocks;

 
template <bool _allRows = false, bool _allCols = false>
class MatrixBlocksRef;
 
namespace internal {
template <bool condition>
struct static_if {
  template <class Then, class Else>
  static constexpr inline Then& rr(Then& f, Else&) {
    return f;
  }
  template <class Then, class Else>
  static constexpr inline Then pp(Then f, Else) {
    return f;
  }
};
template <>
struct static_if<false> {
  template <class Then, class Else>
  static constexpr inline Else& rr(Then&, Else& s) {
    return s;
  }
  template <class Then, class Else>
  static constexpr inline Else pp(Then, Else s) {
    return s;
  }
};
 
struct empty_struct {
  typedef MatrixXd::Index Index;
  constexpr empty_struct() = default;
  template <typename In_t>
  constexpr empty_struct(In_t) {}
  template <typename In0_t, typename In1_t>
  constexpr empty_struct(In0_t, In1_t) {}
  static constexpr inline Index size() { return 1; }
  inline constexpr const Index& operator[](const Index& i) const { return i; }
};
 
template <bool _allRows, bool _allCols>
struct traits<MatrixBlocks<_allRows, _allCols> > {
  enum { AllRows = _allRows, AllCols = _allCols };
  typedef
      typename internal::conditional<_allRows, internal::empty_struct,
                                     BlockIndex::segments_t>::type RowIndices_t;
  typedef
      typename internal::conditional<_allCols, internal::empty_struct,
                                     BlockIndex::segments_t>::type ColIndices_t;
};
 
template <bool _allRows, bool _allCols>
struct traits<MatrixBlocksRef<_allRows, _allCols> > {
  enum { AllRows = _allRows, AllCols = _allCols };
  typedef typename internal::conditional<_allRows, internal::empty_struct,
                                         const BlockIndex::segments_t&>::type
      RowIndices_t;
  typedef typename internal::conditional<_allCols, internal::empty_struct,
                                         const BlockIndex::segments_t&>::type
      ColIndices_t;
};
 
template <typename ArgType, int _Rows, int _Cols, bool _allRows, bool _allCols>
struct traits<MatrixBlockView<ArgType, _Rows, _Cols, _allRows, _allCols> > {
#if HPP_EIGEN_USE_EVALUATOR
  typedef typename ArgType::StorageIndex StorageIndex;
#else   // HPP_EIGEN_USE_EVALUATOR
  typedef typename ArgType::Index Index;
#endif  // HPP_EIGEN_USE_EVALUATOR
  typedef typename traits<ArgType>::StorageKind StorageKind;
  typedef typename traits<ArgType>::XprKind XprKind;
  typedef typename ArgType::Scalar Scalar;
  enum {
#if !HPP_EIGEN_USE_EVALUATOR
    CoeffReadCost = ArgType::CoeffReadCost,
#endif  // !HPP_EIGEN_USE_EVALUATOR
    Flags = ~PacketAccessBit & ~DirectAccessBit & ~ActualPacketAccessBit &
            ~LinearAccessBit & ArgType::Flags,
    RowsAtCompileTime = (_allRows ? ArgType::RowsAtCompileTime : _Rows),
    ColsAtCompileTime = (_allCols ? ArgType::ColsAtCompileTime : _Cols),
    MaxRowsAtCompileTime = ArgType::MaxRowsAtCompileTime,
    MaxColsAtCompileTime = ArgType::MaxColsAtCompileTime
  };
};
 
#if HPP_EIGEN_USE_EVALUATOR
template <typename Derived, typename ArgType, int _Rows, int _Cols,
          bool _allRows, bool _allCols, typename Functor, typename Scalar>
struct Assignment<Derived,
                  MatrixBlockView<ArgType, _Rows, _Cols, _allRows, _allCols>,
                  Functor, Dense2Dense, Scalar> {
  typedef MatrixBlockView<ArgType, _Rows, _Cols, _allRows, _allCols>
      OtherDerived;
  static EIGEN_STRONG_INLINE void run(Derived& dst, const OtherDerived& src,
                                      const Functor& func) {
    dst.resize(src.rows(), src.cols());
    typedef Block<Derived> BlockDerived;
    typedef Assignment<BlockDerived, typename OtherDerived::BlockConstXprType,
                       Functor>
        AssignmentType;
    for (typename OtherDerived::block_iterator b(src); b.valid(); ++b) {
      BlockDerived bdst(dst.block(b.ro(), b.co(), b.rs(), b.cs()));
      AssignmentType::run(bdst, src._block(b), func);
    }
  }
};
#else   // HPP_EIGEN_USE_EVALUATOR
template <typename Derived, typename ArgType, int _Rows, int _Cols,
          bool _allRows, bool _allCols>
struct assign_selector<
    Derived, MatrixBlockView<ArgType, _Rows, _Cols, _allRows, _allCols>, false,
    false> {
  typedef MatrixBlockView<ArgType, _Rows, _Cols, _allRows, _allCols>
      OtherDerived;
  static EIGEN_STRONG_INLINE Derived& run(Derived& dst,
                                          const OtherDerived& other) {
    other.writeTo(dst);
    return dst;
  }
  template <typename ActualDerived, typename ActualOtherDerived>
  static EIGEN_STRONG_INLINE Derived& evalTo(ActualDerived& dst,
                                             const ActualOtherDerived& other) {
    other.evalTo(dst);
    return dst;
  }
};
template <typename Derived, typename ArgType, int _Rows, int _Cols,
          bool _allRows, bool _allCols>
struct assign_selector<
    Derived, MatrixBlockView<ArgType, _Rows, _Cols, _allRows, _allCols>, false,
    true> {
  typedef MatrixBlockView<ArgType, _Rows, _Cols, _allRows, _allCols>
      OtherDerived;
  static EIGEN_STRONG_INLINE Derived& run(Derived& dst,
                                          const OtherDerived& other) {
    other.writeTo(dst.transpose());
    return dst;
  }
  template <typename ActualDerived, typename ActualOtherDerived>
  static EIGEN_STRONG_INLINE Derived& evalTo(ActualDerived& dst,
                                             const ActualOtherDerived& other) {
    Transpose<ActualDerived> dstTrans(dst);
    other.evalTo(dstTrans);
    return dst;
  }
};
#endif  // HPP_EIGEN_USE_EVALUATOR
 
template <typename Src, typename Dst>
struct eval_matrix_block_view_to {};
template <typename Src, typename _ArgType, int _Rows, int _Cols, bool _allRows,
          bool _allCols>
struct eval_matrix_block_view_to<
    Src, MatrixBlockView<_ArgType, _Rows, _Cols, _allRows, _allCols> > {
  // MatrixBlockView <- matrix
  typedef MatrixBlockView<_ArgType, _Rows, _Cols, _allRows, _allCols> Dst;
  static void run(const Src& src, Dst& dst) {
    for (typename Dst::block_iterator b(dst); b.valid(); ++b)
      dst._block(b) = src.block(b.ro(), b.co(), b.rs(), b.cs());
  }
};
template <typename _ArgType, int _Rows, int _Cols, bool _allRows, bool _allCols,
          typename Dst>
struct eval_matrix_block_view_to<
    MatrixBlockView<_ArgType, _Rows, _Cols, _allRows, _allCols>, Dst> {
  // matrix <- MatrixBlockView
  typedef MatrixBlockView<_ArgType, _Rows, _Cols, _allRows, _allCols> Src;
  static void run(const Src& src, Dst& dst) {
    for (typename Src::block_iterator b(src); b.valid(); ++b)
      dst.block(b.ro(), b.co(), b.rs(), b.cs()) = src._block(b);
  }
};
template <typename _ArgType, int _Rows, int _Cols, bool _allRows, bool _allCols,
          typename _ArgType2, int _Rows2, int _Cols2, bool _allRows2,
          bool _allCols2>
struct eval_matrix_block_view_to<
    MatrixBlockView<_ArgType, _Rows, _Cols, _allRows, _allCols>,
    MatrixBlockView<_ArgType2, _Rows2, _Cols2, _allRows2, _allCols2> > {
  // MatrixBlockView <- MatrixBlockView
  typedef MatrixBlockView<_ArgType, _Rows, _Cols, _allRows, _allCols> Src;
  typedef MatrixBlockView<_ArgType2, _Rows2, _Cols2, _allRows2, _allCols2> Dst;
  static void run(const Src& src, Dst& dst) {
    typename Dst::block_iterator db(dst);
    for (typename Src::block_iterator sb(src); sb.valid(); ++sb) {
      assert(db.valid());
      dst._block(db) = src._block(sb);
      ++db;
    }
    assert(!db.valid());
  }
};
 
template <typename ReturnType, typename View, bool AllRows = View::AllRows,
          bool AllCols = View::AllCols>
struct access_block_from_matrix_block_view {
  typedef typename View::size_type size_type;
  template <typename Derived>
  static ReturnType run(Derived& d, size_type r, size_type c, size_type rs,
                        size_type cs) {
    return ReturnType(d, r, c, rs, cs);
  }
};
template <typename ReturnType, typename View>
struct access_block_from_matrix_block_view<ReturnType, View, false, true> {
  typedef typename View::size_type size_type;
  template <typename Derived>
  static ReturnType run(Derived& d, size_type r, size_type, size_type rs,
                        size_type) {
    return d.middleRows(r, rs);
  }
};
template <typename ReturnType, typename View>
struct access_block_from_matrix_block_view<ReturnType, View, true, false> {
  typedef typename View::size_type size_type;
  template <typename Derived>
  static ReturnType run(Derived& d, size_type, size_type c, size_type,
                        size_type cs) {
    return d.middleCols(c, cs);
  }
};
 
struct dont_print_indices {
  template <typename BlockIndexType>
  static void run(std::ostream&, const BlockIndexType&) {}
};
struct print_indices {
  template <typename BlockIndexType>
  static void run(std::ostream& os, const BlockIndexType& bi) {
    for (std::size_t i = 0; i < bi.size(); ++i)
      os << "[ " << bi[i].first << ", " << bi[i].second << "], ";
  }
};
 
#if HPP_EIGEN_USE_EVALUATOR
template <typename ArgType, int _Rows, int _Cols, bool _allRows, bool _allCols>
struct unary_evaluator<
    MatrixBlockView<ArgType, _Rows, _Cols, _allRows, _allCols> >
    : evaluator_base<
          MatrixBlockView<ArgType, _Rows, _Cols, _allRows, _allCols> > {
  typedef MatrixBlockView<ArgType, _Rows, _Cols, _allRows, _allCols> XprType;
  typedef typename XprType::Scalar Scalar;
 
  enum {
    CoeffReadCost = evaluator<ArgType>::CoeffReadCost,
    Flags = ~PacketAccessBit & ~DirectAccessBit & ~ActualPacketAccessBit &
            ~LinearAccessBit & ArgType::Flags,
    Alignment = 0
  };
  EIGEN_DEVICE_FUNC explicit unary_evaluator(const XprType& view)
      : m_view(view) {}
 
  const XprType& m_view;
};
#endif  // HPP_EIGEN_USE_EVALUATOR
}  // namespace internal
 
#define EIGEN_MATRIX_BLOCKS_PUBLIC_INTERFACE(Derived) \
  enum { AllRows = _allRows, AllCols = _allCols };    \
  typedef MatrixBlocksBase<Derived> Base;             \
  typedef typename Base::size_type size_type;         \
  typedef typename Base::segments_t segments_t;       \
  typedef typename Base::segment_t segment_t;         \
  typedef typename Base::RowIndices_t RowIndices_t;   \
  typedef typename Base::ColIndices_t ColIndices_t;

 
template <typename Derived>
class MatrixBlocksBase {
 public:
  enum {
    AllRows = internal::traits<Derived>::AllRows,
    AllCols = internal::traits<Derived>::AllCols,
    OneDimension = bool(AllRows) || bool(AllCols)
  };
  typedef hpp::constraints::size_type size_type;
  typedef BlockIndex::segment_t segment_t;
  typedef BlockIndex::segments_t segments_t;
  typedef typename internal::traits<Derived>::RowIndices_t RowIndices_t;
  typedef typename internal::traits<Derived>::ColIndices_t ColIndices_t;

  template <typename MatrixType, int _Rows = MatrixType::RowsAtCompileTime,
            int _Cols = MatrixType::ColsAtCompileTime>
  struct View {
    typedef MatrixBlockView<MatrixType, _Rows, _Cols, AllRows, AllCols> type;
  };  // struct View
 
  Derived const& derived() const { return static_cast<Derived const&>(*this); }
  Derived& derived() { return static_cast<Derived&>(*this); }

  template <typename MatrixType>
  EIGEN_STRONG_INLINE typename View<MatrixType>::type lview(
      const MatrixBase<MatrixType>& other) const {
    MatrixType& o = const_cast<MatrixBase<MatrixType>&>(other).derived();
    if (Derived::OneDimension)
      return typename View<MatrixType>::type(o, nbIndices(), indices());
    else
      return typename View<MatrixType>::type(o, nbRows(), rows(), nbCols(),
                                             cols());
  }

  template <typename MatrixType>
  EIGEN_STRONG_INLINE typename View<const MatrixType>::type rview(
      const MatrixBase<MatrixType>& other) const {
    if (Derived::OneDimension)
      return typename View<const MatrixType>::type(other.derived(), nbIndices(),
                                                   indices());
    else
      return typename View<const MatrixType>::type(other.derived(), nbRows(),
                                                   rows(), nbCols(), cols());
  }
 
  MatrixBlocksRef<AllCols, AllRows> transpose() const {
    return MatrixBlocksRef<AllCols, AllRows>(nbCols(), cols(), nbRows(),
                                             rows());
  }
 
  MatrixBlocksRef<AllRows, true> keepRows() const {
    assert(!AllRows);
    return MatrixBlocksRef<AllRows, true>(nbRows(), rows());
  }
 
  MatrixBlocksRef<true, AllCols> keepCols() const {
    assert(!AllCols);
    return MatrixBlocksRef<true, AllCols>(nbCols(), cols());
  }

  inline const segments_t& indices() const {
    return internal::static_if<AllRows>::rr(cols(), rows());
  }

  inline const RowIndices_t& rows() const { return derived().rows(); }

  inline const ColIndices_t& cols() const { return derived().cols(); }

  inline const size_type& nbIndices() const {
    return AllRows ? nbCols() : nbRows();
  }

  inline const size_type& nbRows() const { return derived().nbRows(); }

  inline const size_type& nbCols() const { return derived().nbCols(); }

  MatrixBlocks<AllRows, AllCols> block(size_type i, size_type j, size_type ni,
                                       size_type nj) const {
    return MatrixBlocks<AllRows, AllCols>(BlockIndex::extract(rows(), i, ni),
                                          BlockIndex::extract(cols(), j, nj));
  }

  MatrixBlocks<AllRows, AllCols> middleRows(size_type i, size_type ni) const {
    return MatrixBlocks<AllRows, AllCols>(BlockIndex::extract(rows(), i, ni),
                                          cols());
  }

  MatrixBlocks<AllRows, AllCols> middleCols(size_type j, size_type nj) const {
    return MatrixBlocks<AllRows, AllCols>(rows(),
                                          BlockIndex::extract(cols(), j, nj));
  }
 
 protected:
  MatrixBlocksBase() {}

  MatrixBlocksBase(const MatrixBlocksBase&) {}
};  // class MatrixBlocks
 
template <bool _allRows, bool _allCols>
class MatrixBlocks
    : public MatrixBlocksBase<MatrixBlocks<_allRows, _allCols> > {
 public:
  EIGEN_MATRIX_BLOCKS_PUBLIC_INTERFACE(MatrixBlocks)
 
  
  MatrixBlocks() : m_nbRows(0), m_nbCols(0), m_rows(), m_cols() {}

  MatrixBlocks(const segments_t& rows, const segments_t& cols)
      : m_nbRows(BlockIndex::cardinal(rows)),
        m_nbCols(BlockIndex::cardinal(cols)),
        m_rows(rows),
        m_cols(cols) {}

  MatrixBlocks(const size_type& nbRows, const RowIndices_t& rows,
               const size_type& nbCols, const ColIndices_t& cols)
      : m_nbRows(nbRows), m_nbCols(nbCols), m_rows(rows), m_cols(cols) {}

  MatrixBlocks(size_type start, size_type size)
      : m_nbRows(_allRows ? 0 : size),
        m_nbCols(_allCols ? 0 : size),
        m_rows(1, BlockIndex::segment_t(start, size)),
        m_cols(1, BlockIndex::segment_t(start, size)) {}

  MatrixBlocks(const segments_t& idx)
      : m_nbRows(_allRows ? 0 : BlockIndex::cardinal(idx)),
        m_nbCols(_allCols ? 0 : BlockIndex::cardinal(idx)),
        m_rows(idx),
        m_cols(idx) {}

  MatrixBlocks(const segment_t& idx)
      : m_nbRows(_allRows ? 0 : idx.second),
        m_nbCols(_allCols ? 0 : idx.second),
        m_rows(segments_t(1, idx)),
        m_cols(segments_t(1, idx)) {}

  template <typename MBDerived>
  MatrixBlocks(const MatrixBlocksBase<MBDerived>& other)
      : m_nbRows(other.nbRows()),
        m_nbCols(other.nbCols()),
        m_rows(other.rows()),
        m_cols(other.cols()) {
    EIGEN_STATIC_ASSERT((bool(AllRows) == bool(MBDerived::AllRows)) &&
                            (bool(AllCols) == bool(MBDerived::AllCols)),
                        YOU_MIXED_MATRICES_OF_DIFFERENT_SIZES);
  }
 
  MatrixBlocks(const MatrixBlocks& other) = default;
 
  MatrixBlocks& operator=(const MatrixBlocks& other) {
    m_nbRows = other.m_nbRows;
    m_nbCols = other.m_nbCols;
    m_rows = other.m_rows;
    m_cols = other.m_cols;
    return *this;
  }

  inline void clearRows() {
    m_rows.clear();
    m_nbRows = 0;
  }

  inline void clearCols() {
    m_cols.clear();
    m_nbCols = 0;
  }

  inline void addRow(const size_type& row, const size_type size) {
    m_rows.push_back(segment_t(row, size));
    m_nbRows += size;
  }

  inline void addCol(const size_type& col, const size_type size) {
    m_cols.push_back(segment_t(col, size));
    m_nbCols += size;
  }

  template <bool Sort, bool Shrink, bool Cardinal>
  inline void updateRows() {
    update<Sort, Shrink, Cardinal>(m_rows, m_nbRows);
  }

  template <bool Sort, bool Shrink, bool Cardinal>
  inline void updateCols() {
    update<Sort, Shrink, Cardinal>(m_cols, m_nbCols);
  }

  inline const RowIndices_t& rows() const { return m_rows; }

  inline const ColIndices_t& cols() const { return m_cols; }

  inline const size_type& nbRows() const { return m_nbRows; }

  inline const size_type& nbCols() const { return m_nbCols; }
 
  template <bool Sort, bool Shrink, bool Cardinal>
  inline void updateIndices() {
    update<Sort, Shrink, Cardinal>(
        internal::static_if<_allRows>::rr(m_cols, m_rows),
        _allRows ? m_nbCols : m_nbRows);
  }
 
  size_type m_nbRows, m_nbCols;
  RowIndices_t m_rows;
  ColIndices_t m_cols;
 
 private:
  template <bool Sort, bool Shrink, bool Cardinal>
  static inline void update(segments_t& b, size_type& idx) {
    if (Sort) BlockIndex::sort(b);
    if (Shrink) BlockIndex::shrink(b);
    if (Cardinal) idx = BlockIndex::cardinal(b);
  }
};  // class MatrixBlocks

template <bool _allRows, bool _allCols>
class MatrixBlocksRef
    : public MatrixBlocksBase<MatrixBlocksRef<_allRows, _allCols> > {
 public:
  EIGEN_MATRIX_BLOCKS_PUBLIC_INTERFACE(MatrixBlocksRef)
 
  
  MatrixBlocksRef(const size_type& nbRows, const RowIndices_t& rows,
                  const size_type& nbCols, const ColIndices_t& cols)
      : m_nbRows(nbRows), m_nbCols(nbCols), m_rows(rows), m_cols(cols) {}

  template <typename Derived1, typename Derived2>
  MatrixBlocksRef(const MatrixBlocksBase<Derived1>& rows,
                  const MatrixBlocksBase<Derived2>& cols)
      : m_nbRows(rows.nbIndices()),
        m_nbCols(cols.nbIndices()),
        m_rows(rows.indices()),
        m_cols(cols.indices()) {
    EIGEN_STATIC_ASSERT(
        bool(Derived1::OneDimension) && bool(Derived2::OneDimension),
        YOU_MIXED_MATRICES_OF_DIFFERENT_SIZES);
  }

  MatrixBlocksRef(const size_type& nidx, const segments_t& idx)
      : m_nbRows(_allRows ? 0 : nidx),
        m_nbCols(_allCols ? 0 : nidx),
        m_rows(idx),
        m_cols(idx) {}

  MatrixBlocksRef(const MatrixBlocksRef& other)
      : Base(other),
        m_nbRows(other.nbRows()),
        m_nbCols(other.nbCols()),
        m_rows(other.rows()),
        m_cols(other.cols()) {}

  inline const RowIndices_t& rows() const { return m_rows; }

  inline const ColIndices_t& cols() const { return m_cols; }

  inline const size_type& nbRows() const { return m_nbRows; }

  inline const size_type& nbCols() const { return m_nbCols; }
 
  const size_type m_nbRows, m_nbCols;
  RowIndices_t m_rows;
  ColIndices_t m_cols;
};  // class MatrixBlocksRef
 
template <typename Derived>
std::ostream& operator<<(std::ostream& os,
                         const MatrixBlocksBase<Derived>& mbi) {
  typedef
      typename internal::conditional<Derived::AllRows,
                                     internal::dont_print_indices,
                                     internal::print_indices>::type row_printer;
  typedef
      typename internal::conditional<Derived::AllCols,
                                     internal::dont_print_indices,
                                     internal::print_indices>::type col_printer;
  if (!Derived::AllRows) {
    os << "Rows: ";
    row_printer::run(os, mbi.rows());
    if (!Derived::AllCols) os << hpp::iendl;
  }
  if (!Derived::AllCols) {
    os << "Cols: ";
    col_printer::run(os, mbi.cols());
  }
  return os;
}
 
typedef Eigen::MatrixBlocks<false, true> RowBlockIndices;
typedef Eigen::MatrixBlocks<true, false> ColBlockIndices;

template <typename _ArgType, int _Rows = _ArgType::RowsAtCompileTime,
          int _Cols = _ArgType::ColsAtCompileTime, bool _allRows = false,
          bool _allCols = false>
class MatrixBlockView
    : public MatrixBase<
          MatrixBlockView<_ArgType, _Rows, _Cols, _allRows, _allCols> > {
 public:
  typedef hpp::constraints::size_type size_type;
  enum { Rows = _Rows, Cols = _Cols, AllRows = _allRows, AllCols = _allCols };
  struct block_iterator {
    const MatrixBlockView& view;
    size_type row, col;
    internal::variable_if_dynamic<size_type, (_allRows ? 0 : Dynamic)> _ro;
    internal::variable_if_dynamic<size_type, (_allCols ? 0 : Dynamic)> _co;
    block_iterator(const MatrixBlockView& v)
        : view(v), row(0), col(0), _ro(0), _co(0) {}

    size_type ro() const { return _ro.value(); }
    size_type co() const { return _co.value(); }
    size_type ri() const {
      return internal::static_if<AllRows>::pp(std::make_pair(0, view.m_nbRows),
                                              view.m_rows[row])
          .first;
    }

    size_type ci() const {
      return internal::static_if<AllCols>::pp(std::make_pair(0, view.m_nbCols),
                                              view.m_cols[col])
          .first;
    }

    size_type rs() const {
      return internal::static_if<AllRows>::pp(std::make_pair(0, view.m_nbRows),
                                              view.m_rows[row])
          .second;
    }

    size_type cs() const {
      return internal::static_if<AllCols>::pp(std::make_pair(0, view.m_nbCols),
                                              view.m_cols[col])
          .second;
    }
    // ++it
    block_iterator& operator++() {
      if (!AllRows) {
        _ro.setValue(_ro.value() + rs());
      } else {
        assert(view.m_rows.size() == 1);
        assert(row == 0);
      }
      ++row;
      if (row == (size_type)view.m_rows.size()) {
        row = 0;
        _ro.setValue(0);
        if (!AllCols) {
          _co.setValue(_co.value() + cs());
        }
        ++col;
        if (AllCols) {
          // All the blocks have been iterated on.
          assert(!valid());
        }
        // if (col < (size_type)view.m_cols.size()) _co.setValue(0);
      }
      return *this;
    };
    // it++
    block_iterator operator++(int) {
      block_iterator copy(*this);
      operator++();
      return copy;
    };
    constexpr bool valid() const {
      return (AllRows || view.m_rows.size() > 0) &&
             (col < (size_type)(AllCols ? 1 : view.m_cols.size()));
    }
  };
  typedef MatrixBase<
      MatrixBlockView<_ArgType, _Rows, _Cols, _allRows, _allCols> >
      Base;
  EIGEN_GENERIC_PUBLIC_INTERFACE(MatrixBlockView)
 
  typedef Matrix<Scalar, RowsAtCompileTime, ColsAtCompileTime> PlainObject;
  // typedef typename internal::ref_selector<MatrixBlockView>::type Nested;
  typedef _ArgType ArgType;
  typedef typename internal::ref_selector<ArgType>::type ArgTypeNested;
  typedef typename internal::remove_all<ArgType>::type NestedExpression;
  // typedef typename Base::CoeffReturnType CoeffReturnType;
  // typedef typename Base::Scalar Scalar;
 
  template <typename Derived>
  struct block_t {
    typedef Block<Derived,
                  (AllRows ? Derived::RowsAtCompileTime : Eigen::Dynamic),
                  (AllCols ? Derived::ColsAtCompileTime : Eigen::Dynamic),
                  (AllCols ? (bool)Derived::IsRowMajor
                           : (AllRows ? (bool)!Derived::IsRowMajor : false))>
        type;
  };
  typedef typename block_t<ArgType>::type BlockXprType;
  typedef typename block_t<const ArgType>::type BlockConstXprType;
 
  typedef MatrixBlocks<_allRows, _allCols> MatrixIndices_t;
  typedef typename MatrixIndices_t::segments_t Indices_t;
  typedef typename internal::conditional<_allRows, const internal::empty_struct,
                                         const Indices_t&>::type RowIndices_t;
  typedef typename internal::conditional<_allCols, const internal::empty_struct,
                                         const Indices_t&>::type ColIndices_t;
 
  // using Base::operator=;
 
  MatrixBlockView(ArgType& arg, const size_type& nbRows,
                  const RowIndices_t rows, const size_type& nbCols,
                  const ColIndices_t cols)
      : m_arg(arg),
        m_nbRows(nbRows),
        m_rows(rows),
        m_nbCols(nbCols),
        m_cols(cols) {}

  MatrixBlockView(ArgType& arg, const size_type& nbIndices,
                  const Indices_t& indices)
      : m_arg(arg),
        m_nbRows(_allRows ? arg.rows() : nbIndices),
        m_rows(indices),
        m_nbCols(_allCols ? arg.cols() : nbIndices),
        m_cols(indices) {}
 
  EIGEN_STRONG_INLINE size_type rows() const { return m_nbRows; }
  EIGEN_STRONG_INLINE size_type cols() const { return m_nbCols; }
 
  EIGEN_STRONG_INLINE CoeffReturnType coeff(size_type index) const {
    assert(false &&
           "It is not possible to access the coefficients of "
           "MatrixBlockView this way.");
  }
  EIGEN_STRONG_INLINE CoeffReturnType coeff(size_type row,
                                            size_type col) const {
    assert(false &&
           "It is not possible to access the coefficients of "
           "MatrixBlockView this way.");
  }
  EIGEN_STRONG_INLINE Scalar& coeffRef(size_type index) {
    assert(false &&
           "It is not possible to access the coefficients of "
           "MatrixBlockView this way.");
  }
  EIGEN_STRONG_INLINE Scalar& coeffRef(size_type row, const size_type& col) {
    assert(false &&
           "It is not possible to access the coefficients of "
           "MatrixBlockView this way.");
  }
  template <typename Dest>
  EIGEN_STRONG_INLINE void evalTo(Dest& dst) const {
    internal::eval_matrix_block_view_to<MatrixBlockView, Dest>::run(*this, dst);
  }
 
  template <typename Dest>
  EIGEN_STRONG_INLINE void writeTo(Dest& dst) const {
    dst.resize(rows(), cols());
    evalTo(dst.derived());
  }
 
  EIGEN_STRONG_INLINE PlainObject eval() const {
    PlainObject dst;
    writeTo(dst);
    return dst;
  }
 
  template <typename OtherDerived>
  EIGEN_STRONG_INLINE MatrixBlockView& operator=(
      const EigenBase<OtherDerived>& other) {
    EIGEN_STATIC_ASSERT_LVALUE(ArgType);
    internal::eval_matrix_block_view_to<OtherDerived, MatrixBlockView>::run(
        other.derived(), *this);
    return *this;
  }
 
  EIGEN_STRONG_INLINE size_type _blocks() const {
    return m_rows.size() * m_cols.size();
  }
  EIGEN_STRONG_INLINE BlockXprType _block(const block_iterator& b) {
    return internal::access_block_from_matrix_block_view<
        BlockXprType, MatrixBlockView>::template run<ArgType>(m_arg, b.ri(),
                                                              b.ci(), b.rs(),
                                                              b.cs());
  }
  EIGEN_STRONG_INLINE const BlockConstXprType
  _block(const block_iterator& b) const {
    return internal::access_block_from_matrix_block_view<
        const BlockConstXprType,
        MatrixBlockView>::template run<const ArgType>(m_arg, b.ri(), b.ci(),
                                                      b.rs(), b.cs());
  }
  EIGEN_STRONG_INLINE block_iterator _block_iterator() const {
    return block_iterator(*this);
  }
 
  EIGEN_STRONG_INLINE bool isZero(
      const RealScalar& prec = NumTraits<Scalar>::dummy_precision()) const {
    for (block_iterator block(*this); block.valid(); ++block)
      if (!m_arg.block(block.ri(), block.ci(), block.rs(), block.cs())
               .isZero(prec))
        return false;
    return true;
  }
 
  EIGEN_STRONG_INLINE MatrixBlockView &setZero() { return setConstant(Scalar(0)); }
  EIGEN_STRONG_INLINE MatrixBlockView &setConstant(const Scalar &value) {
    for (block_iterator block(*this); block.valid(); ++block)
      _block(block).setConstant(value);
    return *this;
  }
 
  ArgType& m_arg;
  size_type m_nbRows;
  RowIndices_t m_rows;
  size_type m_nbCols;
  ColIndices_t m_cols;
};  // MatrixBlockView

 
}  // namespace Eigen
 
#include <hpp/constraints/impl/matrix-view-operation.hh>
#include <hpp/constraints/impl/matrix-view.hh>
 
#undef HPP_EIGEN_USE_EVALUATOR
 
namespace hpp {
template <int Option>
struct prettyPrint<constraints::segment_t, Option> {
  static std::ostream& run(std::ostream& os, const constraints::segment_t& s) {
    return os << "[ " << s.first << ", " << s.first + s.second << " ]";
  }
};
}  // namespace hpp
 
#endif  // HPP_CONSTRAINTS_MATRIX_VIEW_HH