hpp-constraints  4.11.0
Definition of basic geometric constraints for motion planning
matrix-view.hh
Go to the documentation of this file.
1 // Copyright (c) 2017, Joseph Mirabel
2 // Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
3 //
4 // This file is part of hpp-constraints.
5 // hpp-constraints is free software: you can redistribute it
6 // and/or modify it under the terms of the GNU Lesser General Public
7 // License as published by the Free Software Foundation, either version
8 // 3 of the License, or (at your option) any later version.
9 //
10 // hpp-constraints is distributed in the hope that it will be
11 // useful, but WITHOUT ANY WARRANTY; without even the implied warranty
12 // of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
13 // General Lesser Public License for more details. You should have
14 // received a copy of the GNU Lesser General Public License along with
15 // hpp-constraints. If not, see <http://www.gnu.org/licenses/>.
16 
17 #ifndef HPP_CONSTRAINTS_MATRIX_VIEW_HH
18 #define HPP_CONSTRAINTS_MATRIX_VIEW_HH
19 
20 #include <Eigen/Core>
21 #include <vector>
22 #include <iostream>
23 #include <hpp/util/indent.hh>
24 #include <hpp/pinocchio/util.hh>
25 #include <hpp/constraints/fwd.hh>
26 
27 # define HPP_EIGEN_USE_EVALUATOR EIGEN_VERSION_AT_LEAST(3,2,92)
28 
29 namespace Eigen {
30 
33 
37  struct BlockIndex {
44 
47  static size_type cardinal (const segments_t& a);
48 
53  template <typename Derived>
54  static segments_t fromLogicalExpression
55  (const Eigen::ArrayBase<Derived>& array);
56 
59  static void sort (segments_t& a);
60 
64  static void shrink (segments_t& a);
65 
67  static bool overlap (const segment_t& a, const segment_t& b);
68 
70  static segments_t sum (const segment_t& a, const segment_t& b);
71 
73  static void add (segments_t& a, const segment_t& b);
74 
76  static void add (segments_t& a, const segments_t& b);
77 
79  static segments_t difference (const segment_t& a, const segment_t& b);
80 
83  static segments_t difference (const segments_t& a, const segment_t& b);
84 
87  static segments_t difference (const segment_t& a, const segments_t& b);
88 
91  static segments_t difference (const segments_t& a, const segments_t& b);
92 
99  static segments_t split (segments_t& segments, const size_type& cardinal);
100 
107  static segments_t extract (const segments_t& segments, size_type start,
108  size_type cardinal);
109  }; // struct BlockIndex
110 
111  template <typename ArgType, int _Rows, int _Cols, bool _allRows, bool _allCols> class MatrixBlockView;
112 
123  template <bool _allRows = false, bool _allCols = false> class MatrixBlocks;
124 
126 
127  template <bool _allRows = false, bool _allCols = false> class MatrixBlocksRef;
128 
129  namespace internal {
130  template <bool condition> struct static_if {
131  template <class Then, class Else> static constexpr inline Then& rr (Then& f, Else&) { return f; }
132  template <class Then, class Else> static constexpr inline Then pp (Then f, Else ) { return f; }
133  };
134  template <> struct static_if <false> {
135  template <class Then, class Else> static constexpr inline Else& rr (Then&, Else& s) { return s; }
136  template <class Then, class Else> static constexpr inline Else pp (Then , Else s) { return s; }
137  };
138 
139  struct empty_struct {
140  typedef MatrixXd::Index Index;
141  constexpr empty_struct () = default;
142  template <typename In_t> constexpr empty_struct (In_t) {}
143  template <typename In0_t, typename In1_t> constexpr empty_struct (In0_t, In1_t) {}
144  static constexpr inline Index size() { return 1; }
145  inline constexpr const Index& operator[](const Index& i) const { return i; }
146  };
147 
148  template <bool _allRows, bool _allCols>
149  struct traits< MatrixBlocks <_allRows, _allCols> >
150  {
151  enum {
152  AllRows = _allRows,
153  AllCols = _allCols
154  };
155  typedef typename internal::conditional<_allRows, internal::empty_struct, BlockIndex::segments_t>::type RowIndices_t;
156  typedef typename internal::conditional<_allCols, internal::empty_struct, BlockIndex::segments_t>::type ColIndices_t;
157  };
158 
159  template <bool _allRows, bool _allCols>
160  struct traits< MatrixBlocksRef <_allRows, _allCols> >
161  {
162  enum {
163  AllRows = _allRows,
164  AllCols = _allCols
165  };
166  typedef typename internal::conditional<_allRows, internal::empty_struct, const BlockIndex::segments_t&>::type RowIndices_t;
167  typedef typename internal::conditional<_allCols, internal::empty_struct, const BlockIndex::segments_t&>::type ColIndices_t;
168  };
169 
170  template <typename ArgType, int _Rows, int _Cols, bool _allRows, bool _allCols>
171  struct traits< MatrixBlockView <ArgType, _Rows, _Cols, _allRows, _allCols> >
172  {
173 # if HPP_EIGEN_USE_EVALUATOR
174  typedef typename ArgType::StorageIndex StorageIndex;
175 # else // HPP_EIGEN_USE_EVALUATOR
176  typedef typename ArgType::Index Index;
177 # endif // HPP_EIGEN_USE_EVALUATOR
178  typedef typename traits<ArgType>::StorageKind StorageKind;
179  typedef typename traits<ArgType>::XprKind XprKind;
180  typedef typename ArgType::Scalar Scalar;
181  enum {
182 # if !HPP_EIGEN_USE_EVALUATOR
183  CoeffReadCost = ArgType::CoeffReadCost,
184 # endif // !HPP_EIGEN_USE_EVALUATOR
185  Flags = ~PacketAccessBit & ~DirectAccessBit & ~ActualPacketAccessBit & ~LinearAccessBit & ArgType::Flags,
186  RowsAtCompileTime = (_allRows ? ArgType::RowsAtCompileTime : _Rows),
187  ColsAtCompileTime = (_allCols ? ArgType::ColsAtCompileTime : _Cols),
188  MaxRowsAtCompileTime = ArgType::MaxRowsAtCompileTime,
189  MaxColsAtCompileTime = ArgType::MaxColsAtCompileTime
190  };
191  };
192 
193 # if HPP_EIGEN_USE_EVALUATOR
194  template<typename Derived, typename ArgType, int _Rows, int _Cols, bool _allRows, bool _allCols, typename Functor, typename Scalar>
195  struct Assignment<Derived, MatrixBlockView <ArgType, _Rows, _Cols, _allRows, _allCols>, Functor, Dense2Dense, Scalar> {
197  static EIGEN_STRONG_INLINE void run(Derived& dst, const OtherDerived& src, const Functor& func) {
198  dst.resize(src.rows(), src.cols());
199  typedef Block<Derived> BlockDerived;
200  typedef Assignment<BlockDerived, typename OtherDerived::BlockConstXprType, Functor> AssignmentType;
201  for (typename OtherDerived::block_iterator b (src); b.valid(); ++b) {
202  BlockDerived bdst (dst.block(b.ro(), b.co(), b.rs(), b.cs()));
203  AssignmentType::run(bdst, src._block(b), func);
204  }
205  }
206  };
207 # else // HPP_EIGEN_USE_EVALUATOR
208  template<typename Derived, typename ArgType, int _Rows, int _Cols, bool _allRows, bool _allCols>
209  struct assign_selector<Derived, MatrixBlockView <ArgType, _Rows, _Cols, _allRows, _allCols>,false,false> {
211  static EIGEN_STRONG_INLINE Derived& run(Derived& dst, const OtherDerived& other) { other.writeTo(dst); return dst; }
212  template<typename ActualDerived, typename ActualOtherDerived>
213  static EIGEN_STRONG_INLINE Derived& evalTo(ActualDerived& dst, const ActualOtherDerived& other) { other.evalTo(dst); return dst; }
214  };
215  template<typename Derived, typename ArgType, int _Rows, int _Cols, bool _allRows, bool _allCols>
216  struct assign_selector<Derived, MatrixBlockView <ArgType, _Rows, _Cols, _allRows, _allCols>,false,true> {
218  static EIGEN_STRONG_INLINE Derived& run(Derived& dst, const OtherDerived& other) { other.writeTo(dst.transpose()); return dst; }
219  template<typename ActualDerived, typename ActualOtherDerived>
220  static EIGEN_STRONG_INLINE Derived& evalTo(ActualDerived& dst, const ActualOtherDerived& other) { Transpose<ActualDerived> dstTrans(dst); other.evalTo(dstTrans); return dst; }
221  };
222 # endif // HPP_EIGEN_USE_EVALUATOR
223 
224  template <typename Src, typename Dst> struct eval_matrix_block_view_to {};
225  template <typename Src, typename _ArgType, int _Rows, int _Cols, bool _allRows, bool _allCols>
226  struct eval_matrix_block_view_to <Src, MatrixBlockView<_ArgType, _Rows, _Cols, _allRows, _allCols> > {
227  // MatrixBlockView <- matrix
229  static void run (const Src& src, Dst& dst) {
230  for (typename Dst::block_iterator b (dst); b.valid(); ++b)
231  dst._block(b) = src.block(b.ro(), b.co(), b.rs(), b.cs());
232  }
233  };
234  template <typename _ArgType, int _Rows, int _Cols, bool _allRows, bool _allCols, typename Dst>
235  struct eval_matrix_block_view_to <MatrixBlockView<_ArgType, _Rows, _Cols, _allRows, _allCols>, Dst > {
236  // matrix <- MatrixBlockView
238  static void run (const Src& src, Dst& dst) {
239  for (typename Src::block_iterator b (src); b.valid(); ++b)
240  dst.block(b.ro(), b.co(), b.rs(), b.cs()) = src._block(b);
241  }
242  };
243  template <typename _ArgType , int _Rows , int _Cols , bool _allRows , bool _allCols ,
244  typename _ArgType2, int _Rows2, int _Cols2, bool _allRows2, bool _allCols2>
246  MatrixBlockView<_ArgType , _Rows , _Cols , _allRows , _allCols >,
247  MatrixBlockView<_ArgType2, _Rows2, _Cols2, _allRows2, _allCols2> > {
248  // MatrixBlockView <- MatrixBlockView
251  static void run (const Src& src, Dst& dst) {
252  typename Dst::block_iterator db (dst);
253  for (typename Src::block_iterator sb (src); sb.valid(); ++sb) {
254  assert (db.valid());
255  dst._block(db) = src._block(sb);
256  ++db;
257  }
258  assert (!db.valid());
259  }
260  };
261 
262  template <typename ReturnType, typename View, bool AllRows = View::AllRows, bool AllCols = View::AllCols>
264  {
265  typedef typename View::size_type size_type;
266  template <typename Derived>
267  static ReturnType run (Derived& d, size_type r, size_type c, size_type rs, size_type cs)
268  {
269  return ReturnType (d, r, c, rs, cs);
270  }
271  };
272  template <typename ReturnType, typename View>
273  struct access_block_from_matrix_block_view <ReturnType, View, false, true>
274  {
275  typedef typename View::size_type size_type;
276  template <typename Derived>
277  static ReturnType run (Derived& d, size_type r, size_type, size_type rs, size_type)
278  {
279  return d.middleRows (r, rs);
280  }
281  };
282  template <typename ReturnType, typename View>
283  struct access_block_from_matrix_block_view <ReturnType, View, true, false>
284  {
285  typedef typename View::size_type size_type;
286  template <typename Derived>
287  static ReturnType run (Derived& d, size_type, size_type c, size_type, size_type cs)
288  {
289  return d.middleCols (c, cs);
290  }
291  };
292 
293  struct dont_print_indices { template <typename BlockIndexType> static void run (std::ostream&, const BlockIndexType&) {} };
294  struct print_indices {
295  template <typename BlockIndexType>
296  static void run (std::ostream& os, const BlockIndexType& bi) {
297  for (std::size_t i = 0; i < bi.size(); ++i)
298  os << "[ " << bi[i].first << ", " << bi[i].second << "], ";
299  }
300  };
301 
302 # if HPP_EIGEN_USE_EVALUATOR
303  template <typename ArgType, int _Rows, int _Cols, bool _allRows, bool _allCols>
304  struct unary_evaluator <MatrixBlockView <ArgType, _Rows, _Cols, _allRows, _allCols> >
305  : evaluator_base <MatrixBlockView <ArgType, _Rows, _Cols, _allRows, _allCols> >
306  {
308 
309  enum {
310  CoeffReadCost = evaluator<ArgType>::CoeffReadCost,
311  Flags = ~PacketAccessBit & ~DirectAccessBit & ~ActualPacketAccessBit & ~LinearAccessBit & ArgType::Flags,
312  Alignment = 0
313  };
314  EIGEN_DEVICE_FUNC explicit unary_evaluator (const XprType& view)
315  : m_view (view)
316  {}
317 
318  const XprType& m_view;
319  };
320 # endif // HPP_EIGEN_USE_EVALUATOR
321  } // namespace internal
322 
323 #define EIGEN_MATRIX_BLOCKS_PUBLIC_INTERFACE(Derived) \
324  enum { \
325  AllRows = _allRows, \
326  AllCols = _allCols \
327  }; \
328  typedef MatrixBlocksBase<Derived> Base; \
329  typedef typename Base::size_type size_type; \
330  typedef typename Base::segments_t segments_t; \
331  typedef typename Base::segment_t segment_t; \
332  typedef typename Base::RowIndices_t RowIndices_t; \
333  typedef typename Base::ColIndices_t ColIndices_t;
334 
337 
338  template <typename Derived>
340  {
341  public:
342  enum {
343  AllRows = internal::traits<Derived>::AllRows,
344  AllCols = internal::traits<Derived>::AllCols,
345  OneDimension = bool(AllRows) || bool(AllCols)
346  };
353  typedef typename internal::traits<Derived>::RowIndices_t RowIndices_t;
354  typedef typename internal::traits<Derived>::ColIndices_t ColIndices_t;
355 
357  template <typename MatrixType, int _Rows = MatrixType::RowsAtCompileTime, int _Cols = MatrixType::ColsAtCompileTime> struct View {
359  }; // struct View
360 
361  Derived const& derived () const { return static_cast<Derived const&> (*this); }
362  Derived & derived () { return static_cast<Derived &> (*this); }
363 
368  template <typename MatrixType>
369  EIGEN_STRONG_INLINE typename View<MatrixType>::type lview(const MatrixBase<MatrixType>& other) const {
370  MatrixType& o = const_cast<MatrixBase<MatrixType>&>(other).derived();
371  if (Derived::OneDimension)
372  return typename View<MatrixType>::type (o, nbIndices(), indices());
373  else
374  return typename View<MatrixType>::type (o, nbRows(), rows(), nbCols(), cols());
375  }
376 
381  template <typename MatrixType>
382  EIGEN_STRONG_INLINE typename View<const MatrixType>::type rview(const MatrixBase<MatrixType>& other) const {
383  if (Derived::OneDimension)
384  return typename View<const MatrixType>::type (other.derived(), nbIndices(), indices());
385  else
386  return typename View<const MatrixType>::type (other.derived(), nbRows(), rows(), nbCols(), cols());
387  }
388 
390  {
391  return MatrixBlocksRef<AllCols, AllRows> (nbCols(), cols(), nbRows(), rows());
392  }
393 
395  {
396  assert (!AllRows);
397  return MatrixBlocksRef<AllRows, true> (nbRows(), rows());
398  }
399 
401  {
402  assert (!AllCols);
403  return MatrixBlocksRef<true, AllCols> (nbCols(), cols());
404  }
405 
411  inline const segments_t& indices() const
412  {
413  return internal::static_if<AllRows>::rr(cols(), rows());
414  }
415 
418  inline const RowIndices_t& rows() const
419  {
420  return derived().rows();
421  }
422 
425  inline const ColIndices_t& cols() const
426  {
427  return derived().cols();
428  }
429 
435  inline const size_type& nbIndices() const
436  {
437  return AllRows ? nbCols() : nbRows();
438  }
439 
442  inline const size_type& nbRows() const
443  {
444  return derived().nbRows();
445  }
446 
449  inline const size_type& nbCols() const
450  {
451  return derived().nbCols();
452  }
453 
458  (size_type i, size_type j, size_type ni, size_type nj) const
459  {
460  return MatrixBlocks <AllRows, AllCols> (BlockIndex::extract (rows (), i, ni),
461  BlockIndex::extract (cols (), j, nj));
462  }
463 
468  (size_type i, size_type ni) const
469  {
470  return MatrixBlocks <AllRows, AllCols> (BlockIndex::extract (rows (), i, ni),
471  cols ());
472  }
473 
478  (size_type j, size_type nj) const
479  {
480  return MatrixBlocks <AllRows, AllCols> (rows (),
481  BlockIndex::extract (cols (), j, nj));
482  }
483 
484  protected:
487 
490  }; // class MatrixBlocks
491 
492  template <bool _allRows, bool _allCols>
493  class MatrixBlocks : public MatrixBlocksBase < MatrixBlocks <_allRows, _allCols> >
494  {
495  public:
497 
498 
499  MatrixBlocks () : m_nbRows(0), m_nbCols(0), m_rows(), m_cols() {}
500 
505  MatrixBlocks (const segments_t& rows,
506  const segments_t& cols) :
507  m_nbRows(BlockIndex::cardinal(rows)),
508  m_nbCols(BlockIndex::cardinal(cols)), m_rows(rows), m_cols(cols)
509  {
510 # ifndef NDEBUG
511  // test that input is sorted
512  segments_t r (rows); BlockIndex::sort (r);
513  assert (r == rows);
514  segments_t c (cols); BlockIndex::sort (c);
515  assert (c == cols);
516 #endif
517  }
518 
525  MatrixBlocks (const size_type& nbRows, const RowIndices_t& rows,
526  const size_type& nbCols, const ColIndices_t& cols) :
527  m_nbRows(nbRows), m_nbCols(nbCols), m_rows(rows), m_cols(cols)
528  {
529 # ifndef NDEBUG
530  // test that input is sorted
531  segments_t r (rows); BlockIndex::sort (r);
532  assert (r == rows);
533  segments_t c (cols); BlockIndex::sort (c);
534  assert (c == cols);
535 #endif
536  }
537 
543  MatrixBlocks (size_type start, size_type size)
544  : m_nbRows(_allRows ? 0 : size)
545  , m_nbCols(_allCols ? 0 : size)
546  , m_rows(1, BlockIndex::segment_t (start, size))
547  , m_cols(1, BlockIndex::segment_t (start, size))
548  {}
549 
555  MatrixBlocks (const segments_t& idx)
556  : m_nbRows(_allRows ? 0 : BlockIndex::cardinal(idx))
557  , m_nbCols(_allCols ? 0 : BlockIndex::cardinal(idx))
558  , m_rows(idx), m_cols(idx)
559  {}
560 
565  MatrixBlocks (const segment_t& idx)
566  : m_nbRows(_allRows ? 0 : idx.second)
567  , m_nbCols(_allCols ? 0 : idx.second)
568  , m_rows(segments_t(1,idx)), m_cols(segments_t(1,idx))
569  {}
570 
572  template <typename MBDerived>
574  : m_nbRows(other.nbRows())
575  , m_nbCols(other.nbCols())
576  , m_rows(other.rows()), m_cols(other.cols())
577  {
578  EIGEN_STATIC_ASSERT(
579  (bool(AllRows) == bool(MBDerived::AllRows)) && (bool(AllCols) == bool(MBDerived::AllCols)),
580  YOU_MIXED_MATRICES_OF_DIFFERENT_SIZES);
581  }
582 
584  inline void clearRows ()
585  {
586  m_rows.clear();
587  m_nbRows = 0;
588  }
589 
591  inline void clearCols ()
592  {
593  m_cols.clear();
594  m_nbCols = 0;
595  }
596 
600  inline void addRow (const size_type& row, const size_type size)
601  {
602  m_rows.push_back(segment_t (row, size));
603  m_nbRows += size;
604  }
605 
609  inline void addCol (const size_type& col, const size_type size)
610  {
611  m_cols.push_back(segment_t (col, size));
612  m_nbCols += size;
613  }
614 
619  template<bool Sort, bool Shrink, bool Cardinal>
620  inline void updateRows() {
621  update<Sort, Shrink, Cardinal> (m_rows, m_nbRows);
622  }
623 
628  template<bool Sort, bool Shrink, bool Cardinal>
629  inline void updateCols() {
630  update<Sort, Shrink, Cardinal> (m_cols, m_nbCols);
631  }
632 
635  inline const RowIndices_t& rows() const
636  {
637  return m_rows;
638  }
639 
642  inline const ColIndices_t& cols() const
643  {
644  return m_cols;
645  }
646 
649  inline const size_type& nbRows() const
650  {
651  return m_nbRows;
652  }
653 
656  inline const size_type& nbCols() const
657  {
658  return m_nbCols;
659  }
660 
661  template<bool Sort, bool Shrink, bool Cardinal>
662  inline void updateIndices() {
663  update<Sort, Shrink, Cardinal> (
664  internal::static_if<_allRows>::rr(m_cols, m_rows),
665  _allRows ? m_nbCols : m_nbRows);
666  }
667 
668  size_type m_nbRows, m_nbCols;
669  RowIndices_t m_rows;
670  ColIndices_t m_cols;
671 
672  private:
673  template<bool Sort, bool Shrink, bool Cardinal>
674  static inline void update(segments_t& b, size_type& idx) {
675  if (Sort) BlockIndex::sort(b);
676  if (Shrink) BlockIndex::shrink(b);
677  if (Cardinal) idx = BlockIndex::cardinal(b);
678  }
679  }; // class MatrixBlocks
680 
682  template <bool _allRows, bool _allCols>
683  class MatrixBlocksRef : public MatrixBlocksBase < MatrixBlocksRef <_allRows, _allCols> >
684  {
685  public:
687 
688 
689  MatrixBlocksRef (const size_type& nbRows, const RowIndices_t& rows,
693  const size_type& nbCols, const ColIndices_t& cols) :
694  m_nbRows(nbRows), m_nbCols(nbCols),
695  m_rows (rows ), m_cols (cols )
696  {}
697 
699  template <typename Derived1, typename Derived2>
701  const MatrixBlocksBase<Derived2>& cols) :
702  m_nbRows(rows.nbIndices()), m_nbCols(cols.nbIndices()),
703  m_rows (rows. indices()), m_cols (cols. indices())
704  {
705  EIGEN_STATIC_ASSERT( bool(Derived1::OneDimension) && bool (Derived2::OneDimension),
706  YOU_MIXED_MATRICES_OF_DIFFERENT_SIZES);
707  }
708 
714  MatrixBlocksRef (const size_type& nidx, const segments_t& idx)
715  : m_nbRows(_allRows ? 0 : nidx)
716  , m_nbCols(_allCols ? 0 : nidx)
717  , m_rows(idx), m_cols(idx)
718  {}
719 
721  MatrixBlocksRef (const MatrixBlocksRef& other)
722  : Base (other)
723  , m_nbRows(other.nbRows())
724  , m_nbCols(other.nbCols())
725  , m_rows(other.rows())
726  , m_cols(other.cols())
727  {}
728 
731  inline const RowIndices_t& rows() const
732  {
733  return m_rows;
734  }
735 
738  inline const ColIndices_t& cols() const
739  {
740  return m_cols;
741  }
742 
745  inline const size_type& nbRows() const
746  {
747  return m_nbRows;
748  }
749 
752  inline const size_type& nbCols() const
753  {
754  return m_nbCols;
755  }
756 
757  const size_type m_nbRows, m_nbCols;
758  RowIndices_t m_rows;
759  ColIndices_t m_cols;
760  }; // class MatrixBlocksRef
762 
763  template <typename Derived>
764  std::ostream& operator<< (std::ostream& os, const MatrixBlocksBase<Derived>& mbi)
765  {
766  typedef typename internal::conditional<Derived::AllRows, internal::dont_print_indices, internal::print_indices>::type row_printer;
767  typedef typename internal::conditional<Derived::AllCols, internal::dont_print_indices, internal::print_indices>::type col_printer;
768  if (!Derived::AllRows) {
769  os << "Rows: ";
770  row_printer::run (os, mbi.rows());
771  if (!Derived::AllCols) os << hpp::iendl;
772  }
773  if (!Derived::AllCols) {
774  os << "Cols: ";
775  col_printer::run (os, mbi.cols());
776  }
777  return os;
778  }
779 
782 
809  template <typename _ArgType, int _Rows = _ArgType::RowsAtCompileTime, int _Cols = _ArgType::ColsAtCompileTime, bool _allRows = false, bool _allCols = false>
810  class MatrixBlockView : public MatrixBase< MatrixBlockView<_ArgType, _Rows, _Cols, _allRows, _allCols> >
811  {
812  public:
814  enum {
815  Rows = _Rows,
816  Cols = _Cols,
817  AllRows = _allRows,
818  AllCols = _allCols
819  };
820  struct block_iterator {
822  size_type row, col;
823  internal::variable_if_dynamic<size_type, (_allRows ? 0 : Dynamic) > _ro;
824  internal::variable_if_dynamic<size_type, (_allCols ? 0 : Dynamic) > _co;
825  block_iterator (const MatrixBlockView& v) : view(v), row (0), col (0), _ro(0), _co(0) {}
827  size_type ro() const { return _ro.value(); }
829  size_type co() const { return _co.value(); }
831  size_type ri() const { return internal::static_if<AllRows>::pp(std::make_pair(0,view.m_nbRows), view.m_rows[row]).first; }
833  size_type ci() const { return internal::static_if<AllCols>::pp(std::make_pair(0,view.m_nbCols), view.m_cols[col]).first; }
835  size_type rs() const { return internal::static_if<AllRows>::pp(std::make_pair(0,view.m_nbRows), view.m_rows[row]).second; }
837  size_type cs() const { return internal::static_if<AllCols>::pp(std::make_pair(0,view.m_nbCols), view.m_cols[col]).second; }
838  // ++it
840  {
841  _ro.setValue(_ro.value() + rs());
842  ++row;
843  if (row == (size_type)view.m_rows.size()) {
844  row = 0;
845  _ro.setValue(0);
846  _co.setValue(_co.value() + cs());
847  ++col;
848  // if (col < (size_type)view.m_cols.size()) _co.setValue(0);
849  }
850  return *this;
851  };
852  // it++
853  block_iterator operator++(int) { block_iterator copy(*this); operator++(); return copy; };
854  constexpr bool valid () const {
855  return (AllRows || view.m_rows.size()>0)
856  && (col < (size_type)(AllCols ? 1 : view.m_cols.size()));
857  }
858  };
859  typedef MatrixBase< MatrixBlockView<_ArgType, _Rows, _Cols, _allRows, _allCols> > Base;
860  EIGEN_GENERIC_PUBLIC_INTERFACE(MatrixBlockView)
861 
862  typedef Matrix<Scalar, RowsAtCompileTime, ColsAtCompileTime> PlainObject;
863  // typedef typename internal::ref_selector<MatrixBlockView>::type Nested;
864  typedef _ArgType ArgType;
865  typedef typename internal::ref_selector<ArgType>::type ArgTypeNested;
866  typedef typename internal::remove_all<ArgType>::type NestedExpression;
867  // typedef typename Base::CoeffReturnType CoeffReturnType;
868  // typedef typename Base::Scalar Scalar;
869 
870  template <typename Derived>
871  struct block_t {
872  typedef Block<Derived,
873  (AllRows ? Derived::RowsAtCompileTime : Eigen::Dynamic),
874  (AllCols ? Derived::ColsAtCompileTime : Eigen::Dynamic),
875  (AllCols ? (bool)Derived::IsRowMajor
876  : (AllRows ? (bool)!Derived::IsRowMajor : false)
877  )> type ;
878  };
881 
884  typedef typename internal::conditional<_allRows, const internal::empty_struct, const Indices_t& >::type RowIndices_t;
885  typedef typename internal::conditional<_allCols, const internal::empty_struct, const Indices_t& >::type ColIndices_t;
886 
887  // using Base::operator=;
888 
889  MatrixBlockView (ArgType& arg, const size_type& nbRows,
890  const RowIndices_t rows, const size_type& nbCols,
891  const ColIndices_t cols) :
892  m_arg (arg), m_nbRows(nbRows), m_rows(rows), m_nbCols(nbCols),
893  m_cols(cols)
894  {
895  }
896 
898  MatrixBlockView (ArgType& arg, const size_type& nbIndices,
899  const Indices_t& indices) :
900  m_arg (arg), m_nbRows(_allRows ? arg.rows() : nbIndices),
901  m_rows(indices), m_nbCols(_allCols ? arg.cols() : nbIndices),
902  m_cols(indices)
903  {}
904 
905  EIGEN_STRONG_INLINE size_type rows() const { return m_nbRows; }
906  EIGEN_STRONG_INLINE size_type cols() const { return m_nbCols; }
907 
908  EIGEN_STRONG_INLINE CoeffReturnType coeff (size_type index) const
909  {
910  assert(false && "It is not possible to access the coefficients of "
911  "MatrixBlockView this way.");
912  }
913  EIGEN_STRONG_INLINE CoeffReturnType coeff (size_type row, size_type col)
914  const
915  {
916  assert(false && "It is not possible to access the coefficients of "
917  "MatrixBlockView this way.");
918  }
919  EIGEN_STRONG_INLINE Scalar& coeffRef (size_type index)
920  {
921  assert(false && "It is not possible to access the coefficients of "
922  "MatrixBlockView this way.");
923  }
924  EIGEN_STRONG_INLINE Scalar& coeffRef (size_type row, const size_type& col)
925  {
926  assert(false && "It is not possible to access the coefficients of "
927  "MatrixBlockView this way.");
928  }
929  template <typename Dest>
930  EIGEN_STRONG_INLINE void evalTo (Dest& dst) const {
932  }
933 
934  template <typename Dest>
935  EIGEN_STRONG_INLINE void writeTo (Dest& dst) const {
936  dst.resize(rows(), cols());
937  evalTo(dst.derived());
938  }
939 
940  EIGEN_STRONG_INLINE PlainObject eval () const {
941  PlainObject dst;
942  writeTo(dst);
943  return dst;
944  }
945 
946  template <typename OtherDerived>
947  EIGEN_STRONG_INLINE MatrixBlockView& operator= (const EigenBase<OtherDerived>& other) {
948  EIGEN_STATIC_ASSERT_LVALUE(ArgType);
950  return *this;
951  }
952 
953  EIGEN_STRONG_INLINE size_type _blocks() const { return m_rows.size() * m_cols.size(); }
954  EIGEN_STRONG_INLINE BlockXprType _block(const block_iterator& b)
955  {
957  ::template run <ArgType> (m_arg, b.ri(), b.ci(), b.rs(), b.cs());
958  }
959  EIGEN_STRONG_INLINE const BlockConstXprType _block(const block_iterator& b) const
960  {
962  ::template run <const ArgType> (m_arg, b.ri(), b.ci(), b.rs(), b.cs());
963  }
964  EIGEN_STRONG_INLINE block_iterator _block_iterator() const { return block_iterator(*this); }
965 
966  EIGEN_STRONG_INLINE bool isZero (const RealScalar& prec = NumTraits<Scalar>::dummy_precision()) const
967  {
968  for (block_iterator block (*this); block.valid(); ++block)
969  if (!m_arg.block(
970  block.ri(), block.ci(),
971  block.rs(), block.cs())
972  .isZero(prec))
973  return false;
974  return true;
975  }
976 
978  size_type m_nbRows;
979  RowIndices_t m_rows;
980  size_type m_nbCols;
981  ColIndices_t m_cols;
982  }; // MatrixBlockView
983 
985 
986 } // namespace Eigen
987 
990 
991 # undef HPP_EIGEN_USE_EVALUATOR
992 
993 namespace hpp {
994  template <int Option>
995  struct prettyPrint<constraints::segment_t, Option> {
996  static std::ostream& run (std::ostream& os, const constraints::segment_t& s) {
997  return os << "[ " << s.first << ", " << s.first + s.second << " ]";
998  }
999  };
1000 }
1001 
1002 #endif // HPP_CONSTRAINTS_MATRIX_VIEW_HH
static segments_t split(segments_t &segments, const size_type &cardinal)
void addCol(const size_type &col, const size_type size)
Definition: matrix-view.hh:609
Matrix< Scalar, RowsAtCompileTime, ColsAtCompileTime > PlainObject
Definition: matrix-view.hh:862
MatrixBlocksRef< AllCols, AllRows > transpose() const
Definition: matrix-view.hh:389
MatrixBlockView< ArgType, _Rows, _Cols, _allRows, _allCols > OtherDerived
Definition: matrix-view.hh:217
internal::variable_if_dynamic< size_type,(_allRows ? 0 :Dynamic) > _ro
Definition: matrix-view.hh:823
Definition: matrix-view.hh:127
MatrixIndices_t::segments_t Indices_t
Definition: matrix-view.hh:883
MatrixXd::Index Index
Definition: matrix-view.hh:140
constexpr const Index & operator[](const Index &i) const
Definition: matrix-view.hh:145
const RowIndices_t & rows() const
Definition: matrix-view.hh:418
static ReturnType run(Derived &d, size_type r, size_type, size_type rs, size_type)
Definition: matrix-view.hh:277
const MatrixBlockView & view
Definition: matrix-view.hh:821
Definition: matrix-view.hh:123
MatrixBlocks(size_type start, size_type size)
Definition: matrix-view.hh:543
size_type cs() const
number of ColS
Definition: matrix-view.hh:837
MatrixBlocksRef< true, AllCols > keepCols() const
Definition: matrix-view.hh:400
const size_type & nbCols() const
Definition: matrix-view.hh:656
internal::variable_if_dynamic< size_type,(_allCols ? 0 :Dynamic) > _co
Definition: matrix-view.hh:824
MatrixBlockView(ArgType &arg, const size_type &nbRows, const RowIndices_t rows, const size_type &nbCols, const ColIndices_t cols)
Definition: matrix-view.hh:889
static size_type cardinal(const segments_t &a)
Definition: matrix-view.hh:37
internal::conditional< _allRows, internal::empty_struct, const BlockIndex::segments_t & >::type RowIndices_t
Definition: matrix-view.hh:166
size_type ro() const
Row in the Output matrix
Definition: matrix-view.hh:827
const ColIndices_t & cols() const
Definition: matrix-view.hh:425
const Derived & d
Definition: matrix-view-operation.hh:126
Block< Derived,(AllRows ? Derived::RowsAtCompileTime :Eigen::Dynamic),(AllCols ? Derived::ColsAtCompileTime :Eigen::Dynamic),(AllCols ?(bool) Derived::IsRowMajor :(AllRows ?(bool)!Derived::IsRowMajor :false))> type
Definition: matrix-view.hh:877
traits< ArgType >::XprKind XprKind
Definition: matrix-view.hh:179
Definition: active-set-differentiable-function.hh:24
block_iterator(const MatrixBlockView &v)
Definition: matrix-view.hh:825
EIGEN_STRONG_INLINE BlockXprType _block(const block_iterator &b)
Definition: matrix-view.hh:954
Definition: matrix-view.hh:294
MatrixBase< MatrixBlockView< _ArgType, _Rows, _Cols, _allRows, _allCols > > Base
Definition: matrix-view.hh:859
Derived & derived()
Definition: matrix-view.hh:362
EIGEN_STRONG_INLINE void evalTo(Dest &dst) const
Definition: matrix-view.hh:930
void updateIndices()
Definition: matrix-view.hh:662
block_t< ArgType >::type BlockXprType
Definition: matrix-view.hh:879
MatrixBlockView< _ArgType, _Rows, _Cols, _allRows, _allCols > Dst
Definition: matrix-view.hh:228
EIGEN_STRONG_INLINE block_iterator _block_iterator() const
Definition: matrix-view.hh:964
size_type m_nbRows
Definition: matrix-view.hh:668
MatrixBlockView< MatrixType, _Rows, _Cols, AllRows, AllCols > type
Definition: matrix-view.hh:358
MatrixBlocks(const segments_t &idx)
Definition: matrix-view.hh:555
MatrixBlocks(const size_type &nbRows, const RowIndices_t &rows, const size_type &nbCols, const ColIndices_t &cols)
Definition: matrix-view.hh:525
Definition: fwd.hh:27
static constexpr Else & rr(Then &, Else &s)
Definition: matrix-view.hh:135
internal::remove_all< ArgType >::type NestedExpression
Definition: matrix-view.hh:866
static EIGEN_STRONG_INLINE Derived & evalTo(ActualDerived &dst, const ActualOtherDerived &other)
Definition: matrix-view.hh:213
static segments_t difference(const segment_t &a, const segment_t &b)
Compute the set difference between two segments.
Definition: matrix-view.hh:130
size_type co() const
Col in the Output matrix
Definition: matrix-view.hh:829
#define EIGEN_MATRIX_BLOCKS_PUBLIC_INTERFACE(Derived)
Definition: matrix-view.hh:323
void clearRows()
Clear rows.
Definition: matrix-view.hh:584
std::vector< segment_t > segments_t
Definition: fwd.hh:72
hpp::constraints::size_type size_type
Definition: matrix-view.hh:813
static EIGEN_STRONG_INLINE Derived & evalTo(ActualDerived &dst, const ActualOtherDerived &other)
Definition: matrix-view.hh:220
std::pair< size_type, size_type > segment_t
Definition: fwd.hh:71
MatrixBlockView< ArgType, _Rows, _Cols, _allRows, _allCols > OtherDerived
Definition: matrix-view.hh:210
ColIndices_t m_cols
Definition: matrix-view.hh:670
static ReturnType run(Derived &d, size_type r, size_type c, size_type rs, size_type cs)
Definition: matrix-view.hh:267
hpp::constraints::segment_t segment_t
Interval of indices [first, first + second - 1].
Definition: matrix-view.hh:41
EIGEN_STRONG_INLINE size_type rows() const
Definition: matrix-view.hh:905
constexpr empty_struct(In0_t, In1_t)
Definition: matrix-view.hh:143
void addRow(const size_type &row, const size_type size)
Definition: matrix-view.hh:600
EIGEN_STRONG_INLINE size_type _blocks() const
Definition: matrix-view.hh:953
const size_type & nbCols() const
Definition: matrix-view.hh:449
MatrixBlocksBase()
Empty constructor.
Definition: matrix-view.hh:486
static void run(std::ostream &os, const BlockIndexType &bi)
Definition: matrix-view.hh:296
EIGEN_STRONG_INLINE Scalar & coeffRef(size_type index)
Definition: matrix-view.hh:919
EIGEN_STRONG_INLINE void writeTo(Dest &dst) const
Definition: matrix-view.hh:935
MatrixBlocks(const segment_t &idx)
Definition: matrix-view.hh:565
block_iterator operator++(int)
Definition: matrix-view.hh:853
block_iterator & operator++()
Definition: matrix-view.hh:839
Definition: matrix-view.hh:139
EIGEN_STRONG_INLINE const BlockConstXprType _block(const block_iterator &b) const
Definition: matrix-view.hh:959
MatrixBlocks< _allRows, _allCols > MatrixIndices_t
Definition: matrix-view.hh:882
hpp::constraints::size_type size_type
Index of vector or matrix.
Definition: matrix-view.hh:39
Eigen::MatrixBlocks< false, true > RowBlockIndices
Definition: matrix-view.hh:780
constexpr bool valid() const
Definition: matrix-view.hh:854
static void shrink(segments_t &a)
static constexpr Else pp(Then, Else s)
Definition: matrix-view.hh:136
assert(d.lhs()._blocks()==d.rhs()._blocks())
static void add(segments_t &a, const segment_t &b)
In place addition of a segment_t to segments_t.
Definition: matrix-view.hh:339
Definition: matrix-view.hh:111
static constexpr Index size()
Definition: matrix-view.hh:144
static segments_t fromLogicalExpression(const Eigen::ArrayBase< Derived > &array)
Definition: matrix-view.hh:20
const size_type & nbIndices() const
Definition: matrix-view.hh:435
size_type ri() const
Row in the Input matrix
Definition: matrix-view.hh:831
BlockIndex::segments_t segments_t
vector of segments
Definition: matrix-view.hh:352
size_type ci() const
Col in the Input matrix
Definition: matrix-view.hh:833
Definition: matrix-view.hh:293
RowIndices_t m_rows
Definition: matrix-view.hh:669
EIGEN_STRONG_INLINE Scalar & coeffRef(size_type row, const size_type &col)
Definition: matrix-view.hh:924
internal::conditional< _allRows, const internal::empty_struct, const Indices_t &>::type RowIndices_t
Definition: matrix-view.hh:884
block_t< const ArgType >::type BlockConstXprType
Definition: matrix-view.hh:880
View::size_type size_type
Definition: matrix-view.hh:265
const size_type & nbRows() const
Definition: matrix-view.hh:649
static void sort(segments_t &a)
const segments_t & indices() const
Definition: matrix-view.hh:411
EIGEN_STRONG_INLINE View< const MatrixType >::type rview(const MatrixBase< MatrixType > &other) const
Definition: matrix-view.hh:382
static constexpr Then pp(Then f, Else)
Definition: matrix-view.hh:132
ColIndices_t m_cols
Definition: matrix-view.hh:981
_ArgType ArgType
Definition: matrix-view.hh:864
static constexpr Then & rr(Then &f, Else &)
Definition: matrix-view.hh:131
MatrixBlockView< _ArgType, _Rows, _Cols, _allRows, _allCols > Src
Definition: matrix-view.hh:237
ArgType & m_arg
Definition: matrix-view.hh:977
Definition: matrix-view.hh:820
static EIGEN_STRONG_INLINE Derived & run(Derived &dst, const OtherDerived &other)
Definition: matrix-view.hh:218
MatrixBlocks(const MatrixBlocksBase< MBDerived > &other)
Copy constructor.
Definition: matrix-view.hh:573
RowIndices_t m_rows
Definition: matrix-view.hh:979
EIGEN_STRONG_INLINE CoeffReturnType coeff(size_type index) const
Definition: matrix-view.hh:908
size_type m_nbRows
Definition: matrix-view.hh:978
EIGEN_STRONG_INLINE PlainObject eval() const
Definition: matrix-view.hh:940
internal::conditional< _allCols, internal::empty_struct, const BlockIndex::segments_t & >::type ColIndices_t
Definition: matrix-view.hh:167
EIGEN_STRONG_INLINE View< MatrixType >::type lview(const MatrixBase< MatrixType > &other) const
Definition: matrix-view.hh:369
static bool overlap(const segment_t &a, const segment_t &b)
Whether two segments overlap.
static void run(std::ostream &, const BlockIndexType &)
Definition: matrix-view.hh:293
size_type m_nbCols
Definition: matrix-view.hh:980
MatrixBlocksRef< AllRows, true > keepRows() const
Definition: matrix-view.hh:394
const ColIndices_t & cols() const
Definition: matrix-view.hh:642
Eigen::MatrixBlocks< true, false > ColBlockIndices
Definition: matrix-view.hh:781
internal::traits< Derived >::ColIndices_t ColIndices_t
Definition: matrix-view.hh:354
static segments_t sum(const segment_t &a, const segment_t &b)
Compute the union of tws segments.
void clearCols()
Clear cols.
Definition: matrix-view.hh:591
EIGEN_STRONG_INLINE size_type cols() const
Definition: matrix-view.hh:906
pinocchio::size_type size_type
Definition: fwd.hh:36
Smaller matrix composed by concatenation of the blocks.
Definition: matrix-view.hh:357
EIGEN_STRONG_INLINE CoeffReturnType coeff(size_type row, size_type col) const
Definition: matrix-view.hh:913
hpp::constraints::segments_t segments_t
vector of segments
Definition: matrix-view.hh:43
internal::traits< Derived >::RowIndices_t RowIndices_t
Definition: matrix-view.hh:353
Derived const & derived() const
Definition: matrix-view.hh:361
MatrixBlockView(ArgType &arg, const size_type &nbIndices, const Indices_t &indices)
Valid only when _allRows or _allCols is true.
Definition: matrix-view.hh:898
const size_type & nbRows() const
Definition: matrix-view.hh:442
const RowIndices_t & rows() const
Definition: matrix-view.hh:635
MatrixBlocksBase(const MatrixBlocksBase &)
Copy constructor.
Definition: matrix-view.hh:489
void updateCols()
Definition: matrix-view.hh:629
MatrixBlocks(const segments_t &rows, const segments_t &cols)
Definition: matrix-view.hh:505
static EIGEN_STRONG_INLINE Derived & run(Derived &dst, const OtherDerived &other)
Definition: matrix-view.hh:211
static std::ostream & run(std::ostream &os, const constraints::segment_t &s)
Definition: matrix-view.hh:996
internal::ref_selector< ArgType >::type ArgTypeNested
Definition: matrix-view.hh:865
hpp::constraints::size_type size_type
Index of vector or matrix.
Definition: matrix-view.hh:348
EIGEN_STRONG_INLINE bool isZero(const RealScalar &prec=NumTraits< Scalar >::dummy_precision()) const
Definition: matrix-view.hh:966
internal::conditional< _allRows, internal::empty_struct, BlockIndex::segments_t >::type RowIndices_t
Definition: matrix-view.hh:155
traits< ArgType >::StorageKind StorageKind
Definition: matrix-view.hh:178
size_type row
Definition: matrix-view.hh:822
static segments_t extract(const segments_t &segments, size_type start, size_type cardinal)
BlockIndex::segment_t segment_t
Interval of indices [first, first + second - 1].
Definition: matrix-view.hh:350
internal::conditional< _allCols, const internal::empty_struct, const Indices_t &>::type ColIndices_t
Definition: matrix-view.hh:885
internal::conditional< _allCols, internal::empty_struct, BlockIndex::segments_t >::type ColIndices_t
Definition: matrix-view.hh:156
static ReturnType run(Derived &d, size_type, size_type c, size_type, size_type cs)
Definition: matrix-view.hh:287
Definition: matrix-view.hh:224
void updateRows()
Definition: matrix-view.hh:620
constexpr empty_struct(In_t)
Definition: matrix-view.hh:142
size_type rs() const
number of RowS
Definition: matrix-view.hh:835
Definition: matrix-view.hh:871