Directory: | ./ |
---|---|
File: | include/pinocchio/multibody/visitor/joint-unary-visitor.hpp |
Date: | 2024-08-27 18:20:05 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 22 | 45 | 48.9% |
Branches: | 8 | 38 | 21.1% |
Line | Branch | Exec | Source |
---|---|---|---|
1 | // | ||
2 | // Copyright (c) 2015-2023 CNRS INRIA | ||
3 | // | ||
4 | |||
5 | #ifndef __pinocchio_multibody_visitor_joint_unary_visitor_hpp__ | ||
6 | #define __pinocchio_multibody_visitor_joint_unary_visitor_hpp__ | ||
7 | |||
8 | #include <boost/variant/apply_visitor.hpp> | ||
9 | #include <boost/variant/get.hpp> | ||
10 | |||
11 | #include "pinocchio/multibody/visitor/fusion.hpp" | ||
12 | #include "pinocchio/multibody/joint/joint-base.hpp" | ||
13 | |||
14 | namespace pinocchio | ||
15 | { | ||
16 | namespace fusion | ||
17 | { | ||
18 | |||
19 | /// | ||
20 | /// \brief Base structure for \b Unary visitation of a JointModel. | ||
21 | /// This structure provides runners to call the right visitor according to the number of | ||
22 | /// arguments. This should be used when deriving new rigid body algorithms. | ||
23 | /// | ||
24 | template<typename JointVisitorDerived, typename ReturnType = void> | ||
25 | struct JointUnaryVisitorBase | ||
26 | { | ||
27 | |||
28 | template< | ||
29 | typename Scalar, | ||
30 | int Options, | ||
31 | template<typename, int> | ||
32 | class JointCollectionTpl, | ||
33 | typename ArgsTmp> | ||
34 | 1327228 | static ReturnType run( | |
35 | const JointModelTpl<Scalar, Options, JointCollectionTpl> & jmodel, | ||
36 | JointDataTpl<Scalar, Options, JointCollectionTpl> & jdata, | ||
37 | ArgsTmp args) | ||
38 | { | ||
39 | InternalVisitorModelAndData<JointModelTpl<Scalar, Options, JointCollectionTpl>, ArgsTmp> | ||
40 |
2/4✓ Branch 1 taken 667605 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 667605 times.
✗ Branch 5 not taken.
|
1327228 | visitor(jdata, args); |
41 |
1/2✓ Branch 1 taken 667605 times.
✗ Branch 2 not taken.
|
2654456 | return boost::apply_visitor(visitor, jmodel); |
42 | } | ||
43 | |||
44 | template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl> | ||
45 | ✗ | static ReturnType run( | |
46 | const JointModelTpl<Scalar, Options, JointCollectionTpl> & jmodel, | ||
47 | JointDataTpl<Scalar, Options, JointCollectionTpl> & jdata) | ||
48 | { | ||
49 | InternalVisitorModelAndData<JointModelTpl<Scalar, Options, JointCollectionTpl>, NoArg> | ||
50 | ✗ | visitor(jdata); | |
51 | ✗ | return boost::apply_visitor(visitor, jmodel); | |
52 | } | ||
53 | |||
54 | template<typename JointModelDerived, typename ArgsTmp> | ||
55 | static ReturnType run( | ||
56 | const JointModelBase<JointModelDerived> & jmodel, | ||
57 | typename JointModelBase<JointModelDerived>::JointDataDerived & jdata, | ||
58 | ArgsTmp args) | ||
59 | { | ||
60 | InternalVisitorModelAndData<JointModelDerived, ArgsTmp> visitor(jdata, args); | ||
61 | return visitor(jmodel.derived()); | ||
62 | } | ||
63 | |||
64 | template<typename JointModelDerived> | ||
65 | static ReturnType run( | ||
66 | const JointModelBase<JointModelDerived> & jmodel, | ||
67 | typename JointModelBase<JointModelDerived>::JointDataDerived & jdata) | ||
68 | { | ||
69 | InternalVisitorModelAndData<JointModelDerived, NoArg> visitor(jdata); | ||
70 | return visitor(jmodel.derived()); | ||
71 | } | ||
72 | |||
73 | template< | ||
74 | typename Scalar, | ||
75 | int Options, | ||
76 | template<typename, int> | ||
77 | class JointCollectionTpl, | ||
78 | typename ArgsTmp> | ||
79 | static ReturnType | ||
80 | 339730 | run(const JointModelTpl<Scalar, Options, JointCollectionTpl> & jmodel, ArgsTmp args) | |
81 | { | ||
82 |
2/4✓ Branch 1 taken 192902 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 192902 times.
✗ Branch 5 not taken.
|
339730 | InternalVisitorModel<ArgsTmp> visitor(args); |
83 |
1/2✓ Branch 1 taken 192902 times.
✗ Branch 2 not taken.
|
679460 | return boost::apply_visitor(visitor, jmodel); |
84 | } | ||
85 | |||
86 | template< | ||
87 | typename Scalar, | ||
88 | int Options, | ||
89 | template<typename, int> | ||
90 | class JointCollectionTpl, | ||
91 | typename ArgsTmp> | ||
92 | static ReturnType | ||
93 | ✗ | run(const JointDataTpl<Scalar, Options, JointCollectionTpl> & jdata, ArgsTmp args) | |
94 | { | ||
95 | ✗ | InternalVisitorModel<ArgsTmp> visitor(args); | |
96 | ✗ | return boost::apply_visitor(visitor, jdata); | |
97 | } | ||
98 | |||
99 | template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl> | ||
100 | ✗ | static ReturnType run(const JointModelTpl<Scalar, Options, JointCollectionTpl> & jmodel) | |
101 | { | ||
102 | ✗ | InternalVisitorModel<NoArg> visitor; | |
103 | ✗ | return boost::apply_visitor(visitor, jmodel); | |
104 | } | ||
105 | |||
106 | template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl> | ||
107 | static ReturnType run(const JointDataTpl<Scalar, Options, JointCollectionTpl> & jdata) | ||
108 | { | ||
109 | InternalVisitorModel<NoArg> visitor; | ||
110 | return boost::apply_visitor(visitor, jdata); | ||
111 | } | ||
112 | |||
113 | template<typename JointModelDerived, typename ArgsTmp> | ||
114 | static ReturnType run(const JointModelBase<JointModelDerived> & jmodel, ArgsTmp args) | ||
115 | { | ||
116 | InternalVisitorModel<ArgsTmp> visitor(args); | ||
117 | return visitor(jmodel.derived()); | ||
118 | } | ||
119 | |||
120 | template<typename JointDataDerived, typename ArgsTmp> | ||
121 | static ReturnType run(const JointDataBase<JointDataDerived> & jdata, ArgsTmp args) | ||
122 | { | ||
123 | InternalVisitorModel<ArgsTmp> visitor(args); | ||
124 | return visitor(jdata.derived()); | ||
125 | } | ||
126 | |||
127 | template<typename JointModelDerived> | ||
128 | static ReturnType run(const JointModelBase<JointModelDerived> & jmodel) | ||
129 | { | ||
130 | InternalVisitorModel<NoArg> visitor; | ||
131 | return visitor(jmodel.derived()); | ||
132 | } | ||
133 | |||
134 | template<typename JointDataDerived> | ||
135 | static ReturnType run(const JointDataBase<JointDataDerived> & jdata) | ||
136 | { | ||
137 | InternalVisitorModel<NoArg> visitor; | ||
138 | return visitor(jdata.derived()); | ||
139 | } | ||
140 | |||
141 | private: | ||
142 | template<typename JointModel, typename ArgType> | ||
143 | struct InternalVisitorModelAndData : public boost::static_visitor<ReturnType> | ||
144 | { | ||
145 | typedef typename JointModel::JointDataDerived JointData; | ||
146 | |||
147 | 1327228 | InternalVisitorModelAndData(JointData & jdata, ArgType args) | |
148 | 1327228 | : jdata(jdata) | |
149 | 1327228 | , args(args) | |
150 | { | ||
151 | 1327228 | } | |
152 | |||
153 | template<typename JointModelDerived> | ||
154 | 747133 | ReturnType operator()(const JointModelBase<JointModelDerived> & jmodel) const | |
155 | { | ||
156 |
1/2✓ Branch 1 taken 79528 times.
✗ Branch 2 not taken.
|
747133 | return bf::invoke( |
157 | &JointVisitorDerived::template algo<JointModelDerived>, | ||
158 | ✗ | bf::append( | |
159 |
1/2✓ Branch 1 taken 79528 times.
✗ Branch 2 not taken.
|
747133 | boost::ref(jmodel.derived()), |
160 | 747133 | boost::ref( | |
161 | 747133 | boost::get<typename JointModelBase<JointModelDerived>::JointDataDerived>(jdata)), | |
162 | 1494266 | args)); | |
163 | } | ||
164 | |||
165 | ReturnType operator()(const JointModelVoid) | ||
166 | { | ||
167 | return; | ||
168 | } | ||
169 | |||
170 | JointData & jdata; | ||
171 | ArgType args; | ||
172 | }; | ||
173 | |||
174 | template<typename JointModel> | ||
175 | struct InternalVisitorModelAndData<JointModel, NoArg> | ||
176 | : public boost::static_visitor<ReturnType> | ||
177 | { | ||
178 | typedef typename JointModel::JointDataDerived JointData; | ||
179 | |||
180 | ✗ | InternalVisitorModelAndData(JointData & jdata) | |
181 | ✗ | : jdata(jdata) | |
182 | { | ||
183 | } | ||
184 | |||
185 | template<typename JointModelDerived> | ||
186 | ✗ | ReturnType operator()(const JointModelBase<JointModelDerived> & jmodel) const | |
187 | { | ||
188 | ✗ | return bf::invoke( | |
189 | &JointVisitorDerived::template algo<JointModelDerived>, | ||
190 | bf::make_vector( | ||
191 | ✗ | boost::ref(jmodel.derived()), | |
192 | ✗ | boost::ref( | |
193 | ✗ | boost::get<typename JointModelBase<JointModelDerived>::JointDataDerived>(jdata)))); | |
194 | } | ||
195 | |||
196 | JointData & jdata; | ||
197 | }; | ||
198 | |||
199 | template<typename ArgType, typename Dummy = void> | ||
200 | struct InternalVisitorModel : public boost::static_visitor<ReturnType> | ||
201 | { | ||
202 | 339730 | InternalVisitorModel(ArgType args) | |
203 | 339730 | : args(args) | |
204 | { | ||
205 | 339730 | } | |
206 | |||
207 | template<typename JointModelDerived> | ||
208 | 192902 | ReturnType operator()(const JointModelBase<JointModelDerived> & jmodel) const | |
209 | { | ||
210 |
0/2✗ Branch 1 not taken.
✗ Branch 2 not taken.
|
192902 | return bf::invoke( |
211 | &JointVisitorDerived::template algo<JointModelDerived>, | ||
212 |
0/2✗ Branch 2 not taken.
✗ Branch 3 not taken.
|
578706 | bf::append(boost::ref(jmodel.derived()), args)); |
213 | } | ||
214 | |||
215 | template<typename JointDataDerived> | ||
216 | ✗ | ReturnType operator()(const JointDataBase<JointDataDerived> & jdata) const | |
217 | { | ||
218 | ✗ | return bf::invoke( | |
219 | &JointVisitorDerived::template algo<JointDataDerived>, | ||
220 | ✗ | bf::append(boost::ref(jdata.derived()), args)); | |
221 | } | ||
222 | |||
223 | ReturnType operator()(const JointModelVoid) | ||
224 | { | ||
225 | return; | ||
226 | } | ||
227 | |||
228 | ArgType args; | ||
229 | }; | ||
230 | |||
231 | template<typename Dummy> | ||
232 | struct InternalVisitorModel<NoArg, Dummy> : public boost::static_visitor<ReturnType> | ||
233 | { | ||
234 | ✗ | InternalVisitorModel() | |
235 | { | ||
236 | } | ||
237 | |||
238 | template<typename JointModelDerived> | ||
239 | ✗ | ReturnType operator()(const JointModelBase<JointModelDerived> & jmodel) const | |
240 | { | ||
241 | ✗ | return JointVisitorDerived::template algo<JointModelDerived>(jmodel.derived()); | |
242 | } | ||
243 | |||
244 | template<typename JointDataDerived> | ||
245 | ReturnType operator()(const JointDataBase<JointDataDerived> & jdata) const | ||
246 | { | ||
247 | return JointVisitorDerived::template algo<JointDataDerived>(jdata.derived()); | ||
248 | } | ||
249 | }; | ||
250 | }; // struct JointUnaryVisitorBase | ||
251 | |||
252 | } // namespace fusion | ||
253 | } // namespace pinocchio | ||
254 | |||
255 | #endif // ifndef __pinocchio_multibody_visitor_joint_unary_visitor_hpp__ | ||
256 |