hpp-constraints  4.12.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  {
585  m_nbRows = other.m_nbRows;
586  m_nbCols = other.m_nbCols;
587  m_rows = other.m_rows;
588  m_cols = other.m_cols;
589  return *this;
590  }
592  inline void clearRows ()
593  {
594  m_rows.clear();
595  m_nbRows = 0;
596  }
597 
599  inline void clearCols ()
600  {
601  m_cols.clear();
602  m_nbCols = 0;
603  }
604 
608  inline void addRow (const size_type& row, const size_type size)
609  {
610  m_rows.push_back(segment_t (row, size));
611  m_nbRows += size;
612  }
613 
617  inline void addCol (const size_type& col, const size_type size)
618  {
619  m_cols.push_back(segment_t (col, size));
620  m_nbCols += size;
621  }
622 
627  template<bool Sort, bool Shrink, bool Cardinal>
628  inline void updateRows() {
629  update<Sort, Shrink, Cardinal> (m_rows, m_nbRows);
630  }
631 
636  template<bool Sort, bool Shrink, bool Cardinal>
637  inline void updateCols() {
638  update<Sort, Shrink, Cardinal> (m_cols, m_nbCols);
639  }
640 
643  inline const RowIndices_t& rows() const
644  {
645  return m_rows;
646  }
647 
650  inline const ColIndices_t& cols() const
651  {
652  return m_cols;
653  }
654 
657  inline const size_type& nbRows() const
658  {
659  return m_nbRows;
660  }
661 
664  inline const size_type& nbCols() const
665  {
666  return m_nbCols;
667  }
668 
669  template<bool Sort, bool Shrink, bool Cardinal>
670  inline void updateIndices() {
671  update<Sort, Shrink, Cardinal> (
672  internal::static_if<_allRows>::rr(m_cols, m_rows),
673  _allRows ? m_nbCols : m_nbRows);
674  }
675 
676  size_type m_nbRows, m_nbCols;
677  RowIndices_t m_rows;
678  ColIndices_t m_cols;
679 
680  private:
681  template<bool Sort, bool Shrink, bool Cardinal>
682  static inline void update(segments_t& b, size_type& idx) {
683  if (Sort) BlockIndex::sort(b);
684  if (Shrink) BlockIndex::shrink(b);
685  if (Cardinal) idx = BlockIndex::cardinal(b);
686  }
687  }; // class MatrixBlocks
688 
690  template <bool _allRows, bool _allCols>
691  class MatrixBlocksRef : public MatrixBlocksBase < MatrixBlocksRef <_allRows, _allCols> >
692  {
693  public:
695 
696 
697  MatrixBlocksRef (const size_type& nbRows, const RowIndices_t& rows,
701  const size_type& nbCols, const ColIndices_t& cols) :
702  m_nbRows(nbRows), m_nbCols(nbCols),
703  m_rows (rows ), m_cols (cols )
704  {}
705 
707  template <typename Derived1, typename Derived2>
709  const MatrixBlocksBase<Derived2>& cols) :
710  m_nbRows(rows.nbIndices()), m_nbCols(cols.nbIndices()),
711  m_rows (rows. indices()), m_cols (cols. indices())
712  {
713  EIGEN_STATIC_ASSERT( bool(Derived1::OneDimension) && bool (Derived2::OneDimension),
714  YOU_MIXED_MATRICES_OF_DIFFERENT_SIZES);
715  }
716 
722  MatrixBlocksRef (const size_type& nidx, const segments_t& idx)
723  : m_nbRows(_allRows ? 0 : nidx)
724  , m_nbCols(_allCols ? 0 : nidx)
725  , m_rows(idx), m_cols(idx)
726  {}
727 
729  MatrixBlocksRef (const MatrixBlocksRef& other)
730  : Base (other)
731  , m_nbRows(other.nbRows())
732  , m_nbCols(other.nbCols())
733  , m_rows(other.rows())
734  , m_cols(other.cols())
735  {}
736 
739  inline const RowIndices_t& rows() const
740  {
741  return m_rows;
742  }
743 
746  inline const ColIndices_t& cols() const
747  {
748  return m_cols;
749  }
750 
753  inline const size_type& nbRows() const
754  {
755  return m_nbRows;
756  }
757 
760  inline const size_type& nbCols() const
761  {
762  return m_nbCols;
763  }
764 
765  const size_type m_nbRows, m_nbCols;
766  RowIndices_t m_rows;
767  ColIndices_t m_cols;
768  }; // class MatrixBlocksRef
770 
771  template <typename Derived>
772  std::ostream& operator<< (std::ostream& os, const MatrixBlocksBase<Derived>& mbi)
773  {
774  typedef typename internal::conditional<Derived::AllRows, internal::dont_print_indices, internal::print_indices>::type row_printer;
775  typedef typename internal::conditional<Derived::AllCols, internal::dont_print_indices, internal::print_indices>::type col_printer;
776  if (!Derived::AllRows) {
777  os << "Rows: ";
778  row_printer::run (os, mbi.rows());
779  if (!Derived::AllCols) os << hpp::iendl;
780  }
781  if (!Derived::AllCols) {
782  os << "Cols: ";
783  col_printer::run (os, mbi.cols());
784  }
785  return os;
786  }
787 
790 
817  template <typename _ArgType, int _Rows = _ArgType::RowsAtCompileTime, int _Cols = _ArgType::ColsAtCompileTime, bool _allRows = false, bool _allCols = false>
818  class MatrixBlockView : public MatrixBase< MatrixBlockView<_ArgType, _Rows, _Cols, _allRows, _allCols> >
819  {
820  public:
822  enum {
823  Rows = _Rows,
824  Cols = _Cols,
825  AllRows = _allRows,
826  AllCols = _allCols
827  };
828  struct block_iterator {
830  size_type row, col;
831  internal::variable_if_dynamic<size_type, (_allRows ? 0 : Dynamic) > _ro;
832  internal::variable_if_dynamic<size_type, (_allCols ? 0 : Dynamic) > _co;
833  block_iterator (const MatrixBlockView& v) : view(v), row (0), col (0), _ro(0), _co(0) {}
835  size_type ro() const { return _ro.value(); }
837  size_type co() const { return _co.value(); }
839  size_type ri() const { return internal::static_if<AllRows>::pp(std::make_pair(0,view.m_nbRows), view.m_rows[row]).first; }
841  size_type ci() const { return internal::static_if<AllCols>::pp(std::make_pair(0,view.m_nbCols), view.m_cols[col]).first; }
843  size_type rs() const { return internal::static_if<AllRows>::pp(std::make_pair(0,view.m_nbRows), view.m_rows[row]).second; }
845  size_type cs() const { return internal::static_if<AllCols>::pp(std::make_pair(0,view.m_nbCols), view.m_cols[col]).second; }
846  // ++it
848  {
849  _ro.setValue(_ro.value() + rs());
850  ++row;
851  if (row == (size_type)view.m_rows.size()) {
852  row = 0;
853  _ro.setValue(0);
854  _co.setValue(_co.value() + cs());
855  ++col;
856  // if (col < (size_type)view.m_cols.size()) _co.setValue(0);
857  }
858  return *this;
859  };
860  // it++
861  block_iterator operator++(int) { block_iterator copy(*this); operator++(); return copy; };
862  constexpr bool valid () const {
863  return (AllRows || view.m_rows.size()>0)
864  && (col < (size_type)(AllCols ? 1 : view.m_cols.size()));
865  }
866  };
867  typedef MatrixBase< MatrixBlockView<_ArgType, _Rows, _Cols, _allRows, _allCols> > Base;
868  EIGEN_GENERIC_PUBLIC_INTERFACE(MatrixBlockView)
869 
870  typedef Matrix<Scalar, RowsAtCompileTime, ColsAtCompileTime> PlainObject;
871  // typedef typename internal::ref_selector<MatrixBlockView>::type Nested;
872  typedef _ArgType ArgType;
873  typedef typename internal::ref_selector<ArgType>::type ArgTypeNested;
874  typedef typename internal::remove_all<ArgType>::type NestedExpression;
875  // typedef typename Base::CoeffReturnType CoeffReturnType;
876  // typedef typename Base::Scalar Scalar;
877 
878  template <typename Derived>
879  struct block_t {
880  typedef Block<Derived,
881  (AllRows ? Derived::RowsAtCompileTime : Eigen::Dynamic),
882  (AllCols ? Derived::ColsAtCompileTime : Eigen::Dynamic),
883  (AllCols ? (bool)Derived::IsRowMajor
884  : (AllRows ? (bool)!Derived::IsRowMajor : false)
885  )> type ;
886  };
889 
892  typedef typename internal::conditional<_allRows, const internal::empty_struct, const Indices_t& >::type RowIndices_t;
893  typedef typename internal::conditional<_allCols, const internal::empty_struct, const Indices_t& >::type ColIndices_t;
894 
895  // using Base::operator=;
896 
897  MatrixBlockView (ArgType& arg, const size_type& nbRows,
898  const RowIndices_t rows, const size_type& nbCols,
899  const ColIndices_t cols) :
900  m_arg (arg), m_nbRows(nbRows), m_rows(rows), m_nbCols(nbCols),
901  m_cols(cols)
902  {
903  }
904 
906  MatrixBlockView (ArgType& arg, const size_type& nbIndices,
907  const Indices_t& indices) :
908  m_arg (arg), m_nbRows(_allRows ? arg.rows() : nbIndices),
909  m_rows(indices), m_nbCols(_allCols ? arg.cols() : nbIndices),
910  m_cols(indices)
911  {}
912 
913  EIGEN_STRONG_INLINE size_type rows() const { return m_nbRows; }
914  EIGEN_STRONG_INLINE size_type cols() const { return m_nbCols; }
915 
916  EIGEN_STRONG_INLINE CoeffReturnType coeff (size_type index) const
917  {
918  assert(false && "It is not possible to access the coefficients of "
919  "MatrixBlockView this way.");
920  }
921  EIGEN_STRONG_INLINE CoeffReturnType coeff (size_type row, size_type col)
922  const
923  {
924  assert(false && "It is not possible to access the coefficients of "
925  "MatrixBlockView this way.");
926  }
927  EIGEN_STRONG_INLINE Scalar& coeffRef (size_type index)
928  {
929  assert(false && "It is not possible to access the coefficients of "
930  "MatrixBlockView this way.");
931  }
932  EIGEN_STRONG_INLINE Scalar& coeffRef (size_type row, const size_type& col)
933  {
934  assert(false && "It is not possible to access the coefficients of "
935  "MatrixBlockView this way.");
936  }
937  template <typename Dest>
938  EIGEN_STRONG_INLINE void evalTo (Dest& dst) const {
940  }
941 
942  template <typename Dest>
943  EIGEN_STRONG_INLINE void writeTo (Dest& dst) const {
944  dst.resize(rows(), cols());
945  evalTo(dst.derived());
946  }
947 
948  EIGEN_STRONG_INLINE PlainObject eval () const {
949  PlainObject dst;
950  writeTo(dst);
951  return dst;
952  }
953 
954  template <typename OtherDerived>
955  EIGEN_STRONG_INLINE MatrixBlockView& operator= (const EigenBase<OtherDerived>& other) {
956  EIGEN_STATIC_ASSERT_LVALUE(ArgType);
958  return *this;
959  }
960 
961  EIGEN_STRONG_INLINE size_type _blocks() const { return m_rows.size() * m_cols.size(); }
962  EIGEN_STRONG_INLINE BlockXprType _block(const block_iterator& b)
963  {
965  ::template run <ArgType> (m_arg, b.ri(), b.ci(), b.rs(), b.cs());
966  }
967  EIGEN_STRONG_INLINE const BlockConstXprType _block(const block_iterator& b) const
968  {
970  ::template run <const ArgType> (m_arg, b.ri(), b.ci(), b.rs(), b.cs());
971  }
972  EIGEN_STRONG_INLINE block_iterator _block_iterator() const { return block_iterator(*this); }
973 
974  EIGEN_STRONG_INLINE bool isZero (const RealScalar& prec = NumTraits<Scalar>::dummy_precision()) const
975  {
976  for (block_iterator block (*this); block.valid(); ++block)
977  if (!m_arg.block(
978  block.ri(), block.ci(),
979  block.rs(), block.cs())
980  .isZero(prec))
981  return false;
982  return true;
983  }
984 
986  size_type m_nbRows;
987  RowIndices_t m_rows;
988  size_type m_nbCols;
989  ColIndices_t m_cols;
990  }; // MatrixBlockView
991 
993 
994 } // namespace Eigen
995 
998 
999 # undef HPP_EIGEN_USE_EVALUATOR
1000 
1001 namespace hpp {
1002  template <int Option>
1003  struct prettyPrint<constraints::segment_t, Option> {
1004  static std::ostream& run (std::ostream& os, const constraints::segment_t& s) {
1005  return os << "[ " << s.first << ", " << s.first + s.second << " ]";
1006  }
1007  };
1008 }
1009 
1010 #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:617
Matrix< Scalar, RowsAtCompileTime, ColsAtCompileTime > PlainObject
Definition: matrix-view.hh:870
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:831
Definition: matrix-view.hh:127
MatrixIndices_t::segments_t Indices_t
Definition: matrix-view.hh:891
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:829
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:845
MatrixBlocksRef< true, AllCols > keepCols() const
Definition: matrix-view.hh:400
const size_type & nbCols() const
Definition: matrix-view.hh:664
internal::variable_if_dynamic< size_type,(_allCols ? 0 :Dynamic) > _co
Definition: matrix-view.hh:832
MatrixBlockView(ArgType &arg, const size_type &nbRows, const RowIndices_t rows, const size_type &nbCols, const ColIndices_t cols)
Definition: matrix-view.hh:897
static size_type cardinal(const segments_t &a)
Definition: matrix-view.hh:37
size_type m_nbCols
Definition: matrix-view.hh:676
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:835
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:885
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:833
EIGEN_STRONG_INLINE BlockXprType _block(const block_iterator &b)
Definition: matrix-view.hh:962
Definition: matrix-view.hh:294
MatrixBase< MatrixBlockView< _ArgType, _Rows, _Cols, _allRows, _allCols > > Base
Definition: matrix-view.hh:867
Derived & derived()
Definition: matrix-view.hh:362
EIGEN_STRONG_INLINE void evalTo(Dest &dst) const
Definition: matrix-view.hh:938
void updateIndices()
Definition: matrix-view.hh:670
block_t< ArgType >::type BlockXprType
Definition: matrix-view.hh:887
MatrixBlockView< _ArgType, _Rows, _Cols, _allRows, _allCols > Dst
Definition: matrix-view.hh:228
EIGEN_STRONG_INLINE block_iterator _block_iterator() const
Definition: matrix-view.hh:972
size_type m_nbRows
Definition: matrix-view.hh:676
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:874
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:837
#define EIGEN_MATRIX_BLOCKS_PUBLIC_INTERFACE(Derived)
Definition: matrix-view.hh:323
void clearRows()
Clear rows.
Definition: matrix-view.hh:592
std::vector< segment_t > segments_t
Definition: fwd.hh:72
hpp::constraints::size_type size_type
Definition: matrix-view.hh:821
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:678
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:913
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:608
EIGEN_STRONG_INLINE size_type _blocks() const
Definition: matrix-view.hh:961
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:927
EIGEN_STRONG_INLINE void writeTo(Dest &dst) const
Definition: matrix-view.hh:943
MatrixBlocks(const segment_t &idx)
Definition: matrix-view.hh:565
block_iterator operator++(int)
Definition: matrix-view.hh:861
block_iterator & operator++()
Definition: matrix-view.hh:847
Definition: matrix-view.hh:139
EIGEN_STRONG_INLINE const BlockConstXprType _block(const block_iterator &b) const
Definition: matrix-view.hh:967
MatrixBlocks< _allRows, _allCols > MatrixIndices_t
Definition: matrix-view.hh:890
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:788
constexpr bool valid() const
Definition: matrix-view.hh:862
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:839
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:841
Definition: matrix-view.hh:293
RowIndices_t m_rows
Definition: matrix-view.hh:677
EIGEN_STRONG_INLINE Scalar & coeffRef(size_type row, const size_type &col)
Definition: matrix-view.hh:932
internal::conditional< _allRows, const internal::empty_struct, const Indices_t &>::type RowIndices_t
Definition: matrix-view.hh:892
block_t< const ArgType >::type BlockConstXprType
Definition: matrix-view.hh:888
View::size_type size_type
Definition: matrix-view.hh:265
const size_type & nbRows() const
Definition: matrix-view.hh:657
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:989
_ArgType ArgType
Definition: matrix-view.hh:872
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:985
Definition: matrix-view.hh:828
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:987
EIGEN_STRONG_INLINE CoeffReturnType coeff(size_type index) const
Definition: matrix-view.hh:916
size_type m_nbRows
Definition: matrix-view.hh:986
EIGEN_STRONG_INLINE PlainObject eval() const
Definition: matrix-view.hh:948
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:988
MatrixBlocksRef< AllRows, true > keepRows() const
Definition: matrix-view.hh:394
const ColIndices_t & cols() const
Definition: matrix-view.hh:650
Eigen::MatrixBlocks< true, false > ColBlockIndices
Definition: matrix-view.hh:789
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:599
EIGEN_STRONG_INLINE size_type cols() const
Definition: matrix-view.hh:914
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:921
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:906
const size_type & nbRows() const
Definition: matrix-view.hh:442
const RowIndices_t & rows() const
Definition: matrix-view.hh:643
MatrixBlocksBase(const MatrixBlocksBase &)
Copy constructor.
Definition: matrix-view.hh:489
void updateCols()
Definition: matrix-view.hh:637
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:1004
internal::ref_selector< ArgType >::type ArgTypeNested
Definition: matrix-view.hh:873
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:974
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
MatrixBlocks & operator=(const MatrixBlocks &other)
Definition: matrix-view.hh:583
size_type row
Definition: matrix-view.hh:830
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:893
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:628
constexpr empty_struct(In_t)
Definition: matrix-view.hh:142
size_type rs() const
number of RowS
Definition: matrix-view.hh:843
Definition: matrix-view.hh:879