hpp-constraints 6.0.0
Definition of basic geometric constraints for motion planning
Loading...
Searching...
No Matches
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>
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
41namespace Eigen {
42
45
49struct BlockIndex {
56
59 static size_type cardinal(const segments_t& a);
60
65 template <typename Derived>
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,
121}; // struct BlockIndex
122
123template <typename ArgType, int _Rows, int _Cols, bool _allRows, bool _allCols>
124class MatrixBlockView;
125
136template <bool _allRows = false, bool _allCols = false>
137class MatrixBlocks;
138
140
141template <bool _allRows = false, bool _allCols = false>
143
144namespace internal {
145template <bool condition>
146struct 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};
156template <>
157struct 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
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
179template <bool _allRows, bool _allCols>
180struct 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
190template <bool _allRows, bool _allCols>
191struct 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
201template <typename ArgType, int _Rows, int _Cols, bool _allRows, bool _allCols>
202struct 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
225template <typename Derived, typename ArgType, int _Rows, int _Cols,
226 bool _allRows, bool _allCols, typename Functor, typename Scalar>
227struct 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
246template <typename Derived, typename ArgType, int _Rows, int _Cols,
247 bool _allRows, bool _allCols>
248struct 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};
265template <typename Derived, typename ArgType, int _Rows, int _Cols,
266 bool _allRows, bool _allCols>
267struct 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
287template <typename Src, typename Dst>
289template <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};
300template <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};
311template <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
331template <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};
341template <typename ReturnType, typename View>
342struct 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};
350template <typename ReturnType, typename View>
351struct 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
373template <typename ArgType, int _Rows, int _Cols, bool _allRows, bool _allCols>
374struct 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
406template <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>
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
516 size_type nj) const {
517 return MatrixBlocks<AllRows, AllCols>(BlockIndex::extract(rows(), i, ni),
518 BlockIndex::extract(cols(), j, nj));
519 }
520
525 return MatrixBlocks<AllRows, AllCols>(BlockIndex::extract(rows(), i, ni),
526 cols());
527 }
528
533 return MatrixBlocks<AllRows, AllCols>(rows(),
534 BlockIndex::extract(cols(), j, nj));
535 }
536
537 protected:
540
543}; // class MatrixBlocks
544
545template <bool _allRows, bool _allCols>
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
618 MatrixBlocks(const MatrixBlocks& other) = default;
619
621 m_nbRows = other.m_nbRows;
622 m_nbCols = other.m_nbCols;
623 m_rows = other.m_rows;
624 m_cols = other.m_cols;
625 return *this;
626 }
628 inline void clearRows() {
629 m_rows.clear();
630 m_nbRows = 0;
631 }
632
634 inline void clearCols() {
635 m_cols.clear();
636 m_nbCols = 0;
637 }
638
642 inline void addRow(const size_type& row, const size_type size) {
643 m_rows.push_back(segment_t(row, size));
644 m_nbRows += size;
645 }
646
650 inline void addCol(const size_type& col, const size_type size) {
651 m_cols.push_back(segment_t(col, size));
652 m_nbCols += size;
653 }
654
659 template <bool Sort, bool Shrink, bool Cardinal>
660 inline void updateRows() {
661 update<Sort, Shrink, Cardinal>(m_rows, m_nbRows);
662 }
663
668 template <bool Sort, bool Shrink, bool Cardinal>
669 inline void updateCols() {
670 update<Sort, Shrink, Cardinal>(m_cols, m_nbCols);
671 }
672
675 inline const RowIndices_t& rows() const { return m_rows; }
676
679 inline const ColIndices_t& cols() const { return m_cols; }
680
683 inline const size_type& nbRows() const { return m_nbRows; }
684
687 inline const size_type& nbCols() const { return m_nbCols; }
688
689 template <bool Sort, bool Shrink, bool Cardinal>
690 inline void updateIndices() {
691 update<Sort, Shrink, Cardinal>(
692 internal::static_if<_allRows>::rr(m_cols, m_rows),
693 _allRows ? m_nbCols : m_nbRows);
694 }
695
699
700 private:
701 template <bool Sort, bool Shrink, bool Cardinal>
702 static inline void update(segments_t& b, size_type& idx) {
703 if (Sort) BlockIndex::sort(b);
704 if (Shrink) BlockIndex::shrink(b);
705 if (Cardinal) idx = BlockIndex::cardinal(b);
706 }
707}; // class MatrixBlocks
708
710template <bool _allRows, bool _allCols>
711class MatrixBlocksRef
712 : public MatrixBlocksBase<MatrixBlocksRef<_allRows, _allCols> > {
713 public:
715
716
720 MatrixBlocksRef(const size_type& nbRows, const RowIndices_t& rows,
721 const size_type& nbCols, const ColIndices_t& cols)
722 : m_nbRows(nbRows), m_nbCols(nbCols), m_rows(rows), m_cols(cols) {}
723
725 template <typename Derived1, typename Derived2>
726 MatrixBlocksRef(const MatrixBlocksBase<Derived1>& rows,
727 const MatrixBlocksBase<Derived2>& cols)
728 : m_nbRows(rows.nbIndices()),
729 m_nbCols(cols.nbIndices()),
730 m_rows(rows.indices()),
731 m_cols(cols.indices()) {
732 EIGEN_STATIC_ASSERT(
733 bool(Derived1::OneDimension) && bool(Derived2::OneDimension),
734 YOU_MIXED_MATRICES_OF_DIFFERENT_SIZES);
735 }
736
742 MatrixBlocksRef(const size_type& nidx, const segments_t& idx)
743 : m_nbRows(_allRows ? 0 : nidx),
744 m_nbCols(_allCols ? 0 : nidx),
745 m_rows(idx),
746 m_cols(idx) {}
747
749 MatrixBlocksRef(const MatrixBlocksRef& other)
750 : Base(other),
751 m_nbRows(other.nbRows()),
752 m_nbCols(other.nbCols()),
753 m_rows(other.rows()),
754 m_cols(other.cols()) {}
755
758 inline const RowIndices_t& rows() const { return m_rows; }
759
762 inline const ColIndices_t& cols() const { return m_cols; }
763
766 inline const size_type& nbRows() const { return m_nbRows; }
767
770 inline const size_type& nbCols() const { return m_nbCols; }
771
772 const size_type m_nbRows, m_nbCols;
773 RowIndices_t m_rows;
774 ColIndices_t m_cols;
775}; // class MatrixBlocksRef
777
778template <typename Derived>
779std::ostream& operator<<(std::ostream& os,
780 const MatrixBlocksBase<Derived>& mbi) {
781 typedef
782 typename internal::conditional<Derived::AllRows,
784 internal::print_indices>::type row_printer;
785 typedef
786 typename internal::conditional<Derived::AllCols,
788 internal::print_indices>::type col_printer;
789 if (!Derived::AllRows) {
790 os << "Rows: ";
791 row_printer::run(os, mbi.rows());
792 if (!Derived::AllCols) os << hpp::iendl;
793 }
794 if (!Derived::AllCols) {
795 os << "Cols: ";
796 col_printer::run(os, mbi.cols());
797 }
798 return os;
799}
800
803
830template <typename _ArgType, int _Rows = _ArgType::RowsAtCompileTime,
831 int _Cols = _ArgType::ColsAtCompileTime, bool _allRows = false,
832 bool _allCols = false>
834 : public MatrixBase<
835 MatrixBlockView<_ArgType, _Rows, _Cols, _allRows, _allCols> > {
836 public:
838 enum { Rows = _Rows, Cols = _Cols, AllRows = _allRows, AllCols = _allCols };
842 internal::variable_if_dynamic<size_type, (_allRows ? 0 : Dynamic)> _ro;
843 internal::variable_if_dynamic<size_type, (_allCols ? 0 : Dynamic)> _co;
845 : view(v), row(0), col(0), _ro(0), _co(0) {}
847 size_type ro() const { return _ro.value(); }
849 size_type co() const { return _co.value(); }
851 size_type ri() const {
852 return internal::static_if<AllRows>::pp(std::make_pair(0, view.m_nbRows),
853 view.m_rows[row])
854 .first;
855 }
857 size_type ci() const {
858 return internal::static_if<AllCols>::pp(std::make_pair(0, view.m_nbCols),
859 view.m_cols[col])
860 .first;
861 }
863 size_type rs() const {
864 return internal::static_if<AllRows>::pp(std::make_pair(0, view.m_nbRows),
865 view.m_rows[row])
866 .second;
867 }
869 size_type cs() const {
870 return internal::static_if<AllCols>::pp(std::make_pair(0, view.m_nbCols),
871 view.m_cols[col])
872 .second;
873 }
874 // ++it
876 if (!AllRows) {
877 _ro.setValue(_ro.value() + rs());
878 } else {
879 assert(view.m_rows.size() == 1);
880 assert(row == 0);
881 }
882 ++row;
883 if (row == (size_type)view.m_rows.size()) {
884 row = 0;
885 _ro.setValue(0);
886 if (!AllCols) {
887 _co.setValue(_co.value() + cs());
888 }
889 ++col;
890 if (AllCols) {
891 // All the blocks have been iterated on.
892 assert(!valid());
893 }
894 // if (col < (size_type)view.m_cols.size()) _co.setValue(0);
895 }
896 return *this;
897 };
898 // it++
900 block_iterator copy(*this);
901 operator++();
902 return copy;
903 };
904 constexpr bool valid() const {
905 return (AllRows || view.m_rows.size() > 0) &&
906 (col < (size_type)(AllCols ? 1 : view.m_cols.size()));
907 }
908 };
909 typedef MatrixBase<
912 EIGEN_GENERIC_PUBLIC_INTERFACE(MatrixBlockView)
913
914 typedef Matrix<Scalar, RowsAtCompileTime, ColsAtCompileTime> PlainObject;
915 // typedef typename internal::ref_selector<MatrixBlockView>::type Nested;
916 typedef _ArgType ArgType;
917 typedef typename internal::ref_selector<ArgType>::type ArgTypeNested;
918 typedef typename internal::remove_all<ArgType>::type NestedExpression;
919 // typedef typename Base::CoeffReturnType CoeffReturnType;
920 // typedef typename Base::Scalar Scalar;
921
922 template <typename Derived>
923 struct block_t {
924 typedef Block<Derived,
925 (AllRows ? Derived::RowsAtCompileTime : Eigen::Dynamic),
926 (AllCols ? Derived::ColsAtCompileTime : Eigen::Dynamic),
927 (AllCols ? (bool)Derived::IsRowMajor
928 : (AllRows ? (bool)!Derived::IsRowMajor : false))>
930 };
933
936 typedef typename internal::conditional<_allRows, const internal::empty_struct,
937 const Indices_t&>::type RowIndices_t;
938 typedef typename internal::conditional<_allCols, const internal::empty_struct,
939 const Indices_t&>::type ColIndices_t;
940
941 // using Base::operator=;
942
943 MatrixBlockView(ArgType& arg, const size_type& nbRows,
944 const RowIndices_t rows, const size_type& nbCols,
945 const ColIndices_t cols)
946 : m_arg(arg),
947 m_nbRows(nbRows),
948 m_rows(rows),
949 m_nbCols(nbCols),
950 m_cols(cols) {}
951
953 MatrixBlockView(ArgType& arg, const size_type& nbIndices,
954 const Indices_t& indices)
955 : m_arg(arg),
956 m_nbRows(_allRows ? arg.rows() : nbIndices),
957 m_rows(indices),
958 m_nbCols(_allCols ? arg.cols() : nbIndices),
959 m_cols(indices) {}
960
961 EIGEN_STRONG_INLINE size_type rows() const { return m_nbRows; }
962 EIGEN_STRONG_INLINE size_type cols() const { return m_nbCols; }
963
964 EIGEN_STRONG_INLINE CoeffReturnType coeff(size_type index) const {
965 assert(false &&
966 "It is not possible to access the coefficients of "
967 "MatrixBlockView this way.");
968 }
969 EIGEN_STRONG_INLINE CoeffReturnType coeff(size_type row,
970 size_type col) const {
971 assert(false &&
972 "It is not possible to access the coefficients of "
973 "MatrixBlockView this way.");
974 }
975 EIGEN_STRONG_INLINE Scalar& coeffRef(size_type index) {
976 assert(false &&
977 "It is not possible to access the coefficients of "
978 "MatrixBlockView this way.");
979 }
980 EIGEN_STRONG_INLINE Scalar& coeffRef(size_type row, const size_type& col) {
981 assert(false &&
982 "It is not possible to access the coefficients of "
983 "MatrixBlockView this way.");
984 }
985 template <typename Dest>
986 EIGEN_STRONG_INLINE void evalTo(Dest& dst) const {
988 }
989
990 template <typename Dest>
991 EIGEN_STRONG_INLINE void writeTo(Dest& dst) const {
992 dst.resize(rows(), cols());
993 evalTo(dst.derived());
994 }
995
996 EIGEN_STRONG_INLINE PlainObject eval() const {
997 PlainObject dst;
998 writeTo(dst);
999 return dst;
1000 }
1001
1002 template <typename OtherDerived>
1003 EIGEN_STRONG_INLINE MatrixBlockView& operator=(
1004 const EigenBase<OtherDerived>& other) {
1005 EIGEN_STATIC_ASSERT_LVALUE(ArgType);
1007 other.derived(), *this);
1008 return *this;
1009 }
1010
1011 EIGEN_STRONG_INLINE size_type _blocks() const {
1012 return m_rows.size() * m_cols.size();
1013 }
1014 EIGEN_STRONG_INLINE BlockXprType _block(const block_iterator& b) {
1016 BlockXprType, MatrixBlockView>::template run<ArgType>(m_arg, b.ri(),
1017 b.ci(), b.rs(),
1018 b.cs());
1019 }
1020 EIGEN_STRONG_INLINE const BlockConstXprType
1021 _block(const block_iterator& b) const {
1023 const BlockConstXprType,
1024 MatrixBlockView>::template run<const ArgType>(m_arg, b.ri(), b.ci(),
1025 b.rs(), b.cs());
1026 }
1027 EIGEN_STRONG_INLINE block_iterator _block_iterator() const {
1028 return block_iterator(*this);
1029 }
1030
1031 EIGEN_STRONG_INLINE bool isZero(
1032 const RealScalar& prec = NumTraits<Scalar>::dummy_precision()) const {
1033 for (block_iterator block(*this); block.valid(); ++block)
1034 if (!m_arg.block(block.ri(), block.ci(), block.rs(), block.cs())
1035 .isZero(prec))
1036 return false;
1037 return true;
1038 }
1039
1045}; // MatrixBlockView
1046
1048
1049} // namespace Eigen
1050
1053
1054#undef HPP_EIGEN_USE_EVALUATOR
1055
1056namespace hpp {
1057template <int Option>
1058struct prettyPrint<constraints::segment_t, Option> {
1059 static std::ostream& run(std::ostream& os, const constraints::segment_t& s) {
1060 return os << "[ " << s.first << ", " << s.first + s.second << " ]";
1061 }
1062};
1063} // namespace hpp
1064
1065#endif // HPP_CONSTRAINTS_MATRIX_VIEW_HH
Definition matrix-view.hh:835
block_t< constArgType >::type BlockConstXprType
Definition matrix-view.hh:932
internal::remove_all< ArgType >::type NestedExpression
Definition matrix-view.hh:918
MatrixBlockView(ArgType &arg, const size_type &nbIndices, const Indices_t &indices)
Valid only when _allRows or _allCols is true.
Definition matrix-view.hh:953
internal::conditional< _allCols, constinternal::empty_struct, constIndices_t & >::type ColIndices_t
Definition matrix-view.hh:939
internal::ref_selector< ArgType >::type ArgTypeNested
Definition matrix-view.hh:917
EIGEN_STRONG_INLINE void writeTo(Dest &dst) const
Definition matrix-view.hh:991
EIGEN_STRONG_INLINE size_type _blocks() const
Definition matrix-view.hh:1011
EIGEN_STRONG_INLINE BlockXprType _block(const block_iterator &b)
Definition matrix-view.hh:1014
EIGEN_STRONG_INLINE size_type cols() const
Definition matrix-view.hh:962
EIGEN_STRONG_INLINE block_iterator _block_iterator() const
Definition matrix-view.hh:1027
ArgType & m_arg
Definition matrix-view.hh:1040
EIGEN_STRONG_INLINE bool isZero(const RealScalar &prec=NumTraits< Scalar >::dummy_precision()) const
Definition matrix-view.hh:1031
EIGEN_STRONG_INLINE const BlockConstXprType _block(const block_iterator &b) const
Definition matrix-view.hh:1021
hpp::constraints::size_type size_type
Definition matrix-view.hh:837
EIGEN_STRONG_INLINE PlainObject eval() const
Definition matrix-view.hh:996
RowIndices_t m_rows
Definition matrix-view.hh:1042
block_t< ArgType >::type BlockXprType
Definition matrix-view.hh:931
internal::conditional< _allRows, constinternal::empty_struct, constIndices_t & >::type RowIndices_t
Definition matrix-view.hh:937
ColIndices_t m_cols
Definition matrix-view.hh:1044
size_type m_nbRows
Definition matrix-view.hh:1041
EIGEN_STRONG_INLINE size_type rows() const
Definition matrix-view.hh:961
MatrixBlocks< _allRows, _allCols > MatrixIndices_t
Definition matrix-view.hh:934
EIGEN_STRONG_INLINE Scalar & coeffRef(size_type row, const size_type &col)
Definition matrix-view.hh:980
_ArgType ArgType
Definition matrix-view.hh:916
size_type m_nbCols
Definition matrix-view.hh:1043
EIGEN_STRONG_INLINE MatrixBlockView & operator=(const EigenBase< OtherDerived > &other)
Definition matrix-view.hh:1003
MatrixBlockView(ArgType &arg, const size_type &nbRows, const RowIndices_t rows, const size_type &nbCols, const ColIndices_t cols)
Definition matrix-view.hh:943
Matrix< Scalar, RowsAtCompileTime, ColsAtCompileTime > PlainObject
Definition matrix-view.hh:914
MatrixIndices_t::segments_t Indices_t
Definition matrix-view.hh:935
EIGEN_STRONG_INLINE CoeffReturnType coeff(size_type row, size_type col) const
Definition matrix-view.hh:969
EIGEN_STRONG_INLINE void evalTo(Dest &dst) const
Definition matrix-view.hh:986
EIGEN_STRONG_INLINE Scalar & coeffRef(size_type index)
Definition matrix-view.hh:975
EIGEN_STRONG_INLINE CoeffReturnType coeff(size_type index) const
Definition matrix-view.hh:964
MatrixBase< MatrixBlockView< _ArgType, _Rows, _Cols, _allRows, _allCols > > Base
Definition matrix-view.hh:911
Definition matrix-view.hh:407
internal::traits< Derived >::RowIndices_t RowIndices_t
Definition matrix-view.hh:420
EIGEN_STRONG_INLINE View< MatrixType >::type lview(const MatrixBase< MatrixType > &other) const
Definition matrix-view.hh:438
MatrixBlocks< AllRows, AllCols > middleRows(size_type i, size_type ni) const
Definition matrix-view.hh:524
BlockIndex::segments_t segments_t
vector of segments
Definition matrix-view.hh:419
MatrixBlocksBase(const MatrixBlocksBase &)
Copy constructor.
Definition matrix-view.hh:542
const RowIndices_t & rows() const
Definition matrix-view.hh:489
MatrixBlocksBase()
Empty constructor.
Definition matrix-view.hh:539
EIGEN_STRONG_INLINE View< constMatrixType >::type rview(const MatrixBase< MatrixType > &other) const
Definition matrix-view.hh:453
BlockIndex::segment_t segment_t
Interval of indices [first, first + second - 1].
Definition matrix-view.hh:417
const segments_t & indices() const
Definition matrix-view.hh:483
MatrixBlocks< AllRows, AllCols > block(size_type i, size_type j, size_type ni, size_type nj) const
Definition matrix-view.hh:515
hpp::constraints::size_type size_type
Index of vector or matrix.
Definition matrix-view.hh:415
MatrixBlocksRef< AllCols, AllRows > transpose() const
Definition matrix-view.hh:463
internal::traits< Derived >::ColIndices_t ColIndices_t
Definition matrix-view.hh:421
const size_type & nbIndices() const
Definition matrix-view.hh:500
MatrixBlocksRef< AllRows, true > keepRows() const
Definition matrix-view.hh:468
Derived & derived()
Definition matrix-view.hh:431
const ColIndices_t & cols() const
Definition matrix-view.hh:493
const size_type & nbCols() const
Definition matrix-view.hh:510
MatrixBlocks< AllRows, AllCols > middleCols(size_type j, size_type nj) const
Definition matrix-view.hh:532
const size_type & nbRows() const
Definition matrix-view.hh:506
MatrixBlocksRef< true, AllCols > keepCols() const
Definition matrix-view.hh:473
Derived const & derived() const
Definition matrix-view.hh:430
Definition matrix-view.hh:142
Definition matrix-view.hh:547
void updateCols()
Definition matrix-view.hh:669
MatrixBlocks(const segments_t &idx)
Definition matrix-view.hh:590
void addRow(const size_type &row, const size_type size)
Definition matrix-view.hh:642
const ColIndices_t & cols() const
Definition matrix-view.hh:679
const RowIndices_t & rows() const
Definition matrix-view.hh:675
MatrixBlocks(const segment_t &idx)
Definition matrix-view.hh:600
MatrixBlocks(const MatrixBlocks &other)=default
size_type m_nbCols
Definition matrix-view.hh:696
void updateIndices()
Definition matrix-view.hh:690
void updateRows()
Definition matrix-view.hh:660
void clearCols()
Clear cols.
Definition matrix-view.hh:634
MatrixBlocks & operator=(const MatrixBlocks &other)
Definition matrix-view.hh:620
const size_type & nbCols() const
Definition matrix-view.hh:687
MatrixBlocks(const size_type &nbRows, const RowIndices_t &rows, const size_type &nbCols, const ColIndices_t &cols)
Definition matrix-view.hh:570
MatrixBlocks(const segments_t &rows, const segments_t &cols)
Definition matrix-view.hh:558
void addCol(const size_type &col, const size_type size)
Definition matrix-view.hh:650
const size_type & nbRows() const
Definition matrix-view.hh:683
ColIndices_t m_cols
Definition matrix-view.hh:698
RowIndices_t m_rows
Definition matrix-view.hh:697
MatrixBlocks(size_type start, size_type size)
Definition matrix-view.hh:579
size_type m_nbRows
Definition matrix-view.hh:696
MatrixBlocks(const MatrixBlocksBase< MBDerived > &other)
Copy constructor.
Definition matrix-view.hh:608
void clearRows()
Clear rows.
Definition matrix-view.hh:628
std::ostream & operator<<(std::ostream &os, const MatrixBlocksBase< Derived > &mbi)
Definition matrix-view.hh:779
Eigen::MatrixBlocks< false, true > RowBlockIndices
Definition matrix-view.hh:801
Eigen::MatrixBlocks< true, false > ColBlockIndices
Definition matrix-view.hh:802
#define EIGEN_MATRIX_BLOCKS_PUBLIC_INTERFACE(Derived)
Definition matrix-view.hh:394
Definition fwd.hh:38
assert(d.lhs()._blocks()==d.rhs()._blocks())
const Derived & d
Definition matrix-view-operation.hh:138
std::pair< size_type, size_type > segment_t
Definition fwd.hh:83
pinocchio::size_type size_type
Definition fwd.hh:47
std::vector< segment_t > segments_t
Definition fwd.hh:84
Definition active-set-differentiable-function.hh:36
Definition matrix-view.hh:49
static void shrink(segments_t &a)
static segments_t fromLogicalExpression(const Eigen::ArrayBase< Derived > &array)
Definition matrix-view.hh:31
hpp::constraints::size_type size_type
Index of vector or matrix.
Definition matrix-view.hh:51
static segments_t difference(const segments_t &a, const segments_t &b)
static segments_t difference(const segment_t &a, const segment_t &b)
Compute the set difference between two segments.
static segments_t split(segments_t &segments, const size_type &cardinal)
static void sort(segments_t &a)
hpp::constraints::segment_t segment_t
Interval of indices [first, first + second - 1].
Definition matrix-view.hh:53
hpp::constraints::segments_t segments_t
vector of segments
Definition matrix-view.hh:55
static segments_t sum(const segment_t &a, const segment_t &b)
Compute the union of tws segments.
static size_type cardinal(const segments_t &a)
static void add(segments_t &a, const segment_t &b)
In place addition of a segment_t to segments_t.
static segments_t extract(const segments_t &segments, size_type start, size_type cardinal)
static bool overlap(const segment_t &a, const segment_t &b)
Whether two segments overlap.
static void add(segments_t &a, const segments_t &b)
In place addition of segments_t to segments_t.
static segments_t difference(const segments_t &a, const segment_t &b)
static segments_t difference(const segment_t &a, const segments_t &b)
Definition matrix-view.hh:839
size_type col
Definition matrix-view.hh:841
block_iterator operator++(int)
Definition matrix-view.hh:899
size_type rs() const
number of RowS
Definition matrix-view.hh:863
size_type ri() const
Row in the Input matrix
Definition matrix-view.hh:851
block_iterator(const MatrixBlockView &v)
Definition matrix-view.hh:844
size_type co() const
Col in the Output matrix
Definition matrix-view.hh:849
size_type cs() const
number of ColS
Definition matrix-view.hh:869
size_type ro() const
Row in the Output matrix
Definition matrix-view.hh:847
size_type ci() const
Col in the Input matrix
Definition matrix-view.hh:857
block_iterator & operator++()
Definition matrix-view.hh:875
constexpr bool valid() const
Definition matrix-view.hh:904
const MatrixBlockView & view
Definition matrix-view.hh:840
Definition matrix-view.hh:923
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:929
Smaller matrix composed by concatenation of the blocks.
Definition matrix-view.hh:426
MatrixBlockView< MatrixType, _Rows, _Cols, AllRows, AllCols > type
Definition matrix-view.hh:427
static ReturnType run(Derived &d, size_type r, size_type, size_type rs, size_type)
Definition matrix-view.hh:345
static ReturnType run(Derived &d, size_type, size_type c, size_type, size_type cs)
Definition matrix-view.hh:354
View::size_type size_type
Definition matrix-view.hh:334
static ReturnType run(Derived &d, size_type r, size_type c, size_type rs, size_type cs)
Definition matrix-view.hh:336
static EIGEN_STRONG_INLINE Derived & evalTo(ActualDerived &dst, const ActualOtherDerived &other)
Definition matrix-view.hh:278
static EIGEN_STRONG_INLINE Derived & run(Derived &dst, const OtherDerived &other)
Definition matrix-view.hh:272
MatrixBlockView< ArgType, _Rows, _Cols, _allRows, _allCols > OtherDerived
Definition matrix-view.hh:271
static EIGEN_STRONG_INLINE Derived & run(Derived &dst, const OtherDerived &other)
Definition matrix-view.hh:253
static EIGEN_STRONG_INLINE Derived & evalTo(ActualDerived &dst, const ActualOtherDerived &other)
Definition matrix-view.hh:259
MatrixBlockView< ArgType, _Rows, _Cols, _allRows, _allCols > OtherDerived
Definition matrix-view.hh:252
Definition matrix-view.hh:360
static void run(std::ostream &, const BlockIndexType &)
Definition matrix-view.hh:362
Definition matrix-view.hh:168
constexpr empty_struct(In0_t, In1_t)
Definition matrix-view.hh:174
constexpr const Index & operator[](const Index &i) const
Definition matrix-view.hh:176
static constexpr Index size()
Definition matrix-view.hh:175
MatrixXd::Index Index
Definition matrix-view.hh:169
constexpr empty_struct()=default
constexpr empty_struct(In_t)
Definition matrix-view.hh:172
MatrixBlockView< _ArgType, _Rows, _Cols, _allRows, _allCols > Src
Definition matrix-view.hh:305
MatrixBlockView< _ArgType, _Rows, _Cols, _allRows, _allCols > Dst
Definition matrix-view.hh:294
Definition matrix-view.hh:288
Definition matrix-view.hh:364
static void run(std::ostream &os, const BlockIndexType &bi)
Definition matrix-view.hh:366
static constexpr Else & rr(Then &, Else &s)
Definition matrix-view.hh:159
static constexpr Else pp(Then, Else s)
Definition matrix-view.hh:163
Definition matrix-view.hh:146
static constexpr Then & rr(Then &f, Else &)
Definition matrix-view.hh:148
static constexpr Then pp(Then f, Else)
Definition matrix-view.hh:152
traits< ArgType >::StorageKind StorageKind
Definition matrix-view.hh:208
internal::conditional< _allRows, internal::empty_struct, constBlockIndex::segments_t & >::type RowIndices_t
Definition matrix-view.hh:195
internal::conditional< _allCols, internal::empty_struct, constBlockIndex::segments_t & >::type ColIndices_t
Definition matrix-view.hh:198
internal::conditional< _allRows, internal::empty_struct, BlockIndex::segments_t >::type RowIndices_t
Definition matrix-view.hh:184
internal::conditional< _allCols, internal::empty_struct, BlockIndex::segments_t >::type ColIndices_t
Definition matrix-view.hh:187
static std::ostream & run(std::ostream &os, const constraints::segment_t &s)
Definition matrix-view.hh:1059