GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: include/pinocchio/multibody/visitor/joint-binary-visitor.hpp Lines: 43 43 100.0 %
Date: 2023-08-09 08:43:58 Branches: 16 32 50.0 %

Line Branch Exec Source
1
//
2
// Copyright (c) 2019 INRIA
3
//
4
5
#ifndef __pinocchio_multibody_visitior_joint_binary_visitor_hpp__
6
#define __pinocchio_multibody_visitior_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 arguments.
24
    ///        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(const JointModelBase<JointModelDerived1> & jmodel1,
32
                            const JointModelBase<JointModelDerived2> & jmodel2,
33
                            typename JointModelBase<JointModelDerived1>::JointDataDerived & jdata1,
34
                            typename JointModelBase<JointModelDerived2>::JointDataDerived & jdata2,
35
                            ArgsTmp args)
36
      {
37
        InternalVisitorModelAndData<JointModelDerived1,JointModelDerived2,ArgsTmp> visitor(jdata1,jdata2,args);
38
        return visitor(jmodel1.derived(),jmodel2.derived());
39
      }
40
41
      template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ArgsTmp>
42
21
      static ReturnType run(const JointModelTpl<Scalar,Options,JointCollectionTpl> & jmodel1,
43
                            const JointModelTpl<Scalar,Options,JointCollectionTpl> & jmodel2,
44
                            JointDataTpl<Scalar,Options,JointCollectionTpl> & jdata1,
45
                            JointDataTpl<Scalar,Options,JointCollectionTpl> & jdata2,
46
                            ArgsTmp args)
47
      {
48
        typedef JointModelTpl<Scalar,Options,JointCollectionTpl> JointModel;
49

21
        InternalVisitorModelAndData<JointModel,JointModel,ArgsTmp> visitor(jdata1,jdata2,args);
50
42
        return boost::apply_visitor(visitor,jmodel1,jmodel2);
51
      }
52
53
      template<typename JointModelDerived1, typename JointModelDerived2>
54
      static ReturnType run(const JointModelBase<JointModelDerived1> & jmodel1,
55
                            const JointModelBase<JointModelDerived2> & jmodel2,
56
                            typename JointModelBase<JointModelDerived1>::JointDataDerived & jdata1,
57
                            typename JointModelBase<JointModelDerived2>::JointDataDerived & jdata2)
58
      {
59
        InternalVisitorModelAndData<JointModelDerived1,JointModelDerived2,NoArg> visitor(jdata1,jdata2);
60
        return visitor(jmodel1.derived(),jmodel2.derived());
61
      }
62
63
      template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
64
21
      static ReturnType run(const JointModelTpl<Scalar,Options,JointCollectionTpl> & jmodel1,
65
                            const JointModelTpl<Scalar,Options,JointCollectionTpl> & jmodel2,
66
                            JointDataTpl<Scalar,Options,JointCollectionTpl> & jdata1,
67
                            JointDataTpl<Scalar,Options,JointCollectionTpl> & jdata2)
68
      {
69
        typedef JointModelTpl<Scalar,Options,JointCollectionTpl> JointModel;
70
21
        InternalVisitorModelAndData<JointModel,JointModel,NoArg> visitor(jdata1,jdata2);
71
42
        return boost::apply_visitor(visitor,jmodel1,jmodel2);
72
      }
73
74
      template<typename JointModelDerived1, typename JointModelDerived2, typename ArgsTmp>
75
      static ReturnType run(const JointModelBase<JointModelDerived1> & jmodel1,
76
                            const JointModelBase<JointModelDerived2> & jmodel2,
77
                            ArgsTmp args)
78
      {
79
        InternalVisitorModel<ArgsTmp> visitor(args);
80
        return visitor(jmodel1.derived(),jmodel2.derived());
81
      }
82
83
      template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ArgsTmp>
84
21
      static ReturnType run(const JointModelTpl<Scalar,Options,JointCollectionTpl> & jmodel1,
85
                            const JointModelTpl<Scalar,Options,JointCollectionTpl> & jmodel2,
86
                            ArgsTmp args)
87
      {
88

21
        InternalVisitorModel<ArgsTmp> visitor(args);
89
42
        return boost::apply_visitor(visitor,jmodel1,jmodel2);
90
      }
91
92
      template<typename JointModelDerived1, typename JointModelDerived2>
93
      static ReturnType run(const JointModelBase<JointModelDerived1> & jmodel1,
94
                            const JointModelBase<JointModelDerived2> & jmodel2)
95
      {
96
        InternalVisitorModel<NoArg> visitor;
97
        return visitor(jmodel1.derived(),jmodel2.derived());
98
      }
99
100
      template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
101
21
      static ReturnType run(const JointModelTpl<Scalar,Options,JointCollectionTpl> & jmodel1,
102
                            const JointModelTpl<Scalar,Options,JointCollectionTpl> & jmodel2)
