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