hpp-constraints  4.15.1
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 
5 // Redistribution and use in source and binary forms, with or without
6 // modification, are permitted provided that the following conditions are
7 // met:
8 //
9 // 1. Redistributions of source code must retain the above copyright
10 // notice, this list of conditions and the following disclaimer.
11 //
12 // 2. Redistributions in binary form must reproduce the above copyright
13 // notice, this list of conditions and the following disclaimer in the
14 // documentation and/or other materials provided with the distribution.
15 //
16 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
17 // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
18 // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
19 // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
20 // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
21 // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
22 // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
23 // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
24 // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
25 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
26 // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
27 // DAMAGE.
28 
29 #ifndef HPP_CONSTRAINTS_MATRIX_VIEW_HH
30 #define HPP_CONSTRAINTS_MATRIX_VIEW_HH
31 
32 #include <Eigen/Core>
33 #include <hpp/constraints/fwd.hh>
34 #include <hpp/pinocchio/util.hh>
35 #include <hpp/util/indent.hh>
36 #include <iostream>
37 #include <vector>
38 
39 #define HPP_EIGEN_USE_EVALUATOR EIGEN_VERSION_AT_LEAST(3, 2, 92)
40 
41 namespace Eigen {
42 
45 
49 struct BlockIndex {
56 
59  static size_type cardinal(const segments_t& a);
60 
65  template <typename Derived>
66  static segments_t fromLogicalExpression(
67  const Eigen::ArrayBase<Derived>& array);
68 
71  static void sort(segments_t& a);
72 
76  static void shrink(segments_t& a);
77 
79  static bool overlap(const segment_t& a, const segment_t& b);
80 
82  static segments_t sum(const segment_t& a, const segment_t& b);
83 
85  static void add(segments_t& a, const segment_t& b);
86 
88  static void add(segments_t& a, const segments_t& b);
89 
91  static segments_t difference(const segment_t& a, const segment_t& b);
92 
95  static segments_t difference(const segments_t& a, const segment_t& b);
96 
99  static segments_t difference(const segment_t& a, const segments_t& b);
100 
103  static segments_t difference(const segments_t& a, const segments_t& b);
104 
111  static segments_t split(segments_t& segments, const size_type& cardinal);
112 
119  static segments_t extract(const segments_t& segments, size_type start,
120  size_type cardinal);
121 }; // struct BlockIndex
122 
123 template <typename ArgType, int _Rows, int _Cols, bool _allRows, bool _allCols>
125 
136 template <bool _allRows = false, bool _allCols = false>
138 
140 
141 template <bool _allRows = false, bool _allCols = false>
143 
144 namespace internal {
145 template <bool condition>
146 struct static_if {
147  template <class Then, class Else>
148  static constexpr inline Then& rr(Then& f, Else&) {
149  return f;
150  }
151  template <class Then, class Else>
152  static constexpr inline Then pp(Then f, Else) {
153  return f;
154  }
155 };
156 template <>
157 struct static_if<false> {
158  template <class Then, class Else>
159  static constexpr inline Else& rr(Then&, Else& s) {
160  return s;
161  }
162  template <class Then, class Else>
163  static constexpr inline Else pp(Then, Else s) {
164  return s;
165  }
166 };
167 
168 struct empty_struct {
169  typedef MatrixXd::Index Index;
170  constexpr empty_struct() = default;
171  template <typename In_t>
172  constexpr empty_struct(In_t) {}
173  template <typename In0_t, typename In1_t>
174  constexpr empty_struct(In0_t, In1_t) {}
175  static constexpr inline Index size() { return 1; }
176  inline constexpr const Index& operator[](const Index& i) const { return i; }
177 };
178 
179 template <bool _allRows, bool _allCols>
180 struct traits<MatrixBlocks<_allRows, _allCols> > {
181  enum { AllRows = _allRows, AllCols = _allCols };
182  typedef
183  typename internal::conditional<_allRows, internal::empty_struct,
185  typedef
186  typename internal::conditional<_allCols, internal::empty_struct,
188 };
189 
190 template <bool _allRows, bool _allCols>
191 struct traits<MatrixBlocksRef<_allRows, _allCols> > {
192  enum { AllRows = _allRows, AllCols = _allCols };
193  typedef typename internal::conditional<_allRows, internal::empty_struct,
194  const BlockIndex::segments_t&>::type
196  typedef typename internal::conditional<_allCols, internal::empty_struct,
197  const BlockIndex::segments_t&>::type
199 };
200 
201 template <typename ArgType, int _Rows, int _Cols, bool _allRows, bool _allCols>
202 struct traits<MatrixBlockView<ArgType, _Rows, _Cols, _allRows, _allCols> > {
203 #if HPP_EIGEN_USE_EVALUATOR
204  typedef typename ArgType::StorageIndex StorageIndex;
205 #else // HPP_EIGEN_USE_EVALUATOR
206  typedef typename ArgType::Index Index;
207 #endif // HPP_EIGEN_USE_EVALUATOR
208  typedef typename traits<ArgType>::StorageKind StorageKind;
209  typedef typename traits<ArgType>::XprKind XprKind;
210  typedef typename ArgType::Scalar Scalar;
211  enum {
212 #if !HPP_EIGEN_USE_EVALUATOR
213  CoeffReadCost = ArgType::CoeffReadCost,
214 #endif // !HPP_EIGEN_USE_EVALUATOR
215  Flags = ~PacketAccessBit & ~DirectAccessBit & ~ActualPacketAccessBit &
216  ~LinearAccessBit & ArgType::Flags,
217  RowsAtCompileTime = (_allRows ? ArgType::RowsAtCompileTime : _Rows),
218  ColsAtCompileTime = (_allCols ? ArgType::ColsAtCompileTime : _Cols),
219  MaxRowsAtCompileTime = ArgType::MaxRowsAtCompileTime,
220  MaxColsAtCompileTime = ArgType::MaxColsAtCompileTime
221  };
222 };
223 
224 #if HPP_EIGEN_USE_EVALUATOR
225 template <typename Derived, typename ArgType, int _Rows, int _Cols,
226  bool _allRows, bool _allCols, typename Functor, typename Scalar>
227 struct Assignment<Derived,
228  MatrixBlockView<ArgType, _Rows, _Cols, _allRows, _allCols>,
229  Functor, Dense2Dense, Scalar> {
231  OtherDerived;
232  static EIGEN_STRONG_INLINE void run(Derived& dst, const OtherDerived& src,
233  const Functor& func) {
234  dst.resize(src.rows(), src.cols());
235  typedef Block<Derived> BlockDerived;
236  typedef Assignment<BlockDerived, typename OtherDerived::BlockConstXprType,
237  Functor>
238  AssignmentType;
239  for (typename OtherDerived::block_iterator b(src); b.valid(); ++b) {
240  BlockDerived bdst(dst.block(b.ro(), b.co(), b.rs(), b.cs()));
241  AssignmentType::run(bdst, src._block(b), func);
242  }
243  }
244 };
245 #else // HPP_EIGEN_USE_EVALUATOR
246 template <typename Derived, typename ArgType, int _Rows, int _Cols,
247  bool _allRows, bool _allCols>
248 struct assign_selector<
249  Derived, MatrixBlockView<ArgType, _Rows, _Cols, _allRows, _allCols>, false,
250  false> {
253  static EIGEN_STRONG_INLINE Derived& run(Derived& dst,
254  const OtherDerived& other) {
255  other.writeTo(dst);
256  return dst;
257  }
258  template <typename ActualDerived, typename ActualOtherDerived>
259  static EIGEN_STRONG_INLINE Derived& evalTo(ActualDerived& dst,
260  const ActualOtherDerived& other) {
261  other.evalTo(dst);
262  return dst;
263  }
264 };
265 template <typename Derived, typename ArgType, int _Rows, int _Cols,
266  bool _allRows, bool _allCols>
267 struct assign_selector<
268  Derived, MatrixBlockView<ArgType, _Rows, _Cols, _allRows, _allCols>, false,
269  true> {
272  static EIGEN_STRONG_INLINE Derived& run(Derived& dst,
273  const OtherDerived& other) {
274  other.writeTo(dst.transpose());
275  return dst;
276  }
277  template <typename ActualDerived, typename ActualOtherDerived>
278  static EIGEN_STRONG_INLINE Derived& evalTo(ActualDerived& dst,
279  const ActualOtherDerived& other) {
280  Transpose<ActualDerived> dstTrans(dst);
281  other.evalTo(dstTrans);
282  return dst;
283  }
284 };
285 #endif // HPP_EIGEN_USE_EVALUATOR
286 
287 template <typename Src, typename Dst>
289 template <typename Src, typename _ArgType, int _Rows, int _Cols, bool _allRows,
290  bool _allCols>
292  Src, MatrixBlockView<_ArgType, _Rows, _Cols, _allRows, _allCols> > {
293  // MatrixBlockView <- matrix
295  static void run(const Src& src, Dst& dst) {
296  for (typename Dst::block_iterator b(dst); b.valid(); ++b)
297  dst._block(b) = src.block(b.ro(), b.co(), b.rs(), b.cs());
298  }
299 };
300 template <typename _ArgType, int _Rows, int _Cols, bool _allRows, bool _allCols,
301  typename Dst>
303  MatrixBlockView<_ArgType, _Rows, _Cols, _allRows, _allCols>, Dst> {
304  // matrix <- MatrixBlockView
306  static void run(const Src& src, Dst& dst) {
307  for (typename Src::block_iterator b(src); b.valid(); ++b)
308  dst.block(b.ro(), b.co(), b.rs(), b.cs()) = src._block(b);
309  }
310 };
311 template <typename _ArgType, int _Rows, int _Cols, bool _allRows, bool _allCols,
312  typename _ArgType2, int _Rows2, int _Cols2, bool _allRows2,
313  bool _allCols2>
315  MatrixBlockView<_ArgType, _Rows, _Cols, _allRows, _allCols>,
316  MatrixBlockView<_ArgType2, _Rows2, _Cols2, _allRows2, _allCols2> > {
317  // MatrixBlockView <- MatrixBlockView
320  static void run(const Src& src, Dst& dst) {
321  typename Dst::block_iterator db(dst);
322  for (typename Src::block_iterator sb(src); sb.valid(); ++sb) {
323  assert(db.valid());
324  dst._block(db) = src._block(sb);
325  ++db;
326  }
327  assert(!db.valid());
328  }
329 };
330 
331 template <typename ReturnType, typename View, bool AllRows = View::AllRows,
332  bool AllCols = View::AllCols>
334  typedef typename View::size_type size_type;
335  template <typename Derived>
336  static ReturnType run(Derived& d, size_type r, size_type c, size_type rs,
337  size_type cs) {
338  return ReturnType(d, r, c, rs, cs);
339  }
340 };
341 template <typename ReturnType, typename View>
342 struct access_block_from_matrix_block_view<ReturnType, View, false, true> {
343  typedef typename View::size_type size_type;
344  template <typename Derived>
345  static ReturnType run(Derived& d, size_type r, size_type, size_type rs,
346  size_type) {
347  return d.middleRows(r, rs);
348  }
349 };
350 template <typename ReturnType, typename View>
351 struct access_block_from_matrix_block_view<ReturnType, View, true, false> {
352  typedef typename View::size_type size_type;
353  template <typename Derived>
354  static ReturnType run(Derived& d, size_type, size_type c, size_type,
355  size_type cs) {
356  return d.middleCols(c, cs);
357  }
358 };
359 
361  template <typename BlockIndexType>
362  static void run(std::ostream&, const BlockIndexType&) {}
363 };
365  template <typename BlockIndexType>
366  static void run(std::ostream& os, const BlockIndexType& bi) {
367  for (std::size_t i = 0; i < bi.size(); ++i)
368  os << "[ " << bi[i].first << ", " << bi[i].second << "], ";
369  }
370 };
371 
372 #if HPP_EIGEN_USE_EVALUATOR
373 template <typename ArgType, int _Rows, int _Cols, bool _allRows, bool _allCols>
374 struct unary_evaluator<
375  MatrixBlockView<ArgType, _Rows, _Cols, _allRows, _allCols> >
376  : evaluator_base<
377  MatrixBlockView<ArgType, _Rows, _Cols, _allRows, _allCols> > {
379 
380  enum {
381  CoeffReadCost = evaluator<ArgType>::CoeffReadCost,
382  Flags = ~PacketAccessBit & ~DirectAccessBit & ~ActualPacketAccessBit &
383  ~LinearAccessBit & ArgType::Flags,
384  Alignment = 0
385  };
386  EIGEN_DEVICE_FUNC explicit unary_evaluator(const XprType& view)
387  : m_view(view) {}
388 
389  const XprType& m_view;
390 };
391 #endif // HPP_EIGEN_USE_EVALUATOR
392 } // namespace internal
393 
394 #define EIGEN_MATRIX_BLOCKS_PUBLIC_INTERFACE(Derived) \
395  enum { AllRows = _allRows, AllCols = _allCols }; \
396  typedef MatrixBlocksBase<Derived> Base; \
397  typedef typename Base::size_type size_type; \
398  typedef typename Base::segments_t segments_t; \
399  typedef typename Base::segment_t segment_t; \
400  typedef typename Base::RowIndices_t RowIndices_t; \
401  typedef typename Base::ColIndices_t ColIndices_t;
402 
405 
406 template <typename Derived>
408  public:
409  enum {
410  AllRows = internal::traits<Derived>::AllRows,
411  AllCols = internal::traits<Derived>::AllCols,
412  OneDimension = bool(AllRows) || bool(AllCols)
413  };
420  typedef typename internal::traits<Derived>::RowIndices_t RowIndices_t;
421  typedef typename internal::traits<Derived>::ColIndices_t ColIndices_t;
422 
424  template <typename MatrixType, int _Rows = MatrixType::RowsAtCompileTime,
425  int _Cols = MatrixType::ColsAtCompileTime>
426  struct View {
428  }; // struct View
429 
430  Derived const& derived() const { return static_cast<Derived const&>(*this); }
431  Derived& derived() { return static_cast<Derived&>(*this); }
432 
437  template <typename MatrixType>
438  EIGEN_STRONG_INLINE typename View<MatrixType>::type lview(
439  const MatrixBase<MatrixType>& other) const {
440  MatrixType& o = const_cast<MatrixBase<MatrixType>&>(other).derived();
441  if (Derived::OneDimension)
442  return typename View<MatrixType>::type(o, nbIndices(), indices());
443  else
444  return typename View<MatrixType>::type(o, nbRows(), rows(), nbCols(),
445  cols());
446  }
447 
452  template <typename MatrixType>
453  EIGEN_STRONG_INLINE typename View<const MatrixType>::type rview(
454  const MatrixBase<MatrixType>& other) const {
455  if (Derived::OneDimension)
456  return typename View<const MatrixType>::type(other.derived(), nbIndices(),
457  indices());
458  else
459  return typename View<const MatrixType>::type(other.derived(), nbRows(),
460  rows(), nbCols(), cols());
461  }
462 
464  return MatrixBlocksRef<AllCols, AllRows>(nbCols(), cols(), nbRows(),
465  rows());
466  }
467 
469  assert(!AllRows);
470  return MatrixBlocksRef<AllRows, true>(nbRows(), rows());
471  }
472 
474  assert(!AllCols);
475  return MatrixBlocksRef<true, AllCols>(nbCols(), cols());
476  }
477 
483  inline const segments_t& indices() const {
484  return internal::static_if<AllRows>::rr(cols(), rows());
485  }
486 
489  inline const RowIndices_t& rows() const { return derived().rows(); }
490 
493  inline const ColIndices_t& cols() const { return derived().cols(); }
494 
500  inline const size_type& nbIndices() const {
501  return AllRows ? nbCols() : nbRows();
502  }
503 
506  inline const size_type& nbRows() const { return derived().nbRows(); }
507 
510  inline const size_type& nbCols() const { return derived().nbCols(); }
511 
515  MatrixBlocks<AllRows, AllCols> block(size_type i, size_type j, size_type ni,
516  size_type nj) const {
518  BlockIndex::extract(cols(), j, nj));
519  }
520 
524  MatrixBlocks<AllRows, AllCols> middleRows(size_type i, size_type ni) const {
526  cols());
527  }
528 
532  MatrixBlocks<AllRows, AllCols> middleCols(size_type j, size_type nj) const {
533  return MatrixBlocks<AllRows, AllCols>(rows(),
534  BlockIndex::extract(cols(), j, nj));
535  }
536 
537  protected:
540 
543 }; // class MatrixBlocks
544 
545 template <bool _allRows, bool _allCols>
546 class MatrixBlocks
547  : public MatrixBlocksBase<MatrixBlocks<_allRows, _allCols> > {
548  public:
550 
551 
552  MatrixBlocks() : m_nbRows(0), m_nbCols(0), m_rows(), m_cols() {}
553 
558  MatrixBlocks(const segments_t& rows, const segments_t& cols)
559  : m_nbRows(BlockIndex::cardinal(rows)),
560  m_nbCols(BlockIndex::cardinal(cols)),
561  m_rows(rows),
562  m_cols(cols) {}
563 
570  MatrixBlocks(const size_type& nbRows, const RowIndices_t& rows,
571  const size_type& nbCols, const ColIndices_t& cols)
572  : m_nbRows(nbRows), m_nbCols(nbCols), m_rows(rows), m_cols(cols) {}
573 
580  : m_nbRows(_allRows ? 0 : size),
581  m_nbCols(_allCols ? 0 : size),
582  m_rows(1, BlockIndex::segment_t(start, size)),
583  m_cols(1, BlockIndex::segment_t(start, size)) {}
584 
591  : m_nbRows(_allRows ? 0 : BlockIndex::cardinal(idx)),
592  m_nbCols(_allCols ? 0 : BlockIndex::cardinal(idx)),
593  m_rows(idx),
594  m_cols(idx) {}
595 
601  : m_nbRows(_allRows ? 0 : idx.second),
602  m_nbCols(_allCols ? 0 : idx.second),
603  m_rows(segments_t(1, idx)),
604  m_cols(segments_t(1, idx)) {}
605 
607  template <typename MBDerived>
609  : m_nbRows(other.nbRows()),
610  m_nbCols(other.nbCols()),
611  m_rows(other.rows()),
612  m_cols(other.cols()) {
613  EIGEN_STATIC_ASSERT((bool(AllRows) == bool(MBDerived::AllRows)) &&
614  (bool(AllCols) == bool(MBDerived::AllCols)),
615  YOU_MIXED_MATRICES_OF_DIFFERENT_SIZES);
616  }
617 
619  m_nbRows = other.m_nbRows;
620  m_nbCols = other.m_nbCols;
621  m_rows = other.m_rows;
622  m_cols = other.m_cols;
623  return *this;
624  }
626  inline void clearRows() {
627  m_rows.clear();
628  m_nbRows = 0;
629  }
630 
632  inline void clearCols() {
633  m_cols.clear();
634  m_nbCols = 0;
635  }
636 
640  inline void addRow(const size_type& row, const size_type size) {
641  m_rows.push_back(segment_t(row, size));
642  m_nbRows += size;
643  }
644 
648  inline void addCol(const size_type& col, const size_type size) {
649  m_cols.push_back(segment_t(col, size));
650  m_nbCols += size;
651  }
652 
657  template <bool Sort, bool Shrink, bool Cardinal>
658  inline void updateRows() {
659  update<Sort, Shrink, Cardinal>(m_rows, m_nbRows);
660  }
661 
666  template <bool Sort, bool Shrink, bool Cardinal>
667  inline void updateCols() {
668  update<Sort, Shrink, Cardinal>(m_cols, m_nbCols);
669  }
670 
673  inline const RowIndices_t& rows() const { return m_rows; }
674 
677  inline const ColIndices_t& cols() const { return m_cols; }
678 
681  inline const size_type& nbRows() const { return m_nbRows; }
682 
685  inline const size_type& nbCols() const { return m_nbCols; }
686 
687  template <bool Sort, bool Shrink, bool Cardinal>
688  inline void updateIndices() {
689  update<Sort, Shrink, Cardinal>(
690  internal::static_if<_allRows>::rr(m_cols, m_rows),
691  _allRows ? m_nbCols : m_nbRows);
692  }
693 
694  size_type m_nbRows, m_nbCols;
695  RowIndices_t m_rows;
697 
698  private:
699  template <bool Sort, bool Shrink, bool Cardinal>
700  static inline void update(segments_t& b, size_type& idx) {
701  if (Sort) BlockIndex::sort(b);
702  if (Shrink) BlockIndex::shrink(b);
703  if (Cardinal) idx = BlockIndex::cardinal(b);
704  }
705 }; // class MatrixBlocks
706 
708 template <bool _allRows, bool _allCols>
709 class MatrixBlocksRef
710  : public MatrixBlocksBase<MatrixBlocksRef<_allRows, _allCols> > {
711  public:
713 
714 
715  MatrixBlocksRef(const size_type& nbRows, const RowIndices_t& rows,
719  const size_type& nbCols, const ColIndices_t& cols)
720  : m_nbRows(nbRows), m_nbCols(nbCols), m_rows(rows), m_cols(cols) {}
721 
723  template <typename Derived1, typename Derived2>
725  const MatrixBlocksBase<Derived2>& cols)
726  : m_nbRows(rows.nbIndices()),
727  m_nbCols(cols.nbIndices()),
728  m_rows(rows.indices()),
729  m_cols(cols.indices()) {
730  EIGEN_STATIC_ASSERT(
731  bool(Derived1::OneDimension) && bool(Derived2::OneDimension),
732  YOU_MIXED_MATRICES_OF_DIFFERENT_SIZES);
733  }
734 
740  MatrixBlocksRef(const size_type& nidx, const segments_t& idx)
741  : m_nbRows(_allRows ? 0 : nidx),
742  m_nbCols(_allCols ? 0 : nidx),
743  m_rows(idx),
744  m_cols(idx) {}
745 
747  MatrixBlocksRef(const MatrixBlocksRef& other)
748  : Base(other),
749  m_nbRows(other.nbRows()),
750  m_nbCols(other.nbCols()),
751  m_rows(other.rows()),
752  m_cols(other.cols()) {}
753 
756  inline const RowIndices_t& rows() const { return m_rows; }
757 
760  inline const ColIndices_t& cols() const { return m_cols; }
761 
764  inline const size_type& nbRows() const { return m_nbRows; }
765 
768  inline const size_type& nbCols() const { return m_nbCols; }
769 
770  const size_type m_nbRows, m_nbCols;
771  RowIndices_t m_rows;
772  ColIndices_t m_cols;
773 }; // class MatrixBlocksRef
775 
776 template <typename Derived>
777 std::ostream& operator<<(std::ostream& os,
778  const MatrixBlocksBase<Derived>& mbi) {
779  typedef
780  typename internal::conditional<Derived::AllRows,
782  internal::print_indices>::type row_printer;
783  typedef
784  typename internal::conditional<Derived::AllCols,
785  internal::dont_print_indices,
786  internal::print_indices>::type col_printer;
787  if (!Derived::AllRows) {
788  os << "Rows: ";
789  row_printer::run(os, mbi.rows());
790  if (!Derived::AllCols) os << hpp::iendl;
791  }
792  if (!Derived::AllCols) {
793  os << "Cols: ";
794  col_printer::run(os, mbi.cols());
795  }
796  return os;
797 }
798 
801 
828 template <typename _ArgType, int _Rows = _ArgType::RowsAtCompileTime,
829  int _Cols = _ArgType::ColsAtCompileTime, bool _allRows = false,
830  bool _allCols = false>
831 class MatrixBlockView
832  : public MatrixBase<
833  MatrixBlockView<_ArgType, _Rows, _Cols, _allRows, _allCols> > {
834  public:
836  enum { Rows = _Rows, Cols = _Cols, AllRows = _allRows, AllCols = _allCols };
837  struct block_iterator {
839  size_type row, col;
840  internal::variable_if_dynamic<size_type, (_allRows ? 0 : Dynamic)> _ro;
841  internal::variable_if_dynamic<size_type, (_allCols ? 0 : Dynamic)> _co;
843  : view(v), row(0), col(0), _ro(0), _co(0) {}
845  size_type ro() const { return _ro.value(); }
847  size_type co() const { return _co.value(); }
849  size_type ri() const {
850  return internal::static_if<AllRows>::pp(std::make_pair(0, view.m_nbRows),
851  view.m_rows[row])
852  .first;
853  }
855  size_type ci() const {
856  return internal::static_if<AllCols>::pp(std::make_pair(0, view.m_nbCols),
857  view.m_cols[col])
858  .first;
859  }
861  size_type rs() const {
862  return internal::static_if<AllRows>::pp(std::make_pair(0, view.m_nbRows),
863  view.m_rows[row])
864  .second;
865  }
867  size_type cs() const {
868  return internal::static_if<AllCols>::pp(std::make_pair(0, view.m_nbCols),
869  view.m_cols[col])
870  .second;
871  }
872  // ++it
874  _ro.setValue(_ro.value() + rs());
875  ++row;
876  if (row == (size_type)view.m_rows.size()) {
877  row = 0;
878  _ro.setValue(0);
879  _co.setValue(_co.value() + cs());
880  ++col;
881  // if (col < (size_type)view.m_cols.size()) _co.setValue(0);
882  }
883  return *this;
884  };
885  // it++
887  block_iterator copy(*this);
888  operator++();
889  return copy;
890  };
891  constexpr bool valid() const {
892  return (AllRows || view.m_rows.size() > 0) &&
893  (col < (size_type)(AllCols ? 1 : view.m_cols.size()));
894  }
895  };
896  typedef MatrixBase<
899  EIGEN_GENERIC_PUBLIC_INTERFACE(MatrixBlockView)
900 
901  typedef Matrix<Scalar, RowsAtCompileTime, ColsAtCompileTime> PlainObject;
902  // typedef typename internal::ref_selector<MatrixBlockView>::type Nested;
903  typedef _ArgType ArgType;
904  typedef typename internal::ref_selector<ArgType>::type ArgTypeNested;
905  typedef typename internal::remove_all<ArgType>::type NestedExpression;
906  // typedef typename Base::CoeffReturnType CoeffReturnType;
907  // typedef typename Base::Scalar Scalar;
908 
909  template <typename Derived>
910  struct block_t {
911  typedef Block<Derived,
912  (AllRows ? Derived::RowsAtCompileTime : Eigen::Dynamic),
913  (AllCols ? Derived::ColsAtCompileTime : Eigen::Dynamic),
914  (AllCols ? (bool)Derived::IsRowMajor
915  : (AllRows ? (bool)!Derived::IsRowMajor : false))>
917  };
920 
923  typedef typename internal::conditional<_allRows, const internal::empty_struct,
924  const Indices_t&>::type RowIndices_t;
925  typedef typename internal::conditional<_allCols, const internal::empty_struct,
926  const Indices_t&>::type ColIndices_t;
927 
928  // using Base::operator=;
929 
930  MatrixBlockView(ArgType& arg, const size_type& nbRows,
931  const RowIndices_t rows, const size_type& nbCols,
932  const ColIndices_t cols)
933  : m_arg(arg),
934  m_nbRows(nbRows),
935  m_rows(rows),
936  m_nbCols(nbCols),
937  m_cols(cols) {}
938 
940  MatrixBlockView(ArgType& arg, const size_type& nbIndices,
941  const Indices_t& indices)
942  : m_arg(arg),
943  m_nbRows(_allRows ? arg.rows() : nbIndices),
944  m_rows(indices),
945  m_nbCols(_allCols ? arg.cols() : nbIndices),
946  m_cols(indices) {}
947 
948  EIGEN_STRONG_INLINE size_type rows() const { return m_nbRows; }
949  EIGEN_STRONG_INLINE size_type cols() const { return m_nbCols; }
950 
951  EIGEN_STRONG_INLINE CoeffReturnType coeff(size_type index) const {
952  assert(false &&
953  "It is not possible to access the coefficients of "
954  "MatrixBlockView this way.");
955  }
956  EIGEN_STRONG_INLINE CoeffReturnType coeff(size_type row,
957  size_type col) const {
958  assert(false &&
959  "It is not possible to access the coefficients of "
960  "MatrixBlockView this way.");
961  }
962  EIGEN_STRONG_INLINE Scalar& coeffRef(size_type index) {
963  assert(false &&
964  "It is not possible to access the coefficients of "
965  "MatrixBlockView this way.");
966  }
967  EIGEN_STRONG_INLINE Scalar& coeffRef(size_type row, const size_type& col) {
968  assert(false &&
969  "It is not possible to access the coefficients of "
970  "MatrixBlockView this way.");
971  }
972  template <typename Dest>
973  EIGEN_STRONG_INLINE void evalTo(Dest& dst) const {
975  }
976 
977  template <typename Dest>
978  EIGEN_STRONG_INLINE void writeTo(Dest& dst) const {
979  dst.resize(rows(), cols());
980  evalTo(dst.derived());
981  }
982 
983  EIGEN_STRONG_INLINE PlainObject eval() const {
984  PlainObject dst;
985  writeTo(dst);
986  return dst;
987  }
988 
989  template <typename OtherDerived>
990  EIGEN_STRONG_INLINE MatrixBlockView& operator=(
991  const EigenBase<OtherDerived>& other) {
992  EIGEN_STATIC_ASSERT_LVALUE(ArgType);
994  other.derived(), *this);
995  return *this;
996  }
997 
998  EIGEN_STRONG_INLINE size_type _blocks() const {
999  return m_rows.size() * m_cols.size();
1000  }
1001  EIGEN_STRONG_INLINE BlockXprType _block(const block_iterator& b) {
1003  BlockXprType, MatrixBlockView>::template run<ArgType>(m_arg, b.ri(),
1004  b.ci(), b.rs(),
1005  b.cs());
1006  }
1007  EIGEN_STRONG_INLINE const BlockConstXprType
1008  _block(const block_iterator& b) const {
1010  const BlockConstXprType,
1011  MatrixBlockView>::template run<const ArgType>(m_arg, b.ri(), b.ci(),
1012  b.rs(), b.cs());
1013  }
1014  EIGEN_STRONG_INLINE block_iterator _block_iterator() const {
1015  return block_iterator(*this);
1016  }
1017 
1018  EIGEN_STRONG_INLINE bool isZero(
1019  const RealScalar& prec = NumTraits<Scalar>::dummy_precision()) const {
1020  for (block_iterator block(*this); block.valid(); ++block)
1021  if (!m_arg.block(block.ri(), block.ci(), block.rs(), block.cs())
1022  .isZero(prec))
1023  return false;
1024  return true;
1025  }
1026 
1028  size_type m_nbRows;
1029  RowIndices_t m_rows;
1030  size_type m_nbCols;
1031  ColIndices_t m_cols;
1032 }; // MatrixBlockView
1033 
1035 
1036 } // namespace Eigen
1037 
1040 
1041 #undef HPP_EIGEN_USE_EVALUATOR
1042 
1043 namespace hpp {
1044 template <int Option>
1045 struct prettyPrint<constraints::segment_t, Option> {
1046  static std::ostream& run(std::ostream& os, const constraints::segment_t& s) {
1047  return os << "[ " << s.first << ", " << s.first + s.second << " ]";
1048  }
1049 };
1050 } // namespace hpp
1051 
1052 #endif // HPP_CONSTRAINTS_MATRIX_VIEW_HH
std::vector< segment_t > segments_t
Definition: fwd.hh:84
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:648
Matrix< Scalar, RowsAtCompileTime, ColsAtCompileTime > PlainObject
Definition: matrix-view.hh:901
MatrixBlocksRef< AllCols, AllRows > transpose() const
Definition: matrix-view.hh:463
MatrixBlockView< ArgType, _Rows, _Cols, _allRows, _allCols > OtherDerived
Definition: matrix-view.hh:271
Definition: matrix-view.hh:142
MatrixIndices_t::segments_t Indices_t
Definition: matrix-view.hh:922
MatrixXd::Index Index
Definition: matrix-view.hh:169
constexpr const Index & operator[](const Index &i) const
Definition: matrix-view.hh:176
const RowIndices_t & rows() const
Definition: matrix-view.hh:489
static ReturnType run(Derived &d, size_type r, size_type, size_type rs, size_type)
Definition: matrix-view.hh:345
const MatrixBlockView & view
Definition: matrix-view.hh:838
Definition: matrix-view.hh:137
MatrixBlocks(size_type start, size_type size)
Definition: matrix-view.hh:579
size_type cs() const
number of ColS
Definition: matrix-view.hh:867
MatrixBlocksRef< true, AllCols > keepCols() const
Definition: matrix-view.hh:473
const size_type & nbCols() const
Definition: matrix-view.hh:685
MatrixBlockView(ArgType &arg, const size_type &nbRows, const RowIndices_t rows, const size_type &nbCols, const ColIndices_t cols)
Definition: matrix-view.hh:930
EIGEN_STRONG_INLINE MatrixBlockView & operator=(const EigenBase< OtherDerived > &other)
Definition: matrix-view.hh:990
static size_type cardinal(const segments_t &a)
Definition: matrix-view.hh:49
size_type m_nbCols
Definition: matrix-view.hh:694
internal::conditional< _allRows, internal::empty_struct, const BlockIndex::segments_t & >::type RowIndices_t
Definition: matrix-view.hh:195
size_type ro() const
Row in the Output matrix
Definition: matrix-view.hh:845
const ColIndices_t & cols() const
Definition: matrix-view.hh:493
const Derived & d
Definition: matrix-view-operation.hh:138
traits< ArgType >::XprKind XprKind
Definition: matrix-view.hh:209
Definition: active-set-differentiable-function.hh:36
block_iterator(const MatrixBlockView &v)
Definition: matrix-view.hh:842
EIGEN_STRONG_INLINE BlockXprType _block(const block_iterator &b)
Definition: matrix-view.hh:1001
Definition: matrix-view.hh:364
MatrixBase< MatrixBlockView< _ArgType, _Rows, _Cols, _allRows, _allCols > > Base
Definition: matrix-view.hh:898
Derived & derived()
Definition: matrix-view.hh:431
EIGEN_STRONG_INLINE void evalTo(Dest &dst) const
Definition: matrix-view.hh:973
void updateIndices()
Definition: matrix-view.hh:688
block_t< ArgType >::type BlockXprType
Definition: matrix-view.hh:918
MatrixBlockView< _ArgType, _Rows, _Cols, _allRows, _allCols > Dst
Definition: matrix-view.hh:294
EIGEN_STRONG_INLINE block_iterator _block_iterator() const
Definition: matrix-view.hh:1014
size_type m_nbRows
Definition: matrix-view.hh:694
MatrixBlockView< MatrixType, _Rows, _Cols, AllRows, AllCols > type
Definition: matrix-view.hh:427
MatrixBlocks(const segments_t &idx)
Definition: matrix-view.hh:590
MatrixBlocks< AllRows, AllCols > block(size_type i, size_type j, size_type ni, size_type nj) const
Definition: matrix-view.hh:515
MatrixBlocks(const size_type &nbRows, const RowIndices_t &rows, const size_type &nbCols, const ColIndices_t &cols)
Definition: matrix-view.hh:570
Definition: fwd.hh:38
static constexpr Else & rr(Then &, Else &s)
Definition: matrix-view.hh:159
internal::remove_all< ArgType >::type NestedExpression
Definition: matrix-view.hh:905
static EIGEN_STRONG_INLINE Derived & evalTo(ActualDerived &dst, const ActualOtherDerived &other)
Definition: matrix-view.hh:259
static segments_t difference(const segment_t &a, const segment_t &b)
Compute the set difference between two segments.
Definition: matrix-view.hh:146
size_type co() const
Col in the Output matrix
Definition: matrix-view.hh:847
#define EIGEN_MATRIX_BLOCKS_PUBLIC_INTERFACE(Derived)
Definition: matrix-view.hh:394
void clearRows()
Clear rows.
Definition: matrix-view.hh:626
hpp::constraints::size_type size_type
Definition: matrix-view.hh:835
static EIGEN_STRONG_INLINE Derived & evalTo(ActualDerived &dst, const ActualOtherDerived &other)
Definition: matrix-view.hh:278
std::pair< size_type, size_type > segment_t
Definition: fwd.hh:83
MatrixBlockView< ArgType, _Rows, _Cols, _allRows, _allCols > OtherDerived
Definition: matrix-view.hh:252
ColIndices_t m_cols
Definition: matrix-view.hh:696
static ReturnType run(Derived &d, size_type r, size_type c, size_type rs, size_type cs)
Definition: matrix-view.hh:336
hpp::constraints::segment_t segment_t
Interval of indices [first, first + second - 1].
Definition: matrix-view.hh:53
EIGEN_STRONG_INLINE size_type rows() const
Definition: matrix-view.hh:948
constexpr empty_struct(In0_t, In1_t)
Definition: matrix-view.hh:174
void addRow(const size_type &row, const size_type size)
Definition: matrix-view.hh:640
EIGEN_STRONG_INLINE size_type _blocks() const
Definition: matrix-view.hh:998
const size_type & nbCols() const
Definition: matrix-view.hh:510
internal::conditional< _allRows, const internal::empty_struct, const Indices_t & >::type RowIndices_t
Definition: matrix-view.hh:924
MatrixBlocksBase()
Empty constructor.
Definition: matrix-view.hh:539
static void run(std::ostream &os, const BlockIndexType &bi)
Definition: matrix-view.hh:366
EIGEN_STRONG_INLINE Scalar & coeffRef(size_type index)
Definition: matrix-view.hh:962
internal::variable_if_dynamic< size_type,(_allRows ? 0 :Dynamic)> _ro
Definition: matrix-view.hh:840
EIGEN_STRONG_INLINE void writeTo(Dest &dst) const
Definition: matrix-view.hh:978
MatrixBlocks(const segment_t &idx)
Definition: matrix-view.hh:600
block_iterator operator++(int)
Definition: matrix-view.hh:886
block_iterator & operator++()
Definition: matrix-view.hh:873
Definition: matrix-view.hh:168
EIGEN_STRONG_INLINE const BlockConstXprType _block(const block_iterator &b) const
Definition: matrix-view.hh:1008
MatrixBlocks< _allRows, _allCols > MatrixIndices_t
Definition: matrix-view.hh:921
hpp::constraints::size_type size_type
Index of vector or matrix.
Definition: matrix-view.hh:51
Eigen::MatrixBlocks< false, true > RowBlockIndices
Definition: matrix-view.hh:799
constexpr bool valid() const
Definition: matrix-view.hh:891
static void shrink(segments_t &a)
static constexpr Else pp(Then, Else s)
Definition: matrix-view.hh:163
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:407
Definition: matrix-view.hh:124
static constexpr Index size()
Definition: matrix-view.hh:175
static segments_t fromLogicalExpression(const Eigen::ArrayBase< Derived > &array)
Definition: matrix-view.hh:31
std::ostream & operator<<(std::ostream &os, const MatrixBlocksBase< Derived > &mbi)
Definition: matrix-view.hh:777
const size_type & nbIndices() const
Definition: matrix-view.hh:500
MatrixBlocks< AllRows, AllCols > middleRows(size_type i, size_type ni) const
Definition: matrix-view.hh:524
size_type ri() const
Row in the Input matrix
Definition: matrix-view.hh:849
BlockIndex::segments_t segments_t
vector of segments
Definition: matrix-view.hh:419
size_type ci() const
Col in the Input matrix
Definition: matrix-view.hh:855
Definition: matrix-view.hh:360
RowIndices_t m_rows
Definition: matrix-view.hh:695
EIGEN_STRONG_INLINE Scalar & coeffRef(size_type row, const size_type &col)
Definition: matrix-view.hh:967
block_t< const ArgType >::type BlockConstXprType
Definition: matrix-view.hh:919
View::size_type size_type
Definition: matrix-view.hh:334
const size_type & nbRows() const
Definition: matrix-view.hh:681
static void sort(segments_t &a)
const segments_t & indices() const
Definition: matrix-view.hh:483
EIGEN_STRONG_INLINE View< const MatrixType >::type rview(const MatrixBase< MatrixType > &other) const
Definition: matrix-view.hh:453
internal::conditional< _allCols, const internal::empty_struct, const Indices_t & >::type ColIndices_t
Definition: matrix-view.hh:926
static constexpr Then pp(Then f, Else)
Definition: matrix-view.hh:152
ColIndices_t m_cols
Definition: matrix-view.hh:1031
_ArgType ArgType
Definition: matrix-view.hh:903
static constexpr Then & rr(Then &f, Else &)
Definition: matrix-view.hh:148
MatrixBlockView< _ArgType, _Rows, _Cols, _allRows, _allCols > Src
Definition: matrix-view.hh:305
ArgType & m_arg
Definition: matrix-view.hh:1027
Definition: matrix-view.hh:837
static EIGEN_STRONG_INLINE Derived & run(Derived &dst, const OtherDerived &other)
Definition: matrix-view.hh:272
MatrixBlocks(const MatrixBlocksBase< MBDerived > &other)
Copy constructor.
Definition: matrix-view.hh:608
RowIndices_t m_rows
Definition: matrix-view.hh:1029
EIGEN_STRONG_INLINE CoeffReturnType coeff(size_type index) const
Definition: matrix-view.hh:951
size_type m_nbRows
Definition: matrix-view.hh:1028
EIGEN_STRONG_INLINE PlainObject eval() const
Definition: matrix-view.hh:983
internal::conditional< _allCols, internal::empty_struct, const BlockIndex::segments_t & >::type ColIndices_t
Definition: matrix-view.hh:198
EIGEN_STRONG_INLINE View< MatrixType >::type lview(const MatrixBase< MatrixType > &other) const
Definition: matrix-view.hh:438
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:362
size_type m_nbCols
Definition: matrix-view.hh:1030
MatrixBlocksRef< AllRows, true > keepRows() const
Definition: matrix-view.hh:468
const ColIndices_t & cols() const
Definition: matrix-view.hh:677
Eigen::MatrixBlocks< true, false > ColBlockIndices
Definition: matrix-view.hh:800
internal::traits< Derived >::ColIndices_t ColIndices_t
Definition: matrix-view.hh:421
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:632
EIGEN_STRONG_INLINE size_type cols() const
Definition: matrix-view.hh:949
pinocchio::size_type size_type
Definition: fwd.hh:47
Smaller matrix composed by concatenation of the blocks.
Definition: matrix-view.hh:426
EIGEN_STRONG_INLINE CoeffReturnType coeff(size_type row, size_type col) const
Definition: matrix-view.hh:956
hpp::constraints::segments_t segments_t
vector of segments
Definition: matrix-view.hh:55
MatrixBlocks< AllRows, AllCols > middleCols(size_type j, size_type nj) const
Definition: matrix-view.hh:532
internal::traits< Derived >::RowIndices_t RowIndices_t
Definition: matrix-view.hh:420
Derived const & derived() const
Definition: matrix-view.hh:430
MatrixBlockView(ArgType &arg, const size_type &nbIndices, const Indices_t &indices)
Valid only when _allRows or _allCols is true.
Definition: matrix-view.hh:940
const size_type & nbRows() const
Definition: matrix-view.hh:506
const RowIndices_t & rows() const
Definition: matrix-view.hh:673
MatrixBlocksBase(const MatrixBlocksBase &)
Copy constructor.
Definition: matrix-view.hh:542
void updateCols()
Definition: matrix-view.hh:667
MatrixBlocks(const segments_t &rows, const segments_t &cols)
Definition: matrix-view.hh:558
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:916
static EIGEN_STRONG_INLINE Derived & run(Derived &dst, const OtherDerived &other)
Definition: matrix-view.hh:253
static std::ostream & run(std::ostream &os, const constraints::segment_t &s)
Definition: matrix-view.hh:1046
internal::ref_selector< ArgType >::type ArgTypeNested
Definition: matrix-view.hh:904
hpp::constraints::size_type size_type
Index of vector or matrix.
Definition: matrix-view.hh:415
EIGEN_STRONG_INLINE bool isZero(const RealScalar &prec=NumTraits< Scalar >::dummy_precision()) const
Definition: matrix-view.hh:1018
internal::conditional< _allRows, internal::empty_struct, BlockIndex::segments_t >::type RowIndices_t
Definition: matrix-view.hh:184
traits< ArgType >::StorageKind StorageKind
Definition: matrix-view.hh:208
MatrixBlocks & operator=(const MatrixBlocks &other)
Definition: matrix-view.hh:618
size_type row
Definition: matrix-view.hh:839
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:417
internal::conditional< _allCols, internal::empty_struct, BlockIndex::segments_t >::type ColIndices_t
Definition: matrix-view.hh:187
static ReturnType run(Derived &d, size_type, size_type c, size_type, size_type cs)
Definition: matrix-view.hh:354
Definition: matrix-view.hh:288
void updateRows()
Definition: matrix-view.hh:658
constexpr empty_struct(In_t)
Definition: matrix-view.hh:172
internal::variable_if_dynamic< size_type,(_allCols ? 0 :Dynamic)> _co
Definition: matrix-view.hh:841
size_type rs() const
number of RowS
Definition: matrix-view.hh:861
Definition: matrix-view.hh:910