hpp-constraints  4.9.1
Definition of basic geometric constraints for motion planning
matrix-view-operation.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_IMPL_MATRIX_VIEW_OPERATION_HH
18 #define HPP_CONSTRAINTS_IMPL_MATRIX_VIEW_OPERATION_HH
19 
20 namespace Eigen {
27 #define HPP_EIGEN_DECLARE_TEMPLATE_ARGS_MATRIX_BLOCK_VIEW \
28  typename _ArgType, int _Rows, int _Cols, bool _allRows, bool _allCols
29 #define HPP_EIGEN_MATRIX_BLOCK_VIEW \
30  MatrixBlockView<_ArgType, _Rows, _Cols, _allRows, _allCols>
31 
32 #define HPP_EIGEN_SPECIALIZE_CwiseBinaryOpImpl( \
33  LHS_TPL, LHS_TYPE, RHS_TPL, RHS_TYPE) \
34  template <typename BinaryOp, LHS_TPL, RHS_TPL> \
35  class CwiseBinaryOpImpl <BinaryOp, LHS_TYPE, RHS_TYPE, Dense> \
36  : public internal::dense_xpr_base< \
37  CwiseBinaryOp<BinaryOp, LHS_TYPE, RHS_TYPE > >::type \
38  { \
39  typedef LHS_TYPE Lhs_t; \
40  typedef RHS_TYPE Rhs_t; \
41  typedef CwiseBinaryOp<BinaryOp, LHS_TYPE, RHS_TYPE > Derived; \
42  public: \
43  \
44  typedef typename internal::dense_xpr_base<Derived >::type Base; \
45  EIGEN_DENSE_PUBLIC_INTERFACE( Derived ) \
46  \
47  template <typename OtherDerived> \
48  void evalTo (MatrixBase<OtherDerived>& other) const; \
49  };
50 
51 #define HPP_EIGEN_DEFINE_CwiseBinaryOpImpl_evalTo( \
52  LHS_TPL, LHS_TYPE, RHS_TPL, RHS_TYPE) \
53 template <typename BinaryOp, LHS_TPL, RHS_TPL> \
54 template <typename OtherDerived> \
55 void CwiseBinaryOpImpl<BinaryOp, LHS_TYPE, RHS_TYPE, Dense>::evalTo \
56 (MatrixBase<OtherDerived>& other) const
57 
58 # if HPP_EIGEN_USE_EVALUATOR
59 
60 #define HPP_EIGEN_SPECIALIZE_ASSIGN_SELECTOR( \
61  LHS_TPL, LHS_TYPE, RHS_TPL, RHS_TYPE) \
62  template<typename Derived, typename BinaryOp, LHS_TPL, RHS_TPL, \
63  typename Functor, typename Scalar> \
64  struct Assignment<Derived, \
65  CwiseBinaryOp<BinaryOp, LHS_TYPE, RHS_TYPE >, \
66  Functor, Dense2Dense, Scalar> { \
67  typedef CwiseBinaryOp<BinaryOp, LHS_TYPE, RHS_TYPE> CwiseDerived; \
68  static EIGEN_STRONG_INLINE void run \
69  (Derived& dst, const CwiseDerived& o, const Functor&) \
70  { o.evalTo(dst); } \
71  };
72 
73 # else // HPP_EIGEN_USE_EVALUATOR
74 
75 #define HPP_EIGEN_SPECIALIZE_ASSIGN_SELECTOR_IMPL( \
76  LHS_TPL, LHS_TYPE, RHS_TPL, RHS_TYPE, \
77  need_to_transpose, EVAL_TO_BODY) \
78  template<typename Derived, typename BinaryOp, LHS_TPL, RHS_TPL> \
79  struct assign_selector<Derived, \
80  CwiseBinaryOp<BinaryOp, LHS_TYPE, RHS_TYPE >, \
81  false,need_to_transpose> { \
82  typedef CwiseBinaryOp<BinaryOp, LHS_TYPE, RHS_TYPE> CwiseDerived; \
83  static EIGEN_STRONG_INLINE Derived& run \
84  (Derived& dst, const CwiseDerived& o) \
85  { dst.resize(o.rows(), o.cols()); o.evalTo(dst); return dst; } \
86  template<typename ActualDerived, typename ActualOtherDerived> \
87  static EIGEN_STRONG_INLINE Derived& evalTo \
88  (ActualDerived& dst, const ActualOtherDerived& other) \
89  { EVAL_TO_BODY return dst; } \
90  };
91 
92 #define HPP_EIGEN_EVAL_TO_BODY_NORMAL other.evalTo(dst);
93 #define HPP_EIGEN_EVAL_TO_BODY_TRANSPOSE Transpose<ActualDerived> dstTrans(dst); other.evalTo(dstTrans);
94 
95 #define HPP_EIGEN_SPECIALIZE_ASSIGN_SELECTOR( \
96  LHS_TPL, LHS_TYPE, RHS_TPL, RHS_TYPE) \
97  HPP_EIGEN_SPECIALIZE_ASSIGN_SELECTOR_IMPL( \
98  HPP_EIGEN_LHS_TPL, HPP_EIGEN_LHS_TYPE, \
99  HPP_EIGEN_RHS_TPL, HPP_EIGEN_RHS_TYPE, \
100  false, HPP_EIGEN_EVAL_TO_BODY_NORMAL) \
101  HPP_EIGEN_SPECIALIZE_ASSIGN_SELECTOR_IMPL( \
102  HPP_EIGEN_LHS_TPL, HPP_EIGEN_LHS_TYPE, \
103  HPP_EIGEN_RHS_TPL, HPP_EIGEN_RHS_TYPE, \
104  true, HPP_EIGEN_EVAL_TO_BODY_TRANSPOSE)
105 
106 # endif // HPP_EIGEN_USE_EVALUATOR
107 
108  // --- matrix op view -- //
109 #define HPP_EIGEN_LHS_TPL typename Lhs
110 #define HPP_EIGEN_LHS_TYPE Lhs
111 #define HPP_EIGEN_RHS_TPL HPP_EIGEN_DECLARE_TEMPLATE_ARGS_MATRIX_BLOCK_VIEW
112 #define HPP_EIGEN_RHS_TYPE const HPP_EIGEN_MATRIX_BLOCK_VIEW
113 
114  HPP_EIGEN_SPECIALIZE_CwiseBinaryOpImpl(
115  HPP_EIGEN_LHS_TPL, HPP_EIGEN_LHS_TYPE,
116  HPP_EIGEN_RHS_TPL, HPP_EIGEN_RHS_TYPE)
117  HPP_EIGEN_DEFINE_CwiseBinaryOpImpl_evalTo(
118  HPP_EIGEN_LHS_TPL, HPP_EIGEN_LHS_TYPE,
119  HPP_EIGEN_RHS_TPL, HPP_EIGEN_RHS_TYPE)
120  {
121  typedef const Block<Lhs_t> BlockLhs;
122  typedef const typename Rhs_t::
123  template block_t< typename Rhs_t::ArgType >::type BlockRhs;
124  typedef CwiseBinaryOp < BinaryOp, BlockLhs, BlockRhs > BlockCwiseBOp;
125 
126  const Derived& d = derived();
127  for(typename Rhs_t::block_iterator block (d.rhs()); block.valid(); ++block) {
128  BlockRhs rhs = d.rhs()._block(block);
129  BlockLhs lhs = d.lhs().block(block.ro(), block.co(), block.rs(), block.cs());
130  other.derived().block(block.ro(), block.co(), block.rs(), block.cs())
131  = BlockCwiseBOp (lhs, rhs, d.functor());
132  }
133  }
134 
135  namespace internal {
136  HPP_EIGEN_SPECIALIZE_ASSIGN_SELECTOR(
137  HPP_EIGEN_LHS_TPL, HPP_EIGEN_LHS_TYPE,
138  HPP_EIGEN_RHS_TPL, HPP_EIGEN_RHS_TYPE)
139  }
140 #undef HPP_EIGEN_LHS_TPL
141 #undef HPP_EIGEN_LHS_TYPE
142 #undef HPP_EIGEN_RHS_TPL
143 #undef HPP_EIGEN_RHS_TYPE
144 
145  // --- view op matrix -- //
146 #define HPP_EIGEN_LHS_TPL HPP_EIGEN_DECLARE_TEMPLATE_ARGS_MATRIX_BLOCK_VIEW
147 #define HPP_EIGEN_LHS_TYPE const HPP_EIGEN_MATRIX_BLOCK_VIEW
148 #define HPP_EIGEN_RHS_TPL typename Rhs
149 #define HPP_EIGEN_RHS_TYPE Rhs
150  HPP_EIGEN_SPECIALIZE_CwiseBinaryOpImpl(
151  HPP_EIGEN_LHS_TPL, HPP_EIGEN_LHS_TYPE,
152  HPP_EIGEN_RHS_TPL, HPP_EIGEN_RHS_TYPE)
153  HPP_EIGEN_DEFINE_CwiseBinaryOpImpl_evalTo(
154  HPP_EIGEN_LHS_TPL, HPP_EIGEN_LHS_TYPE,
155  HPP_EIGEN_RHS_TPL, HPP_EIGEN_RHS_TYPE)
156  {
157  typedef const typename Lhs_t::
158  template block_t< typename Lhs_t::ArgType >::type BlockLhs;
159  typedef const Block<Rhs_t> BlockRhs;
160  typedef CwiseBinaryOp < BinaryOp, BlockLhs, BlockRhs > BlockCwiseBOp;
161 
162  const Derived& d = derived();
163  for(typename Lhs_t::block_iterator block (d.lhs()); block.valid(); ++block) {
164  BlockLhs lhs = d.lhs()._block(block);
165  BlockRhs rhs = d.rhs().block(block.ro(), block.co(), block.rs(), block.cs());
166  other.derived().block(block.ro(), block.co(), block.rs(), block.cs())
167  = BlockCwiseBOp (lhs, rhs, d.functor());
168  }
169  }
170 
171  namespace internal {
172  HPP_EIGEN_SPECIALIZE_ASSIGN_SELECTOR(
173  HPP_EIGEN_LHS_TPL, HPP_EIGEN_LHS_TYPE,
174  HPP_EIGEN_RHS_TPL, HPP_EIGEN_RHS_TYPE)
175  }
176 #undef HPP_EIGEN_LHS_TPL
177 #undef HPP_EIGEN_LHS_TYPE
178 #undef HPP_EIGEN_RHS_TPL
179 #undef HPP_EIGEN_RHS_TYPE
180 
181  // --- view op view -- //
182 #define HPP_EIGEN_LHS_TPL HPP_EIGEN_DECLARE_TEMPLATE_ARGS_MATRIX_BLOCK_VIEW
183 #define HPP_EIGEN_LHS_TYPE const HPP_EIGEN_MATRIX_BLOCK_VIEW
184 #define HPP_EIGEN_RHS_TPL typename _ArgType2, int _Rows2, int _Cols2, bool _allRows2, bool _allCols2
185 #define HPP_EIGEN_RHS_TYPE const MatrixBlockView<_ArgType2, _Rows2, _Cols2, _allRows2, _allCols2>
186 
187  HPP_EIGEN_SPECIALIZE_CwiseBinaryOpImpl(
188  HPP_EIGEN_LHS_TPL, HPP_EIGEN_LHS_TYPE,
189  HPP_EIGEN_RHS_TPL, HPP_EIGEN_RHS_TYPE)
190  HPP_EIGEN_DEFINE_CwiseBinaryOpImpl_evalTo(
191  HPP_EIGEN_LHS_TPL, HPP_EIGEN_LHS_TYPE,
192  HPP_EIGEN_RHS_TPL, HPP_EIGEN_RHS_TYPE)
193  {
194  typedef const typename Lhs_t::
195  template block_t< typename Lhs_t::ArgType >::type BlockLhs;
196  typedef const typename Rhs_t::
197  template block_t< typename Rhs_t::ArgType >::type BlockRhs;
198  typedef CwiseBinaryOp < BinaryOp, BlockLhs, BlockRhs > BlockCwiseBOp;
199 
200  const Derived& d = derived();
201  assert (d.lhs()._blocks() == d.rhs()._blocks());
202  typename Lhs_t::block_iterator lblock (d.lhs());
203  typename Rhs_t::block_iterator rblock (d.rhs());
204  while (lblock.valid()) {
205  BlockLhs lhs = d.lhs()._block(lblock);
206  BlockRhs rhs = d.rhs()._block(lblock);
207  assert (lblock.rs() == rblock.rs() && lblock.cs() == rblock.cs());
208  assert (lblock.ro() == rblock.ro() && lblock.co() == rblock.co());
209  other.derived().block(rblock.ro(), rblock.co(), rblock.rs(), rblock.cs())
210  = BlockCwiseBOp (lhs, rhs, d.functor());
211  ++lblock; ++rblock;
212  }
213  assert (!lblock.valid() && !rblock.valid());
214  }
215 
216  namespace internal {
217  HPP_EIGEN_SPECIALIZE_ASSIGN_SELECTOR(
218  HPP_EIGEN_LHS_TPL, HPP_EIGEN_LHS_TYPE,
219  HPP_EIGEN_RHS_TPL, HPP_EIGEN_RHS_TYPE)
220  }
221 #undef HPP_EIGEN_LHS_TPL
222 #undef HPP_EIGEN_LHS_TYPE
223 #undef HPP_EIGEN_RHS_TPL
224 #undef HPP_EIGEN_RHS_TYPE
225 
226 # if !HPP_EIGEN_USE_EVALUATOR
227 # undef HPP_EIGEN_EVAL_TO_BODY_NORMAL
228 # undef HPP_EIGEN_EVAL_TO_BODY_TRANSPOSE
229 # undef HPP_EIGEN_SPECIALIZE_ASSIGN_SELECTOR_IMPL
230 # endif // !HPP_EIGEN_USE_EVALUATOR
231 
232 #undef HPP_EIGEN_SPECIALIZE_CwiseBinaryOpImpl
233 #undef HPP_EIGEN_SPECIALIZE_ASSIGN_SELECTOR
234 #undef HPP_EIGEN_DEFINE_CwiseBinaryOpImpl_evalTo
235 #undef HPP_EIGEN_DECLARE_TEMPLATE_ARGS_MATRIX_BLOCK_VIEW
236 #undef HPP_EIGEN_MATRIX_BLOCK_VIEW
237 
238 } // namespace Eigen
239 
240 #endif // HPP_CONSTRAINTS_MATRIX_VIEW_OPERATION_HH
Eigen::HPP_EIGEN_RHS_TPL
HPP_EIGEN_RHS_TPL
Definition: matrix-view-operation.hh:118
Eigen::d
const Derived & d
Definition: matrix-view-operation.hh:126
HPP_EIGEN_SPECIALIZE_ASSIGN_SELECTOR
#define HPP_EIGEN_SPECIALIZE_ASSIGN_SELECTOR( LHS_TPL, LHS_TYPE, RHS_TPL, RHS_TYPE)
Definition: matrix-view-operation.hh:95
Eigen::HPP_EIGEN_SPECIALIZE_CwiseBinaryOpImpl
HPP_EIGEN_SPECIALIZE_CwiseBinaryOpImpl(HPP_EIGEN_LHS_TPL, HPP_EIGEN_LHS_TYPE, HPP_EIGEN_RHS_TPL, HPP_EIGEN_RHS_TYPE) HPP_EIGEN_DEFINE_CwiseBinaryOpImpl_evalTo(HPP_EIGEN_LHS_TPL
Eigen::BlockRhs
const Rhs_t::template block_t< typename Rhs_t::ArgType >::type BlockRhs
Definition: matrix-view-operation.hh:123
Eigen::HPP_EIGEN_LHS_TYPE
HPP_EIGEN_LHS_TYPE
Definition: matrix-view-operation.hh:118
Eigen
Definition: fwd.hh:26
Eigen::BlockCwiseBOp
CwiseBinaryOp< BinaryOp, BlockLhs, BlockRhs > BlockCwiseBOp
Definition: matrix-view-operation.hh:124
Eigen::HPP_EIGEN_RHS_TYPE
HPP_EIGEN_RHS_TYPE
Definition: matrix-view-operation.hh:120
Eigen::assert
assert(d.lhs()._blocks()==d.rhs()._blocks())
HPP_EIGEN_DEFINE_CwiseBinaryOpImpl_evalTo
#define HPP_EIGEN_DEFINE_CwiseBinaryOpImpl_evalTo( LHS_TPL, LHS_TYPE, RHS_TPL, RHS_TYPE)
Definition: matrix-view-operation.hh:51
Eigen::lblock
Lhs_t::block_iterator lblock(d.lhs())
Eigen::rblock
Rhs_t::block_iterator rblock(d.rhs())
HPP_EIGEN_LHS_TPL
#define HPP_EIGEN_LHS_TPL
Definition: matrix-view-operation.hh:182