103
      {
104
21
        InternalVisitorModel<NoArg> visitor;
105
42
        return boost::apply_visitor(visitor,jmodel1,jmodel2);
106
      }
107
108
    private:
109
110
      template<typename JointModel1, typename JointModel2, typename ArgType>
111
      struct InternalVisitorModelAndData
112
      : public boost::static_visitor<ReturnType>
113
      {
114
        typedef typename JointModel1::JointDataDerived JointData1;
115
        typedef typename JointModel2::JointDataDerived JointData2;
116
117
21
        InternalVisitorModelAndData(JointData1 & jdata1,
118
                                    JointData2 & jdata2,
119
                                    ArgType args)
120
        : jdata1(jdata1)
121
        , jdata2(jdata2)
122
21
        , args(args)
123
21
        {}
124
125
        template<typename JointModelDerived1, typename JointModelDerived2>
126
42
        ReturnType operator()(const JointModelBase<JointModelDerived1> & jmodel1,
127
                              const JointModelBase<JointModelDerived2> & jmodel2) const
128
        {
129
42
          return bf::invoke(&JointVisitorDerived::template algo<JointModelDerived1,JointModelDerived2>,
130
42
                            bf::append(boost::ref(jmodel1.derived()),
131
42
                                       boost::ref(jmodel2.derived()),
132
84
                                       boost::ref(boost::get<typename JointModelBase<JointModelDerived1>::JointDataDerived >(jdata1)),
133
84
                                       boost::ref(boost::get<typename JointModelBase<JointModelDerived2>::JointDataDerived >(jdata2)),
134
84
                                       args));
135
        }
136
137
        JointData1 & jdata1;
138
        JointData2 & jdata2;
139
140
        ArgType args;
141
      };
142
143
      template<typename JointModel1, typename JointModel2>
144
      struct InternalVisitorModelAndData<JointModel1,JointModel2,NoArg>
145
      : public boost::static_visitor<ReturnType>
146
      {
147
        typedef typename JointModel1::JointDataDerived JointData1;
148
        typedef typename JointModel2::JointDataDerived JointData2;
149
150
21
        InternalVisitorModelAndData(JointData1 & jdata1, JointData2 & jdata2)
151
        : jdata1(jdata1)
152
21
        , jdata2(jdata2)
153
21
        {}
154
155
        template<typename JointModelDerived1, typename JointModelDerived2>
156
42
        ReturnType operator()(const JointModelBase<JointModelDerived1> & jmodel1,
157
                              const JointModelBase<JointModelDerived2> & jmodel2) const
158
        {
159
42
          return bf::invoke(&JointVisitorDerived::template algo<JointModelDerived1,JointModelDerived2>,
160
42
                            bf::make_vector(boost::ref(jmodel1.derived()),
161
42
                                            boost::ref(jmodel2.derived()),
162
84
                                            boost::ref(boost::get<typename JointModelBase<JointModelDerived1>::JointDataDerived >(jdata1)),
163
126
                                            boost::ref(boost::get<typename JointModelBase<JointModelDerived2>::JointDataDerived >(jdata2))));
164
        }
165
166
        JointData1 & jdata1;
167
        JointData2 & jdata2;
168
      };
169
170
      template<typename ArgType, typename Dummy = void>
171
      struct InternalVisitorModel
172
      : public boost::static_visitor<ReturnType>
173
      {
174
21
        InternalVisitorModel(ArgType args)
175
21
        : args(args)
176
21
        {}
177
178
        template<typename JointModel1, typename JointModel2>
179
42
        ReturnType operator()(const JointModelBase<JointModel1> & jmodel1,
180
                              const JointModelBase<JointModel2> & jmodel2) const
181
        {
182
42
          return bf::invoke(&JointVisitorDerived::template algo<JointModel1,JointModel2>,
183
42
                            bf::append(boost::ref(jmodel1.derived()),
184
42
                                       boost::ref(jmodel2.derived()),
185
84
                                       args));
186
        }
187
188
        ArgType args;
189
      };
190
191
      template<typename Dummy>
192
      struct InternalVisitorModel<NoArg,Dummy>
193
      : public boost::static_visitor<ReturnType>
194
      {
195
21
        InternalVisitorModel() {}
196
197
        template<typename JointModel1, typename JointModel2>
198
21
        ReturnType operator()(const JointModelBase<JointModel1> & jmodel1,
199
                              const JointModelBase<JointModel2> & jmodel2) const
200
        {
201
          return JointVisitorDerived::
202
21
          template algo<JointModel1,JointModel2>(jmodel1.derived(),
203
21
                                                 jmodel2.derived());
204
        }
205
      };
206
207
    }; // struct JointBinaryVisitorBase
208
209
  } // namespace fusion
210
} // namespace pinocchio
211
212
#endif // ifndef __pinocchio_multibody_visitior_joint_binary_visitor_hpp__