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