hpp-constraints: include/hpp/constraints/impl/matrix-view-operation.hh Source File

hpp-constraints 9.0.2 Definition of basic geometric constraints for motion planning

Loading...
Searching...
No Matches
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
127 HPP_EIGEN_SPECIALIZE_CwiseBinaryOpImpl(HPP_EIGEN_LHS_TPL, HPP_EIGEN_LHS_TYPE,
128 HPP_EIGEN_RHS_TPL, HPP_EIGEN_RHS_TYPE)
129 HPP_EIGEN_DEFINE_CwiseBinaryOpImpl_evalTo(HPP_EIGEN_LHS_TPL,
130 HPP_EIGEN_LHS_TYPE,
131 HPP_EIGEN_RHS_TPL,
132 HPP_EIGEN_RHS_TYPE) {
133 typedef const Block<Lhs_t> BlockLhs;
134 typedef const typename Rhs_t::template block_t<typename Rhs_t::ArgType>::type
135 BlockRhs;
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 {
149 HPP_EIGEN_SPECIALIZE_ASSIGN_SELECTOR(HPP_EIGEN_LHS_TPL, HPP_EIGEN_LHS_TYPE,
150 HPP_EIGEN_RHS_TPL, HPP_EIGEN_RHS_TYPE)
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
162 HPP_EIGEN_SPECIALIZE_CwiseBinaryOpImpl(HPP_EIGEN_LHS_TPL, HPP_EIGEN_LHS_TYPE,
163 HPP_EIGEN_RHS_TPL, HPP_EIGEN_RHS_TYPE)
164 HPP_EIGEN_DEFINE_CwiseBinaryOpImpl_evalTo(HPP_EIGEN_LHS_TPL,
165 HPP_EIGEN_LHS_TYPE,
166 HPP_EIGEN_RHS_TPL,
167 HPP_EIGEN_RHS_TYPE) {
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 {
184 HPP_EIGEN_SPECIALIZE_ASSIGN_SELECTOR(HPP_EIGEN_LHS_TPL, HPP_EIGEN_LHS_TYPE,
185 HPP_EIGEN_RHS_TPL, HPP_EIGEN_RHS_TYPE)
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
200 HPP_EIGEN_SPECIALIZE_CwiseBinaryOpImpl(HPP_EIGEN_LHS_TPL, HPP_EIGEN_LHS_TYPE,
201 HPP_EIGEN_RHS_TPL, HPP_EIGEN_RHS_TYPE)
202 HPP_EIGEN_DEFINE_CwiseBinaryOpImpl_evalTo(HPP_EIGEN_LHS_TPL,
203 HPP_EIGEN_LHS_TYPE,
204 HPP_EIGEN_RHS_TPL,
205 HPP_EIGEN_RHS_TYPE) {
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 {
230 HPP_EIGEN_SPECIALIZE_ASSIGN_SELECTOR(HPP_EIGEN_LHS_TPL, HPP_EIGEN_LHS_TYPE,
231 HPP_EIGEN_RHS_TPL, HPP_EIGEN_RHS_TYPE)
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_SPECIALIZE_ASSIGN_SELECTOR
#define HPP_EIGEN_SPECIALIZE_ASSIGN_SELECTOR(LHS_TPL, LHS_TYPE, RHS_TPL, RHS_TYPE)
Definition matrix-view-operation.hh:110
HPP_EIGEN_LHS_TPL
#define HPP_EIGEN_LHS_TPL
Definition matrix-view-operation.hh:122
HPP_EIGEN_DEFINE_CwiseBinaryOpImpl_evalTo
#define HPP_EIGEN_DEFINE_CwiseBinaryOpImpl_evalTo(LHS_TPL, LHS_TYPE, RHS_TPL, RHS_TYPE)
Definition matrix-view-operation.hh:62
HPP_EIGEN_SPECIALIZE_CwiseBinaryOpImpl
#define HPP_EIGEN_SPECIALIZE_CwiseBinaryOpImpl(LHS_TPL, LHS_TYPE, RHS_TPL, RHS_TYPE)
Definition matrix-view-operation.hh:44
Eigen
Definition fwd.hh:39
Eigen::HPP_EIGEN_RHS_TPL
HPP_EIGEN_RHS_TPL
Definition matrix-view-operation.hh:131
Eigen::BlockCwiseBOp
CwiseBinaryOp< BinaryOp, BlockLhs, BlockRhs > BlockCwiseBOp
Definition matrix-view-operation.hh:136
Eigen::lblock
Lhs_t::block_iterator lblock(d.lhs())
Eigen::BlockRhs
const Rhs_t::template block_t< typenameRhs_t::ArgType >::type BlockRhs
Definition matrix-view-operation.hh:135
Eigen::HPP_EIGEN_RHS_TYPE
HPP_EIGEN_RHS_TYPE
Definition matrix-view-operation.hh:132
Eigen::assert
assert(d.lhs()._blocks()==d.rhs()._blocks())
Eigen::rblock
Rhs_t::block_iterator rblock(d.rhs())
Eigen::HPP_EIGEN_LHS_TYPE
HPP_EIGEN_LHS_TYPE
Definition matrix-view-operation.hh:130
Eigen::d
const Derived & d
Definition matrix-view-operation.hh:138